A Nonlinear Double Model for Multisensor-Integrated Navigation Using the Federated EKF Algorithm for Small UAVs

Aimed at improving upon the disadvantages of the single centralized Kalman filter for integrated navigation, including its fragile robustness and low solution accuracy, a nonlinear double model based on the improved decentralized federated extended Kalman filter (EKF) for integrated navigation is proposed. The multisensor error model is established and simplified in this paper according to the near-ground short distance navigation applications of small unmanned aerial vehicles (UAVs). In order to overcome the centralized Kalman filter that is used in the linear Gaussian system, the improved federated EKF is designed for multisensor-integrated navigation. Subsequently, because of the navigation requirements of UAVs, especially for the attitude solution accuracy, this paper presents a nonlinear double model that consists of the nonlinear attitude heading reference system (AHRS) model and nonlinear strapdown inertial navigation system (SINS)/GPS-integrated navigation model. Moreover, the common state parameters of the nonlinear double model are optimized by the federated filter to obtain a better attitude. The proposed algorithm is compared with multisensor complementary filtering (MSCF) and multisensor EKF (MSEKF) using collected flight sensors data. The simulation and experimental tests demonstrate that the proposed algorithm has a good robustness and state estimation solution accuracy.


Introduction
Unmanned aerial vehicles (UAV) have taken a prominent role in a variety of applications, mostly military but also civilian. There are many significant advantages to using UAVs, such as for aerial surveillance [1], three-dimensional mapping models [2], search and rescue [3], and inspection [4] in complex environments. In recent years, there has been great interest in developing autonomous unmanned flight systems (AUFS) with advanced onboard capabilities, which include three critical types of technology: those of navigation, guidance, and control. The main focus of this paper is the research of navigation algorithm technology [5]. Traditionally, navigation [6] is the process of monitoring and controlling the UAV from one place to another place. In the broad sense, it can be defined as the process of data acquisition, analysis, extraction, and the inference of information. Moreover, it is a vital part of the UAV to interact with complex environments, and provide efficient and reliable state estimation parameters, which include attitude, velocity, and position. The accurate state estimation can help the UAV to perform trajectory control using certain advanced control system methods, such as the P-PI cascade control system [7], and the artificial cognitive control system based to global filter. For the navigation error model, Liang and Jia [35] proposed a fusion framework based on the federated filter, which is composed of EKF and differential decoupling KF (DDKF) for fault tolerance. Meanwhile, the prediction covariance matrix is extended by using the fault factor. Finally, the fault state and measurement are processed normally. Therefore, the federated filter [33] is very suitable for multisensor-integrated navigation as a new decentralized filter approach. It is often used for information distribution, as it is allocated among the different subsystems, and helps to improve filter performance.
In this paper, with a focus on model nonlinearity and state solution accuracy, a nonlinear double-model multisensor-integrated navigation method is proposed based on the federated extended Kalman filter fusion framework. A nonlinear double-model system consists of the nonlinear attitude heading reference system (AHRS) model and the nonlinear SINS/GPS model. The nonlinear attitude heading reference system model was used to perform the first local EKF sensor fusion to estimate the attitude error ϕ ϕ ϕ n,1 and gyroscope bias ε ε ε b,1 r in Equation (22). The nonlinear SINS/GPS-integrated navigation model was used to perform the second local EKF sensor fusion to estimate the attitude error δϕ ϕ ϕ n,2 , velocity error ε ε ε b,2 r , position error δv v v n,2 , gyroscope bias δp p p n,2 , and accelerometer bias b,2 r in Equation (23). Then, the common states (δϕ ϕ ϕ n and ε ε ε b r in Equation (24)) of the nonlinear double model were optimized by the main filter to obtain the best attitude estimation. The main contributions of this study relative to other studies include the following: 1. The multisensor error mathematical model, which includes the gyroscope error model, the accelerometer error model, and the SINS error model, are described in this paper. Furthermore, these error models are reasonably simplified to apply to small-UAV near-ground short distance navigation; 2. A federated EKF approach is proposed combined with the federated filter and EKF, which not only solves the model nonlinearity, but also retains excellent filter precision; 3. During UAV flight, the requirement of attitude solution accuracy is of great importance. Thus, a nonlinear double model is proposed, including the nonlinear AHRS model and the nonlinear SINS/GPS model. The common attitude solutions of the nonlinear double model are fused in the main filter of the federated filter.
The rest of this paper is organized as follows. In Section 2, the multisensor error mathematical model is introduced and simplified. The double-model EKF algorithm for the federated filter is proposed in Section 3. Then, the simulation and experimental test in Section 4 demonstrate the performance of the proposed algorithm. Finally, the conclusions are given in Section 5.

Gyroscope Error Model
The zero offset bias is the main influence factor of the gyroscope output precision when the UAV is in a static situation. Therefore, eliminating the zero offset error can improve the angular velocity accuracy of the gyroscope measurement. However, the random drift error of the gyroscope will change when the UAV is in flight, so the gyroscope error needs to be modeled [36] and estimated.
where ε ε ε 0 is the gyroscope constant error, ε ε ε r is the gyroscope random drift error, and ω ω ω g is the gyroscope error white noise. The first-order Markov model of the gyroscope random drift error ε ε ε r is Equation (2).
where τ g is the first-order Markov time constant, and ω ω ω gr is the first-order Markov random white noise. Actually, ε ε ε 0 is the constant when the gyroscope is running, and it can be derived by Equation (3).
Therefore, the constant error can be taken into account in the random drift error. The gyroscope error model [37] can be derived from Equations (1)- (3).

Accelerometer Error Model
Typically, the error of the accelerometer includes the constant error and random drift error. Constant error can be eliminated using sensor calibration [38]. However, random drift error is time-varying and can be estimated using the filter algorithm to compensate. The accelerometer error model [35] can be represented by Equation (5).
where 0 is the constant error, r is the random drift error, and ω ω ω a is the white noise of the accelerometer. The random drift error can be assumed to be the first-order Markov time constant in this paper, and can be derived from Equation (6).
where τ a is the first-order Markov time constant, and ω ω ω ar is the white noise. The constant error can be taken into account in the random drift error, the accelerometer error model can be derived from Equations (5) and (6).

SINS Error Model
The attitude of the SINS error model [13] can be written as Equation (8).
where δϕ ϕ ϕ is the attitude error of the UAV, ε ε ε n g is gyroscope bias in the navigation system, ω ω ω n in is the angular rate in the navigation system relative to the inertial system, and ε ε ε n g is the gyroscope error in the navigation system.
The velocity of the SINS error model [13] can be represented by Equation (9).
where v v v n is the velocity of the UAV, δv v v n is the velocity error in the navigation system, ϕ ϕ ϕ is the attitude of the UAV, f f f n is the special force of the accelerometer, ω ω ω n ie is the angular rate in the earth system relative to the inertial system, ω ω ω n en is angular rate in the navigation system relative to the earth system, and n a is the accelerometer error in the navigation system. The position of the SINS error model [13] can be described using Equation (10).
where [L λ h] T is the longitude, latitude, and height in the navigation system, respectively; [v n x v n y v n z ] T is the velocity in the navigation system; and R e is the radius of earth.
In this paper, MEMS IMU sensors are used, which have a large noise compared to high-precision inertial devices. At the same time, the error state of the system only changes slightly under filter conditions, when the state of the SINS error equation selects the error quantity. Moreover, the SINS error model is quite complex in terms of computational complexity for the study of the embedded MEMS sensor fusion algorithm. Thus, there are some reasonable simplifications for the SINS error model in order to decrease the computational complexity and improve the update frequency of the navigation algorithm.
There is the pretty low height and relatively slow flight speed when a small UAV is in flight, and the update frequency of the proposed algorithm is high when the airborne CPU is running. Therefore, the rotation of navigation system caused by the change in the position of the UAV can be ignored during the algorithm running period. In addition, the measurement accuracy of the earth rotation angular rate is small relative to the gyroscope, and so the earth rotation angular rate is considered to be 0. The UAV coordinate system is shown in Figure 1. In order to more intuitively observe the velocity and position change of the UAV, the navigation system is the north-east-down (N-E-D), and the body system is the front-right-down (X-Y-Z).
where b is the body system, n is the navigation system, and C C C n b is the direction cosine matrix from the body system to the navigation system.

Extended Kalman Filter
The Kalman filter problem generally is assumed to be linear in the mathematical model. However, the model in practical applications is a nonlinear system. In this paper, the designed method relies on the extended Kalman filter [23] for the nonlinear model system.
In Equation (12), the jacobian matrix of Φ k/k−1 and H k are obtained by the partial derivatives of f (X k−1 ) and h(X k/k−1 ), respectively.

Federated Filter
There are generally two optimal combining methods for a multisensor-integrated navigation system: centralized KF and decentralized KF, respectively. Theoretically, centralized KF, which uses single KF to centrally process all the subsystem information of navigation, can give an optimal estimation of the state. As a result of the high state estimation dimension in practice, the computation is too large to run in real time for the algorithm. In addition, the centralized KF has a poor fault tolerance and is not conductive to fault diagnoses. However, the decentralized KF can use multiple filters to make estimations for the different subsystems of navigation so as to reduce error coupling. There are many decentralized KF algorithms currently. For example, an approach of decentralized algorithms named the federated filter was proposed by Carlson [32,33]. The federated filter is a special form of decentralized Kalman filtering method; it consists of several subsystem filters and a main filter. It is a decentralized filtering method with block estimation and a two-step cascade.
The federated filter is often seen as a two-stage filter structure, as is shown in Figure 2. In this paper, the reference system in Figure 2 is the SINS; the output of which is given to the main filter on the one hand, and is also given to the local filter as the measurement value on the other. Then, the local state estimation and covariance are fed together with the main filter to obtain the global optimal estimation. β i (i = 1, 2, ..., n) is the information distribution coefficient, and according to the principle of information distribution, different β i can be used to obtain the different structures and characteristics of the federated filter. If there exists n local state estimations X 1 , X 2 , ..., X n and the corresponding covariances P 1 , P 2 , ..., P n , (P ij = 0(i = j)), the global optimal estimation [39] can be expressed as Equation (14).
where the X g and P g are the global estimation and covariance, respectively; and the X i and P i are the ith state estimation and covariance of the local filter, respectively.
where the X ci is the common state estimation of the local filter, and X bi is the unique state estimation of the local filter. In this paper, we only fuse the common state estimation to get the global optimal estimation.

The Improved Federated EKF
The EKF has a good processing ability for nonlinear problems, and it is also easier to implement in practical applications. In addition, the federated filter has good fault tolerance compared to centralized KF. Moreover, the fault can be easily detected and isolated when the subsystem fails, then rest of the systems can be reconstructed. Thus, the proposed algorithm for filter precision is large and the computation of the fusion algorithm from the local filter to global filter is small, which is beneficial to execute in real time.
Aimed at the application of multisensor fusion, the improved federated EKF algorithm is proposed in this paper, which uses EKF to optimize the nonlinear model of the local filter, and then globally optimizes for the common state of the local EKF in order to meet the requirements of autonomous position navigation. Additionally, it can improve flight stability and reliability. Equation (16) is the improved federated EKF recursion formula.
where i is the ith local EKF, X g and P g are the results that are globally optimized for the common state estimation. The improved federated EKF fusion diagram is shown in Figure 3. Two local EKFs are used in this paper: one is to fuse the nonlinear AHRS model problem, and the other is to fuse the nonlinear SINS/GPS-integrated navigation problem, which will be introduced detail later.

The Nonlinear Double Model
Typically, this should be mathematically modeled when the actual system with nonlinear characteristics is described. Therefore, a unified nonlinear Gaussian state space model system is established for the multimodel of the federated filter based on the nonlinear theory [22].
where i is the ith nonlinear model, X i k is the state parameter, f (X i k−1 ) is the nonlinear state function, ω i k−1 is the process noise, Z i k is the measurement quantity, h(X i k ) is the nonlinear measurement parameter, and ν i k is the measurement noise. Moreover, ω i k−1 and ν i k are the Gaussian white noise.

The Nonlinear AHRS Model
The AHRS can affect UAV flight stability, which is regulated by fusing the gyroscope, accelerometer, and magnetometer. According to the multisensor error mathematical model in Section 2, the nonlinear error model of AHRS in Equation (18) is established.
where δϕ ϕ ϕ n,1 is the attitude error in the navigation system, ω ω ω b,1 g is the Gaussian white noise of the gyroscope in the body system, and ε ε ε b,1 r is the bias of the gyroscope in the body system. The measurement function of the AHRS can be written by Equation (19).
where φ a k and θ a k are the roll and pitch calculated by the accelerometer, respectively; ψ m k is the yaw calculated by the magnetometer; φ k , θ k and ψ k are the attitude update value form the gyroscope; ν a k is the Gaussian white noise of the measurement from the accelerometer, and ν b k is the Gaussian white noise of the measurement from magnetometer.

The Nonlinear SINS/GPS-Integrated Navigation Model
This paper uses the low-cost IMU that causes the state estimation to diverge over long running times. Thus, the GPS is selected as another subsystem of the multisensor-integrated navigation system of the UAV. The GPS, which has a good long-term stability, calculates the velocity and position of the carrier by utilizing the received satellite navigation signals. It can be seen that the SINS/GPS is the current mainstream choice because of the complementary performance of SINS and GPS. However, it is necessary to establish a nonlinear mathematical model that accurately reflects the system in order to better analyze the actual SINS/GPS system.
where δϕ ϕ ϕ n,2 , δv v v n,2 , and δp p p n,2 are the errors for attitude, velocity, and position, respectively; ε ε ε b,2 r and b,2 r are the bias of the gyroscope and accelerometer, respectively; and ω ω ω b,2 gr and ω ω ω b,2 ar are the Gaussian white noise of the gyroscope and accelerometer, respectively.
The measurement function of the SINS/GPS can be established by Equation (21).
where p p p a k and v v v a k are the velocity and position of the GPS, respectively; p p p k and v v v k are the velocity and position of SINS, respectively; ν c k is the Gaussian white noise of the GPS.

Common State Fusion
The common state parameters of the nonlinear double model, which are the error of attitude δϕ ϕ ϕ n and gyroscope bias ε ε ε b r , are fused by Equations (22) and (23) under the federated filter framework. In addition, the gyroscope bias and the accelerometer bias of the main filter are fed back into the SINS so as to reduce sensor errors.
The error of attitude and the gyroscope bias of Equations (22) and (23) are taken as the common state estimation. On the one hand, the attitude is of a high accuracy because the UAV is a flexible, maneuverable aircraft; on the other hand, the accurate attitude can make the velocity and position of the UAV more reliable in the navigation system. The common state fusion algorithm of the federated filter is obtained from Equation (25).
where X C and P C are the common state estimation and covariance, respectively; X A and P A are the state estimation and covariance of the nonlinear AHRS model, respectively; X B and P B are the state estimation and covariance of the nonlinear SINS/GPS-integrated navigation model, respectively; X B [1 :6] is the error of attitude and gyroscope bias; and P B [1:6,1:6] is the corresponding covariance.
The unique state parameter of the nonlinear SINS/GPS-integrated navigation model is used as part of the main filter state output in the federated filter framework.
where X B [7:15] is the unique state quantity which includes the velocity error, position error, and accelerometer bias.

The Federated Double-Model EKF Algorithm
The local EKF and federated filter were introduced in Section 2, and a federated EKF approach was proposed. After that, a nonlinear double model was designed in order to increase the solution accuracy and fault tolerance of the filter system. Finally, the federated double-model EKF multisensor fusion algorithm was investigated. The body system is "front-right-down", and the navigation system is "north-east-down".
The establishment of the federated double-model EKF algorithm can make state estimation more accurate, because the UAV requires higher accuracy for attitude calculations compared to ground robots. This problem is effectively solved by the nonlinear double model in this paper. The decentralized multimodel can globally optimize the common state (error of attitude and gyroscope bias) of the two nonlinear models, which is suitable for the rapid maneuvering of the UAV. The traditional SINS/GPS-integrated navigation diverges when the GPS signal is interfered with or invalid; however, the algorithm in this paper reconstructs the rest of the sensor information in the case of GPS failure, which can guarantee that the normal attitude is provided to the UAV. The federated double-model EKF fusion diagram is shown in Figure 4. The nonlinear AHRS local EKF system includes three kinds of sensors: gyroscope, accelerometer, and magnetometer. As shown in Figure 5, the measurement of attitude by the three-axis accelerometer and magnetometer is calculated by FastEuler solution, and subtracted from the attitude that is obtained by the gyroscope attitude update. Then, the local EKF AHRS fusion is used to optimize in order to obtain the error of attitude and gyroscope bias. The FastEuler solution is derived from Equations (27) and (28).
The nonlinear SINS/GPS model system includes three kinds of sensors: the gyroscope, accelerometer, and GPS. As shown in Figure 6, the three-axis gyroscope angular velocity is calculated by the quaternion attitude update formula to obtain the direction cosine matrix C C C n b , and converting the three-axis accelerometer measurement from the body system to the navigation system in order to use the dead reckoning to calculate the velocity v v v and position p p p. The longitude, latitude, and height of GPS data is calculated using Equation (29) to obtain the position p p p in the navigation system. The ground speed (Gndspd) and course degree (coursDeg) of the GPS data are solved using Equation (30) to obtain the velocity v v v in the navigation system. Then, the error measurement of the velocity and position are calculated using the ( v v v − v v v) and ( p p p − p p p). Finally, the local EKF is used to calculate the error of the attitude, velocity, and position, respectively.
where R e is the Earth's radius (about 6,378,145 m), ∆λ and ∆h are the differences in latitude and height in the adjacent time interval, L 0 is the longitude at the initial time, the Gndspd and coursDed are the ground rate and course degree at the current time, respectively.

Data Acquisition and Flight Platform
This paper used the experimental platform shown in Figure 7 to obtain the raw flight sensors data. It includes the three-axis angular velocity, three-axis acceleration, three-axis magnetic values, velocity of GPS, and position of GPS. The flight sensors data condition with GPS and without GPS were collected by the vertical take-off and landing (VTOL ) UAV.

Flight Sensors Data with GPS
The flight sensors data were collected with the GPS of the UAV using the experimental platform shown in Figure 7 whilst performing some maneuvers. The proposed federated double-model EKF (FDMEKF) was compared with multisensor complementary filtering (MSCF) (the MSCF takes advantage of the complementary nature of sensor frequencies to estimate the state parameters of the UAV) and the multisensor EKF (MSEKF) (the MSEKF is only used to process the nonlinear SINS/GPS model to estimate the state parameters of the UAV).
As can be seen in the previous figures, the attitude error of MSCF is the largest compared with the other two algorithms in Figure 8. The sensor noise is an important factor affecting the solution accuracy of the low-cost sensors. However, the MSCF cannot suppress the noise of the low-cost sensors very well. This is because the MSCF model does not consider the sensor noise, it just uses the complementary characteristics of the sensor frequency. The MSEKF model introduces the sensor noise in Equation (17) (i = 1), and can handle the nonlinear SINS/GPS model. However, the state solution accuracy of the single nonlinear model is not as good as the FDMEKF. The FDMEKF not only considers the sensor noise in Equation (17) (i = 2), but also describes the two local EKF methods to the nonlinear AHRS model and the nonlinear SINS/GPS model. Then, it fuses the common state of the two models to improve the attitude solution accuracy compared with the MSCF and the MSEKF.
It can be seen that the attitude of MSCF is influenced by the sensor noise from the diagram of the UAV attitude in Figure 9, so that the roll of MSCF demonstrates an occasional abnormal burr phenomenon. The attitude of MSEKF has a certain deviation compared to the truth, and the roll of MSEKF does not track the roll of the truth after 540 s. Compared with the two previous kinds of filters, the attitude of the FDMEKF can track the truth very well. Firstly, since the FDMEKF can eliminate the sensor noise compared to the MSCF. Secondly, as a result of the nonlinear double model based on the decentralized filter, the filter accuracy is higher than the single model centralized filter, and is more suitable for the UAV. In order to more intuitively compare the attitude accuracy of the three filters, Table 1 provides the mean absolute errors (MAE), standard deviation (STD), and root mean square errors (RMSE) of the attitude obtained by MSCF, MSEKF, and FDMEKF. It can be seen that the MAE, STD, and RMSE of the FDMEKF are also much smaller than those of the other two filters. This is because the FDMEKF improves the attitude solution accuracy through fusing the common states of the two nonlinear models using Equation (14).    Figure 10 depicts the velocity error of the UAV. It is evident that the velocity error of the FDMEKF is smallest because it considers the GPS velocity noise. Moreover, the improvement in the FDMEKF attitude accuracy reduces the cumulative error caused by the dead reckoning from acceleration.
It can be seen that the FDMEKF can better track the velocity of the truth compared with the velocity of MSCF and the MSEKF in Figure 11, respectively. In the velocity of the MSCF, a large magnitude of oscillations appear in the filtering curve when the UAV performs the maneuver. This is because the MSCF does not eliminate the large GPS velocity noise from the low-cost sensors. Table 2 provides the mean absolute errors (MAE), standard deviations (STD), and root mean square errors (RMSE) of the velocity obtained by MSCF, MSEKF, and FDMEKF. It can be clearly seen that the FDMEKF has a good performance in terms of the UAV velocity solution accuracy.   Figure 12 shows the position error of the UAV. It is evident that the position error of MSCF is large because it ignores the GPS position noise when calculating the position solution. Meanwhile, it was already shown that the velocity error of the FDMEKF is the smallest. When the velocity is integrated to get the position, the velocity error is transferred to the position error. Thus, the velocity error also affects the position error. Therefore, the position error of the FDMEKF is also better than the other two filters.   It can be found that the FDMEKF can better track the position of truth compared with the position of MSCF and the MSEKF in Figure 13, respectively. Moreover, the position curves of the FDMEKF are smoother than those of the MSCF and the MSEKF. Table 3 provides the mean absolute errors (MAE), standard deviations (STD), and root mean square errors (RMSE) of the position obtained by MSCF, MSEKF, and FDMEKF. It illustrates that the position solution accuracy of the FDMEKF is better than the MSCF and the MSEKF.

Flight Sensors Data without GPS
The flight sensors data without GPS of the UAV were collected by the flight platform as shown in Figure 7. This is to verify that when the GPS fails, the proposed algorithm can still provide a reliable attitude for the UAV.
The velocity and position of the state estimation parameters are inaccurate when GPS signal is disturbed in city buildings and jungle environment. At this time, the reliable attitude solution is very important when the UAV is in flight. Figure 14 shows that the attitude error curves of the MSCF and the MSEKF have large fluctuations. The attitude error of the FDMEKF can converge to 2 degrees. The nonlinear SINS/GPS of the double model fails when the GPS signal is disturbed, but the FDMEKF can still provide the reliable attitude depending on the nonlinear AHRS of the double model. It can be seen that the attitude curves of the FDMEKF are closest to the truth compared with the MSCF and MSEKF, as shown in Figure 15. The attitude solution of the MSCF is inaccurate due to the lack of the low frequency characteristics of the GPS signal. Meanwhile, the attitude solution of the MSCF has a divergent trend due to the failure of the nonlinear SINS/GPS model. Table 4 provides the mean absolute errors (MAE), standard deviations (STD), and root mean square errors (RMSE) of the attitude obtained by the MSCF and MSEKF. It clearly illustrates that the FDMEKF can still provide a good attitude solution accuracy when the GPS signal fails.

Conclusions
A nonlinear double model approach based on the decentralized federated extended Kalman filter (EKF) is proposed in this paper for the multisensor fusion algorithm of small UAVs. The contributions of this paper include the following: (1) The multisensor error mathematical model is established and simplified to a reasonable extent; (2) the federated EKF is further developed to enhance the filter accuracy and robustness; (3) a novel nonlinear double model is designed to obtain a more accurate attitude solution for cases in which there is GPS and there is not. Simulations and experimental test results demonstrate that the proposed filter algorithm can effectively provide the state estimation in order to meet the flight requirements of a UAV, as well as having a much higher solution accuracy.