Skip to content
This repository has been archived by the owner on May 1, 2024. It is now read-only.

EKF: use low pass filtered mag measurements for heading initialization #524

Merged
merged 1 commit into from
Dec 5, 2018
Merged

EKF: use low pass filtered mag measurements for heading initialization #524

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions EKF/ekf_helper.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down Expand Up @@ -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;

Expand Down