local position acceleration: use mean value between two publications (#…

…24105)

To avoid aliasing on the ned acceleration, add an accumulation of acceleration to improve the downsampling

src/modules/ekf2/EKF/estimator_interface.h

@@ -240,7 +240,10 @@ class EstimatorInterface
 	const matrix::Quatf &getQuaternion() const { return _output_predictor.getQuaternion(); }
 	float getUnaidedYaw() const { return _output_predictor.getUnaidedYaw(); }
 	Vector3f getVelocity() const { return _output_predictor.getVelocity(); }
-	const Vector3f &getVelocityDerivative() const { 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(); }

src/modules/ekf2/EKF/output_predictor/output_predictor.cpp

@@ -109,7 +109,8 @@ void OutputPredictor::reset()
 
 	_R_to_earth_now.setIdentity();
 	_vel_imu_rel_body_ned.setZero();
-	_vel_deriv.setZero();
+	_delta_vel_sum.setZero();
+	_delta_vel_dt = 0.f;
 
 	_delta_angle_corr.setZero();
 
@@ -210,9 +211,8 @@ void OutputPredictor::calculateOutputStates(const uint64_t time_us, const Vector
 	delta_vel_earth(2) += CONSTANTS_ONE_G * delta_velocity_dt;
 
 	// calculate the earth frame velocity derivatives
-	if (delta_velocity_dt > 0.001f) {
-		_vel_deriv = delta_vel_earth / delta_velocity_dt;
-	}
+	_delta_vel_sum += delta_vel_earth;
+	_delta_vel_dt += delta_velocity_dt;
 
 	// save the previous velocity so we can use trapezoidal integration
 	const Vector3f vel_last(_output_new.vel);
@@ -389,3 +389,19 @@ void OutputPredictor::applyCorrectionToOutputBuffer(const Vector3f &vel_correcti
 	// update output state to corrected values
 	_output_new = _output_buffer.get_newest();
 }
+
+matrix::Vector3f OutputPredictor::getVelocityDerivative() const
+{
+	if (_delta_vel_dt > FLT_EPSILON) {
+		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();
+}

src/modules/ekf2/EKF/output_predictor/output_predictor.h

@@ -101,8 +101,10 @@ 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 velocity derivative in earth frame
-	const matrix::Vector3f &getVelocityDerivative() const { return _vel_deriv; }
+	// get the mean velocity derivative in earth frame since reset (see `resetVelocityDerivativeAccumulation()`)
+	matrix::Vector3f getVelocityDerivative() const;
+
+	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); }
@@ -178,7 +180,8 @@ class OutputPredictor
 	outputVert _output_vert_new{};		// vertical filter output on the non-delayed time horizon
 	matrix::Matrix3f _R_to_earth_now{};		// rotation matrix from body to earth frame at current time
 	matrix::Vector3f _vel_imu_rel_body_ned{};		// velocity of IMU relative to body origin in NED earth frame
-	matrix::Vector3f _vel_deriv{};		// velocity derivative at the IMU in NED earth frame (m/s/s)
+	matrix::Vector3f _delta_vel_sum{};	// accumulation of delta velocity at the IMU in NED earth frame (m/s/s)
+	float _delta_vel_dt{};			// duration of delta velocity integration (s)
 
 	// output predictor states
 	matrix::Vector3f _delta_angle_corr{};	///< delta angle correction vector (rad)

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);