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

EKF: Correct magnetic yaw measurement using learned mag biases

Browse Source 
Don't apply bias corrections when biases are being learned to avoid possible circular data dependency.
This commit is contained in:
Paul Riseborough
2017-11-13 07:05:56 +11:00
parent c70363c501
commit 44c50ab2df
1 changed files with 12 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/mag_fusion.cpp
+12 -2
View File
@@ -469,7 +469,12 @@ void Ekf::fuseHeading()
Dcmf R_to_earth(euler321);
// rotate the magnetometer measurements into earth frame using a zero yaw angle
mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
if (_control_status.flags.mag_3D) {
// don't apply bias corrections if we are learning them
mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
} else {
mag_earth_pred = R_to_earth * (_mag_sample_delayed.mag - _state.mag_B);
}
// the angle of the projection onto the horizontal gives the yaw angle
measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination;
@@ -556,7 +561,12 @@ void Ekf::fuseHeading()
R_to_earth(2,2) = cp*cr;
// rotate the magnetometer measurements into earth frame using a zero yaw angle
mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
if (_control_status.flags.mag_3D) {
// don't apply bias corrections if we are learning them
mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
} else {
mag_earth_pred = R_to_earth * (_mag_sample_delayed.mag - _state.mag_B);
}
// the angle of the projection onto the horizontal gives the yaw angle
measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination;