From 4a69b410150ff4010ce30010aaeadafa2976f699 Mon Sep 17 00:00:00 2001 From: kamilritz Date: Sun, 21 Jun 2020 13:31:51 +0200 Subject: [PATCH] Increase matrix library usage even more --- EKF/airspeed_fusion.cpp | 2 +- EKF/drag_fusion.cpp | 7 ++----- EKF/gps_yaw_fusion.cpp | 6 +----- EKF/mag_fusion.cpp | 18 +++--------------- EKF/optflow_fusion.cpp | 6 +----- EKF/sideslip_fusion.cpp | 6 +----- EKF/vel_pos_fusion.cpp | 6 +----- 7 files changed, 10 insertions(+), 41 deletions(-) diff --git a/EKF/airspeed_fusion.cpp b/EKF/airspeed_fusion.cpp index 4b63549262..ffc7a04b6d 100644 --- a/EKF/airspeed_fusion.cpp +++ b/EKF/airspeed_fusion.cpp @@ -215,7 +215,7 @@ void Ekf::fuseAirspeed() // only apply covariance and state corrections if healthy if (healthy) { // apply the covariance corrections - P = P - KHP; + P -= KHP; // correct the covariance matrix for gross errors fixCovarianceErrors(true); diff --git a/EKF/drag_fusion.cpp b/EKF/drag_fusion.cpp index 28f1f04fd4..4283420610 100644 --- a/EKF/drag_fusion.cpp +++ b/EKF/drag_fusion.cpp @@ -292,11 +292,8 @@ void Ekf::fuseDrag() // only apply covariance and state corrections if healthy if (healthy) { // apply the covariance corrections - for (unsigned row = 0; row < _k_num_states; row++) { - for (unsigned column = 0; column < _k_num_states; column++) { - P(row,column) = P(row,column) - KHP(row,column); - } - } + P -= KHP; + // correct the covariance matrix for gross errors fixCovarianceErrors(true); diff --git a/EKF/gps_yaw_fusion.cpp b/EKF/gps_yaw_fusion.cpp index 1c92aa2022..6a9a4f183e 100644 --- a/EKF/gps_yaw_fusion.cpp +++ b/EKF/gps_yaw_fusion.cpp @@ -261,11 +261,7 @@ void Ekf::fuseGpsAntYaw() if (healthy) { _time_last_gps_yaw_fuse = _time_last_imu; // apply the covariance corrections - for (unsigned row = 0; row < _k_num_states; row++) { - for (unsigned column = 0; column < _k_num_states; column++) { - P(row,column) = P(row,column) - KHP(row,column); - } - } + P -= KHP; // correct the covariance matrix for gross errors fixCovarianceErrors(true); diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 2cb3210c78..1325bd3f55 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -416,11 +416,7 @@ void Ekf::fuseMag() // only apply covariance and state corrections if healthy if (healthy) { // apply the covariance corrections - for (unsigned row = 0; row < _k_num_states; row++) { - for (unsigned column = 0; column < _k_num_states; column++) { - P(row,column) = P(row,column) - KHP(row,column); - } - } + P -= KHP; // correct the covariance matrix for gross errors fixCovarianceErrors(true); @@ -752,11 +748,7 @@ void Ekf::updateQuaternion(const float innovation, const float variance, const f // only apply covariance and state corrections if healthy if (healthy) { // apply the covariance corrections - for (unsigned row = 0; row < _k_num_states; row++) { - for (unsigned column = 0; column < _k_num_states; column++) { - P(row,column) = P(row,column) - KHP(row,column); - } - } + P -= KHP; // correct the covariance matrix for gross errors fixCovarianceErrors(true); @@ -1045,11 +1037,7 @@ void Ekf::fuseDeclination(float decl_sigma) // only apply covariance and state corrections if healthy if (healthy) { // apply the covariance corrections - for (unsigned row = 0; row < _k_num_states; row++) { - for (unsigned column = 0; column < _k_num_states; column++) { - P(row,column) = P(row,column) - KHP(row,column); - } - } + P -= KHP; // correct the covariance matrix for gross errors fixCovarianceErrors(true); diff --git a/EKF/optflow_fusion.cpp b/EKF/optflow_fusion.cpp index 94b6fd631e..caa9916ac9 100644 --- a/EKF/optflow_fusion.cpp +++ b/EKF/optflow_fusion.cpp @@ -494,11 +494,7 @@ void Ekf::fuseOptFlow() // only apply covariance and state corrections if healthy if (healthy) { // apply the covariance corrections - for (unsigned row = 0; row < _k_num_states; row++) { - for (unsigned column = 0; column < _k_num_states; column++) { - P(row,column) = P(row,column) - KHP(row,column); - } - } + P -= KHP; // correct the covariance matrix for gross errors fixCovarianceErrors(true); diff --git a/EKF/sideslip_fusion.cpp b/EKF/sideslip_fusion.cpp index 5d9f9d01ea..ef5b5c9486 100644 --- a/EKF/sideslip_fusion.cpp +++ b/EKF/sideslip_fusion.cpp @@ -263,11 +263,7 @@ void Ekf::fuseSideslip() // only apply covariance and state corrections if healthy if (healthy) { // apply the covariance corrections - for (unsigned row = 0; row < _k_num_states; row++) { - for (unsigned column = 0; column < _k_num_states; column++) { - P(row,column) = P(row,column) - KHP(row,column); - } - } + P -= KHP; // correct the covariance matrix for gross errors fixCovarianceErrors(true); diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index 7b0ea7b75b..8042e57dff 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -190,11 +190,7 @@ void Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int o // only apply covariance and state corrections if healthy if (healthy) { // apply the covariance corrections - for (unsigned row = 0; row < _k_num_states; row++) { - for (unsigned column = 0; column < _k_num_states; column++) { - P(row, column) = P(row, column) - KHP(row, column); - } - } + P -= KHP; // correct the covariance matrix for gross errors fixCovarianceErrors(true);