From 4fe6d699660e2402d58d16ae83e9427ece94ab51 Mon Sep 17 00:00:00 2001 From: Nicolas MARTIN <59083163+NicolasM0@users.noreply.github.com> Date: Fri, 3 Jan 2025 12:43:39 +0100 Subject: [PATCH] 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 | 5 +++- .../EKF/output_predictor/output_predictor.cpp | 24 +++++++++++++++---- .../EKF/output_predictor/output_predictor.h | 9 ++++--- src/modules/ekf2/EKF2.cpp | 1 + 4 files changed, 31 insertions(+), 8 deletions(-) diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index e878d100dc9a..121230027831 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/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(); } diff --git a/src/modules/ekf2/EKF/output_predictor/output_predictor.cpp b/src/modules/ekf2/EKF/output_predictor/output_predictor.cpp index 5657d68a2029..05c452b05d80 100644 --- a/src/modules/ekf2/EKF/output_predictor/output_predictor.cpp +++ b/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(); +} diff --git a/src/modules/ekf2/EKF/output_predictor/output_predictor.h b/src/modules/ekf2/EKF/output_predictor/output_predictor.h index 602993b53309..e90bdf94812e 100644 --- a/src/modules/ekf2/EKF/output_predictor/output_predictor.h +++ b/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) diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 93c9ed617a0f..c85527b9121a 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1556,6 +1556,7 @@ void EKF2::PublishLocalPosition(const hrt_abstime ×tamp) // 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);