Dual Kalman Filter Based on a Single Direction under Colored Measurement Noise for INS-Based Integrated Human Localization

Abstract

For inertial-based integrated pedestrian navigation, the navigation environment might affect the positioning accuracy in different directions. Meanwhile, complex filtering algorithms can reduce computational efficiency. Therefore, one dual Kalman filter (KF) based on a single direction under a colored measurement noise (CMN) scheme is developed herein to improve the robustness and operational efficiency. The proposed method involves designing a data fusion model for the KF that integrates data from an inertial navigation system (INS) and ultrawideband (UWB). Subsequently, the INS/UWB integrated model-based KF under CMN (cKF) will be derived. Then, two sub-cKFs are proposed to fuse the data in the east and north directions, respectively. The empirical findings highlight the superior performance of the proposed approach over the KF for position estimation accuracy and runtime reduction, demonstrating its effectiveness.

1. Introduction

Pedestrian positioning has extensive applications across diverse industries. In intelligent transportation, precise positioning can help urban planners better understand the flow of people, optimizing road design and traffic management [1]. In security monitoring, pedestrian positioning can help monitors respond to security incidents quicker and more accurately [2]. In healthcare, pedestrian positioning can help monitor patient activity, provide better care and management, and more [3,4]. However, with increasing diversity of application areas and the complexity of positioning environments, higher demands are being placed on pedestrian positioning technology. Higher positioning accuracy, faster response time, lower power consumption, and better ability to adapt to various environments are required [5,6]. Therefore, the development and innovation of pedestrian positioning technologies have become critical to meet the growing demands and promote the development and progress of various industries.

Many approaches have been proposed to achieve human localization. Among them, inertial navigation systems (INSs) have long been a cornerstone of navigation and localization, offering continuous position updates by integrating measurements from inertial sensors. However, the performance of an INS degrades over time due to sensor errors, drift, and noise accumulation [7,8]. Therefore, researchers have proposed integrating INSs with other sensors, such as global positioning system (GPS) receivers, to improve localization accuracy [9,10]. This paper investigates data fusion with an INS using ultrawideband (UWB). With technological advancements and large-scale production in recent years, the implementation cost of UWB technology has been gradually reduced. The low power consumption, high-precision positioning, and strong penetration ability are more prominent than in other positioning sensors. Therefore, this sensor is selected for data fusion [11,12,13,14]. This combination approach has critical applications in many fields [15,16]. For example, Pan and Jia et al. proposed a tight combination of a UWB single-base station and an INS for smart vehicle localization through the data fusion of the UWB single-base station distance and angle measurements for the narrow and long space environment [17]. In indoor localization, Kang and Duan et al. used UWB normal range measurements to correct the predicted position of the employed INS for improving the problem of non-line-of-sight interference affecting localization accuracy [18]. In hospitals, wheelchair or pedestrian high accuracy positioning and continuous detection, the user’s safety, and ensuring normal workflow are crucial. In hospital-oriented outdoor scenarios, complex environments, such as centralized high-rise buildings and trees, frequently lead to satellite signal loss, resulting in discontinuous positioning information. Therefore, the INS system is used for positioning in the case of satellite signal loss to ensure the continuous detection of pedestrians in hospitals [19,20,21,22]. Note that the Inertial Measurement Units (IMUs) typically operate at significantly higher sampling frequencies compared to GPS and/or UWB systems. This discrepancy makes the study of localization using UWB and IMU particularly relevant to the academic community [23].

The diversity of localization techniques also means that the use of data fusion filters is more demanding to improve localization accuracy. The Kalman filter (KF) is a widely used sensor fusion technique in integrated navigation systems. It combines noisy sensor measurements with a dynamic model of the system to estimate its true state [24,25,26]. Researchers have proposed many types of KFs for different models and application scenarios. To reduce the computational load of nonlinear navigation systems, Zhao introduced a hybrid method combining the cubature KF (CKF) and the extended KF (EKF). This approach employs a linear filtering method for estimating the linear state and a nonlinear filtering method for estimating the nonlinear state [27]. The hybrid filtering method retains the CKF’s ability to manage nonlinear problems while reducing the system’s computation time. In bearing-only tracking (BOT) systems, Sun and Davies et al. proposed the adaptive kernel KF (AKKF), which substantially reduces the number of particles and improves the performance of the algorithm compared to the unseasoned Kalman filter (UKF), particle filter (PF), and Gaussian particle filter (GPF) [28]. The creation of hybrid KF and neural network models is a considerable development in state estimation, and this hybrid model has high flexibility in its use, either alternatively or in a mixture [29]. The hybrid model can be divided into two types: the state–space model, where the KF neural network trains the equations or parameters, and the neural network model, whose parameters are updated using the KF [30,31]. This study shows that the hybrid model performs better in accuracy and flexibility than a single KF or neural network model [32,33]. The traditional KF assumes that the measurement noise is Gaussian white noise; however, when applied in real-world environments, it typically exhibits colored characteristics, meaning that the noise at one point correlates with the noise at adjacent points [34,35]. This outcome violates the white noise assumption and can lead to suboptimal fusion performance. To address this limitation, the KF filter is proposed to be improved for the colored measurement noise (CMN) model using the backward Euler (BE) method, and the effectiveness of this method for CMN removal is also demonstrated; however, the system operation speed is not considered [36].

In order to improve the localization accuracy and reduce the computational load, this paper proposes a unidirectional dual Kalman-based filter for the sensor localization problem under CMN by using matrix segmentation. First, the data from the sensor is processed, and the system is modeled. Then, the state vector is divided into two states in a single direction, and one KF is used to estimate the state in this direction. Another KF is used to estimate the state in the other direction while considering the effect of cross-covariance on these two subfilters. Through experimental validation in real-world environments, the results show that the proposed dual KF has better localization accuracy and stability under colored noise than traditional methods, and there is an improvement in the system operation speed.

This study has made notable progress in the following aspects:

• First, a single-direction equation of state and an observation equation are established based on the characteristics of inertial sensors and UWB measurements.
• Then, for the influence of the navigation environment on the positioning accuracy in different directions, a dual KF based on a single direction under CMN is proposed to effectively address the positioning issue in the presence of CMN.
• Finally, the feasibility and effectiveness of the proposed method in practical applications are proved through actual measurements, and a comparison is made regarding the operation speed of the system. The experimental results show that the algorithm proposed herein is valuable for improving the system running time.

The remaining sections of this paper are organized as follows: first, a model for the unidirectional INS/UWB-based navigation system is introduced. Then, the unidirectional dual KF-based method under CMN is described in detail. Next, experimental tests are conducted to compare the positioning accuracy and system runtime for KF, dual KF, and dual cKF. Finally, conclusions are given.

2. Dual KF based on a Single Direction under CMN

This section outlines the derivation of the dual KF under CMN, focusing on a single direction. The derivation is based on the model presented in Section 2. First, we will derive the improved data fusion model under CMN and its KF. Then, we split the filter into two subfilters according to different directions.

2.1. KF under CMN

In this article, we consider a system composed of Equations (1) and (2).

$${\mathbf{s}}_n=\mathbf{A}{\mathbf{s}}_{n-1}+{\mu }_n,$$

(1)

$${\mathbf{z}}_n={\mathbf{H}}_n{\mathbf{s}}_n+a{\eta }_{n-1}+{\mathbf{v}}_n,$$

(2)

where ${\mathbf{s}}_n$ is the state vector, n is the time index, $\mathbf{A}$ is system matrix, ${\mu }_n\sim \mathcal{N}(0,\mathbf{Q})$ is the system noise, and $\mathbf{Q}$ is its covariance, a is the color measurement noise factor, ${\eta }_{n-1}$ is the color measurement noise, and ${\mathbf{v}}_n\sim \mathcal{N}(0,\mathbf{R})$ is the measurement noise, and $\mathbf{R}$ is its covariance.

We reduce the effect of the CMN by introducing one new measurement ${\mathbf{y}}_n$, which can be computed using Equation (2).

$${\mathbf{y}}_n={\mathbf{z}}_n-a{\mathbf{z}}_{n-1},$$

(3)

With Equation (1), we build one new measurement equation, which is shown in Equation (2).

$${\mathbf{y}}_n=\left({\mathbf{H}}_n-{\mathbf{U}}_{\mathrm{n}}\right){\mathbf{s}}_n+{\mathbf{U}}_{\mathrm{n}}{\mu }_n+{\gamma }_n={\mathbf{D}}_n{\mathbf{s}}_n+{\overline{\gamma }}_n,$$

(4)

where ${\mathbf{U}}_{\mathrm{n}}={\displaystyle \frac{\mathrm{a}{\mathbf{H}}_n}{\mathbf{A}}}$, ${\mathbf{D}}_n={\mathbf{H}}_n-{\mathbf{U}}_{\mathrm{n}}$ is the observation matrix, and ${\overline{\gamma }}_n={\mathbf{U}}_{\mathrm{n}}{\mu }_n+{\gamma }_n$ is the measurement noise.

$${\overline{\mathbf{R}}}_n=E\left\{{\overline{\gamma }}_n{\overline{\gamma }}_n^T\right\}={\mathbf{U}}_n\mathbf{Q}{{\mathbf{U}}_n}^T+{\mathbf{R}}_n={\mathbf{U}}_n{\mathbf{\Phi }}_n+{\mathbf{R}}_n,$$

(5)

where ${\mathbf{\Phi }}_n=\mathbf{Q}{{\mathbf{U}}_n}^T$. Now, with the model (1) (3), we can list the method of the KF under CMN as Algorithm 1. With the new measurement Equation (3), we compute the new equation of state.

Algorithm 1: Kalman filter (KF) under CMN based on the model (27) (4) when ${\mu }_n$ and ${\overline{\gamma }}_n$ are correlated

Equation (1) can be rewritten as (6).

$$\begin{array}{ccc}{\mathbf{s}}_n& =& \mathbf{A}{\mathbf{s}}_{n-1}+{\mu }_n+{\beta }_n[{\mathbf{y}}_n-({\mathbf{D}}_n{\mathbf{s}}_n+{\overline{\gamma }}_n)]\hfill \\ & =& \underset{{\mathbf{F}}_n}{\underbrace{(\mathrm{I}-{\beta }_n{D}_n)\mathbf{A}}}{\mathbf{s}}_{n-1}+\underset{{\mathbf{w}}_n}{\underbrace{{\beta }_n{\mathbf{y}}_n}}+\underset{{\mathbf{v}}_n}{\underbrace{(\mathrm{I}-{\beta }_n{\mathbf{D}}_n){\mu }_n-{\beta }_n{\overline{\gamma }}_n}}\hfill \\ & =& {\mathbf{F}}_n{\mathbf{s}}_{n-1}+{\mathbf{w}}_n+{\mathbf{v}}_n\hfill \end{array},$$

(6)

where ${\mathbf{w}}_n$ is the input, and ${\mathbf{v}}_n\sim \mathcal{N}(0,{\mathbf{\Theta }}_n)$ is the new system noise, which can be computed using Equation (7). The algorithm for the KF under CMN is presented as Algorithm 2.

$$\begin{array}{ccc}{\mathbf{\Theta }}_n& =& E\left\{\left[\right(\mathbf{I}-{\beta }_n{\mathbf{D}}_n){\mu }_n-{\beta }_n{\overline{\gamma }}_n]{\left[\right(\mathbf{I}-{\beta }_n{\mathbf{D}}_n){\mu }_n-{\beta }_n{\overline{\gamma }}_n]}^T\right\}\hfill \\ & =& (\mathbf{I}-{\beta }_n{\mathbf{H}}_n)\mathbf{Q}(\mathbf{I}-{\beta }_n{\mathbf{H}}_n{)}^T+{\beta }_n\mathbf{R}{{\beta }_n}^T\hfill \end{array},$$

(7)

Algorithm 2: Kalman filter (KF) under CMN based on the model (27) (4) when ${\mu }_n$ and ${\overline{\gamma }}_n$ are not correlated

2.2. Dual KF under CMN Based on a Single Direction

This study employs the dual KF. For the direction, we rewrite the state vector as ${\mathbf{s}}_n={\left[\begin{array}{ccccc}\mathsf{\Delta }{x}_n& \mathsf{\Delta }v{x}_n& |& \mathsf{\Delta }{y}_n& \mathsf{\Delta }v{y}_n\end{array}\right]}^T={\left[\begin{array}{ccc}\mathbf{s}{\mathrm{x}}_n& |& \mathbf{s}{\mathrm{y}}_n\end{array}\right]}^T$, and we rewrite the following matrices: ${\mathbf{U}}_n=\left[\begin{array}{cc}\mathbf{U}{\mathrm{x}}_n& \mathbf{U}{y}_n\end{array}\right]$. Here, $\mathbf{U}{\mathrm{x}}_n=\left(a\mathbf{H}{x}_n\right)/\mathbf{A}{x}_{\mathrm{n}}$, $\mathbf{U}{\mathrm{y}}_n=\left(a\mathbf{H}{y}_n\right)/\mathbf{A}{y}_{\mathrm{n}}$, ${\mathbf{D}}_{\mathrm{n}}=\left[\begin{array}{cc}\mathbf{D}{x}_n& \mathbf{D}{y}_n\end{array}\right]$. Here, $\mathbf{D}{x}_n=\mathbf{H}{x}_n-\mathbf{U}{x}_n$, $\mathbf{D}{y}_n=\mathbf{H}{y}_n-\mathbf{U}{y}_n$, and ${\mathbf{\Phi }}_n={\left[\begin{array}{cc}\mathbf{\Phi }{\mathrm{x}}_n& \mathbf{\Phi }{y}_n\end{array}\right]}^T$. Here, $\mathbf{\Phi }{\mathrm{x}}_n=\mathbf{Q}{\mathrm{x}}_n\mathbf{U}{\mathrm{x}}_n^T$, $\mathbf{\Phi }{\mathrm{y}}_n=\mathbf{Q}{\mathrm{y}}_n\mathbf{U}{\mathrm{y}}_n^T$,

$${\mathbf{H}}_n=\left[\begin{array}{cccc}{\displaystyle \frac{\left(x_n^I-x_n^1\right)}{{\left({\left(x_n^I-x_n^1\right)}^2+{\left(y_n^I-y_n^1\right)}^2\right)}^{\frac{1}{2}}}}& 0& {\displaystyle \frac{\left(y_n^I-y_n^i\right)}{{\left({\left(x_n^I-x_n^1\right)}^2+{\left(y_n^I-y_n^1\right)}^2\right)}^{\frac{1}{2}}}}& 0\\ {\displaystyle \frac{\left(x_n^I-x_n^2\right)}{{\left({\left(x_n^I-x_n^2\right)}^2+{\left(y_n^I-y_n^2\right)}^2\right)}^{\frac{1}{2}}}}& 0& {\displaystyle \frac{\left(y_n^I-y_n^i\right)}{{\left({\left(x_n^I-x_n^2\right)}^2+{\left(y_n^I-y_n^2\right)}^2\right)}^{\frac{1}{2}}}}& 0\\ {\displaystyle \frac{\left(x_n^I-x_n^3\right)}{{\left({\left(x_n^I-x_n^3\right)}^2+{\left(y_n^I-y_n^3\right)}^2\right)}^{\frac{1}{2}}}}& 0& {\displaystyle \frac{\left(y_n^I-y_n^i\right)}{{\left({\left(x_n^I-x_n^3\right)}^2+{\left(y_n^I-y_n^3\right)}^2\right)}^{\frac{1}{2}}}}& 0\\ {\displaystyle \frac{\left(x_n^I-x_n^4\right)}{{\left({\left(x_n^I-x_n^4\right)}^2+{\left(y_n^I-y_n^4\right)}^2\right)}^{\frac{1}{2}}}}& 0& {\displaystyle \frac{\left(y_n^I-y_n^i\right)}{{\left({\left(x_n^I-x_n^i\right)}^2+{\left(y_n^I-y_n^i\right)}^2\right)}^{\frac{1}{2}}}}& 0\end{array}\right]=\left[\begin{array}{ccc}\mathbf{H}{x}_n& |& \mathbf{H}{y}_n\end{array}\right],$$

(8)

Thus, we can rewrite Equation (32) so that

$${\mathbf{z}}_n=\mathbf{H}{x}_n\mathbf{s}{\mathrm{x}}_n+\mathbf{H}{y}_n\mathbf{s}{\mathrm{y}}_n+a{\eta }_{n-1}+{\mathbf{v}}_n,$$

(9)

and ${\overline{\mathbf{R}}}_n$ can be computed as

$${\overline{\mathbf{R}}}_n=\mathbf{U}{x}_n\mathbf{\Phi }{x}_n+\mathbf{U}{y}_n\mathbf{\Phi }{y}_n+{\mathbf{R}}_n,$$

(10)

When ${\mathbf{v}}_n$ and ${\overline{\gamma }}_n$ is uncorrelated, we can obtain

$$\begin{array}{ccc}{\beta }_n& =& {\mathbf{\Phi }}_n{({\mathbf{H}}_n{\mathbf{\Phi }}_n+\mathbf{R})}^{-1}\hfill \\ & =& \left[\begin{array}{cc}\mathbf{\Phi }{\mathrm{x}}_n{(\mathbf{H}{x}_n\mathbf{\Phi }{x}_n+\mathbf{R})}^{-1}& \mathbf{0}\\ \mathbf{0}& \mathbf{\Phi }{\mathrm{x}}_n{(\mathbf{H}{x}_n\mathbf{\Phi }{x}_n+\mathbf{R})}^{-1}\end{array}\right]\hfill \\ & =& \left[\begin{array}{cc}\beta x_n& \mathbf{0}\\ \mathbf{0}& \beta y_n\end{array}\right]\hfill \end{array},$$

(11)

$$\begin{array}{ccc}{\mathbf{\Theta }}_n& =& \left[\begin{array}{cc}(\mathbf{I}-\beta {\mathrm{x}}_n\mathbf{H}{\mathrm{x}}_n)\mathbf{Q}\mathrm{x}{(\mathbf{I}-\beta {\mathrm{x}}_n\mathbf{H}{\mathrm{x}}_n)}^T& \mathbf{0}\\ \mathbf{0}& (\mathbf{I}-\beta {\mathrm{y}}_n\mathbf{H}{\mathrm{y}}_n)\mathbf{Q}\mathrm{y}{(\mathbf{I}-\beta {\mathrm{y}}_n\mathbf{H}{\mathrm{y}}_n)}^T\end{array}\right]\hfill \\ & =& \left[\begin{array}{cc}\mathbf{\Theta }{x}_n& 0\\ 0& \mathbf{\Theta }{y}_n\end{array}\right]\hfill \end{array},$$

(12)

where $\beta x_n=\mathbf{\Phi }{x}_n{(\mathbf{H}{x}_n\mathbf{\Phi }{x}_n+\mathbf{R})}^{-1}$, $\beta y_n=\mathbf{\Phi }{y}_n{(\mathbf{H}{y}_n\mathbf{\Phi }{y}_n+\mathbf{R})}^{-1}$,

$${\mathbf{F}}_n=\left[\begin{array}{cc}(\mathbf{I}-\beta x_n\mathbf{D}{x}_n)\mathbf{A}x& 0\\ 0& (\mathbf{I}-\beta y_n\mathbf{D}{y}_n)\mathbf{A}y\end{array}\right]=\left[\begin{array}{cc}\mathbf{F}{x}_n& 0\\ 0& \mathbf{F}{y}_n\end{array}\right],$$

(13)

In this work, we set ${\overline{\mathbf{K}}}_{\mathrm{n}}=\left[\begin{array}{c}\overline{\mathbf{K}}{x}_{\mathrm{n}}\\ \overline{\mathbf{K}}{y}_{\mathrm{n}}\end{array}\right]$, ${\widehat{\mathbf{P}}}_n=\left[\begin{array}{cc}\widehat{\mathbf{P}}{11}_n& \widehat{\mathbf{P}}{12}_n\\ \widehat{\mathbf{P}}{21}_n& \widehat{\mathbf{P}}{22}_n\end{array}\right]$, thus, we can compute the following equations.

$$\widehat{\mathbf{P}}{11}_n=\mathbf{F}{x}_n\widehat{\mathbf{P}}{11}_n\mathbf{F}{x}_n^T+\mathbf{\Theta }{x}_n,$$

(14)

$$\widehat{\mathbf{P}}{12}_{\mathrm{n}}=\mathbf{F}{x}_{\mathrm{n}}\widehat{\mathbf{P}}{12}_{\mathrm{n}}\mathbf{F}{y}_n^T,$$

(15)

$$\widehat{\mathbf{P}}{21}_n=\mathbf{F}{y}_n\widehat{\mathbf{P}}{21}_n\mathbf{F}{x}_n^T,$$

(16)

$$\widehat{\mathbf{P}}{22}_n=\mathbf{F}{y}_n\widehat{\mathbf{P}}{22}_n\mathbf{F}{y}_n^T+\mathbf{\Theta }{y}_n,$$

(17)

Based on the equations above, we can obtain the measurement update steps based on the direction, which are listed in Equations (18) and (19).

$$\mathbf{s}{x}_n=\mathbf{s}{x}_n+\overline{\mathbf{K}}{x}_n({\mathbf{y}}_n-\mathbf{D}{x}_n{x}_n-\mathbf{D}{y}_n{y}_n),$$

(18)

$$\mathbf{s}{y}_n=\mathbf{s}{y}_n+\overline{\mathbf{K}}{y}_n({\mathbf{y}}_n-\mathbf{D}{x}_n{x}_n-\mathbf{D}{y}_n{y}_n),$$

(19)

The KF’s gain under CMN can be computed using the formulas given in Equations (20) and (21).

$$\overline{\mathbf{K}}{x}_n=\left(\widehat{\mathbf{P}}{11}_n\mathbf{D}{x}_n^T+\widehat{\mathbf{P}}{12}_n\mathbf{D}{y}_n^T\right){\overline{\mathbf{G}}}_n^{-1},$$

(20)

$$\overline{\mathbf{K}}{\mathrm{y}}_n=\left(\widehat{\mathbf{P}}{21}_n\mathbf{D}{x}_n^T+\widehat{\mathbf{P}}{22}_n\mathbf{D}{y}_n^T\right){\overline{\mathbf{G}}}_n^{-1},$$

(21)

 where

$${\mathbf{G}}_n=\mathbf{D}{x}_n\widehat{\mathbf{P}}{11}_n\mathbf{D}{x}_n^T+\mathbf{D}{y}_n\widehat{\mathbf{P}}{21}_n\mathbf{D}{x}_n^T+\mathbf{D}{x}_n\widehat{\mathbf{P}}{12}_n\mathbf{D}{y}_n^T+\mathbf{D}{y}_n\widehat{\mathbf{P}}{22}_n\mathbf{D}{y}_n^T+\mathbf{U}{x}_n\mathbf{\Phi }{x}_n+\mathbf{U}{y}_n\mathbf{\Phi }{y}_n+\mathbf{R},$$

(22)

We can obtain the following equations:

$$\widehat{\mathbf{P}}{11}_n=\widehat{\mathbf{P}}{11}_n-(\overline{\mathbf{K}}{x}_n\mathbf{D}{x}_n\widehat{\mathbf{P}}{11}_n+\overline{\mathbf{K}}{x}_n\mathbf{D}{\mathrm{y}}_n\widehat{\mathbf{P}}{21}_n),$$

(23)

$$\widehat{\mathbf{P}}{12}_n=\widehat{\mathbf{P}}{12}_n-(\overline{\mathbf{K}}{x}_n\mathbf{D}{x}_n\widehat{\mathbf{P}}{12}_n+\overline{\mathbf{K}}{x}_n\mathbf{D}{y}_n\widehat{\mathbf{P}}{22}_n),$$

(24)

$$\widehat{\mathbf{P}}{21}_n=\widehat{\mathbf{P}}{21}_n-(\overline{\mathbf{K}}{y}_n\mathbf{D}{x}_n\widehat{\mathbf{P}}{11}_n+\overline{\mathbf{K}}{y}_n\mathbf{D}{y}_n\widehat{\mathbf{P}}{21}_n),$$

(25)

$$\widehat{\mathbf{P}}{22}_{\mathrm{n}}=\widehat{\mathbf{P}}{22}_{\mathrm{n}}-(\overline{\mathbf{K}}{y}_{\mathrm{n}}\mathbf{D}{x}_{\mathrm{n}}\widehat{\mathbf{P}}{12}_{\mathrm{n}}+\overline{\mathbf{K}}{y}_{\mathrm{n}}\mathbf{D}{y}_{\mathrm{n}}\widehat{\mathbf{P}}{22}_{\mathrm{n}}),$$

(26)

   The algorithm for the KF under CMN based on a single direction is presented as Algorithm 3. Figure 1 illustrates the configuration of the KF under CMN based on a single direction. The figure shows that in the proposed method, the state vector is split into two subfilters based on direction. Each subfilter estimates its own one-step prediction and measurement update with the distance difference between the INS and UWB. Additionally, the one-step prediction from one subfilter is used to assist with the measurement update of the other. Finally, the outputs of both subfilters are merged to calculate the final output.

Algorithm 3: Kalman filter (KF) under the colored measurement noise (CMN) based on a single direction.

3. UWB-Assisted INS-Based Human Localization Using a Dual KF Based on a Single Direction under CMN

This section focuses on using dual KFs for UWB-assisted INS human positioning under CMN. First, the scheme for the UWB-assisted INS-based human localization will be designed. Second, a data fusion model will be derived based on the characteristics of the measured data.

3.1. Scheme of the Dual KF under CMN

Herein, we employ UWB to assist with INS human localization. Figure 2 illustrates the configuration of the dual KF designed for INS-based integrated human localization under CMN based on a single direction. The foot-mounted inertial measurement unit (IMU) measures the human position, and the INS-based distances $r_n^{I,1,2,3,4}$ between the human and the UWB reference nodes (RNs) are computed. Furthermore, the UWB-based distances $r_n^{U,1,2,3,4}$ between the human and UWB RNs are computed. It should be pointed out that the IMUs cannot directly measure position, it uses acceleration and gyroscope data to calculate the velocity in the navigation frame and further calculate the position. Then, the difference of $\mathsf{\Delta }{r}_n^{I,1,2,3,4}=r_n^{I,1,2,3,4}-r_n^{U,1,2,3,4}$ is computed, serving as an input to the dual KF under CMN. The filter produces an error estimate of the INS position ${\mathbf{L}}_n^I$. Then, the difference of ${\mathbf{L}}_n^I-\mathsf{\Delta }{\widehat{\mathbf{L}}}_n^I$ is computed as the final output.

3.2. The Data Fusion Model for the Dual KF under CMN

This subsection focuses on deriving the data fusion model for the proposed dual KF under CMN. Equation (27) presents the state equation utilized in this study.

$$\underset{{\mathbf{s}}_n}{\underbrace{\left[\begin{array}{c}\mathsf{\Delta }{x}_n\\ \mathsf{\Delta }v{x}_n\\ \mathsf{\Delta }{y}_n\\ \mathsf{\Delta }v{y}_n\end{array}\right]}}=\underset{\mathbf{A}}{\underbrace{\left[\begin{array}{cc}\mathbf{A}x& \mathbf{0}\\ \mathbf{0}& \mathbf{A}y\end{array}\right]}}\underset{{\mathbf{s}}_{n-1}}{\underbrace{\left[\begin{array}{c}\mathsf{\Delta }{x}_{n-1}\\ \mathsf{\Delta }v{x}_{n-1}\\ \mathsf{\Delta }{y}_{n-1}\\ \mathsf{\Delta }v{y}_{n-1}\end{array}\right]}}+{\mu }_n,$$

(27)

$$\mathbf{A}x=\mathbf{A}y=\left[\begin{array}{cc}1& \mathsf{\Delta }n\\ 0& 1\end{array}\right],$$

(28)

where $(\mathsf{\Delta }{x}_n,\mathsf{\Delta }{y}_n)$ is the position error at the time index n, $(\mathsf{\Delta }v{x}_n,\mathsf{\Delta }v{y}_n)$ is the velocity error at the time index n, $\mathsf{\Delta }n$ is the sample time, ${\mu }_n\sim \mathcal{N}(0,{\mathbf{Q}}_n)$ is the system noise, and ${\mathbf{Q}}_n=\left[\begin{array}{cc}{\mathbf{Q}}_{\mathsf{\Delta }x,n}& \mathbf{0}\\ \mathbf{0}& {\mathbf{Q}}_{\mathsf{\Delta }y,n}\end{array}\right]$ is its covariance.

For the measurement equation, the distance $r_n^{I,i},i\in [1,4]$ measured by the INS can be computed using Equation (29).

$$r_n^{I,i}={\left({\left(x_n^I-x_n^i\right)}^2+{\left(y_n^I-y_n^i\right)}^2\right)}^{{\displaystyle \frac{1}{2}}},i\in \left[1,4\right],$$

(29)

where $(x_n^I,y_n^I)$ is the position measured by the INS at the time index n, and $(x_n^i,y_n^i),i\in [1,4]$ is the $i^{th}$ UWB RN’s position. Perform a first-order Taylor expansion on the above equation to obtain

$$r_n^{I,i}=r_n^{R,i}+{\displaystyle \frac{\left(x_n^I-x_n^i\right)}{{\left({\left(x_n^I-x_n^i\right)}^2+{\left(y_n^I-y_n^i\right)}^2\right)}^{\frac{1}{2}}}}\mathsf{\Delta }{x}_n+{\displaystyle \frac{\left(y_n^I-y_n^i\right)}{{\left({\left(x_n^I-x_n^i\right)}^2+{\left(y_n^I-y_n^i\right)}^2\right)}^{\frac{1}{2}}}}\mathsf{\Delta }{y}_n,i\in \left[1,4\right],$$

(30)

where $r_n^{R,i}$ is the real position at the time index n. Then, we can obtain the following equation:

$$r_n^{I,i}-r_n^{U,i}={\displaystyle \frac{\left(x_n^I-x_n^i\right)}{{\left({\left(x_n^I-x_n^i\right)}^2+{\left(y_n^I-y_n^i\right)}^2\right)}^{\frac{1}{2}}}}\mathsf{\Delta }{x}_n+{\displaystyle \frac{\left(y_n^I-y_n^i\right)}{{\left({\left(x_n^I-x_n^i\right)}^2+{\left(y_n^I-y_n^i\right)}^2\right)}^{\frac{1}{2}}}}\mathsf{\Delta }{y}_n+{\eta }_n^i,i\in \left[1,4\right],$$

(31)

where ${\eta }_n^i$ represents the CMN for the $i^{th}$ distance at time index n. It can be expressed by ${\eta }_n^i=a^i{\eta }_{n-1}^i+v_n^i$. Here, $v_n^i\sim \mathcal{N}(0,\mathbf{R})$ is the Gaussian noise. Thus, we obtained the measurement equation employed in this study.

$$\begin{array}{c}\underset{{\mathbf{z}}_n}{\underbrace{\left[\begin{array}{c}{r}_n^{I,1}-r_n^{U,1}\\ r_n^{I,2}-r_n^{U,2}\\ r_n^{I,3}-r_n^{U,3}\\ r_n^{I,4}-r_n^{U,4}\end{array}\right]}}=\underset{{\mathbf{H}}_n}{\underbrace{\left[\begin{array}{cccc}{\displaystyle \frac{\left(x_n^I-x_n^1\right)}{{\left({\left(x_n^I-x_n^1\right)}^2+{\left(y_n^I-y_n^1\right)}^2\right)}^{\frac{1}{2}}}}& 0& {\displaystyle \frac{\left(y_n^I-y_n^i\right)}{{\left({\left(x_n^I-x_n^1\right)}^2+{\left(y_n^I-y_n^1\right)}^2\right)}^{\frac{1}{2}}}}& 0\\ {\displaystyle \frac{\left(x_n^I-x_n^2\right)}{{\left({\left(x_n^I-x_n^2\right)}^2+{\left(y_n^I-y_n^2\right)}^2\right)}^{\frac{1}{2}}}}& 0& {\displaystyle \frac{\left(y_n^I-y_n^i\right)}{{\left({\left(x_n^I-x_n^2\right)}^2+{\left(y_n^I-y_n^2\right)}^2\right)}^{\frac{1}{2}}}}& 0\\ {\displaystyle \frac{\left(x_n^I-x_n^3\right)}{{\left({\left(x_n^I-x_n^3\right)}^2+{\left(y_n^I-y_n^3\right)}^2\right)}^{\frac{1}{2}}}}& 0& {\displaystyle \frac{\left(y_n^I-y_n^i\right)}{{\left({\left(x_n^I-x_n^3\right)}^2+{\left(y_n^I-y_n^3\right)}^2\right)}^{\frac{1}{2}}}}& 0\\ {\displaystyle \frac{\left(x_n^I-x_n^4\right)}{{\left({\left(x_n^I-x_n^4\right)}^2+{\left(y_n^I-y_n^4\right)}^2\right)}^{\frac{1}{2}}}}& 0& {\displaystyle \frac{\left(y_n^I-y_n^i\right)}{{\left({\left(x_n^I-x_n^i\right)}^2+{\left(y_n^I-y_n^i\right)}^2\right)}^{\frac{1}{2}}}}& 0\end{array}\right]}}\underset{{\mathbf{s}}_n}{\underbrace{\left[\begin{array}{c}\mathsf{\Delta }{x}_n\\ \mathsf{\Delta }v{x}_n\\ \mathsf{\Delta }{y}_n\\ \mathsf{\Delta }v{y}_n\end{array}\right]}}\hfill \\ +a\underset{{\eta }_{n-1}}{\underbrace{\left[\begin{array}{c}{\eta }_{n-1}^1\\ {\eta }_{n-1}^2\\ {\eta }_{n-1}^3\\ {\eta }_{n-1}^4\end{array}\right]}}+\underset{{\mathbf{v}}_n}{\underbrace{\left[\begin{array}{c}{v}_n^1\\ v_n^2\\ v_n^3\\ v_n^4\end{array}\right]}},\hfill \end{array},$$

(32)

4. Discussion

In this section, a self-built experiment is conducted to analyze the localization accuracy and algorithm runtime of the proposed algorithm. First, the initial stage of the experiment will be introduced. Then, the proposed dual KF based on a single direction under CMN will be investigated for localization performance.

4.1. Initial Experimental Stage

In this study, the real-world test is conducted near the University of Jinan’s No. 1 building, as depicted in Figure 3. The figure shows that we employ four UWB RNs placed around the experimental site. The target pedestrian is equipped with a foot-mounted IMU, real-time kinematic (RTK), UWB blind node (BN), and one computer. In order to obtain the 2D human’s position, the layout of UWB base stations needs to ensure that the target pedestrian can obtain the distance of at least three UWB stations while walking; the parameter of the UWB localization system used in this work is listed in

Table 1. The parameters of the UWB localization system used in the test.

Parameter	Value
Data availability frequency	1 s
Distance error	0.25 m
Localization error	0.35 m

. In this article, we ensure that the distance information from all the UWB stations can be obtained during pedestrian walking. Here, the foot-mounted IMU measures the ${\mathbf{r}}_n^{I,1,2,3,4}$, the UWB BN measures the ${\mathbf{r}}_n^{U,1,2,3,4}$, and the RTK is the reference standard for the experiment; its parameter is listed in

Table 2. The parameters of the R-T-K used in the test.

Parameter	Value
Data availability frequency	1 s
R-T-K accuracy (plane)	$\pm \left(8+1\times {10}^6\times D\right)\mathrm{mm}$
R-T-K accuracy (height)	$\pm \left(15+1\times {10}^6\times D\right)\mathrm{mm}$
size	119 mm × 119 mm × 85 mm
Weigh	0.73 kg
Operation temperature	−45 ℃ ∼ +75 ℃

. All sensor data are input to the computer. The test structure is shown in Figure 4. Note that the inertial sensors need to be calibrated before the test, including static and dynamic calibration. Static calibration is performed without any movement by recording the output offset value of the sensor and correcting it to the zero position. Dynamic calibration criteria are used to calibrate sensors during specific actions or movements. By performing actions or movements under known conditions and comparing the sensor output, the offset error of the sensor can be accurately calculated and corrected. In this work, we use the software provided by the sensor to compensate before the experiment begins directly.

4.2. Localization Accuracy Investigation

In this subsection, we performed two real tests using the test system proposed in the previous section. This study employs the KF and dual KF as the comparison filters.

Figure 5 shows the reference path and those measured using the INS + zero velocity update (ZUPT), UWB, KF, dual KF, and the dual cKF in Test 1. In this figure, we employ the black dashed line to denote the reference path, the gray solid line is used to denote the INS with ZUPT method, the green solid line is used to denote the UWB-only solution, the cyan dashed line is employed to denote the path measured by the KF, the blue dashed line represents the trajectory obtained from the dual KF, while the proposed dual KF based on a single direction under CMN (denoted as dual cKF) is depicted by the red dashed line. Meanwhile, we use pink to show the positions of the UWB RNs, and the start and finish points are also shown in this figure.

From this figure, we can easily find that while ZUPT effectively reduces position drift errors, there is a noticeable error accumulation over time. The UWB system demonstrates stability in determining the target human’s position, as observed in its solution when compared to INS. All the filters’ solutions can provide trajectories similar to the reference trajectory, which are better than the UWB’s solution. The solutions of the KF and dual KF methods are similar to those in Test 1. The proposed dual cKF method’s path can provide a stable solution.

Figure 6 and Figure 7 display the reference value and the position in the east and north directions measured by the INS + ZUPT, UWB, KF, dual KF, and the dual cKF in Test 1. In this figure, we employ the black dashed line to denote the reference path, the gray solid line is used to denote the INS with the ZUPT method, the green dashed line is used to denote the UWB-only solution, the cyan solid line is employed to denote the position measured by the KF, the solid blue line represents the trajectory derived from the dual KF method, while the proposed dual cKF method is indicated by the solid red line. From the Figure 6, we can see that the difference between the value of the INS+ZUPT and the reference value is significant. When compared with the INS+ZUPT’s solution, the UWB and three filter solutions are closer to the reference value. Here, the Dual KF’s position is similar to the KF, and its value is slightly closer to the reference value, which shows the effectiveness of the dual filter model in reducing the localization error. Compared with the dual KF, the dual cKF’s curve is smoother. Although the values of the cKF algorithm are far from the reference value compared to the KF algorithm in some time periods, most of the time, its estimated values are closer to the reference value than KF and dual KF algorithms, which shows the effectiveness. In the north direction, from Figure 7, we can see that dual filter methods’ solutions are closer to the reference value, and the proposed method can provide a closer solution than the other methods, demonstrating the efficacy of the proposed approach.

Figure 8 shows the UWB distance errors and the position errors of the methods in Test 1. Here, the distance $i,i\in [1,4]$ means the distance error between the target human and the ith UWB RN. From the figure, we can infer that the proposed method has the best performance. Figure 9 shows the position root-mean-square errors (RMSEs) in the east and north directions measured by UWB, the KF, the dual KF, and the dual cKF in Test 1. In this figure, we employ the green solid line to denote the UWB solution’s RMSEs, the cyan solid line is employed to denote the position measured by the dual KF, the solution obtained from the KF method is represented by the solid blue line, while the proposed dual cKF method is depicted by the solid red line.

Figure 9a shows that all the filters are effective in reducing the localization error when compared with the UWB’s solution in the east direction. When compared with the KF, the dual KF can further improve the localization error, which shows the dual model’s effectiveness. To the north direction, from Figure 9b, we can see that the KF’s position RMSE in the north direction is smaller than that of the UWB at the last time index. The dual KF provides the north position with a smaller RMSE than the KF method does at the last time index. The proposed dual cKF method’s north position RMSE is the smallest at the last time index, highlighting the effectiveness of the proposed approach.

Figure 10 illustrates the cumulative distribution function (CDF) of position error measured by the UWB, KF, dual KF, and dual cKF in Test 1. In this figure, we employ the black solid line to denote the UWB solution’s position error, the blue solid line is employed to denote the position measured by the dual KF, the solid green line represents the trajectory derived from the KF method, while the proposed dual cKF method is indicated by the solid red line. This figure illustrates that the proposed method achieves the smallest position error compared to other methods at the 0.9 point. This result underscores the effectiveness of the proposed approach in reducing localization errors.

Table 3. Root-mean-square errors (RMSEs) of the INS, UWB, KF, dual KF, and dual cKF in Test 1.

Methods	East Direction (m)	North Direction (m)	Mean (m)
INS	0.207	0.981	0.153
UWB	0.220	0.149	0.185
KF	0.163	0.144	0.154
dual KF	0.149	0.130	0.140
dual cKF	0.143	0.119	0.131

shows the RMSEs of the INS, UWB, KF, dual KF, and the dual cKF in Test 1. From the table, we can see that the UWB’s solution has better localization performance, and its mean localization error is 0.185 m. Both the KF and dual KF methods decrease the localization error from 0.185 m to 0.154 m and 0.140 m, respectively. In contrast, the proposed dual cKF achieves a mean localization error of 0.131 m, representing a reduction in localization error of approximately 29.19% compared to the UWB solution.

Table 4. The average running time of the KF, dual KF, and dual cKF in Test 1.

Methods	Running Time (ms)
KF	0.16
Sub-KF 1	0.06
Sub-KF 2	0.05
Sub-cKF 1	0.15
Sub-cKF 2	0.09

shows the average running time of the KF, dual KF, and dual cKF in Test 1. Note that the sub-KF 1 and 2 are the subfilters of the dual KF, and the sub-cKF 1 and 2 are the subfilters of the dual cKF. The table shows that the average running time of the subfilters of the dual KF and dual cKF is lower than that of the KF method. We must emphasize that in actual operation, the two subfilters of the dual KF and dual cKF run in parallel; therefore, the running time of the dual KF and dual cKF can take the larger of the two subfilters. Therefore, the proposed method effectively reduces the running time.

In Test 2, the reference path and those for the INS + ZUPT, UWB, KF, dual KF, and dual cKF measurements are shown in Figure 11. In this figure, we employ the black dashed line to denote the reference path, the gray solid line is used to denote the INS with the ZUPT method, the green solid line is used to denote the UWB-only solution, the cyan dashed line is employed to denote the path measured by the KF, the trajectory from the dual KF method is shown as the blue dashed line, and the proposed dual cKF method is depicted by the red dashed line. Meanwhile, we use pink to show the positions of the UWB RNs, and the start and finish points are also shown.

Similar to Test 1, the figures show a noticeable accumulation of errors in the INS’s solution. All the filters’ and the UWB’s solutions can provide positions similar to the reference value. Compared with other methods, the proposed dual cKF method can provide a solution closer to the reference path, demonstrating the effectiveness of this approach.

Figure 12 and Figure 13 show the positions of the INS + ZUPT, UWB, KF, dual KF, and dual cKF measured in the east and north directions in Test 2 and the reference values. The figures show different trajectories in Test 2. Figure 14 shows the UWB distance errors and the position errors of the methods in Test 2. From the figure, we can infer that the proposed method has the best performance. The black dashed line indicates the reference path, while the gray solid line shows the INS with the ZUPT method, the green dashed line represents the UWB-only solution, the cyan solid line depicts the position estimated by the KF, the blue solid line illustrates the trajectory from the dual KF method, and the red solid line represents the trajectory from the proposed dual cKF method. From Figure 12, we can see that the INS’s east position has some big errors from the time index of 100 to 250 and from the time index of 500 to 600. Moreover, all the filters’ solutions can provide positions similar to the reference value, which is better than the UWB’s solution. Compared to the other methods, the proposed dual cKF method demonstrates its effectiveness by providing a solution that closely matches the reference path. From Figure 13, we can see that all the filters’ solutions can provide positions similar to the reference value, which is better than the UWB’s solution. Compared to the other methods, the proposed dual cKF method demonstrates its effectiveness by providing a solution that closely matches the reference path.

Figure 15 shows the position RMSEs in the east and north directions for UWB, the KF, the dual KF, and the dual cKF measured in Test 2. The green solid line represents the RMSEs of the UWB solution, the cyan solid line indicates the position measured by the dual KF, the solution derived from the KF method is shown by the blue solid line, and the proposed dual cKF method is depicted by the red solid line. From the figures, we can see that the KF’s position RMSE in the east direction is smaller than the UWB’s value at the last time index. The dual KF provides the east position with a smaller RMSE than the KF method does at the last time index. The proposed dual cKF method’s east position RMSE is the smallest at the last time index, demonstrating the efficacy of the proposed method. In the north direction, all the filter position RMSEs are similar but smaller than the UWB solutions. The proposed dual cKF method has the smallest localization error, although its value is slightly lower than that of the KF and dual KF.

Figure 16 shows the position error CDF measured by UWB, the KF, the dual KF, and the dual cKF in Test 2. The black solid line denotes the UWB solution’s position error, the blue solid line indicates the position measured by the dual KF, the solution derived from the KF method is represented by the solid green line, while the proposed dual cKF method is indicated by the solid red line. From this figure, it is evident that the proposed method exhibits the smallest position error compared to the other methods at the 0.9 point, indicating its effectiveness in reducing localization errors. However, its value is only slightly lower than the KF and dual KF.

Table 5. RMSEs of the INS, UWB, KF, dual KF, and dual cKF in Test 2.

Methods	East Direction (m)	North Direction (m)	Mean (m)
INS	0.581	0.681	0.631
UWB	0.290	0.237	0.264
KF	0.283	0.212	0.248
dual KF	0.281	0.211	0.246
dual cKF	0.274	0.210	0.242

shows the RMSEs of the INS, UWB, KF, dual KF, and dual cKF in Test 2. The table shows that the UWB’s solution has better localization performance, with a mean localization error of 0.264 m. The localization error was reduced from 0.264 m to 0.248 m and 0.246 m when using the KF and dual KF methods, respectively. The proposed dual cKF has a mean localization error of 0.242 m, the smallest of all filters.

Table 6. Average running time of the KF, dual KF, and dual cKF in Test 2.

Methods	Running Time (ms)
KF	0.10
Sub-KF 1	0.04
Sub-KF 2	0.04
Sub-cKF 1	0.08
Sub-cKF 2	0.06

lists the average running times of the KF, dual KF, and dual cKF. The table shows that the average running time of the dual KF and dual cKF subfilters is lower than that of the KF method. We must emphasize that in an actual operation, the two subfilters of the dual KF and dual cKF run in parallel; therefore, the running time of the dual KF and dual cKF can take the larger of the two subfilters. Therefore, the proposed method effectively reduces the running time.

5. Conclusions

This study addresses the issue of varying positioning accuracy in different directions due to environmental factors in inertial-based integrated pedestrian navigation by introducing a dual KF based on a single direction under a CMN scheme. In the proposed method, the data fusion model for the KF fusing the data of INS and UWB was designed initially. Subsequently, the dual INS/UWB integrated model-based cKF was designed, which employed two sub-cKFs to fuse the data independently in the east and north directions. Then, the real test was employed to verify the proposed method’s performance. The empirical results indicate that the proposed approach outperforms the KF in terms of position estimation accuracy and runtime reduction, highlighting its effectiveness.