Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 16:23:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF : Fix bug in reset of position, height and velocity states

Browse Source 
If the inertial solution velocity or position needs to be reset to the GPS or baro, the stored state history for the corresponding states should also be reset.
Otherwise the next GPS or baro measurement will be compared to an invalid previous state and will be rejected. This is particularly a problem if IMU saturation or timeout has occurred because the previous states could be out by a large amount
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-17 04:52:23 +10:00 committed by Randy Mackay
parent 20d35b4bd1
commit 1f1670279b
1 changed files with 30 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -407,12 +407,17 @@ void NavEKF::ResetPosition(void)
// read the GPS
readGpsData();
// write to state vector
states[7] = gpsPosNE.x + gpsPosGlitchOffsetNE.x;
states[8] = gpsPosNE.y + gpsPosGlitchOffsetNE.y;
// write to state vector and compensate for GPS latency
states[7] = gpsPosNE.x + gpsPosGlitchOffsetNE.x + 0.001f*velNED.x*float(_msecPosDelay);
states[8] = gpsPosNE.y + gpsPosGlitchOffsetNE.y + 0.001f*velNED.y*float(_msecPosDelay);
}
// reset the glitch ofset correction states
gpsPosGlitchOffsetNE.zero();
// stored horizontal position states to prevent subsequent GPS measurements from being rejected
for (uint8_t i=0; i<=49; i++){
storedStates[i].position[0] = state.position[0];
storedStates[i].position[1] = state.position[1];
}
}
// resets velocity states to last GPS measurement or to zero if in static mode
@ -425,20 +430,25 @@ void NavEKF::ResetVelocity(void)
} else if (_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_3D) {
// 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
// Set vertical GPS velocity to 0 if mode > 0 (assume 0 if no VZ measurement)
if (_fusionModeGPS >= 1) {
velNED[2] = 0.0f;
}
// reset vertical velocity states
if (_fusionModeGPS <= 0) {
states[6] = velNED[2]; // down velocity from blended accel data
states[25] = velNED[2]; // down velocity from IMU1 accel data
states[29] = velNED[2]; // down velocity from IMU2 accel data
// reset filter velocity states
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
states[6] = velNED[2]; // down velocity from blended accel data
states[25] = velNED[2]; // down velocity from IMU1 accel data
states[29] = velNED[2]; // down velocity from IMU2 accel data
// reset stored 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];
storedStates[i].velocity[2] = velNED[2];
}
}
}
@ -452,6 +462,10 @@ void NavEKF::ResetHeight(void)
states[9] = -hgtMea; // down position from blended accel data
state.posD1 = -hgtMea; // down position from IMU1 accel data
state.posD2 = -hgtMea; // down position from IMU2 accel data
// reset stored vertical position states to prevent subsequent GPS measurements from being rejected
for (uint8_t i=0; i<=49; i++){
storedStates[i].position[2] = -hgtMea;
}
}
// this function is used to initialise the filter whilst moving, using the AHRS DCM solution