A Practical Method for Implementing an Attitude and Heading Reference System

Abstract

This paper describes a practical and reliable algorithm for implementing an Attitude and Heading Reference System (AHRS). This kind of system is essential for real time vehicle navigation, guidance and control applications. When low cost sensors are used, efficient and robust algorithms are required for performance to be acceptable. The proposed method is based on an Extended Kalman Filter (EKF) in a direct configuration. In this case, the filter is explicitly derived from both the kinematic and error models. The selection of this kind of EKF configuration can help in ensuring a tight integration of the method for its use in filter-based localization and mapping systems in autonomous vehicles. Experiments with real data show that the proposed method is able to maintain an accurate and drift-free attitude and heading estimation. An additional result is to show that there is no ostensible reason for preferring that the filter have an indirect configuration over a direct configuration for implementing an AHRS system.

Keywords

Attitude Estimation Sensor Fusion Vehicle Navigation Inertial Measurement Kalman Filtering

1. Introduction

The orientation of a vehicle in space is often referred to as Attitude. A combination of instruments capable of maintaining an accurate estimate of the vehicle attitude, while it manoeuvres, is called an AHRS (Attitude and Heading Reference System). An AHRS is a fundamental prerequisite for addressing several navigation and control problems. The first implementations of AHRS were based only on gyroscopes. Gyros are prone to bias, which could produce large errors after long periods of integration. This fact meant that attitude estimation was limited to very expensive applications because sensors with long term bias stability are very expensive, even now.

Filtering techniques are often required if less reliable (low cost) gyros are used. Using filtering techniques, other sensors (i.e., accelerometers and magnetometers) can be combined with gyros in order to limit the attitude errors over time. The recent production of solid-state or MEMS gyroscopes, 3-axis accelerometers and magnetometers and powerful microcontrollers have made the development of small, low cost and reliable AHRS devices possible.

With new hardware becoming available (i.e., MEMS sensors and microcontrollers), several approaches for AHRS systems have appeared in the literature, especially in the last decade. Different taxonomies can be employed in order to classify the AHRS methods available. Some of these criteria are: i) sensory input, ii) attitude representation and iii) estimation technique.

Nowadays, AHRS are typically based on gyros that are updated by gravity sensors (i.e., accelerometers) for pitch and roll, and by magnetic field sensors for yaw. Nevertheless, depending on the application, it is common to find approaches which rely on alternative sensors to bound attitude errors in time; for example, GPS or air speed sensors for UAVs and star trackers for spacecraft [1 –6].

The 3×3 orthogonal attitude matrix (Rotation Matrix or Direction Cosine Matrix) is the fundamental representation of the attitude. The requirement of orthogonality imposes six constraints on its nine elements, reflecting the fact that the special orthogonal group SO(3) of the rotation matrices has three dimensions. Therefore, although in some approaches attitude is computed directly over the Direction Cosine Matrix (DCM) [7 –8], most approaches use lower-dimensional parametrizations of SO(3), with earlier approaches using a three-dimensional parameterization (e.g., Euler angles) [1,3,7,9]. On the other hand, higher-dimensional parametrizations can avoid the singularities or discontinuities present in all three-dimensional representations. The four-component quaternion has the lowest dimensionality possible for a globally nonsingular representation of SO(3). Unit quaternions provide a convenient mathematical notation for representing orientations and rotations of objects in three dimensions [2,10,4 –6,11,12]. Compared with Euler angles, quaternions are simpler to compose and avoid the problem of gimbal lock. Compared to rotation matrices, quaternions are numerically more stable and may be more computationally efficient. A good review for attitude representations is given in [13].

Several estimation techniques have been used for attitude determination. Schemes presented in [1,2] use the Linear Filtering and Iterated least-squares methods, respectively. The linear Kalman Filter (KF) commonly used to estimate the system state variables and to suppress the measurement noise has been recognized as one of the most powerful state estimation techniques. Some methods which rely on linear Kalman Filtering are [3,10]. Due to the nonlinear nature of the problem, the nonlinear version of the Kalman Filter (The Extended Kalman Filter or EKF) has been the technique typically used to compute the attitude solution from multiple sensor sources. There are two basic ways of implementing the EKF: total state space formulation (also referred to as the direct formulation) and error state space formulation (also referred as the indirect formulation).

EKF in the indirect formulation estimates a state vector which represents the errors between the estimated state and the estimated nominal trajectory. An error model for each component of the state is needed in order to estimate the measurement residual. The measurement in the error state space formulation is made up entirely of system errors and is almost independent of the kinematic model. Most of the approaches follow this kind of configuration [4,5,7,11,12]. The differences among those methods mainly consist of variations in the design of the error models.

In EKF in the direct configuration, the vector state is updated implicitly with the predicted state and the measurement residual (the difference between the predicted and current measurement). In this kind of EKF configuration, the system is essentially derived from the system kinematics. One of the characteristics of the direct configuration is its conceptual clarity and simplicity. The differences between both kinds of configurations can considerably impact the development process of applications based on the EKF. A good review of EKF and its configurations can be found in [14].

In addition, it is possible to find other methods which rely on variations of Kaiman filtering such as the Unscented Kaiman filtering [15]. Another interesting family of methods for attitude estimation is nonlinear observers [6,8]. Nonlinear observers often exhibit global convergence which means they can converge from any initial guess. A good review of several filtering methods for attitude estimation is given in [16]. Another kind of method relies on estimation techniques coming from the artificial intelligence (AI) research community. For instance in [9], attitude estimation relies on a digital neural network.

Although it is possible to find different methodologies in the literature, EKF is still the standard estimation technique for attitude estimation. Nevertheless, the use of EKF in direct configuration has been much less explored than its counterpart, the EKF in indirect configuration (see Table 1). This happens especially when system errors (e.g., gyro bias) are to be included in the vector state. An example of direct linear Kaiman filtering for attitude and position estimation (GPS + Inertial navigation system) can be found in [17]. Nevertheless, since this method is also intended for position estimation, it is highly dependent on GPS measurements, and thus limits its usability for applications which rely solely on attitude estimation. Moreover, the LTI (linear time invariant) approach of this method can affect the performance of the estimations due to the non-linear nature of the problem.

Table 1.
In the “Sensors” column, the initials stand for: G = gyroscope, A = accelerometer, M = Magnetometer; the number before the initial means the number of axes, (e.g., 3G = 3-axis gyroscope). The “Gyro Bias” column indicates whether the method estimates the bias of gyros online. In the “Estimation Technique” column, KF = Linear Kalman Filter, i-EKF = Extended Kalman Filter in the indirect configuration, d-EKF = Extended Kalman Filter in the direct configuration, UKF = Unscented Kalman Filter. KF1 indicates that an extra linear Kalman Filter is used for the Gyro Bias estimation. In the “Attitude Representation” column, DCM stands for Direction Cosine Matrix.

Method Sensors Gyro Bias Estimation Technique Attitude Representation

[1] 3G, GPS Y Linear Filtering Euler

[2] 3G,3A,3M, GPS Y Iterated least squares/KF Euler

[3] 3G,3A,3M, GPS Y KF Quaternion

[10] 3G,1A,3M Y KF + KF¹ DCM

[4] 3G, Star-Tracker Y i-EKF Quaternion

[7] 3G,3A,3M Y i-EKF Euler

[5] 3G,3A,3M Air-Speed Y i-EKF Quaternion

[11] 3G,3A,3M Y i-EKF Quaternion

[12] 3G,3A,3M N i-EKF Quaternion

[6] 3G,3A, Air-Speed N Complementary Filtering Quaternion

[8] 3G,3A,3M Y Complementary Filtering DCM/Quaternion

[9] 3G,3A,3M N Neural Network Euler

[15] 3G,3A,3M Y UKF Quaternion

This work 3G,3A,3M Y d-EKF Quaternion

Method	Sensors	Gyro Bias	Estimation Technique	Attitude Representation
[1]	3G, GPS	Y	Linear Filtering	Euler
[2]	3G,3A,3M, GPS	Y	Iterated least squares/KF	Euler
[3]	3G,3A,3M, GPS	Y	KF	Quaternion
[10]	3G,1A,3M	Y	KF + KF¹	DCM
[4]	3G, Star-Tracker	Y	i-EKF	Quaternion
[7]	3G,3A,3M	Y	i-EKF	Euler
[5]	3G,3A,3M Air-Speed	Y	i-EKF	Quaternion
[11]	3G,3A,3M	Y	i-EKF	Quaternion
[12]	3G,3A,3M	N	i-EKF	Quaternion
[6]	3G,3A, Air-Speed	N	Complementary Filtering	Quaternion
[8]	3G,3A,3M	Y	Complementary Filtering	DCM/Quaternion
[9]	3G,3A,3M	N	Neural Network	Euler
[15]	3G,3A,3M	Y	UKF	Quaternion
This work	3G,3A,3M	Y	d-EKF	Quaternion

On the other hand, the EKF in its direct configuration has been widely used (for about two decades) by the research community on autonomous robots, to implement methods of localization, mapping or both: SLAM (Simultaneous Localization and Mapping), see [18].

The method presented in this work is motivated by the necessity of having a practical and reliable method for attitude and heading estimation that can be tightly integrated with filter-based SLAM methods in a straightforward manner. In this sense, it is important to note that most of the currently available algorithms for SLAM use a loosely-coupled approach for incorporating attitude measurements. In other words, in a loosely-coupled approach, the SLAM algorithm takes the attitude and orientation estimated by an AHRS unit as a high-level input. On the other hand, since the proposed method was derived using the indirect configuration of the EKF, it can be easily plugged into a filter-based SLAM algorithm using a tightly-coupled approach. Thus, low-level measurements (i.e., from gyros, accelerometers, magnetometers) can be incorporated directly into the SLAM scheme to aid in attitude and heading determination.

This paper considerably extends the authors' previous work [19] where the idea of an AHRS based on an EKF in a direct configuration is introduced. Some of the most important additions included in this work are:

A new (discrete) kinematic nonlinear model is used in the prediction equations of the filter, in order to improve the performance of the method when operating at a low sample rate.

The actual rotational speed of the body is included in the system state vector, in order to improve the observability of the bias of gyros.

In order to detect instants when the body is in a non-accelerating mode, the Stance Hypothesis Optimal Detector (SHOE) [20] is used.

A novel method is developed for updating yaw (heading) measurements, in order to improve the modularity and scalability of the method.

In order to validate the performance of the proposed method, a comparative study with real data is presented, where the proposed method is compared (in different conditions) with a related method. In experiments, the high-performance miniature unit 3DM-GX3®45 from McroStrain® is used as ground truth.

The paper is organized as follows: in Section 2, the proposed method is described. It is important to note that the paper is presented in a self-contained style, since all the required equations for implementing the proposed method are included. Section 3 presents the experimental results. In Section 4, the final remarks are presented. An appendix with the transformations required for implementing the proposed method is also included.

2. Method description

2.1 Vector state and system specification

The goal of the proposed method is to estimate the following system state $\overset{\circ}{x}$ :

\hat{x} = {[\begin{matrix} q^{n b} & ω^{b} & x_{g} \end{matrix}]}^{'}

(1)

where q^nb = [q₁, q₂, q₃, q₄] is a unit quaternion representing the orientation (roll, pitch and yaw) of the body (device); ω^b = [ω_x ω_y ω_z] is the bias-compensated velocity rotation of the body expressed in the body frame; x_g = [x_{g_x}x_{g_y}x_{g_z}] is the bias of gyros.

Figure 1 shows the relationship between the body frame^b and the local reference frameⁿ. In this work, the axes of the coordinate systems follow the NED (North, East, Down) convention. For simplicity's sake, in Fig. 1, the orientation of the body is illustrated by Euler angles α, β and γ, denoting roll, pitch and yaw, respectively. Euler angles can be computed from quaternion q^nb using Eq. (34).

Figure 1.

The rotation between the body (device) frame (in red) and the local reference frame (in blue) is illustrated by Euler angles α, β and γ (roll, pitch and yaw respectively). Attitude and heading reference systems are often used for stabilization applications, for instance, to control a quadrotor.

In order to estimate the system state $\overset{\circ}{x}$ , measurements obtained with an inertial measurement unit (IMU) of 9-DOF are considered. The IMU is formed by a 3-axis gyroscope, a 3-axis accelerometer, and a 3-axis magnetometer.

2.1.1 Gyroscope measurements

The angular rate ω^b of the vehicle, measured by the gyros (in the body frame) and indicated as y_g, can be modelled by:

y_{g} = ω^{b} + x_{g} + v_{g}

(2)

where x_g is an additive error (bias) and v_g is a Gaussian white noise with a power spectral density (PSD) σ_g².

2.1.2 Accelerometer measurements

The acceleration of the device a^b, measured by the accelerometers (in the body frame) as y_a, can be modelled by:

y_{a} = a^{b} - g^{b} + x_{a} + v_{a}

(3)

where g^b is the gravity vector expressed in the body frame, x_a is an additive error (bias), and v_a is a Gaussian white noise with PSD σ_a². Bias in the accelerometers triads is often relatively small, thus in this work it is neglected.

2.1.3 Magnetometer measurements

The Earth field m^b measured (in the body frame) as y_m can be modelled by:

y_{m} = m^{b} + x_{m} + v_{m}

(4)

where v_m is a Gaussian white noise with PSD σ_m². Magnetometer bias x_m could be fairly large but extremely slow in time-varying; therefore, in this work it is not considered for online estimations; instead a calibration technique, as presented in [21], could be used to set its initial value.

2.2 Architecture of the system

Figure 2 shows the architecture of the system, which is defined by the typical loop of the prediction-update steps in the EKF in the direct configuration:

System Prediction: Prediction equations propagate during the estimation of the system state, by means of the measurements obtained from gyroscopes. Prediction equations offer correct estimates at a high frequency, but only for a short period of time.

System Update: The unavoidable small errors in gyro readings produce large errors in attitude estimation after long periods of integration. The use of aiding sensors capable of measuring external references becomes essential in order to limit the estimation error. In this work, the gravity vector g and the magnetic Earth field m are used as external references for correcting roll, pitch and yaw estimations:

i)
During the periods when the device is in a non-accelerating mode (variable rate), information about the attitude of the device-vehicle (roll and pitch) is incorporated into the system by observing the gravity vector.
ii)
Information about the heading (yaw) of the device-vehicle is incorporated into the system (at a predefined constant rate) by observing the Earth's magnetic field.

Figure 2.
System architecture: the typical loop of prediction-update steps can be clearly observed, defined by an EKF in direct configuration (the vector state is implicitly updated from the predicted state and the measurement residual). We can observe that pitch and roll updates are only carried out when the device is in a non-accelerating mode (variable rate), whereas yaw updates are carried out at a fixed rate. Note that due to the modular design, for instance, replacing magnetometers with other sensors in order to correct yaw should be straightforward.
2.3 System Prediction

At every step k, when gyroscope measurements are available, the system state $\overset{\circ}{x}$ is updated by the following (discrete) nonlinear model.

\begin{array}{l} q_{(k + 1)}^{n b} = (\cos (‖ w ‖) I_{4 \times 4} + \frac{\sin (‖ w ‖)}{‖ w ‖} W) q_{(k)}^{n b} \\ ω_{(k + 1)}^{b} = - (y_{g (k)} - x_{g (k)}) \\ x_{g(k + 1)} = (1‐ λ_{x g} Δ t {)x}_{g(k)} \end{array}

(5)

In the model represented by Eq. (5), a closed form solution of $\dot{q}$ = 1/2(W)q is used to integrate the current bias-compensated velocity rotation ω^b over the quaternion q^nb. In this case w = [ω^b_(k+1)Δt/2]′ and:

W = [\begin{matrix} 0 & {‐w}_{1} & {‐w}_{2} & {‐w}_{3} \\ w_{1} & 0 & {‐w}_{3} & w_{2} \\ w_{2} & w_{3} & 0 & {‐w}_{1} \\ w_{3} & {‐w}_{2} & w_{1} & 0 \end{matrix}]

(6)

Furthermore, an alternative kinematic model for modelling the orientation of a camera using a quaternion can be found in a previous work by the authors [22]. Parameter λ_xg is a correlation time factor which models how fast the bias of the gyro varies. Δt is the sampling time of the system.

The state covariance matrix P takes a step forward by:

P_{(k + 1)} = \nabla F_{x} P_{(k)} \nabla F_{x}^{'} + \nabla F_{u} U \nabla F_{u}^{'}

(7)

The measurement noise of the gyroscope v_g is incorporated into the system by means of the process' noise covariance matrix U, through parameter σ_g²:

U = d i a g [\begin{matrix} σ_{g}^{2} I_{3 \times 3} & σ_{x g}^{2} I_{3 \times 3} \end{matrix}]

(8)

The full model used to propagate the sensor bias error is: bias_k+1=(1-λΔt)bias_k + v_b, where v_b models the uncertainty in the bias drift. The uncertainty in the bias for the gyro v_xg is incorporated into the system through the noise covariance matrix U via PSD parameter σ_xg².

\nabla F_{x} = [\begin{matrix} \frac{\partial f q^{n b}}{\partial q^{n b}} & \frac{\partial f q^{n b}}{\partial ω^{b}} & 0 \\ 0 & 0 & \frac{\partial f ω^{b}}{\partial x_{g}} \\ 0 & 0 & \frac{\partial f x_{g}}{\partial x_{g}} \end{matrix}] \nabla F_{u} = [\begin{matrix} 0 & 0 \\ \frac{\partial f ω^{b}}{\partial y_{g}} & 0 \\ 0 & \frac{\partial f x_{g}}{\partial v_{xg}} \end{matrix}]

(9)

The Jacobian ∇F_x is formed by the partial derivatives of the nonlinear prediction model (Eq. 5) with respect to the system state $\overset{\circ}{x}$ . In Jacobian notation, “∂fx/∂y” is used for partial derivatives and it must be read as the partial derivative of the function f (which estimates the state variable x) with respect to the variable y. Jacobian ∇F_u is formed by the partial derivatives of the nonlinear prediction model (Eq. 5) with respect to the system inputs.

2.4 System Updates

The filter can be updated as follows:

{\hat{x}}_{k} = {\hat{x}}_{k + 1} + W (z_{i} - h_{i})

(10)

P_{k} = P_{k + 1} - W S_{i} W^{'}

(11)

where z_i is the current measurement and h_i = h( $\overset{\circ}{x}$ ) is the predicted measurement; W is the Kaiman gain computed from:

W = P_{k + 1} \nabla H_{i}^{'} S_{i}^{- 1}

(12)

S_i is the innovation covariance matrix:

S_{i} = \nabla H_{i} P_{k + 1} \nabla H_{i}^{'} + R_{i}

(13)

∇H_i is the Jacobian formed by the partial derivatives of the measurement prediction model h( $\overset{\circ}{x}$ ) with respect to the system state $\overset{\circ}{x}$ . R_i is the measurement noise covariance matrix. Equations (10) to (13) will be used for system updates together with the proper definitions of z_i, h_i, ∇H_i and R_i.

2.4.1 Roll and pitch updates

If the device is not accelerating, (i.e. a^b ≈ 0), then Eq. (3) can be approximated as y_a ≈ −g^b + v_a (x_a is neglected). In this situation, accelerometer measurements y_a provide noisy observations in the gravity vector (in the body frame). The gravity vector g is used as an external reference for correcting roll and pitch estimations.

In order to detect the time (corresponding to k instants) that the body is in a non-accelerating mode, the Stance Hypothesis Optimal Detector (SHOE) is used [20].

The gravity vector g is predicted as measured by the accelerometers as h_g:

h_{g} = R^{n b} [\begin{matrix} 0 \\ 0 \\ g_{c} \end{matrix}]

(14)

where g_c is the gravity constant and R^nb is the navigation to body rotation matrix, computed from the current quaternion q^nb (using Eq. (33)).

If the device is not accelerating and a minimum period (corresponding to t₁ seconds) has elapsed since the last roll and pitch update, then the filter is updated (using Eqs. (10) to (13)) with:

z_{i} = y_{a} h_{i} = h_{g} R_{i} = I_{3 \times 3} σ_{a}^{2} \nabla H_{i} = \partial h_{g} / \partial \hat{x}

(15)

2.4.2 Yaw updates

The model h_y used for predicting the heading (yaw) of the device is defined as:

h_{γ} = atan2 (2 (q_{2} q_{3} - q_{1} q_{4}), 1 - 2 (q_{3}^{2} + q_{4}^{2}))

(16)

where q^nb = [q₁, q₂, q₃, q₄] is the current quaternion; atan2 is a two-argument function that computes the arctangent of y/x given y and x, within the range [−π, π].

As can be observed in Eq. (16), the model does not predict how the Earth's magnetic field will be measured. Instead, the model directly predicts the yaw angle to be measured. The selection of this measurement prediction model is based on the scalability of the system. In this sense, an alternative measurement device could be directly attached to the AHRS in order to correct the heading estimations.

An example of the above case could be a vehicle equipped with GPS. Certainly, in order to update the yaw of the vehicle using GPS measurements, the heading (yaw) should coincide all the time with the vehicle's course, which is measured by the GPS. In the case of aerial vehicles (e.g., helicopters, quadrotors, etc.), where the above assumption is not valid, then another reference, such as the Earth's magnetic field, could be used instead to update the yaw.

In order to use the proposed measurement prediction model h_γ in the 3-axis magnetometer which is included in the 9-DOF IMU, a yaw measurement z_γⁿ is obtained from the measured magnetic field y_m.

Due to the angle of the inclination of the magnetic field vector, the measured magnetic vector is first projected to the north-east plane, by removing its z component:

m^{n} = R^{b n} y_{m}

(17)

m_{1}^{n} = [\begin{matrix} m_{x}^{n} & m_{y}^{n} & 0 \end{matrix}]

(18)

where mⁿ = [m_xⁿ, m_yⁿ, m_zⁿ] and R^bn is the body to navigation rotation matrix, computed from the current quaternion q^nb. The magnetic field vector mⁿ₁ (expressed in the navigation frame), from which the z component has been removed, is projected back to the body frame by:

m^{b} = R^{n b} m^{n}_{1}

(19)

where m^b = [m_x^b, m_y^b, m_z^b] and R^nb is the navigation to body rotation matrix, computed from the current quaternion q^nb. Finally, the measured yaw z_γⁿ is obtained by:

z_{γ}^{n} = atan2 (- m_{y}^{b}, m_{x}^{b})

(20)

In this work, it is assumed that the angle of the declination of the magnetic field is ignored or is previously known. Measurements z_γⁿ are assumed to be corrupted by Gaussian white noise v_γ with PSD σ_γ².

At constants intervals of t₂ seconds, the filter is updated (using Equations (10) to (13)) by:

z_{i} = z_{γ}^{n} h_{i} = h_{γ} R_{i} = σ_{γ}^{2} \nabla H_{i} = \partial h_{γ} / \hat{x}

(21)

2.5 System Initialization

An initial period of time t ∊ [0,T] is used for system initialization tasks. During this period, the device is assumed to be non-accelerating.

2.5.1 Initial Attitude

The method for estimating the initial orientation q^nb_ini is based on the method proposed in [23]. The body frame gravity vector ${\bar{g}}^{b}$ = [g₁, g₂, g₃]′ is estimated by:

{\overline{g}}^{b} = \frac{1}{T} \int_{0}^{T} - y_{a} (t) d t

(22)

The initial roll and pitch values can be computed respectively by:

α_{i n i} = atan2 (g_{2}, g_{3})

(23)

β_{i n i} = atan2 (- g_{1}, \sqrt{{(g_{2})}^{2} + {(g_{3})}^{2}})

(24)

The initial yaw value is estimated as follows:

m^{w} = [\begin{matrix} \cos (β) & \sin (β) \sin (α) & \sin (θ) \cos (α) \\ 0 & \cos (α) & - \sin (α) \\ - \sin (β) & \cos (β) \sin (α) & \cos (θ) \cos (α) \end{matrix}] {\overline{m}}^{b}

(25)

where m^w = [m_x^w, m_y^w, m_z^w] and:

{\overline{m}}^{b} = \frac{1}{T} \int_{0}^{T} y_{m} (t) d t

(26)

and

γ_{i n i} = atan2 (- m_{y}^{w}, m_{x}^{w})

(27)

The initial quaternion q^nb_ini is computed from the initial Euler angles α_ini, β_ini and γ_ini:

q^{n b}_{i n i} = f (α_{i n i}, β_{i n i}, γ_{i n i})

(28)

To compute the above transformation, an initial body to rotation matrix R^nb_ini is computed from the initial Euler angles using Eq. (35). Then the initial quaternion q^nb_ini is computed from R^nb_ini using Eq. (36).

2.5.2 Initial Gyro Bias

Initial gyro bias x_g(ini) is estimated from:

x_{g(i n i)} = \frac{1}{T} \int_{0}^{T} y_{g} (t) d t

(29)

2.5.3 Initial vector state and covariance matrix

The system vector state is initialized as follows:

{\hat{x}}_{i n i} = {[\begin{matrix} q^{n b}_{i n i} & 0_{1 \times 3} & x_{g (i n i)} \end{matrix}]}^{'}

(30)

The covariance matrix of the system is initialized as follows:

P_{i n i} = [\begin{matrix} P {(q^{n b}_{i n i})}_{4 \times 4} & 0 & 0 \\ 0 & ε I_{3 \times 3} & 0 \\ 0 & 0 & \frac{σ_{g}^{2}}{T} I_{3 \times 3} \end{matrix}]

(31)

where ε is a very small arbitrary positive value. The covariance matrix for the initial attitude P(q^nb_ini) is computed from:

P (q^{n b}_{i n i}) = \nabla_{q} [\begin{matrix} \frac{σ_{a}^{2}}{T} I_{2 \times 2} & 0 \\ 0 & \frac{σ_{m}^{2}}{T} \end{matrix}] \nabla_{q}^{'}

(32)

∇_q is the Jacobian formed by the partial derivatives of the transformation defined in Eq. (28) with respect to the Euler angles.

3. Experimental Results

In order to validate the performance of the proposed method, a comparative study with real data is presented. In this case, the output estimated by the proposed algorithm (Direct method) is compared with the output obtained from the method described in [11] and [23], which is based on an EKF in indirect formulation (Indirect method). For a comparative study, the output obtained from a commercial 3DM-GX3®45 AURS unit is considered as the ground truth. This high-performance miniature unit from MicroStrain® has a retail cost of about 5,000 USD.

For each test, the 3DM-GX3®45 was randomly gyrated while held in a hand. At the same time, raw data obtained from the accelerometers, gyroscopes and magnetometers included in the unit, along with the attitude computed by the same unit, were recorded in a plain text file at a frequency of 100 Hz. Several data captures were carried out trying to cover different dynamic circumstances such as periodic and soft turns, as well as random and strong shakes. Each capture lasts about three minutes.

A MATLAB implementation of both the proposed approach (Direct method), as well as the Indirect method, were executed in off-line mode on a desktop Intel i5 PC, using raw sensor data stored in plain text files as input signals. The execution time was: i) Direct method = 736 microseconds/step; ii) Indirect method = 586 microseconds/step. It is important to note that for the Indirect method the size of the system state is six (actual rotational velocity is not included), instead of nine. So (as is typical in EKF applications) difference in execution time should be mostly related to the size of the system state.

The outputs obtained with: i) the Direct method, ii) the Indirect method and iii) the 3DM-GX3®45 unit have been compared. Table 3 shows the values for the parameters used in the experiments for both the Direct and the Indirect methods.

Table 2.
Mean absolute error (degrees)

No extra bias 100Hz 50 Hz 25 Hz

Roll (Direct) 0.65 0.84 2.62

Roll (Indirect) 0.66 0.83 2.50

Pitch (Direct) 0.36 0.58 1.80

Pitch (Indirect) 0.35 0.56 1.74

Yaw (Direct) 0.68 0.96 2.42

Yaw (Indirect) 0.81 1.02 2.10

Extra Bias 100Hz 50 Hz 25 Hz

Roll (Direct) 1.12 1.28 3.01

Roll (Indirect) 1.39 1.54 2.92

Pitch (Direct) 0.87 1.07 2.30

Pitch (Indirect) 0.98 1.19 2.33

Yaw (Direct) 2.52 3.52 5.76

Yaw (Indirect) 3.10 3.33 5.21

No extra bias	100Hz	50 Hz	25 Hz
Roll (Direct)	0.65	0.84	2.62
Roll (Indirect)	0.66	0.83	2.50
Pitch (Direct)	0.36	0.58	1.80
Pitch (Indirect)	0.35	0.56	1.74
Yaw (Direct)	0.68	0.96	2.42
Yaw (Indirect)	0.81	1.02	2.10
Extra Bias	100Hz	50 Hz	25 Hz
Roll (Direct)	1.12	1.28	3.01
Roll (Indirect)	1.39	1.54	2.92
Pitch (Direct)	0.87	1.07	2.30
Pitch (Indirect)	0.98	1.19	2.33
Yaw (Direct)	2.52	3.52	5.76
Yaw (Indirect)	3.10	3.33	5.21

Table 3.

Values of parameters used in experiments

Parameter	Description	Value	Unit
σ_g²	PSD for gyroscopes	2.2 × 10⁻³	(rad/s)²
σ_a²	PSD for accelerometers	1.2 × 10⁻²	(m/s²)²
σ_γ²	PSD for heading readings.	6.0 × 10⁻³	(rad)²
σ_xg²	PSD for drift rate of gyro bias	4.0 × 10⁻¹¹	(rad/s²)²
λ_xg	Correlation time for gyro bias	1.0 × 10⁻³	s ⁻¹
Δt	Sampling time	1.0 to 4.0 × 10⁻²	s
t ₁	Minimum time between roll and pitch updates	5.0 × 10⁻²	s
t ₂	Minimum time between yaw updates	1.0 × 10⁻¹	s

In experiments, the mean absolute error (MAE) was used to compare the performance of both methods:

MAE = \frac{1}{n} \sum_{k = 1}^{n} ‖ f_{k} - y_{k} ‖

where n is the number of samples, f_k is the angle measured by the 3DM-GX3®45 unit at instant k, and y_k is the angle estimated by any method at instant k. In experiments, for purposes of clarity, Euler angles are obtained every time that they are needed from the current estimated quaternion q^nb using Eq. (34).

For a comparative study, two aspects were evaluated:

The methods' performance at estimating the gyro bias x_g. That is, the ability of the filters to converge when the initial conditions differ considerably from the actual value, in order to minimize the error in estimations.

The performance of the methods when the frequency of operations is reduced (or the sample time is increased).

For the case (a), the methods were executed over the input signals stored in the plain text files. After that, the methods were run again over the same input signals, a huge extra bias x_g(a) was artificially introduced into each gyro measurement y_g, so that: y_g = ω^b + x_g + v_g + x_g(a) (see Eq. 2). In experiments x_g(a) = [.05 −.05 .025] radians.

For the case (b), the methods were first executed over all the samples captured. After this operation, the methods were executed again but in this case, samples were periodically skipped in order to emulate different frequencies of operation. In this case, 100Hz, 50Hz and 25Hz were considered.

Table 2 shows the average MAE obtained with the Direct method and the Indirect method for several captures of data (considering all the conditions previously described). As can be appreciated, the computed MAE is in general very similar for both methods. In a more detailed observation, the Direct method performs slightly better for converging (and thus minimizing the error in estimation) when an initial huge gyro bias is present. On the other hand, the Indirect method shows a slightly better response at a very low frequency of operation.

Figure 3 shows the progression over time for the estimations obtained for a test with random turns and strong shakes. The plots correspond to the response of both methods when an extra gyro bias and a frequency of operation of 100Hz are considered. In Fig. 3, at the beginning of the test (before second 30), the adverse effect in the estimated roll, pitch and yaw due to the integration of the contaminated gyro measurements can be clearly appreciated (observe the absolute error corresponding to this period). However, the estimated gyro bias rapidly converges to its actual value due to the system updates carried out in the filters. When the gyro bias is estimated then the absolute error is minimized. For this test, it can also be appreciated that the convergence of the Direct method is faster than the Indirect Method, thus accelerating the minimization of errors in estimation.

Figure 3.

Estimation results for a test with random turns and strong shakes with a duration about 170 seconds. In the experiments the attitude obtained from the 3DM-GX3®45 is considered as a ground truth (shown in black). The experimental results obtained with the proposed scheme (Direct method) are shown in green. The results obtained with the scheme of [11] and [23], (Indirect method) are shown in red. Besides the computed Euler angles (roll α, pitch β, yaw γ), the estimated gyroscopes bias x_g and the absolute error are also shown. The robustness of the methods for converging in the presence of a huge initial bias of gyro is tested. In this case measurements y_g have been artificially contaminated with an extra bias x_g(a) = [.05 −.05 .025]. Nevertheless, for both methods it can be observed how the MAE decreases as the estimated gyro bias converges to its actual value. Although both methods are able to minimize the error in estimates over the time, for this case it can be observed a slightly better transient response for the Direct method. Lower plots illustrate periods for roll and pitch (left) and yaw (right) updates.

4. Conclusions

This work presents a practical method for implementing an attitude and heading reference system. The estimated vector state is formed by 10 state variables representing: i) the orientation of the body (device), ii) the bias-compensated velocity rotation of the body, and iii) the bias of the gyroscopes. The system input is obtained from a 9-DOF IMU formed by a 3-axis gyroscope, a 3-axis accelerometer and a 3-axis magnetometer.

The architecture of the system is based on an Extended Kalman filtering approach in a direct configuration. Experiments with real data show that the proposed method is able to maintain an accurate and drift-free attitude and heading estimation. Moreover, it is capable of estimating the parameters of the error of sensors (i.e., gyro bias) in a robust manner, thereby improving the system estimations even when the quality of the measurements obtained from the gyros is very poor. Therefore, the accuracy of the estimates is almost only limited by the pre-calibration of accelerometers and magnetometers. Based on the experimental results, it is considered that the method is robust enough for use along with low cost sensors.

In its normal operation mode at 100 Hz and using the same input signals, the average difference between the orientation estimated by the proposed method and orientation obtained from a retail unit (3DM-GX3®45), is lower than one degree. Furthermore, a comparative study shows that the performance of the proposed scheme is at least similar to an EKF method in an indirect configuration but, at the same time, has the advantages of clarity and simplicity commonly associated with the implementation of the EKF in a direct configuration. The modularity of the proposed architecture allows for scalability in the system. In such a case, an alternative measurement device could be easily attached to the system (replacing the magnetometers), in order to correct the heading estimations. Moreover, since the proposed method was derived using the indirect configuration of the EKF, it can be easily plugged into a filter-based SLAM algorithm using a tightly-coupled approach.

The EKF in general is not an optimal estimator (owing to its linearization nature). In addition, if the process is modelled incorrectly, the filter may quickly diverge. Furthermore, it has been seen that the EKF tends to underestimate the true covariance matrix and therefore the filter could become inconsistent. With the above fact in mind, and considering that other estimation techniques could be even more robust, for instance, to the presence of non-linearity (e.g., UKF or particle filters), EKF can provide a reasonable performance and is arguably still the de facto standard in navigation systems. However, to our knowledge, at least all of the recent approaches found in the literature are based on the filter having an indirect configuration (also called error configuration). In this sense, based on the experimental results presented in this work, an ostensible reason to prefer the indirect configuration of the filter over the direct configuration is not observed, at least for implementing an AHRS system.

Footnotes

5.

This research has been funded by the Spanish Ministry of Science project DPI2010–17112.

Appendix

In this appendix some useful transformations are included:

The rotation matrix R^nb can be computed from the quaternion q by:

(33)

R^{n b} = [\begin{matrix} (q_{1}^{2} + q_{2}^{2} - q_{3}^{2} - q_{4}^{2}) & 2 (q_{2} q_{3} - q_{1} q_{4}) & 2 (q_{1} q_{3} + q_{2} q_{4}) \\ 2 (q_{2} q_{3} + q_{1} q_{4}) & (q_{1}^{2} - q_{2}^{2} + q_{3}^{2} - q_{4}^{2}) & 2 (q_{3} q_{4} - q_{1} q_{2}) \\ 2 (q_{2} q_{4} - q_{1} q_{3}) & 2 (q_{1} q_{2} + q_{3} q_{4}) & (q_{1}^{2} - q_{2}^{2} - q_{3}^{2} + q_{4}^{2}) \end{matrix}]

Euler angles α, β and γ (roll, pitch and yaw, respectively) can be computed from a quaternion q by:

(34)

\begin{matrix} α = atan2 (\begin{matrix} 2 (q_{3} q_{4} - q_{1} q_{2}) & , & 1 - 2 (q_{2}^{2} + q_{3}^{2}) \end{matrix}) \\ β = asin (- 2 (q_{1} q_{3} + q_{2} q_{4})) \\ γ = atan2 (\begin{matrix} 2 (q_{2} q_{3} - q_{1} q_{4}) & , & 1 - 2 (q_{3}^{2} + q_{4}^{2}) \end{matrix}) \end{matrix}

The navigation to body rotation matrix R^nb can be computed from Euler angles α, β and γ by:

(35)

R^{n b} = [\begin{matrix} c γ c β & s γ c β & - s β \\ - s γ c φ + c γ s β s φ & c γ c φ + s γ s β s α & c β s α \\ s γ s φ + c γ s β c φ & - c γ s φ + s γ s β c α & c β c α \end{matrix}]

where cx = cos(x) and sx=sin(x).

A quaternion q^nb can be computed from rotation matrix R^nb by:

(36)

\begin{array}{l} q_{1} = \sqrt{1 + R^{n b} (1, 1) + R^{n b} (2, 2) + R^{n b} (3, 3)} \\ q^{n b} = [\begin{matrix} q_{1} \\ \frac{R^{n b} (3, 2) - R^{n b} (2, 3)}{4 q_{1}} \\ \frac{R^{n b} (1, 3) - R^{n b} (3, 1)}{4 q_{1}} \\ \frac{R^{n b} (2, 1) - R^{n b} (1, 2)}{4 q_{1}} \end{matrix}] \end{array}

Since a rotation matrix is orthogonal, then

(37)

R^{n b} = (R^{b n})^{'} and R^{b n} = (R^{n b})^{'}

References

Gebre-Egziabher

Hayward

R.C

Powell

J.D

(1998) A Low-Cost GPS/Inertial Attitude Heading Reference System (AHRS) for General Aviation Applications. IEEE Position, Location and Navigation Symposium.

Gebre-Egziabher

Hayward

R.C

Powell

J.D

(2000) Design of multi-sensor attitude determination systems. IEEE Transactions on Aerospace and Electronic Systems, vol.40, no.2, pp. 627–649.

Gebre-Egziabher

Hayward

R.C

Powell

J.D

(2004) Design of multi-sensor attitude determination systems. IEEE Transactions on Aerospace and Electronic Systems, vol.40, no.2, pp. 627–649.

Markley

F.L

(2003) Attitude error representations for Kalman filtering. Journal of guidance control and dynamics 26, 311–317.

Castellanos

J.F.G

Lesecq

Marchand

Delamare

(2005) A low-cost air data attitude heading reference system for the tourism airplane applications. IEEE Sensors.

Euston

Coote

Mahony

Jonghyuk

Hamel

(2008) A complementary filter for attitude estimation of a fixed-wing UAV. Intelligent Robots and Systems.

Wang

Yang

Hatch

R.R

Zhang

(2004) Adaptive filter for a miniature MEMS based attitude and heading reference system. Position Location and Navigation Symposium.

Mahony

Hamel

Pflimlin

T.M

(2008) Nonlinear Complemenary Filters on the Special Orthogonal Group. IEEE Transactions on Automatic Control, Vol.53, No. 5, pp. 1203–1218.

Córdoba

M.A

(2007) Attitude and heading reference system I-AHRS for the Efigenia autonomous unmanned aerial vehicles UAV based on MEMS sensor and a neural network strategy for attitude estimation. Mediterranean Conference on Control & Automatics.

10.

Wang

Xiong

Zhou

Wei

Lan

(2005) Constrained Filtering Method for MAV Attitude Determination. Procs. of the IEEE IMTC conference.

11.

Yang

, (2005) Method and apparatus for adaptive filter based attitude updating. U.S. Patent 2005/0240347.

12.

Young

S.S

(2010) Orientation Estimation Using a Quaternion-Based Indirect Kalman Filter with Adaptive Estimation of External Acceleration. IEEE Transactions on Instrumentation and Measurement, vol.59, no.12, pp.3296–3305.

13.

Shuster

M.D

(1993) A Survey of Attitude Representations. The Journal of the Astronautical Sciences, Vol. 41, No. 4, pp. 439–517.

14.

Dah-Jing

Ta-Shun

(2010) Critical remarks on the linearised and Extended Kalman filters with geodetic navigation examples. Measurement, Vol. 43, No. 9, pp. 1077–1089.

15.

Marina

H.G

Pereda

F.J

Giron-Sierra

J.M

Espinosa

(2012) UAV Attitude Estimation Using Unscented Kalman Filter and TRIAD. IEEE Transactions on Industrial Electronics, col.59, no.11, pp.4465–4474.

16.

Markley

Crassidis

Cheng

(2005) Nonlinear Attitude Filtering Methods. AIAA Guidance, Navigation, and Control Conference.

17.

Honghui

Moore

J.B

(2002) Direct Kalman filtering approach for GPS/INS integration. IEEE Transactions on Aerospace and Electronic Systems, vol.38, no.2, pp.687–693.

18.

Durrant-Whyte

Bailey

(2006) Simultaneous localization and mapping: part I. Robotics Automation Magazine, IEEE, 13(2):99–110.

19.

Munguía

Grau

(2011) Attitude and Heading System based on EKF total state configuration. IEEE International Symposium on Industrial Electronics.

20.

Skog

Handel

Nilsson

J.O

Rantakokko

(2010) Zero-Velocity Detection An Algorithm Evaluation. IEEE Transactions on Biomedical Engineering, vol.57, no.11, pp.2657–2666.

21.

Vasconcelos

J.F

Elkaim

Silvestre

Oliveira

Cardeira

(2008) A Geometric Approach to Strapdown Magnetometer Calibration in Sensor Frame. IFAC Workshop Navigation, Guidance and Control of Underwater Vehicles.

22.

Munguía

Grau

(2009) Closing Loops With a Virtual Sensor Based on Monocular SLAM. IEEE Transactions on Instrumentation and Measurement, vol.58, no.8, pp. 2377–2384.

23.

Farrell

(2008) Aided Navigation, Mac Graw-Hill.