diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index a4f04a668c..353d13dddc 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -283,14 +283,14 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) // keep previous covariance for (unsigned i = 0; i < State::mag_I.dof; i++) { unsigned row = State::mag_I.idx + i; - for (unsigned col = 0; col < _k_num_states; col++) { + for (unsigned col = 0; col < State::size; col++) { nextP(row, col) = nextP(col, row) = P(row, col); } } for (unsigned i = 0; i < State::mag_B.dof; i++) { unsigned row = State::mag_B.idx + i; - for (unsigned col = 0; col < _k_num_states; col++) { + for (unsigned col = 0; col < State::size; col++) { nextP(row, col) = nextP(col, row) = P(row, col); } } @@ -308,14 +308,14 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) // keep previous covariance for (unsigned i = 0; i < State::wind_vel.dof; i++) { unsigned row = State::wind_vel.idx + i; - for (unsigned col = 0 ; col < _k_num_states; col++) { + for (unsigned col = 0 ; col < State::size; col++) { nextP(row, col) = nextP(col, row) = P(row, col); } } } // covariance matrix is symmetrical, so copy upper half to lower half - for (unsigned row = 0; row < _k_num_states; row++) { + for (unsigned row = 0; row < State::size; row++) { for (unsigned column = 0 ; column < row; column++) { P(row, column) = P(column, row) = nextP(column, row); } @@ -515,7 +515,7 @@ bool Ekf::checkAndFixCovarianceUpdate(const SquareMatrix24f &KHP) { bool healthy = true; - for (int i = 0; i < _k_num_states; i++) { + for (int i = 0; i < State::size; i++) { if (P(i, i) < KHP(i, i)) { P.uncorrelateCovarianceSetVariance<1>(i, 0.0f); healthy = false; diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 512e4572ba..28ceeb25a7 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -60,14 +60,12 @@ enum class Likelihood { LOW, MEDIUM, HIGH }; class Ekf final : public EstimatorInterface { public: - static constexpr uint8_t _k_num_states{24}; ///< number of EKF states - - typedef matrix::Vector Vector24f; - typedef matrix::SquareMatrix SquareMatrix24f; + typedef matrix::Vector Vector24f; + typedef matrix::SquareMatrix SquareMatrix24f; typedef matrix::SquareMatrix Matrix2f; template - using SparseVector24f = matrix::SparseVectorf<24, Idxs...>; + using SparseVector24f = matrix::SparseVectorf; Ekf() { @@ -82,6 +80,8 @@ public: // should be called every time new data is pushed into the filter bool update(); + static uint8_t getNumberOfStates() { return State::size; } + void getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const; void getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const; void getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const; @@ -1016,8 +1016,8 @@ private: 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++) { + for (unsigned row = 0; row < State::size; row++) { + for (unsigned col = 0; col < State::size; col++) { // Instad of literally computing KHP, use an equvalent // equation involving less mathematical operations KHP(row, col) = KS(row) * K(col); diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation_utils.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation_utils.py index 9213c70f0a..f2b00ac0da 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation_utils.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation_utils.py @@ -110,6 +110,7 @@ def generate_px4_state(states): f.write(f"\tstatic constexpr IdxDof {state_name}{{{start_index}, {tangent_dim}}};\n") start_index += tangent_dim + f.write(f"\tstatic constexpr uint8_t size{{{start_index}}};\n") f.write("};\n") # namespace State f.write("}\n") # namespace estimator f.write("#endif // !EKF_STATE_H\n") diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/state.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/state.h index 5f3a0759f2..964fbcd71f 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/state.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/state.h @@ -17,6 +17,7 @@ namespace State { static constexpr IdxDof mag_I{16, 3}; static constexpr IdxDof mag_B{19, 3}; static constexpr IdxDof wind_vel{22, 2}; + static constexpr uint8_t size{24}; }; } #endif // !EKF_STATE_H diff --git a/src/modules/ekf2/EKF/vel_pos_fusion.cpp b/src/modules/ekf2/EKF/vel_pos_fusion.cpp index e596571bae..739f392a68 100644 --- a/src/modules/ekf2/EKF/vel_pos_fusion.cpp +++ b/src/modules/ekf2/EKF/vel_pos_fusion.cpp @@ -198,7 +198,7 @@ bool Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int s Vector24f 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 < _k_num_states; row++) { + for (int row = 0; row < State::size; row++) { Kfusion(row) = P(row, state_index) / innov_var; } @@ -206,8 +206,8 @@ bool Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int s SquareMatrix24f KHP; - for (unsigned row = 0; row < _k_num_states; row++) { - for (unsigned column = 0; column < _k_num_states; column++) { + for (unsigned row = 0; row < State::size; row++) { + for (unsigned column = 0; column < State::size; column++) { KHP(row, column) = Kfusion(row) * P(state_index, column); } } diff --git a/src/modules/ekf2/EKF/yaw_fusion.cpp b/src/modules/ekf2/EKF/yaw_fusion.cpp index c50a35bf20..91c888df74 100644 --- a/src/modules/ekf2/EKF/yaw_fusion.cpp +++ b/src/modules/ekf2/EKF/yaw_fusion.cpp @@ -78,7 +78,7 @@ bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const Vector24f &H_Y Vector24f Kfusion; const float heading_innov_var_inv = 1.f / aid_src_status.innovation_variance; - for (uint8_t row = 0; row < _k_num_states; row++) { + for (uint8_t row = 0; row < State::size; row++) { for (uint8_t col = 0; col <= 3; col++) { Kfusion(row) += P(row, col) * H_YAW(col); } diff --git a/src/modules/ekf2/EKF/zero_gyro_update.cpp b/src/modules/ekf2/EKF/zero_gyro_update.cpp index 51551fec93..74b9dec0c3 100644 --- a/src/modules/ekf2/EKF/zero_gyro_update.cpp +++ b/src/modules/ekf2/EKF/zero_gyro_update.cpp @@ -86,7 +86,7 @@ void Ekf::fuseDeltaAngBias(const float innov, const float innov_var, const int o const unsigned state_index = obs_index + 10; // calculate kalman gain K = PHS, where S = 1/innovation variance - for (int row = 0; row < _k_num_states; row++) { + for (int row = 0; row < State::size; row++) { K(row) = P(row, state_index) / innov_var; } diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 508614618d..48354773a4 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1758,7 +1758,7 @@ void EKF2::PublishStates(const hrt_abstime ×tamp) // publish estimator states estimator_states_s states; states.timestamp_sample = _ekf.time_delayed_us(); - states.n_states = Ekf::_k_num_states; + states.n_states = Ekf::getNumberOfStates(); _ekf.getStateAtFusionHorizonAsVector().copyTo(states.states); _ekf.covariances_diagonal().copyTo(states.covariances); states.timestamp = _replay_mode ? timestamp : hrt_absolute_time();