From da1de2cc4d63f430b9f1810a3225855b18fb13f2 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 10 Feb 2016 10:02:30 +1100 Subject: [PATCH] EKF: Correct for sensor noise and baro offset during alignment --- EKF/ekf.cpp | 3 +-- EKF/ekf_helper.cpp | 8 +------- 2 files changed, 2 insertions(+), 9 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 32e4b87c47..8a9987dfea 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -272,10 +272,9 @@ 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; diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 5796cd8628..56f11c3be9 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -85,13 +85,7 @@ 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 - } + _state.pos(2) = _baro_at_alignment - baro_newest.hgt; } #if defined(__PX4_POSIX) && !defined(__PX4_QURT)