Nonlinear Kalman Smoothers in Python: Extended RTS Smoother and Unscented RTS Smoother

Introduction

https://yuhi-sa.github.io/en/posts/20260704_kalman_smoother/1/ compared the RTS, fixed-lag, and fixed-point smoothers for linear Gaussian state-space models. That article ended by noting that the same three-way classification carries over to nonlinear systems, without implementing them. This article picks up where it left off: we derive and implement the Extended RTS Smoother (EKS) and Unscented RTS Smoother (URTS) — the backward passes corresponding to the https://yuhi-sa.github.io/en/posts/20260224_ekf/1/ and https://yuhi-sa.github.io/en/posts/20260226_ukf/1/ — and compare their accuracy on a strongly nonlinear benchmark.

Recap: EKF and UKF Forward Passes

For the nonlinear state-space model

the EKF linearizes \(f, h\) using Jacobians \(F_k, H_k\) , while the UKF numerically approximates the mean and covariance of the nonlinear transformation using sigma points (see https://yuhi-sa.github.io/en/posts/20260224_ekf/1/ and https://yuhi-sa.github.io/en/posts/20260226_ukf/1/ for details). Both produce, at each time step, a predicted mean/covariance \(m_k^-, P_k^-\) and an updated mean/covariance \(m_k, P_k\) using observations up to time \(k\) .

Extended RTS Smoother (EKS)

The linear RTS backward recursion

extends naturally to nonlinear systems by replacing the constant transition matrix \(F_{k+1}\) with the Jacobian computed by the EKF at each time step, \(F_{k+1} = \left.\frac{\partial f}{\partial x}\right|_{x=m_k}\) . Using the \(m_k, P_k, m_k^-, P_k^-, F_k\) values saved during the forward filtering pass, we compute the recursion backward from \(k=T-1\) to \(k=0\) .

Unscented RTS Smoother (URTS)

The Unscented RTS Smoother, derived by Särkkä (2008), replaces the Jacobian with a numerical cross-covariance computed from the same sigma points used during filtering. From the mean/covariance \(m_k, P_k\) at time \(k\) , we generate sigma points \(\mathcal{X}_{k}^{(i)}\) , propagate them through \(f\) to get \(\mathcal{X}_{k+1|k}^{(i)} = f(\mathcal{X}_k^{(i)})\) , and compute:

The gain \(G_k = C_{k+1} (P_{k+1}^-)^{-1}\) is then plugged into the same update equations as the linear RTS smoother. Where EKS relies on a local linear (Jacobian) approximation, URTS uses the actual statistics of the nonlinear transform as captured by the sigma points.

Python Implementation

Benchmark model

We use a classic strongly nonlinear benchmark (Kitagawa, 1987; also used in Julier & Uhlmann’s original UKF paper, 1997):

with \(Q=10,\ R=1\) . The state transition function has a sharp, peaked curvature near \(x=\pm 1\) .

import numpy as np

Q, R, T = 10.0, 1.0, 60


def f(x, k):
    return 0.5 * x + 25 * x / (1 + x**2) + 8 * np.cos(1.2 * k)


def fprime(x, k):
    return 0.5 + 25 * (1 - x**2) / (1 + x**2) ** 2


def h(x):
    return x**2 / 20.0


def hprime(x):
    return x / 10.0

EKF filter + EKS smoother

def ekf_filter(ys):
    m, P = np.zeros(T), np.zeros(T)
    m_pred, P_pred, F_hist = np.zeros(T), np.zeros(T), np.zeros(T)
    m[0], P[0] = 0.1, 5.0
    m_pred[0], P_pred[0] = m[0], P[0]
    for k in range(1, T):
        F = fprime(m[k - 1], k - 1)
        F_hist[k] = F
        m_pred[k] = f(m[k - 1], k - 1)
        P_pred[k] = F * P[k - 1] * F + Q
        Hk = hprime(m_pred[k])
        S = Hk * P_pred[k] * Hk + R
        K = P_pred[k] * Hk / S
        m[k] = m_pred[k] + K * (ys[k] - h(m_pred[k]))
        P[k] = (1 - K * Hk) * P_pred[k]
    return m, P, m_pred, P_pred, F_hist


def eks_smoother(m, P, m_pred, P_pred, F_hist):
    ms, Ps = m.copy(), P.copy()
    for k in range(T - 2, -1, -1):
        F = F_hist[k + 1]
        G = P[k] * F / P_pred[k + 1]
        ms[k] = m[k] + G * (ms[k + 1] - m_pred[k + 1])
        Ps[k] = P[k] + G * (Ps[k + 1] - P_pred[k + 1]) * G
    return ms, Ps

UKF filter + URTS smoother

def sigma_points(mean, cov, alpha=1.0, beta=2.0, kappa=2.0):
    n = 1
    lam = alpha**2 * (n + kappa) - n
    c = n + lam
    sqrt_c = np.sqrt(c * cov)
    pts = np.array([mean, mean + sqrt_c, mean - sqrt_c])
    wm = np.array([lam / c, 1 / (2 * c), 1 / (2 * c)])
    wc = wm.copy()
    wc[0] += 1 - alpha**2 + beta
    return pts, wm, wc


def ukf_filter(ys):
    m, P = np.zeros(T), np.zeros(T)
    m_pred, P_pred = np.zeros(T), np.zeros(T)
    sig_store = [None] * T
    m[0], P[0] = 0.1, 5.0
    m_pred[0], P_pred[0] = m[0], P[0]
    for k in range(1, T):
        pts, wm, wc = sigma_points(m[k - 1], P[k - 1])
        pts_pred = f(pts, k - 1)
        mp = np.sum(wm * pts_pred)
        Pp = np.sum(wc * (pts_pred - mp) ** 2) + Q
        m_pred[k], P_pred[k] = mp, Pp
        sig_store[k] = (pts, wc, pts_pred, mp)

        pts2, wm2, wc2 = sigma_points(mp, Pp)
        y_pred = h(pts2)
        y_mean = np.sum(wm2 * y_pred)
        Pyy = np.sum(wc2 * (y_pred - y_mean) ** 2) + R
        Pxy = np.sum(wc2 * (pts2 - mp) * (y_pred - y_mean))
        K = Pxy / Pyy
        m[k] = mp + K * (ys[k] - y_mean)
        P[k] = Pp - K * Pyy * K
    return m, P, m_pred, P_pred, sig_store


def urts_smoother(m, P, m_pred, P_pred, sig_store):
    ms, Ps = m.copy(), P.copy()
    for k in range(T - 2, -1, -1):
        pts, wc, pts_pred, mp = sig_store[k + 1]
        C = np.sum(wc * (pts - m[k]) * (pts_pred - mp))
        G = C / P_pred[k + 1]
        ms[k] = m[k] + G * (ms[k + 1] - m_pred[k + 1])
        Ps[k] = P[k] + G * (Ps[k + 1] - P_pred[k + 1]) * G
    return ms, Ps

We use alpha=1.0, beta=2.0, kappa=2.0. Using the commonly recommended default alpha=1e-3 (a very tight sigma-point spread, suited for high-dimensional systems) on this strongly nonlinear benchmark caused the sigma points to cluster too close to the mean to capture the nonlinearity, and the UKF diverged — we confirmed this directly (see the caveat section below).

Monte Carlo evaluation (200 trials)

rng = np.random.default_rng(0)


def simulate(x0=0.1):
    xs, ys = np.zeros(T), np.zeros(T)
    x = x0
    for k in range(T):
        if k > 0:
            x = f(x, k - 1) + rng.normal(0, np.sqrt(Q))
        xs[k] = x
        ys[k] = h(x) + rng.normal(0, np.sqrt(R))
    return xs, ys


rmse = lambda a, b: np.sqrt(np.mean((a - b) ** 2))
res = {"ekf": [], "eks": [], "ukf": [], "uks": []}
for _ in range(200):
    xs, ys = simulate()
    m, P, m_pred, P_pred, F_hist = ekf_filter(ys)
    ms, Ps = eks_smoother(m, P, m_pred, P_pred, F_hist)
    mu, Pu, mu_pred, Pu_pred, sig_store = ukf_filter(ys)
    msu, Psu = urts_smoother(mu, Pu, mu_pred, Pu_pred, sig_store)
    res["ekf"].append(rmse(m, xs))
    res["eks"].append(rmse(ms, xs))
    res["ukf"].append(rmse(mu, xs))
    res["uks"].append(rmse(msu, xs))

for k, v in res.items():
    print(f"{k}: mean RMSE = {np.mean(v):.4f} (std {np.std(v):.4f})")

Results (200-trial Monte Carlo, mean RMSE):

Two patterns emerge:

1. UKF substantially outperforms EKF (RMSE 8.88 vs. 19.39). Near \(x=\pm1\) , \(\frac{25x}{1+x^2}\) has very high curvature, so the EKF’s first-order Jacobian linearization breaks down easily, while the UKF’s sigma points capture the actual nonlinear transformation numerically and remain robust.
2. Both smoothers consistently improve on their filters (EKF→EKS reduces RMSE by ≈7.9%, UKF→URTS by ≈7.0%). This confirms that the improvement pattern observed for linear Gaussian systems in https://yuhi-sa.github.io/en/posts/20260704_kalman_smoother/1/ also holds under strong nonlinearity.

Caveat: Choosing Sigma-Point Parameters

Setting alpha too small (e.g., alpha=1e-3) makes the sigma points cluster very close to the mean, which prevents them from sampling the strongly nonlinear region and causes the UKF to diverge — we observed this directly on this benchmark. alpha needs to be tuned to the state dimension and the strength of the nonlinearity: smaller values work well for high-dimensional real systems, while a low-dimensional, strongly nonlinear system like this benchmark is more stable with a larger value (around 1.0).

Summary

1. The Extended RTS Smoother (EKS) is a straightforward extension that plugs the EKF’s Jacobian into the same recursion as the linear RTS smoother.
2. The Unscented RTS Smoother (URTS) achieves nonlinear smoothing without any Jacobian, using cross-covariances computed from sigma points.
3. On the strongly nonlinear Kitagawa benchmark, a 200-trial Monte Carlo study showed the UKF family (RMSE 8.88 → 8.26) substantially outperforming the EKF family (19.39 → 17.86), with both smoothers consistently improving over their forward filters.
4. The sigma-point parameter alpha must be tuned to the strength of the nonlinearity — too small a value causes the UKF to diverge.

Related Articles

• https://yuhi-sa.github.io/en/posts/20260704_kalman_smoother/1/ — RTS, fixed-lag, and fixed-point smoother comparison for linear Gaussian systems; the foundational article for this one
• https://yuhi-sa.github.io/en/posts/20260224_ekf/1/ — Extended Kalman Filter theory and Jacobian derivation
• https://yuhi-sa.github.io/en/posts/20260226_ukf/1/ — Unscented Kalman Filter and sigma-point theory
• https://yuhi-sa.github.io/en/posts/20260223_rts_smoother/1/ — Fundamentals of the forward-backward recursion for the linear RTS smoother
• https://yuhi-sa.github.io/en/posts/20260223_particle_filter/1/ — Particle filters and particle smoothers for non-Gaussian systems

References

• Särkkä, S. (2008). “Unscented Rauch-Tung-Striebel Smoother.” IEEE Transactions on Automatic Control, 53(3), 845-849.
• Särkkä, S. (2013). Bayesian Filtering and Smoothing. Cambridge University Press.
• Julier, S. J., & Uhlmann, J. K. (1997). “New extension of the Kalman filter to nonlinear systems.” Proceedings of SPIE, 3068.
• Kitagawa, G. (1987). “Non-Gaussian state-space modeling of nonstationary time series.” Journal of the American Statistical Association, 82(400), 1032-1041.