Invariant RTS Smoothing

• Invariant RTS (IRTS) is a smoothing algorithm that extends classical Rauch–Tung–Striebel methods to matrix Lie groups, providing state-independent linearizations.
• It leverages the geometric structure of Lie groups to maintain invariant Jacobians, which enhances robustness and convergence in high-dimensional state estimation.
• IRTS outperforms conventional EKF-based smoothers by eliminating repeated relinearizations, leading to improved efficiency in sensor fusion scenarios.

The Invariant Rauch–Tung–Striebel (IRTS) smoother is an extension of the classical Rauch–Tung–Striebel (RTS) smoothing algorithm to systems whose states evolve on a matrix Lie group. IRTS uniquely leverages the geometric structure of Lie groups to maintain state-estimate-independent linearizations of both the process and measurement models—given certain technical invariance conditions. This approach addresses core deficiencies of standard EKF-based smoothers, particularly the dependence of Jacobians on local state estimates, thereby enhancing robustness and convergence in batch state estimation tasks that involve high-dimensional geometric states and sensor fusion scenarios (Laan et al., 2024).

1. State-Space Models on Matrix Lie Groups

Let $\mathcal{G} \subset \mathbb{R}^{n\times n}$ denote a matrix Lie group with associated Lie algebra $\mathfrak{g}$ and exponential/logarithmic maps $\exp: \mathfrak{g} \to \mathcal{G}$, $\log: \mathcal{G} \to \mathfrak{g}$. State-space models for invariant smoothing are formulated as follows:

• Process Model (Group-Affine, Left-Invariant Noise):

$$\dot{X}(t) = F(X(t), u(t)) + X(t)W(t), \qquad W(t) \in \mathfrak{g},\; w(t)=W(t)^\vee \sim \mathcal{N}(0, Q)$$

$F$ satisfies the group-affinity condition, i.e., for any $X_1, X_2 \in \mathcal{G}$, $$F(X_1 X_2, u) = X_1 F(X_2, u) + F(X_1, u) X_2 - X_1 F(I, u) X_2$$.

The discrete-time process is,

$$X_{k+1} = F(X_k, u_k)\exp(\Delta t\,w_k), \qquad w_k \sim \mathcal{N}(0, Q_k)$$

• Measurement Models:
  • Left-invariant: $y_k^L = X_k b_k + v_k$
  • Right-invariant: $y_k^R = X_k^{-1} b_k + v_k$
  • with $v_k \sim \mathcal{N}(0, R_k)$.

These formulations permit the exploitation of group symmetries, which underpins the invariance properties of IRTS (Laan et al., 2024).

2. Forward Filtering with State-Independent Jacobians

The forward pass adapts the invariant extended Kalman filter (IEKF) to the smoothing context. The core construction uses an invariant error state representation. For left-invariant error,

$$\delta X_k = X_k^{-1} \hat{X}_k,\qquad \delta\xi_k = \log(\delta X_k)^\vee \in \mathbb{R}^m$$

For group-affine $F$, the linearization of small-error dynamics yields,

$\delta\xi_{k+1} \approx A_k \delta\xi_k + L_k w_k$

Here, $A_k$ and $L_k$ depend exclusively on $u_k$ and possibly $k$ but crucially not on the current state estimate $\hat{X}_k$.

Covariance propagation reduces to the form of linear Kalman filtering: $$\check{P}_{k+1} = A_k \hat{P}_k A_k^T + L_k Q_k L_k^T$$

Measurement update utilizes the invariance property:

• For left-invariant measurements,

$$z_k = \log(\check{X}_k^{-1} y^L_k)^\vee = H_k \delta\xi_k + M_k v_k + O(\|\delta\xi_k\|^2)$$

with Jacobians $H_k$, $M_k$ independent of $\check{X}_k$.

The Kalman gain and state update equations use these constant Jacobians. For right-invariant measurements, the form is analogous, but with $\log(y_k^R \check{X}_k^{-1})^\vee$ as innovation, and state update entering on the left.

A summary table of key quantities (for the left-invariant case):

Step	Expression	State-Estimate Dependence
Error Definition	$\delta X_k = X_k^{-1}\hat{X}_k$, $\delta\xi_k$	No
Prediction	$\delta\xi_{k+1} \approx A_k \delta\xi_k + L_k w_k$	No
Update	$z_k = H_k \delta\xi_k + M_k v_k$	No
Gain/Correction	Standard Kalman form, using $H_k$, $M_k$	No

The independence from $\hat{X}_k$ addresses a principal limitation of classical EKF-based methods, which require re-computation of Jacobians at each state estimate (Laan et al., 2024).

3. Backward Smoother Pass and Algorithm

Given the forward IEKF estimates $\{\check{P}_{k+1}, \hat{P}_k, \hat{X}_k\}$, the IRTS smoother defines the backward pass as:

• Smoother gain: $$K^s_k = \hat{P}_{f,k}\,A_k^T\,\check{P}_{f,k+1}^{-1}$$
• Smoother innovation: $$z^s_k = \log(\hat{X}_{s,k+1}^{-1}\check{X}_{f,k+1})^\vee$$
• Smoothed state update (left-invariant): $$\hat{X}_{s,k} = \hat{X}_{f,k}\,\exp(-(K^s_k z^s_k)^\wedge)$$
• Smoothed covariance: $$\hat{P}_{s,k} = \hat{P}_{f,k} - K^s_k(\check{P}_{f,k+1} - \hat{P}_{s,k+1})(K^s_k)^T$$

For right-invariant smoothing, the correction acts on the left: $$\hat{X}_{s,k} = \exp(-(K^s_k z^s_k)^\wedge)\hat{X}_{f,k}$$.

Online smoothing is performed in one forward/backward sweep, involving $O(N)$ small Riccati steps (Laan et al., 2024).

4. Conditions for Invariance and Implications

The necessary and sufficient technical criteria for state-independent Jacobians are formalized as follows:

• $F(X, u)$ must be group-affine.
• Measurements must be strictly left- or right-invariant.
• The error representation must be chosen to match the measurement invariance.

Under these, all relevant Jacobians ($A_k$, $H_k$, $M_k$) are independent of the propagated state estimates. This property eliminates the need to relinearize at each step, which in the EKF results in sensitivity to poor initializations or large state corrections (Laan et al., 2024). A plausible implication is improved robustness and convergence, especially in scenarios with large initial uncertainty.

5. Comparison to MRTS and Invariant Gauss–Newton

• Multiplicative RTS (MRTS): Constructs error as $\delta C = C\hat{C}^T$ for attitude (with additive representation for translation and biases), resulting in Jacobians that generally depend on $\hat{C}$, $\hat{r}$.
• IRTS: Jacobians are functions solely of input signals and known constants. Robustness to initially poor state estimates is observed, as the linearization is unaffected by intermediate trajectory errors.
• Block-sparse Invariant GN or Multiplicative GN (IGN/MGN): These batch methods treat the entire trajectory in one joint optimization, requiring linearization about a reference trajectory and inversion of a large Hessian. In contrast, IRTS achieves comparable or superior accuracy and convergence speed in a single forward–backward sweep, providing direct smoothed covariances.

Empirical results (Starry Night dataset) indicate:

• For small initial errors, IRTS and MRTS produce similar RMSE.
• With large initial errors, IRTS outperforms MRTS across attitude, position, and bias estimation.
• One IRTS pass outperforms several IGN/MGN iterations, both in RMSE and result consistency.
• Unlike GN-based approaches, IRTS directly produces smoothed covariance estimates without large-scale matrix inversion (Laan et al., 2024).

6. SE(3) Pose with Sensor Bias Example and Error Representation Transform

Consider estimation of the $\mathrm{SE}(3)$ pose (rotation $C \in \mathrm{SO}(3)$, translation $r \in \mathbb{R}^3$), gyro-bias $\beta^1 \in \mathbb{R}^3$, and velocity-bias $\beta^2 \in \mathbb{R}^3$, collectively as $X$ in an embedded Lie group. Although the inclusion of biases breaks strict group-affinity, left-invariant error linearization yields a system matrix $A$ that is a function only of control inputs and biases, not of the pose itself.

The approach allows fusion of mixed-invariance sensors (e.g., left-invariant GPS, right-invariant stereo landmarks) via an Error Representation Transform (ERT). Covariances are transformed between left- and right-invariant forms using the adjoint representation prior to update, preserving the constancy of Jacobians across updates.

7. Advantages, Limitations, and Outlook

Advantages:

• Jacobians are state-estimate-independent under group-affinity and matching invariance, enhancing robustness.
• One-shot online smoothing with $O(N)$ computational complexity.
• Direct computation of smoothed covariances ($\hat{P}_{s,k}$) at each step.
• Seamless integration of mixed left-/right-invariant measurement updates through ERT.

Limitations:

• Group-affinity is required for the process model; many practical systems, especially with biases, are only approximately affine.
• Correct invariant error representations must be specified per sensor type.
• Systems with highly non-affine or hybrid dynamics require further methodological extension (Laan et al., 2024).

In summary, IRTS extends the benefits of invariant filtering—robust, state-independent linearizations and efficient smoothing—to the full smoothing context. It achieves competitive or improved accuracy relative to established multiplicative and batch Gauss–Newton methods, often with reduced computational and implementation complexity.