ekf2: move vel/pos reset helpers

2026-10-23 05:03:34 +08:00 · 2024-02-20 13:16:24 -05:00
parent 22b957696d
commit 8243b4f474
3 changed files with 179 additions and 177 deletions
@@ -114,3 +114,82 @@ void Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src)
 		aid_src.fused = false;
 	}
 }
+
+void Ekf::resetHorizontalVelocityTo(const Vector2f &new_horz_vel, const Vector2f &new_horz_vel_var)
+{
+	const Vector2f delta_horz_vel = new_horz_vel - Vector2f(_state.vel);
+	_state.vel.xy() = new_horz_vel;
+
+	if (PX4_ISFINITE(new_horz_vel_var(0))) {
+		P.uncorrelateCovarianceSetVariance<1>(State::vel.idx, math::max(sq(0.01f), new_horz_vel_var(0)));
+	}
+
+	if (PX4_ISFINITE(new_horz_vel_var(1))) {
+		P.uncorrelateCovarianceSetVariance<1>(State::vel.idx + 1, math::max(sq(0.01f), new_horz_vel_var(1)));
+	}
+
+	_output_predictor.resetHorizontalVelocityTo(delta_horz_vel);
+
+	// record the state change
+	if (_state_reset_status.reset_count.velNE == _state_reset_count_prev.velNE) {
+		_state_reset_status.velNE_change = delta_horz_vel;
+
+	} else {
+		// there's already a reset this update, accumulate total delta
+		_state_reset_status.velNE_change += delta_horz_vel;
+	}
+
+	_state_reset_status.reset_count.velNE++;
+
+	// Reset the timout timer
+	_time_last_hor_vel_fuse = _time_delayed_us;
+}
+
+void Ekf::resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var)
+{
+	const float delta_vert_vel = new_vert_vel - _state.vel(2);
+	_state.vel(2) = new_vert_vel;
+
+	if (PX4_ISFINITE(new_vert_vel_var)) {
+		P.uncorrelateCovarianceSetVariance<1>(State::vel.idx + 2, math::max(sq(0.01f), new_vert_vel_var));
+	}
+
+	_output_predictor.resetVerticalVelocityTo(delta_vert_vel);
+
+	// record the state change
+	if (_state_reset_status.reset_count.velD == _state_reset_count_prev.velD) {
+		_state_reset_status.velD_change = delta_vert_vel;
+
+	} else {
+		// there's already a reset this update, accumulate total delta
+		_state_reset_status.velD_change += delta_vert_vel;
+	}
+
+	_state_reset_status.reset_count.velD++;
+
+	// Reset the timout timer
+	_time_last_ver_vel_fuse = _time_delayed_us;
+}
+
+void Ekf::resetHorizontalVelocityToZero()
+{
+	ECL_INFO("reset velocity to zero");
+	_information_events.flags.reset_vel_to_zero = true;
+
+	// Used when falling back to non-aiding mode of operation
+	resetHorizontalVelocityTo(Vector2f{0.f, 0.f}, 25.f);
+}
+
+void Ekf::resetVerticalVelocityToZero()
+{
+	// we don't know what the vertical velocity is, so set it to zero
+	// Set the variance to a value large enough to allow the state to converge quickly
+	// that does not destabilise the filter
+	resetVerticalVelocityTo(0.0f, 10.f);
+}
+
+void Ekf::resetVelocityTo(const Vector3f &new_vel, const Vector3f &new_vel_var)
+{
+	resetHorizontalVelocityTo(Vector2f(new_vel), Vector2f(new_vel_var(0), new_vel_var(1)));
+	resetVerticalVelocityTo(new_vel(2), new_vel_var(2));
+}