AP_NavEKF3: handle position jump caused by change in GPS receiver

If the GPS receiver selection changes and we are using GPS for height, the vertical position will be reset to the new GPS height measurement.
correct output observer history when doing a GPS height reset
This commit is contained in:
priseborough 2017-01-28 22:01:35 +11:00 committed by Randy Mackay
parent 8c46d4316b
commit 01551a4423
3 changed files with 67 additions and 10 deletions

View File

@ -92,6 +92,8 @@ void NavEKF3_core::ResetPosition(void)
} else {
// Use GPS data as first preference if fresh data is available
if ((imuSampleTime_ms - lastTimeGpsReceived_ms < 250 && posResetSource == DEFAULT) || posResetSource == GPS) {
// record the ID of the GPS for the data we are using for the reset
last_gps_idx = gpsDataNew.sensor_idx;
// write to state vector and compensate for offset between last GPS measurement and the EKF time horizon
stateStruct.position.x = gpsDataNew.pos.x + 0.001f*gpsDataNew.vel.x*(float(imuDataDelayed.time_ms) - float(gpsDataNew.time_ms));
stateStruct.position.y = gpsDataNew.pos.y + 0.001f*gpsDataNew.vel.y*(float(imuDataDelayed.time_ms) - float(gpsDataNew.time_ms));
@ -239,14 +241,6 @@ void NavEKF3_core::SelectVelPosFusion()
gpsDataToFuse = storedGPS.recall(gpsDataDelayed,imuDataDelayed.time_ms);
// Determine if we need to fuse position and velocity data on this time step
if (gpsDataToFuse && PV_AidingMode == AID_ABSOLUTE) {
// Don't fuse velocity data if GPS doesn't support it
if (frontend->_fusionModeGPS <= 1) {
fuseVelData = true;
} else {
fuseVelData = false;
}
fusePosData = true;
// correct GPS data for position offset of antenna phase centre relative to the IMU
Vector3f posOffsetBody = _ahrs->get_gps().get_antenna_offset(gpsDataDelayed.sensor_idx) - accelPosOffset;
if (!posOffsetBody.is_zero()) {
@ -262,6 +256,16 @@ void NavEKF3_core::SelectVelPosFusion()
gpsDataDelayed.pos.y -= posOffsetEarth.y;
gpsDataDelayed.hgt += posOffsetEarth.z;
}
// Don't fuse velocity data if GPS doesn't support it
if (frontend->_fusionModeGPS <= 1) {
fuseVelData = true;
} else {
fuseVelData = false;
}
fusePosData = true;
} else {
fuseVelData = false;
fusePosData = false;
@ -275,6 +279,59 @@ void NavEKF3_core::SelectVelPosFusion()
// Select height data to be fused from the available baro, range finder and GPS sources
selectHeightForFusion();
// if we are using GPS, check for a change in receiver and reset position and height
if (gpsDataToFuse && PV_AidingMode == AID_ABSOLUTE && gpsDataDelayed.sensor_idx != last_gps_idx) {
// record the ID of the GPS that we are using for the reset
last_gps_idx = gpsDataDelayed.sensor_idx;
// Store the position before the reset so that we can record the reset delta
posResetNE.x = stateStruct.position.x;
posResetNE.y = stateStruct.position.y;
// Set the position states to the position from the new GPS
stateStruct.position.x = gpsDataNew.pos.x;
stateStruct.position.y = gpsDataNew.pos.y;
// Calculate the position offset due to the reset
posResetNE.x = stateStruct.position.x - posResetNE.x;
posResetNE.y = stateStruct.position.y - posResetNE.y;
// Add the offset to the output observer states
for (uint8_t i=0; i<imu_buffer_length; i++) {
storedOutput[i].position.x += posResetNE.x;
storedOutput[i].position.y += posResetNE.y;
}
outputDataNew.position.x += posResetNE.x;
outputDataNew.position.y += posResetNE.y;
outputDataDelayed.position.x += posResetNE.x;
outputDataDelayed.position.y += posResetNE.y;
// store the time of the reset
lastPosReset_ms = imuSampleTime_ms;
// If we are alseo using GPS as the height reference, reset the height
if (activeHgtSource == HGT_SOURCE_GPS) {
// Store the position before the reset so that we can record the reset delta
posResetD = stateStruct.position.z;
// write to the state vector
stateStruct.position.z = -hgtMea;
// Calculate the position jump due to the reset
posResetD = stateStruct.position.z - posResetD;
// Add the offset to the output observer states
outputDataNew.position.z += posResetD;
outputDataDelayed.position.z += posResetD;
for (uint8_t i=0; i<imu_buffer_length; i++) {
storedOutput[i].position.z += posResetD;
}
// store the time of the reset
lastPosResetD_ms = imuSampleTime_ms;
}
}
// 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

View File

@ -249,7 +249,7 @@ void NavEKF3_core::InitialiseVariables()
baroStoreIndex = 0;
rangeStoreIndex = 0;
magStoreIndex = 0;
gpsStoreIndex = 0;
last_gps_idx = 0;
tasStoreIndex = 0;
ofStoreIndex = 0;
delAngCorrection.zero();

View File

@ -848,7 +848,7 @@ private:
uint8_t magStoreIndex; // Magnetometer data storage index
gps_elements gpsDataNew; // GPS data at the current time horizon
gps_elements gpsDataDelayed; // GPS data at the fusion time horizon
uint8_t gpsStoreIndex; // GPS data storage index
uint8_t last_gps_idx; // sensor ID of the GPS receiver used for the last fusion or reset
output_elements outputDataNew; // output state data at the current time step
output_elements outputDataDelayed; // output state data at the current time step
Vector3f delAngCorrection; // correction applied to delta angles used by output observer to track the EKF