Notch filter #13549
Notch filter #13549
Conversation
|
Nice! |
|
Looks good. If we want to get fancy later we might want to consider unifying with LowPassFilter2p and have a generic biquad cascade IIR base or template. Do you want to take a pass in this PR to start using the filter? Then actually filter the data here. I was also considering doing all the filtering downstream on only the primary (#12730). I'll revisit that idea later after killing off DriverFramework. Once it's out of the way it'll be easier to change things in the sensor pipeline. |
bresch
force-pushed
the
dev-notch-filter-upstream
branch
from
484c966
to
bc87523
Compare
src/modules/sensors/sensor_params.c
Outdated
| * @reboot_required true | ||
| * @group Sensors | ||
| */ | ||
| PARAM_DEFINE_FLOAT(IMU_GYRO_NOTCH_W, 20.0f); |
There was a problem hiding this comment.
Probably overthinking this, but another option could be to use NF for "notch filter" and then have room to be a bit more descriptive with frequency and bandwidth.
IMU_GYRO_NOTCH_F -> IMU_GYRO_NF_FREQ
IMU_GYRO_NOTCH_W -> IMU_GYRO_NF_BW
…h freq <= 0 and add test for disabled filter
set the notch center frequency and bandwidth
bresch
force-pushed
the
dev-notch-filter-upstream
branch
from
bc87523
to
12fb7b8
Compare
|
@dagar I rebased, changed the names of the parameters and implemented the "array" version of NotchFilter for the new |
| @@ -54,7 +54,7 @@ class LowPassFilter2pArray : public LowPassFilter2p | |||
| * | |||
| * @return retrieve the filtered result | |||
| */ | |||
| inline float apply(const int16_t samples[], uint8_t num_samples) | |||
| inline float apply(const float samples[], uint8_t num_samples) | |||
There was a problem hiding this comment.
I know this is ugly, but it was done to avoid copying all the data again. Let me see if it matters (we're barely using the FIFOs currently).
There was a problem hiding this comment.
I had to do that because I wanted the NotchFilterArray template to output a float array, so then the lowpass needs to accept a float array as well
|
I'll find some flash to free up to get this in. |
|
@dagar What would be the next step here? Is the array implementation too heavy? |
The extra stack is a bit of a concern. The IMU drivers using FIFOs aren't really used yet. Let's bring in the core change and readd the filter array (FIFO data) once we're able to test easily. |
When vibrations are coming from structural resonances, they are usually at low frequencies and setting a lowpass filter to avoid feedback amplification often ruins the phase margin of the system. Reducing the phase margin limits the loop gains and leads to poor rate tracking and disturbance rejection.
Structural resonances have usually a very narrow frequency amplification band and depending on the material, only lightly damped (carbon fiber plates have really little damping).
The notch filter is a good solution to this problem as the phase loss before the cutoff frequency is much less than on a 2nd order Butterworth low-pass filter as shown on the plot below:
As an example, take a drone that has a resonance at 20Hz and more noise at 80Hz. Currently, a low-pass filter has to be set below 20Hz to effectively reduce the amplitude of the resonance. With this PR, a notch filter can be placed at 20Hz (with the required bandwidth) and a low-pass Butterworth between 50 and 70Hz (depending on the amplitude of the noise at 80Hz). Then gains robustness and can be returned to gain more performance if required.
Also, since most of the noise comes from the blades passing over the arms, we can in the future create a bank of notch filters that tracks the frequency of each rotor is an RPM feedback signal is available (DShot, UAVCAN for example).
Derivation of the discrete time notch filter (bilinear transform) taken from :
Rusty Allred, Digital Filters for Everyone - 3rd edition, 2015
Edit: add group delay graph