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 magnetometer fusion to use declination from the specified source

Browse Source
This commit is contained in:
Paul Riseborough 2016-02-12 14:18:30 +11:00 committed by Roman
parent 83945581ed
commit 2bbe7f9a1c
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
EKF/mag_fusion.cpp
View File

@ -515,7 +515,7 @@ void Ekf::fuseHeading()
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)) - math::radians(_params.mag_declination_deg);
float innovation = atan2f(mag_earth_pred(1), mag_earth_pred(0)) - _mag_declination;
innovation = math::constrain(innovation, -0.5f, 0.5f);
_heading_innov = innovation;
@ -694,7 +694,7 @@ void Ekf::fuseDeclination()
Kfusion[23] = t14 * (P[23][17] * t25 - P[23][16] * t26);
// calculate innovation and constrain
float innovation = atanf(t4) - math::radians(_params.mag_declination_deg);
float innovation = atanf(t4) - _mag_declination;
innovation = math::constrain(innovation, -0.5f, 0.5f);
// zero attitude error states and perform the state correction