# Complementary filters¶

A complementary filter is a quick and effective method for blending measurements from an accelerometer and a gyroscope to generate an estimate for orientation. This webpage briefly explains why such a filter is necessary, how it works, and then offers some alternative filters that you might consider. As a case-study problem, we will consider estimating the tilt angle of an inverted pendulum.

## System under consideration¶

As a case study problem, let us consider an inverted pendulum (just like in the reaction wheel laboratory assignment). In order to control this pendulum, we must measure the angle $\theta$ shown below. In order to determine this angle, we will use an accelerometer and a gyroscope mounted on the pendulum arm.

## Accelerometer vs. Gyroscope¶

With an accelerometer and a gyroscope, we have two different methods for estimating the tilt angle of the inverted pendulum. Unfortunately, there's a problem with each method!

### Computing accelerometer angle¶

The accelerometer measures the direction of gravity, which (in the laboratory) is always toward the center of the Earth. If the accelerometer is oriented on the arm as shown in the image below, then the tilt angle is given by:

\begin{align} \theta &= \arctan{\frac{a_x}{a_y}} \end{align}

If we assume that the tilt angle is small, then we can use a small angle approximation! Thus the tilt angle is given by:

\begin{align} \theta &\approx \frac{a_x}{a_y} \end{align}

The problem with the accelerometer is that it is noisy. It becomes especially noisy in the presence of any sort of mechanical noise (e.g. a DC motor attached to the pendulum). In the demonstration video, you can see the accelerometer angle plotted on the VGA display as I move the inverted pendulum back and forth. When I turn on a DC motor attached to the pendulum arm, the noise increases. This is much too noisy to use in a feedback controller.

### What's good about the accelerometer?¶

Because gravity always points in the same direction, the measurement error from the gyroscope remains zero-mean. The accelerometer does not accumulate any measurement bias, like the gyroscope does. The measured acceleration is equal to the true acceleration plus zero-mean Guassian noise, where the variance of that Gaussian noise can be quite large.

### Computing gyro angle¶

The gyroscope directly measures rotation rate. So, if we know the initial angle for our system ($\theta_0$) and our sample rate ($\frac{1}{\Delta t}$), we can use the gyroscope to integrate that angle for all time.

\begin{align} \theta = \theta_0 + g_z \Delta t \end{align}

By integrating the gyro measurements to maintain an estimate for angle, we are doing dead reckoning. This works great over short periods of time, but eventually leads to a large bias in our angle estimate. You can see this in the demonstration video. Even as I hold the pendulum at a constant angle, the gyroscope error continues to accumulate a bias. This bias is driven by a random process, so it may wander in any direction.

### What's good about the gyro?¶

Over short periods of time, the amount of noise in the gyroscope measurements is much lower than that from the accelerometer. You can see this in the demonstration video. As I move the pendulum back and forth, note that there is very little noise on the (red) gyroscope measurements, and a lot of noise on the (white) accelerometer measurements.

## Complementary filter¶

We desire the short-term accuracy of the gyro angle estimates, and the long-term stability of the accelerometer angle estimates. A complementary filter achieves this! A complementary filter low-passes the accelerometer measurements and high-passes the gyroscope measurements. This gives us the benefit of the gyro over short time periods, and the stability of the accelerometer over long time periods.

The algorithm is summarized as follows:

1. Gather a set of accelerometer and gyro measurements
2. Compute the accelerometer angle
3. Compute the change in angle that the gyro measures, by multiplying the raw measurement by the timestep (0.001 seconds in the example code below)
4. Compute the new complementary angle estimate by performing a weighted average of the accelerometer angle and the sum of the previous complementary angle estimate and the change in angle measured by the gyro.

Choosing these weights is a part of your tuning process. The greater the weight on the accelerometer, the less bias and more noise you see in your angle estimates. Start with an accelerometer weight of ~0.01 and a gyro weight of ~0.99. You can initialize the complementary angle using the accelerometer angle. The snippet below encodes the algorithm above.

// Gather measurements

// Accelerometer angle (degrees - 15.16 fixed point)
accel_angle = multfix15(divfix(acceleration, acceleration), oneeightyoverpi) ;

// Gyro angle delta (measurement times timestep) (15.16 fixed point)
gyro_angle_delta = multfix15(gyro, zeropt001) ;

// Complementary angle (degrees - 15.16 fixed point)
complementary_angle = multfix15(complementary_angle - gyro_angle_delta, zeropt999) + multfix15(accel_angle, zeropt001);


## What are the alternatives?¶

There are other methods for estimating attitude and other system states that are both more accurate and more complicated. See the links below.