ROS2 Kalman Filter for 1D Range Sensor Fusion

Problem Statement

Implement a 1D Kalman Filter class and a function that fuses a sequence of noisy range measurements into a filtered estimate.

Part 1 — Implement the KalmanFilter1D class

class KalmanFilter1D:
    def __init__(self, x0, P0, Q, R): ...
    def predict(self, dt=1.0) -> float: ...       # returns predicted state
    def update(self, z: float) -> float: ...       # returns updated state

• x0: initial state estimate (e.g. 0.0)
• P0: initial error covariance (uncertainty, e.g. 1.0)
• Q: process noise covariance (how much the true state can change between steps)
• R: measurement noise covariance (sensor noise variance)

Predict step:

P = P + Q          # propagate covariance
# x unchanged (constant position model)

Update step:

K = P / (P + R)    # Kalman gain: 0=trust prediction, 1=trust measurement
x = x + K * (z - x)  # update state
P = (1 - K) * P   # update covariance

Part 2 — Implement the fusion function

def kalman_fuse(measurements: list[float], Q=0.01, R=0.5) -> list[float]:

• Create a KalmanFilter1D with x0=measurements[0], P0=1.0, Q=Q, R=R
• For each measurement: call predict() then update(z)
• Return the list of updated state estimates (same length as input)

🤖 Why This Matters in Real Robots

The robot_localization ROS2 package (EKF + UKF) uses this exact structure, extended to multiple dimensions. Autonomous vehicles fuse GPS (high R, low frequency) with IMU (low R, high frequency) using the same predict/update cycle. Understanding the scalar case is the prerequisite to the full EKF used in every navigation stack.