Add variable low-pass filter for wheel slip detection in LaunchCalculator

[jamesdooley4]

Rather than directly looking at the pose-derived speed, using a variable low-pass filter on the error between wheelSpeeds & the pose-derived speeds would more directly measure the error between what the vision-corrected pose is doing & what the wheels are doing. Something like:

      // Slip = (pose-derived velocity) - (wheel velocity). When wheels match pose, slip
      // is ~0; when the robot is pushed or wheels slip, slipRaw is the motion delta.
      double slipRawX = twist.dx / poseDt - wheelSpeeds.vxMetersPerSecond;
      double slipRawY = twist.dy / poseDt - wheelSpeeds.vyMetersPerSecond;
      double slipRawOmega = twist.dtheta / poseDt - wheelSpeeds.omegaRadiansPerSecond;

Then determine the alpha based on the magnitude of the slip value:

      // Switch to fast tracking when slip is large enough to be a real event
      // rather than vision noise. Hysteresis prevents flicker.
      double slipRawMag = Math.hypot(slipRawX, slipRawY);
      slipFastMode = slipRawMag > slipFastMode ? FAST_SLIP_THRESHOLD_EXIT : FAST_SLIP_THRESHOLD_ENTER;
      double alpha = slipFastMode ? SLIP_FILTER_ALPHA_FAST : SLIP_FILTER_ALPHA_SLOW;

Then calculate the filtered slip value using the alpha contribution:

      double newSlipX = alpha * filteredSlip.vxMetersPerSecond + (1 - alpha) * slipRawX;
      double newSlipY = alpha * filteredSlip.vyMetersPerSecond + (1 - alpha) * slipRawY;
      double newSlipOmega =
          alpha * filteredSlip.omegaRadiansPerSecond + (1 - alpha) * slipRawOmega;

      // Cap the filtered slip magnitude
      double newSlipMag = Math.hypot(newSlipX, newSlipY);
      if (newSlipMag > MAX_SLIP_MAGNITUDE) {
        double scale = MAX_SLIP_MAGNITUDE / newSlipMag;
        newSlipX *= scale;
        newSlipY *= scale;
      }
      newSlipOmega = MathUtil.clamp(newSlipOmega, -MAX_SLIP_OMEGA, MAX_SLIP_OMEGA);
      filteredSlip = new ChassisSpeeds(newSlipX, newSlipY, newSlipOmega);

And finally, blend in to the effectiveSpeeds and reset slipFastMode if the robot stops or dT is bogus:

      effectiveSpeeds =
          new ChassisSpeeds(
              wheelSpeeds.vxMetersPerSecond + blendAlpha * filteredSlip.vxMetersPerSecond,
              wheelSpeeds.vyMetersPerSecond + blendAlpha * filteredSlip.vyMetersPerSecond,
              wheelSpeeds.omegaRadiansPerSecond
                  + blendAlpha * filteredSlip.omegaRadiansPerSecond);
    } else {
      // Reset slip filter when robot stops or poseDt is out of range
      filteredSlip = new ChassisSpeeds(0, 0, 0);
      slipFastMode = false;
    }

And the various constants:

  // Variable-alpha low-pass filter for the slip estimate. Vision-correction noise and
  // real wheel slip live in different magnitude regimes:
  //   - Vision noise: ~1 cm correction over 20 ms = ~0.5 m/s, single-frame transients.
  //   - Real stuck/push: wheels at 2 m/s vs robot at 0 = 2+ m/s, sustained over many frames.
  // We use a heavy filter (slow) below the threshold to reject noise, and a light filter
  // (fast) above it to track real events with minimal lag. Hysteresis on the threshold
  // prevents flicker between modes.
  private static final double SLIP_FILTER_ALPHA_SLOW = 0.85; // noise rejection
  private static final double SLIP_FILTER_ALPHA_FAST = 0.20; // fast tracking
  private static final double FAST_SLIP_THRESHOLD_ENTER = 2.0; // m/s
  private static final double FAST_SLIP_THRESHOLD_EXIT = 0.5; // m/s (hysteresis)
  private boolean slipFastMode = false;

  // Safety caps on the filtered slip. Prevents a runaway pose-estimator glitch, such as
  // a bad vision measurement that spikes the derivative, from injecting nonsense into the
  // calculation. Real robot pushes and rotational disturbances stay well below these.
  private static final double MAX_SLIP_MAGNITUDE = 5.0; // m/s
  private static final double MAX_SLIP_OMEGA = 8.0; // rad/s (~1.3 rotations/s)

Originally posted by @jamesdooley4 in #243