Sensor fusion

Odometry can estimate how far the robot moved, but wheel slip makes that estimate drift. An IMU can measure turning, but it does not tell the robot where it is on the field. A camera can estimate the robot's position relative to a known target, but only when that target is visible and the camera geometry is configured correctly.

Sensor fusion means combining measurements with different strengths and limitations to maintain one better estimate. It does not automatically make a robot accurate: bad calibration, incorrect axes, or stale readings can make a combined estimate worse.

Sensor selection

Before combining readings, display each source separately:

1. Odometry: Does the estimated position move in the expected direction when you push or drive the robot a measured distance?
2. IMU: Does the heading increase in the expected direction during a slow turn? Is it in degrees or radians?
3. Vision: Does a known AprilTag detection report a plausible camera-relative pose? Does the robot know the camera's position and orientation on its frame?

Write down the coordinate convention for each one. FTC code and libraries may use different axis names or heading signs. Do not combine values until they refer to the same origin, axes, units, and moment in time.

IMU and encoder fusion

Use encoder-based odometry to estimate translation and an IMU to measure heading. The IMU helps estimate rotation; it does not replace the encoder measurements needed to estimate forward or sideways travel.

The steps are:

1. Read the encoder changes since the previous loop.
2. Convert those changes to robot-relative forward and sideways movement using calibrated constants.
3. Read the IMU heading and normalize its units and sign.
4. Rotate the robot-relative movement into the field coordinate frame using the heading convention chosen by your localization system.
5. Add that field-relative change to the previous position estimate.
6. Compare the result with a measured test and correct calibration before adding another sensor.

The example below turns that plan into a small differential-drive heading and position estimate. It combines wheel-based heading change with the IMU's yaw, then uses the corrected heading to put the wheel travel onto field X/Y. It does not support strafing, camera localization, or every robot coordinate convention.

First, convert each encoder delta into inches using your calibrated COUNTS_PER_INCH. The track width is the distance between the left and right wheels that produce the encoder readings. Configure and initialize the FTC IMU for its real mounting orientation before using this update.

public final class FusedOdometry {
    private final double trackWidthInches;
    private final double encoderHeadingWeight = 0.10;
    private int previousLeftTicks;
    private int previousRightTicks;
    private boolean started;
    private double xInches;
    private double yInches;
    private double headingRadians;
    private double initialRawYaw;
    private final double startHeadingRadians;

    public FusedOdometry(double trackWidthInches, double startX, double startY,
                         double startHeadingRadians) {
        if (!Double.isFinite(trackWidthInches) || trackWidthInches <= 0.0
                || !Double.isFinite(startX) || !Double.isFinite(startY)
                || !Double.isFinite(startHeadingRadians)) {
            throw new IllegalArgumentException("Finite pose and positive track width required");
        }
        this.startHeadingRadians = startHeadingRadians;
        this.trackWidthInches = trackWidthInches;
        this.xInches = startX;
        this.yInches = startY;
    }

    public void update(int leftTicks, int rightTicks,
                       double countsPerInch, double imuYawRadians) {
        if (!Double.isFinite(countsPerInch) || countsPerInch <= 0.0
                || !Double.isFinite(imuYawRadians)) {
            throw new IllegalArgumentException("Positive scale and finite yaw required");
        }
        if (!started) {
            previousLeftTicks = leftTicks;
            previousRightTicks = rightTicks;
            initialRawYaw = imuYawRadians;
            headingRadians = startHeadingRadians;
            started = true;
            return;
        }

        double leftInches = (leftTicks - previousLeftTicks) / countsPerInch;
        double rightInches = (rightTicks - previousRightTicks) / countsPerInch;
        previousLeftTicks = leftTicks;
        previousRightTicks = rightTicks;

        double forwardInches = (leftInches + rightInches) / 2.0;
        double wheelTurn = (rightInches - leftInches) / trackWidthInches;
        double predictedHeading = wrapRadians(headingRadians + wheelTurn);
        double fieldYaw = wrapRadians(startHeadingRadians + wrapRadians(imuYawRadians - initialRawYaw));
        double imuCorrection = wrapRadians(fieldYaw - predictedHeading);
        double correctedHeading = wrapRadians(
            predictedHeading + (1.0 - encoderHeadingWeight) * imuCorrection
        );

        // Use the middle of this small movement to rotate local travel into field axes.
        double middleHeading = headingRadians + 0.5 * wrapRadians(correctedHeading - headingRadians);
        xInches += forwardInches * Math.cos(middleHeading);
        yInches += forwardInches * Math.sin(middleHeading);
        headingRadians = correctedHeading;
    }

    private static double wrapRadians(double angle) {
        if (!Double.isFinite(angle)) throw new IllegalArgumentException("Finite angle required");
        return Math.atan2(Math.sin(angle), Math.cos(angle));
    }

    public double xInches() { return xInches; }
    public double yInches() { return yInches; }
    public double headingRadians() { return headingRadians; }
}

import kotlin.math.PI
import kotlin.math.cos
import kotlin.math.sin

class FusedOdometry(
    private val trackWidthInches: Double,
    startX: Double,
    startY: Double,
    private val startHeadingRadians: Double
) {
    init {
        require(trackWidthInches.isFinite() && trackWidthInches > 0.0)
        require(startX.isFinite() && startY.isFinite() && startHeadingRadians.isFinite())
    }
    private var initialRawYaw = 0.0
    private val encoderHeadingWeight = 0.10
    private var previousLeftTicks = 0
    private var previousRightTicks = 0
    private var started = false
    private var xInches = startX
    private var yInches = startY
    private var headingRadians = 0.0

    fun update(leftTicks: Int, rightTicks: Int,
               countsPerInch: Double, imuYawRadians: Double) {
        require(countsPerInch.isFinite() && countsPerInch > 0.0 && imuYawRadians.isFinite())
        if (!started) {
            previousLeftTicks = leftTicks
            previousRightTicks = rightTicks
            initialRawYaw = imuYawRadians
            headingRadians = startHeadingRadians
            started = true
            return
        }

        val leftInches = (leftTicks - previousLeftTicks) / countsPerInch
        val rightInches = (rightTicks - previousRightTicks) / countsPerInch
        previousLeftTicks = leftTicks
        previousRightTicks = rightTicks

        val forwardInches = (leftInches + rightInches) / 2.0
        val wheelTurn = (rightInches - leftInches) / trackWidthInches
        val predictedHeading = wrapRadians(headingRadians + wheelTurn)
        val fieldYaw = wrapRadians(startHeadingRadians + wrapRadians(imuYawRadians - initialRawYaw))
        val imuCorrection = wrapRadians(fieldYaw - predictedHeading)
        val correctedHeading = wrapRadians(
            predictedHeading + (1.0 - encoderHeadingWeight) * imuCorrection
        )

        // Use the middle of this small movement to rotate local travel into field axes.
        val middleHeading = headingRadians + 0.5 * wrapRadians(correctedHeading - headingRadians)
        xInches += forwardInches * cos(middleHeading)
        yInches += forwardInches * sin(middleHeading)
        headingRadians = correctedHeading
    }

    private fun wrapRadians(angle: Double): Double {
        require(angle.isFinite())
        return kotlin.math.atan2(sin(angle), cos(angle))
    }

    fun xInches() = xInches
    fun yInches() = yInches
    fun headingRadians() = headingRadians
}

Call it once each OpMode loop after reading the encoders and IMU. For the FTC SDK IMU API, getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS) supplies the yaw value after initialization. Construct FusedOdometry with track width, start X, start Y, and field start heading in radians, then call update() once before movement. It records initial raw yaw and aligns subsequent yaw changes with that field heading. Keep the counterclockwise-positive convention; initialize the hub mounting correctly. Reset pose, yaw offset, and encoder baselines together. A raw yaw of zero does not necessarily mean field +X. Display the estimate while pushing the robot by hand before using it to drive:

double imuYaw = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
localizer.update(leftMotor.getCurrentPosition(), rightMotor.getCurrentPosition(),
    COUNTS_PER_INCH, imuYaw);

telemetry.addData("X (in)", "%.1f", localizer.xInches());
telemetry.addData("Y (in)", "%.1f", localizer.yInches());
telemetry.addData("Heading (deg)", "%.1f",
    Math.toDegrees(localizer.headingRadians()));
telemetry.update();

val imuYaw = imu.robotYawPitchRollAngles.getYaw(AngleUnit.RADIANS)
localizer.update(leftMotor.currentPosition, rightMotor.currentPosition,
    COUNTS_PER_INCH, imuYaw)

telemetry.addData("X (in)", "%.1f", localizer.xInches())
telemetry.addData("Y (in)", "%.1f", localizer.yInches())
telemetry.addData("Heading (deg)", "%.1f",
    Math.toDegrees(localizer.headingRadians()))
telemetry.update()

Use fresh readings at a consistent cadence: this fixed per-update blend is rate-dependent. Reject invalid input before fusion.

The 0.10 encoder weight is only a starting illustration: it means this example corrects most of its heading prediction toward IMU yaw each update. Tune from measured turns and investigate any persistent bias first. This weighted blend is an educational complementary estimate, not a covariance-based Kalman filter. For a mecanum or three-dead-wheel robot, include calibrated sideways movement too; do not use the two-wheel equations above as if they measured strafe.

For path following, use a current localization library and follow its localizer guide rather than mixing snippets from different frameworks.

An IMU may reduce heading error from wheel slip, but it can still have bias, noise, initialization requirements, and mounting errors. Fusion does not “eliminate drift.” It changes how the estimate uses each sensor's information.

Camera measurements

AprilTag pose is relative to the camera. To compare it with the robot's field position, the program needs:

• the tag's known location and orientation on the field;
• the camera's fixed position and orientation on the robot;
• a consistent coordinate frame and units;
• a fresh detection whose pose is usable from the current view.

When these are known, a localization system can use the camera observation to correct accumulated motion-estimate error. A raw tag ftcPose value is not the robot's field pose by itself. BIOBUZZ also uses AprilTag clusters, so check the SDK version and current game layout before assuming every detection has one standalone tag ID.

See AprilTag Detection for current SDK result types and pose guidance.

Correction testing

1. Place the robot at a measured starting pose and reset the localization estimate.
2. Move it slowly along a short, measured route while displaying odometry and IMU values.
3. Stop at a known field location where a target is visible.
4. Compare the camera-based estimate with the odometry estimate and the measured location.
5. Check for a consistent offset, a sign/axis mistake, stale detections, or a camera-mount error before trusting a correction.

Keep telemetry for each source and for the combined estimate. If the combined value suddenly jumps, it should be possible to tell which measurement caused it. Do not command high-speed movement from a measurement you have not tested.

Common mistakes

• Assuming an IMU gives field position: it measures rotation and acceleration, not an absolute (x, y) location.
• Treating a tag pose as field coordinates: camera-relative pose needs field-tag and camera-on-robot transforms.
• Mixing degrees and radians or changing axis conventions: the result can look plausible while pointing the wrong way.
• Combining measurements from different times: delayed camera frames may describe where the robot was, not where it is now.
• Believing more sensors always means a better estimate: a badly calibrated sensor can make the estimate worse.
• Using old season tag assumptions: verify current field targets and SDK detection types.

Next steps

• Odometry — measure movement and understand drift.
• Vision Basics — distinguish image coordinates from field pose.
• AprilTag Detection — inspect FTC SDK tag and cluster results.
• Pedro Pathing — follow the current library's localization instructions.