The Invariant Rauch-Tung-Striebel Smoother

Abstract

This paper presents an invariant Rauch-Tung-Striebel (IRTS) smoother applicable to systems with states that are an element of a matrix Lie group. In particular, the extended Rauch-Tung-Striebel (RTS) smoother is adapted to work within a matrix Lie group framework. The main advantage of the invariant RTS (IRTS) smoother is that the linearization of the process and measurement models is independent of the state estimate resulting in state-estimate-independent Jacobians when certain technical requirements are met. A sample problem is considered that involves estimation of the three dimensional pose of a rigid body on SE(3), along with sensor biases. The multiplicative RTS (MRTS) smoother is also reviewed and is used as a direct comparison to the proposed IRTS smoother using experimental data. Both smoothing methods are also compared to invariant and multiplicative versions of the Gauss-Newton approach to solving the batch state estimation problem.

The Interacting Multiple Model Filter and Smoother on Boxplus-Manifolds

“… (M)-EKS: The same as the ⊞-EKS, but it uses the simplified covariance smoothing ( 56 ). This algorithm uses the ⊞-EKF for filtering but its smoothing step is identical to the MEKS or the left invariant EKS on quaternions [ 17 ]. ⊞ -IMM: The IMM on ⊞-manifolds, as derived in Section 3 .…”

“… NM-RTSIMMS: An IMM smoother that handles mixing such as the N-IMM and uses the (M)-EKS for mode-matched smoothing. This IMM basically combines the approaches in [ 5 ] and [ 17 ]. …”

“…The transforms can be dropped to reduce computational and algorithmic complexity. Thus, we recommend to use the approach of [ 17 ] for quaternion smoothing. For other manifolds, the reference transform may be relevant to achieve consistent results.…”

“…The presented ⊞-EKS differs from existing RTS smoothers on quaternions [ 17 ]. Since smoothing on a quaternion state is a special case of our general ⊞-EKS, it should coincide.…”

“…Thus, it is difficult to maintain manifold structures like the rotation quaternion in the filter. As it is often required to estimate orientations in target tracking, various extensions [ 14 , 15 , 16 , 17 ] have been developed to handle rotation quaternions or matrices correctly without destroying their manifold properties, e.g., unit norm or orthogonality. To our knowledge, no such extension exists for the IMM nor the IMM smoother.…”