ekf2: uncorrelate position covariance after velocity reset (PX4#23644)

chfriedrich98 · Sep 6, 2024 · 44967bd · 44967bd
1 parent 1337fca
commit 44967bd
Show file tree

Hide file tree

Showing 3 changed files with 495 additions and 490 deletions.
diff --git a/src/modules/ekf2/EKF/velocity_fusion.cpp b/src/modules/ekf2/EKF/velocity_fusion.cpp
@@ -91,6 +91,9 @@ void Ekf::resetHorizontalVelocityTo(const Vector2f &new_horz_vel, const Vector2f
 		P.uncorrelateCovarianceSetVariance<1>(State::vel.idx + 1, math::max(sq(0.01f), new_horz_vel_var(1)));
 	}
 
+	P.uncorrelateCovarianceSetVariance<1>(State::pos.idx, P(State::pos.idx, State::pos.idx));
+	P.uncorrelateCovarianceSetVariance<1>(State::pos.idx + 1, P(State::pos.idx + 1, State::pos.idx + 1));
+
 	_output_predictor.resetHorizontalVelocityTo(delta_horz_vel);
 
 	// record the state change
@@ -117,6 +120,8 @@ void Ekf::resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var)
 		P.uncorrelateCovarianceSetVariance<1>(State::vel.idx + 2, math::max(sq(0.01f), new_vert_vel_var));
 	}
 
+	P.uncorrelateCovarianceSetVariance<1>(State::pos.idx + 2, P(State::pos.idx + 2, State::pos.idx + 2));
+
 	_output_predictor.resetVerticalVelocityTo(delta_vert_vel);
 
 	// record the state change