Thanks to the great work of @andyp1per (see Notch filter is not a notch filter?) I was able to log and do FFT analysis on post filtered gyro, and something seemed wrong.

Here’s one FFT post filter with GYRO_FILTER = 80hz (and notch at 144hz, badnwidth 14hz, attenuation 30db) and with “normal” flight speed:

Here’s another with GYRO_FILTER = 100hz (and notch at 147hz, badnwidth 60hz, attenuation 55db), on a fast flight (I zoomed to have same y axis):

Something seems strange, why I can still see frequencies higher than low pass filter cutoff?

There was a bug in @andyp1per implementation, when applying notch filter the lowpass was ignored, ok, tested again after a quick fix and… nothing changes. I can still see high frequencies in post filtered FFT.

That seemed very strange to me, so I studied the biquad filter implementation and **found a couple of bugs in the parameter calculation and in the filter function**.

Here’s a quick python sketch with the ardupilot filter algorithm vs standard implementation.

I know this test is on my rewriting of ardupilot C code in python, not a testing on the real C implementation, but I’m planning to run this on a log and I’m also writing a C unit test (but I need some help on how to build it).

Python test output:

You can see in the top graph that the current implementation (green) is almost the same of the raw signal (yellow), while the standard LPF (red) removes the high frequencies.

It is very surprising to see copter flying well even without a LPF filter on gyro and accelerometer, and that’s why I saw a dramatic change in flight performance when I set up the notch filter.

Here’s the issue on github:

Here’s the pull with the fix:

Tomorrow I’ll test the fix on a real drone and post here the FFT results!