Skip to main content

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

Abstract

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., 2018, 2019; Zhuang et al., 2019c). As an important technology to assist indoor medical care, localization and tracking of target personnel appear in many works (Chen et al., 2020; 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 tag-based 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 pre-placed devices that cannot always be deployed properly in indoor spaces. To overcome this problem, several self-contained 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 self-contained 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 above-mentioned 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.

Fig. 1
figure 1

The distance with the colored measurement noise and white measurement noise

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 \in [1,M]\), is employed to estimate the target human position by fusing ranges \(r^{UWB}_j\) and \(r^{INS}_j\) from the target person to the jth UWB Reference Node (RN) under CMN at a discrete time index n. The main filter fuses the results of sub-filters to produce an optimal estimate.

Fig. 2
figure 2

INS/UWB integrated human localization scheme for distributed localization under CMN

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:

$$\begin{aligned} {\varvec{x}}_n^{(j)} &= {{\varvec{F}}^{(j)}}{\varvec{x}}_{n - 1}^{(j)} + {\varvec{w}}_n^{(j)}\\&= { \left[ {\begin{array}{*{20}{c}} 1&\quad {{T^{(j)}}}&\quad 0&\quad 0\\ 0&\quad 1&\quad 0&\quad 0\\ 0&\quad 0&\quad 1&\quad {{T^{(j)}}}\\ 0&\quad 0&\quad 0&\quad 1 \end{array}} \right] {\varvec{x}}_{n - 1}^{(j)} + {\varvec{w}}_n^{(j)}} \end{aligned}$$
(1)

where the state vector is defined as

$$\begin{aligned} {\varvec{x}}_n^{(j)} = \left[ {\begin{array}{*{20}c} {\delta {\mathrm{Pos}}_n^{E{(j)}} } &\quad {\delta {\mathrm{Vel}}_n^{E{(j)}} } & \quad {\delta {\mathrm{Pos}}_n^{N{(j)}} } &\quad {\delta {\mathrm{Vel}}_n^{N{(j)}} } \\ \end{array}} \right] ^{\mathrm{T}}, \end{aligned}$$

in which \(( {\delta {\mathrm{Pos}}_n^{E{(j)}} ,\delta {\mathrm{Pos}}_n^{N{(j)}} })\) and \(( {\delta {\mathrm{Vel}}_n^{E{(j)}} ,\delta {\mathrm{Vel}}_n^{N{(j)}} } )\) are the position and velocity errors in east and north directions, \(T^{(j)}\) is the sample time for the jth sub-filter, \({\varvec{w}}_n^{(j)} \sim {\mathcal {N}} ({\varvec{0}}, {\varvec{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

$$\begin{aligned} y_n^{(j)} &= \delta {\mathrm{r}}_n^{(j)} = {\mathrm{r}}_n^{I(j)}- {\mathrm{r}}_n^{U(j)}\\ & = \frac{{\delta _{{\mathrm{x}}n}^{(j)}}}{{\sqrt{\delta _{{\mathrm{x}}n}^{{{(j)}^2}} + \delta _{{\mathrm{y}}n}^{{{(j)}^2}}} }}\delta {\mathrm{Pos}}_n^{E(j)} + \frac{{\delta _{{\mathrm{y}}n}^{(j)}}}{{\sqrt{\delta _{{\mathrm{x}}n}^{{{(j)}^2}} + \delta _{{\mathrm{y}}n}^{{{(j)}^2}}} }}\delta {\mathrm{Pos}}_n^{N(j)} + {\varvec{v}}_n^{(j)}\\ & = {\left[ {\begin{array}{*{20}{c}} {\frac{{\delta _{{\mathrm{x}}n}^{(j)}}}{{\sqrt{\delta _{{\mathrm{x}}n}^{{{(j)}^2}} + \delta _{{\mathrm{y}}n}^{{{(j)}^2}}} }}}\\ 0\\ {\frac{{\delta _{{\mathrm{y}}n}^{(j)}}}{{\sqrt{\delta _{{\mathrm{x}}n}^{{{(j)}^2}} + \delta _{{\mathrm{y}}n}^{{{(j)}^2}}} }}}\\ 0 \end{array}} \right] ^{\mathrm{T}}}{\varvec{x}}_n^{(j)} + {\varvec{v}}_n^{(j)}\\ & = {\varvec{H}}_n^{(j)}{\varvec{x}}_n^{(j)} + {\varvec{v}}_n^{(j)} \end{aligned}$$
(2)

where \(j \in [1,M]\), \(\delta _{{\mathrm {x}}n}^{(j)} = {\mathrm{Pos}}_n^{E,I} - x^{(j)}\), \(\delta _{{\mathrm {y}}n}^{(j)} = {\mathrm{Pos}}_n^{N,I} - y^{(j)}\), \(\left( {{\mathrm{Pos}}_n^{E,I} ,{\mathrm{Pos}}_n^{N,I}} \right)\) denotes INS positions in east and north directions, and \({\varvec{v}}_n\) is Gauss–Markov CMN represented with

$$\begin{aligned} {\varvec{v}}_n^{\left( j \right) } = \alpha _n^{\left( j \right) } {\varvec{v}}_{n-1}^{\left( j \right) } + \gamma _n^{\left( j \right) } \end{aligned}$$
(3)

where \(\gamma _n^{\left( j \right) } \sim {\mathcal {N}} ({\varvec{0}},{\varvec{R}})\) is white Gaussian driving noise and \(\alpha _n^{\left( j \right) }\) is the CFM.

To address the effects of CMN and apply filtering algorithms, we use measurement differencing and write the new observation equation as

$$\begin{aligned} {\varvec{z}}_n^{\left( j \right) }&= {\varvec{y}}_n^{\left( j \right) } - \alpha _n^{\left( j \right) } {\varvec{y}}_{n-1}^{\left( j \right) } \nonumber \\&= {\varvec{H}}_n^{\left( j \right) } {\varvec{x}}_n^{\left( j \right) } + {\varvec{v}}_n^{\left( j \right) } - \alpha _n^{\left( j \right) } {\varvec{H}}_{n - 1}^{\left( j \right) } {\varvec{x}}_{n - 1}^{\left( j \right) } \nonumber \\&\quad - \alpha _n^{\left( j \right) }{\varvec{v}}_{n - 1}^{\left( j \right) } \end{aligned}$$
(4)

From (1) and (3) we obtain

$$\begin{aligned} {\varvec{x}}_{n - 1}^{(j)}&= {{\varvec{F}}^{(j)^{-1}}} ( {{\varvec{x}}_n^{(j)} - {\varvec{w}}_n^{(j)}} ) \end{aligned}$$
(5)
$$\begin{aligned} {\varvec{v}} _{n - 1}^{(j)}&= {\alpha _n^{(j)^{-1}} } ( {{\varvec{v}} _n^{(j)} - \gamma _n^{(j)}} ) \end{aligned}$$
(6)

and substitute them into (4), giving

$$\begin{aligned} {\varvec{z}}_n^{(j)}&= ( {\varvec{H}}_n^{(j)} - {\varvec{T}}_n^{(j)} ) {\varvec{x}}_n^{(j)} + {\varvec{T}}_n^{(j)} {\varvec{w}}_n^{(j)} + \gamma _n^{(j)} \nonumber \\&= {\varvec{D}}_n^{(j)} {\varvec{x}}_n^{(j)} + {\bar{\gamma }}_n^{(j)} \end{aligned}$$
(7)

where \({\varvec{T}}_n^{(j)} = \alpha _n^{(j)} {\varvec{H}}_n^{(j)} {\varvec{F}}^{(j)^{-1}}\), \({\varvec{D}}_n^{(j)} = {\varvec{H}}_n^{(j)} - {\varvec{T}}_n^{(j)}\), \({{\bar{\gamma }}} _n^{(j)} ={\varvec{T}}_n^{(j)} {\varvec{w}}_n^{(j)} + \gamma _n^{(j)}\) and \({\bar{\gamma }}_n^{(j)} \sim {\mathcal {N}} ( {{\varvec{0}},{\bar{{\varvec{R}}}} } )\) is white Gaussian noise

$$\begin{aligned} {\bar{\gamma }}_n^{(j)} = {\varvec{T}}_n^{(j)} {\varvec{w}}_n^{(j)} + \gamma _n^{(j)} \end{aligned}$$
(8)

with the covariance

$$\begin{aligned} \overline{\varvec{R}} &= {\mathrm{E}}\{ {{\bar{\gamma }}} _{\mathrm{n}}^{({\mathrm{j}})}{{\bar{\gamma }}} _{\mathrm{n}}^{{{({\mathrm{j}})}^{\mathrm{T}}}}\} = {\varvec{T}}_{\mathrm{n}}^{({\mathrm{j}})}{{\varvec{Q}}^{({\mathrm{j}})}}{\varvec{T}}_{\mathrm{n}}^{{{({\mathrm{j}})}^{\mathrm{T}}}}+ {\varvec{R}}\\ & = {\varvec{T}}_n^{(j)}{\varvec{\Phi }}_n^{(j)} + {\varvec{R}} \end{aligned}$$
(9)

where \({\varvec{\Phi }}_n^{(j)} = {\varvec{Q}}^{(j)} {\varvec{T}}_n^{(j)^T}\). It follows from (8) that the observation noise \({\bar{\gamma }}_n^{(j)}\) is time-correlated with system noise \({\varvec{w}}_n^{(j)}\) and the KF cannot be applied straightforwardly. To de-correlate noise, we follow Shmaliy et al. (2020) and modify the state equation (1) as

$$\begin{aligned} {\varvec{x}}_n^{(j)} &= {{\varvec{F}}^{(j)}}{\varvec{x}}_{n - 1}^{(j)} + {\varvec{w}}_n^{(j)} + {\varvec{\beta }}_n^{(j)}[{\varvec{z}}_n^{(j)} - ({\varvec{D}}_n^{(j)}{\varvec{x}}_n^{(j)} + {\bar{{{\varvec{\gamma }}} }}_n^{(j)})]\\ & = ({\varvec{I}} - {\varvec{\beta }}_n^{(j)}{\varvec{D}}_n^{(j)}){{\varvec{F}}^{(j)}}{\varvec{x}}_{n - 1}^{(j)} + {\varvec{\beta }}_n^{(j)}{\varvec{z}}_n^{(j)}\\ &\quad + ({\varvec{I}} - {\varvec{\beta }}_n^{(j)}{\varvec{D}}_n^{(j)}){\varvec{w}}_n^{(j)} - {\varvec{\beta }}_n^{(j)}{\bar{{{\varvec{\gamma }}} }}_n^{(j)}\\ & = {\varvec{A}}_n^{(j)}{\varvec{x}}_{n - 1}^{(j)} + {\varvec{u}}_n^{(j)} + {\varvec{\eta }}_n^{(j)} \end{aligned}$$
(10)

where \({{\varvec{\eta }}_n^{(j)} } \sim {\mathcal {N}} ({\varvec{0}},{\varvec{\Theta }}_n^{(j)})\) has the covariance

$$\begin{aligned} {\varvec{\Theta }}_{\mathbf {n}}^{({\mathbf {j}})} &= {\mathbf {E}}\left\{ {[({\mathbf {I}} - {\varvec{\beta }}_{\mathbf {n}}^{({{j}})}{\mathbf {D}}_{\mathbf {n}}^{({{j}})}){\mathbf {w}}_{\mathbf {n}}^{({{j}})} - {\varvec{\beta }}_{\mathbf {n}}^{({{j}})}{ \bar{{{\boldsymbol{\gamma }}} }}_{\mathbf {n}}^{({{j}})}]} \right. \\ &\quad \left. {{{[({\mathbf {I}} - {\varvec{\beta }}_{\mathbf {n}}^{({{j}})}{\mathbf {D}}_{\mathbf {n}}^{({{j}})}){\mathbf {w}}_{\mathbf {n}}^{({{j}})} - {\varvec{\beta }}_{\mathbf {n}}^{({{j}})}{\bar {{{\boldsymbol{\gamma }}} }}_{\mathbf {n}}^{({{j}})}]}^{\mathrm{T}}}} \right\} \\ & = ({\mathbf {I}} - {\varvec{\beta }}_{\mathbf {n}}^{({{j}})}{\mathbf {H}}_{\mathbf {n}}^{({{j}})}){\mathbf {Q}}_{\mathbf {n}}^{({{j}})}{({\mathbf {I}} - {\varvec{\beta }}_{\mathbf {n}}^{({{j}})}{\mathbf {H}}_{\mathbf {n}}^{({{j}})})^{\mathrm{T}}}\\ &\quad + {\varvec{\beta }}_{\mathbf {n}}^{({{j}})}{\mathbf {R}}{({\varvec{\beta }}_{\mathbf {n}}^{({{j}})})^{\mathrm{T}}} \end{aligned}$$
(11)

The noise vectors \({{\varvec{\eta }}_n^{(j)} }\) and \({{{\bar{\gamma }}} _n^{(j)} }\) will be de-correlated if the conditions \({\mathrm {E}} \{ {{\varvec{\eta }}_n^{(j)} ( {{{\bar{\gamma }}} _n^{(j)} } )^{\mathrm{T}} } \} = 0\) is satisfied that can be achieved with

$$\begin{aligned} {\varvec{\beta }}_n^{(j)}&= \Phi _n^{(j)} ( {{\varvec{H}}_n^{(j)} \Phi _n^{(j)} + {\varvec{R}}} )^{ - 1}\,, \end{aligned}$$
(12)
$$\begin{aligned} {\varvec{\Theta} }_n^{(j)}&= ( {{\varvec{I}} - {\varvec{\beta }}_n^{(j)} {\varvec{H}}_n^{(j)} } ){\varvec{Q}}^{( j )} ( {{\varvec{I}} - {\varvec{\beta }}_n^{(j)} {\varvec{D}}_n^{(j)} } )^{\mathrm{T}} \end{aligned}$$
(13)
figure a

Provided the de-correlation, the algorithm of the sub-KF method operating under CMN is described in Algorithm 1. Unlike the standard KF, Algorithm 1 requires the CFM \(\alpha _n^{(j)}\) at each n. To set \(\alpha _n^{(j)}\) properly, we take notice of possible time variations in \(\alpha _n^{(j)}\), and make CMF switching by the following steps:

  • Set several possible values of \(\alpha _n^{(j),i}, i\in [1,q]\).

  • Run q sub-KFs with \(\alpha _n^{(j),i}, i\in [1,q]\) in parallel.

  • Compute the Mahalanobis distance (Mahalanobis, 1936)

    $$\begin{aligned} L_n^{\alpha _n^{(j),i} } = ( {{\varvec{z}}_n^{(j)} - {\varvec{D}}_n^{(j)} {{\hat{\varvec{x}}}}_n^{(j)-} } )^T {{\varvec{R}}^{(j)^{-1}} } ( {{\varvec{z}}_n^{(j)} - {\varvec{D}}_n^{(j)} {{\hat{\varvec{x}}}}_n^{(j)-} } ) \end{aligned}$$
    (14)
  • Find \(\alpha _{n,{\mathrm {opt}}}^{(j)}\) by solving the minimization problem

    $$\begin{aligned} \alpha _{n,{\mathrm {opt}}}^{(j)} = \mathop {\arg \min }\limits _{\alpha _n^{(j),i} } {L_n^{\alpha ^{(j),i} } } \end{aligned}$$
    (15)

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 \(\alpha _{n,{\mathrm {opt}}}^{(j)}\), which is further used in the main filter.

figure b
Fig. 3
figure 3

Structure of the sub-KF for CMN with switch CFM

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 \(\hat{\varvec{x}}_n\) and localization error covariance \({\varvec{P}}_n\) as

$$\begin{aligned} \hat{\varvec{x}}_n&= {\varvec{P}}_n ( {\varvec{P}}_n^{(1)^{-1}} \hat{\varvec{x}}_n^{(1)} + {\varvec{P}}_n^{(2)^{-1}} \hat{\varvec{x}}_n^{(2)} \nonumber \\&\quad + \dots + {\varvec{P}}_n^{(M)^{-1}} \hat{\varvec{x}}_n^{(M)}) \,, \end{aligned}$$
(16)
$$\begin{aligned} {\varvec{P}}_n^{-1}&= {\varvec{P}}_n^{(1)^{-1}} + {\varvec{P}}_n^{(2)^{-1}} + \cdots + {\varvec{P}}_n^{(M)^{-1}} \end{aligned}$$
(17)

A pseudo code of the main dKF for CMN with switch CFM is listed as Algorithm 3.

figure c

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.

Fig. 4
figure 4

Testing experimental setup

Fig. 5
figure 5

The target human

Localization errors

To test this filter, we specify the state space model for \(q=5\) and \(T^{(j)} = 0.45\, {\mathrm {s}}, j\in [1,4]\), with \(\alpha _n^{(j),1}=0.1\), \(\alpha _n^{(j),2}=0.3\), \(\alpha _n^{(j),3}=0.5\), \(\alpha _n^{(j),4}=0.7\), \(\alpha _n^{(j),5}=0.9\).

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 \({\mathrm{Pos}}\_{\mathrm{error}}\) produced by INS, UWB, KF, and dKF in Test 1 is computed as

$$\begin{aligned} {\mathrm{Pos}}\_{\mathrm{error}} = \sqrt{\left({\mathrm{Pos}}^{\mathrm{E}} - {{\mathrm{Pos}}^{\mathrm{E,R}}}\right) ^2 + \left( {\mathrm{Pos}}^{\mathrm{N}} - {\mathrm {Pos}}^{\mathrm{N,R}} \right) ^2 } \end{aligned}$$
(18)

where \(\left( {\mathrm {Pos}}^{\mathrm{E,R}},{\mathrm { Pos}}^{\mathrm{N,R}}\right)\) 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.

Table 1 Position RMSEs produced by the INS, UWB, KF, and dKF in Test 1
Fig. 6
figure 6

The CDFs of \({\mathrm {Pos}}\_{\mathrm{error}}\) produced by the KF and dKF in Test 1

Test 1

The trajectories planned or estimated by the INS, UWB, dKF for CMN, and dKF for CMN with switch 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 Zero-velocity 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 \({\mathrm{Pos}}\_{\mathrm{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.

Fig. 7
figure 7

Trajectories by plan or given by the INS, UWB, dKF for CMN, and dKF for CMN with switch CMN in test 1

Fig. 8
figure 8

Estimates in the east direction produced by the INS, UWB, dKF, and dKF for CMN with switch CFM in test 1

Fig. 9
figure 9

Estimates in the north direction produced by the INS, UWB, dKF, and dKF for CMN with switch CFM in test 1

Fig. 10
figure 10

The CDFs of \({\varvec{Pos}}\_{\varvec{error}}\) produced by the INS, UWB, dKF, and dKF for CMN with switch CFM in test 1

Table 2 Position RMSEs produced by the INS, UWB, dKF for CMN, and dKF for CMN with switch CFM in test 1

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 \({\varvec{Pos}}\_{\varvec{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).

Fig. 11
figure 11

Trajectories by plan or given by the INS, UWB, dKF, and dKF for CMN with switch CMN in test 2

Fig. 12
figure 12

The CDFs of \({\mathrm {Pos}}\_{\mathrm{error}}\) produced by the INS, UWB, dKF, and dKF for CMN with switch CFM in test 2

Table 3 Position RMSEs produced by the INS, UWB, dKF for CMN, and dKF for CMN with switch CFM in test 2

The performances for the dKF for CMN with switch CFM and with constant CFM

We compare the performances of the dKFs designed to the trajectories given by INS and UWB. The CDFs of \({\varvec{Pos}}\_{\varvec{error}}\) produced by the dKF for CMN with switch CFM and with constant CFM in test 1 and test 2 are shown in Figs. 13 and 14 respectively. To compare errors, we set \(\alpha ^{(j)}_{\mathrm{opt}}=0.15\), \(j\in {[}1,4{]}\), in constant CFM for all data. From this 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.

Fig. 13
figure 13

The CDFs of \({\mathrm {Pos}}\_{\mathrm{error}}\) produced by the dKF for CMN with switch CFM and constant CFM in test 1

Fig. 14
figure 14

The CDFs of \({\mathrm {Pos}}\_{\mathrm{error}}\) produced by the dKF for CMN with switch CFM and constant CFM in test 2

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.

Availability of data and materials

The raw data were provided by University of Jinan, Jinan, China. The raw data used in this study are available from the corresponding author upon request.

Abbreviations

CDF:

Cumulative distribution function

CFM:

Colouredness factor matrix

CMN:

Colored measurement noise

dKF:

Distributed Kalman filter

EKF:

Extended Kalman filter

GPS:

Global positioning system

IEKF:

Iterated extended Kalman filter

IMU:

Inertial measurement unit

INS:

Inertial navigation system

KF:

Kalman filter

RFID:

Radio frequency identification

RMSE:

Root mean square error

UKF:

Unscented Kalman filter

UWB:

Ultra wide band

UWB RN:

UWB reference node

UWB BN:

UWB blind node

ZUPT:

Zero-velocity update

References

  • Chen, G., Meng, X., Wang, Y., Zhang, Y., Peng, T., & Yang, H. (2015). Integrated WiFi/PDR/Smartphone using an unscented Kalman filter algorithm for 3D indoor localization. Sensors, 15(9), 24595–24614.

    Article  Google Scholar 

  • Chen, N., Li, M., Yuan, H., Su, X., & Li, Y. (2020). Survey of pedestrian detection with occlusion. Complex and Intelligent Systems, 7, 1–11.

    Google Scholar 

  • El-Sheimy, N., & Li, Y. (2021). Indoor navigation: State of the art and future trends. Satellite Navigation, 2, 7.

    Article  Google Scholar 

  • El-Sheimy, N., & Youssef, A. (2020). Inertial sensors technologies for navigation applications: State of the art and future trends. Satellite Navigation, 1, 2.

    Article  Google Scholar 

  • Fu, Q., & Retscher, G. (2009). Active RFID trilateration and location fingerprinting based on RSSI for pedestrian navigation. Journal of Navigation, 62(2), 323–340.

    Article  Google Scholar 

  • Gu, Y., Song, Q., Li, Y., & Ma, M. (2015). Foot-mounted pedestrian navigation based on particle filter with an adaptive weight updating strategy. Journal of Navigation, 68(01), 23–38.

    Article  Google Scholar 

  • Hsu, Y. L., Wang, J. S., & Chang, C. W. (2017). A wearable inertial pedestrian navigation system with quaternion-based extended Kalman filter for pedestrian localization. IEEE Sensors Journal, 17(10), 3193–3206.

    Article  Google Scholar 

  • Li, R., Zheng, S., Wang, E., Chen, J., Feng, S., Wang, D., & Dai, L. (2020). Inertial sensors technologies for navigation applications: State of the art and future trends. Satellite Navigation, 1, 12.

    Article  Google Scholar 

  • Li, Y., Zhuang, Y., Lan, H., Zhang, P., Niu, X., & El-Sheimy, N. (2016). Self-contained indoor pedestrian navigation using smartphone sensors and magnetic features. IEEE Sensors Journal, 16(19), 7173–7182.

    Article  Google Scholar 

  • Mahalanobis, P. C. (1936). On the generalised distance in statistics. Proceedings of the National Institute of Sciences, 2, 49–55.

    MATH  Google Scholar 

  • Mosavi, M. R., & Shafiee, F. (2016). Narrowband interference suppression for GPS navigation using neural networks. GPS Solutions, 20(3), 341–351.

    Article  Google Scholar 

  • Norrdine, A., Kasmi, Z., & Blankenbach, J. (2016). Step detection for ZUPT-aided inertial pedestrian navigation system using foot-mounted permanent magnet. IEEE Sensors Journal, 16(17), 6766–6773.

    Article  Google Scholar 

  • Sekaran, J. F., Kaluvan, H., & Irudhayaraj, L. (2020). Modeling and analysis of GPS-GLONASS navigation for car like mobile robot. Journal of Electrical Engineering and Technology, 15(5), 927–935.

    Article  Google Scholar 

  • Shmaliy, Y. S., Zhao, S., & Ahn, C. K. (2020). Kalman and UFIR state estimation with coloured measurement noise using backward Euler method. IET Signal Process, 14(2), 64–71.

    Article  Google Scholar 

  • Tian, Q., Wang, I. K., & Salcic, Z. (2020). An INS and UWB fusion approach with adaptive ranging error mitigation for pedestrian tracking. IEEE Sensors Journal, 20(8), 4372–4381.

    Article  Google Scholar 

  • Tzitzis, A., Megalou, S., Siachalou, S., Emmanouil, T. G., Kehagias, A., Yioultsis, T. V., & Dimitriou, A. G. (2019). Localization of RFID tags by a moving robot, via phase unwrapping and non-linear optimization. IEEE Journal of Radio Frequency Identification, 3(4), 216–226.

    Article  Google Scholar 

  • Xu, Y., Ahn, C. K., Shmaliy, Y. S., Chen, X., & Bu, L. (2019). Indoor INS/UWB-based human localization with missing data utilizing predictive UFIR filtering. IEEE/CAA Journal of Automatica Sinica, 6(4), 952–960.

    Article  Google Scholar 

  • Xu, Y., Chen, X., & Li, Q. (2013). Autonomous integrated navigation for indoor robots utilizing on-line iterated extended Rauch–Tung–Striebel smoothing. Sensors, 13(12), 15937–15953.

    Article  Google Scholar 

  • Xu, Y., Li, Y., Ahn, C. K., & Chen, X. (2020). Seamless indoor pedestrian tracking by fusing INS and UWB measurements via LS-SVM assisted UFIR filter. Neurocomputing, 388, 301–308.

    Article  Google Scholar 

  • Xu, Y., Shmaliy, Y. S., Ahn, C. K., Shen, T., & Zhuang, Y. (2021). Tightly-coupled integration of INS and UWB using fixed-lag extended UFIR smoothing for quadrotor localization. IEEE Internet of Things Journal, 8(3), 1716–1727.

    Article  Google Scholar 

  • Xu, Y., Shmaliy, Y. S., Ahn, C. K., Tian, G., & Chen, X. (2018). Robust and accurate UWB-based indoor robot localisation using integrated EKF/EFIR filtering. IET Radar Sonar and Navigation, 12(7), 750–756.

    Article  Google Scholar 

  • Yu, K., Wen, K., Li, Y., Zhang, S., & Zhang, K. (2019). A novel NLOS mitigation algorithm for UWB localization in harsh indoor environments. IEEE Transactions on Vehicular Technology, 68(1), 686–699.

    Article  Google Scholar 

  • Zhang, Y., Tan, X., & Changsheng, Z. (2020). UWB/INS integrated pedestrian positioning for robust indoor environments. IEEE Sensors Journal, 20(23), 14401–14409.

    Article  Google Scholar 

  • Zhao, S., & Huang, B. (2020). Trial-and-error or avoiding a guess? Initialization of the Kalman filter. Automatica, 121, 109184.

    MathSciNet  Article  Google Scholar 

  • Zhao, S., Shmaliy, Y. S., & Liu, F. (2016). Fast Kalman-like optimal unbiased FIR filtering with applications. IEEE Transactions on Signal Processing, 64(9), 2284–2297.

    MathSciNet  Article  Google Scholar 

  • Zhuang, Y., & El-Sheimy, N. (2015). Tightly-coupled integration of WiFi and mems sensors on handheld devices for indoor pedestrian navigation. IEEE Sensors Journal, 16(1), 224–234.

    Article  Google Scholar 

  • Zhuang, Y., Hua, L., Wang, Q., Cao, Y., & Thompson, J. S. (2019a). Visible light positioning and navigation using noise measurement and mitigation. IEEE Transactions on Vehicular Technology, 68(11), 11094–11106.

    Article  Google Scholar 

  • Zhuang, Y., Wang, Q., Shi, M., Cao, P., Qi, L., & Yang, J. (2019b). Low-power centimeter-level localization for indoor mobile robots based on ensemble Kalman smoother using received signal strength. IEEE Internet of Things Journal, 6(4), 6513–6522.

    Article  Google Scholar 

  • Zhuang, Y., Yang, J., Qi, L., Li, Y., Cao, Y., & El-Sheimy, N. (2019c). A pervasive integration platform of low-cost MEMS sensors and wireless signals for indoor localization. IEEE Internet of Things Journal, 5(6), 4616–4631.

    Article  Google Scholar 

Download references

Acknowledgements

Yuan Xu do hope his wife Chen Fu will soon get well again.

Funding

This work is supported by NSFC Grant 61803175, Shandong Key R&D Program 2019JZZY021005 and Mexican Consejo Nacional de Cienciay Tecnologıa Project A1-S-10287 Grant CB2017-2018.

Author information

Authors and Affiliations

Authors

Contributions

We all conceived the idea and contributed to the writing of the paper. All authors read and approved the final manuscript.

Corresponding author

Correspondence to Yuan Zhuang.

Ethics declarations

Competing interests

The authors declare that they have no competing interests.

Ethical approval

This article does not contain any studies with human participants performed by any of the authors.

Additional information

Publisher’s Note

Springer Nature remains neutral with regard to jurisdictional claims in published maps and institutional affiliations.

Rights and permissions

Open Access This article is licensed under a Creative Commons Attribution 4.0 International License, which permits use, sharing, adaptation, distribution and reproduction in any medium or format, as long as you give appropriate credit to the original author(s) and the source, provide a link to the Creative Commons licence, and indicate if changes were made. The images or other third party material in this article are included in the article's Creative Commons licence, unless indicated otherwise in a credit line to the material. If material is not included in the article's Creative Commons licence and your intended use is not permitted by statutory regulation or exceeds the permitted use, you will need to obtain permission directly from the copyright holder. To view a copy of this licence, visit http://creativecommons.org/licenses/by/4.0/.

Reprints and Permissions

About this article

Verify currency and authenticity via CrossMark

Cite this article

Xu, Y., Cao, J., Shmaliy, Y.S. et al. Distributed Kalman filter for UWB/INS integrated pedestrian localization under colored measurement noise. Satell Navig 2, 22 (2021). https://doi.org/10.1186/s43020-021-00053-z

Download citation

  • Received:

  • Accepted:

  • Published:

  • DOI: https://doi.org/10.1186/s43020-021-00053-z

Keywords

  • Distributed filtering
  • Kalman filter
  • Colored measurement noise
  • Human localization