From 2a88fc6cfdd218d437b63ae59d1c8526dc001914 Mon Sep 17 00:00:00 2001 From: Roman Date: Tue, 5 Jan 2016 07:44:37 +0100 Subject: [PATCH] initialise vertical position correctly --- EKF/ekf.cpp | 8 ++++++++ EKF/ekf_helper.cpp | 2 +- 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index cca1996cb8..c9370d01af 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -172,6 +172,14 @@ bool Ekf::initialiseFilter(void) resetVelocity(); resetPosition(); + // initialize vertical position with newest baro measurement + baroSample baro_init = _baro_buffer.get_newest(); + if (baro_init.time_us == 0) { + return false; + } + + _state.pos(2) = -baro_init.hgt; + initialiseCovariance(); return true; diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 080454a2cf..1bf6bb28dd 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -78,7 +78,7 @@ void Ekf::resetPosition() } baroSample baro_newest = _baro_buffer.get_newest(); - _state.pos(2) = baro_newest.hgt; + _state.pos(2) = -baro_newest.hgt; } #if defined(__PX4_POSIX) && !defined(__PX4_QURT)