Estimating attitude by fusing the sensors

The last lesson set up a puzzle with an obvious shape. The gyroscope is right about how fast the craft is turning but drifts when you integrate it into an angle. The accelerometer is right about which way is down on average but shakes too much to trust from moment to moment. Each is good on exactly the timescale the other is bad on, and that is the opening for a fix: use the gyro for the fast changes and the accelerometer for the slow truth, and blend them so that the drift of the one is continually pulled back by the other. The classic blend is the complementary filter, and it is one line of arithmetic that a small flight controller can run thousands of times a second.

The complementary filter in one equation

Suppose the controller holds an estimate of the tilt angle, $\theta$, and updates it every small time step $\Delta t$. Two new pieces of information arrive each step: the gyroscope's rate $\omega$, which predicts a new angle by integration, and the accelerometer's own direct estimate of tilt, ${\theta }_{\text{acc}}$. The complementary filter combines them as

with $\alpha$ a constant just below one, often about 0.98. Read the two pieces. The first term takes the old angle, adds the gyro's motion over this step, and keeps 98 per cent of it: this is the fast, responsive part, the integrated gyro, and on its own it would drift. The second term nudges the estimate 2 per cent of the way toward whatever the accelerometer currently says down is: this is the slow correction that cancels the drift. Over a fraction of a second the gyro dominates and the estimate tracks quick motion cleanly; over many seconds the small accelerometer pull wins and holds the estimate locked to true level. Each term covers the band the other cannot, which is why it is called complementary.

The single knob $\alpha$ sets where the handover happens. Its time constant is roughly $\tau =\alpha \Delta t/(1-\alpha )$, the timescale below which the gyro is trusted and above which the accelerometer takes over. Push $\alpha$ toward one and the filter leans harder on the gyro, smoother but slower to correct drift; pull it down and it trusts the shaky accelerometer more, quicker to level but jumpier. The right value trusts the gyro over the seconds of a manoeuvre and lets the accelerometer own the minutes.

Example. A filter runs at $\Delta t=0.001$ s with $\alpha =0.98$. Roughly what is its time constant, and what does that mean?

$\tau =0.98\times 0.001/(1-0.98)=0.00098/0.02=0.049$ s. Below about 50 milliseconds the estimate follows the gyroscope; above it, the accelerometer steadily corrects any drift. So a fast flick is tracked by the gyro and a slow lean is anchored by gravity.

Now you. The same 0.001 s loop is set to $\alpha =0.995$. What is the time constant now, and is drift corrected faster or slower than before?

Why gravity cannot give heading

The accelerometer anchors two of the three rotations and only two. It reads the direction of gravity, which fixes how the craft is tilted in pitch and roll, but gravity points the same way whichever direction the craft is facing, so it says nothing about heading, the yaw angle. Spin a level craft on the spot and every accelerometer axis reads exactly as before. The gyro can measure the rate of that spin and integrate it, but with nothing to correct against, the yaw estimate drifts freely, and this is why a quadcopter's heading wanders over a long hover while its level stays true. To pin heading down needs a third sensor, a magnetometer, which reads the Earth's magnetic field the way the accelerometer reads gravity and gives an absolute compass direction to correct the yaw drift against. Many small racing craft simply do without, accepting slow heading drift because the pilot is steering by sight, while any craft that must hold a heading or return home carries one, and pays for it with sensitivity to the magnetic mess of its own motor currents.

Angles, and the trap of three of them

Describing an orientation by three angles, pitch, roll and yaw, is intuitive and is how a pilot thinks, but it hides a defect that bites when a craft points straight up or flips fully over. At certain orientations two of the three angles line up and control one and the same rotation, so a degree of freedom is lost and the maths becomes singular, a failure called gimbal lock. It is why acrobatic flight controllers do not store attitude as three angles internally but as a quaternion, a four-number object that represents any orientation without a singular point anywhere. Quaternions are harder to picture and the arithmetic is less transparent, but they never jam, and a craft that must fly through vertical and inverted has to use them. The three angles remain the language the pilot and the tuning screens speak; the quaternion is the language the controller keeps its own state in.

The estimate the rest of the craft depends on

What the fusion produces, whether by a plain complementary filter or by its more elaborate cousin the Kalman filter that carries an explicit model of the sensor noise, is a single trustworthy estimate of the craft's attitude and rotation rate, updated every control cycle. It neither drifts like the raw gyro nor shakes like the raw accelerometer. Everything the flight controller does downstream leans on it. The controller of the next lesson compares this estimate against what the pilot asked for and works out the correction, and the mixer turns that correction into motor speeds. A quadcopter is in the end a fast loop from this estimate to those four motors, and the quality of the estimate sets the ceiling on how well the craft can be made to fly. A noisy or laggy attitude estimate cannot be rescued by any amount of tuning downstream, which is why clean sensor mounting and honest fusion come before the control loop, not after it.