fixed a bug in rotational filtering

src/main/java/frc/robot/constants/VisionConstants.java

@@ -16,10 +16,6 @@ public class VisionConstants {
             TRANSLATIONAL_STANDARD_ERROR_METERS_FOR_MULTITAG = 0.3,
             ROTATIONAL_STANDARD_ERROR_RADIANS_FOR_MULTITAG = Math.toRadians(6),
 
-            /* only do odometry calibration if standard error is not greater than */
-            TRANSLATIONAL_STANDARD_DEVS_THRESHOLD_DISCARD_RESULT = 0.4,
-            ROTATIONAL_STANDARD_DEVS_THRESHOLD_DISCARD_RESULT = Math.toRadians(20),
-
             /* standard deviation for odometry and gyros */
             ODOMETRY_TRANSLATIONAL_STANDARD_ERROR_METERS = 0.04,
             GYRO_ROTATIONAL_STANDARD_ERROR_RADIANS = Math.toRadians(0.3);
@@ -37,9 +33,9 @@ public class VisionConstants {
                     720,
                     new Translation2d(
                             0.330, -0.127), // the outing position of the camera in relative to the robot center
-                    0.254, // the mounting height, in meters
+                    0.15, // the mounting height, in meters
                     Rotation2d.fromDegrees(0), // the camera facing, 0 is front, positive is counter-clockwise
-                    30, // camera pitch angle, in degrees
+                    32, // camera pitch angle, in degrees
                     180 // camera roll angle, 0 for up-right and 180 for upside-down
                     ),
             new PhotonCameraProperties(
@@ -53,9 +49,9 @@ public class VisionConstants {
                     1280,
                     720,
                     new Translation2d(0.229, 0.348),
-                    0.2,
+                    0.15,
                     Rotation2d.fromDegrees(30),
-                    30,
+                    35,
                     180 // upside-down
                     ),
             new PhotonCameraProperties(
@@ -69,9 +65,9 @@ public class VisionConstants {
                     1280,
                     720,
                     new Translation2d(0.229, -0.348),
-                    0.2,
+                    0.15,
                     Rotation2d.fromDegrees(-30),
-                    30,
+                    35,
                     180 // upside-down
                     ),
             new PhotonCameraProperties(
@@ -85,7 +81,7 @@ public class VisionConstants {
                     1280,
                     720,
                     new Translation2d(-0.229, 0.330),
-                    0.2,
+                    0.15,
                     Rotation2d.fromDegrees(150),
                     35,
                     180 // upside-down
@@ -101,7 +97,7 @@ public class VisionConstants {
                     1280,
                     720,
                     new Translation2d(-0.229, -0.330),
-                    0.2,
+                    0.15,
                     Rotation2d.fromDegrees(-150),
                     35,
                     180 // upside-down

src/main/java/frc/robot/subsystems/drive/SwerveDrive.java

@@ -23,7 +23,6 @@
 import edu.wpi.first.wpilibj2.command.FunctionalCommand;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.Robot;
-import frc.robot.constants.DriveControlLoops;
 import frc.robot.constants.FieldConstants;
 import frc.robot.subsystems.MapleSubsystem;
 import frc.robot.subsystems.drive.IO.*;
@@ -192,10 +191,8 @@ private void updateRobotFacingWithOdometry(SwerveModulePosition[] modulesDelta)
 
     @Override
     public void runRawChassisSpeeds(ChassisSpeeds speeds) {
-        OptionalDouble angularVelocityOverride = ChassisHeadingController.getInstance()
-                .calculate(
-                        getMeasuredChassisSpeedsFieldRelative(),
-                        getPoseWithLookAhead(0, DriveControlLoops.ROTATIONAL_LOOKAHEAD_TIME));
+        OptionalDouble angularVelocityOverride =
+                ChassisHeadingController.getInstance().calculate(getMeasuredChassisSpeedsFieldRelative(), getPose());
         if (angularVelocityOverride.isPresent()) {
             speeds = new ChassisSpeeds(
                     speeds.vxMetersPerSecond, speeds.vyMetersPerSecond, angularVelocityOverride.getAsDouble());

src/main/java/frc/robot/subsystems/vision/apriltags/AprilTagVision.java

@@ -73,9 +73,13 @@ private Pose2d displayVisionPointEstimateResult(Optional<RobotPoseEstimationResu
 
     private double getResultsTimeStamp() {
         if (inputs.camerasInputs.length == 0) return Timer.getTimestamp();
-        double totalTimeStampSeconds = 0;
-        for (AprilTagVisionIO.CameraInputs cameraInputs : inputs.camerasInputs)
-            totalTimeStampSeconds += cameraInputs.timeStampSeconds;
-        return totalTimeStampSeconds / inputs.camerasInputs.length;
+        double totalTimeStampSeconds = 0, camerasUsed = 0;
+        for (AprilTagVisionIO.CameraInputs cameraInputs : inputs.camerasInputs) {
+            if (cameraInputs.newPipeLineResultAvailable) {
+                totalTimeStampSeconds += cameraInputs.timeStampSeconds;
+                camerasUsed++;
+            }
+        }
+        return totalTimeStampSeconds / camerasUsed;
     }
 }

src/main/java/frc/robot/subsystems/vision/apriltags/AprilTagVisionIO.java

@@ -1,6 +1,5 @@
 package frc.robot.subsystems.vision.apriltags;
 
-import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.wpilibj.DriverStation;
 import java.util.Arrays;
@@ -13,6 +12,7 @@ public interface AprilTagVisionIO {
     class CameraInputs {
         public static final int MAX_TARGET_PER_CAMERA = 5;
         public boolean cameraConnected;
+        public boolean newPipeLineResultAvailable;
         public double timeStampSeconds;
         public int currentTargetsCount;
         public final int[] fiducialMarksID;
@@ -26,6 +26,7 @@ public CameraInputs() {
         }
 
         public void clear(boolean cameraConnected) {
+            this.newPipeLineResultAvailable = false;
             this.cameraConnected = cameraConnected;
             this.timeStampSeconds = 0;
             this.currentTargetsCount = 0;
@@ -34,6 +35,7 @@ public void clear(boolean cameraConnected) {
         }
 
         public void fromPhotonPipeLine(PhotonPipelineResult pipelineResult) {
+            this.newPipeLineResultAvailable = true;
             this.cameraConnected = true;
             this.timeStampSeconds = pipelineResult.getTimestampSeconds();
             this.currentTargetsCount = Math.min(pipelineResult.getTargets().size(), MAX_TARGET_PER_CAMERA);
@@ -48,11 +50,10 @@ public void fromPhotonPipeLine(PhotonPipelineResult pipelineResult) {
                     : Optional.empty();
         }
 
-        private static final Transform3d NULL_TRANSFORM = new Transform3d(-114514, -114514, -114514, new Rotation3d());
-
         public void fromLog(LogTable table, int cameraID) {
             final String cameraKey = "Camera" + cameraID;
             this.cameraConnected = table.get(cameraKey + "Connected", false);
+            this.newPipeLineResultAvailable = table.get(cameraKey + "NewPipeLineResultAvailable", false);
             this.timeStampSeconds = table.get(cameraKey + "TimeStampSeconds", 0.0);
             this.currentTargetsCount = table.get(cameraKey + "CurrentTargetsCount", 0);
             final int[] fiducialMarkIDLogged = table.get(cameraKey + "FiducialMarksID", new int[MAX_TARGET_PER_CAMERA]);
@@ -74,6 +75,7 @@ public void fromLog(LogTable table, int cameraID) {
         public void writeToLog(LogTable table, int cameraID) {
             final String cameraKey = "Camera" + cameraID;
             table.put(cameraKey + "Connected", cameraConnected);
+            table.put(cameraKey + "NewPipeLineResultAvailable", this.newPipeLineResultAvailable);
             table.put(cameraKey + "TimeStampSeconds", timeStampSeconds);
             table.put(cameraKey + "CurrentTargetsCount", currentTargetsCount);
             table.put(cameraKey + "FiducialMarksID", fiducialMarksID);

src/main/java/frc/robot/subsystems/vision/apriltags/MapleMultiTagPoseEstimator.java

@@ -76,13 +76,15 @@ private void fetchRobotPose3dEstimationsFromCameraInputs(
                     + camerasProperties.size());
 
         for (int i = 0; i < cameraInputs.length; i++)
-            fetchSingleCameraInputs(cameraInputs[i], camerasProperties.get(i), currentOdometryPose);
+            if (cameraInputs[i].newPipeLineResultAvailable)
+                fetchSingleCameraInputs(cameraInputs[i], camerasProperties.get(i), currentOdometryPose);
     }
 
     private void fetchSingleCameraInputs(
             AprilTagVisionIO.CameraInputs cameraInput,
             PhotonCameraProperties cameraProperty,
             Pose2d currentOdometryPose) {
+
         calculateVisibleTagsPosesForLog(cameraInput, cameraProperty, currentOdometryPose);
 
         /* if there is multi-solvepnp result, we only trust that */
@@ -181,16 +183,16 @@ private Optional<RobotPoseEstimationResult> getEstimationResultFromValidObservat
         if (!resultsCountSufficient) return Optional.empty();
 
         final List<Statistics.Estimation> robotPoseEstimationsXMeters = new ArrayList<>(),
-                robotPoseEstimationsYMeters = new ArrayList<>(),
-                robotPoseEstimationsThetaRadians = new ArrayList<>();
+                robotPoseEstimationsYMeters = new ArrayList<>();
+        final List<Statistics.RotationEstimation> robotPoseEstimationsTheta = new ArrayList<>();
 
         for (Pose3d robotPoseEstimationSingleTag : validRobotPoseEstimationsSingleTag) {
             robotPoseEstimationsXMeters.add(new Statistics.Estimation(
                     robotPoseEstimationSingleTag.getX(), TRANSLATIONAL_STANDARD_ERROR_METERS_FOR_SINGLE_OBSERVATION));
             robotPoseEstimationsYMeters.add(new Statistics.Estimation(
                     robotPoseEstimationSingleTag.getY(), TRANSLATIONAL_STANDARD_ERROR_METERS_FOR_SINGLE_OBSERVATION));
-            robotPoseEstimationsThetaRadians.add(new Statistics.Estimation(
-                    robotPoseEstimationSingleTag.getRotation().getZ(),
+            robotPoseEstimationsTheta.add(new Statistics.RotationEstimation(
+                    robotPoseEstimationSingleTag.getRotation().toRotation2d(),
                     ROTATIONAL_STANDARD_ERROR_RADIANS_FOR_SINGLE_OBSERVATION));
         }
 
@@ -199,42 +201,35 @@ private Optional<RobotPoseEstimationResult> getEstimationResultFromValidObservat
                     robotPoseEstimationMultiTag.getX(), TRANSLATIONAL_STANDARD_ERROR_METERS_FOR_MULTITAG));
             robotPoseEstimationsYMeters.add(new Statistics.Estimation(
                     robotPoseEstimationMultiTag.getY(), TRANSLATIONAL_STANDARD_ERROR_METERS_FOR_MULTITAG));
-            robotPoseEstimationsThetaRadians.add(new Statistics.Estimation(
-                    robotPoseEstimationMultiTag.getRotation().getZ(), ROTATIONAL_STANDARD_ERROR_RADIANS_FOR_MULTITAG));
+            robotPoseEstimationsTheta.add(new Statistics.RotationEstimation(
+                    robotPoseEstimationMultiTag.getRotation().toRotation2d(),
+                    ROTATIONAL_STANDARD_ERROR_RADIANS_FOR_MULTITAG));
         }
 
         final Statistics.Estimation
                 robotPoseFinalEstimationXMeters = Statistics.linearFilter(robotPoseEstimationsXMeters),
-                robotPoseFinalEstimationYMeters = Statistics.linearFilter(robotPoseEstimationsYMeters),
-                robotPoseFinalEstimationThetaRadians = Statistics.linearFilter(robotPoseEstimationsThetaRadians);
+                robotPoseFinalEstimationYMeters = Statistics.linearFilter(robotPoseEstimationsYMeters);
+        final Statistics.RotationEstimation robotPoseFinalEstimationTheta =
+                Statistics.rotationFilter(robotPoseEstimationsTheta);
 
         final Translation2d translationPointEstimate =
                 new Translation2d(robotPoseFinalEstimationXMeters.center(), robotPoseFinalEstimationYMeters.center());
-        final Rotation2d rotationPointEstimate = Rotation2d.fromRadians(robotPoseFinalEstimationThetaRadians.center());
+        final Rotation2d rotationPointEstimate = robotPoseFinalEstimationTheta.center();
 
-        final double estimationStandardErrorX = robotPoseFinalEstimationXMeters.standardDeviation(),
-                estimationStandardErrorY = robotPoseFinalEstimationYMeters.standardDeviation(),
-                estimationStandardErrorTheta = robotPoseFinalEstimationThetaRadians.standardDeviation();
+        final double estimationStandardDevsX = robotPoseFinalEstimationXMeters.standardDeviation(),
+                estimationStandardDevsY = robotPoseFinalEstimationYMeters.standardDeviation(),
+                estimationStandardErrorTheta = robotPoseFinalEstimationTheta.standardDeviationRad();
 
         Logger.recordOutput(
-                "Vision/MeasurementErrors/translationalStandardError",
-                Math.hypot(estimationStandardErrorX, estimationStandardErrorY));
+                "Vision/MeasurementErrors/translationalStandardDevs",
+                Math.hypot(estimationStandardDevsX, estimationStandardDevsY));
         Logger.recordOutput(
-                "Vision/MeasurementErrors/rotationalStandardError", Math.toDegrees(estimationStandardErrorTheta));
-
-        final double translationStdDev = Math.hypot(
-                Statistics.getStandardDeviation(robotPoseEstimationsXMeters),
-                Statistics.getStandardDeviation(robotPoseEstimationsYMeters));
-        final double rotationStdDev = Statistics.getStandardDeviation(robotPoseEstimationsThetaRadians);
-        Logger.recordOutput("Vision/MeasurementErrors/translationalStdDev", translationStdDev);
-        Logger.recordOutput("Vision/MeasurementErrors/rotationalStdDev", Math.toDegrees(rotationStdDev));
-        if (translationStdDev > TRANSLATIONAL_STANDARD_DEVS_THRESHOLD_DISCARD_RESULT
-                || rotationStdDev > ROTATIONAL_STANDARD_DEVS_THRESHOLD_DISCARD_RESULT) return Optional.empty();
+                "Vision/MeasurementErrors/rotationalStandardDevs", Math.toDegrees(estimationStandardErrorTheta));
 
         return Optional.of(new RobotPoseEstimationResult(
                 new Pose2d(translationPointEstimate, rotationPointEstimate),
-                estimationStandardErrorX,
-                estimationStandardErrorY,
+                estimationStandardDevsX,
+                estimationStandardDevsY,
                 estimationStandardErrorTheta));
     }
 

src/main/java/frc/robot/utils/CustomMaths/Statistics.java

@@ -1,7 +1,9 @@
 package frc.robot.utils.CustomMaths;
 
+import edu.wpi.first.math.geometry.Rotation2d;
 import java.util.Arrays;
 import java.util.List;
+import java.util.stream.Collectors;
 
 public final class Statistics {
     public static double getMean(double[] dataSet) {
@@ -51,14 +53,36 @@ public static double getBestFitLineIntersect(double[] dataSetX, double[] dataSet
         final double slope = getBestFitLineSlope(dataSetX, dataSetY);
         return getMean(dataSetY) - slope * getMean(dataSetX);
     }
-
+    /**
+     *
+     *
+     * <h2>Stores a numerical estimation of something known to be coming from a normal distribution.</h2>
+     *
+     * @param center the center of the estimation
+     * @param standardDeviation the standard deviation of the estimation
+     */
     public record Estimation(double center, double standardDeviation) {}
 
+    /**
+     *
+     *
+     * <h2>Apply a linear filter to find the best estimation over a list of estimations.</h2>
+     *
+     * @see #linearFilter(Estimation...)
+     */
     public static Estimation linearFilter(List<Estimation> estimations) {
-        return linearFilter(estimations.toArray(Estimation[]::new));
+        return linearFilter(estimations.toArray(new Estimation[0]));
     }
 
-    /** Given a set of estimations towards a value, calculates the overall estimation */
+    /**
+     *
+     *
+     * <h2>Apply a linear filter to find the best estimation over a few estimations.</h2>
+     *
+     * <p>The estimations with less standard deviation get trusted more.
+     *
+     * @return the overall best estimation according to all the given estimations
+     */
     public static Estimation linearFilter(Estimation... estimations) {
         if (estimations == null || estimations.length == 0)
             throw new IllegalArgumentException("At least one estimation is required.");
@@ -69,9 +93,9 @@ public static Estimation linearFilter(Estimation... estimations) {
         double sumWeights = 0.0;
 
         for (Estimation estimation : estimations) {
-            double variance = estimation.standardDeviation * estimation.standardDeviation;
+            double variance = estimation.standardDeviation() * estimation.standardDeviation();
             double weight = 1.0 / variance;
-            sumWeightedCenters += weight * estimation.center;
+            sumWeightedCenters += weight * estimation.center();
             sumWeights += weight;
         }
 
@@ -81,13 +105,75 @@ public static Estimation linearFilter(Estimation... estimations) {
         return new Estimation(combinedCenter, combinedStandardDeviation);
     }
 
-    public static double getStandardDeviation(List<Estimation> estimations) {
-        return getStandardDeviation(estimations.toArray(Estimation[]::new));
+    /**
+     *
+     *
+     * <h2>Stores a rotational estimation of something known to be coming from a normal distribution.</h2>
+     *
+     * @param center the center of the estimation
+     * @param standardDeviationRad the standard deviation of the estimation, in radians
+     */
+    public record RotationEstimation(Rotation2d center, double standardDeviationRad) {}
+
+    /**
+     *
+     *
+     * <h2>Apply a rotational filter to find the best estimation over a list of rotational estimations.</h2>
+     *
+     * @see #rotationFilter(RotationEstimation...)
+     * @return the overall best rotational estimation according to all the given estimations
+     */
+    public static RotationEstimation rotationFilter(List<RotationEstimation> rotationEstimations) {
+        return rotationFilter(rotationEstimations.toArray(new RotationEstimation[0]));
     }
 
-    public static double getStandardDeviation(Estimation... estimations) {
-        return getStandardDeviation(Arrays.stream(estimations)
-                .mapToDouble(estimation -> estimation.center)
-                .toArray());
+    /**
+     *
+     *
+     * <h2>Apply a rotational filter to find the best estimation over a few rotational estimations.</h2>
+     *
+     * <p>The estimations with less standard deviation get trusted more.
+     *
+     * @return the overall best rotational estimation according to all the given estimations
+     */
+    public static RotationEstimation rotationFilter(RotationEstimation... rotationEstimations) {
+        if (rotationEstimations == null || rotationEstimations.length == 0)
+            throw new IllegalArgumentException("At least one rotation estimation is required.");
+
+        if (rotationEstimations.length == 1) return rotationEstimations[0];
+
+        // Transform the rotation estimations into Estimation objects for cosines
+        List<Estimation> cosEstimations = Arrays.stream(rotationEstimations)
+                .map(est -> new Estimation(Math.cos(est.center().getRadians()), est.standardDeviationRad()))
+                .collect(Collectors.toList());
+
+        // Apply linearFilter to compute the weighted mean of cosines
+        Estimation cosEstimation = linearFilter(cosEstimations);
+
+        // Transform the rotation estimations into Estimation objects for sines
+        List<Estimation> sinEstimations = Arrays.stream(rotationEstimations)
+                .map(est -> new Estimation(Math.sin(est.center().getRadians()), est.standardDeviationRad()))
+                .collect(Collectors.toList());
+
+        // Apply linearFilter to compute the weighted mean of sines
+        Estimation sinEstimation = linearFilter(sinEstimations);
+
+        // Compute the resultant length R
+        double R = Math.sqrt(Math.pow(cosEstimation.center(), 2) + Math.pow(sinEstimation.center(), 2));
+
+        // Compute the circular standard deviation
+        double circularStdDev;
+        if (R > 0) circularStdDev = Math.sqrt(-2.0 * Math.log(R));
+        else
+            // If R is 0, the angles are uniformly distributed; standard deviation is maximal
+            circularStdDev = Math.PI;
+
+        // Compute the mean angle using the weighted means of cosines and sines
+        double meanTheta = Math.atan2(sinEstimation.center(), cosEstimation.center());
+
+        // Normalize the angle to be within [-pi, pi]
+        meanTheta = Rotation2d.fromRadians(meanTheta).getRadians();
+
+        return new RotationEstimation(new Rotation2d(meanTheta), circularStdDev);
     }
 }