dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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 <julian@oes.ch>
This commit is contained in:
kritz 2020-05-20 08:24:23 +02:00 committed by GitHub
parent 716caa5168
commit bf78044bcd
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 5 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
EKF/ekf_helper.cpp
View File

@ -1215,8 +1215,8 @@ void Ekf::update_deadreckoning_status()
_time_last_aiding = _time_last_imu - _params.no_aid_timeout_max; _time_last_aiding = _time_last_imu - _params.no_aid_timeout_max;
} }
// report if we have been deadreckoning for too long // report if we have been deadreckoning for too long, initial state is deadreckoning until aiding is present
_deadreckon_time_exceeded = isTimedOut(_time_last_aiding, (uint64_t)_params.valid_timeout_max); _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 // calculate the inverse rotation matrix from a quaternion rotation

8
test/test_EKF_fusionLogic.cpp
View File

@ -71,11 +71,9 @@ class EkfFusionLogicTest : public ::testing::Test {
TEST_F(EkfFusionLogicTest, doNoFusion) TEST_F(EkfFusionLogicTest, doNoFusion)
{ {
// GIVEN: a tilt and heading aligned filter // GIVEN: a tilt and heading aligned filter
// WHEN: having no aiding source EKF should not have a valid position estimate // WHEN: having no aiding source
// THEN: EKF should not have a valid position estimate
// TODO: for the first 5 second it still has some valid local position EXPECT_FALSE(_ekf->local_position_is_valid());
// that needs to change
EXPECT_TRUE(_ekf->local_position_is_valid());
_sensor_simulator.runSeconds(4); _sensor_simulator.runSeconds(4);