Angle refactor + tests!

.github/workflows/linting.yml → .github/workflows/ci.yml

@@ -15,4 +15,15 @@ jobs:
  distribution: 'zulu'
  java-version: 17
  - name: Run spotless linter
- run: ./gradlew spotlessCheck
+ run: ./gradlew spotlessCheck
+
+ tests:
+ runs-on: ubuntu-latest
+ steps:
+ - uses: actions/checkout@v4
+ - uses: actions/setup-java@v4
+ with:
+ distribution: 'zulu'
+ java-version: 17
+ - name: Run tests
+ run: ./gradlew test

quail/src/main/java/com/mineinjava/quail/SwerveModuleBase.java

@@ -1,6 +1,6 @@
 package com.mineinjava.quail;
 
-import com.mineinjava.quail.util.Util;
+import com.mineinjava.quail.util.MathUtil;
 import com.mineinjava.quail.util.geometry.Vec2d;
 
 public class SwerveModuleBase {
@@ -47,7 +47,7 @@ public SwerveModuleBase(
  * @return the angle to set the steering motor to
  */
  public double calculateNewAngleSetpoint(double angle) {
- double shortestAngle = Util.deltaAngle(currentAngle, angle);
+ double shortestAngle = MathUtil.deltaAngle(currentAngle, angle);
  return currentAngle = currentAngle + shortestAngle;
  }
 
@@ -56,7 +56,7 @@ public double calculateNewAngleSetpoint(double angle) {
  * opposite direction and rotate less than 90 degrees
  */
  public double calculateOptimizedAngle(double angle) {
- double deltaAngle = Util.deltaAngle(currentAngle, angle);
+ double deltaAngle = MathUtil.deltaAngle(currentAngle, angle);
  if (Math.abs(deltaAngle) > Math.PI / 2) {
  motorFlipper = -1;
  return angle + Math.PI;

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

@@ -0,0 +1,80 @@
+package com.mineinjava.quail.localization;
+
+import com.mineinjava.quail.util.MathUtil;
+import com.mineinjava.quail.util.geometry.Pose2d;
+import com.mineinjava.quail.util.geometry.Vec2d;
+import java.util.ArrayList;
+
+public class KalmanFilterLocalizer implements Localizer {
+ private Vec2d poseEstimate = new Vec2d(0, 0);
+ 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) {
+ poseEstimate = initialPose;
+ this.looptime = looptime;
+ }
+
+ /**
+ * Updates the pose estimate 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 poseEstimateLatency the time since the last vision update
+ */
+ public Vec2d update(Vec2d observedPose, Vec2d velocity, double poseEstimateLatency, double w) {
+ KalmanPose2d currentVelocity = new KalmanPose2d(velocity, System.currentTimeMillis());
+ velocities.add(currentVelocity); // add the current velocity to the list of velocities
+ // trim the list of velocities to only include the relevant ones.
+ while (velocities.size() > 0
+ && velocities.get(0).timestamp < System.currentTimeMillis() - poseEstimateLatency) {
+ velocities.remove(0);
+ }
+ // distance traveled since the vision update
+ Vec2d deltaPosSinceVision = new Vec2d(0, 0);
+ for (KalmanPose2d vel : velocities) {
+ deltaPosSinceVision =
+ deltaPosSinceVision.add(
+ vel); // don't scale here, we'll do it later (distributive property)
+ }
+ deltaPosSinceVision =
+ deltaPosSinceVision.scale(looptime); // scale by the looptime: distance = velocity * time
+ // update the vision pose estimate with the delta from velocity
+ Vec2d visionPoseEstimate = observedPose.add(deltaPosSinceVision);
+ // update the last pose estimate with the velocity
+ Vec2d kinematicsPoseEstimate = this.poseEstimate.add(velocity.scale(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)));
+ return this.poseEstimate;
+ }
+
+ public Pose2d getPose() {
+ return new Pose2d(poseEstimate.x, poseEstimate.y, this.heading);
+ }
+
+ public void setPose(Pose2d pose) {
+ this.poseEstimate = new Vec2d(pose.x, pose.y);
+ }
+
+ public void setHeading(double heading) {
+ this.heading = heading;
+ }
+}
+
+class KalmanPose2d extends Vec2d {
+ public double timestamp = 0;
+
+ public KalmanPose2d(double x, double y, double timestamp) {
+ super(x, y);
+ this.timestamp = timestamp;
+ }
+
+ public KalmanPose2d(Vec2d vec, double timestamp) {
+ super(vec.x, vec.y);
+ this.timestamp = timestamp;
+ }
+}

quail/src/main/java/com/mineinjava/quail/pathing/ConstraintsPair.java

@@ -1,36 +1,39 @@
 package com.mineinjava.quail.pathing;
 
+/**
+ * Represents a pair of constraints (max velocity and max acceleration) for use in the path
+ * follower. This can be either translational or angular velocity. If angular velocity, units are in
+ * radians.
+ */
 public class ConstraintsPair {
 
- private double velocity;
- private double acceleration;
+ private final double velocity;
+ private final double acceleration;
 
  /**
- * Represents a pair of constraints (max velocity and max acceleration) for use in the path
- * follower Can be either translational or angular velocity. If angular velocity, units are in
- * radians.
+ * Constructs a new ConstraintsPair with the specified max velocity and max acceleration.
  *
- * @param maxVelocity your units/s
- * @param maxAcceleration your units/s^2
+ * @param maxVelocity the maximum velocity in units/s
+ * @param maxAcceleration the maximum acceleration in units/s^2
  */
  public ConstraintsPair(double maxVelocity, double maxAcceleration) {
  this.velocity = maxVelocity;
  this.acceleration = maxAcceleration;
  }
 
  /**
- * Returns the constraints' max velocity
+ * Returns the maximum velocity of the constraints.
  *
- * @return
+ * @return the maximum velocity in units/s
  */
  public double getMaxVelocity() {
  return velocity;
  }
 
  /**
- * Returns the constraints' max acceleration
+ * Returns the maximum acceleration of the constraints.
  *
- * @return
+ * @return the maximum acceleration in units/s^2
  */
  public double getMaxAcceleration() {
  return acceleration;

.github/workflows/linting.yml → .github/workflows/ci.yml

@@ -15,4 +15,15 @@ jobs:
  distribution: 'zulu'
  java-version: 17
  - name: Run spotless linter
- run: ./gradlew spotlessCheck
+ run: ./gradlew spotlessCheck
+
+ tests:
+ runs-on: ubuntu-latest
+ steps:
+ - uses: actions/checkout@v4
+ - uses: actions/setup-java@v4
+ with:
+ distribution: 'zulu'
+ java-version: 17
+ - name: Run tests
+ run: ./gradlew test

quail/src/main/java/com/mineinjava/quail/SwerveModuleBase.java

@@ -1,6 +1,6 @@
 package com.mineinjava.quail;
 
-import com.mineinjava.quail.util.Util;
+import com.mineinjava.quail.util.MathUtil;
 import com.mineinjava.quail.util.geometry.Vec2d;
 
 public class SwerveModuleBase {
@@ -47,7 +47,7 @@ public SwerveModuleBase(
  * @return the angle to set the steering motor to
  */
  public double calculateNewAngleSetpoint(double angle) {
- double shortestAngle = Util.deltaAngle(currentAngle, angle);
+ double shortestAngle = MathUtil.deltaAngle(currentAngle, angle);
  return currentAngle = currentAngle + shortestAngle;
  }
 
@@ -56,7 +56,7 @@ public double calculateNewAngleSetpoint(double angle) {
  * opposite direction and rotate less than 90 degrees
  */
  public double calculateOptimizedAngle(double angle) {
- double deltaAngle = Util.deltaAngle(currentAngle, angle);
+ double deltaAngle = MathUtil.deltaAngle(currentAngle, angle);
  if (Math.abs(deltaAngle) > Math.PI / 2) {
  motorFlipper = -1;
  return angle + Math.PI;

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

@@ -0,0 +1,80 @@
+package com.mineinjava.quail.localization;
+
+import com.mineinjava.quail.util.MathUtil;
+import com.mineinjava.quail.util.geometry.Pose2d;
+import com.mineinjava.quail.util.geometry.Vec2d;
+import java.util.ArrayList;
+
+public class KalmanFilterLocalizer implements Localizer {
+ private Vec2d poseEstimate = new Vec2d(0, 0);
+ 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) {
+ poseEstimate = initialPose;
+ this.looptime = looptime;
+ }
+
+ /**
+ * Updates the pose estimate 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 poseEstimateLatency the time since the last vision update
+ */
+ public Vec2d update(Vec2d observedPose, Vec2d velocity, double poseEstimateLatency, double w) {
+ KalmanPose2d currentVelocity = new KalmanPose2d(velocity, System.currentTimeMillis());
+ velocities.add(currentVelocity); // add the current velocity to the list of velocities
+ // trim the list of velocities to only include the relevant ones.
+ while (velocities.size() > 0
+ && velocities.get(0).timestamp < System.currentTimeMillis() - poseEstimateLatency) {
+ velocities.remove(0);
+ }
+ // distance traveled since the vision update
+ Vec2d deltaPosSinceVision = new Vec2d(0, 0);
+ for (KalmanPose2d vel : velocities) {
+ deltaPosSinceVision =
+ deltaPosSinceVision.add(
+ vel); // don't scale here, we'll do it later (distributive property)
+ }
+ deltaPosSinceVision =
+ deltaPosSinceVision.scale(looptime); // scale by the looptime: distance = velocity * time
+ // update the vision pose estimate with the delta from velocity
+ Vec2d visionPoseEstimate = observedPose.add(deltaPosSinceVision);
+ // update the last pose estimate with the velocity
+ Vec2d kinematicsPoseEstimate = this.poseEstimate.add(velocity.scale(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)));
+ return this.poseEstimate;
+ }
+
+ public Pose2d getPose() {
+ return new Pose2d(poseEstimate.x, poseEstimate.y, this.heading);
+ }
+
+ public void setPose(Pose2d pose) {
+ this.poseEstimate = new Vec2d(pose.x, pose.y);
+ }
+
+ public void setHeading(double heading) {
+ this.heading = heading;
+ }
+}
+
+class KalmanPose2d extends Vec2d {
+ public double timestamp = 0;
+
+ public KalmanPose2d(double x, double y, double timestamp) {
+ super(x, y);
+ this.timestamp = timestamp;
+ }
+
+ public KalmanPose2d(Vec2d vec, double timestamp) {
+ super(vec.x, vec.y);
+ this.timestamp = timestamp;
+ }
+}

quail/src/main/java/com/mineinjava/quail/pathing/ConstraintsPair.java

@@ -1,36 +1,39 @@
 package com.mineinjava.quail.pathing;
 
+/**
+ * Represents a pair of constraints (max velocity and max acceleration) for use in the path
+ * follower. This can be either translational or angular velocity. If angular velocity, units are in
+ * radians.
+ */
 public class ConstraintsPair {
 
- private double velocity;
- private double acceleration;
+ private final double velocity;
+ private final double acceleration;
 
  /**
- * Represents a pair of constraints (max velocity and max acceleration) for use in the path
- * follower Can be either translational or angular velocity. If angular velocity, units are in
- * radians.
+ * Constructs a new ConstraintsPair with the specified max velocity and max acceleration.
  *
- * @param maxVelocity your units/s
- * @param maxAcceleration your units/s^2
+ * @param maxVelocity the maximum velocity in units/s
+ * @param maxAcceleration the maximum acceleration in units/s^2
  */
  public ConstraintsPair(double maxVelocity, double maxAcceleration) {
  this.velocity = maxVelocity;
  this.acceleration = maxAcceleration;
  }
 
  /**
- * Returns the constraints' max velocity
+ * Returns the maximum velocity of the constraints.
  *
- * @return
+ * @return the maximum velocity in units/s
  */
  public double getMaxVelocity() {
  return velocity;
  }
 
  /**
- * Returns the constraints' max acceleration
+ * Returns the maximum acceleration of the constraints.
  *
- * @return
+ * @return the maximum acceleration in units/s^2
  */
  public double getMaxAcceleration() {
  return acceleration;

.github/workflows/linting.yml → .github/workflows/ci.yml

@@ -15,4 +15,15 @@ jobs:
  distribution: 'zulu'
  java-version: 17
  - name: Run spotless linter
- run: ./gradlew spotlessCheck
+ run: ./gradlew spotlessCheck
+
+ tests:
+ runs-on: ubuntu-latest
+ steps:
+ - uses: actions/checkout@v4
+ - uses: actions/setup-java@v4
+ with:
+ distribution: 'zulu'
+ java-version: 17
+ - name: Run tests
+ run: ./gradlew test

quail/src/main/java/com/mineinjava/quail/SwerveModuleBase.java

@@ -1,6 +1,6 @@
 package com.mineinjava.quail;
 
-import com.mineinjava.quail.util.Util;
+import com.mineinjava.quail.util.MathUtil;
 import com.mineinjava.quail.util.geometry.Vec2d;
 
 public class SwerveModuleBase {
@@ -47,7 +47,7 @@ public SwerveModuleBase(
  * @return the angle to set the steering motor to
  */
  public double calculateNewAngleSetpoint(double angle) {
- double shortestAngle = Util.deltaAngle(currentAngle, angle);
+ double shortestAngle = MathUtil.deltaAngle(currentAngle, angle);
  return currentAngle = currentAngle + shortestAngle;
  }
 
@@ -56,7 +56,7 @@ public double calculateNewAngleSetpoint(double angle) {
  * opposite direction and rotate less than 90 degrees
  */
  public double calculateOptimizedAngle(double angle) {
- double deltaAngle = Util.deltaAngle(currentAngle, angle);
+ double deltaAngle = MathUtil.deltaAngle(currentAngle, angle);
  if (Math.abs(deltaAngle) > Math.PI / 2) {
  motorFlipper = -1;
  return angle + Math.PI;

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

@@ -0,0 +1,80 @@
+package com.mineinjava.quail.localization;
+
+import com.mineinjava.quail.util.MathUtil;
+import com.mineinjava.quail.util.geometry.Pose2d;
+import com.mineinjava.quail.util.geometry.Vec2d;
+import java.util.ArrayList;
+
+public class KalmanFilterLocalizer implements Localizer {
+ private Vec2d poseEstimate = new Vec2d(0, 0);
+ 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) {
+ poseEstimate = initialPose;
+ this.looptime = looptime;
+ }
+
+ /**
+ * Updates the pose estimate 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 poseEstimateLatency the time since the last vision update
+ */
+ public Vec2d update(Vec2d observedPose, Vec2d velocity, double poseEstimateLatency, double w) {
+ KalmanPose2d currentVelocity = new KalmanPose2d(velocity, System.currentTimeMillis());
+ velocities.add(currentVelocity); // add the current velocity to the list of velocities
+ // trim the list of velocities to only include the relevant ones.
+ while (velocities.size() > 0
+ && velocities.get(0).timestamp < System.currentTimeMillis() - poseEstimateLatency) {
+ velocities.remove(0);
+ }
+ // distance traveled since the vision update
+ Vec2d deltaPosSinceVision = new Vec2d(0, 0);
+ for (KalmanPose2d vel : velocities) {
+ deltaPosSinceVision =
+ deltaPosSinceVision.add(
+ vel); // don't scale here, we'll do it later (distributive property)
+ }
+ deltaPosSinceVision =
+ deltaPosSinceVision.scale(looptime); // scale by the looptime: distance = velocity * time
+ // update the vision pose estimate with the delta from velocity
+ Vec2d visionPoseEstimate = observedPose.add(deltaPosSinceVision);
+ // update the last pose estimate with the velocity
+ Vec2d kinematicsPoseEstimate = this.poseEstimate.add(velocity.scale(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)));
+ return this.poseEstimate;
+ }
+
+ public Pose2d getPose() {
+ return new Pose2d(poseEstimate.x, poseEstimate.y, this.heading);
+ }
+
+ public void setPose(Pose2d pose) {
+ this.poseEstimate = new Vec2d(pose.x, pose.y);
+ }
+
+ public void setHeading(double heading) {
+ this.heading = heading;
+ }
+}
+
+class KalmanPose2d extends Vec2d {
+ public double timestamp = 0;
+
+ public KalmanPose2d(double x, double y, double timestamp) {
+ super(x, y);
+ this.timestamp = timestamp;
+ }
+
+ public KalmanPose2d(Vec2d vec, double timestamp) {
+ super(vec.x, vec.y);
+ this.timestamp = timestamp;
+ }
+}

quail/src/main/java/com/mineinjava/quail/pathing/ConstraintsPair.java

@@ -1,36 +1,39 @@
 package com.mineinjava.quail.pathing;
 
+/**
+ * Represents a pair of constraints (max velocity and max acceleration) for use in the path
+ * follower. This can be either translational or angular velocity. If angular velocity, units are in
+ * radians.
+ */
 public class ConstraintsPair {
 
- private double velocity;
- private double acceleration;
+ private final double velocity;
+ private final double acceleration;
 
  /**
- * Represents a pair of constraints (max velocity and max acceleration) for use in the path
- * follower Can be either translational or angular velocity. If angular velocity, units are in
- * radians.
+ * Constructs a new ConstraintsPair with the specified max velocity and max acceleration.
  *
- * @param maxVelocity your units/s
- * @param maxAcceleration your units/s^2
+ * @param maxVelocity the maximum velocity in units/s
+ * @param maxAcceleration the maximum acceleration in units/s^2
  */
  public ConstraintsPair(double maxVelocity, double maxAcceleration) {
  this.velocity = maxVelocity;
  this.acceleration = maxAcceleration;
  }
 
  /**
- * Returns the constraints' max velocity
+ * Returns the maximum velocity of the constraints.
  *
- * @return
+ * @return the maximum velocity in units/s
  */
  public double getMaxVelocity() {
  return velocity;
  }
 
  /**
- * Returns the constraints' max acceleration
+ * Returns the maximum acceleration of the constraints.
  *
- * @return
+ * @return the maximum acceleration in units/s^2
  */
  public double getMaxAcceleration() {
  return acceleration;

.github/workflows/linting.yml → .github/workflows/ci.yml

@@ -15,4 +15,15 @@ jobs:
  distribution: 'zulu'
  java-version: 17
  - name: Run spotless linter
- run: ./gradlew spotlessCheck
+ run: ./gradlew spotlessCheck
+
+ tests:
+ runs-on: ubuntu-latest
+ steps:
+ - uses: actions/checkout@v4
+ - uses: actions/setup-java@v4
+ with:
+ distribution: 'zulu'
+ java-version: 17
+ - name: Run tests
+ run: ./gradlew test

quail/src/main/java/com/mineinjava/quail/SwerveModuleBase.java

@@ -1,6 +1,6 @@
 package com.mineinjava.quail;
 
-import com.mineinjava.quail.util.Util;
+import com.mineinjava.quail.util.MathUtil;
 import com.mineinjava.quail.util.geometry.Vec2d;
 
 public class SwerveModuleBase {
@@ -47,7 +47,7 @@ public SwerveModuleBase(
  * @return the angle to set the steering motor to
  */
  public double calculateNewAngleSetpoint(double angle) {
- double shortestAngle = Util.deltaAngle(currentAngle, angle);
+ double shortestAngle = MathUtil.deltaAngle(currentAngle, angle);
  return currentAngle = currentAngle + shortestAngle;
  }
 
@@ -56,7 +56,7 @@ public double calculateNewAngleSetpoint(double angle) {
  * opposite direction and rotate less than 90 degrees
  */
  public double calculateOptimizedAngle(double angle) {
- double deltaAngle = Util.deltaAngle(currentAngle, angle);
+ double deltaAngle = MathUtil.deltaAngle(currentAngle, angle);
  if (Math.abs(deltaAngle) > Math.PI / 2) {
  motorFlipper = -1;
  return angle + Math.PI;

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

@@ -0,0 +1,80 @@
+package com.mineinjava.quail.localization;
+
+import com.mineinjava.quail.util.MathUtil;
+import com.mineinjava.quail.util.geometry.Pose2d;
+import com.mineinjava.quail.util.geometry.Vec2d;
+import java.util.ArrayList;
+
+public class KalmanFilterLocalizer implements Localizer {
+ private Vec2d poseEstimate = new Vec2d(0, 0);
+ 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) {
+ poseEstimate = initialPose;
+ this.looptime = looptime;
+ }
+
+ /**
+ * Updates the pose estimate 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 poseEstimateLatency the time since the last vision update
+ */
+ public Vec2d update(Vec2d observedPose, Vec2d velocity, double poseEstimateLatency, double w) {
+ KalmanPose2d currentVelocity = new KalmanPose2d(velocity, System.currentTimeMillis());
+ velocities.add(currentVelocity); // add the current velocity to the list of velocities
+ // trim the list of velocities to only include the relevant ones.
+ while (velocities.size() > 0
+ && velocities.get(0).timestamp < System.currentTimeMillis() - poseEstimateLatency) {
+ velocities.remove(0);
+ }
+ // distance traveled since the vision update
+ Vec2d deltaPosSinceVision = new Vec2d(0, 0);
+ for (KalmanPose2d vel : velocities) {
+ deltaPosSinceVision =
+ deltaPosSinceVision.add(
+ vel); // don't scale here, we'll do it later (distributive property)
+ }
+ deltaPosSinceVision =
+ deltaPosSinceVision.scale(looptime); // scale by the looptime: distance = velocity * time
+ // update the vision pose estimate with the delta from velocity
+ Vec2d visionPoseEstimate = observedPose.add(deltaPosSinceVision);
+ // update the last pose estimate with the velocity
+ Vec2d kinematicsPoseEstimate = this.poseEstimate.add(velocity.scale(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)));
+ return this.poseEstimate;
+ }
+
+ public Pose2d getPose() {
+ return new Pose2d(poseEstimate.x, poseEstimate.y, this.heading);
+ }
+
+ public void setPose(Pose2d pose) {
+ this.poseEstimate = new Vec2d(pose.x, pose.y);
+ }
+
+ public void setHeading(double heading) {
+ this.heading = heading;
+ }
+}
+
+class KalmanPose2d extends Vec2d {
+ public double timestamp = 0;
+
+ public KalmanPose2d(double x, double y, double timestamp) {
+ super(x, y);
+ this.timestamp = timestamp;
+ }
+
+ public KalmanPose2d(Vec2d vec, double timestamp) {
+ super(vec.x, vec.y);
+ this.timestamp = timestamp;
+ }
+}

quail/src/main/java/com/mineinjava/quail/pathing/ConstraintsPair.java

@@ -1,36 +1,39 @@
 package com.mineinjava.quail.pathing;
 
+/**
+ * Represents a pair of constraints (max velocity and max acceleration) for use in the path
+ * follower. This can be either translational or angular velocity. If angular velocity, units are in
+ * radians.
+ */
 public class ConstraintsPair {
 
- private double velocity;
- private double acceleration;
+ private final double velocity;
+ private final double acceleration;
 
  /**
- * Represents a pair of constraints (max velocity and max acceleration) for use in the path
- * follower Can be either translational or angular velocity. If angular velocity, units are in
- * radians.
+ * Constructs a new ConstraintsPair with the specified max velocity and max acceleration.
  *
- * @param maxVelocity your units/s
- * @param maxAcceleration your units/s^2
+ * @param maxVelocity the maximum velocity in units/s
+ * @param maxAcceleration the maximum acceleration in units/s^2
  */
  public ConstraintsPair(double maxVelocity, double maxAcceleration) {
  this.velocity = maxVelocity;
  this.acceleration = maxAcceleration;
  }
 
  /**
- * Returns the constraints' max velocity
+ * Returns the maximum velocity of the constraints.
  *
- * @return
+ * @return the maximum velocity in units/s
  */
  public double getMaxVelocity() {
  return velocity;
  }
 
  /**
- * Returns the constraints' max acceleration
+ * Returns the maximum acceleration of the constraints.
  *
- * @return
+ * @return the maximum acceleration in units/s^2
  */
  public double getMaxAcceleration() {
  return acceleration;

.github/workflows/linting.yml → .github/workflows/ci.yml

@@ -15,4 +15,15 @@ jobs:
  distribution: 'zulu'
  java-version: 17
  - name: Run spotless linter
- run: ./gradlew spotlessCheck
+ run: ./gradlew spotlessCheck
+
+ tests:
+ runs-on: ubuntu-latest
+ steps:
+ - uses: actions/checkout@v4
+ - uses: actions/setup-java@v4
+ with:
+ distribution: 'zulu'
+ java-version: 17
+ - name: Run tests
+ run: ./gradlew test

quail/src/main/java/com/mineinjava/quail/SwerveModuleBase.java

@@ -1,6 +1,6 @@
 package com.mineinjava.quail;
 
-import com.mineinjava.quail.util.Util;
+import com.mineinjava.quail.util.MathUtil;
 import com.mineinjava.quail.util.geometry.Vec2d;
 
 public class SwerveModuleBase {
@@ -47,7 +47,7 @@ public SwerveModuleBase(
  * @return the angle to set the steering motor to
  */
  public double calculateNewAngleSetpoint(double angle) {
- double shortestAngle = Util.deltaAngle(currentAngle, angle);
+ double shortestAngle = MathUtil.deltaAngle(currentAngle, angle);
  return currentAngle = currentAngle + shortestAngle;
  }
 
@@ -56,7 +56,7 @@ public double calculateNewAngleSetpoint(double angle) {
  * opposite direction and rotate less than 90 degrees
  */
  public double calculateOptimizedAngle(double angle) {
- double deltaAngle = Util.deltaAngle(currentAngle, angle);
+ double deltaAngle = MathUtil.deltaAngle(currentAngle, angle);
  if (Math.abs(deltaAngle) > Math.PI / 2) {
  motorFlipper = -1;
  return angle + Math.PI;

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

@@ -0,0 +1,80 @@
+package com.mineinjava.quail.localization;
+
+import com.mineinjava.quail.util.MathUtil;
+import com.mineinjava.quail.util.geometry.Pose2d;
+import com.mineinjava.quail.util.geometry.Vec2d;
+import java.util.ArrayList;
+
+public class KalmanFilterLocalizer implements Localizer {
+ private Vec2d poseEstimate = new Vec2d(0, 0);
+ 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) {
+ poseEstimate = initialPose;
+ this.looptime = looptime;
+ }
+
+ /**
+ * Updates the pose estimate 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 poseEstimateLatency the time since the last vision update
+ */
+ public Vec2d update(Vec2d observedPose, Vec2d velocity, double poseEstimateLatency, double w) {
+ KalmanPose2d currentVelocity = new KalmanPose2d(velocity, System.currentTimeMillis());
+ velocities.add(currentVelocity); // add the current velocity to the list of velocities
+ // trim the list of velocities to only include the relevant ones.
+ while (velocities.size() > 0
+ && velocities.get(0).timestamp < System.currentTimeMillis() - poseEstimateLatency) {
+ velocities.remove(0);
+ }
+ // distance traveled since the vision update
+ Vec2d deltaPosSinceVision = new Vec2d(0, 0);
+ for (KalmanPose2d vel : velocities) {
+ deltaPosSinceVision =
+ deltaPosSinceVision.add(
+ vel); // don't scale here, we'll do it later (distributive property)
+ }
+ deltaPosSinceVision =
+ deltaPosSinceVision.scale(looptime); // scale by the looptime: distance = velocity * time
+ // update the vision pose estimate with the delta from velocity
+ Vec2d visionPoseEstimate = observedPose.add(deltaPosSinceVision);
+ // update the last pose estimate with the velocity
+ Vec2d kinematicsPoseEstimate = this.poseEstimate.add(velocity.scale(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)));
+ return this.poseEstimate;
+ }
+
+ public Pose2d getPose() {
+ return new Pose2d(poseEstimate.x, poseEstimate.y, this.heading);
+ }
+
+ public void setPose(Pose2d pose) {
+ this.poseEstimate = new Vec2d(pose.x, pose.y);
+ }
+
+ public void setHeading(double heading) {
+ this.heading = heading;
+ }
+}
+
+class KalmanPose2d extends Vec2d {
+ public double timestamp = 0;
+
+ public KalmanPose2d(double x, double y, double timestamp) {
+ super(x, y);
+ this.timestamp = timestamp;
+ }
+
+ public KalmanPose2d(Vec2d vec, double timestamp) {
+ super(vec.x, vec.y);
+ this.timestamp = timestamp;
+ }
+}

quail/src/main/java/com/mineinjava/quail/pathing/ConstraintsPair.java

@@ -1,36 +1,39 @@
 package com.mineinjava.quail.pathing;
 
+/**
+ * Represents a pair of constraints (max velocity and max acceleration) for use in the path
+ * follower. This can be either translational or angular velocity. If angular velocity, units are in
+ * radians.
+ */
 public class ConstraintsPair {
 
- private double velocity;
- private double acceleration;
+ private final double velocity;
+ private final double acceleration;
 
  /**
- * Represents a pair of constraints (max velocity and max acceleration) for use in the path
- * follower Can be either translational or angular velocity. If angular velocity, units are in
- * radians.
+ * Constructs a new ConstraintsPair with the specified max velocity and max acceleration.
  *
- * @param maxVelocity your units/s
- * @param maxAcceleration your units/s^2
+ * @param maxVelocity the maximum velocity in units/s
+ * @param maxAcceleration the maximum acceleration in units/s^2
  */
  public ConstraintsPair(double maxVelocity, double maxAcceleration) {
  this.velocity = maxVelocity;
  this.acceleration = maxAcceleration;
  }
 
  /**
- * Returns the constraints' max velocity
+ * Returns the maximum velocity of the constraints.
  *
- * @return
+ * @return the maximum velocity in units/s
  */
  public double getMaxVelocity() {
  return velocity;
  }
 
  /**
- * Returns the constraints' max acceleration
+ * Returns the maximum acceleration of the constraints.
  *
- * @return
+ * @return the maximum acceleration in units/s^2
  */
  public double getMaxAcceleration() {
  return acceleration;

.github/workflows/linting.yml → .github/workflows/ci.yml

@@ -15,4 +15,15 @@ jobs:
  distribution: 'zulu'
  java-version: 17
  - name: Run spotless linter
- run: ./gradlew spotlessCheck
+ run: ./gradlew spotlessCheck
+
+ tests:
+ runs-on: ubuntu-latest
+ steps:
+ - uses: actions/checkout@v4
+ - uses: actions/setup-java@v4
+ with:
+ distribution: 'zulu'
+ java-version: 17
+ - name: Run tests
+ run: ./gradlew test

quail/src/main/java/com/mineinjava/quail/SwerveModuleBase.java

@@ -1,6 +1,6 @@
 package com.mineinjava.quail;
 
-import com.mineinjava.quail.util.Util;
+import com.mineinjava.quail.util.MathUtil;
 import com.mineinjava.quail.util.geometry.Vec2d;
 
 public class SwerveModuleBase {
@@ -47,7 +47,7 @@ public SwerveModuleBase(
  * @return the angle to set the steering motor to
  */
  public double calculateNewAngleSetpoint(double angle) {
- double shortestAngle = Util.deltaAngle(currentAngle, angle);
+ double shortestAngle = MathUtil.deltaAngle(currentAngle, angle);
  return currentAngle = currentAngle + shortestAngle;
  }
 
@@ -56,7 +56,7 @@ public double calculateNewAngleSetpoint(double angle) {
  * opposite direction and rotate less than 90 degrees
  */
  public double calculateOptimizedAngle(double angle) {
- double deltaAngle = Util.deltaAngle(currentAngle, angle);
+ double deltaAngle = MathUtil.deltaAngle(currentAngle, angle);
  if (Math.abs(deltaAngle) > Math.PI / 2) {
  motorFlipper = -1;
  return angle + Math.PI;

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

@@ -0,0 +1,80 @@
+package com.mineinjava.quail.localization;
+
+import com.mineinjava.quail.util.MathUtil;
+import com.mineinjava.quail.util.geometry.Pose2d;
+import com.mineinjava.quail.util.geometry.Vec2d;
+import java.util.ArrayList;
+
+public class KalmanFilterLocalizer implements Localizer {
+ private Vec2d poseEstimate = new Vec2d(0, 0);
+ 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) {
+ poseEstimate = initialPose;
+ this.looptime = looptime;
+ }
+
+ /**
+ * Updates the pose estimate 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 poseEstimateLatency the time since the last vision update
+ */
+ public Vec2d update(Vec2d observedPose, Vec2d velocity, double poseEstimateLatency, double w) {
+ KalmanPose2d currentVelocity = new KalmanPose2d(velocity, System.currentTimeMillis());
+ velocities.add(currentVelocity); // add the current velocity to the list of velocities
+ // trim the list of velocities to only include the relevant ones.
+ while (velocities.size() > 0
+ && velocities.get(0).timestamp < System.currentTimeMillis() - poseEstimateLatency) {
+ velocities.remove(0);
+ }
+ // distance traveled since the vision update
+ Vec2d deltaPosSinceVision = new Vec2d(0, 0);
+ for (KalmanPose2d vel : velocities) {
+ deltaPosSinceVision =
+ deltaPosSinceVision.add(
+ vel); // don't scale here, we'll do it later (distributive property)
+ }
+ deltaPosSinceVision =
+ deltaPosSinceVision.scale(looptime); // scale by the looptime: distance = velocity * time
+ // update the vision pose estimate with the delta from velocity
+ Vec2d visionPoseEstimate = observedPose.add(deltaPosSinceVision);
+ // update the last pose estimate with the velocity
+ Vec2d kinematicsPoseEstimate = this.poseEstimate.add(velocity.scale(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)));
+ return this.poseEstimate;
+ }
+
+ public Pose2d getPose() {
+ return new Pose2d(poseEstimate.x, poseEstimate.y, this.heading);
+ }
+
+ public void setPose(Pose2d pose) {
+ this.poseEstimate = new Vec2d(pose.x, pose.y);
+ }
+
+ public void setHeading(double heading) {
+ this.heading = heading;
+ }
+}
+
+class KalmanPose2d extends Vec2d {
+ public double timestamp = 0;
+
+ public KalmanPose2d(double x, double y, double timestamp) {
+ super(x, y);
+ this.timestamp = timestamp;
+ }
+
+ public KalmanPose2d(Vec2d vec, double timestamp) {
+ super(vec.x, vec.y);
+ this.timestamp = timestamp;
+ }
+}

quail/src/main/java/com/mineinjava/quail/pathing/ConstraintsPair.java

@@ -1,36 +1,39 @@
 package com.mineinjava.quail.pathing;
 
+/**
+ * Represents a pair of constraints (max velocity and max acceleration) for use in the path
+ * follower. This can be either translational or angular velocity. If angular velocity, units are in
+ * radians.
+ */
 public class ConstraintsPair {
 
- private double velocity;
- private double acceleration;
+ private final double velocity;
+ private final double acceleration;
 
  /**
- * Represents a pair of constraints (max velocity and max acceleration) for use in the path
- * follower Can be either translational or angular velocity. If angular velocity, units are in
- * radians.
+ * Constructs a new ConstraintsPair with the specified max velocity and max acceleration.
  *
- * @param maxVelocity your units/s
- * @param maxAcceleration your units/s^2
+ * @param maxVelocity the maximum velocity in units/s
+ * @param maxAcceleration the maximum acceleration in units/s^2
  */
  public ConstraintsPair(double maxVelocity, double maxAcceleration) {
  this.velocity = maxVelocity;
  this.acceleration = maxAcceleration;
  }
 
  /**
- * Returns the constraints' max velocity
+ * Returns the maximum velocity of the constraints.
  *
- * @return
+ * @return the maximum velocity in units/s
  */
  public double getMaxVelocity() {
  return velocity;
  }
 
  /**
- * Returns the constraints' max acceleration
+ * Returns the maximum acceleration of the constraints.
  *
- * @return
+ * @return the maximum acceleration in units/s^2
  */
  public double getMaxAcceleration() {
  return acceleration;

.github/workflows/linting.yml → .github/workflows/ci.yml

@@ -15,4 +15,15 @@ jobs:
  distribution: 'zulu'
  java-version: 17
  - name: Run spotless linter
- run: ./gradlew spotlessCheck
+ run: ./gradlew spotlessCheck
+
+ tests:
+ runs-on: ubuntu-latest
+ steps:
+ - uses: actions/checkout@v4
+ - uses: actions/setup-java@v4
+ with:
+ distribution: 'zulu'
+ java-version: 17
+ - name: Run tests
+ run: ./gradlew test

quail/src/main/java/com/mineinjava/quail/SwerveModuleBase.java

@@ -1,6 +1,6 @@
 package com.mineinjava.quail;
 
-import com.mineinjava.quail.util.Util;
+import com.mineinjava.quail.util.MathUtil;
 import com.mineinjava.quail.util.geometry.Vec2d;
 
 public class SwerveModuleBase {
@@ -47,7 +47,7 @@ public SwerveModuleBase(
  * @return the angle to set the steering motor to
  */
  public double calculateNewAngleSetpoint(double angle) {
- double shortestAngle = Util.deltaAngle(currentAngle, angle);
+ double shortestAngle = MathUtil.deltaAngle(currentAngle, angle);
  return currentAngle = currentAngle + shortestAngle;
  }
 
@@ -56,7 +56,7 @@ public double calculateNewAngleSetpoint(double angle) {
  * opposite direction and rotate less than 90 degrees
  */
  public double calculateOptimizedAngle(double angle) {
- double deltaAngle = Util.deltaAngle(currentAngle, angle);
+ double deltaAngle = MathUtil.deltaAngle(currentAngle, angle);
  if (Math.abs(deltaAngle) > Math.PI / 2) {
  motorFlipper = -1;
  return angle + Math.PI;

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

@@ -0,0 +1,80 @@
+package com.mineinjava.quail.localization;
+
+import com.mineinjava.quail.util.MathUtil;
+import com.mineinjava.quail.util.geometry.Pose2d;
+import com.mineinjava.quail.util.geometry.Vec2d;
+import java.util.ArrayList;
+
+public class KalmanFilterLocalizer implements Localizer {
+ private Vec2d poseEstimate = new Vec2d(0, 0);
+ 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) {
+ poseEstimate = initialPose;
+ this.looptime = looptime;
+ }
+
+ /**
+ * Updates the pose estimate 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 poseEstimateLatency the time since the last vision update
+ */
+ public Vec2d update(Vec2d observedPose, Vec2d velocity, double poseEstimateLatency, double w) {
+ KalmanPose2d currentVelocity = new KalmanPose2d(velocity, System.currentTimeMillis());
+ velocities.add(currentVelocity); // add the current velocity to the list of velocities
+ // trim the list of velocities to only include the relevant ones.
+ while (velocities.size() > 0
+ && velocities.get(0).timestamp < System.currentTimeMillis() - poseEstimateLatency) {
+ velocities.remove(0);
+ }
+ // distance traveled since the vision update
+ Vec2d deltaPosSinceVision = new Vec2d(0, 0);
+ for (KalmanPose2d vel : velocities) {
+ deltaPosSinceVision =
+ deltaPosSinceVision.add(
+ vel); // don't scale here, we'll do it later (distributive property)
+ }
+ deltaPosSinceVision =
+ deltaPosSinceVision.scale(looptime); // scale by the looptime: distance = velocity * time
+ // update the vision pose estimate with the delta from velocity
+ Vec2d visionPoseEstimate = observedPose.add(deltaPosSinceVision);
+ // update the last pose estimate with the velocity
+ Vec2d kinematicsPoseEstimate = this.poseEstimate.add(velocity.scale(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)));
+ return this.poseEstimate;
+ }
+
+ public Pose2d getPose() {
+ return new Pose2d(poseEstimate.x, poseEstimate.y, this.heading);
+ }
+
+ public void setPose(Pose2d pose) {
+ this.poseEstimate = new Vec2d(pose.x, pose.y);
+ }
+
+ public void setHeading(double heading) {
+ this.heading = heading;
+ }
+}
+
+class KalmanPose2d extends Vec2d {
+ public double timestamp = 0;
+
+ public KalmanPose2d(double x, double y, double timestamp) {
+ super(x, y);
+ this.timestamp = timestamp;
+ }
+
+ public KalmanPose2d(Vec2d vec, double timestamp) {
+ super(vec.x, vec.y);
+ this.timestamp = timestamp;
+ }
+}

quail/src/main/java/com/mineinjava/quail/pathing/ConstraintsPair.java

@@ -1,36 +1,39 @@
 package com.mineinjava.quail.pathing;
 
+/**
+ * Represents a pair of constraints (max velocity and max acceleration) for use in the path
+ * follower. This can be either translational or angular velocity. If angular velocity, units are in
+ * radians.
+ */
 public class ConstraintsPair {
 
- private double velocity;
- private double acceleration;
+ private final double velocity;
+ private final double acceleration;
 
  /**
- * Represents a pair of constraints (max velocity and max acceleration) for use in the path
- * follower Can be either translational or angular velocity. If angular velocity, units are in
- * radians.
+ * Constructs a new ConstraintsPair with the specified max velocity and max acceleration.
  *
- * @param maxVelocity your units/s
- * @param maxAcceleration your units/s^2
+ * @param maxVelocity the maximum velocity in units/s
+ * @param maxAcceleration the maximum acceleration in units/s^2
  */
  public ConstraintsPair(double maxVelocity, double maxAcceleration) {
  this.velocity = maxVelocity;
  this.acceleration = maxAcceleration;
  }
 
  /**
- * Returns the constraints' max velocity
+ * Returns the maximum velocity of the constraints.
  *
- * @return
+ * @return the maximum velocity in units/s
  */
  public double getMaxVelocity() {
  return velocity;
  }
 
  /**
- * Returns the constraints' max acceleration
+ * Returns the maximum acceleration of the constraints.
  *
- * @return
+ * @return the maximum acceleration in units/s^2
  */
  public double getMaxAcceleration() {
  return acceleration;

.github/workflows/linting.yml → .github/workflows/ci.yml

@@ -15,4 +15,15 @@ jobs:
  distribution: 'zulu'
  java-version: 17
  - name: Run spotless linter
- run: ./gradlew spotlessCheck
+ run: ./gradlew spotlessCheck
+
+ tests:
+ runs-on: ubuntu-latest
+ steps:
+ - uses: actions/checkout@v4
+ - uses: actions/setup-java@v4
+ with:
+ distribution: 'zulu'
+ java-version: 17
+ - name: Run tests
+ run: ./gradlew test

quail/src/main/java/com/mineinjava/quail/SwerveModuleBase.java

@@ -1,6 +1,6 @@
 package com.mineinjava.quail;
 
-import com.mineinjava.quail.util.Util;
+import com.mineinjava.quail.util.MathUtil;
 import com.mineinjava.quail.util.geometry.Vec2d;
 
 public class SwerveModuleBase {
@@ -47,7 +47,7 @@ public SwerveModuleBase(
  * @return the angle to set the steering motor to
  */
  public double calculateNewAngleSetpoint(double angle) {
- double shortestAngle = Util.deltaAngle(currentAngle, angle);
+ double shortestAngle = MathUtil.deltaAngle(currentAngle, angle);
  return currentAngle = currentAngle + shortestAngle;
  }
 
@@ -56,7 +56,7 @@ public double calculateNewAngleSetpoint(double angle) {
  * opposite direction and rotate less than 90 degrees
  */
  public double calculateOptimizedAngle(double angle) {
- double deltaAngle = Util.deltaAngle(currentAngle, angle);
+ double deltaAngle = MathUtil.deltaAngle(currentAngle, angle);
  if (Math.abs(deltaAngle) > Math.PI / 2) {
  motorFlipper = -1;
  return angle + Math.PI;

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

@@ -0,0 +1,80 @@
+package com.mineinjava.quail.localization;
+
+import com.mineinjava.quail.util.MathUtil;
+import com.mineinjava.quail.util.geometry.Pose2d;
+import com.mineinjava.quail.util.geometry.Vec2d;
+import java.util.ArrayList;
+
+public class KalmanFilterLocalizer implements Localizer {
+ private Vec2d poseEstimate = new Vec2d(0, 0);
+ 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) {
+ poseEstimate = initialPose;
+ this.looptime = looptime;
+ }
+
+ /**
+ * Updates the pose estimate 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 poseEstimateLatency the time since the last vision update
+ */
+ public Vec2d update(Vec2d observedPose, Vec2d velocity, double poseEstimateLatency, double w) {
+ KalmanPose2d currentVelocity = new KalmanPose2d(velocity, System.currentTimeMillis());
+ velocities.add(currentVelocity); // add the current velocity to the list of velocities
+ // trim the list of velocities to only include the relevant ones.
+ while (velocities.size() > 0
+ && velocities.get(0).timestamp < System.currentTimeMillis() - poseEstimateLatency) {
+ velocities.remove(0);
+ }
+ // distance traveled since the vision update
+ Vec2d deltaPosSinceVision = new Vec2d(0, 0);
+ for (KalmanPose2d vel : velocities) {
+ deltaPosSinceVision =
+ deltaPosSinceVision.add(
+ vel); // don't scale here, we'll do it later (distributive property)
+ }
+ deltaPosSinceVision =
+ deltaPosSinceVision.scale(looptime); // scale by the looptime: distance = velocity * time
+ // update the vision pose estimate with the delta from velocity
+ Vec2d visionPoseEstimate = observedPose.add(deltaPosSinceVision);
+ // update the last pose estimate with the velocity
+ Vec2d kinematicsPoseEstimate = this.poseEstimate.add(velocity.scale(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)));
+ return this.poseEstimate;
+ }
+
+ public Pose2d getPose() {
+ return new Pose2d(poseEstimate.x, poseEstimate.y, this.heading);
+ }
+
+ public void setPose(Pose2d pose) {
+ this.poseEstimate = new Vec2d(pose.x, pose.y);
+ }
+
+ public void setHeading(double heading) {
+ this.heading = heading;
+ }
+}
+
+class KalmanPose2d extends Vec2d {
+ public double timestamp = 0;
+
+ public KalmanPose2d(double x, double y, double timestamp) {
+ super(x, y);
+ this.timestamp = timestamp;
+ }
+
+ public KalmanPose2d(Vec2d vec, double timestamp) {
+ super(vec.x, vec.y);
+ this.timestamp = timestamp;
+ }
+}

quail/src/main/java/com/mineinjava/quail/pathing/ConstraintsPair.java

@@ -1,36 +1,39 @@
 package com.mineinjava.quail.pathing;
 
+/**
+ * Represents a pair of constraints (max velocity and max acceleration) for use in the path
+ * follower. This can be either translational or angular velocity. If angular velocity, units are in
+ * radians.
+ */
 public class ConstraintsPair {
 
- private double velocity;
- private double acceleration;
+ private final double velocity;
+ private final double acceleration;
 
  /**
- * Represents a pair of constraints (max velocity and max acceleration) for use in the path
- * follower Can be either translational or angular velocity. If angular velocity, units are in
- * radians.
+ * Constructs a new ConstraintsPair with the specified max velocity and max acceleration.
  *
- * @param maxVelocity your units/s
- * @param maxAcceleration your units/s^2
+ * @param maxVelocity the maximum velocity in units/s
+ * @param maxAcceleration the maximum acceleration in units/s^2
  */
  public ConstraintsPair(double maxVelocity, double maxAcceleration) {
  this.velocity = maxVelocity;
  this.acceleration = maxAcceleration;
  }
 
  /**
- * Returns the constraints' max velocity
+ * Returns the maximum velocity of the constraints.
  *
- * @return
+ * @return the maximum velocity in units/s
  */
  public double getMaxVelocity() {
  return velocity;
  }
 
  /**
- * Returns the constraints' max acceleration
+ * Returns the maximum acceleration of the constraints.
  *
- * @return
+ * @return the maximum acceleration in units/s^2
  */
  public double getMaxAcceleration() {
  return acceleration;