A switched adaptive strategy for state estimation in MEMS-based inertial navigation systems with application for a flight test

Abstract

This paper proposes a switched model to improve the estimation of Euler angles and decrease the inertial navigation system (INS) error, when the centrifugal acceleration occurs. Depending on the situation, one of the subsystems of the proposed switched model is activated for the estimation procedure. During global positioning system (GPS) outages, an extended Kalman filter (EKF) operates in the prediction mode and corrects the INS information, based on the system error model. Compared with previous works, the main advantages of the proposed switched-based adaptive EKF (SAEKF) method are (i) elimination of INS error, during the centrifugal acceleration, and (ii) high accuracy in estimating the attitude and positioning, particularly during GPS outages. To validate the efficiency of the proposed method in various trajectories, an experimental flight test is performed and discussed, involving a microelectromechanical (MEMS)-based INS. The comparative study shows that the proposed method considerably improves the accuracy in various scenarios.

1. Introduction

In recent years, various investigations have been devoted to develop the navigation technologies for autonomous vehicles. Huh adopted a laser navigation system (Huh et al., 2013), as an expensive method for mass-production vehicles. However, an integrated navigation system (INS) with low cost and high precision is required (Ward et al., 2006). To estimate the attitude and position, with acceptable accuracy and affordable cost, data fusion and prediction algorithms may be used (Abdel-Hafez et al., 2015). Compared with microelectromechanical (MEMS) gyroscopes, fibre optic gyros (FOGs) may be more accurate and reliable. Although using a FOG is an expensive strategy, it may provide a higher level of stability than the MEMS technology, with noise suppression capability. Of course, the global positioning system (GPS), as one of the most widespread technologies of vehicle localisation, may also provide the accurate position and velocity information (Xu et al., 2018). However, the GPS may suffer from widespread varieties of interferences, such as multipath effects, electromagnetic interference, block of signals, interruptions and outages (Chen and Fang, 2014). Such interferences may be taken as external disturbances for the GPS. To overcome the disadvantages of the GPS, it is usually integrated with an INS, which forms a self-contained system. Nevertheless, MEMS-based INS may be not appropriate for inertial navigation over an extended period (Groves, 2015), as the accuracy deteriorates over time, due to the sensors' bias error drift, bias stability of gyroscope and misalignment (Quinchia et al., 2013). Considering such drawbacks, the MEMS-based inertial sensors may be preferred in some vehicles, due to the low-cost localisation system (Liu et al., 2010). During GPS outages, the INS provides positioning information, which assists GPS signal reacquisition after an outage. It also reduces the search domain, required for detecting and correcting GPS cycle slips (El-Rabbany, 2002). Compared with FOG-based INS and ring laser gyro-based INS, the MEMS-based performance may provide lower accuracy and reliability (Xu et al., 2018). An autonomous geolocalisation system for near-Earth applications is represented in Bessaad (Bessaad et al., 2021), based on the strap-down INS.

Kalman filtering, as one of the most common fusion algorithms, is used to fuse the information of both the INS and GPS (Toledo-Moreo et al., 2007; Feng et al., 2014). This method requires a dynamic model of the INS, a stochastic model of the inertial sensor errors and a priori information about the data covariance (Jie et al., 2006). The Kalman filter (KF), considered as the benchmark for INS/GPS integration (Obradovic et al., 2007), has been widely used for data fusion. Some investigations have focused on combining GPS and INS navigation, using both a KF and EKF to compensate the errors, generated by information fusion (Wang et al., 2006). The adaptive EKF (AEKF) is one of the strategies, used to improve the estimation of the covariance matrices (Liu et al., 2018). A new adaptive KF has been presented to provide the accurate and robust mini quadrotor position information in indoor environments (Xu et al., 2020). An adaptive KF may be also used to adjust the noise covariance of GPS measurements, under different positioning accuracies (Zhang and Hsu, 2018). An enhanced adaptive nonlinear filter has recently been represented to integrate a star tracker sensor with a system inertial navigation system (SINS). The proposed filter adapts the measurement noise covariance matrix, based on the available data (Bessaad et al., 2022). The main objective of the INS/GPS procedure is to collect images in a period of time to obtain exact velocity and position information (Chen et al., 2019). A vision measurement system may be integrated with inertial sensor data and the GPS to calculate the attitude and position (Vetrella et al., 2019). Integrated INS/GPS data also provides positioning information during GPS outages. If GPS outage occurs, the KF corrects the INS information, based on the system error model. Many researchers have also offered artificial intelligence, as an error predicting and compensating method, to further compensate the INS errors. AI-based approaches commonly include an adaptive neuro-fuzzy inference system (Abdel-Hamid et al., 2007; Jaradat and Abdel-Hafez, 2014) and a radial basis function neural network (Lei and Li, 2013). Nevertheless, complex computation and long learning time may generate a long delay, which is a drawback for the real-time system of autonomous vehicles (Zhao et al., 2015). Some researchers also used different combination forms of GPS and INS, such as tightly-coupled GPS/INS navigation (Wendel et al., 2006), which may be too complex for implementation.

In spite of the existing achievements, GPS noise composition may occur due to surrounding buildings and electromagnetic interferences. The MEMS-INS is also affected by noise, which seriously deteriorates the estimation accuracy. In addition, different situations, such as linear and centrifugal acceleration, may occur in navigation manoeuvres, which cause additional errors. In such situations, the measurement of inertial sensor is not accurate, and the errors may cause incorrect predictions. For different modes in navigation, such as static, low dynamic, high vibration, linear acceleration, centrifugal acceleration and additional magnetic field, the existing KF-based algorithms adopt the same navigation system model. However, a precise estimation should be achieved in such modes as centrifugal acceleration, during the vehicle's rotation. Thus, a switched-based model is proposed here, which adopts the corresponding subsystem, depending on the operation mode of the vehicle. Then, a switched AEKF is developed, considering the centrifugal acceleration condition. The switching strategy, as a promising analysis and synthesis strategy, has been also applied to other applications, such as robotics (Noghreian and Koofigar, 2021) and hybrid-energy systems (Noghreian and Koofigar, 2020).

The organisation of the paper is as follows. The mathematical model for the integrated INS/GPS navigation system and the proposed switched model are presented in Section 2. In Section 3, the EKF and AEKF methods are formulated to be used in navigation. The efficiency of the proposed SAEKF in precise estimation is emphasised during the centrifugal acceleration. In Section 4, an experimental study is discussed in a flight test, compared with the performance of the EKF and AEKF. Finally, Section 5 provides the concluding remarks.

2. Proposed switched mathematical model

In an INS, a mathematical model for INS/GPS integration is used to estimate the system states. This section represents the conventional model, followed by the proposed switched-based one.

2.1 Conventional navigation model

The navigation frame (n-frame) is commonly selected as the east-north-up (E-N-U) geography frame. The body frame b presents a coordination with the x-axis towards the forward direction, the y-axis for the transverse direction and the z-axis towards the vertical direction of the vehicle. Because the navigation equations are nonlinear, the EKF may be used for estimation purposes. The nonlinear attitude, velocity and position equations can be formulated as (Noureldin et al., 2013)

(1)\begin{equation}\left\{ {\begin{array}{@{}c} {\dot{x}(t )= f({x(t )} )}\\ {z(t )= h({x(t )} )} \end{array}} \right.\end{equation}

with

(2)\begin{gather}f(x )= \left[ {\begin{array}{@{}c@{}} {{{\dot{r}}^n}}\\ {{{\dot{v}}^n}}\\ {\begin{array}{@{}c@{}} {\dot{q}}\\ {{{\dot{\omega }}_b}} \end{array}} \end{array}} \right] = \left[ {\begin{array}{@{}c@{}} {{D^{ - 1}}{v^n}}\\ {R_b^n{f^b} - ({2\Omega _{ie}^n + \Omega _{en}^n} ){v^n} + {g^n}}\\ {\begin{array}{@{}c@{}} {\dfrac{1}{2}\Omega ({\omega ,{\omega_b}} )q}\\ 0 \end{array}} \end{array}} \right]\end{gather}

(3)\begin{gather}{r^n} = {\left[ {\begin{array}{@{}ccc@{}} l& \lambda & h \end{array}} \right]^T}\end{gather}

(4)\begin{gather}{v^n} = {\left[ {\begin{array}{@{}ccc@{}} {{v_e}}& {{v_n}}& {{v_u}} \end{array}} \right]^T}\end{gather}

(5)\begin{gather}q = {\left[ {\begin{array}{@{}ccc@{}} {{q_0}}& {{q_1}}& {\begin{array}{@{}cc@{}} {{q_2}}& {{q_3}} \end{array}} \end{array}} \right]^T}\end{gather}

(6)\begin{gather}{\omega _b} = {\left[ {\begin{array}{@{}ccc@{}} {{\omega_{bx}}}& {{\omega_{by}}}& {{\omega_{bz}}} \end{array}} \right]^T}\end{gather}

(7)\begin{gather}{f^b} = {\left[ {\begin{array}{@{}ccc@{}} {{a_x}}& {{a_y}}& {{a_z}} \end{array}} \right]^T}\end{gather}

(8)\begin{gather}{D^{ - 1}} = \left[ {\begin{array}{*{20}{c}} 0& {\dfrac{1}{{{R_M} + h}}}& 0\\ {\dfrac{1}{{({{R_N} + h} )cosl}}}& 0& 0\\ 0& 0& 1 \end{array}} \right]\end{gather}

(9)\begin{gather}z = {\left[ {\begin{array}{@{}ccc@{}} {\begin{array}{@{}ccc@{}} l& \lambda & h \end{array}}& {\begin{array}{@{}ccc@{}} {{v_e}}& {{v_n}}& {{v_u}} \end{array}} \end{array} \begin{array}{@{}ccc@{}} \phi & \theta & \psi \end{array}} \right]^T}\end{gather}

and

(10)\begin{equation}x = {[{l,\lambda ,h,{v_e},{v_n},{v_u},{q_0},{q_1},{q_2},{q_3},{\omega_{bx}},{\omega_{by}},{\omega_{bz}}} ]^T}\end{equation}

denotes the state vector, and ${g^n}$ is the gravity vector in navigation frame. The latitude, longitude and altitude are denoted by $l, \lambda ,$ and h, respectively; ${v_e}, {v_n}, {v_u}$ present the velocity on three axes in the navigation frame; and ${q_0}, {q_1}, {q_2}, {q_3}$ are quaternion parameters.

The gyro biases, denoted by ${\omega _{bx}},{\omega _{by}},{\omega _{bz}}$, are unknown and ${\omega _x}, {\omega _y}, {\omega _z}$are angular velocities, measured by the gyroscope sensors. ${f^b}$ is the acceleration vector in the body frame and $R_b^n$ is the rotation matrix:

(11)\begin{equation}\dot{q} = \left[ {\begin{array}{@{}l@{}} {\dfrac{{ - 1}}{2}{q_1}({{\omega_x} - {\omega_{bx}}} )+ \dfrac{{ - 1}}{2}{q_2}({{\omega_y} - {\omega_{by}}} )+ \dfrac{{ - 1}}{2}{q_3}({{\omega_z} - {\omega_{bz}}} )}\\ {\dfrac{1}{2}{q_0}({{\omega_x} - {\omega_{bx}}} )+ \dfrac{{ - 1}}{2}{q_3}({{\omega_y} - {\omega_{by}}} )+ \dfrac{1}{2}{q_2}({{\omega_z} - {\omega_{bz}}} )}\\ {\dfrac{1}{2}{q_3}({{\omega_x} - {\omega_{bx}}} )+ \dfrac{1}{2}{q_0}({{\omega_y} - {\omega_{by}}} )+ \dfrac{{ - 1}}{2}{q_1}({{\omega_z} - {\omega_{bz}}} )}\\ {\dfrac{{ - 1}}{2}{q_2}({{\omega_x} - {\omega_{bx}}} )+ \dfrac{1}{2}{q_1}({{\omega_y} - {\omega_{by}}} )+ \dfrac{1}{2}{q_0}({{\omega_z} - {\omega_{bz}}} )} \end{array}} \right]\end{equation}

where $\Omega _{en}^n$ is the skew-symmetric matrix of the rotation rate vector of the navigation frame, versus the Earth frame, and $\Omega _{ie}^n$ is the skew-symmetric matrix of the turn rate of the Earth around its spin axis. $\Omega ({\omega ,{\omega_b}} )$ is the skew-symmetric matrix of the angular velocity with gyroscope biases and

(12)\begin{gather}\Omega _{ie}^n = \left[ {\begin{array}{@{}ccc@{}} 0& { - {\omega^e}sinl}& {{\omega^e}cosl}\\ {{\omega^e}sinl}& 0& 0\\ { - {\omega^e}cosl}& 0& 0 \end{array}} \right]\end{gather}

(13)\begin{gather} \Omega _{en}^n = \left[{\begin{array}{@{}ccc@{}} 0& {\dfrac{{ - {v_e}tanl}}{{{R_N} + h}}}& {\dfrac{{{v_e}}}{{{R_N} + h}}}\\ {\dfrac{{{v_e}tanl}}{{{R_N} + h}}}& 0& {\dfrac{{{v_n}}}{{{R_M} + h}}}\\ {\dfrac{{ - {v_e}}}{{{R_N} + h}}}& {\dfrac{{ - {v_n}}}{{{R_M} + h}}}& 0 \end{array}} \right]\end{gather}

in which ${R_M}$ and ${R_N}$ are the meridian and normal radius of curvature respectively, calculated by

(14)\begin{equation}\begin{array}{l} {R_M} = \dfrac{{R({1 - {e^2}} )}}{{{{({1 - {e^2}si{n^2}l} )}^{1 \cdot 5}}}}\\ {R_N} = \dfrac{R}{{{{({1 - {e^2}si{n^2}l} )}^{0 \cdot 5}}}} \end{array}\end{equation}

where R is the length of the semi-major axis and e stands for the major eccentricity of the ellipsoid.

Figure 1 demonstrates the mechanism of an INS in the navigation frame. In the position calculation, the discrete form of the latitude and longitude equations may be rewritten as

(15)\begin{gather}{l_k} = {l_{k - 1}} + \frac{{{v_n}}}{{{R_M} + h}}dt\end{gather}

(16)\begin{gather}{\lambda _k} = {\lambda _{k - 1}} + \frac{{{v_e}}}{{({{R_N} + h} )\textrm{cos}({{l_{k - 1}}} )}}dt\end{gather}

at time step k, where dt is the sampling time of the inertial sensors.

Figure 1. Block diagram of an INS mechanism in navigation frame

The navigation frame and the Earth frame are illustrated in Figure 2. The x-axis in the Earth frame passes through the intersection of the equatorial plane and the prime meridian (ie, the Greenwich meridian). The z-axis is through the conventional terrestrial pole and the y-axis completes the right-hand coordinate system in the equatorial plane.

Figure 2. Navigation ENU frame and Earth frame

By the measurement model function $h(x )$, the relationship between the measurements and systems’ states is generally given by,

(17)\begin{equation}z = h(x )+ v\end{equation}

where v is the measurement noise vector. More precisely, the measurement function $h(x )$ is formed by

(18)\begin{equation}h(x )= {\left[ {\begin{array}{@{}ccc@{}} {{H_{XYZ}}}& {{H_{UVW}}}& {{H_{\phi \theta \psi }}(x )} \end{array}} \right]^T}\end{equation}

where

(19)\begin{gather}{H_{XYZ}} = \left[ {\begin{array}{@{}cc@{}} {{I_{3\ast 3}}}& {{0_{3\ast 10}}} \end{array}} \right].x\end{gather}

(20)\begin{gather}{H_{UVW}} = \left[ {\begin{array}{@{}ccc@{}} {{0_{3\ast 3}}}& {{I_{3\ast 3}}}& {{0_{3\ast 7}}} \end{array}} \right].x\end{gather}

(21)\begin{gather} {H_{\phi \theta \psi }}(x )= \left[ {\begin{array}{@{}c@{}} {arctan\left( {\dfrac{{2{q_0}{q_1} + 2{q_2}{q_3}}}{{q_0^2 - q_1^2 - q_2^2 + q_3^2}}} \right)}\\ {arcsin({2{q_0}{q_2} - 2{q_3}{q_1}} )}\\ {arctan\left( {\dfrac{{2{q_0}{q_3} + 2{q_2}{q_1}}}{{q_0^2 + q_1^2 - q_2^2 - q_3^2}}} \right)} \end{array}} \right]\end{gather}

The rotation matrix $R_b^n$ (b frame to n frame), is given by

(22)\begin{align} R_b^n = \left[ {\begin{array}{@{}lll@{}} {cos(\phi )cos(\psi )- sin(\phi )sin(\theta )sin(\psi )}& { - cos(\theta )sin(\psi )}& {sin(\phi )cos(\psi ) + cos(\phi )sin(\theta )sin(\psi )}\\ {cos(\phi )sin(\psi )+ sin(\phi )sin(\theta )cos(\psi )}& {cos(\theta )cos(\psi )}& {sin(\phi )sin(\psi )- cos(\phi )sin(\theta )cos(\psi )}\\ { - sin(\phi )cos(\theta )}& {sin(\theta )}& {cos(\phi )cos(\theta )} \end{array}} \right]\end{align}

The parameter descriptions in a navigation system are summarised in Table 1.

Table 1. Parameter description in a navigation system

2.2 Proposed switched model

To improve the state estimation in an INS, a switched model with two subsystems is defined here, including a main model and an auxiliary one. The second model is adopted in a centrifugal acceleration mode, in which the sensors may exhibit inaccurate measurements. In other words, when the vehicle is turning, the centrifugal acceleration mode is on and the estimation with the conventional mathematical model causes considerable errors, mainly due to gyro biases. Thus, in such manoeuvring, the second (auxiliary) subsystem is activated to improve the estimation performance and reduce errors.

In general, the switched model with two subsystems may be represented as (Lunze and Lamnabhi-Lagarrigue, 2009)

(23)\begin{equation}\left\{ {\begin{array}{@{}c@{}} {\dot{x}(t )= {f_\sigma }({x(t )} )}\\ {z(t )= {h_\sigma }({x(t )} )} \end{array}} \right., \sigma \in \{{ 1,2 } \}\end{equation}

where $\sigma$ is referred to switching signal, indicating which subsystem is activated at time t. In this context, switching means changing from one currently active subsystem to another. Switching may depend on time (time-dependent), or may be a function of states (mode-dependent) or a function of the external input (Liberzon, 2003).

Subsystem 1, used for most of the operating modes of navigation except for the centrifugal acceleration mode, is specified by

(24)\begin{equation} \textrm{sybsystem } 1:\ \left\{ {\begin{array}{@{}c} {\dot{x}(t )= {f_1}({x(t )} )}\\ {z(t )= {h_1}({x(t )} )} \end{array}} \right. \end{equation}

where the states vector x includes the position, velocity, quaternion and gyro biases as

(25)\begin{equation}x = {[{l,\lambda ,h,{v_e},{v_n},{v_u},{q_0},{q_1},{q_2},{q_3},{\omega_{bx}},{\omega_{by}},{\omega_{bz}}} ]^T}\end{equation}

In dynamical Equation (24),

(26)\begin{equation} {f_1}(x )= \left[ {\begin{array}{@{}c@{}} {{{\dot{r}}^n}}\\ {{{\dot{v}}^n}}\\ {\begin{array}{@{}c@{}} {\dot{q}}\\ {{{\dot{\omega }}_b}} \end{array}} \end{array}} \right] = \left[ {\begin{array}{@{}c@{}} {{D^{ - 1}}{v^n}}\\ {R_b^n{f^b} - ({2\Omega _{ie}^n + \Omega _{en}^n} ){v^n} + {g^n}}\\ {\begin{array}{@{}c@{}} {\dfrac{1}{2}\Omega ({\omega ,{\omega_b}} )q}\\ 0 \end{array}} \end{array}} \right]\end{equation}

and

(27)\begin{equation}{h_1}(x )= {\left[ {\begin{array}{@{}ccc@{}} {{H_{XYZ}}}& {{H_{UVW}}}& {{H_{1({\phi \theta \psi } )}}(x )} \end{array}} \right]^T}\end{equation}

in which

(28)\begin{gather}{H_{XYZ}} = \left[ {\begin{array}{@{}cc@{}} {{I_{3\ast 3}}}& {{0_{3\ast 10}}} \end{array}} \right].x\end{gather}

(29)\begin{gather}{H_{UVW}} = \left[ {\begin{array}{@{}ccc@{}} {{0_{3\ast 3}}}& {{I_{3\ast 3}}}& {{0_{3\ast 7}}} \end{array}} \right].x\end{gather}

(30)\begin{gather}{H_{1({\phi \theta \psi } )}}(x )= \left[ {\begin{array}{@{}c@{}} {\textrm{arctan}\left( {\dfrac{{2{q_0}{q_1} + 2{q_2}{q_3}}}{{q_0^2 - q_1^2 - q_2^2 + q_3^2}}} \right)}\\ {\textrm{arcsin}({2{q_0}{q_2} - 2{q_3}{q_1}} )}\\ {\textrm{arctan}\left( {\dfrac{{2{q_0}{q_3} + 2{q_2}{q_1}}}{{q_0^2 + q_1^2 - q_2^2 - q_3^2}}} \right)} \end{array}} \right]\end{gather}

The second subsystem, activated when the system enters the centrifugal acceleration mode, is given by

(31)\begin{equation}\textrm{sybsystem } \textrm{2}:\ \left\{{\begin{array}{@{}c@{}} {\dot{x}(t )= {f_2}({x(t )} )}\\ {z(t )= {h_2}({x(t )} )} \end{array}} \right. \end{equation}

with

(32)\begin{equation}x = {[{l,\lambda ,h,{v_e},{v_n},{v_u},{q_0},{q_1},{q_2},{q_3}} ]^T}\end{equation}

which shows that the gyro biases are not updated during the centrifugal acceleration mode.

In subsystem 2, described by Equation (31), the process model and output measurement are respectively represented by

(33)\begin{equation}{f_2}(x )= \left[ {\begin{array}{@{}c@{}} {{{\dot{r}}^n}}\\ {{{\dot{v}}^n}}\\ {\dot{q}} \end{array}} \right] = \left[ {\begin{array}{@{}c@{}} {{D^{ - 1}}{v^n}}\\ {R_b^n{f^b} - ({2\Omega _{ie}^n + \Omega _{en}^n} ){v^n} + {g^n}}\\ {\dfrac{1}{2}\Omega (\omega )q} \end{array}} \right]\end{equation}

and

(34)\begin{equation}{h_2}(x )= {\left[ {\begin{array}{*{20}{c}} {{H_{XYZ}}}& {{H_{UVW}}}& {{H_2}(x )} \end{array}} \right]^T}\end{equation}

where

\begin{gather}{H_{XYZ}} = \left[ {\begin{array}{@{}cc@{}} {{I_{3\ast 3}}}& {{0_{3\ast 7}}} \end{array}} \right].x\nonumber\end{gather}

\begin{gather}{H_{UVW}} = \left[ {\begin{array}{@{}ccc@{}} {{0_{3\ast 3}}}& {{I_{3\ast 3}}}& {{0_{3\ast 4}}} \end{array}} \right].x\nonumber\end{gather}

(35)\begin{gather}{H_2}(x )= \left[ {\begin{array}{@{}l@{}} { - 2({{q_1}{q_3} - {q_0}{q_2}} )- gtan(\phi )\sin (\theta )}\\ { - 2({{q_2}{q_3} + {q_0}{q_1}} )- gsin(\phi )\cos (\theta )}\\ { - {q_0}^2 + {q_1}^2 + {q_2}^2 - {q_3}^2 - gtan(\phi )\sin (\phi )\cos (\theta )}\\ {{b_x}({{q_0}^2 + {q_1}^2 - {q_2}^2 - {q_3}^2} )+ 2{b_z}({{q_1}{q_3} - {q_0}{q_2}} )}\\ {2{b_x}({{q_1}{q_2} - {q_0}{q_3}} )+ 2{b_z}({{q_2}{q_3} + {q_0}{q_1}} )}\\ {2{b_x}({{q_1}{q_3} + {q_0}{q_2}} )+ {b_z}({{q_0}^2 - {q_1}^2 - {q_2}^2 + {q_3}^2} )} \end{array}} \right]\end{gather}

(36)\begin{gather}z = {\left[ {\begin{array}{@{}cc@{}} {\begin{array}{@{}ccc@{}} l& \lambda & h \end{array}}& {\begin{array}{@{}ccc@{}} {{v_e}}& {{v_n}}& {{v_u}} \end{array}} \end{array} \begin{array}{@{}ccc@{}} {\begin{array}{@{}ccc@{}} {{a_x}}& {{a_y}}& {{a_z}} \end{array}}& {\begin{array}{@{}ccc@{}} {{m_x}}& {{m_y}}& {{m_z}}\!\!\end{array}} \end{array}} \right]^T}\end{gather}

in which ${m_x}, {m_y}, {m_z}$ are the magnetometer sensor measurements on the three axes.

3. Proposed algorithm for integrated INS/GPS

3.1 EKF and adaptive EKF

The Kalman filter, as a set of mathematical equations, provides an efficient computational (recursive) strategy to estimate the state of a process. The conventional EKF, as an extension of the classic KF for nonlinear systems, is introduced by

(37)\begin{gather}\hat{x}_k^ -{=} f({{{\hat{x}}_{k - 1}},{u_{k - 1}},0} )\end{gather}

(38)\begin{gather}P_k^ -{=} {A_k}{P_{k - 1}}A_k^T + {Q_{k - 1}}\end{gather}

(39)\begin{gather}{K_k} = P_k^ - H_k^T{({{H_k}P_k^ - H_k^T + {R_k}} )^{ - 1}}\end{gather}

(40)\begin{gather}{\hat{x}_k} = \hat{x}_k^ -{+} {K_k}({{z_k} - h({\hat{x}_k^ - ,0} )} )\end{gather}

(41)\begin{gather}{P_k} = ({I - {K_k}{H_k}} )P_k^ - \end{gather}

where $\hat{x}_k^ -$ and $P_k^ -$ are, respectively, the predicted state vector and the covariance matrix; ${\hat{x}_k}$ and ${P_k}$ are the corrected ones; ${K_k}$ is the filter gain matrix; and subscript k denotes the discrete time step.

To enhance the estimation performance, the covariance matrices ${R_k}$ and ${Q_k}$ may be updated by an adaptive mechanism as (Liu et al., 2018)

(42)\begin{gather}{\hat{C}_{rk}} = \frac{1}{N}\mathop \sum \limits_{j = {j_0}}^k {r_j}r_j^T\end{gather}

(43)\begin{gather}{\hat{R}_k} = {\hat{C}_{rk}} - {H_k}P_k^ - H_k^T\end{gather}

(44)\begin{gather}{\hat{Q}_k} = \frac{1}{N}\mathop \sum \limits_{j = {j_0}}^k \Delta {x_j}x_j^T + {P_k} + {A_k}{P_k}A_k^T\end{gather}

where ${r_k} = {z_k} - h({\hat{x}_k^ - } )$ and $\Delta {x_k} = {K_k}{r_k}$. Incorporating (Equation (43)) into (Equation (39)), the modified ${K_k}$ can be written as

(45)\begin{equation}{K_k} = P_k^ - H_k^T{\left( {\frac{1}{N}\mathop \sum \limits_{j = {j_0}}^k {r_j}r_j^T} \right)^{ - 1}}\end{equation}

3.2 Proposed switched-based AEKF algorithm

Based on the proposed switched measurement model (Equation (23)) in Section 2.2, a switched-based AEKF algorithm is represented here. The block diagram of INS/GPS integration, using the proposed SAEKF, is illustrated in Figure 3.

Figure 3. Block diagram of INS/GPS integration using proposed SAEKF

The proposed algorithm for state estimation may be summarised as follows:

1. Step 1: Depending on the mode of operation, one of the subsystems (Equations (24) or (31)) is adopted in the navigation equations.

2. Step 2: Predicting the process model, the new state $\hat{x}_k^ -$ and error covariance $P_k^ -$ are calculated, in the $k$’th discrete time step, using Equations (37) and (38). Depending on the mode of operation, one of Equations (26) or (33) is adopted.

3. Step 3: Calculate the adaptive covariance matrices ${\hat{R}_k}$ and ${\hat{Q}_k}$ by Equations (43) and (44).

4. Step 4: Update the Kalman gain ${K_k}$, state ${\hat{x}_k}$ and covariance error ${P_k}$ for the next discrete time step by Equations (39)–(41). Depending on the mode of operation, either Equation (27) or (34) is adopted in the navigation equations.

5. Step 5: Go to Step 1 for a new set of samples.

Remark 1. When the accelerometer sensor data presents an error, the KF experiences an error, which causes a switch to the second model. Such error is the difference between the predicted variables and the measured values, generated by the measurement residual ${z_k} - h({\hat{x}_k^ - } )$.

The main effect of the centrifugal acceleration in the normal rotation may be on the y-axis, used to calculate the roll angle. In the proposed method, the measurement model is changed, according to the new mode of centrifugal acceleration. The measured values of the accelerometer sensor on the y-axis and the corrected measured acceleration by the new measurement model are shown in Figure 4.

Figure 4. Raw and compensated acceleration and acceleration fits with the roll angle measured by the mechanical gyroscope (red), (a) raw acceleration measured by the sensor (blue) and (b) compensated acceleration with the new model (blue)

As shown in Figure 5, by creating the centrifugal acceleration, an increment in the error of the Euler angle estimation is included in the conventional KF. By the proposed method, the roll angle error is reduced by adopting a new subsystem (Equation (33) and (34)), as demonstrated in Figure 6. By correctly choosing the covariance matrices’ values, the KF's estimation error is reduced during the centrifugal acceleration mode.

Figure 5. Roll angle error ${z_k} - h({\hat{x}_k^ - } )$ in conventional EKF, ie, subsystem mode (30)

Figure 6. Roll angle error ${z_k} - h({\hat{x}_k^ - } )$ in the proposed SAEKF method

Remark 2. In the proposed algorithm, if the centrifugal acceleration mode takes longer than 3 sec, the new centrifugal mode is activated. In other words, very fast-turning movement in the airplane manoeuvres, which causes a high switching frequency between the two subsystems, does not occur.

Remark 3. The switch time in the algorithm is determined based on the roll angle in turning. By the designer choosing the time duration ${t_c}$ and roll angle threshold ${\phi _c}$, the centrifugal acceleration mode and the switching trigger are activated when $|\phi |> {\phi _c}$, for $t > {t_c}$.

4. Experimental results

To investigate the performance of the proposed switching method, compared with the existing EKF, the flight data is stored on a storage memory. To this end, several aerial manoeuvres have been tested to evaluate the performance of the method. The results of the experimental data have been shown in the time interval, corresponding to the duration of the flight. When the airplane turns, the centrifugal force causes an error in the measurement model. The proposed switched model is adopted to solve the problem, especially to compensate for the effect of centrifugal acceleration on the roll angle. To make a comparative analysis, the conventional EKF, AEKF, integral mechanisation and the proposed algorithm have been implemented and discussed. After installing the INS box along with a GPS antenna on the airplane, the filters operate in parallel, and the outputs are saved on the memory. The technical specifications of the main instruments are reported in Table 2.

Table 2. Technical specifications of the main instruments

4.1 Euler angle estimation of an airplane test

In conventional models of INS, the gyro values are integrated to calculate the Euler angles, as shown in Figure 1. Such a method may be efficient if the gyro bias is negligible, as in advanced technologies such as mechanical gyro and fibre optics. Conversely, the adopted sensors in MEMS-based INS, given in Table 2, may not present high accuracy. By using the integral mechanism, EKF, AEKF and the proposed SAEKF in the INS, the roll angle estimation is illustrated and compared in Figure 7. In general, the estimation error may increase during the centrifugal acceleration mode. As specified in Figure 7, the centrifugal acceleration occurred six times in the test, denoted by numbers 1–6. In the proposed SAEKF, the estimation error is considerably small compared to the other three methods. In the underlying flight test, the mechanical gyro is used as a reference to check the accuracy of the estimation performance. The accuracy of the mechanical gyro is 0 ⋅ 5^°. Compared to the EKF, the AEKF provides a longer duration to deviate from the correct value, with lower estimation error. In particular, during the rotation of flight test, the roll estimation error is reduced by using the SAEKF, as shown in Figure 8. From a comparison viewpoint, Table 3 lists the maximum, average and root mean square (RMS) errors during the whole flight, which confirms that the proposed SAEKF presents less error. The error specifications are also represented in Table 4 for various algorithms. Maximum and RMS errors in the six rotations show that the proposed method is more accurate during the centrifugal acceleration.

Figure 7. Comparing roll angle, EKF (blue), AEKF (red), proposed SAEKF (orange) and only integral mechanisation (purple) with mechanical gyro reference (green)

Figure 8. Roll error with mechanical gyro as reference, EKF Error (blue), AEKF Error (red), proposed SAEKF error (orange) and only integral mechanisation error (purple)

Table 3. Maximum, mean and RMS error of roll angle in the whole flight test

Table 4. Maximum and RMS error of roll angle during centrifugal acceleration in flight rotation

The pitch and yaw angles in different algorithms are demonstrated in Figure 9. Figure 10 shows the error between the mechanical gyro and EKF methods in pitch angle and the error between the GPS and EKF methods in yaw angle. The RMS error of the pitch angle, using various algorithms, is also given in Table 5. When the airplane turns and the roll angle changes, the measurement model, which is calculated by the acceleration sensor, shows 0 for the roll angle. Figure 11 illustrates the performance of the three applied algorithms. Applying the EKF and AEKF, the correct angle is calculated only for the first few seconds. Meanwhile, when the airplane is in horizontal attitude, it takes time for roll angle to be corrected. Such a drawback is removed by using the proposed SAEKF, as it activates the corresponding subsystem for the centrifugal acceleration mode.

Figure 9. Pitch and yaw angle, EKF (blue), AEKF (red), proposed SAEKF (orange), only integral mechanisation (purple) with mechanical gyro reference (green)

Figure 10. Pitch and yaw error with mechanical gyro as reference, EKF error (blue), AEKF error (red), proposed SAEKF error (orange) and only integral mechanisation error (purple)

Figure 11. Roll angle in centrifugal acceleration mode, the measured value (green), and the estimation (red) by (a) EKF, (b) AEKF and (c) proposed SAEKF

Table 5. Maximum, mean and RMS error of pitch angle in the whole flight test

4.2 Position localisation of an airplane during GPS outages

Position localisation is based on the mathematical equations in the navigation systems with the inertial sensors’ data. The gyro rate and magnetic sensor, the roll, pitch and yaw angles are calculated using the accelerometer sensors. Consequently, by converting the values in different coordinates and twice integrating the acceleration, the displacement and position of the aircraft can be determined. When the GPS is disconnected and the information is not available, positioning may fail and the accuracy of calculations is lost.

The experimental test is performed to check the accuracy of the position and motion estimation, when GPS information is unavailable. During the flight, the GPS is disconnected four times and reconnected after one minute. When the GPS information is unavailable, the position is estimated from the relationships governing the inertial navigation. Once the GPS is reconnected and the information is usable, the error generated during the GPS outages is quickly reduced. Applying three algorithms of the EKF, AEKF and proposed SAEKF, Figures 12–14 demonstrate the performance during GPS outages. Mechanical gyro and GPS information is used as the references to compare the estimation of the Euler angle and position, respectively. The position accuracy of the reference is two meters.

Figure 12. GPS data (red) and latitude estimation (blue) with (a) EKF, (b) AEKF and (c) proposed SAEKF

Figure 13. Longitude estimation: GPS data (red) and INS output (blue), by using (a) EKF, (b) AEKF and (c) proposed SAEKF

Figure 14. Position in 2D diagram, GPS data (red) and INS output (blue), with (a) EKF, (b) AEKF and (c) proposed SAEKF

When an EKF is used to determine the position, errors in the estimation of Euler angles affect the relations governing the inertial navigation. The position error is also increased by the integrating procedure.

By using an AEKF, the estimation of Euler angles improves and the latitude and longitude calculation errors are reduced. The error of determining the latitude and longitude is shown, respectively, in Figures 12 and 13, when the GPS is disconnected.

During centrifugal acceleration mode, the proposed method adopts the appropriate model to estimate the Euler angles. This reduces the measurement model's error and facilitates calculation of the latitude and longitude with less error. Figure 14 also demonstrates the 2D diagram and the path on the geographical coordinates.

As illustrated in Figure 14, the GPS is disconnected four times, denoted by numbers 1 to 4. Figure 15, which compares the INS error with three methods, EKF, AEKF and proposed SAEKF, confirms that the error of positioning with the proposed method is less than with the other two methods during the GPS outages.

Figure 15. INS error with three methods EKF, AEKF and proposed SAEKF, (a) latitude, (b) longitude, (c) total position error

By applying the three aforementioned methods when GPS is disconnected, the generated errors are those listed in Table 6. From a comparison viewpoint, the errors are considerably reduced by using the proposed method, particularly when centrifugal acceleration occurs. The reported maximum and RMS errors show that the proposed method is more accurate in position localisation.

Table 6. Comparison of maximum and RMS error in three methods when GPS is disconnected

5. Conclusions

In this paper, a new filtering algorithm is proposed for an inaccurate integrated MEMS-based INS/GPS system. The presented SAEKF prevents increased errors of Kalman filtering estimation during the centrifugal acceleration and improves the utilisation rate of the observation data in the filter. The bias stability of MEMS gyroscopes is considerably greater than with other technologies, affecting the accuracy of Euler angles calculation. In addition, the proposed method, which adopts a corrected measurement switched model, can increase the estimation accuracy without any additional sensors. To investigate the performance of the proposed method with other common methods, flight test data is applied. A comparison study with the conventional EKF and AEKF shows the validity and superiority of the proposed SAEKF algorithm by experimental test, particularly in the roll estimation during the centrifugal acceleration. The precise angle estimation also affects the position estimation and presents less error in the position estimation during GPS outages.