Introduction
The Kalman filter is optimal for linear Gaussian systems but cannot be directly applied to nonlinear systems. The Extended Kalman Filter (EKF) uses Jacobian-based linearization, but accuracy degrades with high nonlinearity.
The Unscented Kalman Filter (UKF) passes a small set of sigma points through the nonlinear function, achieving high-accuracy estimation without computing Jacobians.
Unscented Transform
Sigma Point Generation
For an \(n\)-dimensional state \(\mathbf{x}\) (mean \(\bar{\mathbf{x}}\), covariance \(\mathbf{P}\)), generate \(2n+1\) sigma points:
\[\boldsymbol{\chi}_0 = \bar{\mathbf{x}} \tag{1}\]\[\boldsymbol{\chi}_i = \bar{\mathbf{x}} + \left(\sqrt{(n + \lambda) \mathbf{P}}\right)_i, \quad i = 1, \ldots, n \tag{2}\]\[\boldsymbol{\chi}_{i+n} = \bar{\mathbf{x}} - \left(\sqrt{(n + \lambda) \mathbf{P}}\right)_i, \quad i = 1, \ldots, n \tag{3}\]where \(\lambda = \alpha^2(n + \kappa) - n\) is a scaling parameter.
Weights
\[W_0^{(m)} = \frac{\lambda}{n + \lambda}, \quad W_0^{(c)} = \frac{\lambda}{n + \lambda} + (1 - \alpha^2 + \beta) \tag{4}\]\[W_i^{(m)} = W_i^{(c)} = \frac{1}{2(n + \lambda)}, \quad i = 1, \ldots, 2n \tag{5}\]Typical parameters: \(\alpha = 10^{-3}\), \(\beta = 2\) (optimal for Gaussian), \(\kappa = 0\).
Transformed Statistics
After passing sigma points through nonlinear function \(f\):
\[\boldsymbol{\mathcal{Y}}_i = f(\boldsymbol{\chi}_i) \tag{6}\]\[\bar{\mathbf{y}} = \sum_{i=0}^{2n} W_i^{(m)} \boldsymbol{\mathcal{Y}}_i \tag{7}\]\[\mathbf{P}_y = \sum_{i=0}^{2n} W_i^{(c)} (\boldsymbol{\mathcal{Y}}_i - \bar{\mathbf{y}})(\boldsymbol{\mathcal{Y}}_i - \bar{\mathbf{y}})^T \tag{8}\]UKF Algorithm
Predict Step
- Generate sigma points from \((\hat{\mathbf{x}}_{k-1}, \mathbf{P}_{k-1})\)
- Propagate through state transition: \(\boldsymbol{\chi}_{k|k-1} = f(\boldsymbol{\chi}_{k-1})\)
- Compute predicted mean and covariance:
Update Step
- Transform predicted sigma points through observation function \(h\)
- Compute Kalman gain:
- Update state and covariance:
EKF vs UKF
| Feature | EKF | UKF |
|---|---|---|
| Linearization | Jacobian (1st order) | Sigma points (2nd order accuracy) |
| Jacobian required | Yes | No |
| Nonlinear accuracy | Moderate | High |
| Computational cost | \(O(n^2)\) | \(O(n^3)\) (Cholesky decomposition) |
Python Implementation
Tracking a circular motion in 2D:
import numpy as np
import matplotlib.pyplot as plt
class UKF:
def __init__(self, n, m, f, h, Q, R, alpha=1e-3, beta=2, kappa=0):
self.n = n
self.m = m
self.f = f
self.h = h
self.Q = Q
self.R = R
self.lam = alpha**2 * (n + kappa) - n
self.gamma = np.sqrt(n + self.lam)
self.Wm = np.full(2 * n + 1, 1 / (2 * (n + self.lam)))
self.Wc = np.full(2 * n + 1, 1 / (2 * (n + self.lam)))
self.Wm[0] = self.lam / (n + self.lam)
self.Wc[0] = self.lam / (n + self.lam) + (1 - alpha**2 + beta)
def sigma_points(self, x, P):
L = np.linalg.cholesky(P)
sigmas = np.zeros((2 * self.n + 1, self.n))
sigmas[0] = x
for i in range(self.n):
sigmas[i + 1] = x + self.gamma * L[:, i]
sigmas[i + 1 + self.n] = x - self.gamma * L[:, i]
return sigmas
def predict(self, x, P, dt):
sigmas = self.sigma_points(x, P)
sigmas_pred = np.array([self.f(s, dt) for s in sigmas])
x_pred = np.dot(self.Wm, sigmas_pred)
P_pred = self.Q.copy()
for i in range(2 * self.n + 1):
d = sigmas_pred[i] - x_pred
P_pred += self.Wc[i] * np.outer(d, d)
return x_pred, P_pred, sigmas_pred
def update(self, x_pred, P_pred, sigmas_pred, z):
sigmas_z = np.array([self.h(s) for s in sigmas_pred])
z_pred = np.dot(self.Wm, sigmas_z)
Pzz = self.R.copy()
Pxz = np.zeros((self.n, self.m))
for i in range(2 * self.n + 1):
dz = sigmas_z[i] - z_pred
dx = sigmas_pred[i] - x_pred
Pzz += self.Wc[i] * np.outer(dz, dz)
Pxz += self.Wc[i] * np.outer(dx, dz)
K = Pxz @ np.linalg.inv(Pzz)
x_new = x_pred + K @ (z - z_pred)
P_new = P_pred - K @ Pzz @ K.T
return x_new, P_new
# --- Circular motion model ---
def state_transition(x, dt):
px, py, vx, vy = x
return np.array([px + vx * dt, py + vy * dt, vx, vy])
def observation(x):
return np.array([x[0], x[1]])
# --- Simulation ---
np.random.seed(42)
dt = 0.1
T = 10
steps = int(T / dt)
omega = 0.5
true_states = np.zeros((steps, 4))
true_states[0] = [5, 0, 0, 5 * omega]
for k in range(1, steps):
t = k * dt
true_states[k] = [5 * np.cos(omega * t), 5 * np.sin(omega * t),
-5 * omega * np.sin(omega * t),
5 * omega * np.cos(omega * t)]
R = np.diag([0.5**2, 0.5**2])
measurements = true_states[:, :2] + np.random.multivariate_normal(
[0, 0], R, steps)
Q = np.diag([0.01, 0.01, 0.1, 0.1])
ukf = UKF(n=4, m=2, f=state_transition, h=observation, Q=Q, R=R)
x_est = np.array([measurements[0, 0], measurements[0, 1], 0, 2.5])
P_est = np.eye(4)
estimates = [x_est.copy()]
for k in range(1, steps):
x_pred, P_pred, sigmas_pred = ukf.predict(x_est, P_est, dt)
x_est, P_est = ukf.update(x_pred, P_pred, sigmas_pred, measurements[k])
estimates.append(x_est.copy())
estimates = np.array(estimates)
# --- Visualization ---
plt.figure(figsize=(8, 8))
plt.plot(true_states[:, 0], true_states[:, 1], 'b-', linewidth=2,
label='True')
plt.scatter(measurements[:, 0], measurements[:, 1], c='gray', s=10,
alpha=0.3, label='Measurements')
plt.plot(estimates[:, 0], estimates[:, 1], 'r--', linewidth=1.5,
label='UKF estimate')
plt.xlabel('x')
plt.ylabel('y')
plt.title('UKF: Circular Motion Tracking')
plt.legend()
plt.axis('equal')
plt.grid(True, alpha=0.3)
plt.tight_layout()
plt.show()
Related Articles
- Particle Filter Python Implementation - For higher nonlinearity, particle filters are more suitable.
- Unscented Transform Fundamentals - Detailed explanation of the core Unscented Transform.
- Wiener Filter - Optimal linear filter for stationary processes; contrasts with UKF’s role.
- Frequency Characteristics of the EMA Filter - Compare the simplest filter with UKF’s sophistication.
References
- Julier, S. J., & Uhlmann, J. K. (2004). “Unscented Filtering and Nonlinear Estimation”. Proceedings of the IEEE, 92(3), 401-422.
- Wan, E. A., & van der Merwe, R. (2000). “The Unscented Kalman Filter for Nonlinear Estimation”. Proceedings of the IEEE Adaptive Systems for Signal Processing, Communications, and Control Symposium.
- Simon, D. (2006). Optimal State Estimation. Wiley. Chapter 14.