fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-28 22:13:34 +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-14 22:01:53 +01:00
committed by Roman
parent 83945581ed
commit 2bbe7f9a1c
1 changed files with 2 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/mag_fusion.cpp
+2 -2
View File
@@ -515,7 +515,7 @@ void Ekf::fuseHeading()
matrix::Dcm<float> R_to_earth(_state.quat_nominal); matrix::Dcm<float> R_to_earth(_state.quat_nominal);
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 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); innovation = math::constrain(innovation, -0.5f, 0.5f);
_heading_innov = innovation; _heading_innov = innovation;
@@ -694,7 +694,7 @@ void Ekf::fuseDeclination()
Kfusion[23] = t14 * (P[23][17] * t25 - P[23][16] * t26); Kfusion[23] = t14 * (P[23][17] * t25 - P[23][16] * t26);
// calculate innovation and constrain // 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); innovation = math::constrain(innovation, -0.5f, 0.5f);
// zero attitude error states and perform the state correction // zero attitude error states and perform the state correction