Notch filter is not a notch filter?

ArduCopter Copter 3.6
1

I’m seeing weird behaviour when using the notch filter. I’ve set it at 218Hz, 20Hz BW, 15dB attenuation and I lose all control of my quad. So I’ve been digging into the maths a bit in Filter/NotchFilter.cpp and I would dearly love someone to explain what is going on. The implementation is not like betaflight’s notch filter which seems to be mostly a copy of https://github.com/wooters/miniDSP/blob/master/biquad.c which is an implementation of http://shepazu.github.io/Audio-EQ-Cookbook/audio-eq-cookbook.html

In the commentary for both of those it indicates that the attenuation is only used for peaking and shelving filters. Whereas the AC implementation is using the gain and manipulating the coefficients in a way that does not look like any of the BiQuad filters described. Before I start diving in and creating an alternative implementation can anyone tell me what the underlying maths is for the existing one? I’m assuming that either the implementation is wrong or that the configuration yields bizarre results, and it’s too difficult to decipher what is going on.

2

I’ve also had my copter lose control when enabling the notch filter… As did Paul last year (he was active in the micro arducopter thread last year).
But strangely the last time I enabled it, with the same parameters, it worked and I’ve had no problems with it. I wonder if it has to do w/ master vs 3.6, or maybe it’s related to other parameters that I’ve changed since then. It’s working for me now, and I’m running master… And my full parameter list is one of the last posts in the micro arducopter thread, if you want to look at my config that IS working. For me, the HUD would start spinning in MP when I enabled the notch, when it wasn’t working.

Anyway, you aren’t the only one seeing those issues. Hopefully someone w/ more experience w/ the code will see your post and offer some information.

This post: Notch filter testing
mentions Leonardthall designed the filter, so maybe reach out to him…

3

I have a suspicion that this is related to http://dspguide.com/ch19/3.htm - “Two parameters must be selected before using these equations: f , the center frequency, and BW , the bandwidth (measured at an amplitude of 0.707). Both of these are expressed as a fraction of the sampling frequency, and therefore must be between 0 and 0.5.” Which would imply that you can’t have a centre frequency > 200Hz if the loop rate is 400Hz. I wonder if everything started working for you when you switched to a loop rate of 800Hz.

4

Hello,

the biquad filter isn’t the same as a notch filter …
Mathmaticaly, the current implementation seems good to me, but we clearly lack way to debug it.
On ardupilot the sensor loop for imu is > 1000hz (I think 8kHz on unfiltered), so that isn’t related to the 400Hz main loop.
If you could share your log in failure, it would help to investigate the errors !

5

@khancyr NotchFilter::apply() is called from AP_InertialSensor::update() which is called from Copter::fast_loop() which is 400Hz?

1 Like
6

Has anyone written any tests for the current notch filter? Be good to compare to other implementations.

7

I can 100% confirm that this is down to INS_NOTCH_FREQ > SCHED_LOOP_RATE/2.
If I have:

INS_NOTCH_FREQ=215
SCHED_LOOP_RATE=400

then no control.

INS_NOTCH_FREQ=215
SCHED_LOOP_RATE=800

copter behaves as normal. Gratifying that the maths is correct, but could do with some additions to the documentation.

1 Like
8

Hi all,

I just came across this thread. I am the person that designed this notch filter… that isn’t a notch filter. It is a peakingEQ filter so it has a centre frequency, bandwidth and attenuation variable. I can’t recall where I got the maths but I recall that it was similar to the beta flight implementation. It is also the same as Cookbook formulae you linked to but I think they missed the definition of alpha for the EQ filter.

I have been using this pretty extensively on my industrial tunes and have not seen an issue yet.

I have red through the posts above but do we have a reproduceable issue here? Could you provide the method to reproduce the problem?

9

We’ve got small copters and are needing to use a relatively high frequency, over 200hz. And I think what Andy figured out is that if the frequency is over twice the loop rate, the copter loses control. So that should be reproducible. But is maybe not fixable and not a real problem if the issue is documented.

I can update the wiki later and add a line about it.
Edit, I just noticed the github post… I’ll wait to update the documentation until we figure out exactly what’s going on… Changing the loop rate changes the results for us… But from the few posts on github it sounds like that rate might not exactly be the issue…

10

Yeh, with small aircraft you need to keep the loop rate as high as possible. The D term filters for example need to be up around 50 Hz for small aircraft so that may break if you go to 100 Hz loop rate. If it was set to 100 Hz then it may break at 200 Hz loop rate.

The Notch filter is independent of the loop rate so it can’t be that (unless something is messed up).

I don’t see any indication that there was any link to the notch filter other than the aircraft was unflyable after they turned it on. Without a log we can’t evaluate it.

On that note @andyp1per, can you give us a log of that flight?

11

Well, I saw the same thing…
When I tried using the notch at over 200hz with the loop rate at 400, the copter was unable to fly, and the hud in MP was spinning with the notch enabled.
Increasing the loop rate fixed the issue.

I unfortunately had logging disabled at the time, so don’t have a log of it. If Andy can’t supply a log, I will get some more data later this week when I get some time w/ my copters.

1 Like
12

I’ll get a log tonite hopefully, it’s trivial to reproduce. Note that I have used the notch filter at lower frequencies successfully on this copter, I’ll try and get examples of the different scenarios.

I still contest that this is happening at the fast loop rate because NotchFilter::apply() is called from AP_InertialSensor::update() which is called from Copter::fast_loop() which is 400Hz, but maybe I am reading it wrong.

1 Like
13

Thanks for the explanation. Usually with a notch the bandwidth is defined by the -3dB points. Is the user-defined attenuation at the bandwidth edges or at the peak/trough? Any reason it’s not a notch filter? I read that notches can become unstable at higher attenuation. The betaflight implementation appears to have some dodginess as well, they set:

const float omega = 2.0f * M_PI_FLOAT * filterFreq * refreshRate * 0.000001f

but I think mathematically it should be

const float omega = 2.0f * M_PI_FLOAT * filterFreq / refreshRate

it just so happens that the refresh rate is usually 1000.
I’m tempted to try and implement all of these in python and see what they look like in practice.

14

You are correct!!! Bugger!!!

This should be happening at the full rate. That needs to be fixed!!

So yes it will crap out at 200 Hz!!!

Fantastic catch, I owe you a beer!!

So the frequency is the centre frequency and the bandwidth is approximately the 3 db points. When you set a it to a large depth this will be the same as what you would expect from a notch filter. The attenuation is the maximum depth of the Notch.

image
image1536×1008 46.8 KB

This lets us use the notch filter for more than simply filtering the propeller noise. There are a number of other uses to address some control dynamics problems.

2 Likes
15

Ok, understand - so my next question is what attenuation gives the same profile as the betaflight notch filter? :slight_smile:

And how did you generate those graphs? Can you share the code?

16

So the performance approaches the notch filter as the attenuation gets larger. Because it is a log scale you can’t ever get perfect notch. You have effectively got a full notch filter at 40 dB attenuation.

The code is all Matlab so you would need access to a licence to use my code.

17

:slightly_smiling_face: thank you @andyp1per, I was having same issue!

@Leonardthall Were can I move NotchFilter::apply() to test it?

18

Sorry @guglie, it isn’t that simple. I will chat to Tridge and Randy tomorrow and see what we can do.

1 Like
19

@Leonardthall Ok, thanks!
Let me know if I can help by the way

1 Like
20

I have put up a potential fix https://github.com/andyp1per/ardupilot/commit/44451f545594ec7044921a3f387242b468acd636

2 Likes