[BUG] Pose Estimator

[nikinatt]

*Describe the bug
Robot odometry pose is moving to large coordinates on AdvantageScope (robot is not physically moving in real life)
To Reproduce
Steps to reproduce the behavior:

1. Go to 'AdvantageScope'
2. Click on 'Robot Odometry Pose'
3. See error

This seems to be a result of the new pose buffering

Screen.Recording.2026-02-07.091416.mp4

[tbowers7]

That video is frightening!

[noahkr07]

Just want to add on to the "This seems to be a result of the new pose buffering." After commenting out pose buffering in Vision and Drive, it seems to behave normally (as in, no flying across the field). My comments made vision stop working, but the kinematics-based pose stopped oscillating.

[tbowers7]

Haven't been able to replicate. What do you do before it starts oscillating?

[nikinatt]

After connecting to AdvantageScope, and adding the 2D odometry pose, the robot begins to oscillate back and forth with larger distances as each second passes. This is without the robot being enabled. (happens with enabled and disabled)

This only occurs with the cameras on (does not matter if they see april tags or not)

Camera offsets are configured correctly, and this was not an issue in the previous release of AzRBSI (without pose buffering)

[tbowers7]

@noahkr07 , @nikinatt : I have confirmed and replicated the bug. Working on it...