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
parent 40e174b81c
commit fff2bd50f6
3 changed files with 23 additions and 7 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
_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();
resetPosition();
// initialise the state covariance matrix
initialiseCovariance();
return true;

2
EKF/ekf.h
View File

@ -181,6 +181,8 @@ private:
void resetPosition();
void resetHeight();
void makeCovSymetrical();
void limitCov();

23
EKF/ekf_helper.cpp
View File

@ -54,7 +54,7 @@ void Ekf::resetVelocity()
// if we have a valid GPS measurement use it to initialise velocity states
gpsSample gps_newest = _gps_buffer.get_newest();
if (_time_last_imu - gps_newest.time_us < 100000) {
if (_time_last_imu - gps_newest.time_us < 400000) {
_state.vel = gps_newest.vel;
} else {
@ -66,19 +66,32 @@ void Ekf::resetVelocity()
// gps measurement then use for position initialisation
void Ekf::resetPosition()
{
// if we have a valid GPS measurement use it to initialise position states
// if we have a fresh GPS measurement, use it to initialise position states and correct the position for the measurement delay
gpsSample gps_newest = _gps_buffer.get_newest();
if (_time_last_imu - gps_newest.time_us < 100000) {
_state.pos(0) = gps_newest.pos(0);
_state.pos(1) = gps_newest.pos(1);
float time_delay = 1e-6f * (float)(_time_last_imu - gps_newest.time_us);
if (time_delay < 0.4f) {
_state.pos(0) = gps_newest.pos(0) + gps_newest.vel(0) * time_delay;
_state.pos(1) = gps_newest.pos(1) + gps_newest.vel(1) * time_delay;
} else {
// XXX use the value of the last known position
}
}
// Reset height state using the last baro measurement
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();
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)