diff --git a/src/modules/ekf2/EKF/airspeed_fusion.cpp b/src/modules/ekf2/EKF/airspeed_fusion.cpp index 261ee9a338..06d4ce8fb0 100644 --- a/src/modules/ekf2/EKF/airspeed_fusion.cpp +++ b/src/modules/ekf2/EKF/airspeed_fusion.cpp @@ -115,15 +115,13 @@ void Ekf::fuseAirspeed(estimator_aid_source1d_s &airspeed) sym::ComputeAirspeedHAndK(getStateAtFusionHorizonAsVector(), P, innov_var, FLT_EPSILON, &H, &K); - SparseVector24f<4,5,6,22,23> H_sparse(H); - if (update_wind_only) { for (unsigned row = 0; row <= 21; row++) { K(row) = 0.f; } } - const bool is_fused = measurementUpdate(K, H_sparse, airspeed.innovation); + const bool is_fused = measurementUpdate(K, airspeed.innovation_variance, airspeed.innovation); airspeed.fused = is_fused; _fault_status.flags.bad_airspeed = !is_fused; diff --git a/src/modules/ekf2/EKF/drag_fusion.cpp b/src/modules/ekf2/EKF/drag_fusion.cpp index dda97c9dfc..bf21400e32 100644 --- a/src/modules/ekf2/EKF/drag_fusion.cpp +++ b/src/modules/ekf2/EKF/drag_fusion.cpp @@ -331,7 +331,7 @@ void Ekf::fuseDrag(const dragSample &drag_sample) // if the innovation consistency check fails then don't fuse the sample if (_drag_test_ratio(axis_index) <= 1.0f) { - measurementUpdate(Kfusion, Hfusion, _drag_innov(axis_index)); + measurementUpdate(Kfusion, _drag_innov_var(axis_index), _drag_innov(axis_index)); } } } diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 69434b161b..6105e0c0d3 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -793,35 +793,7 @@ private: // update the rotation matrix which transforms EV navigation frame measurements into NED void calcExtVisRotMat(); - // matrix vector multiplication for computing K<24,1> * H<1,24> * P<24,24> - // that is optimized by exploring the sparsity in H - template - SquareMatrix24f computeKHP(const Vector24f &K, const SparseVector24f &H) const - { - // K(HP) and (KH)P are equivalent (matrix multiplication is associative) - // but K(HP) is computationally much less expensive - Vector24f HP; - for (unsigned i = 0; i < H.non_zeros(); i++) { - const size_t row = H.index(i); - for (unsigned col = 0; col < _k_num_states; col++) { - HP(col) = HP(col) + H.atCompressedIndex(i) * P(row, col); - } - } - - SquareMatrix24f KHP; - for (unsigned row = 0; row < _k_num_states; row++) { - for (unsigned col = 0; col < _k_num_states; col++) { - KHP(row, col) = K(row) * HP(col); - } - } - - return KHP; - } - - // measurement update with a single measurement - // returns true if fusion is performed - template - bool measurementUpdate(Vector24f &K, const SparseVector24f &H, float innovation) + bool measurementUpdate(Vector24f &K, float innovation_variance, float innovation) { for (unsigned i = 0; i < 3; i++) { if (_accel_bias_inhibit[i]) { @@ -829,10 +801,16 @@ private: } } - // apply covariance correction via P_new = (I -K*H)*P - // first calculate expression for KHP - // then calculate P - KHP - const SquareMatrix24f KHP = computeKHP(K, H); + const Vector24f KS = K * innovation_variance; + SquareMatrix24f KHP; + + for (unsigned row = 0; row < _k_num_states; row++) { + for (unsigned col = 0; col < _k_num_states; col++) { + // Instad of literally computing KHP, use an equvalent + // equation involving less mathematical operations + KHP(row, col) = KS(row) * K(col); + } + } const bool is_healthy = checkAndFixCovarianceUpdate(KHP); diff --git a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp index 7ccc5feb33..61363ce683 100644 --- a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp +++ b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp @@ -132,7 +132,7 @@ void Ekf::fuseGpsYaw() // only calculate gains for states we are using Vector24f Kfusion = P * Hfusion / gnss_yaw.innovation_variance; - const bool is_fused = measurementUpdate(Kfusion, Hfusion, gnss_yaw.innovation); + const bool is_fused = measurementUpdate(Kfusion, gnss_yaw.innovation_variance, gnss_yaw.innovation); _fault_status.flags.bad_hdg = !is_fused; gnss_yaw.fused = is_fused; diff --git a/src/modules/ekf2/EKF/mag_fusion.cpp b/src/modules/ekf2/EKF/mag_fusion.cpp index 45ee59006a..a9b2dea226 100644 --- a/src/modules/ekf2/EKF/mag_fusion.cpp +++ b/src/modules/ekf2/EKF/mag_fusion.cpp @@ -200,7 +200,7 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo } } - if (measurementUpdate(Kfusion, Hfusion, aid_src_mag.innovation[index])) { + if (measurementUpdate(Kfusion, aid_src_mag.innovation_variance[index], aid_src_mag.innovation[index])) { fused[index] = true; limitDeclination(); @@ -312,9 +312,7 @@ bool Ekf::fuseYaw(const float innovation, const float variance, estimator_aid_so _innov_check_fail_status.flags.reject_yaw = false; } - SparseVector24f<0,1,2,3> Hfusion(H_YAW); - - if (measurementUpdate(Kfusion, Hfusion, aid_src_status.innovation)) { + if (measurementUpdate(Kfusion, aid_src_status.innovation_variance, aid_src_status.innovation)) { _time_last_heading_fuse = _imu_sample_delayed.time_us; @@ -353,7 +351,7 @@ bool Ekf::fuseDeclination(float decl_sigma) // Calculate the Kalman gains Vector24f Kfusion = P * Hfusion / innovation_variance; - const bool is_fused = measurementUpdate(Kfusion, Hfusion, innovation); + const bool is_fused = measurementUpdate(Kfusion, innovation_variance, innovation); _fault_status.flags.bad_mag_decl = !is_fused; diff --git a/src/modules/ekf2/EKF/optflow_fusion.cpp b/src/modules/ekf2/EKF/optflow_fusion.cpp index 07696ee0a4..402bfa3647 100644 --- a/src/modules/ekf2/EKF/optflow_fusion.cpp +++ b/src/modules/ekf2/EKF/optflow_fusion.cpp @@ -142,7 +142,7 @@ void Ekf::fuseOptFlow() SparseVector24f<0,1,2,3,4,5,6> Hfusion(H); Vector24f Kfusion = P * Hfusion / _aid_src_optical_flow.innovation_variance[index]; - if (measurementUpdate(Kfusion, Hfusion, _aid_src_optical_flow.innovation[index])) { + if (measurementUpdate(Kfusion, _aid_src_optical_flow.innovation_variance[index], _aid_src_optical_flow.innovation[index])) { fused[index] = true; } } diff --git a/src/modules/ekf2/EKF/sideslip_fusion.cpp b/src/modules/ekf2/EKF/sideslip_fusion.cpp index fae55e0c4a..650ca36f75 100644 --- a/src/modules/ekf2/EKF/sideslip_fusion.cpp +++ b/src/modules/ekf2/EKF/sideslip_fusion.cpp @@ -109,15 +109,13 @@ void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip) sym::ComputeSideslipHAndK(getStateAtFusionHorizonAsVector(), P, sideslip.innovation_variance, FLT_EPSILON, &H, &K); - SparseVector24f<0,1,2,3,4,5,6,22,23> H_sparse(H); - if (update_wind_only) { for (unsigned row = 0; row <= 21; row++) { K(row) = 0.f; } } - const bool is_fused = measurementUpdate(K, H_sparse, sideslip.innovation); + const bool is_fused = measurementUpdate(K, sideslip.innovation_variance, sideslip.innovation); sideslip.fused = is_fused; _fault_status.flags.bad_sideslip = !is_fused; diff --git a/src/modules/ekf2/test/change_indication/iris_gps.csv b/src/modules/ekf2/test/change_indication/iris_gps.csv index fd6ef9ea69..4fbc0f45e6 100644 --- a/src/modules/ekf2/test/change_indication/iris_gps.csv +++ b/src/modules/ekf2/test/change_indication/iris_gps.csv @@ -306,9 +306,9 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7 30390000,0.98,-0.0061,-0.013,0.19,-0.055,0.044,0.76,-0.066,0.049,-1.3,-1.4e-05,-5.7e-05,1.7e-06,0.00021,-0.00012,-0.0011,0.2,0.002,0.43,0,0,0,0,0,1.7e-06,8.2e-05,8.1e-05,4.8e-05,0.013,0.013,0.0079,0.037,0.037,0.035,3.1e-11,3.1e-11,1.2e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 30490000,0.98,-0.0061,-0.013,0.18,-0.057,0.044,0.77,-0.072,0.054,-1.2,-1.4e-05,-5.7e-05,1.8e-06,0.00021,-0.00012,-0.0011,0.2,0.002,0.43,0,0,0,0,0,1.7e-06,8.3e-05,8.1e-05,4.8e-05,0.014,0.014,0.008,0.041,0.041,0.036,3.1e-11,3.1e-11,1.2e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 30590000,0.98,-0.0065,-0.014,0.18,-0.053,0.041,0.76,-0.065,0.05,-1.2,-1.4e-05,-5.7e-05,1.9e-06,0.00021,-0.00013,-0.0011,0.2,0.002,0.43,0,0,0,0,0,1.7e-06,8.2e-05,8.1e-05,4.8e-05,0.013,0.013,0.008,0.037,0.037,0.035,3e-11,3e-11,1.1e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -30690000,0.98,-0.0068,-0.014,0.18,-0.051,0.04,0.76,-0.07,0.054,-1.1,-1.4e-05,-5.7e-05,1.9e-06,0.00021,-0.00013,-0.0011,0.2,0.002,0.43,0,0,0,0,0,1.7e-06,8.2e-05,8.1e-05,4.7e-05,0.013,0.013,0.008,0.041,0.041,0.035,3e-11,3e-11,1.1e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 +30690000,0.98,-0.0068,-0.014,0.18,-0.051,0.04,0.76,-0.07,0.054,-1.1,-1.4e-05,-5.7e-05,1.9e-06,0.00022,-0.00013,-0.0011,0.2,0.002,0.43,0,0,0,0,0,1.7e-06,8.2e-05,8.1e-05,4.7e-05,0.013,0.013,0.008,0.041,0.041,0.035,3e-11,3e-11,1.1e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 30790000,0.98,-0.0065,-0.013,0.18,-0.044,0.035,0.76,-0.063,0.052,-1,-1.4e-05,-5.7e-05,1.9e-06,0.00022,-0.00013,-0.0011,0.2,0.002,0.43,0,0,0,0,0,1.7e-06,8.2e-05,8.1e-05,4.7e-05,0.013,0.013,0.008,0.037,0.037,0.035,3e-11,3e-11,1.1e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -30890000,0.98,-0.0059,-0.013,0.18,-0.044,0.032,0.76,-0.067,0.056,-0.95,-1.4e-05,-5.7e-05,1.8e-06,0.00022,-0.00013,-0.0011,0.2,0.002,0.43,0,0,0,0,0,1.7e-06,8.2e-05,8.1e-05,4.7e-05,0.013,0.013,0.008,0.041,0.041,0.035,3e-11,3e-11,1.1e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 +30890000,0.98,-0.0059,-0.013,0.18,-0.044,0.032,0.76,-0.067,0.056,-0.95,-1.4e-05,-5.7e-05,1.8e-06,0.00022,-0.00013,-0.0011,0.2,0.002,0.43,0,0,0,0,0,1.7e-06,8.2e-05,8.1e-05,4.7e-05,0.013,0.013,0.008,0.041,0.04,0.035,3e-11,3e-11,1.1e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 30990000,0.98,-0.0061,-0.013,0.18,-0.037,0.026,0.76,-0.057,0.049,-0.88,-1.4e-05,-5.7e-05,1.8e-06,0.00022,-0.00014,-0.0011,0.2,0.002,0.43,0,0,0,0,0,1.7e-06,8.2e-05,8.1e-05,4.7e-05,0.013,0.013,0.0079,0.037,0.037,0.035,3e-11,3e-11,1.1e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 31090000,0.98,-0.0062,-0.013,0.18,-0.035,0.025,0.76,-0.061,0.051,-0.81,-1.4e-05,-5.7e-05,1.8e-06,0.00022,-0.00014,-0.0011,0.2,0.002,0.43,0,0,0,0,0,1.7e-06,8.2e-05,8.1e-05,4.7e-05,0.013,0.013,0.008,0.04,0.04,0.036,3e-11,3e-11,1.1e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 31190000,0.98,-0.0064,-0.013,0.18,-0.031,0.021,0.76,-0.052,0.046,-0.74,-1.4e-05,-5.7e-05,1.9e-06,0.00023,-0.00014,-0.0011,0.2,0.002,0.43,0,0,0,0,0,1.7e-06,8.2e-05,8.1e-05,4.7e-05,0.013,0.013,0.008,0.037,0.037,0.035,2.9e-11,2.9e-11,1.1e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0