Override single tags to be completely excluded

frc868 · Mar 23, 2024 · 33f73a7 · 33f73a7
1 parent dec007b
commit 33f73a7
Showing 1 changed file with 4 additions and 1 deletion.
diff --git a/src/main/java/com/techhounds/houndutil/houndlib/AprilTagPhotonCamera.java b/src/main/java/com/techhounds/houndutil/houndlib/AprilTagPhotonCamera.java
@@ -132,7 +132,10 @@ public Matrix<N3, N1> getEstimationStdDevs(Pose2d estimatedPose, Matrix<N3, N1>
             avgDist += tagPose.get().toPose2d().getTranslation().getDistance(estimatedPose.getTranslation());
         }
         if (numTags == 0)
-            return estStdDevs;
+            return singleTagStdDevs;
+        if (numTags < 2) {
+            return singleTagStdDevs;
+        }
         avgDist /= numTags;
 
         if (avgDist > 6)