Performance Enhancement of INS and UWB Fusion Positioning Method Based on Two-Level Error Model

Abstract

In GNSS-denied environments, especially when losing measurement sensor data, inertial navigation system (INS) accuracy is critical to the precise positioning of vehicles, and an accurate INS error compensation model is the most effective way to improve INS accuracy. To this end, a two-level error model is proposed, which comprehensively utilizes the mechanism error model and propagation error model. Based on this model, the INS and ultra-wideband (UWB) fusion positioning method is derived relying on the extended Kalman filter (EKF) method. To further improve accuracy, the data prefiltering algorithm of the wavelet shrinkage method based on Stein’s unbiased risk estimate–Shrink (SURE-Shrink) threshold is summarized for raw inertial measurement unit (IMU) data. The experimental results show that by employing the SURE-Shrink wavelet denoising method, positioning accuracy is improved by 76.6%; by applying the two-level error model, the accuracy is further improved by 84.3%. More importantly, at the point when the vehicle motion state changes, adopting the two-level error model can provide higher computational stability and less fluctuation in trajectory curves.

1. Introduction

In global navigation satellite system-denied (GNSS-denied) environments, the multi-sensor fusion positioning method is mainstream, and most of them are based on INS to realize fusion positioning. With the development of micro-electro-mechanical system (MEMS) technology, applications of the inertial navigation system (INS) on small UAVs are becoming more and more extensive. This is because compared to high-precision inertial sensors, MEMS-IMU has the advantages of small size and low cost. These merits of MEMS-IMU present an attractive option for advanced applications, such as intelligent navigation and positioning in GNSS-denied environments [1]. Nevertheless, the drawbacks of high noise levels, instability of characteristics and large stochastic variance make it a challenge to use MEMS-IMU for extended periods. The noise (or error) estimation and compensation of MEMS-IMU are the key points to realize the high-precision positioning of INS-based fusion methods.

At present, INS error estimation and compensation methods are divided into two aspects: one is modeling the errors of MEMS-IMU from a mechanism level [2,3,4,5,6,7]; the other is establishing the error propagation model based on the principle of navigation. In terms of IMU error modeling methods, especially MEMS-IMU, some studies divide IMU errors into two types, which are deterministic errors, such as scale factor, bias and misalignment, and stochastic errors such as bias instability and scale factor instability [2]. The measurement of deterministic error is the main part of this type of error compensation algorithm, and extensive experimentation is required to determine the error parameters [2,3,4]. The authors of [2] summarized the methodology of how to define deterministic errors by a 27-state static test setup and a 60-state dynamic test setup and how these errors were used in error compensation models. A novel dynamic test setup was proposed in [3], which overcame the limitations in the conventional calibrations with the static or quasi-static test setups. Although an accurate error model can be established based on the above methods, many parameters need to be calibrated, and the calibration process is complicated. Other studies model inertial sensor errors from the perspective of random processes, considering MEMS-IMU errors as stochastic noise [5,8,9,10,11]. The authors of [5] proposed a model for the combined residuals and random errors, where residuals referred to the error left after the compensation of deterministic errors, and the model was still stochastic. For random noise modeling, the commonly used methods are the Allan variance [8], PSD [9], the Gauss–Markov (GM) model, the autoregressive (AR) model [10], etc. The above methods are all based on models to realize error estimation and compensation. The authors of [12,13,14,15,16] introduced unknown inputs into systems to reduce the impact of error (or noise) on the accuracy of filter estimations. Because the unknown input of the system is usually generated by factors such as environmental disturbances and mutations, it is difficult to establish a corresponding model and obtain prior information. The author of [12] derived the robust two-stage Kalman filter based on the U-V transform, which was an unknown input decoupled filter. In this method, filters were divided into bias estimator and bias-free estimator. Different from designing an unknown input decoupled filter, [13] developed the optimal filter based on the minimum variance unbiased estimation and realized the simultaneous estimation of the input and the state of a linear discrete-time system. The authors of [14] introduced an internal model approach to unknown input problems and generalized the classical internal model principle to the case with arbitrary unknown disturbances. This is a generalized model for a certain class of problems. The authors of [15] proposed a robust three-stage unscented Kalman filter and solved the unknown input problem in navigation for the Mars entry phase. The authors of [16] improved the accuracy of multiple homogeneous MEMS gyroscope fusions when suffering from unknown environmental disturbances. This article considers reducing the measurement error of IMU from the perspective of the model method and does not conduct an in-depth study of the problems of systems with unknown inputs. We summarize a set of error modeling methods. It is different from unknown input problems in that the relatively accurate prior information of noise can be obtained by using Allan variance. Additionally, the estimation errors are compensated for at the mechanism level and propagation level.

Another way to analyze and compensate for an INS error is by building an error propagation model based on navigation principles [17,18,19,20,21]. There are two basic approaches to the deviation in INS error models in the literature: the phi-angle approach (or the true frame approach) and the psi-angle approach (or the computer frame approach) [17,18]. The attitude calculation in INS has three approaches: Euler angle, direction cosine matrix and quaternion. The quaternion method has received extensive attention for the advantages of less computation, higher accuracy and avoiding singularities [20,21]. Nevertheless, most of the error propagation models were established based on the geographic frame and relied on latitude and longitude to provide location information. In GNSS-denied environments, such as an indoor environment, these error models were inconvenient to use. Meanwhile, there are a few pieces of research combining the mechanism error model with the propagation error model.

This paper aims to comprehensively utilize the mechanism error model and propagation error model, propose the two-level error model and summarize a complete set of methods from data prefiltering to data fusion. The rest of this article is organized as follows. In Section 2, we introduce the data prefiltering method in relation to raw IMU data. In Section 3, we propose the two-level error model and EKF fusion method based on this. The different experiments and results are shown in Section 4. Section 5 summarizes this article.

2. Data Prefiltering

The vast majority of multi-sensor positioning methods are based on INS, which rely on IMU, especially MEMS-IMU, to achieve positioning and navigation. MEMS-IMU includes one accelerometer and one gyroscope, which can measure three-axes acceleration and three-axes angular velocity. The measurements of MEMS-IMU always contain a certain amount of noise, which leads to non-negligible drifts in vehicle path estimation due to time integration. There are two categories of noise: short-term noise with a high frequency and long-term noise with a low frequency. In the time domain, the vehicle motion information mixes with such noise, and they cannot be distinguished directly. While in the frequency domain, the movement frequency of a vehicle is usually no more than 20 Hz, and a land vehicle is usually below 5 Hz [22], whereas short-term noise can reach more than 50 Hz. Therefore, we need to employ data prefiltering methods before utilizing the two-level error model to eliminate short-term noise whose frequency is around the vehicle movement frequency.

2.1. Wavelet Denoising Method

Fourier transform (FT) is the most common and extremely useful method for frequency analysis. However, through transforming the signal to the frequency domain by FT, the time-domain information is lost. Short-time Fourier transform (STFT) can overcome this drawback by adding a window function. However, both time localization and frequency resolution cannot be optimal at the same time. Therefore, performing a wavelet transform to realize IMU signal decomposition in both the time and frequency domains is a superior method.

From multiresolution analysis, $\varphi \left(x\right)$ and $\psi \left(x\right)$ are the scaling function and wavelet function, respectively, and they satisfy the following equations:

$$\varphi \left(x\right)=\sqrt{2}{\displaystyle \sum _n{h}_n\varphi \left(2x-n\right)}$$

(1)

$$\psi \left(x\right)=\sqrt{2}{\displaystyle \sum _k{g}_k\varphi \left(2x-k\right)}$$

(2)

The coefficients of the scaling and wavelet functions obey the following equations:

$$h_n=\sqrt{2}{\displaystyle \int \varphi \left(x\right)\varphi \left(2x-n\right)dx}$$

(3)

$$g_k=\sqrt{2}{\displaystyle \int \psi \left(x\right)\varphi \left(2x-k\right)dx}$$

(4)

where $h_n$ has the characteristic of a finite impulse response (FIR) low-pass digital filter, and the FIR digital filter that consists of the wavelet coefficient, $g_k$, is a high-pass digital filter [23]. By performing discrete-time FT as Equations (5) and (6), we can get the frequency response of both filters.

$$H\left(w\right)={\displaystyle \sum _{n=-\infty }^{\infty }{h}_n{e}^{-inw}}$$

(5)

$$G\left(w\right)={\displaystyle \sum _{k=-\infty }^{\infty }{g}_k{e}^{-ikw}}$$

(6)

Based on the Mallat algorithm [24] and the filter bank method, the wavelet denoising method is divided into three parts: decomposition, threshold denoising method and reconstruction. For example, the three-level decomposition is shown in Figure 1. $x\left(n\right)$ is the raw IMU signal in the time domain. By applying convolution with digital filter banks derived from the scaling and wavelet functions, the raw signal is decomposed into the wavelet coefficient, $d_1$, with high frequency and the scaling coefficient, $c_1$, with low frequency. Both $d_1$ and $c_1$ are in the frequency domain. After three times repetitions, there are four parts of the signal corresponding to different frequency intervals. The coefficients containing motion information have a large amplitude, so setting a suitable thresholding in specific methods can eliminate a certain amount of noise. After thresholding denoising, by implementing the inverse wavelet transform that is $\tilde{G}$ and $\tilde{H}$ in Figure 1 on ${\tilde{c}}_3$ and ${\tilde{d}}_3$, we can get the reconstructed coefficient, ${\tilde{c}}_2$. Additionally, if repeated three times, the denoised IMU signal, $\tilde{x}\left(n\right)$, can be obtained. In the decomposition part, digital filter banks and downsampling are taken to realize the discrete wavelet transform. While in the reconstruction part, digital filter banks and upsampling are taken to reconstruct the signal.

Hard thresholding [25] and soft thresholding [26] are two common thresholding methods, and some scholars have conducted in-depth research on the selection of thresholds [25,27,28,29]. In the hard-thresholding method, coefficients with amplitudes less than thresholding are set to 0, and coefficients that are greater than thresholding remain unchanged, and the equation is as (7).

$$T^{hard}\left(x\right)=\left\{\begin{array}{c}0,\hspace{1em}\left|x\right|\le T\\ x,\hspace{1em}\left|x\right|>T\end{array}\right.$$

(7)

While in the soft-thresholding method, coefficients with magnitudes less than thresholding are set to 0, and coefficients with magnitudes larger than thresholding are reduced to the difference in them, and the equation is

$$T^{soft}\left(x\right)=\left\{\begin{array}{cc}\hfill 0,\hfill & \left|x\right|\le T\hfill \\ \hfill \mathrm{sgn}\left(x\right)\left(\left|x\right|-T\right),\hfill & \left|x\right|>T\hfill \end{array}\right.$$

(8)

Considering the characteristics of the raw IMU signal, this paper presents the soft-thresholding method for IMU data denoising. More detailed information will be introduced in Section 2.2.

2.2. Implementation Details

The fundamental theory of the wavelet shrinkage method is that wavelet function has a better time–frequency property, and discrete wavelet transform (DWT) has an ability to “focus” on signals because of its multiresolution property. The “focus” ability makes signal energy fasten on several coefficients, while noise is evenly distributed across the whole scale space, and this is determined by the distribution property of noise.

The IMU data acquired from MEMS-IMU usually include a large amount of noise; one reason is that MEMS-IMU lacks high accuracy, and the other is that the movements of a vehicle always bring vibrations, and this affects the measurement of MEMS-IMU. We perform two levels of wavelet decomposition on account of the UAV motion frequency and IMU sampling frequency. Based on the soft-thresholding method, we selected the SURE-Shrink algorithm to evaluate the noise threshold. This algorithm is a hybrid algorithm based on the SURE threshold and Universal threshold; it considers the differences in the statistical properties of the coefficients of different wavelet sub-bands, and it is one of the best sub-band adaptive wavelet shrinkage algorithms. The calculation is as follows:

$$T=\left\{\begin{array}{c}{T}_{sure},\hspace{1em}{S}_N^2\le \frac{{\eta }_N}{\sqrt{N}}\\ T_{univ},\hspace{1em}{S}_N^2>\frac{{\eta }_N}{\sqrt{N}}\end{array}\right.$$

(9)

where

$${\eta }_N={\log }_2{\left(N\right)}^{3/2}$$

(10)

$$S_N^2={\displaystyle \sum _t\left(Y_t^2-1\right)/N}$$

(11)

Here, $N$ is the number of coefficients of the wavelet sub-band, and $Y_t$ is the t-th coefficient of the current wavelet sub-band. The two thresholds, $T_{univ}$ and $T_{sure}$, are calculated by

$$T_{univ}={\sigma }_n\sqrt{2\ln N}$$

(12)

$$T_{sure}=\arg \underset{T>0}{\min }\left\{N\cdot {\sigma }_n^2+{\displaystyle \sum _{i=0}^{N-1}\max \left\{Y_i,T^2\right\}-2{\sigma }_n^2\#\left\{\left|Y_t\right|\le T\right\}}\right\}$$

(13)

where $\#\left\{\cdot \right\}$ represents the number of elements that satisfy the conditions. From [25], ${\sigma }_n$ can be estimated as the median absolute deviation in the wavelet coefficients at the finest level and divided by 0.6745. The finest level of the wavelet coefficients contains the highest frequency level of the signal, which we think most of them are noise. Additionally, the equation form is

$${\sigma }_n=median\left(\left|d_N-median\left(d_N\right)\right|\right)/0.6745$$

(14)

Here, $d_N$ is the wavelet coefficients of the finest level. Additionally, the flow chart of the wavelet shrinkage algorithm is shown in Figure 2.

The denoising results are detailed in Section 4.1.2.

3. Methodology

In this chapter, according to the mechanism of MEMS-IMU errors and the motion characteristics of UAV, we introduce the two-level error model EKF method in detail, which is the main innovation of this paper. The two-level error model is established from two levels: one is from the mechanism, and the other is from the propagation. In the mechanism error model, we aim to build an error model that can describe the accelerometer and gyroscope sensor errors simply and generally. While in the propagation error model, we intend to establish the error propagation path based on the state equations of the navigation system.

3.1. Mechanism Error Model

From the mechanism of error occurrence, the error variables we focus on are $\delta a$ and $\delta w$, which are errors of state $a$ and $w$. For these two variables, we selected the stochastic process model as the mechanism error model; the arguments are as follows:

• It is convenient to analyze and calculate the parameters of the stochastic model;
• There are little application condition limitations on the stochastic model;
• There is no need to calibrate a large number of parameters in contrast to other methods.

The common stochastic models used to describe errors are the autoregressive (AR) model and Gauss–Markov (GM) process model. More detailed information about the mechanism error model is discussed in Section 3.1.1 and Section 3.1.2.

3.1.1. Stochastic Process Model

In most KF implementations for the INS-based fusion positioning method, the first-order GM model is used to describe inertial sensor errors with a decaying exponential autocorrelation sequence [30,31,32,33]. The first-order GM model for an inertial sensor error is given as:

$$\dot{x}\left(t\right)=-\beta x\left(t\right)+\sqrt{2\beta {\sigma }^2}w\left(t\right)$$

(15)

Here, $\beta$ is the reciprocal of correlation time, and ${\sigma }^2$ is the variance in system noise $w\left(t\right)$. The discrete-time equation is written as follows:

$$x_k=\left(I-\beta \Delta t\right)x_{k-1}+\sqrt{2\beta {\sigma }^2}{w}_k\Delta t$$

(16)

where $\Delta t$ is the time interval.

The other stochastic model of inertial sensor errors is the higher-order AR model. To use this model, long-term measurements from each inertial sensor while stationary are required for computing the higher-order AR model. The pth-order AR model for a discrete-time-domain sequence can be described by the following difference equation:

$$y\left(n\right)=-{\displaystyle \sum _{k=1}^p{\alpha }_{k}y\left(n-k\right)+{\beta }_{0}w\left(n\right)}$$

(17)

where ${\alpha }_1,{\alpha }_2,{\alpha }_3,\dots ,{\alpha }_p$ are the model parameters, and ${\beta }_0$ is standard deviation in sensor white noise. However, to achieve higher accuracy, the model needs to use a higher order. Since the AR model is applied to all six axes of inertial sensors, each increase in the model order will lead to six more states added to the KF state error vector.

3.1.2. Implementation Details

Our aim in this paper is not to establish a noise model to describe the physical properties of sensor errors in detail but merely to derive generic, simple noise models that are suitable for the INS-based fusion positioning method. The mechanism error model is derived from (16); in the discrete-time domain, the equation is as follows:

$$x_{k+1}=\left(I-\frac{\Delta t}{{\tau }_b}\right)x_k+w_k+v_k$$

(18)

where $x_k$ is the slowly varying process with the correlation time, ${\tau }_b$, in the discrete-time domain, $w_k$ is the white noise component of $x_k$, and $v_k$ represents the white noise component of the error model. and $v$ are independent, zero mean, white Gaussian processes of strength ${\sigma }_b$ and ${\sigma }_w$, and in the discrete-time domain:

$$E\left[w_k{w}_k^T\right]=\Delta t\cdot {\sigma }_b^2$$

(19)

$$E\left[v_k{v}_k^T\right]=\frac{{\sigma }_w^2}{\Delta t}$$

(20)

We define $\overline{w}=w+v$, and then

$$E\left[\overline{w}{\overline{w}}^T\right]=\Delta t\cdot {\sigma }_b^2+\frac{{\sigma }_w^2}{\Delta t}$$

(21)

Now the mechanism error model in the discrete-time domain becomes

$$x_{k+1}=\left(I-\frac{\Delta t}{{\tau }_b}\right)x_k+{\overline{w}}_k$$

(22)

When we apply the mechanism error model to six axes of inertial sensors, the parameters that need to be obtained beforehand are $\theta =\left\{{\tau }_b,{\sigma }_b,{\sigma }_w\right\}$. By keeping the IMU stationary for more than 3 h, which means the outputs of IMU only contain noise (including long-term noise and short-term noise), the parameters ${\sigma }_b$, ${\sigma }_w$ can be directly read off of the Allan deviation plot [33], and ${\tau }_b$ is the correlation time of the output signals.

3.2. Propagation Error Model

For the propagation error model, we derive model equations based on the state differential equations of navigation systems. The variables we focus on are $\delta p$, $\delta v$, $\delta q$, which are errors of state $p$, $v$, $q$. We use quaternions, $q$, instead of Euler angles to describe the rotations of navigation systems because solving in quaternions can avoid singularities and the gimbal lock problem. The differential equations that characterize the motion of navigation systems are given as:

$$\dot{p}\left(t\right)=v\left(t\right)$$

(23)

$$\dot{v}\left(t\right)=C\left[q\left(t\right)\right]a\left(t\right)+g\left[p\left(t\right)\right]$$

(24)

$$\dot{q}\left(t\right)=\mathsf{\Omega }\left[w\left(t\right)\right]q\left(t\right)=Q\left[q\left(t\right)\right]w\left(t\right)$$

(25)

where $p,v$ are the position and velocity of the navigation systems, respectively, in the inertial reference frame; $a$ is the specific acceleration vector; the gravitational acceleration $g\left[p\left(t\right)\right]$ changes with the unit position; and the rotation matrix based on Hamilton form can be calculated by

$$C\left[q\left(t\right)\right]=\left[\begin{array}{ll}{q}_0^2+q_1^2-q_2^2-q_3^2\hfill & 2q_1{q}_2-2q_0{q}_3\hfill \\ 2q_0{q}_3+2q_1{q}_2\hfill & q_0^2-q_1^2+q_2^2-q_3^2\hfill \\ 2q_1{q}_3-2q_0{q}_2\hfill & 2q_0{q}_1+2q_2{q}_3\hfill \end{array}\right.\left.\begin{array}{c}2q_0{q}_2+2q_1{q}_3\\ 2q_2{q}_3-2q_0{q}_1\\ q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]$$

(26)

The differential equation of quaternion is given as (27):

$$\dot{q}=\frac{1}{2}q\otimes w=Q(q)·w=\mathsf{\Omega }(w)·q$$

(27)

where $w$ is the angular velocity, and $q={\left[q_w,q_v\right]}^T$ is the Hamilton form of quaternions with the scalar part, $q_w$, and the vector part, $q_v$; in this form, rotation corresponds to the right-hand rule. $\otimes$ denotes quaternion multiplication; the latter two are equivalent forms, which are matrix multiplication rather than quaternion multiplication. Additionally, the matrix $\mathsf{\Omega }\left(w\right),Q\left(q\right)$ is given as:

$$\mathsf{\Omega }(w)=\frac{1}{2}\left[\begin{array}{cccc}0& -w_1& -w_2& -w_3\\ w_1& 0& w_3& -w_2\\ w_2& -w_3& 0& w_1\\ w_3& w_2& -w_1& 0\end{array}\right]=\frac{1}{2}\left[\begin{array}{cc}0& -w^T\\ w& -{\left[w\right]}_{\times }\end{array}\right]$$

(28)

$$Q\left(q\right)=\frac{1}{2}\left[\begin{array}{ccc}-q_1& -q_2& -q_3\\ q_0& -q_3& q_2\\ q_3& q_0& -q_1\\ -q_2& q_1& q_0\end{array}\right]$$

(29)

$${\left[V\right]}_{\times }=\left[\begin{array}{ccc}0& -V_3& V_2\\ V_3& 0& -V_1\\ -V_2& V_1& 0\end{array}\right]$$

(30)

Equations (23)–(25) characterize the true attitude and position of a navigation system based on true specific acceleration and angular velocity. However, in an actual system, only measurements are available. For a quantity $z$, $z_m=$ the measurement of $z$ and $\delta z=$ the error of $z$. Additionally, the true quantity can be characterized by the sum of measurement and error. In the navigation system, there are equations listed as (31)–(35).

$$p=p_m+\delta p$$

(31)

$$v=v_m+\delta v$$

(32)

$$q=q_m+\delta q$$

(33)

$$a=a_m+\delta a$$

(34)

$$w=w_m+\delta w$$

(35)

The differential equation of the position error can be derived as follows:

$$\delta \dot{p}=\dot{p}-{\dot{p}}_m=v-v_m=\delta v$$

(36)

The differential equation of the velocity error can be derived as:

$$\begin{array}{c}\delta \dot{v}=\dot{v}-{\dot{v}}_m=C\left(q\right)a+g\left(p\right)-C\left(q_m\right)a_m-g\left(p_m\right)\hfill \\ =C\left(q_m+\delta q\right)\left(a_m+\delta a\right)+g\left(p_m+\delta p\right)-C\left(q_m\right)a_m-g\left(p_m\right)\hfill \end{array}$$

(37)

The rotation matrix and gravitational acceleration can be approximated with measurements, with first-order errors, as (38) and (39).

$$C\left(q_m+\delta q\right)=C\left(q_m\right)+\frac{\partial C}{\partial q_m}·\delta q$$

(38)

$$g\left(p_m+\delta p\right)=g\left(p_m\right)+\frac{\partial g}{\partial p_m}·\delta p$$

(39)

Then, the new differential Equation (40) of the velocity error can be obtained from (38) and (39). Considering sufficiently small errors, the products of errors can be neglected, and (40) can finally become (41).

$$\delta \dot{v}=\left[C\left(q_m\right)+\frac{\partial C}{\partial q_m}·\delta q\right]·\left(a_m+\delta a\right)+\left[g\left(p_m\right)+\frac{\partial g}{\partial p_m}·\delta p\right]-C\left(q_m\right)a_m-g\left(p_m\right)$$

(40)

$$\delta \dot{v}=C\left(q_m\right)·\delta a+\frac{\partial C}{\partial q_m}{a}_m·\delta q+\frac{\partial g}{\partial p_m}·\delta p$$

(41)

The differential equation of quaternion can be derived as

$$\begin{array}{c}\delta \dot{q}=\dot{q}-{\dot{q}}_m=\mathsf{\Omega }\left(w_m+\delta w\right)·\left(q_m+\delta q\right)-Q\left(q_m\right)·w_m\hfill \\ =\mathsf{\Omega }\left(w_m+\delta w\right)·\delta q+\mathsf{\Omega }\left(w_m+\delta w\right)·q_m-Q\left(q_m\right)·w_m\hfill \end{array}$$

(42)

It can be easily approved that $Q\left(q\right)·w=\mathsf{\Omega }\left(w\right)·q$, and (42) becomes

$$\begin{array}{c}\delta \dot{q}=\mathsf{\Omega }\left(w_m+\delta w\right)\delta q+Q\left(q_m\right)\left(w_m+\delta w\right)-Q\left(q_m\right)w_m\hfill \\ =\mathsf{\Omega }\left(w_m+\delta w\right)\delta q+Q\left(q_m\right)\delta w\hfill \end{array}$$

(43)

Neglecting the products of small errors, the error equation of quaternions becomes

$$\delta \dot{q}=\mathsf{\Omega }\left(w_m\right)\delta q+Q\left(q_m\right)\delta w$$

(44)

The propagation error model can finally be obtained:

$$\delta \dot{p}=\delta v$$

(45)

$$\delta \dot{v}=C\left(q_m\right)·\delta a+\frac{\partial C}{\partial q_m}{a}_m·\delta q+\frac{\partial g}{\partial p_m}·\delta p$$

(46)

$$\delta \dot{q}=\mathsf{\Omega }\left(w_m\right)\delta q+Q\left(q_m\right)\delta w$$

(47)

3.3. Error Model EKF Method

3.3.1. Basic EKF Method

The continuous-time nonlinear system’s state equation is

$$\dot{x}=f\left(x,u,w,t\right),\hspace{1em}w~\left(0,Q\right)$$

(48)

Additionally, the continuous-time nonlinear measurement equation is

$$y=h\left(x,v,t\right),\hspace{1em}v~\left(0,R\right)$$

(49)

The discrete-time nonlinear system equations can be obtained by discretization, and (48) and (49) become

$$x_k=f_{k-1}\left(x_{k-1},u_{k-1},w_{k-1}\right),\hspace{1em}{w}_k~\left(0,Q_k\right)$$

(50)

$$y_k=h_k\left(x_k,v_k\right),\hspace{1em}{v}_k~\left(0,R_k\right)$$

(51)

Applying Taylor series expansion and keeping the first order at $x_{k-1}={\widehat{x}}_{k-1}^{+}$, the state Equation (50) becomes

$$x_k=F_{k-1}{x}_{k-1}+{\tilde{u}}_{k-1}+L_{k-1}{w}_{k-1}$$

(52)

Here,

$$F_{k-1}={\left.\frac{\partial f_{k-1}}{\partial x}\right|}_{{\widehat{x}}_{k-1}^{+}},L_{k-1}={\left.\frac{\partial f_{k-1}}{\partial w}\right|}_{{\widehat{x}}_{k-1}^{+}}$$

(53)

$${\tilde{u}}_{k-1}=f_{k-1}\left({\widehat{x}}_{k-1}^{+},u_{k-1},0\right)-F_{k-1}{\widehat{x}}_{k-1}^{+}$$

(54)

Additionally, the nonlinear measurement equation becomes (55) after a first-order approximation at $x_k={\widehat{x}}_k^{-}$.

$$y_k=H_k{x}_k+z_k+M_k{v}_k$$

(55)

Here,

$$H_k={\left.\frac{\partial h_k}{\partial x}\right|}_{{\widehat{x}}_k^{-}},M_k={\left.\frac{\partial h_k}{\partial v}\right|}_{{\widehat{x}}_k^{-}}$$

(56)

$$z_k=h_k\left({\widehat{x}}_k^{-},0\right)-H_k{\widehat{x}}_k^{-}$$

(57)

Here, the basic Kalman filter equations can be applied after the above discretization and linearization; there are the state estimations of (58) and (59) and the measurement updates of (60)–(62).

$$P_k^{-}=F_{k-1}{P}_{k-1}^{+}{F}_{k-1}^T+L_{k-1}{Q}_{k-1}{L}_{k-1}^T$$

(58)

$${\widehat{x}}_k^{-}=f_{k-1}\left({\widehat{x}}_{k-1}^{+},u_{k-1},0\right)$$

(59)

$$K_k=P_k^{-}{H}_k^T{\left(H_k{P}_k^{-}{H}_k^T+M_k{R}_k{M}_k^T\right)}^{-1}$$

(60)

$${\widehat{x}}_k^{+}={\widehat{x}}_k^{-}+K_k\left[y_k-h_k\left({\widehat{x}}_k^{-},0\right)\right]$$

(61)

$$P_k^{+}=\left(I-K_k{H}_k\right)P_k^{-}$$

(62)

3.3.2. Two-Level Error Model EKF Method

In this section, the two-level error model is applied to the EKF method. We take the ultra-wideband (UWB) system as the measurement and derive the EKF method based on this. The state vectors that need to be considered include $p:$ position; $v:$ velocity; and $q:$ quaternion; especially, in this paper, we need to discriminate their nominal states and error states. Therefore, the state vector is given as follows:

$$x={\left[p^{1\times 3},v^{1\times 3},q^{1\times 4},\delta p^{1\times 3},\delta v^{1\times 3},\delta q^{1\times 4},\delta a^{1\times 3},\delta w^{1\times 3}\right]}^T$$

(63)

where $\delta a$ is the error state of acceleration obtained by the accelerometer, and $\delta w$ is the error state of angular velocity obtained by the gyroscope. The state vector, $x$, is a $26\times 1$ column vector. In order to facilitate the subsequent matrix derivation, the following statements are given here. Variables except for quaternion, which is a $1\times 4$ row vector, are composed of three elements, which are (we take the position as an example): $p^{1\times 3}=\left[p_1,p_2,p_3\right]$, and they match the x, y and z axes, respectively.

The elements in the state vector are divided into three groups: sensor errors corresponding to mechanism error model variables, $\delta a,\delta w$; motion errors corresponding to propagation error model variables, $\delta p,\delta v,\delta q$; and motion nominal state variables, $p,v,q$. Modifications need to be applied to the traditional EKF method to adopt the two-level error model. At the moment, k, the state estimation of the navigation system is divided into two layers: (a) the motion nominal state performs a prediction based on sensor errors of moment k-1 and is updated based on UWB measurements at the moment, k; (b) the motion errors perform a prediction based on their state at the moment, k-1, and sensor errors at the moment, k-1. Additionally, the sensor errors at the moment, k, are predicted by the mechanism error model. The true motion state of the navigation system at the moment, k, is the sum of the motion nominal state and motion errors. The data fusion process is shown in Figure 3.

The state equations with propagation error models in continuous time are as follows:

$$\dot{p}=v$$

(64)

$$\dot{v}=C·\left(a_m+\delta a\right)+g$$

(65)

$$\dot{q}=\frac{1}{2}q\otimes \left(w_m+\delta w\right)$$

(66)

$$\delta \dot{p}=\delta v$$

(67)

$$\delta \dot{v}=C\left(q\right)·\delta a+\frac{\partial C}{\partial q}{a}_m·\delta q+\frac{\partial g}{\partial p}·\delta p$$

(68)

$$\delta \dot{q}=\mathsf{\Omega }\left(w_m\right)\delta q+Q\left(q\right)\delta w$$

(69)

By discretization, (64)–(69) become

$$p^k=p^{k-1}+\frac{1}{2}\left(v^k+v^{k-1}\right)·\Delta t$$

(70)

$$v^k=v^{k-1}+\left[C^{k-1}\left(a_m^{k-1}+\delta a^{k-1}\right)+g^{k-1}\right]·\Delta t$$

(71)

$$q^k=q^{k-1}\otimes q\left\{\left(w_m^{k-1}+\delta w^{k-1}\right)·\Delta t\right\}$$

(72)

$$\delta p^k=\delta p^{k-1}+\frac{1}{2}\left(\delta v^k+\delta v^{k-1}\right)·\Delta t$$

(73)

$$\delta v^k=\delta v^{k-1}+\left[{\left(\frac{\partial C}{\partial q}\right)}^{k-1}·a_m^{k-1}·\Delta t\right]·\delta q^{k-1}+C\left(q^{k-1}\right)·\delta a^{k-1}·\Delta t$$

(74)

$$\delta q^k=Q\left(q^{k-1}\right)·\delta w^{k-1}+\mathsf{\Omega }\left(w_m^{k-1}\right)·\delta q^{k-1}$$

(75)

Additionally, the mechanism error models are

$$\delta a_{k+1}=\left(I-\frac{\Delta t}{{\tau }_{b,a}}\right)\delta a_k+{\overline{w}}_{a,k}$$

(76)

$$\delta w_{k+1}=\left(I-\frac{\Delta t}{{\tau }_{b,w}}\right)\delta w_k+{\overline{w}}_{w,k}$$

(77)

where

$$E\left[{\overline{w}}_{a,k}{\overline{w}}_{a,k}^T\right]=\Delta t\cdot {\sigma }_{b,a}^2+\frac{{\sigma }_{w,a}^2}{\Delta t}$$

(78)

$$E\left[{\overline{w}}_{w,k}{\overline{w}}_{w,k}^T\right]=\Delta t\cdot {\sigma }_{b,w}^2+\frac{{\sigma }_{w,w}^2}{\Delta t}$$

(79)

Applying Taylor series expansion and keeping the first order, the state equations finally become

$$x_k=F·x_{k-1}+G·w_{k-1}$$

(80)

where $w_{k-1}={\left[w_a^{k-1},w_w^{k-1}\right]}^T$, and $w_a$ is the associated white noise process of the accelerometer with the known covariance; $w_w$ is that of the gyroscope with the known covariance. The state transition matrix F is a $26\times 26$ matrix and is calculated as:

$$F=\left[\begin{array}{llllllll}{F}_{11}\hfill & F_{12}\hfill & 0\hfill & 0\hfill & 0\hfill & 0\hfill & 0\hfill & 0\hfill \\ 0\hfill & F_{22}\hfill & F_{23}\hfill & 0\hfill & 0\hfill & 0\hfill & F_{27}\hfill & 0\hfill \\ 0\hfill & 0\hfill & F_{33}\hfill & 0\hfill & 0\hfill & 0\hfill & 0\hfill & F_{38}\hfill \\ 0\hfill & 0\hfill & 0\hfill & F_{44}\hfill & F_{45}\hfill & 0\hfill & 0\hfill & 0\hfill \\ 0\hfill & 0\hfill & 0\hfill & 0\hfill & F_{55}\hfill & F_{56}\hfill & F_{57}\hfill & 0\hfill \\ 0\hfill & 0\hfill & 0\hfill & 0\hfill & 0\hfill & F_{66}\hfill & 0\hfill & F_{68}\hfill \\ 0\hfill & 0\hfill & 0\hfill & 0\hfill & 0\hfill & 0\hfill & F_{77}\hfill & 0\hfill \\ 0\hfill & 0\hfill & 0\hfill & 0\hfill & 0\hfill & 0\hfill & 0\hfill & F_{88}\hfill \end{array}\right]$$

(81)

The derivations in the individual elements of the state transition matrix F can be found in Appendix A. For the noise vectors $w_a$ and $w_w$, the noise coefficient matrix G is a $26\times 6$ matrix and is calculated by

$$G={\left[\begin{array}{llllllll}0\hfill & 0\hfill & 0\hfill & 0\hfill & 0\hfill & 0\hfill & I_{3\times 3}·\Delta t\hfill & 0\hfill \\ 0\hfill & 0\hfill & 0\hfill & 0\hfill & 0\hfill & 0\hfill & 0\hfill & I_{3\times 3}·\Delta t\hfill \end{array}\right]}^T$$

(82)

The measurement vector z contains four distances because of the deployment of four UWB base stations. The distance is measured based on the arrival time difference of electromagnetic waves. Additionally, the vector z is given as (83).

$$z={\left[d_1,d_2,d_3,d_4\right]}^T$$

(83)

$$d_i\left(k\right)=\sqrt{{\left(p_1\left(k\right)-x_i\right)}^2+{\left(p_2\left(k\right)-y_i\right)}^2+{\left(p_3\left(k\right)-z_i\right)}^2},\hspace{1em}i=1,2,3,4$$

(84)

where $p\left(k\right)={\left[p_1\left(k\right),p_2\left(k\right),p_3\left(k\right)\right]}^T$, in which subscript (1,2,3) corresponds with the axes $\left(x,y,z\right)$. $\left(x_i,y_i,z_i\right)$ make up the coordinate of the i-th UWB base station.

The discrete-time measurement equation is given by (85):

$$z=H·x+r$$

(85)

where H is the measurement coefficient matrix, and $r$ is the measurement noise vector, which is the assumed white noise. The matrix H is derived by (86).

$$H_{4\times 26}=\left[H_{p,4\times 3}^d,0_{4\times 23}\right]$$

(86)

The matrix $H_{p,4\times 3}^d$ has the form of

$$H_{p,4\times 3}^d=\left[\begin{array}{lll}\frac{p_1\left(k\right)-x_1}{R_1}\hfill & \frac{p_2\left(k\right)-y_1}{R_1}\hfill & \frac{p_3\left(k\right)-z_1}{R_1}\hfill \\ \frac{p_1\left(k\right)-x_2}{R_2}\hfill & \frac{p_2\left(k\right)-y_2}{R_2}\hfill & \frac{p_3\left(k\right)-z_2}{R_2}\hfill \\ \frac{p_1\left(k\right)-x_3}{R_3}\hfill & \frac{p_2\left(k\right)-y_3}{R_3}\hfill & \frac{p_3\left(k\right)-z_3}{R_3}\hfill \\ \frac{p_1\left(k\right)-x_4}{R_4}\hfill & \frac{p_2\left(k\right)-y_4}{R_4}\hfill & \frac{p_3\left(k\right)-z_4}{R_4}\hfill \end{array}\right]$$

(87)

where

$$R_i=\sqrt{{\left(p_1\left(k\right)-x_i\right)}^2+{\left(p_2\left(k\right)-y_i\right)}^2+{\left(p_3\left(k\right)-z_i\right)}^2}$$

(88)

The algorithm flow of the two-level error model EKF is shown in Algorithm 1.

Algorithm 1. Process of two-level error model EKF.

Algorithm 1: Two-Level Error Model EKF

State Variables: $\mathit{x}={\left[\mathit{p},\mathit{v},\mathit{q},\delta \mathit{p},\delta \mathit{v},\delta \mathit{q},\delta \mathit{a},\delta \mathit{w}\right]}^T$

Initialization: ${\mathit{x}}_0,{\mathit{P}}_0$

Input: ${\mathit{x}}_{k-1},{\mathit{P}}_{k-1}{,}^1{\mathit{u}}_{k-1},{\mathit{z}}_k$

Output: ${\widehat{\mathit{x}}}_k,{\widehat{\mathit{P}}}_k$

$1: {\mathit{a}}_{k-1}\leftarrow {\mathit{a}}_m^{k-1}+\delta {\mathit{a}}_{k-1}$$; {\mathit{w}}_{k-1}\leftarrow {\mathit{w}}_m^{k-1}+\delta {\mathit{w}}_{k-1}$; ${\mathit{u}}_{k-1}^{+}\leftarrow {\left[{\mathit{a}}_{k-1},{\mathit{w}}_{k-1}\right]}^T$

$2: {\widehat{\mathit{x}}}_k^{-}\leftarrow f_{k-1}\left({\mathit{x}}_{k-1},{\mathit{u}}_{k-1}^{+}\right)$

$3: {\mathit{P}}_k^{-}\leftarrow {\mathit{F}}_{k-1}{\mathit{P}}_{k-1}{\mathit{F}}_{k-1}^T+{\mathit{G}}_{k-1}{}^2{\mathit{Q}}_{k-1}{\mathit{G}}_{k-1}^T$

$4: {\mathit{K}}_k\leftarrow {\mathit{P}}_k^{-}{\mathit{H}}_k^T{\left({\mathit{H}}_k{\mathit{P}}_k^{-}{\mathit{H}}_k^T{+}^3{\mathit{R}}_k\right)}^{-1}$

$5: {\widehat{\mathit{x}}}_k^{+}\leftarrow {\widehat{\mathit{x}}}_k^{-}+{\mathit{K}}_k\left[{\mathit{z}}_k-h_k\left({\widehat{\mathit{x}}}_k^{-},0\right)\right]$

$6: {\widehat{\mathit{P}}}_k\leftarrow \left(I-{\mathit{K}}_k{\mathit{H}}_k\right){\mathit{P}}_k^{-}$

$7: {\widehat{\mathit{p}}}_k\leftarrow {\widehat{\mathit{p}}}_k^{+}+\delta {\widehat{\mathit{p}}}_k^{+}$$; {\widehat{\mathit{v}}}_k\leftarrow {\widehat{\mathit{v}}}_k^{+}+\delta {\widehat{\mathit{v}}}_k^{+}$$; {\widehat{\mathit{q}}}_k\leftarrow {\widehat{\mathit{q}}}_k^{+}\otimes \delta {\widehat{\mathit{q}}}_k^{+}$

$8: {\widehat{\mathit{x}}}_k\leftarrow {\left[{\widehat{\mathit{p}}}_k,{\widehat{\mathit{v}}}_k,{\widehat{\mathit{q}}}_k,\delta {\widehat{\mathit{p}}}_k^{+},\delta {\widehat{\mathit{v}}}_k^{+},\delta {\widehat{\mathit{q}}}_k^{+},\delta {\widehat{\mathit{a}}}_k^{+},\delta {\widehat{\mathit{w}}}_k^{+}\right]}^T$  $9: \mathbf{R}\mathbf{e}\mathbf{t}\mathbf{u}\mathbf{r}\mathbf{n}\mathbf{:} {\widehat{\mathit{x}}}_k,{\widehat{\mathit{P}}}_k$

$${\mathit{u}}_{k-1}={\left[{\mathit{a}}_m^{k-1},{\mathit{w}}_m^{k-1}\right]}^T, {\mathit{Q}}_{k-1}=E\left[{\mathit{w}}_{k-1}{\mathit{w}}_{k-1}^T\right], {\mathit{R}}_k=E\left[{\mathit{r}}_k{\mathit{r}}_k^T\right].$$

4. Experiment Results and Discussion

In this section, we discuss the experiment results of the two-level error model EKF method based on the UWB-drone dataset [34]. We present the results in three sections. In Section 4.1, we introduce the dataset used in this paper and present the prefiltering results. In Section 4.2, the parameters in the two-level error model, which need to be acquired beforehand, are shown. Additionally, in Section 4.3, the comparison results of the two-level error model and the basic EKF method are presented.

4.1. Dataset Description and Prefiltering

4.1.1. Dataset Description

The UWB-drone dataset is about UWB-based UAV localization in GNSS-denied environments, and we selected UAV/anchor_in_room_corners to carry out the experiment. This dataset contains UAV IMU information, UWB anchor distance and position and Mocap data as the ground truth. The positions of the x–y planes in relation to UWB and Mocap are shown in Figure 4a, and the distance sequence figure of the four anchors is shown in Figure 4b.

4.1.2. Data Prefiltering Results

The data prefiltering results of the gyroscope are shown in Figure 5, with the accelerometer similarly. From the three-axes raw data figures and their spectrum maps, it can be seen that the raw signals from IMU have obvious fluctuations. After applying two-level wavelet decomposition and reconstruction from the SURE-Shrink thresholding method, the results are shown in Figure 5c,g,k, and the last column of figures corresponds to the spectrum map. The fluctuation of raw signals is significantly reduced after denoising. In the spectrum maps of data after denoising, the high-frequency signal amplitude is significantly weakened.

4.2. Two-Level Error Model Parameter Estimation Results

In the two-level error model, especially the mechanism error model, we need to estimate the parameters $\theta =\left\{{\tau }_b,{\sigma }_b,{\sigma }_w\right\}$ beforehand. ${\tau }_b$ can be obtained by calculating correlation time, while ${\sigma }_b,{\sigma }_w$ can be obtained from the Allan variance plot. The dataset UAV/anchor_in_room_corners does not contain long-term IMU stationary data, and the experiment was conducted on a UAV with a Pixhawk2.4 controller whose IMU was MPU6000. Thus, we performed the inertial sensor stationary experiment on MPU6000 for 5 h to obtain static IMU data, and then, we created the Allan variance plot, as shown Figure 6. In this figure, the raw data of a six-axes IMU are plotted in solid lines, and the prediction curves of six-axes data are plotted in fine-dotted lines. ${\sigma }_w$ is the strength of the rate or the acceleration white noise process and is often termed “angular/velocity random walk”, while in Figure 6, this corresponds to the curve with slope −1/2. ${\sigma }_b$ is termed “bias diffusion” and corresponds to the curve with slope 1/2. The fine-dotted curves are plotted according to the estimated parameters ${\sigma }_b,{\sigma }_w$.

The parameter estimation results are shown in

Table 1. Results of model parameter estimation of MPU6000.

Sensor	Axis	${\mathit{\sigma }}_{\mathit{w}}$	${\mathit{\sigma }}_{\mathit{b}}$
Accelerometer	x	$2.419\times {10}^{-3}$	$4.402\times {10}^{-5}$
	y	$2.725\times {10}^{-3}$	$1.63\times {10}^{-4}$
	z	$3.9\times {10}^{-3}$	$1.403\times {10}^{-5}$
Gyroscope	x	$1.52\times {10}^{-4}$	0
	y	$8.2\times {10}^{-5}$	0
	z	$1.02\times {10}^{-4}$	0

.

4.3. Two-Level Error Model-Based EKF Method Results

We now apply the two-level error model proposed in Section 3 to the UAV dataset. We show that the two-level error model can improve the attitude estimation performance of the fusion algorithm in the UAV motion process by comparing three-axes trajectory plots, which indicate attitude estimation performance, and error figures. The methods we use to compare are the EKF method based on the two-level error model, the basic EKF method and the EKF method based on the data after wavelet denoising.

Figure 7 shows the trajectory comparison of three methods in the x, y and z axes. The ground truth is plotted in black lines, and the UWB position is plotted in red lines. The sub-graphs g–i correspond to the EKF method based on the two-level error model; a–c correspond to the basic EKF method; and d–f correspond to the EKF method based on the data after denoising. By comparing d–f with a–c, we can see that the SURE-Shrink wavelet denoising method brings a large performance enhancement to the EKF fusion method, and the mean positioning error is eliminated to 0.457 m from 1.949 m, as shown in Figure 8. At the points where the UAV motion state changes, applying the two-level error model can provide better computational stability, which manifests by less curve fluctuations in the figures—blue widow parts in d–f as examples.

Applying the EKF fusion method based on raw IMU data does not effectively improve the UAV positioning accuracy, even though the UWB positions are quite close to the ground truth, as shown in Figure 7a–c. However, by applying the two-level error model, the fusion positions are closer to ground truth than the UWB positions, as shown partially enlarged in Figure 9. At the beginning of EKF based on the two-level error model fusion, the curve has large fluctuations because the initial gain matrix deviates far from the optimal value. Compared to x and y axes’ results of the two-level error model EKF method, the z-axis result has larger fluctuations. The reasons that cause these fluctuations can be divided into two aspects: one is that the UWB positioning accuracy is poorer in the z-axis than the x and y axes, which causes large fluctuations in the UWB position results; the other is that the inertial data in this UAV dataset generally contain much vibration, and these two reasons explain the large fluctuations.

Figure 9 shows the error figures of the three methods. Applying the EKF fusion method on raw inertial data causes a large error, and the average error of the basic EKF method is 1.949 m. After applying the SURE-Shrink wavelet denoising method, the fusion accuracy is greatly improved, and the average error is eliminated to 0.457 m; the accuracy is improved by 76.6%. After applying the two-level error model, the computational stability is largely enhanced at UAV motion state change points, and the accuracy is improved by 84.3%.

5. Conclusions

This paper set out to modify the INS-based fusion positioning method by providing generic noise models. In this paper, the IMU wavelet denoising method based on SURE-Shrink threshold was summarized, and the two-level error model was proposed, which includes the mechanism error model and the propagation error model. The mechanism error model was established from stochastic process theory, and the propagation error model was derived from navigation principles; both compensate for the inertial sensor errors. We derived the EKF fusion method based on the two-level error model, and the experimental verification was carried out with UWB measurements. The experimental results suggest that applying the wavelet denoising method could largely improve positioning accuracy by 76.6% compared to the basic EKF method. Additionally, applying the two-level error model could further improve positioning accuracy by 84.3% compared to basic EKF. Meanwhile, at the points where the UAV motion state changes, using the two-level error model could provide higher computational stability and less trajectory curve fluctuations.

At the end of the curve for the z-axis in the two-level error model EKF method in Figure 7i, huge data fluctuations exist in the fusion. This is an interesting phenomenon, and we do not consider the case of unknown inputs in the system. This is an issue for future research to explore.