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

ekf2: update quaternion covariance at yaw reset(#22123)

Browse Source 
- Preserve tilt variance while doing a yaw reset
 - Yaw variance is now correctly set instead of increased

---------

Co-authored-by: Dominique <dominique.leblanc@donecle.com>
Co-authored-by: bresch <[brescianimathieu@gmail.com](mailto:brescianimathieu@gmail.com)>
This commit is contained in:
Mathieu Bresciani
2023-09-25 09:36:42 -04:00
committed by GitHub
co-authored by Dominique bresch
parent 514e0330e5
commit b3f460f30c
5 changed files with 761 additions and 760 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/covariance.cpp
+8 -3
View File
@@ -527,15 +527,20 @@ bool Ekf::checkAndFixCovarianceUpdate(const SquareMatrixState &KHP)
void Ekf::resetQuatCov(const float yaw_noise)
{
const float tilt_var = sq(fmaxf(_params.initial_tilt_err, 1.0e-2f));
Vector3f rot_var_ned(tilt_var, tilt_var, 0.f);
const float tilt_var = sq(math::max(_params.initial_tilt_err, 0.01f));
float yaw_var = sq(0.01f);
// update the yaw angle variance using the variance of the measurement
if (PX4_ISFINITE(yaw_noise)) {
// using magnetic heading tuning parameter
rot_var_ned(2) = (sq(fmaxf(yaw_noise, 1.0e-2f)));
yaw_var = math::max(sq(yaw_noise), yaw_var);
}
resetQuatCov(Vector3f(tilt_var, tilt_var, yaw_var));
}
void Ekf::resetQuatCov(const Vector3f &rot_var_ned)
{
// clear existing quaternion covariance
// Optimization: avoid the creation of a <4> function
P.uncorrelateCovarianceSetVariance<2>(State::quat_nominal.idx, 0.0f);