Filter NavX2 inertial velocity

build.gradle

@@ -97,7 +97,7 @@ dependencies {
   nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop)
   simulationRelease wpi.sim.enableRelease()
 
-  implementation 'com.github.lasarobotics:PurpleLib:87a27eedff'
+  implementation 'com.github.lasarobotics:PurpleLib:172583a8d6'
 
   implementation 'org.apache.commons:commons-math3:3.+'
 

src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java

@@ -38,6 +38,7 @@
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.filter.LinearFilter;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
@@ -184,6 +185,9 @@ public Hardware(NavX2 navx,
   private Field2d m_field;
   private Alliance m_currentAlliance;
 
+  private LinearFilter m_inertialVelocityXFilter;
+  private LinearFilter m_inertialVelocityYFilter;
+
   private boolean m_isTractionControlEnabled = true;
   private boolean m_autoAimFront = false;
   private boolean m_autoAimBack = false;
@@ -226,6 +230,8 @@ public DriveSubsystem(Hardware drivetrainHardware, PIDConstants pidf, ControlCen
     );
     this.m_currentAlliance = Alliance.Blue;
     this.m_allianceCorrection = GlobalConstants.ROTATION_ZERO;
+    this.m_inertialVelocityXFilter = LinearFilter.singlePoleIIR(0.1, GlobalConstants.ROBOT_LOOP_PERIOD);
+    this.m_inertialVelocityXFilter = LinearFilter.singlePoleIIR(0.1, GlobalConstants.ROBOT_LOOP_PERIOD);
 
     // Calibrate and reset navX
     while (m_navx.isCalibrating()) stop();
@@ -798,6 +804,10 @@ public void periodic() {
     // Update current heading
     m_currentHeading = new Rotation2d(getPose().getX() - m_previousPose.getX(), getPose().getY() - m_previousPose.getY());
 
+    // Filter NavX2 velocity
+    m_navx.getInputs().xVelocity = Units.MetersPerSecond.of(m_inertialVelocityXFilter.calculate(m_navx.getInputs().xVelocity.in(Units.MetersPerSecond)));
+    m_navx.getInputs().yVelocity = Units.MetersPerSecond.of(m_inertialVelocityYFilter.calculate(m_navx.getInputs().yVelocity.in(Units.MetersPerSecond)));
+
     if (RobotBase.isSimulation()) return;
     smartDashboard();
     logOutputs();