better pose during defense -> master

src/main/java/frc/robot/Robot.java

@@ -47,6 +47,7 @@
 import frc.robot.subsystems.swerve.factories.imu.IMUFactory;
 import frc.robot.subsystems.swerve.factories.modules.ModulesFactory;
 import frc.robot.statemachine.shooterstatehandler.TurretCalculations;
+import frc.robot.vision.RobotPoseObservation;
 import frc.utils.GamePeriodUtils;
 import frc.utils.auto.AutonomousChooser;
 import frc.robot.subsystems.swerve.factories.modules.drive.KrakenX60DriveBuilder;
@@ -64,6 +65,7 @@
 import org.littletonrobotics.junction.Logger;
 
 import java.util.List;
+import java.util.Optional;
 import java.util.function.Supplier;
 
 /**
@@ -321,7 +323,12 @@ public void periodic() {
 
 		getLimelights().forEach(Limelight::updateHardwareInputs);
 		getLimelights().forEach(Limelight::updateMT1);
-		getLimelights().forEach(limelight -> limelight.getIndependentRobotPose().ifPresent(poseEstimator::updateVision));
+
+		RobotPoseObservation[] observationsArray = getLimelights().stream()
+			.map(Limelight::getIndependentRobotPose)
+			.flatMap(Optional::stream)
+			.toArray(RobotPoseObservation[]::new);
+		poseEstimator.updateVision(new RobotPoseObservation[][] {observationsArray});
 
 		poseEstimator.log();
 		ShootingCalculations

src/main/java/frc/robot/poseestimator/IVisionEstimator.java

@@ -5,7 +5,7 @@
 
 public interface IVisionEstimator {
 
-	void updateVision(RobotPoseObservation... robotPoseVisionData);
+	void updateVision(RobotPoseObservation[]... robotPoseVisionData);
 
 	Pose2d getEstimatedPose();
 

src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java

@@ -2,6 +2,7 @@
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import frc.constants.MathConstants;
 import frc.utils.math.StandardDeviations2D;
 
 
@@ -15,6 +16,8 @@ public class WPILibPoseEstimatorConstants {
 
 	public static final StandardDeviations2D VISION_STD_DEV_COLLISION_REDUCTION = new StandardDeviations2D();
 
+	public static final StandardDeviations2D VISION_STD_DEV_SIMILARITY_REDUCTION = new StandardDeviations2D(0.1, 0.1, 0.1);
+
 	public static final double MINIMUM_COLLISION_IMU_ACCELERATION_G = 2;
 
 	public static final Rotation2d MINIMUM_TILT_IMU_ROLL = Rotation2d.fromDegrees(4);
@@ -37,4 +40,10 @@ public class WPILibPoseEstimatorConstants {
 
 	public static double IMU_XY_ACCELERATION_G_BUFFER_SIZE_SECONDS = 2;
 
+	public static double SIMILAR_POSE_TRANSLATION_NORM_TOLERANCE_METERS = 1;
+
+	public static Rotation2d SIMILAR_POSE_ROTATION_TOLERANCE = MathConstants.QUARTER_CIRCLE;
+
+	public static double SIMILAR_POSE_TIMESTAMP_TOLERANCE_SECONDS = 0.2;
+
 }

src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java

@@ -1,6 +1,5 @@
 package frc.robot.poseestimator.WPILibPoseEstimator;
 
-import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.estimator.PoseEstimator;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
@@ -12,16 +11,18 @@
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.Odometry;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
-import edu.wpi.first.math.numbers.N1;
-import edu.wpi.first.math.numbers.N3;
 import frc.robot.vision.RobotPoseObservation;
 import frc.robot.poseestimator.IPoseEstimator;
 import frc.robot.poseestimator.OdometryData;
 import frc.utils.buffers.RingBuffer.RingBuffer;
+import frc.utils.math.StandardDeviations2D;
 import frc.utils.math.StatisticsMath;
 import frc.utils.pose.PoseUtil;
 import org.littletonrobotics.junction.Logger;
 
+import java.util.Arrays;
+import java.util.Comparator;
+import java.util.List;
 import java.util.Optional;
 
 public class WPILibPoseEstimatorWrapper implements IPoseEstimator {
@@ -33,8 +34,8 @@ public class WPILibPoseEstimatorWrapper implements IPoseEstimator {
 	private final RingBuffer<Rotation2d> poseToIMUYawDifferenceBuffer;
 	private final TimeInterpolatableBuffer<Rotation2d> imuYawBuffer;
 	private final TimeInterpolatableBuffer<Translation2d> imuXYAccelerationGBuffer;
-	private RobotPoseObservation lastVisionObservation;
 	private OdometryData lastOdometryData;
+	private double lastVisionUpdateTimestamp;
 	private boolean isIMUOffsetCalibrated;
 
 	public WPILibPoseEstimatorWrapper(
@@ -125,8 +126,8 @@ public void updateOdometry(OdometryData data) {
 	}
 
 	@Override
-	public void updateVision(RobotPoseObservation... visionRobotPoseObservations) {
-		for (RobotPoseObservation visionRobotPoseObservation : visionRobotPoseObservations) {
+	public void updateVision(RobotPoseObservation[]... visionRobotPoseObservations) {
+		for (RobotPoseObservation[] visionRobotPoseObservation : visionRobotPoseObservations) {
 			updateVision(visionRobotPoseObservation);
 		}
 	}
@@ -170,8 +171,8 @@ public void log() {
 		Logger.recordOutput(logPath + "/estimatedPose", getEstimatedPose());
 		Logger.recordOutput(logPath + "/odometryPose", getOdometryPose());
 		Logger.recordOutput(logPath + "/predictedOdometryPose", getPredictedOdometryPose());
-		if (lastVisionObservation != null) {
-			Logger.recordOutput(logPath + "/lastVisionUpdate", lastVisionObservation.timestampSeconds());
+		if (lastVisionUpdateTimestamp != 0) {
+			Logger.recordOutput(logPath + "/lastVisionUpdate", lastVisionUpdateTimestamp);
 		}
 		Logger.recordOutput(logPath + "/lastOdometryUpdate", lastOdometryData.getTimestampSeconds());
 		Logger.recordOutput(logPath + "/isIMUOffsetCalibrated", isIMUOffsetCalibrated);
@@ -207,9 +208,22 @@ public void log() {
 		);
 	}
 
-	private void updateVision(RobotPoseObservation visionRobotPoseObservation) {
-		addVisionMeasurement(visionRobotPoseObservation);
+	private void updateVision(RobotPoseObservation[] visionRobotPoseObservations) {
+		List<RobotPoseObservation> similarVisionRobotPoseObservations = Arrays.stream(visionRobotPoseObservations)
+			.filter(observation -> Arrays.stream(visionRobotPoseObservations).anyMatch(other -> getAreObservationsSimilar(observation, other)))
+			.toList();
+		Arrays.stream(visionRobotPoseObservations)
+			.forEach(observation -> addVisionMeasurement(observation, similarVisionRobotPoseObservations.contains(observation)));
 
+		Arrays.stream(visionRobotPoseObservations).forEach(this::updateIMUOffset);
+
+		lastVisionUpdateTimestamp = Arrays.stream(visionRobotPoseObservations)
+			.max(Comparator.comparingDouble(RobotPoseObservation::timestampSeconds))
+			.orElse(new RobotPoseObservation())
+			.timestampSeconds();
+	}
+
+	public void updateIMUOffset(RobotPoseObservation visionRobotPoseObservation) {
 		getEstimatedPoseToIMUYawDifference(
 			imuYawBuffer.getSample(visionRobotPoseObservation.timestampSeconds()),
 			visionRobotPoseObservation.timestampSeconds()
@@ -234,16 +248,15 @@ public void resetIsIMUOffsetCalibrated() {
 		isIMUOffsetCalibrated = false;
 	}
 
-	private void addVisionMeasurement(RobotPoseObservation visionObservation) {
+	private void addVisionMeasurement(RobotPoseObservation visionObservation, boolean isObservationSimilar) {
 		poseEstimator.addVisionMeasurement(
 			visionObservation.robotPose(),
 			visionObservation.timestampSeconds(),
-			getCollisionCompensatedVisionStdDevs(visionObservation)
+			getSimilarityAccountedVisionStdDevs(getCollisionCompensatedVisionStdDevs(visionObservation), isObservationSimilar).asColumnVector()
 		);
-		this.lastVisionObservation = visionObservation;
 	}
 
-	private Matrix<N3, N1> getCollisionCompensatedVisionStdDevs(RobotPoseObservation visionObservation) {
+	private StandardDeviations2D getCollisionCompensatedVisionStdDevs(RobotPoseObservation visionObservation) {
 		boolean isColliding = imuXYAccelerationGBuffer.getSample(visionObservation.timestampSeconds())
 			.map(
 				(imuAccelerationG) -> PoseUtil
@@ -252,10 +265,24 @@ private Matrix<N3, N1> getCollisionCompensatedVisionStdDevs(RobotPoseObservation
 			.orElse(false);
 
 		return isColliding
-			? visionObservation.stdDevs()
-				.asColumnVector()
-				.minus(WPILibPoseEstimatorConstants.VISION_STD_DEV_COLLISION_REDUCTION.asColumnVector())
-			: visionObservation.stdDevs().asColumnVector();
+			? new StandardDeviations2D(
+				visionObservation.stdDevs()
+					.asColumnVector()
+					.minus(WPILibPoseEstimatorConstants.VISION_STD_DEV_COLLISION_REDUCTION.asColumnVector())
+			)
+			: visionObservation.stdDevs();
+	}
+
+	private StandardDeviations2D getSimilarityAccountedVisionStdDevs(
+		StandardDeviations2D visionStandardDeviations,
+		boolean isObservationSimilar
+	) {
+		return isObservationSimilar
+			? new StandardDeviations2D(
+				visionStandardDeviations.asColumnVector()
+					.minus(WPILibPoseEstimatorConstants.VISION_STD_DEV_SIMILARITY_REDUCTION.asColumnVector())
+			)
+			: visionStandardDeviations;
 	}
 
 	private Optional<Rotation2d> getEstimatedPoseToIMUYawDifference(Optional<Rotation2d> gyroYaw, double timestampSeconds) {
@@ -271,4 +298,18 @@ private Pose2d getPredictedOdometryPose() {
 			);
 	}
 
+	private boolean getAreObservationsSimilar(RobotPoseObservation observation, RobotPoseObservation other) {
+		return !observation.equals(other)
+			&& Math.abs(observation.timestampSeconds() - other.timestampSeconds())
+				< WPILibPoseEstimatorConstants.SIMILAR_POSE_TIMESTAMP_TOLERANCE_SECONDS
+			&& getArePosesSimilar(observation.robotPose(), other.robotPose());
+	}
+
+	private boolean getArePosesSimilar(Pose2d pose, Pose2d other) {
+		return PoseUtil.getDifference(pose.getTranslation(), other.getTranslation())
+			< WPILibPoseEstimatorConstants.SIMILAR_POSE_TRANSLATION_NORM_TOLERANCE_METERS
+			&& PoseUtil.getDifferenceRadians(pose.getRotation(), other.getRotation())
+				< WPILibPoseEstimatorConstants.SIMILAR_POSE_ROTATION_TOLERANCE.getRadians();
+	}
+
 }

src/main/java/frc/utils/math/StandardDeviations2D.java

@@ -19,6 +19,10 @@ public StandardDeviations2D(StandardDeviations2D standardDeviations2D) {
 		this(standardDeviations2D.xStandardDeviations, standardDeviations2D.yStandardDeviations, standardDeviations2D.angleStandardDeviations);
 	}
 
+	public StandardDeviations2D(Matrix<N3, N1> columnVector) {
+		this(columnVector.get(0, 0), columnVector.get(1, 0), columnVector.get(2, 0));
+	}
+
 	public Matrix<N3, N1> asColumnVector() {
 		return VecBuilder.fill(xStandardDeviations, yStandardDeviations, angleStandardDeviations);
 	}

src/main/java/frc/utils/pose/PoseUtil.java

@@ -87,6 +87,14 @@ public static double[] rotation3DToRotationArray(Rotation3d rotation3d, AngleUni
 		};
 	}
 
+	public static double getDifference(Translation2d Translation1, Translation2d Translation2) {
+		return Translation1.minus(Translation2).getNorm();
+	}
+
+	public static double getDifferenceRadians(Rotation2d rotation1, Rotation2d rotation2) {
+		return Math.abs(rotation1.minus(rotation2).getRadians());
+	}
+
 	public static boolean getIsColliding(Translation2d imuAccelerationG, double minimumCollisionIMUAccelerationG) {
 		return imuAccelerationG.getNorm() >= minimumCollisionIMUAccelerationG;
 	}