Hi, our team has recently run into an issue with our WPILIb pose estimation odometry system in which our pose begins to perform random “Archemedes spins” on the field.
We’ve been able to isolate the cause of error to how our robot code takes the limelight pose and applies it to our odometry as shown through this test comparing pure odometry poses with a vision+odometry pose using Advantage kit’s replay simulator.
We currently use 2 limelights using the limelight networktables api to gather vision poses. After which we fuse both collected vision poses into the pose estimator at the same frame.
We’ve also noticed that it tends to happen on the red alliance side more frequently, but there are still multiple instances of it happening on the blue alliance.
We’re not quite sure where in our processing logic would cause the robot to spinng uncontrollably. Any help or guidance with this issue would be appreciated!
Here is the link to the branch we ran during a scrimmage.
Interesting, I’m curious what changing the yaw pitch and roll of the limelight would do? We’ve been able to roughly estimate the limelight positions from the cad. It’s a little rough estimated but nonetheless I’m not sure how that would play into the spinning seen in the log file.
I’m also not quite sure what the changing the gyro/pid would do as it’s inherently chassis odometry related. Could you elaborate?
Your StDevs for both vision and odometry seem high. You don’t want to have two incredibly high deviations since you’re essentially trusting neither. Reduce your odometry deviations and trust your navX inputs more.
Log the Pose2D of each of the Limelight botposes. We have seen this spiraling before and it was from the gyro value from the tags being flipped 180 degrees from what the robot gyro is reporting.
Your limelight botpose does not seem to be updating frequently even when it has a target but the robot is moving. The Tx and Ty does change which might indicate you are continuously passing an old bot pose into the estimator.
I’ve seen this spiraling when the gyro value does not match the vision rotation value. I noticed in your code in VisionIOLimelight.java line 70, you set the rotation value to the default Rotation2d() and not to the vision’s rotation. Try changing it to visionPoseInfo[2] and let me know how it turns out.
Edit: I believe that if the vision rotation is off, it will cause spiraling regardless of the vision rotation standard deviation.
VecBuilder.fill(1, 1, 0.7), //odometry standard devs
VecBuilder.fill(5, 5, 99999.0) //vision pose estimators standard dev are increase x, y, rotatinal radians values to trust vision less
999999.0 basically tells the pose estimator to never trust the vision’s rotation estimations. When the gyro reading doesn’t match the actual rotation of your robot, it never gets corrected.
How does that lead to a spiral? Well let’s say your robot is in position A facing rotation a, but the odometry thinks it’s in position B facing rotation b.
The results coming from the camera will show position A and rotation a. But the pose estimator is told to never trust the rotation of vision, it would still think that your robot is in rotation b, and it goes:
Vision tells me I’m at position A and rotation a, but I’m actually at rotation b. So the vision must be lying about my position. No worries, let me transform position A by a rotation of (b - a) so that I get my actual position C.
The resulting position, C, is just some random position on the field. It’s nowhere even close to your actual position, so the odometry will go crazy.
I’m skimming while at work… so haven’t really read the entirety of the thread…
Didn’t 6328 experience this issue early on in the 2024 season? I think they wrote their own pose estimator to account for this. Something todo with Twists.
If i can find it and it’s relevant i’ll post a link…
Yes, this is the same behavior. Here’s where we describe the reason for switching:
This behavior is inherent in how the WPILib pose estimator works (not a bug in user code). We switched from using twists to transforms in order to avoid it, which is technically “incorrect” but produces far nicer behavior in practice when initializing the pose. We also compared the two approaches during a match via log replay and found there was essentially no measurable difference.
