Simple Sensor Fusion with Complementary Filter
Let's say you are on an airplane traveling at low altitude.
Ground hazards concern you. Thus, you must pay attention to the ground clearance.
You are lucky since the airplane is equipped with two sensors: a GPS and a laser altimeter.
The first sensor, the GPS, reports the plane's altitude relative to mean sea level.
It helps knowing when the airplane has changed altitude, but it is blind to ground obstacles.
The altitude measurement is stable and looks relatively plausible for an airplane.
But there are sometimes jumps in altitude that are probably due to satellite conditions.
The second sensor, the laser altimeter, reports the airplane's height above ground.
You think to yourself that this sensor will be a lot more useful, since it actually perceives the ground.
Unfortunately, you notice that it gives a very noisy measurement.
The height fluctuates a lot, but a drop in ground clearance is still noticeable when the airplane passes over a tree.
You are not completely satisfied with either of these sensors.
The GPS gives you a stable measurement, but it doesn't report a change of clearance when there are obstacles on the ground, which could be dangerous.
The laser altimeter does perceive them, but gives a clearance that varies too much to be physically plausible for an aircraft.
You wonder, is it possible to somehow combine the strengths of both sensors (stability from the GPS and sharpness from the laser altimeter) into a reliable altitude measurement for your aircraft?
It turns out it is possible with a complementary filter — one of the simplest ways to do sensor fusion.
The first thing to notice is that the GPS can be trusted for short-term dynamics. Since it is stable, if its measurement doesn't change, we can say that the aircraft has not moved in the short term.
On the contrary, the laser altimeter is trustworthy for longer-term dynamics because if its average value changes over multiple timestamps, it means that there is something underneath the aircraft.
Ideally, you would like to keep the high-frequency data from the GPS because it anchors the airplane to a reliable altitude measurement that doesn't fluctuate a lot.
But you would also like to take advantage of the low-frequency changes in the laser altimeter that signal obstacles on the ground and that provide a fresh measurement not affected by external circumstances like satellite positioning.
Let's examine in detail how to do that.
To use the short-term dynamics of the GPS signal, we want to blend the signal towards the rate of change. If the change is high, it means the airplane is rapidly gaining altitude and we want to capture that information.
Thus, the reported altitude for short-term dynamics is the previous altitude plus the change of altitude.
Where:
- y — filtered altitude (output)
- y_prev — filtered altitude at the previous time step
- x, x_prev — current and previous GPS reading
- α — smoothing factor between 0 and 1
α is a parameter to adjust how much the rate of change affects the reported altitude.
Try to see how changing it affects the result.
This is called a high-pass filter because high frequencies pass through, which keeps the short-term dynamics of the signal.
You should notice that a low α keeps the reported airplane altitude constant whereas a high alpha lets the rate of change in measurements affect the altitude.
When α is 1, the filter keeps everything, so the output simply tracks the raw GPS signal.
Now, for the laser altimeter, we would like to keep the long-term dynamics.
The intuitive way of doing that is to have a weighted average between the new reading and the previous output.
The true long-term signal accumulates through the previous output while we nudge the new readings towards it.
Where:
- y — filtered altitude (output)
- y_prev — filtered altitude at the previous time step
- x — current laser-altimeter reading
- α — smoothing factor between 0 and 1
An alpha (α) parameter close to 1 will smooth the signal because each new output y will be very close to the previous one.
An alpha close to 0 will let the new measurement influence the output, effectively tracking the input more closely.
This is a low-pass filter because low frequencies pass through, which keeps the long-term dynamics of the signal.
Now, if we combine these two filters, the output signal will have both the short-term and long-term dynamics of the altitude input, which makes it complementary.
Where:
- y — fused altitude estimate (output)
- y_prev — fused altitude at the previous time step
- x_gps, x_prev_gps — current and previous GPS reading
- x_laser — current laser-altimeter reading
- α — smoothing factor between 0 and 1
The laser altimeter measurements help discover the trees. The ground clearance drops.
GPS measurements are a reliable source of altitude changes. This is especially noticeable before approaching the forest, where the complementary filter shows a slow increase in ground clearance induced by the slow increase in altitude. The low-pass-filtered altimeter data is still too much affected by noise to capture that movement accurately.
With the complementary filter we get the best of both worlds from each sensor signal.
They are fused into a more reliable and precise ground-clearance estimation that a single signal would not be able to provide.