fixed a bug in rotation pose estimator

Shenzhen-Robotics-Alliance · Dec 4, 2024 · 2e8ca46 · 2e8ca46
1 parent 40f5076
commit 2e8ca46
Showing 1 changed file with 18 additions and 19 deletions.
diff --git a/src/main/java/frc/robot/subsystems/vision/apriltags/MapleMultiTagPoseEstimator.java b/src/main/java/frc/robot/subsystems/vision/apriltags/MapleMultiTagPoseEstimator.java
@@ -179,43 +179,42 @@ private Optional<RobotPoseEstimationResult> getEstimationResultFromValidObservat
                 validRobotPoseEstimationsSingleTag.size() > 1 || (!validRobotPoseEstimationsMultiTag.isEmpty());
         if (!resultsCountSufficient) return Optional.empty();
 
-        final List<Statistics.Estimation> robotPoseEstimationsXMeters = new ArrayList<>(),
-                robotPoseEstimationsYMeters = new ArrayList<>(),
-                robotPoseEstimationsThetaRadians = new ArrayList<>();
+        final List<Statistics.Estimation> robotPoseEstimationsXMeters = new ArrayList<>();
+        final List<Statistics.Estimation> 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));
         }
         for (Pose3d robotPoseEstimationMultiTag : validRobotPoseEstimationsMultiTag) {
             robotPoseEstimationsXMeters.add(new Statistics.Estimation(
                     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);
+        final Statistics.Estimation robotPoseFinalEstimationXMeters =
+                Statistics.linearFilter(robotPoseEstimationsXMeters);
+        final Statistics.Estimation 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 double estimationStandardErrorX = robotPoseFinalEstimationXMeters.standardDeviation(),
-                estimationStandardErrorY = robotPoseFinalEstimationYMeters.standardDeviation(),
-                estimationStandardErrorTheta = robotPoseFinalEstimationThetaRadians.standardDeviation();
 
         return Optional.of(new RobotPoseEstimationResult(
-                new Pose2d(translationPointEstimate, rotationPointEstimate),
-                estimationStandardErrorX,
-                estimationStandardErrorY,
-                estimationStandardErrorTheta));
+                new Pose2d(translationPointEstimate, robotPoseFinalEstimationTheta.center()),
+                robotPoseFinalEstimationXMeters.standardDeviation(),
+                robotPoseFinalEstimationYMeters.standardDeviation(),
+                robotPoseFinalEstimationTheta.standardDeviationRad()));
     }
 
     /** Log the filtering data */