remove 1 + from distance based variance and multi tag varaince

team401 · Dec 12, 2024 · f116cf4 · f116cf4
1 parent 3c8463d
commit f116cf4
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 12 deletions.
diff --git a/vision/src/main/java/coppercore/vision/CoreVisionConstants.java b/vision/src/main/java/coppercore/vision/CoreVisionConstants.java
@@ -14,10 +14,8 @@ public final class CoreVisionConstants {
     // TODO: Tune this value.
     // This value mainly exists so that this file stays named CoreVisionConstants.
     public static final double maxAcceptedDistanceMeters = 10.0;
-    public static final Matrix<N3, N1> singleTagStdDev = VecBuilder.fill(0.08, 0.08, 0.12);
-    public static final Matrix<N3, N1> multiTagStdDev = VecBuilder.fill(0.02, 0.02, 0.06);
-    public static final Matrix<N3, N1> rejectionStdDev =
-            VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE);
+    public static final double linearStdDevFactor = 0.02;
+    public static final double angularStdDevFactor = 0.06;
     public static final double singleTagDistanceCutoff = 4.0;
     public static final double distanceFactor = 1.0;
     public static final double maxZCutoff = 1.0;

diff --git a/vision/src/main/java/coppercore/vision/VisionLocalizer.java b/vision/src/main/java/coppercore/vision/VisionLocalizer.java
@@ -2,6 +2,7 @@
 
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.numbers.N1;
@@ -120,21 +121,18 @@ private boolean shouldRejectPose(VisionIO.PoseObservation observation) {
 
     private Matrix<N3, N1> getLatestVariance(
             VisionIO.PoseObservation observation, int cameraIndex) {
-        Matrix<N3, N1> stdDev = CoreVisionConstants.singleTagStdDev;
         double avgDistanceFromTarget = observation.averageTagDistance();
         int numTags = observation.tagCount();
-        if (numTags > 1) {
-            stdDev = CoreVisionConstants.multiTagStdDev;
-        }
-        // distance based variance
-        stdDev = stdDev.times(1 + (Math.pow(avgDistanceFromTarget, 2) / numTags));
+        double linearStdDev = CoreVisionConstants.linearStdDevFactor * Math.pow(avgDistanceFromTarget, 2) / numTags;
+        double angularStdDev = CoreVisionConstants.angularStdDevFactor * Math.pow(avgDistanceFromTarget, 2) / numTags;
 
         // adjustment based on position of camera
         if (cameraIndex < this.cameraStdDevFactors.length) {
-            stdDev = stdDev.times(this.cameraStdDevFactors[cameraIndex]);
+            linearStdDev *= this.cameraStdDevFactors[cameraIndex];
+            angularStdDev *= this.cameraStdDevFactors[cameraIndex];
         }
 
-        return stdDev;
+        return VecBuilder.fill(linearStdDev, linearStdDev, angularStdDev);
     }
 
     private void logCameraData(