Coupled GPS/MEMS IMU Attitude Determination of Small UAVs with COTS

Abstract

This paper proposes an attitude determination system for small Unmanned Aerial Vehicles (UAV) with a weight limit of 5 kg and a small footprint of 0.5 m×0.5 m. The system is realized by coupling single-frequency Global Positioning System (GPS) code and carrier-phase measurements with the data acquired from a Micro-Electro-Mechanical System (MEMS) Inertial Measurement Unit (IMU) using consumer-grade Components-Off-The-Shelf (COTS) only. The sensor fusion is accomplished using two Extended Kalman Filters (EKF) that are coupled by exchanging information about the currently estimated baseline. With a baseline of 48 cm, the static heading accuracy of the proposed system is comparable to the one of a commercial single-frequency GPS heading system with an accuracy of approximately 0.25°/m. Flight testing shows that the proposed system is able to obtain a reliable and stable GPS heading estimation without an aiding magnetometer.

1. Introduction

Within the Helmholtz Alliance for Robotic Exploration of Extreme Environments (ROBEX), small Unmanned Aerial Vehicles (UAVs) are developed to support an Autonomous Underwater Vehicle (AUV) during explorations below Arctic sea ice. In order to operate the AUV successfully at the marginal ice zone, it is crucial to keep track of the permanently-moving ice edge. By deploying GPS-based tracking devices at the ice edge, the ice drift can be continuously observed before and during AUV operations. In a first attempt, the Alfred Wegener Institute for Polar and Marine Research (AWI) deployed suitable tracking systems manually at the ice edge using zodiacs and a radio-controlled UAV. However, the manual deployment of the tracking systems turned out to be dangerous and time consuming [1]. The UAV’s limited operation range was identified as a major disadvantage and created the demand for more autonomous UAVs in Arctic environments.

While a variety of consumer-grade and professional solutions for autonomous UAVs exists, the Arctic environment is still a challenge for small autonomous UAVs. One critical aspect is a reliable heading estimation despite the weak horizontal component of the Earth’s magnetic field in high latitudes. A reliable approach to determine the vehicle heading in the presence of magnetic disturbances is to estimate the relative position between several Global Navigation Satellite System (GNSS) receivers. Typical GNSS attitude determination systems consist of two or more GNSS receivers that are deployed on a moving platform at a sufficiently large distance from each other. The so-called baselines usually range from 1 m for cars up to 40 m or more for aircraft and ships [2,3,4,5]. However, smaller baselines down to 2–3 L1 carrier wavelengths (L1 = 1575.42 MHz) are also possible [6]. In order to obtain the required measurement accuracy, carrier-phase observations are used. Usually, those observations are strongly influenced by error terms, such as tropospheric and ionospheric delay, receiver clock errors, multi-path errors and receiver noise. However, by calculating Single-Difference (SD) between receivers and Double-Difference (DD) between receivers and satellites, some of the error terms in the carrier-phase observation can be eliminated.

Already in the early 1990s, research on GPS-based attitude determination for aircraft was conducted and successfully tested in flight [7,8]. Since then, systems based on high-end dual-frequency (L1 and L2 band) GNSS receivers emerged as the state of the art solution. However, the upcoming of inexpensive MEMS inertial sensors and the growing demand of small UAVs for various tasks encouraged the development of less expensive GNSS attitude determination systems.

Concoli et al. [9] describe the use of multi-GNSS receivers for the attitude determination of a UAV on a conceptional level and simulate the influence of different baseline length.

Falco et al. [10] describe an approach for their 1.5 m×1.5 m footprint UAV based on an array of four single-frequency GPS receivers combined with a consumer-grade MEMS IMU. In their approach, DDs are exploited to calculate the attitude. The heading angle was estimated during a UAV flight test with a mean error below one degree using a short baseline array of 50 cm×50 cm. The navigation system was implemented on the LOGAM platform, a customized board with four GNSS receivers, an IMU, a barometer and a micro-controller. Since no details about the used components are given, a full price for the system cannot be estimated. However, the price for the four utilized Bullet III antennas is about 350$.

Eling et al. [11] combine the measurements of a tactical-grade IMU (ADIS16488) with a dual-frequency (Novatel OEM 615) and a single-frequency (u-Blox Lea6T) GPS receiver. The processing was performed on the National Instruments embedded processing platform sbRIO9606. The dual-frequency receiver is used for the absolute positioning and as one receiver for the moving baseline. The single-frequency receiver is used as the second baseline receiver. With a baseline of 92 cm, a heading accuracy of 0.2° can be achieved. Two geodetic-grade antennas (3G + C, navXperience) were used. Using tactical- to geodetic-grade only, the total system costs are very high compared to the solution proposed in this article.

This article will focus on the GPS/IMU-based attitude determination of a small UAV with a take-off weight of less than 5 kg and a very small footprint of 0.5 m×0.5 m using low-cost, consumer-grade COTS only. The total costs for the proposed navigation system are less than 400$. The system is compared to the commercial Static Heading Determination System from ANavS and tested during flight.

2. System Architecture

The attitude determination system in this approach is based on two single-frequency GNSS receivers (Neo-M8T; u-Blox, Thalwil, Switzerland) with active low-cost patch antennas, a consumer-grade MEMS IMU (LSM9DS0; STMicroelectronics, Geneva, Switzerland) and an embedded processing platform (UDOO Neo; Seco, Arezzo, Italy). The system combines the carrier-phase of the L1 frequency GPS signal, the GPS pseudo-ranges, as well as the accelerometer, magnetometer and gyroscope measurements of the IMU using Real-Time Kinematics (RTK). Figure 1 shows the UAV concept.

The origin of the UAV fixed body frame is in its center of mass; the origin of the RTK frame is fixed to one of the GPS antennas, as shown in the figure. The GPS antennas are mounted on the UAV booms at a distance of 48 cm, while the IMU is mounted in the UAV’s center of mass. The misalignment error due to installation imprecisions between the IMU and GPS antennas is considered to be very small and therefore neglected.

The matrix ${\mathbf{R}}^{br}$ to rotate a vector from the body in the RTK frame is given by:

$${\mathbf{R}}^{br}=\left[\begin{array}{ccc}0& -1& 0\\ -1& 0& 0\\ 0& 0& -1\end{array}\right]$$

(1)

The UAV autopilot and its attitude determination system are based on the UDOO Neo. The UDOO Neo incorporates an i.MX6 SoloX, a heterogeneous dual-core processor from NXP. The processor features an ARM Cortex-A9 running at 1 GHz and an ARM Cortex-M4 at 200 MHz in one chip and allows inter-core communication using shared memory. The A9 core is running Linux and the Robot Operating System (ROS) [12]. The M4 core is running the Real-time Onboard Dependable Operating System (RODOS) [13]. Therefore, the bigger core is used for computationally heavy tasks with soft real-time requirements, like the GPS heading estimation, while the small core handles firm real-time tasks, such as high-frequency attitude determination and attitude control. The computational tasks for the attitude determination of the system are distributed among both processors, as shown in Figure 2.

The Cortex-M4 is interfaced with the IMU and runs the quaternion Extended Kalman Filter (EKF) described in Section 2.2. During initialization, accelerometer, gyroscope and magnetometer measurements are combined in order to estimate the unit quaternion ${\mathbf{q}}^{nb}$ that encodes the rotation of a vector from the North-East-Down (NED) navigation frame into to the body fixed frame. The calculated heading is relative to Magnetic North and therefore distorted by the angular offset between the magnetic and geographic North Pole.

The Cortex-A9 is interfaced with the two raw observation data GPS receivers. It calculates the UAV’s current position, the magnetic declination, as well as the RTK-based GPS heading. The calculation of the current longitude λ and the latitude φ is based on the code-observations from i visible satellites ${\mathbf{P}}_{r,b}^i$ of the rover and the base GPS receiver, respectively. The magnetic declination δ can be calculated using the World Magnetic Model (WMM) [14]. In order to estimate the current GPS heading, the magnetic declination and the distorted attitude quaternion ${\mathbf{q}}^{nb}$ are used to express the baseline with respect to the NED frame:

$$\Delta {\mathbf{x}}^n=\mathit{C}\left(\delta ,{\mathbf{q}}^{nb}\right)={\mathbf{R}}_z\left(\delta \right)·{\mathbf{R}}^{nb}·{\mathbf{R}}^{br}·\Delta {\mathbf{x}}^r$$

(2)

where ${\mathbf{R}}_z\left(\delta \right)$ describes an elemental rotation of δ around the z-axis, ${\mathbf{R}}^{nb}$ is the rotation matrix that rotates a vector from the navigation frame into the body frame and can be calculated from ${\mathbf{q}}^{nb}$ and $\Delta {\mathbf{x}}^r={\left[0.4800\right]}^T$ is the baseline expressed in the RTK frame. The baseline $\Delta {\mathbf{x}}^n$ is then transformed into the Earth-Centered Earth-Fixed (ECEF) frame and combined with the phase-based observations ${\mathbf{\Phi }}_{r,b}^i$ using the RTK approach described in Section 2.3. If a valid RTK-based baseline estimation is found, we can calculate the GPS heading ${\gamma }_{GPS}$ in the NED frame. Since the quaternion EKF should be able to easily integrate heading information obtained from the magnetometer or the GPS measurements, the estimated GPS heading will be referenced towards Magnetic North again:

$${\gamma }_{GPS}={\mathit{C}}^{\prime }\left(\delta ,\Delta {\mathbf{x}}^n\right)=\mathrm{atan}2\left(\Delta x_y^n,\Delta x_x^n\right)-\delta$$

(3)

where atan2 returns the four-quadrant inverse tangent of $y/x$ given y and x. Depending on the application and the requirements, either the GPS heading ${\gamma }_{GPS}$, or the magnetometer measurements, or both can be used as input in the attitude determination algorithm. In our application, we want to use the magnetometer as little as possible and, therefore, use it only during the initialization. Nevertheless, a full attitude solution without a magnetic reference at all is possible, as well. In this case, the quaternion EKF is idle until a valid GPS heading is obtained. Although the RTK GPS baseline estimation can be also used to determine the UAV’s roll angle, only the accelerometer and gyroscope measurements are used for the UAV’s roll and pitch estimation. Calibration techniques, as presented in [15], can be used to compensate accelerometer misalignment errors.

2.1. Quaternion Math

This section will briefly review the quaternion math used in the described approach to give a comprehensive view for the reader. A more detailed description is given by Diebel in [16]. A quaternion is a hyper complex number of rank four and can be represented in the following way:

$$\mathbf{q}=q_0+q_1\mathit{i}+q_2\mathit{j}+q_3\mathit{k}$$

(4)

Another, very commonly-used representation considers the first part to be a scalar, while the rest can be expressed as a vector:

$$\mathbf{q}={\left[\begin{array}{cccc}{q}_0& q_1& q_2& q_3\end{array}\right]}^T={\left[\begin{array}{cc}{q}_0& {\overrightarrow{\mathbf{q}}}^T\end{array}\right]}^T$$

(5)

The norm of a quaternion is simply defined by:

$$\mathrm{Norm}\left(\mathbf{q}\right)=∥\mathbf{q}∥=\sqrt{{q_0}^2+{q_1}^2+{q_2}^2+{q_3}^2}$$

(6)

The conjugate of quaternion $\mathbf{q}$ can be obtained by negating its vector part and is denoted by $q^{*}$:

$$\mathrm{Conj}\left(\mathbf{q}\right)={\mathbf{q}}^{*}={\left[\begin{array}{cc}{q}_0& -{\overrightarrow{\mathbf{q}}}^T\end{array}\right]}^T$$

(7)

Unit quaternions can be used to describe rotations in three-dimensional space. For the special case of a unitary quaternion, the inverse of the quaternion equals its conjugate:

$$\mathrm{Inv}\left(\mathbf{q}\right)={\mathbf{q}}^{-1}=\frac{{\mathbf{q}}^{*}}{{∥\mathbf{q}∥}^2}$$

(8)

The product of two quaternions $\mathbf{q}$ and $\mathbf{p}$ is calculated using the Kronecker product and denoted with $\otimes$. Two subsequent rotations $\mathbf{q}$ and $\mathbf{p}$ can be expressed in a single rotation by multiplying both quaternions. It is important to mention that the quaternion multiplication is not commutative.

The quaternion multiplication can therefore be denoted as:

$$\begin{array}{ccc}\mathbf{q}\otimes \mathbf{p}& =& \left[\begin{array}{c}{q}_0{p}_0-\overrightarrow{\mathbf{q}}·\overrightarrow{\mathbf{p}}\\ q_0\overrightarrow{\mathbf{p}}+p_0\overrightarrow{\mathbf{q}}+\overrightarrow{\mathbf{q}}\times \overrightarrow{\mathbf{p}}\end{array}\right]\\ & =& \left[\begin{array}{cccc}{q}_0& -q_1& -q_2& -q_3\\ q_1& q_0& -q_3& q_2\\ q_2& q_3& q_0& -q_1\\ q_3& -q_2& q_1& q_0\end{array}\right]\left[\begin{array}{c}{p}_0\\ p_1\\ p_2\\ p_3\end{array}\right]\\ & =& \mathbf{Q}(\mathbf{q})\mathbf{p}\end{array}$$

(9)

If a measurement of the rotational velocity in the body frame ${\mathit{\omega }}^b$ and a quaternion $\mathbf{q}$ that describes the rotation of a vector from the fixed navigation frame into the body frame are given, we can easily calculate the quaternion’s derivative $\dot{\mathbf{q}}$ with:

$$\dot{\mathbf{q}}=\frac{1}{2}\mathbf{q}\otimes \left[\begin{array}{c}0\\ {\mathit{\omega }}^b\end{array}\right]=\frac{1}{2}\mathbf{Q}\left(\mathbf{q}\right)\left[\begin{array}{c}0\\ {\mathit{\omega }}^b\end{array}\right]$$

(10)

A vector $\mathbf{v}$ given in three-dimensional space can be simply rotated from a fixed frame to a body frame, given that the rotation between the two frames is described by $\mathbf{q}$, with:

$$\left[\begin{array}{c}0\\ \mathbf{w}\end{array}\right]=\mathbf{q}\otimes \left[\begin{array}{c}0\\ \mathbf{v}\end{array}\right]\otimes {\mathbf{q}}^{*}$$

(11)

The same rotation can be obtained by expressing the quaternion q as a rotation matrix from the fixed frame n to the body frame b with:

$$\mathbf{w}={\mathbf{R}}^{nb}\mathbf{v}=\left[\begin{array}{ccc}{q_0}^2+{q_1}^2-{q_2}^2-{q_3}^2& 2\left(q_1{q}_2-q_0{q}_3\right)& 2\left(q_0{q}_2+q_1{q}_3\right)\\ 2\left(q_1{q}_2+q_0{q}_3\right)& {q_0}^2-{q_1}^2+{q_2}^2-{q_3}^2& 2\left(q_2{q}_3-q_0{q}_1\right)\\ 2\left(q_1{q}_3-q_0{q}_2\right)& 2\left(q_0{q}_1+q_2{q}_3\right)& {q_0}^2-{q_1}^2-{q_2}^2+{q_3}^2\end{array}\right]\overrightarrow{v}$$

(12)

For a more convenient way to interpret quaternions, they can be converted to Euler angles using the following equation:

$$\left[\begin{array}{c}\mathsf{\Phi }\\ \mathsf{\Theta }\\ \mathsf{\Psi }\end{array}\right]=\left[\begin{array}{c}\mathrm{atan}2\left(2\left(q_0{q}_1+q_2{q}_3\right),1-2\left({q_1}^2+{q_2}^2\right)\right)\\ \mathrm{asin}\left(2\left(q_0{q}_2-q_1{q}_3\right)\right)\\ \mathrm{atan}2\left(2\left(q_0{q}_3+q_1{q}_2\right),1-2\left({q_2}^2+{q_3}^2\right)\right)\end{array}\right]$$

(13)

2.2. Quaternion Extended Kalman Filter

The IMU measurements are combined with the GPS heading estimation in an EKF. The implemented filter is based on the approach used by Munguia et al. [17], but is augmented by the RTK GPS heading and designed in such a way that update steps with different sensor rates are possible.

2.2.1. Measurement Models

The IMU consists of a three-axis gyroscope, a three-axis accelerometer and a three-axis magnetometer. The gyroscope measurement vector ${\mathbf{y}}_g$ consists of the angular rate in the body frame ${\mathit{\omega }}^b$ and additive errors that can be modeled by:

$${\mathbf{y}}_g={\mathit{\omega }}^b+{\mathbf{b}}_g+{\mathit{\upsilon }}_g$$

(14)

where ${\mathbf{b}}_g$ is the temperature-dependent gyro bias and ${\mathit{\upsilon }}_g$ is Gaussian white noise with a variance of ${{\sigma }_g}^2$. The accelerometer measurement vector ${\mathbf{y}}_a$ is defined by:

$${\mathbf{y}}_a={\mathbf{a}}^b-{\mathbf{g}}^b+{\mathbf{b}}_a+{\mathit{\upsilon }}_a$$

(15)

where ${\mathbf{a}}^b$ is the device acceleration in the body frame, ${\mathbf{g}}^b$ is the gravity vector expressed in the body frame, ${\mathbf{b}}_a$ is the accelerometer bias and ${\mathit{\upsilon }}_a$ is Gaussian white noise with a variance of ${{\sigma }_a}^2$. Since the bias in the accelerometer triads can be minimized through proper calibration methods [15], it will be neglected in this filter.

The magnetometer measurement vector ${\mathbf{y}}_m$ can be described as:

$${\mathbf{y}}_m={\mathbf{m}}^b+{\mathbf{b}}_m+{\mathit{\upsilon }}_m$$

(16)

where ${\mathbf{m}}^b$ is the surrounding magnetic field, ${\mathbf{b}}_m$ is a constant magnetometer bias and ${\mathit{\upsilon }}_m$ is Gaussian white noise with a variance of ${{\sigma }_m}^2$. The magnetometer bias ${\mathbf{b}}_m$ can be compensated for by estimating the so-called hard and soft iron effects using appropriate calibration methods as presented in [18]. Ideally, the surrounding magnetic field ${\mathbf{m}}^b$ is identical to the Earth’s magnetic field. However, metal structures close to the magnetometer, as well as changing currents in the proximity of the magnetometer disturb the Earth’s magnetic field. Compensating for those errors is a very challenging task and not always possible. For simplicity, ${\mathbf{m}}^b$ is assumed to be identical to the Earth’s magnetic field in this filter design.

Finally, the RTK GPS heading measurement $y_{GPS}$ is modeled by:

$$y_{GPS}={\gamma }_{GPS}+{\upsilon }_{GPS}$$

(17)

where ${\gamma }_{GPS}$ describes the north heading referenced to the magnetic pole, in order to allow easier integration of GPS heading and magnetometer measurements, and ${\upsilon }_{GPS}$ is Gaussian white noise with a variance of ${{\sigma }_{GPS}}^2$.

2.2.2. System Propagation

The system state $\mathbf{x}\left(k\right|k)$ at the time k is given by:

$$\widehat{\mathbf{x}}\left(k|k\right)={\left[\begin{array}{ccc}{{\mathbf{q}}^{nb}\left(k|k\right)}^T& {{\mathit{\omega }}^b\left(k|k\right)}^T& {{\mathbf{b}}_g^{}\left(k|k\right)}^T\end{array}\right]}^T$$

(18)

where ${\mathbf{q}}^{nb}$ is a unit quaternion representing the orientation of the UAV fixed body frame with respect to the navigation frame and ${\mathit{\omega }}^b$ and ${\mathbf{b}}_g$ are defined according to Equation (14).

The system state can be predicted at fixed rate $\Delta t$ with the following propagation model:

$$\widehat{\mathbf{x}}\left(k+1|k\right)=f\left(k,\widehat{\mathbf{x}}\left(k|k\right),{\mathbf{y}}_g\left(k|k\right)\right)=\left[\begin{array}{c}{\mathbf{q}}^{nb}+\Delta t·\underset{\begin{array}{c}{\dot{\mathbf{q}}}^{nb}\end{array}}{\underbrace{\frac{1}{2}{\mathbf{q}}^{nb}\otimes \left[\begin{array}{c}0\\ {\mathit{\omega }}^b\end{array}\right]}}\\ {\mathbf{y}}_g-{\mathbf{b}}_g\\ \left(1-{\lambda }_{xg}·\Delta t\right){\mathbf{b}}_g\end{array}\right]$$

(19)

where ${\dot{\mathbf{q}}}^{nb}$ is the quaternion derivative and ${\lambda }_{xg}$ is a time correlation factor that models how fast the gyroscope bias ${\mathbf{b}}_g$ can vary.

The system state covariance matrix $\mathbf{P}$ can be propagated by:

$$\mathbf{P}\left(k+1|k\right)={\nabla \mathbf{F}}_x\mathbf{P}\left(k\right|k){{\nabla \mathbf{F}}_x}^T+\mathbf{Q}$$

(20)

where ${\nabla \mathbf{F}}_x$ is the Jacobian of the system prediction function f and $\mathbf{Q}$ is the system noise covariance matrix. The Jacobian of the system prediction function is evaluated for the latest state estimate and given by:

$$\nabla {\mathbf{F}}_x={\left.\frac{\partial f\left(k\right)}{\partial \mathbf{x}}\right|}_{\mathbf{x}=\widehat{\mathbf{x}}\left(k\right|k)}$$

(21)

The system noise covariance matrix can be expressed as:

$$\mathbf{Q}=\left[\begin{array}{ccc}{0}_{4\times 4}& 0& 0\\ 0& {{\sigma }_g}^2·{\mathbf{I}}_{3\times 3}& 0\\ 0& 0& {{\sigma }_{b_g}}^2·{\mathbf{I}}_{3\times 3}\end{array}\right]$$

(22)

where ${{\sigma }_g}^2$ is defined according to Equation (14) and ${{\sigma }_{b_g}}^2$ is the variance of the gyroscope bias.

2.2.3. System Updates

The filter is designed in such a way that measurement updates can occur independently from each other. While the system state is propagated at fixed intervals of 5 ms, the accelerometer updates occur only if the body is not accelerating. The magnetometer measurements are only used as long as no valid GPS heading is available, and the GPS heading is integrated at 10 Hz as soon as it is computed by the RTK GPS heading estimation. The measurement update for the system state and the system state covariance is given by the following equations:

$$\widehat{\mathbf{x}}(k+1|k+1)=\widehat{\mathbf{x}}(k+1|k)+W\left({\mathbf{z}}_i-{\widehat{\mathbf{z}}}_i\right)$$

(23)

$$\mathbf{P}(k+1|k+1)=\mathbf{P}(k+1|k)+\mathbf{W}{\mathbf{S}}_i{\mathbf{W}}^T$$

(24)

where ${\mathbf{z}}_i$ is the current measurement and ${\widehat{\mathbf{z}}}_i$ is the measurement prediction. $\mathbf{W}$ is the Kalman filter gain, and ${\mathbf{S}}_i$ is the residual covariance matrix. i describes the sensor used for the particular measurement update and can be summarized as $i\in \left\{a,{\gamma }_m,{\gamma }_{GPS}\right\}$: a represents the accelerometer measurement update, ${\gamma }_m$ the yaw angle update based on the magnetometer and ${\gamma }_{GPS}$ the yaw angle update based on the estimated GPS heading. The residual covariance matrix ${\mathbf{S}}_i$ can be calculated by:

$${\mathbf{S}}_i=\nabla {\mathbf{H}}_i\mathbf{P}(k+1|k){\nabla {\mathbf{H}}_i}^T+{\mathbf{R}}_i$$

(25)

where $\nabla {\mathbf{H}}_i$ is the Jacobian of the respective measurement prediction model $h_i$ evaluated for the current system state prediction:

$$\nabla {\mathbf{H}}_i={\left.\frac{\partial h_i(k+1)}{\partial \mathbf{x}}\right|}_{\mathbf{x}=\widehat{\mathbf{x}}(k+1|k)}$$

(26)

Finally, the Kalman filter gain is given by:

$$\mathbf{W}=\mathbf{P}(k+1|k){\nabla {\mathbf{H}}_i}^T{{\mathbf{S}}_i}^{-1}$$

(27)

2.2.4. Roll and Pitch Update

For a non-accelerating body, Earth’s gravitational acceleration can be used to estimate the body’s roll and pitch angle. If the accelerometer measurements remain within certain boundaries, the UAV is assumed to be non-accelerating. A comprehensive evaluation of different approaches to detect an accelerating body is given by Skog et al. [19]. The algorithm utilized in this approach to detect non-accelerated conditions is the stance hypothesis optimal detector. In order to use the gravity vector as an external reference, the Earth’s gravitational acceleration measured in the navigation frame needs to be expressed in the body frame using the latest attitude estimation. The measurement prediction is then given by:

$${\widehat{\mathbf{z}}}_a=h_a\left[k+1,\widehat{\mathbf{x}}(k+1|k)\right]={\mathbf{R}}^{bn}\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]$$

(28)

where ${\mathbf{R}}^{bn}$ is the inverse of ${\mathbf{R}}^{nb}$ and can be used to represent a vector given in the navigation frame with respect to the body frame. ${\mathbf{R}}^{nb}$ can be calculated from ${\mathbf{q}}^{nb}$ using Equation (12).

For the gravitational acceleration, we can assume $g=-1$, if the raw accelerometer measurements ${\mathbf{y}}_a$ are normalized:

$${\mathbf{z}}_a=\frac{{\mathbf{y}}_a}{|{\mathbf{y}}_a|}$$

(29)

With accelerometer variance of ${{\sigma }_a}^2$, the measurement noise covariance matrix is given by:

$${\mathbf{R}}_a={{\sigma }_a}^2{\mathbf{I}}_{3\times 3}$$

(30)

2.2.5. Yaw Update

In a similar manner, different external heading reference systems can be used to estimate the body’s yaw angle. In this approach, the external heading solution is provided from a magnetometer and a GPS-based heading system. The magnetometer is only used for the initial heading determination until a valid GPS solution is found. Once a valid solution is found, only the GPS estimated heading is used in the yaw update step. Using the latest attitude estimation, the current heading angle can be predicted by:

$${\widehat{\mathbf{z}}}_{{\gamma }_{GPS}}={\widehat{\mathbf{z}}}_{{\gamma }_m}=h_{\gamma }\left[k+1,\widehat{\mathbf{x}}(k+1|k)\right]=\mathrm{atan}2\left(2\left(q_0{q}_3+q_1{q}_2\right),1-2\left({q_2}^2+{q_3}^2\right)\right)$$

(31)

Using a magnetometer, the magnetic heading can be obtained by projecting the magnetic field observation into the northeast plane. The projected magnetic field vector ${\mathbf{m}}_p^n$ can be calculated by rotating the observed magnetic field vector ${\mathbf{y}}_m$ into the navigation frame and removing its z component:

$$\begin{array}{ccc}{\mathbf{m}}^n& =& {\mathbf{R}}^{nb}{\mathbf{y}}_m\\ {\mathbf{m}}_p^n& =& {\left[\begin{array}{cccc}0& m_x^n& m_y^n& 0\end{array}\right]}^T\end{array}$$

(32)

After the z component of the transferred vector ${\mathbf{m}}^n$ is removed, it is transferred back into the system’s body frame:

$${\mathbf{m}}^b={\mathbf{R}}^{bn}{\mathbf{y}}_m$$

(33)

Finally, the magnetic heading is given by:

$$z_{{\gamma }_m}=\mathrm{atan}2\left(-{m_y}^b,{m_x}^b\right)$$

(34)

Using a GPS heading reference system that provides the Magnetic North heading as output, the GPS yaw measurement is simply obtained by:

$$z_{{\gamma }_{GPS}}={\gamma }_{GPS}$$

(35)

In both cases, for the GPS and the magnetic reference system, the yaw measurements $z_{{\gamma }_j}$ are assumed to have Gaussian white noise with a variance of ${{\sigma }_{{\gamma }_j}}^2$ where $j\in \left\{m,GPS\right\}$. The according measurement noise covariance matrices are therefore given by:

$$R_{{\gamma }_j}={{\sigma }_{{\gamma }_j}}^2$$

(36)

Similar to the non-accelerating body detection for the roll and pitch update, invalid GPS measurements have to be detected and should not be integrated into the final attitude estimation of the quaternion EKF. Two criteria are applied at every GPS yaw update step in order to validate whether the estimated RTK GPS heading is reliable or not. First, the length of the estimated baseline has to be within an acceptable range if compared to the a priori known value, and second, the GPS heading must be based on an integer fixed solution.

During filter initialization, the sampling variance is additionally compared to the expected variance of the GPS heading in a static scenario. The GPS/IMU sensor fusion will be only started if the observed sampling variance is below a fixed threshold.

2.3. Real-Time Kinematics

The GPS heading estimation system is combining code measurements, carrier-phase measurements, the a priori known baseline length and the current baseline orientation in an EKF. The filter approach is based on the open source Real-Time Kinematics Library (RTKLIB) by Takasu [20], but augmented by the baseline orientation measurement and, therefore, the IMU coupling. The EKF tries to estimate the position and velocity of one antenna (rover) relative to the other antenna (base), as well as the carrier-phase ambiguity float solutions. The obtained float solutions are then resolved into fixed integer solutions. Once a valid three-dimensional baseline estimation is calculated, its heading can be easily derived.

Since only L1 carrier-phase signals are considered, the measurement models for the DD carrier phase ${\mathbf{\Phi }}_{rb}^{ij}$ and the DD pseudo ranges ${\mathbf{P}}_{rb}^{ij}$ are given by:

$${\mathbf{\Phi }}_{rb}^{ij}={\mathit{\rho }}_{rb}^{ij}+{\lambda }_1\left({\mathbf{B}}_{rb,1}^i-{\mathbf{B}}_{rb,1}^j\right)+{\mathit{ϵ}}_{\mathsf{\Phi }}$$

(37)

$${\mathbf{P}}_{rb}^{ij}={\mathit{\rho }}_{rb}^{ij}+{\mathit{ϵ}}_P$$

(38)

where ${\mathit{\rho }}_{rb}^{ij}$ is the geometric range DD, ${\lambda }_1$ is the L1 carrier wavelength and ${\mathit{ϵ}}_{\mathsf{\Phi },P}$ are the carrier phase and pseudo range measurement errors, respectively. In order to avoid the hand-over problems when the reference satellite is changing [20], internally Single-Difference ambiguities ${\mathbf{B}}_{rb,1}^i$ are used instead of Double-Difference ambiguities ${\mathbf{B}}_{rb,1}^{ij}$. The system state for the EKF is therefore given by:

$$\widehat{\mathbf{x}}\left(k\right|k)={\left[\begin{array}{ccc}{{\mathbf{r}}_r\left(k\right|k)}^T& {{\mathbf{v}}_r\left(k\right|k)}^T& {{\mathbf{B}}_{rb,1}^i\left(k\right|k)}^T\end{array}\right]}^T$$

(39)

where ${\mathbf{r}}_r$ and ${\mathbf{v}}_r$ are the rover position and velocity, respectively. The system state and its covariance can be predicted by:

$$\widehat{\mathbf{x}}(k+1|k)=\mathbf{F}\widehat{\mathbf{x}}\left(k\right|k)$$

(40)

$$\mathbf{P}\left(k+1|k\right)=\nabla \mathbf{F}\mathbf{P}\left(k\right|k){\nabla \mathbf{F}}^T+\mathbf{Q}$$

(41)

The system propagation matrix $\mathbf{F}$ and the system noise covariance $\mathbf{Q}$ are given by:

$$\mathbf{F}=\left[\begin{array}{ccc}{\mathbf{I}}_{3\times 3}& \Delta t·{\mathbf{I}}_{3\times 3}& 0\\ 0& {\mathbf{I}}_{3\times 3}& 0\\ 0& 0& {\mathbf{I}}_{m\times m}\end{array}\right],\mathbf{Q}=\left[\begin{array}{ccc}{0}_{3\times 3}& 0& 0\\ 0& {\mathbf{Q}}_v& 0\\ 0& 0& 0_{m\times m}\end{array}\right]$$

(42)

where $\Delta t$ is the sample time of the GPS receiver and with:

$${\mathbf{Q}}_v={{\mathbf{R}}^{en}}^T\mathrm{diag}\left({\sigma }_{vn}^2\Delta t,{\sigma }_{ve}^2\Delta t,{\sigma }_{vd}^2\Delta t\right){\mathbf{R}}^{en}$$

(43)

where ${\mathbf{R}}^{en}$ describes a rotation matrix from the ECEF frame into the local coordinate frame (NED) at the receiver antenna position and ${\sigma }_{vn}$, ${\sigma }_{ve}$ and ${\sigma }_{vd}$ are the standard deviations of the rover velocity in north, east and down direction, respectively.

The measurement vector in RTKLIB consists of the baseline length $|{\Delta x}^e|$, the DD carrier-phase measurements ${\mathbf{\Phi }}_{rb}^{ij}$ and the DD pseudo-range measurements ${\mathbf{P}}_{rb}^{ij}$, where the subscript r is the rover, the subscript b is the base, the superscript j is the reference satellite and the superscript $i\in \{1,2,\dots ,m\}$ is the number of valid DD measurements. This measurement vector is extended by $\Delta x^e$, the baseline vector in the ECEF frame, and thus reads:

$$z={\left[\begin{array}{cccccccc}{\mathbf{\Phi }}_{rb}^{i1}& \dots & {\mathbf{\Phi }}_{rb}^{im}& {\mathbf{P}}_{rb}^{i1}& \dots & {\mathbf{P}}_{rb}^{im}& |{\Delta \mathbf{x}}^e|& {{\Delta \mathbf{x}}^e}^T\end{array}\right]}^T$$

(44)

The measurement update equations for the system state and the system state covariances are similar to the quaternion EKF approach and can be derived from Equations (23)–(27).

The measurement prediction matrix is given by:

$$\widehat{\mathbf{z}}=h\left[k+1,\widehat{\mathbf{x}}(k+1|k)\right]=\left[\begin{array}{c}{\mathbf{h}}_{{\mathsf{\Phi }}_{rb,1}}\\ {\mathbf{h}}_{P_{rb,1}}\\ {\mathbf{h}}_{\Delta x^e}\\ {\mathbf{h}}_{|{\Delta x}^e|}\end{array}\right]=\left[\begin{array}{c}{\mathbf{h}}_{{\mathsf{\Phi }}_{rb,1}}\\ {\mathbf{h}}_{P_{rb,1}}\\ {\mathbf{r}}_r-{\mathbf{r}}_b\\ |{\mathbf{r}}_r-{\mathbf{r}}_b|\end{array}\right]$$

(45)

where ${\mathbf{r}}_b$ is the code-based calculated single position of the base antenna and ${\mathbf{h}}_{{\mathsf{\Phi }}_{rb,1}}$ and ${\mathbf{h}}_{P_{rb,1}}$ depend on the DD geometric range ${\mathit{\rho }}_{rb,1}^{ij}$ and L1 the carrier wavelength ${\lambda }_1$ as follows:

$${\mathbf{h}}_{{\mathsf{\Phi }}_{rb,1}}=\left[\begin{array}{c}{\rho }_{rb,1}^{i1}+{\lambda }_1\left(B_{rb,1}^i-B_{rb,1}^1\right)\\ {\rho }_{rb,1}^{i2}+{\lambda }_1\left(B_{rb,1}^i-B_{rb,1}^2\right)\\ ⋮\\ {\rho }_{rb,1}^{im}+{\lambda }_1\left(B_{rb,1}^i-B_{rb,1}^m\right)\end{array}\right],{\mathbf{h}}_{P_{rb,1}}=\left[\begin{array}{c}{\rho }_{rb,1}^{i1}\\ {\rho }_{rb,1}^{i2}\\ ⋮\\ {\rho }_{rb,1}^{im}\end{array}\right]$$

(46)

This yields the following measurement Jacobian matrix:

$$\begin{array}{cc}\mathbf{H}=\left[\begin{array}{ccc}-\mathbf{D}\mathbf{E}& 0& {\lambda }_1\mathbf{D}\\ -\mathbf{D}\mathbf{E}& 0& 0\\ \frac{{\left({\mathbf{r}}_r-{\mathbf{r}}_b\right)}^T}{\left|{\mathbf{r}}_r-{\mathbf{r}}_b\right|}& 0& 0\\ {\mathbf{I}}_{3\times 3}& 0& 0\end{array}\right],& \mathbf{D}=\left[\begin{array}{cccc}1& -1& & \\ ⋮& & \ddots & \\ 1& & & -1\end{array}\right]\end{array}$$

(47)

where $\mathbf{D}$ is the double differencing matrix and $\mathbf{E}={({\mathbf{e}}_r^1,{\mathbf{e}}_r^2,...,{\mathbf{e}}_r^m)}^T$ describes the line of sight vectors from the rover antenna to the satellite $i\in \{1,2,\dots ,m\}$. Finally, the measurement noise matrix $\mathbf{R}$ is given by:

$$\mathbf{R}=\left[\begin{array}{cccc}\mathbf{D}{\Sigma }_{\mathsf{\Phi }}{\mathbf{D}}^T& 0& 0& 0\\ 0& \mathbf{D}{\Sigma }_P{\mathbf{D}}^T& 0& 0\\ 0& 0& {{\sigma }_{|{\Delta x}^e|}}^2& 0\\ 0& 0& 0& {\Sigma }_{{\Delta x}^e}\end{array}\right]$$

(48)

where ${\Sigma }_{\mathsf{\Phi },P}=diag({{\sigma }_{\mathsf{\Phi },P}^1}^2,\dots ,{{\sigma }_{\mathsf{\Phi },P}^m}^2)$ describes the variances of the carrier-phase and the pseudo-range measurements, respectively. ${\sigma }_{|{\Delta x}^e|}$ is the accuracy of the a priori known baseline length and ${\Sigma }_{{\Delta x}^e}$ the accuracy of the estimated baseline orientation using the IMU coupling given by:

$${\Sigma }_{{\Delta x}^e}={{\mathbf{R}}^{en}}^{T}diag\left({{\sigma }_n}^2,{{\sigma }_e}^2,{{\sigma }_d}^2\right){\mathbf{R}}^{en}$$

(49)

where ${{\sigma }_n}^2$, ${{\sigma }_e}^2$, ${{\sigma }_d}^2$ describe the accuracy of the IMU baseline estimation in the NED frame.

The results of this measurement update are real-valued DD ambiguities. Fixing these real-valued ambiguities to integers improves the quality of the solution significantly. The integer ambiguity resolution is done for each measurement instantaneously using the MLAMBDAmethod [21]. At this point, it should be pointed out that the baseline length constraint, as well as the baseline orientation constraint are used iteratively when estimating the float solution. No additional baseline constraints are applied during the integer fixing step, as is the case for the baseline-constraint LAMBDA method proposed by [22].

Once the integer ambiguities are fixed, the RTK-based baseline estimation in the ECEF frame is simply given by:

$$\Delta {\mathbf{x}}^e={\mathbf{r}}_r(k+1|k+1)-{\mathbf{r}}_b(k+1|k+1);$$

(50)

Using our current position estimate, this can be simply transformed into the NED frame.

3. Evaluation

In order to validate the performance of the proposed approach, various experiments were conducted using a real-world system. All experiments were carried out on the UAV platform shown in Figure 3. The experiments were performed in Würzburg, Germany, on different days. To increase the quality of the GPS raw measurements, ground planes were used to reduce multipath effects as suggested by [23]. The GPS raw measurements were obtained at 10 Hz, while the IMU provided data at 200 Hz.

The following section will discuss two of the tested scenarios in detail with the focus on the accuracy of the heading determination. Therefore, the system is first compared to other available GPS heading solutions in a static scenario. Subsequently, the system is evaluated during flight, covering the typical dynamic range of the system.

3.1. Static Evaluation

In the static scenario, the proposed method is compared to the results of the RTKLIB only and a commercial system, the Static Heading Determination System from ANavS. The results of the ANavS system are considered to be the ground truth. The proposed algorithm is implemented on the UAV itself, and the here presented data are calculated on-board in real time while the results for the two references systems were obtained by post-processing the recorded raw data. The proposed method is compared to the ANavS reference system in Figure 4 for the total experiment duration. After a short initialization time, both systems provide a valid, stable and accurate heading solution. Although the UAV was placed on a field with a sufficient distance to trees and buildings in the attempt to provide a clear view of the sky for both GPS antennas for this experiment, two satellites’ locks were temporarily lost during the measurement. The impact of the loss of the two satellite signals can be clearly seen after approximately 251 s. No valid reference heading could be provided during this time.

Comparing the system precision, it can be seen that the combined GPS/IMU approach has much lower noise than the reference solution.

Table 1. Standard deviations during the static scenario.

System	ANavS	QEKF/RTK	RTK	RTKLIB
$\mathbf{\sigma }$	$0.{37}^{\circ }$	$0.{039}^{\circ }$	$0.{30}^{\circ }$	$0.{35}^{\circ }$

lists the measured standard deviations during this static scenario for the ANavS reference system, the proposed method and its RTK GPS-only solution, as well as the standard RTKLIB solution. The standard deviations are calculated for the time between the first fix of all systems ($t=45\mathrm{s}$) until the two satellite locks are lost ($t=245\mathrm{s}$). Due to the combination of the high data rate IMU and the RTK GPS heading, the system precision is significantly improved by the quaternion EKF.

Figure 5 shows the progress of the yaw estimates for the different systems from a cold start until fixed integer solutions are obtained. All systems obtain a valid solution after less than 45 s.

The reference system from ANavS does not provide any heading information until the carrier-phase integer ambiguities are fixed and a possible solution is found. The time until the first heading estimation is provided takes under open-sky conditions approximately 15 s. During the first 30 s, the orbital data of the GPS satellites are gathered.

The proposed system is initialized based on the observed magnetic heading. In addition to the overall system output, the RTK GPS heading estimates are also shown. Thereby, a comparison between the different systems can be done in a more convenient way. After the orbital data are collected, the integer ambiguities are fixed for every epoch using the baseline length and its currently-estimated orientation. The jump after approximately 21 s indicates the moment when enough orbital data are acquired to estimate the receiver position, and the WMM declination corrections can be applied. In this scenario, the first correct ambiguity fix is obtained after 123 epochs, which equals 12.3 s. Nevertheless, it takes some additional time for the quaternion EKF to adapt the gyroscope biases and adjust its heading estimation.

As a second reference, the initialization of RTKLIB’s static heading estimation only is also shown in Figure 5. The data are obtained using the RTKLIB default settings for a moving baseline of 48 cm. Using the RTKLIB’s fix and hold approach for the integer ambiguities, the first correct heading is estimated 19.8 s after the orbital data are obtained.

Figure 6 shows the impact of the aforementioned loss of satellite lock in more detail. After approximately 251 s, the signal of two satellite locks was lost presumably by accidentally shielding the GPS antennas in a specific direction.

Figure 7 shows the Signal-to-Noise Ratio (SNR) of the tracked satellites during the satellite loss for one of the GPS receivers. In total, eight satellites are tracked. The SNR of Satellites G27 and G32 drops below 28 dB after 251 s and recovers again after approximately 15 s. Additionally, the signal strength of the four other satellites is decreased during this time.

Although shielding is a very unlikely scenario for the final application, the reaction of the different systems is quite interesting. While the commercial system re-obtains a valid heading first at $t\approx 265\mathrm{s}$, there are strong fluctuations and a temporary error of 20 degrees. In contrast to that, the proposed method takes admittedly longer to re-obtain a correct fix ($t\approx 275\mathrm{s}$). However, the maximum error of the proposed method remains under five degrees due to the IMU stabilization. Additionally, no sudden fluctuations can be observed, allowing a more stable flight. In contrast, the RTKLIB solution does not manage to obtain a valid GPS heading again.

During the satellite loss, the measurements of the IMU and the RTK GPS heading are contradicting each other. While the gyroscope measurements correctly indicate no change in orientation, the GPS heading indicates a change. Instead of trusting the GPS heading blindly, the gyroscope biases are adapted according to the time correlation factor ${\lambda }_{xg}$ in Equation (19), while the IMU estimated baseline orientation constraint allows one to keep the RTK GPS heading stable.

3.2. Dynamic Evaluation

In the second scenario, the proposed system is tested during flight. After the initial GPS fix is obtained, the UAV takes off and hovers at a height of 5 m for several seconds. Next, the UAV flies in a clock-wards circle of approximately 11 m in diameter while facing away from the circle center. The circle is completed after 30 s. Subsequently, the UAV moves up and down at roughly the same position without changing its heading. The recorded raw GPS positions of one receiver are shown in Figure 8.

The upper plot in Figure 9 shows the estimated heading during the flight. Again, in addition to the overall system output, the GPS heading estimates are shown. Furthermore, the heading calculated by post-processing the recorded data from the IMU and the magnetometer measurements is displayed, as well. Since the ANavS system is designed for static applications and due to the lack of a lightweight reference system that can be mounted on the UAV platform, the magnetic heading during flight is considered as the ground truth. It shall be noted that the magnetic heading is error-prone due to magnetic disturbances by the UAV’s motors during flight and therefore can only provide an accuracy of a few degrees. Nevertheless, it can still be used to verify the functionality of the proposed method in flight.

The time until all orbit data are gathered takes again roughly 30 s. In contrast to the static evaluation, an integer ambiguity fix could be obtained immediately with the first measurement. Both systems show a smooth circular heading estimation and remain constant once the circular flight is completed. The differences between the magnetic and the GPS-based heading can be caused by the poor calibration of the constant magnetic offsets and the aforementioned additional time-varying magnetic effects, e.g., high electrical currents in the sensor vicinity due to the UAV operation. The lower plot shows whether the update criteria for the yaw update are met or not (see Section 2.2.5) and indicate therefore if the estimated RTK GPS heading is used in the quaternion EKF.

4. Conclusions

This paper presents a method that allows an autonomous UAV to navigate during flight independently of magnetic field measurements. The proposed method combines single-frequency L1 GPS measurements with the measurements of a consumer-grade MEMS IMU. The sensor fusion is accomplished by using two EKFs that are coupled by exchanging information about the currently-estimated baseline orientation. The first EKF combines the raw GPS code and carrier-phase measurements with a priori baseline information and calculates the RTK GPS heading. The second EKF combines magnetometer, accelerometer and gyroscope measurements with GPS baseline information and computes a full attitude solution for the UAV. The attitude solution is used to predict the baseline for the next update of the GPS EKF. Magnetometer measurements are used during initialization to allow a short time to first fix. During flight, the GPS heading estimates are combined with accelerometer and gyroscope measurements only. However, the proposed approach can be easily adjusted to modify the influence of the magnetometer depending on the application and therefore the UAV’s magnetic environment.

The system is implemented on a heterogeneous dual-core. Due to a real-time requirement-dependent task distribution between both cores, no additional or specialized hardware is needed, allowing one to use only low-cost Components-Off-The-Shelf, dropping the total hardware costs of the proposed navigation system to less than 400$. The static evaluation shows that the accuracy of the L1 GPS heading estimates can be compared to the results obtained with a commercially available system. The manufacturer states their accuracy with a 0.25°/m baseline resulting in an accuracy of approximately 0.5° for a system with a baseline of 48 cm. This is also in accordance with the findings reported by Eling et al. in [11]. The proposed IMU sensor integration improves the standard RTKLIB approach significantly. The combination of IMU and GPS measurements can additionally improve the obtained precision. The precision in static applications is observed to be below 0.05°. The proposed system was successfully tested during flight. It provides a promising alternative to overcome the heading estimation problem of small UAVs in environments with magnetic disturbances or close to the magnetic poles.