Distributed Kalman filter for UWB/INS integrated pedestrian localization under colored measurement noise

Colored Measurement Noise (CMN) has a great impact on the accuracy of human localization in indoor environments with Inertial Navigation System (INS) integrated with Ultra Wide Band (UWB). To mitigate its influence, a distributed Kalman Filter (dKF) is developed for Gauss–Markov CMN with switching Colouredness Factor Matrix (CFM). In the proposed scheme, a data fusion filter employs the difference between the INS- and UWB-based distance measurements. The main filter produces a final optimal estimate of the human position by fusing the estimates from local filters. The effect of CMN is overcome by using measurement differencing of noisy observations. The tests show that the proposed dKF developed for CMN with CFM can reduce the localization error compared to the original dKF, and thus effectively improve the localization accuracy.


Introduction
With the improvement of people's living standards, the population aging problem has become increasingly serious in China. Consequently, health care for elderly people has gradually received due attention and become a new research area (Li et al., 2016;Xu et al., 2018Xu et al., , 2019Zhuang et al., 2019c). As an important technology to assist indoor medical care, localization and tracking of target personnel appear in many works Tian et al., 2020), and many approaches have been developed to provide the localization with sufficient accuracy.
The Global Positioning System (GPS) is most widely used for localization solution (El-Sheimy & Youssef, 2020;Li et al., 2020;Mosavi & Shafiee, 2016). For example, the GPS signals are used in Sekaran et al. (2020) to navigate a robot car. A drawback of GPS is its signals are not always available in indoor environments (El-Sheimy & Li, 2021).
Accordingly, the short-range communication technologies, such as the Radio Frequency IDentification (RFID) (Tzitzis et al., 2019), bluetooth, Wireless Fidelity (WiFi), and Ultra Wide Band (UWB), have been developed for GPS-denied spaces. For example, an active RFID tagbased pedestrian navigation scheme was proposed in Fu and Retscher (2009). In Zhuang and El-Sheimy (2015); WiFi was used to assist the micro electromechanical systems sensors for indoor pedestrian navigation. An improved UWB localization structure was investigated in Yu et al. (2019) in a harsh indoor environment.
The localization techniques discussed above can provide social navigation in indoor environment with a sufficient accuracy. Compared to the RFID, bluetooth, and WiFi, the UWB-based can provide more accurate. Consequently, several UWB-based solutions were proposed in the last decades. However, these short-range communication and localization techniques require the preplaced devices that cannot always be deployed properly in indoor spaces. To overcome this problem, several selfcontained localization structures were proposed, such as the indoor pedestrian navigation scheme (Li et al., 2016) and a foot-mounted pedestrian navigation system based on Inertial Navigation System (INS) (Gu et al., 2015).
The INS-based navigation can be organized as a selfcontained system. However, the accuracy is acceptable only in a short time interval due to the accumulation of drift errors. This shortcoming can be circumvented by integrating the INS-and short range-based communication technologies, as shown in Zhuang et al. (2019a). The INS/UWB integrated scheme is a typical example, but there exist many other approaches. For example, in Xu et al. (2021) and Zhang et al. (2020); an UWB/INS integrated pedestrian navigation algorithm was proposed, which employed the INS to assist the UWB to improve robustness. Another INS/UWB integrated scheme was designed for the quadrotor localization in Xu et al. (2021). The seamless indoor pedestrian tracking using the fusion of INS and UWB data is discussed in Xu et al. (2020). The advantages of the integrated schemes are the higher accuracy and robustness.
It is obvious that data fusion can improve the localization accuracy (Zhao & Huang, 2020). In a hybrid navigation technology, Kalman Filter (KF) is typically a linear fusing model (Norrdine et al., 2016;Zhao et al., 2016;Zhuang et al., 2019b). For nonlinear models, it is often organized using the Extended Kalman Filter (EKF) (Hsu et al., 2017), Iterated Extended Kalman Filter (IEKF) (Xu et al., 2013), and Unscented Kalman Filter (UKF) (Chen et al., 2015). Note that the abovementioned filters are centralized. Although such filters can fuse sensor's data, the drawbacks, compared to the distributed filters, are: higher operation complexity and poorer fault tolerance. Moreover, the sensor data can be affected by Colored Measurement Noise (CMN). For example, Fig. 1 displays the UWB-derived distance with the CMN and white measurement noise. One thus can infer that the CMN is an important error factor in sensor data. It worth noticing that although the KF-based algorithms solve the problem of multi-sensor data fusion in an integrated navigation system and improve the localization accuracy, they are not efficient under CMN observed in UWB data.
To mitigate the effect of CMN on the navigation accuracy in INS/UWB integrated schemes in indoor environments, in this paper we modify the distributed KF (dKF) under Gauss-Markov CMN with an assumption that the Colouredness Factor Matrix (CFM) can switch at some points due to unstable operation conditions. A local filter employs the differences between the INS-measured and UWB-measured distances. The main filter produces the final estimates by fusing the estimates provided by local filters. The effect of CMN is mitigated in local filters using measurement differencing. The experiments show that the dKF modified for CMN with switch CFM can reduce the localization Root Mean Square Error (RMSE) by 26.85% compared to the standard dKF.
The rest of this work is structured as below. First, the INS/UWB integration for human localization scenario operating under CMN is described. Second, a dKF is developed for CMN with switch CFM. Third, the experiment is introduced. Fourth, the comparisons are made in terms of the localization accuracy given by INS, UWB, dKF, and dKF modified for CMN with switch CFM. Finally, conclusions are drawn.

INS/UWB integrated human navigation under CMN
The proposed INS/UWB integrated human localization scheme affected by CMN is shown in Fig. 2. In this structure, the INS and UWB subsystems work in parallel. The fusing filter is organized in the way that one key filter works together with M sub-filters. The jth sub-filter, j ∈ [1, M] , is employed to estimate the target

Design of dKF for CMN with switch CFM
In this section, we modify the dKF under Gauss-Markov CMN. First, we consider the state-space model of the navigation problem. Then, the dKF is designed under CMN assuming switch CFM. Finally, the main filter fuses the results of the sub-filters.

Sub-filters for CMN
The state equation representing the 2D human dynamics and related to the jth sub-filter is described by: where the state vector is defined as in which (δPos n , δVel N (j) n ) are the position and velocity errors in east and north directions, T (j) is the sample time for the jth sub-filter, w (j) n ∼ N (0, Q (j) ) is noise in the jth sub-filter. The observation equation corresponding to the data obtained by the jth sub-filter is written as is white Gaussian driving noise and α (j) n is the CFM.
To address the effects of CMN and apply filtering algorithms, we use measurement differencing and write the new observation equation as From (1) and (3) we obtain and substitute them into (4), giving where T n is time-correlated with system noise w (j) n and the KF cannot be applied straightforwardly. To decorrelate noise, we follow Shmaliy et al. (2020) and modify the state equation (1) as The noise vectors η  • Compute the Mahalanobis distance (Mahalanobis, 1936) • Find α (j) n,opt by solving the minimization problem (11) A pseudo code of the sub-KF for CMN with switch CFM is listed as Algorithm 2 and the structure of this filter is shown in Fig. 3. Having selected a proper range for CFM, this algorithm determines α (j) n,opt , which is further used in the main filter.

Distributed KF for CMN with switch CMN
The dKF algorithm used in the proposed navigation system is responsible for fusing data collected from local sub-filters and estimating the object position x n and localization error covariance P n as

Experimental setup
To test the dKF designed under CMN with switch CFM, we exploited the INS/UWB human localization system deployed in the No. 14 building of the University of Jinan, Jinan, China, as shown in Fig. 4. The target person equipped with experimental devices was pictured in Fig. 5. In this work, we conducted two tests, where we exploited the INS and the UWB localization systems described in Xu et al. (2019). The testbed worked as follows. We placed UWB RNs in indoor spaces according to designed positions and installed a Blind Node (BN) on a mobbing target. The target person wore an Inertial Measurement Unit (IMU) to obtain the INS results. The encoder was used to measure the distance from the start point. In this experiment, a target traveled along a planned trajectory, which was complicated with obstacles. Moreover, the method to obtain the ground truth coordinates in the experimental test can be founded in Xu et al. (2019). It has two phases: (1) establishing the mapping between the distance walking along the planned path from the start point and the ground truth coordinate and (2) encoding, to measure the walking distance and calculate the ground truth coordinates through the constructed mapping.

Localization errors
To test this filter, we specify the state space model for q = 5 and T (j) = 0.45 s, j ∈ [1, 4] , with α

Error comparison of KF and dKF
Since the models (1)-(7) are linear, the linear data fusion is used in this paper. The localization error Pos_error produced by INS, UWB, KF, and dKF in Test 1 is computed as where Pos E,R , Pos N,R denote the reference coordinates. The cumulative distribution functions (CDFs) are sketched in Fig. 6. From this figure, one can see that the KF and dKF can reduce the localization error over the INS and UWB. Also, the dKF has a better performance than the KF. The INS, UWB, KF, and dKF results in Test 1 are given in Table 1, which suggests that the dKF gives the smallest localization error.

Test 1
The trajectories planned or estimated by the INS, UWB, dKF for CMN, and dKF for CMN with switch (18) CMN in test 1 are shown in Fig. 7. As can be seen, the errors in the INS outputs are accumulated, but at a lower rate due to the implementation of the Zerovelocity Update (ZUPT). Figures 8 and 9 show the estimates in the east and north directions produced by the INS, UWB, dKF, and dKF for CMN with switch CFM in test 1. Compared to INS, the UWB trajectory is close to the planned path. It is also noticed that the estimate produced by the dKF is not as accurate as that by the dKF for CMN with switch CFM, whose outputs are closer to the planned path. Figure 10 sketches the CDFs of the localization errors Pos_error produced by different estimators in test 1. It is clear that the proposed dKF for CMN with switch CFM produces the smallest errors compared to the INS, UWB, and dKF. Table 2 shows the localization results which indicating that the dKF for CMN with switch CFM has the smallest errors.

Test 2
We employed another test to verify the performance of the proposed method. Trajectories planned or given by the INS, UWB, dKF, and dKF for CMN with switch CMN   in test 2 are shown in Fig. 11. One can see that the UWB trajectory is close to the planned path, unlike that of INS. It is also noticed that the estimate produced by the dKF is not as accurate as that by the dKF for CMN with switch CFM, whose output is much closer to the planned path. The CDFs of Pos_error produced by the INS, UWB, dKF, and dKF for CMN with switch CFM in test 2 are shown in Fig. 12. Thus, we conclude that the proposed dKF for CMN with switch CFM gives the smallest errors of 0.9, which is reduced by about 27.4% relative to dKF (Table 3).

The performances for the dKF for CMN with switch CFM and with constant CFM
We compare the performances of the dKFs designed   figure, we infer that the performances of the dKFs for CMN with switch CFM and with constant CFM are similar in the tests. It should also be emphasized that the method of setting a constant CFM is offline in this work, which is obtained from all the test data. The procedure is not designed and not the best choice for real time applications. Compared with dKF for CMN with constant CFM, the proposed dKF for CMN with switch CFM can obtain the CFM adaptively. Moreover, its performance is similar to the dKF for CMN with constant CFM. Figures 13  and 14 also show that the dKF for CMN with switch CFM can perform online and the results are very close to the optimal ones.

Conclusion
The dKF designed in this paper under CMN with switch CFM has demonstrated the ability to improve online the performance of the INS/UWB integrated human localization system in indoor environments. The effect was achieved with determining the optimal CFM by solving the minimization problem and modifying the KF-based fusion filter. Accordingly, the effect of the CMN has been essentially mitigated in the output of the main dKF. The tests demonstrated a better performance of the dKF for CMN with switch CFM over the dKF for CMN. It also shows that the accuracy of the INS/UWB integrated system-based human localization can be improved compared to the standard dKF.