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

AP_NavEKF2: Improve initialisation of magnetic field learning

Browse Source 
Use the more robust, but less accurate compass heading fusion up to 5m altitude
Wait for the magnetometer data fusion time offset to be correct before using data to reset states
Don't reset magnetic field states if the vehicle is rotating rapidly as timing offsets will produce large errors
When doing the yaw angle reset, apply the reset increment to all quaternions stored in the output buffer to avoid transients produced by yaw rotations and the 0.25 second fusion time horizon offset.
Only do the one yaw and mag reset at 5m, not two at 1.5 and 5.0m
Always re-do the yaw and mag reset when leaving the ground.
This commit is contained in:
Paul Riseborough 2015-10-28 20:06:40 +11:00 committed by Randy Mackay
parent 844ed95718
commit f539b597a3
4 changed files with 34 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp
View File

@ -36,9 +36,6 @@ void NavEKF2_core::controlFilterModes()
// Used during initial bootstrap alignment of the filter
checkAttitudeAlignmentStatus();
// Control reset of yaw and magnetic field states
controlMagYawReset();
// Set the type of inertial navigation aiding used
setAidingMode();
@ -65,13 +62,20 @@ void NavEKF2_core::setWindMagStateLearningMode()
((frontend._magCal == 3) && firstMagYawInit) || // when initial in-air yaw and field reset has completed
(frontend._magCal == 4); // all the time
// Deny mag calibration request if we aren't using the compass, it has been inhibited by the user, or we do not have an absolute position reference
// Deny mag calibration request if we aren't using the compass, it has been inhibited by the user,
// we do not have an absolute position reference or are on the ground (unless explicitly requested by the user)
// If we do nto have absolute position (eg GPS) then the earth field states cannot be learned
bool magCalDenied = !use_compass() || (frontend._magCal == 2) || (PV_AidingMode == AID_NONE);
bool magCalDenied = !use_compass() || (frontend._magCal == 2) || (PV_AidingMode == AID_NONE) || (onGround && frontend._magCal != 4);
// Inhibit the magnetic field calibration if not requested or denied
inhibitMagStates = (!magCalRequested || magCalDenied);
// If magnetometer states are inhibited, we clear the flag indicating that the magnetic field in-flight initialisation has been completed
// because it will need to be done again
if (inhibitMagStates) {
firstMagYawInit = false;
}
// Adjust the indexing limits used to address the covariance, states and other EKF arrays to avoid unnecessary operations
// if we are not using those states
if (inhibitMagStates && inhibitWindStates) {

31
libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
View File

@ -22,23 +22,20 @@ void NavEKF2_core::controlMagYawReset()
{
// Monitor the gain in height and reset the magnetic field states and heading when initial altitude has been gained
// This is done to prevent magnetic field distoration from steel roofs and adjacent structures causing bad earth field and initial yaw values
if (inFlight && !firstMagYawInit && (stateStruct.position.z - posDownAtTakeoff) < -1.5f) {
// Do the first in-air yaw and earth mag field initialisation when the vehicle has gained 1.5m of altitude after commencement of flight
Vector3f eulerAngles;
stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z);
stateStruct.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
StoreQuatReset();
// Delay if rotating rapidly as time offsets will produce errors in the magnetic field states
if (inFlight && !firstMagYawInit && (stateStruct.position.z - posDownAtTakeoff) < -5.0f && (imuDataDelayed.delVel.length())/imuDataDelayed.delAngDT < 1.0f) {
firstMagYawInit = true;
// reset the timer used to prevent magnetometer fusion from affecting attitude until initial field learning is complete
magFuseTiltInhibit_ms = imuSampleTime_ms;
} else if (inFlight && !secondMagYawInit && (stateStruct.position.z - posDownAtTakeoff) < -5.0f) {
// Do the second and final yaw and earth mag field initialisation when the vehicle has gained 5.0m of altitude after commencement of flight
// This second and final correction is needed for flight from large metal structures where the magnetic field distortion can extend up to 5m
// Update the yaw angle and earth field states using the magnetic field measurements
Quaternion tempQuat;
Vector3f eulerAngles;
stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z);
tempQuat = stateStruct.quat;
stateStruct.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
StoreQuatReset();
secondMagYawInit = true;
magFuseTiltInhibit_ms = imuSampleTime_ms;
// calculate the change in the quaternion state and apply it to the ouput history buffer
tempQuat = stateStruct.quat/tempQuat;
StoreQuatRotate(tempQuat);
}
// perform a yaw alignment check against GPS if exiting on-ground mode for fly forward type vehicle (plane)
@ -135,11 +132,17 @@ void NavEKF2_core::SelectMagFusion()
magTimeout = true;
}
bool temp = RecallMag();
// check for availability of magnetometer data to fuse
bool newMagDataAvailable = RecallMag();
if (newMagDataAvailable) {
// Control reset of yaw and magnetic field states
controlMagYawReset();
}
// determine if conditions are right to start a new fusion cycle
// wait until the EKF time horizon catches up with the measurement
bool dataReady = (temp && statesInitialised && use_compass() && yawAlignComplete);
bool dataReady = (newMagDataAvailable && statesInitialised && use_compass() && yawAlignComplete);
if (dataReady) {
// ensure that the covariance prediction is up to date before fusing data
if (!covPredStep) CovariancePrediction();

4
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -175,7 +175,7 @@ bool NavEKF2_core::RecallOF()
bool NavEKF2_core::getMagOffsets(Vector3f &magOffsets) const
{
// compass offsets are valid if we have finalised magnetic field initialisation and magnetic field learning is not prohibited and primary compass is valid
if (secondMagYawInit && (frontend._magCal != 2) && _ahrs->get_compass()->healthy(0)) {
if (firstMagYawInit && (frontend._magCal != 2) && _ahrs->get_compass()->healthy(0)) {
magOffsets = _ahrs->get_compass()->get_offsets(0) - stateStruct.body_magfield*1000.0f;
return true;
} else {
@ -211,7 +211,7 @@ void NavEKF2_core::readMagData()
Vector3f nowMagOffsets = _ahrs->get_compass()->get_offsets(0);
bool changeDetected = (!is_equal(nowMagOffsets.x,lastMagOffsets.x) || !is_equal(nowMagOffsets.y,lastMagOffsets.y) || !is_equal(nowMagOffsets.z,lastMagOffsets.z));
// Ignore bias changes before final mag field and yaw initialisation, as there may have been a compass calibration
if (changeDetected && secondMagYawInit) {
if (changeDetected && firstMagYawInit) {
stateStruct.body_magfield.x += (nowMagOffsets.x - lastMagOffsets.x) * 0.001f;
stateStruct.body_magfield.y += (nowMagOffsets.y - lastMagOffsets.y) * 0.001f;
stateStruct.body_magfield.z += (nowMagOffsets.z - lastMagOffsets.z) * 0.001f;

1
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -102,7 +102,6 @@ void NavEKF2_core::InitialiseVariables()
badMag = false;
badIMUdata = false;
firstMagYawInit = false;
secondMagYawInit = false;
dtIMUavg = 0.0025f;
dt = 0;
velDotNEDfilt.zero();