Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 02:55:07 +08:00
Code Issues Actions 19 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/covariance.cpp
+24 -2
View File
@@ -282,6 +282,25 @@ void Ekf::resetQuatCov(const Vector3f &rot_var_ned)
P.uncorrelateCovarianceSetVariance<State::quat_nominal.dof>(State::quat_nominal.idx, rot_var_ned); P.uncorrelateCovarianceSetVariance<State::quat_nominal.dof>(State::quat_nominal.idx, rot_var_ned);
} }
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::resetGyroBiasZCov()
{
P.uncorrelateCovarianceSetVariance<1>(State::gyro_bias.idx + 2, sq(_params.switch_on_gyro_bias));
}
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));
}
#if defined(CONFIG_EKF2_MAGNETOMETER) #if defined(CONFIG_EKF2_MAGNETOMETER)
void Ekf::resetMagCov() void Ekf::resetMagCov()
{ {
@@ -295,7 +314,10 @@ void Ekf::resetMagCov()
} }
#endif // CONFIG_EKF2_MAGNETOMETER #endif // CONFIG_EKF2_MAGNETOMETER
void Ekf::resetGyroBiasZCov() #if defined(CONFIG_EKF2_WIND)
void Ekf::resetWindCov()
{ {
P.uncorrelateCovarianceSetVariance<1>(State::gyro_bias.idx + 2, sq(_params.switch_on_gyro_bias)); // 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
src/modules/ekf2/EKF/ekf_helper.cpp
-19
View File
@@ -302,13 +302,6 @@ void Ekf::resetGyroBias()
resetGyroBiasCov(); 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() void Ekf::resetAccelBias()
{ {
// Zero the accel bias states // Zero the accel bias states
@@ -317,13 +310,6 @@ void Ekf::resetAccelBias()
resetAccelBiasCov(); 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: // 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 // 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. // Innovation Test Ratios - these are the ratio of the innovation to the acceptance threshold.
@@ -675,11 +661,6 @@ void Ekf::resetWindToZero()
resetWindCov(); 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 #endif // CONFIG_EKF2_WIND
void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed) void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed)