Understanding Kalman Filter: A Simple, Step‑by‑Step Guide

When sensor data is noisy, extracting the true underlying signal is a common challenge in robotics, navigation, and many engineering fields. The Kalman filter provides a mathematically elegant solution that continuously combines predictions and measurements to produce an optimal estimate.

What is a Kalman Filter?

Kalman filter is an algorithm that predicts the state of a system (e.g., position, velocity) over time even when sensor data is noisy and uncertain.

The "state" can be any quantity that evolves with time, such as:

Position (1‑D coordinate, GPS latitude/longitude)

Velocity or acceleration

Direction or angle

Temperature, altitude, battery level, etc.

Conceptually, the filter works like an experienced captain who uses the ship’s current speed and direction (prediction) together with occasional lighthouse sightings (noisy measurements) to chart the most likely course.

One‑Dimensional Intuition: Bayesian Update

Consider a blindfolded walk down a long corridor. You have two information sources:

Prediction (step count): You estimate you have walked about 10 m, but the variance is large.

Measurement (touching a wall marker): You feel a marker at roughly 12 m, also with some error.

The Kalman filter does not simply average 10 m and 12 m. It performs a weighted average where the source with smaller variance (higher certainty) receives more weight, yielding a posterior estimate that is more certain than either source alone.

Code: 1‑D Kalman Filter

def update_belief(prior_mean, prior_var, meas_value, meas_var):
    """Weighted average update (Bayes rule).
    Args:
        prior_mean: μ_prior
        prior_var: σ²_prior
        meas_value: z
        meas_var: σ²_meas
    Returns:
        [new_mean, new_var]
    """
    combined_mean = (meas_var * prior_mean + prior_var * meas_value) / (prior_var + meas_var)
    combined_var = 1 / (1 / prior_var + 1 / meas_var)
    return [combined_mean, combined_var]

def predict_state(current_mean, current_var, motion_change, motion_var):
    """Predict next state based on motion model.
    Args:
        motion_change: u (expected movement)
        motion_var: σ²_motion (uncertainty of motion)
    """
    predicted_mean = current_mean + motion_change
    predicted_var = current_var + motion_var
    return [predicted_mean, predicted_var]

# Simulation data
sensor_readings = [5.0, 6.0, 8.0, 9.0]
motions_applied = [1.0, 2.0, 2.0, 1.0]
sensor_noise = 3.0   # measurement variance
motion_uncertainty = 2.0
estimated_position = 0.0
position_uncertainty = 500.0

print("=== Start 1‑D Kalman Filter ===")
for i in range(len(sensor_readings)):
    estimated_position, position_uncertainty = update_belief(
        estimated_position, position_uncertainty, sensor_readings[i], sensor_noise)
    print(f"Update {i+1}: position={estimated_position:.2f}, uncertainty={position_uncertainty:.2f}")
    estimated_position, position_uncertainty = predict_state(
        estimated_position, position_uncertainty, motions_applied[i], motion_uncertainty)
    print(f"Predict {i+1}: position={estimated_position:.2f}, uncertainty={position_uncertainty:.2f}")
print("=== Final estimate ===")
print(f"position={estimated_position:.2f} ± {position_uncertainty**0.5:.2f} (std dev)")

Running the script shows that, despite starting with a huge uncertainty (variance = 500), the estimate quickly converges toward the true trajectory and the variance shrinks to a single‑digit value.

Extending to Multiple Dimensions

Real‑world problems rarely involve a single scalar. For a robot moving on a plane, the state becomes a vector x = [position_x, position_y, velocity_x, velocity_y]ᵀ and uncertainty is represented by a covariance matrix P, which captures both variances and correlations.

State vector: x (e.g., position and velocity)

Covariance matrix: P (uncertainty and correlation)

State‑transition matrix: F (how the state evolves)

Control input: u (external motion command)

Measurement matrix: H (maps state to sensor space)

Measurement noise covariance: R Identity matrix: I (used in the update step)

The core equations (shown here without derivation) are:

Prediction:

x' = F @ x + u

P' = F @ P @ F.T

Update:

y = z - H @ x          # innovation (measurement residual)

S = H @ P @ H.T + R   # innovation covariance

K = P @ H.T @ inv(S)   # Kalman gain

x = x + K @ y          # state update

P = (I - K @ H) @ P   # covariance update

Code: 2‑D Kalman Filter (Position + Velocity)

import numpy as np

class KalmanFilter2D:
    """Simple 2‑D Kalman filter estimating position and velocity."""
    def __init__(self, dt=1.0):
        self.dt = dt
        # State vector [x, y, vx, vy]ᵀ
        self.x = np.zeros((4, 1))
        # Large initial uncertainty
        self.P = np.eye(4) * 1000
        # Constant‑velocity motion model
        self.F = np.array([[1, 0, dt, 0],
                           [0, 1, 0, dt],
                           [0, 0, 1, 0],
                           [0, 0, 0, 1]])
        # We only measure position
        self.H = np.array([[1, 0, 0, 0],
                           [0, 1, 0, 0]])
        self.R = np.eye(2) * 1.0   # measurement noise
        self.Q = np.eye(4) * 0.01  # process noise
        self.I = np.eye(4)

    def predict(self):
        self.x = self.F @ self.x
        self.P = self.F @ self.P @ self.F.T + self.Q
        return self.x[:2].flatten()

    def update(self, z):
        z = np.array(z).reshape(2, 1)
        y = z - self.H @ self.x                     # innovation
        S = self.H @ self.P @ self.H.T + self.R      # innovation covariance
        K = self.P @ self.H.T @ np.linalg.inv(S)    # Kalman gain
        self.x = self.x + K @ y                     # state update
        self.P = (self.I - K @ self.H) @ self.P      # covariance update
        return self.x.flatten()

if __name__ == "__main__":
    kf = KalmanFilter2D(dt=1.0)
    # Simulated true trajectory (constant velocity)
    true_states = []
    for t in range(10):
        true_states.append([t*2.0, t*1.0, 2.0, 1.0])
    np.random.seed(42)
    measurements = []
    for tx, ty, _, _ in true_states:
        noisy_x = tx + np.random.randn()*1.5
        noisy_y = ty + np.random.randn()*1.5
        measurements.append([noisy_x, noisy_y])
    print("=== 2‑D Kalman Filter Demo ===
")
    print("time | true (x,y) | meas (x,y) | est (x,y) | est (vx,vy)")
    print("-"*70)
    for i, meas in enumerate(measurements):
        kf.predict()
        est = kf.update(meas)
        tx, ty = true_states[i][0], true_states[i][1]
        mx, my = meas
        ex, ey, evx, evy = est
        print(f"t={i:02d} | ({tx:5.1f},{ty:5.1f}) | ({mx:5.1f},{my:5.1f}) | ({ex:5.1f},{ey:5.1f}) | ({evx:4.1f},{evy:4.1f})")
    print("
=== Effect Analysis ===")
    print("Even with noisy position measurements, the filter produces a smooth, accurate trajectory and correctly infers velocity.")

The output demonstrates that the filter quickly locks onto the true path, reduces uncertainty, and recovers velocity even though the sensor only provides position.

Real‑World Applications

Apollo lunar navigation computers

GPS receivers that fuse satellite signals with inertial measurements

Autonomous driving stacks combining camera, radar, and LiDAR data

Drone stabilization using IMU and visual cues

Financial time‑series filtering for trend detection

Biomedical signal processing (EEG, ECG) to extract clean waveforms

Key Takeaways

Predict + Update: Both steps are essential; skipping either degrades performance.

Quantify Uncertainty: Covariance matrices explicitly express what the filter does not know.

Optimal Fusion: The Kalman gain automatically balances trust between prediction and measurement based on their uncertainties.

For deeper exploration, consider extensions such as the Extended Kalman Filter (EKF) for nonlinear systems, the Unscented Kalman Filter (UKF) for more accurate nonlinear handling, and Particle Filters for highly non‑Gaussian distributions.