aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF: Improve mag earth state reset accuracy

Browse Source 
This removes errors in the in-flight reset of the earth field states by:

1) Using a state vector and magnetometer measurement from the same time coordinate
2) Not using the AHRS trim offsets in the calculation
This commit is contained in:
Paul Riseborough 2015-11-01 08:53:33 +00:00 committed by Randy Mackay
parent 0dd5a7c4fa
commit 9b37c29efa
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -1036,7 +1036,7 @@ void NavEKF::SelectMagFusion()
// This is done to prevent magnetic field distoration from steel roofs and adjacent structures causing bad earth field and initial yaw values
// Do not do this alignment if the vehicle is rotating rapidly as timing erors in the mag data will cause significant errors
Vector3f eulerAngles;
getEulerAngles(eulerAngles);
statesAtMagMeasTime.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z);
state.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
firstMagYawInit = true;
} else if (vehicleArmed && !secondMagYawInit && (state.position.z - posDownAtArming) < -5.0f && !assume_zero_sideslip() && deltaRot < 0.1745f) {
@ -1044,7 +1044,7 @@ void NavEKF::SelectMagFusion()
// This second and final correction is needed for flight from large metal structures where the magnetic field distortion can extend up to 5m
// Do not do this alignment if the vehicle is rotating rapidly as timing erors in the mag data will cause significant errors
Vector3f eulerAngles;
getEulerAngles(eulerAngles);
statesAtMagMeasTime.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z);
state.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
secondMagYawInit = true;
}