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

EKF: Update yaw innovation calculation to match revised derivation

Browse Source 
The new derivation does not use magnetic field measurements in the observation model and instead fuses in a heading measurement directly.
This commit is contained in:
Paul Riseborough 2016-02-19 16:18:07 +11:00
parent 2b2c490382
commit 7f121e81e4
1 changed files with 7 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
EKF/mag_fusion.cpp
View File

@ -532,12 +532,15 @@ void Ekf::fuseHeading()
H_YAW[1] = t14 * (t15 * (q0 * q1 * 2.0f - q2 * q3 * 2.0f) + t9 * t10 * (q0 * q2 * 2.0f + q1 * q3 * 2.0f));
H_YAW[2] = t14 * (t15 * (t2 - t3 + t4 - t5) + t9 * t10 * (t7 - t8));
// calculate innovation
// rotate body field magnetic field measurement into earth frame and compare to published declination to get heading measurement
// TODO - enable use of an off-board heading measurement
// rotate body field magnetic field measurement into earth frame and compare horizontal vector with published declination to get yaw measurement
// TODO - enable use of an off-board yaw measurement
matrix::Dcm<float> R_to_earth(_state.quat_nominal);
matrix::Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
float innovation = 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;
// calculate the innovation
matrix::Euler<float> euler(_state.quat_nominal);
float innovation = euler(2) - measured_yaw;
// wrap the innovation to the interval between +-pi
innovation = matrix::wrap_pi(innovation);