Maximum Correntropy Criterion Kalman/Allan Variance-Assisted FIR Integrated Filter for Indoor Localization

Abstract

To obtain more accurate information on using an inertial navigation system (INS)-based integrated localization system, an integrated filter with maximum correntropy criterion Kalman filter (mccKF) and finite impulse response (FIR) is proposed for the fusion of INS-based multisource sensor data in this work. In the realm of medical applications, precise localization is crucial for various aspects, such as tracking the movement of a medical instrument within the human body or monitoring its position in the human body during procedures. This study uses ultra-wideband (UWB) technology to rectify the position errors of the INS. In this method, the difference between the positions of the INS and UWB is used as the measurement of the filter. The main data fusion filter in this study is the mccKF, which utilizes the maximum correntropy criterion (mcc) method to enhance the robustness of the Kalman filter (KF). This filter is used for fusing data from multiple sources, including the INS. Moreover, we use the Mahalanobis distance to verify the performance of the mccKF. If the performance of the mccKF is lower than the preset threshold, the Allan Variance-assisted FIR filter is used to replace the mccKF, which is designed in this work. This adaptive approach ensures the resilience of the system in demanding medical environments. Two practical experiments were performed to evaluate the effectiveness of the proposed approach. The findings indicate that the mccKF/FIR integrated method reduces the localization error by approximately 32.43% and 37.5% compared with the KF and mccKF, respectively. These results highlight the effectiveness of the proposed approach.

1. Introduction

Nowadays, the inertial navigation system (INS)-based measurement is widely used in several fields [1]. For instance, in the research performed by [2], a design integrating multisensor fusion was suggested. This design includes an INS, a global navigation satellite system (GNSS), and light detection and ranging (LiDAR) for implementing 3D simultaneous localization and mapping (SLAM; INS/GNSS/3D LiDAR-SLAM). Furthermore, the research performed in [3] delved into the utilization of GNSSs/INSs for assisted imaging systems in uncrewed aerial vehicles (UAVs). In addition, a method for analyzing gait in individuals with Parkinson’s disease using wearable technology based on the INS was designed to evaluate motor ability in [4]. Notably, INS-based measurement technology is gradually becoming a research hotspot.

In INS-based localization, reducing the position cumulative error of the INS is the most important issue that needs to be addressed [5]. Several approaches have been proposed to overcome this problem based on two aspects: integrated navigation methods and data fusion filters. The integrated navigation method has several approaches. For instance, in [6], a GNSS/INS integrated navigation system is supported by a back propagation (BP) neural network with motion constraints. This approach offers supplementary information to address the issue of rapid divergence in the positioning error of integrated navigation systems during GNSS outages. A new robust adaptive method, utilizing a tight real-time kinematic (RTK)/INS architecture, was proposed in [7]. It enhances the robust estimation method by considering two factors, including ambiguity variance, for RTK-related observations. To overcome the problem of inaccuracy in lateral velocity constraint parameters, a navigation method based on a BP neural network for an integrated GNSS/INS/odometer (OD) with nonholonomic constraints (NHCs) has been proposed [8], considering the state of vehicle motion. In the work by the authors of [9], the utilization of an adaptive model set aims to increase the effectiveness of an integrated navigation system compared to an INS with Doppler velocity log in challenging measurement environments. In [10], a microelectromechanical system (MEMS)–INS-based ultra-wideband (UWB) integrated method for indoor navigation was designed. Notably, the principle of the abovementioned examples is to employ different navigation and positioning technologies to continuously correct the calculation errors in INS navigation.

Based on the INS integrated navigation scheme, a data fusion filter can further improve the localization accuracy [11]. Meanwhile, several approaches have been proposed to deal with the data fusion problem. For example, in [12], one robust Kalman filter (KF) was proposed for loosely coupled global positioning system (GPS)/INS integration systems. A dual-rate KF has been devised for integrating time-differenced GPS measurements with INS data [13]. For nonlinear cases, the extended KF (EKF) was considered in [14]. In [15], the EKF was proposed for INS/GPS integrated navigation. Meanwhile, a locally weighted linear regression (LWLR)/long short-term memory (LSTM)-assisted EKF was designed for the integrated navigation of an INS/celestial navigation system [16]. Notably, Kalman-based approaches for data fusion have difficulty in overcoming the disadvantage of accurately capturing environmental noise. To overcome this problem, the finite impulse response (FIR) filter was developed in [17]. The FIR filter only uses the size of the data fusion window and state vector. Alternatively, the FIR filter only requires the size of the data fusion window because the data fusion model is already built. Simultaneously, several approaches have been suggested. For example, in [18], the extended FIR filter was introduced for the fusion of recent INS and UWB measurements. One self-tuning FIR filter has been proposed for processes with unknown measurement noise covariance [19]. Notably, the calculation of the size of the optimal data fusion window is considerably important.

Nowadays, both the mccKF and the FIR have been widely used in localization. However, is should be pointed out that there are many shortcomings. For mccKF, this method can improve the robustness; however, the matrix in the estimation process is prone to singularity, which can cause the mccKF to not work. Moreover, the accuracy of the noise will also affect the localization accuracy. The FIR filter has better robustness; however, if the noise of the filter affects the accuracy, we have to say that its performance is worse than that of the KF method. Thus, we propose an mccKF/FIR filtering approach; when the KF method cannot provide a stable solution, the FIR can. To obtain more accurate position information using the INS-based integrated localization system, a maximum correntropy criterion KF (mccKF)/FIR) integrated filter is proposed for the fusion of INS-based multisource sensor data in this work. Herein, UWB is used to correct the position error of the INS. In this method, the difference between the positions of the INS and UWB is used as the measurement of the filter. The main data fusion filter in this context is mccKF, which uses the mcc method to improve the robustness of the KF. Its application involves the fusion of INS-based multisource sensory data. Moreover, we use the Mahalanobis distance to verify the performance of the mccKF. If the performance of the mccKF is below the preset threshold, an Allan Variance-assisted FIR filter is used to replace the mccKF, which employs the Allan Variance method to improve the noise accuracy. Two practical tests were performed to evaluate the effectiveness of the proposed methodology. The results of these experiments verify the efficacy of the proposed strategy.

The remaining sections of this paper are organized as follows: Section 2 discusses the principle of indoor integrated localization system by fusing the INS and UWB. Section 3 presents the design of an mccKF/FIR integrated filter for INS-based integrated navigation. The analysis of the experimental tests is summarized in Section 4, and the conclusions are provided in Section 5.

2. Indoor Integrated Localization by Fusing INS and UWB

This subsection discusses the design of the indoor integrated localization scheme by combining INS and UWB data. Furthermore, we briefly introduce the state and measurement equations used by the proposed filter.

2.1. Indoor Integrated Localization Scheme

Figure 1 illustrates the indoor integrated localization scheme, which combines INS and UWB data. The figure shows that the integrated localization scheme uses UWB and the INS to obtain the position of UWB- and INS-based target drones ${\mathbf{P}}_k^{UWB}$ and ${\mathbf{P}}_k^{INS}$ at time index k, respectively. Subsequently, the difference $\delta {\widehat{\mathbf{P}}}_k^{INS}={\mathbf{P}}_k^{INS}-{\mathbf{P}}_k^{UWB}$ between ${\mathbf{P}}_k^{UWB}$ and ${\mathbf{P}}_k^{INS}$ is used as the measurement of the mccKF/FIR integrated filter, which will be discussed in detail in the subsequent section. Meanwhile, the sampling time of the integrated localization scheme is used as the measurement of the mccKF/FIR integrated filter. With these measurements, the mccKF/FIR integrated filter outputs the estimation of the position $\delta {\mathbf{P}}_k^{INS}$, and this value is used to correct the ${\mathbf{P}}_k^{INS}$.

2.2. State and Measurement Equations

Expanding on the integrated scheme described in Section 2.1, the state and measurement equations of the mccKF/FIR integrated filter are presented. The state equation used herein is presented as Equation (1).

$$\underset{\delta {\mathbf{s}}_k^{-}}{\underbrace{\left[\begin{array}{c}\delta {\mathbf{s}}_{E,k}^{-}\\ \delta {\mathbf{s}}_{N,k}^{-}\\ \delta {\mathbf{s}}_{U,k}^{-}\\ \Delta k_k^{-}\end{array}\right]}}=\underset{\mathbf{F}}{\underbrace{\left[\begin{array}{cccc}{\mathbf{F}}_E& \mathbf{0}& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& {\mathbf{F}}_N& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& \mathbf{0}& {\mathbf{F}}_U& \mathbf{0}\\ \mathbf{0}& \mathbf{0}& \mathbf{0}& 1\end{array}\right]}}\underset{\delta {\mathbf{s}}_{k-1}}{\underbrace{\left[\begin{array}{c}\delta {\mathbf{s}}_{E,k-1}\\ \delta {\mathbf{s}}_{N,k-1}\\ \delta {\mathbf{s}}_{U,k-1}\\ \Delta k_k\end{array}\right]}}+{\varpi }_k,$$

(1)

where $\delta {\mathbf{s}}_{E,k}^{-}={\left[\begin{array}{cc}\delta P_{E,k}^{-}& \delta V_{E,k}^{-}\end{array}\right]}^T$, $\delta {\mathbf{s}}_{N,k}^{-}={\left[\begin{array}{cc}\delta P_{N,k}^{-}& \delta V_{N,k}^{-}\end{array}\right]}^T$, $\delta {\mathbf{s}}_{U,k}^{-}={\left[\begin{array}{cc}\delta P_{U,k}^{-}& \delta V_{U,k}^{-}\end{array}\right]}^T$, $\delta {\mathbf{s}}_{E,k}={\left[\begin{array}{cc}\delta P_{E,k}^{-}& \delta V_{E,k}^{-}\end{array}\right]}^T$, $\delta {\mathbf{s}}_{N,k}={\left[\begin{array}{cc}\delta P_{N,k}& \delta V_{N,k}\end{array}\right]}^T$, $\delta {\mathbf{s}}_{U,k}={\left[\begin{array}{cc}\delta P_{U,k}& \delta V_{U,k}\end{array}\right]}^T$, and ${\varpi }_k\sim \mathcal{N}(0,{\mathbf{Q}}_k)$ represents the system noise, and ${\mathbf{F}}_E$, ${\mathbf{F}}_N$, and ${\mathbf{F}}_U$ can be defined as follows:

$${\mathbf{F}}_E={\mathbf{F}}_N={\mathbf{F}}_U=\left[\begin{array}{cc}1& \Delta k\\ 0& 1\end{array}\right],$$

(2)

where $\Delta k$ represents the sampling time. The measurement equation utilized herein can be expressed as follows:

$${\mathbf{y}}_k=\left[\begin{array}{c}{P}_{\mathrm{E},\mathrm{k}}^I-P_{\mathrm{E},\mathrm{k}}^U\\ P_{\mathrm{N},\mathrm{k}}^I-P_{N,k}^U\\ P_{\mathrm{U},\mathrm{k}}^I-P_{\mathrm{U},\mathrm{k}}^U\\ \Delta \widehat{k}\end{array}\right]=\underset{\mathbf{H}}{\underbrace{\left[\begin{array}{ccccccc}1& 0& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 0& 1\end{array}\right]}}\delta {\mathbf{s}}_k^{-}+{\upsilon }_k,$$

(3)

where $(P_{E,k}^{INS},P_{N,k}^{INS},P_{U,k}^{INS})$ represents the position of the INS-based target drone at time index k, ${(P_{E,k}^{UWB},P_{N,k}^{UWB},P_{U,k})}^{UWB}$ represents the position of the UWB-based target drone at time index k, and ${\upsilon }_k\sim \mathcal{N}(0,{\mathbf{R}}_k)$ represents the measurement noise.

3. mccKF/FIR Integrated Filter

In this section, the mccKF/FIR integrated filter mentioned in Section 2.1 is presented. First, we provide a brief introduction to the mccKF method. Subsequently, the FIR filter based on the state and measurement equations mentioned in Section 2.2 is constructed. Finally, the mccKF/FIR integrated filter is proposed.

3.1. Maximum Correntropy Criterion

Correntropy quantifies the resemblance between two random variables $A,B\in \mathbb{R}$:

$$C\left(\mathrm{A},\mathrm{B}\right)=E\left[\alpha \left(\mathrm{A},\mathrm{B}\right)\right]=\int {\alpha \left(\mathrm{A},\mathrm{B}\right)df}_{AB}\left(\mathrm{A},\mathrm{B}\right),$$

(4)

where $f_{AB}\left(\mathrm{A},\mathrm{B}\right)$ represents the joint distribution function. And $\alpha \left(\mathrm{A},\mathrm{B}\right)$ represents its kernel, which is defined as follows:

$$\alpha \left(\mathrm{A},\mathrm{B}\right)=G_{\sigma }\left(A-B\right)=exp\left(-{\displaystyle \frac{{\left(A-B\right)}^2}{\sigma }}\right),$$

(5)

Notably, obtaining $f_{AB}\left(\mathrm{A},\mathrm{B}\right)$ is challenging. Therefore, we can estimate the correntropy using the following equation:

$$\widehat{C}\left(\mathrm{A},\mathrm{B}\right)={\displaystyle \frac{1}{M}}\sum _{g=1}^M{G}_{\sigma }\left(\delta e\left(g\right)\right),$$

(6)

where $\delta e\left(g\right)=A\left(g\right)-B\left(g\right)$. In this work, we set $\delta e\left(g\right),g\in [1,M]$ is available, and the mcc cost function can be written as follows:

$$maxJ=max{\displaystyle \frac{1}{M}}\sum _{g=1}^M{G}_{\sigma }\left(\delta e\left(g\right)\right),$$

(7)

3.2. mccKF Filter

With model (1) and (2), the data fusion filter, with the KF being the most famous example, has the potential to enhance localization accuracy. Based on model (1) and (2), the KF method initially uses one-step prediction, which can be written as follows:

$$\delta {\mathbf{s}}_k^{KF-}=\mathbf{F}\delta {\mathbf{s}}_{k-1}^{KF}+{{\varpi }_k}^{KF},$$

(8)

$${\mathbf{P}}_k^{KF-}=\mathbf{F}{\mathbf{P}}_{k-1}^{KF}{\mathbf{F}}^T+\mathbf{Q},$$

(9)

Subsequently, with measurement ${\mathbf{y}}_k$, the KF uses the updated measurement to provide the estimated value using the following equations:

$${\mathbf{K}}_k^{KF}={\mathbf{P}}_k^{KF-}{\mathbf{H}}^T{\left(\mathbf{H}{\mathbf{P}}_k^{KF-}{\mathbf{H}}^T+\mathbf{R}\right)}^{-1},$$

(10)

$$\delta {\mathbf{s}}_k^{KF}=\delta {\mathbf{s}}_k^{KF-}+{\mathbf{K}}_k^{KF}\left({\mathbf{y}}_k-\mathbf{H}\delta {\mathbf{s}}_k^{KF-}\right),$$

(11)

$${\mathbf{P}}_k^{KF}=\left(\mathbf{I}-{\mathbf{K}}_k^{KF}\mathbf{H}\right){\mathbf{P}}_k^{KF-},$$

(12)

The KF algorithm based on model (1) and (2) can be listed as Algorithm 1.

Algorithm 1: KF algorithm based on model (1) and (2)

The mcc-KF employs the mcc as the optimization criterion to obtain the posterior estimates, which is robust against heavy-tailed non-Gaussian noises. We rewrite Equations (1) and (2) using Equations (8) and (9) as follows:

$$\left[\begin{array}{c}\delta {\hat{\mathbf{s}}}_k\\ {\mathbf{y}}_k\end{array}\right]=\left[\begin{array}{c}\mathbf{I}\\ \mathbf{H}\end{array}\right]\delta {\mathbf{s}}_k+{\Lambda }_k,$$

(13)

where

$${\Lambda }_k=\left[\begin{array}{c}-\left(\delta {\mathbf{s}}_k-\delta {\widehat{\mathbf{s}}}_k\right)\\ {\upsilon }_k\end{array}\right],$$

(14)

And we obtain the following equation:

$$E\left[{\Lambda }_k{\Lambda }_k^T\right]=\left[\begin{array}{cc}{\mathbf{P}}_k^{KF}& \mathbf{0}\\ \mathbf{0}& {\mathbf{R}}_{\mathrm{k}}\end{array}\right]=\left[\begin{array}{cc}{\chi }_{k,{\mathbf{P}}_k^{KF}}^{KF}{\left({\chi }_{k,{\mathbf{P}}_k^{KF}}^{KF}\right)}^T& \mathbf{0}\\ \mathbf{0}& {\chi }_{k,{\mathbf{R}}_{\mathrm{k}}}^{KF}{\left({\chi }_{k,{\mathbf{R}}_{\mathrm{k}}}^{KF}\right)}^T\end{array}\right]={\chi }_k^{KF}{\left({\chi }_k^{KF}\right)}^T,$$

(15)

where ${\chi }_k^{KF}$ can be computed using Cholesky decomposition. In addition, Equation (13) can be rewritten as follows:

$$\underset{{\mathbf{y}}_k^1}{\underbrace{{\left({\chi }_k^{KF}\right)}^{-1}\left[\begin{array}{c}\delta {\widehat{\mathbf{s}}}_k\\ {\mathbf{y}}_k\end{array}\right]}}=\underset{{\mathbf{H}}_k^1}{\underbrace{{\left({\chi }_k^{KF}\right)}^{-1}\left[\begin{array}{c}\mathbf{I}\\ \mathbf{H}\end{array}\right]}}\delta {\mathbf{s}}_k+\underset{{\upsilon }_k^1}{\underbrace{{\left({\chi }_k^{KF}\right)}^{-1}{\Lambda }_k}},$$

(16)

Thus, the optimal estimate of $\delta {\widehat{\mathbf{s}}}_k$ can be computed as follows:

$$\delta {\widehat{\mathbf{s}}}_k^{KF}=\underset{\delta {\mathbf{s}}_k^{KF}}{argmax}J\left(\delta {\mathbf{s}}_k^{KF}\right)=\underset{\delta {\mathbf{s}}_k^{KF}}{argmax}\sum _{g=1}^M{G}_{\sigma }\left(\delta e\left(g\right)\right),$$

(17)

Consequently, we can obtain one fixed-point iterative algorithm, and the steps of this method can be expressed as follows:

$${\mathbf{G}}_{\delta {\widehat{\mathbf{s}}}_k^{KF}}^{KF\left(s-1\right)}=diag\left(G_{\sigma }\left(\delta e_{1,k}^{\left(s-1\right)}\right),G_{\sigma }\left(\delta e_{2,k}^{\left(s-1\right)}\right),...,G_{\sigma }\left(\delta e_{7,k}^{\left(s-1\right)}\right)\right),$$

(18)

$${\mathbf{G}}_{{\mathbf{y}}_k}^{KF\left(s-1\right)}=diag\left(G_{\sigma }\left(\delta e_{8,k}^{\left(s-1\right)}\right),G_{\sigma }\left(\delta e_{9,k}^{\left(s-1\right)}\right),G_{\sigma }\left(\delta e_{10,k}^{\left(s-1\right)}\right),G_{\sigma }\left(\delta e_{11,k}^{\left(s-1\right)}\right)\right),$$

(19)

$${\widehat{\mathbf{P}}}_k^{KF\left(s-1\right)}={\chi }_{k,{\mathbf{P}}_k^{KF}}^{KF}{\left({\mathbf{G}}_{\delta {\widehat{\mathbf{s}}}_k^{KF}}^{KF\left(s-1\right)}\right)}^{-1}{\left({\chi }_{k,{\mathbf{P}}_k^{KF}}^{KF}\right)}^T,$$

(20)

$${\mathbf{R}}_k^{KF\left(s-1\right)}={\chi }_{k,{\mathbf{R}}_{\mathrm{k}}}^{KF}{\left({\mathbf{G}}_{{\mathbf{y}}_k}^{KF\left(s-1\right)}\right)}^{-1}{\left({\chi }_{k,{\mathbf{R}}_{\mathrm{k}}}^{KF}\right)}^T,$$

(21)

$$\delta e\left(g\right)={\mathbf{y}}_{k,g}^1-{\mathbf{H}}_{k,g}^1\delta {\mathbf{s}}_{k,g},$$

(22)

$${\mathbf{K}}_k^{KF\left(s-1\right)}={\widehat{\mathbf{P}}}_k^{KF\left(s-1\right)}{\mathbf{H}}^T{\left({\mathbf{H}\widehat{\mathbf{P}}}_k^{KF\left(s-1\right)}{\mathbf{H}}^T+{\mathbf{R}}_k^{KF\left(s-1\right)}\right)}^{-1},$$

(23)

$$\delta {\widehat{\mathbf{s}}}_k^{KF\left(s\right)}=\delta {\mathbf{s}}_k^{KF-}+{\mathbf{K}}_k^{KF\left(s-1\right)}\left({\mathbf{y}}_k-\mathbf{H}\delta {\mathbf{s}}_k^{KF-}\right),$$

(24)

Thus, the mccKF based on model (1) and (2) can be listed in Algorithm 2.

Algorithm 2: The mccKF algorithm based on model (1) and (2)

3.3. Allan Variance

In this work, we employ Allan Variance to assist the FIR filter, which is able to improve the accuracy of the noise estimation. Firstly, we are able to obtain the following equation with ${\mathbf{y}}_k$:

$${\sigma }_{{\mathbf{y}}_k}^2={\displaystyle \frac{1}{2}}E\left({\left({\mathbf{y}}_{k+1}-{\mathbf{y}}_k\right)}^2\right)$$

(25)

Based on Equation (25), we can obtain that

$${\widehat{\mathbf{R}}}_k={\displaystyle \frac{1}{2\left(k-1\right)}}\sum _{j=1}^{N-1}\left[{\left({\mathbf{y}}_{k+1}-{\mathbf{y}}_k\right)}^2\right],$$

(26)

where N is the sum of the data groups. Then, we can obtain

$${\widehat{\mathbf{R}}}_k=\left(1-{\lambda }_{k-1}\right){\widehat{\mathbf{R}}}_{k-1}+{\displaystyle \frac{1}{2}}{\lambda }_{k-1}{\left({\mathbf{y}}_k-{\mathbf{y}}_{k-1}\right)}^2,$$

(27)

3.4. FIR Filter

It is evident from the aforementioned section that the mccKF filter enhances robustness compared with the conventional KF method. However, the mccKF depends on noise statistics. Meanwhile, the FIR filter maintains the robustness of the proposed filter. Based on model (1) and (2), the FIR filter estimates the error of the INS using the measurement at time index $j\in [k-N_F+1,k]$. The FIR filter uses the KF to fuse the data in the dead zone. When the time index is $k>N_F$, where $N_F$ is the size of the filtering window, the FIR performs one-step prediction, which is listed as follows:

$$\delta {\mathbf{s}}_j^{F-}=\mathbf{F}\delta {\mathbf{s}}_j^F,$$

(28)

$${\mathbf{P}}_j^{F-}={\mathbf{FP}}_j^F{\mathbf{F}}^T+\mathbf{Q},$$

(29)

Then, the measurement update can be performed using the following equations:

$${\mathbf{G}}_v^F={\left({\mathbf{H}}^T\mathbf{H}+{\left(\mathbf{FH}{\mathbf{F}}^T\right)}^{-1}\right)}^{-1},$$

(30)

$${\mathbf{K}}_v^F={\mathbf{G}}_v^F\mathbf{H},$$

(31)

$$\delta {\overline{\mathbf{s}}}_v^F=\delta {\overline{\mathbf{s}}}_v^{F-}+{\mathbf{K}}_v^F\left({\mathbf{y}}_v-\mathbf{H}\delta {\overline{\mathbf{s}}}_v^{F-}\right),$$

(32)

$${\overline{\mathbf{P}}}_v^F=\left(\mathbf{I}-{\mathbf{K}}_v^F\mathbf{H}\right){\overline{\mathbf{P}}}_v^{F-},$$

(33)

Thus, the FIR filter based on model (1) and (2) can be listed in Algorithm 3.

Algorithm 3: FIR filter algorithm based on model (1) and (2)

3.5. mccKF/FIR Integrated Filter

In this study, we used the mccKF/FIR integrated filter, which is shown in Figure 2. First, we performed the one-step prediction of the mccKF $\delta {\mathbf{s}}_k^{-}$ and ${\mathbf{P}}_k^{-}$ with $\delta {\mathbf{s}}_{k-1}$ and ${\mathbf{P}}_{k-1}$. The Mahalanobis distance can be used to determine differences in the data distribution. Subsequently, the Mahalanobis distance is computed using the following equation:

$$M{d}_k={\left({\mathbf{y}}_k-\mathbf{H}\delta {\mathbf{s}}_k^{KF-}\right)}^T\mathbf{R}\left({\mathbf{y}}_k-\mathbf{H}\delta {\mathbf{s}}_k^{KF-}\right),$$

(34)

When $M{d}_k>door$, we used the FIR filter to provide the output, and when $M{d}_k<door$, we directly updated the measurement.

4. Results

In this study, a practical test was conducted to validate the efficacy of the proposed approach. First, we provide an overview of the test setup. Second, the localization accuracy of the proposed mccKF/FIR integrated filter is compared with that of a traditional filter.

4.1. The Design of the Testbed

In this subsection, the testbed used herein is presented. In this study, a practical test was conducted in building No. 1 of the University of Jinan, China. Figure 3 and Figure 4 illustrates the real test environment utilized for the experiment. In the test, we used crazyflie2.0 as the target drone, and six DW-1000-based UWB reference nodes (UWB RNs) with known coordinates and one UWB blind node fixed on the target drone to measure ${\mathbf{P}}_k^{UWB}$. Meanwhile, one IMU fixed on the target drone was used to measure the INS-derived position ${\mathbf{P}}_k^{UWB}$.

4.1.1. Test 1

Herein, two practical tests were performed to demonstrate the enhancements achieved by the proposed method. In this subsection, the result of Test 1 is discussed. First, for the filter, we set $\Delta k=0.02s$, $N_F=8$, $N_s=5$, and $Rate\_door=0.2$, and then ${\mathbf{Q}}_k$ and ${\mathbf{R}}_k$ were set as follows:

$${\mathbf{Q}}_k=\left[\begin{array}{cc}{\mathbf{Q}}_{1,k}& {\mathbf{0}}_{4\times 3}\\ {\mathbf{0}}_{2\times 4}& {\mathbf{Q}}_{2,k}\\ {\mathbf{0}}_{1\times 4}& {\mathbf{Q}}_{3,k}\end{array}\right]\times 0.25^2,$$

(35)

$${\mathbf{Q}}_{1,k}=0.01\times \left[\begin{array}{cccc}0.25\times \Delta k^2& 0.5\times \Delta k& 0& 0\\ 0.5\times \Delta k& 1& 0& 0\\ 0& 0& 0.25\times \Delta k^2& 0.5\times \Delta k\\ 0& 0& 0.5\times \Delta k& 1\end{array}\right],$$

(36)

$${\mathbf{Q}}_{2,k}=\left[\begin{array}{cc}0.25\times \Delta k^2& 0.5\times \Delta k\\ 0.5\times \Delta k& 1\end{array}\right]\times 1.5^2,$$

(37)

$${\mathbf{R}}_k=\left[\begin{array}{cccc}0.35& 0& 0& 0\\ 0& 0.35& 0& 0\\ 0& 0& 0.05& 0\\ 0& 0& 0& 0.01\end{array}\right],$$

(38)

$${\mathbf{Q}}_{3,k}=1,$$

(39)

The reference path used in Test 1 is shown in Figure 5, and the positions measured by the KF, adaptive KF (AKF), mccKF, and mccKF/Allan-assisted FIR integrated filters in the east, north, and upward directions in Test 1 are compared in Figure 6, Figure 7 and Figure 8. The figures reveal that the KF, mccKF, and proposed mccKF/Allan-assisted FIR integrated filters provide the position in the east, north, and upward directions, respectively. The AKF has big errors. The situations in Figure 7 and Figure 8 are similar. These figures show that the KF and mccKF exhibit weak divergence in the east and north directions. When compared with the KF and mccKF, the positions measured by the proposed mccKF/FIR integrated filter are closer to the reference position, indicating that the suggested approach is more effective in minimizing localization errors. Figure 8 shows that all solutions of the filters are similar, and the positions measured by the proposed have small fluctuations when compared with other filters.

The cumulative distribution function (CDF) of the position errors, as measured by the KF, AKF, mccKF, and mccKF/Allan-assisted FIR integrated filter in Test 1, are depicted in Figure 9. The figure shows that the localization error of the mccKF is less than that of the KF when the CDF is 0.9. Compared with the mccKF and KF, the proposed mccKF/FIR integrated filter exhibits minimal localization error when the CDF is 0.9. This highlights the efficacy of the suggested approach in minimizing localization errors.

To provide a more detailed demonstration of the effectiveness of the proposed approach, we use the position root mean square error (RMSE) in each time index, which is computed using the following equation:

$$RMS{E}_{\mathrm{k}}={\displaystyle \frac{1}{k}}\sum _{j=1}^k\sqrt{{\left({\widehat{P}}_{E,j}-P_{E,j}\right)}^2+{\left({\widehat{P}}_{N,j}-P_{N,j}\right)}^2+{\left({\widehat{P}}_{U,j}-P_{U,j}\right)}^2},$$

(40)

where $\left({\widehat{P}}_{E,j},{\widehat{P}}_{N,j},{\widehat{P}}_{U,j}\right)$ represents the measurement of $\left(P_{E,j},P_{N,j},P_{U,j}\right)$ at time index j. And Figure 10 shows the RMSE of the position errors measured by the KF, mccKF, and Allan-assisted mccKF/FIR integrated filter in Test 1. From this figure, we can see that the proposed method has the smallest RMSE errors.

Table 1. Position RMSEs of the KF, mccKF, and mccKF/Allan-assisted FIR integrated filter in Test 1.

Algorithms	RMSE (m)
	East Direction	North Direction	Upward Direction	Mean
KF	0.20	0.20	0.28	0.23
mccKF	0.15	0.14	0.28	0.19
mccKF/FIR	0.12	0.11	0.26	0.16

lists the position RMSEs of the KF, mccKF, and mccKF/Allan-assisted FIR integrated filter in Test 1. The table shows that the mean RMSE of the mccKF is slightly lower than that of the KF. The mccKF/Allan-assisted FIR integrated filter reduces the localization error from 0.23 to 0.16 m when compared with the KF, which has the lowest localization error. All the figures and the table illustrate that, in Test 1, the proposed mccKF/Allan-assisted FIR filter is more efficient in minimizing localization errors when compared with the KF and mccKF.

4.1.2. Test 2

In this study, the real test was performed following another reference path, which is shown in Figure 11. Figure 12, Figure 13 and Figure 14 display the positions measured by the KF, AKF, mccKF, and the mccKF/Allan-assisted FIR integrated filter in the east, north, and upward directions in Test 2. The figures show that the performances of the filters in the east and north directions are different from those in the upward direction. In Figure 12 and Figure 13, similar to Test 1, the KF and mccKF exhibit some divergence in the east and north directions, particularly when the position changes. When compared with the KF and mccKF, the position of the proposed mccKF/Allan-assisted FIR filter is more stable, and its solution is closer to the reference value. In the case of upward direction, the positions of all the filters are similar. In Test 2, the estimation of the position in the upward direction by all the filters is initially very accurate, and later, a small amplitude of vibration is observed. Overall, the estimations of the target aircraft position by all the filters are achieved in this direction. For the filter, we set the following:

$${\mathbf{Q}}_k=\left[\begin{array}{cc}{\mathbf{Q}}_{1,k}& {\mathbf{0}}_{4\times 3}\\ {\mathbf{0}}_{2\times 4}& {\mathbf{Q}}_{2,k}\\ {\mathbf{0}}_{1\times 4}& {\mathbf{Q}}_{3,k}\end{array}\right]\times 0.1^2,$$

(41)

$${\mathbf{Q}}_{1,k}=0.01\times \left[\begin{array}{cccc}0.25\times \Delta k^2& 0.5\times \Delta k& 0& 0\\ 0.5\times \Delta k& 1& 0& 0\\ 0& 0& 0.25\times \Delta k^2& 0.5\times \Delta k\\ 0& 0& 0.5\times \Delta k& 1\end{array}\right],$$

(42)

$${\mathbf{Q}}_{2,k}=\left[\begin{array}{cc}0.25\times \Delta k^2& 0.5\times \Delta k\\ 0.5\times \Delta k& 1\end{array}\right]\times 0.{05}^2,$$

(43)

$${\mathbf{R}}_k=\left[\begin{array}{cccc}0.15& 0& 0& 0\\ 0& 0.15& 0& 0\\ 0& 0& 0.15& 0\\ 0& 0& 0& 0.01\end{array}\right],$$

(44)

$${\mathbf{Q}}_{3,k}=1,$$

(45)

The position RMSEs of the KF, mccKF, and mccKF/Allan-assisted FIR integrated filter in Test 2 are presented in Figure 15. The figure shows that the RMSEs of the mccKF and mccKF/Allan-assisted FIR are the same when the time index is <480 s. After 480 s, all the filters exhibit large differences. The localization error of the KF method rapidly increases, and this method has the largest RMSE when compared with the mccKF and proposed method. The increase in the RMSE of the mccKF is low compared with that of the KF. Compared with the KF and mccKF, although the RMSE of the proposed mccKF/Allan-assisted FIR integrated filter increases, it has the lowest value in all the filters.

Table 2. Position RMSEs of the KF, mccKF, and mccKF/Allan-assisted FIR integrated filter in Test 2.

Algorithms	RMSE (m)
	East Direction	North Direction	Upward Direction	Mean
KF	0.11	0.15	0.29	0.18
mccKF	0.14	0.17	0.27	0.19
mccKF/Allan-assisted FIR	0.10	0.10	0.20	0.13

lists the position RMSEs of the KF, mccKF, and mccKF/Allan-assisted FIR integrated filter in Test 2. The table reveals that, in comparison with the KF and mccKF, the proposed method exhibits the lowest RMSE. It reduces the localization error from the KF’s 0.18 m to 0.13 m, resulting in a reduction in the localization error by approximately 30%. The position CDFs of the KF, mccKF, and mccKF/Allan-assisted FIR integrated filter in Test 2 are shown in Figure 16. From the figure, we can see that the proposed method has the smallest localization error.

All the figures and the table indicate that, in Test 2, the proposed mccKF/Allan-assisted FIR filter is more efficient in minimizing localization errors when compared with the KF and mccKF. Tests 1 and 2 reveal that the proposed approach exhibits minimal localization error. Therefore, the suggested approach is the most effective.

5. Conclusions

To improve the robustness of the data fusion for INS-based integrated localization, an mccKF/FIR integrated filter was proposed herein. In this approach, the mccKF serves as the primary data fusion filter and is used for fusing data from multiple sources, including the INS. In this work, we used the Mahalanobis distance to verify the performance of the mccKF. If the performance of the mccKF is poor, the FIR filter is used to replace the mccKF. Two practical tests were performed to assess the performance of the suggested approach. The findings indicate that the mccKF/FIR integrated method is effective in minimizing localization errors when compared with the KF and mccKF. This observation emphasizes the efficacy of the proposed method.