Interacting Multiple Model UAV Navigation Algorithm Based on a Robust Cubature Kalman Filter

To improve the precision and robustness of Unmanned Aerial Vehicle (UAV) integrated navigation systems, this paper presents an Interacting Multiple Model (IMM) navigation algorithm based on a Robust Cubature Kalman Filter (RCKF) with modified Zero Velocity Update (ZUPT) method assistance. This algorithm has a two-level fusion structure. At the bottom level, the Global Positioning System/Inertial Navigation System (GPS/INS) integrated navigation model and the Dynamic Zero Velocity Update/Inertial Navigation System (DZUPT/INS) integrated navigation model are established by modifying the Zero Velocity Update (ZUPT) method. Subsequently, the RCKF algorithm adopts a robust factor to weaken the influence of measurement outliers on the filter solution. At the top level, the estimation results of the GPS/INS integrated navigation model and the DZUPT/INS integrated navigation model are fused by the IMM algorithm. In addition to enhancing the robustness of filter estimation in the presence of measurement outliers, the proposed navigation algorithm also corrects navigation errors with ZUPT method assistance. Simulation and experimental analyses demonstrate the performance of the proposed navigation algorithm for UAVs.


I. INTRODUCTION
Inertial Navigation System (INS) is a navigation system that is able to calculate the position, velocity and attitude of a UAV with fairly good short-term navigation accuracy. In this UAV navigation system, INS consists of a low-cost inertial measurement unit (IMU) [1]. Due to the drifts of a low-cost IMU, navigation errors of INS increase over time, which leads to degradation of the navigation results [2]. GPS can provide velocity and position information, but the GPS refresh rate is low. Therefore, GPS/INS integration has been widely used in UAV navigation systems, which can make full use of these two independent systems, i.e., the high-precision trajectory information of GPS and the short-term stability of INS.
The Kalman Filter (KF) is widely used in INS/GPS integration. However, the essence of the integrated navigation system is nonlinear; therefore, the traditional KF, which can only The associate editor coordinating the review of this manuscript and approving it for publication was Chao Tan . be applied to linear systems, is no longer suitable for nonlinear systems. For nonlinear system filtering, Quinchia and Falco choose an extended Kalman filter (EKF) to linearize nonlinear systems [3], [4]. EKF linearizes the nonlinear system model via the first-order Taylor expansion. However, EKF is only effective for approximately linear nonlinear systems, and EKF can only achieve first-order accuracy. The Cubature Kalman Filter (CKF) is an improvement to EKF that generalizes the Kalman Filter for both linear and nonlinear systems. The CKF calculates a group of points with the same weight according to the spherical-radial cubature rule [5], [6]. The algorithm propagates these points directly through a nonlinear system equation to estimate the state [7]- [10]; the CKF does not need to linearize the nonlinear model and can achieve third-order accuracy. However, the performance of the CKF depends on the accuracy of measurement information and will seriously degrade when outliers are contained in the measurement. The Robust Kalman Filter (RKF) can be used to control outliers [11]- [13]. If the observation information of the navigation system contains outliers, the RKF can reduce the interference from the measurement outliers by reducing their weight [14]- [16]. In addition, in terms of a low-cost GPS receiver, the GPS velocity will have residual errors due to the influence of Doppler observation noise, satellite velocity errors, receiver errors and other factors in the GPS velocity measurement process [17]- [19]. Therefore, the navigation solution for an INS/GPS navigation system will be affected by the GPS velocity error.
To reduce interference due to measurement outliers on the UAV INS/GPS integrated navigation system, Hu applied the RKF to the system [20], which has better robustness to deal with measurement outliers than existing algorithms. However, the GPS velocity error still impacts the navigation results. To reduce the influence of the accumulation of GPS velocity errors on the results, Cai applied ZUPT to the vehicle navigation system, and this method can correct navigation errors when the vehicle velocity is 0 [21]. However, the velocity of a fixed wing UAV is not 0 in flight, so the ZUPT condition is not met. The method used to identify the ZUPT condition should also be considered, and the fault detection method was used to identify the ZUPT condition in previous research [22], [23]. However, these fault detection methods lead to expensive calculations and have a certain time delay. In underwater vehicles, Yao uses the IMM algorithm to solve the ZUPT condition identification problem. The IMM algorithm selects the optimal weight according to the innovation and innovation covariance and outputs the appropriate navigation parameters [24]- [28], avoiding time delay. However, because the flight environment of UAVs is different from that of underwater vehicles, it is necessary to modify the ZUPT method according to the characteristics of UAVs and to apply the ZUPT method to integrated navigation systems in UAVs.
To overcome the above problems, this paper proposes the IMM navigation algorithm, which is based on RCKF, and applies the ZUPT method to a UAV navigation system for the first time. In the DZUPT method, we only apply ZUPT to the Z-axis when a UAV is in a level flight state since the Z-axis velocity of the navigation frame is 0 during the period of level flight, which meets the ZUPT condition. Then, the GPS/INS integrated navigation model and the DZUPT/INS integrated navigation model are established. Subsequently, RCKF is designed using the CKF and the IGG III weight function, which could reduce the interference due to truncation errors and measurement outliers on the filtering results. Based on the RCKF algorithm, the IMM algorithm is used to fuse the DZUPT/INS model and the GPS/INS model. The IMM algorithm calculates the weight of the DZUPT/INS model and the GPS/INS model in the navigation system. The weight of the DZUPT/INS navigation model increases when the system meets the ZUPT condition, and the weight of the DZUPT/INS navigation model decreases when the system does not meet the ZUPT condition. Therefore, the proposed IMM navigation algorithm uses the ZUPT algorithm to correct the navigation parameters and realize smooth switching between the DZUPT/INS model and the GPS/INS model.
The main contributions of this paper are summarized below. First, we present a DZUPT method by modifying the traditional ZUPT, which is applied to the navigation system of a UAV for the first time to reduce navigation errors. Second, we develop the RCKF algorithm to enhance the robustness of the CKF to measurement outliers. Third, IMM algorithm is introduced to fuse the GPS/INS navigation model and the DZUPT/INS navigation model, which improves the precision and robustness of the navigation system at the same time.
The rest of this paper is arranged as follows. In Section 2, the model of DZUPT/INS and GPS/INS is given. Then, the RCKF algorithm is derived in Section 3, and in Section 4, the implementation process of the IMM navigation algorithm based on RCKF is introduced. Experiments and analyses are shown in Section 5, and conclusions are drawn in Section 6.

A. STATE EQUATION
Set i as the inertial frame (i frame), select the ''North-East-Down'' geographical coordinate system as the navigation frame (n frame), set the ''Front-Right-Down'' frame as the body frame (b frame) and set the e frame as the Earth-Centered, Earth Fixed (ECEF) frame [29].
The GPS/INS model and the DZUPT/INS model use the same state equation, which is established by combining the INS nonlinear error equations with the inertial measurement unit error equations.
The state vector is defined as where φ = φ x φ y φ z is the attitude error, δv = δv x δv y δv z is the velocity error, and δp = δL δλ δh is the position error. The constant drift of the gyro is The attitude and velocity error equations of the INS nonlinear error equations can be described as [30] where C n n , C n b and C n b are the rotation matrices [31]; v n = v x v y v z T is the velocity in n frame; δg n is the gravity error,f b is the measurements of the specific force; δf b and VOLUME 8, 2020 δω b ib are the measurement error of acceleration and gyroscope, respectively; ∇ b is constant bias; ω b a is zero-mean white noise; ε b is the constant drift; ω b g is zero-mean white noise; ω n ie is the earth's rotational velocity vector; ω n en is the angular velocity vector generated by carrier movement; ω n in = ω n ie + ω n en is the relative rotation angular velocity between the n frame and the i frame; and δω n ie , δω n en and δω n in are the calculation error of ω n ie , ω n en and ω n in , respectively. δω n ie , δω n en and δω n in can be described as where L, λ and h are latitude, longitude, and height, respectively; R m and R n are the radii of the curvature in the meridian and prime vertical, respectively; and C −1 w can be expressed as [32] The position error model is described as follows: The gyro constant drift ε b and the accelerometer zero-bias ∇ b are expressed by random constants as The system state equation can be obtained as follows: where f (·) is a nonlinear function composed of (2), (3), (6) and (7), and the process noise vector is described as Discretizing (8) by the improved Euler discretization formulation [33], the discrete state equation of the system can be described asẊ where f (·) is the nonlinear function in discrete form and w k is the process noise in discrete form.

B. MEASUREMENT EQUATION OF THE GPS/INS MODEL
In the GPS/INS navigation model, (9) is the state equation, and the difference value Z g,k between the INS velocity v n and GPS velocity v g is taken as the measurement. The measurement equation is described as where In (10), v g is the measurement noise of the system and corresponds to the velocity error of the GPS receiver, and the variance of v g is R g .

C. MEASUREMENT EQUATION OF THE DZUPT/INS MODEL
In the DZUPT method, the ZUPT method can only be applied to the Z-axis since the Z-axis velocity v z in n frame is close to 0 when the UAV is in level flight. In y 0 ] T , the state equation is still taken as (9), and the default Z-axis velocity in the n frame is 0; the difference value Z d,k between INS velocity v n and DZUPT velocity v d is taken as the measurement value. Then, there is the following equation in the n frame. The difference value Z d,k can be described as The measurement equation can be described as where the H d,k matrix can be described as In (13), v d is the measurement noise, corresponding to the measurement error of the DZUPT method and the variance of v d is R d .

III. RCKF ALGORITHM
The concept of the CKF is briefly reviewed before the introduction of RCKF. The CKF uses the spherical-radial cubature rule to solve high-dimensional nonlinear filtering problems, and the CKF process involves the following steps: Time Updating: The equations are as follows: where I n is the unit matrix, n is the dimension of the state, [·] i is the column of the matrix [·], and i = 1, 2, . . . , 2n. Evaluating the cubature points Using the nonlinear error model to evaluate the propagated cubature points Measurement Updating: Evaluate the cubature points Evaluate the propagated cubature points and the predicted measurement Calculate the Kalman gain The update state and the corresponding error covariance matrix can be obtained from the following equations: As mentioned previously, the performance of the CKF will deteriorate in the presence of measurement outliers. In this paper, RCKF is proposed to enhance the robustness of the CKF and overcome the aforementioned limitation of the CKF.
RCKF first constructs the CKF model, and then, the algorithm adjusts the weight of the observation information through the equivalent weight function by adjusting the value of R k to reduce measurement outlier interference on the navigation results. The relationship between the measurement noise matrix R k and the robust equivalent weight matrixP K is as follows: The robustness of the RCKF estimator mainly depends on the equivalent weight function. The widely used equivalent weight functions are the Huber weight function, the Turkey weight function, the Andrews weight function, the IGG (Institute of the Geodesy and Geophysics) weight function and the IGG III weight function. The IGG III weight function was chosen in the RCKF algorithm since it has a better correction effect than other weight functions when the measurements contain outliers [34].
The IGG III weight function is shown as Algorithm 1.

Algorithm 1 IGG III Weight Function
Step 1: Take the values of k 0 and k 1 according to the actual situation, in general, k 0 = 1.0 − 1.5 and k 1 = 2.5 − 8.0. In this paper, k 0 = 1.0 and k 1 = 4.0.
Step 2: Calculate the standardized residuals s ii .
Step 3: Calculate the weight reduction function γ ii .
Step 4: Calculate the robust equivalent weight matrixP K .
In (31),P K ii is the (i, i) element of the robust equivalent weight matrix, and P k ii is the (i, i) element of the robust equivalent weight matrix at the previous cycle.
The IGG III weight function divides the measurement data into three types: effective measurement, available measurement and harmful measurement.
From the equivalent weight function above, we can see that if s ii ≤ k 0 , then the estimator is equal to the traditional least square method; if s ii ≥ k 1 , then the corresponding measurement will be given zero weight; if k 0 < s ii < k 1 , then the weight of the measurement will be reduced, which could decrease the influence of measurement outliers on the filter results.

IV. IMM UAV NAVIGATION ALGORITHM BASED ON RCKF
The DZUPT/INS model can correct navigation errors when the UAV is in level flight; however, when the UAV is not in level flight, the DZUPT/INS integrated navigation model no longer plays an important role, and the system mainly relies on the GPS/INS model to obtain the correct navigation parameters. The data fusion and weight calculation of the two integrated navigation models are realized by the IMM algorithm.
The essence of the IMM algorithm is to obtain the weight of the DZUPT/INS model and the GPS/INS model, and the two models run in parallel. After calculating the innovation and innovation covariance matrix of each model, the overall state estimate is obtained by combining all the state estimates from each model. Moreover, the ZUPT detector has been used to identify the ZUPT condition in prior research. In contrast, the proposed IMM navigation algorithm employs the innovation and innovation covariance information to autonomously VOLUME 8, 2020 identify the optimal model in the current moment, which avoids complex identification of the ZUPT detector and reduces the time delay.

A. PRINCIPLE OF IMM NAVIGATION
Each model in the IMM algorithm has its own filter, and the transfer between each model is determined by the Markov transfer matrix, which is described as P =   p 11 · · · p 1r · · · · · · · · · p r1 · · · p rr   (32) where p ij represents the probability of the system transfer from the i model to the j model. The IMM algorithm can be described in the following four parts based on the Markov transition matrix between different models.
Step 1: Interaction (Model j, j = 1, 2) Given the state estimationx i,k−1|k−1 and the model probability u i,k−1 of each filter at the end of the previous cycle, the state estimationx 0j,k−1|k−1 and estimation covariance P 0j,k−1|k−1 of each filter are updated according the model transition probability.
where, µ ij,k−1|k−1 is the model transition probability, x 0j,k−1|k−1 is the state estimation of model j, and P 0 j,k−1|k−1 is the corresponding estimation covariance.
Step 2: Model Filtering (Model j, j = 1, 2) Given the interacted state estimation covariance matrix of each model, RCKF is operated in individual filters and the innovations and their covariance matrices should be recorded to update the model probability in the next step, which are whereẑ j,k|k−1 is the predicted measurement of model j and P j,k|k−1 is the predicted estimate covariance of model j.
Step 3: Model Probability Update (Model j, j = 1, 2) The model probability is updated according to the innovation and innovation covariance matrices. Assuming the innovation obeys the Gaussian distribution with a mean value of 0 and a variance of S j,k , the likelihood function is where n is the dimension of the measurement vector. The probability of model j is updated according to the likelihood function j,k , the Markov model transition probability p ij and the previous model probability µ i,k−1 . and the model probability is The likelihood function j,k characterizes the relative performance of one model with respect to the other. It can be seen from (39) that the closer to zero the innovation vector v j,k is, the large j,k will be, leading to a larger model probability for the model j. Therefore, the IMM estimation always follows one model that outperforms the other.
Step 4: Output Combination Given the newly updated weight, the outputs of individual filters are integrated according their different model probability.

B. STRUCTURE OF THE IMM NAVIGATION ALGORITHM BASED ON RCKF
From the above sections, the structure of the proposed IMM navigation algorithm based on RCKF can be divided into the following four parts: Interaction, Model filtering, Model probability update, and Output combination, as shown in Fig. 1. It can be seen from Fig. 1 that the algorithm first obtains the state estimationx 0 j,k−1 and the corresponding estimation covariance P 0 j,k−1 calculated from the state estimation and covariance of each filter at the end of previous cycle. Then, x 0 1,k−1 and P 0 1,k−1 are used to solve the navigation parameters through the GPS/INS model,x 0 2,k−1 and P 0 2,k−1 are used to solve the navigation parameters through the DZUPT/INS model, and the model probability is updated according the innovations and innovation covariance matrices. Finally,x k/k and P k/k of the filter are obtained by weighting the state estimation and the corresponding estimation covariance of individual filters according their different model probability.

V. EXPERIMENT AND ANALYSIS
In this section, simulation and experimental analyses were conducted to verify the effectiveness of the algorithm. First, the effectiveness of the DZUPT/INS integrated navigation model was verified by using the experimental data collected by the UAV when in level flight. Subsequently, experimental data involving various maneuvers were simulated to verify the improvements of the proposed IMM navigation algorithm based on RCKF.  The INS/GPS integrated system carried by the fixed-wing UAV was the Ellipse-N inertial navigation system. The system can collect the original gyroscope, magnetometer, GPS and accelerometer information. The reference navigation parameters were also obtained by the Elipse-N navigation system. The roll precision and pitch precision in Elipse-N were both 0.2 • , the yaw precision was 0.5 • , the horizontal position precision was 2 m, and the vertical position precision was 0.1 m. The indexes of the gyroscope, accelerometer, and GPS receiver in Elipse-N are shown in Table 1.
The experimental data were collected from a flight trial within a continuous time period of 760 s, including hover, level, acceleration, and deceleration flight. The starting position of the UAV was at north latitude 34.0277615 • , east longitude 108.6926932 • , and altitude of 599.87 m. The starting navigation velocity was 19.8 m/s in the north, −8.4 m/s in the east and 0.41 m/s in the downward direction. The initial yaw was −28.5 • , and the pitch and roll were both 0 • . The filter cycle of the INS/GPS model and the INS/DZUPT model was 0.02s. Fig. 2 illustrates the fixed wing UAV equipped with the Elipse-N navigation system.
The initial error covariance matrices and process noise covariance matrix were set as The measurement noise variances of the GPS/INS model were set as The measurement noise variances of the DZUPT/INS model were set as At the beginning of the flight, the UAV made a circling motion. After circling to a certain height, the UAV maintained a stable altitude and flew to the target point at a speed of 22 m/s. Then, the UAV turned around at the target point and returned to the starting point at a speed of 24 m/s. The trajectory is shown by the blue line in Fig. 3, and the flight site was in Huyi District, Xi'an City, Shaanxi Province, China.   experimental data that met the DZUPT condition (UAV in level flight). The navigation parameters of the two models were compared with the reference navigation parameters to evaluate the performance of the DZUPT/INS model. The experimental results are as follows: Fig. 4 shows that the errors between the output attitude angles of the two models and the reference values are controlled within 1 • and that the errors of the output attitude angles of the DZUPT/INS model are smaller, which shows that the DZUPT/INS model can correct navigation errors and improve the navigation accuracy.    The UAV was in a level flight state, and the vertical velocity was 0 in the selected time period; therefore, only the velocity error data of the X-axis and Y-axis are compared here. Fig. 5 illustrates that the velocity error of both models can be controlled within 0.2 m/s. However, the accuracy of the velocity parameters output by the DZUPT/INS model is higher because the attitude error is reduced by the DZUPT method, which improves the precision of the velocity measurement through the velocity error model. Table 3 Table 3.
The vertical position was basically unchanged in the selected time period, so only the position error data of the X-axis and Y-axis are compared here. Fig. 6 illustrates that both models can control the position error within 0.3 m. The accuracy of the position parameters output by the DZUPT/INS model is higher than that of the GPS/INS model because the velocity error is reduced by the DZUPT method, which improves the position precision through the position error model. Table 4 Table 4.

B. EXPERIMENTS AND ANALYSIS OF THE PROPOSED IMM NAVIGATION ALGORITHM
A simulation was carried out using the IMM navigation algorithm based on RCKF. The values of the Markov transition probability matrix P and the initial model probability µ 0 were set as The simulation results of the attitudes and positions are shown in Fig. 7 and Fig. 8, respectively, and the model probability is shown in Fig. 9.   Fig. 7 that the performance of the CKF degrades because of interference from measurement outliers. However, the RCKF algorithm could control the interference from measurement outliers on the navigation results by adjusting the value of the measurement noise matrix. RCKF and the proposed IMM algorithm based on RCKF have strong robustness to measurement outliers, resulting in an improved attitude filtering accuracy. The attitude errors of RCKF are less than 4 • , whereas those of the IMM algorithm are less than 2 • since the proposed algorithm uses the DZUPT method to correct the navigation error and obtain more accurate navigation parameters in periods of level flight.
The Mean Absolute Errors (MAE) and RMSE of the attitude errors determined by the CKF, RCKF and IMM algorithms over the entire simulation time are listed in Table 5. The statistical results presented in Table 5 also verify that the proposed IMM navigation algorithm has a better attitude filtering accuracy than the CKF and RCKF in the presence of measurement errors.     than that of the CKF since the RCKF algorithm could control the interference from measurement outliers on the navigation results. As expected, the position error of the proposed IMM algorithm is smaller than that of the CKF and RCKF, which in the north, east and downward directions are within 2.4548 m, 2.3392 m, and 2.1816 m, respectively. This improvement is because the proposed IMM navigation algorithm not only uses the RCKF algorithm to control the interference from measurement outliers but also corrects navigation errors in the level flight period by using the DZUPT algorithm. The statistical results presented in Table 6 also verify that the proposed IMM navigation algorithm has a better position filtering accuracy than the CKF and RCKF in the presence of measurement errors.   Fig. 10 show that the probability of the DZUPT/INS model changes with UAV maneuvering due to the adjustment of the IMM navigation algorithm. For example, during the time interval from 350 s to 390 s, the UAV was in level flight, which met the DZUPT condition; therefore, the probability of the DZUPT/INS model was close to 0.8. The altitude of the UAV changed during the time interval from 390 s to 420 s, which did not meet the DZUPT condition, so the probability of the DZUPT/INS model changed to approximately 0. In this time period, the GPS/INS model played a major role. When the UAV was in level flight again during the time interval from 420 s to 470 s, the probability of the DZUPT/INS model was close to 0.8, and the DZUPT/INS model played a major role again.
The above experimental results and analyses demonstrate that the proposed IMM navigation algorithm outperforms the CKF and RCKF. The proposed IMM navigation algorithm could improves navigation accuracy with the assistance of the DZUPT method.

VI. CONCLUSION
This paper presents an IMM UAV navigation algorithm based on RCKF. The DZUPT method for a UAV is established to correct navigation errors, and the RCKF algorithm is developed to enhance the filter robustness by weakening the influence of measurement outliers. Therefore, IMM navigation is introduced to fuse the GPS/INS navigation model and DZUPT/INS navigation model. Thus, the proposed navigation algorithm not only enhances the filter robustness but also corrects navigation errors with the assistance of the DZUPT method. The experimental results and analyses indicate that the proposed IMM navigation algorithm has a much higher navigation accuracy than the CKF and RCKF.
In future work, we will investigate the corresponding DZUPT methods for each UAV flight mode to further correct navigation errors and improve navigation accuracy.
XIAOXIONG LIU received the degree in navigation, guidance, and control from the School of Automatics, Northwestern Polytechnical University, Xi'an, China, in 2006. He is currently an Associate Professor at the School of Automatics, Northwestern, Polytechnical University. His research interests include UAV navigation, fault diagnosis, and nonlinear flight control.
WEIGUO ZHANG was a Visiting Researcher at the University of Salford, Salford, U.K., from 1988 to 1991. He is currently a Professor with the School of Automatics, Northwestern Polytechnical University. China. His research interests include navigation, guidance and control, intelligent flight control, and fault-tolerant control.
YUE YANG received the M.S. degree from the School of Automatics, Northwestern Polytechnical University, Xi'an, China, in 2018, where he is currently pursuing the Ph.D. degree in control science and engineering. His current research interests include vision-based navigation, sensor fusion, and UAV navigation.