Hi there,
we are trying to fly into a building with GPS and SLAM (publishing to /mavros/vision_pose) in POSCTL.
When getting inside, GPS quality decreases, EKF2 should stop using it and rely on the external vision pose (SLAM). This works here: log 1 and log 2.
But most of the times, as soon as GPS is considered bad by EKF2, failsafe is enabled stating “no local position”: log 3.
Sometimes also throwing the error “critical navigation failure! check sensor calibration”: log 4 and log 5.
I can’t spot any meaningful differences in the logs.
Any help is much appreciated. Thanks.
Seems the “critical navigation failure” may be caused by estimator_check in Commander.cpp:
There are hard-coded values implying the vehicle needs to be flying for at least 30 seconds (sufficient_time) or faster than 5 m/s (sufficient_speed) for some checks to pass.
This is the case for the logs of the successful tests.
If these checks don’t pass, as soon as pos_test_ratio or vel_test_ratio is > 1, _nav_test_failed is set (“Critical navigation failure!”).
But the vision position data looks pretty good, so maybe there is something wrong with the switching from GPS to EV.
Regarding the other error (no local position): it seems to be thrown in state_machine_helper.cpp in check_invalid_pos_nav_state as soon as condition_local_position_valid is false.
I followed the code and apparently the EKF thinks it’s deadreckoning (EstimatorInterface::local_position_is_valid()).
_is_dead_reckoning is set in Ekf::update_deadreckoning_status() when there is no vel/pos fuse in the last second and _deadreckon_time_exceeded after 5 seconds.
And in fuseVelPosHeight there is no vel/pos fuse when _vel_pos_test_ratio > 1, which indicates some sort of high innovation : state variance ratio.
I was thinking maybe there is a reset from GPS position to EV position, which results in a discontinuity, thus the high ratio. But I’ve been studying this log (in which we were testing the 30 seconds hypothesis) and there is a reset/discontinuity of the EKF position states, but only at ~8 seconds after stopping GPS use.
pos_test_ratio already increases wildly in the exact moment when stopping GPS use.
Edit: OK, maybe everything is caused by said discontinuity:
In Ekf::controlGpsFusion the gps control flag is set to false as soon as GPS checks are failing and EV is fused (“EKF GPS data quality poor - stopping use”), but nothing else - I would expect something like resetVelocity() and/or resetPosition().
In Ekf::controlExternalVisionFusion there is _fuse_hpos_as_odom = false whenever EV is used and GPS is not.
Again, no resets.
Later in that method: _vel_pos_innov[3] = _state.pos(0) - _ev_sample_delayed.posNED(0);
But if I understand correctly, _state.pos is still in GPS coordinates, so the innovation is subject to a discontinuity -> huge values -> pos_test_ratio explodes -> no more pos fusion whatsoever.
