New filter for distance sensor validation

[haumarco]

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:

[github-actions]

🔎 FLASH Analysis

px4_fmu-v5x [Total VM Diff: 768 byte (0.04 %)]

    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%    +768  +0.0%    +768    .text
  +0.4%    +736  +0.4%    +736    src/modules/ekf2/modules__ekf2_unity.cpp
  +0.0%     +16  +0.0%     +16    ../../src/modules/uxrce_dds_client/uxrce_dds_client.cpp
  +0.0%     +13  +0.0%     +13    [section .text]
  +0.2%      +3  +0.2%      +3    ../../src/systemcmds/ver/ver.cpp
+0.0%     +82  [ = ]       0    .debug_abbrev
   +11%     +56  [ = ]       0    ../../src/lib/version/version.c
  +0.5%     +26  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
+0.0%    +100  [ = ]       0    .debug_frame
+0.1% +14.1Ki  [ = ]       0    .debug_info
  -0.2%      -4  [ = ]       0    ../../src/lib/version/version.c
  +0.0%     +29  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp
  +0.0%     +13  [ = ]       0    ../../src/modules/uxrce_dds_client/uxrce_dds_client.cpp
  +0.1%     +13  [ = ]       0    msg/topics_sources/estimator_status_flags.cpp
  +0.0%      +9  [ = ]       0    msg/topics_sources/uORBMessageFieldsGenerated.cpp
  +1.5% +14.0Ki  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
+0.1% +3.68Ki  [ = ]       0    .debug_line
  -1.3%     -25  [ = ]       0    ../../src/lib/version/version.c
  +0.0%     +27  [ = ]       0    ../../src/modules/uxrce_dds_client/uxrce_dds_client.cpp
  +1.0% +3.68Ki  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
  -0.2%      -2  [ = ]       0    task/task_cancelpt.c
+0.2% +9.78Ki  [ = ]       0    .debug_loc
  +0.0%    +151  [ = ]       0    [section .debug_loc]
  +1.3% +9.63Ki  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
+0.2% +2.62Ki  [ = ]       0    .debug_ranges
  -2.6%      -8  [ = ]       0    ../../src/lib/version/version.c
  +0.1%     +48  [ = ]       0    [section .debug_ranges]
  +1.2% +2.58Ki  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
  +3.1%      +2  [ = ]       0    task/task_cancelpt.c
+0.2% +5.29Ki  [ = ]       0    .debug_str
  +2.2%      +8  [ = ]       0    ../../platforms/common/pab_manifest.c
  -9.2%    -182  [ = ]       0    ../../src/lib/battery/battery.cpp
  -1.9%     -60  [ = ]       0    ../../src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp
 -87.3% -2.19Ki  [ = ]       0    ../../src/modules/landing_target_estimator/KalmanFilter.cpp
  -7.8%     -33  [ = ]       0    ../../src/modules/landing_target_estimator/LandingTargetEstimator.cpp
  -0.1%     -16  [ = ]       0    ../../src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp
  +2.5% +7.75Ki  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
-1.0%      -2  [ = ]       0    .shstrtab
+0.0%     +74  [ = ]       0    .strtab
  -8.1%     -32  [ = ]       0    ../../src/lib/version/version.c
  +0.0%     +32  [ = ]       0    [section .strtab]
  +0.5%     +74  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
-5.0%    -768  [ = ]       0    [Unmapped]
+0.1% +35.7Ki  +0.0%    +768    TOTAL

px4_fmu-v6x [Total VM Diff: 768 byte (0.04 %)]

    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%    +768  +0.0%    +768    .text
  +0.4%    +736  +0.4%    +736    src/modules/ekf2/modules__ekf2_unity.cpp
  +0.0%     +16  +0.0%     +16    ../../src/modules/uxrce_dds_client/uxrce_dds_client.cpp
  +0.0%     +13  +0.0%     +13    [section .text]
  +0.2%      +3  +0.2%      +3    ../../src/systemcmds/ver/ver.cpp
+0.0%     +82  [ = ]       0    .debug_abbrev
   +11%     +56  [ = ]       0    ../../src/lib/version/version.c
  +0.5%     +26  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
+0.0%    +100  [ = ]       0    .debug_frame
+0.1% +14.1Ki  [ = ]       0    .debug_info
  -0.2%      -4  [ = ]       0    ../../src/lib/version/version.c
  +0.0%     +29  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp
  +0.0%     +13  [ = ]       0    ../../src/modules/uxrce_dds_client/uxrce_dds_client.cpp
  +0.1%     +13  [ = ]       0    msg/topics_sources/estimator_status_flags.cpp
  +0.0%      +9  [ = ]       0    msg/topics_sources/uORBMessageFieldsGenerated.cpp
  +1.5% +14.0Ki  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
+0.1% +3.68Ki  [ = ]       0    .debug_line
  -1.3%     -25  [ = ]       0    ../../src/lib/version/version.c
  +0.0%     +27  [ = ]       0    ../../src/modules/uxrce_dds_client/uxrce_dds_client.cpp
  +1.0% +3.68Ki  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
  -0.2%      -2  [ = ]       0    task/task_cancelpt.c
+0.2% +9.85Ki  [ = ]       0    .debug_loc
  +0.0%     +15  [ = ]       0    ../../src/modules/commander/Commander.cpp
  +0.1%    +181  [ = ]       0    [section .debug_loc]
  +1.3% +9.66Ki  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
+0.2% +2.62Ki  [ = ]       0    .debug_ranges
  -2.6%      -8  [ = ]       0    ../../src/lib/version/version.c
  +0.1%     +48  [ = ]       0    [section .debug_ranges]
  +1.2% +2.58Ki  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
  -3.0%      -2  [ = ]       0    task/task_cancelpt.c
+0.2% +5.29Ki  [ = ]       0    .debug_str
  +2.2%      +8  [ = ]       0    ../../platforms/common/pab_manifest.c
  -9.2%    -182  [ = ]       0    ../../src/lib/battery/battery.cpp
  -1.9%     -60  [ = ]       0    ../../src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp
 -87.3% -2.19Ki  [ = ]       0    ../../src/modules/landing_target_estimator/KalmanFilter.cpp
  -7.8%     -33  [ = ]       0    ../../src/modules/landing_target_estimator/LandingTargetEstimator.cpp
  -0.1%     -16  [ = ]       0    ../../src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp
  +2.5% +7.75Ki  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
-0.9%      -2  [ = ]       0    .shstrtab
+0.0%     +74  [ = ]       0    .strtab
  -8.1%     -32  [ = ]       0    ../../src/lib/version/version.c
  +0.0%     +32  [ = ]       0    [section .strtab]
  +0.5%     +74  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
-1.0%    -768  [ = ]       0    [Unmapped]
+0.1% +35.7Ki  +0.0%    +768    TOTAL

Updated: 2025-01-10T10:01:38

[bresch]

I'm wondering if using sequential fusion would actually give nicer code in the end so you wouldn't need to build all those 2x2 matrices

[bresch]

Suggested change

	_P = Matrix<float, RangeFilter::size, RangeFilter::size>(p);
	_P = SquareMatrix<float, RangeFilter::size>(p);

[AlexKlimaj]

@dakejahl Does this help our height issues?

[AlexKlimaj]

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

[haumarco]

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