Class PoseFusion

java.lang.Object
frc.spectrumLib.localization.PoseFusion
public class PoseFusion extends Object
The robot's pose estimate, and the machinery to compare every source against it.

Three kinds of estimator run side by side, all fed the same odometry samples:

  • Fused is the robot pose: a WPILib SwerveDrivePoseEstimator, corrected by the accepted measurements of every enabled source. This is what the robot drives on.
  • Odometry uses wheels and gyro only, and is never corrected. It is the baseline every source is trying to beat, and it shows how far the robot drifts without help.
  • Shadow, one per source is odometry plus only that source's accepted measurements, whether or not the source is enabled. This is the comparison: after a match each shadow track is "what the robot would have believed with only this source", and they can be overlaid in AdvantageScope. Seeds and explicit resets are applied to every shadow, so they all start from the same place.

Everything here runs on the main loop from logged inputs, so the whole comparison is regenerated in AdvantageKit replay, and a gate or std-dev change can be tested against a real match without the robot.

Fusion runs where the drivetrain loop runs, on SystemCore. The fused track is WPILib's estimator. A TagEkf runs alongside it on the same odometry, seeds and resets, taking every camera's tags one at a time. It is logged for comparison only and never moves the robot pose.

  • Constructor Details

    • PoseFusion

      public PoseFusion(SwerveDriveKinematics kinematics, Rotation2d gyro, SwerveModulePosition[] positions, double[] odometryStdDevs)
      Creates the estimators.
      Parameters:
      kinematics - drivetrain kinematics
      gyro - initial gyro angle
      positions - initial module positions
      odometryStdDevs - state std-devs [x, y, theta] (m, m, rad) for the wheel model

  • Method Details

    • register

      public void register(PoseSource source)
      Registers a source so it gets a shadow estimator. Call once per source at startup.
      Parameters:
      source - the source

    • addOdometry

      public void addOdometry(double timestampSeconds, Rotation2d gyro, SwerveModulePosition[] positions)
      Feeds one odometry sample to every estimator.
      Parameters:
      timestampSeconds - sample time, Timer.getTimestamp() base
      gyro - gyro yaw at the sample
      positions - module positions at the sample

    • addOdometry

      public void addOdometry(double timestampSeconds, Rotation2d gyro, SwerveModulePosition[] positions, boolean includeShadows)
      Feeds one odometry sample to the fused and odometry tracks, and to the shadows when includeShadows.

      The fused pose takes every 250 Hz sample. The shadows are comparison tracks and take one per robot loop (the caller passes true for the loop's last sample): module positions are absolute, so skipping intermediate samples only coarsens the arc between them, and it cuts the shadows' cost by the number of samples per loop.

      Parameters:
      timestampSeconds - sample time, Timer.getTimestamp() base
      gyro - gyro yaw at the sample
      positions - module positions at the sample
      includeShadows - whether the shadow tracks take this sample

    • addOdometry

      public void addOdometry(double timestampSeconds, Rotation2d gyro, SwerveModulePosition[] positions, SwerveModulePosition[] rawPositions, boolean includeShadows)
      As addOdometry(double, Rotation2d, SwerveModulePosition[], boolean), with separate module positions for the odometry-only track: the fused pose and shadows can take adjusted (e.g. tilt-scaled) wheel travel while OdometryPose keeps the raw wheels and gyro.
      Parameters:
      timestampSeconds - sample time, Timer.getTimestamp() base
      gyro - gyro yaw at the sample
      positions - module positions for the fused pose and the shadows
      rawPositions - module positions for the odometry-only track
      includeShadows - whether the shadow tracks take this sample

    • apply

      public void apply(PoseSource source, List<PoseSource.Result> results, boolean fuse)
      Applies one source's results: every accepted measurement goes to its shadow, and to the fused pose if the source is enabled. Also logs each measurement's disagreement with the fused pose at its own capture time, which is the per-frame comparison metric.
      Parameters:
      source - the source the results came from
      results - this loop's results
      fuse - whether accepted measurements also move the fused pose

    • seed

      public void seed(Pose2d pose, double timestampSeconds, double xyStd, double thetaStd)
      Adds one measurement straight to the fused pose and every shadow. Used for seeding, the one event every track should share.
      Parameters:
      pose - measured pose
      timestampSeconds - capture time
      xyStd - translation std-dev, metres
      thetaStd - heading std-dev, radians

    • seedFusedOnly

      public void seedFusedOnly(Pose2d pose, double timestampSeconds, double xyStd, double thetaStd)
      Like seed(Pose2d, double, double, double) but leaves the tag EKF alone, for a pose built from the fused translation that would otherwise count odometry twice in the EKF.
      Parameters:
      pose - measured pose
      timestampSeconds - capture time
      xyStd - translation std-dev, metres
      thetaStd - heading std-dev, radians

    • resetPose

      public void resetPose(Pose2d pose)
      Hard-resets every estimator to a pose (auto start, driver reorient).
      Parameters:
      pose - the new pose

    • setWheelHeading

      public void setWheelHeading(boolean fromWheels)
      Whether the odometry heading is coming from the wheels, both gyros being out.

    • getPose

      public Pose2d getPose()
      The fused robot pose.

    • sampleAt

      public Optional<Pose2d> sampleAt(double timestampSeconds)
      The fused pose at a past time, if still in the buffer.

    • getLastGyro

      public Rotation2d getLastGyro()
      The gyro yaw of the latest odometry sample, as the odometry reads it.

    • getOdometryPose

      public Pose2d getOdometryPose()
      Wheels-and-gyro-only pose.

    • getShadowPose

      public Optional<Pose2d> getShadowPose(String source)
      A source's shadow pose.

    • setTruth

      public void setTruth(Optional<Pose2d> truthPose)
      Sets the simulation ground truth used for the ErrorVsTruth metrics.
      Parameters:
      truthPose - the true pose, or empty

    • log

      public void log()
      Logs every track. Call once per loop after all sources are applied.

    • getTagEkf

      public TagEkf getTagEkf()

    • getTruth

      public Optional<Pose2d> getTruth()
      Ground truth pose in simulation; empty on a robot and in replay without it logged.