PX4
New filter for distance sensor validation
Solved Problem
The distance sensor is being fused or used to reset the terrain state, even though its measurements do not accurately represent the distance to the ground due to obstructions caused by rain, snow, fog etc.
Solution
Now the distance sensor can be in 3 different states based on the lpf test ratio of a simple kalman filter: consistent, inconsistent and unknown. A terrain reset can only happen when the state is consistent. The sensor will be used for normal fusion in both consistent and inconsistent states. Together with the already existing fog detector, the robustness against sensor blockage/failure is increased.
new:
old:
🔎 FLASH Analysis
px4_fmu-v5x [Total VM Diff: 1216 byte (0.06 %)]
FILE SIZE VM SIZE
-------------- --------------
+0.1% +1.18Ki +0.1% +1.18Ki .text
+27% +552 +27% +552 Ekf::controlRangeHaglFusion()
+122% +360 +122% +360 RangeFinderConsistencyCheck::update()
[NEW] +116 [NEW] +116 RangeFinderConsistencyCheck::evaluateState()
+3.4% +92 +3.4% +92 EKF2::PublishAidSourceStatus()
+2.7% +92 +2.7% +92 Ekf::Ekf()
[NEW] +68 [NEW] +68 RangeFinderConsistencyCheck::init()
+1.2% +24 +1.2% +24 Ekf::controlOpticalFlowFusion()
-100.0% +24 -100.0% +24 [53 Others]
+1.5% +20 +1.5% +20 ucdr_serialize_estimator_status_flags()
+4.8% +16 +4.8% +16 EKF2::PublishInnovationTestRatios()
+4.8% +16 +4.8% +16 EKF2::PublishInnovations()
+1.6% +16 +1.6% +16 EKF2::PublishStatusFlags()
+3.1% +16 +3.1% +16 Ekf::fuseGnssYaw()
+3.5% +12 +3.5% +12 EKF2::PublishInnovationVariances()
+1.2% +12 +1.2% +12 Ekf::controlGpsFusion()
+0.2% +12 +0.2% +12 Ekf::controlMagFusion()
-1.2% -16 -1.2% -16 Ekf::controlGnssHeightFusion()
-5.6% -16 -5.6% -16 Ekf::updateRangeHagl()
-7.7% -24 -7.7% -24 Ekf::stopGnssFusion()
-2.9% -44 -2.9% -44 Ekf::controlBaroHeightFusion()
[DEL] -140 [DEL] -140 RangeFinderConsistencyCheck::updateConsistency()
+0.1% +8 +0.1% +8 .ramfunc
+0.3% +4 +0.3% +4 Ekf::controlGravityFusion()
+1.2% +4 +1.2% +4 Ekf::measurementUpdate()
+14% +1 +14% +1 __stm32_phywrite_veneer
-12.5% -1 -12.5% -1 __nxsem_restore_baseprio_irq_veneer
+0.0% +83 [ = ] 0 .debug_abbrev
+0.0% +40 [ = ] 0 .debug_aranges
+0.0% +184 [ = ] 0 .debug_frame
+0.0% +10.7Ki [ = ] 0 .debug_info
+0.1% +6.26Ki [ = ] 0 .debug_line
+67% +2 [ = ] 0 [Unmapped]
+0.1% +6.26Ki [ = ] 0 [section .debug_line]
+0.2% +5.79Ki [ = ] 0 .debug_loclists
+0.3% +1.51Ki [ = ] 0 .debug_rnglists
[DEL] -3 [ = ] 0 [Unmapped]
+0.3% +1.51Ki [ = ] 0 [section .debug_rnglists]
+0.0% +624 [ = ] 0 .debug_str
-0.4% -1 [ = ] 0 .shstrtab
+0.0% +41 [ = ] 0 .strtab
[NEW] +52 [ = ] 0 RangeFinderConsistencyCheck::evaluateState()
[NEW] +43 [ = ] 0 RangeFinderConsistencyCheck::init()
+2.1% +1 [ = ] 0 RangeFinderConsistencyCheck::update()
[DEL] -55 [ = ] 0 RangeFinderConsistencyCheck::updateConsistency()
-39.0% -16 [ = ] 0 __nxsched_get_tcb_veneer
+89% +16 [ = ] 0 __wd_start_veneer
+0.0% +64 [ = ] 0 .symtab
+33% +16 [ = ] 0 EKF2::PublishEventFlags()
-50.0% -16 [ = ] 0 EKF2::PublishGpsStatus()
+100% +16 [ = ] 0 EKF2::PublishInnovationTestRatios()
+100% +16 [ = ] 0 EKF2::PublishInnovationVariances()
+100% +16 [ = ] 0 EKF2::PublishInnovations()
-50.0% -16 [ = ] 0 EKF2::PublishWindEstimate()
+50% +16 [ = ] 0 EKF2::PublishYawEstimatorStatus()
-33.3% -16 [ = ] 0 EKF2::print_usage()
+100% +16 [ = ] 0 EKF2::updateParamsImpl()
-66.7% -32 [ = ] 0 Ekf::controlBetaFusion()
+20% +16 [ = ] 0 Ekf::controlEvPosFusion()
+20% +16 [ = ] 0 Ekf::controlRangeHaglFusion()
-50.0% -16 [ = ] 0 Ekf::get_ekf_lpos_accuracy()
+33% +16 [ = ] 0 Ekf::reset()
+100% +16 [ = ] 0 Ekf::resetAccelBias()
-33.3% -16 [ = ] 0 Ekf::resetGyroBias()
+100% +16 [ = ] 0 Ekf::resetWindCov()
-25.0% -16 [ = ] 0 Ekf::resetWindToExternalObservation()
-25.0% -16 [ = ] 0 Ekf::updateHorizontalDeadReckoningstatus()
-66.7% -32 [ = ] 0 Ekf::updateRangeHagl()
-99.5% +64 [ = ] 0 [15 Others]
-12.1% -1.19Ki [ = ] 0 [Unmapped]
+0.1% +25.2Ki +0.1% +1.19Ki TOTAL
px4_fmu-v6x [Total VM Diff: 1192 byte (0.06 %)]
FILE SIZE VM SIZE
-------------- --------------
+0.1% +1.16Ki +0.1% +1.16Ki .text
+27% +552 +27% +552 Ekf::controlRangeHaglFusion()
+122% +360 +122% +360 RangeFinderConsistencyCheck::update()
[NEW] +116 [NEW] +116 RangeFinderConsistencyCheck::evaluateState()
+3.4% +92 +3.4% +92 EKF2::PublishAidSourceStatus()
+2.7% +92 +2.7% +92 Ekf::Ekf()
[NEW] +68 [NEW] +68 RangeFinderConsistencyCheck::init()
+1.2% +24 +1.2% +24 Ekf::controlOpticalFlowFusion()
+1.5% +20 +1.5% +20 ucdr_serialize_estimator_status_flags()
+4.8% +16 +4.8% +16 EKF2::PublishInnovationTestRatios()
+4.8% +16 +4.8% +16 EKF2::PublishInnovations()
+1.6% +16 +1.6% +16 EKF2::PublishStatusFlags()
+3.1% +16 +3.1% +16 Ekf::fuseGnssYaw()
+3.5% +12 +3.5% +12 EKF2::PublishInnovationVariances()
+1.2% +12 +1.2% +12 Ekf::controlGpsFusion()
+0.2% +12 +0.2% +12 Ekf::controlMagFusion()
-100.0% +8 -100.0% +8 [53 Others]
-1.2% -16 -1.2% -16 Ekf::controlGnssHeightFusion()
-5.6% -16 -5.6% -16 Ekf::updateRangeHagl()
-7.7% -24 -7.7% -24 Ekf::stopGnssFusion()
-2.9% -44 -2.9% -44 Ekf::controlBaroHeightFusion()
[DEL] -140 [DEL] -140 RangeFinderConsistencyCheck::updateConsistency()
+0.0% +83 [ = ] 0 .debug_abbrev
+0.0% +40 [ = ] 0 .debug_aranges
+0.0% +184 [ = ] 0 .debug_frame
+0.0% +10.7Ki [ = ] 0 .debug_info
+0.1% +6.26Ki [ = ] 0 .debug_line
+67% +2 [ = ] 0 [Unmapped]
+0.1% +6.26Ki [ = ] 0 [section .debug_line]
+0.2% +5.86Ki [ = ] 0 .debug_loclists
+0.3% +1.52Ki [ = ] 0 .debug_rnglists
[NEW] +2 [ = ] 0 [Unmapped]
+0.3% +1.51Ki [ = ] 0 [section .debug_rnglists]
+0.0% +631 [ = ] 0 .debug_str
-0.4% -1 [ = ] 0 .shstrtab
+0.0% +41 [ = ] 0 .strtab
[NEW] +52 [ = ] 0 RangeFinderConsistencyCheck::evaluateState()
[NEW] +43 [ = ] 0 RangeFinderConsistencyCheck::init()
+2.1% +1 [ = ] 0 RangeFinderConsistencyCheck::update()
[DEL] -55 [ = ] 0 RangeFinderConsistencyCheck::updateConsistency()
+0.0% +64 [ = ] 0 .symtab
+33% +16 [ = ] 0 EKF2::PublishEventFlags()
-50.0% -16 [ = ] 0 EKF2::PublishGpsStatus()
+100% +16 [ = ] 0 EKF2::PublishInnovationTestRatios()
+100% +16 [ = ] 0 EKF2::PublishInnovationVariances()
+100% +16 [ = ] 0 EKF2::PublishInnovations()
-50.0% -16 [ = ] 0 EKF2::PublishWindEstimate()
+50% +16 [ = ] 0 EKF2::PublishYawEstimatorStatus()
-33.3% -16 [ = ] 0 EKF2::print_usage()
+100% +16 [ = ] 0 EKF2::updateParamsImpl()
-66.7% -32 [ = ] 0 Ekf::controlBetaFusion()
+67% +32 [ = ] 0 Ekf::controlEvHeightFusion()
+20% +16 [ = ] 0 Ekf::controlEvPosFusion()
+20% +16 [ = ] 0 Ekf::controlRangeHaglFusion()
-50.0% -16 [ = ] 0 Ekf::get_ekf_lpos_accuracy()
+33% +16 [ = ] 0 Ekf::reset()
+100% +16 [ = ] 0 Ekf::resetAccelBias()
-33.3% -16 [ = ] 0 Ekf::resetGyroBias()
+100% +16 [ = ] 0 Ekf::resetWindCov()
-25.0% -16 [ = ] 0 Ekf::resetWindToExternalObservation()
-25.0% -16 [ = ] 0 Ekf::updateHorizontalDeadReckoningstatus()
-16.6% -1.16Ki [ = ] 0 [Unmapped]
+0.1% +25.3Ki +0.1% +1.16Ki TOTAL
Updated: 2025-08-26T13:16:21
![github-actions[bot] avatar](https://avatars.githubusercontent.com/in/15368?v=4 "Published by a pub.dev verified publisher"){kind=small}
This was a quick indoor flight in manual mode.
https://review.px4.io/plot_app?log=e3d3cf76-563d-4a23-9308-d37cbdcd34c3
Vs current main
https://review.px4.io/plot_app?log=6d902213-02ef-4ff8-8474-d341bfcc7bd9
@AlexKlimaj Your vertical velocity estimate seems to be off quite a lot. Can you increase the bias noise on the acc so the bias will converge a bit quicker? The new filter uses the velocity estimate of the EKF to verify if the measurement is correct or not but when the EKF estimate is off and has a low uncertainty then it will discard the range measurements.
Did a quick flight indoors with the Dexi (Flow, range, and baro. EKF2_HGT_REF=1). Lots of wandering up and down, have to fight with the throttle to keep the altitude in place
https://review.px4.io/plot_app?log=e0095444-5705-490b-82e3-ea1efb4784e5
@dakejahl if i replay your log with main it looks pretty much the same. seems like the classic indoor z/vz inconsistency based on suboptimal bias-uncertainties. Or do you have a log where this is not the issue?
Okay let's take a step back and just focus on outdoor flights.
@haumarco Here's an outdoor flight. EKF replay is enabled in this log. This is based on main from today with this PR and https://github.com/PX4/PX4-Autopilot/pull/24751 merged into it.
Dexi airframe (quad)
- No GPS
- Distance sensor
- Optical Flow
- Barometer
Mostly default params except reduced EKF2_RNG_NOISE to 0.01. So the EKF2_HGT_REF is gps (so it fallbacks to baro), although throughout most of the flight "range aiding" was active, but doesn't appear to work very well since the altitude is fluctuating quite a bit (there was a little bit of wind). I kept the throttle around 0 for most of the flight, for simply testing altitude hold performance. MPC_HOLD_DZ is left at the default of 0.1.
https://review.px4.io/plot_app?log=ddbf2f12-14a9-4400-815c-2520cd347313
This pull request has been mentioned on Discussion Forum for PX4, Pixhawk, QGroundControl, MAVSDK, MAVLink. There might be relevant details there:
https://discuss.px4.io/t/px4-sync-q-a-april-23-2025/45198/3
We should get this in. I've got a branch that improves altitude hold with a rangefinder which is based off of this work.
@haumarco can you share the log those screenshots in the description are from? Or any other logs you have of this PR with a distance sensor would be helpful
@haumarco can you provide an update please? I am working on a branch based off of this and I'd like to know the status. Is this code being tested/flown in Auterions version of PX4? Do you have any flight logs you can share? Can we get this merged soon?
@dakejahl It was tested on a VTOL and 2 different quadcopters. I will react to the review this week which will be followed by another set of testflights. I'll share the logs.