From b09a293bb1d181875f142037ce26cb429ee79ace Mon Sep 17 00:00:00 2001 From: DANNA2828 Date: Sun, 5 Jul 2026 15:21:44 +0300 Subject: [PATCH 1/6] danna - man there is no way this is getting merged today --- src/main/java/frc/robot/Robot.java | 12 ++- .../robot/poseestimator/IVisionEstimator.java | 2 +- .../WPILibPoseEstimatorConstants.java | 2 + .../WPILibPoseEstimatorWrapper.java | 75 ++++++++++++------- .../frc/utils/math/StandardDeviations2D.java | 4 + src/main/java/frc/utils/pose/PoseUtil.java | 4 + 6 files changed, 71 insertions(+), 28 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a68886440a..8410cf0428 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -47,6 +47,7 @@ import frc.robot.subsystems.swerve.factories.imu.IMUFactory; import frc.robot.subsystems.swerve.factories.modules.ModulesFactory; import frc.robot.statemachine.shooterstatehandler.TurretCalculations; +import frc.robot.vision.RobotPoseObservation; import frc.utils.GamePeriodUtils; import frc.utils.auto.AutonomousChooser; import frc.robot.subsystems.swerve.factories.modules.drive.KrakenX60DriveBuilder; @@ -63,6 +64,7 @@ import frc.utils.time.TimeUtil; import org.littletonrobotics.junction.Logger; +import java.util.ArrayList; import java.util.List; import java.util.function.Supplier; @@ -321,7 +323,15 @@ public void periodic() { getLimelights().forEach(Limelight::updateHardwareInputs); getLimelights().forEach(Limelight::updateMT1); - getLimelights().forEach(limelight -> limelight.getIndependentRobotPose().ifPresent(poseEstimator::updateVision)); + + ArrayList observations = new ArrayList<>(); + getLimelights().forEach(limelight -> limelight.getIndependentRobotPose().ifPresent(observations::add)); + RobotPoseObservation[] observationsArray = observations.toArray(new RobotPoseObservation[0]); + + ArrayList listOfObservationArrays = new ArrayList<>(); + listOfObservationArrays.add(observationsArray); + + poseEstimator.updateVision(listOfObservationArrays.toArray(RobotPoseObservation[][]::new)); poseEstimator.log(); ShootingCalculations diff --git a/src/main/java/frc/robot/poseestimator/IVisionEstimator.java b/src/main/java/frc/robot/poseestimator/IVisionEstimator.java index 555a8015c0..1d7ec4a8f4 100644 --- a/src/main/java/frc/robot/poseestimator/IVisionEstimator.java +++ b/src/main/java/frc/robot/poseestimator/IVisionEstimator.java @@ -5,7 +5,7 @@ public interface IVisionEstimator { - void updateVision(RobotPoseObservation... robotPoseVisionData); + void updateVision(RobotPoseObservation[]... robotPoseVisionData); Pose2d getEstimatedPose(); diff --git a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java index e305a3adb3..f0f552ae58 100644 --- a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java +++ b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java @@ -15,6 +15,8 @@ public class WPILibPoseEstimatorConstants { public static final StandardDeviations2D VISION_STD_DEV_COLLISION_REDUCTION = new StandardDeviations2D(); + public static final StandardDeviations2D VISION_STD_DEV_IDK_REDUCTION = new StandardDeviations2D(0.1, 0.1, 0.1); + public static final double MINIMUM_COLLISION_IMU_ACCELERATION_G = 2; public static final Rotation2d MINIMUM_TILT_IMU_ROLL = Rotation2d.fromDegrees(4); diff --git a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java index ed1b98a326..81d498fb30 100644 --- a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java +++ b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java @@ -1,6 +1,5 @@ package frc.robot.poseestimator.WPILibPoseEstimator; -import edu.wpi.first.math.Matrix; import edu.wpi.first.math.estimator.PoseEstimator; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -12,16 +11,18 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.Odometry; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; import frc.robot.vision.RobotPoseObservation; import frc.robot.poseestimator.IPoseEstimator; import frc.robot.poseestimator.OdometryData; import frc.utils.buffers.RingBuffer.RingBuffer; +import frc.utils.math.StandardDeviations2D; import frc.utils.math.StatisticsMath; import frc.utils.pose.PoseUtil; import org.littletonrobotics.junction.Logger; +import java.util.Arrays; +import java.util.Comparator; +import java.util.List; import java.util.Optional; public class WPILibPoseEstimatorWrapper implements IPoseEstimator { @@ -33,8 +34,8 @@ public class WPILibPoseEstimatorWrapper implements IPoseEstimator { private final RingBuffer poseToIMUYawDifferenceBuffer; private final TimeInterpolatableBuffer imuYawBuffer; private final TimeInterpolatableBuffer imuXYAccelerationGBuffer; - private RobotPoseObservation lastVisionObservation; private OdometryData lastOdometryData; + private double lastVisionUpdateTimestamp; private boolean isIMUOffsetCalibrated; public WPILibPoseEstimatorWrapper( @@ -125,8 +126,8 @@ public void updateOdometry(OdometryData data) { } @Override - public void updateVision(RobotPoseObservation... visionRobotPoseObservations) { - for (RobotPoseObservation visionRobotPoseObservation : visionRobotPoseObservations) { + public void updateVision(RobotPoseObservation[]... visionRobotPoseObservations) { + for (RobotPoseObservation[] visionRobotPoseObservation : visionRobotPoseObservations) { updateVision(visionRobotPoseObservation); } } @@ -170,8 +171,8 @@ public void log() { Logger.recordOutput(logPath + "/estimatedPose", getEstimatedPose()); Logger.recordOutput(logPath + "/odometryPose", getOdometryPose()); Logger.recordOutput(logPath + "/predictedOdometryPose", getPredictedOdometryPose()); - if (lastVisionObservation != null) { - Logger.recordOutput(logPath + "/lastVisionUpdate", lastVisionObservation.timestampSeconds()); + if (lastVisionUpdateTimestamp != 0) { + Logger.recordOutput(logPath + "/lastVisionUpdate", lastVisionUpdateTimestamp); } Logger.recordOutput(logPath + "/lastOdometryUpdate", lastOdometryData.getTimestampSeconds()); Logger.recordOutput(logPath + "/isIMUOffsetCalibrated", isIMUOffsetCalibrated); @@ -207,19 +208,32 @@ public void log() { ); } - private void updateVision(RobotPoseObservation visionRobotPoseObservation) { - addVisionMeasurement(visionRobotPoseObservation); + private void updateVision(RobotPoseObservation[] visionRobotPoseObservations) { + List goodRobotPoseObservations = Arrays.stream(visionRobotPoseObservations) + .filter( + observation -> Arrays.stream(visionRobotPoseObservations) + .anyMatch(other -> !observation.equals(other) && PoseUtil.getDifference(observation.robotPose(), other.robotPose()) < 1) + ) + .toList(); + + Arrays.stream(visionRobotPoseObservations) + .forEach(observation -> addVisionMeasurement(observation, goodRobotPoseObservations.contains(observation))); - getEstimatedPoseToIMUYawDifference( - imuYawBuffer.getSample(visionRobotPoseObservation.timestampSeconds()), - visionRobotPoseObservation.timestampSeconds() - ).ifPresent(yawDifference -> { - poseToIMUYawDifferenceBuffer.insert(yawDifference); + Arrays.stream(visionRobotPoseObservations).forEach(observation -> { + getEstimatedPoseToIMUYawDifference(imuYawBuffer.getSample(observation.timestampSeconds()), observation.timestampSeconds()) + .ifPresent(yawDifference -> { + poseToIMUYawDifferenceBuffer.insert(yawDifference); - if (!isIMUOffsetCalibrated) { - updateIsIMUOffsetCalibrated(); - } + if (!isIMUOffsetCalibrated) { + updateIsIMUOffsetCalibrated(); + } + }); }); + + lastVisionUpdateTimestamp = Arrays.stream(visionRobotPoseObservations) + .max(Comparator.comparingDouble(RobotPoseObservation::timestampSeconds)) + .orElse(new RobotPoseObservation()) + .timestampSeconds(); } private void updateIsIMUOffsetCalibrated() { @@ -234,16 +248,15 @@ public void resetIsIMUOffsetCalibrated() { isIMUOffsetCalibrated = false; } - private void addVisionMeasurement(RobotPoseObservation visionObservation) { + private void addVisionMeasurement(RobotPoseObservation visionObservation, boolean isGood) { poseEstimator.addVisionMeasurement( visionObservation.robotPose(), visionObservation.timestampSeconds(), - getCollisionCompensatedVisionStdDevs(visionObservation) + getIDKCompensatedVisionStdDevs(getCollisionCompensatedVisionStdDevs(visionObservation), isGood).asColumnVector() ); - this.lastVisionObservation = visionObservation; } - private Matrix getCollisionCompensatedVisionStdDevs(RobotPoseObservation visionObservation) { + private StandardDeviations2D getCollisionCompensatedVisionStdDevs(RobotPoseObservation visionObservation) { boolean isColliding = imuXYAccelerationGBuffer.getSample(visionObservation.timestampSeconds()) .map( (imuAccelerationG) -> PoseUtil @@ -252,10 +265,20 @@ private Matrix getCollisionCompensatedVisionStdDevs(RobotPoseObservation .orElse(false); return isColliding - ? visionObservation.stdDevs() - .asColumnVector() - .minus(WPILibPoseEstimatorConstants.VISION_STD_DEV_COLLISION_REDUCTION.asColumnVector()) - : visionObservation.stdDevs().asColumnVector(); + ? new StandardDeviations2D( + visionObservation.stdDevs() + .asColumnVector() + .minus(WPILibPoseEstimatorConstants.VISION_STD_DEV_COLLISION_REDUCTION.asColumnVector()) + ) + : visionObservation.stdDevs(); + } + + private StandardDeviations2D getIDKCompensatedVisionStdDevs(StandardDeviations2D visionStandardDeviations, boolean isGood) { + return isGood + ? new StandardDeviations2D( + visionStandardDeviations.asColumnVector().minus(WPILibPoseEstimatorConstants.VISION_STD_DEV_IDK_REDUCTION.asColumnVector()) + ) + : visionStandardDeviations; } private Optional getEstimatedPoseToIMUYawDifference(Optional gyroYaw, double timestampSeconds) { diff --git a/src/main/java/frc/utils/math/StandardDeviations2D.java b/src/main/java/frc/utils/math/StandardDeviations2D.java index 69db61a909..2841b2250e 100644 --- a/src/main/java/frc/utils/math/StandardDeviations2D.java +++ b/src/main/java/frc/utils/math/StandardDeviations2D.java @@ -19,6 +19,10 @@ public StandardDeviations2D(StandardDeviations2D standardDeviations2D) { this(standardDeviations2D.xStandardDeviations, standardDeviations2D.yStandardDeviations, standardDeviations2D.angleStandardDeviations); } + public StandardDeviations2D(Matrix columnVector) { + this(columnVector.get(0, 0), columnVector.get(1, 0), columnVector.get(2, 0)); + } + public Matrix asColumnVector() { return VecBuilder.fill(xStandardDeviations, yStandardDeviations, angleStandardDeviations); } diff --git a/src/main/java/frc/utils/pose/PoseUtil.java b/src/main/java/frc/utils/pose/PoseUtil.java index 24f15b9b8f..0348be89f8 100644 --- a/src/main/java/frc/utils/pose/PoseUtil.java +++ b/src/main/java/frc/utils/pose/PoseUtil.java @@ -87,6 +87,10 @@ public static double[] rotation3DToRotationArray(Rotation3d rotation3d, AngleUni }; } + public static double getDifference(Pose2d pose1, Pose2d pose2) { + return Math.abs(pose1.minus(pose2).getTranslation().getNorm()) + Math.abs(pose1.minus(pose2).getRotation().getRadians()); + } + public static boolean getIsColliding(Translation2d imuAccelerationG, double minimumCollisionIMUAccelerationG) { return imuAccelerationG.getNorm() >= minimumCollisionIMUAccelerationG; } From b7d6b124a90ed161d3e77f36052d50a7aca50ce1 Mon Sep 17 00:00:00 2001 From: DANNA2828 Date: Sun, 5 Jul 2026 15:28:13 +0300 Subject: [PATCH 2/6] danna - i like good code --- src/main/java/frc/robot/Robot.java | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 8410cf0428..89af4e3018 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -64,8 +64,8 @@ import frc.utils.time.TimeUtil; import org.littletonrobotics.junction.Logger; -import java.util.ArrayList; import java.util.List; +import java.util.Optional; import java.util.function.Supplier; /** @@ -324,14 +324,11 @@ public void periodic() { getLimelights().forEach(Limelight::updateHardwareInputs); getLimelights().forEach(Limelight::updateMT1); - ArrayList observations = new ArrayList<>(); - getLimelights().forEach(limelight -> limelight.getIndependentRobotPose().ifPresent(observations::add)); - RobotPoseObservation[] observationsArray = observations.toArray(new RobotPoseObservation[0]); - - ArrayList listOfObservationArrays = new ArrayList<>(); - listOfObservationArrays.add(observationsArray); - - poseEstimator.updateVision(listOfObservationArrays.toArray(RobotPoseObservation[][]::new)); + RobotPoseObservation[] observationsArray = getLimelights().stream() + .map(Limelight::getIndependentRobotPose) + .flatMap(Optional::stream) + .toArray(RobotPoseObservation[]::new); + poseEstimator.updateVision(new RobotPoseObservation[][] {observationsArray}); poseEstimator.log(); ShootingCalculations From 60c578c585297a148f146e312a54f9062e0646fe Mon Sep 17 00:00:00 2001 From: DANNA2828 Date: Sun, 5 Jul 2026 15:35:16 +0300 Subject: [PATCH 3/6] danna - i like good code yes yes --- src/main/java/frc/utils/pose/PoseUtil.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/utils/pose/PoseUtil.java b/src/main/java/frc/utils/pose/PoseUtil.java index 0348be89f8..db8570bb04 100644 --- a/src/main/java/frc/utils/pose/PoseUtil.java +++ b/src/main/java/frc/utils/pose/PoseUtil.java @@ -88,7 +88,7 @@ public static double[] rotation3DToRotationArray(Rotation3d rotation3d, AngleUni } public static double getDifference(Pose2d pose1, Pose2d pose2) { - return Math.abs(pose1.minus(pose2).getTranslation().getNorm()) + Math.abs(pose1.minus(pose2).getRotation().getRadians()); + return Math.abs(pose1.minus(pose2).getTranslation().getNorm()) + Math.abs(pose1.getRotation().minus(pose2.getRotation()).getRadians()); } public static boolean getIsColliding(Translation2d imuAccelerationG, double minimumCollisionIMUAccelerationG) { From 1cd1d9564767e2b7015209fa8f82c4780ecbed24 Mon Sep 17 00:00:00 2001 From: DANNA2828 Date: Sun, 5 Jul 2026 19:38:45 +0300 Subject: [PATCH 4/6] danna - no way this is getting merged my god --- .../WPILibPoseEstimatorConstants.java | 5 +++ .../WPILibPoseEstimatorWrapper.java | 33 ++++++++++++------- src/main/java/frc/utils/pose/PoseUtil.java | 8 +++-- 3 files changed, 33 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java index f0f552ae58..eea09a9133 100644 --- a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java +++ b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java @@ -2,6 +2,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import frc.constants.MathConstants; import frc.utils.math.StandardDeviations2D; @@ -39,4 +40,8 @@ public class WPILibPoseEstimatorConstants { public static double IMU_XY_ACCELERATION_G_BUFFER_SIZE_SECONDS = 2; + public static double SIMILAR_POSE_TRANSLATION_NORM_TOLERANCE_METERS = 1; + + public static Rotation2d SIMILAR_POSE_ROTATION_TOLERANCE = MathConstants.QUARTER_CIRCLE; + } diff --git a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java index 81d498fb30..397b2e2d15 100644 --- a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java +++ b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java @@ -212,23 +212,14 @@ private void updateVision(RobotPoseObservation[] visionRobotPoseObservations) { List goodRobotPoseObservations = Arrays.stream(visionRobotPoseObservations) .filter( observation -> Arrays.stream(visionRobotPoseObservations) - .anyMatch(other -> !observation.equals(other) && PoseUtil.getDifference(observation.robotPose(), other.robotPose()) < 1) + .anyMatch(other -> !observation.equals(other) && getArePosesSimilar(observation, other)) ) .toList(); Arrays.stream(visionRobotPoseObservations) .forEach(observation -> addVisionMeasurement(observation, goodRobotPoseObservations.contains(observation))); - Arrays.stream(visionRobotPoseObservations).forEach(observation -> { - getEstimatedPoseToIMUYawDifference(imuYawBuffer.getSample(observation.timestampSeconds()), observation.timestampSeconds()) - .ifPresent(yawDifference -> { - poseToIMUYawDifferenceBuffer.insert(yawDifference); - - if (!isIMUOffsetCalibrated) { - updateIsIMUOffsetCalibrated(); - } - }); - }); + Arrays.stream(visionRobotPoseObservations).forEach(this::updateIMUOffset); lastVisionUpdateTimestamp = Arrays.stream(visionRobotPoseObservations) .max(Comparator.comparingDouble(RobotPoseObservation::timestampSeconds)) @@ -236,6 +227,19 @@ private void updateVision(RobotPoseObservation[] visionRobotPoseObservations) { .timestampSeconds(); } + public void updateIMUOffset(RobotPoseObservation visionRobotPoseObservation) { + getEstimatedPoseToIMUYawDifference( + imuYawBuffer.getSample(visionRobotPoseObservation.timestampSeconds()), + visionRobotPoseObservation.timestampSeconds() + ).ifPresent(yawDifference -> { + poseToIMUYawDifferenceBuffer.insert(yawDifference); + + if (!isIMUOffsetCalibrated) { + updateIsIMUOffsetCalibrated(); + } + }); + } + private void updateIsIMUOffsetCalibrated() { double poseToIMUYawDifferenceStdDev = StatisticsMath.calculateStandardDeviations(poseToIMUYawDifferenceBuffer, Rotation2d::getRadians); isIMUOffsetCalibrated = poseToIMUYawDifferenceStdDev < WPILibPoseEstimatorConstants.MAX_POSE_TO_IMU_YAW_DIFFERENCE_STD_DEV @@ -294,4 +298,11 @@ private Pose2d getPredictedOdometryPose() { ); } + private boolean getArePosesSimilar(RobotPoseObservation robotPoseObservation, RobotPoseObservation other) { + return PoseUtil.getDifference(robotPoseObservation.robotPose().getTranslation(), other.robotPose().getTranslation()) + < WPILibPoseEstimatorConstants.SIMILAR_POSE_TRANSLATION_NORM_TOLERANCE_METERS + && PoseUtil.getDifferenceRadians(robotPoseObservation.robotPose().getRotation(), other.robotPose().getRotation()) + < WPILibPoseEstimatorConstants.SIMILAR_POSE_ROTATION_TOLERANCE.getRadians(); + } + } diff --git a/src/main/java/frc/utils/pose/PoseUtil.java b/src/main/java/frc/utils/pose/PoseUtil.java index db8570bb04..458c6664a5 100644 --- a/src/main/java/frc/utils/pose/PoseUtil.java +++ b/src/main/java/frc/utils/pose/PoseUtil.java @@ -87,8 +87,12 @@ public static double[] rotation3DToRotationArray(Rotation3d rotation3d, AngleUni }; } - public static double getDifference(Pose2d pose1, Pose2d pose2) { - return Math.abs(pose1.minus(pose2).getTranslation().getNorm()) + Math.abs(pose1.getRotation().minus(pose2.getRotation()).getRadians()); + public static double getDifference(Translation2d Translation1, Translation2d Translation2) { + return Translation1.minus(Translation2).getNorm(); + } + + public static double getDifferenceRadians(Rotation2d rotation1, Rotation2d rotation2) { + return Math.abs(rotation1.minus(rotation2).getRadians()); } public static boolean getIsColliding(Translation2d imuAccelerationG, double minimumCollisionIMUAccelerationG) { From dcf3d2edf3217e8f4d6a2676f527cac80f25ccaf Mon Sep 17 00:00:00 2001 From: DANNA2828 Date: Sun, 5 Jul 2026 21:27:40 +0300 Subject: [PATCH 5/6] danna - added close timestamps --- .../WPILibPoseEstimatorConstants.java | 2 ++ .../WPILibPoseEstimatorWrapper.java | 18 +++++++++++------- 2 files changed, 13 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java index eea09a9133..d46551dff2 100644 --- a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java +++ b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java @@ -44,4 +44,6 @@ public class WPILibPoseEstimatorConstants { public static Rotation2d SIMILAR_POSE_ROTATION_TOLERANCE = MathConstants.QUARTER_CIRCLE; + public static double SIMILAR_POSE_TIMESTAMP_TOLERANCE_SECONDS = 0.2; + } diff --git a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java index 397b2e2d15..6b8c48f24b 100644 --- a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java +++ b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java @@ -210,10 +210,7 @@ public void log() { private void updateVision(RobotPoseObservation[] visionRobotPoseObservations) { List goodRobotPoseObservations = Arrays.stream(visionRobotPoseObservations) - .filter( - observation -> Arrays.stream(visionRobotPoseObservations) - .anyMatch(other -> !observation.equals(other) && getArePosesSimilar(observation, other)) - ) + .filter(observation -> Arrays.stream(visionRobotPoseObservations).anyMatch(other -> getAreObservationsSimilar(observation, other))) .toList(); Arrays.stream(visionRobotPoseObservations) @@ -298,10 +295,17 @@ private Pose2d getPredictedOdometryPose() { ); } - private boolean getArePosesSimilar(RobotPoseObservation robotPoseObservation, RobotPoseObservation other) { - return PoseUtil.getDifference(robotPoseObservation.robotPose().getTranslation(), other.robotPose().getTranslation()) + private boolean getAreObservationsSimilar(RobotPoseObservation observation, RobotPoseObservation other) { + return !observation.equals(other) + && Math.abs(observation.timestampSeconds() - other.timestampSeconds()) + < WPILibPoseEstimatorConstants.SIMILAR_POSE_TIMESTAMP_TOLERANCE_SECONDS + && getArePosesSimilar(observation.robotPose(), other.robotPose()); + } + + private boolean getArePosesSimilar(Pose2d pose, Pose2d other) { + return PoseUtil.getDifference(pose.getTranslation(), other.getTranslation()) < WPILibPoseEstimatorConstants.SIMILAR_POSE_TRANSLATION_NORM_TOLERANCE_METERS - && PoseUtil.getDifferenceRadians(robotPoseObservation.robotPose().getRotation(), other.robotPose().getRotation()) + && PoseUtil.getDifferenceRadians(pose.getRotation(), other.getRotation()) < WPILibPoseEstimatorConstants.SIMILAR_POSE_ROTATION_TOLERANCE.getRadians(); } From ebf60dbbefcc15611e22e69d40eec8da6a80f873 Mon Sep 17 00:00:00 2001 From: DANNA2828 Date: Sun, 5 Jul 2026 21:40:20 +0300 Subject: [PATCH 6/6] danna - names --- .../WPILibPoseEstimatorConstants.java | 2 +- .../WPILibPoseEstimatorWrapper.java | 19 +++++++++++-------- 2 files changed, 12 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java index d46551dff2..a944ce3e79 100644 --- a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java +++ b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java @@ -16,7 +16,7 @@ public class WPILibPoseEstimatorConstants { public static final StandardDeviations2D VISION_STD_DEV_COLLISION_REDUCTION = new StandardDeviations2D(); - public static final StandardDeviations2D VISION_STD_DEV_IDK_REDUCTION = new StandardDeviations2D(0.1, 0.1, 0.1); + public static final StandardDeviations2D VISION_STD_DEV_SIMILARITY_REDUCTION = new StandardDeviations2D(0.1, 0.1, 0.1); public static final double MINIMUM_COLLISION_IMU_ACCELERATION_G = 2; diff --git a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java index 6b8c48f24b..e50cf11f89 100644 --- a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java +++ b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java @@ -209,12 +209,11 @@ public void log() { } private void updateVision(RobotPoseObservation[] visionRobotPoseObservations) { - List goodRobotPoseObservations = Arrays.stream(visionRobotPoseObservations) + List similarVisionRobotPoseObservations = Arrays.stream(visionRobotPoseObservations) .filter(observation -> Arrays.stream(visionRobotPoseObservations).anyMatch(other -> getAreObservationsSimilar(observation, other))) .toList(); - Arrays.stream(visionRobotPoseObservations) - .forEach(observation -> addVisionMeasurement(observation, goodRobotPoseObservations.contains(observation))); + .forEach(observation -> addVisionMeasurement(observation, similarVisionRobotPoseObservations.contains(observation))); Arrays.stream(visionRobotPoseObservations).forEach(this::updateIMUOffset); @@ -249,11 +248,11 @@ public void resetIsIMUOffsetCalibrated() { isIMUOffsetCalibrated = false; } - private void addVisionMeasurement(RobotPoseObservation visionObservation, boolean isGood) { + private void addVisionMeasurement(RobotPoseObservation visionObservation, boolean isObservationSimilar) { poseEstimator.addVisionMeasurement( visionObservation.robotPose(), visionObservation.timestampSeconds(), - getIDKCompensatedVisionStdDevs(getCollisionCompensatedVisionStdDevs(visionObservation), isGood).asColumnVector() + getSimilarityAccountedVisionStdDevs(getCollisionCompensatedVisionStdDevs(visionObservation), isObservationSimilar).asColumnVector() ); } @@ -274,10 +273,14 @@ private StandardDeviations2D getCollisionCompensatedVisionStdDevs(RobotPoseObser : visionObservation.stdDevs(); } - private StandardDeviations2D getIDKCompensatedVisionStdDevs(StandardDeviations2D visionStandardDeviations, boolean isGood) { - return isGood + private StandardDeviations2D getSimilarityAccountedVisionStdDevs( + StandardDeviations2D visionStandardDeviations, + boolean isObservationSimilar + ) { + return isObservationSimilar ? new StandardDeviations2D( - visionStandardDeviations.asColumnVector().minus(WPILibPoseEstimatorConstants.VISION_STD_DEV_IDK_REDUCTION.asColumnVector()) + visionStandardDeviations.asColumnVector() + .minus(WPILibPoseEstimatorConstants.VISION_STD_DEV_SIMILARITY_REDUCTION.asColumnVector()) ) : visionStandardDeviations; }