An active beacon-based leader vehicle tracking system ACTA IMEKO ISSN: 2221-870X December 2019, Volume 8, Number 4, 33 - 40 ACTA IMEKO | www.imeko.org December 2019 | Volume 8 | Number 4 | 33 An active beacon-based leader vehicle tracking system Stanislaw Goll1,2, Elena Zakharova 1,2 1 Department of Information-Measuring and Biomedical Engineering, Ryazan State Radio Engineering University (RSREU), Gagarin Street, 59/1, Ryazan, Russia 2 LLC KB Avrora, Skomoroshinskaja Street 9, of 3, Ryazan, Russia Section: RESEARCH PAPER Keywords: ultrasound; mobile robot convoying; ultrasonic beacon; Kalman filter; Rauch-Tung-Striebel smoother Citation: Stanislaw Goll, Elena Zakharova, An active beacon-based leader vehicle tracking system, Acta IMEKO, vol. 8, no. 4, article 7, December 2019, identifier: IMEKO-ACTA-08 (2019)-04-07 Editor: Yvan Baudoin, International CBRNE Institute, Belgium Received November 23, 2018; In final form June 26, 2019; Published December 2019 Copyright: This is an open-access article distributed under the terms of the Creative Commons Attribution 3.0 License, which permits unrestricted use, distribution, and reproduction in any medium, provided the original author and source are credited. Corresponding author: Elena Zakharova, e-mail: zaharova@kb-avrora.ru 1. INTRODUCTION The convoying scenario is one of the most important tasks in modern robotics. A mobile robot autonomously following a leader can be widely used in such areas as agriculture, transportation, and the military. Many well-known international trials, such as ELROB, include this scenario either as an independent one or as a part of a more complex scenario. The scenario is focused on the mobile robot autonomously following some leader, which can be either a human operator or another vehicle – either autonomous or remote-controlled. To detect the leader, different systems can be used, including GNSS systems [1], [2], video and infrared cameras [3]-[7], LiDARs [8]-[10], directional antennas [11], radars, ultrasonic rangefinders [12]-[14], and their combinations [15], [16]. The nature of these systems, however, can impose various restrictions on the conditions of their use. GNSS systems perform poorly in urban areas and inside buildings. LiDARs and radars can be used to measure the distance between the vehicle and the surrounding objects, but it is not an easy task to detect the leader based on range data. Cameras, both video and infrared, can help with leader detection but depend greatly on environmental conditions. Moreover, if the obstacle appears between the leader and the robot, the accuracy of their relative positions’ estimate can be severely decreased. The mathematical methods used for localisation, obstacle detection, and occupancy grid mapping are not reliable enough to build a robust convoying system. Such methods usually require a redundant sensor data of a different nature. The leader detection suffers from the same constraints and, to make things even more difficult, the rough weather conditions, dense vegetation, smoke, and other factors should also be considered. In section 2, we discuss the implementation of an ultrasonic- based leader detection system that consists of an active beacon carried by the leader and a set of several ultrasonic receivers mounted on the convoyed robot. In section 3, different methods of computing the estimate of the beacon’s position are described. Conclusions of this study, experimental results, and dynamic errors for all methods are illustrated in section 4. Finally, recommendations for future work are given in the last section. 2. THE APPROACH In this article, we propose an ultrasonic-based leader detection system that includes an active beacon carried by the ABSTRACT This article focuses on mobile robot convoying along a path travelled by a certain leader carrying the active ultrasonic beacon. The robot is equipped with the three-dimensional receiver array in order to receive both the ultrasonic wave and the RF wave marking the beginning of the measurement cycle. To increase measurement reliability, each receiver contains two independent measurement channels with automatic gain control. The distance measurements are pre-processed in order to identify the artefacts and then either remove them or replace them with the interpolated value. To estimate the position of the beacon in the robot’s local coordinate system, several methods are used, including the least squares method with subsequent exponential smoothing, the linear Kalman filter, the Rauch-Tung-Striebel smoother, the extended Kalman Filter, the unscented Kalman filter, and the particle filter. The experiments were undertaken in order to estimate the estimation method preferable for following the leader’s path. mailto:zaharova@kb-avrora.ru ACTA IMEKO | www.imeko.org December 2019 | Volume 8 | Number 4 | 34 leader and a set of N ultrasonic receivers mounted on the convoyed robot (Figure 1). The active beacon transmits the ultrasonic waves with constant time intervals between the waves. At the same time that the ultrasonic wave is sent, the radio frequency wave marking the beginning of the measurement cycle is also transmitted. For the each of the N receivers, the time interval between the moment of the radio frequency wave and ultrasonic wave arrivals is measured according to the time-of- flight principle. These intervals are proportional to the distances between the beacon and the respective receivers and can be calculated as n nr c= , n 1, , N= , where c is the speed of the ultrasound in the air, and n is the time interval measured for the th n receiver. To estimate the beacon’s coordinates in the robot’s local coordinate system, the system of N nonlinear equations is solved. 2 m 2 m 2 m 2 n n n nr ( x x ) ( y y ) ( z z )= − + − + − , (1) where n 1, , N= , T m m m u x y z =   is a vector containing the coordinates of the beacon, and   T n n nx y z are the coordinates of the nth receiver in the convoyed robot’s local coordinate system. During the system operation, an obstacle can cause line-of- sight loss between one or several receivers and the beacon. Moreover, the ultrasonic wave can be reflected by the surrounding objects, causing the problem of multipath propagation. For these reasons, acquired measurements can contain artefact distances nr (Figure 2(a)), which are identified using the threshold constant p. The distance measurement n ,kr is considered an artefact if the following condition is met: n ,k n ,k 1r r −−  , (2) where p is an adjustable threshold constant, and k is the consequent measurement number. If the artefact is detected, the linear least squares extrapolation is used to calculate the substitute estimate based on the last V estimates  n ,k 1 n ,k Vr , ,r− − . If the series of M artefacts is detected in the receiver’s measurements, or the substitute estimate cannot be calculated due to the unreliable measurements present in the last V estimates, the corresponding equations are excluded from the system (1). When the reliable measurements (i.e. condition (2) is not met for the two consecutive measurements) arrive, the receiver is included back in the system (1), and the artefact removal procedure becomes applicable again. The system must contain at least three reliable measurements from different receivers to calculate the beacon’s position estimate u . If there are not enough reliable measurements available, the robot stops until the beacon’s position can be calculated again. To make more robust measurements, each receiver contains two independent measurement channels with partly overlapping beam patterns and automatic gain control. If both channels succeeded at the distance measurement at some moment of time k , then the average value is used as a resulting measurement. If one of the channels failed to provide a measurement, then the over channel’s measurement is used as a resulting measurement. 3. POSITION ESTIMATION Many of the effective estimation methods are based on the linear models and use normally distributed values. Figure 2b and 2c show the histograms of centred values for the measured distances nr between the beacon and the th n receiver with the b a ... Beacon 3 N 2 z 1 x y rN r1 Figure 1. (a) The active beacon and the receiver array mounted on the mobile robot (b) Convoying scenario with the two robots. b Artifacts a c Figure 2. Histogram of centered values for the single receiver’s measurements: a – measurement series containing artifacts, b – 11 meter distance between the beacon and the receiver, c – 3 meter distance between the beacon and the receiver. ACTA IMEKO | www.imeko.org December 2019 | Volume 8 | Number 4 | 35 beacon positioned along the axis of the receiver’s beam pattern at a distance of 11 and 3 metres respectively. Clearly, the histograms are multimodal, with the constant distances c  between the modes, where  is the period of ultrasound. Since the envelope’s shape can be approximated with the Gaussian function, we can assume that the distances nr are distributed normally, with the variance depending on the beacon’s position. We can use a linear combination of the equations to transform the nonlinear equations of (1) into the linear ones [17]. For each pair of receivers, the difference of the squared distances to the beacon is calculated as follows: 2 2 m 2 m 2 m 2 i j i i i m 2 m 2 m 2 j j j r r ( x x ) ( y y ) ( z z ) ( x x ) ( y y ) ( z z ) , − = − + − + − − − − − − − where i , j 1, , N= , i j , 2 NC – binomial factor, 2 NL C= – the number of receiver combinations. As a result, system (1) can be written in the form of the linear system Bu g= , (3) where B is the system matrix, with L rows containing j i j i j i2( x x ) 2( y y ) 2( z z ) − − −  , and g is the column vector of L constant terms ( )2 2 2 2 2 2 2 2i j j i j i j ir r x x y y z z− + − + − + − , i , j 1, , N= , i j . Generally, the system (3) is inconsistent due to the measurement noise. Hence, to estimate the position of the beacon, the least squares method can be used: ( ) 1 T T u B B B g . − = To compute the estimate u of the beacon’s position, different methods can be used; therefore, several estimates were computed in the course of this research. During the research, the receiver array of N 4= receivers was used; therefore, the system (3) contains 2 4L C 6= = equations. 3.1. Least squares method with exponential smoothing ( )s sk k k 1u u 1 u  −= + − , ( )0; 1  , (4) where  is the adjustable smoothing factor. 3.2. Kalman filter with the non-stationary measurement noise matrix The overall filter design is undertaken according to [18]. The stochastic system model can be described as follows: k 1 k k k k k x Ax , g Cx ,   +  = +  = + , (5) where T y zm m m x k k k k k k kx x y z v v v =   is the state vector containing the beacon’s coordinates and velocity components; 1 0 0 t 0 0 0 1 0 0 t 0 0 0 1 0 0 t A 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1          =           is the stationary transition matrix of the dynamic model, where t is the time interval between the consecutive measurements; T k 1,k L ,kg g g=    is the measurement vector; C is the stationary measurement model, with L rows j i j i j i2( x x ) 2( y y ) 2( z z ) 0 0 0 − − −  , i , j 1, , N= , i j ; y zx k k k k0 0 0 a a a  =   is process noise, ~ℕ ( | 0⃗ ); ( )2 2 2a a aQ diag 0, 0, 0, , ,  = is the stationary process noise covariance matrix; kP is the covariance matrix of the state; k is the observation noise vector, ~ℕ( | 0⃗ ); kR is the non-stationary measurement noise covariance matrix. To estimate the measurement noise covariance [19], we assume that T T T k k k k kE s s E CP C    = −    , where k k ks g Cx= − is residual, when 0 k k T T d d k d k D R , k D, R 1 s s CP C , k D, D = −   =  +    D is the estimation window size. The initial state vector is assumed to be T m m m 0 0 0 0x x y z 0 0 0 =   , input: output: begin for Prediction Update if then end else end end end 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 0 0 0x , P , R , A, Q ,C , D k kx , P k 1k k 1 x Ax −− = T k 1k k 1 P AP A Q−− = + kk k 1 k k 1 s g Cx − − = − ( ) 1 T T k kk k 1 k k 1 G P C R CP C − − − = + k kk k 1 k k 1 x x G s − − = + ( )k k k k 1P I G C P −= − k k ks g Cx= − k T T k 1 d d k d k D 1 R s s CP C D + = − = + k D k 1 0R R+ = k 1 K= → ACTA IMEKO | www.imeko.org December 2019 | Volume 8 | Number 4 | 36 where ( ) T 1 m m m T T 0 0 0 0x y z B B B g −   =   . The posteriori error covariance matrix and the measurement noise covariance matrix are written as ( )2 2 2 2 2 20 u u u v v vP diag , , , , ,     = and 2 2 0 g g L R diag , ,  =     respectively. 3.3. Rauch-Tung-Striebel smoother The sequence of the beacon’s state estimates  k k 1 k Tx , x , , x− − can be used to set the desired path for the mobile robot. If the corresponding covariance matrices  k k 1 k TP , P , , P− − are also known, then this path-to-be can be smoothed using the Rauch-Tung-Striebel smoother. The estimate and covariance sequences are rewritten as  T T 1 0x , x , , x− and  T T 1 0P , P , , P− accordingly. The initial smoothed estimate is s T Tx x= with the covariance s T TP P= . Then the smoother is applied from the last time step to the first (i.e., t T 1, ,0= − ) according to the description given in [18]. input: output: begin for end end 1 2 3 4 5 6 7 8 9 10 11    T T 1 0 T T 1 0x , x , , x , P , P , , P , A, Q,T− − t T 0= →    s s s s s sT T 1 0 T T 1 0x , x , , x , P , P , , P− − tt 1 t x Ax + = T tt 1 t P AP A Q + = + T 1 t t t 1 t S P A P − + = ( )s st t t t 1 t 1 tx x S x x+ += + − ( )s s Tt t t t 1 tt 1 tP P S P P S+ += + − 3.4. Extended Kalman filter According to [18], the stochastic system model can be described as follows: ( ) k 1 k k k k k x Ax , r h x ,   +  = +  = + (6) ( )kh x is the vector-valued function containing N elements m 2 m 2 m 2 k n k n k n( x x ) ( y y ) ( z z )− + − + − , n 1, , N= ; T k 1,k N ,kr r r=    is the measurement vector; kP is the covariance matrix of the state; kR is the non-stationary measurement noise covariance matrix. To estimate the measurement noise covariance [20], we assume that T T T k k k k k k kE s s E H P H    = −    , where ( )k k ks r h x= − is residual, when 0 k k T T d d k k k d k D R , k D, R 1 s s H P H , k D, D = −   =  +    (7) D is the estimation window size, k k k 1 H dh dx − = is the Jacobian matrix with N rows T m nk k 1 m 2 m 2 m 2 n n nk k 1 k k 1 k k 1 m nk k 1 m 2 m 2 m 2 n n nk k 1 k k 1 k k 1 m nk k 1 m 2 m 2 m 2 n n nk k 1 k k 1 k k 1 ( x x ) ( x x ) ( y y ) ( z z ) ( y y ) ( x x ) ( y y ) ( z z ) ( z z ) ( x x ) ( y y ) ( z z ) 0 0 0 − − − − − − − − − − − −  −    − + − + −    −    − + − + −     −     − + − + −             , n 1, , N= . On the initial step T m m m 0 0 0 0x x y z 0 0 0 =   , where ( ) T 1 m m m T T 0 0 0 0x y z B B B g −   =   ; ( )2 2 2 2 2 20 u u u v v vP diag , , , , ,     = ; 2 2 0 r r N R diag , ,  =     (8) input: output: begin for Prediction Update if then end else end end end 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 0 0 0x , P , R , A, Q , h , D k kx , P k 1k k 1 x Ax −− = T k 1k k 1 P AP A Q−− = + k kk k 1 k k 1 x x G s − − = + ( )k k k k k 1P I G H P −= − k D k 1 0R R+ = k 1 K= → ( )kk k 1 k k 1s r h x− −= − k k k 1 H dh dx − = ( ) 1 T T k k k k kk k 1 k k 1 G P H R H P H − − − = + ( )k k ks r h x= − k T T k 1 d d k k k d k D 1 R s s H P H D + = − = + ACTA IMEKO | www.imeko.org December 2019 | Volume 8 | Number 4 | 37 3.5. Unscented Kalman filter In the initial step, the set of sigma points is computed as 0 0 0 0 0 U 1 U U U U 2U 1 x x U 0 P P     +  = + + −       , where T m m m 0 0 0 0x x y z 0 0 0 =   , (9) ( ) T 1 m m m T T 0 0 0 0x y z B B B g −   =   . Each sigma point has its own weight, calculated by the formulas ( )m m c c0 2U 2U 1 T m m 2U 1 W w w diag w , , w w w ,   + +   = −           −       (10) input: output: begin for Prediction Update if then end else end end end 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 0 0 0X , P , R , A, Q ,W , ,U , D, h k kx , P k D k 1 0R R+ = k k 1A  −= m kk k 1 x w − =  k kk k 1 P W Q    − = + m kk k 1 k k 1 s r w − − = −  ( )k k 1 k k 1h − −= ( ) 1 T T k kk k 1 k k 1 k k 1 k k 1 G W W R    − − − − − = + k kk k 1 k k 1 x x G s − − = + ( )T Tk k k kk k 1 k k 1 k k 1P P G W R G − − −= − + m k k ks r w= −  k T T k 1 d d k k d k D 1 R s s W D  + = − = + k 1 K= →  k k 1 k k 1 k k 1x x − − −= +  k k kx x = + k kU 0 P P  + −  k k 1 k k 1 U 0 P P − −  + −   where T m m m 0 2Uw w w =   , m 0w U   = + , c 2 0w 1 U     = + − + + , ( ) m c i i 1 w w 2 U  = = + , i 1, , 2U= ; U 6= refers to the number of components in the state vector kx ; ( )2 U U  = + − is a scaling parameter; , ,   are the parameters of the method; kR is the non-stationary measurement noise covariance matrix. To estimate the measurement noise covariance [21], we assume that T T T k k k k k kE s s E W      = −    , where m k k ks r w= −  is residual, 0 k k T T d d k k d k D R , k D, R 1 s s W , k D, D   = −   =  +    (11) D is the estimation window size, ( )k kh = , kX is the array of sigma points. The initial value of ( )2 2 2 2 2 20 u u u v v vP diag , , , , , ,     = 2 2 0 r r N R diag , ,  =     . The filter implementation complies with [18]. 3.6. Particle filter To describe the beacon’s position, the following state vector T y zm m m x x x y z v v v =   is used, where ( )0x ~ x x , . The initial estimate is T m m m 0 0 0 0x x y z 0 0 0 =   , where ( ) T 1 m m m T T 0 0 0 0 0u x y z B B B g −  = =   , ( )2 2 2 2 2 2u u u v v vdiag , , , , ,      = . Based on the distribution ( )0x x , 1 , the set of particles 0X is generated. For each particle j ,kx , the weight j ,0w 1 J= is set, with the sum of the weights being J j ,0 j 1 w w 1 = = = , j 1, , J= . ACTA IMEKO | www.imeko.org December 2019 | Volume 8 | Number 4 | 38 input: output: begin for for for end end for end The particles with the weighs less than are removed where – is the size of the new particle set New particles are added to the set according to the normal distribution end end 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20      0 1 N 1 N 1 NX , A, , N , J , , x x , y y , z z  k 1 K= → kx k k 1X AX −= ( ) ( ) ( ) 2 2 2 m m m n , j ,k j ,k n j ,k n j ,k nr̂ x x y y z z= − + − + − ( ) ( )j ,k 1, j ,k 1, j ,k N , j ,k N , j ,kˆ ˆw r r , r r , =   k k j ,kj 1 w w w  = =   ( )k xx x , k j ,k j ,kj 1 x w x ,  = =  n 1 N= → j 1 J= → j 1 J= →  old j ,k j ,kX x w , j 1, , J=  = ( ) new j ,k k xX x x x , , j 1, , J = = − k old newX X X= +  4. RESULTS During the experiments, an array of N 4= receivers was statically placed, while a mobile robot carrying the active beacon was moved in front of it following a rectangular path (Figure 3) with a width of 4 m and a length of 7 m (1st experiment) or 10.5 m (2nd experiment). The robot moved clockwise, starting and finishing at the coordinates ( )2; 1− in the array’s local coordinate system. For each beacon’s position, we computed a total of six estimates using the methods described above. The number of particles in the particle filter is J 5000= . To compare the performance of these methods, we calculated the average of the Root Mean Square Errors (RMSE). Dynamic errors in the form of distances between the estimate of the active beacon trajectory and the exemplary trajectory in the two-dimensional 2 and three-dimensional coordinate systems 3 in time are presented in Figure 3. The grey area on the dynamic error graphs marks the parts of the active beacon path corresponding to the smallest sides of the rectangle. On the left-hand side, the path estimates and their dynamic errors are shown, which are calculated based on the shifted differences of the square distances between the beacon and each of the receivers ng . On the right-hand side, the path estimates and their dynamic errors are shown, which are calculated based on the distances between the beacon and each of the receivers nr . Table 1 shows the RMSEs of the beacon’s position estimates during the first experiment. Errors RMSE(|Δ2|) and RMSE(|Δ3|) correspond to the estimates made in the two- dimensional and three-dimensional coordinate systems respectively. The Rauch-Tung-Striebel smoother outperforms other considered methods in terms of RSMEs and dynamic errors. However, the path estimates shown in Figure 3 appear to be more parallelogrammatic than rectangular, altering the shape of the original path. These alterations were caused by the side wind during the experiment. During the second experiment, no wind was present. Furthermore, the length of the ground truth rectangle was increased to 10.5 m. The path estimates and the corresponding dynamic errors are shown in Figure 4. The RSMEs are shown in Table 2. Clearly, the Rauch-Tung-Striebel smoother demonstrates the best estimate again, despite changes in the experimental conditions. – 3 – 2 – 1 0 1 2 3 0 1 2 3 4 5 6 7 8 – 3 – 2 – 1 0 1 2 3 0 1 2 3 4 5 6 7 8 1 2 3 4 5 6 0.5 1.5 y,m y,m x,m x,m 5 15 25 35 0 1 2 0.5 1.5 5 15 25 35 0 1 2 0.5 1.5 5 15 25 35 0 1 2 5 15 25 35 0 0.5 1 1.5 2 t,s t,s t,s t,s |Δ2| |Δ3| |Δ2| |Δ3| Figure 3. Path estimates and the corresponding dynamic errors in the 1st experiment: 1. Least squares method with exponential smoothing. 2. Kalman filter. 3. Kalman filter with Rauch-Tung-Striebel smoother. 4. Unscented Kalman filter. 5. Particle filter. 6. Extended Kalman filter. Table 1. The root mean square errors of the beacon’s position estimates during the first experiment RMSE(|Δ2|), m RMSE(|Δ3|), m ES 0.7365 0.8948 KF 0.6161 1.0029 RTS 0.4776 0.7410 EKF 0.5569 0.9255 UKF 0.4922 0.8875 PF 0.4830 1.0422 ACTA IMEKO | www.imeko.org December 2019 | Volume 8 | Number 4 | 39 5. FUTURE RESEARCH The main goal of our future research in this area is to eliminate biases caused by the wind and measurement errors during the estimation of the system’s structural parameters. To estimate these parameters, we plan to perform a calibration procedure involving the 3D LiDAR mounted on the robot. Given the 3 cm accuracy of the LiDAR measurements and usage of the machine learning algorithms, such calibration should outperform the current manual measurements made with the slide rule. Moreover, it should also allow for estimating the size and the shape of the receivers. Currently, the effective measurement range is up to 20 m; however, it can be extended to 100 m. This can be achieved by combining the ultrasonic measurements with the GNSS-based measurements. In this case, it is possible to mitigate the relative displacement between the robot’s and beacon’s GNSS receivers by mixing the ultrasonic and the GNSS data, while the distance between the robot and the beacon is relatively small. REFERENCES [1] M. Spencer, D. Jones, M. Kraehling, K. Stol, Trajectory-based autonomous vehicle following using a robotic driver, Proc. of the Australasian Conference on Robotics and Automation, 2009, Brisbane, Australia, Australian Robotics and Automation Association, pp. 1-10. [2] N. Wong, K. A. Stol, C. Chambers, R. Halkyard, Autonomous vehicle following using a robotic driver, Proc. of the IEEE 15th International Conference on Mechatronics and Machine Vision in Practice, 2-4 December 2008, Auckland, New Zealand, pp.115-120. [3] Z. Changchen, C. Weihai, Z. Zhiwen, L. Jingmeng, An RGBD data based vehicle detection algorithm for vehicle following systems, Proc. of the 2013 8th IEEE Conference on Industrial Electronics and Applications (ICIEA), 19-21 June 2013, Melbourne, Australia, pp. 1506-1511. [4] R. Vidal, O. Shakernia, S. Sastry, Following the flock: distributed formation control with omnidirectional vision-based motion segmentation and visual servoing. IEEE Robotics & Automation Magazine 11(4) (2004) pp. 14-20. [5] J. Krejsa, S. Vechet, Infrared beacons based localization of mobile robot. Electronics and Electrical Engineering 1(117) (2012) pp. 17-22. [6] C. Fries, H.-J. Wuensche, Monocular template-based vehicle tracking for autonomous convoy driving, Proc. of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2014, pp. 2727-2732 [7] L. Zhang, T. Ahamed, Y. Zhang, P. Gao, T. Takigawa, Vision- based leader vehicle trajectory tracking for multiple agricultural vehicles, Sensors 16(4) (2016) p. 578. [8] T. C. Ng, J. Ibanez-Guzman, J. Shen, Z. Gong, H. Wang, C. Cheng, Vehicle following with obstacle avoidance capabilities in natural environments, Proc. of the IEEE Int. Conf. Robotics and Automation ICRA '04, April 2004, New Orleans, LA, USA (5) pp. 4283-4288. [9] C. Fries, H.-J. Wuensche, Autonomous convoy driving by night: the vehicle tracking system, Proc. of the IEEE Conf. Technol. Pract. Robot Appl. TePRA, 2015, pp. 1-6. [10] M. Himmelsbach, A. Muller, T. Luttel, H.-J. Wunsche, LIDAR- based 3D object perception, Proc. of the 1st International Workshop on Cognition for Technical Systems, October 2008, Munchen, pp. 1–7. [11] B.-C. Min, E. T. Matson, Robotic follower system using bearing- only tracking with directional antennas, Proc. of the 2nd International Conference on Robot Intelligence Technology and Applications (RITA 2013) 18-20 December 2013, Denver, Colorado, USA, pp. 37-58. [12] S. Shoval, J. Borenstein, Measuring the relative position and orientation between two mobile robots with binaural sonar, Proc. of the 9th ANS International Topical Meeting on Robotics and Remote Systems, 4-8 March 2001, Seattle, Washington, USA. [13] F. Hoflinger, L. M. Reindl, TDOA based localization using interacting multiple model estimator and ultrasonic transmitter/receiver, Proc. of the 9th IEEE International Multi- Conference on Systems, Signals and Devices, 20-23 March 2012, Chemnitz, Germany (1) pp.1063-1069. [14] E. S. Ningrum, R. Hakkun, A. Alasiry, 2016, Tracking and formation control of leader-follower cooperative mobile robot based on trilateration data. International Journal of Engineering Technology EMITTER 3(2) (2016) pp. 88-98. [15] T. C. Ng, J. I. Guzman, M. D. Adams, 2005, Autonomous vehicle following systems: a virtual trailer link model, Proc. of the IEEE/RSJ International Conference on Intelligent Robots and Systems, 2-6 August 2005, Edmonton, Alta., Canada, pp. 3057- 3062. 0.5 1.5 20 40 60 80 0 1 2 t,s 20 40 60 800 0.5 1 1.5 2 t,s20 40 60 800 0.5 1 1.5 2 t,s 20 40 60 800 0.5 1 1.5 2 t,s -3 -2 -1 0 1 2 30 2 4 6 8 10 12 y,m x,m 4 5 6 -3 -2 -1 0 1 2 30 2 4 6 8 10 12 1 2 3 y,m x,m |Δ2| |Δ3| |Δ2| |Δ3| Figure 4. Path estimates and the corresponding dynamic errors in the 2nd experiment: 1 – least squares method with the exponential smoothing, 2 – Kalman Filter, 3 – Kalman Filter with Rauch-Tung-Striebel smoother, 4 – Unscented Kalman Filter, 5 – Particle Filter, 6 – Extended Kalman Filter. Table 2. The root mean square errors of the beacon’s position estimates during the second experiment. RMSE(|Δ2|), m RMSE(|Δ3|), m ES 0.7502 1.0236 KF 0.5318 0.7447 RTS 0.3682 0.5431 EKF 0.4386 0.6350 UK F 0.4471 0.6552 PF 0.5005 0.6052 ACTA IMEKO | www.imeko.org December 2019 | Volume 8 | Number 4 | 40 [16] D. Simon, B. Theisen, Convoy active safety technology: environmental understanding and navigation with use of low cost sensors, Proc. of the NDiA Ground Vehicle Systems Engineering and Technology Symposium, 2012. [17] A. Montanha, A. M. Polidorio, F. J. Dominguez-Mayo, M. J. Escalona, 2D triangulation of signals source by pole-polar geometric models, Sensors 19(5) (2019) p. 1020. [18] J. Hartikainen, S. Sumo, Optimal filtering with Kalman filters and smoothers, a manual for the Matlab toolbox EKF/UKF, Department of Biomedical Engineering and Computational Science, Aalto University School of Science, Espoo, Finland, August 2011, p.129. [19] J. Wang, W. Ding, J. Wang, Improving adaptive Kalman filter in GPS/SDINS integration with neural network, Proc. of the ION GNSS 2007, 25-28 September 2007, Fort Worth, TX, USA (1) pp. 571-579. [20] S. Akhlaghi, N. Zhou, Z. Huang, Adaptive adjustment of noise covariance in Kalman filter for dynamic state estimation, arXiv preprint arXiv:1702.00884, 2017, pp.1-5. [21] B. Zheng, P. Fu, B. Li, X. Yuan, A robust adaptive unscented Kalman filter for nonlinear estimation with uncertain noise covariance, Sensors 18 (2018) p. 808.