---
title: Extended Kalman Filter with RTS Smoothing
url: https://www.emergentmind.com/topics/extended-kalman-filter-with-rauch-tung-striebel-smoothing
type: topic
---

# Extended Kalman Filter with RTS Smoothing

The Extended Kalman Filter (EKF) with Rauch–Tung–Striebel (RTS) smoothing is a canonical approach for joint estimation and smoothing in nonlinear dynamical systems. It extends the linear Kalman filter by local linearization of nonlinear process and measurement models, and augments the filtering estimate with a fixed-interval backward pass (the RTS smoother) that refines the entire trajectory using future as well as past data. This methodology is foundational in applications such as mobility-aware localization, visual-inertial odometry (VIO), and robust system identification, providing a statistically principled mechanism for maximum a posteriori trajectory estimation in the presence of nonlinearities and Gaussian noise.

## 1. Mathematical Model and EKF Formulation

Consider a discrete-time nonlinear state-space model parameterized as

\[
x_{k+1} = f(x_k) + w_k,\quad w_k \sim \mathcal{N}(0, Q_k)
\]
\[
z_k = h(x_k) + v_k,\quad v_k \sim \mathcal{N}(0, R_k)
\]

Here, $x_k \in \mathbb{R}^n$ denotes the latent state, $z_k \in \mathbb{R}^d$ the measurement, $f$ and $h$ are sufficiently smooth transition and measurement functions, and $Q_k$, $R_k$ are positive-definite process and measurement noise covariances. In practical localization contexts, the state may compose position, velocity, and sensor biases, e.g., in a 3D constant-velocity-plus-bias model,

\[
x_k = \begin{bmatrix} x_k\\ y_k\\ z_k\\ v_{x,k}\\ v_{y,k}\\ v_{z,k}\\ b_k \end{bmatrix}
\]
with block state-transition matrix

\[
F = \begin{bmatrix}
I_3 & \Delta t\,I_3 & 0_{3\times1}\\
0_{3\times3} & I_3 & 0_{3\times1}\\
0_{1\times3} & 0_{1\times3} & 1
\end{bmatrix}
\]

The Extended Kalman Filter executes the following recursions [2510.06861, 2404.15359, 1303.1993]:

- **Prediction:**
  \[
  \hat{x}_{k|k-1} = f(\hat{x}_{k-1|k-1}),\quad
  P_{k|k-1} = F_k\,P_{k-1|k-1}F_k^\top + Q_k
  \]
  where $F_k = \frac{\partial f}{\partial x}\big|_{x=\hat{x}_{k-1|k-1}}$.
- **Linearize measurement model:**
  \[
  H_k = \frac{\partial h}{\partial x}\bigg|_{x=\hat{x}_{k|k-1}}
  \]
- **Innovation and update gain:**
  \[
  y_k = z_k - h(\hat{x}_{k|k-1}),\;
  S_k = H_k P_{k|k-1} H_k^\top + R_k,\;
  K_k = P_{k|k-1} H_k^\top S_k^{-1}
  \]
- **Update:**
  \[
  \hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k y_k,\quad
  P_{k|k} = (I - K_k H_k) P_{k|k-1}
  \]

This forward EKF provides sequential state estimates, but is myopic in that it only incorporates past and current information at each time step.

## 2. Rauch–Tung–Striebel Smoothing Algorithm

RTS smoothing refines pointwise filter estimates by incorporating information from the entire observation window. Given the full sequence of filter means and covariances $(\hat{x}_{k|k}, P_{k|k})$, and prior predictions $(\hat{x}_{k+1|k}, P_{k+1|k})$, the fixed-interval backward smoother, initialized at the final time step $N$, proceeds for $k = N-1,\ldots,0$:

- **Smoothing Gain:**
  \[
  C_k = P_{k|k} F^\top (F P_{k|k} F^\top + Q_k)^{-1}
  \]
- **Smoothed State and Covariance:**
  \[
  \hat{x}_{k|N} = \hat{x}_{k|k} + C_k (\hat{x}_{k+1|N} - \hat{x}_{k+1|k})
  \]
  \[
  P_{k|N} = P_{k|k} + C_k\left(P_{k+1|N} - P_{k+1|k}\right)C_k^\top
  \]

This recursion recursively corrects each state estimate utilizing the overall data, thereby reducing estimator variance and bias relative to pure filtering [2510.06861, 1303.1993, 2404.15359].

## 3. Optimality, Computational Complexity, and the Optimization Viewpoint

EKF+RTS smoothing can be interpreted as a single Gauss–Newton iteration on the maximum a posteriori (MAP) nonlinear smoothing objective:

\[
\min_{x_1,\dots,x_N}
\frac{1}{2}\|x_1-g_1(x_0)\|_{Q_1^{-1}}^2 +
\sum_{k=2}^N \frac{1}{2}\|x_k-g_k(x_{k-1})\|_{Q_k^{-1}}^2 +
\sum_{k=1}^N \frac{1}{2}\|z_k-h_k(x_k)\|_{R_k^{-1}}^2
\]

A block-tridiagonal least-squares problem arises and is solved efficiently by the forward-backward EKF+RTS sweep with $O(n^3N)$ complexity, $n$ denoting state dimension and $N$ the trajectory length. This framework admits natural generalizations, such as iterated or dynamically iterated filters (DIFs), where repeated linearization about the smoothed trajectory and step-size damping (Levenberg–Marquardt) can be incorporated to improve local convergence and robustness. Each DIF iteration consists of an EKF+RTS sweep about updated linearization points, yielding improved MSE and tuning stability compared to non-iterated schemes [2404.15359, 1303.1993].

## 4. Algorithmic Enhancements: Adaptive Noise, Outlier Rejection, and State Transformation

State-of-the-art frameworks supplement the prototypical EKF+RTS with algorithmic enhancements for practical robustness:

- **Adaptive Noise Scaling:** The normalized innovation squared (NIS) statistic is used to adapt process and measurement covariances via
  \[
  Q_k \leftarrow \gamma Q_k,\quad R_k \leftarrow \gamma R_k
  \]
  where $\gamma$ is tuned to ensure observed innovation statistics match theoretical expectations, promoting estimator consistency [2510.06861].

- **Chi-Square Gating:** Outlier rejection is achieved by Mahalanobis gating,
  \[
  d_k^2 = y_k^\top S_k^{-1} y_k \stackrel{?}{<} \chi^2_{d,1-\alpha}
  \]
  where detections exceeding the threshold are excluded from state updates [2510.06861].

- **Double State Transformation (DST):** Advanced visual-inertial odometry algorithms (e.g., SP-VIO) enforce observability-consistent parameterizations by transforming both velocity and position errors. This ensures the correct 4D nullspace (yaw and translation) in VIO, maintaining filter consistency throughout forward and smoothing passes. The DST-RTS variant of RTS smoothing applies the smoothing equations in the DST-parameterized error space, ensuring unobservable directions are exactly preserved [2411.07551].

## 5. Performance, Consistency, and Empirical Results

EKF+RTS smoothing methods with the above enhancements demonstrably improve estimator precision and consistency:

- In mmWave-based pedestrian localization, un-smoothed EKF achieves Absolute Trajectory Error (ATE) ≈ 0.67 m, reduced to ATE = 0.19 m with adaptive noise and RTS smoothing. Relative Pose Error (RPE), NEES, and RMSE are similarly improved, with NEES converging to its statistical value ($\approx$ state dimension) and RMSE reduced by factors of 4–5 [2510.06861].
- The RTS smoother itself typically halves the ATE versus forward EKF, leveraging future data for improved state recovery.
- In VIO (SP-VIO employing DST-EKF + DST-RTS), RMSE reductions of 33.8% over MSCKF baseline are reported, with strong resilience to visual outages: under induced vision dropouts, combining DST-RTS limits error growth to +35.1% (versus +246% for EKF-only, +77.6% for batch optimization) [2411.07551].
- DIF/EKF–RTS variants further exhibit superior mean-squared error and increased robustness to parameter tuning by re-linearizing the full system and introducing principled step-size damping [2404.15359].

## 6. Applications and Extensions

The EKF+RTS smoothing architecture is foundational in:

- Mobility-aware localization in communication (mmWave) channels, enabling high-accuracy position tracking for both pedestrian and vehicular dynamics without ancillary ranging sensors [2510.06861].
- Embedded visual-inertial odometry systems where real-time efficiency, low drift, and robust recovery from perceptual aliasing and visual dropout are essential [2411.07551].
- Nonlinear system identification and constrained smoothing, where the same Gauss–Newton structure underlies sparse estimation, robust statistics, and constraint handling [1303.1993].

The methodology is extensible to frameworks wherein process and measurement models are highly nonlinear, or for systems with non-Gaussian noise, by embedding the core EKF–RTS recursions within iterated or adaptive schemes (DIF, IEKF, DST-EKF, Levenberg–Marquardt damping), as recent literature demonstrates [2404.15359].

## 7. Observability, Consistency, and Theoretical Guarantees

The filter's consistency and the preservation of physical unobservables depend on both the parameterization and linearization scheme. For example, standard EKF and MSCKF may lose one dimension in the unobservable yaw direction due to linearization drift. DST-EKF and its associated RTS smoother (DST-RTS) enforce exact observability by transforming errors in velocity and position with respect to the reference attitude, ensuring the correct algebraic nullspace is retained throughout the forward–backward procedure [2411.07551]. This corrects spurious information gain in the covariance and stabilizes estimator consistency.

---

**Summary Table: Key Algorithmic Elements of EKF–RTS Smoothing**

| Component           | Description                                | Source                |
|---------------------|--------------------------------------------|-----------------------|
| EKF Forward Pass    | Nonlinear filtering via local linearization| [2510.06861, 1303.1993]|
| RTS Backward Pass   | Smoothing gain & state refinement          | [2510.06861, 1303.1993]|
| Adaptive Noise      | Online $\gamma$ scaling using NIS          | [2510.06861]           |
| Mahalanobis Gating  | Outlier/rejection by chi-square threshold  | [2510.06861]           |
| DST Parametrization | Consistency via velocity/position transforms| [2411.07551]           |
| Iterated/Damped EKF | Gauss–Newton updates, LM regularization    | [2404.15359]           |

The EKF with Rauch–Tung–Striebel smoothing remains a foundational and extensible tool in modern state estimation, with substantial practical and theoretical advances contributed by recent research on adaptivity, consistency, and efficient implementation.

Source: https://www.emergentmind.com/topics/extended-kalman-filter-with-rauch-tung-striebel-smoothing