From e9310f8535b3ede12ef5edf5f4b79f0e0909f645 Mon Sep 17 00:00:00 2001 From: Mineinjava <65673396+Mineinjava@users.noreply.github.com> Date: Thu, 8 Aug 2024 22:41:24 -0700 Subject: [PATCH 1/6] add rotation to kalman filter fixes #50 needs testing --- .../localization/KalmanFilterLocalizer.java | 66 +++++++++++++------ 1 file changed, 45 insertions(+), 21 deletions(-) diff --git a/quail/src/main/java/com/mineinjava/quail/localization/KalmanFilterLocalizer.java b/quail/src/main/java/com/mineinjava/quail/localization/KalmanFilterLocalizer.java index 7a9bb61..d90055d 100644 --- a/quail/src/main/java/com/mineinjava/quail/localization/KalmanFilterLocalizer.java +++ b/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 velocities = new ArrayList(); // / 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; } @@ -55,11 +55,24 @@ public KalmanFilterLocalizer(Vec2d initialPose, double looptime) { * @param w how much weight to "trust" the vision estimate * @param timestampMillis the current system time in ms */ - public Vec2d update( - Vec2d observedPose, - Vec2d velocity, + + /** + * Updates the pose estimate. + * + *

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 poseEstimateLatency the time since the last vision update + * @param w how much weight to "trust" the vision estimate + * @param timestampMillis the current system time in ms + */ + 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 @@ -69,23 +82,34 @@ 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) + deltaTranslationSinceVision = + deltaTranslationSinceVision.add( + vel.vec()); // don't scale here, we'll do it later (distributive property) + deltaRotationSinceVision += vel.heading; } - deltaPosSinceVision = - deltaPosSinceVision.scale(looptime); // scale by the looptime: distance = velocity * time + + deltaTranslationSinceVision = + deltaTranslationSinceVision.scale(this.looptime); // scale by the looptime: distance = velocity * time + 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))); + + 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() { @@ -100,7 +124,7 @@ public Pose2d getPose() { * @param pose new translational pose to use */ public void setPose(Pose2d pose) { - this.poseEstimate = new Vec2d(pose.x, pose.y); + this.poseEstimate = pose; } /** @@ -116,16 +140,16 @@ public void setHeading(double heading) { } /** Modified translational pose but it has a timestamp. */ -class KalmanPose2d extends Vec2d { +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; } } From 2f0c19efed74c24f9b147f97f051fddea196be61 Mon Sep 17 00:00:00 2001 From: Mineinjava <65673396+Mineinjava@users.noreply.github.com> Date: Thu, 8 Aug 2024 22:51:41 -0700 Subject: [PATCH 2/6] start testing --- .../quail/localization/KalmanFilterLocalizer.java | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/quail/src/main/java/com/mineinjava/quail/localization/KalmanFilterLocalizer.java b/quail/src/main/java/com/mineinjava/quail/localization/KalmanFilterLocalizer.java index d90055d..5da2d11 100644 --- a/quail/src/main/java/com/mineinjava/quail/localization/KalmanFilterLocalizer.java +++ b/quail/src/main/java/com/mineinjava/quail/localization/KalmanFilterLocalizer.java @@ -44,18 +44,6 @@ public KalmanFilterLocalizer(Pose2d initialPose, double looptime) { this.looptime = looptime; } - /** - * Updates the pose estimate. - * - *

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 poseEstimateLatency the time since the last vision update - * @param w how much weight to "trust" the vision estimate - * @param timestampMillis the current system time in ms - */ - /** * Updates the pose estimate. * From 395916de54dffc4c4bedaf02e7d0f76197bac3de Mon Sep 17 00:00:00 2001 From: Mineinjava <65673396+Mineinjava@users.noreply.github.com> Date: Thu, 8 Aug 2024 22:52:42 -0700 Subject: [PATCH 3/6] fix format --- .../localization/KalmanFilterLocalizer.java | 23 +++++++++++-------- 1 file changed, 14 insertions(+), 9 deletions(-) diff --git a/quail/src/main/java/com/mineinjava/quail/localization/KalmanFilterLocalizer.java b/quail/src/main/java/com/mineinjava/quail/localization/KalmanFilterLocalizer.java index 5da2d11..8bfd842 100644 --- a/quail/src/main/java/com/mineinjava/quail/localization/KalmanFilterLocalizer.java +++ b/quail/src/main/java/com/mineinjava/quail/localization/KalmanFilterLocalizer.java @@ -74,30 +74,35 @@ public Pose2d update( double deltaRotationSinceVision = 0d; for (KalmanPose2d vel : velocities) { deltaTranslationSinceVision = - deltaTranslationSinceVision.add( - vel.vec()); // don't scale here, we'll do it later (distributive property) + deltaTranslationSinceVision.add( + vel.vec()); // don't scale here, we'll do it later (distributive property) deltaRotationSinceVision += vel.heading; } deltaTranslationSinceVision = - deltaTranslationSinceVision.scale(this.looptime); // scale by the looptime: distance = velocity * time + deltaTranslationSinceVision.scale( + this.looptime); // scale by the looptime: distance = velocity * time deltaRotationSinceVision = deltaRotationSinceVision * this.looptime; // update the vision pose estimate with the delta from velocity - Vec2d updatedPoseSinceVision = observedPose.vec().add(deltaTranslationSinceVision ); + Vec2d updatedPoseSinceVision = observedPose.vec().add(deltaTranslationSinceVision); double updatedRotationSinceVision = observedPose.heading + deltaRotationSinceVision; // update the last pose estimate with the velocity - Vec2d kinematicsTranslationEstimate = this.poseEstimate.vec().add(velocity.vec().scale(this.looptime)); - double kinematicsRotationEstimate = this.poseEstimate.heading + (velocity.heading * 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) - Vec2d translationEstimate = ((updatedPoseSinceVision.scale(w)).add(kinematicsTranslationEstimate.scale(1 - w))); - double rotationEstimate = (updatedRotationSinceVision*hw) + (kinematicsRotationEstimate * (1-hw)); + 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() { From a489c8a34a9c57016770e619835367b6dcf2ee68 Mon Sep 17 00:00:00 2001 From: Mineinjava <65673396+Mineinjava@users.noreply.github.com> Date: Wed, 11 Dec 2024 12:56:35 -0800 Subject: [PATCH 4/6] finish adding rotation --- .../localization/KalmanFilterLocalizer.java | 35 +++++++++++-------- 1 file changed, 20 insertions(+), 15 deletions(-) diff --git a/quail/src/main/java/com/mineinjava/quail/localization/KalmanFilterLocalizer.java b/quail/src/main/java/com/mineinjava/quail/localization/KalmanFilterLocalizer.java index 8bfd842..309ef38 100644 --- a/quail/src/main/java/com/mineinjava/quail/localization/KalmanFilterLocalizer.java +++ b/quail/src/main/java/com/mineinjava/quail/localization/KalmanFilterLocalizer.java @@ -49,10 +49,11 @@ public KalmanFilterLocalizer(Pose2d initialPose, double looptime) { * *

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 Pose2d update( @@ -62,6 +63,7 @@ public Pose2d update( 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. @@ -73,15 +75,13 @@ public Pose2d update( Vec2d deltaTranslationSinceVision = new Vec2d(0, 0); double deltaRotationSinceVision = 0d; for (KalmanPose2d vel : velocities) { - deltaTranslationSinceVision = - deltaTranslationSinceVision.add( - vel.vec()); // 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; } - deltaTranslationSinceVision = - deltaTranslationSinceVision.scale( - this.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 @@ -94,10 +94,13 @@ public Pose2d update( 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) + // 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); @@ -106,7 +109,7 @@ public Pose2d update( /** 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); } /** @@ -114,7 +117,7 @@ public Pose2d getPose() { * *

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 = pose; @@ -123,8 +126,6 @@ public void setPose(Pose2d pose) { /** * Sets the current heading. * - *

Call this on every frame with the robot's heading - * * @param heading Robot heading */ public void setHeading(double heading) { @@ -132,7 +133,11 @@ public void setHeading(double heading) { } } -/** Modified translational pose but it has a timestamp. */ +/** + * Modified translational pose but it has a timestamp. + * + * @see Pose2d + */ class KalmanPose2d extends Pose2d { public double timestamp = 0; From f146c9a21e042ac97796d902717dfcc78eac97e1 Mon Sep 17 00:00:00 2001 From: Mineinjava <65673396+Mineinjava@users.noreply.github.com> Date: Wed, 11 Dec 2024 12:59:32 -0800 Subject: [PATCH 5/6] Add localizer test --- .../pathing/KalmanFilterLocalizerTest.java | 29 +++++++++++++++++++ 1 file changed, 29 insertions(+) create mode 100644 quail/src/test/java/quail/pathing/KalmanFilterLocalizerTest.java diff --git a/quail/src/test/java/quail/pathing/KalmanFilterLocalizerTest.java b/quail/src/test/java/quail/pathing/KalmanFilterLocalizerTest.java new file mode 100644 index 0000000..35f8977 --- /dev/null +++ b/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 + +package quail.pathing; + +import org.junit.jupiter.api.Test; + +/** KalmanFilterLocalizer */ +public class KalmanFilterLocalizerTest { + @Test + void constructor() {} +} From 6431fd0af27b449854531a5e8f6b53d38a1ea02f Mon Sep 17 00:00:00 2001 From: Mineinjava <65673396+Mineinjava@users.noreply.github.com> Date: Wed, 11 Dec 2024 13:01:24 -0800 Subject: [PATCH 6/6] fix tests --- quail/src/test/java/quail/pathing/PathFollowerTest.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/quail/src/test/java/quail/pathing/PathFollowerTest.java b/quail/src/test/java/quail/pathing/PathFollowerTest.java index 49a144a..bf2853d 100644 --- a/quail/src/test/java/quail/pathing/PathFollowerTest.java +++ b/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)