From ebf962bf68084fabd8de74e218621f64c21b685c Mon Sep 17 00:00:00 2001 From: bresch <[brescianimathieu@gmail.com](mailto:brescianimathieu@gmail.com)> Date: Wed, 20 Sep 2023 10:27:21 +0200 Subject: [PATCH] ekf2: remove size in name of state vector and matrix types Then the state vector size can be changes without having to update the name --- src/modules/ekf2/EKF/airspeed_fusion.cpp | 4 +-- src/modules/ekf2/EKF/covariance.cpp | 4 +-- src/modules/ekf2/EKF/drag_fusion.cpp | 4 +-- src/modules/ekf2/EKF/ekf.h | 26 +++++++++---------- src/modules/ekf2/EKF/ekf_helper.cpp | 4 +-- src/modules/ekf2/EKF/gps_yaw_fusion.cpp | 8 +++--- src/modules/ekf2/EKF/gravity_fusion.cpp | 2 +- src/modules/ekf2/EKF/mag_fusion.cpp | 14 +++++----- src/modules/ekf2/EKF/mag_heading_control.cpp | 2 +- src/modules/ekf2/EKF/optflow_fusion.cpp | 12 ++++----- src/modules/ekf2/EKF/sideslip_fusion.cpp | 4 +-- src/modules/ekf2/EKF/vel_pos_fusion.cpp | 4 +-- src/modules/ekf2/EKF/yaw_fusion.cpp | 8 +++--- src/modules/ekf2/EKF/zero_gyro_update.cpp | 2 +- .../EKF/zero_innovation_heading_update.cpp | 2 +- 15 files changed, 50 insertions(+), 50 deletions(-) diff --git a/src/modules/ekf2/EKF/airspeed_fusion.cpp b/src/modules/ekf2/EKF/airspeed_fusion.cpp index 7e2bec4d9b..8a8057e6f1 100644 --- a/src/modules/ekf2/EKF/airspeed_fusion.cpp +++ b/src/modules/ekf2/EKF/airspeed_fusion.cpp @@ -194,8 +194,8 @@ void Ekf::fuseAirspeed(const airspeedSample &airspeed_sample, estimator_aid_sour _fault_status.flags.bad_airspeed = false; - Vector24f H; // Observation jacobian - Vector24f K; // Kalman gain vector + VectorState H; // Observation jacobian + VectorState K; // Kalman gain vector sym::ComputeAirspeedHAndK(getStateAtFusionHorizonAsVector(), P, innov_var, FLT_EPSILON, &H, &K); diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index 353d13dddc..32cfe7fa83 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -227,7 +227,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) } // predict the covariance - SquareMatrix24f nextP; + SquareMatrixState nextP; // calculate variances and upper diagonal covariances for quaternion, velocity, position and gyro bias states sym::PredictCovariance(getStateAtFusionHorizonAsVector(), P, @@ -511,7 +511,7 @@ void Ekf::constrainStateVar(const IdxDof &state, float min, float max) // if the covariance correction will result in a negative variance, then // the covariance matrix is unhealthy and must be corrected -bool Ekf::checkAndFixCovarianceUpdate(const SquareMatrix24f &KHP) +bool Ekf::checkAndFixCovarianceUpdate(const SquareMatrixState &KHP) { bool healthy = true; diff --git a/src/modules/ekf2/EKF/drag_fusion.cpp b/src/modules/ekf2/EKF/drag_fusion.cpp index ce7730d938..a11a599be7 100644 --- a/src/modules/ekf2/EKF/drag_fusion.cpp +++ b/src/modules/ekf2/EKF/drag_fusion.cpp @@ -85,7 +85,7 @@ void Ekf::fuseDrag(const dragSample &drag_sample) _state.vel(2)); const Vector3f rel_wind_body = _state.quat_nominal.rotateVectorInverse(rel_wind_earth); const float rel_wind_speed = rel_wind_body.norm(); - const Vector24f state_vector_prev = getStateAtFusionHorizonAsVector(); + const VectorState state_vector_prev = getStateAtFusionHorizonAsVector(); Vector2f bcoef_inv; @@ -105,7 +105,7 @@ void Ekf::fuseDrag(const dragSample &drag_sample) bcoef_inv(1) = bcoef_inv(0); } - Vector24f Kfusion; + VectorState Kfusion; // perform sequential fusion of XY specific forces for (uint8_t axis_index = 0; axis_index < 2; axis_index++) { diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 28ceeb25a7..79fb7f5207 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -60,12 +60,12 @@ enum class Likelihood { LOW, MEDIUM, HIGH }; class Ekf final : public EstimatorInterface { public: - typedef matrix::Vector Vector24f; - typedef matrix::SquareMatrix SquareMatrix24f; + typedef matrix::Vector VectorState; + typedef matrix::SquareMatrix SquareMatrixState; typedef matrix::SquareMatrix Matrix2f; template - using SparseVector24f = matrix::SparseVectorf; + using SparseVectorState = matrix::SparseVectorf; Ekf() { @@ -304,7 +304,7 @@ public: void getGravityInnovRatio(float &grav_innov_ratio) const { grav_innov_ratio = Vector3f(_aid_src_gravity.test_ratio).max(); } // get the state vector at the delayed time horizon - matrix::Vector getStateAtFusionHorizonAsVector() const; + matrix::Vector getStateAtFusionHorizonAsVector() const; // get the wind velocity in m/s const Vector2f &getWindVelocity() const { return _state.wind_vel; }; @@ -624,7 +624,7 @@ private: float _yaw_delta_ef{0.0f}; ///< Recent change in yaw angle measured about the earth frame D axis (rad) float _yaw_rate_lpf_ef{0.0f}; ///< Filtered angular rate about earth frame D axis (rad/sec) - SquareMatrix24f P{}; ///< state covariance matrix + SquareMatrixState P{}; ///< state covariance matrix #if defined(CONFIG_EKF2_DRAG_FUSION) Vector2f _drag_innov{}; ///< multirotor drag measurement innovation (m/sec**2) @@ -807,8 +807,8 @@ private: // update quaternion states and covariances using an innovation, observation variance and Jacobian vector bool fuseYaw(estimator_aid_source1d_s &aid_src_status); - bool fuseYaw(estimator_aid_source1d_s &aid_src_status, const Vector24f &H_YAW); - void computeYawInnovVarAndH(float variance, float &innovation_variance, Vector24f &H_YAW) const; + bool fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H_YAW); + void computeYawInnovVarAndH(float variance, float &innovation_variance, VectorState &H_YAW) const; #if defined(CONFIG_EKF2_GNSS_YAW) void controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passing, bool gps_checks_failing); @@ -972,7 +972,7 @@ private: float getMagDeclination(); #endif // CONFIG_EKF2_MAGNETOMETER - void clearInhibitedStateKalmanGains(Vector24f &K) const + void clearInhibitedStateKalmanGains(VectorState &K) const { // gyro bias: states 10, 11, 12 for (unsigned i = 0; i < 3; i++) { @@ -1009,12 +1009,12 @@ private: } } - bool measurementUpdate(Vector24f &K, float innovation_variance, float innovation) + bool measurementUpdate(VectorState &K, float innovation_variance, float innovation) { clearInhibitedStateKalmanGains(K); - const Vector24f KS = K * innovation_variance; - SquareMatrix24f KHP; + const VectorState KS = K * innovation_variance; + SquareMatrixState KHP; for (unsigned row = 0; row < State::size; row++) { for (unsigned col = 0; col < State::size; col++) { @@ -1041,7 +1041,7 @@ private: // if the covariance correction will result in a negative variance, then // the covariance matrix is unhealthy and must be corrected - bool checkAndFixCovarianceUpdate(const SquareMatrix24f &KHP); + bool checkAndFixCovarianceUpdate(const SquareMatrixState &KHP); // limit the diagonal of the covariance matrix // force symmetry when the argument is true @@ -1054,7 +1054,7 @@ private: // generic function which will perform a fusion step given a kalman gain K // and a scalar innovation value - void fuse(const Vector24f &K, float innovation); + void fuse(const VectorState &K, float innovation); #if defined(CONFIG_EKF2_BARO_COMPENSATION) float compensateBaroForDynamicPressure(float baro_alt_uncompensated) const; diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index b12527447e..42ec11c4f0 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -810,7 +810,7 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const *status = soln_status.value; } -void Ekf::fuse(const Vector24f &K, float innovation) +void Ekf::fuse(const VectorState &K, float innovation) { _state.quat_nominal -= K.slice(State::quat_nominal.idx, 0) * innovation; _state.quat_nominal.normalize(); @@ -909,7 +909,7 @@ Vector3f Ekf::calcRotVecVariances() const float Ekf::getYawVar() const { - Vector24f H_YAW; + VectorState H_YAW; float yaw_var = 0.f; computeYawInnovVarAndH(0.f, yaw_var, H_YAW); diff --git a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp index f5747e7ea2..9f01d2f380 100644 --- a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp +++ b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp @@ -64,7 +64,7 @@ void Ekf::updateGpsYaw(const gpsSample &gps_sample) float heading_innov_var; { - Vector24f H; + VectorState H; sym::ComputeGnssYawPredInnovVarAndH(getStateAtFusionHorizonAsVector(), P, _gps_yaw_offset, R_YAW, FLT_EPSILON, &heading_pred, &heading_innov_var, &H); } @@ -91,7 +91,7 @@ void Ekf::fuseGpsYaw() return; } - Vector24f H; + VectorState H; { float heading_pred; @@ -102,7 +102,7 @@ void Ekf::fuseGpsYaw() sym::ComputeGnssYawPredInnovVarAndH(getStateAtFusionHorizonAsVector(), P, _gps_yaw_offset, gnss_yaw.observation_variance, FLT_EPSILON, &heading_pred, &heading_innov_var, &H); } - const SparseVector24f<0,1,2,3> Hfusion(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) { @@ -131,7 +131,7 @@ void Ekf::fuseGpsYaw() // calculate the Kalman gains // only calculate gains for states we are using - Vector24f Kfusion = P * Hfusion / gnss_yaw.innovation_variance; + VectorState Kfusion = P * Hfusion / 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/gravity_fusion.cpp b/src/modules/ekf2/EKF/gravity_fusion.cpp index 57fe147658..635b53e9f5 100644 --- a/src/modules/ekf2/EKF/gravity_fusion.cpp +++ b/src/modules/ekf2/EKF/gravity_fusion.cpp @@ -60,7 +60,7 @@ void Ekf::controlGravityFusion(const imuSample &imu) // calculate kalman gains and innovation variances Vector3f innovation; // innovation of the last gravity fusion observation (m/s**2) Vector3f innovation_variance; - Vector24f Kx, Ky, Kz; // Kalman gain vectors + VectorState Kx, Ky, Kz; // Kalman gain vectors sym::ComputeGravityInnovVarAndKAndH( getStateAtFusionHorizonAsVector(), P, measurement, measurement_var, FLT_EPSILON, &innovation, &innovation_variance, &Kx, &Ky, &Kz); diff --git a/src/modules/ekf2/EKF/mag_fusion.cpp b/src/modules/ekf2/EKF/mag_fusion.cpp index abb6f69cce..3ca71359a4 100644 --- a/src/modules/ekf2/EKF/mag_fusion.cpp +++ b/src/modules/ekf2/EKF/mag_fusion.cpp @@ -62,9 +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 - SparseVector24f<0,1,2,3,16,17,18,19,20,21> Hfusion; - Vector24f H; - const Vector24f state_vector = getStateAtFusionHorizonAsVector(); + 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; @@ -214,7 +214,7 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo } } - Vector24f Kfusion = P * Hfusion / aid_src_mag.innovation_variance[index]; + VectorState Kfusion = P * Hfusion / aid_src_mag.innovation_variance[index]; if (!update_all_states) { for (unsigned row = 0; row <= 15; row++) { @@ -263,7 +263,7 @@ bool Ekf::fuseDeclination(float decl_sigma) // observation variance (rad**2) const float R_DECL = sq(decl_sigma); - Vector24f H; + VectorState H; float decl_pred; float innovation_variance; @@ -277,10 +277,10 @@ bool Ekf::fuseDeclination(float decl_sigma) return false; } - SparseVector24f<16,17> Hfusion(H); + SparseVectorState<16,17> Hfusion(H); // Calculate the Kalman gains - Vector24f Kfusion = P * Hfusion / innovation_variance; + VectorState Kfusion = P * Hfusion / innovation_variance; const bool is_fused = measurementUpdate(Kfusion, innovation_variance, innovation); diff --git a/src/modules/ekf2/EKF/mag_heading_control.cpp b/src/modules/ekf2/EKF/mag_heading_control.cpp index b54a879f0e..dab53aeb0e 100644 --- a/src/modules/ekf2/EKF/mag_heading_control.cpp +++ b/src/modules/ekf2/EKF/mag_heading_control.cpp @@ -119,7 +119,7 @@ void Ekf::controlMagHeadingFusion(const magSample &mag_sample, const bool common aid_src.time_last_fuse = _time_delayed_us; } else { - Vector24f H_YAW; + VectorState H_YAW; computeYawInnovVarAndH(aid_src.observation_variance, aid_src.innovation_variance, H_YAW); if ((aid_src.innovation_variance - aid_src.observation_variance) > sq(_params.mag_heading_noise / 2.f)) { diff --git a/src/modules/ekf2/EKF/optflow_fusion.cpp b/src/modules/ekf2/EKF/optflow_fusion.cpp index 484b3213f8..2db9f1c8d7 100644 --- a/src/modules/ekf2/EKF/optflow_fusion.cpp +++ b/src/modules/ekf2/EKF/optflow_fusion.cpp @@ -77,10 +77,10 @@ void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src) aid_src.observation_variance[0] = R_LOS; aid_src.observation_variance[1] = R_LOS; - const Vector24f state_vector = getStateAtFusionHorizonAsVector(); + const VectorState state_vector = getStateAtFusionHorizonAsVector(); Vector2f innov_var; - Vector24f H; + VectorState H; sym::ComputeFlowXyInnovVarAndHx(state_vector, P, range, R_LOS, FLT_EPSILON, &innov_var, &H); innov_var.copyTo(aid_src.innovation_variance); @@ -98,10 +98,10 @@ void Ekf::fuseOptFlow() // a positive offset in earth frame leads to a smaller height above the ground. float range = predictFlowRange(); - const Vector24f state_vector = getStateAtFusionHorizonAsVector(); + const VectorState state_vector = getStateAtFusionHorizonAsVector(); Vector2f innov_var; - Vector24f H; + VectorState H; sym::ComputeFlowXyInnovVarAndHx(state_vector, P, range, R_LOS, FLT_EPSILON, &innov_var, &H); innov_var.copyTo(_aid_src_optical_flow.innovation_variance); @@ -148,8 +148,8 @@ void Ekf::fuseOptFlow() } } - SparseVector24f<0,1,2,3,4,5,6> Hfusion(H); - Vector24f Kfusion = P * Hfusion / _aid_src_optical_flow.innovation_variance[index]; + SparseVectorState<0,1,2,3,4,5,6> Hfusion(H); + VectorState Kfusion = P * Hfusion / _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; diff --git a/src/modules/ekf2/EKF/sideslip_fusion.cpp b/src/modules/ekf2/EKF/sideslip_fusion.cpp index 0c83901945..439fc942ff 100644 --- a/src/modules/ekf2/EKF/sideslip_fusion.cpp +++ b/src/modules/ekf2/EKF/sideslip_fusion.cpp @@ -135,8 +135,8 @@ void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip) _fault_status.flags.bad_sideslip = false; - Vector24f H; // Observation jacobian - Vector24f K; // Kalman gain vector + VectorState H; // Observation jacobian + VectorState K; // Kalman gain vector sym::ComputeSideslipHAndK(getStateAtFusionHorizonAsVector(), P, sideslip.innovation_variance, FLT_EPSILON, &H, &K); diff --git a/src/modules/ekf2/EKF/vel_pos_fusion.cpp b/src/modules/ekf2/EKF/vel_pos_fusion.cpp index 739f392a68..071f01ed5e 100644 --- a/src/modules/ekf2/EKF/vel_pos_fusion.cpp +++ b/src/modules/ekf2/EKF/vel_pos_fusion.cpp @@ -195,7 +195,7 @@ void Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src) // Helper function that fuses a single velocity or position measurement bool Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int state_index) { - Vector24f Kfusion; // Kalman gain vector for any single observation - sequential fusion is used. + VectorState Kfusion; // Kalman gain vector for any single observation - sequential fusion is used. // calculate kalman gain K = PHS, where S = 1/innovation variance for (int row = 0; row < State::size; row++) { @@ -204,7 +204,7 @@ bool Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int s clearInhibitedStateKalmanGains(Kfusion); - SquareMatrix24f KHP; + SquareMatrixState KHP; for (unsigned row = 0; row < State::size; row++) { for (unsigned column = 0; column < State::size; column++) { diff --git a/src/modules/ekf2/EKF/yaw_fusion.cpp b/src/modules/ekf2/EKF/yaw_fusion.cpp index 91c888df74..d6eec32a60 100644 --- a/src/modules/ekf2/EKF/yaw_fusion.cpp +++ b/src/modules/ekf2/EKF/yaw_fusion.cpp @@ -41,13 +41,13 @@ // update quaternion states and covariances using the yaw innovation and yaw observation variance bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status) { - Vector24f H_YAW; + VectorState H_YAW; computeYawInnovVarAndH(aid_src_status.observation_variance, aid_src_status.innovation_variance, H_YAW); return fuseYaw(aid_src_status, H_YAW); } -bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const Vector24f &H_YAW) +bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H_YAW) { // define the innovation gate size float gate_sigma = math::max(_params.heading_innov_gate, 1.f); @@ -75,7 +75,7 @@ bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const Vector24f &H_Y // calculate the Kalman gains // only calculate gains for states we are using - Vector24f Kfusion; + VectorState Kfusion; const float heading_innov_var_inv = 1.f / aid_src_status.innovation_variance; for (uint8_t row = 0; row < State::size; row++) { @@ -136,7 +136,7 @@ bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const Vector24f &H_Y return false; } -void Ekf::computeYawInnovVarAndH(float variance, float &innovation_variance, Vector24f &H_YAW) const +void Ekf::computeYawInnovVarAndH(float variance, float &innovation_variance, VectorState &H_YAW) const { if (shouldUse321RotationSequence(_R_to_earth)) { sym::ComputeYaw321InnovVarAndH(getStateAtFusionHorizonAsVector(), P, variance, FLT_EPSILON, &innovation_variance, &H_YAW); diff --git a/src/modules/ekf2/EKF/zero_gyro_update.cpp b/src/modules/ekf2/EKF/zero_gyro_update.cpp index 74b9dec0c3..1221b79c4e 100644 --- a/src/modules/ekf2/EKF/zero_gyro_update.cpp +++ b/src/modules/ekf2/EKF/zero_gyro_update.cpp @@ -82,7 +82,7 @@ void Ekf::controlZeroGyroUpdate(const imuSample &imu_delayed) void Ekf::fuseDeltaAngBias(const float innov, const float innov_var, const int obs_index) { - Vector24f K; // Kalman gain vector for any single observation - sequential fusion is used. + VectorState K; // Kalman gain vector for any single observation - sequential fusion is used. const unsigned state_index = obs_index + 10; // calculate kalman gain K = PHS, where S = 1/innovation variance diff --git a/src/modules/ekf2/EKF/zero_innovation_heading_update.cpp b/src/modules/ekf2/EKF/zero_innovation_heading_update.cpp index 16cc545819..9bb0b61a8d 100644 --- a/src/modules/ekf2/EKF/zero_innovation_heading_update.cpp +++ b/src/modules/ekf2/EKF/zero_innovation_heading_update.cpp @@ -56,7 +56,7 @@ void Ekf::controlZeroInnovationHeadingUpdate() aid_src_status.observation_variance = obs_var; aid_src_status.innovation = 0.f; - Vector24f H_YAW; + VectorState H_YAW; computeYawInnovVarAndH(obs_var, aid_src_status.innovation_variance, H_YAW);