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.

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.

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!

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 (which we *can't* for the helicopter lab, but we *can* for the reaction wheel lab), then we can use a small angle approximation! Thus the tilt angle is given by:

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.

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.

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.

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.

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.

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:

- Gather a set of accelerometer and gyro measurements
- Compute the accelerometer angle
- 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) - 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
mpu6050_read_raw(acceleration, gyro);
// Accelerometer angle (degrees - 15.16 fixed point)
// Only ONE of the two lines below will be used, depending whether or not a small angle approximation is appropriate
// SMALL ANGLE APPROXIMATION
accel_angle = multfix15(divfix(acceleration[0], acceleration[1]), oneeightyoverpi) ;
// NO SMALL ANGLE APPROXIMATION
accel_angle = multfix15(float2fix15(atan2(-filtered_ax, filtered_ay) + PI), oneeightyoverpi);
// Gyro angle delta (measurement times timestep) (15.16 fixed point)
gyro_angle_delta = multfix15(gyro[2], zeropt001) ;
// Complementary angle (degrees - 15.16 fixed point)
complementary_angle = multfix15(complementary_angle - gyro_angle_delta, zeropt999) + multfix15(accel_angle, zeropt001);
```

If you are not using a small angle approximation to compute the accelerometer angle (like in the helicopter lab), you may need to low-pass the raw accelerometer measurements before computing the accelerometer angle. Why? Think about how white noise propatates through the `atan2`

function.

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