aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF2: Don't correct GPS for zero position offset

Browse Source
This commit is contained in:
priseborough 2016-10-15 09:19:02 +11:00 committed by Andrew Tridgell
parent f025c96e63
commit 1890bbdafa
1 changed files with 13 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
View File

@ -192,28 +192,26 @@ void NavEKF2_core::SelectVelPosFusion()
// Don't fuse velocity data if GPS doesn't support it
if (frontend->_fusionModeGPS <= 1) {
fuseVelData = true;
} else {
fuseVelData = false;
}
fusePosData = true;
// correct GPS velocity data for position offset relative to the IMU
// TOTO use a filtered angular rate with a group delay that matches the GPS delay
// correct GPS data for position offset of antenna phase centre relative to the IMU
Vector3f posOffsetBody = gpsDataDelayed.body_offset - accelPosOffset;
if (fuseVelData) {
Vector3f angRate = imuDataDelayed.delAng * (1.0f/imuDataDelayed.delAngDT);
Vector3f velOffsetBody = angRate % posOffsetBody;
Vector3f velOffsetEarth = prevTnb.mul_transpose(velOffsetBody);
gpsDataDelayed.vel -= velOffsetEarth;
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;
}
// correct GPS position data for position offset relative to the IMU
Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
gpsDataDelayed.pos.x -= posOffsetEarth.x;
gpsDataDelayed.pos.y -= posOffsetEarth.y;
gpsDataDelayed.hgt += posOffsetEarth.z;
} else {
fuseVelData = false;
fusePosData = false;