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
|
// keep previous covariance
|
||||||
for (unsigned i = 0; i < State::mag_I.dof; i++) {
|
for (unsigned i = 0; i < State::mag_I.dof; i++) {
|
||||||
unsigned row = State::mag_I.idx + 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);
|
nextP(row, col) = nextP(col, row) = P(row, col);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
for (unsigned i = 0; i < State::mag_B.dof; i++) {
|
for (unsigned i = 0; i < State::mag_B.dof; i++) {
|
||||||
unsigned row = State::mag_B.idx + 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);
|
nextP(row, col) = nextP(col, row) = P(row, col);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -308,14 +308,14 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
|
||||||
// keep previous covariance
|
// keep previous covariance
|
||||||
for (unsigned i = 0; i < State::wind_vel.dof; i++) {
|
for (unsigned i = 0; i < State::wind_vel.dof; i++) {
|
||||||
unsigned row = State::wind_vel.idx + 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);
|
nextP(row, col) = nextP(col, row) = P(row, col);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// covariance matrix is symmetrical, so copy upper half to lower half
|
// 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++) {
|
for (unsigned column = 0 ; column < row; column++) {
|
||||||
P(row, column) = P(column, row) = nextP(column, row);
|
P(row, column) = P(column, row) = nextP(column, row);
|
||||||
}
|
}
|
||||||
|
@ -515,7 +515,7 @@ bool Ekf::checkAndFixCovarianceUpdate(const SquareMatrix24f &KHP)
|
||||||
{
|
{
|
||||||
bool healthy = true;
|
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)) {
|
if (P(i, i) < KHP(i, i)) {
|
||||||
P.uncorrelateCovarianceSetVariance<1>(i, 0.0f);
|
P.uncorrelateCovarianceSetVariance<1>(i, 0.0f);
|
||||||
healthy = false;
|
healthy = false;
|
||||||
|
|
|
@ -60,14 +60,12 @@ enum class Likelihood { LOW, MEDIUM, HIGH };
|
||||||
class Ekf final : public EstimatorInterface
|
class Ekf final : public EstimatorInterface
|
||||||
{
|
{
|
||||||
public:
|
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;
|
typedef matrix::SquareMatrix<float, 2> Matrix2f;
|
||||||
template<int ... Idxs>
|
template<int ... Idxs>
|
||||||
|
|
||||||
using SparseVector24f = matrix::SparseVectorf<24, Idxs...>;
|
using SparseVector24f = matrix::SparseVectorf<State::size, Idxs...>;
|
||||||
|
|
||||||
Ekf()
|
Ekf()
|
||||||
{
|
{
|
||||||
|
@ -82,6 +80,8 @@ public:
|
||||||
// should be called every time new data is pushed into the filter
|
// should be called every time new data is pushed into the filter
|
||||||
bool update();
|
bool update();
|
||||||
|
|
||||||
|
static uint8_t getNumberOfStates() { return State::size; }
|
||||||
|
|
||||||
void getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const;
|
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 getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const;
|
||||||
void getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, 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;
|
const Vector24f KS = K * innovation_variance;
|
||||||
SquareMatrix24f KHP;
|
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
|
// Instad of literally computing KHP, use an equvalent
|
||||||
// equation involving less mathematical operations
|
// equation involving less mathematical operations
|
||||||
KHP(row, col) = KS(row) * K(col);
|
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")
|
f.write(f"\tstatic constexpr IdxDof {state_name}{{{start_index}, {tangent_dim}}};\n")
|
||||||
start_index += tangent_dim
|
start_index += tangent_dim
|
||||||
|
|
||||||
|
f.write(f"\tstatic constexpr uint8_t size{{{start_index}}};\n")
|
||||||
f.write("};\n") # namespace State
|
f.write("};\n") # namespace State
|
||||||
f.write("}\n") # namespace estimator
|
f.write("}\n") # namespace estimator
|
||||||
f.write("#endif // !EKF_STATE_H\n")
|
f.write("#endif // !EKF_STATE_H\n")
|
||||||
|
|
|
@ -17,6 +17,7 @@ namespace State {
|
||||||
static constexpr IdxDof mag_I{16, 3};
|
static constexpr IdxDof mag_I{16, 3};
|
||||||
static constexpr IdxDof mag_B{19, 3};
|
static constexpr IdxDof mag_B{19, 3};
|
||||||
static constexpr IdxDof wind_vel{22, 2};
|
static constexpr IdxDof wind_vel{22, 2};
|
||||||
|
static constexpr uint8_t size{24};
|
||||||
};
|
};
|
||||||
}
|
}
|
||||||
#endif // !EKF_STATE_H
|
#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.
|
Vector24f Kfusion; // Kalman gain vector for any single observation - sequential fusion is used.
|
||||||
|
|
||||||
// calculate kalman gain K = PHS, where S = 1/innovation variance
|
// 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;
|
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;
|
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);
|
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;
|
Vector24f Kfusion;
|
||||||
const float heading_innov_var_inv = 1.f / aid_src_status.innovation_variance;
|
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++) {
|
for (uint8_t col = 0; col <= 3; col++) {
|
||||||
Kfusion(row) += P(row, col) * H_YAW(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;
|
const unsigned state_index = obs_index + 10;
|
||||||
|
|
||||||
// calculate kalman gain K = PHS, where S = 1/innovation variance
|
// 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;
|
K(row) = P(row, state_index) / innov_var;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -1758,7 +1758,7 @@ void EKF2::PublishStates(const hrt_abstime ×tamp)
|
||||||
// publish estimator states
|
// publish estimator states
|
||||||
estimator_states_s states;
|
estimator_states_s states;
|
||||||
states.timestamp_sample = _ekf.time_delayed_us();
|
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.getStateAtFusionHorizonAsVector().copyTo(states.states);
|
||||||
_ekf.covariances_diagonal().copyTo(states.covariances);
|
_ekf.covariances_diagonal().copyTo(states.covariances);
|
||||||
states.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
|
states.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
|
||||||
|
|
Loading…
Reference in New Issue