[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...

[tbowers7]

Analysis from ChatGPT:

That plot (purple exploding to tens of thousands of feet) is exactly what you see when the time-alignment math is applying the odometry delta in the wrong reference frame. The fused pose stays sane because it’s only aligning raw camera estimates (which are usually already close to odom), but the smoother repeatedly re-aligns already-fused field poses across a sliding window — and a small frame mistake turns into a huge “ping-pong” once you do it over and over.

What’s wrong

Your time alignment does this:

Transform2d ts_T_tf = odomAtTF.minus(odomAtTs);
return visionPoseAtTs.transformBy(ts_T_tf);

In WPILib, odomAtTF.minus(odomAtTs) gives you a Transform2d that maps odomAtTs → odomAtTF, and that transform’s translation is expressed in the coordinate frame of odomAtTs.

But visionPoseAtTs.transformBy(transform) applies the transform as if it’s expressed in the coordinate frame of visionPoseAtTs.

Those frames are only the same if visionPoseAtTs.getRotation() is essentially equal to odomAtTs.getRotation(). When they differ (even a little), you’re effectively rotating the odom delta by the wrong heading — and in the smoothing step you do that to multiple historical fused estimates every loop, which creates the “wild oscillation” and can blow up to enormous values.

The fix

For aligning field-frame poses, don’t use Pose2d.transformBy() with a transform expressed in some other pose’s frame. Instead, compute the delta in field coordinates and apply it directly:

private Pose2d timeAlignPose(Pose2d poseAtTs, double ts, double tFusion) {
  Optional<Pose2d> odomAtTsOpt = drive.getPoseAtTime(ts);
  Optional<Pose2d> odomAtTFOpt = drive.getPoseAtTime(tFusion);
  if (odomAtTsOpt.isEmpty() || odomAtTFOpt.isEmpty()) return null;

  Pose2d odomAtTs = odomAtTsOpt.get();
  Pose2d odomAtTF = odomAtTFOpt.get();

  // Field-frame translation delta
  Translation2d dTrans = odomAtTF.getTranslation().minus(odomAtTs.getTranslation());
  // Heading delta
  Rotation2d dTheta = odomAtTF.getRotation().minus(odomAtTs.getRotation());

  return new Pose2d(
      poseAtTs.getTranslation().plus(dTrans),
      poseAtTs.getRotation().plus(dTheta));
}

That version is frame-consistent for “move this field pose forward by the same field motion odom experienced.”

Why it shows up in Smoothed but not Fused

• Fused: you align a small set of raw camera poses once each loop → the frame error is usually bounded because vision is close to odom.

• Smoothed: you align many prior fused poses every loop. If the transform is applied in the wrong frame, some of those aligned poses jump to the “other side,” and inverse-variance fusing between “left” and “right” jumps creates the oscillation you plotted.

If you swap in the corrected timeAlignPose() above, the purple trace should immediately stop the giant oscillations and become a slightly-lower-noise version of the fused estimate (assuming your std devs aren’t absurdly small).

[tbowers7]

The red plot is the original SmoothedPose, and the green plot is the corrected SmoothedPose using the fix described above. This fix is in #141 .

[tbowers7]

@nikinatt , @noahkr07 In your branch, you added code for Gyro in the drive/ subsystem. Can you tell me about that? In the main idiom, the Pigeon2 is queried in the IMU subsystem... was that somehow not working for you?