Attitude anti-interference federal filtering algorithm for MEMS-SINS/GPS/magnetometer/SV integrated navigation system

Abstract

The attitude measurement systems composed of magnetic, angular rate and gravity sensors in the navigation system of an unmanned aerial vehicle are vulnerable to disturbed magnetic field observations. In this paper, an anti-interference integrated navigation federal Kalman filter is designed with fusing the information of microelectromechanical-system-based strapdown inertial navigation system, gravity field, geomagnetic field and solar vector (direction of sun). The filter works in the mode of fusion reset or no reset according to environmental conditions or manual setting. In addition, the characteristic is discovered that disturbed magnetic field observations can interfere with the state estimation covariance matrixes of quaternions, which in turn affect the fusion update of estimated quaternions in no reset mode of the federal filter. In order to eliminate the effect of magnetic field observations on the attitude information of pitch and roll angles, a method of correction update for a yaw angle component in quaternions using the magnetometer is designed, and an Euler angle information fusion method is developed by constructing a pseudo-covariance matrix of Euler angels in fusion update of the filter. Thus, when there exist disturbed magnetic field observations and the filter is in no reset mode, the estimated pitch and roll angles of the main filter after fusion update are not affected, and the yaw angle estimation errors of the main filter decrease relative to the estimation errors of the sub-filter which is corrected by a magnetometer. The effectiveness of the algorithm is illustrated by a numerical simulation and a semi-physical simulation.

Introduction

For small and medium-sized unmanned aerial vehicles (UAVs), improving the accuracy and stability of a navigation system using relatively low-cost sensors is one of the crucial issues to be considered in the design of an integrated navigation system. The magnetic, angular rate and gravity (MARG) sensors including three-axis microelectromechanical system (MEMS) gyroscope, accelerometer and magnetometer are widely used for attitude determination of UAVs or other vehicles.^1–4 In addition, a MEMS assisted with vision sensors has also been used for real-time ego-motion tracking.^5,6 The MEMS gyroscope and accelerometer constitute an inertial measurement unit (IMU) which measures the angular rate and acceleration of a UAV, and the accelerometer is also used to sense the gravity field at the same time. The magnetometer is used to measure the geomagnetic field. However, accelerometers and magnetometers are sensitive to body vibration and disturbing magnetic field, respectively, in the process of measuring gravity field and geomagnetic field. Moreover, the problems of nonlinear energy and vibration have been studied in Zhang and colleagues.^7–10 Thus, it is necessary to develop the anti-interference attitude estimation method for navigation systems.

In the paper of Hemerly et al., ¹¹ an extended Kalman filter for implementing an attitude and heading reference system (AHRS) with acceleration compensation by global positioning system (GPS) was introduced. In the filter, the derivative of the velocity obtained by the GPS is removed from the acceleration measured by the accelerometer which is also used to sense the gravity field. For exploring anti-interference attitude measurement methods, an improved antenna-array-based communication system ¹² was applied for attitude estimation by computing the line-of-sight (LOS) path between the base station and UAV. In addition, a complete framework for attitude estimation by exploiting the three-dimensional (3D) LOS vector obtained from the antenna array system was proposed. In order to improve the capability of resisting harmful acceleration, an attitude estimation method which is robust for harmful accelerative environment was designed using MARG sensors and GPS receiver in the work of No et al. ¹³ The navigation system excludes accelerometer and estimates pseudo-attitude information from the velocity measured by the GPS receiver. In addition, a magnetic pitch angle determination method which extracts the pitch angle and yaw angle information from the magnetic vector was proposed in order to complement the deviation of pseudo-attitude. The paper of Tong et al. ¹⁴ proposed a novel adaptive extended Kalman filter which includes a multiplicative extended Kalman filter (MEKF) and a hidden Markov model (HMM) recognizer. What is more, the HMM recognizer can obtain precise attitude information using MARG sensors in the circumstance of 3D motion tracking.

Usually, the disturbed magnetic field observations can affect not only the estimation of yaw component but also that of roll and pitch components in quaternions when using Kalman filter for state estimation of quaternions with MARG sensors. To solve this problem, a factored quaternion algorithm (FQA) ¹⁵ was proposed, in which the yaw angle component is extracted from the quaternions. Moreover, in the paper of Zhang and Nie, ¹⁶ a Kalman filter based on FQA was designed. The filter eliminates the effect on roll and pitch angle components in quaternions caused by magnetic field observations. What is more, complementary filters for attitude estimation using a magnetometer have the property that disturbed magnetic field observations have no effect on attitude estimation of pitch and roll angles. A novel quaternion-based fast complementary filter for attitude estimation that fuses gyroscope and accelerometer together was proposed. ¹⁷ In the novel complementary filter, the accelerometer-based attitude determination is transformed into a linear system, and the linear system is studied through control theory. The solution of the linear system can be computed with only one matrix multiplication, and thus the filter is fast. In addition, the magnetic output is fused with gravity estimated by a second complementary filter using gyroscope and accelerometer when there is no significantly disturbing magnetic field being imposed on the magnetometer.

In recent years, the methods for obtaining 3D attitude information of vehicles using atmospheric polarization modes have achieved fruitful results.^18–20 The information of atmospheric polarization modes can also be obtained in the fields of view obscured by barriers. Thus, the method is more suitable for a navigation system to measure attitude information of UAVs. In addition, the system using this method also belongs to the celestial navigation system (CNS), the theories and applications of which mature gradually.^21,22 Moreover, in the paper of Yang et al., ²³ multiple polarization sensors and a solar vector (SV) measurement algorithm were designed. The algorithm extracts the SV information from atmospheric polarization using the method of least squares.

In the problem of attitude estimation of a UAV using Kalman filter, the interference of body vibration and linear acceleration on the gravity field measured by an accelerometer can be reduced by constructing adaptive observation noise variance matrixes.^14,16 For the interference of disturbed magnetic field observations, an adaptive observation noise variance matrix of the magnetometer can be constructed for attitude estimation, but the disturbed magnetic field observations still affect the whole attitude estimation. The Kalman filter based on FQA can eliminate the interference to roll and pitch angle components in quaternions caused by disturbed magnetic field observations. But redundant sensors are not introduced which are used to assist the microelectromechanical-system-based strapdown inertial navigation system (MEMS-SINS) in the filter, and the estimated yaw angle will also be severely affected. Thus, it is necessary to develop an anti-magnetic field federal filtering algorithm with redundant sensors.

Considering that the attitude information obtained from atmospheric polarization modes measured by multiple polarization sensors is relived from the interference of body vibration and disturbing magnetic field, this paper proposes a federal Kalman filter that fuses MEMS-SINS, GPS, magnetometer and SV to estimate the state of UAV. In addition, the disturbed magnetic field observations affect the fusion update of state estimation in no reset (NR) mode of the filter through the state estimation covariance matrix of quaternions. In order to eliminate the effect of disturbed magnetic field observations on the attitude information of pitch and roll angles, we propose a method of correction update for yaw angle component in quaternions using the magnetometer. Moreover, an Euler angle information fusion method in fusion update of the filter is proposed. Thus, these methods improve the anti-interference, especially anti-magnetic field interference, capability of attitude estimation in the navigation system of a UAV.

Attitude anti-interference integrated navigation system model

The relationship between two coordinate frames referred to as “B” and “N” is used to represent the attitude of vehicles:

1. “B” represents the body-fixed frame which is a sensor frame with each axis pointing along each of the IMU axes.

2. “N” represents the local earth-fixed reference coordinate frame which is taken as the east-north-up reference system. The x, y and z axes are positive to the geographic east, north and up, respectively.

Thus, suppose that $R_{\mathrm{N}}^{\mathrm{B}}$ and $R_{\mathrm{B}}^{\mathrm{N}}$ are the rotation matrixes constructed by the quaternions of the real attitudes of the UAV dynamic model in the numerical simulation. Correspondingly, the rotation matrixes $C_{\mathrm{N}}^{\mathrm{B}}$ and $C_{\mathrm{B}}^{\mathrm{N}}$ are constructed using the estimated quaternions of the filter.

The model of SV in the navigation coordinate frame

When the earth rotates around the sun, the angle between the line which connects the earth and the sun and the equatorial plane of the earth will vary. The rotation of the earth causes the point of direct sunlight to move around the surface of the earth. In order to clearly construct the model of SV in the navigation coordinate frame, assume that the sun rotates around the earth. Thus, the SV in the geocentric inertial coordinate frame $O{X}_{\mathrm{I}}{Y}_{\mathrm{I}}{Z}_{\mathrm{I}}$ is depicted in Figure 1, and the model of unit SV in the geocentric inertial coordinate frame is

$$s_{\mathrm{I}}=\frac{1}{1-e\cos E}{R}_x(-i_s)R_z(-{\omega }_s)s_o$$

(1)

where $s_o=[\begin{array}{ccc}\cos E-e\hfill & \sin E\sqrt{(1-e^2)}\hfill & 0\hfill \end{array}{]}^{\mathrm{T}}$ , $i_s$ is the obliquity of the ecliptic, ${\omega }_s$ is the argument of perihelion of the earth revolution orbit, E is the eccentric anomaly of the earth revolution orbit, e is the eccentricity of the earth revolution orbit and $R(·)$ represents the rotation matrix of the rotating corresponding angle around the corresponding axis.

According to Kepler equation

$$E-e\sin E=M={\mathrm{\Omega }}_s(t-t_s)$$

(2)

the real-time solution of the eccentric anomaly E can be obtained, where M is the mean anomaly of the earth revolution orbit, $t_s$ is the moment when the earth passes the perihelion and ${\mathrm{\Omega }}_s$ is the earth revolution angular velocity.

Assume that the latitude and longitude of the local position are $l_a$ and $l_o$ , respectively. Thus, the model of unit SV in the “N” coordinate frame is

$$s_{\mathrm{N}}=R_y\left(\frac{\pi }{2}\right)R_x\left(\frac{\pi }{2}\right)R_y(-l_a)R_z(l_o+{\mathrm{\Omega }}_{e}t)s_{\mathrm{I}}$$

(3)

where ${\mathrm{\Omega }}_e$ is the earth rotation angular velocity. Thus, as long as the current date and time as well as the latitude and longitude are known, the unit SV in the navigation frame can be obtained.

Figure 1.

The solar vector in the geocentric inertial coordinate frame.

Establishment of the sensor model

According to the work of Yang et al. ²³ who constructed multiple polarization sensors to measure the SV, the SV model based on multiple polarization sensors in the “B” coordinate frame is constructed in this paper. In addition, the number of polarization sensors is selected as 5 for constructing the SV model; the coordinate frame of the polarization sensor is defined as “i.” Thus, the rotation matrix from the “B” coordinate frame to the “i” coordinate frame of polarization sensor i is represented by

$$R_{\mathrm{B}}^{\mathrm{i}}=R_y\left({\alpha }_i\right)R_z\left({\lambda }_i\right)$$

(4)

where the incidence angle ${\alpha }_i(i=1,\dots ,5)$ is taken as $\pi /3$ and the incidence angle ${\lambda }_i(i=1,\dots ,5)$ is taken as $0,\pi /4,\pi /2,\pi \mathrm{and}3\pi /2$ , respectively, in this paper.

The SV in the coordinate frame of polarization sensor i is $s_{\mathrm{i}}=R_{\mathrm{B}}^{\mathrm{i}}{R}_{\mathrm{N}}^{\mathrm{B}}{s}_{\mathrm{N}}$ . Thus, the polarization azimuth and the angle between incident light and scattered light measured by polarization sensor i are

$$\{\begin{array}{c}{\phi }_i=\arctan \frac{s_{\mathrm{i}}\left(2\right)}{s_{\mathrm{i}}\left(1\right)}\hfill \\ \multicolumn{1}{c}{{\zeta }_i=\arctan \frac{\sqrt{s_{\mathrm{i}}^2\left(1\right)+s_{\mathrm{i}}^2\left(2\right)}}{s_{\mathrm{i}}^2\left(3\right)}}\end{array}$$

(5)

The polarization degree of scattered light for polarization sensor i is

$$d=\frac{1-{\cos }^2{\zeta }_i}{1+{\cos }^2{\zeta }_i}{d}_{\max }$$

(6)

where $d_{\max }$ represents the maximum polarization degree which is taken as 1 in this paper.

According to the design principle of the polarization sensor, the two-channel signal outputs of the model of polarization sensor i are

$$\{\begin{array}{c}{U}_{i,1}=0.5\log \frac{1-d·\cos 2{\phi }_i}{1+d·\cos 2{\phi }_i}+n_{i,1}^u\hfill \\ \multicolumn{1}{c}{U_{i,2}=0.5\log \frac{1-d·\cos 2({\phi }_i+\pi /4)}{1+d·\cos 2({\phi }_i+\pi /4)}+n_{i,2}^u}\end{array}$$

(7)

where $n_{i,1}^u$ and $n_{i,2}^u$ represent the random white noise drifts with variance ${\sigma }_u^2$ .

According to the outputs $U_{i,1}$ and $U_{i,2}$ of polarization sensor i, the polarization azimuth can be obtained

$${\phi }_i^m=\frac{1}{2}\arctan (\cot \frac{\pi }{2}-\frac{1+{10}^{2U_{i,1}}}{1+{10}^{2U_{i,2}}}·\frac{1-{10}^{2U_{i,2}}}{1-{10}^{2U_{i,1}}}\csc \frac{\pi }{2})$$

(8)

Thus, the maximum polarization direction in the “i” coordinate frame of polarization sensor i is

$$u_i={\left[\begin{array}{ccc}\cos {\phi }_i^m\hfill & \sin {\phi }_i^m\hfill & 0\hfill \end{array}\right]}^{\mathrm{T}}$$

(9)

In addition, $u_i$ is expressed as $u_{\mathrm{B},i}=(R_{\mathrm{B}}^{\mathrm{i}}{)}^{-1}{u}_i$ in the “B” coordinate frame.

Assume that the SV is not in the XY plane of the “N” coordinate frame and the projection of the SV in the z-axis direction of the “B” coordinate frame is 1. According to the fact that the SV is perpendicular to the direction of maximum polarization, the least-squares solution of the SV in the x- and y-axis directions is

$${\left[\begin{array}{cc}{x}_s\hfill & y_s\hfill \end{array}\right]}^{\mathrm{T}}={\left(A_s^{\mathrm{T}}{A}_s\right)}^{-1}{A}_s^{\mathrm{T}}{b}_s$$

(10)

where $A_s$ and $b_s$ satisfy

$$\{\begin{array}{c}{A}_s=\left[\begin{array}{cc}{u}_{\mathrm{B},1}\left(1\right)\hfill & u_{\mathrm{B},1}(2)\hfill \\ \multicolumn{1}{c}{u_{\mathrm{B},2}\left(1\right)}& u_{\mathrm{B},2}\left(2\right)\hfill \\ \multicolumn{1}{c}{⋮}& ⋮\hfill \\ \multicolumn{1}{c}{u_{\mathrm{B},5}\left(1\right)}& u_{\mathrm{B},5}\left(2\right)\hfill \end{array}\right]\hfill \\ \multicolumn{1}{c}{b_s=-{\left[\begin{array}{cccc}{u}_{\mathrm{B},1}\left(3\right)\hfill & u_{\mathrm{B},2}\left(3\right)\hfill & \cdots \hfill & u_{\mathrm{B},5}\left(3\right)\hfill \end{array}\right]}^{\mathrm{T}}}\end{array}$$

(11)

Thus, the model of the measured unit SV is

$$s_{\mathrm{B}}=\frac{{\left[\begin{array}{ccc}{x}_s\hfill & y_s\hfill & 1\hfill \end{array}\right]}^{\mathrm{T}}}{\sqrt{x_s^2+y_s^2+1}}$$

(12)

It is assumed that the IMU sensors have been calibrated and the magnetometer has also been ellipsoidal corrected. Thus, the models of gyroscope, accelerometer and magnetometer can be established as follows

$$\{\begin{array}{c}{\omega }_{\mathrm{B}}={\omega }_t+{\epsilon }_r^w+n_w\hfill \\ \multicolumn{1}{c}{a_{\mathrm{B}}=R_{\mathrm{N}}^{\mathrm{B}}{g}_{\mathrm{N}}+a_v+b_a+{\epsilon }_r^a+n_a}\\ \multicolumn{1}{c}{m_{\mathrm{B}}=R_{\mathrm{N}}^{\mathrm{B}}(h_{\mathrm{N}}+h_m)+n_m}\end{array}$$

(13)

where ${\omega }_{\mathrm{B}}$ , $a_{\mathrm{B}}$ and $m_{\mathrm{B}}$ represent the measuring outputs of gyroscope, accelerometer and magnetometer in the “B” coordinate frame, respectively; ${\omega }_t$ represent the actual angular velocity of the UAV dynamic model; $g_{\mathrm{N}}$ , $h_{\mathrm{N}}$ and $h_m$ represent the gravity field in the “N” coordinate frame, the theoretical value of the local geomagnetic field and disturbing magnetic field being imposed on the magnetometer, respectively; $a_v$ and $b_a$ represent the vehicle linear acceleration and the bias of the accelerometer obtained by the accelerometer calibration; $n_w$ , $n_a$ and $n_m$ represent the random white noise drift vectors of gyroscope, accelerometer and magnetometer, respectively; ${\epsilon }_r^w$ and ${\epsilon }_r^a$ are the first-order Markov process drifts of the gyroscope and accelerometer, respectively. The dynamic models of ${\epsilon }_r^w$ and ${\epsilon }_r^a$ are as follows

$$\{\begin{array}{c}{\stackrel{·}{\epsilon }}_r^w=-\frac{1}{T_w}{\epsilon }_r^w+w_r\hfill \\ \multicolumn{1}{c}{{\stackrel{·}{\epsilon }}_r^a=-\frac{1}{T_a}{\epsilon }_r^a+w_a}\end{array}$$

(14)

where $T_w$ and $T_a$ are the correlation times and $w_r$ and $w_a$ are the random white noise drifts of the first-order Markov process.

The position and velocity model in the “N” coordinate frame obtained by the GPS is established as follows

$$\{\begin{array}{c}{p}^G=p_t^G+n_p\hfill \\ \multicolumn{1}{c}{v^G=v_t^G+n_v}\end{array}$$

(15)

where $p_t^G$ and $V_t^G$ represent the actual position and velocity of the UAV dynamic model in the “N” coordinate frame and $n_p$ and $n_v$ are the random white noise drift vectors with variances ${\sigma }_p^2$ and ${\sigma }_v^2$ , respectively.

System structure design of the filter

The federal filter of MEMS-SINS/GPS/magnetometer/SV integrated navigation is designed and the structure of the filter is shown in Figure 2. Sub-filter 1 assists MEMS-SINS to estimate positions, velocities and attitudes; sub-filter 2 assists MEMS-SINS to estimate attitudes only. The attitude estimation information of all sub-filters is fused in the main filter. In addition, the estimated yaw angle component in quaternions of sub-filter 1 is corrected by the magnetometer. Thus, the estimated attitude information of the main filter after fusion update combines the advantages of gravity field, geomagnetic field and SV. Under the circumstance that there is disturbing magnetic field being imposed on the magnetometer, it will affect the whole attitude estimation when the magnetic field measured by the magnetometer is directly used to assist the MEMS-SINS for attitude estimation. In addition, the yaw angle calculated by outputs of the magnetometer which has been compensated by the inclination angle of the UAV cannot be fused with the attitude quaternions estimated by sub-filter 2. What is more, the inclination angle of the UAV is defined as the angle between XY planes in the “B” coordinate frame and the “N” coordinate frame. For the two purposes that the filter can prevent the disturbed magnetic field observations from interfering the attitude estimation of pitch and roll angles and that the attitude estimations of sub-filters 1 and 2 can be fused, the yaw angle calculated by magnetometer outputs which have been compensated by inclination angle of UAV is corrected by the estimated quaternions of sub-filter 1.

Figure 2.

The structure of attitude anti-interference federal filter which fuses MEMS-SINS/GPS/magnetometer/SV.

The federal filter works in the fusion reset (FR) mode in order to improve the estimation accuracy when the environmental conditions are relatively ideal and the sensor noises are relatively small: The time update and the fusion update of attitude estimation are completed in the main filter; the main filter transmits the estimated information of attitude, position and velocity after time update to sub-filter 1 and transmits only attitude information after time update to sub-filter 2.

The federal filter works in the NR mode in order to improve the capability of fault tolerance when the environmental conditions are relatively poor or the sensor noises are relatively large: The time update of the filter is carried out in each sub-filter; the main filter only completes the fusion update of attitude through the Euler angle information fusion method proposed in this paper.

State equation of the UAV dynamic model

The motion states of the UAV usually include attitude quaternions, positions and velocities in the 3D space. Thus, the state vector of the UAV dynamic model is constructed as

$$x={\left[\begin{array}{ccc}{q}^{\mathrm{T}}\hfill & p^{\mathrm{T}}\hfill & v^{\mathrm{T}}\hfill \end{array}\right]}^{\mathrm{T}}$$

(16)

where q, p and v represent the states of attitude quaternions, positions and velocities, respectively.

In order to make the numerical simulation more close to the actual situation of hardware, the dynamic model of the UAV is established according to the measurement data of sensor models which have been established in equation (13). Thus, the dynamic model of attitude is as follows

$$\stackrel{·}{q}=\frac{1}{2}\mathrm{\Omega }q$$

(17)

where the $\mathrm{\Omega }$ is given by

$$\mathrm{\Omega }=\left[\begin{array}{cccc}0\hfill & -{\omega }_{\mathrm{B}}^x\hfill & -{\omega }_{\mathrm{B}}^y\hfill & -{\omega }_{\mathrm{B}}^z\hfill \\ \multicolumn{1}{c}{{\omega }_{\mathrm{B}}^x}& 0\hfill & {\omega }_{\mathrm{B}}^z\hfill & -{\omega }_{\mathrm{B}}^y\hfill \\ \multicolumn{1}{c}{{\omega }_{\mathrm{B}}^y}& -{\omega }_{\mathrm{B}}^z\hfill & 0\hfill & {\omega }_{\mathrm{B}}^x\hfill \\ \multicolumn{1}{c}{{\omega }_{\mathrm{B}}^z}& {\omega }_{\mathrm{B}}^y\hfill & -{\omega }_{\mathrm{B}}^x\hfill & 0\hfill \end{array}\right]$$

(18)

The linear acceleration of the UAV measured by the accelerometer model in the “N” coordinate frame is

$$a_{\mathrm{N}}=C_{\mathrm{B}}^{\mathrm{N}}(a_{\mathrm{B}}-b_a)-g_{\mathrm{N}}$$

(19)

which is taken as the input to the second-order integral dynamic model of the UAV. Thus, the second-order integral dynamic model is

$$\{\begin{array}{c}\stackrel{·}{p}=v\hfill \\ \multicolumn{1}{c}{\stackrel{·}{v}=a_{\mathrm{N}}}\end{array}$$

(20)

According to equations (17), (19) and (20), the state equation of the UAV based on sensor models can be obtained

$$\stackrel{·}{x}=\mathrm{Ax}+B{a}_{\mathrm{N}}$$

(21)

where

$$\{\begin{array}{c}A={\left[\begin{array}{ccc}1/2\mathrm{\Omega }\hfill & 0_{4\times 3}\hfill & 0_{4\times 3}\hfill \\ \multicolumn{1}{c}{0_{3\times 4}}& 0_{3\times 3}\hfill & I_{3\times 3}\hfill \\ \multicolumn{1}{c}{0_{3\times 4}}& 0_{3\times 3}\hfill & 0_{3\times 3}\hfill \end{array}\right]}_{10\times 10}\hfill \\ \multicolumn{1}{c}{B={\left[\begin{array}{c}{0}_{7\times 3}\hfill \\ \multicolumn{1}{c}{I_{3\times 3}}\end{array}\right]}_{10\times 3}}\end{array}$$

(22)

In addition, the process noise and input noise are introduced by the angular velocity measured by the gyroscope in system matrix A and input vector $a_{\mathrm{N}}$ , respectively.

In the FR mode of the filter, sub-filters obtain the attitude estimation information from the main filter after time update, while in the NR mode of the filter the time update of attitude estimation is carried out in each sub-filter.

Observation matrix of the state

In the filter designed in this paper, sub-filter 1 observes the gravity field vector and sub-filter 2 observes the SV and the gravity field vector simultaneously.

Observation matrix of sub-filter 1

The observed gravity field is

$$y_{1,k}\left(q\right)=C_{\mathrm{N}}^{\mathrm{B}}{g}_{\mathrm{N}}$$

(23)

Expand $y_{1,k}(q)$ by Taylor series at the state of quaternions. The observation matrix of the gravity field in the “B” coordinate frame can be obtained as

$$H_{g,k}={\frac{\partial y_{1,k}\left(q\right)}{\partial q}|}_{q={\hat{q}}_{k|k-1}}$$

(24)

where ${\hat{q}}_{k|k-1}$ represents the estimated quaternions after time update.

Since sub-filter 1 also measures the position and velocity information provided by the GPS, the observation matrix of sub-filter 1 can be constructed as

$$H_{1,k}={\left[\begin{array}{cc}{H}_{g,k}\hfill & 0_{3\times 6}\hfill \\ \multicolumn{1}{c}{0_{6\times 4}}& I_{6\times 6}\hfill \end{array}\right]}_{9\times 10}$$

(25)

Observation matrix of sub-filter 2

The observed SV and gravity field vector is

$$y_{2,k}\left(q\right)=\left[\begin{array}{cc}{C}_{\mathrm{N}}^{\mathrm{B}}\hfill & \\ & C_{\mathrm{N}}^{\mathrm{B}}\hfill \end{array}\right]\left[\begin{array}{c}{s}_{\mathrm{N}}\hfill \\ \multicolumn{1}{c}{g_{\mathrm{N}}}\end{array}\right]$$

(26)

Expand $y_{2,k}(q)$ by Taylor series at the state of quaternions. The observation matrix of sub-filter 2 can be obtained as

$$H_{2,k}={\frac{\partial y_{2,k}\left(q\right)}{\partial q}|}_{q={\hat{q}}_{k|k-1}}$$

(27)

Attitude anti-interference algorithm in the navigation system

In the algorithm, a method is developed that uses a magnetometer to correct the yaw angle component in estimated quaternions of sub-filter 1 with only the observation of the gravity field. Thus, the whole attitude estimation errors of sub-filter 1 converge to a certain range, and the attitude quaternions estimated by sub-filters can be fused in the main filter. In the NR mode of the filter, an Euler angle information fusion method is developed relative to the method of quaternions information fusion update. Thus, in the progress of information fusion update of the filter, this method avoids the estimated pitch and roll angles of the main filter after fusion update being interfered by disturbing the magnetic field being imposed on the magnetometer. Moreover, the estimated yaw angle of the main filter after fusion update has certain anti-magnetic interference capability.

The switch conditions of the FR and NR modes

Due to there existing a feedback structure, the estimation accuracy of each sub-filter will be improved, but its fault tolerance will be reduced in the FR mode of the filter. Moreover, although the estimation accuracy of each sub-filter is improved theoretically, it may actually be affected by the low-precision information fed back by the main filter owing to the larger noise of sensors in one sub-filter. Thus, it is necessary to manually set the mode of filter to FR or NR according to the accuracy of sensors.

Suppose the situation cannot take place that the magnetometer and multiple polarization sensors are unavailable at the same time. Define the marker variables $f_s$ and $f_m$ to indicate whether the multiple polarization sensors and magnetometer are available. The criterion for judging whether the multiple polarization sensors are available is

$$\{\begin{array}{c}{f}_s=0,U_{\min }⩽{\delta }_u\hfill \\ \multicolumn{1}{c}{f_s=1,U_{\min }>{\delta }_u}\end{array}$$

(28)

where $U_{\min }$ is the minimum of all signals output by multiple polarization sensors and ${\delta }_u$ is the threshold value given according to the practical situation. $f_s=0$ and $f_s=1$ denote the unavailability and availability of multiple polarization sensors, respectively. In the case of $f_s=0$ , the federal filter works in the NR mode and the estimated quaternions of sub-filter 1 corrected by the magnetometer are taken as the obtained attitude of the UAV.

As shown in Figure 3, the disturbing magnetic field being imposed on the magnetometer could lead to a change in the direction of the magnetic field $m_{\mathrm{B}}$ measured by the magnetometer, but the value of $||m_{\mathrm{B}}||$ remains within a certain range. Thus, under the condition of $f_s=1$ , the criterion for judging whether the magnetometer is available is designed as

$$f_m=1,\{\begin{array}{c}\Vert h_{\mathrm{N}}\Vert -{\delta }_m⩽\Vert m_{\mathrm{B}}\Vert ⩽\Vert h_{\mathrm{N}}\Vert +{\delta }_m\hfill \\ \multicolumn{1}{c}{|{\psi }_k^m-{\psi }_k^s|⩽{\delta }_a}\end{array}$$

(29)

where ${\delta }_m$ and ${\delta }_a$ are the threshold values given according to the practical situation, ${\psi }_k^m$ is the yaw angle calculated from the output of the magnetometer after inclination compensation and ${\psi }_k^s$ is the yaw angle transformed by estimated quaternions of sub-filter 2. $f_m=1$ indicates that the magnetometer is available. As long as the criterion is not satisfied, $f_m=0$ . If the condition $f_m=0$ is satisfied, the federal filter works in the NR mode and the estimated quaternions of sub-filter 2 are taken as the obtained attitude of the UAV.

Figure 3.

Disturbed magnetic field measured by the magnetometer caused by the disturbing magnetic field.

When the marker variable $f_s$ or $f_m$ varies from 0 to 1, there may exist the circumstance that the multiple polarization sensors or magnetometer is still unavailable, but no fault is detected or there is a delay. Thus, the federal filter also works in the NR mode and the attitude information fused in the main filter is taken as the obtained attitude of the UAV for improving the anti-interference capability of attitude estimation.

The algorithm in FR mode of the federal filter

The steps of the algorithm are as follows.

Time update

Since the state equation (21) is a continuous-time system, equation (21) is discretized by the fourth-order Runge–Kutta method in order to update the state in discrete time. Then the time update of the state can be obtained as

$$\{\begin{array}{c}{S}_1=A{\hat{x}}_{k-1}+B{a}_{\mathrm{N}}\hfill \\ \multicolumn{1}{c}{S_2=A({\hat{x}}_{k-1}+S_1{T}_h/2)+B{a}_{\mathrm{N}}}\\ \multicolumn{1}{c}{S_3=A({\hat{x}}_{k-1}+S_2{T}_h/2)+B{a}_{\mathrm{N}}}\\ \multicolumn{1}{c}{S_4=A({\hat{x}}_{k-1}+S_3{T}_h)+B{a}_{\mathrm{N}}}\end{array}$$

(30)

$$\{\begin{array}{c}\mathrm{dx}=\frac{(S_1+2S_2+2S_3+S_4)T_h}{6}\hfill \\ \multicolumn{1}{c}{{\hat{x}}_{k/k-1}={\hat{x}}_{k-1}+\mathrm{dx}}\end{array}$$

(31)

where $T_h$ is the sampling period, ${\hat{x}}_{k-1}$ is the state estimation at moment $k-1$ and ${\hat{x}}_{k/k-1}$ is the state estimation after time update.

The time update of the state estimation covariance matrix is

$$\{\begin{array}{c}\mathrm{dP}=(A{P}_{k-1}+P_{k-1}{A}^{\mathrm{T}}+B{R}_u{B}^{\mathrm{T}}+Q)T_h\hfill \\ \multicolumn{1}{c}{P_{k/k-1}=P_{k-1}+\mathrm{dP}}\end{array}$$

(32)

where $R_u$ is the input noise variance matrix and Q is the process noise variance matrix of state equation (21); $P_{k-1}$ is the state estimation covariance matrix at moment $k-1$ ; $P_{k/k-1}$ is the covariance matrix after time update.

Information distribution

$$\{\begin{array}{c}{\hat{x}}_{1,k/k-1}={\hat{x}}_{k/k-1}\hfill \\ \multicolumn{1}{c}{{\hat{x}}_{2,k/k-1}={\hat{x}}_{k/k-1}^q}\end{array}$$

(33)

$$\{\begin{array}{c}{P}_{1,k/k-1}^{-1}={\beta }_1{P}_{k/k-1}^{-1}\hfill \\ \multicolumn{1}{c}{P_{2,k/k-1}^{-1}={\beta }_2{(P_{k/k-1}^q)}^{-1}}\end{array}$$

(34)

where ${\hat{x}}_{k/k-1}^q$ and $P_{k/k-1}^q$ are the estimated quaternions and state estimation covariance matrix about quaternions of the filter after time update, respectively. The values of information distribution factors ${\beta }_1$ and ${\beta }_2$ satisfy their sum being equal to one. Considering that the sensors of sub-filters do not have the same characteristics, the method based on the trace of the covariance matrix is adopted to realize the distribution of ${\beta }_1$ and ${\beta }_2$ . The implementation of this method is as follows

$${\beta }_i=\frac{\mathrm{tr}\left(P_{i,k-1}\right)}{\sum _{i=1}^N\mathrm{tr}\left(P_{i,k-1}\right)}$$

(35)

where $\mathrm{tr}(·)$ represents the trace operation for matrixes.

Measurement update

$$\{\begin{array}{c}{K}_i=P_{i,k/k-1}{H}_{i,k}^{\mathrm{T}}{(H_{i,k}{P}_{i,k/k-1}{H}_{i,k}^{\mathrm{T}}+R_i)}^{-1}\hfill \\ \multicolumn{1}{c}{{\hat{x}}_{i,k}={\hat{x}}_{i,k/k-1}+K_i(Z_{i,k}-H_{i,k}{\hat{x}}_{i,k/k-1})}\\ \multicolumn{1}{c}{P_{i,k}=(I-K_i{H}_{i,k})P_{i,k/k-1}}\end{array}$$

(36)

where $R_i$ is the measurement noise variance matrix and $K_i$ is the Kalman filtering gain matrix. In addition, the measurement vectors are

$$\{\begin{array}{c}{Z}_{1,k}={\left[\begin{array}{ccc}{(a_{\mathrm{B}}-b_a-a_v)}^{\mathrm{T}}\hfill & {\left(p^G\right)}^{\mathrm{T}}\hfill & {\left(v^G\right)}^{\mathrm{T}}\hfill \end{array}\right]}^{\mathrm{T}}\hfill \\ \multicolumn{1}{c}{Z_{2,k}={\left[\begin{array}{cc}{\left(s_{\mathrm{B}}\right)}^{\mathrm{T}}\hfill & {(a_{\mathrm{B}}-b_a-a_v)}^{\mathrm{T}}\hfill \end{array}\right]}^{\mathrm{T}}}\end{array}$$

(37)

Correction update of yaw angle component

The magnetometer output after compensation by the inclination angle of the UAV is

$$m_c=C_{\mathrm{yx}}{m}_{\mathrm{B}}$$

(38)

where the matrix $C_{\mathrm{yx}}$ constructed by pitch and roll angles of the UAV is

$$\begin{array}{c}\multicolumn{1}{c}{C_{\mathrm{yx}}=C_{-y}\left({\hat{\theta }}_{k/k-1}^f\right)C_{-x}\left({\hat{\gamma }}_{k/k-1}^f\right)=\left[\begin{array}{ccc}\cos {\hat{\theta }}_{k/k-1}^f\hfill & \sin {\hat{\theta }}_{k/k-1}^f\sin {\hat{\gamma }}_{k/k-1}^f\hfill & \sin {\hat{\theta }}_{k/k-1}^f\cos {\hat{\gamma }}_{k/k-1}^f\hfill \\ \multicolumn{1}{c}{0}& \cos {\hat{\gamma }}_{k/k-1}^f\hfill & -\sin {\hat{\gamma }}_{k/k-1}^f\hfill \\ \multicolumn{1}{c}{-\sin {\hat{\theta }}_{k/k-1}^f}& \cos {\hat{\theta }}_{k/k-1}^f\sin {\hat{\gamma }}_{k/k-1}^f\hfill & \cos {\hat{\theta }}_{k/k-1}^f\cos {\hat{\gamma }}_{k/k-1}^f\hfill \end{array}\right]}\end{array}$$

(39)

where ${\hat{\gamma }}_{k/k-1}^f$ and ${\hat{\theta }}_{k/k-1}^f$ are, respectively, the roll and pitch angles transformed by the estimated quaternions of the main filter after time update.

Thus, the yaw angle calculated by the output of the magnetometer after inclination compensation is

$${\psi }_k^m=\arctan \frac{m_c\left(2\right)}{m_c\left(1\right)}+{\mathrm{\Gamma }}_k$$

(40)

where ${\mathrm{\Gamma }}_k$ is the geomagnetic declination of the local position.

In order to correct the estimated quaternions of sub-filter 1 using ${\psi }_k^m$ , calculate the difference between ${\psi }_k^m$ and the yaw angle ${\hat{\psi }}_{k/k-1}^f$ transformed by estimated quaternions of the main filter after time update

$${\psi }_k^e={\psi }_k^m-{\hat{\psi }}_{k/k-1}^f$$

(41)

According to the property of quaternions, an arbitrary rotation from frame F to frame G with the rotation axis e and the rotation angle $\alpha$ in the 3D space can be expressed as

$$\stackrel{G}{F}q={\left[\begin{array}{cccc}\cos \frac{\alpha }{2}\hfill & e_x\sin \frac{\alpha }{2}\hfill & e_y\sin \frac{\alpha }{2}\hfill & e_z\sin \frac{\alpha }{2}\hfill \end{array}\right]}^{\mathrm{T}}$$

(42)

Take the z-axis of the “B” coordinate frame as the rotation axis and ${\psi }_k^e$ as the rotation angle. Thus, the rotation quaternions of correction update under the circumstance of current attitude of the UAV can be constructed as

$$q_k^c={\left[\begin{array}{cccc}\cos \frac{{\psi }_k^e}{2}\hfill & 0\hfill & 0\hfill & \sin \frac{{\psi }_k^e}{2}\hfill \end{array}\right]}^{\mathrm{T}}$$

(43)

Accordingly, the correction update for estimated quaternions of sub-filter 1 is

$${\hat{x}}_{1c,k}^q=q_k^c\otimes {\hat{x}}_{1,k}^q=\left[\begin{array}{c}{q}_k^c(1){\hat{x}}_{1,k}^q\left(1\right)-q_k^c\left(2\right){\hat{x}}_{1,k}^q\left(2\right)-q_k^c\left(3\right){\hat{x}}_{1,k}^q\left(3\right)-q_k^c\left(4\right){\hat{x}}_{1,k}^q\left(4\right)\hfill \\ \multicolumn{1}{c}{q_k^c(1){\hat{x}}_{1,k}^q\left(2\right)+q_k^c\left(2\right){\hat{x}}_{1,k}^q\left(1\right)+q_k^c\left(3\right){\hat{x}}_{1,k}^q\left(4\right)-q_k^c\left(4\right){\hat{x}}_{1,k}^q\left(3\right)}\\ \multicolumn{1}{c}{q_k^c(1){\hat{x}}_{1,k}^q\left(3\right)-q_k^c\left(2\right){\hat{x}}_{1,k}^q(4)+q_k^c\left(3\right){\hat{x}}_{1,k}^q\left(1\right)+q_k^c\left(4\right){\hat{x}}_{1,k}^q\left(2\right)}\\ \multicolumn{1}{c}{q_k^c(1){\hat{x}}_{1,k}^q\left(4\right)+q_k^c\left(2\right){\hat{x}}_{1,k}^q\left(3\right)-q_k^c\left(3\right){\hat{x}}_{1,k}^q\left(2\right)+q_k^c\left(4\right){\hat{x}}_{1,k}^q\left(1\right)}\end{array}\right]$$

(44)

where ⊗ denotes the multiplication of quaternions, ${\hat{x}}_{1,k}^q$ is the state estimation about quaternions of sub-filter 1 and ${\hat{x}}_{1c,k}^q$ denotes attitude quaternions after correction update.

Accordingly, the yaw angle component in estimation quaternions obtained mainly by gyroscope integration is replaced by the yaw angle measured by the magnetometer. In addition, the primary diagonal elements of the quaternion state estimation covariance matrix $P_{1,k}^q$ which is part of $P_{1,k}$ converge to a smaller range which is mainly determined by the sensor noise as $t\to \infty$ . Thus, after the operation of ${\hat{x}}_{1c,k}^q=q_k^c\otimes {\hat{x}}_{1,k}^q$ , the state estimation covariance matrix $P_{1,k}^q$ corresponds to a more accurate state estimation ${\hat{x}}_{1c,k}^q$ in fusion update. Thus, the covariance matrix $P_{1,k}^q$ can be treated as unchanging for fusion update in the main filter after correction update of the yaw angle component in quaternions.

Information fusion update

The attitude information of sub-filters is fused as follows

$$\{\begin{array}{c}{P}_{f,k}^q={({\left(P_{1,k}^q\right)}^{-1}+P_{2,k}^{-1})}^{-1}\hfill \\ \multicolumn{1}{c}{{\hat{x}}_{f,k}^q=P_{f,k}^q({\left(P_{1,k}^q\right)}^{-1}{\hat{x}}_{1c,k}^q+P_{2,k}^{-1}{\hat{x}}_{2,k})}\end{array}$$

(45)

where $P_{f,k}^q$ and ${\hat{x}}_{f,k}^q$ are the estimation covariance matrix and estimated quaternions of the main filter after fusion update, respectively.

The algorithm in NR mode of the federal filter

According to the scheme of the federal filter in the NR mode, the following changes are made to the algorithm of federal filter in the FR mode:

1. The estimation information of the main filter after fusion update no longer resets the sub-filters, and the time update is carried out separately in each sub-filter. In addition, the Euler angles transformed by estimated quaternions of the main filter after time update in equations (39) and (41) are replaced by the corresponding Euler angles transformed by estimated quaternions in sub-filter 1. Thus, there is no cross-contamination between sub-filters.

2. The disturbed magnetic field observations will interfere with the estimated quaternions and the observation matrix $H_{g,k}$ of sub-filter 1 through the magnetometer. Accordingly, the estimated covariance matrix of attitude quaternions $P_{1,k}^q$ is interfered. Thus, the fusion update method of attitude quaternions shown in equation (45) will result in the pitch and roll components in quaternions being interfered by disturbed magnetic field observations after fusion update. In order to prevent the attitude information of pitch and roll angles from being interfered by disturbed magnetic field observations after fusion update of the main filter, the Euler angle information fusion method is developed.

Transform the estimated quaternions of sub-filter $i$ to Euler angles

$$\{\begin{array}{l}{\widehat{\psi }}_{i,k}=\arctan \left(\frac{2\left({\widehat{x}}_{i,k}\left(2\right){\widehat{x}}_{i,k}\left(3\right)+{\widehat{x}}_{i,k}\left(1\right){\widehat{x}}_{i,k}\left(4\right)\right)}{1-2\left({\widehat{x}}_{i,k}^2\left(3\right)+{\widehat{x}}_{i,k}^2\left(4\right)\right)}\right)\hfill \\ {\widehat{\theta }}_{i,k}=\arcsin \left(2\left({\widehat{x}}_{i,k}\left(1\right){\widehat{x}}_{i,k}\left(3\right)-{\widehat{x}}_{i,k}\left(2\right){\widehat{x}}_{i,k}\left(4\right)\right)\right)\hfill \\ {\widehat{\gamma }}_{i,k}=\arctan \left(\frac{2\left({\widehat{x}}_{i,k}\left(1\right){\widehat{x}}_{i,k}\left(2\right)+{\widehat{x}}_{i,k}\left(3\right){\widehat{x}}_{i,k}\left(4\right)\right)}{1-2\left({\widehat{x}}_{i,k}^2\left(2\right)+{\widehat{x}}_{i,k}^2\left(3\right)\right)}\right)\hfill \end{array}$$

(46)

where ${\hat{\psi }}_{i,k}$ , ${\hat{\theta }}_{i,k}$ and ${\hat{\gamma }}_{i,k}$ represent the estimated yaw, pitch and roll angles of sub-filter i, respectively. In addition, the vector of estimated Euler angles is constructed as

$${\hat{E}}_i={\left[\begin{array}{ccc}{\hat{\psi }}_{i,k}\hfill & {\hat{\theta }}_{i,k}\hfill & {\hat{\gamma }}_{i,k}\hfill \end{array}\right]}^{\mathrm{T}}$$

(47)

The inverse of the state estimation covariance matrix basically serves as the reliability evaluation matrix of state estimation in the progress of fusion update. In addition, the covariance matrix of estimated Euler angles cannot be obtained directly. Thus, a pseudo-estimation covariance matrix of estimated Euler angles of sub-filter i for fusion update is defined as follows

$$P_{i,k}^E=\left[\begin{array}{ccc}{P}_{i,k}^{\psi }\hfill & & \\ & P_{i,k}^{\theta }\hfill & \\ & & P_{i,k}^{\gamma }\hfill \end{array}\right]$$

(48)

where $P_{i,k}^{\theta }$ , $P_{i,k}^{\theta }$ and $P_{i,k}^{\gamma }$ represent the pseudo-estimation mean square errors of yaw, pitch and roll angles, respectively, of sub-filter i.

Considering the conversion equations of attitude quaternions and Euler angles (equation (46)), the increase of the corresponding estimation errors of the quaternion will lead to the increase in Euler angle errors. According to this property, the pseudo-estimation mean square errors of Euler angles are constructed as

$$\{\begin{array}{c}\multicolumn{1}{c}{P_{i,k}^{\psi }=\frac{P_{i,k}(1,1)+P_{i,k}(2,2)+P_{i,k}(3,3)+P_{i,k}(4,4)}{1-P_{i,k}(3,3)-P_{i,k}(4,4)}}\\ \multicolumn{1}{c}{P_{i,k}^{\theta }=P_{i,k}(1,1)+P_{i,k}(2,2)+P_{i,k}(3,3)+P_{i,k}(4,4)}\\ \multicolumn{1}{c}{P_{i,k}^{\gamma }=\frac{P_{i,k}(1,1)+P_{i,k}(2,2)+P_{i,k}(3,3)+P_{i,k}(4,4)}{1-P_{i,k}(2,2)-P_{i,k}(3,3)}}\end{array}$$

(49)

Certainly, equation (49) is one of many forms of pseudo-estimation mean square errors of Euler angles.

Then the information fusion update of Euler angles is as follows

$$\{\begin{array}{c}{P}_k^E={({\left(P_{1,k}^E\right)}^{-1}+{\left(P_{2,k}^E\right)}^{-1})}^{-1}\hfill \\ \multicolumn{1}{c}{{\hat{E}}_f=P_k^E({\left(P_{1,k}^E\right)}^{-1}{\hat{E}}_1+{\left(P_{2,k}^E\right)}^{-1}{\hat{E}}_2)}\end{array}$$

(50)

where $P_k^E$ is the pseudo-estimation covariance matrix of Euler angles after fusion update and ${\hat{E}}_f$ is the vector of estimated Euler angles after fusion update.

Numerical simulation and semi-physical simulation

Numerical simulation

The algorithm designed in this paper is simulated by MATLAB. The simulation step size is given as $\Delta t={10}^{-3}\mathrm{s}$ . Suppose the latitude and longitude of the local position are $l_a=40°$ and $l_o=123°$ , respectively. In addition, assume that the UAV is cruising in a small range, and the latitude and longitude are treated as unchanged. The numerical simulation time is given as 500 s. The initial moment of the simulation is given as 14:00 on 1 June. The unit gravity field vector and the unit geomagnetic field vector are given as $g_{\mathrm{N}}=[\begin{array}{ccc}0\hfill & 0\hfill & 1\hfill \end{array}{]}^{\mathrm{T}}$ and $h_{\mathrm{N}}=[\begin{array}{ccc}1\hfill & 0\hfill & 0\hfill \end{array}{]}^{\mathrm{T}}$ , respectively. The magnetic declination and inclination of the geomagnetic field are assumed to be zero. The standard deviations of random white noise of multiple polarization sensors and the magnetometer are given as ${\sigma }_u=0.01$ and ${\sigma }_m=0.003$ , respectively. The random white noise and first-order Markov process drifts of the gyroscope are 0.002 and 0.001 rad/s, respectively; the correlation time of the Markov process drift is taken as $T_w=7200/\mathrm{s}$ . The random white noise and first-order Markov process drifts of the accelerometer are 0.8 and 0.002G, respectively; the correlation time of the Markov process drift is taken as $T_w=3600/\mathrm{s}$ . The random white noise drifts of position and velocity of the GPS are given as 1 m and 0.1 m/s, respectively. The given UAV motion states include acceleration, turning and climbing. The numerical simulation is carried out in the FR and NR modes of the federal filter, respectively:

1. Manually set the filter’s work mode to FR. The multiple polarization sensors work normally and there are not disturbed magnetic field observations. The position and velocity estimation errors are shown in Figures 4 and 5. The attitude estimation errors of the main filter after fusion update are shown in Figure 6. The attitude estimation errors of MEMS-SINS are shown in Figure 7.

2. Manually set the filter’s work mode to NR. The multiple polarization sensors work normally. But in the 300th second of the simulation, there appear disturbed magnetic field observations as follows

$$h_m=\left[\begin{array}{c}-0.8\sin (\pi t/100)\hfill \\ \multicolumn{1}{c}{0.8+0.5\sin (\pi t/100)}\\ \multicolumn{1}{c}{0.8+0.5\sin (\pi t/100)}\end{array}\right]$$

(51)

Figure 4.

Position estimation errors of the filter in the X, Y and Z directions.

Figure 5.

Velocity estimation errors of the filter in the X, Y and Z directions.

Figure 6.

Estimation errors of yaw, pitch and roll angles of the filter after fusion update.

Figure 7.

Estimation errors of yaw, pitch and roll angles of MEMS-SINS.

The attitude estimation errors of sub-filter 1, sub-filter 2 and the main filter using the methods of quaternions information fusion and Euler angle information fusion, respectively, are shown in Figures 8–10.

Figure 8.

Yaw angle estimation errors of sub-filter 1, sub-filter 2 and the main filter in the NR mode of the filter.

Figure 9.

Pitch angle estimation errors of sub-filter 1, sub-filter 2 and the main filter in the NR mode of the filter.

Figure 10.

Roll angle estimation errors of sub-filter 1, sub-filter 2 and the main filter in the NR mode of the filter.

Semi-physical simulation

Because the MARG sensors in Pixhawk flight controller of a UAV which is shown in Figure 11 have been corrected, the data of MARG sensors are exported for the semi-physical simulation. The UAV with the Pixhawk flight controller is placed on the ground, and it is assumed that the UAV dynamic model in the numerical simulation is also in static state. The transmission speed of IMU and magnetometer data is changed to 1000 Hz by changing the Mavlink module of px4 source code of the Pixhawk flight controller. The IMU and magnetometer data are collected to log file through ground control station for more than 50 s and the log file is read through MATLAB for IMU and magnetometer data for 50 s. Since the attitude of the UAV may not be at a complete level, the collected acceleration data are subjected to zero offset processing. In the process of calculating yaw angle according to the magnetometer data, it should be compensated by the inclination angle calculated by the collected accelerometer data. Then the calculated yaw angle is used as the yaw angle of the UAV model in the numerical simulation. The collected IMU and magnetometer data are simulated together with the SV model based on multiple polarization sensors in the numerical simulation:

1. Manually set the filters’ work mode to NR. The multiple polarization sensors work normally. The attitude estimation errors of sub-filter 1, sub-filter 2 and the main filter using the methods of quaternions information fusion and Euler angle information fusion, respectively, are shown in Figures 12–14 under the condition of the absence of disturbed magnetic field observations.

2. Manually set the filters’ work mode to NR. The multiple polarization sensors work normally. The given disturbing magnetic field being imposed on the magnetometer is shown in Figure 15, where a magnet block with north and south poles is moving back and forth. The attitude estimation errors of sub-filter 1, sub-filter 2 and the main filter using the methods of quaternions information fusion and Euler angle information fusion, respectively, are shown in Figures 16–18 under the condition of disturbed magnetic field observations.

3. Manually set the filters’ work mode to FR. The multiple polarization sensors work normally. In addition, once the disturbing magnetic field is detected, the filter will work in the NR mode. The thresholds in equation (29) are given as ${\delta }_m=0.1$ and ${\delta }_a=2.5°$ . Under the circumstance of changing the federal filter work mode, the attitude estimation errors of sub-filter 1, sub-filter 2 and the main filter using the methods of quaternions information fusion and Euler angle information fusion, respectively, are shown in Figures 19–21. In the figures, the outputs of attitude information estimated by the method of Euler angle information fusion are zero when the filter works in the FR mode; the outputs of attitude information estimated by the method of quaternions information fusion are zero when the filter works in the NR mode.

Figure 11.

The UAV with Pixhawk flight controller.

Figure 12.

Yaw angle estimation errors of sub-filter 1, sub-filter 2 and the main filter in the NR mode of the filter without disturbed magnetic field observations.

Figure 13.

Pitch angle estimation errors of sub-filter 1, sub-filter 2 and the main filter in the NR mode of the filter without disturbed magnetic field observations.

Figure 14.

Roll angle estimation errors of sub-filter 1, sub-filter 2 and the main filter in the NR mode of the filter without disturbed magnetic field observations.

Figure 15.

The dynamic magnetic field distortion applied to the magnetometer of the UAV.

Figure 16.

Yaw angle estimation errors of sub-filter 1, sub-filter 2 and the main filter in the NR mode of the filter with disturbed magnetic field observations.

Figure 17.

Pitch angle estimation errors of sub-filter 1, sub-filter 2 and the main filter in the NR mode of the filter with disturbed magnetic field observations.

Figure 18.

Roll angle estimation errors of sub-filter 1, sub-filter 2 and the main filter in the NR mode of the filter with disturbed magnetic field observations.

Figure 19.

Yaw angle estimation errors of sub-filter 1, sub-filter 2 and the main filter under the circumstance of changing mode.

Figure 20.

Pitch angle estimation errors of sub-filter 1, sub-filter 2 and the main filter under the circumstance of changing mode.

Figure 21.

Roll angle estimation errors of sub-filter 1, sub-filter 2 and the main filter under the circumstance of changing mode.

Analysis of the simulations

According to the results of numerical simulation in Figures 4 and 5, the estimation errors of position and velocity converge to a relatively small range. It can be seen from Figure 6 that the estimation errors of yaw angle are basically within 0.2 degrees and the estimation errors of pitch and roll angles are basically within 0.5 degrees after fusion update of the filter. It can be seen from Figure 7 that the attitude estimation errors of MEMS-SINS are divergent. The results of numerical simulation and semi-physical simulation shown in Figures 8–10, 12–14 and 16–18 verify the correctness of theoretical analysis for the methods of yaw angle component correction update and Euler angle information fusion. Thus, the algorithm designed in this paper is effective for engineering practice.

According to the results shown in Figures 12–14, the methods of Euler angle information fusion and quaternions information fusion basically have the same estimation accuracy, and the estimation accuracy of the main filter after fusion update is higher than that of the sub-filter is lower. According to the results shown in Figures 8–10 and 16–18, when there is disturbing magnetic field being imposed on the magnetometer, the yaw angle estimation errors of the main filter after fusion update are almost half those of sub-filter 1; the pitch and roll angles estimated by sub-filters 1 and 2 are not affected by the disturbing magnetic field, but the pitch and roll angle components in estimated quaternions of the main filter after fusion update using the method of quaternions information fusion are affected seriously; however, the estimated pitch and roll angles of the main filter after fusion update using the method of Euler angle information fusion are not affected. Thus, the conclusion that disturbed magnetic field observations affect the state estimation covariance matrixes of quaternions is verified.

According to the results shown in Figures 19–21, the federal filter working in the FR mode smoothly switches to the NR mode when disturbing the magnetic field being imposed on the magnetometer has been detected. In addition, the estimation errors of pitch and roll angles of sub-filter 1 are reduced after the filter switches to the NR mode, since sub-filter 1 is assigned lower precision attitude information from the main filter in the FR mode of the filter.

Conclusion

Considering the problem that the attitude measurement system composed of MARG sensors in the navigation system of a UAV is very susceptible to disturbed magnetic field observations, an attitude anti-interference federal filtering algorithm for the MEMS-SINS/GPS/magnetometer/SV integrated navigation system is developed in this paper, which combines the advantages of the gravity field measured by the accelerometer, the geomagnetic field measured by the magnetometer and the SV obtained through the multiple polarization sensors. In addition, the algorithm mainly enhances the anti-magnetic interference capability of attitude estimation. The yaw angle calculated by the output of the magnetometer which has been compensated by the inclination angle of the UAV is used to correct the attitude quaternions updated in sub-filter 1, and the method of Euler angle information fusion is designed in the NR mode of the federal filter. Thus, the estimated pitch and roll angles of the main filter after fusion update are not affected by disturbed magnetic field observations, and the estimated yaw angle of the main filter after fusion update has certain anti-magnetic interference capability. The accuracy and anti-magnetic interference capability of the filter after fusion update are guaranteed simultaneously. Thus, the algorithm designed in this paper has certain engineering application value.