fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 16:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: access state covariance using enum

Browse Source
This commit is contained in:
bresch
2023-09-19 09:37:50 -04:00
committed by Daniel Agar
parent 9c41c06325
commit 779ea3f4d1
15 changed files with 256 additions and 181 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf_helper.cpp
+34 -35
View File
@@ -67,11 +67,11 @@ void Ekf::resetHorizontalVelocityTo(const Vector2f &new_horz_vel, const Vector2f
_state.vel.xy() = new_horz_vel;
if (PX4_ISFINITE(new_horz_vel_var(0))) {
P.uncorrelateCovarianceSetVariance<1>(4, math::max(sq(0.01f), new_horz_vel_var(0)));
P.uncorrelateCovarianceSetVariance<1>(State::vel.idx, math::max(sq(0.01f), new_horz_vel_var(0)));
}
if (PX4_ISFINITE(new_horz_vel_var(1))) {
P.uncorrelateCovarianceSetVariance<1>(5, math::max(sq(0.01f), new_horz_vel_var(1)));
P.uncorrelateCovarianceSetVariance<1>(State::vel.idx + 1, math::max(sq(0.01f), new_horz_vel_var(1)));
}
_output_predictor.resetHorizontalVelocityTo(delta_horz_vel);
@@ -97,7 +97,7 @@ void Ekf::resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var)
_state.vel(2) = new_vert_vel;
if (PX4_ISFINITE(new_vert_vel_var)) {
P.uncorrelateCovarianceSetVariance<1>(6, math::max(sq(0.01f), new_vert_vel_var));
P.uncorrelateCovarianceSetVariance<1>(State::vel.idx + 2, math::max(sq(0.01f), new_vert_vel_var));
}
_output_predictor.resetVerticalVelocityTo(delta_vert_vel);
@@ -133,11 +133,11 @@ void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f
_state.pos.xy() = new_horz_pos;
if (PX4_ISFINITE(new_horz_pos_var(0))) {
P.uncorrelateCovarianceSetVariance<1>(7, math::max(sq(0.01f), new_horz_pos_var(0)));
P.uncorrelateCovarianceSetVariance<1>(State::pos.idx, math::max(sq(0.01f), new_horz_pos_var(0)));
}
if (PX4_ISFINITE(new_horz_pos_var(1))) {
P.uncorrelateCovarianceSetVariance<1>(8, math::max(sq(0.01f), new_horz_pos_var(1)));
P.uncorrelateCovarianceSetVariance<1>(State::pos.idx + 1, math::max(sq(0.01f), new_horz_pos_var(1)));
}
_output_predictor.resetHorizontalPositionTo(delta_horz_pos);
@@ -180,7 +180,7 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v
if (PX4_ISFINITE(new_vert_pos_var)) {
// the state variance is the same as the observation
P.uncorrelateCovarianceSetVariance<1>(9, math::max(sq(0.01f), new_vert_pos_var));
P.uncorrelateCovarianceSetVariance<1>(State::pos.idx + 2, math::max(sq(0.01f), new_vert_pos_var));
}
const float delta_z = new_vert_pos - old_vert_pos;
@@ -369,14 +369,14 @@ void Ekf::getAuxVelInnovVar(float aux_vel_innov_var[2]) const
matrix::Vector<float, 24> Ekf::getStateAtFusionHorizonAsVector() const
{
matrix::Vector<float, 24> state;
state.slice<4, 1>(0, 0) = _state.quat_nominal;
state.slice<3, 1>(4, 0) = _state.vel;
state.slice<3, 1>(7, 0) = _state.pos;
state.slice<3, 1>(10, 0) = _state.gyro_bias;
state.slice<3, 1>(13, 0) = _state.accel_bias;
state.slice<3, 1>(16, 0) = _state.mag_I;
state.slice<3, 1>(19, 0) = _state.mag_B;
state.slice<2, 1>(22, 0) = _state.wind_vel;
state.slice<State::quat_nominal.dof, 1>(State::quat_nominal.idx, 0) = _state.quat_nominal;
state.slice<State::vel.dof, 1>(State::vel.idx, 0) = _state.vel;
state.slice<State::pos.dof, 1>(State::pos.idx, 0) = _state.pos;
state.slice<State::gyro_bias.dof, 1>(State::gyro_bias.idx, 0) = _state.gyro_bias;
state.slice<State::accel_bias.dof, 1>(State::accel_bias.idx, 0) = _state.accel_bias;
state.slice<State::mag_I.dof, 1>(State::mag_I.idx, 0) = _state.mag_I;
state.slice<State::mag_B.dof, 1>(State::mag_B.idx, 0) = _state.mag_B;
state.slice<State::wind_vel.dof, 1>(State::wind_vel.idx, 0) = _state.wind_vel;
return state;
}
@@ -466,7 +466,7 @@ void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const
// report absolute accuracy taking into account the uncertainty in location of the origin
// If not aiding, return 0 for horizontal position estimate as no estimate is available
// TODO - allow for baro drift in vertical position error
float hpos_err = sqrtf(P(7, 7) + P(8, 8) + sq(_gpos_origin_eph));
float hpos_err = sqrtf(P.trace<2>(State::pos.idx) + sq(_gpos_origin_eph));
// If we are dead-reckoning, use the innovations as a conservative alternate measure of the horizontal position error
// The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors
@@ -484,14 +484,14 @@ void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const
}
*ekf_eph = hpos_err;
*ekf_epv = sqrtf(P(9, 9) + sq(_gpos_origin_epv));
*ekf_epv = sqrtf(P(State::pos.idx + 2, State::pos.idx + 2) + sq(_gpos_origin_epv));
}
// get the 1-sigma horizontal and vertical position uncertainty of the ekf local position
void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const
{
// TODO - allow for baro drift in vertical position error
float hpos_err = sqrtf(P(7, 7) + P(8, 8));
float hpos_err = sqrtf(P.trace<2>(State::pos.idx));
// If we are dead-reckoning for too long, use the innovations as a conservative alternate measure of the horizontal position error
// The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors
@@ -509,13 +509,13 @@ void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const
}
*ekf_eph = hpos_err;
*ekf_epv = sqrtf(P(9, 9));
*ekf_epv = sqrtf(P(State::pos.idx + 2, State::pos.idx + 2));
}
// get the 1-sigma horizontal and vertical velocity uncertainty
void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const
{
float hvel_err = sqrtf(P(4, 4) + P(5, 5));
float hvel_err = sqrtf(P.trace<2>(State::vel.idx));
// If we are dead-reckoning for too long, use the innovations as a conservative alternate measure of the horizontal velocity error
// The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors
@@ -548,7 +548,7 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const
}
*ekf_evh = hvel_err;
*ekf_evv = sqrtf(P(6, 6));
*ekf_evv = sqrtf(P(State::vel.idx + 2, State::vel.idx + 2));
}
/*
@@ -618,10 +618,10 @@ void Ekf::resetGyroBias()
// Zero the corresponding covariances and set
// variances to the values use for initial alignment
P.uncorrelateCovarianceSetVariance<3>(10, sq(_params.switch_on_gyro_bias));
P.uncorrelateCovarianceSetVariance<State::gyro_bias.dof>(State::gyro_bias.idx, sq(_params.switch_on_gyro_bias));
// Set previous frame values
_prev_gyro_bias_var = P.slice<3, 3>(10, 10).diag();
_prev_gyro_bias_var = P.slice<State::gyro_bias.dof, State::gyro_bias.dof>(State::gyro_bias.idx, State::gyro_bias.idx).diag();
}
void Ekf::resetAccelBias()
@@ -631,10 +631,10 @@ void Ekf::resetAccelBias()
// Zero the corresponding covariances and set
// variances to the values use for initial alignment
P.uncorrelateCovarianceSetVariance<3>(13, sq(_params.switch_on_accel_bias));
P.uncorrelateCovarianceSetVariance<State::accel_bias.dof>(State::accel_bias.idx, sq(_params.switch_on_accel_bias));
// Set previous frame values
_prev_accel_bias_var = P.slice<3, 3>(13, 13).diag();
_prev_accel_bias_var = P.slice<State::accel_bias.dof, State::accel_bias.dof>(State::accel_bias.idx, State::accel_bias.idx).diag();
}
// get EKF innovation consistency check status information comprising of:
@@ -812,23 +812,22 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const
void Ekf::fuse(const Vector24f &K, float innovation)
{
_state.quat_nominal -= K.slice<4, 1>(0, 0) * innovation;
_state.quat_nominal -= K.slice<State::quat_nominal.dof, 1>(State::quat_nominal.idx, 0) * innovation;
_state.quat_nominal.normalize();
_R_to_earth = Dcmf(_state.quat_nominal);
_state.vel -= K.slice<3, 1>(4, 0) * innovation;
_state.pos -= K.slice<3, 1>(7, 0) * innovation;
_state.gyro_bias -= K.slice<3, 1>(10, 0) * innovation;
_state.accel_bias -= K.slice<3, 1>(13, 0) * innovation;
_state.mag_I -= K.slice<3, 1>(16, 0) * innovation;
_state.mag_B -= K.slice<3, 1>(19, 0) * innovation;
_state.wind_vel -= K.slice<2, 1>(22, 0) * innovation;
_state.vel -= K.slice<State::vel.dof, 1>(State::vel.idx, 0) * innovation;
_state.pos -= K.slice<State::pos.dof, 1>(State::pos.idx, 0) * innovation;
_state.gyro_bias -= K.slice<State::gyro_bias.dof, 1>(State::gyro_bias.idx, 0) * innovation;
_state.accel_bias -= K.slice<State::accel_bias.dof, 1>(State::accel_bias.idx, 0) * innovation;
_state.mag_I -= K.slice<State::mag_I.dof, 1>(State::mag_I.idx, 0) * innovation;
_state.mag_B -= K.slice<State::mag_B.dof, 1>(State::mag_B.idx, 0) * innovation;
_state.wind_vel -= K.slice<State::wind_vel.dof, 1>(State::wind_vel.idx, 0) * innovation;
}
void Ekf::uncorrelateQuatFromOtherStates()
{
P.slice<_k_num_states - 4, 4>(4, 0) = 0.f;
P.slice<4, _k_num_states - 4>(0, 4) = 0.f;
P.uncorrelateCovarianceBlock<State::quat_nominal.dof>(State::quat_nominal.idx);
}
void Ekf::updateDeadReckoningStatus()
@@ -1070,5 +1069,5 @@ void Ekf::resetWindToZero()
_state.wind_vel.setZero();
// start with a small initial uncertainty to improve the initial estimate
P.uncorrelateCovarianceSetVariance<2>(22, _params.initial_wind_uncertainty);
P.uncorrelateCovarianceSetVariance<State::wind_vel.dof>(State::wind_vel.idx, _params.initial_wind_uncertainty);
}