From c4c41c49e5b798eb38ddb604f87ef0dbfb9b5bbd Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 14 Feb 2024 20:46:28 -0500 Subject: [PATCH] ekf2: move fuseVelPosHeight() -> fuseDirectStateMeasurement() - don't bother keeping bad_vel_{N,E,D} and bad_pos_{N,E,D} fault status bits --- msg/EstimatorStatusFlags.msg | 12 +- .../EKF/aid_sources/ZeroVelocityUpdate.cpp | 2 +- src/modules/ekf2/EKF/common.h | 12 +- src/modules/ekf2/EKF/ekf.h | 6 +- src/modules/ekf2/EKF/ekf_helper.cpp | 36 +++++ src/modules/ekf2/EKF/vel_pos_fusion.cpp | 131 ++---------------- src/modules/ekf2/EKF2.cpp | 6 - 7 files changed, 60 insertions(+), 145 deletions(-) diff --git a/msg/EstimatorStatusFlags.msg b/msg/EstimatorStatusFlags.msg index b8c5d3e073..c6e0504f15 100644 --- a/msg/EstimatorStatusFlags.msg +++ b/msg/EstimatorStatusFlags.msg @@ -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 diff --git a/src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.cpp b/src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.cpp index c1e4f1fb5b..4745f759c7 100644 --- a/src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.cpp +++ b/src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.cpp @@ -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; diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 683f24814a..20b6fc3dbe 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -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; }; diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 79bede8c26..3393c7fd0d 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -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) diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index b9e300f0a2..419947f1c9 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -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; +} diff --git a/src/modules/ekf2/EKF/vel_pos_fusion.cpp b/src/modules/ekf2/EKF/vel_pos_fusion.cpp index b58e32b650..e0a094d8a4 100644 --- a/src/modules/ekf2/EKF/vel_pos_fusion.cpp +++ b/src/modules/ekf2/EKF/vel_pos_fusion.cpp @@ -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; } } diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index af957c24f2..f0a4ac7955 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1896,12 +1896,6 @@ void EKF2::PublishStatusFlags(const hrt_abstime ×tamp) 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;