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

EKF: Fix bugs in position and velocity resets 

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-10 12:53:24 +11:00 committed by Roman
parent da1de2cc4d
commit 9d7340e187
2 changed files with 9 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
EKF/ekf.cpp
View File

@ -272,9 +272,10 @@ bool Ekf::initialiseFilter(void)
// calculate the averaged barometer reading // calculate the averaged barometer reading
_baro_at_alignment = _baro_sum / (float)_baro_counter; _baro_at_alignment = _baro_sum / (float)_baro_counter;
// set the velocity to the GPS measurement (by definition, the initial position and height is at the origin)
resetVelocity(); resetVelocity();
resetPosition();
// initialise the state covariance matrix
initialiseCovariance(); initialiseCovariance();
return true; return true;

8
EKF/ekf_helper.cpp
View File

@ -85,7 +85,13 @@ void Ekf::resetHeight()
{ {
// if we have a valid height measurement, use it to initialise the vertical position state // if we have a valid height measurement, use it to initialise the vertical position state
baroSample baro_newest = _baro_buffer.get_newest(); 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) #if defined(__PX4_POSIX) && !defined(__PX4_QURT)