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

AP_NavEKF2: remember mag field states between flight on same power cycle

Browse Source 
Remember the mag bias and earth field states learned during flight when the vehicle  lands.
This improves performance for vehicles that do multiple flight on one power cycle
This commit is contained in:
priseborough 2016-07-02 18:46:42 +10:00
parent 136df7cb5c
commit 51dbed2338
4 changed files with 76 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp
View File

@ -111,9 +111,19 @@ void NavEKF2_core::setWindMagStateLearningMode()
inhibitMagStates = true;
} else if (inhibitMagStates && !setMagInhibit) {
inhibitMagStates = false;
// when commencing use of magnetic field states, set the variances equal to the observation uncertainty
for (uint8_t index=16; index<=21; index++) {
P[index][index] = sq(frontend->_magNoise);
if (magFieldLearned) {
// if we have already learned the field states, then retain the learned variances
P[16][16] = earthMagFieldVar.x;
P[17][17] = earthMagFieldVar.y;
P[18][18] = earthMagFieldVar.z;
P[19][19] = bodyMagFieldVar.x;
P[20][20] = bodyMagFieldVar.y;
P[21][21] = bodyMagFieldVar.z;
} else {
// set the variances equal to the observation variances
for (uint8_t index=16; index<=21; index++) {
P[index][index] = sq(frontend->_magNoise);
}
}
// request a reset of the yaw and magnetic field states if not done before
if (!magStateInitComplete || (!finalInflightMagInit && inFlight)) {

25
libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
View File

@ -265,6 +265,26 @@ void NavEKF2_core::SelectMagFusion()
}
}
// If the final yaw reset has been performed and the state variances are sufficiently low
// record that the earth field has been learned.
bool earthMagFieldConverged = false;
if (!magFieldLearned && finalInflightMagInit) {
earthMagFieldConverged = (P[16][16] < sq(0.01f)) && (P[17][17] < sq(0.01f)) && (P[18][18] < sq(0.01f));
}
if (magFieldLearned || earthMagFieldConverged) {
magFieldLearned = true;
}
// record the last learned field variances
if (magFieldLearned && !inhibitMagStates) {
earthMagFieldVar.x = P[16][16];
earthMagFieldVar.y = P[17][17];
earthMagFieldVar.z = P[18][18];
bodyMagFieldVar.x = P[19][19];
bodyMagFieldVar.y = P[20][20];
bodyMagFieldVar.z = P[21][21];
}
// stop performance timer
hal.util->perf_end(_perf_FuseMagnetometer);
}
@ -1062,6 +1082,11 @@ void NavEKF2_core::FuseDeclination(float declErr)
// align the NE earth magnetic field states with the published declination
void NavEKF2_core::alignMagStateDeclination()
{
// don't do this if we already have a learned magnetic field
if (magFieldLearned) {
return;
}
// get the magnetic declination
float magDecAng = use_compass() ? _ahrs->get_compass()->get_declination() : 0;

73
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -261,6 +261,7 @@ void NavEKF2_core::InitialiseVariables()
posDownAtLastMagReset = stateStruct.position.z;
yawInnovAtLastMagReset = 0.0f;
quatAtLastMagReset = stateStruct.quat;
magFieldLearned = false;
// zero data buffers
storedIMU.reset();
@ -1374,55 +1375,51 @@ Quaternion NavEKF2_core::calcQuatAndFieldStates(float roll, float pitch)
// calculate yaw angle rel to true north
yaw = magDecAng - magHeading;
// calculate initial filter quaternion states using yaw from magnetometer if mag heading healthy
// otherwise use existing heading
if (!badMagYaw) {
// store the yaw change so that it can be retrieved externally for use by the control loops to prevent yaw disturbances following a reset
Vector3f tempEuler;
stateStruct.quat.to_euler(tempEuler.x, tempEuler.y, tempEuler.z);
// this check ensures we accumulate the resets that occur within a single iteration of the EKF
if (imuSampleTime_ms != lastYawReset_ms) {
yawResetAngle = 0.0f;
}
yawResetAngle += wrap_PI(yaw - tempEuler.z);
lastYawReset_ms = imuSampleTime_ms;
// calculate an initial quaternion using the new yaw value
initQuat.from_euler(roll, pitch, yaw);
// zero the attitude covariances becasue the corelations will now be invalid
zeroAttCovOnly();
} else {
initQuat = stateStruct.quat;
// calculate initial filter quaternion states using yaw from magnetometer
// store the yaw change so that it can be retrieved externally for use by the control loops to prevent yaw disturbances following a reset
Vector3f tempEuler;
stateStruct.quat.to_euler(tempEuler.x, tempEuler.y, tempEuler.z);
// this check ensures we accumulate the resets that occur within a single iteration of the EKF
if (imuSampleTime_ms != lastYawReset_ms) {
yawResetAngle = 0.0f;
}
yawResetAngle += wrap_PI(yaw - tempEuler.z);
lastYawReset_ms = imuSampleTime_ms;
// calculate an initial quaternion using the new yaw value
initQuat.from_euler(roll, pitch, yaw);
// zero the attitude covariances becasue the corelations will now be invalid
zeroAttCovOnly();
// calculate initial Tbn matrix and rotate Mag measurements into NED
// to set initial NED magnetic field states
initQuat.rotation_matrix(Tbn);
stateStruct.earth_magfield = Tbn * magDataDelayed.mag;
// don't do this if the earth field has already been learned
if (!magFieldLearned) {
initQuat.rotation_matrix(Tbn);
stateStruct.earth_magfield = Tbn * magDataDelayed.mag;
// align the NE earth magnetic field states with the published declination
alignMagStateDeclination();
// align the NE earth magnetic field states with the published declination
alignMagStateDeclination();
// zero the magnetic field state associated covariances
zeroRows(P,16,21);
zeroCols(P,16,21);
// set initial earth magnetic field variances
P[16][16] = sq(frontend->_magNoise);
P[17][17] = P[16][16];
P[18][18] = P[16][16];
// set initial body magnetic field variances
P[19][19] = sq(frontend->_magNoise);
P[20][20] = P[19][19];
P[21][21] = P[19][19];
// zero the magnetic field state associated covariances
zeroRows(P,16,21);
zeroCols(P,16,21);
// set initial earth magnetic field variances
P[16][16] = sq(frontend->_magNoise);
P[17][17] = P[16][16];
P[18][18] = P[16][16];
// set initial body magnetic field variances
P[19][19] = sq(frontend->_magNoise);
P[20][20] = P[19][19];
P[21][21] = P[19][19];
// clear bad magnetic yaw status
badMagYaw = false;
// clear mag state reset request
magStateResetRequest = false;
}
// record the fact we have initialised the magnetic field states
recordMagReset();
// clear mag state reset request
magStateResetRequest = false;
} else {
// this function should not be called if there is no compass data but if is is, return the
// current attitude

3
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -795,6 +795,9 @@ private:
float posDownObsNoise; // observation noise variance on the vertical position used by the state and covariance update step (m^2)
Vector3f delAngCorrected; // corrected IMU delta angle vector at the EKF time horizon (rad)
Vector3f delVelCorrected; // corrected IMU delta velocity vector at the EKF time horizon (m/s)
bool magFieldLearned; // true when the magnetic field has been learned
Vector3f earthMagFieldVar; // NED earth mag field variances for last learned field (mGauss^2)
Vector3f bodyMagFieldVar; // XYZ body mag field variances for last learned field (mGauss^2)
Vector3f outputTrackError;