Nonlinear Filtering Tutorial

This tutorial covers Extended Kalman Filter (EKF) and Unscented Kalman Filter (UKF) for tracking with nonlinear measurements.

Problem Setup

We track an object in 2D using range and bearing measurements from a sensor at the origin. The measurement model is nonlinear:

\[\begin{split}h(x) = \begin{bmatrix} \sqrt{x^2 + y^2} \\ \arctan(y/x) \end{bmatrix}\end{split}\]

Extended Kalman Filter

The EKF linearizes the nonlinear functions using Jacobians.

import numpy as np
from pytcl.dynamic_estimation import ekf_predict, ekf_update

# State: [x, vx, y, vy]
dt = 0.1

# Linear dynamics (constant velocity)
def f(x):
    F = np.array([[1, dt, 0, 0], [0, 1, 0, 0],
                  [0, 0, 1, dt], [0, 0, 0, 1]])
    return F @ x

def F_jacobian(x):
    return np.array([[1, dt, 0, 0], [0, 1, 0, 0],
                     [0, 0, 1, dt], [0, 0, 0, 1]])

# Nonlinear measurement (range, bearing)
def h(x):
    r = np.sqrt(x[0]**2 + x[2]**2)
    theta = np.arctan2(x[2], x[0])
    return np.array([r, theta])

def H_jacobian(x):
    r = np.sqrt(x[0]**2 + x[2]**2)
    return np.array([
        [x[0]/r, 0, x[2]/r, 0],
        [-x[2]/r**2, 0, x[0]/r**2, 0]
    ])

# Filter parameters
Q = np.diag([0.01, 0.1, 0.01, 0.1])
R = np.diag([0.5, 0.01])  # Range (m), bearing (rad)

# Initialize
x = np.array([100.0, -5.0, 50.0, 2.0])
P = np.diag([10.0, 1.0, 10.0, 1.0])

# Filter step - compute Jacobians at current/predicted state
F = F_jacobian(x)  # Evaluate Jacobian at current state
pred = ekf_predict(x, P, f, F, Q)
z = np.array([112.0, 0.47])  # Measurement
H = H_jacobian(pred.x)  # Evaluate Jacobian at predicted state
upd = ekf_update(pred.x, pred.P, z, h, H, R)

Automatic Jacobian Computation

If analytical Jacobians are difficult to derive, use numerical differentiation:

from pytcl.dynamic_estimation import ekf_predict_auto, ekf_update_auto

# Same f and h functions, no Jacobians needed
pred = ekf_predict_auto(x, P, f, Q)
upd = ekf_update_auto(pred.x, pred.P, z, h, R)

Unscented Kalman Filter

The UKF uses sigma points to propagate uncertainty through nonlinear functions, avoiding Jacobian computation entirely.

from pytcl.dynamic_estimation import ukf_predict, ukf_update

# Same f and h functions
pred = ukf_predict(x, P, f, Q)
upd = ukf_update(pred.x, pred.P, z, h, R)

The UKF is often more accurate than the EKF for highly nonlinear systems.

Cubature Kalman Filter

The CKF uses spherical-radial cubature points, providing a good balance between accuracy and computational cost:

from pytcl.dynamic_estimation import ckf_predict, ckf_update

pred = ckf_predict(x, P, f, Q)
upd = ckf_update(pred.x, pred.P, z, h, R)

Complete Tracking Example

Here is a complete range-bearing tracking example comparing EKF and UKF:

import numpy as np
from pytcl.dynamic_estimation import (
    ekf_predict, ekf_update,
    ukf_predict, ukf_update
)

# Setup
dt = 0.1
n_steps = 100
np.random.seed(42)

def f(x):
    F = np.array([[1, dt, 0, 0], [0, 1, 0, 0],
                  [0, 0, 1, dt], [0, 0, 0, 1]])
    return F @ x

def F_jac(x):
    return np.array([[1, dt, 0, 0], [0, 1, 0, 0],
                     [0, 0, 1, dt], [0, 0, 0, 1]])

def h(x):
    r = np.sqrt(x[0]**2 + x[2]**2)
    theta = np.arctan2(x[2], x[0])
    return np.array([r, theta])

def H_jac(x):
    r = np.sqrt(x[0]**2 + x[2]**2)
    return np.array([[x[0]/r, 0, x[2]/r, 0],
                     [-x[2]/r**2, 0, x[0]/r**2, 0]])

Q = np.diag([0.01, 0.1, 0.01, 0.1])
R = np.diag([1.0, 0.02])

# Generate truth and measurements
x_true = np.array([100.0, -5.0, 50.0, 2.0])
truth, measurements = [], []
for _ in range(n_steps):
    truth.append(x_true.copy())
    z_true = h(x_true)
    z_noisy = z_true + np.random.multivariate_normal(np.zeros(2), R)
    measurements.append(z_noisy)
    x_true = f(x_true) + np.random.multivariate_normal(np.zeros(4), Q)

# Run EKF
x_ekf = np.array([100.0, -5.0, 50.0, 2.0])
P_ekf = np.diag([10.0, 1.0, 10.0, 1.0])
ekf_est = []
for z in measurements:
    F = F_jac(x_ekf)  # Evaluate Jacobian at current state
    pred = ekf_predict(x_ekf, P_ekf, f, F, Q)
    H = H_jac(pred.x)  # Evaluate Jacobian at predicted state
    upd = ekf_update(pred.x, pred.P, z, h, H, R)
    x_ekf, P_ekf = upd.x, upd.P
    ekf_est.append(x_ekf.copy())

# Run UKF
x_ukf = np.array([100.0, -5.0, 50.0, 2.0])
P_ukf = np.diag([10.0, 1.0, 10.0, 1.0])
ukf_est = []
for z in measurements:
    pred = ukf_predict(x_ukf, P_ukf, f, Q)
    upd = ukf_update(pred.x, pred.P, z, h, R)
    x_ukf, P_ukf = upd.x, upd.P
    ukf_est.append(x_ukf.copy())

# Compare
truth = np.array(truth)
ekf_est = np.array(ekf_est)
ukf_est = np.array(ukf_est)

ekf_rmse = np.sqrt(np.mean((truth[:, 0] - ekf_est[:, 0])**2 +
                           (truth[:, 2] - ekf_est[:, 2])**2))
ukf_rmse = np.sqrt(np.mean((truth[:, 0] - ukf_est[:, 0])**2 +
                           (truth[:, 2] - ukf_est[:, 2])**2))

print(f"EKF Position RMSE: {ekf_rmse:.3f}")
print(f"UKF Position RMSE: {ukf_rmse:.3f}")

Filter Selection Guidelines

  • EKF: Use when you have analytical Jacobians and moderate nonlinearity

  • UKF: Use for highly nonlinear systems or when Jacobians are unavailable

  • CKF: Good compromise between EKF and UKF computational cost

Next Steps