Skip to main content

Robot Localization with MegaTag2

Introduced in 2024, Megatag2 is a precise and ambiguity-free AprilTag-based localizer for mobile robots. It was built with the following goals:

  • Eliminate the pose ambiguity problem and increase robustness against image/corner noise.
  • Provide excellent pose estimates given a single tag, no matter the perspective.
  • Increase robustness against physical AprilTag placement inaccuracies
  • Reduce the amount of filtering necessary for good pose estimation results

Megatag2 provides excellent results at any distance given a single tag. This means it is perfectly viable to focus only on tags that are both relevant and within tolerance, and filter out all other tags. If a tag is not in the correct location, filter it out with the dynamic filter feature introduced alongside MegaTag2.

int[] validIDs = {3,4};
LimelightHelpers.SetFiducialIDFiltersOverride("limelight", validIDs);

Unlike MT1, MT2 assumes that you know your robot's heading (yaw). Optionally, Megatag2 accepts a complete robot orientation and angular velocities.

Requirements:

  • Your Limelight's robot-space pose has been configured in the webUI or via the API
  • A field map (.fmap) has been uploaded
  • LimelightHelpers.SetRobotOrientation(robotYawInDegrees,0,0,0,0,0) is called every frame in robot-side code
  • SetRobotOrientation assumes a centered (see the map generator) or blue-corner origin. CCW-positive, 0 degrees -> facing red alliance wall in FRC.

NetworkTables Keys:

  • botpose_orb_wpiblue
  • botpose_orb_wpired
  • botpose_orb

JSON Keys:

  • botpose_orb_wpiblue
  • botpose_orb_wpired
  • botpose_orb
  • (Per fiducial target) t6r_fs_orb - robot pose in field space using megatag2 based on this tag alone (no multitag)

Notice the difference between MegaTag2 (red robot) and Megatag (blue robot) in this highly ambiguous single-tag case

  • Gold Cylinder / Red Robot: Unfiltered Megatag2 botpose
  • Yellow Cylinders: Unfiltered single-tag Megatag2 botposes
  • White Cylinder/Blue Robot: MegaTag1 Botpose
  • Green Cylinder: Individual per-tag bot pose (MT1)
  • Blue Cylinder: Average of individual per-tag bot poses (MT1)
info

In 2024, most of the WPILib Ecosystem transitioned to a single-origin coordinate system. In 2023, your coordinate system origin changed based on your alliance color.

For 2024 and beyond, the origin of your coordinate system should always be the "blue" origin. FRC teams should always use botpose_orb_wpiblue for pose-related functionality

Using WPILib's Pose Estimator​

  LimelightHelpers.SetRobotOrientation("limelight", m_poseEstimator.getEstimatedPosition().getRotation().getDegrees(), 0, 0, 0, 0, 0);
      LimelightHelpers.PoseEstimate mt2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight");
      if(Math.abs(m_gyro.getRate()) > 720) // if our angular velocity is greater than 720 degrees per second, ignore vision updates
      {
        doRejectUpdate = true;
      }
      if(mt2.tagCount == 0)
      {
        doRejectUpdate = true;
      }
      if(!doRejectUpdate)
      {
        m_poseEstimator.setVisionMeasurementStdDevs(VecBuilder.fill(.7,.7,9999999));
        m_poseEstimator.addVisionMeasurement(
            mt2.pose,
            mt2.timestampSeconds);
      }

Configuring your Limelight's Robot-Space Pose​

LL Forward, LL Right, and LL Up represent distances along the Robot's forward, right, and up vectors if you were to embody the robot. (in meters). LL Roll, Pitch, and Yaw represent the rotation of your Limelight in degrees. You can modify these values and watch the 3D model of the Limelight change in the 3D viewer. Limelight uses this configuration internally to go from the target pose in camera space -> robot pose in field space.