Extended Kalman Filter with RTS Smoothing

• Extended Kalman Filter with RTS smoothing is a state estimation method for nonlinear systems that combines forward filtering with a fixed-interval backward pass to refine trajectory estimates.
• The approach leverages local linearization and future data incorporation to reduce estimator variance and bias, enhancing overall precision.
• Algorithmic enhancements such as adaptive noise scaling, Mahalanobis gating, and DST parameterization further improve consistency and robustness across various applications.

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 (Orimogunje et al., 8 Oct 2025, Kullberg et al., 2024, Aravkin et al., 2013):

• 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 (Orimogunje et al., 8 Oct 2025, Aravkin et al., 2013, Kullberg et al., 2024).

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 (Kullberg et al., 2024, Aravkin et al., 2013).

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 (Orimogunje et al., 8 Oct 2025).

• 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 (Orimogunje et al., 8 Oct 2025).

• 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 (Du et al., 2024).

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 (Orimogunje et al., 8 Oct 2025).
• 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) (Du et al., 2024).
• 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 (Kullberg et al., 2024).

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 (Orimogunje et al., 8 Oct 2025).
• Embedded visual-inertial odometry systems where real-time efficiency, low drift, and robust recovery from perceptual aliasing and visual dropout are essential (Du et al., 2024).
• Nonlinear system identification and constrained smoothing, where the same Gauss–Newton structure underlies sparse estimation, robust statistics, and constraint handling (Aravkin et al., 2013).

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 (Kullberg et al., 2024).

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 (Du et al., 2024). 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	(Orimogunje et al., 8 Oct 2025, Aravkin et al., 2013)
RTS Backward Pass	Smoothing gain & state refinement	(Orimogunje et al., 8 Oct 2025, Aravkin et al., 2013)
Adaptive Noise	Online $\gamma$ scaling using NIS	(Orimogunje et al., 8 Oct 2025)
Mahalanobis Gating	Outlier/rejection by chi-square threshold	(Orimogunje et al., 8 Oct 2025)
DST Parametrization	Consistency via velocity/position transforms	(Du et al., 2024)
Iterated/Damped EKF	Gauss–Newton updates, LM regularization	(Kullberg et al., 2024)

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.