EKF: Fix non GPS aiding data reset logic #418
Conversation
| bool do_reset = _time_last_imu - _time_last_of_fuse > _params.no_gps_timeout_max; | ||
| if (do_reset) { | ||
| resetVelocity(); | ||
| resetPosition(); |
There was a problem hiding this comment.
I will remove those info print statements. We can tell from the log file if optical flow use is stopped or the EKF stops receiving optical flow data.
EKF/ekf_helper.cpp
Outdated
| @@ -125,6 +155,20 @@ bool Ekf::resetPosition() | |||
| // use EV accuracy to reset variances | |||
| setDiag(P,7,8,sq(_ev_sample_delayed.posErr)); | |||
|
|
|||
| } else if (_control_status.flags.opt_flow) { | |||
| if (!_control_status.flags.in_air) { | |||
| // we are likely starting OF for the first time so reset the horizontal position and vertical velocity states | |||
There was a problem hiding this comment.
Comment is inconsistent with what is done below. Where are we resetting vertical velocity?
There was a problem hiding this comment.
yes, I found that when horizontal velocity is diverged, the vertical velocity is also not correct sometimes.
There was a problem hiding this comment.
Will fix comment
If data is only source of aiding and has been rejected for too long - reset using flow data as a velocity reference. If flow data is unavailable for too long - declare optical flow use stopped. Use consistent time periods for all resets
If data is only source of aiding and has been rejected for too long - reset using data as a position. Don't reset velocity if there is another source of aiding constraining it. If data is unavailable for too long, declare external vision use stopped. Use consistent time periods for all resets.
Make the distinction between the no_gps_timeout_max and no_aid_timeout_max parameters clearer
Reset estimate to sensor value if rejected for 10 seconds Protect against user motion when on ground. Fix unnecessary duplication of terrain validity check and separate validity update and reporting.
priseborough
force-pushed
the
pr-ekfNavReset
branch
from
7b4199f to
849613d
Compare
|
Replay test log here: https://logs.px4.io/plot_app?log=bbbeaf62-23a6-4980-9bc8-991386e6f469 |
Optical flow use information can be obtained from the estimator_status.control_mode_flags message
|
@priseborough could you merge this pr? I have test it, everything work as expected. |
EKF/terrain_estimator.cpp
Outdated
| return true; | ||
|
|
||
| } else { | ||
| return false; | ||
| } | ||
| } | ||
|
|
||
| // determine terrain validity | ||
| void Ekf::update_terrain_valid() |
There was a problem hiding this comment.
This function does almost the same as get_terrain_valid(). Can we reuse that one to set _hagl_valid otherwise we need to add/remove checks in both locations if we change something.
This fixes the following problems with use of optical flow and external vision data.
Both the optical flow and external vision data fusion logic were missing functionality required to ensure that the EKF would reset to the observation if innovation checks failed for too long and the data was the only source of aiding.
The velocity reset method did not handle optical flow measurements and instead initialised velocity to zero if optical flow data was the aiding source.
The velocity reset function did not separate the optical flow and external vision use cases properly.
The position reset function selected optical flow data before external vision data, even though optical flow data does not provide position information.
The timeouts used to declare stoppage of optical flow and external vision use were hardcoded.
If the vehicle is on ground and shaken and / or rotated rapidly whilst relying on optical flow for aiding, the estimator would diverge and have to reset.
Testing https://logs.px4.io/plot_app?log=bbbeaf62-23a6-4980-9bc8-991386e6f469