forked from Archive/PX4-Autopilot
ekf2: auto-generate state vector size constant
This commit is contained in:
parent
8a9a303354
commit
dac337efc4
|
@ -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;
|
||||
|
|
|
@ -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, _k_num_states> Vector24f;
|
||||
typedef matrix::SquareMatrix<float, _k_num_states> SquareMatrix24f;
|
||||
typedef matrix::Vector<float, State::size> Vector24f;
|
||||
typedef matrix::SquareMatrix<float, State::size> 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 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);
|
||||
|
|
|
@ -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")
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
@ -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();
|
||||
|
|
Loading…
Reference in New Issue