fixed merged positions

src/main/java/frc/robot/helpers/MythicalMath.java

@@ -4,6 +4,9 @@
 
 package frc.robot.helpers;
 
+import java.security.PublicKey;
+
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 
 /** Add your docs here. */
@@ -19,5 +22,25 @@ public static double DistanceFromOrigin3d(double XfromOrigin, double YfromOrigin
         double Distance2d = Math.sqrt(Math.pow(YfromOrigin, 2)+Math.pow(XfromOrigin, 2));
         return Math.sqrt(Math.pow(Distance2d, 2)+Math.pow(ZfromOrigin, 2));
     }
+
+    public static Pose2d multiplyOnlyPos(Pose2d pose, Double scalar)
+    {
+        return new Pose2d(pose.getX()*scalar,pose.getY()*scalar,pose.getRotation());
+    }
+
+    public static Pose2d divideOnlyPos(Pose2d pose, Double scalar)
+    {
+        return new Pose2d(pose.getX()/scalar, pose.getY()/scalar,pose.getRotation());
+    }
 
+    /**
+     * 
+     * @param pose1
+     * @param pose2
+     * @return  poses added together, with pose1's rotation
+     */
+    public static Pose2d addOnlyPosTogether(Pose2d pose1, Pose2d pose2)
+    {
+     return new Pose2d(pose1.getX()+pose2.getX(), pose1.getY()+pose2.getY() ,pose1.getRotation());
+    }
 }

src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java

@@ -320,6 +320,7 @@ public void updateOdometryWithVision() {
 				doRejectUpdate = true;
 			}
 			if(!doRejectUpdate) {
+				Logger.recordOutput("Vision/MergesPose", estimate.pose);
 				odometer.addVisionMeasurement(estimate.pose, estimate.timestampSeconds);
 			}
 			RobotState.getInstance().LimelightsUpdated = true;

src/main/java/frc/robot/subsystems/Limelight.java

@@ -126,15 +126,16 @@ public PoseEstimate mergedPose(PoseEstimate pose1, PoseEstimate pose2) {
 		double limelight2y = LimelightHelpers.getTargetPose3d_CameraSpace(APRILTAG_LIMELIGHT3_NAME).getY();
 		double limelight2x = LimelightHelpers.getTargetPose3d_CameraSpace(APRILTAG_LIMELIGHT3_NAME).getX();
 		double limelight2z = LimelightHelpers.getTargetPose3d_CameraSpace(APRILTAG_LIMELIGHT3_NAME).getZ();
-    // use those coordinates to find the distance to the closest apriltag for each limelight
-    double distance1 = MythicalMath.DistanceFromOrigin3d(limelight1y, limelight1x, limelight1z);
-    double distance2 = MythicalMath.DistanceFromOrigin3d(limelight2y, limelight2x, limelight2z);
-    //ccombines the two positions in an epic, proportional way
-    double confidenceSource1 = 1/distance1;
-    double confidenceSource2 = 1/distance2;
-    Pose2d scaledPose1 = pose1.pose.times(confidenceSource1);
-    Pose2d scaledPose2 = pose2.pose.times(confidenceSource2);
-    Pose2d newPose = scaledPose1.plus(new Transform2d(scaledPose2.getTranslation(), new Rotation2d())).div(confidenceSource1+confidenceSource2);
+        // use those coordinates to find the distance to the closest apriltag for each limelight
+        double distance1 = MythicalMath.DistanceFromOrigin3d(limelight1x, limelight1y, limelight1z);
+        double distance2 = MythicalMath.DistanceFromOrigin3d(limelight2x, limelight2y, limelight2z);
+        //ccombines the two positions in an epic, proportional way
+        double confidenceSource1 = Math.pow(1/distance1,2);
+        double confidenceSource2 = Math.pow(1/distance2,2);
+        Pose2d scaledPose1 = MythicalMath.multiplyOnlyPos(pose1.pose, confidenceSource1);
+        Pose2d scaledPose2 = MythicalMath.multiplyOnlyPos(pose2.pose, confidenceSource2);
+    Pose2d newPose = MythicalMath.divideOnlyPos((MythicalMath.addOnlyPosTogether(scaledPose1, scaledPose2)), (confidenceSource1+confidenceSource2));
+    //Pose2d newPose = scaledPose1.plus(new Transform2d(scaledPose2.getTranslation(), new Rotation2d())).div(confidenceSource1+confidenceSource2);
     pose1.pose = newPose;
     return pose1;
     }