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

AP_NavEKF3: CorrectGPSForAntennaOffset always corrects vel

This commit is contained in:
Randy Mackay 2020-10-22 09:12:12 +09:00
parent e25579cc00
commit 3d4e1cd5c5
1 changed files with 7 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -327,13 +327,13 @@ void NavEKF3_core::CorrectGPSForAntennaOffset(gps_elements &gps_data) const
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;