dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge pull request #92 from priseborough/pr-ekf2HgtResetFix 

EKF: Fix vulnerability in height reset
This commit is contained in:
Paul Riseborough 2016-04-12 11:09:06 +10:00
parent 61b919754d 006b6b58e4
commit 64e95910c4
2 changed files with 16 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
EKF/control.cpp
View File

@ -184,9 +184,7 @@ void Ekf::controlFusionModes()
_control_status.flags.rng_hgt = false;
// adjust the height offset so we can use the GPS
_hgt_sensor_offset = _state.pos(2) + gps_init.hgt - _gps_alt_ref;
if (!baro_hgt_available) {
printf("EKF baro hgt timeout - switching to gps\n");
}
printf("EKF baro hgt timeout - switching to gps\n");
}
}

18
EKF/ekf_helper.cpp
View File

@ -87,11 +87,15 @@ bool Ekf::resetPosition()
// Reset height state using the last height measurement
void Ekf::resetHeight()
{
// Get the most recent GPS data
gpsSample gps_newest = _gps_buffer.get_newest();
if (_control_status.flags.rng_hgt) {
rangeSample range_newest = _range_buffer.get_newest();
if (_time_last_imu - range_newest.time_us < 2 * RNG_MAX_INTERVAL) {
_state.pos(2) = _hgt_sensor_offset - range_newest.rng;
P[8][8] = sq(_params.range_noise);
} else {
// TODO: reset to last known range based estimate
@ -107,6 +111,7 @@ void Ekf::resetHeight()
if (_time_last_imu - baro_newest.time_us < 2 * BARO_MAX_INTERVAL) {
_state.pos(2) = _hgt_sensor_offset - baro_newest.hgt + _baro_hgt_offset;
P[8][8] = sq(_params.baro_noise);
} else {
// TODO: reset to last known baro based estimate
@ -114,11 +119,9 @@ void Ekf::resetHeight()
} else if (_control_status.flags.gps_hgt) {
// initialize vertical position and velocity with newest gps measurement
gpsSample gps_newest = _gps_buffer.get_newest();
if (_time_last_imu - gps_newest.time_us < 2 * GPS_MAX_INTERVAL) {
_state.pos(2) = _hgt_sensor_offset - gps_newest.hgt + _gps_alt_ref;
_state.vel(2) = gps_newest.vel(2);
P[8][8] = sq(gps_newest.hacc);
} else {
// TODO: reset to last known gps based estimate
@ -129,6 +132,15 @@ void Ekf::resetHeight()
_baro_hgt_offset = baro_newest.hgt + _state.pos(2);
}
// If we are using GPS, then use it to reset the vertical velocity
if (_control_status.flags.gps && (_time_last_imu - gps_newest.time_us < 2 * GPS_MAX_INTERVAL)) {
_state.vel(2) = gps_newest.vel(2);
P[5][5] = sq(1.5f * gps_newest.sacc);
} else {
P[5][5] = fminf(sq(_state.vel(2)),1000.0f);
_state.vel(2) = 0.0f;
}
}
// Reset heading and magnetic field states