add rotation to kalman filter

quail/src/main/java/com/mineinjava/quail/localization/KalmanFilterLocalizer.java

@@ -33,13 +33,13 @@
  * @see https://astr0clad.github.io/quail_docs/localization/vision/
  */
 public class KalmanFilterLocalizer implements Localizer {
-  private Vec2d poseEstimate = new Vec2d(0, 0);
+  private Pose2d poseEstimate = new Pose2d();
   private double looptime = 0;
   private ArrayList<KalmanPose2d> velocities =
       new ArrayList<KalmanPose2d>(); // / list of velocities, most recent last
   public double heading = 0;
 
-  public KalmanFilterLocalizer(Vec2d initialPose, double looptime) {
+  public KalmanFilterLocalizer(Pose2d initialPose, double looptime) {
     poseEstimate = initialPose;
     this.looptime = looptime;
   }
@@ -49,18 +49,21 @@ public KalmanFilterLocalizer(Vec2d initialPose, double looptime) {
    *
    * <p>Updates based on the current velocity and the time since the last vision update
    *
-   * @param observedPose the current pose estimate (get this from vision usually)
-   * @param velocity the current velocity
+   * @param observedPose the current pose estimate (get this from vision usually), include rotation
+   * @param velocity the current velocity, include rotation
    * @param poseEstimateLatency the time since the last vision update
-   * @param w how much weight to "trust" the vision estimate
+   * @param w how much weight to "trust" the vision estimate for position
+   * @param hw how much weight to "trust" the vision estimate for heading
    * @param timestampMillis the current system time in ms
    */
-  public Vec2d update(
-      Vec2d observedPose,
-      Vec2d velocity,
+  public Pose2d update(
+      Pose2d observedPose,
+      Pose2d velocity,
       double poseEstimateLatency,
       double w,
+      double hw,
       double timestampMillis) {
+
     KalmanPose2d currentVelocity = new KalmanPose2d(velocity, timestampMillis);
     velocities.add(currentVelocity); // add the current velocity to the list of velocities
     // trim the list of velocities to only include the relevant ones.
@@ -69,63 +72,82 @@ public Vec2d update(
       velocities.remove(0);
     }
     // distance traveled since the vision update
-    Vec2d deltaPosSinceVision = new Vec2d(0, 0);
+    Vec2d deltaTranslationSinceVision = new Vec2d(0, 0);
+    double deltaRotationSinceVision = 0d;
     for (KalmanPose2d vel : velocities) {
-      deltaPosSinceVision =
-          deltaPosSinceVision.add(
-              vel); // don't scale here, we'll do it later (distributive property)
+      // don't scale here, we'll do it later (distributive property)
+      deltaTranslationSinceVision = deltaTranslationSinceVision.add(vel.vec());
+      deltaRotationSinceVision += vel.heading;
     }
-    deltaPosSinceVision =
-        deltaPosSinceVision.scale(looptime); // scale by the looptime: distance = velocity * time
+
+    // scale by the looptime: distance = velocity * time
+    deltaTranslationSinceVision = deltaTranslationSinceVision.scale(this.looptime);
+    deltaRotationSinceVision = deltaRotationSinceVision * this.looptime;
+
     // update the vision pose estimate with the delta from velocity
-    Vec2d visionPoseEstimate = observedPose.add(deltaPosSinceVision);
+    Vec2d updatedPoseSinceVision = observedPose.vec().add(deltaTranslationSinceVision);
+    double updatedRotationSinceVision = observedPose.heading + deltaRotationSinceVision;
     // update the last pose estimate with the velocity
-    Vec2d kinematicsPoseEstimate = this.poseEstimate.add(velocity.scale(this.looptime));
+    Vec2d kinematicsTranslationEstimate =
+        this.poseEstimate.vec().add(velocity.vec().scale(this.looptime));
+    double kinematicsRotationEstimate =
+        this.poseEstimate.heading + (velocity.heading * this.looptime);
+
     // update the pose estimate with a weighted average of the vision and kinematics pose estimates
-    w = MathUtil.clamp(w, 0, 1); // make sure w is between 0 and 1 (inclusive)
-    this.poseEstimate = ((visionPoseEstimate.scale(w)).add(kinematicsPoseEstimate.scale(1 - w)));
+    // make sure w is between 0 and 1 (inclusive)
+    w = MathUtil.clamp(w, 0, 1);
+    hw = MathUtil.clamp(hw, 0, 1);
+
+    Vec2d translationEstimate =
+        ((updatedPoseSinceVision.scale(w)).add(kinematicsTranslationEstimate.scale(1 - w)));
+
+    double rotationEstimate =
+        (updatedRotationSinceVision * hw) + (kinematicsRotationEstimate * (1 - hw));
+    this.poseEstimate = new Pose2d(translationEstimate, rotationEstimate);
     return this.poseEstimate;
   }
 
   /** Returns the current pose estimate */
   public Pose2d getPose() {
-    return new Pose2d(poseEstimate.x, poseEstimate.y, this.heading);
+    return new Pose2d(poseEstimate.x, poseEstimate.y, poseEstimate.heading);
   }
 
   /**
    * Sets the current position.
    *
    * <p>Completely overrides the old position
    *
-   * @param pose new translational pose to use
+   * @param pose new pose to use
    */
   public void setPose(Pose2d pose) {
-    this.poseEstimate = new Vec2d(pose.x, pose.y);
+    this.poseEstimate = pose;
   }
 
   /**
    * Sets the current heading.
    *
-   * <p>Call this on every frame with the robot's heading
-   *
    * @param heading Robot heading
    */
   public void setHeading(double heading) {
     this.heading = heading;
   }
 }
 
-/** Modified translational pose but it has a timestamp. */
-class KalmanPose2d extends Vec2d {
+/**
+ * Modified translational pose but it has a timestamp.
+ *
+ * @see Pose2d
+ */
+class KalmanPose2d extends Pose2d {
   public double timestamp = 0;
 
-  public KalmanPose2d(double x, double y, double timestamp) {
-    super(x, y);
+  public KalmanPose2d(double x, double y, double heading, double timestamp) {
+    super(x, y, heading);
     this.timestamp = timestamp;
   }
 
-  public KalmanPose2d(Vec2d vec, double timestamp) {
-    super(vec.x, vec.y);
+  public KalmanPose2d(Pose2d pos, double timestamp) {
+    super(pos.x, pos.y, pos.heading);
     this.timestamp = timestamp;
   }
 }

quail/src/test/java/quail/pathing/KalmanFilterLocalizerTest.java

@@ -0,0 +1,29 @@
+// Copyright (C) Marcus Kauffman 2023-Present
+
+// This work would not have been possible without the work of many
+// contributors, most notably Colin Montigel. See ACKNOWLEDGEMENT.md for
+// more details.
+
+// This file is part of Quail.
+
+// Quail is free software: you can redistribute it and/or modify it
+// underthe terms of the GNU General Public License as published by the
+// Free Software Foundation, version 3.
+
+// Quail is distributed in the hope that it will be useful, but WITHOUT
+// ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+// FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+// for more details.
+
+// You should have received a copy of the GNU General Public License
+// along with Quail. If not, see <https://www.gnu.org/licenses/>
+
+package quail.pathing;
+
+import org.junit.jupiter.api.Test;
+
+/** KalmanFilterLocalizer */
+public class KalmanFilterLocalizerTest {
+  @Test
+  void constructor() {}
+}

quail/src/test/java/quail/pathing/PathFollowerTest.java

@@ -64,7 +64,7 @@ void setUp() {
                 add(poseEnd);
               }
             });
-    KalmanFilterLocalizer localizer = new KalmanFilterLocalizer(new Pose2d().vec(), 1d);
+    KalmanFilterLocalizer localizer = new KalmanFilterLocalizer(new Pose2d(), 1d);
     ConstraintsPair translationPair =
         new ConstraintsPair(
             1, 1000); // needs super high accel because looptimes are so short (approx 10khz)