SwerveDrivePoseEstimator doesn't work unless I reset it

I’m using a limelight plus my module positions to estimate my robot pose with SwerveDrivePoseEstimator. The pose estimator works completely fine when it is only using the modules (can’t see april tags) and when it is using the modules and april tags, but if it sees april tags and then stops seeing them, the estimated pose seems to revert to the pose it was at before it saw april tags.

The only way I have been able to fix this is to reset the pose estimator after every time I add a vision measurement to the newly estimated pose.

odometry.resetPosition(odometry.getEstimatedPosition().getRotation(), getModulePositions(), odometry.getEstimatedPosition());

How should I fix this?

I think we will need to see more of your code in order to help. Can you share a link, say to GitHub?

It is in the FirstCompCode branch. Most of my pose estimation code is in DriveCommandBase.

I see you have a method “addPoseEstimatorSwerveMeasurement” but I don’t see where you are calling it… you need to update the odometry every “loop” even if you are using vision data.

1 Like

I’m calling it at the beginning of the drive command base loop. I’ll move it to the periodic of the drive subsystem. Also, the time stamp I use it gotten directly from the limelight, do I need to convert it to the Roborio’s time stamp?

Yes, something like:

odometry.addVisionMeasurement(botPose,
                            Timer.getFPGATimestamp() - latency);
1 Like

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.