From 9e962f3efa3ba208246296f09cafab156deaa939 Mon Sep 17 00:00:00 2001 From: bresch <[brescianimathieu@gmail.com](mailto:brescianimathieu@gmail.com)> Date: Wed, 20 Sep 2023 10:55:06 +0200 Subject: [PATCH] ekf2: update more hardcoded indexes --- src/modules/ekf2/EKF/airspeed_fusion.cpp | 8 ++-- src/modules/ekf2/EKF/ekf.h | 50 +++++++++++------------ src/modules/ekf2/EKF/sideslip_fusion.cpp | 6 +-- src/modules/ekf2/EKF/zero_gyro_update.cpp | 7 +--- 4 files changed, 32 insertions(+), 39 deletions(-) diff --git a/src/modules/ekf2/EKF/airspeed_fusion.cpp b/src/modules/ekf2/EKF/airspeed_fusion.cpp index 8a8057e6f1..2668147e2b 100644 --- a/src/modules/ekf2/EKF/airspeed_fusion.cpp +++ b/src/modules/ekf2/EKF/airspeed_fusion.cpp @@ -200,9 +200,9 @@ void Ekf::fuseAirspeed(const airspeedSample &airspeed_sample, estimator_aid_sour sym::ComputeAirspeedHAndK(getStateAtFusionHorizonAsVector(), P, innov_var, FLT_EPSILON, &H, &K); if (update_wind_only) { - for (unsigned row = 0; row <= 21; row++) { - K(row) = 0.f; - } + const Vector2f K_wind = K.slice(State::wind_vel.idx, 0); + K.setZero(); + K.slice(State::wind_vel.idx, 0) = K_wind; } const bool is_fused = measurementUpdate(K, aid_src.innovation_variance, aid_src.innovation); @@ -258,7 +258,7 @@ void Ekf::resetWindCovarianceUsingAirspeed(const airspeedSample &airspeed_sample const float Wy = -_state.wind_vel(0) * sin_yaw + _state.wind_vel(1) * cos_yaw; // it is safer to remove all existing correlations to other states at this time - P.uncorrelateCovarianceSetVariance<2>(22, 0.0f); + P.uncorrelateCovarianceSetVariance(State::wind_vel.idx, 0.0f); P(22, 22) = R_TAS * sq(cos_yaw) + R_yaw * sq(-Wx * sin_yaw - Wy * cos_yaw) + initial_wind_var_body_y * sq(sin_yaw); P(22, 23) = R_TAS * sin_yaw * cos_yaw + R_yaw * (-Wx * sin_yaw - Wy * cos_yaw) * (Wx * cos_yaw - Wy * sin_yaw) - diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 79fb7f5207..8eb1ba235d 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -310,22 +310,22 @@ public: const Vector2f &getWindVelocity() const { return _state.wind_vel; }; // get the wind velocity var - Vector2f getWindVelocityVariance() const { return P.slice<2, 2>(22, 22).diag(); } + Vector2f getWindVelocityVariance() const { return P.slice(State::wind_vel.idx, State::wind_vel.idx).diag(); } // get the full covariance matrix - const matrix::SquareMatrix &covariances() const { return P; } + const matrix::SquareMatrix &covariances() const { return P; } // get the diagonal elements of the covariance matrix - matrix::Vector covariances_diagonal() const { return P.diag(); } + matrix::Vector covariances_diagonal() const { return P.diag(); } // get the orientation (quaterion) covariances - matrix::SquareMatrix orientation_covariances() const { return P.slice<4, 4>(0, 0); } + matrix::SquareMatrix orientation_covariances() const { return P.slice(State::quat_nominal.idx, State::quat_nominal.idx); } // get the linear velocity covariances - matrix::SquareMatrix velocity_covariances() const { return P.slice<3, 3>(4, 4); } + matrix::SquareMatrix velocity_covariances() const { return P.slice(State::vel.idx, State::vel.idx); } // get the position covariances - matrix::SquareMatrix position_covariances() const { return P.slice<3, 3>(7, 7); } + matrix::SquareMatrix position_covariances() const { return P.slice(State::pos.idx, State::pos.idx); } // ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined bool collect_gps(const gpsMessage &gps) override; @@ -356,9 +356,9 @@ public: void resetGyroBias(); void resetAccelBias(); - Vector3f getVelocityVariance() const { return P.slice<3, 3>(4, 4).diag(); }; + Vector3f getVelocityVariance() const { return velocity_covariances().diag(); }; - Vector3f getPositionVariance() const { return P.slice<3, 3>(7, 7).diag(); } + Vector3f getPositionVariance() const { return position_covariances().diag(); } // First argument returns GPS drift metrics in the following array locations // 0 : Horizontal position drift rate (m/s) @@ -406,12 +406,12 @@ public: #endif } - // gyro bias (states 10, 11, 12) + // gyro bias const Vector3f &getGyroBias() const { return _state.gyro_bias; } // get the gyroscope bias in rad/s Vector3f getGyroBiasVariance() const { return P.slice(State::gyro_bias.idx, State::gyro_bias.idx).diag(); } // get the gyroscope bias variance in rad/s float getGyroBiasLimit() const { return _params.gyro_bias_lim; } - // accel bias (states 13, 14, 15) + // accel bias const Vector3f &getAccelBias() const { return _state.accel_bias; } // get the accelerometer bias in m/s**2 Vector3f getAccelBiasVariance() const { return P.slice(State::accel_bias.idx, State::accel_bias.idx).diag(); } // get the accelerometer bias variance in m/s**2 float getAccelBiasLimit() const { return _params.acc_bias_lim; } @@ -974,38 +974,34 @@ private: void clearInhibitedStateKalmanGains(VectorState &K) const { - // gyro bias: states 10, 11, 12 - for (unsigned i = 0; i < 3; i++) { + for (unsigned i = 0; i < State::gyro_bias.dof; i++) { if (_gyro_bias_inhibit[i]) { - K(10 + i) = 0.f; + K(State::gyro_bias.idx + i) = 0.f; } } - // accel bias: states 13, 14, 15 - for (unsigned i = 0; i < 3; i++) { + for (unsigned i = 0; i < State::accel_bias.dof; i++) { if (_accel_bias_inhibit[i]) { - K(13 + i) = 0.f; + K(State::accel_bias.idx + i) = 0.f; } } - // mag I: states 16, 17, 18 if (!_control_status.flags.mag) { - K(16) = 0.f; - K(17) = 0.f; - K(18) = 0.f; + for (unsigned i = 0; i < State::mag_I.dof; i++) { + K(State::mag_I.idx + i) = 0.f; + } } - // mag B: states 19, 20, 21 if (!_control_status.flags.mag) { - K(19) = 0.f; - K(20) = 0.f; - K(21) = 0.f; + for (unsigned i = 0; i < State::mag_B.dof; i++) { + K(State::mag_B.idx + i) = 0.f; + } } - // wind: states 22, 23 if (!_control_status.flags.wind) { - K(22) = 0.f; - K(23) = 0.f; + for (unsigned i = 0; i < State::wind_vel.dof; i++) { + K(State::wind_vel.idx + i) = 0.f; + } } } diff --git a/src/modules/ekf2/EKF/sideslip_fusion.cpp b/src/modules/ekf2/EKF/sideslip_fusion.cpp index 439fc942ff..b1bfc4db3d 100644 --- a/src/modules/ekf2/EKF/sideslip_fusion.cpp +++ b/src/modules/ekf2/EKF/sideslip_fusion.cpp @@ -141,9 +141,9 @@ void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip) sym::ComputeSideslipHAndK(getStateAtFusionHorizonAsVector(), P, sideslip.innovation_variance, FLT_EPSILON, &H, &K); if (update_wind_only) { - for (unsigned row = 0; row <= 21; row++) { - K(row) = 0.f; - } + const Vector2f K_wind = K.slice(State::wind_vel.idx, 0); + K.setZero(); + K.slice(State::wind_vel.idx, 0) = K_wind; } const bool is_fused = measurementUpdate(K, sideslip.innovation_variance, sideslip.innovation); diff --git a/src/modules/ekf2/EKF/zero_gyro_update.cpp b/src/modules/ekf2/EKF/zero_gyro_update.cpp index 1221b79c4e..571bde209a 100644 --- a/src/modules/ekf2/EKF/zero_gyro_update.cpp +++ b/src/modules/ekf2/EKF/zero_gyro_update.cpp @@ -59,10 +59,7 @@ void Ekf::controlZeroGyroUpdate(const imuSample &imu_delayed) const float obs_var = sq(math::constrain(_params.gyro_noise, 0.f, 1.f)); - Vector3f innov_var{ - P(10, 10) + obs_var, - P(11, 11) + obs_var, - P(12, 12) + obs_var}; + const Vector3f innov_var = getGyroBiasVariance() + obs_var; for (int i = 0; i < 3; i++) { fuseDeltaAngBias(innovation(i), innov_var(i), i); @@ -83,7 +80,7 @@ void Ekf::controlZeroGyroUpdate(const imuSample &imu_delayed) void Ekf::fuseDeltaAngBias(const float innov, const float innov_var, const int obs_index) { VectorState K; // Kalman gain vector for any single observation - sequential fusion is used. - const unsigned state_index = obs_index + 10; + const unsigned state_index = obs_index + State::gyro_bias.idx; // calculate kalman gain K = PHS, where S = 1/innovation variance for (int row = 0; row < State::size; row++) {