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

AP_NavEKF2: Fix timing offset in position reset calculation

Browse Source 
the correction for GPS latency now uses the correct time for that observation
This commit is contained in:
Paul Riseborough 2015-11-06 09:41:42 +11:00
parent 803817821d
commit d6f7156f4e
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
View File

@ -62,8 +62,8 @@ void NavEKF2_core::ResetPosition(void)
stateStruct.position.y = lastKnownPositionNE.y;
} else if (!gpsNotAvailable) {
// 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(lastTimeGpsReceived_ms));
stateStruct.position.y = gpsDataNew.pos.y + 0.001f*gpsDataNew.vel.y*(float(imuDataDelayed.time_ms) - float(lastTimeGpsReceived_ms));
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));
}
for (uint8_t i=0; i<IMU_BUFFER_LENGTH; i++) {
storedOutput[i].position.x = stateStruct.position.x;