Distributed Finite Memory Estimation From Relative Measurements for Multiple-Robot Localization in Wireless Sensor Networks

Mobile robot localizations have been extensively studied, and various algorithms for multiple-robot localization have been developed. However, existing methods for multiple-robot localization often exhibit poor performance under harsh conditions, such as missing measurements and sudden appearance of obstacles. To overcome this problem, this paper proposes a novel method for multiple-robot localization in wireless sensor networks. The proposed method is theoretically based on the finite memory estimation and utilizes relative distance and angle measurements between robots. Thus, the proposed method is referred to as distributed finite memory estimation from relative measurements (DFMERM). Due to the finite memory structure, the DFMERM has inherent robustness against computational and modeling errors. Moreover, the novel distributed localization method using relative measurements shows the robustness against missing measurements. Robust DFMERM localization performance is experimentally demonstrated using multiple mobile robots under the harsh conditions.

cause bottlenecks and data loss [10]- [12]. The distributed algorithms can solve this problem by processing the data in distributed multiple servers. Various algorithms for multiplerobot localization have been developed [13]- [24], and especially, in [13], the problem of the distributed localization of multiple mobile robots when the communication link with neighboring nodes is temporarily lost was studied. In addition, machine learning (ML)-based localization algorithms for WSNs were developed to obtain an accurate location and minimize the adverse effects of internal or external factors [25]. A ML-based localization algorithm, the Bayesian technique with sequential Monte Carlo was used to estimate the location of unknown nodes [26]. The Bayesian compressive sensing technique for adaptive localization was developed to estimate the target locations, considering environmental noise variance [27]. The variational Bayesian interface based iterative technique was introduced to recover sparse signals and faulty prior information [28]. However, these algorithms are based on infinite impulse response (IIR) estimation, and they may exhibit estimation divergence due to the accumulation of computational or modeling errors.
IIR estimation is familiar to engineers because the Kalman filter (KF) is one of the most commonly known IIR estimation algorithms. Different versions of KF have been studied and applied to diverse engineering problems, including the distributed localization [14], [16], [17]. However, KFs suffer from filter divergence, which is caused by the accumulation of computational or modeling errors. To overcome this problem, finite memory estimation (FME) algorithms [5], [12], [29]- [37] that use only recent finite measurements have been studied. Distributed FME were studied in [32]- [34], and they exhibited better robustness than those of distributed IIR filters. However, these studies were limited to the problem of estimating the location of a single robot in a WSN.
In this paper, we propose a novel distributed FME algorithm for multiple-robot localization; thus, it can estimate the pose (i.e., position and heading angle) of all individual robots in a WSN. The proposed FME is referred to as the distributed FME from relative measurements (DFMERM). The technique for refining sensor measurements proposed in our previous study [31] is evolved and extended to the distributed multiple-robot localization problem. The measurements of relative distance and heading angle between robots are transmitted to neighbors through robot-to-robot direct communication, which facilitates distributed localization. In addition, the novel measurement redefining technique proposed in this paper helps the localization algorithm to cope with missing measurements. Lastly, the finite memory (FM) structure makes the DFMERM robust against the computational errors caused by the missing measurements. Superior DFMERM robustness and reliable localization performance are experimentally demonstrated using the multiple mobile robots under harsh conditions, including missing measurements and sudden appearance of an obstacle. The main contributions and novelty of this paper can be summarized as follows: 1) The proposed DFMERM is the first distributed FME algorithm that is capable of multiple-robot localization; existing distributed FME algorithms [32]- [34] are only available for single-robot localization. 2) We designed a novel distributed multiple-robot localization system, in which a few robots are equipped with expensive sensors that are capable of absolute positioning, whereas the majority of robots have lowcost sensors that can only measure relative distance and bearing between robots. This reduces the implementation cost of multiple-robot localization systems, and the cost reduction effect increases as the number of robots increases. 3) A novel FME algorithm to overcome the missing measurement problem was proposed. The missing measurements were replaced by the predicted measurements, and the measurement equation was redefined.
The novel FME was derived using the new state-space model based on redefined measurements. The resulting DFMERM experimentally exhibited robustness against missing measurements. 4) The proposed DFMERM exhibited superior localization performance that is better than that of the conventional IIR localization algorithm. The DFMERM was more robust than the distributed IIR filter against not only the missing measurements but also sudden appearance of an obstacle because the DFMERM has the FM filter structure, which is inherently robust against modeling and computational errors. The remainder of this paper is organized as follows. Section II describes scheme of the distributed multiplerobot localization using relative measurements. Section III derives the DFMERM by utilizing the Frobenius norm and Lagrange multiplier method. Section IV presents the results of experiments using six mobile robots. Finally, conclusions are drawn in Section V.

II. SCHEME OF DISTRIBUTED MULTIPLE-ROBOT LOCALIZATION USING RELATIVE MEASUREMENTS
We consider a situation in which multiple mobile robots are in a 2-dimensional (2D) indoor space. We divide the robots into two groups, Groups A and B. Group A is composed of a few robots equipped with high-resolution sensors that can obtain their positions. Group B has many robots equipped with low-cost sensors that can only measure relative distance and/or bearing. This paper focuses on the localization of multiple robots in Group B using the relative measurements.

A. KINEMATIC MODEL
The state and control input of the i-th robot at a discrete time step k are defined as follows: where p i,k = x i,k y i,k T , with positions x i,k and y i,k being in the x-and y-axes, respectively. θ i,k is the heading angle of the i-th robot with respect to the x-axis, and v l i,k and v θ i,k are the linear and angular velocities of the i-th robot. We consider the nonholonomic unicycle kinematic model of the mobile robot described in Fig. 1. The relationship between s i,k and s i,k+1 with time variation t is defined as [8], [31] where ω i,k is process noise vector of the i-th robot at time k, andf i,k is defined as

B. MEASUREMENT
We consider a directed graph, which is denoted as G = (K, E), where K and E are the set of robots and links, respectively. The number of robots is M = K . In E, the direction from robot j to i is denoted by (i, j). The neighbors for robot i are defined as N i = {j : (i, j) ∈ E}. Moreover, robot i can fetch the estimated state of Group A neighboring robots through directed communication. Accordingly, J i = |N i | denotes the number of Group A i-th neighbors. We assume that the i-th robot can obtain the estimated positions of neighbors at every sampling time.
The description of the measurement is shown in Fig. 2. The measurement at time k of the i-th robot is where and π i,k is the measurement noise vector of the i-th robot at time k.p j,k is the estimated two-dimensional position of the j-th robot at time k, d j i,k denotes the center distance between the i-th and j-th robots at time k, η j i,k denotes the angle from the heading of the i-th robot to the j-th robot at time k, and

C. REDEFINED MEASUREMENT
As the number of robots increases during position estimation for the multiple mobile robot system, temporal failure of the communication link occurs more frequently, which makes it difficult to estimate the robot positions accurately, and it may cause a significant failure in the estimation [13]. In this case, the estimation algorithm needs to handle temporal missing communication. Therefore, we rewrite (5) using previously stored estimated values [31]: is the predicted value using the previous estimated stateŝ i,k−1 , andπ i,k is the redefined measurement noise vector of the i-th robot at time k. I 2J i +1 is the identity matrix size of 2J i +1, and γ i,k is a diagonal matrix consisting of a binary value at the current step k. It is set to 0 when a measurement is missing; otherwise, it is 1. γ i,k can also be used to account for transient errors in the sensor.

III. DISTRIBUTED FINITE MEMORY ESTIMATION FROM RELATIVE MEASUREMENT
This section describes the DFMERM for estimating the state of a problem formulated in Section II. We consider the robot index i as the filtering number of all robots. Utilizing the nonlinear functions (3) and (7), the discrete-time system models can be represented by the process and measurement equations: where We assume that w i,k andπ i,k are zero-mean white Gaussian noise vectors. The noise covariances of the i-th robot with respect to w i,k andπ i,k are defined as Utilizing the Jacobian matrices, (10) (11) and (12), the linearization of nonlinear system (8) and (9) are defined as follows: We simplify linearized system (13) and (14) as follows: where The proposed algorithm in discrete-time space requires recent N measurements, where N denotes horizon size. To stack N measured values, the linearized systems (15) and (16) where From (18), the stacked matrix of filter i with the number of N is established as follows: where To obtain the resultŝ i,k+N in the DFMERM filtering algorithm, we utilize the relationship between s i,k and s i,k+N . Using i,k+N , W i,k+N , and (15), and s i,k+N is obtained as follows: N ) . (22) We assume the systems have an unbiased condition to obtain gains. The state estimator algorithm with the filtering index i is defined aŝ To derive unbiasedness in the condition defined as E[s i,k+N ] = E[ŝ i,k+N ], the expectation of the above equation is given as follows: where i,k+N and W i,k+N constitutingπ i,k and w i,k are zero when the expectation is calculated. The conditions for the gain matrices can be obtained from (25) as follows: To find the gains Q i,k+N and O i,k+N in (26) and (27), we set the appropriate optimization problem. Set the following cost function using the Frobenius norm defined as · F : We use the Lagrange multiplier method to minimize the cost function. The Lagrange multiplier defined as is used to solve the cost function. Thus, we can define the following function: In this case, the optimization problem can be solved to obtain the gain Q i,k+N when the partial derivative of L i,k+N with respect to Q i,k+N equals zero. Then, Q i,k+N is obtained as follows: By substituting (30) into (26): The estimated stateŝ i,k+N from applying the gain matrix Q i,k+N and O i,k+N is obtained as follows: Remark 1: If there is a high noise effect that is not covered in noise covariances in the system, the iterative estimation algorithm does not work properly. However, using the filter of the FM structure, the noise, which has a significant effect on the system, is canceled after N sampling times. Moreover, the FM structure algorithm can achieve better convergence to the reference in such a situation.
Remark 2: In the DFMERM, the horizon size should be carefully selected. The computational cost increases with horizon size. However, the noise-ignoring performance increases with horizon size. It is important to set an appropriate horizon size to trade off between computational cost and noise ignoring performance.
Remark 3: The DFMERM does not consider noise statistics. Thus, the DFMERM guarantees robustness against incorrect noise information. Moreover, obtaining the gain matrix by minimizing the Frobenius norm of the filter can reduce the error caused by mismodeling and an undefined noise effect.

IV. EXPERIMENT
In this section, the experiment for the distributed localization of multiple mobile robots is performed in the real world. The information, such as friction and slipping on the ground, are undefined in the experiment. We implemented the DFMERM and distributed IIR filter with ROBOTIS's TurtleBot3 Burger mobile robot platform. We used six robots in the experiment, and the network topology is shown in Fig. 3. A LiDAR scanner was used for relative measurement. We investigated two practical scenarios. Notably, the distributed FM estimation algorithm proposed in [32]- [34] cannot be applied to the experiment because they estimate only one target state.

A. MISSING MEASUREMENT CASE
The temporal sensor process delay and communication error can occur in real-world situations, and this fault should be addressed to perform the task successfully. During the experiment, 100 measurements were collected with  2 (rad/s). We considered a missing measurement scenario with the following time step: robot 3 at times 29 ≤ k ≤ 33 and 80 ≤ k ≤ 86, robot 4 at times 29 ≤ k ≤ 33, robot 5 at times 29 ≤ k ≤ 33 and 75 ≤ k ≤ 79, and robot 6 at times 29 ≤ k ≤ 33 and 75 ≤ k ≤ 79. The initial states of 1-st, 2-nd, · · · , 6-th robots are given as 0. The initial state covariance of the i-th robot was considered as P i,0 = diag(0.1 0.1 0.1), where i = 1, 2, · · · 6. The real trajectories and trajectories for the localization results with the DFMERM and the distributed IIR filter of 3-rd, 4-th, 5-th, and 6-th robots are shown in Figs. 4-7. Fig. 4 reveals that the localization performance of the distributed IIR filter is degraded in sections 29 ≤ k ≤ 33 and 80 ≤ k ≤ 86 where missing measurements occurred in the 3-rd robot. In addition, the 3-rd robot receives measurement values from the 6-th robot, where missing measurement occurred in 29 ≤ k ≤ 33 and 75 ≤ k ≤ 79, and the estimation error that occurred at those sections affected 3-rd robot. As a result, the section with the performance degradation appears longer than the section where the actual missing VOLUME 10, 2022  measurement occurred. On the other hand, the proposed algorithm exhibits robust performance even in situations with missing measurements, and it can be confirmed that the output is close to the real value. Both the distributed IIR filter and proposed algorithm show similar performance, as shown in Fig. 5, except for one section where a measurement is missing for 4-th robot. The distributed IIR filter shows a large error in the section where a measurement is missing, and the performance deteriorates, whereas the output of the proposed algorithm approaches near the real value without large errors. The trajectories of 5-th and 6-th robots, where measurements were missing in the same 29 ≤ k ≤ 33 and 75 ≤ k ≤ 79 sections, are shown in Figs. 6-7, and the two robots are connected to each other in the network topology. Therefore, in a situation where measurements are missing, the estimated state of each robot affects each the performance of other robots. In particular, it can be seen that the estimated states of the 5-th and 6-th robots through the distributed IIR filter bounce in a similar direction with large errors, resulting in poor performance. The DFMERM shows robust performance, although the error increased compared to where a measurement was missing in 3-rd and 4-th robots. Fig. 8 shows the estimation errors of both algorithms; A denotes the interval for 29 ≤ k ≤ 33, B denotes the interval for 75 ≤ k ≤ 79, and C denotes the interval for 80 ≤ k ≤ 86. In interval A in Fig. 8, where all sensors are missing, we can see a large difference in the performance   of both algorithms. In interval B, the sensors of the 5-th and 6-th robots were turned off, and in interval C the sensor of the 3-rd robot was turned off. Similarly, the robust performance of the DFMERM in intervals B and C in Fig. 8 is notable. The performance of error handling is clearly shown in Table 1 as root-mean-square error (RMSE) results. The DFMERM exhibited RMSE results that were more than two times lower. The distributed IIR filter periodically showed an error peak and divergence. However, in the DFMERM, the sum of the absolute estimation error did not grow rapidly and had small divergence. Therefore the DFMERM was more effective at handling missing measurement errors. The comparison of both filters with the average operation time in Table 2 was conducted in Raspberry Pi 3 B+ model with a 1.4GHz CPU. The average operation was approximately 1.4 times higher in DFMERM. However, the DFMERM can be used in realtime applications because both filters exhibited sufficiently low computational load.

B. APPEARANCE OF OBSTACLE CASE
In the second scenario, we considered the obstacle case where other robots could not be sensed. At k = 120, the 3-rd robot was kidnapped and placed on the opposite side of 5986 VOLUME 10, 2022  the obstacle. A total of 260 measurements were collected with t = 0.5 (s). We considered the topology network as 1 → 5, 1 → 3, 2 → 4, 2 → 6, 4 → 3, 5 → 6, and 6 → 5. The initial states of the 1-th, 2-th, · · · , 6-th robots are given as 0. 6  The horizon size was N = 7. Noise covariances and initial state covariances were set to be equal those in scenario 1. The obstacle and real and estimated trajectory graphs are shown in Figs. 9 and 10, where both the distributed IIR filter and DFMERM exhibit poor performances when the 3-rd robot was kidnapped. In particular, in the distributed IIR filter, the error size changes drastically, whereas the proposed algorithm yielded a smaller error size. Moreover, as can be seen in Fig. 9, before and after the 3-rd robot was kidnapped and immediately after being kidnapping was over, the distributed IIR filter required some time-steps for stabilization, and continuous fluctuations were generated compared to the real value. However, Fig. 10 reveals that the proposed algorithm quickly and accurately approaches the real value immediately after the kidnapping was over.
The sum of the absolute estimation error is shown in Fig. 11; D denotes the interval where an obstacle appears.   The error gradually increased during the kidnap in both filters. However, in the distributed IIR filter, starting with the attack, the error rapidly increased. The DFMERM was more than twice as effective as the distributed IIR filter when the error was at the peak for both filters. The RMSE results show that the DFMERM had approximately 2.8 and 4.7 times better performance, in terms of the x-and yaxes, than the distributed IIR filter, as shown in Table 3. As in the missing measurement case, the average operating time in Table 4 sufficiently indicates potential real-world applicability. Furthermore, according to the experimental results, the DFMERM is sufficiently applicable to WSNs in terms of robustness.
In addition to the experiments, a simulation was conducted to clarify the computational complexity. The simulation consisted of 25 nodes more robots than in the previous two experiments. The overall network topology for the simulation is shown in Fig. 12.
We assumed that the robots in nodes 1, 2, and 24 were in Group A, and their positions are estimated in a 2D plane in advance. The sampling interval was set to 0.1s, and the simulation was run up to 500 time steps. The average computational complexity during the simulation is shown in Table 5.   The mean computational complexity of the distributed IIR filter was about 1.39 times faster than that of the proposed method, similar to in the previous experiments. However, the DFMERM had no difficulty in executing the algorithm in a loop within a given sampling time. Therefore, both experiments and the simulation confirmed that although the proposed algorithm is 1.4 times slower on average in terms of mean computational complexity than the distributed IIR filter, it can be implemented in a real-time experiment or simulation within the sampling time.
Remark 4: In the experiment involving an obstacle, only one obstacle was considered. When the number of obstacles increased, the obstacles interrupted communications between the robots and time delay occurred. This delay degraded the localization accuracy of both the DFMERM and the distributed IIR filter. Thus, it is required to study a new localization algorithm considering the time delay for the multiple-obstacle case.

V. CONCLUSION
In this paper, we proposed a novel distributed multiplerobot localization algorithm, namely, DFMERM. Due to the FM structure, the DFMERM has inherent robustness against modeling and computational errors. Moreover, the proposed algorithm overcomes the missing measurement problem by utilizing redefined measurements. The experimental results demonstrated that the DFMERM exhibited superior robustness against missing measurements and the sudden appearance of an obstacle compared with the conventional distributed IIR filter. We intend to apply the DFMERM to the 3D localization of unmanned aerial vehicles in future work.