How to test out localization (odometry + gyro + vision measurements) & questions about Pose2d

Technical Programming
1

Hi everyone!

When I call Pose2d.getRotation().getRadians(), will it give me something from 0 - 2pi radians? Because if I combine the Pose2d from vision measurements w the odometry pose, I feel like it is important to make sure that both angle measurements are from 0 - 2pi.

Also, would anyone say that it is worth it to use custom Standard Deviations for vision depending on the location on the field (I can’t do confidence bcs Limelight doesn’t have such a setting) and for odometry depending on how many collisions there have been?

Lastly, how do I test out my localization standard deviations to find the best ones, accurately? I feel like something with simulations is the way to go but I’m not exactly sure. Have you guys found something that works well (both for testing and for the standard deviations)?

2

It’ll return the angle given to it (imo, that was a design mistake, but some teams rely on that behavior; Rotation3d doesn’t do this). You can use MathUtil (WPILib API 2024.3.2) to wrap angles.

Yes. See tuning guidance here:

1 Like