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%     +96  [ = ]       0    .debug_frame
+0.0% +13.8Ki  [ = ]       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.4% +13.7Ki  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
+0.1% +3.69Ki  [ = ]       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.1%      -1  [ = ]       0    task/task_cancelpt.c
+0.2% +9.18Ki  [ = ]       0    .debug_loc
  +0.1%    +183  [ = ]       0    [section .debug_loc]
  +1.2% +9.00Ki  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
+0.2% +2.58Ki  [ = ]       0    .debug_ranges
  -2.6%      -8  [ = ]       0    ../../src/lib/version/version.c
  +0.1%     +48  [ = ]       0    [section .debug_ranges]
  +1.2% +2.54Ki  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
  -3.0%      -2  [ = ]       0    task/task_cancelpt.c
+0.2% +5.21Ki  [ = ]       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.68Ki  [ = ]       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
-4.6%    -768  [ = ]       0    [Unmapped]
+0.1% +34.7Ki  +0.0%    +768    TOTAL

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

    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%    +760  +0.0%    +760    .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%      +8  +0.0%      +8    [section .text]
+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%     +96  [ = ]       0    .debug_frame
+0.1% +13.8Ki  [ = ]       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.4% +13.7Ki  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
+0.1% +3.69Ki  [ = ]       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.1%      -1  [ = ]       0    task/task_cancelpt.c
+0.2% +9.25Ki  [ = ]       0    .debug_loc
  +0.0%    +145  [ = ]       0    [section .debug_loc]
  +1.2% +9.11Ki  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
+0.2% +2.58Ki  [ = ]       0    .debug_ranges
  -2.6%      -8  [ = ]       0    ../../src/lib/version/version.c
  +0.1%     +48  [ = ]       0    [section .debug_ranges]
  +1.2% +2.54Ki  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
+0.2% +5.21Ki  [ = ]       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.68Ki  [ = ]       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%    -760  [ = ]       0    [Unmapped]
+0.1% +34.7Ki  +0.0%    +760    TOTAL

Updated: 2024-12-20T13:03:26

[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]

You also need to initialize the covariance elements as height and terrain states are correlated:
https://colab.research.google.com/drive/11gIyOub-rub4uL7mZHH1gGICL3fTTEyH?usp=sharing

[h_var,      h_var   ]
[h_var, d_var + h_var]

[bresch]

Suggested change

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

[bresch]

if you set the output to be None in symforce it will create a function that returns H instead of having to pass it via a pointer (

PX4-Autopilot/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py

Line 738 in defccfa

generate_px4_function(compute_airspeed_h, output_names=None)

)

[bresch]

do you need to have that in the init function? That's anyway only called when there was no sensor data for a long time, right? Then you could just set it to UNKNOWN