Distributed Frobenius-Norm Finite Memory Interacting Multiple Model Estimation for Mobile Robot Localization

This paper proposes a novel state estimation algorithm, called the distributed Frobenius-norm finite memory interacting multiple model (DFFM-IMM) estimation algorithm, for mobile robot localization in wireless sensor networks (WSNs). The proposed algorithm involves finite memory estimation based on recent finite measurements; such estimation facilitates robust localization in cases of missing measurements and robot kidnapping. Furthermore, the proposed algorithm employs IMM, which facilitates accurate localization if a mobile robot abruptly changes its speed and course. Notably, average-consensus-based distributed processing renders the proposed DFFM-IMM algorithm computationally efficient, and hence, real-time processing for very short sampling times of the WSN is possible. The proposed algorithm’s performance is demonstrated by comparing it with a centralized Frobenius-norm finite memory IMM (CFFM-IMM) estimation algorithm and a localization algorithm on the basis of simulations and experiments.


I. INTRODUCTION
In recent years, mobile robots have been widely used for a variety of applications, such as house cleaning, food delivery, and museum guidance [1], [2], [3]. Mobile robots are equipped with autonomous navigation systems that provide information on their current position in their workspace. If the position information is not accurate, the autonomous navigation system cannot operate properly. Hence, their accurate localization is essential, and algorithms for their accurate localization have been extensively studied over the past few decades [4], [5], [6].
Localization algorithms can be classified into two categories: outdoor and indoor localization algorithms. Outdoor localization is usually based on the Global Positioning System (GPS) [7], [8], which does not operate in GPS shadow areas such as tunnels and indoor spaces. To overcome this The associate editor coordinating the review of this manuscript and approving it for publication was Mauro Gaggero . limitation, researchers have studied algorithms based on a combination of the GPS and an inertial navigation system (INS) [9], [10], [11], [12], [13], [14], [15]. For indoor localization, wireless sensor networks (WSNs) based on radio frequency (RF) and ultra-wide band (UWB) signals have been used. In indoor environments, there are generally many obstacles that interrupt the transmission of wireless signals, and localization algorithms that can help overcome the interruption have been studied [16], [17], [18].
Localization algorithms are actually state estimation algorithms (also referred to as stochastic filters), and they estimate the position of targets (e.g., a mobile robot) by using noisy measurements. The Kalman filter (KF) is one of the most well-known state estimation algorithms, and various modifications of this filter have been used for a variety of localization systems. However, KFs may exhibit poor estimation performance owing to modeling or computational errors. This is because KFs have an infinite impulse response (IIR) structure that uses all past measurements to estimate the VOLUME 10, 2022 This work is licensed under a Creative Commons Attribution 4.0 License. For more information, see https://creativecommons.org/licenses/by/4.0/ current state. In the IIR filter structure, errors accumulate over time, resulting in poor estimation or even filter divergence.
To overcome this problem, researchers have studied finite memory estimation (FME) algorithms in which only recent finite measurements are used [19], [20], [21], [22], [23], [24], [25], [26], [27], [28], [29], [30], [31]. These algorithms are also referred to as finite impulse response (FIR) filters since they have FIR structures. Various versions of FME algorithms have been developed and applied to applications of localization systems [28], [29], [30], [31]. Localization (i.e., state estimation) algorithms require motion and measurement models. If changes in the target's motion are not significant, localization algorithms based on a single motion model can achieve sufficiently accurate localization. However, if the course and speed of the target change significantly or abruptly, multiple motion models are required. Interacting multiple model (IMM) estimation is a representative estimation algorithm that employs multiple models, and it has been successfully used for solving the problem of tracking maneuvering targets [32], [33], [34], [35]. However, existing IMM-based localization algorithms are centralized estimation algorithms, which implies that all measurement information is centralized in a computation center (algorithm). By contrast, distributed estimation algorithms, have multiple computation centers, each of which processes part of the measurements. Consequently, distributed estimation algorithms are advantageous in terms of computational efficiency [36], [37], [38].
In this paper, we propose a novel state estimation algorithm for mobile robot localization using WSN. The proposed algorithm is based on FME, which renders the localization robust against temporary modeling/computational errors. Two versions of FME algorithms, namely batch and iterative types, are derived using the Frobenius-norm. To deal with significant motion changes of a mobile robot, we use a combination of FME and an IMM in the proposed algorithm. In particular, the proposed algorithm uses a distributed processing method, called average consensus, for achieving high computational efficiency, and hence, the proposed algorithm is referred to as distributed Frobenius-norm finite memory IMM (DFFM-IMM) estimation. To evaluate the localization performance of DFFM-IMM estimation, simulations and experiments were performed for mobile robot localization in two scenarios: missing measurement case and kidnapped robot case. The proposed algorithm was compared with three algorithms including the centralized Frobenius-norm finite memory IMM (CFFM-IMM), distributed KF-based IMM (DKF-IMM), and multiple model particle filter (MMPF) [39], and it was more robust than the three algorithms. Moreover, compared with CFFM-IMM estimation, DFFM-IMM estimation required a shorter computation time, which was sufficiently short to facilitate realtime processing.
The remainder of this paper is organized as follows. Section II describes motion and measurement models used for mobile robot localization using a WSN, and Section III presents DFFM-IMM estimation. Sections IV and V present the simulation and experimental results, respectively, and Section VI summarizes the conclusions.

II. MULTIPLE MODEL-BASED INDOOR LOCALIZATION
This section describes a multiple model-based mobile robot localization scheme involving a WSN. The multiple motion model for mobile robot localization can be represented as where x n is the state vector at discrete time n, and it is defined as where (x n , y n ) and (ẋ n ,ẏ n ) are the two-dimensional (2D) positions and velocities, respectively, w n is the process noise vector, which is assumed to be zero-mean white Gaussian noise with covariance Q n , and A j n is the system matrix for j-th mode (model). The transition probability from mode i to mode j is defined as where γ n is the mode variable at time n; D is defined as D = {1, 2, . . . , D}, and D is the total number of models (modes). Generally, a mobile robot can have three motions: straight motion and clockwise and anticlockwise rotation. For straight motion, the constant velocity (CV) motion model is suitable, and for rotation motion, the coordinated turn (CT) model is suitable. Multiple-model algorithms combining CV and CT models have widely been used for maneuvering target tracking [40], [41], [42]. The system matrices for the three types of motion are as follows: where j = 1 corresponds to the CV model, j = 2, 3 corresponds to CT models with clockwise and anticlockwise rotation, respectively, ω j n is the mode-dependent turning rate, and T is the sampling interval.
In WSN-based indoor localization, anchor nodes are installed at fixed locations, and a mobile tag is attached to the mobile robot. Through wireless communications, distances between the mobile tag and anchor nodes are measured. The measurement vector is defined as: where (x n , y n ) is the position of the mobile robot (mobile tag) at time n, and (x a,l , y a,l ) is the fixed position of the l-th anchor node. The measurement model that relates the measurement vector y n to the state vector x n is given by where h n (·) is the vector representation of the nonlinear function in (7) and v n is the measurement noise vector, which is zero-mean white Gaussian noise, with covariance R n . The measurement model is nonlinear, and the proposed FME algorithm requires linear models. Hence, we linearize the nonlinear measurement model. Linearization of the measurement model around the estimated statex n by using Taylor series expansion gives where H n and z n are defined as By defining the auxiliary measurement signal to beỹ n = y n − z n , we obtain the new measurement model which reduces linearization errors [27], [28].

III. DISTRIBUTED FROBENIUS-NORM FINITE MEMORY INTERACTING MULTIPLE MODEL ESTIMATION
where

A. CENTRALIZED FROBENIUS-NORM FINITE MEMORY IMM ESTIMATION
First, we derive the CFFM-IMM. Figure 1 shows the structure of CFFM-IMM estimation; α is the horizon size of the batch form of the CFFM-IMM. The algorithm is performed through five processes: 1) initialization, 2) mixing, 3) prediction, 4) updating, and 5) output [35].

1) INITIALIZATION
As shown in Fig. 1, CFFM-IMM estimation should initialize the mode probabilities µ j t . We assume that the initial mode probabilities are given equally by the total number of models D in the initialization process as follows: where t = m + α − 1.
Since we use the equation of multiple models (5) and (6), the multiple batch form of the CFFM-IMM, which can initialize multiple states, is considered. Therefore, the state estimationx n can be redefined as the state estimation of the j th system modex j n . Moreover, equations (10) and (12) can be rewritten as H j n andỹ j n . Therefore, equations (13) ∼ (24) can be re-defined as follows: Before mixing the initial multiple state estimations, we should choose the horizon size of the batch form of the CFFM-IMM α to be between [n, m] to calculate the batch form of the CFFM-IMM and the iterative form of the CFFM-IMM N − α. The state of the batch form of the CFFM-IMM is given bŷ where K j t,m is the estimation gain of the j th system mode. By assuming the unbiased condition E[ and substituting the first rows of (13) and (26) into (14), we obtain: Here is the zero vector under the unbiased condtition. Therefore, in order to satisfy the unbiased condition, we modify the condition as Next, we define an objective function K j t,m based on the weighted Frobenius-norm · F to obtain the estimation gain [24], [25], [26]: where is the diagonal weight matrix of the j th system mode and j is the weight parameter of the j th system mode (0 < j ≤ 1) that provides more weight to the latest data and less weight to past data to improve the robustness, and I L×L is the L × L identity matrix.
To obtain a solution for K j t,m that satisfies condition (28) and to minimize the objective function (29), the Lagrange function L K j t,m can be defined as where φ is the Lagrange multiplier. The partial derivative of (30) with respect to K j t,m is The estimation gain K j t,m can be found using (28) and (31):

2) MIXING
Multiple state estimationsx j k−1 are combined to obtain the j th model estimationx 0j k−1 as follows [34]: where t + 1 ≤ k ≤ n and µ ij k−1 is the mixing probability that the i th system mode was valid at time step k − 1 given that j th system model is valid at time step k conditioned on measurements Y j n,m can be defined using the model transition probability π ij as follows: where c is the normalization constant that is used to normalize all the system modes of π ij µ i k−1 in order to obtain the mode probability, which is as follows:

3) PREDICTION
By mixed states (33), the j th prior model estimation can be obtained using the j th system model equation (5) and (6).

4) UPDATING
Since the batch form of the CFFM-IMM estimation structure is a matrix of stacked data at each time step by horizon size α, the batch form cannot easily interact with the multiple models at each time step in the updating phase. Hence, the iterative form of CFFM-IMM estimation should be used. To derive the iterative form, we rewrite (26) aŝ where C j k,t+1 can be derived from properties of inverse matrices by using F j t+1 j k and H j k,t+1 as follows [37]: which yields the following summation notation b s=0 : can be derived as follows: where k can be derived by introducing a weight parameter ( j ) −2 before the sigma b s=0 . The parameter k−1 is easily derived by changing the horizon size N − α to N − α − 1 as follows: Then, (42) can be rewritten to be similar to (41) as follows: where˜ k−1 can be derived in a manner similar to the equation for k−1 ;Ỹ j k−1 is the previous step of (43) and can be expressed as follows: Using (26) and (43), we can rewrite (37) aŝ From (41), (K j k−1 ) −1 can be defined as Finally, the iterative form of the CFFM-IMM equation can be obtained by substituting (K j k−1 ) −1 in (44) to (45) as follows: The iterative form of CFFM-IMM estimation begins with k = t + 1 and ends with k = n. Then, we should update the previous mode probabilities µ i k−1 to obtain the update mode probabilities µ j k for combining the iterative form of the CFFM-IMM estimation.
Using Bayes' rule [31], we obtain the update mode probabilities as where L j k is the likelihood function ofỹ k , given by andc is the normalization constant whose expression can be derived in a manner similar to (35) as follows: VOLUME 10, 2022

5) OUTPUT
Finally, the combination of multiple state estimations can be obtained by combining (46) with (47): Note that if the time step k is such that m < k < n, then the algorithm returns to the second phase (mixing) and repeats it up to the time step k = n. Therefore, while the CFFM-IMM sendsx j k and µ j k to the second phase, it does not send the combined state estimationx k .

B. DISTRIBUTED FROBENIUS-NORM FINITE MEMORY IMM ESTIMATION
In the CFFM-IMM, the computation time increases depending on the number of anchors. If the number of anchors increases, the size of matrices associated with the measurement data increases, and this places a burden on the computation time. These matrices are not only used in the CFFM-IMM equations but also in the likelihood function to calculate the system mode probabilities to combine multiple states with the mode probabilities as shown in (50).
In this section, we propose DFFM-IMM estimation. The DFFM-IMM is derived on the basis of the CFFM-IMM estimation structure, as shown in Fig. 1. We use a distributed algorithm, called the average consensus approach [37], in the equation related to the measurement data to reduce the computation time.

1) INITIALIZATION
In the initialization process, the initial mode probabilities are the same as those in (25). The estimation gain K j t,m in (37) can be derived as follows: whereḠ j m+s is a fused matrix related to the linearization measurement matrix of each anchor based on the average consensus. The parameterḠ j m+s is given bȳ The superscript (l) indicates that the matrix element is related to the l th anchor. To explain more in detail, if we use the first anchor, then the first rows of (11) and (12) are used.
We defineκ j t as We divide (53) by the number of anchors L, and use (51) to derive the following expression for the DFFM-IMM estimation gain K j µ(t,m) based on the average consensus: whereK j µ(t,m) = ( 1 Lκ j t,m ) −1 . Using (54), we can transform the initial statex j t as follows: wherez j m+s is the fused measurement data of each anchor based on the average consensus. The parameterz j m+s can be expressed as follows:

3) UPDATING
To derive the iterative form of DFFM-IMM estimation, we rewrite the iterative form of the CFFM-IMM estimation equation (46) as follows: Then, the iterative form of the DFFM-IMM estimation equation can be derived from (57) aŝ where G j k , andz Next, in this paper, we apply the average consensus approach to the likelihood function to decrease its computation time. Equation (48) can be the multivariate Gaussian normal as follows: where j k is the error between the measurement dataỹ Using the average consensus approach in (62), we can transform L j k as where J j k and k are the fused Mahalanobis distance and the fused measurement noise covariance matrix of each anchor based on the average consensus at the k th time step. The parameter ξ j k is H j kx j k|k−1 at the k th time step. These parameters can be expressed as follows: Note that R (l) k denotes the (l, l) diagonal matrix element, which is a scalar value. Consequently, the centralized likelihood function in (48) should perform a matrix operation that places a burden on the computation time. Notably, the distributed likelihood function in (64) performs a scalar operation that can reduce the computation time.

4) OUTPUT
Finally, the mode probabilities in (47) can be obtained by substituting (48) into (64) and then combining the multiple state estimationx j k with the mode probabilities to obtain the output state estimation of the DFFM-IMM estimation, which is identical to (50). The DFFM-IMM performs again if the time step k is such that m < k < n up to the time step k = n, similar to the CFFM-IMM. The process of DFFM-IMM to obtain the final output is described in Algorithm 1.
Remark 1: In the FM-IMM estimation algorithms, including the CFFM-IMM and the DFFM-IMM, the horizon size of the batch form of the FM-IMM (α) should be equal to the number of states to make the generalized noise power gain (GNPG) unity [21] and to reach the next step as fast as possible for interacting with the multiple models. This is because the batch form cannot make the j th model interact with the i th model.

Remark 2:
The horizon size of the iterative form of the FM-IMM should be chosen with consideration. It should be greater than or equal to the number of states for interacting sufficiently with the multiple estimations and for the GNPG to be unity. In other words, the total horizon size N is the sum of the horizon size of the batch form (α) and the iterative form of the FM-IMM (N − α).

IV. SIMULATION
The section presents the results of a mobile robot localization simulation involving a WSN. We simulated the proposed algorithm using the Gazebo robot simulator. Figure 2 shows the simulation environment. Four (virtual) anchor nodes were located at the fixed 2D positions, (0, 0), (10, 0), (10,10), and (0, 10), where all values are in meters. A mobile robot, Turtlebot3 Burger, equipped with a (virtual) mobile tag traveled in a 2D space with dimensions 10m × 10m. Figure 3 shows information flows in the robot localization system. The virtual tag received a signal from the anchor nodes and provided distance measurements. The remote PC received the measurement data y n from the Gazebo simulator via the robot operation system (ROS). In the computer, localization algorithms were run in MATLAB by using the measurement data. The sampling interval T was 35ms, the weight gain was set to 0.9, and the turn rate for the clockwise and anticlockwise CT models were set as ω j=2 n = 0.05 and ω j=3 n = −0.05 (rad/s). The transition probabilities between the models can be set by considering the mean sojourn time of each mode [43], [44]. We measured the sojourn time of the mobile robot for three modes (i.e., straight movement and left/right turns) in the experiments. Based on the mean sojourn time, the transition probabilities were set as follows: The process and measurement noise covariances were considered to be Furthermore, the horizon size was N = 10, and the horizon size of the batch-form FME was set as α = 4. In wireless communications, transmission delays or packet dropout sometimes occurs, and it leads to missing measurements. To deal with a missing measurements situation, we can rewrite the measurement equation (12) as where β n is a binary variable that indicates whether data are missing (β n = 0 for missing; otherwise β n = 1). Using this ) −Tz j m+s 10: end for 11: for k = m + α, m + α + 1, · · · , n do 12: for j = 1, 2, · · · , D do 13: for i = 1, 2, · · · , D do 14: k ) 23:  Figure 4 shows real and estimated trajectories of the mobile robot. Four localization algorithms (DKF-IMM, MMPF, CFFM-IMM, and DFFM-IMM) were used to estimate the positions of the mobile robot. Figure 5 shows the positioning errors of the four localization algorithms, which were computed by using the formula where (x n , y n ) and (x n ,ŷ n ) are the true and estimated positions, respectively. Missing measurement situations occurred for 700 ≤ n ≤ 1000 and 3000 ≤ n ≤ 3300, where  one of the anchor nodes did not transmit signals. In these situations, the positioning error of both DKF-IMM and MMPF rapidly increased, as shown in Fig. 5. However, two FME-based algorithms (i.e., CFFM-IMM and DFFM-IMM) exhibited significantly smaller errors than the IIR-type algorithms (i.e., DKF-IMM and MMPF). The IIR-type algorithms accumulate the errors caused by missing measurements but the FME-based algorithms do not. In this simulation, the MMPF used 2,000 particles, which was selected based on the simulation results shown in Fig. 6. It is well-known that estimation accuracy of the PF improves as the number of particles increases. Figure 6 shows that positioning errors of the MMPF decreased as the number of particles increased. When the number of particles was larger than 2,000, real-time processing was not feasible. Thus, we used 2,000 particles for the MMPF. As the sampling time, T was 35 ms, which included the measurement update rate of the simulation (approximately 30 Hz) and the computation time of the algorithm, a localization algorithm had to operate properly within a maximum time of 2 ms. Our proposed algorithm, the DFFM-IMM, was sufficiently fast to estimate the position within the given time.  On the other hand, the computation time of the CFFM-IMM was 2 ms or more which resulted in the algorithm skipping the estimation as shown in Fig. 5. Thus, if the computation time of an algorithm exceeds the time which excludes the measurement update rate from the sampling time, it can skip the estimation process, which would increase the position error.
We computed the averaged positioning error (APE) for the four algorithms, and they are presented in Table 1. The APE is defined as where t f is the final time step of the simulation. The APE of the DFFM-IMM was significantly smaller than those of the other three algorithms. Table 2 shows the computation time required for a single estimation. FME-based algorithms generally require more computation time compared with IIR-type algorithms. Thus, both the CFFM-IMM and DFFM-IMM required more computation time than the DKF-IMM, as shown in Table 2. However, compared with the CFFM-IMM, the DFFM-IMM showed a shorter computation time since it employed a distributed algorithm (i.e., average consensus).

V. EXPERIMENT
This section presents experimental results to demonstrate the high performance of the proposed DFFM-IMM algorithm. The video of the experiments is available in [45]. Figure 7 shows the experimental set up and indoor localization environment. The mobile robot used in the experiments was Turtlebot3 Burger, made by ROBOTIS. We used a wirelesssensor-based positioning system made by Pozyx, and it consisted of anchor nodes and a mobile tag. The system is based on ultra-wideband (UWB) technology; its minimum and maximum pulse repetition frequencies (PRFs) are 9 and 138 Hz, repectively. In the experiments, we set the PRF to 125 Hz; it took 8 ms for the USB sensor to transmit the distance information to MATLAB via ROS communication. Four anchor nodes (i.e., UWB sensors) were used; it took 32 ms to transmit the four distance measurements. The four anchor nodes were installed at the fixed positions (0, 0), (6, 0), (6,6), and (0, 6), where all values are in meters. Figure 8 depicts the experimental set up, and it also shows information flows. Each anchor node transmitted distance data to the mobile tag, and the update rate was 125 Hz. The tag was connected to a single board computer, a Raspberry Pi 3B that could obtain the distance data from the tag via serial communication. Wireless connection between Turtlebot3 and a remote PC was established through a Wi-Fi router. A ROS was used to set up the wireless network. The remote PC received the measurement data from the ROS, and the proposed algorithms were run in real-time in MATLAB. The initial position of the robot was (2.4, 0.6). The design parameters for the localization algorithms were set to be the same as those used for the simulation.

A. MISSING MEASUREMENT CASE
First, we conducted experiments for the missing measurement scenario. Missing measurements occurred during the time intervals, 1200 ≤ n ≤ 1700 and 2800 ≤ n ≤ 3100. Figures 9 and 10 show the experimental results for this scenario. Similar to the simulation results, the IIR-type algorithms (i.e., DKF-IMM and MMPF) exhibited a sharp increase in errors. However, the FME-based algorithms (i.e., the CFFM-IMM and DFFM-IMM) exhibited much smaller errors. Fig. 11 shows the localization results of the MMPF using various values of the number of particles in the same experimental situation. Localization accuracy of the MMPF was the best when 2,000 particles were used. However, the use of particles more than 2,000 resulted in too much computation time, and real-time processing was not feasible.
Four UWB sensors measured four distance data at rate of approximately 30 Hz. Therefore, the algorithm should be performed within a maximum of 2 ms, similar to the simulation. Since the CFFM-IMM showed the estimation skipping problem because of the high computation time, it showed poor performance. On the other hand, our proposed DFFM-IMM performed more accurately owing to its superior computational efficiency.

B. KIDNAPPED ROBOT CASE
Small mobile robots may suddenly be moved to a faraway location by humans; this is called robot kidnapping. In this case, the real position is significantly different from the estimated position, which may cause the failure of estimation algorithms. To test the robustness of the proposed algorithm, we conducted localization experiments in a kidnapped robot situation. At time n = 2350, we moved the robot from the position (5.39, 0.77) to the position (3.25, 2.5), as shown in Fig. 12. After the kidnapping, the IIR-type algorithms (i.e., DKF-IMM and MMPF) showed a sharp increase in positioning errors, but the FME-based algorithms (i.e., CFFM-IMM and DFFM-IMM) did not, as shown in Fig. 13. Figure 14 shows the positioning errors of the MMPF for different numbers of particles. Such as in the missing measurement case, more particles resulted in better accuracy. However, an excessive number of particles renders real-time  processing difficult. The experimental results shown in Figs. 12 and 13 demonstrate the robustness of the FME-based localization algorithms against robot kidnapping. Because the CFFM-IMM has the computational time problem, it sometimes showed sharp increases in the error when the robot was not kidnapped. The DFFM-IMM exhibited robust and reliable localization performance without any drastic increase in errors. Table 3 compares the APEs of the four localization algorithms in the experiments. Case A represents the missing measurement case, and Case B refers to the kidnapped robot case. In both cases, the proposed DFFM-IMM produced much smaller APEs than the other algorithms. Table 4 compares the computation times for a single estimation in the experiments. It is known that KF-based algorithms are faster than FME-based algorithms. Accordingly, the computation time of the DKF-IMM was shorter than both the CFFM-IMM and DFFM-IMM. The DFFM-IMM showed a much shorter computation time than the CFFM-IMM since it used the average consensus algorithm. Although the

VI. CONCLUSION
In this paper, we proposed the DFFM-IMM estimation algorithm for WSN-based mobile robot localization. The algorithm was developed using the IMM, FME, Frobeniusnorm, and average consensus algorithms. Compared with the existing algorithms, the proposed DFFM-IMM showed superior localization accuracy (i.e., a smaller APE) under harsh conditions. In the missing measurement case, the APE of the DFFM-IMM was only 61% and 71% of those of the DKF-IMM and MMPF, respectively. In the kidnapped-robot case, the APE of the DFFM-IMM was only 63% and 55% of those of the DKF-IMM and MMPF, respectively. Furthermore, the proposed algorithm showed higher computational efficiency than the CFFM-IMM since it employed an average consensus algorithm. We intend to apply the DFFM-IMM to drone-based three-dimensional localization and GPS-based outdoor localization systems in future works.