From 10db6b6eda0dbd1840c8abee8424ab4a6e200490 Mon Sep 17 00:00:00 2001 From: bresch <[brescianimathieu@gmail.com](mailto:brescianimathieu@gmail.com)> Date: Fri, 29 Sep 2023 14:38:04 +0200 Subject: [PATCH] ekf2: remove sparse vector optimization The sparse vector template requires to know which states are non-zero in the observation jacobian. This complicates the modularity of the code when the state vector or the derivation is changed. The computation cost difference is almost negligible for this size. --- src/modules/ekf2/EKF/ekf.h | 3 --- src/modules/ekf2/EKF/gps_yaw_fusion.cpp | 4 +--- src/modules/ekf2/EKF/mag_fusion.cpp | 10 ++-------- src/modules/ekf2/EKF/optflow_fusion.cpp | 3 +-- 4 files changed, 4 insertions(+), 16 deletions(-) diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 9c8f391d32..2899fca28a 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -63,9 +63,6 @@ public: typedef matrix::Vector VectorState; typedef matrix::SquareMatrix SquareMatrixState; typedef matrix::SquareMatrix Matrix2f; - template - - using SparseVectorState = matrix::SparseVectorf; Ekf() { diff --git a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp index e8e6c2bfef..22bdfb4847 100644 --- a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp +++ b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp @@ -100,8 +100,6 @@ void Ekf::fuseGpsYaw() sym::ComputeGnssYawPredInnovVarAndH(getStateAtFusionHorizonAsVector(), P, _gps_yaw_offset, gnss_yaw.observation_variance, FLT_EPSILON, &heading_pred, &heading_innov_var, &H); } - const SparseVectorState<0,1,2,3> Hfusion(H); - // check if the innovation variance calculation is badly conditioned if (gnss_yaw.innovation_variance < gnss_yaw.observation_variance) { // the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned @@ -129,7 +127,7 @@ void Ekf::fuseGpsYaw() // calculate the Kalman gains // only calculate gains for states we are using - VectorState Kfusion = P * Hfusion / gnss_yaw.innovation_variance; + VectorState Kfusion = P * H / gnss_yaw.innovation_variance; const bool is_fused = measurementUpdate(Kfusion, gnss_yaw.innovation_variance, gnss_yaw.innovation); _fault_status.flags.bad_hdg = !is_fused; diff --git a/src/modules/ekf2/EKF/mag_fusion.cpp b/src/modules/ekf2/EKF/mag_fusion.cpp index 8d18e12f5b..f033750d11 100644 --- a/src/modules/ekf2/EKF/mag_fusion.cpp +++ b/src/modules/ekf2/EKF/mag_fusion.cpp @@ -62,11 +62,9 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo Vector3f innov_var; // Observation jacobian and Kalman gain vectors - SparseVectorState<0,1,2,3,16,17,18,19,20,21> Hfusion; VectorState H; const VectorState state_vector = getStateAtFusionHorizonAsVector(); sym::ComputeMagInnovInnovVarAndHx(state_vector, P, mag, R_MAG, FLT_EPSILON, &mag_innov, &innov_var, &H); - Hfusion = H; innov_var.copyTo(aid_src_mag.innovation_variance); @@ -162,7 +160,6 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo } else if (index == 1) { // recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes) sym::ComputeMagYInnovVarAndH(state_vector, P, R_MAG, FLT_EPSILON, &aid_src_mag.innovation_variance[index], &H); - Hfusion = H; // recalculate innovation using the updated state aid_src_mag.innovation[index] = _state.quat_nominal.rotateVectorInverse(_state.mag_I)(index) + _state.mag_B(index) - mag(index); @@ -191,7 +188,6 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo // recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes) sym::ComputeMagZInnovVarAndH(state_vector, P, R_MAG, FLT_EPSILON, &aid_src_mag.innovation_variance[index], &H); - Hfusion = H; // recalculate innovation using the updated state aid_src_mag.innovation[index] = _state.quat_nominal.rotateVectorInverse(_state.mag_I)(index) + _state.mag_B(index) - mag(index); @@ -212,7 +208,7 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo } } - VectorState Kfusion = P * Hfusion / aid_src_mag.innovation_variance[index]; + VectorState Kfusion = P * H / aid_src_mag.innovation_variance[index]; if (!update_all_states) { for (unsigned row = 0; row <= 15; row++) { @@ -275,10 +271,8 @@ bool Ekf::fuseDeclination(float decl_sigma) return false; } - SparseVectorState<16,17> Hfusion(H); - // Calculate the Kalman gains - VectorState Kfusion = P * Hfusion / innovation_variance; + VectorState Kfusion = P * H / innovation_variance; const bool is_fused = measurementUpdate(Kfusion, innovation_variance, innovation); diff --git a/src/modules/ekf2/EKF/optflow_fusion.cpp b/src/modules/ekf2/EKF/optflow_fusion.cpp index e2f1379d06..88811ee9c2 100644 --- a/src/modules/ekf2/EKF/optflow_fusion.cpp +++ b/src/modules/ekf2/EKF/optflow_fusion.cpp @@ -146,8 +146,7 @@ void Ekf::fuseOptFlow() } } - SparseVectorState<0,1,2,3,4,5,6> Hfusion(H); - VectorState Kfusion = P * Hfusion / _aid_src_optical_flow.innovation_variance[index]; + VectorState Kfusion = P * H / _aid_src_optical_flow.innovation_variance[index]; if (measurementUpdate(Kfusion, _aid_src_optical_flow.innovation_variance[index], _aid_src_optical_flow.innovation[index])) { fused[index] = true;