mirror of https://github.com/ArduPilot/ardupilot
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
This commit is contained in:
parent
87cd048a91
commit
2603b15354
|
@ -28,6 +28,9 @@ void NavEKF2_core::ResetVelocity(void)
|
||||||
zeroRows(P,3,4);
|
zeroRows(P,3,4);
|
||||||
zeroCols(P,3,4);
|
zeroCols(P,3,4);
|
||||||
|
|
||||||
|
gps_elements gps_corrected = gpsDataNew;
|
||||||
|
CorrectGPSForAntennaOffset(gps_corrected);
|
||||||
|
|
||||||
if (PV_AidingMode != AID_ABSOLUTE) {
|
if (PV_AidingMode != AID_ABSOLUTE) {
|
||||||
stateStruct.velocity.zero();
|
stateStruct.velocity.zero();
|
||||||
// set the variances using the measurement noise parameter
|
// set the variances using the measurement noise parameter
|
||||||
|
@ -35,8 +38,8 @@ void NavEKF2_core::ResetVelocity(void)
|
||||||
} else {
|
} else {
|
||||||
// reset horizontal velocity states to the GPS velocity if available
|
// reset horizontal velocity states to the GPS velocity if available
|
||||||
if (imuSampleTime_ms - lastTimeGpsReceived_ms < 250) {
|
if (imuSampleTime_ms - lastTimeGpsReceived_ms < 250) {
|
||||||
stateStruct.velocity.x = gpsDataNew.vel.x;
|
stateStruct.velocity.x = gps_corrected.vel.x;
|
||||||
stateStruct.velocity.y = gpsDataNew.vel.y;
|
stateStruct.velocity.y = gps_corrected.vel.y;
|
||||||
// set the variances using the reported GPS speed accuracy
|
// set the variances using the reported GPS speed accuracy
|
||||||
P[4][4] = P[3][3] = sq(MAX(frontend->_gpsHorizVelNoise,gpsSpdAccuracy));
|
P[4][4] = P[3][3] = sq(MAX(frontend->_gpsHorizVelNoise,gpsSpdAccuracy));
|
||||||
// clear the timeout flags and counters
|
// clear the timeout flags and counters
|
||||||
|
@ -82,6 +85,9 @@ void NavEKF2_core::ResetPosition(void)
|
||||||
zeroRows(P,6,7);
|
zeroRows(P,6,7);
|
||||||
zeroCols(P,6,7);
|
zeroCols(P,6,7);
|
||||||
|
|
||||||
|
gps_elements gps_corrected = gpsDataNew;
|
||||||
|
CorrectGPSForAntennaOffset(gps_corrected);
|
||||||
|
|
||||||
if (PV_AidingMode != AID_ABSOLUTE) {
|
if (PV_AidingMode != AID_ABSOLUTE) {
|
||||||
// reset all position state history to the last known position
|
// reset all position state history to the last known position
|
||||||
stateStruct.position.x = lastKnownPositionNE.x;
|
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
|
// Use GPS data as first preference if fresh data is available
|
||||||
if (imuSampleTime_ms - lastTimeGpsReceived_ms < 250) {
|
if (imuSampleTime_ms - lastTimeGpsReceived_ms < 250) {
|
||||||
// record the ID of the GPS for the data we are using for the reset
|
// 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
|
// 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.x = gps_corrected.pos.x + 0.001f*gps_corrected.vel.x*(float(imuDataDelayed.time_ms) - float(gps_corrected.time_ms));
|
||||||
stateStruct.position.y = gpsDataNew.pos.y + 0.001f*gpsDataNew.vel.y*(float(imuDataDelayed.time_ms) - float(gpsDataNew.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
|
// set the variances using the position measurement noise parameter
|
||||||
P[6][6] = P[7][7] = sq(MAX(gpsPosAccuracy,frontend->_gpsHorizPosNoise));
|
P[6][6] = P[7][7] = sq(MAX(gpsPosAccuracy,frontend->_gpsHorizPosNoise));
|
||||||
// clear the timeout flags and counters
|
// clear the timeout flags and counters
|
||||||
|
@ -241,6 +247,33 @@ bool NavEKF2_core::resetHeightDatum(void)
|
||||||
return true;
|
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 *
|
* FUSE MEASURED_DATA *
|
||||||
********************************************************/
|
********************************************************/
|
||||||
|
@ -274,25 +307,8 @@ void NavEKF2_core::SelectVelPosFusion()
|
||||||
fusePosData = true;
|
fusePosData = true;
|
||||||
extNavUsedForPos = false;
|
extNavUsedForPos = false;
|
||||||
|
|
||||||
// correct GPS data for position offset of antenna phase centre relative to the IMU
|
// correct for antenna position
|
||||||
Vector3f posOffsetBody = AP::gps().get_antenna_offset(gpsDataDelayed.sensor_idx) - accelPosOffset;
|
CorrectGPSForAntennaOffset(gpsDataDelayed);
|
||||||
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;
|
|
||||||
}
|
|
||||||
|
|
||||||
// copy corrected GPS data to observation vector
|
// copy corrected GPS data to observation vector
|
||||||
if (fuseVelData) {
|
if (fuseVelData) {
|
||||||
|
@ -354,8 +370,8 @@ void NavEKF2_core::SelectVelPosFusion()
|
||||||
posResetNE.y = stateStruct.position.y;
|
posResetNE.y = stateStruct.position.y;
|
||||||
|
|
||||||
// Set the position states to the position from the new GPS
|
// Set the position states to the position from the new GPS
|
||||||
stateStruct.position.x = gpsDataNew.pos.x;
|
stateStruct.position.x = gpsDataDelayed.pos.x;
|
||||||
stateStruct.position.y = gpsDataNew.pos.y;
|
stateStruct.position.y = gpsDataDelayed.pos.y;
|
||||||
|
|
||||||
// Calculate the position offset due to the reset
|
// Calculate the position offset due to the reset
|
||||||
posResetNE.x = stateStruct.position.x - posResetNE.x;
|
posResetNE.x = stateStruct.position.x - posResetNE.x;
|
||||||
|
|
|
@ -775,7 +775,10 @@ private:
|
||||||
|
|
||||||
// update timing statistics structure
|
// update timing statistics structure
|
||||||
void updateTimingStatistics(void);
|
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.
|
// Length of FIFO buffers used for non-IMU sensor data.
|
||||||
// Must be larger than the time period defined by IMU_BUFFER_LENGTH
|
// Must be larger than the time period defined by IMU_BUFFER_LENGTH
|
||||||
static const uint32_t OBS_BUFFER_LENGTH = 5;
|
static const uint32_t OBS_BUFFER_LENGTH = 5;
|
||||||
|
|
Loading…
Reference in New Issue