My method of choice for combining the accelerometer and gyro signals into a clean and stable angle estimate is the Complementary Filter. It's computationally efficient and it directly addresses both transient horizontal acceleration and long-term gyro drift. The idea is to use the gyro for short term angle estimates, by numeric integration, but switch to the accelerometer for long-term estimates, by averaging. To do this in a continuous way, low-pass and high-pass filters are used. This is identical to the method I used in the Seg...stick Instructable, Step 10.
The image shows the a Complementary Filter in block diagram form. The sensor signals, converted to degrees and degrees per second, are the inputs. The accelerometer angle is low-pass filtered, reducing the influence of short-duration signals but maintaining a long-term average (from gravity). The gyro angular rate is first integrated, to get an estimate of angle, and then high-pass filtered, to remove any long-term drift. The two filtered signals are summed together to create a single angle estimate that combines the best parts of both sensor signals.
It's one of the few cases where the code is more concise than the block diagram or the written explanation:
// Excerpt from pd_digitalIMU.pde in 4pcb_ARD:
// -------------------------------------------------------------------
rate_pitch = (float) g_pitch * G_GAIN;
angle_pitch = AA * (angle_pitch + rate_pitch * DT);
angle_pitch += (1.0 - AA) * (float) a_pitch * A_GAIN;
// -------------------------------------------------------------------
The first line converts the raw gyro signal, g_pitch, into º/s, as discussed in the previous Step. The end of the second line is the numeric integration. DT is the loop time, 0.01s. A small change in angle, rate_pitch * DT, is added to the previous angle estimate.
The rest of the second line and all of the third line form the high-pass and low-pass filters, in discrete form. The high-pass acts on the integrated angle estimate and the low-pass acts on the accelerometer signal, a_pitch. The constant AA determines the time scale of the high- and low-pass filters, and is a value you can tweak depending on your quadrotor. Larger AA trusts the integrated gyro signal for longer. Shorter AA merges the accelerometer signal in faster.
The value for AA used in 4pcb corresponds with a time constant of 1s. This is the approximate length of time for which the gyro angle estimate is trusted, and the approximate length of time over which the accerometer signal is averaged. Since these are infinite impulse-response (IIR) filters, the transition is smooth and continuous and the time constant only represents where the weighting of the two filters is equal.
I wrote up a more thorough explanation of the Complementary Filter as applied to self-balancing system in this white paper. It covers some of the details of how AA is related to the time constant of the low-pass and high-pass filters, how rate sensor bias affects the filter, and some more details.
With Instructables you can share what you make with the world, and tap into an ever-growing community of creative experts.
How it Works »New Instructable »
