Attitude estimation using MARG sensors for unmanned aerial vehicles

Abstract

In this paper we discuss an extended kalman filter with novel magnetic vector measurement model for attitude estimation of unmanned aerial vehicles (UAVs). As MARG (Magnetic, Angular Rate and Gravity) sensors are used, this extended kalman filter selects attitude error and angular rate drift as the filter states, and performs the information fusion using the magnetic and gravity vector respectively measured by the three-axis accelerometer and the three-axis magnetometer. We propose a new method of magnetic vector fusion different from the past ones. Firstly, the measured earth magnetic vector is transformed from body frame to local frame using estimate direction cosine matrix. And then the innovation is carefully constructed using the cross product between measured vector and referenced earth magnetic vector, which is different from the past vector subtract way. Finally, the measurement model of magnetic vector and equations of kalman filter is presented. Proposed algorithm is implemented on the Cortex-M4 microprocessor platform and experiments is taken relied on three-axis turntable. The results show that the proposed method has a lower computational cost than the past methods, and is able to track the attitude of motions precisely.

Keywords

Extended kalman filter MARG sensors attitude estimation UAV

1. Introduction

Since the Apollo mission in the 1960s, extended kalman filter (EKF) has long been applied to the attitude estimation and navigation of vehicles [1]. At present, due to the advantages of no singularity and less computation, many quaternion-based extended kalman filtering methods have been applied to attitude estimation of various types of aircraft [2, 3]. Otherwise, with further consideration on non-linear and non-Gaussian situations, filtering methods that are better suited to strong nonlinear or non-Gaussian systems are widely studied. Such as the unscented kalman filter (UKF) and the cubature kalman filter (CKF) that can better deal with nonlinear situations than EKF [4, 5, 6], and the particle filter (PF) that can be applied to non-Gaussian systems [7]. But these advance methods also lead to higher computational cost than traditional EKF. Especially in high dynamic systems such as hyper-sonic vehicles, in order to constrain the coning errors and sculling errors in strap down inertial navigation calculations, high attitude updating frequencies are required. However, the on-board embedded computers are not very capable good support for high-frequency (more than 100 Hz) calculations of UKF, CKF or PF algorithms. Therefore, in engineering practice, extended kalman filter is still the first choice for many engineers. Besides Kalman filter there is an alternative approach to attitude estimation treats the disturbance as unknown deterministic signals and seeks to optimize some measure of size of these signals. The most prominent techniques in this domain are complementary filter [8] and minimum-energy filter [9].

This paper focuses on attitude estimation of UAVs using MARG sensors which are composed of three-axis magnetometer, accelerometer and gyroscope. In this problem, extended kalman filter using attitude error as states is considered as a standard solution [10, 11]. According to the terminology in [12], the process of kalman filter can be divided into two steps: time update (prediction) and measurements update (correction). In these conventional solutions, the time update process usually uses the angular rates information measured by the three-axis gyroscope to update the state and the covariance matrix of the filter, and then the gravity and magnetic field vectors measured by the three-axis accelerometer and magnetometer are used for the measurement update process. In the prediction step, these methods have a similar form, and in the correction step, there are several different approaches. A class of approach is updating measurements by directly using of the measured gravity and magnetic field vectors [13], and either batch or sequential update method can be adopted. In batch update method, the calculation of the kalman gain and covariance matrix only needs to do once. But this method involves calculating the inverse of a matrix has the same dimension with the measurement vector (e.g. a 6 by 6 matrix). In sequential update method we do not need to perform matrix inverse calculation, but the kalman gain and covariance matrix have to calculate the same number as the dimension of measurement vector times. In addition, the covariance matrix of measurement noise is required to be a diagonal matrix to realize the sequential method. Otherwise some extra operation must be taken to transform the covariance matrix of measurement noise to a diagonal matrix. In general, people are more willing to use a sequential approach. This class of measurement update approach can directly employ the original noise information of sensors to construct the matrix R, which is used to calculate the kalman gain. So the parameters of kalman filter with this kind of measurement update approach is easily determined and adjusted. Another class of approach is to first determine an attitude parameter using the measured gravity and magnetic field vectors, and then use this attitude parameter for measurement update process [14, 15]. At present, methods of determining attitude by two vectors include: QUEST algorithm by solving a optimization problem [16], TRIAD algorithm for calculating direction cosine matrix (DCM) using vectors’ geometric property [17], algebraic analysis method [18] and so on. Similarly, this class of approach can also be updated using either batch or sequential methods. This kind of approach can reduce the dimension of the measurement model but requires additional attitude determination calculations. Furthermore, the covariance matrix of attitude measurement noise derived from this approach cannot be obtained through the original measurement noise of sensors, which can only be determined empirically. Thus increasing the difficulty of adjusting the filter parameters.

Aiming at the problems of the above two class of measurement update approaches, we improved the measurement model of MARG sensors. On the one hand, it reduces the dimension of measurement model and thus reduces the computational cost of measurement update process. On the other hand, it makes the measurement noise of sensors keep their physical meanings, which makes the covariance matrix of measurement noise of filter easy to determine and adjust. The rest of paper is organized as follows: In Section 2, the coordinate systems, attitude parameters and measurement models are presented; Section 3 describes the traditional extended kalman filter equations based on the rotation vector and MARG sensors; then in Section 4 the improvement of measurement model is presented; the fifth section presents experiments of proposed method and methods in [17, 11] for comparison. The results show that the proposed method has less computational time consuming and the higher estimation accuracy than conventional methods.

2. Notation and definitions

Utilizing unit-quaternion to represent the attitude of vehicles has the advantages of no singularity, less calculation and dynamic equation in linear form. Unit-quaternion is a four components vector defined as $\bm{q}=(q_{s}\quad\bm{q}_{v})^{T}$ , where $\bm{q}=(q_{x}\quad q_{y}\quad q_{z})^{T}$ is the vector part of quaternion, and $q_{s}$ is the scalar part. Moreover, unit-quaternion has $||\bm{q}||=\sqrt{q_{s}^{2}+q_{x}^{2}+q_{y}^{2}+q_{z}^{2}}=1$ . The multiplication of quaternions can be defined as follow:

$\displaystyle\bm{p}\otimes\bm{q}=\left[{{\begin{array}[]{*{20}c}{q_{s}p_{s}-% \bm{q}_{v}^{T}\bm{p}_{v}}\\ {p_{s}\bm{q}_{v}+q_{s}\bm{p}_{v}+\bm{q}_{v}\times\bm{p}_{v}}\\ \end{array}}}\right]$ (1)

Let $\left[\bm{p}_{v}\times\right]$ be a skew symmetric matrix of vector $\bm{p}_{v}$ :

$\displaystyle\left[\bm{p}_{v}\times\right]=\left[{{\begin{array}[]{*{20}c}0&{-% p_{z}}&{p_{y}}\\ {p_{z}}&0&{-p_{x}}\\ {-p_{y}}&{p_{x}}&0\\ \end{array}}}\right]$ (2)

The conjugate of quaternion $\bm{q}$ is defined as $\bm{q}^{*}=(q_{s}\quad-\bm{q}_{v})^{T}$ , and $\bm{q}\otimes\bm{q}^{*}=(1\quad\bm{0}_{1\times 3})^{T}$ holds for unit-quaternion. Thus it implies $\bm{q}^{*}=\bm{q}^{-1}$ . The quaternion kinetic equation is

$\displaystyle\bm{\dot{q}}=\frac{1}{2}\left[{{\begin{array}[]{*{20}c}0\\ {\bm{\omega}}_{ib}^{b}\\ \end{array}}}\right]\otimes\bm{q}$ (3)

where $\bm{\omega}_{ib}^{b}$ denotes the rate of rotation of the body frame with respect to the inertial frame, resolved about the body frame axes, which can be measured by a gyro attached to the UAV.

In this paper, the unit-quaternion is used to describe the attitude of the body frame (b frame) with respect to the local tangent-plane frame (l frame). The details of frame definitions can be found in [19]. Use $\bm{v}_{b}$ to denote the projection of vector $\bm{v}$ in body frame, and $\bm{v}_{l}$ denotes the projection in reference frame. Then the direction cosine matrix that transforms $\bm{v}_{b}$ to $\bm{v}_{l}$ is $\bm{C}_{b}^{l}$ , which implies that $\bm{v}_{l}=\bm{C}_{b}^{l}\bm{v}_{b}$ , $\bm{v}_{b}=\bm{C}_{l}^{b}\bm{v}_{l}$ . And we have $\bm{C}_{l}^{b}=\left({\bm{C}_{b}^{l}}\right)^{T}$ , and $\bm{C}_{l}^{b}\bm{C}_{b}^{l}=\bm{I}_{3}$ . The DCM kinetic equation is

$\displaystyle\bm{\dot{C}}_{b}^{l}=\bm{C}_{b}^{l}\left[{\bm{\omega}_{ib}^{b}% \times}\right]$ (4)

Another useful attitude parameter is rotation vector. A rotation vector can be defined as $\delta\bm{\Phi}=({\phi_{x}}\quad{\phi_{y}}\quad{\phi_{z}})^{T}$ , whose length is $\delta\varphi=\left\|{\delta\bm{\Phi}}\right\|$ . Then the relationship of rotation vector, quaternion and DCM are as follow:

$\displaystyle\bm{q}\left(\bm{\Phi}\right)=\left[{{\begin{array}[]{*{20}c}{\cos% \left({\varphi/2}\right)}\\ {\frac{1}{2}\bm{\Phi}\frac{\sin\left({\varphi/2}\right)}{\varphi/2}}\\ \end{array}}}\right]$ (5) $\displaystyle C_{b}^{l}\left({\bm{\Phi}}\right)=\left({\cos\varphi}\right){\bm% {I}}-\frac{\sin\varphi}{\varphi}\left[{\bm{\Phi}\times}\right]+\frac{1-\cos% \varphi}{\varphi^{2}}{\bm{\Phi\Phi}}^{T}$ (6)

When the length of rotation vector is small it can be approximated as follow formula:

$\displaystyle\bm{q}\left({\bm{\Phi}}\right)=\left[{{\begin{array}[]{*{20}c}1\\ {\frac{1}{2}{\bm{\Phi}}}\\ \end{array}}}\right]$ (7) $\displaystyle\bm{C}_{b}^{l}\left({\bm{\Phi}}\right)={\bm{I}}_{3}+\left[{{\bm{% \Phi}}\times}\right]$ (8)

Let $\bm{\tilde{B}}_{b}$ , $\bm{\tilde{\omega}}_{b}$ , $\bm{\tilde{f}}_{b}$ be the magnetic field vector, angular rate vector and specific force vector measured by MARG sensors, respectively. Correspondingly the true magnetic field, angular rate and gravity vector resolved in body frame are $\bm{B}_{b}$ , ${{\bm{\omega}}}_{b}$ , ${{\bm{f}}}_{b}$ , respectively. A signal ${{\bm{\eta}}}$ with subscripts is a white noise process whose auto-covariance is $E\left[{\bm{\eta}(t){{\bm{\eta}}}(t)^{T}}\right]=\sigma^{2}{{\bm{I}}}_{3}% \delta\left({t-\tau}\right)$ , where $E$ is the mathematical expectation operator, $\sigma^{2}$ is the variance of white noise, and $\delta\left(t\right)$ is the Dirac delta function. For example, ${{\bm{\eta}}}_{B}$ , ${{\bm{\eta}}}_{\omega}$ , ${{\bm{\eta}}}_{r}$ , ${{\bm{\eta}}}_{f}$ are all white noise process with variance $\sigma_{B}^{2}$ , $\sigma_{\omega}^{2}$ , $\sigma_{r}^{2}$ , $\sigma_{f}^{2}$ , respectively. In the latter we will use these notions to represent sensor measurement noise.

3. Extended kalman filter for attitude estimation

3.1 Rotation vector based attitude error representations

The true attitude of UAV can be expressed as follow:

$\displaystyle{{\bm{q}}}=\delta{{\bm{q}}}\otimes{\bm{\hat{q}}}$ (9)

where ${\bm{\hat{q}}}$ is the estimation of attitude, $\delta\bm{q}$ is the error quaternion between true and estimate attitude. $\delta{{\bm{q}}}$ is a unit-quaternion representing the rotation from the attitude estimation ${\bm{\hat{q}}}$ to the true attitude $\bm{q}$ . Some papers directly use the attitude estimation or attitude error quaternion as the state of kalman filter [3]. However, this will increase the dimension of the covariance matrix and thus lead to larger computations. Therefore, we are more inclined to use three-dimension parameters such as rotation vector to represent the attitude error.

Therefore, the task of designing the attitude estimation kalman filter is to use the predicted attitude estimation ${\bm{\hat{q}}}(-)$ and the measurements to determine the estimation of attitude error rotation vector $\delta{\bm{\hat{\Phi}}}$ , and then calculate the updated estimation of attitude ${\bm{\hat{q}}}(+)$ using the Eqs (7) and (9):

$\displaystyle\delta{\bm{\hat{q}}}=\left[{{\begin{array}[]{*{20}c}1\\ {\frac{1}{2}\delta{\bm{\hat{\Phi}}}}\\ \end{array}}}\right]$ (10)

and

$\displaystyle{\bm{\hat{q}}}\left(+\right)=\delta{\bm{\hat{q}}}\otimes{\bm{\hat% {q}}}\left(-\right)$ (11)

The predicted attitude estimation can be calculate by Eq. (3):

$\displaystyle{\bm{\hat{q}}}\left(-\right)=\frac{1}{2}\left[{{\begin{array}[]{*% {20}c}0\\ {{\bm{\hat{\omega}}}_{b}}\\ \end{array}}}\right]\otimes{\bm{\hat{q}}}\left(+\right)$ (12)

where ${\bm{\hat{q}}}\left(+\right)$ in above equation denotes the updated attitude estimation of last time step, and ${\bm{\hat{\omega}}}_{b}$ denotes the estimation of angular rate, which can be calculate as follow:

$\displaystyle{\bm{\hat{\omega}}}_{b}={\bm{\tilde{\omega}}}_{b}+{\bm{\hat{b}}}_% {\omega}$ (13)

where ${\bm{\hat{b}}}_{\omega}$ is the estimation of gyro drift. Then we can define the error of angular rate as follow:

$\displaystyle\delta{{\bm{\omega}}}_{b}={{\bm{\omega}}}_{b}-{\bm{\hat{\omega}}}% _{b}={\bm{\tilde{\omega}}}_{b}+{{\bm{b}}}_{\omega}+{{\bm{\eta}}}_{\omega}-{\bm% {\tilde{\omega}}}_{b}-{\bm{\hat{b}}}_{\omega}={{\bm{b}}}_{\omega}-{\bm{\hat{b}% }}_{\omega}+{\bm{\eta}}_{\omega}$ (14)

3.2 Filter time update

The state vector of the extended kalman filter is chosen to be ${{\bm{x}}}=(\delta{{\bm{\Phi}}}\quad{{{\bm{b}}}_{\omega}})^{T}$ , and we directly give the system equations of filter as follow:

$\displaystyle\left[{{\begin{array}[]{*{20}c}{\delta{\bm{\dot{\Phi}}}}\\ {{\bm{\dot{b}}}_{\omega}}\\ \end{array}}}\right]=\left[{{\begin{array}[]{*{20}c}{-\left[{{\bm{\hat{\omega}% }}_{b}\times}\right]}&{{\bm{I}}_{3}}\\ {{{\bm{0}}}_{3}}&{{{\bm{0}}}_{3}}\\ \end{array}}}\right]\left[{{\begin{array}[]{*{20}c}{\delta{{\bm{\Phi}}}}\\ {{{\bm{b}}}_{\omega}}\\ \end{array}}}\right]+\left[{{\begin{array}[]{*{20}c}{{{\bm{\eta}}}_{\omega}}\\ {{{\bm{\eta}}}_{r}}\\ \end{array}}}\right]$ (15)

And details about the derivation of above formula can be found in [11].

Then we can write the state transition matrix from time $t_{k-1}$ to $t_{k}$ as follow

$\displaystyle{{\bm{F}}}_{k-1}\left({T_{k}}\right)=\left[{{\begin{array}[]{*{20% }c}{{{\bm{I}}}_{3}-\left[{{\bm{\hat{\omega}}}_{b}\left({t_{k-1}}\right)T_{k}% \times}\right]}&{T_{k}{{\bm{I}}}_{3}}\\ {{{\bm{0}}}_{3}}&{{{\bm{I}}}_{3}}\\ \end{array}}}\right]$ (16)

where $T_{k}=t_{k}-t_{k-1}$ . In the rest of paper, we abbreviate $\bm{F}_{k-1}\left({T_{k}}\right)$ as ${{\bm{F}}}_{k-1}$ . Then using the state transition matrix, the state covariance matrix $\bm{P}$ can be update (predict) as follow:

$\displaystyle{{\bm{P}}}_{k|k-1}={{\bm{F}}}_{k-1}{{\bm{P}}}_{k-1|k-1}{{\bm{F}}}% _{k-1}^{T}+{{\bm{Q}}}_{I_{k-1}}$ (17)

It should be noted that in this paper, subscript $k$ denotes variables at the $t_{k}$ moment, and for variables already have subscript, the time $k$ will be write in bracket following the variable. In addition, for variables have multiple state from time $t_{k-1}$ to $t_{k}$ , use subscript $k|k-1$ for the time update value and subscript $k|k$ for the measurement update value. In the above formula, ${{\bm{Q}}}_{I_{k-1}}$ is the discrete-time process noise covariance matrix, calculated as follow:

$\displaystyle{{\bm{Q}}}_{I_{k-1}}=\int\limits_{t_{k-1}}^{t_{k}}{{\bm{F}}_{k-1}% \left(\tau\right){{\bm{Q}}}_{k-1}\left(\tau\right){{\bm{F}}}_{k-1}^{T}\left(% \tau\right)d\tau}$ (18)

with

$\displaystyle{{\bm{Q}}}_{k-1}=\left[{{\begin{array}[]{*{20}c}{\sigma_{\omega}^% {2}{{\bm{I}}}_{3}}&{{{\bm{0}}}_{3}}\\ {{{\bm{0}}}_{3}}&{\sigma_{r}^{2}{{\bm{I}}}_{3}}\\ \end{array}}}\right]$ (19)

The angle random walk variance $\sigma_{\omega}^{2}$ and rate random walk variance $\sigma_{r}^{2}$ can be obtained according to the datasheet or test data of the gyroscope.

3.3 Filter measurements update

In MARG sensors frame, the magnetometer, gyro and accelerometer measurement models can be represent as follow:

$\displaystyle{{\bm{B}}}_{b}={\bm{\tilde{B}}}_{b}+{{\bm{\eta}}}_{B}$ $\displaystyle{{\bm{\omega}}}_{b}={\bm{\tilde{\omega}}}_{b}+{{\bm{b}}}_{\omega}% +{{\bm{\eta}}}_{\omega}$ $\displaystyle{{\bm{g}}}_{b}=-{\bm{\tilde{f}}}_{b}-{{\bm{\eta}}}_{f}$ (20)

Two class of measurement models mentioned in Section 1 have been described in detail in past papers. In [1], the TRIAD method for calculating the attitude DCM using measured gravity and magnetic field vector is presented. By converting the attitude DCM to quaternion, a four-dimension measurement model is build and then the correction process is update based on the model. The literature [11] gives a method of kalman filter measurement update directly using vectorial measurements, and then obtains a six-dimension measurement model. This paper will not go into details about these existing methods.

In fact, when we build different measurement model, it results in different measurement matrix $\bm{H}$ and different innovation vector $\bm{\Delta}$ . And then we can follow the steps below to make the measurement update.

$\displaystyle{{\bm{K}}}_{k}={{\bm{P}}}_{k|k-1}{{\bm{H}}}_{k}^{T}\left({{{\bm{H% }}}_{k}{{\bm{P}}}_{k|k-1}{{\bm{H}}}_{k}^{T}+{{\bm{R}}}_{k}}\right)^{-1}$ (21) $\displaystyle{\bm{\hat{x}}}_{k|k}={\bm{\hat{x}}}_{k|k-1}+{\bm{K}}_{k}\bm{% \Delta}_{k}$ (22) $\displaystyle{{\bm{P}}}_{k|k}=\left({{{\bm{I}}}_{3}-{{\bm{K}}}_{k}{\bm{H}}_{k}% }\right){{\bm{P}}}_{k|k-1}$ (23)

As we have discussed, ${\bm{\hat{x}}}_{k|k-1}$ is the prediction of state after filter time update process, and ${\bm{\hat{x}}}_{k|k}$ is the correction of state after filter measurement update process, ${{\bm{R}}}_{k}$ is the measurement noise auto-covariance matrix at time $t_{k}$ .

It is easy to see that when using the measurement model in [17], the dimension of the measured vector is 4. But the noise of the measurement model is the noise of each quaternion component and can not be determined by accelerometer and Magnetometer measurement noise directly. And when we use the measurement model in [11], the dimension of the measurement vector is 6, which will consume more computations. However, the noise of the measurement model is the original noise of accelerometer and magnetometer, so it can be directly determined.

4. A novel measurement model

4.1 The novel model based measurements update

We will derive a novel measurement model of MARG sensors. Firstly, let us recall the accelerometer model in Eq. (3.3), by combining with the error representation of the DCM it leads:

$\displaystyle-{\bm{\tilde{f}}}_{b}={{\bm{C}}}_{l}^{b}{{\bm{g}}}_{l}+{{\bm{\eta% }}}_{f}=\left({{\bm{\hat{C}}}_{b}^{l}+{\bm{\hat{C}}}_{b}^{l}\left[{\delta{{\bm% {\Phi}}}\times}\right]}\right)^{T}{{\bm{g}}}_{l}+{\bm{\eta}}_{f}={\bm{\hat{C}}% }_{b}^{l}{{\bm{g}}}_{l}-\left[{\delta{{\bm{\Phi}}}\times}\right]{\bm{\hat{C}}}% _{b}^{l}{{\bm{g}}}_{l}+{{\bm{\eta}}}_{f}$ (24)

where ${{\bm{g}}}_{l}=(0\quad 0\quad g)^{T}$ , and $g$ is the local gravity acceleration scalar. Since the measured gravity by accelerometer can only be used to determine the tilt of UAV, i.e. the roll and pitch angle, the rotation vector in above formula can be chosen as $({\phi_{x}}\quad{\phi_{y}}\quad 0)^{T}$ . Then we can get

$\displaystyle{\bm{\hat{C}}}_{b}^{l}{{\bm{g}}}_{l}+{\bm{\tilde{f}}}_{b}=\left[{% {\begin{array}[]{*{20}c}{\hat{g}_{bz}\phi_{y}}\\ {-\hat{g}_{bz}\phi_{x}}\\ {-\hat{g}_{bx}\phi_{y}+\hat{g}_{by}\phi_{x}}\\ \end{array}}}\right]-{{\bm{\eta}}}_{f}$ (25)

Where

$\displaystyle{\bm{\hat{g}}}_{b}=({\hat{g}_{bx}}\quad{\hat{g}_{by}}\quad{\hat{g% }_{bz}})^{T}={\bm{\hat{C}}}_{b}^{l}{{\bm{g}}}_{l}$ (26)

is the estimation of gravity vector projected in body frame. Therefore, the measurement matrix of accelerometer measurement is

$\displaystyle{{\bm{H}}}_{\text{ACC}}=\left[{{\begin{array}[]{*{20}c}{\left[{{% \begin{array}[]{*{20}c}0&{\hat{g}_{bz}}&0\\ {-\hat{g}_{bz}}&0&0\\ {\hat{g}_{by}}&{-\hat{g}_{bx}}&0\\ \end{array}}}\right]}&{{{\bm{0}}}_{3}}\\ \end{array}}}\right]$ (27)

And the innovation generated by accelerometer measurement is $\bm{\Delta}_{\text{ACC}}={\bm{\hat{C}}}_{b}^{l}{{\bm{g}}}_{l}+{\bm{\tilde{f}}}% _{b}$ . The auto-covariance matrix of measurement noise ${{\bm{\eta}}}_{f}$ is ${{\bm{R}}}_{\text{ACC}}$ , which can be directly obtained by characteristic of the accelerometer. Next we derive the novel magnetometer measurement model. Similarly to the above process, by combining the Eqs (4) and (10) we can get

$\displaystyle{{\bm{B}}}_{l}={{\bm{C}}}_{b}^{l}{\bm{\tilde{B}}}_{b}+{{\bm{C}}}_% {b}^{l}{{\bm{\eta}}}_{B}={\bm{\hat{C}}}_{b}^{l}{\bm{\tilde{B}}}_{b}+{\bm{\hat{% C}}}_{b}^{l}\left[{\delta{{\bm{\Phi}}}\times}\right]{\bm{\tilde{B}}}_{b}+{\bm{% \hat{C}}}_{b}^{l}{{\bm{\eta}}}_{B}$ (28)

where ${{\bm{B}}}_{l}$ is the local magnetic field vector projected in the local tangent-plane frame, i.e. in north, east and down directions. Note that we have ignored the multiplication between the error value when deriving the second equal sign. Since the magnetic measurement are always subject to disturbances from building, electronic motors and other sources, we do not want to use it to correct the pitch and roll angles, which usually bring unnecessary extra error to them. Therefore we define two operations as follow:

Operation 1: Substitute the error rotation vector using $(0\quad 0\quad{\phi_{z}})^{T}$ .

Operation 2: Set the third component of vectors to zero on each side of equation.

The design of “Operation 2” is depend on the physical meaning of the Eq. (28). On both side of this equation is a three components vector, which represent the local magnetic field vector projected in north, east and down, respectively. As we can determine the yaw of the UAV just using north and east magnetic values, the magnetic field component in down direction is redundant, and thus we set it to zero. Another advantage of this operation is that we need not to calculate the vertical component of the local geomagnetic field vector. Note that ${{\bm{B}}}_{l}=({B_{N}}\quad{B_{E}}\quad{B_{D}})^{T}$ is the known theoretic local geomagnetic field vector, ${\bm{\hat{B}}}_{l}={\bm{\hat{C}}}_{b}^{l}{\bm{\tilde{B}}}_{b}=({\hat{B}_{N}}% \quad{\hat{B}_{E}}\quad{\hat{B}_{D}})^{T}$ is the estimate local geomagnetic field vector, and ${\bm{\tilde{B}}}_{b}=({\tilde{B}_{bx}}\quad{\tilde{B}_{by}}\quad{\tilde{B}_{bz% }})^{T}$ is the magnetic field measurement. And then apply the “Operation 1” to Eq. (28), which leads

$\displaystyle\left[{{\begin{array}[]{*{20}c}{B_{N}}\\ {B_{E}}\\ {B_{D}}\\ \end{array}}}\right]=\left[{{\begin{array}[]{*{20}c}{\hat{B}_{N}}\\ {\hat{B}_{E}}\\ {\hat{B}_{D}}\\ \end{array}}}\right]+\phi_{z}{\bm{\hat{C}}}_{b}^{l}\left[{{\begin{array}[]{*{2% 0}c}{-\tilde{B}_{by}}\\ {\tilde{B}_{bx}}\\ 0\\ \end{array}}}\right]+{\bm{\hat{C}}}_{b}^{l}{{\bm{\eta}}}_{B}$ (29)

To simplify writing, we define $\left({{\bm{v}}}\right)_{1:2}=({v_{x}}\quad{x_{y}}\quad 0)^{T}$ for a vector ${{\bm{v}}}=({v_{x}}\quad{x_{y}}\quad{x_{z}})^{T}$ . Then apply the “Operation 2” to the above formula, we can get

$\displaystyle\left[{{\begin{array}[]{*{20}c}{B_{N}}\\ {B_{E}}\\ 0\\ \end{array}}}\right]=\left[{{\begin{array}[]{*{20}c}{\hat{B}_{N}}\\ {\hat{B}_{E}}\\ 0\\ \end{array}}}\right]+\phi_{z}\left({{\bm{\hat{C}}}_{b}^{l}\left[{{\begin{array% }[]{*{20}c}{-\tilde{B}_{by}}\\ {\tilde{B}_{bx}}\\ 0\\ \end{array}}}\right]}\right)_{1:2}+\left({{\bm{\hat{C}}}_{b}^{l}{{\bm{\eta}}}_% {B}}\right)_{1:2}$ (30)

After defining the following vectors,

$\displaystyle{{\bm{v}}}_{B}=\left({{\bm{\hat{C}}}_{b}^{l}\left[{{\begin{array}% []{*{20}c}{-\tilde{B}_{by}}\\ {\tilde{B}_{bx}}\\ 0\\ \end{array}}}\right]}\right)_{1:2}=\left[{{\begin{array}[]{*{20}c}{v_{Bx}}\\ {v_{By}}\\ 0\\ \end{array}}}\right]$ (31) $\displaystyle{{\bm{v}}}_{\eta}=\left({{\bm{\hat{C}}}_{b}^{l}{{\bm{\eta}}}_{B}}% \right)_{1:2}=\left[{{\begin{array}[]{*{20}c}{v_{\eta x}}\\ {v_{\eta y}}\\ 0\\ \end{array}}}\right]$ (32)

we apply a cross product of vector $({\hat{B}_{N}}\quad{\hat{B}_{E}}\quad 0)^{T}$ on each side of Eq. (30), it is clear that the first two components are zeros on each side of the formula, and get the third component equation as follow:

$\displaystyle B_{N}\hat{B}_{E}-\hat{B}_{N}B_{E}=\phi_{z}\left({v_{Bx}\hat{B}_{% E}-\hat{B}_{N}v_{By}}\right)+v_{\eta x}\hat{B}_{E}-\hat{B}_{N}v_{\eta y}$ (33)

And then the measurement matrix of magnetometer is

$\displaystyle{{\bm{H}}}_{\text{MAG}}=\left[{{\begin{array}[]{*{20}c}0&0&{\left% ({v_{Bx}\hat{B}_{E}-\hat{B}_{N}v_{By}}\right)}&0&0&0\\ \end{array}}}\right]$ (34)

And the innovation is

$\displaystyle\bm{\Delta}_{\text{MAG}}=B_{N}\hat{B}_{E}-\hat{B}_{N}B_{E}$ (35)

Note that the measurement noise become to $v_{\eta x}\hat{B}_{E}-\hat{B}_{N}v_{\eta y}$ , and can be obtained directly by magnetometer noise performance.

4.2 Computational complexity analysis

Based on the above derivations, the novel measurement model reduces the dimension from [11]’s 6 to 4, and compare to [17] the measurement noise auto-covariance matrix can be determined directly, which allows the user tuning the filter parameters conveniently. When taking batch operations, the proposed method only need to compute the inverse and multiplications of 4 by 4 matrix, while the [11]’s method must compute a 6 by 6 matrix’s inverse. Because of the computational complexity of inverse operation of p-th order matrix is p ${}^{3}$ , the computational cost of proposed method is only 30% of the method in [11]. When taking sequence operations, it is obvious that the proposed method’s calculation consumption is 2/3 of the [11]’s method.

5. Experiments

In this section, we will verify the performance of the proposed method in two aspects: the first is the precision of attitude estimation, and the second is the computational cost. As a comparison, the same experiment is carried out on the extended Kalman algorithm based on the TRIAD measurement model in [17] and based on the vector measurement model in [10], respectively. Algorithms are implemented in a self-developed embedded flight control system based on STM32F407 microprocessor, shown in Fig. 1.

Figure 1.

Experiment hardwares: self-developed flight control system (left) and three-axis turntable (right).

The micro-processor is running at 168 MHz, and the three algorithm operating at 50 Hz. Furthermore, the on-chip timer is used to calculate the algorithm time-consuming. In experiment, we utilize both of step and sinusoidal motions in each axis by a three-axis turntable (see Fig. 1) to test the performance of methods. Specially, the step motion is from zero position to 45 degrees position in each axis, with the angular velocity of 300 degrees per second and the angular acceleration of 300 degrees per second. The amplitude of sinusoidal motion is 45 degrees, with roll and pitch at a frequency of 1 Hz and, limited to the capacity of turntable, yaw at a frequency of 0.25 Hz.

Our filter initial states are all set to zero, and the standard deviation of noises are set as follow: the gyro and gyro bias process noise is 0.03 rad/s and 0.0001 rad/s, respectively; the magnetometer measurement noise is 0.05 Tesla; the accelerometer measurement noise is 0.6 m/s ${}^{2}$ . Experiment results are shown in Figs 2–5, and the filtering error and time consumption of each method is shown in Tables 1 and 2, respectively.

Figure 2.

Roll step motion results.

Figure 3.

Pitch step motion results.

Figure 4.

Yaw step motion results.

Table 1

Filtering error

Algorithm	Steady-error of step motion	Tracking error of sine motion
TRIAD based	1.75 degrees	1.83 degrees
Vector based	1.68 degrees	1.85 degrees
Proposed	1.59 degrees	1.62 degrees

Table 2

Calculates consumption

Algorithm	Computation time
	Maximum	Average
TRIAD based	410 us	392.4 us
Vector based	504 us	475.8 us
Proposed	317 us	294.6 us

Figure 5.

Sinusoidal motion test results.

Figures 2–4 and Table 1 show the results of step motion test. From Figs 2 and 3, we can see that all the three methods can track the change of attitude without overshoot in roll and pitch axis. In contrast, the proposed method has the shortest adjustment time. In the yaw axis step experiment shown in Fig. 4, the TRIAD measurement model and the vector measurement model based methods appear overshoot as the torque motor of the turntable generates additional magnetic fields when activation, that affect the magnetic measurement significantly. In contrast, the proposed method, although fluctuating during the motor start-up, can still track changes in roll angle without overshooting. Figure 5 and Table 1 shows the results of the sinusoidal motion test, and it can be seen from the figure that all three methods can track the sinusoidal motion well, and the proposed method has high filtering accuracy. As can be seen from Table 2, the proposed method has the lowest computational cost, which is approximately 25% lower than the TRIAD-based method and 38% lower than the vector-based method. In addition, when applied to different real systems, both the proposed method and the vector-based method are capable of setting measurement noise directly from the sensor’s datasheet or actual measurement. However, TRIAD-based methods need long-term parameter adjustment based on speculation and experience to obtain satisfactory filtering effects.

6. Conclusions

Aiming at the attitude estimation problem of UAV, this paper firstly presents a extended Kalman filter that uses the attitude error described by the rotation vector and gyro drift as the state vector, and utilizes the gravity and magnetic field measurements for information fusion. And then by transforming the measured magnetic field vector in body frame to reference frame and applying the vector cross production, we propose a new measurement model using MARG sensors. The proposed method has three main advantages. First of all, it is not necessary to obtain the complete geomagnetic information in the location of the drone, which simplifies the complex calculation of the local reference geomagnetic field vector. Secondly, the magnetic field measurement model is also reduced from three-dimensional to one-dimensional, greatly reducing the amount of computation required for data fusion of the magnetic field; Finally, the original measurement noise information of sensors can be used directly to facilitate the determination and adjustment of the filter parameters. The algorithm was implemented on the Cortex-M4 microprocessor platform, and relied on three-axis turntable for experiments. The results show that the method proposed in this paper has a lower computational cost than the previous methods, and can estimate the attitude of the UAV in real time. Furthermore, we would try to improve the gravity measurement model to further reduction of the dimensions in future.

References

Grewal

M.S.

and Andrews

A.P.

, Applications of Kalman Filtering in Aerospace 1960 to the Present, IEEE Control Systems 30(3) (2010), 69–78.

Feng

Zhang

et al., A New Quaternion-Based Kalman Filter for Real-Time Attitude Estimation Using the Two-Step Geometrically-Intuitive Correction Algorithm, Sensors 17(9) (2017), 2146–2156.

Zhang

and Liao

, Attitude measure system based on extended Kalman filter for multi-rotors, Computers and Electronics in Agriculture 134 (2017), 19–26.

Lee

George

and Lee

, Robust Adaptive Unscented Kalman Filter for Spacecraft Attitude Estimation Using Quaternion Measurements, Journal of Aerospace Engineering 30(4) (2017), 04017009.

Zhang

Evangelos

et al., Navigation with IMU/GPS/digital compass with unscented Kalman filter, in: Proceedings of 2005 IEEE International Conference on Mechatronics and Automation, 2005, pp. 1497–1502.

Cui

Chen

et al., Performance analysis of improved iterated cubature Kalman filter and its application to GNSS/INS, ISA Transactions 66 (2017), 460–468.

Poddar

Narkhede

Kumar

et al., PSO Aided Adaptive Complementary Filter for Attitude Estimation, Journal of Intelligent & Robotic Systems, 2017, 1–13.

Euston

Coote

Mahony

et al., A complementary filter for attitude estimation of a fixed-wing UAV, in: Proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems, 2008, pp. 340–345.

Zamani

Trumpf

and Mahony

, Minimum-Energy Filtering for Attitude Estimation, IEEE Transactions on Automatic Control 58(11) (2013), 2917–2921.

10.

Markley

F.L.

, Attitude error representations for Kalman filtering, Journal of Guidance Control and Dynamics 26(2) (2003), 311–317.

11.

Pittelkau

M.E.

, Rotation vector in attitude estimation, Journal of Guidance Control and Dynamics 26(6) (2003), 855–860.

12.

Bishop

and Welch

, An introduction to the kalman filter, Proc of SIGGRAPH, Course 8 (2001), 41.

13.

Zhang

Yang

et al., Multiplicative filtering for spacecraft attitude determination, Journal of National University of Defense Technology 35(6) (2013), 46–52.

14.

Marina

Pereda

F.J.

Giron-Sierra

J.M.

et al., UAV attitude estimation using unscented Kalman filter and TRIAD, IEEE Transactions on Industrial Electronics 59(11) (2012), 4465–4474.

15.

Marins

J.L.

Yun

Bachmann

E.R.

et al., An extended Kalman filter for quaternion-based orientation estimation using MARG sensors, in: Proceedings of 2001 IEEE/RSJ International Conference on Intelligent Robots and Systems, 2001, pp. 2003–2011.

16.

Shuster

M.D.

, The QUEST for better attitudes, The Journal of the Astronautical Sciences 54(3) (2006), 657–683.

17.

Jing

Cui

et al., Attitude estimation for UAV using extended Kalman filter, in: Proceedings of 29th Chinese Control And Decision Conference, 2017, pp. 3307–3312.

18.

Valenti

R.G.

Dryanovski

and Xiao

, Keeping a good attitude: A quaternion-based orientation filter for IMUs and MARGs, Sensors 15(8) (2015), 19302–19330.

19.

Groves

P.D.

, Principles of GNSS, inertial, and multisensor integrated navigation systems (2nd Edition). Artech House, Boston – London, 2013.