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

AP_NavEKF: Do not reset vertical velocity state from GPS

Browse Source 
Doing this can cause large height and height rate errors if large GPS velocity errors cause the GPS tn be rejected for long enough to cause a timeout and reset of states.
This commit is contained in:
priseborough 2014-12-13 18:30:45 +11:00 committed by Andrew Tridgell
parent 597273cfff
commit f6ce25df2a
1 changed files with 12 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -488,40 +488,30 @@ void NavEKF::ResetPosition(void)
}
}
// resets velocity states to last GPS measurement or to zero if in static mode
// Reset velocity states to last GPS measurement if available or to zero if in static mode
// Do not reset vertical velocity using GPS as there is baro alt available to constrain drift
void NavEKF::ResetVelocity(void)
{
if (staticMode) {
state.velocity.zero();
state.vel1.zero();
state.vel2.zero();
} else if (_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_3D) {
} else if (_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_3D && _fusionModeGPS <= 1) {
// read the GPS
readGpsData();
// 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;
state.vel1 = velNED;
state.vel2 = velNED;
// reset stored velocity states to prevent subsequent GPS measurements from being rejected
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 = velNED;
storedStates[i].velocity[0] = velNED[0];
storedStates[i].velocity[1] = velNED[1];
}
}
}
// reset the vertical position state using the last height measurement