fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 00:53: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/ekf_helper.cpp
+11 -12
View File
@@ -945,19 +945,20 @@ void Ekf::updateGroundEffect()
}
#endif // CONFIG_EKF2_BAROMETER
void Ekf::increaseQuatYawErrVariance(float yaw_variance)
{
matrix::SquareMatrix<float, 4> q_cov;
sym::RotVarNedToLowerTriangularQuatCov(getStateAtFusionHorizonAsVector(), Vector3f(0.f, 0.f, yaw_variance), &q_cov);
q_cov.copyLowerToUpperTriangle();
P.slice<4, 4>(0, 0) += q_cov;
}
void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
{
// save a copy of the quaternion state for later use in calculating the amount of reset change
const Quatf quat_before_reset = _state.quat_nominal;
// save a copy of covariance in NED frame to restore it after the quat reset
const matrix::SquareMatrix3f rot_cov = diag(calcRotVecVariances());
Vector3f rot_var_ned_before_reset = matrix::SquareMatrix3f(_R_to_earth * rot_cov * _R_to_earth.T()).diag();
// update the yaw angle variance
if (PX4_ISFINITE(yaw_variance) && (yaw_variance > FLT_EPSILON)) {
rot_var_ned_before_reset(2) = yaw_variance;
}
// update transformation matrix from body to world frame using the current estimate
// update the rotation matrix using the new yaw value
_R_to_earth = updateYawInRotMat(yaw, Dcmf(_state.quat_nominal));
@@ -970,10 +971,8 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
_state.quat_nominal = quat_after_reset;
uncorrelateQuatFromOtherStates();
// update the yaw angle variance
if (PX4_ISFINITE(yaw_variance) && (yaw_variance > FLT_EPSILON)) {
increaseQuatYawErrVariance(yaw_variance);
}
// restore covariance
resetQuatCov(rot_var_ned_before_reset);
// add the reset amount to the output observer buffered data
_output_predictor.resetQuaternion(q_error);