Support full vision variance #680
Conversation
EKF/control.cpp
Outdated
| _ev_vel_innov(0) = _state.vel(0) - vel_aligned(0); | ||
| _ev_vel_innov(1) = _state.vel(1) - vel_aligned(1); | ||
| _ev_vel_innov(2) = _state.vel(2) - vel_aligned(2); | ||
| _ev_vel_innov= _state.vel - vel_aligned; |
There was a problem hiding this comment.
| _ev_vel_innov= _state.vel - vel_aligned; | |
| _ev_vel_innov = _state.vel - vel_aligned; |
|
This is awesome! This came up sometime back for me, when I was working on fast flights down long tunnels with vision. Assuming isotropic measurement uncertainty is not a good assumption especially for visual-inertial odometry systems where the error strongly depends on direction of motion and feature distribution. Another issue I ran into after implementing this was that the covariances too need to be rotated into the navigation frame (when using NED for GPS and an arbitrary vision frame) before being used. I think this is missing in this PR, but could've missed it on mobile. |
|
Also need to review that switching the auxvel interface to 3D doesn't have unexpected effects on the height when the external system doesn't provide a Z vel estimate (like the current landing target estimator implementation) |
@mhkabir I missed that, thanks for catching that. It is now added. Please have a look again. I do not have vision system with proper variance information, so I can not really test this at the moment. |
@mhkabir IMO this should be handled by the EKF2_AID_MASK parameter at some point. Such that you configure which components you want to fuse. At the moment the vertical component fusion is commented out. |
| @@ -305,7 +305,10 @@ void Ekf::controlExternalVisionFusion() | |||
| _ev_pos_innov(1) = _state.pos(1) - _hpos_pred_prev(1) - ev_delta_pos(1); | |||
|
|
|||
| // observation 1-STD error, incremental pos observation is expected to have more uncertainty | |||
| ev_pos_obs_var(0) = ev_pos_obs_var(1) = sq(fmaxf(_ev_sample_delayed.posErr, 0.5f)); | |||
| Matrix3f ev_pos_var = matrix::diag(_ev_sample_delayed.posVar); | |||
There was a problem hiding this comment.
Why not simply
Vector3f ev_pos_var = _ev_rot_mat * _ev_sample_delayed.posVar?
There was a problem hiding this comment.
OK I guess it's nicer to have it as a matrix if we ever end up actually propagating the covariances.
kamilritz
force-pushed
the
ev_variance
branch
from
da6f55f to
6aeef20
Compare
kamilritz
force-pushed
the
ev_variance
branch
2 times, most recently
from
41fc265 to
75099a3
Compare
|
@bresch Squash please next time 😢 Bisection is already near-impossible |
|
@mhkabir oops, sorry for that.; I'll check how the commits look next time. |
This PR add support to use all variance information in the visual_odometry message. It uses a 3d variance for position and velocity. Since we only fuse visual heading and not attitude, we do only support a one-dimensional uncertainty for the heading.
Since external vision fusion in ECL is still fusing single measurement sequentially, we can not profit from the covariance values at the moment, so they are not added to the interface.
Secondly, the interface is expanded such that a 3D auxiliary velocity vector is passed instead of 2d vector. The vertical velocity is not fused at the moment. Since the control logic is not in place at the moment. This will be added, when we add finer fusion control granularity for all sensors.
See here for the corresponding Firmware PR.
Unit tested cases: