search

TechnoTitans / TitanWare2024

Crescendo 2024
Other
3 stars 1 forks source link

SwerveDrivePoseEstimator simplification/optimization #10

Closed HarryXChen3 closed 3 months ago

HarryXChen3 commented 9 months ago

According to the latest software post from mechanical advantage, theres a still open PR to wpilib that changes how the SwerveDrivePoseEstimator functions, simplifying the logic related to having to replay the odometry updates after applying a vision update.

Does this produce more desirable results and is it something we should incorporate in the time-being while the PR is still waiting to be merged/completed? Is it worth it to make a custom pose estimator considering the benefits and drawbacks - the current SwerveDrivePoseEstimator still works and isn't causing us any issues, but would this be better to the point where it would outweigh the extra work and cost to maintain?

This year we record a buffer of all recent odometry poses instead of twists. We then find the difference between the vision and odometry pose and apply that Twist to our estimated pose. This logic was implemented in our code based on this pull request here.

HarryXChen3 commented 9 months ago

here

Though we haven’t tested our pose estimator for performance yet we expect the new method to be faster, since the estimator will not have to replay every single twist in our history every time we add a vision observation or want our estimated pose, instead we only have to apply two Transform2ds to get from our odometry pose at the time to our vision pose and back. If you want more information you can also look at the pull request posted above. Overall, it is just a simplification of the pose estimation logic.

HarryXChen3 commented 9 months ago

From the PR author himself here

Echoing Surya’s comments here - as a part of testing this PR mentioned I checked it side-by-side with the previous implementation. The resulting estimated path is the exact same between both implementations down to 1e-9, so this really is a strict upgrade in performance.

HarryXChen3 commented 9 months ago

I think a reasonable approach to this change would be to take a look at our vision measurement update performance and measure the amount of time it eats up through typical usage, then compare it to the improvement from using the simplified code to justify further implementation.

That will require a robot with cameras and vision setup, as well as a field with a few apriltags mounted, so this may need to be delayed until we have those.

MqxS commented 3 months ago

WPILIB implemented