LL40LS & terrain estimation: Implement proper driver-specific filtering

[philipoe]

Issue

See #9843

Solution

Changes:

1. A driver-specific filtering that is based on datasheets and experiments with this sensor (based on the concept: "If you know the specific weaknesses of your device, why not filter it directly inside the device driver). What is implemented is described here, but essentially we are now looking at a) absolute peak strength b) relative peak strength (basically signal-to-noise) and c) the absolute measurement to detect bull**** data.

2. All distance sensors now have a signal_strength field. I know there is also a covariance field, but it seems like basically no other sensor is using this. Signal strength is also a different concept/metric than covariance (in my opinion it is a bit easier to determine signal strength than true covariance of the measurement), so i think it is good to have both.

3. @priseborough @dagar I changed this line, because before I put the "if" statement the distance data was just always forwarded to the EKF even if the device limits (i.e. min/max range) were exceeded. That did not seem right to me. Can you check this please?

Testing

The filtering was tested on the bench extensively. In contrast to before, I am now getting very clean distance data from the sensor up to 27m distance, i.e. every time that LL40LS messes up the driver detects that and sets signal strength to zero. The EKF then just does not use that data because of this line . I will flight test the whole PR today and give you further feedback.

@ryanjAA or anyone else using the LidarLite: It would be good if you could test this with your sensors (which are slightly different versions) too.

Note: I am not really a low-level driver guy, so any feedback on what I might have messed up in the driver is greatly appreciated.

[priseborough]

Can you please attach a link to a test log for this PR. I will review tomorrow.

[priseborough]

I am resisting putting in sensor type specific threshold checks into the estimator because this will end up with multiple checks to handle the failure mechanisms for different sensor types. My preference is that the validity of sensor specific metrics such as noise variance, S/N ratio, etc is not done by the EKF. The sensor driver has access to full rate data and where estimator agnostic validity checking using pre-filtered and down-sampled data is achievable, it should be done in the sensor driver.

I would prefer a validity flag and a sensor noise variance metric to assist with estimator tuning.

Edit: We can set the distance value to be out of range when invalid so a validity flag is not necessary.

[philipoe]

I am resisting putting in sensor type specific threshold checks into the estimator because this will end up with multiple checks to handle the failure mechanisms for different sensor types. My preference is that the validity of sensor specific metrics such as noise variance, S/N ratio, etc is not done by the EKF. The sensor driver has access to full rate data and where estimator agnostic validity checking using pre-filtered and down-sampled data is achievable, it should be done in the sensor driver.

I think we totally agree with this. My approach in this PR is that each distance sensor driver calculates a signal_strength (as you say, leveraging knowledge about the specific failure mechanisms that are know for this sensor), and the ekf2 then simply has to check whether signal_strength==0.

[priseborough]

So why 0 and not some other threshold? Isn't that sensor specific? If not, then that means we are using the signal strength also as a a de-facto validity indicator and the uORB description should be updated to state that a value of zero indicates an invalid reading.

[philipoe]

@priseborough Log file is here: https://review.px4.io/plot_app?log=2ffbb1b4-f70f-421c-bb2c-892ecb51d623

Quick description of what is going on there:

• Fixed-wing plane, hand-launched, location has a relatively constant terrain altitude at ~538m +/-3m with only a single tree which could disturb the measurement, otherwise the terrain is very clean.
• t=935...1954s: Four autolaunches+autolandings
• t=1985...2384: Stabilized flight, always climbing to an altitude where the distance sensor does not deliver valid readings, and then quickly descending to an altitude where it does. This was done to see how fast the terrain estimate becomes valid.
• t=2384... 2670: Some higher-altitude flight in a region where the LL40LS does not deliver valid data. In contrast, it delivers a lot of outliers there. I did this to test the outlier rejection inside the driver that i added. Seems to work really nicely.
• Afterwards: Manual flight, landing.

Findings/Questions from my side:

• Overall seems to work really nicely. Terrain estimate seems mostly correct (some slight drift observed after launches). If you compare that to the problems i had with the "old" approach here, there is a huge difference: The terrain estimate becomes valid quickly after the distance sensor shows less than 25m, which was not the case before (where the HAGL test ratio first had to slowly converge to <1). Also, when we are actually at >25m AGL (i.e. the max_distance limit of the range sensor), then the terrain altitude is not estimated at all, whereas without this approach (see issue again) the terrain was still always estimated and actually set to ~aircraft_altitude.
• The HAGL test ratio is always very low. One reason is that i set the EKF2_RNG_GATE param to 10 isntead of 5 and slightly increased the range sensor noise parameters. But would you expect such a low HAGL test ratio? As mentioned, before (without this PR in the issue i created) it was much much higher.
• One small potential issue: At t=1777s, the terrain estimation stops briefly before a takeoff. I briefly checked but could not see why it would stop there. Have you seen that before?
• Other small potential issue: I feel like there is a slight delay (ca 1.5 seconds) between the distance sensor delivering its first measurement of <max_distance and the terrain estimate becoming valid. It's not a big deal at high altitude, but i was wondering whether this is a general delay that could later on also influence when we flare (where a 1.5s delay would make a huge difference!). The range sensor time delay parameter is only set to 5ms, so that cannot be it. Do you have any clue?

Apart from the log file, i think the most important code change for you to review @priseborough is still these lines. That is the main thing i am not certain about.

[philipoe]

So why 0 and not some other threshold? Isn't that sensor specific? If not, then that means we are using the signal strength also as a a de-facto validity indicator and the uORB description should be updated to state that a value of zero indicates an invalid reading.

I think it is clear that for any sensor, if signal_strength=0, then the signal is invalid. This is thus not sensor-specific. Also note that what is then actually considered invalid (i.e. signal strength = 0) is decided by the driver, i.e. ekf2 again does not have to deal with this. But sure, if you want i can add a brief comment that signal_strength=0 means invalid signal.

[priseborough]

@philipoe Yes please do. I would prefer the term 'quality' instead of 'signal strength' because there are some optical based techniques (eg stereo camera) where the concept of a return signal strength is not applicable.

[priseborough]

We don't want to change this line. I twill cause loss of height estimation if range finder is a primary height source and the range sensor goes out of range when on ground due to the vehicle being hand carried, etc.

[philipoe]

OK, but what is the alternative solution then?

The current structure does not make sense in my opinion. We set range_finder_updated = false in line 948, but we never use that information anywhere in the code! Effectively, this means that we are not obeying the range_finder.min_distance and range_finder.max_distance values delivered by the drivers. I did see that if rangeSensor=primary height source, then we use those limits as defined here, but if we only use the range sensor for terrain estimation, then we'll always use range data even if its beyond the device limitations, yielding the issues seen in #9843 . Do you see what i mean?

[priseborough]

The prior structure was logical. The code used a value equal to EKF2_MIN_RNG if the range finder returned an invalid reading when the vehicle was on the ground. Invalid readings during on-ground operation due to sensor obstruction from grass, operator handling, sonar reflections, etc were/are very common.

It is important that this PR does not change that behaviour.

[philipoe]

The code used a value equal to EKF2_MIN_RNG if the range finder returned an invalid reading when the vehicle was on the ground. Invalid readings during on-ground operation due to sensor obstruction from grass, operator handling, sonar reflections, etc were/are very common.

I absolutely understand that, and i agree that this is OK (note that my PR does not change that behavior). However, i was never talking about on-ground handling, but solely about handling invalid measurements in air. So while this line is completely fine for me, this one is not, because even if the min_distance and max_distance values delivered by the sensor are exceeded we simply still use them in the terrain estimation, which does not make sense. Do you see what i mean? I think we need a way to completely reject those false measurements when in air. Otherwise we can discuss on slack if you like (but I have bad internet access in the next week because i am on a project demonstration).

[priseborough]

I see what you mean. The result of separate changes over the past few months in the ekf2 module and ecl library is that although min and max range is passed through to the ekf here https://github.com/PX4/Firmware/blob/master/src/modules/ekf2/ekf2_main.cpp#L957, the ekf does not use these limits internally to reject data.

I will submit a bug fix for the ecl library.

[priseborough]

See PX4/PX4-ECL#476

[priseborough]

The upstream bug fix has now been merged. I will now submit a change request to get all the checking, including the decision to use the on ground value into the one function inside the ecl library so that we don't end up again in a situation where having checks in multiple places results in functionality being lost.

[priseborough]

Please change to:

int8 quality # measurement quality in percent (0...100%) or -1 if unknown. A value of 0 indicates an invalid measurement.

[priseborough]

There are sensing modalities that do not use reflected signal strength, eg stereo vision, so this needs to be a generic description.

[mhkabir]

0-255 for full range of uint8 maybe?

[philipoe]

Please change to: int8 quality # measurement quality in percent (0...100%) or -1 if unknown. A value of 0 indicates an invalid measurement.

Changed.

0-255 for full range of uint8 maybe?

We're using int8 instead of uint8 to have the capability to indicate unknown signal quality (i.e. the driver does not support this) with a -1. The signal quality also does not need to be super precise, it is a ballpark-estimate in any case. Given all this, 0....100% is then way more intuitive for a user than using 0 ... 128.

[priseborough]

@philipoe Agree. Lets use 0-100.

[priseborough]

@philipoe Please pull in this commit priseborough@3e0d1df and we can close #9886

[priseborough]

Pull in this commit
priseborough@3e0d1df

and we can merge this and close #9886

[philipoe]

@priseborough I merged the commit you mentioned. But this actually requires changes to a lot of distance sensor drivers because they all now have to set signal_quality=0 when range>range_max or range<range_min. Wouldn't it be easier to keep the range_max and range_min checks there in ekf2_main?

Edit: Thinking about it again it is of course cleaner the way it is since tihs new PR. However, the question is who modifies all the drivers and tests this...

[priseborough]

@philipoe The quality ==0 check this adds to ekf2_main prior to pushing data into the EKF is additional to the range checking that the ekf does internally, so we are not giving up anything we had before with those sensors.

[philipoe]

Ok, great, hadn't seen that...

[priseborough]

https://github.com/PX4/ecl/blob/master/EKF/control.cpp#L1152

[priseborough]

I will give this a test on SITL at commit 3e0d1df but it would be good if someone with a LidarLite can also run a test.

[priseborough]

The sonar data when running 'make posix gazebo_iris_opt_flow' is clipped such that it never exceeds max_distance and min_distance and therefore always passes checks. I will fix this in a separate PR. With the limits adjusted so that distance_sensor.distance can exceed the distance_sensor.max_distance and distance_sensor.min_distance, the test works as expected.

https://logs.px4.io/plot_app?log=ff9d9030-9dd5-4f3d-88a7-eed28bb577e3

Edit: Ready to merge when we get a positive test result from a LidarLite user

[IeiuniumLux]

@philipoe, did you have a chance to test your changes and @priseborough's changes during a test-flight?

I just installed the Lidar-Lite v3 in our Wing Z-84 plane and I was going to test it tomorrow. However, I currently have the v1.8.0 release installed in the PixRacer. Should I rebuild the firmware from master instead and test it?

Is this fix only critical during auto landing or is this also critical for when the vehicle goes above the sensor max limit; which in my case seem to be set to 25m.

[philipoe]

@philipoe, did you have a chance to test your changes and @priseborough's changes during a test-flight?

Not yet, no. I did test this PR up to f94b976 however.

[IeiuniumLux]

@philipoe Your issue #9843 seems to be more concerning about the autoland. If that is the case, then the current LL40LS driver in the v.1.8.0 release should work fine as long as the plane is landed manually. Is that correct?

BTW, do you also get a max_distance of 25m when you run the "ll40ls test" command from the shell?

According to the spec, the max should be up to 40m.

[philipoe]

@philipoe Your issue #9843 seems to be more concerning about the autoland. If that is the case, then the current LL40LS driver in the v.1.8.0 release should work fine as long as the plane is landed manually. Is that correct?

It works fine in the sense that it delivers values. It however cannot determine what the quality of these values is. Of course, if you land the plane manually, then this does not influence anything and thus does not matter.

BTW, do you also get a max_distance of 25m when you run the "ll40ls test" command from the shell?

The 25m max distance value was set some time ago and makes sense, because the sensor actually gets quite unreliable above 25m (although it can of course measure higher distances).