From 552bf824ea3cb5c130e02c37d48ec77e77964096 Mon Sep 17 00:00:00 2001 From: kamilritz Date: Sat, 27 Jun 2020 08:44:32 +0200 Subject: [PATCH] Add typedef for Matrix 24 types --- EKF/airspeed_fusion.cpp | 2 +- EKF/covariance.cpp | 4 ++-- EKF/drag_fusion.cpp | 2 +- EKF/ekf.h | 8 ++++---- EKF/gps_yaw_fusion.cpp | 2 +- EKF/mag_fusion.cpp | 6 +++--- EKF/optflow_fusion.cpp | 2 +- EKF/sideslip_fusion.cpp | 2 +- EKF/vel_pos_fusion.cpp | 2 +- 9 files changed, 15 insertions(+), 15 deletions(-) diff --git a/EKF/airspeed_fusion.cpp b/EKF/airspeed_fusion.cpp index bafdff4819..07b9c395b6 100644 --- a/EKF/airspeed_fusion.cpp +++ b/EKF/airspeed_fusion.cpp @@ -172,7 +172,7 @@ void Ekf::fuseAirspeed() // apply covariance correction via P_new = (I -K*H)*P // first calculate expression for KHP // then calculate P - KHP - matrix::SquareMatrix KHP; + SquareMatrix24f KHP; float KH[5]; for (unsigned row = 0; row < _k_num_states; row++) { diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index a13eddeefb..9a479bfa38 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -218,7 +218,7 @@ void Ekf::predictCovariance() } // compute noise variance for stationary processes - matrix::Vector process_noise; + Vector24f process_noise; // Construct the process noise variance diagonal for those states with a stationary process model // These are kinematic states and their error growth is controlled separately by the IMU noise variances @@ -328,7 +328,7 @@ void Ekf::predictCovariance() SPP[10] = SF[16]; // covariance update - matrix::SquareMatrix nextP; + SquareMatrix24f nextP; // calculate variances and upper diagonal covariances for quaternion, velocity, position and gyro bias states nextP(0,0) = P(0,0) + P(1,0)*SF[9] + P(2,0)*SF[11] + P(3,0)*SF[10] + P(10,0)*SF[14] + P(11,0)*SF[15] + P(12,0)*SPP[10] + (daxVar*SQ[10])/4 + SF[9]*(P(0,1) + P(1,1)*SF[9] + P(2,1)*SF[11] + P(3,1)*SF[10] + P(10,1)*SF[14] + P(11,1)*SF[15] + P(12,1)*SPP[10]) + SF[11]*(P(0,2) + P(1,2)*SF[9] + P(2,2)*SF[11] + P(3,2)*SF[10] + P(10,2)*SF[14] + P(11,2)*SF[15] + P(12,2)*SPP[10]) + SF[10]*(P(0,3) + P(1,3)*SF[9] + P(2,3)*SF[11] + P(3,3)*SF[10] + P(10,3)*SF[14] + P(11,3)*SF[15] + P(12,3)*SPP[10]) + SF[14]*(P(0,10) + P(1,10)*SF[9] + P(2,10)*SF[11] + P(3,10)*SF[10] + P(10,10)*SF[14] + P(11,10)*SF[15] + P(12,10)*SPP[10]) + SF[15]*(P(0,11) + P(1,11)*SF[9] + P(2,11)*SF[11] + P(3,11)*SF[10] + P(10,11)*SF[14] + P(11,11)*SF[15] + P(12,11)*SPP[10]) + SPP[10]*(P(0,12) + P(1,12)*SF[9] + P(2,12)*SF[11] + P(3,12)*SF[10] + P(10,12)*SF[14] + P(11,12)*SF[15] + P(12,12)*SPP[10]) + (dayVar*sq(q2))/4 + (dazVar*sq(q3))/4; diff --git a/EKF/drag_fusion.cpp b/EKF/drag_fusion.cpp index ac77da246c..dbe152b4b7 100644 --- a/EKF/drag_fusion.cpp +++ b/EKF/drag_fusion.cpp @@ -241,7 +241,7 @@ void Ekf::fuseDrag() // apply covariance correction via P_new = (I -K*H)*P // first calculate expression for KHP // then calculate P - KHP - matrix::SquareMatrix KHP; + SquareMatrix24f KHP; float KH[9]; for (unsigned row = 0; row < _k_num_states; row++) { diff --git a/EKF/ekf.h b/EKF/ekf.h index c495f4034c..55c0484cc4 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -47,6 +47,9 @@ class Ekf : public EstimatorInterface { public: + static constexpr uint8_t _k_num_states{24}; ///< number of EKF states + typedef matrix::Vector Vector24f; + typedef matrix::SquareMatrix SquareMatrix24f; Ekf() = default; virtual ~Ekf() = default; @@ -297,9 +300,6 @@ public: void requestEmergencyNavReset() override; private: - - static constexpr uint8_t _k_num_states{24}; ///< number of EKF states - struct { uint8_t velNE_counter; ///< number of horizontal position reset events (allow to wrap if count exceeds 255) uint8_t velD_counter; ///< number of vertical velocity reset events (allow to wrap if count exceeds 255) @@ -382,7 +382,7 @@ private: bool _yaw_use_inhibit{false}; ///< true when yaw sensor use is being inhibited - matrix::SquareMatrix P; ///< state covariance matrix + SquareMatrix24f P; ///< state covariance matrix Vector3f _delta_vel_bias_var_accum; ///< kahan summation algorithm accumulator for delta velocity bias variance Vector3f _delta_angle_bias_var_accum; ///< kahan summation algorithm accumulator for delta angle bias variance diff --git a/EKF/gps_yaw_fusion.cpp b/EKF/gps_yaw_fusion.cpp index fda1343d81..2252e4abc8 100644 --- a/EKF/gps_yaw_fusion.cpp +++ b/EKF/gps_yaw_fusion.cpp @@ -218,7 +218,7 @@ void Ekf::fuseGpsAntYaw() // apply covariance correction via P_new = (I -K*H)*P // first calculate expression for KHP // then calculate P - KHP - matrix::SquareMatrix KHP; + SquareMatrix24f KHP; float KH[4]; for (unsigned row = 0; row < _k_num_states; row++) { diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 5534100e71..69873bd043 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -355,7 +355,7 @@ void Ekf::fuseMag() // apply covariance correction via P_new = (I -K*H)*P // first calculate expression for KHP // then calculate P - KHP - matrix::SquareMatrix KHP {}; + SquareMatrix24f KHP; float KH[10]; for (unsigned row = 0; row < _k_num_states; row++) { @@ -705,7 +705,7 @@ void Ekf::updateQuaternion(const float innovation, const float variance, const f // apply covariance correction via P_new = (I -K*H)*P // first calculate expression for KHP // then calculate P - KHP - matrix::SquareMatrix KHP; + SquareMatrix24f KHP; float KH[4]; for (unsigned row = 0; row < _k_num_states; row++) { @@ -995,7 +995,7 @@ void Ekf::fuseDeclination(float decl_sigma) // first calculate expression for KHP // then calculate P - KHP // take advantage of the empty columns in KH to reduce the number of operations - matrix::SquareMatrix KHP; + SquareMatrix24f KHP; float KH[2]; for (unsigned row = 0; row < _k_num_states; row++) { diff --git a/EKF/optflow_fusion.cpp b/EKF/optflow_fusion.cpp index 757dbe52d3..b411f0f22a 100644 --- a/EKF/optflow_fusion.cpp +++ b/EKF/optflow_fusion.cpp @@ -440,7 +440,7 @@ void Ekf::fuseOptFlow() // apply covariance correction via P_new = (I -K*H)*P // first calculate expression for KHP // then calculate P - KHP - matrix::SquareMatrix KHP; + SquareMatrix24f KHP; float KH[7]; for (unsigned row = 0; row < _k_num_states; row++) { diff --git a/EKF/sideslip_fusion.cpp b/EKF/sideslip_fusion.cpp index 5526e59d2d..099f7db15a 100644 --- a/EKF/sideslip_fusion.cpp +++ b/EKF/sideslip_fusion.cpp @@ -214,7 +214,7 @@ void Ekf::fuseSideslip() // apply covariance correction via P_new = (I -K*H)*P // first calculate expression for KHP // then calculate P - KHP - matrix::SquareMatrix KHP; + SquareMatrix24f KHP; float KH[9]; for (unsigned row = 0; row < _k_num_states; row++) { diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index 9f68e6a01c..5dfac71d72 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -161,7 +161,7 @@ void Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int o Kfusion[row] = P(row, state_index) / innov_var; } - matrix::SquareMatrix KHP; + SquareMatrix24f KHP; for (unsigned row = 0; row < _k_num_states; row++) { for (unsigned column = 0; column < _k_num_states; column++) {