How to filter noisy distance sensor measurement

[Kyuhyong_You]

Below is my distance sensor measurement which looks ugly with lots of noise.
I just hooked up my maxbotics sonar PWM pin to AUX5 on pixhawk board.
I set ll40ls as pwm to utilize this sensor and this is the result.

Looking at the code LidarLitePWM.cpp I see no filtering is going on and just publish as is.
Is there a way I can put this data into a pre defined filter or I have to make one.
Thanks in advance.

Kyu

[tumbili]

Are you flying over very rough terrain or over a more or less flat surface? I've seen these measurements before e.g. when flying over crops. In that case you need to come up with your own solution for getting the data you are interested. If this is the sensor output over a smooth surface I'd say you have a problem with your sensor.

[Kyuhyong_You]

I am flying over flat surface. Maybe I need to change the sensor?

[tumbili]

Yes, you will have to look at the specs of your sensor but it does indeed not look great. What's the min and max range you need, maybe I can suggest a better sensor?

[Kyuhyong_You]

The drone would be mostly fly indoor and within 0.3m to 3m.
I know there are lidar sensors works find but our drone is size constrained and anything bigger than max sonar will not fit. But I would appreciate if you can suggest any sensors.

[matt]

It seems that you have glitchs in your signal, could you screenshot on a smaller window (1s)
Glitchs can be easily removed with a median filter
Matt

[Kyuhyong_You]

Yes, I implemented a moving average filter within the ll40ls pwm driver and got following result.

Log is here
https://logs.px4.io/plot_app?log=a14a6b2d-1ed0-4892-a825-07c97bcfb3f7

This seem to improve a little but not much improvements in holding the altitude.
mc pos control is trying to manipulate pos z setpoint when enabled range sensor aid.
To me, flying with only barometer reference is far better than aid with range sensor. This is quite strange behavior.

[Kyuhyong_You]

I have been investigating this issue and did some FFT analysis of the given data and following is the result

This gives me some idea about applying low pass filter.
Now I combined lowpass filter with average filter and give better results as here.

Added note below--
I also found in Arducopter that it also takes into account for applying low pass filter for their range sensor as per here.

ArduPilot/ardupilot/blob/5ad1dee284c27e86a92622785c6c79e08085327e/ArduCopter/sensors.cpp#L17

In this line of code, Their cutoff frequency is set to 0.25 which is quite match to the above FFT results.

[dakejahl]

You need to throw away those bad measurements. Filtering does not work well when the measurement noise is so high (it's not even noise, it's effectively detection glitches which should be treated like outliers). Try this:

static int last_measurement = 0;
static int data_valid_counter = 0;
bool data_valid = false;
if(current_measurement >last_measurement + delta || current_measurement < last_measurement - delta) {
    data_valid = false;
    data_valid_counter = 0;
} else {
    data_valid_counter++;
    if(data_valid_counter >= 3) {
        data_valid = true;
    }
}

last_measurement= current_measurement;

if(data_valid) {
    /* Publish your data to uORB*/
}

I have an algorithm for my sensor that is similar to this with a bit extra. Essentially what I am suggesting above is imposing a requirement of 3 consecutive measurements that are within a delta of each other before allowing publishing. This ensures that you do not include spurious measurements and it also ensures that the data you are publishing has been stable for at least a little bit.

edit: I don't have my code on this computer, I can advise more on monday. But this example is meant to give you an idea of the type of checks you need to implement. Get creative with it, straight math won't solve this problem, gotta use logic.

[dakejahl]

That sensor is pretty terrible though, I'd try something else if you don't have access to the settings on the ASIC

[Kyuhyong_You]

@dakejahl Thank you for sharing such a valuable info.
Will take a look tomorrow and see how it works.

[Kyuhyong_You]

This is a snap shot of the sonar data and distance delta for each sample below.

This plot gives me an idea about how much of logical filter needed and I think 0.4m / sampling time would be enough.

float distance = float(_pwm.pulse_width) * 1e-3f;   /* 10 usec = 1 cm distance for LIDAR-Lite */
if(fabsf(distance - _distance_prev) > 0.4f) {
    distance = _distance_prev;
}
distance = _lpFilter.apply(distance);   //Apply filter
_distance_prev = distance;              //Save current distance
_range.current_distance = distance;

Now I can get following result.

I applied low pass filter with cutoff frequency =0.5Hz.
But here is another problem.

I still need to figure out why filtered data is about 3 seconds behind the actual data and how to resolve this.
Hopefully if this does not bother the overall flying characteristics, I would be fine too.

[matt]

This delay is surprinsing, what kind of LP filter it is?

Also to remove outliners, I suggested you to use median filter which is different from average filter. Average filter takes into account every value, and attributes it the same weight. So glitchs will strongly impact your filtered value. While median filter will sort your N previous values and keep the (N/2)th, so extremes values like glitchs will be simply ignored.

Matt

[Kyuhyong_You]

Hi Matt,
I am using LowPassFilter implemented inside the math library in PX4 source code.
Do you have any reference for implementing median filter you mentioned?
Thanks,

[matt]

Hello,

I've just write this piece of code, i haven't tested it yet, but it should work

MattMgn/Firmware

Let me know if you implement it

Matt

[Kyuhyong_You]

Hi matt,

Thanks for sharing your work. Actually, the above noise issue was simply solved by replacing sonar sensor with different one. Now I am struggling with other issues.
Anyway, I will definitely check your code and see it works. Thanks,

Kyu