ekf2: auto-generate state vector size constant

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;

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<float, State::size> Vector24f;
-
+	typedef matrix::SquareMatrix<float, State::size> SquareMatrix24f;
-	typedef matrix::Vector<float, _k_num_states> Vector24f;
-	typedef matrix::SquareMatrix<float, _k_num_states> SquareMatrix24f;
 	typedef matrix::SquareMatrix<float, 2> Matrix2f;
 	template<int ... Idxs>
 
-	using SparseVector24f = matrix::SparseVectorf<24, Idxs...>;
+	using SparseVector24f = matrix::SparseVectorf<State::size, Idxs...>;
 
 	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 row = 0; row < State::size; row++) {
-			for (unsigned col = 0; col < _k_num_states; col++) {
+			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);

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")

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

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 row = 0; row < State::size; row++) {
-		for (unsigned column = 0; column < _k_num_states; column++) {
+		for (unsigned column = 0; column < State::size; column++) {
 			KHP(row, column) = Kfusion(row) * P(state_index, column);
 		}
 	}

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);
 			}

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;
 	}
 

src/modules/ekf2/EKF2.cpp

@@ -1758,7 +1758,7 @@ void EKF2::PublishStates(const hrt_abstime &timestamp)
 	// 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();