Ardupilot: INS_NOTCH_FREQ > SCHED_LOOP_RATE/2 results in unflyable copter

[andyp1per]

See https://discuss.ardupilot.org/t/notch-filter-is-not-a-notch-filter/42280

Bug report

According to the maths (http://dspguide.com/ch19/3.htm), notch filters can have a centre frequency at most half of the sample frequency. Since the notch is sampled from the fast loop, this means that the notch can be at most 200Hz for default settings (fast loop is 400Hz). For small copters it is common to want to set a notch at the resonant frequency of the props - for my 180mm quad this is 215Hz for GF 4045BN and 268 Hz for GF 4052 - but if you try this the copter is totally unflyable.

Issue details

• You can't set a notch frequency that is reasonable for a small copter
• If you do you will crash your copter
• None of this is documented

At a minimum the relationship between INS_NOTCH_FREQ and SCHED_LOOP_RATE should be documented.

Version
3.6.9rc2

Platform
[ ] All
[ ] AntennaTracker
[ X ] Copter
[ ] Plane
[ ] Rover
[ ] Submarine

Airframe type
X4

Hardware type
Pixracer

Logs

[khancyr]

It seems that your are right and we get an issue with the sampling and documentation... We should mention Shannon rule at minimum.
About the sampling rate, I am surprised that it is done on the vehicle loop rate... I believed that should be done at imu rate or close enough, depending on computation expensiveness.
Could you share a log with non flyable copter ?

[lthall]

This is done at the imu sample rate. This varies from IMU to IMU but it is up to 8kHz.

I am not sure if we have checked to make sure it is not set higher than the nyquist frequency though. I could see us not doing this because it varies depending on the IMU and we may have been lazy :)

I will need to look into this.

I will need to check.

[andyp1per]

I'll get a log. Note I am motivated to work on this and have the coding skills. I already have a version of the batch sampler that allows me to see the post-filtered gyro values.

[lthall]

Yes this needs to be fixed. It is currently being applied in the fast loop, not the sensor read.

[andyp1per]

What I don't quite understand is if I look at the LPF gyro filtering that also seems to be occuring in the fast loop (look at AP_InertialSensor:update() -> AP_InertialSensor_Backend::update() -> AP_InertialSensor_PX4::update() etc)

[andyp1per]

Also please see https://github.com/ArduPilot/ardupilot/issues/11331 - I think I was getting gyros inconsistent mainly because the filter was crapping out, but my thinking still stands for that issue

[tridge]

after discussions with @lthall we need the following changes:

• apply notch filter to 1kHz data, not update() loop data
• ensure that notch is applied on same gyro as control loops are using (not guaranteed now with EKF lane switching)
• investigate if we can do notch on all axes/imus (is cpu cost acceptable)
• offer ability to do batch logging of post-filtered data, so impact of filtering is visible in FFT

[andyp1per]

I'm happy to work on this if no-one is going to get to it anytime soon. Equally happy to test solutions. I have a fix for (4), although it will need changing when the notch filter filtering changes.

[andyp1per]

I have created https://github.com/andyp1per/ardupilot/commit/44451f545594ec7044921a3f387242b468acd636 and it appears to work. I have lightly tested but am able to bounce my copter around with a notch at 215Hz and loop at 400Hz. I also can see the results in the log :) This is controlled by LOG_BAT_OPT=2.
The whole change address 3 out of 4 items, I have yet to try this with multiple IMUs and I have yet to try flying, but please let me know if this looks like I am on the right track.
It's currently against 3.6.9rc2, but obviously I can fix that.

[andyp1per]

00000133.zip 158Hz notch, post-filter
00000132.zip 158Hz notch, pre-filter
00000131.zip 215Hz notch, pre-filter
00000130.zip 215Hz notch, post-filter

These are logs with the fix applied

[andyp1per]

Seems to be alright with two IMUs at 800Hz:

Post-filter log

I have created a pull request for master https://github.com/ArduPilot/ardupilot/pull/11371

[andyp1per]

This all works pretty well. You can see here an actual flight with the gyro notch at 215Hz clearly visible in the trace. No control issues.
00000140.zip

[andyp1per]

This is fixed now