dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf2: move fuseVelPosHeight() -> fuseDirectStateMeasurement() 

- don't bother keeping bad_vel_{N,E,D} and bad_pos_{N,E,D} fault status bits
This commit is contained in:
Daniel Agar 2024-02-14 20:46:28 -05:00
parent 021dd0d0af
commit c4c41c49e5
7 changed files with 60 additions and 145 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
msg/EstimatorStatusFlags.msg
View File

@ -55,15 +55,9 @@ bool fs_bad_airspeed # 5 - true if fusion of the airspeed has encounte
bool fs_bad_sideslip # 6 - true if fusion of the synthetic sideslip constraint has encountered a numerical error
bool fs_bad_optflow_x # 7 - true if fusion of the optical flow X axis has encountered a numerical error
bool fs_bad_optflow_y # 8 - true if fusion of the optical flow Y axis has encountered a numerical error
bool fs_bad_vel_n # 9 - true if fusion of the North velocity has encountered a numerical error
bool fs_bad_vel_e # 10 - true if fusion of the East velocity has encountered a numerical error
bool fs_bad_vel_d # 11 - true if fusion of the Down velocity has encountered a numerical error
bool fs_bad_pos_n # 12 - true if fusion of the North position has encountered a numerical error
bool fs_bad_pos_e # 13 - true if fusion of the East position has encountered a numerical error
bool fs_bad_pos_d # 14 - true if fusion of the Down position has encountered a numerical error
bool fs_bad_acc_bias # 15 - true if bad delta velocity bias estimates have been detected
bool fs_bad_acc_vertical # 16 - true if bad vertical accelerometer data has been detected
bool fs_bad_acc_clipping # 17 - true if delta velocity data contains clipping (asymmetric railing)
bool fs_bad_acc_bias # 9 - true if bad delta velocity bias estimates have been detected
bool fs_bad_acc_vertical # 10 - true if bad vertical accelerometer data has been detected
bool fs_bad_acc_clipping # 11 - true if delta velocity data contains clipping (asymmetric railing)
# innovation test failures

2
src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.cpp
View File

@ -66,7 +66,7 @@ bool ZeroVelocityUpdate::update(Ekf &ekf, const estimator::imuSample &imu_delaye
for (unsigned i = 0; i < 3; i++) {
const float innovation = ekf.state().vel(i) - vel_obs(i);
ekf.fuseVelPosHeight(innovation, innov_var(i), State::vel.idx + i);
ekf.fuseDirectStateMeasurement(innovation, innov_var(i), State::vel.idx + i);
}
_time_last_zero_velocity_fuse = imu_delayed.time_us;

12
src/modules/ekf2/EKF/common.h
View File

@ -509,15 +509,9 @@ union fault_status_u {
bool bad_sideslip : 1; ///< 6 - true if fusion of the synthetic sideslip constraint has encountered a numerical error
bool bad_optflow_X : 1; ///< 7 - true if fusion of the optical flow X axis has encountered a numerical error
bool bad_optflow_Y : 1; ///< 8 - true if fusion of the optical flow Y axis has encountered a numerical error
bool bad_vel_N : 1; ///< 9 - true if fusion of the North velocity has encountered a numerical error
bool bad_vel_E : 1; ///< 10 - true if fusion of the East velocity has encountered a numerical error
bool bad_vel_D : 1; ///< 11 - true if fusion of the Down velocity has encountered a numerical error
bool bad_pos_N : 1; ///< 12 - true if fusion of the North position has encountered a numerical error
bool bad_pos_E : 1; ///< 13 - true if fusion of the East position has encountered a numerical error
bool bad_pos_D : 1; ///< 14 - true if fusion of the Down position has encountered a numerical error
bool bad_acc_bias : 1; ///< 15 - true if bad delta velocity bias estimates have been detected
bool bad_acc_vertical : 1; ///< 16 - true if bad vertical accelerometer data has been detected
bool bad_acc_clipping : 1; ///< 17 - true if delta velocity data contains clipping (asymmetric railing)
bool bad_acc_bias : 1; ///< 9 - true if bad delta velocity bias estimates have been detected
bool bad_acc_vertical : 1; ///< 10 - true if bad vertical accelerometer data has been detected
bool bad_acc_clipping : 1; ///< 11 - true if delta velocity data contains clipping (asymmetric railing)
} flags;
uint32_t value;
};

6
src/modules/ekf2/EKF/ekf.h
View File

@ -327,8 +327,8 @@ public:
#endif
}
// fuse single velocity and position measurement
bool fuseVelPosHeight(const float innov, const float innov_var, const int state_index);
// fuse single direct state measurement (eg NED velocity, NED position, mag earth field, etc)
bool fuseDirectStateMeasurement(const float innov, const float innov_var, const int state_index);
// gyro bias
const Vector3f &getGyroBias() const { return _state.gyro_bias; } // get the gyroscope bias in rad/s
@ -1137,8 +1137,6 @@ private:
void resetFakePosFusion();
void stopFakePosFusion();
void setVelPosStatus(const int state_index, const bool healthy);
// reset the quaternion states and covariances to the new yaw value, preserving the roll and pitch
// yaw : Euler yaw angle (rad)
// yaw_variance : yaw error variance (rad^2)

36
src/modules/ekf2/EKF/ekf_helper.cpp
View File

@ -1015,3 +1015,39 @@ void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed)
_accel_bias_inhibit[index] = do_inhibit_all_accel_axes || imu_delayed.delta_vel_clipping[index] || !is_bias_observable;
}
}
bool Ekf::fuseDirectStateMeasurement(const float innov, const float innov_var, const int state_index)
{
VectorState 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 < State::size; row++) {
Kfusion(row) = P(row, state_index) / innov_var;
}
clearInhibitedStateKalmanGains(Kfusion);
SquareMatrixState KHP;
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);
}
}
const bool healthy = checkAndFixCovarianceUpdate(KHP);
if (healthy) {
// apply the covariance corrections
P -= KHP;
constrainStateVariances();
// apply the state corrections
fuse(Kfusion, innov);
return true;
}
return false;
}

131
src/modules/ekf2/EKF/vel_pos_fusion.cpp
View File

@ -136,12 +136,14 @@ void Ekf::fuseVelocity(estimator_aid_source2d_s &aid_src)
{
if (!aid_src.innovation_rejected) {
// vx, vy
if (fuseVelPosHeight(aid_src.innovation[0], aid_src.innovation_variance[0], State::vel.idx)
&& fuseVelPosHeight(aid_src.innovation[1], aid_src.innovation_variance[1], State::vel.idx + 1)
if (fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], State::vel.idx)
&& fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], State::vel.idx + 1)
) {
aid_src.fused = true;
aid_src.time_last_fuse = _time_delayed_us;
_time_last_hor_vel_fuse = _time_delayed_us;
} else {
aid_src.fused = false;
}
@ -152,13 +154,16 @@ void Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src)
{
if (!aid_src.innovation_rejected) {
// vx, vy, vz
if (fuseVelPosHeight(aid_src.innovation[0], aid_src.innovation_variance[0], State::vel.idx)
&& fuseVelPosHeight(aid_src.innovation[1], aid_src.innovation_variance[1], State::vel.idx + 1)
&& fuseVelPosHeight(aid_src.innovation[2], aid_src.innovation_variance[2], State::vel.idx + 2)
if (fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], State::vel.idx + 0)
&& fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], State::vel.idx + 1)
&& fuseDirectStateMeasurement(aid_src.innovation[2], aid_src.innovation_variance[2], State::vel.idx + 2)
) {
aid_src.fused = true;
aid_src.time_last_fuse = _time_delayed_us;
_time_last_hor_vel_fuse = _time_delayed_us;
_time_last_ver_vel_fuse = _time_delayed_us;
} else {
aid_src.fused = false;
}
@ -169,12 +174,14 @@ void Ekf::fuseHorizontalPosition(estimator_aid_source2d_s &aid_src)
{
// x & y
if (!aid_src.innovation_rejected) {
if (fuseVelPosHeight(aid_src.innovation[0], aid_src.innovation_variance[0], State::pos.idx)
&& fuseVelPosHeight(aid_src.innovation[1], aid_src.innovation_variance[1], State::pos.idx + 1)
if (fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], State::pos.idx)
&& fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], State::pos.idx + 1)
) {
aid_src.fused = true;
aid_src.time_last_fuse = _time_delayed_us;
_time_last_hor_pos_fuse = _time_delayed_us;
} else {
aid_src.fused = false;
}
@ -185,119 +192,11 @@ void Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src)
{
// z
if (!aid_src.innovation_rejected) {
if (fuseVelPosHeight(aid_src.innovation, aid_src.innovation_variance, State::pos.idx + 2)) {
if (fuseDirectStateMeasurement(aid_src.innovation, aid_src.innovation_variance, State::pos.idx + 2)) {
aid_src.fused = true;
aid_src.time_last_fuse = _time_delayed_us;
}
}
}
// Helper function that fuses a single velocity or position measurement
bool Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int state_index)
{
VectorState 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 < State::size; row++) {
Kfusion(row) = P(row, state_index) / innov_var;
}
clearInhibitedStateKalmanGains(Kfusion);
SquareMatrixState KHP;
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);
}
}
const bool healthy = checkAndFixCovarianceUpdate(KHP);
setVelPosStatus(state_index, healthy);
if (healthy) {
// apply the covariance corrections
P -= KHP;
constrainStateVariances();
// apply the state corrections
fuse(Kfusion, innov);
return true;
}
return false;
}
void Ekf::setVelPosStatus(const int state_index, const bool healthy)
{
switch (state_index) {
case State::vel.idx:
if (healthy) {
_fault_status.flags.bad_vel_N = false;
_time_last_hor_vel_fuse = _time_delayed_us;
} else {
_fault_status.flags.bad_vel_N = true;
}
break;
case State::vel.idx + 1:
if (healthy) {
_fault_status.flags.bad_vel_E = false;
_time_last_hor_vel_fuse = _time_delayed_us;
} else {
_fault_status.flags.bad_vel_E = true;
}
break;
case State::vel.idx + 2:
if (healthy) {
_fault_status.flags.bad_vel_D = false;
_time_last_ver_vel_fuse = _time_delayed_us;
} else {
_fault_status.flags.bad_vel_D = true;
}
break;
case State::pos.idx:
if (healthy) {
_fault_status.flags.bad_pos_N = false;
_time_last_hor_pos_fuse = _time_delayed_us;
} else {
_fault_status.flags.bad_pos_N = true;
}
break;
case State::pos.idx + 1:
if (healthy) {
_fault_status.flags.bad_pos_E = false;
_time_last_hor_pos_fuse = _time_delayed_us;
} else {
_fault_status.flags.bad_pos_E = true;
}
break;
case State::pos.idx + 2:
if (healthy) {
_fault_status.flags.bad_pos_D = false;
_time_last_hgt_fuse = _time_delayed_us;
} else {
_fault_status.flags.bad_pos_D = true;
}
break;
}
}

6
src/modules/ekf2/EKF2.cpp
View File

@ -1896,12 +1896,6 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
status_flags.fs_bad_sideslip = _ekf.fault_status_flags().bad_sideslip;
status_flags.fs_bad_optflow_x = _ekf.fault_status_flags().bad_optflow_X;
status_flags.fs_bad_optflow_y = _ekf.fault_status_flags().bad_optflow_Y;
status_flags.fs_bad_vel_n = _ekf.fault_status_flags().bad_vel_N;
status_flags.fs_bad_vel_e = _ekf.fault_status_flags().bad_vel_E;
status_flags.fs_bad_vel_d = _ekf.fault_status_flags().bad_vel_D;
status_flags.fs_bad_pos_n = _ekf.fault_status_flags().bad_pos_N;
status_flags.fs_bad_pos_e = _ekf.fault_status_flags().bad_pos_E;
status_flags.fs_bad_pos_d = _ekf.fault_status_flags().bad_pos_D;
status_flags.fs_bad_acc_bias = _ekf.fault_status_flags().bad_acc_bias;
status_flags.fs_bad_acc_vertical = _ekf.fault_status_flags().bad_acc_vertical;
status_flags.fs_bad_acc_clipping = _ekf.fault_status_flags().bad_acc_clipping;