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

ekf2: move gyro/accel/wind covariance reset helpers to covariance.cpp

Browse Source
This commit is contained in:
Daniel Agar
2024-05-02 14:22:16 -04:00
parent 63c2ea33c1
commit c1fc893cca
2 changed files with 24 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf_helper.cpp
-19
View File
@@ -302,13 +302,6 @@ void Ekf::resetGyroBias()
resetGyroBiasCov();
}
void Ekf::resetGyroBiasCov()
{
// Zero the corresponding covariances and set
// variances to the values use for initial alignment
P.uncorrelateCovarianceSetVariance<State::gyro_bias.dof>(State::gyro_bias.idx, sq(_params.switch_on_gyro_bias));
}
void Ekf::resetAccelBias()
{
// Zero the accel bias states
@@ -317,13 +310,6 @@ void Ekf::resetAccelBias()
resetAccelBiasCov();
}
void Ekf::resetAccelBiasCov()
{
// Zero the corresponding covariances and set
// variances to the values use for initial alignment
P.uncorrelateCovarianceSetVariance<State::accel_bias.dof>(State::accel_bias.idx, sq(_params.switch_on_accel_bias));
}
// get EKF innovation consistency check status information comprising of:
// status - a bitmask integer containing the pass/fail status for each EKF measurement innovation consistency check
// Innovation Test Ratios - these are the ratio of the innovation to the acceptance threshold.
@@ -675,11 +661,6 @@ void Ekf::resetWindToZero()
resetWindCov();
}
void Ekf::resetWindCov()
{
// start with a small initial uncertainty to improve the initial estimate
P.uncorrelateCovarianceSetVariance<State::wind_vel.dof>(State::wind_vel.idx, sq(_params.initial_wind_uncertainty));
}
#endif // CONFIG_EKF2_WIND
void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed)