Attitude heading reference algorithm based on transformed cubature Kalman filter

Abstract

Stable and accurate attitude estimation is the key to the autonomous control of unmanned aerial vehicle. The Attitude Heading Reference System using micro-electro-mechanical system inertial measurement unit and magnetic sensor as measurement sensors is an indispensable system for attitude estimation of the unmanned aerial vehicle. Aiming at the problem of low precision of the Attitude Heading Reference System caused by the nonlinear attitude model of the micro unmanned aerial vehicle, an attitude heading reference algorithm based on cubature Kalman filter is proposed. Aiming at the nonlocal sampling problem of cubature Kalman filter, the transformed cubature Kalman filter using orthogonal transformation of the sampling point is presented. Meanwhile, an adaptive estimation algorithm of motion acceleration using Kalman filter is proposed, which realizes the online estimation of motion acceleration. The car-based tests show that the algorithm proposed in this paper can accurately estimate the carrier’s motion attitude and motion acceleration without global positioning system. The accuracy of acceleration reaches 0.2 m/s², and the accuracy of attitude reaches 1°.

Introduction

Stable and accurate attitude estimation is a prerequisite for autonomous control and navigation of unmanned aerial vehicle (UAV). ¹ The navigation system using inertial sensors (strapdown inertial navigation system (SINS)) has become one of the most important systems for UAV with its high degree of autonomy. ² Micro-electro-mechanical system (MEMS) gyroscope and accelerometer have been used in navigation system due to its small size and low power consumption. ³ Attitude Heading Reference System (AHRS) using MEMS accelerometer and magnetic sensor as measurement sensors and Kalman filter for attitude estimation is an effective method for attitude estimation of UAV. ⁴

The motion characteristics of the micro UAV result in the nonlinearity of the aircraft attitude mathematical model, so the UAV attitude algorithm is a typical nonlinear filtering problem. At present, extended Kalman filter (EKF) is the most commonly used nonlinear filter algorithm. ⁵ This method propagates state variables through first-order linearization of nonlinear systems. The linearization method introduces large linearization errors, resulting in suboptimal estimation of the filter and even filter divergence. ⁶ Although unscented Kalman filter (UKF) can realize the state estimation of nonlinear system through unscented transform (UT), the UKF algorithm based on symmetric sampling can lead to the non-positive definite of UT transfer variance and instability of filtering when applied to high-dimensional system.^7,8

The EKF and UKF require that the system must be Gaussian noise. ⁹ Particle filter has been proposed to solve nonlinear and non-Gaussian problems, and particle filter has become one of the most commonly used algorithms. ¹⁰ However, traditional particle filtering methods have particle impoverishment (PI) problems, and the computation increases geometrically with the increase in system dimension.^11,12

Arasaratnam and Haykin^13,14 have proposed a cubature Kalman filter (CKF) algorithm based on cubature transformation and proved that CKF has a higher filter accuracy than UKF. CKF guarantees the positive definiteness of the variance matrix. Because the variance matrix needs to be multiplied by a coefficient n during sampling, the sampling point is too far from the center point with the increase in the coefficient n. The filter algorithm is prone to nonlocal sampling problems, which in turn affects the accuracy of the filter algorithm.

On the contrary, the current AHRS is difficult to meet attitude accuracy requirements because the accelerometer cannot distinguish between gravity acceleration and motion acceleration, which results in low accuracy of attitude estimation under maneuvering state. ¹⁵ In view of the attitude estimation and correction problem under the condition of measurement interference, scholars have studied the state-based motion state detection algorithm, fuzzy filter, and adaptive filter to eliminate motion acceleration interference and improve the attitude estimation accuracy.^16,17 However, such algorithms are used to determine whether there is motion acceleration and then decide whether to use the accelerometer output for attitude correction. Because the motion acceleration is not estimated, this kind of algorithm usually has the problem of insufficient or excessive measurement correction, which leads to the low accuracy of attitude estimation.

Aiming at the nonlinearity of the attitude model and attitude estimation is affected by the motion acceleration, and the CKF is introduced into the AHRS to estimate the attitude. In order to solve the nonlocal sampling problem in traditional CKF, the transformed sampling point method is proposed. Meanwhile, based on the motion characteristics of micro UAV, an online adaptive estimation model of motion acceleration is established, which can effectively improve the attitude estimation accuracy of AHRS.

AHRS model based on transformed CKF

The structure of the attitude estimation algorithm based on transformed CKF (TCKF) is shown in Figure 1.

Figure 1.

AHRS nonlinear algorithm based on TCKF.

In Figure 1, ${\mathit{\omega }}_{\mathrm{ib}}^b$ is the vehicle body angular rate measured by MEMS gyroscopes; ${\mathit{f}}^b$ is the acceleration including motion acceleration and gravitational acceleration measured by MEMS accelerometers; $f_{\mathrm{nom}}$ is the acceleration excluding motion acceleration obtained by the motion acceleration adaptive estimation algorithm; ${\gamma }_i,{\theta }_i,{\psi }_i$ are the roll, pitch, and heading calculated using the strapdown inertial navigation algorithm; and $\gamma ,\theta ,\psi$ are the roll, pitch, and heading estimated by the TCKF.

The “north-east-ground” frame is set as the navigation frame. The “front-right” direction of the vehicle body is set as the X–Y direction of the body frame, and the down direction is set as the Z direction of the body frame. The magnetic sensor frame is set in coincidence with the body frame.

TCKF algorithm

Analysis of the CKF algorithm

The nonlinear system is shown in equation (1)

$$\begin{array}{c}{\mathit{x}}_k=f({\mathit{x}}_{k-1})+{\mathit{v}}_{k-1}\hfill \\ {\mathit{z}}_k=h({\mathit{x}}_k)+{\mathit{w}}_k\hfill \end{array}$$

(1)

where ${\mathit{x}}_k$ is the state vector of the dynamic system at discrete time k; ${\mathit{z}}_k$ is the measurement vector; $f(\mathit{x})$ and $h(\mathit{x})$ are the system matrix and measurement matrix of the nonlinear system, respectively; $\mathit{v}$ is the measurement noise; and $\mathit{w}$ is the process noise.

CKF uses cubature rules and deterministic weighted points to sample. For n-dimensional vectors ${\hat{\mathit{x}}}_k$ with mean value $\overline{\mathit{x}}$ and mean square error $\mathit{P}$ , the sampling points $\{{\xi }_i\}$ and their weights $\{{\omega }_i\}$ are shown in equation (2) ¹³

$$\{\begin{array}{c}{\mathit{\xi }}_i={\left[\sqrt{n\mathit{P}}\right]}_i,i=1,\dots ,n\hfill \\ \multicolumn{1}{c}{\begin{array}{c}{\mathit{\xi }}_i=-{\left[\sqrt{n\mathit{P}}\right]}_i,i=n+1,\dots ,2n\hfill \\ \multicolumn{1}{c}{w_i=\frac{1}{2n},i=1,\dots ,2n}\end{array}}\end{array}$$

(2)

where $[\sqrt{\mathit{P}}{]}_i$ represents the ith column of the matrix $\sqrt{\mathit{P}}$ . The nonlinear transformation of any sampling point can be obtained using equation (3)

$$\begin{array}{cc}{x}_i=f(\overline{x}+{\mathit{\xi }}_i),\hfill & i=1,\dots ,2n\hfill \end{array}$$

(3)

Equation (3) can be expanded as follows using the Taylor expansion

$$f\left({\mathit{\xi }}_i\right)=f(\overline{x}+\mathrm{\Delta }x)=f\left(\overline{x}\right)+\frac{{\mathit{D}}_{\mathrm{\Delta }x}^1}{1!}f+\frac{{\mathit{D}}_{\mathrm{\Delta }x}^2}{2!}f+\cdots$$

(4)

where

$$\frac{{\mathit{D}}_{\mathrm{\Delta }\mathit{x}}^k}{k!}f=\frac{1}{k!}{\left(\sum _{i=1}^n\mathrm{\Delta }{x}_i\frac{\partial }{\partial x_i}\right)}^{k}f(\mathit{x}){|}_{\mathit{x}=\overline{\mathit{x}}}$$

According to equation (4), the mean value of $f({\mathit{\xi }}_i)$ can be written as

$$\overline{f}\left(x\right)=E\left[f(\overline{x}+\mathrm{\Delta }x)\right]=f\left(\overline{x}\right)+E[\frac{{\mathit{D}}_{\mathrm{\Delta }x}^1}{1!}f+\frac{{\mathit{D}}_{\mathrm{\Delta }x}^2}{2!}f+\cdots ]$$

(5)

According to the symmetry, the odd moment of $\overline{f}\left(\mathit{x}\right)$ is zero. Therefore

$$\begin{array}{c}\multicolumn{1}{c}{\overline{f}\left(x\right)=E\left[f(\overline{x}+\mathrm{\Delta }x)\right]=f\left(\overline{x}\right)+E[\frac{{\mathit{D}}_{\mathrm{\Delta }x}^2}{2!}f+\frac{{\mathit{D}}_{\mathrm{\Delta }x}^4}{4!}f+\cdots ]}\\ \multicolumn{1}{c}{=f\left(\overline{x}\right)+\frac{{\nabla }^{T}P\nabla }{2!}f+\frac{1}{2n}\sum _{i=1}^{2n}[\frac{{\mathit{D}}_{\mathrm{\Delta }{x}_i}^4}{4!}f+\cdots +\frac{{\mathit{D}}_{\mathrm{\Delta }{x}_i}^{2k}}{2k!}]}\end{array}$$

(6)

Considering $\{\begin{array}{c}\mathrm{\Delta }x={[\sqrt{n\mathit{P}}]}_i,i=1,\dots ,n\hfill \\ \multicolumn{1}{c}{\mathrm{\Delta }x=-{[\sqrt{n\mathit{P}}]}_i,i=n+1,\dots ,2n}\end{array}$ , the factor ${\mathit{D}}_{\mathrm{\Delta }{\mathit{x}}_i}^{2k}f$ in equation (6) can be written as

$${\mathit{D}}_{\mathrm{\Delta }{\mathit{x}}_i}^{2k}f=2n^k{\left(\sum _{j=1}^n{a}_{\mathrm{ij}}\frac{\partial }{\partial x_{\mathrm{ij}}}\right)}^{2k}f(\mathit{x}){|}_{\mathit{x}=\overline{\mathit{x}}}$$

(7)

where $a_{\mathrm{ij}}$ is the ith row and jth column of $\sqrt{\mathit{P}}$ .

According to equations (6) and (7), equation (6) can be written as

$$\begin{array}{l}\overline{f}\left(\mathit{x}\right)=f\left(\overline{\mathit{x}}\right)+\frac{{\mathit{\nabla }}^T\mathit{P}\mathit{\nabla }}{2!}f+\frac{n^{k-1}}{(2k)!}\sum _{i=1}^n{\left(\sum _{j=1}^n{a}_{ij}\frac{\partial }{\partial x_{ij}}\right)}^{2k}\\ f(\mathit{x}){|}_{\mathit{X}=\overline{\mathit{x}}}\end{array}$$

(8)

The higher order information of $\overline{f}(\mathit{x})$ can be shown as

$${\mathrm{hom}}_{\mathrm{CKF}}=\frac{n^{k-1}}{(2k)!}\sum _{i=1}^n{\left(\sum _{j=1}^n{a}_{\mathrm{ij}}\frac{\partial }{\partial x_{\mathrm{ij}}}\right)}^{2k}f(\mathit{x}){|}_{\mathit{X}=\overline{\mathit{x}}}$$

(9)

From equation (9), it can be seen that the mean value after CKF transmission can be accurate to the third order, and the error is only introduced in the fourth and higher order. The higher order information increases significantly with the increase in dimension, which is prone to nonlocal sampling problems.

TCKF algorithm

The orthogonal matrix B in equation (10) is used to transform the sampling points $\{{\xi }_i\}$ in equation (2) to obtain a new set of sampling points

$$\{\begin{array}{c}\mathit{B}=\left[{\mathit{B}}_1|{\mathit{B}}_2|\dots |{\mathit{B}}_m\right]\hfill \\ \multicolumn{1}{c}{\begin{array}{c}{\mathit{B}}_k=({\beta }_{h,1},{\beta }_{h,2},\dots ,{\beta }_{h,m}),h=1,\dots ,m\hfill \end{array}}\\ \multicolumn{1}{c}{{\beta }_{h,2s-1}=\sqrt{\frac{2}{m}}\cos \left(\frac{(2s-1)h\pi }{m}\right)}\\ \multicolumn{1}{c}{{\beta }_{h,2s}=\sqrt{2/m}\sin \left(\frac{(2s-1)k\pi }{m}\right)}\end{array}$$

(10)

where $s$ is the integer from 1 to $[2/m]$ , and $[2/m]$ is the integer part of $2/m$ .

The sampling points $\{{\xi }_i\}$ can be represented as a matrix

$$\mathit{\xi }=[\sqrt{n}{\mathit{e}}_k-\sqrt{n}{\mathit{e}}_k],k=1,\dots ,n$$

(11)

After orthogonal transformation using equation (10), $\mathit{\xi }$ can be rewritten as follows

$$\{\begin{array}{c}\mathit{\chi }=\left[{\mathit{\chi }}_1|{\mathit{\chi }}_2|\dots |{\mathit{\chi }}_m\right]\hfill \\ \multicolumn{1}{c}{\begin{array}{c}{\mathit{\chi }}_h={({\mathit{\chi }}_{h,1},\dots ,{\mathit{\chi }}_{h,m})}^T,k=1,\dots ,2m\hfill \end{array}}\\ \multicolumn{1}{c}{{\mathit{\chi }}_{h,2s-1}=\sqrt{2}\cos \left(\frac{(2s-1)h\pi }{m}\right)}\\ \multicolumn{1}{c}{{\mathit{\chi }}_{h,2s}=\sqrt{2}\sin \left(\frac{(2s-1)h\pi }{m}\right)}\end{array}$$

(12)

where $s=1,\dots ,m$ , and ${\mathit{\chi }}_{h,m}=(-1{)}^h$ .

According to equation (12), the new sampling points $\{{\xi }_i\}$ and their weights $\{{\omega }_i\}$ are shown in equation (13)

$$\begin{array}{c}\mathrm{\zeta }=\sqrt{{\mathit{P}}_x\mathit{\chi }}\hfill \\ w_i=1/2n,i=1,...,2n\hfill \end{array}$$

(13)

The new Gaussian filtering algorithm (TCKF) can be obtained by replacing the sampling points in the CKF with the proposed transformed points.

Similarly, substituting equation (13) into equation (6), the higher order information of TCKF can be written as follows

$$\begin{array}{c}\multicolumn{1}{c}{{\mathrm{hom}}_{\mathrm{TCKF}}=\frac{2^k}{2n}\sum _{i=1}^{2n}{\psi }^{2k}\left(\frac{(2r-1)i\pi }{n}\right)}\\ \multicolumn{1}{c}{\times \frac{1}{(2k)!}\sum _{i=1}^n{\left(\sum _{j=1}^n{a}_{\mathrm{ij}}\frac{\partial }{\partial x_{\mathrm{ij}}}\right)}^{2k}f(\mathit{x}){|}_{\mathit{X}=\overline{\mathit{x}}}}\end{array}$$

(14)

where if n is odd, $\psi =\sin (·)$ , else $\psi =\cos (·)$ .

Considering $(1/2n)\sum _{i=1}^{2n}{\psi }^{2k}(((2r-1)i\pi )/n)\le 1$ from equations (9) and (14), ${\mathrm{hom}}_{\mathrm{TCKF}}/{\mathrm{hom}}_{\mathrm{CKF}}\le n(2/n{)}^k$ .

Therefore, the higher order information transmitted by TCKF is much smaller than CKF. This greatly reduces the problem of nonlocal sampling.

Adaptive AHRS-based TCKF algorithm

AHRS system model

According to the relationship between the body frame and the navigation frame, ${\mathit{\omega }}_{\mathrm{nb}}^b$ of body frame relative to the geographic frame is as shown in equation (15) ¹⁸

$$\left[\begin{array}{c}\stackrel{·}{\gamma }\hfill \\ \multicolumn{1}{c}{\stackrel{·}{\theta }}\\ \multicolumn{1}{c}{\stackrel{·}{\psi }}\end{array}\right]=\frac{1}{\cos \theta }\left[\begin{array}{ccc}\cos \theta \hfill & -\sin \gamma \sin \theta \hfill & \cos \gamma \sin \theta \hfill \\ \multicolumn{1}{c}{0}& \cos \gamma \cos \theta \hfill & -\sin \gamma \cos \theta \hfill \\ \multicolumn{1}{c}{0}& \sin \gamma \hfill & \cos \gamma \hfill \end{array}\right]{\omega }_{\mathrm{nb}}^b$$

(15)

where

$${\mathit{\omega }}_{\mathrm{nb}}^b=\left[\begin{array}{c}{\omega }_{\mathrm{nbx}}^b\hfill \\ \multicolumn{1}{c}{{\omega }_{\mathrm{nby}}^b}\\ \multicolumn{1}{c}{{\omega }_{\mathrm{nbz}}^b}\end{array}\right]={\mathit{\omega }}_{\mathrm{ib}}^b-{\mathit{C}}_n^b({\mathit{\omega }}_{\mathrm{ie}}^n+{\mathit{\omega }}_{\mathrm{en}}^n)$$

where ${\mathit{C}}_n^b$ is the attitude transformation matrix from “north-east-ground” frame to “front-right-down” frame, ${\mathit{\omega }}_{\mathrm{ie}}^n$ is the earth rotation rate, and ${\mathit{\omega }}_{\mathrm{en}}^n$ is the angular rate of the “north-east-ground” frame with respect to the earth frame. ${\mathit{C}}_n^b,{\mathit{\omega }}_{\mathrm{ie}}^n,{\mathit{\omega }}_{\mathrm{en}}^n$ can be solved by the attitude, velocity, and position of the UAV and can be regarded as known quantities.

Therefore, equation (15) can be simply written as

$${\mathit{\omega }}_{\mathrm{nb}}^b=\left[\begin{array}{c}{\omega }_x\hfill \\ \multicolumn{1}{c}{{\omega }_y}\\ \multicolumn{1}{c}{{\omega }_z}\end{array}\right]+\left[\begin{array}{c}{\epsilon }_r\hfill \\ \multicolumn{1}{c}{{\epsilon }_r}\\ \multicolumn{1}{c}{{\epsilon }_r}\end{array}\right]=\mathit{\omega }+{\mathit{\epsilon }}_r$$

(16)

where ${\mathit{\epsilon }}_r$ is a first-order Markov process driven by ${\mathit{\sigma }}_{\mathit{r}}$ , and ${\mathit{\sigma }}_{\mathit{r}}$ is the Gaussian noise

$${\stackrel{·}{\epsilon }}_r=-\frac{1}{T_r}{\epsilon }_r+{\sigma }_r$$

(17)

where $T_r$ is the relevant time.

Extending the noise model to the equation of state, equations (15) and (16) can be written as a nonlinear equation as shown in equation (18)

$$\left[\begin{array}{c}\begin{array}{c}\begin{array}{c}\begin{array}{c}\begin{array}{c}\stackrel{·}{\gamma }\hfill \\ \multicolumn{1}{c}{\stackrel{·}{\theta }}\end{array}\hfill \\ \multicolumn{1}{c}{\stackrel{·}{\psi }}\end{array}\hfill \\ \multicolumn{1}{c}{{\stackrel{·}{\epsilon }}_{\mathrm{rx}}}\end{array}\hfill \\ \multicolumn{1}{c}{{\stackrel{·}{\epsilon }}_{\mathrm{ry}}}\end{array}\hfill \\ \multicolumn{1}{c}{{\stackrel{·}{\epsilon }}_{\mathrm{rz}}}\end{array}\right]=\left[\begin{array}{cc}A\hfill & A\hfill \\ \multicolumn{1}{c}{0_{3\times 3}}& B\hfill \end{array}\right]\left[\begin{array}{c}{\omega }_x\hfill \\ \multicolumn{1}{c}{{\omega }_y}\\ \multicolumn{1}{c}{{\omega }_z}\\ \multicolumn{1}{c}{{\epsilon }_{\mathrm{rx}}}\\ \multicolumn{1}{c}{{\epsilon }_{\mathrm{ry}}}\\ \multicolumn{1}{c}{{\epsilon }_{\mathrm{rz}}}\end{array}\right]+\left[\begin{array}{c}{0}_{3\times 3}\hfill \\ \multicolumn{1}{c}{I_{3\times 3}}\end{array}\right]\left[\begin{array}{c}{\sigma }_{\mathrm{rx}}\hfill \\ \multicolumn{1}{c}{{\sigma }_{\mathrm{ry}}}\\ \multicolumn{1}{c}{{\sigma }_{\mathrm{rz}}}\end{array}\right]$$

(18)

where

$$\mathit{A}=\frac{1}{\cos \theta }\left[\begin{array}{ccc}\cos \theta \hfill & -\sin \gamma \sin \theta \hfill & \cos \gamma \sin \theta \hfill \\ \multicolumn{1}{c}{0}& \cos \gamma \cos \theta \hfill & -\sin \gamma \cos \theta \hfill \\ \multicolumn{1}{c}{0}& \sin \gamma \hfill & \cos \gamma \hfill \end{array}\right]$$

$$\mathit{B}=\left[\begin{array}{ccc}-\frac{1}{T_r}\hfill & 0\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& -\frac{1}{T_r}\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & -\frac{1}{T_r}\hfill \end{array}\right]$$

The state variable vector is chosen as follows

$$\mathit{x}={\left[\begin{array}{cccccc}\gamma \hfill & \theta \hfill & \psi \hfill & {\epsilon }_{\mathrm{rx}}\hfill & {\epsilon }_{\mathrm{ry}}\hfill & {\epsilon }_{\mathrm{rz}}\hfill \end{array}\right]}^T$$

According to equation (18), the state equations of the system can be written as

$$\stackrel{·}{x}\left(t\right)=f(x\left(t\right),\omega (t))+\kappa {\sigma }_r(t)$$

(19)

where $f(·)$ is the nonlinear system state equation.

Without drone acceleration, the acceleration measured by the accelerometers can be simply written as

$$\begin{array}{c}\multicolumn{1}{c}{{\mathit{f}}_{\mathrm{nom}}={\mathit{f}}^b=\left[\begin{array}{c}{f}_{\mathrm{bx}}\hfill \\ \multicolumn{1}{c}{f_{\mathrm{by}}}\\ \multicolumn{1}{c}{f_{\mathrm{bz}}}\end{array}\right]=-{\mathit{C}}_n^b{\mathit{g}}^n+{\mathit{v}}^b=g\left[\begin{array}{c}-\sin \theta \hfill \\ \multicolumn{1}{c}{\cos \theta \sin \gamma }\\ \multicolumn{1}{c}{\cos \theta \cos \gamma }\end{array}\right]}\\ \multicolumn{1}{c}{+{\mathit{v}}^b}\end{array}$$

(20)

where ${\mathit{g}}^n=[0,0,g]$ , and g is the gravitational acceleration.

According to equation (20), the measurement equation can be set as

$$\mathit{y}=\mathit{h}(\mathit{x}(t))+\mathit{v}(t)$$

(21)

where $\mathit{y}=[\begin{array}{ccc}{f}_{\mathrm{bx}}\hfill & f_{\mathrm{by}}\hfill & f_{\mathrm{bz}}\hfill \end{array}]$ , and $h(·)$ is the nonlinear measurement equation.

The nonlinear system composed of equations (19) and (21) can be solved using the TCKF algorithm proposed.

Adaptive estimation algorithm of motion acceleration

Equation (20) is obtained based on the situation that the UAV has no motion acceleration. However, there will be frequent acceleration and deceleration maneuvers during the flight of the UAV, which will introduce the motion acceleration component into the measurement information of the accelerometer. Because the accelerometer cannot distinguish gravity acceleration and motion acceleration, it is impossible to perform attitude estimation using equation (20) directly. This paper designs a motion acceleration adaptive estimation algorithm.

The motion of the micro UAV have low-frequency and slow-changing characteristic. Thus, the first-order Markov model is used to model the motion acceleration as follows

$${\mathit{a}}_k^b=\alpha {\mathit{a}}_{k-1}^b+{\mathit{\epsilon }}_a=\alpha {\mathit{a}}_{k-1}^b+{\mathit{\epsilon }}_{a,k}$$

(22)

where ${\mathit{\epsilon }}_{a,k}$ is the first-order Markov white noise and $\alpha$ is the attenuation coefficient.

The conversion relationship between the body frame and the geographic frame is shown as

$$\begin{array}{c}\multicolumn{1}{c}{{\mathit{f}}^b=-{\mathit{C}}_n^b{\mathit{f}}^n+{\mathit{v}}_a={\mathit{C}}_n^b{\mathit{a}}^n+{\mathit{C}}_n^b((2{\mathit{\omega }}_{\mathrm{ie}}^n+{\mathit{\omega }}_{\mathrm{en}}^n)\times {\mathit{V}}^n)}\\ \multicolumn{1}{c}{-{\mathit{C}}_n^b{\mathit{g}}^n+{\mathit{v}}_a}\end{array}$$

(23)

where ${\mathit{v}}_a$ is acceleration measurement noise and ${\mathit{a}}^n$ is the motion acceleration in geographic frame. Considering that micro UAVs have a slow speed generally, $(2{\mathit{\omega }}_{\mathrm{ie}}^n+{\mathit{\omega }}_{\mathrm{en}}^n)\times {\mathit{V}}^n$ can be ignored. So equation (23) can be written as

$${\mathit{f}}^b={\mathit{a}}^b-{\mathit{g}}^b+{\mathit{v}}_a$$

(24)

where ${\mathit{a}}^b={\mathit{C}}_n^b{\mathit{a}}^n\mathrm{and}{\mathit{g}}^b={\mathit{C}}_n^b{\mathit{g}}^n$ are the motion acceleration and gravity acceleration in the body frame, respectively.

From equation (23), taking the output of the accelerometer as the measurement information, equation (24) can be written as follows

$$\begin{array}{c}\multicolumn{1}{c}{{\mathit{f}}_k^b={\mathit{a}}_k^b-{\mathit{g}}_k^b+{\mathit{v}}_a={\mathit{a}}_k^b-{\mathit{C}}_{n,k}^b{\mathit{g}}^n+{\mathit{v}}_a}\\ \multicolumn{1}{c}{={\mathit{a}}_k^b-{\mathit{C}}_{n,k}^b{\mathit{g}}^n+{\mathit{v}}_a}\end{array}$$

(25)

Taking equation (22) as the state equation and equation (25) as the measurement equation, the Kalman filter is used to estimate the motion acceleration.

Using the estimated motion acceleration ${\mathit{a}}_k^b$ , ${\mathit{f}}_{\mathrm{nom}}^b$ used in equation (20) can be rewritten as

$${\mathit{f}}_{\mathrm{nom}}^b={\mathit{f}}_k^b-{\mathit{a}}_k^b$$

At the same time, equation (22) has shown that the value of the attenuation coefficient is $0<\alpha \le 1$ , which reflects the change of the motion acceleration and also determines the degree of confidence in the estimated motion acceleration at the previous moment. Therefore, adaptive adjustment of $\alpha$ using equation (26) based on the UAV’s motion can improve the estimation precision of the motion acceleration effectively

$$\alpha =0.5\times \left(\frac{a_k^b-3\mathrm{\Omega }}{50\mathrm{\Omega }-3\mathrm{\Omega }}\right)+0.5$$

(26)

where

$$a_k^b=\sqrt{{(f_{x,k}^b)}^2+{(f_{y,k}^b)}^2+{(f_{z,k}^b)}^2}-g$$

$$\mathrm{\Omega }=\sqrt{\frac{\sum _{k=1}^n{(\sqrt{{(f_{x,k}^b)}^2+{(f_{y,k}^b)}^2+{(f_{z,k}^b)}^2}-g)}^2}{n}}$$

Experiments

Car-based TCKF test

In order to verify the attitude estimation performance of the proposed algorithm in practical applications, the MEMS AHRS shown in Figure 2 is designed.

Figure 2.

MEMS Attitude Heading Reference System.

The gyroscope is composed of three ADXRS646 single-axis gyros with an precision of 12°/h, and the accelerometer uses a three-axis ADXL355 with a precision of 5 × 10⁻³ G. The output frequency of gyroscopes and accelerometers is 1000 Hz. Considering the low dynamic characteristics of the UAV, the sampling frequency of the gyroscope and acceleration is set as 100 Hz to reduce the impact of MEMS sensor noise. The system processor adopts STM32F4 series ARM with floating point arithmetic capability, and its frequency is 168 MHz. Considering the computing power of the processor, the frequency of TCKF is set as 10 Hz, and the Kalman filter frequency estimated by motion acceleration is set as 20 Hz. Because the frequency of the motion acceleration estimation algorithm in section “Adaptive AHRS-based TCKF algorithm” is faster than the frequency of the TCKF, the results of motion acceleration estimation algorithm can be smoothed and used as the input of the TCKF.

A car experiment has been conducted in Nanjing University of Science and Technology. In the experiment, commercial SBG Ellipse Inertial/GPS (global positioning system) integrated navigation system using EKF was used as a reference, and the measurement precision of the roll and pitch angle with GPS is 0.2°. The path is shown in Figure 3.

Figure 3.

Tracking map of car experiment.

Results analysis

The comparison of the roll and pitch angle of the TCKF algorithm, CKF algorithm, and SBG, presented in this paper, is shown in Figures 4 and 5.

Figure 4.

Comparison of roll angle.

Figure 5.

Comparison of pitch angle.

It can be seen from the figure that our proposed attitude algorithm using TCKF without GPS is consistent with the SBG results with GPS basically.

As shown in Figures 6 and 7, from 0 to 40 s, without motion acceleration, the attitude error between TCKF/CKF and SBG is less than 0.2°. The proposed motion acceleration adaptive estimation algorithm can estimate the motion acceleration.

Figure 6.

Comparison of pitch angle from 0 to 40 s.

Figure 7.

Comparison of estimated acceleration and actual acceleration from 0 to 40 s.

As shown in Figures 8 and 9, from 200 to 300 s, in the case of acceleration and deceleration, the SBG can accurately reflect the change in attitude angle with GPS assistance.

Figure 8.

Comparison of pitch angle from 200 to 300 s.

Figure 9.

Comparison of estimated acceleration and actual acceleration from 200 to 300 s.

Meanwhile, it can be seen from Figures 6 and 8 that the accuracy of the TCKF algorithm is significantly better than that of the CKF algorithm. The attitude error between our proposed algorithm and SBG is shown in Table 1.

Table 1.

Our proposed algorithm and SBG attitude error (unit: degree).

Error	Roll		Pitch
	CKF	TCKF	CKF	TCKF
Average	0.2260	0.1322	0.4619	0.2900
Mean squaredeviation	0.7913	0.4396	0.7519	0.3872

CKF: cubature Kalman filter; TCKF: transformed cubature Kalman filter.

From Table 1, the attitude error of our proposed algorithm is less than 1°. The algorithm has reached a high precision without GPS with SBG results as a reference. The mean error and mean square error of the TCKF algorithm are improved by 70% compared to CKF.

As shown in Figures 9 and 10, the proposed motion acceleration adaptive estimation algorithm can estimate the trend of the carrier’s motion acceleration, and the error between estimated acceleration and actual acceleration is less than 0.2 m/s². The attitude angle error caused by the acceleration error is less than 1.0°, which is consistent with the error reflected in Figure 7. It can be seen from Figure 10 that the acceleration estimation algorithm has good real-time performance.

Figure 10.

Comparison of estimated acceleration and actual acceleration from 600 to 650 s.

Conclusion

The core of attitude estimation for micro UAV is AHRS which uses micro-electronic mechanical system inertial measurement unit (MEMS-IMU) as measuring sensors. The AHRS nonlinear estimation model and an adaptive motion acceleration estimation algorithm are established. Aiming at the nonlinear filtering and the nonlocal sampling problem of CKF problem, the TCKF algorithm is proposed based on the orthogonal sampling transformation. The car-based test has proved that the presented algorithm has a precision of 1° without GPS. Meanwhile, the accuracy of the TCKF algorithm is improved by 70% compared to CKF. The algorithm proposed in this paper has important practical value for the research and application of AHRS.