Kalman filter

A Kalman filter combines a prediction with a measurement. It is useful when you have a model of what should happen and a sensor that gives noisy readings. The filter weighs each source according to how uncertain it is.

This page starts with a deliberately small example: estimating a nearly stationary distance. It is not a complete robot-localization filter. A moving robot needs a motion model, correctly timed measurements, and consistent coordinate frames.

Overview

Imagine the robot is not moving, but a distance sensor reports 50, 48, 52, and 49 cm. Averaging or filtering can reduce the wobble, but a Kalman filter also keeps track of its uncertainty and can include a prediction model.

For this simple example, assume the distance should stay about the same between readings. That assumption is wrong if the robot or target is moving quickly; then the model must include motion.

Prediction

Keep two values:

• estimate: the current best distance estimate;
• P: how uncertain that estimate is. A larger P means less confidence.

For a stationary target, the predicted distance stays the same. Increase uncertainty by a process-noise value Q, because the world may have changed a little since the last reading:

predicted estimate = previous estimate
predicted uncertainty = previous uncertainty + Q

Q is about changes the model does not capture. Choose it to represent that uncertainty, rather than treating it as a general smoothing adjustment.

Measurement update

Let z be the new measurement and R describe measurement uncertainty. The Kalman gain decides how far to move the prediction toward that measurement:

Then update the estimate:

If the sensor is noisy (large R), K is smaller and the estimate stays closer to the prediction. If the prediction is uncertain (large P), K is larger and the new reading has more influence.

Worked example

Suppose the previous estimate was 50 cm, its uncertainty was 4, and this update adds process noise Q = 1. The next sensor reading is 54 cm, with measurement uncertainty R = 4.

1. Predicted estimate: 50 cm.
2. Predicted uncertainty: 4 + 1 = 5.
3. Kalman gain: 5 / (5 + 4) = 5/9 ≈ 0.556 (approximately).
4. Estimate: 50 + (5/9) × (54 - 50) ≈ 52.22 cm.

The result moves toward 54 cm but does not jump all the way there. After the update, uncertainty decreases because the filter has used another measurement.

Model checks

1. Log raw sensor values while the target is still.
2. Estimate how much those readings vary; use that to reason about measurement noise.
3. Move the target slowly and check whether the stationary-target model follows too slowly.
4. If the real system moves, add a motion model instead of just increasing or decreasing a constant until the output looks smooth.
5. Compare raw readings, predicted values, and filtered estimates in telemetry.

Fix sensor bias, unit mistakes, loose mounts, and model errors before tuning the filter.

Scalar filter

This small class implements the same predict-and-correct steps for one distance value. estimate is measured in centimeters. uncertainty, processNoise, and measurementNoise are variances, so their units are square centimeters. The starting numbers below are examples; estimate them from repeated sensor readings.

public final class ScalarKalmanFilter {
    private double estimate;
    private double uncertainty;
    private final double processNoise;
    private final double measurementNoise;
    private boolean initialized;

    public ScalarKalmanFilter(double processNoise, double measurementNoise) {
        if (!Double.isFinite(processNoise) || processNoise < 0.0
                || !Double.isFinite(measurementNoise) || measurementNoise <= 0.0) {
            throw new IllegalArgumentException("Require Q >= 0 and R > 0, both finite");
        }
        this.processNoise = processNoise;
        this.measurementNoise = measurementNoise;
    }

    public double update(double measurement) {
        if (!Double.isFinite(measurement)) throw new IllegalArgumentException("Finite measurement required");
        if (!initialized) {
            estimate = measurement;
            uncertainty = measurementNoise;
            initialized = true;
            return estimate;
        }

        // Predict: this model expects distance to stay nearly constant.
        uncertainty += processNoise;

        // Correct: move the prediction toward the new reading.
        double gain = uncertainty / (uncertainty + measurementNoise);
        estimate += gain * (measurement - estimate);
        uncertainty *= (1.0 - gain);
        return estimate;
    }
}

class ScalarKalmanFilter(
    private val processNoise: Double,
    private val measurementNoise: Double
) {
    init {
        require(processNoise.isFinite() && processNoise >= 0.0)
        require(measurementNoise.isFinite() && measurementNoise > 0.0)
    }
    private var estimate = 0.0
    private var uncertainty = 0.0
    private var initialized = false

    fun update(measurement: Double): Double {
        require(measurement.isFinite())
        if (!initialized) {
            estimate = measurement
            uncertainty = measurementNoise
            initialized = true
            return estimate
        }

        // Predict: this model expects distance to stay nearly constant.
        uncertainty += processNoise

        // Correct: move the prediction toward the new reading.
        val gain = uncertainty / (uncertainty + measurementNoise)
        estimate += gain * (measurement - estimate)
        uncertainty *= 1.0 - gain
        return estimate
    }
}

The first valid measurement initializes the estimate. On later calls, uncertainty += processNoise says the stationary-distance prediction becomes a little less certain; the gain then decides how much to trust the new reading.

Use it in an OpMode after mapping a distance sensor named frontDistance:

ScalarKalmanFilter filter = new ScalarKalmanFilter(0.2, 4.0); // Variance examples
// Example rig limits, not a universal sensor specification.
double minValidCm = 2.0, maxValidCm = 150.0;

while (opModeIsActive()) {
    double rawCm = frontDistance.getDistance(DistanceUnit.CM);
    if (Double.isFinite(rawCm) && rawCm != DistanceSensor.distanceOutOfRange
            && rawCm >= minValidCm && rawCm <= maxValidCm) {
        double filteredCm = filter.update(rawCm);
        telemetry.addData("Raw (cm)", "%.1f", rawCm);
        telemetry.addData("Filtered (cm)", "%.1f", filteredCm);
    } else {
        telemetry.addLine("Invalid distance; estimate is stale");
    }
    telemetry.update();
    idle();
}

val filter = ScalarKalmanFilter(0.2, 4.0) // Variance examples
// Example rig limits, not a universal sensor specification.
val minValidCm = 2.0
val maxValidCm = 150.0

while (opModeIsActive()) {
    val rawCm = frontDistance.getDistance(DistanceUnit.CM)
    if (rawCm.isFinite() && rawCm != DistanceSensor.distanceOutOfRange &&
        rawCm in minValidCm..maxValidCm) {
        val filteredCm = filter.update(rawCm)
        telemetry.addData("Raw (cm)", "%.1f", rawCm)
        telemetry.addData("Filtered (cm)", "%.1f", filteredCm)
    } else {
        telemetry.addLine("Invalid distance; estimate is stale")
    }
    telemetry.update()
    idle()
}

This is a one-value stationary-target filter, not a moving-robot localization filter. If the measured object moves, increase the model to represent that motion or use a filter designed for the state you are estimating. The filter also does not replace checking whether the sensor reading is in range or physically plausible.

Angle wrapping

Angles wrap around. 179° and -179° are only 2° apart, but their ordinary numeric average is 0°. A simple scalar filter can therefore produce a heading on the opposite side of the field. Use an angle-aware method or a localization library that handles wrapped heading values. Also check whether the IMU has been initialized and mounted according to the FTC SDK instructions.

NextFTC version check

NextFTC's independently versioned control module 0.2.1 exposes predict(inputs) and correct(inputs, outputs) for a state-space model. Source inspection of that release found that correction uses outputs - D * inputs, omitting the predicted-state term C * Xhat. The usual innovation is outputs - (C * Xhat + D * inputs). A nonzero stationary measurement can therefore keep adding corrections instead of converging.

This issue affects the release described here. Before using another release, test it with a known nonzero state and known measurements. The scalar example above is a separate implementation for learning.

Q here is a variance added per update. Keep the measurement cadence consistent; for variable timesteps, derive the process noise from a time-based model. Use fresh measurements and do not repeatedly count a cached sample as independent evidence.

Uses and limitations

A Kalman filter can be useful when you have a measured noise problem and a model that predicts how the system changes. For a single jittery sensor reading with no motion model, an exponential moving average or median filter may be easier to tune and explain. For robot pose, use a localization system designed for the sensors, timing, and geometry you have.

Learn the simpler Low-Pass Filter first, or read Sensor Fusion for the difference between combining robot measurements and merely smoothing one signal.