EKF: split delta velocity accumulztation get and reset functions

PX4 · Dec 16, 2024 · 5c1a6bc · 5c1a6bc
1 parent 44e4393
commit 5c1a6bc
Show file tree

Hide file tree

Showing 4 changed files with 17 additions and 10 deletions.
diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h
@@ -241,8 +241,9 @@ class EstimatorInterface
 	float getUnaidedYaw() const { return _output_predictor.getUnaidedYaw(); }
 	Vector3f getVelocity() const { return _output_predictor.getVelocity(); }
 
-	// get the mean velocity derivative in earth frame since last call
-	Vector3f getVelocityDerivative() { return _output_predictor.getVelocityDerivative(); }
+	// get the mean velocity derivative in earth frame since last reset (see `resetVelocityDerivativeAccumulation()`)
+	Vector3f getVelocityDerivative() const { return _output_predictor.getVelocityDerivative(); }
+	void resetVelocityDerivativeAccumulation() { return _output_predictor.resetVelocityDerivativeAccumulation(); }
 	float getVerticalPositionDerivative() const { return _output_predictor.getVerticalPositionDerivative(); }
 	Vector3f getPosition() const;
 	LatLonAlt getLatLonAlt() const { return _output_predictor.getLatLonAlt(); }

diff --git a/src/modules/ekf2/EKF/output_predictor/output_predictor.cpp b/src/modules/ekf2/EKF/output_predictor/output_predictor.cpp
@@ -390,16 +390,18 @@ void OutputPredictor::applyCorrectionToOutputBuffer(const Vector3f &vel_correcti
 	_output_new = _output_buffer.get_newest();
 }
 
-matrix::Vector3f OutputPredictor::getVelocityDerivative()
+matrix::Vector3f OutputPredictor::getVelocityDerivative() const
 {
-	matrix::Vector3f vel_deriv(0.f, 0.f, 0.f);
-
 	if (_delta_vel_dt > FLT_EPSILON) {
-		vel_deriv = _delta_vel_sum / _delta_vel_dt;
+		return _delta_vel_sum / _delta_vel_dt;
+
+	} else {
+		return matrix::Vector3f(0.f, 0.f, 0.f);
 	}
+}
 
+void OutputPredictor::resetVelocityDerivativeAccumulation()
+{
 	_delta_vel_dt = 0.f;
 	_delta_vel_sum.setZero();
-
-	return vel_deriv;
 }
diff --git a/src/modules/ekf2/EKF/output_predictor/output_predictor.h b/src/modules/ekf2/EKF/output_predictor/output_predictor.h
@@ -101,8 +101,11 @@ class OutputPredictor
 	// get the velocity of the body frame origin in local NED earth frame
 	matrix::Vector3f getVelocity() const { return _output_new.vel - _vel_imu_rel_body_ned; }
 
-	// get the mean velocity derivative in earth frame since last call
-	matrix::Vector3f getVelocityDerivative();
+	// get the mean velocity derivative in earth frame since reset (see `resetVelocityDerivativeAccumulation()`)
+	matrix::Vector3f getVelocityDerivative() const;
+
+	// reset the velocity derivative accumulation
+	void resetVelocityDerivativeAccumulation();
 
 	// get the derivative of the vertical position of the body frame origin in local NED earth frame
 	float getVerticalPositionDerivative() const { return _output_vert_new.vert_vel - _vel_imu_rel_body_ned(2); }

diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp
@@ -1556,6 +1556,7 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
 
 	// Acceleration of body origin in local frame
 	const Vector3f vel_deriv{_ekf.getVelocityDerivative()};
+	_ekf.resetVelocityDerivativeAccumulation();
 	lpos.ax = vel_deriv(0);
 	lpos.ay = vel_deriv(1);
 	lpos.az = vel_deriv(2);