AP_NavEKF2: Don't start GPS aiding until gyro calibration has stabilised
If we start GPS aiding before the gyro bias variances have reduced, glitches on the GPS can cause attitude disturbances that degrade flight accuracy during early flight.
This commit is contained in:
parent
601ab9dad0
commit
aaab250f13
@ -160,13 +160,17 @@ void NavEKF2_core::setAidingMode()
|
||||
// Determine when to commence aiding for inertial navigation
|
||||
// Save the previous status so we can detect when it has changed
|
||||
prevIsAiding = isAiding;
|
||||
// Don't allow filter to start position or velocity aiding until the tilt and yaw alignment is complete
|
||||
bool filterIsStable = tiltAlignComplete && yawAlignComplete;
|
||||
// If GPS usage has been prohiited then we use flow aiding provided optical flow data is present
|
||||
bool useFlowAiding = (frontend->_fusionModeGPS == 3) && optFlowDataPresent();
|
||||
// Start aiding if we have a source of aiding data and the filter attitude algnment is complete
|
||||
// Latch to on
|
||||
isAiding = ((readyToUseGPS() || useFlowAiding) && filterIsStable) || isAiding;
|
||||
// perform aiding checks if not aiding
|
||||
if (!isAiding) {
|
||||
// Don't allow filter to start position or velocity aiding until the tilt and yaw alignment is complete
|
||||
// and IMU gyro bias estimates have stabilised
|
||||
bool filterIsStable = tiltAlignComplete && yawAlignComplete && imuCalCompleted();
|
||||
// If GPS usage has been prohiited then we use flow aiding provided optical flow data is present
|
||||
bool useFlowAiding = (frontend->_fusionModeGPS == 3) && optFlowDataPresent();
|
||||
// Start aiding if we have a source of aiding data and the filter attitude algnment is complete
|
||||
// Latch to on
|
||||
isAiding = (readyToUseGPS() || useFlowAiding) && filterIsStable;
|
||||
}
|
||||
|
||||
// check to see if we are starting or stopping aiding and set states and modes as required
|
||||
if (isAiding != prevIsAiding) {
|
||||
@ -323,6 +327,19 @@ void NavEKF2_core::recordYawReset()
|
||||
}
|
||||
}
|
||||
|
||||
// return true when IMU calibration completed
|
||||
bool NavEKF2_core::imuCalCompleted(void)
|
||||
{
|
||||
// check delta angle bias variances
|
||||
const float delAngBiasVarMax = sq(radians(0.1f * dtEkfAvg));
|
||||
bool gyroCalComplete = (P[9][9] <= delAngBiasVarMax) &&
|
||||
(P[10][10] <= delAngBiasVarMax) &&
|
||||
(P[11][11] <= delAngBiasVarMax);
|
||||
|
||||
return gyroCalComplete;
|
||||
|
||||
}
|
||||
|
||||
// Commands the EKF to not use GPS.
|
||||
// This command must be sent prior to arming
|
||||
// This command is forgotten by the EKF each time the vehicle disarms
|
||||
|
@ -586,6 +586,9 @@ private:
|
||||
// Assess GPS data quality and return true if good enough to align the EKF
|
||||
bool calcGpsGoodToAlign(void);
|
||||
|
||||
// return true when IMU calibration completed
|
||||
bool imuCalCompleted(void);
|
||||
|
||||
// update inflight calculaton that determines if GPS data is good enough for reliable navigation
|
||||
void calcGpsGoodForFlight(void);
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user