From 30636495c7039aed1b7c4ea62f965664aaa22b8e Mon Sep 17 00:00:00 2001 From: Marco Hauswirth Date: Thu, 30 Jan 2025 11:36:53 +0100 Subject: [PATCH] fix uncertainty initialization --- .../range_finder_consistency_check.cpp | 66 ++++++++----------- .../range_finder_consistency_check.hpp | 8 ++- src/modules/ekf2/EKF/covariance.cpp | 2 +- 3 files changed, 32 insertions(+), 44 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp index a6546a6f0c..6fcee9f00f 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp @@ -43,19 +43,20 @@ using namespace matrix; void RangeFinderConsistencyCheck::init(const float &z, const float &z_var, const float &dist_bottom, const float &dist_bottom_var) { - // assume no correlation between z and terrain on initialization + // assume no correlation between z and dist_bottom + // values defined with symforce for H[1, -1] , P = [z_var, 0; 0, z_var + dist_bottom_var], K = [0,1/H(1)] _P(RangeFilter::z.idx, RangeFilter::z.idx) = z_var; - _P(RangeFilter::z.idx, RangeFilter::terrain.idx) = 0.f; - _P(RangeFilter::terrain.idx, RangeFilter::z.idx) = 0.f; - _P(RangeFilter::terrain.idx, RangeFilter::terrain.idx) = z_var + dist_bottom_var; + _P(RangeFilter::z.idx, RangeFilter::terrain.idx) = z_var; + _P(RangeFilter::terrain.idx, RangeFilter::z.idx) = z_var; + _P(RangeFilter::terrain.idx, RangeFilter::terrain.idx) = dist_bottom_var + z_var; - _Ht = sym::RangeValidationFilterH(); + _Ht = sym::RangeValidationFilterH().transpose(); _x(RangeFilter::z.idx) = z; _x(RangeFilter::terrain.idx) = z - dist_bottom; _initialized = true; _state = KinematicState::UNKNOWN; - _test_ratio_lpf.reset(0); + _test_ratio_lpf.reset(0.f); _t_since_first_sample = 0.f; } @@ -71,82 +72,67 @@ void RangeFinderConsistencyCheck::update(const float &z, const float &z_var, con } else if (!_initialized) { init(z, z_var, dist_bottom, dist_bottom_var); - _test_ratio_lpf.setParameters(dt, 1.f); return; } // prediction step _time_last_update_us = time_us; _x(RangeFilter::z.idx) -= dt * vz; - _P(RangeFilter::z.idx, RangeFilter::z.idx) += dt * dt * vz_var + 0.001f; - _P(RangeFilter::terrain.idx, RangeFilter::terrain.idx) += terrain_process_noise; + _P(RangeFilter::z.idx, RangeFilter::z.idx) += dt * dt * vz_var; + _P(RangeFilter::terrain.idx, RangeFilter::terrain.idx) += _terrain_process_noise; // iterate through both measurements (z and dist_bottom) const Vector2f measurements{z, dist_bottom}; for (int measurement_idx = 0; measurement_idx < 2; measurement_idx++) { - float hz; // Jacobian in respect to z - float ht; // Jacobian in respect to terrain float R; bool reject = false; + Vector2f H; if (measurement_idx == 0) { // direct state measurement - hz = 1.f; - ht = 0.f; + H(RangeFilter::z.idx) = 1.f; + H(RangeFilter::terrain.idx) = 0.f; R = z_var; } else if (measurement_idx == 1) { - hz = _Ht(0, 0); - ht = _Ht(0, 1); + H(RangeFilter::z.idx) = _Ht(0); + H(RangeFilter::terrain.idx) = _Ht(1); R = dist_bottom_var; } // residual - const float measurement_pred = hz * _x(RangeFilter::z.idx) + ht * _x(RangeFilter::terrain.idx); + const float measurement_pred = H * _x; const float y = measurements(measurement_idx) - measurement_pred; - // innovation variance H * P * H^T + R - const float S = hz * (hz * _P(RangeFilter::z.idx, RangeFilter::z.idx) - + ht * _P(RangeFilter::terrain.idx, RangeFilter::z.idx)) - + ht * (hz * _P(RangeFilter::z.idx, RangeFilter::terrain.idx) - + ht * _P(RangeFilter::terrain.idx, RangeFilter::terrain.idx)) - + R; - - // kalman gain K = P * H^T / S - float Kz = (hz * _P(RangeFilter::z.idx, RangeFilter::z.idx) - + ht * _P(RangeFilter::z.idx, RangeFilter::terrain.idx)) / S; - const float Kt = (hz * _P(RangeFilter::terrain.idx, RangeFilter::z.idx) - + ht * _P(RangeFilter::terrain.idx, RangeFilter::terrain.idx)) / S; + // for H as col-vector: + // innovation variance S = H^T * P * H + R + // kalman gain K = P * H / S + const float S = (H.transpose() * _P * H + R)(0, 0); + Vector2f K = (_P * H / S); if (measurement_idx == 0) { - Kz = 1.f; + K(RangeFilter::z.idx) = 1.f; } else if (measurement_idx == 1) { _innov = y; const float test_ratio = fminf(sq(y) / (sq(_gate) * S), 4.f); // limit to 4 to limit sensitivity to outliers - _test_ratio_lpf.update(sign(_innov) * test_ratio); + _test_ratio_lpf.update(sign(_innov) * test_ratio, dt); reject = test_ratio > 1.f; } if (!reject) { // update step - _x(RangeFilter::z.idx) += Kz * y; - _x(RangeFilter::terrain.idx) += Kt * y; + _x(RangeFilter::z.idx) += K(RangeFilter::z.idx) * y; + _x(RangeFilter::terrain.idx) += K(RangeFilter::terrain.idx) * y; // covariance update with Joseph form: // P = (I - K H) P (I - K H)^T + K R K^T Matrix2f I; I.setIdentity(); - Matrix K; - K(0, 0) = Kz; - K(1, 0) = Kt; - Vector2f H0; - H0(0) = hz; - H0(1) = ht; - Matrix2f IKH0 = I - K * H0.transpose(); - _P = IKH0 * _P * IKH0.transpose() + K * R * K.transpose(); + Matrix2f IKH = I - K.multiplyByTranspose(H); + _P = IKH * _P * IKH.transpose() + (K * R).multiplyByTranspose(K); } } diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp index 85716474c7..4275510933 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp @@ -64,6 +64,7 @@ public: void setGate(const float gate) { _gate = gate; } void run(const float &z, const float &vz, const matrix::SquareMatrix &P, const float &dist_bottom, const float &dist_bottom_var, uint64_t time_us); + void set_terrain_process_noise(const float terrain_process_noise) { _terrain_process_noise = terrain_process_noise; } void reset() { _state = (_initialized && _state == KinematicState::CONSISTENT) ? KinematicState::UNKNOWN : _state; @@ -71,7 +72,6 @@ public: } uint8_t current_posD_reset_count{0}; - float terrain_process_noise{0.0f}; bool horizontal_motion{false}; private: @@ -80,14 +80,16 @@ private: const float &dist_bottom_var, const uint64_t &time_us); void init(const float &z, const float &z_var, const float &dist_bottom, const float &dist_bottom_var); void evaluateState(const float &dt, const float &vz, const float &vz_var); + float _terrain_process_noise{0.0f}; matrix::SquareMatrix _P{}; - matrix::Matrix _Ht{}; + matrix::Vector2f _Ht{}; matrix::Vector2f _x{}; bool _initialized{false}; float _innov{0.f}; float _innov_var{0.f}; uint64_t _time_last_update_us{0}; - AlphaFilter _test_ratio_lpf{}; + static constexpr float time_constant{1.f}; + AlphaFilter _test_ratio_lpf{time_constant}; float _gate{1.0f}; KinematicState _state{KinematicState::UNKNOWN}; float _t_since_first_sample{0.f}; diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index 98beff1409..7d710052ca 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -226,7 +226,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) // process noise due to terrain gradient terrain_process_noise += sq(imu_delayed.delta_vel_dt * _params.terrain_gradient) * (sq(_state.vel(0)) + sq(_state.vel( 1))); - _rng_consistency_check.terrain_process_noise = terrain_process_noise; + _rng_consistency_check.set_terrain_process_noise(terrain_process_noise); P(State::terrain.idx, State::terrain.idx) += terrain_process_noise; }