fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-27 21:13:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #92 from priseborough/pr-ekf2HgtResetFix

Browse Source 
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
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+1 -3
View File
@@ -184,9 +184,7 @@ void Ekf::controlFusionModes()
_control_status.flags.rng_hgt = false; _control_status.flags.rng_hgt = false;
// adjust the height offset so we can use the GPS // adjust the height offset so we can use the GPS
_hgt_sensor_offset = _state.pos(2) + gps_init.hgt - _gps_alt_ref; _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");
}
} }
} }
EKF/ekf_helper.cpp
+15 -3
View File
@@ -87,11 +87,15 @@ bool Ekf::resetPosition()
// Reset height state using the last height measurement // Reset height state using the last height measurement
void Ekf::resetHeight() void Ekf::resetHeight()
{ {
// Get the most recent GPS data
gpsSample gps_newest = _gps_buffer.get_newest();
if (_control_status.flags.rng_hgt) { if (_control_status.flags.rng_hgt) {
rangeSample range_newest = _range_buffer.get_newest(); rangeSample range_newest = _range_buffer.get_newest();
if (_time_last_imu - range_newest.time_us < 2 * RNG_MAX_INTERVAL) { if (_time_last_imu - range_newest.time_us < 2 * RNG_MAX_INTERVAL) {
_state.pos(2) = _hgt_sensor_offset - range_newest.rng; _state.pos(2) = _hgt_sensor_offset - range_newest.rng;
P[8][8] = sq(_params.range_noise);
} else { } else {
// TODO: reset to last known range based estimate // 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) { if (_time_last_imu - baro_newest.time_us < 2 * BARO_MAX_INTERVAL) {
_state.pos(2) = _hgt_sensor_offset - baro_newest.hgt + _baro_hgt_offset; _state.pos(2) = _hgt_sensor_offset - baro_newest.hgt + _baro_hgt_offset;
P[8][8] = sq(_params.baro_noise);
} else { } else {
// TODO: reset to last known baro based estimate // TODO: reset to last known baro based estimate
@@ -114,11 +119,9 @@ void Ekf::resetHeight()
} else if (_control_status.flags.gps_hgt) { } else if (_control_status.flags.gps_hgt) {
// initialize vertical position and velocity with newest gps measurement // 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) { 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.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 { } else {
// TODO: reset to last known gps based estimate // TODO: reset to last known gps based estimate
@@ -129,6 +132,15 @@ void Ekf::resetHeight()
_baro_hgt_offset = baro_newest.hgt + _state.pos(2); _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 // Reset heading and magnetic field states