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

EKF: use low pass filtered mag measurements for heading initialization

Browse Source
This commit is contained in:
CarlOlsson
2018-11-21 15:02:42 +01:00
committed by Paul Riseborough
parent 27f7846495
commit 1a969aed07
1 changed files with 2 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf_helper.cpp
+2 -2
View File
@@ -591,7 +591,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_AUTOFW) {
// rotate the magnetometer measurements into earth frame using a zero yaw angle
Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
Vector3f mag_earth_pred = R_to_earth * mag_init;
// the angle of the projection onto the horizontal gives the yaw angle
euler321(2) = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination;
@@ -649,7 +649,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_AUTOFW) {
// rotate the magnetometer measurements into earth frame using a zero yaw angle
Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
Vector3f mag_earth_pred = R_to_earth * mag_init;
// the angle of the projection onto the horizontal gives the yaw angle
euler312(0) = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination;