From 8001132d3364999ccee7fdff23a1638cdda2dbc6 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 23 Feb 2024 11:40:12 -0500 Subject: [PATCH] ekf2: ZeroGyroUpdate move to fuseDirectStateMeasurement --- .../ekf2/EKF/aid_sources/ZeroGyroUpdate.cpp | 20 ++++--------------- 1 file changed, 4 insertions(+), 16 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/ZeroGyroUpdate.cpp b/src/modules/ekf2/EKF/aid_sources/ZeroGyroUpdate.cpp index 0831c13230..b9eea13604 100644 --- a/src/modules/ekf2/EKF/aid_sources/ZeroGyroUpdate.cpp +++ b/src/modules/ekf2/EKF/aid_sources/ZeroGyroUpdate.cpp @@ -60,25 +60,13 @@ bool ZeroGyroUpdate::update(Ekf &ekf, const estimator::imuSample &imu_delayed) if (zero_gyro_update_data_ready) { Vector3f gyro_bias = _zgup_delta_ang / _zgup_delta_ang_dt; - Vector3f innovation = ekf.state().gyro_bias - gyro_bias; const float obs_var = sq(math::constrain(ekf.getGyroNoise(), 0.f, 1.f)); - const Vector3f innov_var = ekf.getGyroBiasVariance() + obs_var; - - for (int i = 0; i < 3; i++) { - Ekf::VectorState K; // Kalman gain vector for any single observation - sequential fusion is used. - const unsigned state_index = State::gyro_bias.idx + i; - - // calculate kalman gain K = PHS, where S = 1/innovation variance - for (int row = 0; row < State::size; row++) { - K(row) = ekf.stateCovariance(row, state_index) / innov_var(i); - } - - Ekf::VectorState H{}; - H(State::gyro_bias.idx + i) = 1.f; - - ekf.measurementUpdate(K, H, obs_var, innovation(i), innov_var(i)); + for (unsigned i = 0; i < 3; i++) { + const float innovation = ekf.state().gyro_bias(i) - gyro_bias(i); + const float innov_var = ekf.getGyroBiasVariance()(i) + obs_var; + ekf.fuseDirectStateMeasurement(innovation, innov_var, obs_var, State::gyro_bias.idx + i); } // Reset the integrators