What is the Cubature Kalman Filter?
In the article on Unscented Transformation (UT), we introduced a method for efficiently estimating the mean and covariance of nonlinearly transformed random variables using sigma points. The Unscented Kalman Filter (UKF) applies this UT to the prediction and update steps of a Kalman filter.
However, UKF has the following drawbacks:
- Requires tuning parameters \(\alpha, \beta, \kappa\)
- In high dimensions, weights can become negative, compromising the positive definiteness of the covariance matrix
The Cubature Kalman Filter (CKF) solves these problems by selecting sigma points (cubature points) based on the spherical-radial cubature rule. CKF requires no parameter tuning, and all weights are positive, ensuring numerical stability.
Reference: Arasaratnam, I., & Haykin, S. (2009). “Cubature Kalman Filters.” IEEE Transactions on Automatic Control, 54(6), 1254-1269.
Theory
Gaussian-Weighted Integrals
In nonlinear filtering, we need to compute Gaussian-weighted integrals of the form:
\[ I = \int_{\mathbb{R}^n} f(\mathbf{x}) \cdot \mathcal{N}(\mathbf{x}; \boldsymbol{\mu}, \mathbf{P}) \, d\mathbf{x} \]Using the substitution \(\mathbf{x} = \sqrt{\mathbf{P}} \mathbf{z} + \boldsymbol{\mu}\), this becomes an integral over the standard normal distribution:
\[ I = \int_{\mathbb{R}^n} f(\sqrt{\mathbf{P}} \mathbf{z} + \boldsymbol{\mu}) \cdot \mathcal{N}(\mathbf{z}; \mathbf{0}, \mathbf{I}) \, d\mathbf{z} \]Spherical-Radial Cubature Rule
The integral over the standard normal distribution is decomposed into spherical and radial components. Setting \(\mathbf{z} = r \mathbf{s}\) (where \(r = \|\mathbf{z}\|\) and \(\mathbf{s} = \mathbf{z}/\|\mathbf{z}\|\)):
\[ I = \int_0^{\infty} \int_{U_n} f(r \mathbf{s}) \cdot r^{n-1} e^{-r^2/2} \, d\sigma(\mathbf{s}) \, dr \cdot \frac{1}{(2\pi)^{n/2}} \]where \(U_n\) is the \(n\)-dimensional unit sphere and \(d\sigma(\mathbf{s})\) is the surface area element.
The third-degree cubature rule uses \(2n\) points on the sphere:
\[ \boldsymbol{\xi}_i = \sqrt{n} \, \mathbf{e}_i, \quad i = 1, \ldots, 2n \]where \(\mathbf{e}_i\) are defined as:
\[ \mathbf{e}_i = \begin{cases} \text{the } i\text{-th unit vector} & i = 1, \ldots, n \\\ -\text{the } (i-n)\text{-th unit vector} & i = n+1, \ldots, 2n \end{cases} \]All cubature point weights are equal:
\[ w_i = \frac{1}{2n}, \quad i = 1, \ldots, 2n \tag{1} \]Cubature Point Generation
Given the mean \(\boldsymbol{\mu}\) and covariance \(\mathbf{P}\) of the state \(\mathbf{x}\), cubature points are computed as:
\[ \mathbf{X}_i = \sqrt{\mathbf{P}} \, \boldsymbol{\xi}_i + \boldsymbol{\mu}, \quad i = 1, \ldots, 2n \tag{2} \]where \(\sqrt{\mathbf{P}}\) is the matrix square root via Cholesky decomposition.
Comparison with UKF
| UKF | CKF | |
|---|---|---|
| Number of points | \(2n + 1\) | \(2n\) |
| Parameters | \(\alpha, \beta, \kappa\) | None |
| Weights | Can be negative | All positive (\(1/2n\)) |
| Theoretical basis | Heuristic | Cubature rule |
CKF can also be interpreted as a special case of UKF with \(\alpha=1, \beta=0, \kappa=0\).
CKF Algorithm
State-space model:
\[ \mathbf{x}_k = f(\mathbf{x}_{k-1}) + \mathbf{w}_{k-1}, \quad \mathbf{w}_{k-1} \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}) \]\[ \mathbf{y}_k = h(\mathbf{x}_k) + \mathbf{v}_k, \quad \mathbf{v}_k \sim \mathcal{N}(\mathbf{0}, \mathbf{R}) \]Prediction Step
1. Generate cubature points:
\[ \mathbf{X}_{i,k-1} = \sqrt{\mathbf{P}_{k-1}} \, \boldsymbol{\xi}_i + \hat{\mathbf{x}}_{k-1}, \quad i = 1, \ldots, 2n \tag{3} \]2. Propagate cubature points:
\[ \mathbf{X}_{i,k|k-1}^{*} = f(\mathbf{X}_{i,k-1}) \tag{4} \]3. Predicted mean:
\[ \hat{\mathbf{x}}_{k|k-1} = \frac{1}{2n} \sum_{i=1}^{2n} \mathbf{X}_{i,k|k-1}^{*} \tag{5} \]4. Predicted covariance:
\[ \mathbf{P}_{k|k-1} = \frac{1}{2n} \sum_{i=1}^{2n} \mathbf{X}_{i,k|k-1}^{*} (\mathbf{X}_{i,k|k-1}^{*})^T - \hat{\mathbf{x}}_{k|k-1} \hat{\mathbf{x}}_{k|k-1}^T + \mathbf{Q} \tag{6} \]Update Step
5. Regenerate cubature points:
\[ \mathbf{X}_{i,k|k-1} = \sqrt{\mathbf{P}_{k|k-1}} \, \boldsymbol{\xi}_i + \hat{\mathbf{x}}_{k|k-1}, \quad i = 1, \ldots, 2n \tag{7} \]6. Observation cubature points:
\[ \mathbf{Y}_{i,k|k-1} = h(\mathbf{X}_{i,k|k-1}) \tag{8} \]7. Predicted observation mean:
\[ \hat{\mathbf{y}}_{k|k-1} = \frac{1}{2n} \sum_{i=1}^{2n} \mathbf{Y}_{i,k|k-1} \tag{9} \]8. Innovation covariance and cross-covariance:
\[ \mathbf{S}_k = \frac{1}{2n} \sum_{i=1}^{2n} \mathbf{Y}_{i,k|k-1} \mathbf{Y}_{i,k|k-1}^T - \hat{\mathbf{y}}_{k|k-1} \hat{\mathbf{y}}_{k|k-1}^T + \mathbf{R} \tag{10} \]\[ \mathbf{C}_k = \frac{1}{2n} \sum_{i=1}^{2n} \mathbf{X}_{i,k|k-1} \mathbf{Y}_{i,k|k-1}^T - \hat{\mathbf{x}}_{k|k-1} \hat{\mathbf{y}}_{k|k-1}^T \tag{11} \]9. Kalman gain and state update:
\[ \mathbf{K}_k = \mathbf{C}_k \mathbf{S}_k^{-1} \tag{12} \]\[ \hat{\mathbf{x}}_k = \hat{\mathbf{x}}_{k|k-1} + \mathbf{K}_k (\mathbf{y}_k - \hat{\mathbf{y}}_{k|k-1}) \tag{13} \]\[ \mathbf{P}_k = \mathbf{P}_{k|k-1} - \mathbf{K}_k \mathbf{S}_k \mathbf{K}_k^T \tag{14} \]Python Implementation
We implement CKF using a coordinated turn model in 2D.
State-Space Model Definition
The state vector is \(\mathbf{x} = [p_x, p_y, v, \theta]^T\) (position, velocity, heading angle) with the following nonlinear transition model:
\[ f(\mathbf{x}) = \begin{bmatrix} p_x + \frac{v}{\omega}\sin(\theta + \omega \Delta t) - \frac{v}{\omega}\sin\theta \\\ p_y - \frac{v}{\omega}\cos(\theta + \omega \Delta t) + \frac{v}{\omega}\cos\theta \\\ v \\\ \theta + \omega \Delta t \end{bmatrix} \]The observation model assumes only the position is measurable: \(h(\mathbf{x}) = [p_x, p_y]^T\).
import numpy as np
import scipy.linalg
import matplotlib.pyplot as plt
# ---- State-space model definition ----
dt = 1.0
omega = 0.05 # turn rate [rad/s]
def f(x):
"""Nonlinear state transition function (coordinated turn model)"""
px, py, v, theta = x
if abs(omega) < 1e-6:
return np.array([px + v * np.cos(theta) * dt,
py + v * np.sin(theta) * dt,
v, theta])
return np.array([
px + v / omega * (np.sin(theta + omega * dt) - np.sin(theta)),
py - v / omega * (np.cos(theta + omega * dt) - np.cos(theta)),
v,
theta + omega * dt
])
def h(x):
"""Observation function (position only)"""
return np.array([x[0], x[1]])
n = 4 # state dimension
m = 2 # observation dimension
# Process noise and observation noise
Q = np.diag([0.1, 0.1, 0.01, 0.001])
R = np.diag([1.0, 1.0])
CKF Implementation
def generate_cubature_points(mu, P, n):
"""Generate cubature points (Eq. 2)"""
num_points = 2 * n
sqrt_P = scipy.linalg.cholesky(n * P, lower=True)
points = np.zeros((n, num_points))
for i in range(n):
points[:, i] = mu + sqrt_P[:, i] # +sqrt(nP) columns
points[:, n + i] = mu - sqrt_P[:, i] # -sqrt(nP) columns
return points
def ckf_predict(x_est, P_est):
"""CKF prediction step"""
# Eq. 3: Generate cubature points
X = generate_cubature_points(x_est, P_est, n)
num_points = 2 * n
w = 1.0 / num_points # Eq. 1: equal weights
# Eq. 4: Nonlinear transformation
X_pred = np.zeros_like(X)
for i in range(num_points):
X_pred[:, i] = f(X[:, i])
# Eq. 5: Predicted mean
x_pred = w * np.sum(X_pred, axis=1)
# Eq. 6: Predicted covariance
P_pred = np.zeros((n, n))
for i in range(num_points):
diff = X_pred[:, i] - x_pred
P_pred += w * np.outer(diff, diff)
P_pred += Q
return x_pred, P_pred
def ckf_update(x_pred, P_pred, y):
"""CKF update step"""
# Eq. 7: Regenerate cubature points
X = generate_cubature_points(x_pred, P_pred, n)
num_points = 2 * n
w = 1.0 / num_points
# Eq. 8: Observation cubature points
Y = np.zeros((m, num_points))
for i in range(num_points):
Y[:, i] = h(X[:, i])
# Eq. 9: Predicted observation mean
y_pred = w * np.sum(Y, axis=1)
# Eq. 10: Innovation covariance
S = np.zeros((m, m))
for i in range(num_points):
dy = Y[:, i] - y_pred
S += w * np.outer(dy, dy)
S += R
# Eq. 11: Cross-covariance
C = np.zeros((n, m))
for i in range(num_points):
dx = X[:, i] - x_pred
dy = Y[:, i] - y_pred
C += w * np.outer(dx, dy)
# Eq. 12-14: Kalman gain and state update
K = C @ np.linalg.inv(S)
x_est = x_pred + K @ (y - y_pred)
P_est = P_pred - K @ S @ K.T
return x_est, P_est
Simulation
np.random.seed(42)
T = 100 # number of time steps
# True initial state
x_true = np.array([0.0, 0.0, 1.0, np.pi / 2])
# CKF initial estimate
x_est = np.array([0.5, -0.5, 0.8, np.pi / 2 + 0.1])
P_est = np.diag([1.0, 1.0, 0.5, 0.1])
# Storage arrays
true_states = [x_true.copy()]
measurements = []
estimates = [x_est.copy()]
for k in range(T):
# Update true state
x_true = f(x_true) + np.random.multivariate_normal(np.zeros(n), Q)
true_states.append(x_true.copy())
# Generate measurement
y = h(x_true) + np.random.multivariate_normal(np.zeros(m), R)
measurements.append(y.copy())
# CKF
x_est, P_est = ckf_predict(x_est, P_est)
x_est, P_est = ckf_update(x_est, P_est, y)
estimates.append(x_est.copy())
true_states = np.array(true_states)
measurements = np.array(measurements)
estimates = np.array(estimates)
# ---- Plot results ----
plt.figure(figsize=(10, 8))
plt.plot(true_states[:, 0], true_states[:, 1], "b-", label="True trajectory")
plt.scatter(measurements[:, 0], measurements[:, 1],
c="gray", s=10, alpha=0.5, label="Measurements")
plt.plot(estimates[:, 0], estimates[:, 1], "r--", label="CKF estimate")
plt.xlabel("x")
plt.ylabel("y")
plt.title("Cubature Kalman Filter - Coordinated Turn Tracking")
plt.legend()
plt.axis("equal")
plt.grid(True)
plt.tight_layout()
plt.savefig("ckf_result.png", dpi=150)
plt.show()
# RMSE
rmse_x = np.sqrt(np.mean((true_states[1:, 0] - estimates[1:, 0])**2))
rmse_y = np.sqrt(np.mean((true_states[1:, 1] - estimates[1:, 1])**2))
print(f"Position RMSE: x={rmse_x:.4f}, y={rmse_y:.4f}")
CKF achieves stable estimation without any parameter tuning, making it particularly advantageous for high-dimensional problems where UKF may encounter numerical issues.
References
- Arasaratnam, I., & Haykin, S. (2009). “Cubature Kalman Filters.” IEEE Transactions on Automatic Control, 54(6), 1254-1269.
- Katayama, T. (2011). “Basics of Nonlinear Kalman Filters.” Journal of SICE, 50(9), 638-643.