A Vehicle-Model-Aided Navigation Reconstruction Method for a Multicopter during a GPS Outage

Abstract

The integrated navigation of inertial navigation systems (INS) and the Global Positioning System (GPS) is essential for small unmanned aerial vehicles (UAVs) such as multicopters, providing steady and accurate position, velocity, and attitude information. Nevertheless, decreasing navigation accuracy is a serious threat to flight safety due to the long-term drift error of INS in the absence of GPS measurements. To bridge the GPS outage for multicopters, this paper proposes a novel navigation reconstruction method for small multicopters, which combines the vehicle dynamic model and micro-electro-mechanical system (MEMS) sensors. Firstly, an induced drag model is introduced into the dynamic model of the vehicle, and an efficient online parameter identification method is designed to estimate the model parameters quickly. Secondly, the body velocity can be calculated from the vehicle model and accelerometer measurement. In addition, the nongravitational acceleration estimated from body velocity and radar height are utilized to yield a more accurate attitude estimate. Fusing the information of the attitude, body velocity, magnetic heading, and radar height, a navigation system based on an error-state Kalman filter is reconstructed. Then, an adaptive measurement covariance algorithm based on a fuzzy logic system is designed to reduce the weight due to the disturbed acceleration. Finally, the hardware-in-loop experiment is carried out to demonstrate the effectiveness of the proposed method. Simulation results show that the proposed navigation reconstruction algorithm aided by the vehicle model can significantly improve navigation accuracy during a GPS outage.

1. Introduction

Unmanned aerial vehicles (UAVs) are increasingly used in civil and military applications, such as structural inspection [1], law enforcement [2], transportation [3], and monitoring [4]. Reliable and accurate estimation of attitude, velocity, and position are important feedback signals for UAVs to perform an autonomous flight and ensure flight safety. For small UAVs, inertial measurement units (IMUs) based on the micro-electro-mechanical system (MEMS) sensors, which consist of three-axis accelerometers, three-axis gyroscopes, and three-axis magnetometers, are generally adopted in navigation system to provide high-frequency attitude, angular velocity, and acceleration information [5,6]. The Global Positioning System (GPS) or some external reference sensors need to be utilized to correct long-term drift of the IMU-based inertial navigation system (INS) [7,8]. However, the inherent shortcoming of GPS is that it may be invalid or even lost due to some challenging environments, such as highway tunnels and environments with harsh electromagnetic interference.

Researchers have proposed several methods to solve the state estimation problem without a GPS signal. An artificial neural network (NN)-based technology has recently been used to approximately estimate the growing INS errors during a GPS outage [9,10], while [11] presents a Gaussian process regression approach to address the state estimation of GPS outages. The autoregressive integrated moving average model is applied to predict the INS-only solution [12]. Some similar works can also be found in [13,14]. However, a drawback of these approaches is that a large amount of time and data is required for offline learning, and lot of memory capacity is required for storing the learning parameters. Thus, it may not be a practical solution for small UAVs.

As alternatives to these methods based on artificial NNs, approaches based on hardware redundancy (also called sensor redundancy) have been researched in recent years. Laser and stereo camera [15] have been used to map and navigate in unknown environments. In addition, Qi et al. [16] and Zhao et al. [17] have put forward a vision-aided inertial navigation to estimate position, velocity, and attitude for UAVs in GPS-denied environments. However, such kinds of external sensor configurations increase the cost and complexity of the navigation system, which is not practical for small UAVs limited by the requirements of size, weight, and cost. Contrasting with hardware redundancy ideas, the inclusion of a vehicle model into the navigation system has no need for additional external sensors. The possibility of vehicle-model-aided INS is first introduced by [18]. An attitude estimator combining aircraft kinematics and GPS is proposed for fixed-wing UAVs in [19,20]. However, these algorithms mainly concentrate on fixed-wing UAVs and do not take position and velocity estimation into consideration. Some works [21,22,23,24] prove that a more detailed vehicle model of a quadrotor could improve the estimation accuracy of attitude. Thus, it is necessary to design a more effective navigation algorithm for small multicopters to perform safe flight or emergency landing after GPS failure.

In this paper, a practical navigation reconstruction algorithm aided by vehicle model is proposed for small multicopters to bridge the GPS outage with a minimum number of sensors, such as an IMU and a radar. From the analysis of induced drag caused by the blade flapping, the body velocity can be calculated by the accelerometer measurement. Thus, we develop a more detailed vehicle model including propeller model and blade flapping, and these model parameters are able to be identified online as long as the GPS signal is available. During a GPS outage, nongravitational acceleration estimated from the vehicle model is used to compensate raw accelerometer measurements to obtain an accurate attitude estimation. Depending on the attitude, body velocity, magnetic heading, and radar height measurement, the navigation system based on an error-state Kalman Filter (ESKF) is reconstructed in combination with a fuzzy logic adaptive measurement covariance algorithm to provide a reliable navigation result during a GPS outage. Compared with the conventional observers, such as complementary filters [7,25] or traditional extended Kalman Filters [26], the ESKF [27] usually estimates the error states around the equilibrium point to guarantee the validity of linearization, which can often achieve a better performance. Thus, the ESKF is used for the state estimate during a GPS outage. Finally, the performance of the proposed method is verified by the real-time hardware-in-loop simulation platform.

The main contributions of this paper are as follows:

(1)

A more detailed vehicle model including propeller model and blade flapping is developed. An efficient online parameter identification method is designed to estimate the model parameters quickly.

(2)

The body velocity is derived from the established vehicle model and accelerometer output, and then the nongravitational acceleration is estimated and extracted from the raw accelerometer outputs to yield an accurate attitude estimation.

(3)

Combining the attitude, body velocity, magnetic heading, and radar height measurements, the navigation system based on ESKF is reconstructed during a GPS outage. Besides this, an adaptive measurement covariance algorithm based on a fuzzy logic system is designed to reduce the impact of disturbed acceleration.

This paper’s outline is as follows: In Section 2, the detailed vehicle model including propeller model and blade flapping is presented, and the online parameter identification method is introduced. Section 3 formulates the measurement models, and the navigation system based on ESKF is reconstructed in combination with the fuzzy logic adaptive measurement covariance. The experimental setup and results are presented in Section 4. Conclusions and future work are discussed in Section 5.

2. Modeling and Parameter Identification

Figure 1 illustrates the flow diagram of the navigation reconstruction method aided by vehicle model during a GPS outage that we designed. The GPS/INS integrated navigation results are used to estimate the vehicle parameters. If GPS signal breaks down, these estimated parameters are utilized to calculate the body velocity and attitude angle. Then, fusing the attitude, body velocity, magnetic heading, and radar height measurement, the navigation system is reconstructed to provide navigation information for multicopters to perform safe flight or emergency landing after GPS failure.

This section introduces the detailed vehicle model including propeller model and blade flapping, which is helpful for the subsequent navigation reconstruction. Besides this, an efficient and lightweight parameter identification method based on a Kalman filter (KF) is developed, and we further discuss the condition of observability for the proposed parameter identification method.

2.1. Propeller Modeling

Fixed-pitch propellers are commonly used for multicopters. As shown in Figure 2, the blade element theory of [28,29] are utilized to model the thrust force $T$ (unit: N) and torque $M$ (unit: N·m) of the propeller.

In Figure 2, the blade section $A{A}^{*}$ in $0.5r$ is taken as an example to analyze the force of the propeller. $V_I$ is the incoming flow speed and $U_a=\sqrt{{(\omega r_p)}^2+V_I^2}$ is the flow velocity of section $A{A}^{*}$. ${\beta }_p$ is the propeller blade angle, which is determined by the shape of the blade. Thus, the real attack angle of the propeller is

$${\alpha }_p={\beta }_p-{\tan }^{-1}(\frac{V_I}{\omega r_p})$$

(1)

According to the blade element theory, the blade section $A{A}^{*}$ can be taken as a small wing, and the lift and drag can be calculated as

$$\begin{array}{l}dL=\frac{1}{2}\rho U_a^2{\alpha }_p{L}_{p}ds\\ dD=\frac{1}{2}\rho U_a^2{\alpha }_p{D}_{p}ds\end{array}$$

(2)

where $\rho$ is the air density, $L_p$ and $D_p$ are the lift and drag coefficient, and $ds$ is the small blade area of $A{A}^{*}$. Then, the real pull force of this blade section $A{A}^{*}$ can be written as

$$d{T}_z=dL\cos {\varphi }_p-dD\sin {\varphi }_p$$

(3)

where ${\varphi }_p={\tan }^{-1}(\frac{V_I}{\omega r_p})$ is the flow angle. Combining the above equation, the pull force $d{T}_z$ can be rewritten as

$$d{T}_z=\frac{1}{2}\rho L_{p}ds\sqrt{{(\omega r_p)}^2+V_I^2}\omega r_p{\alpha }_p-\frac{1}{2}\rho D_{p}ds\sqrt{{(\omega r_p)}^2+V_I^2}{V}_I{\alpha }_p$$

(4)

Considering that the small multicopter usually flies at low altitude and does not make high maneuvers, the incoming flow $V_I$ is small enough to be ignored compared to the high speed of blade. By defining $C_L=\frac{1}{2}\rho L_{p}ds{\beta }_p{r}_p^2$, we can get

$$d{T}_z=\frac{1}{2}\rho L_{p}ds{\beta }_p{(\omega r_p)}^2=C_L{\omega }^2$$

(5)

The whole thrust of the propeller can be described as

$$T=2{\displaystyle \underset{0}{\overset{r}{\int }}d{T}_z}={\displaystyle \underset{0}{\overset{r}{\int }}\rho L_{p}ds{\beta }_p{(\omega r_p)}^2}d{r}_p$$

(6)

As shown in Equation (6), the thrust of the propeller is proportional to the square of propeller speed. For simplifying the calculation, the thrust force $T$ produced by the propeller is expressed as

$$T=k_T{\omega }^2$$

(7)

where $k_T$ is the propeller thrust coefficient related to the shape of a propeller [30]. Since the blade shapes for a certain propeller are the same, $k_T$ can be treated as a constant value.

2.2. Blade Flapping

The propeller of a multicopter is not actually fully rigid. As seen in Figure 3, the velocity of a blade with contrary wind is higher than that of a blade with following wind due to the flying velocity of the quadrotor. The higher velocity will produce a larger thrust force, which generates an upward flapping velocity. Thus, it generates an imbalanced force between the two blades of the same propeller and causes the two blades to flap up and down (called blade flapping) when the multicopter moves [21]. The blade flapping causes the propeller disk to be tilted, which produces an induced drag opposite to the motion of the quadrotor. According to [22], the induced drag can be estimated by the following:

$$\overrightarrow{D}=-\eta {\displaystyle {\sum }_{i=1}^4{\omega }_i}\overrightarrow{V}$$

(8)

where $\eta$ is a positive constant, ${\omega }_i$ is the rotational velocity of the ith motor, and $\overrightarrow{V}={\left[u,v,w\right]}^T$ is the inertial velocity expressed in body frame. For a more intuitive understanding of the induced drag, the force schematic of a multicopter after moving can be seen in Figure 4. As we described before, the imbalanced torque generated by the translational velocity of the multicopter causes the propeller disk to be tilted, which will also cause the thrust force of propeller to be tilted in a direction. Considering that the imbalanced torque depends on the magnitude of translational velocity, the component of the thrust force (called induce drag) is also a function of horizontal velocity. Thus, we use a linear coefficient $\eta$ and the summation of propeller rotational rates to estimate this drag according to [22,23,24].

2.3. Vehicle Modeling

In the mathematical model of the quadrotor, two corresponding coordinate frames (see Figure 5) are considered. $O_b-X_b{Y}_b{Z}_b$ denotes the body frame, where $O_b$ is at the center of gravity, $X_b$ pointing front, $Y_b$ pointing left, and $Z_b$ pointing up. $O_n-X_n{Y}_n{Z}_n$ represents the navigation frame which coincides with the geographic frame (east, north, east, upwards). The translational motion equation that will be utilized in navigation reconstruction later is introduced here as

$$m\left(\dot{\overrightarrow{V}}+\overrightarrow{\mathsf{\Omega }}\times \overrightarrow{V}\right)=C_n^{b}mg{\overrightarrow{e}}_3-k_T{\omega }_{ss}{\overrightarrow{b}}_3-\overrightarrow{D}$$

(9)

where $m$ is the mass of the multicopter, $\overrightarrow{\mathsf{\Omega }}={\left[p,q,r\right]}^T$ are the body rotational velocity expressed in body frame, $D=diag\left({\eta }_1{\displaystyle {\sum }_{i=1}^4{\omega }_i·u,}{\eta }_2{\displaystyle {\sum }_{i=1}^4{\omega }_i·v,0}\right)$ is the induced drag, and ${\omega }_{ss}={\displaystyle {\sum }_{i=1}^4{\omega }_i^2}$ is the sum of the squared motor speeds. $g$ is the magnitude of the gravity vector. $C_n^b$ is the rotation matrix from the navigation frame to the body frame, which is defined as

$$\begin{array}{ll}{C}_n^b& =R_y(\varphi )R_x(\theta )R_z(\psi )\\ & =\left[\begin{array}{ccc}\cos \varphi & 0& -\sin \varphi \\ 0& 1& 0\\ \sin \varphi & 0& \cos \varphi \end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \theta & \sin \theta \\ 0& -\sin \theta & \cos \theta \end{array}\right]\left[\begin{array}{ccc}\cos \psi & \sin \psi & 0\\ -\sin \psi & \cos \psi & 0\\ 0& 0& 1\end{array}\right]\end{array}$$

(10)

where $\left[\varphi ,\theta ,\psi \right]$ is the roll, pitch, and yaw angle of the vehicle. Equation (5) can then be rewritten as

$$\left[\begin{array}{c}\dot{u}\\ \dot{v}\\ \dot{w}\end{array}\right]=\left[\begin{array}{c}-g\sin \theta +(vr-wq)-\frac{d_1}{m}u\\ g\sin \varphi \cos \theta +(wp-ur)-\frac{d_2}{m}v\\ g\cos \varphi \cos \theta +(uq-vp)-\frac{k_T{\omega }_{ss}}{m}\end{array}\right]$$

(11)

where $\left[\dot{u},\dot{v},\dot{w}\right]$ are components of motion acceleration along the body-axis, and $d_1={\eta }_1{\displaystyle {\sum }_{i=1}^4{\omega }_i}$ and $d_2={\eta }_2{\displaystyle {\sum }_{i=1}^4{\omega }_i}$ are induced drag coefficients of axes $Y_b$ and $X_b$, respectively. We assume $d_1$ and $d_2$ are constants due to a fact that the summation of propeller rotational rates for multicopters are fairly stable during smooth flight. Another reason is that most small multicopters lack complex rotational speed measurement sensors, thus we can save these sensors by this simplification.

2.4. Parameter Identification and Observability Analysis

Before using the vehicle model to aid navigation reconstruction, some vehicle model parameters $\left[d_1,d_2,k_T\right]$ need to be acquired. Considering that these parameters are affected by the characteristics of the propulsion system and the external environment such as battery voltage, air density, and so on, we design an efficient and lightweight estimator based on a KF to acquire these parameters online. It should be noted that nonlinear Coriolis terms in Equation (11) are small enough for a multicopter to be neglected. Therefore, Equation (11) can be rewritten as

$$\left[\begin{array}{c}{a}_{mx}\\ a_{my}\\ a_{mz}\end{array}\right]=\left[\begin{array}{c}\dot{u}+g\sin \theta \\ \dot{v}-g\sin \varphi \cos \theta \\ \dot{w}-g\cos \varphi \cos \theta \end{array}\right]\approx \left[\begin{array}{c}-\frac{d_1}{m}u\\ -\frac{d_2}{m}v\\ -\frac{k_T{\omega }_{ss}}{m}\end{array}\right]$$

(12)

where ${\left[a_{mx},a_{my},a_{mz}\right]}^T$ are the accelerometer outputs in the body frame. The motor speeds ${\omega }_i,i=1,2,3,4$ can be measured by rotating speed sensors, which are mounted on the airframe. When GPS signal is available, $\overrightarrow{V}={\left[u,v,w\right]}^T$ can be calculated from the integrated navigation of INS and GPS. Thus, we have noticed that the model parameters are directly observable from the accelerometer outputs.

Defining the state vector as $x_d={\left[d_1,d_2,k_T\right]}^T$, we get the following state propagation equation and measurement equations:

$$\begin{array}{l}{\dot{x}}_d=A_d{x}_d+B_d{u}_d\\ y_{acc}=C_{acc}{x}_d\end{array}$$

(13)

where outputs $y_{acc}={\left[a_{mx},a_{my},a_{mz}\right]}^T$ are the accelerometer outputs, with inputs $u_d={\left[0,0,0\right]}^T$. The Jacobian matrix $A_d=0^{3\times 3}\in {ℝ}^{3\times 3}$, $B_d=0^{3\times 1}\in {ℝ}^{3\times 1}$, $C_{acc}=diag\left(-\frac{u}{m},-\frac{v}{m},-\frac{{\omega }_{ss}}{m}\right)\in {ℝ}^{3\times 3}$, combined with system model (13) and a common KF algorithm, can be used to estimate the vehicle model parameters.

Observability is a necessary condition for KF convergence. Thus, we analyze the condition of observability for the model parameters $\left[d_1,d_2,k_T\right]$ based on the theory of linear observability. According to the theory of linear observability, the sufficient and necessary condition of state observability for system (13) is that

$$rank\left(A_d,C_{acc}\right)=rank\left[\begin{array}{c}C\\ C_{acc}{A}_d\\ ⋮\\ C_{acc}{A}_d^{n-1}\end{array}\right]=n$$

(14)

Substituting Equation (13) into (14), we can obtain the following:

$$rank\left(A_d,C_{acc}\right)=rank\left[\begin{array}{c}C\\ C_{acc}{A}_d\\ ⋮\\ C_{acc}{A}_d^{n-1}\end{array}\right]=\left[\begin{array}{ccc}-\frac{u}{m}& 0& 0\\ 0& -\frac{v}{m}& 0\\ 0& 0& -\frac{{\omega }_{ss}}{m}\end{array}\right]$$

(15)

It is obvious that ${\omega }_{ss}={\displaystyle {\sum }_{i=1}^4{\omega }_i^2}$ is usually a positive value in a normal flight. If $u=v=0$, then $rank\left(A_d,C_{acc}\right)=1$, which means that system (13) is unobservable when the multicopter is in hover flight. Therefore, the parameters $\left[d_1,d_2,k_T\right]$ are observable when the velocity of the multicopter is unequal to zero.

3. Vehicle-Model-Aided Navigation Reconstruction Method

In this section, the IMU-based navigation system is reconstructed with the aid of the aforementioned vehicle model during a GPS outage. The ESKF is utilized to estimate the INS drift errors, which are subsequently used to compensate the INS results.

3.1. State Model

The dynamic navigation equations of multicopters for attitude, velocity, and position are written as

$$\{\begin{array}{l}{\dot{q}}_n^b=0.5\left[\begin{array}{cc}0& -{\overrightarrow{\omega }}^T\\ \overrightarrow{\omega }& -{\left[\overrightarrow{\omega }\right]}^{\times }\end{array}\right]·q_n^b\\ {\dot{V}}^n=f^n-\left(2{\omega }_{ie}^n+{\omega }_{en}^n\right)\times V^n+g^n\\ \dot{L}=\frac{V_N}{R_{yt}+H}\\ \dot{\lambda }=\frac{V_E\sec L}{R_{xt}+H}\\ \dot{H}=V_U\end{array}$$

(16)

where $q_n^b$ denotes the attitude quaternion, $\omega$ denotes the angular rate of UAV, $f^n$ is the output of accelerometer expressed in the navigation frame, and $V^n={\left[V_E,V_N,V_U\right]}^T$ are flying velocity expressed in the navigation frame. $R_{xt}$ and $R_{yt}$ are the radiuses of the prime plane and meridian plane, respectively. $L$, $\lambda$, and $H$ are the latitude, longitude, and altitude, respectively. ${\omega }_{ie}^n$ denotes the angular rate of the earth expressed in the navigation frame, and ${\omega }_{en}^n$ is the angular rate of the navigation frame with respect to the Earth-Centered-Earth-Fixed frame.

$${\omega }_{ie}^n={\left[{\omega }_{ie}\cos L,0,{\omega }_{ie}\sin L\right]}^T$$

(17)

$${\omega }_{en}^n={\left[-\frac{V_N}{R_{yt}+H},\frac{V_E}{R_{xt}+H},\frac{V_E}{R_{xt}+H}\tan L\right]}^T$$

(18)

By linearizing equation (16), the linear error propagation model is obtained as

$$\{\begin{array}{l}\delta \dot{\phi }=\delta \phi \times {\omega }_{in}^n+\delta {\omega }_{in}^n+C_b^n{\xi }_g^b\\ \delta {\dot{V}}^n=f^n\times \delta \phi +\delta V^n\times \left(2\delta {\omega }_{ie}^n+\delta {\omega }_{en}^n\right)-\left(2{\omega }_{ie}^n+{\omega }_{en}^n\right)\times \delta V^n+C_b^n{\xi }_a^b\\ \delta \dot{L}=\frac{1}{R_{yt}+H}\delta V_N-\frac{V_N}{{\left(R_{yt}+H\right)}^2}\delta H\\ \delta \dot{\lambda }=\frac{\sec L}{R_{xt}+H}\delta V_E+\frac{V_E\sec L\tan L}{R_{xt}+H}\delta L-\frac{V_E\sec L}{{\left(R_{xt}+H\right)}^2}\delta H\\ \delta \dot{H}=-\delta V_U\end{array}$$

(19)

where $\delta \phi ={\left[\delta \varphi ,\delta \theta ,\delta \psi \right]}^T$ is the vector of attitude angle errors, ${\xi }_g^b$ and ${\xi }_a^b$ are white noise of the gyro and accelerometer respectively, and ${\omega }_{in}^n={\omega }_{ie}^n+{\omega }_{en}^n$ is the angular rate of the navigation frame with respect to the inertial frame. $\delta {\omega }_{in}^n$ and $\delta {\omega }_{en}^n$ are the differentiations of the ${\omega }_{in}^n$ and ${\omega }_{en}^n$ respectively, which are obtained as

$$\delta {\omega }_{ie}^n={\left[-{\omega }_{ie}\sin L·\delta L,0,{\omega }_{ie}\cos L·\delta L\right]}^T$$

(20)

$$\delta {\omega }_{en}^n=\left[\begin{array}{c}-\frac{1}{R_{yt}+H}\delta V_N+\frac{V_N}{{\left(R_{yt}+H\right)}^2}\delta H\\ \frac{1}{R_{xt}+H}\delta V_E-\frac{V_E}{{\left(R_{xt}+H\right)}^2}\delta H\\ \frac{\tan L}{R_{xt}+H}\delta V_E+\frac{V_E{\sec }^{2}L}{R_{xt}+H}\delta L-\frac{V_E\tan L}{{\left(R_{xt}+H\right)}^2}\delta H\end{array}\right]$$

(21)

Thus, the nine-dimensional state variables are chosen as three attitude angle, velocity, and position errors $X_s={\left[\delta \varphi ,\delta \theta ,\delta \psi ,\delta V_N,\delta V_E,\delta V_U,\delta L,\delta \lambda ,\delta H\right]}^T$. According to Equation (19), the state model of the navigation reconstruction method can be expressed as

$${\dot{X}}_s\left(t\right)=F_s\left(t\right)X_s\left(t\right)+G_s\left(t\right)W_s\left(t\right)$$

(22)

$$W_s\left(t\right)={\left[{\xi }_g^b,{\xi }_a^b\right]}^T$$

(23)

where $F_s(t)$ is the linear transfer matrix of the system state, and $W_s(t)$ is the Gaussian white noise.

3.2. Measurement Model

During a GPS outage, the navigation information from INS alone cannot guarantee the estimation performance. To address this problem, a new method to observe the attitude angles and velocities based on the established vehicle model is proposed.

3.2.1. Velocity Measurement Model

Once the model parameters $\left[d_1,d_2,k_T\right]$ are obtained through the parameter identification method discussed above, the aerodynamic model velocity $\left[V_{mx}^b,V_{my}^b\right]$ can be extracted from the accelerometers’ outputs. It should be noted that the bias errors of the accelerometer have been properly calibrated before the multicopter takes off.

$$\{\begin{array}{l}{V}_{mx}^b=-m{a}_{mx}^b/d_1\\ V_{my}^b=-m{a}_{my}^b/d_2\end{array}$$

(24)

where $a_{mx}^b$ and $a_{my}^b$ denote the accelerometers’ outputs in the body-fixed frame.

3.2.2. Attitude Measurement Model

Roll and pitch angle can be observed from the gravity vector $\left[g_x^b,g_y^b,g_z^b\right]$ in the body frame through the following equations:

$${\varphi }_g=\{\begin{array}{ll}\pi +\arctan \left(g_y^b/g_z^b\right)& \mathrm{if}{g}_y^b>0,g_z^b<0\\ \pi /2& \mathrm{if}{g}_y^b>0,g_z^b=0\\ \arctan \left(g_y^b/g_z^b\right)& \mathrm{if}{g}_z^b>0\\ -\pi /2& \mathrm{if}{g}_y^b<0,g_z^b=0\\ -\pi +\arctan \left(g_y^b/g_z^b\right)& \mathrm{if}{g}_y^b<0,g_z^b<0\end{array}$$

(25)

$${\theta }_g=-\arcsin \left(g_x^b/\sqrt{{\left(g_x^b\right)}^2+{\left(g_y^b\right)}^2+{\left(g_z^b\right)}^2}\right)$$

(26)

However, the accelerometer measures specific force, meaning the difference between motion acceleration and gravitational acceleration. The accelerometers’ outputs ${\overrightarrow{f}}^b$ are described as

$${\overrightarrow{f}}^b={\overrightarrow{\dot{V}}}^b+\left(2{\overrightarrow{\omega }}_{ie}^b+{\overrightarrow{\omega }}_{en}^b\right)\times {\overrightarrow{V}}^b-{\overrightarrow{g}}^b$$

(27)

The cross-term $(2{\overrightarrow{\omega }}_{ie}^b+{\overrightarrow{\omega }}_{en}^b)\times {\overrightarrow{V}}_n^b$ is relatively small for a multicopter, thus it can be ignored. Despite this, the effect of translational acceleration ${\overrightarrow{\dot{V}}}^b$ on measured acceleration cannot be neglected in the situation of maneuvering. In order to obtain the individual gravitational acceleration, the translational acceleration ${\overrightarrow{\dot{V}}}^b$ should be subtracted from the raw accelerometer outputs ${\overrightarrow{f}}^b$. A constant acceleration model is applied to estimate the ${\overrightarrow{\dot{V}}}^b$ due to the high sampling rate of IMU. The system and measurement models are expressed as

$$\left[\begin{array}{c}{V}_{x,k}^b\\ {\dot{V}}_{x,k}^b\\ V_{y,k}^b\\ {\dot{V}}_{y,k}^b\\ H_k\\ V_{z,k}^b\\ {\dot{V}}_{z,k}^b\end{array}\right]=\left[\begin{array}{c}{V}_{x,k-1}^b+{\dot{V}}_{x,k-1}^b·\mathsf{\Delta }t\\ {\dot{V}}_{x,k-1}^b\\ V_{y,k-1}^b+{\dot{V}}_{y,k-1}^b·\mathsf{\Delta }t\\ {\dot{V}}_{y,k-1}^b\\ H_{k-1}+V_{z,k-1}^b·\mathsf{\Delta }t+0.5·{\dot{V}}_{z,k}^b·\mathsf{\Delta }{t}^2\\ V_{z,k-1}^b+{\dot{V}}_{z,k-1}^b·\mathsf{\Delta }t\\ {\dot{V}}_{z,k-1}^b\end{array}\right]$$

(28)

$$\left[\begin{array}{c}{V}_{mx,k}^b\\ V_{my,k}^b\\ H_{radar}\end{array}\right]=\left[\begin{array}{c}{V}_{x,k}^b\\ V_{y,k}^b\\ H_k\end{array}\right]$$

(29)

where $V_{mx,k}^b$ and $V_{my,k}^b$ are body velocities observed from the above vehicle model, $H_{radar}$ denotes the radar height, and $\mathsf{\Delta }t$ denotes the sampling time. Once we obtain the translational accelerations ${\left[V_x^b,V_y^b,V_z^b\right]}^T$, roll and pitch angles can be estimated from Equations (25) and (26).

3.2.3. Heading Measurement Model

In addition to measuring the triaxial angular rates and specific force, common IMU also provides the magnetic field strength in body axis as $\left[m_x^b,m_y^b,m_z^b\right]$. Since the magnetic measurement is very susceptible to electronic interference, this information should only be used for correcting the yaw attitude. The horizontal component of the magnetic field can be derived by using roll and pitch angles:

$$\begin{array}{l}{X}_H=m_x^b\sin \varphi \sin \theta +m_y^b\cos \theta -m_z^b\cos \varphi \sin \theta \\ Y_H=m_x^b\cos \varphi +m_z^b\sin \varphi \end{array}$$

(30)

The magnetic heading is given by the following:

$${\psi }_g=\{\begin{array}{ll}\arctan \left(-Y_H/X_H\right)& \mathrm{if}{X}_H>0,Y_H\le 0\\ \pi /2& \mathrm{if}{X}_H=0,Y_H<0\\ \pi +\arctan \left(-Y_H/X_H\right)& \mathrm{if}{X}_H<0\\ 3\pi /2& \mathrm{if}{X}_H=0,Y_H>0\\ 2\pi +\arctan \left(-Y_H/X_H\right)& \mathrm{if}{X}_H>0,Y_H>0\end{array}$$

(31)

3.3. Discrete Forms of Navigation Reconstruction Filter

According to the measurement model, the observation equations can be reconstructed by the attitude, heading, velocity, and radar height observed from the vehicle model and sensors during a GPS outage. The measurement vector is defined as the difference between the INS solutions $\left(I\right)$ and the above observations:

$$Z_s\left(t\right)=\left[\begin{array}{c}{Z}_{\phi }\\ Z_v\\ Z_h\end{array}\right]=\left[\begin{array}{c}{\varphi }_g-{\varphi }_I\\ {\theta }_g-{\theta }_I\\ {\psi }_g-{\psi }_I\\ V_{mx}^n-V_{Ix}^n\\ V_{my}^n-V_{Iy}^n\\ H_{radar}-H_I\end{array}\right]$$

(32)

where $Z_{\phi }$ is the attitude error vector, $Z_v$ is velocity error vector, and $Z_h$ is altitude error vector. Measurement Equation (32) can be rewritten with regard to state $X_s$ as

$$Z_s=H_s{X}_s+V_s$$

(33)

where $V_s$ is the measurement covariance matrix, and $H_s$ is the measurement matrix given as

$$\left[\begin{array}{ccc}{I}_{3\times 3}& 0_{3\times 2}& 0_{3\times 4}\\ 0_{2\times 3}& I_{2\times 2}& 0_{2\times 4}\\ 0_{1\times 3}& 0_{1\times 2}& \left[000,1\right]\end{array}\right]$$

(34)

Combining Equations (19) and (32), the discrete model of navigation reconstruction filter aided by vehicle model is obtained as

$$\begin{array}{l}{X}_{s,k}={\mathsf{\Phi }}_{s,k|k-1}{X}_{s,k-1}+{\mathrm{\Gamma }}_{s,k-1}{W}_{s,k-1}\\ Z_{s,k}=H_{s,k}{X}_{s,k}+V_{s,k}\end{array}$$

(35)

where ${\mathsf{\Phi }}_{s,k|k-1}$ and ${\mathrm{\Gamma }}_{s,k-1}$ are discrete forms of $F_s\left(t\right)$ and $G_s\left(t\right)$, which are given as

$${\mathsf{\Phi }}_{s,k|k-1}=e^{F_s\mathsf{\Delta }t}={\displaystyle \sum _{n=0}^{\infty }{\left[F_s\left(t_k\right)\mathsf{\Delta }t\right]}^n/n!}$$

(36)

$${\mathrm{\Gamma }}_{s,k-1}=\left\{{\displaystyle \sum _{n=1}^{\infty }{\left[F\left(t_k\right)\mathsf{\Delta }t\right]}^{n-1}/n!}\right\}G\left(t_k\right)\mathsf{\Delta }t$$

(37)

where $\mathsf{\Delta }t$ is the discrete time interval. When GPS signal fails, the reconstructed observation vector $Z_s$ is calculated and used to correct the INS solution through the proposed ESKF.

3.4. Adaptive Fuzzy-Logic-Based Covariance Algorithm

The measurement covariance significantly affects the performance of the proposed ESKF. Ideally, the gravitational acceleration subtracted from the raw acceleration measurement should be matching with the direction and magnitude of the true gravity if the motion acceleration is accurately estimated using the above method. However, the accelerometers’ outputs are mixed with biases and noises, which will introduce error into the estimation of motion acceleration. Thus, an adaptive measurement covariance algorithm based on fuzzy logic is used to improve the performance of the estimation process. A fuzzy inference system is one approach imitating the human mind, which includes three main procedures, namely fuzzification, fuzzy reasoning, and defuzzification [31,32]. For the attitude measurement model, the following two variables are designed as the inputs to the fuzzy logic system to represent the effectiveness of observation of the attitude measurement:

$$\begin{array}{l}{e}_1=\arccos \left(\frac{g_m}{\Vert g_m\Vert }·\frac{g_0}{\Vert g_0\Vert }\right)\\ e_2=\Vert g_m\Vert -\Vert g_0\Vert \end{array}$$

(38)

where $e_1$ and $e_2$ denote the inconsistency of angle and magnitude between the estimated $g_m$ and true gravitational acceleration $g_0$.

The velocity measurement model mainly relies on the estimated accuracy of vehicle parameters and accelerometers’ outputs. These vehicle parameters have been proved by the convergence. However, the accelerometers’ outputs usually couple with the sparkle noise caused by the vibration of the airframe, which will introduce error into the estimation of velocity. Thus, the deviation of magnitude between the accelerometers’ outputs and $g_0$ is selected as the fuzzy input to represent the effectiveness of observation of velocity measurement:

$$e_3=\sqrt{a_{mx}^2+a_{my}^2+a_{mz}^2}-g_0$$

(39)

The fuzzy rules for attitude and the velocity measurement model are shown in

Table 1. Fuzzy rules for ${\lambda }_a$.

	${\mathit{e}}_2$	NH	Z	PH
${\mathit{e}}_1$
NM		VL	L	VL
NS		M	S	M
Z		L	VS	L
PS		M	S	M
PM		VL	L	VL

and

Table 2. Fuzzy rules for ${\lambda }_v$.

${\mathit{e}}_3$	${\mathit{\lambda }}_{\mathit{v}}$
NM	VL
NS	M
Z	L
PS	M
PM	VL

, respectively. The linguistic variable set of fuzzy inputs are described as follows: “Negative Medium (NM)”, “Negative Small (NS)”, “Zero (Z)”, “Positive Small (PS)”, “Positive Medium (NS)”, “Negative High (NH)”, “Zero (Z)”, and “Positive High (PH)”, which represent the degree of $e_1$ and $e_2$. The linguistic variable set related to fuzzy outputs consists of the variables “Very Small (VS)”, “Small (S)”, “Medium (M)”, “Large (L)”, and “Very Large (VL)”, which represent the degree of fuzzy output ${\lambda }_a$. Figure 6 and Figure 7 show the bell-shaped and Gaussian membership functions of the fuzzy system. The fuzzy outputs ${\lambda }_a$ and ${\lambda }_v$ are used to adjust the measurement covariance as follows:

$$\begin{array}{l}{R}_a={\lambda }_a·R_{a,0}\\ R_v={\lambda }_v·R_{v,0}\end{array}$$

(40)

where $R_{a,0}$ and $R_{v,0}$ denote the nominal measurement covariance of the attitude and velocity measurement model, respectively. By utilizing the proposed fuzzy inference rules, the measurement covariance is adaptively changed with the fuzzy input parameters.

4. Simulation Results

In order to evaluate the performance of the proposed navigation reconstruction method, the real-time hardware-in-loop simulation platform based on PIXHAWK [33] is presented in this section.

4.1. Hardware in Loop Simulation Platform

The experiment is implemented on our real-time hardware-in-loop simulation platform, as shown in Figure 8. The simulation platform includes five parts: an embedded master control computer, a PX4 flight controller, a quadrotor, an RC transmitter, and a ground station. The master computer is the core of the whole simulation platform, which contains a high-fidelity quadrotor dynamic model generated by the online toolbox of Quan and Dai [34], and the values are listed in

Table 3. Main quadrotor parameters used in experiment.

Parameter	Description	Value
$m$	Mass	1.4 kg
$I_{xx}$	Roll inertia	2.11 × 10−2 kg∙m2
$I_{yy}$	Pitch inertia	2.19 × 10−2 kg∙m2
$I_{zz}$	Yaw inertia	3.66 × 10−2 kg∙m2
$T_m$	Time constant of motor	0.02 s
$d$	Motor moment arm	0.225 m
$C_t$	Thrust coefficient of motor	1.105 × 10−5 N/(rad/s)2
$C_d$	Induced drag coefficient	7.3 × 10−2 N/(m/s)
$C_m$	Drag moment coefficient	1.779 × 10−7 N∙m/(rad/s)

. The detailed dynamic model which includes motor dynamic, propeller model, and flight environment, is developed by S-function and Simulink. The master computer accepts the four motor commands ${\sigma }_1,{\sigma }_2,{\sigma }_3,{\sigma }_4$ to produce the corresponding force, torque, and further simulated navigation information, included three-dimensional positions, velocities, attitudes, and angular rates $P_x,P_y,P_z,V_x,V_y,V_z,\varphi ,\theta ,\psi ,p,q,r$. The RC transmitter generates the desired attitude command ${\varphi }_{cmd},{\theta }_{cmd},{\psi }_{cmd}$ to the flight controller. The flight controller mainly accepts this command, runs the control algorithm, and generates the four motor commands ${\sigma }_1,{\sigma }_2,{\sigma }_3,{\sigma }_4$ to the aircraft.

In order to simulate the observability of the model parameters and assess the performance of the proposed navigation reconstruction algorithm, we planned the trajectory shown in Figure 9. The sensor data generated by a high-fidelity vehicle dynamic model on the master control computer comprises 180 s of continuous recording during the whole flight. From the observability analysis of Section 2.4, we can know that the parameters $\left[d_1,d_2,k_T\right]$ are observable as long as the velocities of the multicopter are unequal to zero. These vehicle parameters can be estimated from any flight phase except for the hovering state during the whole flight when GPS signal is valid.

During the first 20 s, the GPS signal is unobstructed, and the model parameter estimation filter is started. After 20 s, the GPS signal is assumed to be completely unavailable, which means that the absolute position and velocity measurements from GPS are invalid. Then the IMU-based navigation system is reconstructed with the aid of the vehicle model. Thus, the flight data is divided into two main flight phases, as follows: vehicle model parameter identification phase from 0 s to 20 s, and vehicle-model-aided navigation reconstruction phase from 20 s to 180 s.

4.2. Vehicle Model Parameter Identification

Figure 10, Figure 11 and Figure 12 depict the results of the parameter estimation and the truth parameter. The parameter estimation begins from the start of the flight, and we can see that the proposed parameter identification method is able to make sure the estimated parameters converge to the truth values within less than 2 s. The final estimation accuracy of all model parameters is within 1.5%. After the model parameters have been estimated properly, these coefficients can be used to reconstruct the navigation system during a GPS outage.

4.3. The Reconstructed Navigation System

To verify the effectiveness of the proposed navigation reconstruction method during a GPS outage, the INS-only solution (prediction mode of the INS/GPS filter) is implemented for a performance comparison. Besides this, the results of the integrated navigation of INS and GPS are regarded as the true value.

Figure 13 shows the estimated value of roll, pitch, and yaw angles during the flight. From the beginning of 20 s, the GPS signal is obstructed artificially. The reconstructed navigation system based on ESKF starts at this time. We can see that the attitude estimation results of INS-only diverge quickly during a GPS outage. At 180 s, the error of the roll and pitch angle are about 9–10°, and the error of the yaw angle is about 5°, while the results of the proposed method almost track the true value (the GPS/INS solution). The root mean square errors (RMSEs) for the attitude are summarized in

Table 4. Comparison of attitude estimation performance.

RMSE	Proposed Method	INS-Only
Roll	0.225°	5.6074°
Pitch	0.249°	4.8616°
Yaw	0.109°	2.6653°

. The proposed method shows a remarkable improvement compared to the INS-only solution.

Figure 14 depicts the estimation results of velocity during the flight. The period from 20 s to 180 s is enlarged. Obviously, the velocity estimation from the reconstructed navigation system converges much better than the INS-only solution. The accuracy of east and north velocities are about 0.219 m/s and 0.15 m/s at 180 s, respectively. Since the velocity measurements are calculated from vehicle model and accelerometer outputs, the estimation errors of parameters and the time-varying biases of the accelerometer will result in an error of the velocity estimate. However, the velocity of the reconstructed navigation system still meets the requirements of flight control.

Figure 15 shows the position results of the vehicle provided by the reconstructed navigation system during the flight. The errors of east and north positions for the proposed method are about −14.6 m and 12.5 m at 180 s, respectively. Since we do not involve a horizontal position measurement in our filter, the east and north positions cannot converge to the true value due to weak observability. However, the proposed method still effectively mitigates the drift in the position estimation compared to the INS-only solution.

Figure 16 illustrates the calculated fuzzy inputs $e_1$ and $e_2$, and the fuzzy output ${\lambda }_a$, respectively. We can see that the fuzzy output ${\lambda }_a$, which acts as the scaling factor of the attitude measurement model, adaptively increases with the variation of angle and magnitude between the estimated $g_m$ and true gravitational acceleration $g_0$. The increase of scaling factor ${\lambda }_a$ will decrease the dependency of attitude measurement in the results of estimation, thus improving the performance of filter. The same analysis can also be used for the fuzzy input $e_3$ and fuzzy output ${\lambda }_v$ (see Figure 17).

5. Conclusions

This paper proposes a MEMS navigation reconstruction method aided by a vehicle model during a GPS outage. The proposed method makes an attempt to combine the vehicle model and MEMS sensors, which is distinguished from traditional methods, such as artificial NN and hardware redundancy. More specifically, a more detailed vehicle model is developed from the basis of analyzing blade flapping, and an efficient online parameter identification method is designed to estimate the model parameters quickly when GPS signal is available. The body velocity is derived from the established vehicle model and accelerometer output, and then the nongravitational acceleration is estimated and subtracted from the raw acceleration to yield an accurate attitude estimate. Combining the attitude, body velocity, magnetic heading and radar height measurements, the navigation system based on ESKF is reconstructed during a GPS outage. Besides this, we develop an adaptive measurement covariance algorithm based on a fuzzy system to reduce the impact of disturbed acceleration. Hardware-in-loop simulation results show that the proposed approach gains a remarkable estimation improvement compared to the INS-only solution, and has the potential to be a simple and robust substitution for the INS/GPS integrated navigation system for small multicopters during a GPS outage.