Class TagEkf

java.lang.Object
frc.spectrumLib.localization.TagEkf
public class TagEkf extends Object
An extended Kalman filter over (x, y, heading) that takes every camera's tags one at a time, in the style of 971's AOS localizer: each tag is a bearing and a distance from its camera, so a single or far tag still corrects the pose at a low weight instead of being thrown away, and every camera feeds the same estimate.
  • Predict on every odometry sample: wheel translation, gyro heading change. Variance grows with distance driven, and more when the modules disagree with each other or with the gyro (slip). Heading gets very little.
  • Correct per tag, at the frame's capture time. About HISTORY_SECONDS of states are kept; a frame is inserted at its time and odometry and later frames are replayed over it, so the result does not depend on arrival order.
  • Reject a tag whose innovation fails a chi-squared test. If tags keep failing for RELOCALIZE_SECONDS, the covariance is widened so the cameras can pull it back.

Noise values are starting points to tune against logs, not measured.

  • Constructor Details

    • TagEkf

      public TagEkf(Translation2d[] moduleLocations)
      Parameters:
      moduleLocations - module positions from the robot centre, robot frame

  • Method Details

    • setOdometryNoiseScale

      public void setOdometryNoiseScale(double scale)
      Multiplies odometry translation variance, e.g. while the robot is tilted on a bump.
      Parameters:
      scale - 1 for normal driving

    • addOdometry

      public void addOdometry(double timestampSeconds, Rotation2d gyro, SwerveModulePosition[] positions)
      Predicts with one odometry sample.
      Parameters:
      timestampSeconds - sample time
      gyro - gyro yaw
      positions - module positions, same order as the locations

    • reset

      public void reset(Pose2d pose)
      Hard-resets the pose (auto start, driver reorient). Frames captured before it are dropped.
      Parameters:
      pose - the new pose

    • addPose

      public void addPose(Pose2d pose, double timestampSeconds, double xyStd, double thetaStd)
      Applies a whole-pose measurement at its capture time (the seed every track shares).
      Parameters:
      pose - measured pose
      timestampSeconds - capture time
      xyStd - translation std-dev, metres
      thetaStd - heading std-dev, radians

    • addFrame

      public void addFrame(List<TagSighting> frame)
      Applies one camera frame's tags. They must share one capture time.
      Parameters:
      frame - the frame's tags

    • skip

      public void skip(String camera, String reason)
      Counts a tag the caller chose not to apply.
      Parameters:
      camera - the tag's camera
      reason - why, for RejectionCounts

    • clearLoopStats

      public void clearLoopStats()
      Clears this loop's residuals; call after logging.

    • getPose

      public Pose2d getPose()

    • getStdDevs

      public double[] getStdDevs()
      Std-devs [x m, y m, heading rad].

    • isSeeded

      public boolean isSeeded()

    • getAcceptedCount

      public long getAcceptedCount()

    • getRejectedCount

      public long getRejectedCount()

    • getRelocalizations

      public long getRelocalizations()

    • getRejectionCounts

      public Map<String,Long> getRejectionCounts()

    • getCameraStats

      public Map<String, TagEkf.CameraStats> getCameraStats()