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

EKF2: more accurate unaided yaw calculation

Browse Source 
to fix the yaw unlocking in StickYaw
when giving a lot of roll and pitch input
This commit is contained in:
Matthias Grob 2025-04-01 19:23:09 +02:00
parent d514cb4903
commit 9404783c99
1 changed files with 10 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
src/modules/ekf2/EKF/output_predictor/output_predictor.cpp
View File

@ -35,6 +35,7 @@
using matrix::AxisAnglef;
using matrix::Dcmf;
using matrix::Eulerf;
using matrix::Quatf;
using matrix::Vector2f;
using matrix::Vector3f;
@ -196,6 +197,7 @@ void OutputPredictor::calculateOutputStates(const uint64_t time_us, const Vector
const Quatf dq(AxisAnglef{delta_angle_corrected});
// rotate the previous INS quaternion by the delta quaternions
const Quatf quat_nominal_before_update = _output_new.quat_nominal;
_output_new.quat_nominal = _output_new.quat_nominal * dq;
// the quaternions must always be normalised after modification
@ -244,9 +246,14 @@ void OutputPredictor::calculateOutputStates(const uint64_t time_us, const Vector
}
// update auxiliary yaw estimate
const Vector3f unbiased_delta_angle = delta_angle - delta_angle_bias_scaled;
const float spin_del_ang_D = unbiased_delta_angle.dot(Vector3f(_R_to_earth_now.row(2)));
_unaided_yaw = matrix::wrap_pi(_unaided_yaw + spin_del_ang_D);
// rotate the state quternion by the delta quaternion only corrected for bias without EKF corrections
const Vector3f delta_angle_unaided = delta_angle - delta_angle_bias_scaled;
const float yaw_state = Eulerf(quat_nominal_before_update).psi();
const Quatf quat_unaided = quat_nominal_before_update * Quatf(AxisAnglef(delta_angle_unaided));
const float yaw_without_aiding = Eulerf(quat_unaided).psi();
// Yaw before delta quaternion applied and yaw after. The difference is the delta yaw. Accumulate it.
const float unaided_delta_yaw = yaw_without_aiding - yaw_state;
_unaided_yaw = matrix::wrap_pi(_unaided_yaw + unaided_delta_yaw);
}
void OutputPredictor::correctOutputStates(const uint64_t time_delayed_us,