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

EKF: wrap compass yaw estimate

Browse Source
This commit is contained in:
Paul Riseborough
2016-02-19 16:53:55 +11:00
parent 7d6226eb45
commit 90e1bd3e36
1 changed files with 3 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/mag_fusion.cpp
+3
View File
@@ -613,6 +613,9 @@ void Ekf::fuseHeading()
matrix::Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag; matrix::Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
float measured_yaw = atan2f(mag_earth_pred(1), mag_earth_pred(0)) - _mag_declination; float measured_yaw = atan2f(mag_earth_pred(1), mag_earth_pred(0)) - _mag_declination;
// wrap the yaw to the interval between +-pi
measured_yaw = matrix::wrap_pi(measured_yaw);
// calculate the innovation // calculate the innovation
matrix::Euler<float> euler(_state.quat_nominal); matrix::Euler<float> euler(_state.quat_nominal);
float innovation = euler(2) - measured_yaw; float innovation = euler(2) - measured_yaw;