Velocity imu test -> master

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

@@ -7,6 +7,7 @@
 import com.pathplanner.lib.config.ModuleConfig;
 import com.pathplanner.lib.config.RobotConfig;
 import edu.wpi.first.math.geometry.*;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.interpolation.Interpolator;
 import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
 import edu.wpi.first.math.system.plant.DCMotor;
@@ -363,9 +364,13 @@ public void periodic() {
 		limelightRight.getIndependentRobotPose().ifPresent(poseEstimator::updateVision);
 		limelightLeft.getIndependentRobotPose().ifPresent(poseEstimator::updateVision);
 
+		poseEstimator.updateLastEstimatedPose();
 		poseEstimator.log();
+
+		ChassisSpeeds currentEstimatedVelocity = poseEstimator.getFieldRelativeEstimatedVelocity(swerve.getFieldRelativeVelocity());
+
 		ShootingCalculations
-			.updateShootingParams(poseEstimator.getEstimatedPose(), swerve.getFieldRelativeVelocity(), swerve.getIMUAngularVelocityRPS()[2]);
+			.updateShootingParams(poseEstimator.getEstimatedPose(), currentEstimatedVelocity, swerve.getIMUAngularVelocityRPS()[2]);
 
 		Logger.recordOutput("lastBallThrownTimestamp", lastBallThrownTimestamp.get());
 		Logger.recordOutput(

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

@@ -1,6 +1,7 @@
 package frc.robot.poseestimator;
 
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
 
 import java.util.Optional;
 
@@ -17,4 +18,10 @@ public interface IPoseEstimator extends IVisionEstimator, IOdometryEstimator {
 
 	void log();
 
+	ChassisSpeeds getLastEstimatedVelocity();
+
+	void updateLastEstimatedPose();
+
+	ChassisSpeeds getFieldRelativeEstimatedVelocity(ChassisSpeeds swerveVelocity);
+
 }

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 edu.wpi.first.math.kinematics.ChassisSpeeds;
 import frc.utils.math.StandardDeviations2D;
 
 
@@ -35,4 +36,10 @@ public class WPILibPoseEstimatorConstants {
 
 	public static double IMU_XY_ACCELERATION_G_BUFFER_SIZE_SECONDS = 2;
 
+	/**
+	 * Tolerance for detecting swerve wheel slip and smoothing noise. 0.5 m/s matches the skid tolerance threshold. If the difference between raw
+	 * swerve velocity and estimated velocity exceeds this, the wheels are slipping.
+	 */
+	public static final ChassisSpeeds ESTIMATED_VELOCITY_TOLERANCE = new ChassisSpeeds(0.5, 0.5, Math.toRadians(15.0));
+
 }

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

@@ -8,18 +8,18 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Twist2d;
 import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
-import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
-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.kinematics.*;
 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.TimedValue;
 import frc.utils.buffers.RingBuffer.RingBuffer;
 import frc.utils.math.StatisticsMath;
+import frc.utils.math.ToleranceMath;
 import frc.utils.pose.PoseUtil;
+import frc.utils.time.TimeUtil;
 import org.littletonrobotics.junction.Logger;
 
 import java.util.Optional;
@@ -33,7 +33,9 @@ public class WPILibPoseEstimatorWrapper implements IPoseEstimator {
 	private final RingBuffer<Rotation2d> poseToIMUYawDifferenceBuffer;
 	private final TimeInterpolatableBuffer<Rotation2d> imuYawBuffer;
 	private final TimeInterpolatableBuffer<Translation2d> imuXYAccelerationGBuffer;
-	private RobotPoseObservation lastVisionObservation;
+    private final TimedValue<Pose2d> lastEstimatedPose;
+    private final TimedValue<ChassisSpeeds> lastEstimatedVelocity;
+    private RobotPoseObservation lastVisionObservation;
 	private OdometryData lastOdometryData;
 	private boolean isIMUOffsetCalibrated;
 
@@ -72,9 +74,10 @@ public WPILibPoseEstimatorWrapper(
 		this.imuYawBuffer = TimeInterpolatableBuffer.createBuffer(WPILibPoseEstimatorConstants.IMU_YAW_BUFFER_SIZE_SECONDS);
 		this.imuXYAccelerationGBuffer = TimeInterpolatableBuffer
 			.createBuffer(WPILibPoseEstimatorConstants.IMU_XY_ACCELERATION_G_BUFFER_SIZE_SECONDS);
+		this.lastEstimatedPose = new TimedValue<>(new Pose2d(), initialTimestampSeconds);
+		this.lastEstimatedVelocity = new TimedValue<>(new ChassisSpeeds(), initialTimestampSeconds);
 	}
 
-
 	@Override
 	public Pose2d getEstimatedPose() {
 		return poseEstimator.getEstimatedPosition();
@@ -131,6 +134,38 @@ public void updateVision(RobotPoseObservation... visionRobotPoseObservations) {
 		}
 	}
 
+	@Override
+	public void updateLastEstimatedPose() {
+		double timestamp = TimeUtil.getCurrentTimeSeconds();
+		double deltaTime = timestamp - lastEstimatedVelocity.getTimestamp();
+		if (deltaTime > 0) {
+			Pose2d currentPose = getEstimatedPose();
+			ChassisSpeeds rawVelocity = new ChassisSpeeds(
+				(currentPose.getX() - lastEstimatedPose.getValue().getX()) / deltaTime,
+				(currentPose.getY() - lastEstimatedPose.getValue().getY()) / deltaTime,
+				(currentPose.getRotation().getRadians() - lastEstimatedPose.getValue().getRotation().getRadians()) / deltaTime
+			);
+
+			if (ToleranceMath.isNear(new ChassisSpeeds(), rawVelocity, WPILibPoseEstimatorConstants.ESTIMATED_VELOCITY_TOLERANCE)) {
+				lastEstimatedVelocity.setValue(new ChassisSpeeds());
+			} else if (
+				ToleranceMath.isNear(lastEstimatedVelocity.getValue(), rawVelocity, WPILibPoseEstimatorConstants.ESTIMATED_VELOCITY_TOLERANCE)
+			) {
+				lastEstimatedVelocity.setValue(
+					new ChassisSpeeds(
+						(rawVelocity.vxMetersPerSecond + lastEstimatedVelocity.getValue().vxMetersPerSecond) / 2.0,
+						(rawVelocity.vyMetersPerSecond + lastEstimatedVelocity.getValue().vyMetersPerSecond) / 2.0,
+						(rawVelocity.omegaRadiansPerSecond + lastEstimatedVelocity.getValue().omegaRadiansPerSecond) / 2.0
+					)
+				);
+			}
+			// If neither condition is met, it is a massive vision spike. We do NOT update the value, preserving the safe velocity.
+			lastEstimatedVelocity.setTimestamp(timestamp);
+		}
+		lastEstimatedPose.setTimestamp(timestamp);
+		lastEstimatedPose.setValue(getEstimatedPose());
+	}
+
 	@Override
 	public void resetPose(OdometryData odometryData, Pose2d poseMeters) {
 		Logger.recordOutput(logPath + "/lastPoseResetTo", poseMeters);
@@ -226,6 +261,10 @@ private void updateVision(RobotPoseObservation visionRobotPoseObservation) {
 		});
 	}
 
+	public ChassisSpeeds getLastEstimatedVelocity() {
+		return lastEstimatedVelocity.getValue();
+	}
+
 	private void addVisionMeasurement(RobotPoseObservation visionObservation) {
 		poseEstimator.addVisionMeasurement(
 			visionObservation.robotPose(),
@@ -262,4 +301,16 @@ private Optional<Rotation2d> getEstimatedPoseToIMUYawDifference(Optional<Rotatio
 			.flatMap(estimatedPose -> gyroYaw.map(yaw -> estimatedPose.getRotation().minus(yaw)));
 	}
 
+	@Override
+	public ChassisSpeeds getFieldRelativeEstimatedVelocity(ChassisSpeeds swerveVelocity) {
+		if (useEstimatedVelocity(swerveVelocity, lastEstimatedVelocity.getValue())) {
+			return lastEstimatedVelocity.getValue();
+		}
+		return swerveVelocity;
+	}
+
+	private boolean useEstimatedVelocity(ChassisSpeeds swerveVelocity, ChassisSpeeds estimatedVelocity) {
+		return !ToleranceMath.isNear(swerveVelocity, estimatedVelocity, WPILibPoseEstimatorConstants.ESTIMATED_VELOCITY_TOLERANCE);
+	}
+
 }

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

@@ -55,4 +55,10 @@ public static Rotation2d clamp(Rotation2d angle, Rotation2d minAngle, Rotation2d
 		return Rotation2d.fromRadians(MathUtil.clamp(angle.getRadians(), minAngle.getRadians(), maxAngle.getRadians()));
 	}
 
+	public static boolean isNear(ChassisSpeeds wantedSpeed, ChassisSpeeds actualSpeed, ChassisSpeeds tolerance) {
+		return isNear(wantedSpeed.vxMetersPerSecond, actualSpeed.vxMetersPerSecond, tolerance.vxMetersPerSecond)
+			&& isNear(wantedSpeed.vyMetersPerSecond, actualSpeed.vyMetersPerSecond, tolerance.vyMetersPerSecond)
+			&& isNear(wantedSpeed.omegaRadiansPerSecond, actualSpeed.omegaRadiansPerSecond, tolerance.omegaRadiansPerSecond);
+	}
+
 }