From e08e38000c5fc302e8ef395beedf235aa44916bb Mon Sep 17 00:00:00 2001
From: Lucas Bubner <lkbubner@proton.me>
Date: Mon, 25 Nov 2024 23:34:46 +1030
Subject: [PATCH] Set useKf option for ATRA

---
 .../AprilTagRelocalizingAccumulator.java      | 50 +++++++++++++------
 1 file changed, 35 insertions(+), 15 deletions(-)

diff --git a/src/main/java/au/edu/sa/mbhs/studentrobotics/bunyipslib/localization/accumulators/AprilTagRelocalizingAccumulator.java b/src/main/java/au/edu/sa/mbhs/studentrobotics/bunyipslib/localization/accumulators/AprilTagRelocalizingAccumulator.java
index ea9b9f915..5ba747eaf 100644
--- a/src/main/java/au/edu/sa/mbhs/studentrobotics/bunyipslib/localization/accumulators/AprilTagRelocalizingAccumulator.java
+++ b/src/main/java/au/edu/sa/mbhs/studentrobotics/bunyipslib/localization/accumulators/AprilTagRelocalizingAccumulator.java
@@ -52,6 +52,7 @@ public class AprilTagRelocalizingAccumulator extends Accumulator {
     private double lastHeading;
     private boolean active = true;
     private boolean updateHeading = false;
+    private boolean useKf = true;
 
     /**
      * Create a new AprilTag relocalizing accumulator.
@@ -81,6 +82,18 @@ public AprilTagRelocalizingAccumulator setKalmanGains(double R, double Q) {
         return this;
     }
 
+    /**
+     * Set whether to use the Kalman filter for the x, y, and r components.
+     *
+     * @param useKf whether to use the Kalman filter
+     * @return this
+     */
+    @NonNull
+    public AprilTagRelocalizingAccumulator setUseKalmanFilter(boolean useKf) {
+        this.useKf = useKf;
+        return this;
+    }
+
     /**
      * Add a data filter that will apply to the detections of the processor whether to accept processing this tag.
      *
@@ -183,22 +196,29 @@ public void accumulate(@NonNull Twist2dDual<Time> twist) {
                 .map(p -> p.heading.toDouble())
                 .reduce(0.0, Double::sum) / estimates.size();
 
-        Vector2d twistedTwist = pose.heading.times(twist.value().line);
-        Vector2d kfVec = new Vector2d(
-                // Use deltas supplied directly from the pose twist to avoid integrating twice and causing oscillations
-                xf.calculateFromDelta(twistedTwist.x, positionAvg.x),
-                yf.calculateFromDelta(twistedTwist.y, positionAvg.y)
-        );
-
-        double headingTwist = twist.value().angle;
-        if (Math.abs(headingAvgRad - lastHeading) > Math.PI) {
-            // If we're at the heading modulus boundary, the filter should be reset and updated with the new heading
-            headingTwist = pose.heading.inverse().log();
-            rf.reset();
+        Vector2d newVec;
+        Rotation2d newHeading;
+        if (useKf) {
+            Vector2d twistedTwist = pose.heading.times(twist.value().line);
+            newVec = new Vector2d(
+                    // Use deltas supplied directly from the pose twist to avoid integrating twice and causing oscillations
+                    xf.calculateFromDelta(twistedTwist.x, positionAvg.x),
+                    yf.calculateFromDelta(twistedTwist.y, positionAvg.y)
+            );
+
+            double headingTwist = twist.value().angle;
+            if (Math.abs(headingAvgRad - lastHeading) > Math.PI) {
+                // If we're at the heading modulus boundary, the filter should be reset and updated with the new heading
+                headingTwist = pose.heading.inverse().log();
+                rf.reset();
+            }
+            newHeading = Rotation2d.exp(rf.calculateFromDelta(headingTwist, headingAvgRad));
+            lastHeading = headingAvgRad;
+        } else {
+            newVec = positionAvg;
+            newHeading = Rotation2d.exp(headingAvgRad);
         }
-        Rotation2d kfHeading = Rotation2d.exp(rf.calculateFromDelta(headingTwist, headingAvgRad));
-        lastHeading = headingAvgRad;
 
-        pose = new Pose2d(kfVec, updateHeading ? kfHeading : pose.heading);
+        pose = new Pose2d(newVec, updateHeading ? newHeading : pose.heading);
     }
 }