Repository: PX4/PX4-Autopilot
Pull request #24588: EKF2: improve optical flow stability near the ground
Author: login: bresch
Created: 2025-03-21T14:21:38Z | Merged: 2025-03-31T09:34:25Z

### Solved Problem
Some EKF stability issues were occurring near the ground, where the optical flow measurement is near its singularity point (0 distance).

### Solution
- Adjust the optical flow prediction model to exactly match the one used in the derivation
- Avoid unnecessary double division in flow measurement prediction
- Limit flow measurement prediction magnitude by limiting the minimum predicted distance to 1cm
- Re-calculate innovation test ratio after having fused the flow X axis. As we operate close to the singularity, fusing the Y axis without checking the new test ratio can lead to over-corrections as the innovation might be really large

### Test coverage
I was able to trigger issue on a bench setup with high baro drift and by allowing the EKF to start the flow fusion without range finder. The flow fusion then starts with a high terrain height uncertainty and I was able to trigger instabilities by suddenly moving above a discontinuous ground (table to floor) several times. 
The instability was reproducible in replay and this PR was able to fix the issues. Several tests were then conducted with those changes and the EKF always stayed stable.


Diff:
diff --git a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp
index 747fac96f1b1..4c2d63553f44 100644
--- a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp
+++ b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp
@@ -46,18 +46,17 @@ bool Ekf::fuseOptFlow(VectorState &H, const bool update_terrain)
 {
 	const auto state_vector = _state.vector();
 
-	_innov_check_fail_status.flags.reject_optflow_X = (_aid_src_optical_flow.test_ratio[0] > 1.f);
-	_innov_check_fail_status.flags.reject_optflow_Y = (_aid_src_optical_flow.test_ratio[1] > 1.f);
-
 	// if either axis fails we abort the fusion
 	if (_aid_src_optical_flow.innovation_rejected) {
+		_innov_check_fail_status.flags.reject_optflow_X = true;
+		_innov_check_fail_status.flags.reject_optflow_Y = true;
 		return false;
 	}
 
 	// fuse observation axes sequentially
 	for (uint8_t index = 0; index <= 1; index++) {
 		if (index == 0) {
-			// everything was already computed above
+			// everything was already computed before
 
 		} else if (index == 1) {
 			// recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes)
@@ -69,6 +68,16 @@ bool Ekf::fuseOptFlow(VectorState &H, const bool update_terrain)
 			const Vector3f flow_gyro_corrected = _flow_sample_delayed.gyro_rate - _flow_gyro_bias;
 			_aid_src_optical_flow.innovation[1] = predictFlow(flow_gyro_corrected)(1) - static_cast<float>
 							      (_aid_src_optical_flow.observation[1]);
+
+			// recalculate the test ratio as the measurement jacobian is highly non linear
+			// when close to the ground (singularity at 0) and the innovation can suddenly become really
+			// large and destabilize the filter
+			_aid_src_optical_flow.test_ratio[1] = sq(_aid_src_optical_flow.innovation[1]) / (sq(
+					_params.flow_innov_gate) * _aid_src_optical_flow.innovation_variance[1]);
+
+			if (_aid_src_optical_flow.test_ratio[1] > 1.f) {
+				continue;
+			}
 		}
 
 		if (_aid_src_optical_flow.innovation_variance[index] < _aid_src_optical_flow.observation_variance[index]) {
@@ -91,6 +100,9 @@ bool Ekf::fuseOptFlow(VectorState &H, const bool update_terrain)
 	_fault_status.flags.bad_optflow_X = false;
 	_fault_status.flags.bad_optflow_Y = false;
 
+	_innov_check_fail_status.flags.reject_optflow_X = (_aid_src_optical_flow.test_ratio[0] > 1.f);
+	_innov_check_fail_status.flags.reject_optflow_Y = (_aid_src_optical_flow.test_ratio[1] > 1.f);
+
 	_aid_src_optical_flow.time_last_fuse = _time_delayed_us;
 	_aid_src_optical_flow.fused = true;
 
@@ -103,7 +115,7 @@ bool Ekf::fuseOptFlow(VectorState &H, const bool update_terrain)
 	return true;
 }
 
-float Ekf::predictFlowRange() const
+float Ekf::predictFlowHagl() const
 {
 	// calculate the sensor position relative to the IMU
 	const Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body;
@@ -113,17 +125,19 @@ float Ekf::predictFlowRange() const
 
 	// calculate the height above the ground of the optical flow camera. Since earth frame is NED
 	// a positive offset in earth frame leads to a smaller height above the ground.
-	const float height_above_gnd_est = getHagl() - pos_offset_earth(2);
-
-	// calculate range from focal point to centre of image
-	float flow_range = height_above_gnd_est / _R_to_earth(2, 2); // absolute distance to the frame region in view
+	const float height_above_gnd_est = fabsf(getHagl() - pos_offset_earth(2));
 
-	// avoid the flow prediction singularity at range = 0
-	if (fabsf(flow_range) < FLT_EPSILON) {
-		flow_range = signNoZero(flow_range) * FLT_EPSILON;
-	}
+	// Never return a really small value to avoid generating insanely large flow innovations
+	// that could destabilize the filter
+	constexpr float min_hagl = 1e-2f;
 
-	return flow_range;
+	return fmaxf(height_above_gnd_est, min_hagl);
+}
+float Ekf::predictFlowRange() const
+{
+	// calculate range from focal point to centre of image
+	// absolute distance to the frame region in view
+	return predictFlowHagl() / _R_to_earth(2, 2);
 }
 
 Vector2f Ekf::predictFlow(const Vector3f &flow_gyro) const
@@ -142,9 +156,9 @@ Vector2f Ekf::predictFlow(const Vector3f &flow_gyro) const
 	const Vector2f vel_body = _state.quat_nominal.rotateVectorInverse(vel_rel_earth).xy();
 
 	// calculate range from focal point to centre of image
-	const float range = predictFlowRange();
+	const float scale = _R_to_earth(2, 2) / predictFlowHagl();
 
-	return Vector2f(vel_body(1) / range, -vel_body(0) / range);
+	return Vector2f(vel_body(1) * scale, -vel_body(0) * scale);
 }
 
 float Ekf::calcOptFlowMeasVar(const flowSample &flow_sample) const
diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h
index 981b31e094f0..e0dba0eddce2 100644
--- a/src/modules/ekf2/EKF/ekf.h
+++ b/src/modules/ekf2/EKF/ekf.h
@@ -791,6 +791,7 @@ class Ekf final : public EstimatorInterface
 	// calculate optical flow body angular rate compensation
 	void calcOptFlowBodyRateComp(const flowSample &flow_sample);
 
+	float predictFlowHagl() const;
 	float predictFlowRange() const;
 	Vector2f predictFlow(const Vector3f &flow_gyro) const;
 
diff --git a/src/modules/ekf2/test/test_EKF_flow_generated.cpp b/src/modules/ekf2/test/test_EKF_flow_generated.cpp
index 58a0eabdeb6a..ee271df03aca 100644
--- a/src/modules/ekf2/test/test_EKF_flow_generated.cpp
+++ b/src/modules/ekf2/test/test_EKF_flow_generated.cpp
@@ -71,3 +71,22 @@ TEST(FlowGenerated, distBottom0y)
 	sym::ComputeFlowYInnovVarAndH(state.vector(), P, R, FLT_EPSILON, &innov_var, &H);
 	EXPECT_GT(innov_var, 1e12);
 }
+
+TEST(FlowGenerated, distBottomNeg)
+{
+	// GIVEN: a small negative distance to the ground (singularity)
+	StateSample state{};
+	state.quat_nominal = Quatf();
+	state.pos(2) = 1e-3f;
+
+	const float R = sq(radians(sq(0.5f)));
+	SquareMatrixState P = createRandomCovarianceMatrix();
+
+	VectorState H;
+	Vector2f innov_var;
+	sym::ComputeFlowXyInnovVarAndHx(state.vector(), P, R, FLT_EPSILON, &innov_var, &H);
+	EXPECT_GT(innov_var(0), 1e6);
+	EXPECT_GT(innov_var(1), 1e6);
+	sym::ComputeFlowYInnovVarAndH(state.vector(), P, R, FLT_EPSILON, &innov_var(1), &H);
+	EXPECT_GT(innov_var(1), 1e6);
+}


Reviews:
user: login: dagar
state: COMMENTED
author_association: MEMBER
submitted_at: 2025-03-26T00:49:46Z
user: login: dagar
state: COMMENTED
author_association: MEMBER
submitted_at: 2025-03-26T00:51:05Z
user: login: bresch
state: COMMENTED
author_association: MEMBER
submitted_at: 2025-03-28T10:15:30Z
user: login: bresch
state: COMMENTED
author_association: MEMBER
submitted_at: 2025-03-28T13:06:19Z

Review comments:
diff_hunk: @@ -69,6 +69,17 @@ bool Ekf::fuseOptFlow(VectorState &H, const bool update_terrain)
 			const Vector3f flow_gyro_corrected = _flow_sample_delayed.gyro_rate - _flow_gyro_bias;
 			_aid_src_optical_flow.innovation[1] = predictFlow(flow_gyro_corrected)(1) - static_cast<float>
 							      (_aid_src_optical_flow.observation[1]);
+
+			// recalculate the test ratio as the measurement jacobian is highly non linear
+			// when close to the ground (singularity at 0) and the innovation can suddenly become really
+			// large and destabilize the filter
+			_aid_src_optical_flow.test_ratio[1] = sq(_aid_src_optical_flow.innovation[1]) / (sq(
+					_params.flow_innov_gate) * _aid_src_optical_flow.innovation_variance[1]);
+			_innov_check_fail_status.flags.reject_optflow_Y = (_aid_src_optical_flow.test_ratio[1] > 1.f);
+
+			if (_innov_check_fail_status.flags.reject_optflow_Y) {
path: src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp
user: login: dagar
body: If we want to phase out these innov_check_fail_status flags it wouldn't hurt to try to not use them directly. If anything they could be simply updated once together at the end.
created_at: 2025-03-26T00:49:45Z
author_association: MEMBER
diff_hunk: @@ -69,6 +69,17 @@ bool Ekf::fuseOptFlow(VectorState &H, const bool update_terrain)
 			const Vector3f flow_gyro_corrected = _flow_sample_delayed.gyro_rate - _flow_gyro_bias;
 			_aid_src_optical_flow.innovation[1] = predictFlow(flow_gyro_corrected)(1) - static_cast<float>
 							      (_aid_src_optical_flow.observation[1]);
+
+			// recalculate the test ratio as the measurement jacobian is highly non linear
+			// when close to the ground (singularity at 0) and the innovation can suddenly become really
+			// large and destabilize the filter
+			_aid_src_optical_flow.test_ratio[1] = sq(_aid_src_optical_flow.innovation[1]) / (sq(
+					_params.flow_innov_gate) * _aid_src_optical_flow.innovation_variance[1]);
path: src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp
user: login: dagar
body: How exhaustively do we want/need to enforce the innovation gate minimum? Maybe we should just do it once during parameter update rather than all over the place every sample...
created_at: 2025-03-26T00:51:05Z
author_association: MEMBER
diff_hunk: @@ -69,6 +69,17 @@ bool Ekf::fuseOptFlow(VectorState &H, const bool update_terrain)
 			const Vector3f flow_gyro_corrected = _flow_sample_delayed.gyro_rate - _flow_gyro_bias;
 			_aid_src_optical_flow.innovation[1] = predictFlow(flow_gyro_corrected)(1) - static_cast<float>
 							      (_aid_src_optical_flow.observation[1]);
+
+			// recalculate the test ratio as the measurement jacobian is highly non linear
+			// when close to the ground (singularity at 0) and the innovation can suddenly become really
+			// large and destabilize the filter
+			_aid_src_optical_flow.test_ratio[1] = sq(_aid_src_optical_flow.innovation[1]) / (sq(
+					_params.flow_innov_gate) * _aid_src_optical_flow.innovation_variance[1]);
path: src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp
user: login: bresch
body: We probably don't really need to enforce a value at all. If someone want to set the gate to 0, it will just reject all samples, and I think this is actually expected and correct.
created_at: 2025-03-28T10:15:29Z
author_association: MEMBER
diff_hunk: @@ -69,6 +69,17 @@ bool Ekf::fuseOptFlow(VectorState &H, const bool update_terrain)
 			const Vector3f flow_gyro_corrected = _flow_sample_delayed.gyro_rate - _flow_gyro_bias;
 			_aid_src_optical_flow.innovation[1] = predictFlow(flow_gyro_corrected)(1) - static_cast<float>
 							      (_aid_src_optical_flow.observation[1]);
+
+			// recalculate the test ratio as the measurement jacobian is highly non linear
+			// when close to the ground (singularity at 0) and the innovation can suddenly become really
+			// large and destabilize the filter
+			_aid_src_optical_flow.test_ratio[1] = sq(_aid_src_optical_flow.innovation[1]) / (sq(
+					_params.flow_innov_gate) * _aid_src_optical_flow.innovation_variance[1]);
+			_innov_check_fail_status.flags.reject_optflow_Y = (_aid_src_optical_flow.test_ratio[1] > 1.f);
+
+			if (_innov_check_fail_status.flags.reject_optflow_Y) {
path: src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp
user: login: bresch
body: done
created_at: 2025-03-28T13:06:19Z
author_association: MEMBER

PR comments:
user: login: AlexKlimaj
created_at: 2025-03-21T16:41:06Z
body: Here is a log from yesterday on main where it looks like the optical flow estimate diverged on takeoff.

https://review.px4.io/plot_app?log=732b37d4-55e9-4854-bcea-2db7a792fcd5
author_association: MEMBER
user: login: haumarco
created_at: 2025-03-24T08:51:35Z
body: Now the question is: if we fuse OF-0 and it results in a large test ratio for OF-1 after the fusion, should we have fused OF-0 in the first place?

I know the solution with sequential fusion would be to run the calculation in an "independent" KF for one step before fusing it into the real EKF, but that introduces quite a bit of overhead.

Maybe a middle-ground solution could be to decide which direction to fuse first based on the initial test-ratios?
author_association: CONTRIBUTOR
user: login: bresch
created_at: 2025-03-24T14:39:10Z
body: > Now the question is: if we fuse OF-0 and it results in a large test ratio for OF-1 after the fusion, should we have fused OF-0 in the first place?

Well, no, that's why for all other sources we first check the overall test ratio and don't fuse any axis if it fails. And in theory, if it passes we should be able to fuse all axes without issues. However, I think the optical flow is a special case as the jacobian is singular at the origin, so the rounding errors can cause instabilities.

Later I still want to review the sequential fusion logic to use this "best practice" algorithm properly:
![image](https://github.com/user-attachments/assets/309887c9-5441-4b3b-91d2-6cdeed62893c)

> Maybe a middle-ground solution could be to decide which direction to fuse first based on the initial test-ratios?

That could be an idea, yes, but it still doesn't guarantee that the second sample won't create issues so I'm not sure if it's worth adding. Again, this is just to handle the nasty numerical issue that leads to an instability of the filter, none of this affects performance during flight.

author_association: MEMBER
user: login: haumarco
created_at: 2025-03-25T14:43:16Z
body: > Well, no, that's why for all other sources we first check the overall test ratio and don't fuse any axis if it fails. And in theory, if it passes we should be able to fuse all axes without issues. 

> And in theory, if it passes we should be able to fuse all axes without issues.

Yes, that's clear. But as you noted, this is a special case. I just wanted to make sure we don't "only mess it up slightly" by stopping after the first fusion, especially if the first axis was already disadvantageous. Since you've done the testing, I'm fine with the current approach. I just wanted to bring it up.
author_association: CONTRIBUTOR
user: login: bresch
created_at: 2025-03-25T19:54:46Z
body: > I just wanted to make sure we don't "only mess it up slightly" by stopping after the first fusion, especially if the first axis was already disadvantageous.

@haumarco Yes, I generally totally agree with you. Having part of the fusion is kind of weird but with those numerical issues, it's difficult to predict when they will occur (e.g.: everything might look fine just before fusing one of the two axes). And I still hope that we eventually don't need that special case handling the day we have the square root formulation of the EKF and the algorithm I mentioned in just above.
author_association: MEMBER
user: login: dagar
created_at: 2025-03-26T00:46:46Z
body: > Adjust the optical flow prediction model to exactly match the one used in the derivation

Why not literally use the same thing whenever possible? 
author_association: MEMBER
user: login: bresch
created_at: 2025-03-28T13:08:45Z
body: > Why not literally use the same thing whenever possible?

Because it's never exactly the same thing. For example we add the sensor offsets when computing the innovation while we don't count them in the derivation of the jacobian (as they would just make it more complicated without adding any real benefit).

Also, it takes more flash space to make the computation of the innovation in auto-generated code as it would flatten out vector rotations and other common operations.
author_association: MEMBER
user: login: github-actions[bot]
created_at: 2025-03-28T13:10:45Z
body: ## 🔎 FLASH Analysis
<details>
  <summary>px4_fmu-v5x [Total VM Diff: 72 byte (0 %)]</summary>

  ```
      FILE SIZE        VM SIZE    
 --------------  -------------- 
  +0.0%     +72  +0.0%     +72    .text
    +0.0%     +64  +0.0%     +64    src/modules/ekf2/modules__ekf2_unity.cpp
    +0.0%      +5  +0.0%      +5    [section .text]
    +0.2%      +3  +0.2%      +3    ../../src/systemcmds/ver/ver.cpp
  +0.0%     +56  [ = ]       0    .debug_abbrev
     +11%     +56  [ = ]       0    ../../src/lib/version/version.c
  +0.0%     +44  [ = ]       0    .debug_frame
  +0.0%    +116  [ = ]       0    .debug_info
    -0.2%      -4  [ = ]       0    ../../src/lib/version/version.c
    +0.0%    +120  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
  +0.0%    +172  [ = ]       0    .debug_line
    -1.3%     -25  [ = ]       0    ../../src/lib/version/version.c
    +0.1%    +202  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
    -0.4%      -5  [ = ]       0    task/task_cancelpt.c
  -0.0%     -64  [ = ]       0    .debug_loc
    +0.0%     +54  [ = ]       0    [section .debug_loc]
    -0.0%    -118  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
  +0.0%    +146  [ = ]       0    .debug_ranges
    -2.6%      -8  [ = ]       0    ../../src/lib/version/version.c
    +0.1%    +152  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
    +3.1%      +2  [ = ]       0    task/task_cancelpt.c
  +0.0%     +42  [ = ]       0    .debug_str
    +0.0%     +46  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
    -0.0%      -4  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
  +0.5%      +1  [ = ]       0    .shstrtab
  -0.0%      -1  [ = ]       0    .strtab
    -8.1%     -32  [ = ]       0    ../../src/lib/version/version.c
    +0.0%     +32  [ = ]       0    [section .strtab]
    -0.0%      -1  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
  -0.5%     -72  [ = ]       0    [Unmapped]
  +0.0%    +512  +0.0%     +72    TOTAL

  ```
</details>

<details>
  <summary>px4_fmu-v6x [Total VM Diff: 64 byte (0 %)]</summary>

  ```
      FILE SIZE        VM SIZE    
 --------------  -------------- 
  +0.0%     +64  +0.0%     +64    .text
    +0.0%     +64  +0.0%     +64    src/modules/ekf2/modules__ekf2_unity.cpp
  +0.0%     +56  [ = ]       0    .debug_abbrev
     +11%     +56  [ = ]       0    ../../src/lib/version/version.c
  +0.0%     +44  [ = ]       0    .debug_frame
  +0.0%    +116  [ = ]       0    .debug_info
    -0.2%      -4  [ = ]       0    ../../src/lib/version/version.c
    +0.0%    +120  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
  +0.0%    +172  [ = ]       0    .debug_line
    -1.3%     -25  [ = ]       0    ../../src/lib/version/version.c
    +0.1%    +202  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
    -0.4%      -5  [ = ]       0    task/task_cancelpt.c
  -0.0%     -44  [ = ]       0    .debug_loc
    +0.0%     +42  [ = ]       0    [section .debug_loc]
    -0.0%     -86  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
  +0.0%    +146  [ = ]       0    .debug_ranges
    -2.6%      -8  [ = ]       0    ../../src/lib/version/version.c
    +0.1%    +152  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
    +3.1%      +2  [ = ]       0    task/task_cancelpt.c
  +0.0%     +42  [ = ]       0    .debug_str
    +0.0%     +46  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
    -0.0%      -4  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
  +0.5%      +1  [ = ]       0    .shstrtab
  -0.0%      -1  [ = ]       0    .strtab
    -8.1%     -32  [ = ]       0    ../../src/lib/version/version.c
    +0.0%     +32  [ = ]       0    [section .strtab]
    -0.0%      -1  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
  -0.1%     -64  [ = ]       0    [Unmapped]
  +0.0%    +532  +0.0%     +64    TOTAL

  ```
</details>

**Updated: _2025-03-31T08:43:52_**
author_association: NONE
user: login: AlexKlimaj
created_at: 2025-04-01T19:28:10Z
body: Indoor flow only flight. Optical flow seems better. Height estimate without GPS still isn't great.
https://review.px4.io/plot_app?log=1107c065-5fcb-4180-a826-9bbcbd39d4a3
author_association: MEMBER