Fundamentals of Attitude Estimation

1. Introduction

When estimating attitude and heading, the best results are obtained by combining data from multiple types of sensors to take advantage of their relative strengths. For example, rate gyros can be integrated to produce angle estimates that are reliable in the short-term, but that tend to drift in the long-term. Accelerometers, on the other hand, are sensitive to vibration and other non-gravity accelerations in the short-term, but can be trusted in the long-term to provide angle estimates that do no degrade as time progresses.  Combining rate gyros and accelerometers can produce the best of both worlds - angle estimates that are resistant to vibration and immune to long-term angular drift.

In this article, we describe a simple method for combining rate gyros and accelerometers to produce reliable pitch and roll angle estimates. The GP9 and the UM7 use methods that are more sophisticated than what is described here, but the basic intuition behind both methods is the same - gyros are used to give good short-term stability, while accelerometers are used for their better long-term stability.

Note that there are some problems with the simple method described here.  We discuss these problems in more detail near the end of the article.

To fully understand the contents of this article, you should first read Understanding Euler Angles and Sensors for Orientation Estimation.

2. Estimating angles with accelerometers

Recall that the output of a three-axis accelerometer can be modeled as

\mathbf{a}_m = \frac{1}{m}(\mathbf{F} - \mathbf{F}_g),

where \mathbf{a}_m is the measured acceleration, m is the mass of the body, \mathbf{F} is the sum of all forces on the body (including gravity) expressed in the sensor body frame, and \mathbf{F}_g is the force due to gravity, also expressed in the body frame.

If we assume that in general \mathbf{F} = 0, then the output of the accelerometer is given by

\mathbf{a}_m = -\frac{\mathbf{F}_g}{m}.

In the inertial frame, the force of gravity is given by the vector \begin{pmatrix} 0 & 0 & mg \end{pmatrix}^T.  That is, there is no force on the and y axes, and there is mg of force on the z axis.

Rotating the gravity into the body-frame of the sensor, we get

\mathbf{F}_g = R_I^B \begin{pmatrix} 0 \\ 0 \\ mg \end{pmatrix} = \begin{pmatrix} -mg\sin(\theta) \\ mg\cos(\theta)\sin(\phi) \\ mg\cos(\phi)\cos(\theta) \end{pmatrix}.

Thus, the expected accelerometer measurement in the body-frame is given by

\mathbf{a}_m = \begin{pmatrix} g\sin(\theta) \\ -g\cos(\theta)\sin(\phi) \\ -g\cos(\phi)\cos(\theta) \end{pmatrix}.

Let the components of \mathbf{a}_m be given by a_{m,x}a_{m,y}, and a_{m,z}.  Then we can solve for the pitch (\theta) and roll (\phi) angles using

\hat{\theta}_{accel} = \arcsin\left( \frac{a_{m,x}}{g} \right ), and

\hat{\phi}_{accel} = \arctan\left( \frac{a_{m,y}}{a_{m,z}} \right ).

The ^ symbol above the angles indicates that they are estimated angles, not actual angles.

This method provides a quick, simple way to estimate pitch and roll using only accelerometers. However, note that we assumed that the only force acting on the accelerometers was gravity.  Vibration and other external forces will directly influence our measured pitch and roll angles, making them very noisy and, in many cases, unusable by themselves.

3. Estimating angles with rate gyros

Rate gyros can also be used to measure angles.  Unlike accelerometers, rate gyros are nominally unaffected by acceleration, making gyro-based angle estimates immune to external forces that make accelerometers unreliable.

Remember that rate gyros measure angular rates about the x, y, and z axes in the sensor body-frame.  We would like to take those measurements and use them to figure out our Euler Angle rates.  Since roll, pitch, and yaw each occur in different reference frames, we need to take the rate gyro outputs and rotate them into the appropriate frames (more details about this process can be found here).

Let the variables p, q , and r represent the rate gyro outputs on the xy, and z axes, respectively.  Then the Euler Angle rates are given by

\begin{pmatrix} \dot{\phi} \\ \dot{\theta} \\ \dot{\psi} \end{pmatrix} = \begin{pmatrix} p + q\sin(\phi)\tan(\theta) + r\cos(\phi)\tan(\theta) \\ q\cos(\phi) - r\sin(\phi) \\ q\sin(\phi)/\cos(\theta) + r\cos(\phi)/\cos(\theta) \end{pmatrix}.

Since we are only estimating pitch and roll in this example, we will ignore the term for yaw (\psi).

Assuming that the rate gyros are sampled at some fixed interval T, we can estimate pitch and roll using Euler Integration as follows:

\hat{\phi}^+ = \hat{\phi} + T\dot{\phi}, and

\hat{\theta}^+ = \hat{\theta} + T\dot{\theta}.

In the preceding equations, we use the + symbol to distinguish the new angle estimate from the old angle estimate.

This method allows us to use rate gyros to estimate angles that aren't sensitive to vibration and other external forces.  However, because rate gyros are noisy and imperfect, every time we add new gyro measurements, we add errors to our angle estimates.  Over time, errors will accumulate, causing our gyro-based angles estimates to drift over time.  We will solve this problem in the next section.

4. Combining accelerometer and rate gyro data

As described previously, angle estimates based on rate gyros alone drift over time, making them unreliable in the long-term.  In contrast, accelerometers do not cause angle estimates to drift, but they are sensitive to external forces like vibration, making short-term estimates unreliable.  In this section, we combine the outputs of both types of sensor to produce angle estimates that are resistant to vibration and immune to long-term drift.

The estimation process will be split into two steps, a "prediction" step where we will use the rate gyros to estimate incremental changes in the angle, and an "update" step where we will use the accelerometers to correct rate gyro drift.

The prediction step consists of angular rate integration as we saw in the rate gyro section:

\hat{\theta}^+ = \hat{\theta} + T\dot{\theta}, and

\hat{\phi}^+ = \hat{\phi} + T\dot{\phi}.

In the update step, we assume that the accelerometer-based angle is usually something close to the truth.  We take the measured angle (from the accelerometers) and the predicted angle (from the rate gyros), compute the difference, and add a portion of the difference to the final angle estimate as shown below:

\hat{\theta} = \hat{\theta}^+ + L(\hat{\theta}_{accel} - \hat{\theta^+}),

\hat{\phi} = \hat{\phi}^+ + L(\hat{\phi}_{accel} - \hat{\phi^+})

The value L is a constant value between 0 and 1.  Notice that if L is equal to 0, then only the gyro-based angle is used because no accelerometer-based correction is applied.  Similarly, if L is equal to 1, then the estimated angle is equivalent to the accelerometer-based angle.

As L approaches 1, the accelerometers are trusted more and the rate gyros are trusted less.  This makes the angle estimates less sensitive to rate gyro noise and biases, but more sensitive to vibration and other external forces.

As L approaches 0, the rate gyros are trusted more and the accelerometers are trusted less.  This makes the angle estimates more sensitive to rate gyro noise and biases, but less sensitive to vibration and other external forces.

In practice, L can be adjusted based on the desired behavior of the angle estimates, but having high-quality rate gyros allows a lower gain to be used, preventing unwanted accelerations from affecting the angle estimates.  From an intuitive standpoint, the rate gyros estimate the angle in the short-term while the accelerometers gradually pull the angle estimate toward the average angle, whatever that happens to be.

This formulation can be called a "fixed-gain observer," and it bears a lot of similarity to a Kalman Filter.  The main difference is that in a Kalman Filter, the observer gain L is selected optimally using known characteristics of the physical system.  In addition, a Kalman Filter can exploit knowledge of the physical system so that accelerometer data (and other data) needn't be converted to angles before using it to make corrections to the angle estimates.

The UM7 and the UM7-LT both use Kalman Filters to optimally combine rate gyro and accelerometer data.

5. Limitations

The downside of this method is that accelerometers can't always be expected to provide reliable estimates of the actual angle.  In some dynamic systems (most notably in aircraft), accelerometers can provide incorrect angle information for arbitrarily large amounts of time.  This causes the angle estimates to degrade and possibly even become completely unreliable.

More sophisticated filters combine inertial sensors with other types of sensors (like GPS) to allow the angle estimates to maintain their accuracy even during sustained, high-G maneuvers.  Our GP9: GPS-Aided AHRS is an example of a sensor that performs better on moving vehicles.