From 550a6aaec492e669ecd5b89d3fb9767a69bb8385 Mon Sep 17 00:00:00 2001 From: DanielleCarmi09 Date: Thu, 12 Feb 2026 23:05:09 +0200 Subject: [PATCH 1/8] angle std fix --- .../WPILibPoseEstimatorConstants.java | 4 ++-- .../WPILibPoseEstimatorWrapper.java | 13 ++++++++++++- 2 files changed, 14 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java index 8909c4a2c1..9a86475270 100644 --- a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java +++ b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java @@ -13,8 +13,6 @@ public class WPILibPoseEstimatorConstants { public static final StandardDeviations2D DEFAULT_VISION_STD_DEV = new StandardDeviations2D(0.0003, 0.0003, 0.003); - public static final StandardDeviations2D VISION_STD_DEV_COLLISION_REDUCTION = new StandardDeviations2D(); - public static final double MINIMUM_COLLISION_IMU_ACCELERATION_G = 2; public static final Rotation2d MINIMUM_TILT_IMU_ROLL = Rotation2d.fromDegrees(4); @@ -29,6 +27,8 @@ public class WPILibPoseEstimatorConstants { public static final double SKID_ODOMETRY_ACCURACY_REDUCTION_FACTOR = 0; + public static final double IMU_CALIBRATION_VISION_STD_DEVS_ADDITION_FACTOR = 0; + /** * constant represents the addition to odometryAccuracy when the compensated vision X and Y StdDevs average is equal to one */ diff --git a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java index 56f03ed0ee..c6cd171fd8 100644 --- a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java +++ b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java @@ -247,11 +247,22 @@ private void addVisionMeasurement(RobotPoseObservation visionObservation) { poseEstimator.addVisionMeasurement( visionObservation.robotPose(), visionObservation.timestampSeconds(), - compensateByOdometryAccuracy(visionObservation.stdDevs()).asColumnVector() + compensateByIsIMUCalibrated(visionObservation.stdDevs()).asColumnVector() ); this.lastVisionObservation = visionObservation; } + private StandardDeviations2D compensateByIsIMUCalibrated(StandardDeviations2D visionStdDevs) { + return new StandardDeviations2D( + compensateByOdometryAccuracy(visionStdDevs).xStandardDeviations(), + compensateByOdometryAccuracy(visionStdDevs).yStandardDeviations(), + isIMUOffsetCalibrated + ? compensateByOdometryAccuracy(visionStdDevs).angleStandardDeviations() + * WPILibPoseEstimatorConstants.IMU_CALIBRATION_VISION_STD_DEVS_ADDITION_FACTOR + : compensateByOdometryAccuracy(visionStdDevs).angleStandardDeviations() + ); + } + private StandardDeviations2D compensateByOdometryAccuracy(StandardDeviations2D visionStdDevs) { StandardDeviations2D compensatedStdDevs = new StandardDeviations2D( visionStdDevs.xStandardDeviations() * odometryAccuracy, From d8e7cc22dbdd82a9ad2b2bf465d82949b0a794fa Mon Sep 17 00:00:00 2001 From: DanielleCarmi09 Date: Thu, 12 Feb 2026 23:16:15 +0200 Subject: [PATCH 2/8] CR --- .../WPILibPoseEstimatorConstants.java | 2 +- .../WPILibPoseEstimatorWrapper.java | 13 ++++++------- 2 files changed, 7 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java index 9a86475270..5e10a0f652 100644 --- a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java +++ b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java @@ -27,7 +27,7 @@ public class WPILibPoseEstimatorConstants { public static final double SKID_ODOMETRY_ACCURACY_REDUCTION_FACTOR = 0; - public static final double IMU_CALIBRATION_VISION_STD_DEVS_ADDITION_FACTOR = 0; + public static final double CALIBRATED_IMU_OFFSET_VISION_STD_DEVS_ADDITION = 0; /** * constant represents the addition to odometryAccuracy when the compensated vision X and Y StdDevs average is equal to one diff --git a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java index c6cd171fd8..d268802315 100644 --- a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java +++ b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java @@ -247,19 +247,18 @@ private void addVisionMeasurement(RobotPoseObservation visionObservation) { poseEstimator.addVisionMeasurement( visionObservation.robotPose(), visionObservation.timestampSeconds(), - compensateByIsIMUCalibrated(visionObservation.stdDevs()).asColumnVector() + compensateByIsIMUOffsetCalibrated(compensateByOdometryAccuracy(visionObservation.stdDevs())).asColumnVector() ); this.lastVisionObservation = visionObservation; } - private StandardDeviations2D compensateByIsIMUCalibrated(StandardDeviations2D visionStdDevs) { + private StandardDeviations2D compensateByIsIMUOffsetCalibrated(StandardDeviations2D visionStdDevs) { return new StandardDeviations2D( - compensateByOdometryAccuracy(visionStdDevs).xStandardDeviations(), - compensateByOdometryAccuracy(visionStdDevs).yStandardDeviations(), + visionStdDevs.xStandardDeviations(), + visionStdDevs.yStandardDeviations(), isIMUOffsetCalibrated - ? compensateByOdometryAccuracy(visionStdDevs).angleStandardDeviations() - * WPILibPoseEstimatorConstants.IMU_CALIBRATION_VISION_STD_DEVS_ADDITION_FACTOR - : compensateByOdometryAccuracy(visionStdDevs).angleStandardDeviations() + ? visionStdDevs.angleStandardDeviations() * WPILibPoseEstimatorConstants.CALIBRATED_IMU_OFFSET_VISION_STD_DEVS_ADDITION + : visionStdDevs.angleStandardDeviations() ); } From b79a0b669a4c283df7186f62b7d2cd4b5e188f51 Mon Sep 17 00:00:00 2001 From: DanielleCarmi09 Date: Fri, 13 Feb 2026 00:06:43 +0200 Subject: [PATCH 3/8] test --- .../WPILibPoseEstimatorWrapper.java | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java index d268802315..14055cc6e9 100644 --- a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java +++ b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java @@ -244,11 +244,17 @@ private void updateVision(RobotPoseObservation visionRobotPoseObservation) { } private void addVisionMeasurement(RobotPoseObservation visionObservation) { - poseEstimator.addVisionMeasurement( - visionObservation.robotPose(), + RobotPoseObservation visionMeasurment = new RobotPoseObservation( visionObservation.timestampSeconds(), - compensateByIsIMUOffsetCalibrated(compensateByOdometryAccuracy(visionObservation.stdDevs())).asColumnVector() + visionObservation.robotPose(), + compensateByIsIMUOffsetCalibrated(compensateByOdometryAccuracy(visionObservation.stdDevs())) + ); + poseEstimator.addVisionMeasurement( + visionMeasurment.robotPose(), + visionMeasurment.timestampSeconds(), + visionMeasurment.stdDevs().asColumnVector() ); + Logger.recordOutput(logPath + "/visionMeasurement", visionMeasurment.stdDevs()); this.lastVisionObservation = visionObservation; } @@ -257,7 +263,7 @@ private StandardDeviations2D compensateByIsIMUOffsetCalibrated(StandardDeviation visionStdDevs.xStandardDeviations(), visionStdDevs.yStandardDeviations(), isIMUOffsetCalibrated - ? visionStdDevs.angleStandardDeviations() * WPILibPoseEstimatorConstants.CALIBRATED_IMU_OFFSET_VISION_STD_DEVS_ADDITION + ? visionStdDevs.angleStandardDeviations() + WPILibPoseEstimatorConstants.CALIBRATED_IMU_OFFSET_VISION_STD_DEVS_ADDITION : visionStdDevs.angleStandardDeviations() ); } From 0c7b0d400e77bf45e29dddbe136090dafb3272ea Mon Sep 17 00:00:00 2001 From: DanielleCarmi09 Date: Fri, 13 Feb 2026 09:49:46 +0200 Subject: [PATCH 4/8] CR --- .../WPILibPoseEstimatorWrapper.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java index 14055cc6e9..18e2b657ef 100644 --- a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java +++ b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java @@ -244,17 +244,17 @@ private void updateVision(RobotPoseObservation visionRobotPoseObservation) { } private void addVisionMeasurement(RobotPoseObservation visionObservation) { - RobotPoseObservation visionMeasurment = new RobotPoseObservation( + RobotPoseObservation compensatedVisionMeasurement = new RobotPoseObservation( visionObservation.timestampSeconds(), visionObservation.robotPose(), compensateByIsIMUOffsetCalibrated(compensateByOdometryAccuracy(visionObservation.stdDevs())) ); poseEstimator.addVisionMeasurement( - visionMeasurment.robotPose(), - visionMeasurment.timestampSeconds(), - visionMeasurment.stdDevs().asColumnVector() + compensatedVisionMeasurement.robotPose(), + compensatedVisionMeasurement.timestampSeconds(), + compensatedVisionMeasurement.stdDevs().asColumnVector() ); - Logger.recordOutput(logPath + "/visionMeasurement", visionMeasurment.stdDevs()); + Logger.recordOutput(logPath + "/compensatedVisionMeasurement", compensatedVisionMeasurement.stdDevs()); this.lastVisionObservation = visionObservation; } From 1a198fdb8ffc95f218d1701fa9711d7925d37ae4 Mon Sep 17 00:00:00 2001 From: DanielleCarmi09 Date: Fri, 13 Feb 2026 13:00:46 +0200 Subject: [PATCH 5/8] CR --- .../WPILibPoseEstimatorConstants.java | 2 +- .../WPILibPoseEstimatorWrapper.java | 18 +++++++++++------- 2 files changed, 12 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java index 5e10a0f652..0f67e7733d 100644 --- a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java +++ b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java @@ -27,7 +27,7 @@ public class WPILibPoseEstimatorConstants { public static final double SKID_ODOMETRY_ACCURACY_REDUCTION_FACTOR = 0; - public static final double CALIBRATED_IMU_OFFSET_VISION_STD_DEVS_ADDITION = 0; + public static final double CALIBRATED_IMU_OFFSET_VISION_ANGLE_STD_DEVS_ADDITION = 0; /** * constant represents the addition to odometryAccuracy when the compensated vision X and Y StdDevs average is equal to one diff --git a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java index 18e2b657ef..96d01481ff 100644 --- a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java +++ b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java @@ -244,26 +244,30 @@ private void updateVision(RobotPoseObservation visionRobotPoseObservation) { } private void addVisionMeasurement(RobotPoseObservation visionObservation) { - RobotPoseObservation compensatedVisionMeasurement = new RobotPoseObservation( + RobotPoseObservation compensatedVisionObservation = new RobotPoseObservation( visionObservation.timestampSeconds(), visionObservation.robotPose(), - compensateByIsIMUOffsetCalibrated(compensateByOdometryAccuracy(visionObservation.stdDevs())) + compensateByPoseEstimatorConditions(visionObservation) ); poseEstimator.addVisionMeasurement( - compensatedVisionMeasurement.robotPose(), - compensatedVisionMeasurement.timestampSeconds(), - compensatedVisionMeasurement.stdDevs().asColumnVector() + compensatedVisionObservation.robotPose(), + compensatedVisionObservation.timestampSeconds(), + compensatedVisionObservation.stdDevs().asColumnVector() ); - Logger.recordOutput(logPath + "/compensatedVisionMeasurement", compensatedVisionMeasurement.stdDevs()); + Logger.recordOutput(logPath + "/lastCompensatedVisionObservationStdDevs", compensatedVisionObservation.stdDevs()); this.lastVisionObservation = visionObservation; } + private StandardDeviations2D compensateByPoseEstimatorConditions(RobotPoseObservation visionObservation) { + return compensateByIsIMUOffsetCalibrated(compensateByOdometryAccuracy(visionObservation.stdDevs())); + } + private StandardDeviations2D compensateByIsIMUOffsetCalibrated(StandardDeviations2D visionStdDevs) { return new StandardDeviations2D( visionStdDevs.xStandardDeviations(), visionStdDevs.yStandardDeviations(), isIMUOffsetCalibrated - ? visionStdDevs.angleStandardDeviations() + WPILibPoseEstimatorConstants.CALIBRATED_IMU_OFFSET_VISION_STD_DEVS_ADDITION + ? visionStdDevs.angleStandardDeviations() + WPILibPoseEstimatorConstants.CALIBRATED_IMU_OFFSET_VISION_ANGLE_STD_DEVS_ADDITION : visionStdDevs.angleStandardDeviations() ); } From 0e7a31c452f399983f323514e0cd6b0f7a9ca4a8 Mon Sep 17 00:00:00 2001 From: DanielleCarmi09 Date: Fri, 13 Feb 2026 13:07:01 +0200 Subject: [PATCH 6/8] CR --- .../WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java index 96d01481ff..c22e85ed32 100644 --- a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java +++ b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java @@ -247,7 +247,7 @@ private void addVisionMeasurement(RobotPoseObservation visionObservation) { RobotPoseObservation compensatedVisionObservation = new RobotPoseObservation( visionObservation.timestampSeconds(), visionObservation.robotPose(), - compensateByPoseEstimatorConditions(visionObservation) + compensateByPoseEstimatorConditions(visionObservation.stdDevs()) ); poseEstimator.addVisionMeasurement( compensatedVisionObservation.robotPose(), @@ -258,8 +258,8 @@ private void addVisionMeasurement(RobotPoseObservation visionObservation) { this.lastVisionObservation = visionObservation; } - private StandardDeviations2D compensateByPoseEstimatorConditions(RobotPoseObservation visionObservation) { - return compensateByIsIMUOffsetCalibrated(compensateByOdometryAccuracy(visionObservation.stdDevs())); + private StandardDeviations2D compensateByPoseEstimatorConditions(StandardDeviations2D visionStdDevs) { + return compensateByIsIMUOffsetCalibrated(compensateByOdometryAccuracy(visionStdDevs)); } private StandardDeviations2D compensateByIsIMUOffsetCalibrated(StandardDeviations2D visionStdDevs) { From 7864b78b33b8d7dc9925d4d5b6ccbae46e662f94 Mon Sep 17 00:00:00 2001 From: DanielleCarmi09 Date: Fri, 13 Feb 2026 13:36:11 +0200 Subject: [PATCH 7/8] CR --- .../WPILibPoseEstimatorWrapper.java | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java index c22e85ed32..a32fdd020c 100644 --- a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java +++ b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java @@ -229,7 +229,13 @@ private void updateOdometryAccuracy(OdometryData data) { } private void updateVision(RobotPoseObservation visionRobotPoseObservation) { - addVisionMeasurement(visionRobotPoseObservation); + RobotPoseObservation compensatedVisionObservation = new RobotPoseObservation( + visionRobotPoseObservation.timestampSeconds(), + visionRobotPoseObservation.robotPose(), + compensateByPoseEstimatorConditions(visionRobotPoseObservation.stdDevs()) + ); + Logger.recordOutput(logPath + "/lastCompensatedVisionObservationStdDevs", compensatedVisionObservation.stdDevs()); + addVisionMeasurement(compensatedVisionObservation); getEstimatedPoseToIMUYawDifference( imuYawBuffer.getSample(visionRobotPoseObservation.timestampSeconds()), @@ -244,17 +250,11 @@ private void updateVision(RobotPoseObservation visionRobotPoseObservation) { } private void addVisionMeasurement(RobotPoseObservation visionObservation) { - RobotPoseObservation compensatedVisionObservation = new RobotPoseObservation( - visionObservation.timestampSeconds(), - visionObservation.robotPose(), - compensateByPoseEstimatorConditions(visionObservation.stdDevs()) - ); poseEstimator.addVisionMeasurement( - compensatedVisionObservation.robotPose(), - compensatedVisionObservation.timestampSeconds(), - compensatedVisionObservation.stdDevs().asColumnVector() + visionObservation.robotPose(), + visionObservation.timestampSeconds(), + visionObservation.stdDevs().asColumnVector() ); - Logger.recordOutput(logPath + "/lastCompensatedVisionObservationStdDevs", compensatedVisionObservation.stdDevs()); this.lastVisionObservation = visionObservation; } From 78dd09928cb82deb7e1438cfedf548f9579d1cec Mon Sep 17 00:00:00 2001 From: DanielleCarmi09 Date: Fri, 13 Feb 2026 14:12:12 +0200 Subject: [PATCH 8/8] CR --- .../WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java index a32fdd020c..7d190c589d 100644 --- a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java +++ b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java @@ -234,12 +234,11 @@ private void updateVision(RobotPoseObservation visionRobotPoseObservation) { visionRobotPoseObservation.robotPose(), compensateByPoseEstimatorConditions(visionRobotPoseObservation.stdDevs()) ); - Logger.recordOutput(logPath + "/lastCompensatedVisionObservationStdDevs", compensatedVisionObservation.stdDevs()); addVisionMeasurement(compensatedVisionObservation); getEstimatedPoseToIMUYawDifference( - imuYawBuffer.getSample(visionRobotPoseObservation.timestampSeconds()), - visionRobotPoseObservation.timestampSeconds() + imuYawBuffer.getSample(compensatedVisionObservation.timestampSeconds()), + compensatedVisionObservation.timestampSeconds() ).ifPresent(yawDifference -> { poseToIMUYawDifferenceBuffer.insert(yawDifference);