Range finder kinematic consistency check #19340
Conversation
bresch
force-pushed
the
pr-vel-bottom-consistency
branch
from
aee15ce
to
3b02475
Compare
| uint64_t _time_last_inconsistent_us{}; | ||
|
|
||
| static constexpr float _vel_bottom_signed_test_ratio_tau = 2.f; | ||
| static constexpr float _vel_bottom_gate = 0.1f; |
There was a problem hiding this comment.
This seems like a small value for the gate. If this is what is required to achieve an acceptable false positive rate, then perhaps the process or observation noise values need to be tuned instead.
There was a problem hiding this comment.
Good point, in the log I was using to tune the gate, looking at the range finder derivative and its noise (based on range finder parameter noise), we can see that the range finder noise is over-sized.
Tuning a bit the static noise and range-based scaler (EKF2_RNG_NOISE = 0.01 (default = 0.1) and EKF2_RNG_SFE = 0.01 (default = 0.05)), we can now increase the gate to 1 to get a similar performance of the consistency check:
There was a problem hiding this comment.
@priseborough I increased the default gate size to 1 and added a note for the user in the parameter description in ad2ae35
|
The algorithm and its implementation look OK. We need some test data on physical hardware to validate the tuning as EKF vertical velocity errors caused by GPS and baro drift will need to be handled as will range finder noise when operating over long grass. I will check to see if I have a range finder hardware that is suitable for testing. |
bresch
force-pushed
the
pr-vel-bottom-consistency
branch
2 times, most recently
from
3f5f31f
to
c4fb542
Compare
At each new valid range measurement, the time derivative of the distance to the ground is computed and compared with the estimated velocity. The average of a normalized innovation squared statistic check is used to detect a bias in the derivative of distance measurement, indicating that the distance measurements are kinematically inconsistent with the filter.
To pass from invalid to valid: - time hysteresis - some vertical velocity - test ratio < 1 - low-passed signed test ratio < 1 To pass from valid to invalid: - low-passed signed test ratio >= 1
The check assumes a non-moving terrain height
The user needs to tune the range finder noise parameters properly and we shouldn't need such a small gate
bresch
force-pushed
the
pr-vel-bottom-consistency
branch
from
c4fb542
to
5698989
Compare
|
This pull request has been mentioned on Discussion Forum for PX4, Pixhawk, QGroundControl, MAVSDK, MAVLink. There might be relevant details there: |
Describe problem solved by this pull request
It is quite common that range finders are reporting incorrect data due to clouds, water, LOS blockage (e.g.: when attaching a payload) or other malfunction of the sensor itself. In most of those issues, the sensor quality metric is not a reliable source of truth.
Describe your solution
A kinematic consistency check is done by comparing the time derivative of the vertical distance measurement ("vel_bottom") and the vertical velocity estimate of EKF2. An inconsistency is detected when the normalized innovation squared (NIS) is on average greater than a defined threshold.
When the data is said "inconsistent" it requires several conditions to be valid again:
Test data / coverage
SITL testing for now
TODO: