From cc05db4985511b2774b32a738481e5bc19dee0b1 Mon Sep 17 00:00:00 2001 From: Roman Date: Tue, 20 Sep 2016 14:07:50 +0200 Subject: [PATCH 1/2] terrain estimator: pass initialisation return value correctly Signed-off-by: Roman --- EKF/ekf.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 14a1e26fa0..25f721aafd 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -411,8 +411,8 @@ bool Ekf::initialiseFilter(void) // initialise the state covariance matrix initialiseCovariance(); - // initialise the terrain estimator - initHagl(); + // try to initialise the terrain estimator + _terrain_initialised = initHagl(); // reset the essential fusion timeout counters _time_last_hgt_fuse = _time_last_imu; From c6e1d97176a866da62b6f1eb391aa0d199d69eb0 Mon Sep 17 00:00:00 2001 From: Roman Date: Tue, 20 Sep 2016 14:09:42 +0200 Subject: [PATCH 2/2] terrain estimator: initialise with projection Signed-off-by: Roman --- EKF/terrain_estimator.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index a6b320f96b..4256398e28 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -47,9 +47,9 @@ bool Ekf::initHagl() // get most recent range measurement from buffer rangeSample latest_measurement = _range_buffer.get_newest(); - if ((_time_last_imu - latest_measurement.time_us) < 2e5) { + if ((_time_last_imu - latest_measurement.time_us) < 2e5 && _R_to_earth(2,2) > 0.7071f) { // if we have a fresh measurement, use it to initialise the terrain estimator - _terrain_vpos = _state.pos(2) + latest_measurement.rng; + _terrain_vpos = _state.pos(2) + latest_measurement.rng * _R_to_earth(2, 2); // initialise state variance to variance of measurement _terrain_var = sq(_params.range_noise); // success