From 257c1be75d8ff3a876755d9235c00ed454e31b89 Mon Sep 17 00:00:00 2001 From: Nicolas Martin Date: Mon, 2 Dec 2024 13:49:55 +0100 Subject: [PATCH 1/3] EKF/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/EKF/estimator_interface.h | 4 +++- .../EKF/output_predictor/output_predictor.cpp | 22 +++++++++++++++---- .../EKF/output_predictor/output_predictor.h | 7 +++--- 3 files changed, 25 insertions(+), 8 deletions(-) diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index e878d100dc9a..97066b546eca 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -240,7 +240,9 @@ 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 call + Vector3f getVelocityDerivative() { return _output_predictor.getVelocityDerivative(); } 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..d62dfb2e1786 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,17 @@ void OutputPredictor::applyCorrectionToOutputBuffer(const Vector3f &vel_correcti // update output state to corrected values _output_new = _output_buffer.get_newest(); } + +matrix::Vector3f OutputPredictor::getVelocityDerivative() +{ + matrix::Vector3f vel_deriv(0.f, 0.f, 0.f); + + if (_delta_vel_dt > FLT_EPSILON) { + vel_deriv = _delta_vel_sum / _delta_vel_dt; + } + + _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 index 602993b53309..1f6acfacad88 100644 --- a/src/modules/ekf2/EKF/output_predictor/output_predictor.h +++ b/src/modules/ekf2/EKF/output_predictor/output_predictor.h @@ -101,8 +101,8 @@ 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 last call + matrix::Vector3f getVelocityDerivative(); // 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 +178,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) From 3d754e8ec15cdb4b59909502442d583b8684d8cd Mon Sep 17 00:00:00 2001 From: Nicolas Martin Date: Mon, 16 Dec 2024 10:18:01 +0100 Subject: [PATCH 2/3] EKF: split delta velocity accumulztation get and reset functions --- src/modules/ekf2/EKF/estimator_interface.h | 5 +++-- .../ekf2/EKF/output_predictor/output_predictor.cpp | 14 ++++++++------ .../ekf2/EKF/output_predictor/output_predictor.h | 7 +++++-- src/modules/ekf2/EKF2.cpp | 1 + 4 files changed, 17 insertions(+), 10 deletions(-) diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 97066b546eca..121230027831 100644 --- 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 index d62dfb2e1786..05c452b05d80 100644 --- 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 index 1f6acfacad88..30db199d86ac 100644 --- 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 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); From ce6a428dbe89efa29a92e00eb6cdb8bf943f3d5c Mon Sep 17 00:00:00 2001 From: Nicolas MARTIN <59083163+NicolasM0@users.noreply.github.com> Date: Tue, 17 Dec 2024 14:31:22 +0100 Subject: [PATCH 3/3] Remove useless comment Co-authored-by: Mathieu Bresciani --- src/modules/ekf2/EKF/output_predictor/output_predictor.h | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/ekf2/EKF/output_predictor/output_predictor.h b/src/modules/ekf2/EKF/output_predictor/output_predictor.h index 30db199d86ac..e90bdf94812e 100644 --- a/src/modules/ekf2/EKF/output_predictor/output_predictor.h +++ b/src/modules/ekf2/EKF/output_predictor/output_predictor.h @@ -104,7 +104,6 @@ class OutputPredictor // 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