From 2cf3ad4a3d5c3650b9fa32f83a8ed9145d32e9bd Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 26 Jan 2020 17:06:58 +1100 Subject: [PATCH] AP_NavEKF2: fixed use of antenna position when switching GPS primary when GPS primary switches we were using a position which had not been corrected for antenna offset. This was used for calculating the reset for sensor change. This fixes that (trivial fix) and also fixes a similar issue on position reset --- .../AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp | 68 ++++++++++++------- libraries/AP_NavEKF2/AP_NavEKF2_core.h | 5 +- 2 files changed, 46 insertions(+), 27 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp index 61d947f4f2..cd0150eee2 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp @@ -28,6 +28,9 @@ void NavEKF2_core::ResetVelocity(void) zeroRows(P,3,4); zeroCols(P,3,4); + gps_elements gps_corrected = gpsDataNew; + CorrectGPSForAntennaOffset(gps_corrected); + if (PV_AidingMode != AID_ABSOLUTE) { stateStruct.velocity.zero(); // set the variances using the measurement noise parameter @@ -35,8 +38,8 @@ void NavEKF2_core::ResetVelocity(void) } else { // reset horizontal velocity states to the GPS velocity if available if (imuSampleTime_ms - lastTimeGpsReceived_ms < 250) { - stateStruct.velocity.x = gpsDataNew.vel.x; - stateStruct.velocity.y = gpsDataNew.vel.y; + stateStruct.velocity.x = gps_corrected.vel.x; + stateStruct.velocity.y = gps_corrected.vel.y; // set the variances using the reported GPS speed accuracy P[4][4] = P[3][3] = sq(MAX(frontend->_gpsHorizVelNoise,gpsSpdAccuracy)); // clear the timeout flags and counters @@ -82,6 +85,9 @@ void NavEKF2_core::ResetPosition(void) zeroRows(P,6,7); zeroCols(P,6,7); + gps_elements gps_corrected = gpsDataNew; + CorrectGPSForAntennaOffset(gps_corrected); + if (PV_AidingMode != AID_ABSOLUTE) { // reset all position state history to the last known position stateStruct.position.x = lastKnownPositionNE.x; @@ -92,10 +98,10 @@ void NavEKF2_core::ResetPosition(void) // Use GPS data as first preference if fresh data is available if (imuSampleTime_ms - lastTimeGpsReceived_ms < 250) { // record the ID of the GPS for the data we are using for the reset - last_gps_idx = gpsDataNew.sensor_idx; + last_gps_idx = gps_corrected.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)); + stateStruct.position.x = gps_corrected.pos.x + 0.001f*gps_corrected.vel.x*(float(imuDataDelayed.time_ms) - float(gps_corrected.time_ms)); + stateStruct.position.y = gps_corrected.pos.y + 0.001f*gps_corrected.vel.y*(float(imuDataDelayed.time_ms) - float(gps_corrected.time_ms)); // set the variances using the position measurement noise parameter P[6][6] = P[7][7] = sq(MAX(gpsPosAccuracy,frontend->_gpsHorizPosNoise)); // clear the timeout flags and counters @@ -241,6 +247,33 @@ bool NavEKF2_core::resetHeightDatum(void) return true; } +/* + correct GPS data for position offset of antenna phase centre relative to the IMU +*/ +void NavEKF2_core::CorrectGPSForAntennaOffset(gps_elements &gps_data) +{ + const Vector3f &posOffsetBody = AP::gps().get_antenna_offset(gpsDataDelayed.sensor_idx) - accelPosOffset; + if (posOffsetBody.is_zero()) { + return; + } + + // Don't fuse velocity data if GPS doesn't support it + if (fuseVelData) { + // TODO use a filtered angular rate with a group delay that matches the GPS delay + Vector3f angRate = imuDataDelayed.delAng * (1.0f/imuDataDelayed.delAngDT); + Vector3f velOffsetBody = angRate % posOffsetBody; + Vector3f velOffsetEarth = prevTnb.mul_transpose(velOffsetBody); + gps_data.vel.x -= velOffsetEarth.x; + gps_data.vel.y -= velOffsetEarth.y; + gps_data.vel.z -= velOffsetEarth.z; + } + + Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody); + gps_data.pos.x -= posOffsetEarth.x; + gps_data.pos.y -= posOffsetEarth.y; + gps_data.hgt += posOffsetEarth.z; +} + /******************************************************** * FUSE MEASURED_DATA * ********************************************************/ @@ -274,25 +307,8 @@ void NavEKF2_core::SelectVelPosFusion() fusePosData = true; extNavUsedForPos = false; - // correct GPS data for position offset of antenna phase centre relative to the IMU - Vector3f posOffsetBody = AP::gps().get_antenna_offset(gpsDataDelayed.sensor_idx) - accelPosOffset; - if (!posOffsetBody.is_zero()) { - // Don't fuse velocity data if GPS doesn't support it - if (fuseVelData) { - // TODO use a filtered angular rate with a group delay that matches the GPS delay - Vector3f angRate = imuDataDelayed.delAng * (1.0f/imuDataDelayed.delAngDT); - Vector3f velOffsetBody = angRate % posOffsetBody; - Vector3f velOffsetEarth = prevTnb.mul_transpose(velOffsetBody); - gpsDataDelayed.vel.x -= velOffsetEarth.x; - gpsDataDelayed.vel.y -= velOffsetEarth.y; - gpsDataDelayed.vel.z -= velOffsetEarth.z; - } - - Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody); - gpsDataDelayed.pos.x -= posOffsetEarth.x; - gpsDataDelayed.pos.y -= posOffsetEarth.y; - gpsDataDelayed.hgt += posOffsetEarth.z; - } + // correct for antenna position + CorrectGPSForAntennaOffset(gpsDataDelayed); // copy corrected GPS data to observation vector if (fuseVelData) { @@ -354,8 +370,8 @@ void NavEKF2_core::SelectVelPosFusion() 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; + stateStruct.position.x = gpsDataDelayed.pos.x; + stateStruct.position.y = gpsDataDelayed.pos.y; // Calculate the position offset due to the reset posResetNE.x = stateStruct.position.x - posResetNE.x; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index 1ce57d38dc..9dd4585035 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -775,7 +775,10 @@ private: // update timing statistics structure void updateTimingStatistics(void); - + + // correct gps data for antenna position + void CorrectGPSForAntennaOffset(gps_elements &gps_data); + // Length of FIFO buffers used for non-IMU sensor data. // Must be larger than the time period defined by IMU_BUFFER_LENGTH static const uint32_t OBS_BUFFER_LENGTH = 5;