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

: 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, provid ‐ ing steady and accurate position, velocity, and attitude information. Nevertheless, decreasing nav ‐ igation 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 dy ‐ namic 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 nongravita ‐ tional acceleration estimated from body velocity and radar height are utilized to yield a more accu ‐ rate 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 pro ‐ posed navigation reconstruction algorithm aided by the vehicle model can significantly improve navigation accuracy during a GPS outage.


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-electromechanical 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] and [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, [16] and [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] and [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-inloop 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. 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.

Propeller Modeling
Fixed-pitch propellers are commonly used for multicopters. As shown in Figure 2, the blade element theory of [28] and [29] are utilized to model the thrust force T (unit: N) and torque M (unit: N•m) of the propeller.
The whole thrust of the propeller can be described as 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 where T k is the propeller thrust coefficient related to the shape of a propeller [30]. Since the blade shapes for a certain propeller are the same, T k can be treated as a constant value.

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: where  is a positive constant, i  is the rotational velocity of the ith motor, and 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  and the summation of propeller rotational rates to estimate this drag according to [22][23][24].

Vehicle Modeling
In the mathematical model of the quadrotor, two corresponding coordinate frames (see Figure 5 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

 
where m is the mass of the multicopter,   , , are the body rotational velocity expressed in body frame, is the induced drag, and is the sum of the squared motor speeds. g is the magnitude of the gravity vector. b n C is the rotation matrix from the navigation frame to the body frame, which is where   , ,    is the roll, pitch, and yaw angle of the vehicle. Equation (5) can then be rewritten as where   , , u v w    are components of motion acceleration along the body-axis, and are induced drag coefficients of axes b Y and b X , respectively. We assume 1 d and 2 d 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.

Parameter Identification and Observability Analysis
Before using the vehicle model to aid navigation reconstruction, some vehicle model parameters   1 2 , , T d d k 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 where , , The Jacobian matrix 3 3 , , Substituting Equation (13) into (14), we can obtain the following: It is obvious that is usually a positive value in a normal flight. If (13) is unobservable when the multicopter is in hover flight. Therefore, the parameters   1 2 , , T d d k are observable when the velocity of the multicopter is unequal to zero.

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.

State Model
The dynamic navigation equations of multicopters for attitude, velocity, and position are written as , , t a n By linearizing equation (16), the linear error propagation model is obtained as Thus, the nine-dimensional state variables are chosen as three attitude angle, velocity, and position errors According to Equation (19), the state model of the navigation reconstruction method can be expressed as where ( ) s F t is the linear transfer matrix of the system state, and ( ) s W t is the Gaussian white noise.

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.

Velocity Measurement Model
Once the model parameters   1 2 , , T d d k are obtained through the parameter identification method discussed above, the aerodynamic model velocity  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.
where b mx a and b my a denote the accelerometers' outputs in the body-fixed frame.

Attitude Measurement Model
Roll and pitch angle can be observed from the gravity vector However, the accelerometer measures specific force, meaning the difference between motion acceleration and gravitational acceleration. The accelerometers' outputs b f  are described as The cross-term   , , , ,  , roll and pitch angles can be estimated from Equations (25) and (26).

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 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: sin sin cos cos sin The magnetic heading is given by the following:

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   I and the above observations:   Combining Equations (19) and (32), the discrete model of navigation reconstruction filter aided by vehicle model is obtained as where t  is the discrete time interval. When GPS signal fails, the reconstructed observation vector s Z is calculated and used to correct the INS solution through the proposed ESKF.

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: where 1 e and 2 e denote the inconsistency of angle and magnitude between the estimated m g and true gravitational acceleration 0 g .
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 0 g is selected as the fuzzy input to represent the effectiveness of observation of velocity measurement: The fuzzy rules for attitude and the velocity measurement model are shown in Tables 1 and 2, 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 1 e and 2 e . 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 a  . Figures 6 and 7 show the bell-shaped and Gaussian membership functions of the fuzzy system. The fuzzy outputs a  and v  are used to adjust the measurement covariance as follows: where ,0 a R and ,0 v R 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.

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.

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. The detailed dynamic model which includes motor dynamic, propeller model, and flight environment, is developed by Sfunction and Simulink. The master computer accepts the four motor commands   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   1 2 , , T d d k 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.

Vehicle Model Parameter Identification
Figures 10-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.

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. 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   Figure 16 illustrates the calculated fuzzy inputs 1 e and 2 e , and the fuzzy output a  , respectively. We can see that the fuzzy output a  , which acts as the scaling factor of the attitude measurement model, adaptively increases with the variation of angle and magnitude between the estimated m g and true gravitational acceleration 0 g . The increase of scaling factor 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 3 e and fuzzy output v  (see Figure 17).

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.
Funding: This research received no external funding. Data Availability Statement: Data sharing is not applicable to this article.

Conflicts of Interest:
The authors declare no conflict of interest. The funders had no role in the design of the study; in the collection, analyses, or interpretation of data; in the writing of the manuscript, or in the decision to publish the results.