Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: improve angle accuracy during no mag launch

Browse Source
This commit is contained in:
Paul Riseborough 2020-06-08 12:13:49 +10:00 committed by Peter Barker
parent f7c8ee807c
commit d8c2096ad8
3 changed files with 36 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
View File

@ -19,7 +19,7 @@ void NavEKF3_core::controlMagYawReset()
// Vehicles that can use a zero sideslip assumption (Planes) are a special case
// They can use the GPS velocity to recover from bad initial compass data
// This allows recovery for heading alignment errors due to compass faults
if (assume_zero_sideslip() && !finalInflightYawInit && inFlight ) {
if (assume_zero_sideslip() && !finalInflightYawInit && inFlight) {
gpsYawResetRequest = true;
return;
} else {
@ -1499,6 +1499,9 @@ bool NavEKF3_core::EKFGSF_resetMainFilterYaw()
// Fail the magnetomer so it doesn't get used and pull the yaw away from the correct value
allMagSensorsFailed = true;
// record the yaw reset event
recordYawReset();
// reset velocity and position states to GPS - if yaw is fixed then the filter should start to operate correctly
ResetVelocity();
ResetPosition();

39
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -533,15 +533,38 @@ void NavEKF3_core::SelectVelPosFusion()
}
}
// If we are operating without any aiding, fuse in the last known position
// to constrain tilt drift. This assumes a non-manoeuvring vehicle
// Do this to coincide with the height fusion
// If we are operating without any aiding, fuse in constnat position of constant
// velocity measurements to constrain tilt drift. This assumes a non-manoeuvring
// vehicle. Do this to coincide with the height fusion.
if (fuseHgtData && PV_AidingMode == AID_NONE) {
velPosObs[3] = lastKnownPositionNE.x;
velPosObs[4] = lastKnownPositionNE.y;
fusePosData = true;
fuseVelData = false;
if (assume_zero_sideslip() && tiltAlignComplete && motorsArmed) {
// handle special case where we are launching a FW aircraft without magnetometer
fusePosData = false;
velPosObs[0] = 0.0f;
velPosObs[1] = 0.0f;
velPosObs[2] = stateStruct.velocity.z;
bool resetVelNE = !prevMotorsArmed;
// reset states to stop launch accel causing tilt error
if (imuDataDelayed.delVel.x > 1.1f * GRAVITY_MSS * imuDataDelayed.delVelDT) {
lastLaunchAccelTime_ms = imuSampleTime_ms;
fuseVelData = false;
resetVelNE = true;
} else if (lastLaunchAccelTime_ms != 0 && (imuSampleTime_ms - lastLaunchAccelTime_ms) < 10000) {
fuseVelData = false;
resetVelNE = true;
} else {
fuseVelData = true;
}
if (resetVelNE) {
stateStruct.velocity.x = 0.0f;
stateStruct.velocity.y = 0.0f;
}
} else {
fusePosData = true;
fuseVelData = false;
velPosObs[3] = lastKnownPositionNE.x;
velPosObs[4] = lastKnownPositionNE.y;
}
}
// perform fusion

1
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -1040,6 +1040,7 @@ private:
uint32_t lastSynthYawTime_ms; // time stamp when yaw observation was last fused (msec)
uint32_t ekfStartTime_ms; // time the EKF was started (msec)
Vector2f lastKnownPositionNE; // last known position
uint32_t lastLaunchAccelTime_ms;
uint32_t lastDecayTime_ms; // time of last decay of GPS position offset
float velTestRatio; // sum of squares of GPS velocity innovation divided by fail threshold
float posTestRatio; // sum of squares of GPS position innovation divided by fail threshold