Pose estimator doesn't update odometry pose with vision measurements over time, leading to drift

[calcmogul]

Initially reported by FRC team 2363:

Step 9 of PoseEstimator.addVisionMeasurement() sets the PoseEstimator pose estimate to a vision-compensated version of the latest odometry object pose, but the odometry object's pose is only updated by twists passed to PoseEstimator.updateWithTime(). This means the odometry pose is still allowed to drift over time instead of being gradually fixed by vision measurements. In other words, the odometry is only ever getting fixed by one vision measurement instead of a backlog of all of them.

The old implementation applied vision updates to old pose estimates, then replayed odometry updates up to the current time. That was too expensive at the time, so idk if we can fix this properly.

The odometry object pose not being vision-compensated also causes PoseEstimator.resetRotation() to make the translation estimate teleport from the vision-compensated version to the odometry-only version.

[KangarooKoala]

PoseEstimator doesn't directly expose m_odometry without applying the vision correction, so for standard use, are there any issues aside from PoseEstimator.resetRotation() and PoseEstimator.resetTranslation() not accounting for the vision corrections?

Also, as a point of clarification, when is m_odometry fixed by a vision measurement? I thought m_odometry is never corrected by vision measurements.

[calcmogul]

The odometry object pose is never corrected by vision measurements. Effectively, the odometry pose is left to drift while a pose delta is applied on top of it based on m_visionUpdates. This may still be mathematically correct, but it's not numerically stable in general because the odometry estimate error is unbounded. I also don't like that we keep a pose around that doesn't actually get used (it gets subtracted off, otherwise the pose estimate would be wrong), because it makes the implementation more confusing.

The pose estimator is complex and difficult to unit test; our unit test is effectively "does it converge", because the individual pieces aren't unit testable. We should really work on a derivation/proof for it.

[rzblue]

Effectively, the odometry pose is left to drift while a pose delta is applied on top of it based on m_visionUpdates.

Small clarification: odometry poses are only used to calculate deltas since the most recent vision measurement, which are then applied to the fused pose estimate at the most recent vision measurement

[calcmogul]

The way the code has been abstracted makes that very nonobvious.