New filter for distance sensor validation #24106

haumarco · 2024-12-13T13:22:30Z

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 · 2024-12-13T13:28:06Z

🔎 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

src/modules/ekf2/EKF/python/ekf_derivation/derivation.py

src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp

src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp

src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp

bresch · 2024-12-20T16:53:19Z

src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp

-		_time_last_horizontal_motion = time_us;
-	}
+	float p[4] = {z_var, 0.f, 0.f, z_var + dist_bottom_var};
+	_P = Matrix<float, RangeFilter::size, RangeFilter::size>(p);


Suggested change

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

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

src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp

AlexKlimaj · 2024-12-27T02:11:28Z

@dakejahl Does this help our height issues?

AlexKlimaj · 2024-12-31T21:06:31Z

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 · 2025-01-10T09:50:30Z

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

bresch · 2025-01-24T09:36:00Z

src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp

+	float p[4] = {z_var, z_var, z_var, z_var + dist_bottom_var};
+	_P = SquareMatrix<float, RangeFilter::size>(p);


Suggested change

float p[4] = {z_var, z_var, z_var, z_var + dist_bottom_var};

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

_P(RangeFilter::z.idx, RangeFilter::z.idx) = z_var;

_P(RangeFilter::z.idx, RangeFilter::terrain.idx) = z_var;

_P(RangeFilter::terrain.idx, RangeFilter::z.idx) = _P(RangeFilter::z.idx, RangeFilter::terrain.idx);

_P(RangeFilter::terrain.idx, RangeFilter::terrain.idx) = z_var + dist_bottom_var;

Either that with a description (we don't want to rely on a specific state order) on how you get those values, simply do a reset to 0 followed by a forced fusion (with modified K).

bresch · 2025-01-24T09:37:30Z

src/modules/ekf2/EKF/python/ekf_derivation/derivation.py

@@ -725,6 +725,20 @@ def compute_gravity_z_innov_var_and_h(

    return (innov_var, H.T)

+def range_validation_filter() -> sf.Matrix:


Suggested change

def range_validation_filter() -> sf.Matrix:

def range_validation_filter_h() -> sf.Matrix:

bresch · 2025-01-24T09:40:09Z

src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp

 						resetTerrainToRng(aid_src);
 						resetAidSourceStatusZeroInnovation(aid_src);
 					}
 				}

 			} else {
-				ECL_WARN("stopping %s fusion, continuing conditions failing", HGT_SRC_NAME);
+				ECL_WARN("stoppPing %s fusion, continuing conditions failing", HGT_SRC_NAME);


bresch · 2025-01-24T09:41:41Z

src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp

+	_x(RangeFilter::terrain.idx) = z - dist_bottom;
+	_initialized = true;
+	_state = KinematicState::UNKNOWN;
+	_test_ratio_lpf.reset(2.f);


bresch · 2025-01-24T09:43:59Z

src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp

-	const float var = 2.f * dist_bottom_var / (dt * dt);
-	_innov_var = var + vz_var;
+	_x(RangeFilter::z.idx) -= dt * vz;
+	_P(0, 0) += dt * dt * vz_var + 0.001f;


RangeFilter::z.idx ...

bresch · 2025-01-24T10:30:13Z

src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp

-		if ((time_us - _time_last_horizontal_motion) > _signed_test_ratio_tau) {
-			_is_kinematically_consistent = false;
-			_time_last_inconsistent_us = time_us;
+		Vector2f PH = _P.row(i);


This only works for direct states observations, you can't use this for the range finder update.

bresch · 2025-01-24T10:34:35Z

src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp

+	_state = KinematicState::UNKNOWN;
+	_test_ratio_lpf.reset(2.f);
+	_t_since_first_sample = 0.f;
+	_test_ratio_lpf.setAlpha(0.2f);


You can now set the time constant as a parameter in the constructor and specify the dt in the update function to automatically compute the correct alpha parameter. See here for example.

bresch · 2025-01-24T10:38:28Z

src/modules/ekf2/EKF/python/ekf_derivation/derivation.py

+    dist_bottom = state["z"] - state["terrain"]
+
+    H = sf.M22()
+    H[0, :] = sf.V1(state["z"]).jacobian(state.to_storage(), tangent_space=False)


this is a direct state observation, so you don't even need to generate that observation matrix if you split the fusion process completely.

bresch · 2025-01-24T10:42:44Z

src/modules/ekf2/test/test_EKF_height_fusion.cpp

-	_sensor_simulator._baro.setData(_sensor_simulator._baro.getData() + baro_increment);
-	_sensor_simulator.runSeconds(60);
+	const float baro_initial = _sensor_simulator._baro.getData();
+	const float baro_inc = 0.2f;


baro_inc and baro_increment
You could call it baro_slew_rate and multiply it with the dt of 3 seconds you have in the loop

bresch · 2025-01-24T10:45:06Z

src/modules/ekf2/test/test_EKF_terrain.cpp


 	const float corr_terrain_abias_z = P(State::accel_bias.idx + 2,
 					     State::terrain.idx) / sqrtf(_ekf->getAccelBiasVariance()(2) * var_terrain);
-	EXPECT_NEAR(corr_terrain_abias_z, -0.4f, 0.03f);
+	EXPECT_TRUE(corr_terrain_abias_z < -0.1f);


That's a good idea. Could you make sure the norm of the correlation never exceeds 1?

haumarco requested a review from bresch December 13, 2024 13:22

bresch requested changes Dec 13, 2024

View reviewed changes

haumarco force-pushed the pr-ekf2_dist_sensor_validation branch 3 times, most recently from 499efdc to f7052d6 Compare December 20, 2024 10:16

haumarco marked this pull request as ready for review December 20, 2024 12:06

haumarco force-pushed the pr-ekf2_dist_sensor_validation branch 2 times, most recently from 11655eb to 9f42b6e Compare December 20, 2024 12:58

bresch reviewed Dec 20, 2024

View reviewed changes

src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp Outdated Show resolved Hide resolved

bresch reviewed Dec 20, 2024

View reviewed changes

src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp Outdated Show resolved Hide resolved

bresch reviewed Dec 20, 2024

View reviewed changes

src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp Outdated Show resolved Hide resolved

haumarco force-pushed the pr-ekf2_dist_sensor_validation branch from 9f42b6e to 42ea74c Compare December 30, 2024 09:20

haumarco added 9 commits January 10, 2025 10:55

add mini KF to validate range sensor measurements

9b94ea1

handle noise while hovering

e13b456

fix wrong miniEKF init

dfb3af2

fix bugs, rejection was too sensitive

aca4d96

always update lpf

80a7296

add symforce, adjust smaller things

5cddb05

adjust threshold unit tests

f350cad

overhaul approach to work on unit tests and snow/blocking/normal flights

8e652ad

reset state to unknown on init

e488ddb

haumarco force-pushed the pr-ekf2_dist_sensor_validation branch from 42ea74c to e488ddb Compare January 10, 2025 09:55

dagar added the EKF2 label Jan 15, 2025

bresch requested changes Jan 24, 2025

View reviewed changes

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

New filter for distance sensor validation #24106

New filter for distance sensor validation #24106

haumarco commented Dec 13, 2024 •

edited

Loading

github-actions bot commented Dec 13, 2024 •

edited

Loading

bresch left a comment

bresch Dec 20, 2024

AlexKlimaj commented Dec 27, 2024

AlexKlimaj commented Dec 31, 2024 •

edited

Loading

haumarco commented Jan 10, 2025

bresch Jan 24, 2025

bresch Jan 24, 2025

bresch Jan 24, 2025

bresch Jan 24, 2025

bresch Jan 24, 2025

bresch Jan 24, 2025

bresch Jan 24, 2025

bresch Jan 24, 2025

bresch Jan 24, 2025

bresch Jan 24, 2025

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

		float p[4] = {z_var, z_var, z_var, z_var + dist_bottom_var};
		_P = SquareMatrix<float, RangeFilter::size>(p);

-	float p[4] = {z_var, z_var, z_var, z_var + dist_bottom_var};
-	_P = SquareMatrix<float, RangeFilter::size>(p);
+	_P(RangeFilter::z.idx, RangeFilter::z.idx) = z_var;
+	_P(RangeFilter::z.idx, RangeFilter::terrain.idx) = z_var;
+	_P(RangeFilter::terrain.idx, RangeFilter::z.idx) = _P(RangeFilter::z.idx, RangeFilter::terrain.idx);
+	_P(RangeFilter::terrain.idx, RangeFilter::terrain.idx) = z_var + dist_bottom_var;

		@@ -725,6 +725,20 @@ def compute_gravity_z_innov_var_and_h(

		return (innov_var, H.T)

		def range_validation_filter() -> sf.Matrix:

	def range_validation_filter() -> sf.Matrix:
	def range_validation_filter_h() -> sf.Matrix:

New filter for distance sensor validation #24106

Are you sure you want to change the base?

New filter for distance sensor validation #24106

Conversation

haumarco commented Dec 13, 2024 • edited Loading

Solved Problem

Solution

github-actions bot commented Dec 13, 2024 • edited Loading

🔎 FLASH Analysis

bresch left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

AlexKlimaj commented Dec 27, 2024

AlexKlimaj commented Dec 31, 2024 • edited Loading

haumarco commented Jan 10, 2025

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

haumarco commented Dec 13, 2024 •

edited

Loading

github-actions bot commented Dec 13, 2024 •

edited

Loading

AlexKlimaj commented Dec 31, 2024 •

edited

Loading