LULU filter port to iNav as a single replacement for most filters in iNav, simplifying filter configuration drastically #10145

pjpei · 2024-06-17T10:41:28Z

I implemented a new type of filter based on the LULU filter by C.H. Rohwer, a mathematician at a local university. Refer to this wikipedia entry for details on the maths: https://en.wikipedia.org/wiki/Lulu_smoothing

It took real time to optimize this to run effectively.

I would really appreciate if anyone could verify if this works OK on more craft? I'm on Discord if anyone has questions about it. I've tested a local betaflight port on multiple craft and it seems to work nicely with really low delay, and now ported to iNav I hope that it will simplify filtering.

I’ve ported this to Inav and I have found the following:
As a single filter, it’s easy enough to set the filter type to LULU for the gyro and gyro antialiasing filters, while disabling all other filtering.
Recommended starting point: Set looptime to 500hz, set anti-aliasing lpf to 1, set gyro main filter lpf to 3, set dterm lpf to static 120hz pt1, rpmfilter on one harmonic and disable all dynamic filtering.

This will cause any spikes and pits that are more than 3 samples wide to disappear. For oscillating-type noise, like is to be expected from a gyroscope, this means that any sinusoid top or bottom half will be simply snipped off, keeping only the approximate middle value of the sinusoid after filtering.

The gyro lpf is now not measured in hz, but samples, as is the antialiasing lpf. More samples = more filtering but more delay.
Using the lulu on dterm seems to be a mess as the lulu on gyrofilter causes spikes on it, so recommended to use lulu on gyro only.
I've attached a flight log of a really bad frame 40mm whoop.

The bottom line is the smoother knocks out spiky noise in full, by the amount of samples specified by the “lowpass hz” for the implementation. For example, if you set the “gyro lowpass hz” to 4, and you set the filter to LULU in CLI, you will find that any spike or dip in signal for 4 samples or less will disappear.

The LULU filter is a really low latency filter and you’ll see that if you set the sample number to 3 for the gyro LPF and the antialiasing filter samples to 1, it should work in many cases.

Below is a 40mm toothpick with a terrible frame and construction on inav running LULU at 1 sample antialiasing filter and 3 samples gyro LPF.

blackbox_log_2024-06-17_122915.TXT

If you look closely, you’ll see the gyro filter has some sudden changes in slope. This is unfortunately a reality of the dterm as a sinusoidal type noise filtered by LULU causes sudden jumps in the gyro value.

To mitigate the problems caused by the spiky dterm, we set a single gyro lowpass filter when needed. Rigid frames have less of an issue than this terrible ABS plastic frame, but I’ve found for rigid carbon fibre frames it can be run with a very high Hz PT1, while for frames like this 40mm one it works better with a 120Hz PT1. This removes the spiky noise and smoothes it out nicely.

On the example above, there is no other filtering done except the LULU and a 120Hz PT1, and one harmonic RPMfilter. I have disabled all dynamic filters on this whoop as well.

Config dump attached for this specific craft for completion's sake
INAV_8.0.0_cli_20240617_123346.txt

pjpei · 2024-06-17T10:48:55Z

Note that the LULU filter really gets processor intensive above around 6-8 samples of filtering. Check CPU usage before flight!

DzikuVx · 2024-06-17T10:55:36Z

Can't wait to test it!

DzikuVx

@pjpei amazing! Thee are few things we can improve thought

src/main/fc/config.c

DzikuVx · 2024-06-17T11:00:27Z

src/main/fc/config.c

-    }
+//    if (gyroConfig()->adaptiveFilterMaxHz - 5 < gyroConfig()->gyro_main_lpf_hz) {
+//        gyroConfigMutable()->adaptiveFilterMaxHz = gyroConfig()->gyro_main_lpf_hz + 5;
+//    }
 #endif



It might be a good idea to disable LULU when DYNAMIC or ADAPTIVE gyro filter is running

I think if we have specific filter type LULU, as implemented, this should be fixed?

DzikuVx · 2024-06-17T11:05:56Z

src/main/sensors/gyro.c

@@ -244,9 +244,10 @@ static void initGyroFilter(filterApplyFnPtr *applyFn, filter_t state[], uint16_t
 {
    *applyFn = nullFilterApply;
    if (cutoff > 0) {
-        *applyFn = (filterApplyFnPtr)pt1FilterApply;


This function should not be modified in this way, because it changes how both gyro LPF work. By doing so you just enabled double LULU on gyro data

The distinction should happen inside static void gyroInitFilters(void) by adding a new setting to run lulu on gyro

DzikuVx · 2024-06-17T11:07:01Z

src/main/fc/settings.yaml

-        description: "Gyro processing anti-aliasing filter cutoff frequency. In normal operation this filter setting should never be changed. In Hz"
-        default_value: 250
+        description: "Gyro processing anti-aliasing filter cutoff frequency. In normal operation this filter setting should never be changed. In samples"
+        default_value: 1
        field: gyro_anti_aliasing_lpf_hz
        max: 1000
      - name: gyro_main_lpf_hz


This setting shold not be changed, but a new one like 'gyro_lulu_sample_count` added

DzikuVx · 2024-06-17T11:07:46Z

src/main/fc/settings.yaml

-        description: "Gyro processing anti-aliasing filter cutoff frequency. In normal operation this filter setting should never be changed. In Hz"
-        default_value: 250
+        description: "Gyro processing anti-aliasing filter cutoff frequency. In normal operation this filter setting should never be changed. In samples"
+        default_value: 1


Let's not run lulu on antialiasing filter at all. It makes no sense as this filter is only to remove aliasing effect between acquisition and processing

src/main/fc/settings.yaml

src/main/common/lulu.c

pjpei · 2024-06-17T12:56:08Z

Thanks for the review! I made the changes as requested. I added a gyro filter mode "LULU" which is different from the current 3, and that sorts out much of the issues in the previous pull request.

pjpei · 2024-06-17T12:58:11Z

src/main/fc/settings.yaml

-        description: "Gyro processing anti-aliasing filter cutoff frequency. In normal operation this filter setting should never be changed. In samples"
-        default_value: 1
+        description: "Gyro processing anti-aliasing filter cutoff frequency. In normal operation this filter setting should never be changed. In Hz"
+        default_value: 250


I tested this with a bit higher antialiasing filter frequency of 400, the LULU filter seems to be happy with it. We might want to change this to 400 or similar if lulu is used as a default.

pjpei · 2024-06-18T08:10:40Z

I see there's still changes requested? I think I did complete all the changes that were specified, please note if I missed any :)

pjpei · 2024-06-20T11:49:29Z

My testing settings are:

set looptime = 500
set gyro_anti_aliasing_lpf_hz = 1000
set gyro_lulu_sample_count = 3
set gyro_filter_mode = LULU
set dynamic_gyro_notch_enabled = OFF
set dterm_lpf_hz = 120
set dterm_lpf_type = PT1
set yaw_lpf_hz = 0
set switch_disarm_delay = 0

Note the gyro_lulu_sample_count might need to be higher and might go to 2 for sturdy frames. Don't think the other settings need to really change much.

pjpei · 2024-06-21T11:04:15Z

I thought a drawing would work nice for setting the gyro_lulu_sample_count value.

A good starting point is in my mind half the sample count of the large oscillatory vibration, as indicated below. If the oscillation goes up-down-up in a sine-like fashion, it would be half the wavelength we set the samples to. So if the full up-down-up motion goes from zero to top, to bottom, to zero that would mean half the distance from thebegin to the end of this description of a single up-down sinusoidal wave. The LULU smoother would then remove the peak and the pit, leaving only the center value of the sinewave after filtering.

I've found that on my quads a value of 2-3 works well, but for others we might need differing values.

pjpei added 4 commits June 17, 2024 12:10

Implemented basic lulu filtering system

c1729b5

Merge branch 'iNavFlight:master' into master

b6b5165

Fixed bug

8eb6fba

Undid target change

895ff5b

DzikuVx added the Experimental label Jun 17, 2024

DzikuVx requested changes Jun 17, 2024

View reviewed changes

DzikuVx reviewed Jun 17, 2024

View reviewed changes

src/main/common/lulu.c Outdated Show resolved Hide resolved

Made improvements as requested

96217f3

pjpei commented Jun 17, 2024

View reviewed changes

pjpei added 2 commits June 17, 2024 17:35

Updated CLI settings

fc4db8a

Reverted unnecessary changes in setting descriptions

6e5704f

pjpei requested a review from DzikuVx June 18, 2024 09:03

DzikuVx mentioned this pull request Jun 24, 2024

Gyro LULU smoother as gyro filter alternative #10177

Merged

DzikuVx merged commit 6e5704f into iNavFlight:master Jun 24, 2024
22 checks passed

MrD-RC added this to the 8.0 milestone Nov 19, 2024

pjpei mentioned this pull request Dec 26, 2024

Implement lulu filtering for QuickSilver BossHobby/QUICKSILVER#121

Open

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

LULU filter port to iNav as a single replacement for most filters in iNav, simplifying filter configuration drastically #10145

LULU filter port to iNav as a single replacement for most filters in iNav, simplifying filter configuration drastically #10145

pjpei commented Jun 17, 2024

pjpei commented Jun 17, 2024

DzikuVx commented Jun 17, 2024

DzikuVx left a comment

DzikuVx Jun 17, 2024

pjpei Jun 17, 2024

DzikuVx Jun 17, 2024

DzikuVx Jun 17, 2024

DzikuVx Jun 17, 2024

pjpei commented Jun 17, 2024

pjpei Jun 17, 2024

pjpei commented Jun 18, 2024

pjpei commented Jun 20, 2024

pjpei commented Jun 21, 2024 •

edited

Loading

LULU filter port to iNav as a single replacement for most filters in iNav, simplifying filter configuration drastically #10145

LULU filter port to iNav as a single replacement for most filters in iNav, simplifying filter configuration drastically #10145

Conversation

pjpei commented Jun 17, 2024

pjpei commented Jun 17, 2024

DzikuVx commented Jun 17, 2024

DzikuVx left a comment

Choose a reason for hiding this comment

DzikuVx Jun 17, 2024

Choose a reason for hiding this comment

pjpei Jun 17, 2024

Choose a reason for hiding this comment

DzikuVx Jun 17, 2024

Choose a reason for hiding this comment

DzikuVx Jun 17, 2024

Choose a reason for hiding this comment

DzikuVx Jun 17, 2024

Choose a reason for hiding this comment

pjpei commented Jun 17, 2024

pjpei Jun 17, 2024

Choose a reason for hiding this comment

pjpei commented Jun 18, 2024

pjpei commented Jun 20, 2024

pjpei commented Jun 21, 2024 • edited Loading

pjpei commented Jun 21, 2024 •

edited

Loading