A Decorrelated Distributed EKF-SLAM System for the Autonomous Navigation of Mobile Robots

This paper proposes a novel distributed EKF-SLAM system that combines the advantages of EKF-SLAM and distributed SLAM systems. The system model of this novel SLAM system has a distributed structure, and each subsystem is a special SLAM system corresponding to every effectively observed landmark by feeding the heading information from a magnetic compass is introduced into the observation equation. Aim at the correlation problem in distributed SLAM system, a decorrelated distributed EKF (DDEKF) was developed to estimate the robot pose and landmarks. DDEKF reconstructs and extends the maximum allocation covariance (MAC) method so that it can be applied to the distributed structure where the number of local filters is dynamically changed. Then, the local filter estimation results are weighted and fused in the main filter to obtain the estimation result. Finally, the experimental tests were performed in an outdoor environment, and the experiment results demonstrate that the proposed novel distributed EKF-SLAM system has a better performance than the existing SLAM system.


Introduction
Simultaneous localization and mapping, a well-known computational problem abbreviated as SLAM, was proposed as a means of enabling a mobile robot to move through an unknown environment while building a map and simultaneously estimating its position [1]. Unlike traditional navigation systems that rely on prior knowledge of the environment or external reference systems (e.g. GPS), SLAM requires only the utilization of onboard sensors without any other assistance. Therefore, SLAM has attracted extensive attention in mobile robotics and has come to be seen as the primary tool for solving the autonomous navigation problem for various unmanned vehicles, such as unmanned aerial vehicles [2,3], underwater robots [4,5] and space robots [6].
In general, two main types of approaches have been widely used to solve the SLAM problem. The first is smoothing approaches, in which the complete trajectory of the robot is estimated from the full set of measurements. The existing solutions for graph-based SLAM include square root smoothing and mapping (SAM) [7], incremental smoothing and mapping (iSAM) [8], nonlinear constraint network optimization [9], and hierarchical optimization for pose graphs on manifolds [10]. Due to the relatively high complexity of solving the error minimization problem, graph-based SLAM is often considered to require an offline algorithm. The second is filtering approaches, in which the SLAM problem is modeled on the basis of an online state estimation system consisting of the robot's current position and the map. The SLAM approaches that fall into this category include methods based on the extended Kalman filter (EKF), information filters and particle filters. Successful SLAM algorithms that use the EKF have been developed for various applications [11][12][13]. However, linearized series approximations can lead to poor representations of nonlinear functions, and the EKF approach cannot be used to solve non-Gaussian problems. A SLAM system using an information filter maintains a sparse information matrix that preserves the consistency of the Gaussian distribution [14,15]; however, accessing the mean and covariance requires the inversion of a large matrix, which is computationally expensive. The most efficient system for particle-filter-based SLAM (PF-SLAM) is FastSLAM, in which the SLAM problem is decomposed into a robot localization problem and a landmark estimation problem, which are addressed with a particle filter and the EKF, respectively [16][17][18][19].
However, the aforementioned approaches all rely on a system structure based on a centralized filter. Any change in the number of feature points will result in state vector reconstruction and an additional computational burden because all of the state information is contained in one vector, and information of different quality will be unavoidably confused. To overcome these limitations of centralized filters, Dae Hee Won proposed a SLAM system based on a distributed particle filter (DPF) [20,21]. In this distributed SLAM system, the measurements of every feature point are processed in local particle filters (the number of local filters is determined by the number of feature points). Then, the estimated results from the individual local filters are transmitted to a master filter, which calculates the final estimate for the vehicle. The authors of [22,23] developed a distributed SLAM system with an improved DPF approach. In [23], the authors developed an improved FastSLAM system based on a distributed structure for autonomous robot navigation.
Among the aforementioned SLAM systems, the distributed SLAM system proposed in [21,23] is the most representative. There is no doubt that this algorithm is effective and easy to implement in real time for practical applications. However, there are still some drawbacks inherent in this algorithm. In this distributed SLAM system, since a particle filter is used as the estimator for the robot pose, the consistency and long-term accuracy of the distributed SLAM system will be affected by the particle diversity and accuracy. Even when an improved particle filter is used in the distributed SLAM system, inconsistency cannot be avoided in PF-SLAM when resampling is performed. In addition, the feature points on the map and the robot pose are related in every cycle of estimation because of the common prior estimates and common process noise shared by the local filters, and the correlations will become stronger as the number of observations increases.
Since EKF estimation directly provides recursive solutions to localization problems and suitable to the robot and landmark positions, the EKF approach remains the method of choice for the great majority of applications, and EKF-SLAM has been proven to offer the best convergence and consistency [21,22]. Motivated by this previous experience, this paper proposes a decorrelated distributed EKF-SLAM system. In this distributed SLAM system, to guarantee the performance of the system model, a magnetic compass sensor is introduced into the system, and the heading from this magnetic compass is fed into each subsystem as a measurement vector. Because this heading information is considered, the proposed SLAM system not only can revise the robot's orientation during estimation but also can ensure the consistency and accuracy of each subsystem. An observability analysis of the new distributed SLAM system model is presented to provide a theoretical basis for the design of the estimator. To overcome the suboptimality problem caused by the measurement vector correlations, the maximum allocated covariance (MAC) fusion method [24] is implemented and extended to an N-dimensional matrix to improve the computational efficiency of the system and allow it to adapt to a situation with an uncertain number of local filters. The proposed distributed SLAM system is thus more suitable for practical applications because of its system structure and implementation technique.
The contents of this paper are as follows. In Section 2, the basic configuration and model of a typical distributed particle SLAM system are reviewed. Section 3 introduces the novel distributed SLAM system model with heading assistance based on magnetic compass measurements. The decorrelated distributed EKF algorithm for the proposed SLAM system is designed and analyzed in Section 4. Section 5 presents the experimental results, and conclusions are drawn in Section 6.

Distributed System Architecture for Slam
The typical centralized filter structure is considered to yield globally optimal results because the master filter in this structure considers all state information and observed data [11]. The EKF and the unscented Kalman filter (UKF) are the most widely used filtering methods in SLAM, but they always have a centralized structure. Changes in the state and observation vectors reduce the system stability since each change in the number of feature points will cause the entire state vector to be reconstructed, and a large amount of computation required also reduces the real-time capabilities of such a centralized system because of the exponential increase in the computational burden with changes in the state vector. An effective way to solve the above problems is to adopt a distributed parallel synchronous structure that has a master filter to fuse local trajectories within a certain period of time; such a distributed structure can reduce the effect of error messages on the final estimation result, improve the fault tolerance, and allow the system to respond effectively to changes in the number of effective landmarks. The most successful distributed filtering method in SLAM is particle filtering [22]. The configuration of a typical distributed particle SLAM system is shown in Fig. 1.
The DPF structure is an attractive and effective means of realizing a SLAM system. Generally, the estimation model of a distributed particle SLAM system is composed of a motion model and an observation model. The estimation model addresses the problem of a robot with known kinematics, starting at an unknown position and moving through an unknown environment populated with landmarks of a certain type [23]: where X r (k) is the robot motion model, which can be described by a probabilistic Markov chain; X L (k) is the state of the landmarks observed by the robot; and Z k ð Þ and v(k) are considered to be zero-mean Gaussian white measurement noise.
where x r (k), y r (k), and φ r (k) are the robot states defined by the external odometer sensor at time k and γ is the process noise. It is assumed that γ consists of zero-mean white sequences that are uncorrelated with each other, and the covariance is Q. x Lj and y Lj represent the state of the jth landmark. Δx, Δy, and Δφ are the variations of the robot states. Z(k) is the observation equation relating the robot states to the landmarks, as shown in (3): where Z is the observation vector, η is zero-mean Gaussian white measurement noise, Z r is the distance from the beacon to the laser sensor, and Z β is the bearing of the laser sensor measured with respect to the robot coordinate frame. This distributed SLAM system model is based on every effectively observed landmark and establishes a set of local filters that are independent of each other and work in parallel. Each estimated result for the states is transmitted to the fusion filter to obtain the final results. However, this type of distributed structure gives rise to a correlation problem for the following reasons: (1) Collective process noise: Every subsystem can be regarded as a special SLAM system with an observation equation that includes information on only one landmark from the sensor. Thus, correlations between the measurement vectors in different subsystems are inevitably induced because all of the measurements originate from the same sensor. (2) Common prior estimates: Common prior estimates will lead to correlation problems when one track is merged with another track to generate a system track because each track contains common historical information. As shown in Fig. 2, a plurality of parallel filters B1 -B3 and the system track estimate S(k + 1) contain the common local track estimate S(k) propagated from an earlier time.
The conventional track fusion algorithm does not consider the local association problem in the system when all tracks are estimated from only one given target. To overcome this problem, Y. Bar-Shalom proposed a distributed track fusion formula called track-to-track fusion (TTF) in [25]. The TTF formula considers the correlations caused by common process noise. The cross-covariance and the fusion trajectory in a distributed system can be calculated from two state estimates, X a and X b , and their covariance matrixes, P a and P b : where X ij kjk ð Þ is the fusion result, ζ ij (k) is the filter gain of a local processor, and Xî kjk ð Þ and X̂j kjk ð Þ are the local filters' estimates from a single sensor.
The advantage of this method is that the correlations between the local estimation errors are taken into consideration. The disadvantages are that a large amount of information is needed to calculate the cross-covariance between the estimations and the communication requirements are strict.

Novel Distributed Slam System Model
It is clear that a distributed structure is an effective way to realize a SLAM system compared to a centralized structure; however, the existing distributed SLAM systems are all based on the DPF approach, which has the inherent drawbacks of particle impoverishment and high computational complexity. Synthesizing the advantages of EKF-SLAM and distributed SLAM systems, this paper proposes a novel distributed SLAM system that has a distributed structure and uses a distributed EKF (DEKF) as the estimator. Similar to the conventional distributed system model, the state vector of every subsystem is composed of the robot state and one landmark position. Notably, it has been proven that inconsistency in EKF-SLAM is directly related to heading uncertainty [1]. If the true heading variance remains small, the inconsistency grows slowly and is manageable. To overcome the inconsistency problem, heading information from a magnetic compass is fed into the observation model for each subsystem to ensure global observability and consistency.

Novel Distributed SLAM System Model
The novel distributed SLAM state model is constructed in the same way as the conventional model: where v c is the linear velocity and ω c is the angular velocity; these velocities are calculated from encoder information. The

Common prior estimates
Common sensor Landmarks B1 B2 B3 S(k+1) S(k) Fig. 2 Configuration of sensorto-system track integration observation model, which consists of the laser and heading information, is where z φ is the heading of the robot as measured by the magnetic compass. Then, based on every effective observed landmark, we can establish the distributed structure as follows: This novel distributed SLAM system model uses every effectively observed landmark to establish a set of local filters that work in parallel and independently of each other. The heading information is used as observation information to revise the robot's steering and to ensure the global observability of the model.

Observability Analysis of the SLAM System
Observability is an important issue that plays a major role in the filtering and reconstruction of states from inputs and outputs [26]. The observability condition is an indicator of whether the system contains all the necessary information to perform the estimation with a bounded error. In particular, for the case of SLAM, observability implies a bounded error in the localization of both the robot and the landmarks.
Formally, observability simply reflects whether the initial state x(0) can be uniquely deduced from the history of observations. This requires that the observability Gramian be nonsingular; if this condition is met, then all states are observable. For a time-invariant system, which can be approximated by a piecewise constant system with little loss of accuracy and no loss of the characteristic behavior of the system [27], we can ensure that the entire system can be observed by confirming the observability of every distributed local system. In addition, checking the observability condition can be reduced to checking whether the matrix (11) where F and H are the motion and observation Jacobian matrixes, respectively, and N is the size of the state. Consider the SLAM system defined by the observation model in (3) and the state model X = [x r , y r , φ r , (x Lj , y Lj )] T , which takes one landmark as input. When the nonlinear observation model is linearized using the Taylor series expansion, the observation model Jacobian H is Thus, the observability matrix from (11) is Here, Rank(Q) = 4 < 5. It can be shown that the observability matrix is not of full rank when observing the range and bearing of one landmark, meaning that the system is unobservable.
Therefore, we use an additional compass sensor to measure position information in the SLAM system: given the state model in (7) and the observation model in (8) and the observability matrix from (11) is Here, Rank(Q) = 5. It can be shown that the observability matrix becomes of full rank when observing the range and bearing of one landmark and the heading of the robot, demonstrating that the novel distributed SLAM system is observable.

Decorrelation Algorithm for Distributed Slam
Generally, correlations can be taken into account during track fusion, and one method of doing so based on a convex fusion algorithm is named TTF [28]. In accordance with the maximum likelihood (ML) and linear minimum variance (LMV) estimators, the TTF algorithm can be simply extended to Ndimensional fusion. However, the correlations cannot be effectively calculated in the case in which the number of local filters changes dynamically because the correlation calculation requires continuous recursion. The method of covariance intersection (CI) can be used to realize approximate correlation calculations even if the correlations are unknown [29]. The correlation at each moment is calculated from the parameters of the local sensors, and this calculation does not require any recursion, thus effectively solving the problem of dynamic changes in the number of local filters. According to CI theory, the results of the MAC algorithm proposed by Zarei-Jalalabadi are less conservative than the fusion results achieved in the well-known CI case [24], but the applications of the MAC algorithm are still limited because of the absence of weights in the master fusion filter.
Since the recursive computation of the local filters' covariance cannot be implemented in a dynamic distributed system, it is not feasible to use the cross-covariance algorithm to combine two tracks. CI fusion is a popular method used in the TTF algorithm because it does not require knowledge of the real cross-covariance between two tracks. However, because the CI method is too conservative, its fusion performance is insufficient compared with that of the MAC algorithm [24]. Thus, the MAC fusion algorithm is adopted here to combine two tracks with unknown correlations; that is, this method can be used to combine two tracks using only the information from those two tracks, including the local filter states and covariance matrixes. Finally, weighted fusion is performed at the fusion center. The entire framework of the decorrelated distributed EKF (DDEKF)-SLAM system is shown in Fig. 3.
For the distributed Kalman filter algorithm, the local filters' predicted states X i k=k−1 ; i ¼ 1; :::; N n o and predicted covariances P i k=k−1 ; i ¼ 1; :::; N n o can be obtained as follows: Then, the general measurement update equation is as follows: For tracks Xr; PrÞ; r∈ i; j f g À , with an unknown correlation, the MAC-fused track is For the fused track, where W i + W i ≤ 1.
MAC fusion is also applicable to scenarios where the number of parallel filters is dynamically changing because the fusion algorithm is based on the CI fusion method, which does not require historical information. Besides, the MAC algorithm is less conservative than the CI algorithm by reconstructing weight functions W i and W j ; on the other hand, the MAC algorithm is guaranteed to be more conservative than track fusion with a known cross-covariance between the tracks. To enable its application in the distributed SLAM structure, especially in the case of multiple landmarks, we extend the MAC algorithm to N-dimensional fusion and add weights to the fusion results for each subsystem to obtain the optimal estimate. According to the fusion equation of the ML estimator, the fusion result can be obtained as follows: where U ¼ def I; I; ⋯ I; ½ T . According to [30], we can transform the covariance matrix into a diagonal matrix; thus, the MAC expressions can be rewritten as Finally, we add weight information to the fusion center. The weights are obtained as follows: Then, the fused estimate is given by W sisj X̂l N sisj i; j∈ 1; 2; :::; N f g ; i≠j and i < j ð28Þ

Experimental Results and Analysis
In order to evaluate the performance of the proposed distributed structure DDEKF-SLAM system and consider the problem of correlation between local filters to the estimation accuracy, a robot runs in the square to collect the actual data, and the DDEKF algorithm is used to process it. Then, the estimated results are compared with those of the traditional EKF-SLAM system.

Experiment Design
The DDEKF proposed in this paper is suitable for data fusion in a variety of distributed structures. In order to verify the effectiveness of DDEKF in distributed architecture to improve the estimation accuracy, an outdoor experiment based on tree as road punctuation is designed. In this experiment, the way to set the tree as the road punctuation is to verify the algorithm conveniently, and the local filter can be established by identifying different landmarks according to the actual scene. The robot platform used in the experiment uses 2D laser, odometer and magnetic compass as sensors, as shown in Fig. 4  The compass uses magnetic sensors and a two-axis tilt sensor to collect heading information, and its accuracy is 0.3°. In the experiments, trees were used as artificial landmarks, and the SLAM algorithm used dead reckoning and relative observations to estimate the position of the robot and build a navigation map, as shown in Fig. 6. The accuracy of the map reflects the accuracy of the local map origin and the quality of the kinematics model and relative observations.
The real trajectory of the robot was derived from GPS data and is displayed as the red trajectory in Fig. 5. The robot's trajectory started at the left and proceeded in the direction of the arrow; the shape of the first lap was similar to a figure-8 pattern, and the second lap lay along the outer ring of the map.
The laser-based landmark extraction algorithm is similar to the algorithm used in [26], in which the center of a tree trunk is tracked by clustering a number of laser observations. The data collected by the laser sensor from within its projection range were sufficient to determine the thickness of the cylindrical trunk relative to the robot's angle and distance, as shown in Fig. 6. Then, the relative coordinates of the central position of the cylinder could be calculated in accordance with the collected data and the sensor resolution. Feature matching was performed using the nearest neighbor (NN) algorithm, which obtains a positive subset of landmarks in the current map corresponding to a subset of landmarks in a previous map.  Fig. 6 The laser-based landmark extraction algorithm Fig. 7 The GPS, odometer's data and real trees

Performance of algorithms in the real-world environment
The track information in Fig. 7 is provided by the odometer, and the GPS information is added to the picture to facilitate the real track. Intensive trees in the park cause poor GPS performance, so only the robot trajectories recorded when the number of satellites is greater than 12. At the meanwhile, the real tree positions are depicted in Fig. 7, and the tree positions are obtained from the field measurement information combined with the picture information.
The estimated results for the robot's trajectory and the navigation landmarks as obtained with the DDEKF algorithm are shown in Fig. 8. To test the performance of the DDEKF-SLAM system, the experiment was also performed with DPF-SLAM and DEKF-SLAM system for comparison, and these methods have been debugged to the best state in this group of experiments.. Figure 8 shows the pose estimation performance of the three distributed filtering algorithms in the same scene. Among them, the blue dotted line indicates the estimation result of each algorithm, and the red plus sign indicates the estimated landmark  Fig. 8 The comparison of the estimation results point information, the black squares are real trees. The black dots represent the GPS measurement results, which are used as reference information for these experiments.
It is obvious that the DDEKF estimation results more closely approximate to the GPS results than the DPF estimation and DEKF estimation, demonstrating that the DDEKF algorithm achieves better accuracy. Compared to the other two algorithms, the proposed algorithm can estimate the robot's trajectory and the positions of the navigation landmarks very well and shows little error on the estimated landmark positions due to the sensors' inherent noise. Figure 9 is a set of comparative tests in which the number of landmark points is less than or equal to 8 and greater than 8. This set of experiments is mainly used to verify the optimization of the decorrelated distributed system, especially when the number of parallel filters is large.

RMSE of algorithms in the case of the different number of landmarks
In Fig. 9 (a), the error of this localization algorithm comes from the initial stage of the map. The estimation accuracy of the three algorithms differs little when the number of parallel filters is relatively less, and the floating phenomenon of estimation error at this time mainly comes from the sensor error. The results show that the correlation problem in the distributed structure is not obvious when the initial stage of the localization algorithm and the number of landmarks are few.
In Fig. 9 (b) where the number of local filters is relatively large (the number of local filters in this experiment is about 8), the decorrelated distributed structure improves the estimation accuracy of the distributed system, since the correlation is considered in the filtering process. The error of this localization algorithm comes from locating for a period of time, so the cumulative correlation problem is more obvious. Table 1 summarizes the mean values and variances of the estimation results from three algorithms. Besides, the computational cost of the three algorithms is recorded for comparison. These algorithms run on Intel(R) Core (TM) i5-4590 CPU @ 3.30GHz PC.

RMSE of algorithms and Computational Cost
The estimation accuracy of DEKF is slightly lower than DPF, but the computational complexity is low and the computation time is only 20% of DPF. However, the DDEKF system obtains the final estimation result by weighting the fusion result of the MAC. As can be seen from Tab. 1, DDEKF reduces the RMSE and increases the real-time performance of the system, what's more, the calculation time of the DDEKF system is only 28.13% of the DPF.

Conclusion
This paper has proposed a decorrelated distributed SLAM system based on the EKF, which is an effective real-time solution for the automation navigation of mobile robots. The EKF is the best and easiest filter to implement in real time for practical applications of SLAM systems, and the distributed structure can transform the problem of state vector expansion with changing landmarks into dynamic changes in subsystems using a parallel synchronous framework. By integrating the landmarks of these two types of SLAM systems, the proposed DDEKF-SLAM system achieves significant advantages compared to existing SLAM systems. In each local filter, the state vector is composed of the state of the robot and the information on one landmark, and the observation model is composed of laser and heading information, which is used to improve the observability and convergence of the subsystem. To address the correlation problem caused by the common process noise and common prior estimates, the MAC method is implemented to use all local filters as information sources; thus, the local estimates are fused by means of a master filter to achieve global optimality. The experimental results show that the proposed distributed SLAM system has a better estimation accuracy than existing SLAM systems do and consistently achieves more accurate estimation for a continuous trajectory. A disadvantage of the proposed SLAM system is that the stability of the distributed filter with dynamic changes in the subsystems has not been analyzed. For a SLAM system, dynamic changes in subsystems should be considered because this situation commonly occurs. Investigations of an advanced distributed filter and its stability analysis are underway and hopefully will be reported in the near future.
Open Access This article is distributed under the terms of the Creative Comm ons Attribution 4.0 International License (http:// creativecommons.org/licenses/by/4.0/), which permits unrestricted use, distribution, and reproduction in any medium, provided you give appropriate credit to the original author(s) and the source, provide a link to the Creative Commons license, and indicate if changes were made.