Nonlinear Observer for LI-SLAM

• The paper presents a novel nonlinear observer for LI-SLAM that leverages matrix Lie group geometry to fuse inertial and landmark data with bias adaptation.
• It integrates invariant design principles to ensure almost-global convergence and stability under nonlinear dynamics, avoiding linearization pitfalls.
• Empirical simulations show rapid convergence, with landmark errors decaying within 5 s and attitude and position errors limited to <0.1° and <2 cm.

Nonlinear observers for LI-SLAM (Landmark-Inertial Simultaneous Localization and Mapping) provide deterministic and often provably convergent alternatives to stochastic, linearized, and EKF-style methods for fusing inertial (IMU) measurements with landmark position observations. These observers are commonly constructed with geometric structure on matrix Lie groups—most notably $\mathbb{SLAM}_n(3)$, $\operatorname{SE}_{3+n}(3)$, or closely related symmetry groups—to natively handle the coupled rotation, translation, velocity, and feature-position states. Modern approaches emphasize equivariance, bias adaptation, global or almost-global convergence proofs, scalable complexity, and compatibility with the invariance principles central to SLAM’s underlying observability properties.

1. Geometric State Modeling for LI-SLAM

Nonlinear observers for LI-SLAM employ matrix Lie group frameworks to unify pose and landmark representation. For $n$ landmarks, the aggregated state evolves on the product manifold

$$\mathbb{SLAM}_n(3) = \{ X = (T, p_1, \ldots, p_n) \mid T \in \mathbb{SE}(3),\ p_i \in \mathbb{R}^3 \}$$

where $$T = \begin{pmatrix} R & P \ 0 & 1 \end{pmatrix} \in \mathbb{SE}(3)$$, $R \in \mathbb{SO}(3)$ is the body-to-world rotation, and $P$ is position.

Extensions such as $\operatorname{SE}_{3+n}(3)$ (Boughellaba et al., 5 Apr 2025), encode pose, velocity, gravity, and landmark positions into a single block-matrix, enabling joint filtering and explicit encoding of inertial observability constraints (e.g., lack of absolute yaw and global translation observability).

IMU-driven dynamics on these groups take the form: $$\dot{R} = R[\Omega]_\times, \quad \dot{P} = R V, \quad \dot{p}_i = 0$$ with measured velocities $\Omega_{m} = \Omega + b_\Omega$, $V_{m} = V + b_{V}$, where $b_\Omega, b_{V}$ are constant or slowly-varying bias terms.

2. Observer Design Principles and Innovation Construction

The general nonlinear observer structure for LI-SLAM aims to mirror the true kinematics and compensate measurement bias while respecting group geometry: $$\dot{\hat{X}} = \hat{X}(U_{m} - \hat{b}) + \mathcal{A}(\hat{X}, y)$$ where $U_m$ composes $\Omega_m, V_m$, $\hat{b}$ estimates the bias, and the innovation $\mathcal{A}$ is formed from landmark measurement residuals.

In component form: $$\begin{aligned} \dot{\hat{R}} &= \hat{R}[\Omega_{m} - \hat{b}_\Omega - W_\Omega]_\times \ \dot{\hat{P}} &= \hat{R}(V_{m} - \hat{b}_V - W_V) \ \dot{\hat{p}}_i &= -\psi(e_i) e_i \ \dot{\hat{b}}_\Omega &= -\sum_{i=1}^{n} \frac{\Gamma}{\alpha_i}[y_i]_\times \hat{R}^\top e_i \ \dot{\hat{b}}_V &= -\sum_{i=1}^{n} \frac{\Gamma}{\alpha_i} \hat{R}^\top e_i \end{aligned}$$ where $e_i = \hat{p}_i - \hat{R} y_i$ is the $i$th landmark tracking error, and $W_\Omega, W_V$ are innovation gains (often summed over all features). $\psi(e_i)$ is a state-dependent gain accelerating convergence for large errors.

Observers may employ equivariant error and innovation construction, so that the filtering law commutes with changes of inertial frame and preserves geometric symmetries. This can be enforced by rewriting group-based errors, e.g., left-invariant error $E = T^{-1}\hat{T}$, and constructing innovations that depend on invariant output differences $T^{-1} y_i$ (Drayton et al., 2020).

Gradient-based innovation terms or smooth Lyapunov-defined corrections are common, enabling almost-global convergence and avoidance of local covariance updates.

3. Bias Adaptation and Robustness

Robust bias compensation is achieved via direct online adaptation laws driven by projected landmark innovations: $$\begin{aligned} \dot{\hat{b}}_\Omega &= -\sum_{i=1}^{n} \frac{\Gamma}{\alpha_i}[y_i]_\times \hat{R}^\top e_i \ \dot{\hat{b}}_V &= -\sum_{i=1}^{n} \frac{\Gamma}{\alpha_i} \hat{R}^\top e_i \end{aligned}$$ These terms implement geometric projections of innovation vectors through group adjoints (or generalized adjoint operators in LI-SLAM frameworks), cancelling bias effects in the closed-loop dynamics. Bias compensation mechanisms are provably effective for constant velocity biases under suitable persistence of excitation (three noncollinear landmarks) (Drayton et al., 2020).

Some observers incorporate prescribed-performance functions to constrain transient and steady-state error within known decaying tubes, using diffeomorphic error transformation and time-varying gain matrices (Hashim, 2020). This provides systematic convergence criteria and design handles for practical performance.

4. Convergence Analysis and Stability Guarantees

Analytical convergence is established using strict Lyapunov functions over error coordinates. For instance,

$$V = \sum_{i=1}^n \frac{\|e_i\|^2}{2\alpha_i} + \frac{1}{2} \tilde{b}_\Omega^\top \Gamma^{-1} \tilde{b}_\Omega + \frac{1}{2} \tilde{b}_V^\top \Gamma^{-1} \tilde{b}_V$$

provides a measure for landmark and bias errors, decaying under the observer dynamics.

Under the nonlinear observer update laws, one shows: $$\dot{V} \leq -\sum_{i=1}^n \frac{\psi(e_i)}{\alpha_i} \|e_i\|^2 < 0\ \forall e_i \neq 0$$ leading to exponential convergence of feature errors, bounded bias estimation error, and pose error settling to a constant residual corresponding to the unobservable global rigid transformation (Drayton et al., 2020). Barbalat’s Lemma is invoked for asymptotic error decay proof.

Observers designed in the LI-SLAM (Lie-invariant) framework are shown to yield almost-global stability, except for measure-zero 'antipodal' sets in SO(3) (Drayton et al., 2020). For prescribed-performance observers, strict contraction within performance tubes is assured, and error trajectories can be bounded from above throughout the transient phase (Hashim, 2020).

5. Integration with LI-SLAM and Output Equivariance

Nonlinear observer structures are generally compatible with the output-equivariant, symmetry-preserving principles of LI-SLAM (Boughellaba et al., 5 Apr 2025). This enables construction of filtering algorithms which:

• Employ left- or right-invariant error measures
• Inject corrections with gains that are functions of invariant output error, preserving transport symmetry
• Adapt bias parameters through invariant differential projection

Such observers yield error dynamics which commute with any group transformation of the world frame, ensuring structural consistency in the presence of global invariances (unobservable global pose and unconstrained yaw).

Notably, the nonlinear observer described in (Drayton et al., 2020) is immediately applicable to the LI-SLAM framework by replacing group errors and innovation terms with their invariant counterparts, leading to provably exponentially convergent, bias-adaptive, Lie-invariant filters. These unify previously demonstrated special cases (Zhang et al. (2017), Mahony–Hamel (2017)) under a general geometric structure.

6. Empirical Performance and Implementation Guidelines

Simulation studies conducted with rigid-body trajectories (constant angular velocity and translation), up to four landmarks, and significant initial estimation errors demonstrate that:

• Landmark errors decay to machine precision within 5 s
• Attitude and position errors settle to small constants (misalignment $<$ 0.1°, position offset $<$ 2 cm)
• Bias estimates converge within 1% of true values (Drayton et al., 2020)

Implementation steps include:

• Numeric integration of observer differential equations (matrix exponential or quaternion-based integration for rotation, Euler/RK4 for translation and landmark update)
• Selection of innovation gains ($k_w$, $\Gamma$) and feature weights ($\alpha_i$) based on desired bandwidth and convergence speed
• For LI-SLAM adaptation, all innovation and error terms are recast in terms of invariant group features and outputs

Observers admit real-time deployment due to linear per-frame computational complexity ($O(n)$ with the number of landmarks) and avoidance of high-dimensional covariance or Jacobian computation.

7. Comparative Context and Extensions

Nonlinear observers for LI-SLAM fundamentally differ from EKF- or graph-based SLAM methods:

• They avoid explicit linearization and local covariance propagation, eliminating approximation-induced divergence
• Stability guarantees and performance bounds are global (or almost-global), not strictly local
• Observers respect the true system nonlinear kinematics and symmetries, outputting estimates consistent under world-frame transformations

Extensions to stochastic measurement models, higher-order filtering (complementary filters on Lie groups), and modular integration of multiple sensor modalities are developed in related works (Zlotnik et al., 2018, Tawhid et al., 2021). Observers are equipped to handle IMU/LiDAR fusion, prescribed transient bounds, landmark initialization/removal, and bias estimation, providing a rigorous and scalable framework for robust SLAM in embedded and field robotics.