How to filter noisy distance sensor measurement


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?


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. :wink:


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


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

Log is here

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.


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.

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

1 Like

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 {
    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.


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


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


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. :wink:


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.



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?



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

Let me know if you implement it



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,



Hello @Kyuhyong_You ! I was curious to know which sonar sensor worked for you. :slight_smile:
Thanks !


Hi @Vrinda,

I used sonar came with px4flow not just one but 2 and they all have noise issue.
So I just used separate sonar sensor similar to this and it works great.
Not sure if it relates to power issue or not.
Good luck!

1 Like

Thanks much @Kyuhyong_You ! : )


the sonar can work better with median filter?


Sorry @whtcubie I’m not sure what you are talking about.
Better than what filter?
It would have been better if there is some data to compare with.


sorry,I am not very good at English.
I wonder if sonar usemedian filter or low pass filter is better.


It depends on how raw data looks like. If the data has noise only in high frequency range, LPE works better however if the noise is more like white noise, median filter may works better. So you definitely provide the raw data first.