Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
This commit is contained in:
Andrew Tridgell 2020-01-26 17:06:58 +11:00
parent 2603b15354
commit d4c6f462bf
2 changed files with 40 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

59
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -27,6 +27,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
@ -34,8 +37,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
@ -90,13 +93,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
@ -239,6 +245,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 *
********************************************************/
@ -260,22 +288,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) {
@ -285,6 +297,7 @@ void NavEKF3_core::SelectVelPosFusion()
}
fusePosData = true;
CorrectGPSForAntennaOffset(gpsDataDelayed);
} else {
fuseVelData = false;
fusePosData = false;
@ -308,8 +321,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;

5
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -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