Low-pass filter

Suppose a distance sensor says 10.0, 9.4, 10.3, and 9.7 cm while the robot is not moving. The target is steady, but the readings wobble. A low-pass filter smooths those quick changes so code can pay more attention to the general trend.

A filter also adds delay. If you smooth too much, the reported value takes longer to catch up when the target really moves. Use it only when noise is causing a real problem, and compare the filtered result with the raw reading.

Exponential moving average

An exponential moving average blends the newest reading with the previous filtered value:

filtered = alpha × new reading + (1 - alpha) × previous filtered value

alpha is between 0 and 1:

• alpha close to 1 follows changes quickly and smooths less.
• alpha close to 0 smooths more but responds more slowly.

Filter state

The filter must remember its last output. For the first reading, use the reading itself; otherwise a starting value of zero can create a fake jump.

private boolean hasReading = false;
private double filteredDistance;

private var hasReading = false
private var filteredDistance = 0.0

Filter update

private double filter(double newDistance, double alpha) {
    if (!Double.isFinite(alpha) || alpha < 0.0 || alpha > 1.0
            || !Double.isFinite(newDistance)) {
        throw new IllegalArgumentException("Use a finite reading and alpha in [0, 1]");
    }
    if (!hasReading) {
        filteredDistance = newDistance;
        hasReading = true;
    } else {
        filteredDistance = alpha * newDistance
            + (1.0 - alpha) * filteredDistance;
    }
    return filteredDistance;
}

private fun filter(newDistance: Double, alpha: Double): Double {
    require(newDistance.isFinite() && alpha.isFinite() && alpha in 0.0..1.0)
    if (!hasReading) {
        filteredDistance = newDistance
        hasReading = true
    } else {
        filteredDistance = alpha * newDistance + (1.0 - alpha) * filteredDistance
    }
    return filteredDistance
}

The first call initializes the filter. Each later call combines the new measurement with the last filtered output. Call it with one stream of measurements from one sensor; don't reuse the same state for unrelated signals.

Sensor integration

// Example validated range for this test rig; measure your sensor's usable range.
double minValidCm = 2.0;
double maxValidCm = 150.0;
double rawCm = distanceSensor.getDistance(DistanceUnit.CM);
boolean valid = Double.isFinite(rawCm)
    && rawCm != DistanceSensor.distanceOutOfRange
    && rawCm >= minValidCm && rawCm <= maxValidCm;
if (valid) {
    double smoothCm = filter(rawCm, 0.25);
    telemetry.addData("Raw cm", "%.1f", rawCm);
    telemetry.addData("Filtered cm", "%.1f", smoothCm);
} else {
    telemetry.addLine("Invalid distance; filter state unchanged");
}
telemetry.update();

// Example validated range for this test rig; measure your sensor's usable range.
val minValidCm = 2.0
val maxValidCm = 150.0
val rawCm = distanceSensor.getDistance(DistanceUnit.CM)
val valid = rawCm.isFinite() && rawCm != DistanceSensor.distanceOutOfRange &&
    rawCm in minValidCm..maxValidCm
if (valid) {
    val smoothCm = filter(rawCm, 0.25)
    telemetry.addData("Raw cm", "%.1f", rawCm)
    telemetry.addData("Filtered cm", "%.1f", smoothCm)
} else {
    telemetry.addLine("Invalid distance; filter state unchanged")
}
telemetry.update()

These fragments assume a mapped DistanceSensor and imports for com.qualcomm.robotcore.hardware.DistanceSensor and org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit. The SDK unavailable sentinel is finite (Double.MAX_VALUE), and some sensor failures also return large finite values. Reject them before updating the filter. The 2–150 cm bounds describe an example test rig, not every sensor. An invalid reading leaves the estimate stale; do not present it as a fresh measurement.

0.25 is an example starting point, not a universally correct setting. Run the sensor without moving the target, then move it deliberately. Reduce alpha only if the jitter is troublesome and the added lag is acceptable.

Uses and limitations

A low-pass filter can help with noisy distance, color, or encoder-derived signals. It does not correct sensor bias, bad calibration, a loose mount, or an incorrect unit conversion. Fix those causes first.

Avoid blindly filtering heading angles around the wrap boundary. For example, averaging 179° and -179° as ordinary numbers gives 0°, even though the headings are nearly the same. Use angle-aware math for wrapped values.

Practice

1. Hold a stationary target and record raw readings for a few seconds.
2. Add the filter and show raw and filtered values together.
3. Move the target once. Notice how long the filtered value takes to follow.
4. Try a slightly higher and lower alpha. Choose the least smoothing that removes the jitter affecting your decision.

A filter setting is a tradeoff between smoothness and response time. Test it at the same loop rate and sensor conditions where your robot will use it.