Instability in WPIlib pose estimator after adding vision measurements

[spacey-sooty]

Describe the bug
Following about 20% of startups, the output of the WPILib pose estimator will slowly spiral out to infinity while receiving vision updates. Resetting the pose estimator almost always results in a return to stability but at least once it has needed to be reset twice. I have not observed the pose estimator become unstable once it is correctly initialized.

To Reproduce
I have not been able to determine a correlation between any starting conditions and the issue occurring. We did not have any problems at our first competition where we were purely using odometry; only after the first comp when we started incorporating PhotonVision and added vision measurements to the pose estimator did we start seeing these issues.

Screenshots / Videos
I attached the wpilog file from one match where we saw it happen while setting up. Right from the beginning, you can see the pose estimator (NT:PoseEstimator) start spiraling out. At 18.6s, the pose estimator is reset and moves to the correct position based on the vision estimates, but immediately starts to spiral out from there. At 44.6s, the pose estimator is reset again, returns to the correct position, and remains stable for the rest of the match.

FRC_20250329_163546_ILCH_Q70.zip

Originally reported by @Zaque2 in PhotonVision/photonvision#1944