From 1de4cd63425896808a2b53e2f6d96a0c76e50cb7 Mon Sep 17 00:00:00 2001
From: Nicolas Martin <n.martin@elistair.com>
Date: Mon, 2 Dec 2024 13:49:55 +0100
Subject: [PATCH] local position acceleration: use mean value between two
 publications

To avoid aliasing on the ned acceleration, add an accumulation of
acceleration to improve the downsampling
---
 src/modules/ekf2/EKF2.cpp | 21 +++++++++++++++++----
 src/modules/ekf2/EKF2.hpp |  3 +++
 2 files changed, 20 insertions(+), 4 deletions(-)

diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp
index 93c9ed617a0f..629a985cb084 100644
--- a/src/modules/ekf2/EKF2.cpp
+++ b/src/modules/ekf2/EKF2.cpp
@@ -731,6 +731,10 @@ void EKF2::Run()
 		_ekf.setIMUData(imu_sample_new);
 		PublishAttitude(now); // publish attitude immediately (uses quaternion from output predictor)
 
+		// accumulate delta velocity to compute mean acceleration
+		delta_vel_ += _ekf.getVelocityDerivative() * imu_sample_new.delta_vel_dt;
+		vel_integ_duration_ += imu_sample_new.delta_vel_dt;
+
 		// integrate time to monitor time slippage
 		if (_start_time_us > 0) {
 			_integrated_time_us += imu_dt;
@@ -1555,10 +1559,19 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
 	lpos.z_deriv = _ekf.getVerticalPositionDerivative();
 
 	// Acceleration of body origin in local frame
-	const Vector3f vel_deriv{_ekf.getVelocityDerivative()};
-	lpos.ax = vel_deriv(0);
-	lpos.ay = vel_deriv(1);
-	lpos.az = vel_deriv(2);
+	if (vel_integ_duration_ > 0.001f) {
+		lpos.ax = delta_vel_(0) / vel_integ_duration_;
+		lpos.ay = delta_vel_(1) / vel_integ_duration_;
+		lpos.az = delta_vel_(2) / vel_integ_duration_;
+
+	} else {
+		lpos.ax = 0.f;
+		lpos.ay = 0.f;
+		lpos.az = 0.f;
+	}
+
+	delta_vel_.setZero();
+	vel_integ_duration_ = 0.f;
 
 	lpos.xy_valid = _ekf.isLocalHorizontalPositionValid();
 	lpos.v_xy_valid = _ekf.isLocalHorizontalPositionValid();
diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp
index 0bec478e0d73..5d52e2b03763 100644
--- a/src/modules/ekf2/EKF2.hpp
+++ b/src/modules/ekf2/EKF2.hpp
@@ -280,6 +280,9 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem
 	perf_counter_t _ekf_update_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": EKF update")};
 	perf_counter_t _msg_missed_imu_perf{perf_alloc(PC_COUNT, MODULE_NAME": IMU message missed")};
 
+	Vector3f delta_vel_{0.f, 0.f, 0.f};
+	float vel_integ_duration_{0};
+
 	InFlightCalibration _accel_cal{};
 	InFlightCalibration _gyro_cal{};