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

EKF: Fix bugs in position and velocity resets

Browse Source 
The position reset was not being compensated for velocity and measurement delay
The height was being reset with the position. It has been moved into a separate reset function
The maximum accepted GPS delay of 100msec was inadequate
The states  was being incorrectly reset to the GPS position and Baro height on initial alignment.
This commit is contained in:
Paul Riseborough
2016-02-14 09:49:26 +01:00
committed by Roman
parent da1de2cc4d
commit 9d7340e187
2 changed files with 9 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf_helper.cpp
+7 -1
View File
@@ -85,7 +85,13 @@ void Ekf::resetHeight()
{
// if we have a valid height measurement, use it to initialise the vertical position state
baroSample baro_newest = _baro_buffer.get_newest();
_state.pos(2) = _baro_at_alignment - baro_newest.hgt;
if (_time_last_imu - baro_newest.time_us < 200000) {
_state.pos(2) = _baro_at_alignment - baro_newest.hgt;
} else {
// XXX use the value of the last known position
}
}
#if defined(__PX4_POSIX) && !defined(__PX4_QURT)