Added a complementary filter to rangefinder data (for 3.6)

[tridge]

This adds a 2 pole complementary filter to rangefinder data. The low frequency component is from the rangefinder and the high frequency is from the inertial altitude (the main EKF). The cutover frequency is set to 0.25Hz.
Here is an example in SITL:

in this case we've added a 0.6Hz sinusoidal noise source to the rangefinder. The red line is the rangefinder reading (low frequency input). The green line is the INS reading (high frequency). The resulting output is the blue line. The noise has been reduced by a factor of 5x.
The 0.25Hz cutover does mean we will respond more slowly to changing rangefinder conditions. We can add this as a tunable parameter if needed.

[zalice]

@tridge
Visual observations from pilot:
"There are still some altitude oscillations, but they're not as bad. The min/max was ~33.6-36.1. Before I saw a change from almost ~33.2-36.8m. The rate of climb/descent during the oscillations also appears slower than before"

Flight logs will be uploaded later today.

[tridge]

I've added a slew to the complementary filter cutoff frequency when in loiter (time wait on a waypoint) in auto mode. The time constant goes from 0.5Hz in normal flight down to 0.05Hz when in the loiter.
This should smooth out the loiter by getting rid of even quite long time constant noise from the rangefinder

[tridge]

The slew of the cutoff is working well for when we change from a higher cutoff to a lower cutoff, but is producing a nasty artefact when slewing from lower cutoff frequency to higher. Flight test results are good, but only because we happen to not be using the output of the complementary filter during the period where the artefact happens.

In the above:

• RF2.HF is the high frequency input to the complementary filter. This is sourced from inertialnav in copter, so it is the EKF height above origin
• RF2.LF is the low frequency input to the complementary filter. This is sourced from the (noisy) radar
• RF2.Out is the output of the complementary filter.
• RF2.FHz is the cutoff frequency of the complementary filter. This is set to 0.05Hz when in a loiter at a single position and 0.5 when in normal flight. It uses a simple lowpass filter to move from one condition to the other, with a time constant of around 10s.

As you can see, we get an artefact in RF2.Out when we finish a position hold at a waypoint and move to normal flight. We don't get the artefact when we move the other way.
Example SITL log here:
http://uav.tridgell.net/tmp/00000012.BIN

Possible solutions:

• recompute the two delay elements when we change the cutoff frequency to minimise the artefact
• remove the slew of the cutoff when we go from loiter to normal flight and instead call the filters reset function which computes the delay elements to match the current output

[tridge]

I've adjusted the reset handling to fix the issue with increasing filter frequency. It is now quite smooth

[zalice]

@tridge is there any reason not to define get (and set_cutoff_frequency) in the header?