Constrained MEMS-Based INS/UWB Tightly Coupled System for Accurate UGVs Navigation

Abstract

To enhance the navigation performance and robustness of navigation system combining ultrawideband (UWB) and inertial navigation systems (INS) under complex indoor environments, an improved navigation method—Allan variance (AV) to assist a modified adaptive extended Kalman Filter based on the dynamic weight function (DWF-MAEFF)—is proposed. Firstly, AV is used to improved INS error dynamics by modeling the stochastic noise of an inertial sensor; which can compensate for inertial sensor error caused by stochastic noise during integrated navigation. Secondly, the MAEKF is developed by designing the weight function to adjust the weight of measurement noise reasonably and dynamically, which can further improve the robustness of the AEKF algorithm. Field tests were conducted to verify the effectiveness of the proposed navigation method. The result indicated that an improvement of up to 60% over the existing integrated navigation method based on EKF and AEKF can be obtained by the proposed method.

1. Introduction

Global Navigation Satellite Systems (GNSS) are extensively used in the navigation and positioning of unmanned ground vehicles (UGVs) due to its advantages of being real-time, continuous and high precision [1,2]. Unfortunately, it cannot satisfy the demands of an indoor environment, as it may suffer from complete occlusion by building components such as walls and roofs. There has been a surge in the use of ultrawideband (UWB), in view of its low complexity, low power consumption, low cost and high ranging accuracy in a simple indoor positioning system [3,4]. Related researches, such as channel measurement [5], multipath component estimation [6] and positioning error theory [7], have been conducted. However, a simple UWB positioning system cannot provide orientation information for UGVs. Therefore, UWB and Inertial Navigation Systems (INS) are usually paired together in a complementary manner to calculate UGVs’ motion information, including attitude, velocity and position, in real time and accurately [8,9,10]. In recent years, UGVs have been utilized in various fields, such as intelligent transportation, logistics distribution and mining, as well as others [11,12,13]. It will increasingly rely on reliable and accurate navigation and positioning systems, which can be used for mass production at a low cost. Accordingly, we prefer advanced information fusion algorithms based on low-cost MEMS inertial sensors to achieve the same accuracy.

However, existing MEMS-based INS/UWB integrated navigation methods usually utilize the motion model of the carrier as the system dynamic equation rather than the error model of the strapdown inertial navigation system (SINS). For example, an IMU/UWB model designed in the literature [14,15] uses the acceleration model as the dynamic equation for filtering, and IMU measurements are used as the system input. The model cannot provide reliable attitude estimation. Another study [16] designed an INS/UWB integrated system based on the error model of the SINS, and modeled the bias of the gyroscope (gyro) and accelerometer (acc) as a first-order Markov process. Due to the fact that the noise of inertial sensors is colored noise (CN) related to time, the modeling method in [16] may result in mismatch between the calculated covariance matrix of the Kalman filter (KF) and the actual noise characteristics, and in severe cases may even cause significant model errors [17], greatly limiting the navigation accuracy that INS/UWB integrated navigation can achieve. In general, CN belongs to the stochastic error of inertial sensors, which is caused by uncertain factors. For modeling methods of stochastic error of MEMS inertial sensors, the most common are power spectral density (PSD), autocorrelation function (ACF), autoregressive moving average model (ARMA) and Allan variance (AV). The PSD method is neither intuitive nor easy to understand for scholars in the field of nonlinear system analysis [18]. The modeling method based on ACF possesses the merit of clear physical meaning, but there are also obvious drawbacks [19]. According to the research in [20], it is unrealistic to accurately determine statistics such as autocorrelation functions from experimental data in terms of time consumption. The ARMA model has model sensitivity and is not suitable for describing odd power engineering and high-order processes [21]. The AV method can identify various error sources and has the advantage of low computational complexity and ease of understanding, making it very popular in sensor error analysis [22]. That paper proposes a SINS error model based on AV analysis technology, aiming to improve the accuracy and reliability of traditional navigation algorithms.

At present, the common composite patterns of MEMS-based INS/UWB include loose integration (LC) and tight integration (TC). In general, LC is a combination of the UWB solution (i.e., the position or velocity) derived from the UWB and INS information. The degradation of LC systems can be overcome by applying the original UWB observations (i.e., pseudo distance) to measurement updates directly [23]. This mode is called tight coupling (TC) [16,24]. The merit of TC is that it is immune to the geometry of BSs. Even if there are fewer than four BSs available, it can continue to produce a marked effect. Unfortunately, conventional LC and TC may be subject to non-line-of-sight (NLOS) effects, which occur frequently in complex indoor environment [25,26,27]. Although these works yield constructive references, providing comprehensive and high-precision navigation solutions (i.e., attitude, velocity and position) in complex indoor environment remains a challenge for INS/UWB navigation systems based on low-cost inertial sensors.

The KF algorithm is commonly employed in INS/UWB integrated system to realize data fusion which can overcome the shortcomings brought by the separate operation of INS and UWB [28]. The standard KF realizes optimal estimation on the premise that its noise characteristics, including system noise and measurement noise, are Gaussian noise [29]. However, these noise models are not time-invariant in practice. Accordingly, fixed process and measurement noise covariance matrices are not suitable for real-time applications, as they can cause errors in filter estimation. To address this issue, adaptive KF (AKF) technology has been developed based on innovation adaptive estimation (IAE) [30] and residual adaptive estimation (RAE) [31]. AKF based on residual adaptive estimation (RAKF) adjusts the covariance matrix of process noise or measurement noise in real-time by continuously monitoring the residual sequence within the sliding window [32]. However, it is usually necessary to have a large sliding window and store the innovation sequence within the sliding window in order to obtain reliable variance measurements in practical applications. As a consequence, this method is not suitable for rapidly changing dynamic systems [33]. AKF based on innovation adaptive estimation (IAKF) can estimate process noise and measurement noise in real-time by employing measurements [34]. Due to its simple structure and low computational complexity, it has been widely used in engineering. In [35], robust estimation theory was introduced into AKF to generate adaptive robust AKF (ARKF). ARKF is used to minimize outliers in measurement, and also for fast convergence of rapidly changing signals. However, ARKF is unable to effectively identify the NLOS segment with process uncertainty. The serious impact it brings is the loss of accuracy in estimating navigation parameters in the segment.

To sum up, we propose an improved navigation method, Allan variance (AV), to assist a modified adaptive extended Kalman Filter based on the dynamic weight function (DWF-MAEFF) to fulfill low-cost and high-precision navigation of UGVs. The main contributions of this paper are summarized as follows: (1) AV is used to construct a high-precision INS error dynamics by modeling stochastic errors of inertial sensors, which can suppress inertial sensor errors caused by stochastic noise; (2) A modified adaptive Extended Kalman Filter (MAEKF) algorithm is proposed by designing the weight function to adjust the weight of measurement noise reasonably and dynamically, which can further enhance the robustness of the adaptive Kalman filter (AEKF) algorithm. Experiments are conducted in order to verify the effectiveness and superiority of the proposed method. The results show that, with the proposed method, an improvement of navigation accuracy of up to 60% can be obtained compared with existing integrated position algorithms based on the Extended Kalman filter (EKF) and Adaptive Extended Kalman filter (AEKF).

The rest of this paper is structured as follows: Section 2 briefly introduces the error-state model and measurement model. In Section 3, an improved stochastic modeling method of inertial sensor errors based on AV is built. In addition, we improve the optimization-based EKF algorithm to reduce the adverse effects of NLOS. Corresponding experimental results are illustrated in Section 4 to vindicate the performance of the proposed algorithm. In Section 5, the article is concluded.

2. Preliminaries

2.1. INS/UWB LC Architecture

2.1.1. Error-State Model

In MEMS-based INS/UWB integrated navigation, the error-state vector is selected as

$$X={[\begin{array}{ccccc}\varphi & \delta v^n& \delta p^n& {\epsilon }^b& {\nabla }^b\end{array}]}^T$$

(1)

where $n$, defined in the East-North-Up (E-N-U) geographical coordinates, is the navigation frame (n-frame) and $b$ is the body frame (b-frame). $\varphi ={\left[\begin{array}{ccc}{\varphi }_E& {\varphi }_N& {\varphi }_U\end{array}\right]}^T$ is the misalignment error vector. ${\varphi }_E$, ${\varphi }_N$ and ${\varphi }_U$ are the components of the attitude error vector in the East, North and UP directions respectively. $\delta v^n={\left[\begin{array}{ccc}\delta v_E& \delta v_N& \delta v_U\end{array}\right]}^T$ and $\delta p^n={\left[\begin{array}{ccc}\delta p_E& \delta p_N& \delta p_U\end{array}\right]}^T$ are velocity error vector and position error vector, respectively. ${\epsilon }_i$ and ${\nabla }_i$ ($i=x,y,z$) are the gyro bias vector and acc bias vector in the b-frame.

The simplified Strapdown Inertial Navigation (S-SINS) algorithm is used in indoor positioning, owing to the following two reasons: (1) earth rotation information cannot be perceived as a result of the bad degree of accuracy of the MEMS gyro; (2) the effect of the rotation of the earth and curvature caused by the low-speed of the carrier can be ignored. The corresponding error-state dynamic model is as follows:

$$\begin{array}{l}\left\{\begin{array}{l}\dot{\varphi }=-C_b^n({\epsilon }^b+n_w^b)\\ \delta {\dot{v}}^n=C_b^n{f}^b\times \varphi +C_b^n({\nabla }^b+n_a^b)\\ \delta {\dot{p}}^n=\delta v^n\end{array}\right.\\ \left\{\begin{array}{l}{\dot{\epsilon }}^b=-\frac{1}{{\tau }_w}{\epsilon }^b+{\delta }_w\\ {\dot{\nabla }}^b=-\frac{1}{{\tau }_a}{\nabla }^b+{\delta }_a\end{array}\right.\end{array}$$

(2)

where $C_b^n$ is the direction cosine matrix used for representing the transformation from b-frame to n-frame; $f^b$ is specific force measured by acc; ${\epsilon }^b$ and ${\nabla }^b$ are the bias of gyro and acc, respectively; $n_w^b$ and $n_a^b$ are gyro measurement noise and acc measurement noise, respectively; ${\tau }_w$ and ${\tau }_a$ are correlation time constants; ${\delta }_w$ and ${\delta }_a$ are first-order GM process noise of gyro bias and acc bias. Therefore, the state equation can be presented as follows:

$${\dot{X}}_t=F_t{X}_t+G_t{W}_t$$

(3)

with

$$F_t=\left[\begin{array}{ccccc}{0}_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& -C_b^n& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& I_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ \left[C_b^n{f}^b\times \right]& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& C_b^n\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& -\frac{1}{{\tau }_w}{I}_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& -\frac{1}{{\tau }_a}{I}_{3\times 3}\end{array}\right]$$

(4)

$$G_t=\left[\begin{array}{cccc}-C_b^n& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& C_b^n& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& I_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& I_{3\times 3}\end{array}\right],W_t=\left[\begin{array}{c}{n}_w^b\\ n_a^b\\ {\delta }_w\\ {\delta }_a\end{array}\right]$$

(5)

where $F_t$ is the system dynamics matrix; $G_t$ is the matrix of system noise distribution; $W_t$ denotes the system noise vector, which assumes $W_t\sim \left(0,Q_t\right)$; and $Q_t$ is exactly as shown in (6).

$$Q_t=\left[\begin{array}{cccc}{\sigma }_w^2{I}_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& {\sigma }_a^2{I}_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& {\sigma }_{\epsilon }^2{I}_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& {\sigma }_{\nabla }^2{I}_{3\times 3}\end{array}\right]$$

(6)

where ${\sigma }_w^2$, ${\sigma }_a^2$, ${\sigma }_{\epsilon }^2$ and ${\sigma }_{\nabla }^2$ are the variance of $n_w^b$, $n_a^b$, ${\delta }_w$ and ${\delta }_a$.

The discrete-time form of the of error-state can be described as:

$$\begin{array}{c}{X}_k=\left(I+F_{t}T\right)X_{k-1}+G_{t}T{W}_{k-1}\\ \hspace{1em}\hspace{1em}=F_{k/k-1}{X}_{k-1}+G_{k/k-1}{W}_{k-1}\end{array}$$

(7)

where $T$ is the sampling time.

2.1.2. Measurement Model

The position difference between UWB and INS outputs is used as the input of KF, which is beneficial to estimate the error characteristics of inertial sensor in INS/UWB based on the LC. The measurement $Z_k^{LC}$ at time $k$ can be written as

$$Z_k^{LC}={\widehat{p}}_{INS,k}^n-{\widehat{p}}_{UWB,k}^n$$

(8)

The measurement model is

$$Z_k^{LC}=H_k{X}_k+V_k$$

(9)

where $V_k$ is the measurement noise vector modeled as white noise conforming to Gaussian distribution, and with the mean of zero and the covariance matrix $R=d_v^2{I}_{3\times 3}$. $H_k$ is the conversion matrix linking the state vector and the measurement vector.

$$H_k=\left[\begin{array}{ccccc}{0}_{3\times 3}& I_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\end{array}\right]$$

(10)

2.1.3. Kalman Filtering

We integrate INS and UWB in a complementary manner to avert the imperfection of their independent operation. The data fusion of INS and UWB is usually performed by KF [36]. Taking LC as an example, the schematic of INS/UWB integrated navigation is drawn in Figure 1. In the INS/UWB LC system, the error propagation model of INS is taken as the state, and the difference between the position of INS and UWB output is taken as the observation, so the combined filter is constructed. During this period, navigation parameters (attitude, velocity and position) and inertial sensor parameters (stochastic error) need to be focused.

The time update process is:

$$\left\{\begin{array}{l}{X}_k={\Phi }_{k/k-1}{X}_{k-1}+{\Gamma }_{k/k-1}{W}_{k-1}\\ P_{k/k-1}={\Phi }_{k/k-1}{P}_{k-1}{\Phi }_{k/k-1}^T+{\Gamma }_{k/k-1}{Q}_{k-1}{\Gamma }_{k/k-1}^T\end{array}\right.$$

(11)

While the measurement update process is:

$$\left\{\begin{array}{l}{\widehat{X}}_k={\widehat{X}}_{k/k-1}+K_k\left(Z_k-H_k{\widehat{X}}_{k/k-1}\right)\\ K_k=P_{k/k-1}{H}_k^T{\left(H_k{P}_{k/k-1}{H}_k^T+R_k\right)}^{-1}\\ P_k=\left(I-K_k{H}_k\right)P_{k/k-1}\end{array}\right.$$

(12)

where ${\widehat{X}}_{k/k-1}$ is the state one-step prediction vector; $P_{k/k-1}$ is the one-step prediction mean square error vector of ${\widehat{X}}_{k/k-1}$. $K_k$ is filter gain.

2.2. Problem Formulation

For indoor low-cost and high-precision navigation and positioning, MEMS-based INS/UWB integrated systems with an LC-KF framework is restricted for the following reasons: (1) the inaccuracy of the INS error model; (2) filter divergence caused by the UWB outliers; (3) the poor robustness of the system during NLOS effect.

The first limitation arises from errors of the MEMS inertial sensors. In the INS/UWB integrated position system, the stochastic error of the inertial sensor, modeled as a first-order GM process in (2), is compensated by LC-KF. According to IEEE standards for inertial sensors [37], however, five main stochastic errors exist in inertial sensors, namely, Angular Random Walk (ARW), Quantization Noise (QN), Rate Random Walk (RRW), Bias Instability (BI) and Rate Ramp (RR), where RRW, BI and RR belong to CN. Consequently, it is obvious that the modeling method of inertial sensors mentioned above has shortcomings. On the basis of the KF theory, white noise (ARW) can be directly used as the driving noise of the system without modeling. QN can be transformed into equivalent white noise through the error model of INS, so it does not need to be modeled. In order to compensate for the drift of inertial sensors caused by CN in KF, it is essential to model the colored noise.

It is well known that the UWB tag outputs the location under the premise that at least four BTSs are available. For LC-KF architecture, however, when the number of available BSs is less than four, the performance of KF degrades over time, even resulting in filtering divergence.

Obviously, the conventional LC-KF integration scheme, in a complex indoor environment, is limited by the above-mentioned reasons. In order to provide a low-cost and high-precision MEMS-based INS/UWB indoor integrated navigation system, the following three main issues are covered in this paper, namely: (1) the application of tightly coupled (TC) architecture to accommodate fewer base stations; (2) the use of a novel INS error model to eliminate the effect of stochastic error on inertial sensors; (3) the improvement of navigation accuracy by enhancing the robustness of the system to the environment during the period of NLOS.

3. Methodology

An optimization-based MEMS-INS/UWB TC algorithm based on AV is proposed to improve the navigation performance of indoor UGV. The method is (1) based on Allan to improve the model of MEMS-based inertial sensors; (2) based on TC integration to mitigate the effect of fewer BSs; (3) an optimization-based EKF algorithm for suppressing the NLOS error of UWB.

3.1. Improving Error Model of INS

$$\left\{\begin{array}{l}\dot{\varphi }=-C_b^n({\epsilon }^b+n_w^b+w_Q^b+w_C^b)\\ \delta {\dot{v}}^n=C_b^n{f}^b\times \varphi +C_b^n({\nabla }^b+n_a^b+f_Q^b+f_C^b)\\ \delta {\dot{p}}^n=\delta v^n\end{array}\right.$$

(13)

where $w_Q^b$ and $f_Q^b$, respectively, are the QN components in angular rate and specific force; $w_C^b$ is a combination of RRW, BI and RR of angular rate, i.e., $w_C^b=w_B^b+w_K^b+w_R^b$; similarly, $f_C^b$ is a combination of RRW, BI and RR of angular rate, i.e., $f_C^b=f_B^b+f_K^b+f_R^b$.

3.1.1. On-Line Compensation of QN

$w_Q^b$ and $f_Q^b$ can be expressed, respectively, as

$$\left\{\begin{array}{l}{w}_Q^b=\delta {\dot{\alpha }}_Q^b\\ f_Q^b=\delta {\dot{v}}_Q^b\end{array}\right.$$

(14)

where $\delta {\alpha }_Q^b$ is the angular quantization noise of gyro, and $\delta v_Q^b$ is the velocity quantization noise of the acc.

Let us introduce a new formulated attitude error vector $\tilde{\varphi }$, and velocity error vector $\delta {\tilde{v}}^n$, as:

$$\left\{\begin{array}{l}\tilde{\varphi }=\varphi +C_b^n\delta {\alpha }_Q^b\\ \delta {\tilde{v}}^n=\delta v^n-C_b^n\delta v_Q^b\end{array}\right.$$

(15)

Applying (15) to (2), the error equation of INS enhanced by QN can be obtained:

$$\left\{\begin{array}{cc}\hfill \dot{\tilde{\varphi }}& =-C_b^n({\epsilon }^b+n_w^b)-C_b^n{w}_C^b-\left\{C_b^n\left(w_{ib}^b\times \right)\delta {\alpha }_Q^b\right\}\hfill \\ \hfill \delta {\dot{\tilde{v}}}^n& =C_b^n{f}^b\times \tilde{\varphi }+C_b^n({\nabla }^b+n_a^b)+C_b^n{f}_C^b\hfill \\ & -\left\{\left(f^n\times C_b^n\right)\delta {\alpha }_Q^b+C_b^n\left(w_{ib}^b\times \right)\delta v_Q^b\right\}\hfill \\ \hfill \delta {\dot{p}}^n& =\delta {\tilde{v}}^n+\left\{C_b^n\delta v_Q^b\right\}\hfill \end{array}\right.$$

(16)

In (16), QN is converted into white noise. It is obvious that $\delta {\alpha }_Q^b$ related term is added to the attitude error equation, $\delta {\alpha }_Q^b$ related term and $\delta v_Q^b$ related term are added to the velocity error equation.

3.1.2. On-Line Compensation of CN

AV is used to model the coexistence of three kinds of CN. The specific steps can be divided into: (1) the transfer function of the shaped filter of CN is obtained by introducing the spectral decomposition theorem; (2) given the transfer function of the shaping filter, the goal is to derive the stochastic differential equation of CN; (3) a unified equivalent differential equation is used to express three kinds of CN. A brief description is given here, and the details can be seen in [38].

Stochastic differential equations of BI:

$${\dot{y}}_B(t)+\beta y_B(t)=\beta B{u}_B\left(t\right)$$

(17)

where $u_B\left(t\right)$ represents unit white Gaussian noise.

Stochastic differential equations of RRW:

$${\dot{y}}_K(t)=K{u}_K\left(t\right)$$

(18)

where $u_K\left(t\right)$ represents unit white Gaussian noise.

Stochastic differential equations of RR:

$${\ddot{y}}_R(t)+\sqrt{2}{w}_0{\dot{y}}_R(t)+w_0{y}_R(t)=R{u}_R\left(t\right)$$

(19)

where $u_R\left(t\right)$ represents unit white Gaussian noise.

The equivalent model of BI, RRW and RR:

$$\begin{array}{l}{y}^{\left(4\right)}+a_3{y}^{\left(3\right)}+a_2{y}^{\left(2\right)}+a_1\dot{y}+a_{0}y\\ =b_3{u}^{\left(3\right)}+b_2{u}^{\left(2\right)}+b_1\dot{u}+b_{0}u\end{array}$$

(20)

The state-space form of (20) can be expressed as

$${\dot{\overrightarrow{x}}}_i\left(t\right)=A_i{\overrightarrow{x}}_i\left(t\right)+B_i{\overrightarrow{u}}_i\left(t\right)$$

(21)

$${\overrightarrow{y}}_i\left(t\right)=C_i{\overrightarrow{x}}_i\left(t\right)$$

(22)

with

$$A=\left[\begin{array}{cccc}0& 0& 0& -a_0\\ 1& 0& 0& -a_1\\ 0& 1& 0& -a_2\\ 0& 0& 1& -a_3\end{array}\right],B=\left[\begin{array}{c}{b}_0\\ b_1\\ b_2\\ b_3\end{array}\right],C={\left[\begin{array}{c}0\\ 0\\ 0\\ 1\end{array}\right]}^T$$

(23)

where ${\overrightarrow{x}}_i$ $\left(i=1,2,3,4\right)$ denotes the state vector.

3.2. INS/UWB TC-EKF Model Design

The main difference from the LC-KF model in Section 2 is that the measurement in TC positioning is the difference stemming from the distance calculated by the INS and the position of each BS and the measured distance of the UWB. In Figure 2, the framework of the INS/UWB positioning system based on TC is presented.

3.2.1. Error-State Equation

The system model for INS/UWB TC-EKF integration can be represented as follows:

$${\dot{X}}_t=F_t{X}_t+G_t{W}_t+W_{Qt}+W_{Ct}$$

(24)

with

$$W_{Qt}=\left[\begin{array}{c}{F}_{11}\\ F_{21}\\ F_{31}\\ F_{41}\\ F_{51}\end{array}\right]$$

(25)

$$\begin{array}{l}{F}_{11}=C_b^n\left(w_{ib}^b\times \right)\delta {\alpha }_Q^b\\ F_{21}=-\left\{\left(f^n\times C_b^n\right)\delta {\alpha }_Q^b+C_b^n\left(w_{ib}^b\times \right)\delta v_Q^b\right\}\\ F_{31}=C_b^n\delta v_Q^b\\ F_{41}=0\\ F_{51}=0\end{array}$$

(26)

$$W_{Ct}=H_A\left[\begin{array}{c}{w}_C^b\\ f_C^b\\ 0\\ 0\\ 0\end{array}\right],H_A=diag\left[\begin{array}{ccccc}-C_b^n& C_b^n& 0& 0& 0\end{array}\right]$$

(27)

where $W_{Qt}$ is the equivalent white noise after the conversion of QN, and $W_{Ct}$ is colored noise. $w_C^b$ and $f_C^b$ are defined as:

$$w_C^b={\left[\overrightarrow{y}\begin{array}{ccc}{}_1& {\overrightarrow{y}}_2& {\overrightarrow{y}}_3\end{array}\right]}^T,f_C^b={\left[\begin{array}{ccc}{\overrightarrow{y}}_4& {\overrightarrow{y}}_5& {\overrightarrow{y}}_6\end{array}\right]}^T$$

(28)

where ${\overrightarrow{y}}_i\left(i=1,2,\cdots ,6\right)$ satisfies (22). Combining (22) and (28), $W_{Ct}$ can be rewritten as

$$W_{Ct}=H_A{H}_B\left[\begin{array}{c}{\overrightarrow{x}}_1\\ {\overrightarrow{x}}_2\\ ⋮\\ {\overrightarrow{x}}_6\end{array}\right]$$

(29)

where

$$H_B=\left[diag\left(C_1,\cdots ,C_6\right)\right]$$

(30)

Therefore, the state-space of $W_{Ct}$ is described as:

$${\dot{\overrightarrow{x}}}_i\left(t\right)=F_{yy}{\overrightarrow{x}}_i\left(t\right)+{\overrightarrow{u}}_y\left(t\right)$$

(31)

$$W_{Ct}=F_{xy}\overrightarrow{x}\left(t\right)$$

(32)

where $F_{xy}=diag(A_1,\dots ,A_6)$, ${\overrightarrow{u}}_y\left(t\right)={\left[\begin{array}{ccc}{B}_1{\overrightarrow{u}}_1\left(t\right)& \cdots & B_6{\overrightarrow{u}}_6\left(t\right)\end{array}\right]}^T$, $F_{xy}=H_A{H}_B$.

The process noise covariance matrix $Q_C$ of (31) satisfies:

$$E\left[\left({\overrightarrow{u}}_y\left(t\right)\right){\left(u_y\left(t-\tau \right)\right)}^T\right]=Q_y\delta \left(\tau \right)$$

(33)

List ${\overrightarrow{x}}_i={\left[{\overrightarrow{x}}_1,{\overrightarrow{x}}_2,\cdots ,{\overrightarrow{x}}_6\right]}^T$ as a new state variable. Then (24) can be rephrased as

$$\left[\begin{array}{c}\dot{x}\left(t\right)\\ \dot{\overrightarrow{x}}\left(t\right)\end{array}\right]=\left[\begin{array}{cc}{F}_{xx}& F_{xy}\\ 0& F_{yy}\end{array}\right]\left[\begin{array}{c}x\left(t\right)\\ \overrightarrow{x}\left(t\right)\end{array}\right]+\left[\begin{array}{c}{W}_{Qt}\left(t\right)\\ {\overrightarrow{u}}_y\left(t\right)\end{array}\right]$$

(34)

The process noise covariance matrix $Q_C$ of (34) satisfies:

$$E\left[\left(\begin{array}{c}{W}_{Qt}\left(t\right)\\ {\overrightarrow{u}}_y\left(t\right)\end{array}\right){\left(\begin{array}{c}{W}_{Qt}\left(t\right)\\ {\overrightarrow{u}}_y\left(t\right)\end{array}\right)}^T\right]=Q\delta \left(\tau \right)$$

(35)

3.2.2. Measurement Equation

It is assumed that $N$ available UWB BSs are set, the difference between the position is determined by INS and the distance measured by UWB is

$$z_i=‖p_{Ai}^n-p_{INS}^n‖-r_i,i=1,2,\cdots ,N$$

(36)

where $p_{Ai}^n=\left(x_{Ai},y_{Ai},z_{Ai}\right)$ is the coordinate of the UWB BS in n-frame. $p_{INS}^n=\left(x_{INS},y_{INS},z_{INS}\right)$ is the position estimated by INS. $r_i$ represents the distance between the tag and the $ith$ base station.

The measurement of the system is selected as:

$$z={\left[\begin{array}{cccc}{z}_1& z_2& \cdots & z_N\end{array}\right]}^T$$

(37)

The measurement equation is:

$$z_k=f\left(x_k\right)+v_k$$

(38)

with

$$f\left(x_k\right)=\sqrt{{\left(x_{Ai}-x_{INS,k}\right)}^2+{\left(y_{Ai}-y_{INS,k}\right)}^2+{\left(z_{Ai}-z_{INS,k}\right)}^2}-r_i$$

(39)

where $v_k$ is measurement noise, that is, UWB distance measurement noise. The positioning error of INS is:

$$x_{INS,k}=x_{k+1}+\delta x_{k+1},y_{INS,k}=y_{k+1}+\delta y_{k+1},z_{INS,k}=z_{k+1}+\delta z_{k+1}$$

(40)

Substituting (40) to (38), then, the Taylor series expansion is carried out at $\left(0,0,0\right)$ to obtain the linearized measurement equation.

$$z_k=H_k{x}_k+v_k$$

(41)

with

$$H_k=\left[\begin{array}{ccccc}{0}_{N\times 3}& J_{\left(0,0,0\right)}& 0_{N\times 3}& 0_{N\times 3}& 0_{N\times 3}\end{array}\right]$$

(42)

where $J_{\left(0,0,0\right)}$ represents the Jacobian matrix of (41) at $\left(0,0,0\right)$.

$$J_{\left(0,0,0\right)}=\left[\begin{array}{ccc}\frac{x_{INS,k}-x_{A1}}{‖p_{A1}^n-p_{INS}^n‖}& \frac{y_{INS,k}-y_{A1}}{‖p_{A1}^n-p_{INS}^n‖}& \frac{z_{INS,k}-z_{A1}}{‖p_{A1}^n-p_{INS}^n‖}\\ \frac{x_{INS,k}-x_{A2}}{‖p_{A2}^n-p_{INS}^n‖}& \frac{x_{INS,k}-x_{A2}}{‖p_{A2}^n-p_{INS}^n‖}& \frac{x_{INS,k}-x_{A2}}{‖p_{A2}^n-p_{INS}^n‖}\\ ⋮& ⋮& ⋮\\ \frac{x_{INS,k}-x_{AN}}{‖p_{AN}^n-p_{INS}^n‖}& \frac{x_{INS,k}-x_{AN}}{‖p_{AN}^n-p_{INS}^n‖}& \frac{x_{INS,k}-x_{AN}}{‖p_{AN}^n-p_{INS}^n‖}\end{array}\right]$$

(43)

3.3. MAEKF Based on the Dynamic Weight Function for UWB NLOS

In the EKF algorithm, the noise characteristics, including the system noise and measurement noise, are subordinated to the Gaussian distribution. However, the state estimation will be interfered when the system is suffering from NLOS. To assess the impact of NLOS on the current position, an optimization-based EKF algorithm is proposed which can use the probability density function based on the Gaussian distribution to adjust the noise adaptively.

Let’s introduce a NLOS discrimination index $DI$ as

$$DI=\left|‖p_{A,k}^n-p_{INS,k}^n‖-r_k\right|$$

(44)

with

$$r_k={\overline{r}}_k+\mathrm{\Delta }{r}_k+v_{r_k},$$

(45)

where $p_{A,k}^n$ is the $kth$ coordinate of the BS; and $p_{INS,k}^n$ denotes the $kth$ position estimated by INS; ${\overline{r}}_k$ denotes the veritable distance from the tag to the BS; $\mathrm{\Delta }r$ is defined as the systematic error term, such as NLOS error; $v_{r_k}$ is the ranging error.

It is considered that $p_{INS,k}^n$ is not affected by the NLOS and the base station coordinates are fixed. Accordingly, it indicates that the UWB signal is interfered by NLOS during transmission under the condition that the value of $DI$ is too small. Assuming $DI$ satisfies a Gaussian distribution with a mean of 0 and a variance of ${\sigma }_{DI}^2$, i.e., $DI\sim \left(0,{\sigma }_{DI}^2\right)$. Consequently, the probability density function of $DI$ can be used to evaluate the degree to which the current UWB signal is affected by the NLOS, as shown in (46).

$$p={\displaystyle {\int }_0^{\left|DI\right|}\frac{2}{{\sigma }_{DI}\sqrt{2\pi }}}{e}^{-\frac{x^2}{2{\sigma }_{DI}^2}}dx$$

(46)

Therefore, the dynamic weight function is designed in the following form:

$$S(p_i)=\left\{\begin{array}{l}\left(1+p_i\right),p_i\ge th\\ 1,p_i<th\end{array}\right.$$

(47)

where $th$ denotes the threshold, determined by the empirical value.

Based on (47), the adaptive measurement noise is expressed as:

$$R_k=R_0\cdot S(p_i)$$

(48)

In summary, the framework of the proposed method in this paper is presented in Figure 3.

4. Experimental Test and Analysis

In order to validate the availability and superiority of the proposed algorithm in the MEMS-Based INS/UWB integrated positioning system, ground vehicle experiments were carried out. In the experiment, the independently developed MEMS-based INS/UWB integrated position system and reference integrated navigation system with high accuracy are included. Sensor parameter configurations in the MEMS-Based INS/UWB integrated navigation system are described in

Table 1. Relevant parameter information of sensors.

Sensor	Parameter
	Range	Constant Drifts	Random Walk
gyro	$\pm {350}^{\circ }$	$10\mathrm{deg}/\mathrm{h}$	$0.3\mathrm{deg}/\sqrt{\mathrm{h}}$
acc	$\pm 10 \mathrm{g}$	$5 \mathrm{mg}$	$90 \mathsf{\mu }\mathrm{g}/\sqrt{\mathrm{Hz}}$

. The intention of the high-precision reference integrated navigation system is to furnish the high-precision reference of navigation information for the MEMS-Based INS/UWB integrated navigation system. It is composed of LCI-1 strategic IMU, Propak satellite receiver and two GNSS antennas. The accuracy of attitude, velocity and position provided by the high-precision reference integrated navigation system are 0.01°, 0.05 m/s and 0.1 m, respectively. The trajectory of the vehicle in the experiment is shown in Figure 4.

Two experimental schemes were designed to analyze the performance of the proposed algorithm under different conditions. The proposed algorithm was also compared with the existing EKF method and AEKF method. The details of the experimental schemes are as follows:

(1) The experiment was conducting where UWB signal is good. This allows normal measurement of UWB. This can ensure the normal and effective operation of various navigation sensors. The whole process lasted for 166 s.

(2) We actively and intermittently blocked the UWB signal in some time periods in order to simulate a possible NLOS situation. In this way, INS can be measured normally, while UWB may have poor navigation and positioning performance or even abnormal measurement information. The whole process lasted 166 s.

4.1. Performance Comparison of Different Methods for Good UWB Signal

This test was conducted according to the first experimental scheme. Figure 5, Figure 6 and Figure 7 compare errors of attitude, velocity and position of the proposed algorithm with the other two algorithms. The yellow, blue and red lines correspond to the EKF method, the AEKF method and the proposed method, respectively. It can be observed that the fluctuation of the red line is the smallest compared to the yellow and blue lines, meaning that errors obtained by the proposed method in this paper are smaller than those by the EKF and AEKF method.

The root mean square error (RMSE) of errors of navigation information (attitude, velocity, position) of three-axes under different algorithms are listed in

Table 2. The RMSE of navigation error in test scheme 1.

Method	Attitude Error (°)			Velocity Error (m/s)			Position Error (m)
	Pitch	Roll	Yaw	East	North	Up	East	North	Up
EKF	0.745	0.426	1.536	0.166	0.197	0.142	0.161	0.165	0.158
AEKF	0.461	0.227	0.968	0.136	0.146	0.102	0.124	0.126	0.119
Proposed	0.224	0.119	0.424	0.062	0.076	0.052	0.066	0.071	0.062

. RMSE is defined as:

$$RMSE=\sqrt{\frac{1}{N}{\displaystyle \sum _{i=1}^N{\left(x_i-{\widehat{x}}_i\right)}^2}}$$

(49)

where $N$ represents the total number of samples, $x_i$ represents the navigation parameters provided by INS/UWB integrated navigation system at time $i$, ${\widehat{x}}_i$ represents $ith$ the navigation parameters provided by the reference system. The purpose of RMSE is to weigh the deviation between the measured value and real value. The smaller the RMSE, the higher the measurement accuracy.

In order to compare the navigation performance of the proposed algorithm with the other two methods more clearly and intuitively in the first scheme, the radar map of navigation parameter error statistics (RMSE) of the three methods are shown in Figure 8.

From the analysis of above error results, it can be observed that the proposed algorithm and AEKF improve the accuracy of the MEMS-based INS/UWB integrated navigation system over the EKF method. Similarly, the proposed algorithm produces obvious improvement in terms of the accuracy of attitude, velocity and position compared with the existing AEKF method. According to Table 2, the proposed method can obtain better error suppression effect. Compared with the EKF algorithm, the proposed method reduces the RMSE error in attitude, velocity and position from 0.745°, 0.426°, 1.536°; 0.166 m/s, 0.197 m/s, 0.142 m/s; and 0.161 m, 0.165 m, 0.158 m to 0.224°, 0.119°, 0.424°; 0.062 m/s, 0.076 m/s, 0.052 m/s; and 0.066 m, 0.071 m, 0.062 m, respectively, indicating a significant accuracy improvement by 69%, 72%, 72%, 62%, 61%, 63%, 59%, 56% and 60%, respectively. Compared with the existing AEKF method, the proposed method improved attitude accuracy by over 47% and positioning accuracy by over 45%. The reason is that the traditional EKF and AEKF are significantly affected by the stochastic error of inertial sensors. The proposed method uses AV to establish a more accurate INS error model, which suppresses the stochastic error increase of the inertial sensors during the time update stage of filtering. Therefore, its estimation performance is superior to traditional EKF and AEKF. It can also be seen from Figure 8 that the navigation performance of the proposed method is generally better than that of AEKF and EKF in a good navigation environment.

4.2. Performance Comparison of Different Methods for UWB NLOS

This test was performed by applying the second experimental scheme. In scheme 2, the measurement of UWB will be abnormal due to human interference. Figure 9, Figure 10 and Figure 11, respectively, show errors of attitude, velocity and position of the proposed algorithm and the other two methods when the UWB undergoes NLOS. In addition, the corresponding RMSE under three methods is listed in

Table 3. The RMSE of navigation error in test scheme 2.

Method	Attitude Error (°)			Velocity Error (m/s)			Position Error (m)
	Pitch	Roll	Yaw	East	North	Up	East	North	Up
EKF	1.763	1.508	2.329	0.581	0.494	0.491	0.937	0.858	0.843
AEKF	1.438	1.269	2.073	0.517	0.408	0.399	0.863	0.716	0.796
Proposed	0.485	0.397	0.731	0.132	0.104	0.097	0.198	0.156	0.152

. The radar map of the navigation parameter error statistical data (RMSE) of the three methods is plotted as Figure 12.

By analyzing Figure 9, Figure 10 and Figure 11 and Table 3, we evaluated the performance of the proposed algorithm and other two methods. When affected by NLOS, the performance of the MEMS-based INS/UWB integrated system based on traditional EKF mainly relies on INS for navigation. Due to the significant impact of stochastic errors of inertial sensor, the performance of EKF degrades quickly. It can be observed from Table 3 that the proposed method reduces RMSE errors in attitude, velocity and position more than 72%, 77% and 78%, respectively, compared to the EKF method. It indicates that the proposed method outperforms the EKF method. Table 3 also shows that attitude accuracy is improved by more than 60% and position accuracy is improved by more than 70% compared with the existing AEKF method. The reason is that the proposed method designed a dynamic weight function, which can effectively solve the impact of NLOS on the AEKF method. Moreover, the proposed method suppresses the influence of stochastic noise from inertial sensor during the AEKF time update stage by using AV to assist in the establishment of the INS error model. As a result, the proposed method outperforms the AEKF method. It can also be clearly seen from Figure 12 that the proposed method has better navigation performance than AEKF and EKF.

Further, the navigation performance of EKF, AEKF and the proposed method under different test schemes is compared, as shown in Figure 13.

It can be clearly seen from Figure 13 that when the vehicle experiment is conducted from the first scheme to the second scheme, that is, the normal operation of each sensor changes to the abnormal output of some sensors, the navigation performance of the EKF method decreases most significantly. The navigation performance of AEKF method has also decreased, and the error has increased significantly, which indicates that the NLOS error has a great impact on the navigation accuracy of AEKF. However, the proposed method can still achieve better navigation performance under such conditions.

5. Conclusions

In this paper, an improved navigation method, Allan variance (AV) to assist a modified adaptive extended Kalman Filter based on the dynamic weight function (DWF-MAEFF), is proposed to further enhance the performance of MEMS-based INS/UWB integrated systems. A better INS errors model is obtained by using AV to model stochastic errors of inertial sensors. In addition, the MAEKF algorithm is proposed to design the dynamic weight function, which can adjust the weight of measurement noise reasonably and dynamically in order to further enhance and improve the robustness of the AEKF algorithm. The experimental results showed that the proposed method can improve the positioning accuracy by more than 60% compared with existing integrated navigation methods based on EKF and AEKF. In future work, we will integrate GNSS into the INS/UWB fusion scheme to achieve seamless positioning of UGVs.