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

Abstract

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.

1. 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 on a reinforcement learning strategy [8]. In particular, the accuracy of the sensors is limited because of the small UAV hardware performance and carrier space, but the multisensor Kalman filter fusion algorithm [9] allows us to construct state estimation from the different sensors. Moreover, progress in electronics miniaturization and microprocessors [10,11] has made it possible to house these sensors in small and compact devices. In addition, with the rapid development of MEMS [12], the cost of navigation devices has reduced, which brings the field of multisensor fusion algorithms into a period of rapid growth.

In the research of UAV navigation technology, the inertial navigation system (INS) and global satellite navigation system (GNSS) are indispensable. The INS [13,14], which depends on the inertial measurement unit (IMU), can provide the attitude, velocity, and position. Although INS has functional autonomy and a good reliability, and is not disturbed by the external environment, it has the disadvantage that the accumulation of INS errors can increase during the dead reckoning process, especially for low-cost IMUs. Thus, other navigation systems, e.g., global satellite navigation system (GNSS) [14,15], vision navigation [16], lidar simultaneous localization and mapping (Lidar-SLAM) [17], etc., can be employed to assist the INS with long-term precision. The GNSS can provide the velocity and position through the GNSS receiver [18] without signal shadows. Almost all outdoor UAVs are equipped with the GNSS receiver to provide their absolute location [19], and the position error of GNSS is about 1–2 m in the open field. Therefore, for a single navigation system with low-cost devices, the navigation data accuracy makes it difficult to meet the flight needs. A multisensor-integrated navigation system approach, which utilizes the complementarity of different navigation systems, can solve the issues of single navigation systems.

Nowadays, the Kalman filter [20,21], which is considered to be an optimal filter for the linear system model and statistical properties of Gaussian white noise, is widely used in many fields. In the multisensor-integrated navigation system, multiple navigation systems can be fused with the Kalman filter to obtain state estimation parameters. As a result of the UAV in-flight maneuverability and the complexity of the flight environment, the multisensor-integrated navigation model has the characteristics of nonlinearity and uncertainty. However, the Kalman filter, which is only applicable to linear Gaussian systems, is no longer optimal. Thus, a few improved nonlinear filter algorithms [22] have been proposed by certain scholars, such as the extended Kalman filter (EKF) [23], the unscented Kalman filter (UKF) [24,25], cubature Kalman filter (CKF) [26], particle filter (PF) [27], etc. In order to solve the parameter divergence caused by the strapdown inertial navigation system (SINS)/GPS model error, a quadratic EKF algorithm was proposed by Wang and Liu [28]. It can reduce the calculation by retaining the second-order derivation information and two-stage cascade estimation. In addition, for the low accuracy in filtering resulting from motion model errors, Hu and Wang [29] proposed a method based on the establishment of INS/GPS-integrated navigation by the UKF, which embeds the suboptimal fading factor (SFF) in the prediction covariance. Furthermore, a refined strong tracking UKF (RSTUKF) was developed to increase the robustness of UKF-resistant motion model errors. Liu and Qu [30] proposed a method combining the maximum correntropy criterion (MCC) and square-root cubature Kalman filter (SCKF) to apply to the SINS/GPS-integrated system, which not only retains the advantages of SRCF, but also exhibits a robust performance against heavy-tailed non-Gaussian noise.

Everything described above refers to centralized nonlinear navigation filters, which have several drawbacks. These include the heavy computational burden, the high state dimension, and the uncertain filter divergence. Compared with centralized filters, decentralized filters [31] use the redundancy information to detect and isolate the fault system, and construct the remaining subsystems, which can continue to complete the required task. Carlson [32,33] proposed a decentralized filter approach named the federated filter, which is being widely used because of its flexible design, small calculation costs, and good fault tolerance. When one or several navigation subsystems fail, it is easy to detect and separate the faults [34], and the remaining normal navigation subsystems can be quickly constructed to give the required filter states. Furthermore, it is reliable for the fusion algorithm from the local filter 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 ${\mathit{\phi }}^{n,1}$ and gyroscope bias ${\mathit{\epsilon }}_r^{b,1}$ 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 $\delta {\mathit{\phi }}^{n,2}$, velocity error ${\mathit{\epsilon }}_r^{b,2}$, position error $\delta {\mathit{v}}^{n,2}$, gyroscope bias $\delta {\mathit{p}}^{n,2}$, and accelerometer bias ${▽}_r^{b,2}$ in Equation (23). Then, the common states ($\delta {\mathit{\phi }}^n$ and ${\mathit{\epsilon }}_r^b$ 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.

2. Multisensor Error Mathematical Model

2.1. 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.

$${\mathit{\epsilon }}_g={\mathit{\epsilon }}_0+{\mathit{\epsilon }}_r+{\mathit{\omega }}_g,$$

(1)

where ${\mathit{\epsilon }}_0$ is the gyroscope constant error, ${\mathit{\epsilon }}_r$ is the gyroscope random drift error, and ${\mathit{\omega }}_g$ is the gyroscope error white noise. The first-order Markov model of the gyroscope random drift error ${\mathit{\epsilon }}_r$ is Equation (2).

$${\dot{\mathit{\epsilon }}}_r=-\frac{1}{{\tau }_g}{\mathit{\epsilon }}_r+{\mathit{\omega }}_{gr},$$

(2)

where ${\tau }_g$ is the first-order Markov time constant, and ${\mathit{\omega }}_{gr}$ is the first-order Markov random white noise. Actually, ${\mathit{\epsilon }}_0$ is the constant when the gyroscope is running, and it can be derived by Equation (3).

$$\left\{\begin{array}{c}{\dot{\mathit{\epsilon }}}_0=0\hfill \\ {\dot{\mathit{\epsilon }}}_0+{\dot{\mathit{\epsilon }}}_r={\dot{\mathit{\epsilon }}}_r\hfill \end{array}\right.$$

(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).

$${\mathit{\epsilon }}_g={\mathit{\epsilon }}_r+{\mathit{\omega }}_g$$

(4)

2.2. 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).

$${▽}_a={▽}_0+{▽}_r+{\mathit{\omega }}_a,$$

(5)

where ${▽}_0$ is the constant error, ${▽}_r$ is the random drift error, and ${\mathit{\omega }}_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).

$${\dot{▽}}_r=-\frac{1}{{\tau }_a}{▽}_r+{\mathit{\omega }}_{ar},$$

(6)

where ${\tau }_a$ is the first-order Markov time constant, and ${\mathit{\omega }}_{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).

$${▽}_a={▽}_r+{\mathit{\omega }}_a$$

(7)

2.3. SINS Error Model

The attitude of the SINS error model [13] can be written as Equation (8).

$$\delta \dot{\mathit{\phi }}=\delta \mathit{\phi }\times {\mathit{\omega }}_{in}^n+\delta {\mathit{\omega }}_{in}^n-{\mathit{\epsilon }}_g^n,$$

(8)

where $\delta \mathit{\phi }$ is the attitude error of the UAV, ${\mathit{\epsilon }}_g^n$ is gyroscope bias in the navigation system, ${\mathit{\omega }}_{in}^n$ is the angular rate in the navigation system relative to the inertial system, and ${\mathit{\epsilon }}_g^n$ is the gyroscope error in the navigation system.

The velocity of the SINS error model [13] can be represented by Equation (9).

$$\delta {\dot{\mathit{v}}}^n=-\delta \mathit{\phi }\times {\mathit{f}}^n+\delta {\mathit{v}}^n\times (2{\mathit{\omega }}_{ie}^n+{\mathit{\omega }}_{en}^n)+{\mathit{v}}^n\times (2\delta {\mathit{\omega }}_{ie}^n+\delta {\mathit{\omega }}_{en}^n)+{▽}_a^n,$$

(9)

where ${\mathit{v}}^n$ is the velocity of the UAV, $\delta {\mathit{v}}^n$ is the velocity error in the navigation system, $\mathit{\phi }$ is the attitude of the UAV, ${\mathit{f}}^n$ is the special force of the accelerometer, ${\mathit{\omega }}_{ie}^n$ is the angular rate in the earth system relative to the inertial system, ${\mathit{\omega }}_{en}^n$ is angular rate in the navigation system relative to the earth system, and ${▽}_a^n$ is the accelerometer error in the navigation system.

The position of the SINS error model [13] can be described using Equation (10).

$$\left\{\begin{array}{c}\delta \dot{L}=\frac{\delta v_x^n}{R_e+h}-\delta h{\displaystyle \frac{v_x^n}{{(R_e+h)}^2}}\hfill \\ \delta \dot{\lambda }=\frac{\delta v_y^n}{R_e+h}secL+\delta L\frac{v_y^n}{R_e+h}tanLsecL-\delta h\frac{v_y^{n}secL}{{(R_e+h)}^2}\hfill \\ \delta \dot{h}=-\delta v_z^n,\hfill \end{array}\right.$$

(10)

where ${\left[L\lambda h\right]}^T$ is the longitude, latitude, and height in the navigation system, respectively; ${\left[v_x^n{v}_y^n{v}_z^n\right]}^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).

The nonlinear error model of Equations (8)–(10) is simplified, and the ${\left[L\lambda h\right]}^T$ is replaced by the $p^n={\left[p^N{p}^E{p}^D\right]}^T$ regarding the UAV position in the navigation system.

$$\left\{\begin{array}{c}\delta {\dot{\mathit{\phi }}}^n=-{\mathit{C}}_b^n{\mathit{\epsilon }}_r^b-{\mathit{C}}_b^n{\mathit{\omega }}_g^b\hfill \\ \delta {\dot{\mathit{v}}}^n={\mathit{C}}_b^n{\mathit{f}}^b\times \delta {\mathit{\phi }}^n+{\mathit{C}}_b^n{▽}_r^b+{\mathit{C}}_b^n{\mathit{\omega }}_a^b\hfill \\ \delta {\dot{\mathit{p}}}^n=\delta {\mathit{v}}^n\hfill \\ {\dot{\mathit{\epsilon }}}_r^b=-\frac{1}{{\tau }_g}{\mathit{\epsilon }}_r^b+{\mathit{\omega }}_{gr}^b\hfill \\ {\dot{▽}}_r^b=-\frac{1}{{\tau }_a}{▽}_r^b+{\mathit{\omega }}_{ar}^b\hfill \end{array}\right.$$

(11)

where b is the body system, n is the navigation system, and ${\mathit{C}}_b^n$ is the direction cosine matrix from the body system to the navigation system.

3. The Double-Model EKF Algorithm of Federated Filter

3.1. 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.

$$\left\{\begin{array}{c}{X}_{k/k-1}=f\left(X_{k-1}\right)\hfill \\ P_{k/k-1}={\mathrm{\Phi }}_{k/k-1}{P}_{k-1}{\mathrm{\Phi }}_{k/k-1}^T+Q_{k-1}\hfill \\ K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}\hfill \\ X_k=X_{k/k-1}+K_k[Z_k-h\left(X_{k/k-1}\right)]\hfill \\ P_k=(I-K_k{H}_k)P_{k/k-1}\hfill \end{array}\right.$$

(12)

In Equation (12), the jacobian matrix of ${\mathrm{\Phi }}_{k/k-1}$ and $H_k$ are obtained by the partial derivatives of $f\left(X_{k-1}\right)$ and $h\left(X_{k/k-1}\right)$, respectively.

$$\left\{\begin{array}{c}{\mathrm{\Phi }}_{k/k-1}=J\left(f\left(X_{k-1}\right)\right)\hfill \\ H_k=J\left(h\left(X_{k/k-1}\right)\right)\hfill \end{array}\right.$$

(13)

3.2. 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. ${\beta }_i(i=1,2,\dots n)$ is the information distribution coefficient, and according to the principle of information distribution, different ${\beta }_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,\dots ,X_n$ and the corresponding covariances $P_1,P_2,\dots ,P_n$, ($P_{ij}=0(i\ne j)$), the global optimal estimation [39] can be expressed as Equation (14).

$$\left\{\begin{array}{c}{X}_g=P_g\sum _{i=1}^n{P}_i^{-1}{X}_i\hfill \\ P_g={\left(\sum _{i=1}^n{P}_i^{-1}\right)}^{-1},\hfill \end{array}\right.$$

(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.

$$X_i=\left[\begin{array}{c}{X}_{ci}\\ X_{bi}\end{array}\right],$$

(15)

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.

3.3. 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.

$$\left\{\begin{array}{c}{X}_{k/k-1}^i=f\left(X_{k-1}^i\right)\hfill \\ P_{k/k-1}^i={\mathrm{\Phi }}_{k/k-1}^i{P}_{k-1}^i{\left({\mathrm{\Phi }}_{k/k-1}^i\right)}^T+Q_{k-1}^i\hfill \\ K_k^i=P_{k/k-1}^i{\left(H_k^i\right)}^T{(H_k{P}_{k/k-1}^i{\left(H_k^i\right)}^T+R_k^i)}^{-1}\hfill \\ X_k^i=X_{k/k-1}^i+K_k^i[Z_k^i-h\left(X_{k/k-1}^i\right)]\hfill \\ P_k^i=(I-K_k^i{H}_k^i)P_{k/k-1}^i\hfill \\ X_g=P_g\sum _{i=1}^n{\left(P_k^i\right)}^{-1}{X}_k^i\hfill \\ P_g={\left(\sum _{i=1}^n{\left(P_k^i\right)}^{-1}\right)}^{-1}\hfill \end{array}\right.$$

(16)

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.

3.4. 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].

$$\left\{\begin{array}{c}{X}_k^i=f\left(X_{k-1}^i\right)+{\mathit{\omega }}_{k-1}^i\hfill \\ Z_k^i=h\left(X_k^i\right)+{\nu }_k^i\hfill \end{array}\right.$$

(17)

where i is the ith nonlinear model, $X_k^i$ is the state parameter, $f\left(X_{k-1}^i\right)$ is the nonlinear state function, ${\mathit{\omega }}_{k-1}^i$ is the process noise, $Z_k^i$ is the measurement quantity, $h\left(X_k^i\right)$ is the nonlinear measurement parameter, and ${\nu }_k^i$ is the measurement noise. Moreover, ${\mathit{\omega }}_{k-1}^i$ and ${\nu }_k^i$ are the Gaussian white noise.

3.4.1. 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.

$$\left\{\begin{array}{c}\delta {\dot{\mathit{\phi }}}^{n,1}=-{\mathit{C}}_b^n{\mathit{\epsilon }}_r^{b,1}-{\mathit{C}}_b^n{\mathit{\omega }}_g^{b,1}\hfill \\ {\dot{\mathit{\epsilon }}}_r^{b,1}=-\frac{1}{{\tau }_g}{\mathit{\epsilon }}_r^{b,1}+{\mathit{\omega }}_{gr}^{b,1}\hfill \end{array}\right.$$

(18)

where $\delta {\mathit{\phi }}^{n,1}$ is the attitude error in the navigation system, ${\mathit{\omega }}_g^{b,1}$ is the Gaussian white noise of the gyroscope in the body system, and ${\mathit{\epsilon }}_r^{b,1}$ is the bias of the gyroscope in the body system.

The measurement function of the AHRS can be written by Equation (19).

$${\mathit{Z}}_k^1=\left\{\begin{array}{c}\left[\begin{array}{c}{\varphi }_k^a-{\widehat{\varphi }}_k\\ {\theta }_k^a-{\widehat{\theta }}_k\end{array}\right]+{\nu }_k^a\hfill \\ ({\psi }_k^m-{\widehat{\psi }}_k)+{\nu }_k^b\hfill \end{array}\right.$$

(19)

where ${\varphi }_k^a$ and ${\theta }_k^a$ are the roll and pitch calculated by the accelerometer, respectively; ${\psi }_k^m$ is the yaw calculated by the magnetometer; ${\widehat{\varphi }}_k$, ${\widehat{\theta }}_k$ and ${\widehat{\psi }}_k$ are the attitude update value form the gyroscope; ${\nu }_k^a$ is the Gaussian white noise of the measurement from the accelerometer, and ${\nu }_k^b$ is the Gaussian white noise of the measurement from magnetometer.

3.4.2. 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.

$$\left\{\begin{array}{c}\delta {\dot{\mathit{\phi }}}^{n,2}=-{\mathit{C}}_b^n{\mathit{\epsilon }}_r^{b,2}-{\mathit{C}}_b^n{\mathit{\omega }}_g^{b,2}\hfill \\ {\dot{\mathit{\epsilon }}}_r^{b,2}=-\frac{1}{{\tau }_g}{\mathit{\epsilon }}_r^{b,2}+{\mathit{\omega }}_{gr}^{b,2}\hfill \\ \delta {\dot{\mathit{v}}}^{n,2}={\mathit{C}}_b^n{\mathit{f}}^{b,2}\times \delta {\mathit{\phi }}^{n,2}+{\mathit{C}}_b^n{▽}_r^{b,2}+{\mathit{C}}_b^n{\mathit{\omega }}_a^{b,2}\hfill \\ \delta {\dot{\mathit{p}}}^{n,2}=\delta {\mathit{v}}^{n,2}\hfill \\ {\dot{▽}}_r^{b,2}=-\frac{1}{{\tau }_a}{▽}_r^{b,2}+{\mathit{\omega }}_{ar}^{b,2}\hfill \end{array}\right.$$

(20)

where $\delta {\mathit{\phi }}^{n,2}$, $\delta {\mathit{v}}^{n,2}$, and $\delta {\mathit{p}}^{n,2}$ are the errors for attitude, velocity, and position, respectively; ${\mathit{\epsilon }}_r^{b,2}$ and ${▽}_r^{b,2}$ are the bias of the gyroscope and accelerometer, respectively; and ${\mathit{\omega }}_{gr}^{b,2}$ and ${\mathit{\omega }}_{ar}^{b,2}$ are the Gaussian white noise of the gyroscope and accelerometer, respectively.

The measurement function of the SINS/GPS can be established by Equation (21).

$${\mathit{Z}}_k^2=\left[\begin{array}{c}{\mathit{p}}_k^a-{\widehat{\mathit{p}}}_k\\ {\mathit{v}}_k^a-{\widehat{\mathit{v}}}_k\end{array}\right]+{\nu }_k^c$$

(21)

where ${\mathit{p}}_k^a$ and ${\mathit{v}}_k^a$ are the velocity and position of the GPS, respectively; ${\widehat{\mathit{p}}}_k$ and ${\widehat{\mathit{v}}}_k$ are the velocity and position of SINS, respectively; ${\nu }_k^c$ is the Gaussian white noise of the GPS.

3.5. Common State Fusion

The common state parameters of the nonlinear double model, which are the error of attitude $\delta {\mathit{\phi }}^n$ and gyroscope bias ${\mathit{\epsilon }}_r^b$, 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.

$$X^A=\left[\begin{array}{cc}\delta {\mathit{\phi }}^{n,1}& {\mathit{\epsilon }}_r^{b,1}\end{array}\right]$$

(22)

$$X^B=\left[\begin{array}{ccccc}\delta {\mathit{\phi }}^{n,2}& {\mathit{\epsilon }}_r^{b,2}& \delta {\mathit{v}}^{n,2}& \delta {\mathit{p}}^{n,2}& {▽}_r^{b,2}\end{array}\right]$$

(23)

$$X^C=\left[\begin{array}{c}\delta {\mathit{\phi }}^n\\ {\mathit{\epsilon }}_r^b\end{array}\right]$$

(24)

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).

$$\left\{\begin{array}{c}{X}^C=P^C({\left(P^A\right)}^{-1}+{\left(P_{[1:6,1:6]}^B\right)}^{-1}{X}_{[1:6]}^B))\hfill \\ P^C={({\left(P^A\right)}^{-1}+{\left(P_{[1:6,1:6]}^B\right)}^{-1})}^{-1}\hfill \end{array}\right.$$

(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_{[1:6]}^B$ is the error of attitude and gyroscope bias; and $P_{[1:6,1:6]}^B$ 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.

$$X=\left[\begin{array}{c}{X}^C\\ X_{[7:15]}^B\end{array}\right]$$

(26)

where $X_{[7:15]}^B$ is the unique state quantity which includes the velocity error, position error, and accelerometer bias.

3.6. 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).

$$\left\{\begin{array}{c}{\varphi }_a=atan2(-f_y^b,-f_z^b)\hfill \\ {\theta }_a=atan2(f_x^b,-f_z^b)\hfill \end{array}\right.$$

(27)

$$\left\{\begin{array}{c}hx=mxcos{\theta }_a+mysin{\theta }_a+mzsin{\theta }_{a}cos{\varphi }_a\hfill \\ hy=mycos{\varphi }_a-mzsin{\varphi }_a\hfill \\ {\psi }_m=atan2(-hy,hx)\hfill \end{array}\right.$$

(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 ${\mathit{C}}_b^n$, 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 $\widehat{\mathit{v}}$ and position $\widehat{\mathit{p}}$. The longitude, latitude, and height of GPS data is calculated using Equation (29) to obtain the position $\mathit{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 $\mathit{v}$ in the navigation system. Then, the error measurement of the velocity and position are calculated using the $(\widehat{\mathit{v}}-\mathit{v})$ and $(\widehat{\mathit{p}}-\mathit{p})$. Finally, the local EKF is used to calculate the error of the attitude, velocity, and position, respectively.

$$\left\{\begin{array}{c}{\mathit{p}}^N=R^e\Delta \lambda \backslash 100\hfill \\ {\mathit{p}}^E=R^{e}cos\left(L_0\frac{\pi }{180}\right)\Delta \lambda \backslash 100\hfill \\ {\mathit{p}}^D=\Delta h\hfill \end{array}\right.$$

(29)

$$\left\{\begin{array}{c}{\mathit{v}}^N=Gndspd\ast cos(coureDed\ast \frac{\pi }{180})\hfill \\ {\mathit{v}}^E=Gndspd\ast sin(coureDed\ast \frac{\pi }{180}),\hfill \\ {\mathit{v}}^D=v^z\hfill \end{array}\right.$$

(30)

where $R^e$ is the Earth’s radius (about 6,378,145 m), $\Delta \lambda$ and $\Delta 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.

4. Simulation and Experimental Test

4.1. 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.

4.2. 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. Mean absolute errors (MAE), standard deviations (STD), and root mean square errors (RMSE) of the attitude achieved by multisensor complementary filtering (MSCF), multisensor EKF (MSEKF), and the federated double-model EKF (FDMEKF) for the experimental case.

Methods		Attitude (deg)
		Roll	Pitch	Yaw
MSCF	MAE	1.702	1.985	8.065
	STD	2.183	1.706	8.560
	RMSE	1.822	1.434	3.984
MSEKF	MAE	1.651	1.824	8.008
	STD	2.032	1.673	8.260
	RMSE	1.437	1.265	3.460
FDMEKF	MAE	1.564	1.528	7.897
	STD	1.921	1.504	8.417
	RMSE	1.336	1.022	2.817

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. MAE, STD, and RMSE of the velocity achieved by MSCF, MSEKF, and FDMEKF for the experimental case.

Methods		Velocity (m/s)
		VN	VE	VD
MSCF	MAE	0.633	0.786	0.398
	STD	0.879	1.093	0.535
	RMSE	0.641	0.907	0.695
MSEKF	MAE	0.534	0.465	0.325
	STD	0.859	0.829	0.495
	RMSE	0.576	0.716	0.585
FDMEKF	MAE	0.492	0.435	0.309
	STD	0.572	0.529	0.246
	RMSE	0.314	0.476	0.447

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. MAE, STD, and RMSE of the position achieved by MSCF, MSEKF, and FDMEKF for the experimental case.

Methods		Position (m)
		PN	PE	PD
MSCF	MAE	0.940	0.836	1.209
	STD	1.879	1.103	1.535
	RMSE	0.566	0.873	0.672
MSEKF	MAE	0.724	0.617	0.876
	STD	0.959	0.729	0.995
	RMSE	0.361	0.702	0.581
FDMEKF	MAE	0.692	0.531	0.601
	STD	0.658	0.503	0.691
	RMSE	0.223	0.523	0.512

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.

4.3. 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. MAE, STD, and RMSE of the attitude achieved by MSCF, MSEKF, and FDMEKF for the experimental case.

Methods		Attitude (deg)
		Roll	Pitch	Yaw
MSCF	MAE	3.698	2.739	11.135
	STD	4.733	3.449	14.197
	RMSE	4.097	2.557	7.340
MSEKF	MAE	4.133	3.300	11.148
	STD	5.265	4.235	13.761
	RMSE	0.885	2.549	7.982
FDMEKF	MAE	0.970	0.452	1.565
	STD	2.249	1.508	2.495
	RMSE	0.543	0.689	1.231

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.

5. 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.