From 772438f3edd1b0dfa4992839b61e434cf54dca63 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 26 Jan 2020 17:06:58 +1100 Subject: [PATCH] AP_NavEKF3: 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_NavEKF3/AP_NavEKF3_PosVelFusion.cpp | 59 +++++++++++-------- libraries/AP_NavEKF3/AP_NavEKF3_core.h | 5 +- 2 files changed, 40 insertions(+), 24 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index 89762a1af0..3f5378aed6 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp @@ -28,6 +28,9 @@ void NavEKF3_core::ResetVelocity(void) zeroRows(P,4,5); zeroCols(P,4,5); + 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 NavEKF3_core::ResetVelocity(void) } else { // reset horizontal velocity states to the GPS velocity if available if ((imuSampleTime_ms - lastTimeGpsReceived_ms < 250 && velResetSource == DEFAULT) || velResetSource == GPS) { - 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[5][5] = P[4][4] = sq(MAX(frontend->_gpsHorizVelNoise,gpsSpdAccuracy)); // clear the timeout flags and counters @@ -91,13 +94,16 @@ void NavEKF3_core::ResetPosition(void) // set the variances using the position measurement noise parameter P[7][7] = P[8][8] = sq(frontend->_gpsHorizPosNoise); } else { + gps_elements gps_corrected = gpsDataNew; + CorrectGPSForAntennaOffset(gps_corrected); + // 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; + 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[7][7] = P[8][8] = sq(MAX(gpsPosAccuracy,frontend->_gpsHorizPosNoise)); // clear the timeout flags and counters @@ -240,6 +246,28 @@ bool NavEKF3_core::resetHeightDatum(void) return true; } +/* + correct GPS data for position offset of antenna phase centre relative to the IMU + */ +void NavEKF3_core::CorrectGPSForAntennaOffset(gps_elements &gps_data) +{ + const Vector3f &posOffsetBody = AP::gps().get_antenna_offset(gps_data.sensor_idx) - accelPosOffset; + if (posOffsetBody.is_zero()) { + return; + } + 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 -= velOffsetEarth; + } + 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 * ********************************************************/ @@ -261,22 +289,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) { - // 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()) { - 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 -= velOffsetEarth; - } - Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody); - gpsDataDelayed.pos.x -= posOffsetEarth.x; - gpsDataDelayed.pos.y -= posOffsetEarth.y; - gpsDataDelayed.hgt += posOffsetEarth.z; - } - // Don't fuse velocity data if GPS doesn't support it if (frontend->_fusionModeGPS <= 1) { @@ -286,6 +298,7 @@ void NavEKF3_core::SelectVelPosFusion() } fusePosData = true; + CorrectGPSForAntennaOffset(gpsDataDelayed); } else { fuseVelData = false; fusePosData = false; @@ -309,8 +322,8 @@ void NavEKF3_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_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index f0d9dadaec..a5cc29dd30 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -843,7 +843,10 @@ private: // Update the state index limit based on which states are active void updateStateIndexLim(void); - + + // correct GPS data for antenna position + void CorrectGPSForAntennaOffset(gps_elements &gps_data); + // Variables bool statesInitialised; // boolean true when filter states have been initialised bool velHealth; // boolean true if velocity measurements have passed innovation consistency check