From 6c0e22534a90714988a11aac964b484618dcc19e Mon Sep 17 00:00:00 2001
From: rmheuer <63077980+rmheuer@users.noreply.github.com>
Date: Sun, 19 Nov 2023 19:59:59 -0600
Subject: [PATCH] [Robot] Remove unneeded vision update sort

---
 .../swrobotics/robot/subsystems/swerve/SwerveEstimator.java  | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/SwerveEstimator.java b/Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/SwerveEstimator.java
index d226a99..46eb28c 100644
--- a/Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/SwerveEstimator.java
+++ b/Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/SwerveEstimator.java
@@ -101,8 +101,11 @@ public void update(Twist2d driveTwist) {
 
                 List<TagTrackerInput.VisionUpdate> newVisionUpdates = new ArrayList<>();
                 newVisionUpdates.add(visionUpdate);
-                newVisionUpdates.sort(this::compareStdDevs);
+
+                // Insert new update entry for this vision update
                 updates.put(timestamp, new PoseUpdate(prevToVisionTwist, newVisionUpdates));
+
+                // Overwrite nextUpdate with twist after this vision update
                 updates.put(nextUpdate.getKey(), new PoseUpdate(visionToNextTwist, nextUpdate.getValue().visionUpdates));
             }
         }