Comparative Analysis between Error-State and Full-State Error Estimation for KF-Based IMU/GNSS Integration against IMU Faults

Abstract

Considering the inertial measurement unit (IMU) faults risk of an unmanned aerial vehicle (UAV), this paper provides an analysis of the error overboundings of position estimation in a tightly coupled IMU/global navigation satellite system (GNSS) integrated architecture under the IMU fault conditions using an error-state EKF-based approach and provides a comparison to a recently published EKF-based full state method. Simulation results show that both the error overboundings of the error-state and full-state EKFs can fit the state error against the IMU faults, but the error-state EKF is more suitable for UAV navigation system integrity assurance due to its higher calculation efficiency. This study will be extended to the integrity monitoring of multisensor systems.

1. Introduction

Unmanned aerial vehicles (UAVs) are widely used in many aspects of our daily life. The application of UAVs will permeate more aspects of public life in the near future. To ensure the safety of a UAV, its navigation solution should be fully protected by an integrity assurance mechanism, including real-time fault detection and an integrity risk evaluation. Integrity risk is the probability of hazardous misleading information (HMI) [1] and can be evaluated by the error overboundings (i.e., protection levels (PLs)) of the estimated position solution corresponding to the required probability of HMI, i.e., the integrity risk requirement [2,3].

An UAV usually adopts an inertial measurement unit (IMU)/global navigation satellite system (GNSS) integrated navigation system. The configuration of the IMU on UAVs can be performed according to the practical application requirements. UAVs similar to the DJI Phantom series use two IMUs to provide redundant measurements [4]. The experimental test carried out by Rhudy uses four IMUs to provide redundant IMU data [5]. Although the redundantly configured IMU can be used to detect IMU faults, there is still a probability of missed detection, and the fault risk of IMU still exists. Moreover, the low-cost micro-electromechanical system (MEMS) IMU on an UAV has a lower reliability and is more vulnerable to the flight environment than the fiber-optic strapdown IMU on civil aviation aircraft. Giorgio showed that IMUs are more prone to malfunction in high-vibration environments, where UAVs typically operate [6]. Bhatti listed the fault types of IMU sensors and summarized the potential causes of each fault type [7]. The IMU also has a high fault risk in the integrated navigation system for UAVs [8]. Therefore, IMU faults cannot be ignored in the integrity assurance mechanism of UAVs’ integrated navigation architecture.

Many studies have been carried out to assure the navigation integrity of UAVs. However, most studies still place emphasis on assuring the integrity of the GNSS. Some of the studies focus on researching GNSS integrity augmentation systems, which apply GNSS monitoring technology in civil aviation to UAVs. For instance, Pullen developed a local-area differential GNSS architecture around the concept of local-area UAV network operations, which can assure the integrity of UAVs against GNSS faults with a low cost [9]. Lee proposed a high-integrity navigation architecture for the integrated IMU/LAD-GNSS system with a filter based on the extended Kalman filter (EKF), which can support continuous and reliable navigation by overcoming the GNSS’ signal vulnerability [10]. Kim presented a LAD-GNSS architecture for UAVs that communicates with ground subsystems in real time, reducing the vertical PLs by approximately 16% [11].

Other studies have utilized the EKF innovation or residual to monitor the GNSS integrity, which is known as IMU-aided GNSS integrity monitoring. There are two aspects of this research—IMU-aided GNSS fault detection and integrity risk evaluation against a GNSS fault. For the first aspect, Brenner proposed the multiple solution separation (MSS) method [12], Diesel investigated the autonomous integrity monitoring by extrapolation (AIME) method [13], and Crespillo performed a comparison between innovation-based and residual-based fault detection in the EKF for GNSS/INS hybridization [14]. They are all effective for GNSS fault detection in an IMU/GNSS navigation system. For the second aspect, Joerger evaluated the integrity risk in a sequential Kalman filter (KF) by quantifying the worst-case integrity risk based on the relationship between the state error and the GNSS range residual to construct monitor test statistics using a least square (LS) expression [15]. Tanil instead used the recursive expression and applied it to GNSS spoofing detection [16]. Bhattacharyya proposed the real-time PLs computation through the residual-based RAIM algorithm [17]. All of these approaches can monitor the integrity of the GNSS in the IMU/GNSS integrated navigation system.

Different from the above studies, Lee proposed an integrity assurance mechanism for an EKF-based IMU/GNSS integrated system against IMU faults [18]. She calculated the PLs using the EKF innovations and additional uncertain noise boundary terms, by deriving the IMU faults propagation process in the EKF [18]. In her study, the full-state inertial navigation equations, including the attitude, velocity, and position update equations, were used as the EKF system propagation equations. Moreover, the velocity update equation was used as an example to explain how the IMU faults propagate in the EKF.

In our study, different from the method published by Lee et al. [18], the error-state inertial navigation equations, including the attitude error, the velocity error, and the position error update equations, were used to analyze the IMU faults propagation process which are more commonly used as the EKF system propagation equations in IMU/GNSS integrated navigation than the full-state inertial navigation equations [18,19,20]. While being inspired by the previous published research in this area [16,17,18,19,20], the error overboundings against the IMU faults for the EKF state estimation using the error-state inertial navigation equation is derived in this paper. Moreover, the error overboundings using error-state and full-state inertial navigation equations are compared to determine which kind is more conducive to assuring the integrity for the IMU/GNSS integrated navigation architecture against IMU faults. After using the inertial navigation error equation, the magnitude of each dimension of the state vector keep close or consistent, and the occurrence of singular matrices in the filtering calculation process is avoided, which will shorten the filtering time, improve the converge speed, and increase the calculation efficiency. To simplify the description below, the EKF using the full-state inertial navigation equations is named the full-state EKF, whereas the EKF using the error-state inertial navigation equation is named the error-state EKF.

Accordingly, the remainder of this paper is organized as follows: (1) the general structures of the IMU/GNSS integrated navigation architecture using the IMU error-state model are introduced, (2) the detailed IMU faults propagation process in the error-state EKF is specially derived, (3) a description of the real-time error overboundings (PLs) equations in the error-state EKF for IMU faults based on innovations, (4) the simulations for the error overboundings calculation under IMU faults are presented, and (5) the summary and concluding remarks close the paper.

2. Architecture of the IMU/GNSS Integrated Navigation System

2.1. IMU/GNSS Integration Architecture

Figure 1 illustrates an overview of the EKF-based tightly coupled IMU/GNSS integration architecture. The inputs are the GNSS pseudo-range, $\rho$, pseudo-range rate, $\dot{\rho }$, GNSS receiver clock offset, $\delta {\rho }_c$, and GNSS receiver drift, $\delta {\dot{\rho }}_c$, from the GNSS ranging processor and the IMU position, $P_{IMU}$, velocity, $V_{IMU}$, and attitude, $A_{IMU}$, navigation parameters from the inertial navigation processor. The outputs of the integrated navigation system are the corrected IMU position, $\widehat{P}$, velocity, $\widehat{V}$, and attitude, $\widehat{A}$. The integration is completed in the range domain. In this study, the error-state inertial navigation equations based EKF (i.e., the error-state EKF) was used for the IMU/GNSS integration architecture, and the outputs of the IMU/GNSS integration Kalman filter were the UAV navigation parameter error estimation, $\delta \widehat{X}$ [21].

2.2. Error-State EKF Model

Using the error-state IMU equations as the system propagation equation, the state vector includes the IMU attitude error, IMU velocity error, IMU position error, inertial sensor biases, and GNSS receiver clock offset and drift for the error-state EKF [22]. The EKF is described as a system propagation equation (Equation (1)) and a measurement update equation (Equation (2)) [6]. In the measurement updates step, the GNSS pseudo-range and pseudo-range rate measurements are used for the measurement update, and the difference between the GNSS pseudo-range, GNSS pseudo-range rate and the corresponding predicted IMU pseudo-range, IMU pseudo-range rate constitute the EKF innovation. There is another expression of the system propagation equation, the full-state inertial navigation equations based EKF (i.e., the full-state EKF). For either the full-state or error-state in EKF, the essence is to estimate the state error. However, there is a difference between the error-state EKF and the full-state EKF in the system propagation step, the control input is added to the full-state EKF (Appendix A):

$${\widehat{X}}_k^{-}={\Phi }_k{\widehat{X}}_{k-1}+{\Gamma }_{wk}{\overrightarrow{w}}_k$$

(1)

$${\widehat{X}}_k={\widehat{X}}_k^{-}+K_k\underset{r_k}{\underbrace{\left({\overrightarrow{Z}}_k-H_k{\widehat{X}}_k^{-}\right)}}$$

(2)

where ${\widehat{X}}_k^{-}$ is the predicted error-state vector at epoch k, ${\widehat{X}}_k$ is the measurement updated error-state vector at epoch k, Φ_k is the dynamic state transition matrix at epoch k, Γ_wk is the process noise matrix at epoch k, ${\overrightarrow{w}}_k$ is the process noise vector at epoch k, $K_k$ is the Kalman gain at epoch k (Appendix B.1), ${\overrightarrow{Z}}_k$ is the estimate of the measurement produced by the states estimated by the Kalman filter, $H_k$ is the measurement matrix at epoch k (Appendix B.1), and $r_k$ is the EKF innovation vector at epoch k.

3. Error Overboundings against IMU Faults

Different from the full-state EKF, the error-state EKF has no input of the control quantity during the entire filtering process. Compared with the full-state EKF, the error-state EKF keeps the magnitude of each dimension of the state vector close or consistent, which avoids the occurrence of singular matrices in the filtering calculation process, reduces the convergence time and improves the calculation efficiency.

This part analyzes the propagation process of IMU faults for the error-state EKF in detail. Through the analysis, the recursive propagation equation between adjacent epochs of the EKF state error is obtained. Based on the EKF state error propagation equation and the EKF innovation equation, the error overboundings of the EKF state error are given.

3.1. IMU Faults Propagation in the Error-State EKF

In this section, the IMU faults propagation process in the state vector for the error-state EKF is explained. While following a similar approach, it is worth mentioning that the method derived here is a different approach from the work published by Lee et al. [18] where error states instead of full navigation states are considered. The differences between the output measurement value and the true value of the IMU are expressed in Equation (3):

$$\{\begin{array}{c}\delta {\phi }_{\beta }^{\alpha }={\tilde{\phi }}_{\beta }^{\alpha }-{\phi }_{\beta }^{\alpha }\\ \delta v_{\beta \alpha }^r={\tilde{v}}_{\beta \alpha }^r-v_{\beta \alpha }^r\\ \delta r_{\beta \alpha }^r={\tilde{r}}_{\beta \alpha }^r-r_{\beta \alpha }^r\end{array}$$

(3)

where “~” represents the measurement value, ${\phi }_{\beta }^{\alpha }$ is the attitude vector (rad), $v_{\beta \alpha }^r$ is the velocity vector (m/s), and $r_{\beta \alpha }^r$ is the position vector (m).

The inertial navigation error equations in the Earth-centered, Earth-fixed (ECEF) coordinate frame can be seen in Equation (4), including the attitude error, velocity error, and position error update equations:

$$\begin{array}{l}\delta {\dot{\overrightarrow{\phi }}}_{eb}^e=C_b^e\left([{\tilde{\overrightarrow{\omega }}}_{ib}^b\times ]-[{\overrightarrow{\omega }}_{ib}^b\times ]\right)-[{\overrightarrow{\omega }}_{ie}^e\times ]\left({\tilde{\overrightarrow{\phi }}}_{eb}^e-{\overrightarrow{\phi }}_{eb}^e\right)\\ \delta {\dot{\overrightarrow{v}}}_{eb}^e=C_b^e\left({\tilde{\overrightarrow{f}}}_{ib}^b-{\overrightarrow{f}}_{ib}^b\right)-2\left[{\overrightarrow{\omega }}_{ie}^e\times \right]\left({\tilde{\overrightarrow{v}}}_{eb}^e-{\overrightarrow{v}}_{eb}^e\right)+{\widehat{\overrightarrow{g}}}^e-{\overrightarrow{g}}^e\\ \delta {\dot{\overrightarrow{r}}}_{eb}^e={\tilde{\overrightarrow{v}}}_{eb}^e-{\overrightarrow{v}}_{eb}^e\end{array}$$

(4)

where $C_b^e$ is the coordinate transformation matrix from the body frame to the Earth frame (a detailed expression can be found in Appendix B.2), ${\overrightarrow{\omega }}_{ie}^e$ is the Earth rotation rate (rad/s), ${\overrightarrow{g}}^e$ is the gravity vector in the Earth-centered, Earth-fixed frame (m/s²), and $\left[•\times \right]$ represents the calculation of a skew-symmetric matrix.

Neglecting the effects of gravity errors, Equation (4) can be simplified as

$$\begin{array}{l}\delta {\dot{\overrightarrow{\phi }}}_{eb}^e=C_b^e\delta \left[{\overrightarrow{\omega }}_{ib}^b\times \right]-\left[{\overrightarrow{\omega }}_{ie}^e\times \right]\delta {\overrightarrow{\phi }}_{eb}^e\\ \delta {\dot{\overrightarrow{v}}}_{eb}^e=C_b^e\delta {\overrightarrow{f}}_{ib}^b-2\left[{\overrightarrow{\omega }}_{ie}^e\times \right]\delta {\overrightarrow{v}}_{eb}^e\\ \delta {\dot{\overrightarrow{r}}}_{eb}^e=\delta {\overrightarrow{v}}_{eb}^e\end{array}.$$

(5)

Figure 2 shows the updating process of the attitude, velocity, and position in the ECEF coordinate frame for the IMU. When IMU faults occur in the gyroscope or accelerometer, the computed attitude, velocity, and position are sequentially affected, finally resulting in position errors.

Under the condition of a faulty IMU, the inertial navigation error equation in Equation (5) becomes Equation (6), where the apostrophe (’) indicates the faulty parameter. The parameters affected by the fault are expressed as the sum of a normal error term and a faulty additional error term, as shown in Equation (7):

$$\begin{array}{l}\delta {{\dot{\overrightarrow{\phi }}}_{eb}^e}^{\prime }=C_b^e{\left(\delta \left[{\overrightarrow{\omega }}_{ib}^b\times \right]\right)}^{\prime }-\left[{\overrightarrow{\omega }}_{ie}^e\times \right]\delta {\overrightarrow{\phi }}_{eb}^e\\ \delta {{\dot{\overrightarrow{v}}}_{eb}^e}^{\prime }={\left(C_b^e\right)}^{\prime }{\left(\delta {\overrightarrow{f}}_{ib}^b\right)}^{\prime }-2\left[{\overrightarrow{\omega }}_{ie}^e\times \right]\delta {\overrightarrow{v}}_{eb}^e\\ \delta {{\dot{\overrightarrow{r}}}_{eb}^e}^{\prime }={\left(\delta {\overrightarrow{v}}_{eb}^e\right)}^{\prime }\end{array}$$

(6)

$$\begin{array}{c}\delta {{\overrightarrow{\omega }}_{ib}^b}^{\prime }=\delta {\overrightarrow{\omega }}_{ib}^b+\Delta {\overrightarrow{\omega }}_{ib}^b\\ \delta {{\overrightarrow{f}}_{ib}^b}^{\prime }=\delta {\overrightarrow{f}}_{ib}^b+\Delta {\overrightarrow{f}}_{ib}^b\\ \delta {{\overrightarrow{v}}_{eb}^e}^{\prime }=\delta {\overrightarrow{v}}_{eb}^e+\Delta {\overrightarrow{v}}_{eb}^e\end{array}.$$

(7)

Substituting Equation (7) into Equation (6), the inertial navigation error equation under the IMU faults condition can be shown as

$$\begin{array}{l}\delta {{\dot{\overrightarrow{\phi }}}_{eb}^e}^{\prime }=C_b^e\left(\delta \left[{\overrightarrow{\omega }}_{ib}^b\times \right]+\Delta {\overrightarrow{\omega }}_{ib}^b\right)-\left[{\overrightarrow{\omega }}_{ie}^e\times \right]\delta {\overrightarrow{\phi }}_{eb}^e\\ \delta {{\dot{\overrightarrow{v}}}_{eb}^e}^{\prime }=\left(C_b^e+\Delta C_b^e\right)\left(\delta {\overrightarrow{f}}_{ib}^b+\Delta {\overrightarrow{f}}_{ib}^b\right)-2\left[{\overrightarrow{\omega }}_{ie}^e\times \right]\delta {\overrightarrow{v}}_{eb}^e\\ \delta {{\dot{\overrightarrow{r}}}_{eb}^e}^{\prime }=\delta {\overrightarrow{v}}_{eb}^e+\Delta {\overrightarrow{v}}_{eb}^e\end{array}.$$

(8)

Rearrange Equation (8),

$$\begin{array}{l}\delta {{\dot{\overrightarrow{\phi }}}_{eb}^e}^{\prime }=C_b^e\delta \left[{\overrightarrow{\omega }}_{ib}^b\times \right]-\left[{\overrightarrow{\omega }}_{ie}^e\times \right]\delta {\overrightarrow{\phi }}_{eb}^e+\underset{{\overrightarrow{f}}_{\delta {\dot{\overrightarrow{\phi }}}_{eb}^e}}{\underbrace{C_b^e\Delta {\overrightarrow{\omega }}_{ib}^b}}\\ \delta {{\dot{\overrightarrow{v}}}_{eb}^e}^{\prime }=C_b^e\delta {\overrightarrow{f}}_{ib}^b-2\left[{\overrightarrow{\omega }}_{ie}^e\times \right]\delta {\overrightarrow{v}}_{eb}^e+\underset{{\overrightarrow{f}}_{\delta {\dot{\overrightarrow{v}}}_{eb}^e}}{\underbrace{C_b^e\Delta {\overrightarrow{f}}_{ib}^b+\Delta C_b^e\left(\delta {\overrightarrow{f}}_{ib}^b+\Delta {\overrightarrow{f}}_{ib}^b\right)}}\\ \delta {{\dot{\overrightarrow{r}}}_{eb}^e}^{\prime }=\delta {\overrightarrow{v}}_{eb}^e+\underset{{\overrightarrow{f}}_{\delta {\dot{\overrightarrow{r}}}_{eb}^e}}{\underbrace{\Delta {\overrightarrow{v}}_{eb}^e}}\end{array}$$

(9)

where ${\overrightarrow{f}}_{\delta {\dot{\overrightarrow{\phi }}}_b^e}$, ${\overrightarrow{f}}_{\delta {\dot{\overrightarrow{v}}}_{eb}^e}$, and ${\overrightarrow{f}}_{\delta {\dot{\overrightarrow{r}}}_{eb}^e}$ represent the resultant bias vectors for the attitude, velocity, and position error states due to the IMU faults. In the EKF, the accelerometer measurement of the specific force ${\overrightarrow{f}}_{ib,meas}^b$ is represented as Equation (10), which includes the true specific force, the accelerometer bias, and the process noise. In addition, the gyroscope measurement of the angular rate, ${\overrightarrow{\omega }}_{ib,meas}^b$, is represented as Equation (11), which includes the true angular rate, the gyroscope bias, a g-dependent bias related to the specific force, and process noise. The accelerometer bias and gyroscope bias are usually modeled as constant errors [23]:

$${\overrightarrow{f}}_{ib,meas}^b={\overrightarrow{f}}_{ib}^b+{\overrightarrow{b}}_a+{\overrightarrow{w}}_a$$

(10)

$${\overrightarrow{\omega }}_{ib,meas}^b={\overrightarrow{\omega }}_{ib}^b+{\overrightarrow{b}}_g+G_g{\overrightarrow{f}}_{ib}^b+{\overrightarrow{w}}_g.$$

(11)

According to Equations (10) and (11), the accelerometer measurement of the specific force error and the gyroscope measurement of the angular rate error are derived from the accelerometer measurement of the specific force, ${\overrightarrow{f}}_{ib,meas}^b$, and the gyroscope measurement of the angular rate, ${\overrightarrow{\omega }}_{ib,meas}^b$, as shown in Equation (12):

$$\begin{array}{ll}\delta {\overrightarrow{\omega }}_{ib}^b& ={\overrightarrow{\omega }}_{ib,meas}^b-{\overrightarrow{\omega }}_{ib}^b\\ & ={\overrightarrow{b}}_g+G_g{\overrightarrow{f}}_{ib}^b+{\overrightarrow{w}}_g\\ \delta {\overrightarrow{f}}_{ib}^b& ={\overrightarrow{f}}_{ib,meas}^b-{\overrightarrow{f}}_{ib}^b\\ & ={\overrightarrow{b}}_a+{\overrightarrow{w}}_a\end{array}.$$

(12)

Substituting Equation (12) into Equation (9), the inertial navigation error equation under the IMU faults condition is

$$\begin{array}{l}\delta {{\dot{\overrightarrow{\phi }}}_{eb}^e}^{\prime }=C_b^e\left[\left({\overrightarrow{b}}_g+G_g{\overrightarrow{f}}_{ib}^b+{\overrightarrow{w}}_g\right)\times \right]-\left[{\overrightarrow{\omega }}_{ie}^e\times \right]\delta {\overrightarrow{\phi }}_{eb}^e+{\overrightarrow{f}}_{\delta {\dot{\overrightarrow{\phi }}}_{eb}^e}\\ \delta {{\dot{\overrightarrow{v}}}_{eb}^e}^{\prime }=C_b^e\left[\left({\overrightarrow{b}}_a+{\overrightarrow{w}}_a\right)\times \right]-2\left[{\overrightarrow{\omega }}_{ie}^e\times \right]\delta {\overrightarrow{v}}_{eb}^e+{\overrightarrow{f}}_{\delta {\dot{\overrightarrow{v}}}_{eb}^e}\\ \delta {{\dot{\overrightarrow{r}}}_{eb}^e}^{\prime }=\delta {\overrightarrow{v}}_{eb}^e+{\overrightarrow{f}}_{\delta {\dot{\overrightarrow{r}}}_{eb}^e}\end{array}.$$

(13)

Rearranging Equation (13), the inertial navigation error equation under the IMU faults condition (Equation (8)) becomes a continuous form of an EKF error-state equation as a function of EKF error states, process noise, and the state fault vector, as shown in Equation (14):

$$\begin{array}{l}\delta {{\dot{\overrightarrow{\phi }}}_{eb}^e}^{\prime }=\underset{F_{\delta {\overrightarrow{\phi }}_{eb}^e}\delta {\overrightarrow{\phi }}_{eb}^e}{\underbrace{-\left[{\overrightarrow{\omega }}_{ie}^e\times \right]\delta {\overrightarrow{\phi }}_{eb}^e+C_b^e\left[{\overrightarrow{b}}_g\times \right]}}+\underset{G_g{\overrightarrow{w}}_g}{\underbrace{C_b^e\left[\left(G_g{\overrightarrow{f}}_{ib}^b+{\overrightarrow{w}}_g\right)\times \right]}}+{\overrightarrow{f}}_{\delta {\dot{\overrightarrow{\phi }}}_{eb}^e}\\ \delta {{\dot{\overrightarrow{v}}}_{eb}^e}^{\prime }=\underset{F_{\delta {\overrightarrow{v}}_{eb}^e}\delta {\overrightarrow{v}}_{eb}^e}{\underbrace{-2\left[{\overrightarrow{\omega }}_{ie}^e\times \right]\delta {\overrightarrow{v}}_{eb}^e+C_b^e\left[b_a\times \right]}}+\underset{G_a{\overrightarrow{w}}_a}{\underbrace{C_b^e\left[{\overrightarrow{w}}_a\times \right]}}+{\overrightarrow{f}}_{\delta {\dot{\overrightarrow{v}}}_{eb}^e}\\ \delta {{\dot{\overrightarrow{r}}}_{eb}^e}^{\prime }=\delta {\overrightarrow{v}}_{eb}^e+{\overrightarrow{f}}_{\delta {\dot{\overrightarrow{r}}}_{eb}^e}\end{array}.$$

(14)

Based on the integral relationship between the velocity and the position in the error-state inertial navigation equations, Equation (14) can be rewritten as

$$\begin{array}{l}\delta {{\dot{\overrightarrow{\phi }}}_{eb}^e}^{\prime }=F_{\delta {\overrightarrow{\phi }}_{eb}^e}\delta {\overrightarrow{\phi }}_{eb}^e+G_g{\overrightarrow{w}}_g+{\overrightarrow{f}}_{\delta {\dot{\overrightarrow{\phi }}}_{eb}^e}\\ \delta {{\dot{\overrightarrow{v}}}_{eb}^e}^{\prime }=F_{\delta {\overrightarrow{v}}_{eb}^e}\delta {\overrightarrow{v}}_{eb}^e+G_a{\overrightarrow{w}}_a+{\overrightarrow{f}}_{\delta {\dot{\overrightarrow{v}}}_{eb}^e}\\ \delta {{\dot{\overrightarrow{r}}}_{eb}^e}^{\prime }=F_{\delta {\overrightarrow{r}}_{eb}^e}\delta {\overrightarrow{r}}_{eb}^e+G_r{\overrightarrow{w}}_r+{\overrightarrow{f}}_{\delta {\dot{\overrightarrow{r}}}_{eb}^e}\end{array}.$$

(15)

Hence, an EKF state equation under the IMU faults condition generalized as Equation (16) in a continuous form. The error-state vector contains the IMU position error, the IMU velocity error, the IMU attitude error, accelerometer biases, gyroscope biases, the GNSS receiver offset, and the GNSS receiver drift:

$${\overrightarrow{X}}^{\prime }=F\overrightarrow{X}+G_w\overrightarrow{w}+{\overrightarrow{f}}_{\dot{\overrightarrow{X}}}$$

(16)

where

$$\overrightarrow{X}=\left[\begin{array}{c}\begin{array}{c}\delta {\overrightarrow{\phi }}_{eb}^e\\ \delta {\overrightarrow{v}}_{eb}^e\end{array}\\ \delta {\overrightarrow{r}}_{eb}^b\\ {\overrightarrow{b}}_a\\ \begin{array}{c}{\overrightarrow{b}}_g\\ \delta {\rho }_c\\ \delta {\dot{\rho }}_c\end{array}\end{array}\right],\overrightarrow{w}=\left[\begin{array}{c}{\overrightarrow{w}}_a\\ {\overrightarrow{w}}_g\end{array}\right],f_{\dot{\overrightarrow{X}}}=\left[\begin{array}{c}\begin{array}{c}{\overrightarrow{f}}_{\delta {\dot{\overrightarrow{\phi }}}_{eb}^e}\\ {\overrightarrow{f}}_{\delta {\dot{\overrightarrow{v}}}_{eb}^e}\\ {\overrightarrow{f}}_{\delta {\dot{\overrightarrow{r}}}_{eb}^e}\\ 0_3\end{array}\\ \begin{array}{c}{0}_3\\ 0\\ 0\end{array}\end{array}\right],G_w=\left[\begin{array}{cc}{0}_3& -C_b^e\\ -C_b^e& 0_3\\ 0_3& 0_3\\ 0_3& 0_3\\ 0_3& 0_3\\ 0& 0\\ 0& 0\end{array}\right]$$

$$F=\left[\begin{array}{ccccccc}\left[{\overrightarrow{\omega }}_{ie}^e\times \right]& 0_3& 0_3& 0_3& C_b^e& 0& 0\\ -\left[\left({\widehat{C}}_b^e{\widehat{f}}_{ib}^b\right)\times \right]& -2\left[{\overrightarrow{\omega }}_{ie}^e\times \right]& 0& -C_b^e& 0_3& 0& 0\\ 0_3& I_3& 0& 0_3& 0_3& 0& 0\\ 0_3& 0_3& 0_3& 0_3& 0_3& 0& 0\\ 0_3& 0_3& 0_3& 0_3& 0_3& 0& 0\\ 0& 0& 0& 0& 0& 0& I\\ 0& 0& 0& 0& 0& 0& 0\end{array}\right].$$

The discrete form of Equation (16) is shown in Equation (17) [18]:

$${{\overrightarrow{X}}_k}^{\prime }={\Phi }_k{\overrightarrow{X}}_{k-1}+{\overrightarrow{\omega }}_k+{\overrightarrow{f}}_{{\overrightarrow{X}}_k}.$$

(17)

The discrete form of $F$ is shown as

$$\Phi \approx \left[\begin{array}{ccccccc}{I}_3-\left[{\overrightarrow{\omega }}_{ie}^e\times \right]{\tau }_s& 0_3& 0_3& 0_3& C_b^e{\tau }_s& 0& 0\\ -\left[\left({\widehat{C}}_b^e{\widehat{f}}_{ib}^b\right)\times \right]{\tau }_s& I_3-2\left[{\overrightarrow{\omega }}_{ie}^e\times \right]{\tau }_s& {\left[{\overrightarrow{\omega }}_{ie}^e\times \right]}^2{\tau }_s& -C_b^e{\tau }_s& 0_3& 0& 0\\ 0_3& I_3{\tau }_s-{\left[{\overrightarrow{\omega }}_{ie}^e\times \right]}^2{\tau }_s{}^2& I_3{\tau }_s+\left[{\overrightarrow{\omega }}_{ie}^e\times \right]{\tau }_s{}^2/2& 0_3& 0_3& 0& 0\\ 0_3& 0_3& 0_3& I_3& 0_3& 0& 0\\ 0_3& 0_3& 0_3& 0_3& I_3& 0& 0\\ 0& 0& 0& 0& 0& I_3& {\tau }_s\\ 0& 0& 0& 0& 0& 0& I_3\end{array}\right]$$

(18)

where ${\tau }_s$ is the time interval of the adjacent propagation.

Thus, the predicted state under the IMU faults condition at epoch k is

$${{\widehat{X}}_k^{-}}^{\prime }={\Phi }_k{\widehat{X}}_{k-1}+{\overrightarrow{f}}_{{\overrightarrow{X}}_k}.$$

(19)

Substituting Equation (19) into Equation (2), the measurement update state under the IMU faults condition at epoch k is [18]:

$$\begin{array}{ll}{{\widehat{X}}_k}^{\prime }& ={{\widehat{X}}_k^{-}}^{\prime }+{K_k}^{\prime }\left({\overrightarrow{Z}}_k-H_k{{\widehat{X}}_k^{-}}^{\prime }\right)\\ & ={\Phi }_k{\widehat{X}}_{k-1}+{\overrightarrow{f}}_{{\overrightarrow{x}}_k}+{K_k}^{\prime }\left({\overrightarrow{Z}}_k-H_k\left({\Phi }_k{\widehat{X}}_{k-1}+{\overrightarrow{f}}_{{\overrightarrow{X}}_k}\right)\right)\\ & =\left(I-{K_k}^{\prime }{H}_k\right)\left({\Phi }_k{\widehat{X}}_{k-1}+{\overrightarrow{f}}_{{\overrightarrow{X}}_k}\right)+{K_k}^{\prime }{\overrightarrow{Z}}_k\end{array}$$

(20)

where ${K_k}^{\prime }$ is the Kalman gain matrix computed by the faulty state transition matrix due to the IMU faults measurements and $H_k$ is the measurement matrix at epoch k (Appendix B.1).

To simplify Equation (20), a matrix ${L_k}^{\prime }$ is introduced as Equation (21) [18]:

$${L_k}^{\prime }=\left(I-{K_k}^{\prime }{H}_k\right).$$

(21)

Subsequently, Equation (20) can be simplified as

$${{\widehat{X}}_k}^{\prime }={L_k}^{\prime }{\Phi }_k{\widehat{X}}_{k-1}+{L_k}^{\prime }{\overrightarrow{f}}_{{\overrightarrow{x}}_k}+{K_k}^{\prime }{\overrightarrow{Z}}_k.$$

(22)

According to Equation (22), the additional term due to the IMU faults measurement at epoch k is ${L_k}^{\prime }{\overrightarrow{f}}_{{\overrightarrow{X}}_k}$.

3.2. EKF State Error Caused by IMU Faults

The true state can be defined as shown in Equation (23):

$${\overrightarrow{X}}_k={\Phi }_k{\overrightarrow{X}}_{k-1}+{\overrightarrow{\omega }}_k.$$

(23)

According to the previous study [18], the state error propagation between two adjacent epochs can be derived as follows:

In step 1, multiply the matrix ${L_k}^{\prime }$ by the true state, ${\overrightarrow{X}}_k$:

$$\begin{array}{ll}{L_k}^{\prime }{\overrightarrow{X}}_k& ={L_k}^{\prime }\left({\Phi }_k{\overrightarrow{X}}_{k-1}+{\overrightarrow{\omega }}_k\right)\\ & ={L_k}^{\prime }{\Phi }_k{\overrightarrow{X}}_{k-1}+{L_k}^{\prime }{\overrightarrow{\omega }}_k\end{array}.$$

(24)

In step 2, subtract ${L_k}^{\prime }{\overrightarrow{X}}_k$ from ${{\widehat{X}}_k}^{\prime }$ in Equation (22):

$${{\widehat{X}}_k}^{\prime }-{L_k}^{\prime }{\overrightarrow{X}}_k={L_k}^{\prime }{\Phi }_k{\widehat{X}}_{k-1}+{L_k}^{\prime }{\overrightarrow{f}}_{{\overrightarrow{x}}_k}+{K_k}^{\prime }{\overrightarrow{Z}}_k-{L_k}^{\prime }{\overrightarrow{X}}_k.$$

(25)

In step 3, substitute ${\overrightarrow{Z}}_k=H_k{\overrightarrow{X}}_k+{\overrightarrow{v}}_k$ into Equation (25):

$$\begin{array}{ll}{{\widehat{X}}_k}^{\prime }-{L_k}^{\prime }{\overrightarrow{X}}_k& ={L_k}^{\prime }{\Phi }_k{\widehat{X}}_{k-1}+{L_k}^{\prime }{\overrightarrow{f}}_{{\overrightarrow{X}}_k}+{K_k}^{\prime }\left(H_k{\overrightarrow{X}}_k+{\overrightarrow{v}}_k\right)-{L_k}^{\prime }\left({\Phi }_k{\overrightarrow{X}}_{k-1}+{\overrightarrow{\omega }}_k\right)\\ & ={L_k}^{\prime }{\Phi }_k\underset{\delta X_{k-1}}{\underbracket{\left({\widehat{X}}_{k-1}-{\overrightarrow{X}}_{k-1}\right)}}+{L_k}^{\prime }{\overrightarrow{f}}_{{\overrightarrow{X}}_k}+{K_k}^{\prime }{\overrightarrow{v}}_k-{L_k}^{\prime }{\overrightarrow{\omega }}_k+\underleftarrow{\underbracket{{K_k}^{\prime }{H}_k{\overrightarrow{X}}_k}}\end{array}.$$

(26)

Substitute Equation (21) into Equation (26):

$$\begin{array}{l}\underset{\delta X_k}{\underbracket{{{\widehat{X}}_k}^{\prime }-{\overrightarrow{X}}_k}}={L_k}^{\prime }{\Phi }_k\underset{\delta x_{k-1}}{\underbracket{\left({\widehat{X}}_{k-1}-{\overrightarrow{X}}_{k-1}\right)}}+{L_k}^{\prime }{\overrightarrow{f}}_{{\overrightarrow{X}}_k}+{K_k}^{\prime }{\overrightarrow{v}}_k-{L_k}^{\prime }{\overrightarrow{\omega }}_k\\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\downarrow \\ \hspace{1em}\hspace{1em}\delta X_k={L_k}^{\prime }{\Phi }_k\delta X_{k-1}+{L_k}^{\prime }{\overrightarrow{f}}_{{\overrightarrow{X}}_k}+{K_k}^{\prime }{\overrightarrow{v}}_k-{L_k}^{\prime }{\overrightarrow{\omega }}_k\end{array}.$$

(27)

3.3. Error Overboundings

For the error-state EKF, the innovation at epoch k can be expressed as follows:

$$\begin{array}{ll}{\overrightarrow{\gamma }}_k& ={\overrightarrow{Z}}_k-H_k{{\widehat{X}}_k^{-}}^{\prime }\\ & =H_k{\overrightarrow{X}}_k+{\overrightarrow{v}}_k-H_k{{\widehat{X}}_k^{-}}^{\prime }\\ & =H_k\left({\Phi }_k{\overrightarrow{X}}_{k-1}+{\overrightarrow{\omega }}_k\right)+{\overrightarrow{v}}_k-H_k\left({\Phi }_k{\widehat{X}}_{k-1}+{\overrightarrow{f}}_{{\overrightarrow{X}}_k}\right)\\ & =H_k{\Phi }_k\underset{-\delta X_{k-1}}{\underbracket{\left({\overrightarrow{X}}_{k-1}-{\widehat{X}}_{k-1}\right)}}+H_k{\overrightarrow{\omega }}_k+{\overrightarrow{v}}_k-H_k{\overrightarrow{f}}_{{\overrightarrow{X}}_k}\\ & =-H_k{\Phi }_k\delta X_{k-1}+H_k{\overrightarrow{\omega }}_k+{\overrightarrow{v}}_k-H_k{\overrightarrow{f}}_{{\overrightarrow{X}}_k}\end{array}.$$

(28)

As Equations (27) and (28) show, the final EKF state error and EKF innovation using the error-state EKF were the same as that obtained by the full-state EKF in [18]. Based on the method proposed in [18] for the full-state EKF, the error overboundings against the IMU faults for the error-state EKF can be expressed as Equation (29), which was originally derived in [18] for the full-state EKF.

$$P{L}_{error-overboundings}=-V_{k,p}{\overrightarrow{\gamma }}_k\pm K_{md,IMU}\left\{{\sigma }_{V_{k,p}{\overrightarrow{v}}_k,v}+{\sigma }_{{K_{k,p}}^{\prime }{\overrightarrow{v}}_k,v}\right\}.$$

(29)

As we expected, the final error overboundings equation (Equation (29)) can be expressed in an identical manner to those derived based on the full-state EKF in the previous study [18].

4. Simulation and Analysis

4.1. Simulation Conditions

Simulations were conducted to show the error overboundings for UAVs under the IMU faults condition using both the full-state EKF (Appendix C) and the error-state EKF. In the simulations, the GNSS measurement noise and the IMU gyroscope and accelerometer measurement noise were modeled as Gaussian white noise, and the corresponding noise parameters were also used in the error overboundings.

The specific UAV trajectory can be seen in Figure 3, configured as an integration of two 45° turns and one vertical climb. The simulation time was 7 min.

The IMU sensors were set as consumer-grade inertial sensors with the noise parameters listed in

Table 1. IMU noise simulation parameters.

IMU Sensor (Consumer-Grade)
Accelerometer		Gyroscope
Noise ($\left[\mu g/\sqrt{Hz}\right]$)	Bias Noise ($\mu g$)	Noise ($\left[°/h/\sqrt{Hz}\right]$)	Bias Noise ($\left[°/h\right]$)
120	150	50	15

[24]. In the simulation, a 24-satellite GPS constellation was used, and the mask angle was 7.5°. The GNSS measurement noise was introduced into the pseudo-ranges as white Gaussian noise with a mean of zero and a variance of 30 m². The data update frequency was 5 Hz for the GNSS receiver and 100 Hz for the IMU sensors. The integrated Kalman filter update was only performed when the GNSS signal was updated, otherwise only the IMU output was used. The output frequency was 100 Hz, which was the same as the IMU update frequency.

All simulations were performed using MATLAB R2018a on a Windows 10 PC with 12 GB of RAM and an Intel Core i7-6700 3.4 GHz processor.

4.2. Simulation Under Injected IMU Gyroscope and Accelerometer Faults

For the simulations of the error overboundings, the prior probabilities of an IMU gyroscope and accelerometer sensor faults were assumed to be 10⁻³, considering the IMU hardware performance used for a low-cost use and consumer-grade inertial device [25]. In the simulation, the ramp IMU gyroscope and accelerometer faults, with magnitudes of 1.5 m/s² in the accelerometer and 0.03 rad/s in the gyroscope, was injected into the IMU accelerometer and gyroscope for three axes after 200 s. The faults injection lasted 10 s.

As an example, the vertical position errors are presented as the red dotted curve, which is the difference between the EKF estimated vertical position and the true vertical position, in Figure 4. In addition, the vertical error overboundings (VPL_{error.overboundings}) are presented in Figure 4, as blue solid curves for the error-state EKF and green solid curves for the full-state EKF. In order to intuitively present the vertical state error in Figure 4, the state error is represented in Figure 5, as a blue dotted curve for the error-state EKF and a red solid curve for the full-state EKF. In this simulation, the value of K_md,IMU was chosen to be 3.29, which overbounds the noise of the state bias estimates with a missed detection probability of 10⁻³ (Appendix B.3). The simulation results show that the overboundings had the same trend as the position error and that the position error can be accurately overbounded during UAV operations under the missed detection probability requirement. Moreover, the error overboundings of the two EKFs were the same when no fault occurred. When faults were injected, the error overboundings for the error-state were higher than the full-state; the reason for this is that the error-state EKF is more sensitive to the faults than the full-state EKF. The east and north state errors and overboundings (i.e., east error overboundings (EPL_{error.overboundings}), north error overboundings (NPL_{error.overboundings})) are also presented in Figure 6 and Figure 7, respectively, which further proves that the position error can be accurately overbounded and that the error overboundings of the two EKFs are the same in all directions.

4.3. Computational Efficiency Comparison

To compare the efficiency of the full-state and the error-state EKFs in the error overboundings calculation, the filtering time at each epoch is recorded in Figure 8. The filtering time of the error-state EKF was lower and more stable than that of the full-state EKF. The average filtering time for the error-state EKF was approximately 7.573 × 10⁻⁴ s, 16.163% less than that for the full-state error, approximately 9.033 × 10⁻³ s. A detailed comparison can be seen in

Table 2. Filtering time comparison.

Scheme	Maximum	Minimum	Mean	Variance
Error-state	1.868 × 10−3 s	8.03 × 10−5 s	7.573 × 10−4 s	4.8364 × 10−8
Full-state	3.954 × 10−3 s	9.21 × 10−5 s	9.033 × 10−4 s	1.4365 × 10−7

.

Actually, the filtering time required for sparse matrix operations is usually proportional to the number of arithmetic operations in the number of nonzero elements. In the system state propagation period, the state transition matrix Φ_k of the full-state EKF has more nonzero elements than that of the error-state EKF. As shown in Figure 8, taking, for example, the simulation times at 312.5 s and 357.5 s, there are distinct peaks for the full-state EKF because of the sparse matrices in the system propagation equation. Moreover, the convergence process of the EKF was evaluated by 3.66 times variance. As shown in Figure 9, the state error of the error-state EKF converged faster than the full-state EKF, the oscillation before convergence was smaller, and the convergence process was smoother.

To sum up, error-state EKF-based method and full-state EKF-based method are consistent in final equation expression and overbounding results of the error overboundings. However, the qualitative analysis result shows that error-state EKF-based method has a higher calculation efficiency and faster convergence speed, and the quantitative comparison result in Table 2 shows error-state EKF-based method has shorter filtering time.

5. Conclusions

A real-time full-state EKF vertical protection level (VPL) method against IMU sensor faults to assure navigation integrity is proposed in [18]. Based on the method, the final error overbounding equation against the IMU faults for the error-state EKF estimation can be expressed in an identical manner. The IMU faults propagation process was analyzed in this paper for the error-state EKF of tightly coupled IMU/GNSS integrated architecture. Accordingly, the error overboundings against the IMU faults was obtained. The error overboundings for the full-state and the error-state EKFs presented the same form in theoretical derivation, consisting of terms relating to the EKF innovation, the estimated bias, and the remaining position error. The simulation result shows that the derived error overboundings can fit the position error, reflecting the position error change caused by the IMU faults. Moreover, the error overboundings of the error-state EKF were the same as that of the full-state EKF in mathematical simulation. As the error-state EKF is more practical and its error overboundings calculation time was lower, the error-state EKF is recommended to assure the integrity of the IMU/GNSS integrated architecture for an UAV. The proposed error overbounding method with real collected data will be carried out in future work.