From bf78044bcd71eda54e541142e8df691770247f92 Mon Sep 17 00:00:00 2001 From: kritz Date: Wed, 20 May 2020 08:24:23 +0200 Subject: [PATCH] EKF: fix initialization of local position validity 2 (#820) The `_deadreckon_time_exceeded` flag is used in `local_position_is_valid()`. This means that `_params.valid_timeout_max` after startup, in my observed case 5 seconds, the local position switche from valid to invalid and then after a while back to valid again. With this fix, the local position is flagged invalid from boot and gets validated after the first aiding event. Co-authored-by: Julian Oes --- EKF/ekf_helper.cpp | 4 ++-- test/test_EKF_fusionLogic.cpp | 8 +++----- 2 files changed, 5 insertions(+), 7 deletions(-) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 57c4d45a30..53f5d1e77a 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -1215,8 +1215,8 @@ void Ekf::update_deadreckoning_status() _time_last_aiding = _time_last_imu - _params.no_aid_timeout_max; } - // report if we have been deadreckoning for too long - _deadreckon_time_exceeded = isTimedOut(_time_last_aiding, (uint64_t)_params.valid_timeout_max); + // report if we have been deadreckoning for too long, initial state is deadreckoning until aiding is present + _deadreckon_time_exceeded = (_time_last_aiding == 0) || isTimedOut(_time_last_aiding, (uint64_t)_params.valid_timeout_max); } // calculate the inverse rotation matrix from a quaternion rotation diff --git a/test/test_EKF_fusionLogic.cpp b/test/test_EKF_fusionLogic.cpp index 2e69c53d6c..5db60989ee 100644 --- a/test/test_EKF_fusionLogic.cpp +++ b/test/test_EKF_fusionLogic.cpp @@ -71,11 +71,9 @@ class EkfFusionLogicTest : public ::testing::Test { TEST_F(EkfFusionLogicTest, doNoFusion) { // GIVEN: a tilt and heading aligned filter - // WHEN: having no aiding source EKF should not have a valid position estimate - - // TODO: for the first 5 second it still has some valid local position - // that needs to change - EXPECT_TRUE(_ekf->local_position_is_valid()); + // WHEN: having no aiding source + // THEN: EKF should not have a valid position estimate + EXPECT_FALSE(_ekf->local_position_is_valid()); _sensor_simulator.runSeconds(4);