EKF2: optical flow refactoring #22377
Conversation
bresch
force-pushed
the
pr-ekf2-flow-refactor
branch
from
0f650fd
to
67e5dcb
Compare
|
Short indoor flight replay comparison. The navigation result is the same, except pre-flight where main is constantly resetting the position to 0 while this PR lets the EKF navigate using flow even before arming. This pr: Main: |
|
Replayed with the flow gyro set to NAN, the result is also almost identical |
| @@ -66,6 +66,10 @@ void Ekf::runTerrainEstimator(const imuSample &imu_delayed) | |||
| if (!_control_status.flags.in_air) { | |||
| _last_on_ground_posD = _state.pos(2); | |||
| _control_status.flags.rng_fault = false; | |||
|
|
|||
| } else if (!_control_status_prev.flags.in_air) { | |||
| // Let the estimator run freely before arming for bench testing purposes, but reset on takeoff | |||
There was a problem hiding this comment.
May I ask why this reset is required in transition to takeoff?
There was a problem hiding this comment.
Because I've seen cases where a distance sensor would report a high valid data (~3m) while on ground and since since the velocity = flow_rate * distance, it's safer to always takeoff with a small distance estimate.
If the distance is under-estimated, the drone will drift a bit, but if it's over-estimated it can directly diverge, which is much more dangerous.
There was a problem hiding this comment.
I added a comment describing this directly in the code.
|
Outdoor flights:
No issues, good flight behavior. Velocity accuracy compared to GNSS (EKF2 only fuses optical flow) |
bresch
force-pushed
the
pr-ekf2-flow-refactor
branch
from
b8769d0
to
caab5c2
Compare
|
Cleaned-up the commit history and rebased on main. |
Use flow rates instead of integrals in backend. This allows us to delay the data to the mitpoint integration time and simplifies the code in general. Gyro compensation can still be done in EKF2 if needed, but the flow module normally already appends the correct gyro data to the flow message.
bresch
force-pushed
the
pr-ekf2-flow-refactor
branch
from
caab5c2
to
c3df891
Compare
required to parse larger topics
bresch
force-pushed
the
pr-ekf2-flow-refactor
branch
from
2bba379
to
8ef449d
Compare
replaces #21005
Solved Problem
The optical flow control logic is way too complicated to read, has a lot of strange behaviors due to the multiple possible paths in the code and several strange legacy checks.
Furthermore, it tries to compensate the flow due to angular motion by popping the data from the buffer early to be able to accumulate gyro data during the flow integration interval, which makes the code even more complicated. I also think that it wasn't working as expected and that most of the time, only 1 gyro sample is available between the start of the integration and the midpoint (where we want to fuse it), so IMO, there is no real benefit of doing it this way. Also note that this integration is also now done in
VehicleOpticalFlow.Solution
Changelog Entry
For release notes:
Test coverage