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

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

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)

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.

Fig. 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.

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

Testing experimental setup

Fig. 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

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

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

Fig. 8

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

Fig. 9

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

Fig. 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

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

Fig. 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

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

Fig. 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.