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

AP_NavEKF : Fix bug in reset of position and velocity states 

If the inertial solution velocity or position needs to be reset to the GPS, the stored state history for the corresponding states should also be reset.
Otherwise the next GPS measurement will be compared to an invalid previous state and will be rejected.
The position state should be reset to a GPS position corrected for velocity and measurement latency. This will make a noticeable difference for high speed flight vehicles, eg 11m at 50m/s
This commit is contained in:
priseborough 2014-09-07 09:04:19 +10:00 committed by Andrew Tridgell
parent cb4d5986e0
commit dcb252cc05
1 changed files with 13 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -440,9 +440,19 @@ void NavEKF::ResetVelocity(void)
} else if (_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_3D) {
// read the GPS
readGpsData();
// Set vertical GPS velocity to 0 if mode > 0 (assume 0 if no VZ measurement)
if (_fusionModeGPS >= 1) {
velNED[2] = 0.0f;
// reset horizontal velocity states
if (_fusionModeGPS <= 1) {
states[4] = velNED[0]; // north velocity from blended accel data
states[5] = velNED[1]; // east velocity from blended accel data
states[23] = velNED[0]; // north velocity from IMU1 accel data
states[24] = velNED[1]; // east velocity from IMU1 accel data
states[27] = velNED[0]; // north velocity from IMU2 accel data
states[28] = velNED[1]; // east velocity from IMU2 accel data
// over write stored horizontal velocity states to prevent subsequent GPS measurements from being rejected
for (uint8_t i=0; i<=49; i++){
storedStates[i].velocity[0] = velNED[0];
storedStates[i].velocity[1] = velNED[1];
}
}
// reset filter velocity states
state.velocity = velNED;