Landmark Inertial SLAM

• The LI-SLAM approach is defined by symmetry-preserving geometric nonlinear observers on Lie groups, ensuring robust six-DoF pose and landmark estimation.
• It fuses IMU data and direct landmark measurements to achieve prescribed performance with rigorous Lyapunov-based stability and bias adaptation.
• Real-time implementations exhibit linear computational complexity per landmark, proven in experimental validations like the EuRoC quadrotor dataset.

Landmark Inertial Simultaneous Localisation and Mapping (LI-SLAM) refers to the class of estimation techniques that simultaneously recover a robot’s six-degree-of-freedom pose and a map of unknown static landmark positions in three-dimensional space, using inertial measurement unit (IMU) data and direct exteroceptive measurements of landmark positions in the robot body frame. Recent developments emphasize symmetry-preserving geometric nonlinear observers on Lie groups, systematic convergence guarantees (often with prescribed or stochastic performance), and computational efficiency suitable for real-time embedded implementation. Multiple architectures have been established, ranging from adaptive EKF formulations to global almost-surely convergent nonlinear observers, with the field marked by strong attention to rigorous stability proofs and bias adaptation.

1. Mathematical Foundations and State Space Formulation

The state for LI-SLAM is modeled as a pair $$(T, \bar p) \in \mathbb{SLAM}_n(3) = SE(3) \times \overline{\mathcal M}^n$$ where $$T = \begin{bmatrix}R & P \ 0 & 1\end{bmatrix} \in SE(3)$$ encodes the 6-DoF pose (rotation $R\in SO(3)$, translation $P \in \mathbb{R}^3$), and $\bar p = [\bar p_1,\dots,\bar p_n]$ stacks homogeneous landmark positions $\bar p_i = [p_i^\top,1]^\top$. For certain recent observers, this product space is further extended to include velocity and gravity as independent variables, yielding a state on the Lie group $SE_{3+n}(3)$:

$$X = \begin{bmatrix} R & [p\ v\ g\ p_1 \ldots p_n] \ 0 & I_{3+n} \end{bmatrix}$$

and similarly in the “synchronous observer” formalism on $\SE_{n+2}(3)$, one encodes $(R, v, x, p_1, ..., p_n)$. Group operations (multiplication, inversion) are defined by standard block-matrix rules. This formalism admits intrinsic, coordinate-free filter equations and systematic exploitation of the underlying problem symmetries (e.g., SE(3)-invariance, observability up to a global yaw/translation).

2. System and Measurement Models

The system dynamics are driven by rigid-body kinematics: $$\begin{aligned} \dot R &= R [\Omega]_\times \ \dot P &= R V \ \dot p_i &= 0 \end{aligned}$$ where $\Omega$ (body-rate) and $V$ (body-frame velocity) are interpreted as the robot’s true group velocity. IMU measurements of angular velocity ($\Omega_m$) and translational velocity ($V_m$) (or acceleration) are modeled as: $$\Omega_m = \Omega + b_\Omega + n_\Omega,\quad V_m = V + b_V + n_V$$ with $b_U = [b_\Omega^\top, b_V^\top]^\top$ constant unknown biases and $n_U$ measurement noise. Exteroceptive landmark observations are modeled as: $y_i = R^\top (p_i - P) + b^y_i + n^y_i$ for direct range/bearing measurements, or $a_j = R^\top r_j + \text{noise}$ for IMU vector observations (accelerometer, magnetometer). In group form, $y_i = T^{-1}\bar p_i$ in the noise/bias-free case.

3. Geometric Nonlinear Observer and Bias Adaptation

The defining property of recent LI-SLAM observers is the use of symmetry-preserving geometric nonlinear filter design on $\mathbb{SLAM}_n(3)$, $SE_{3+n}(3)$, or $\SE_{n+2}(3)$. The typical estimator evolves as

$$\dot{\hat{T}} = \hat{T} [U_m - \hat b_U - W_U]_\wedge$$

$$\dot{\hat p}_i = -k_1 \Lambda_i E_i + \hat R [y_i]_\times W_\Omega$$

where $W_U = [W_\Omega^\top, W_V^\top]^\top$ is an innovation term that combines IMU-inferred attitude corrections and landmark-derived position corrections. Attitude correction uses multiple IMU reference directions, combined as $M = \sum_j s_j r_j r_j^\top$, and $W_\Omega$ is constructed from the attitude error direction vector $$\Upsilon(\tilde R M) = \text{vex}(\mathcal{P}_a(\tilde R M))$$. The full “observer + bias compensator” includes adaptive bias update laws: $$\dot{\hat b}_\Omega = \Gamma_1/2\,\hat R^\top\Lambda_{\tilde R}\Upsilon(\tilde R M) - \sum_i (\Gamma_2/\alpha_i)[y_i]_\times\hat R^\top\Lambda_i E_i$$

$$\dot{\hat b}_V = - \sum_i (\Gamma_2/\alpha_i)\hat R^\top\Lambda_i E_i$$

for diagonal positive definite $\Gamma$, and landmark error terms $E_i$ computed through a prescribed-performance transformation or direct residuals. Notably, observers in (Hashim et al., 2020, Hashim et al., 2021), and (Hashim, 2021) handle unknown IMU biases in a coupled, adaptive fashion, with stochastic variants incorporating explicit noise adaptation via additional covariance upper-bound dynamical equations.

4. Prescribed Performance, Stability, and Convergence Guarantees

A central feature of advanced LI-SLAM designs is the systematic use of prescribed performance functions (PPFs), e.g.,

$$\xi_\star(t) = (\xi_\star^0 - \xi_\star^\infty) e^{-\ell_\star t} + \xi_\star^\infty$$

governing error decays so that, for each raw error $e_\star$, $-\delta_\star\xi_\star < e_\star < \xi_\star$. This approach strictly bounds transient and steady-state error envelopes by design. Transformed error dynamics (e.g., $$\dot{E}_\star = \Lambda_\star (\dot{e}_\star-(\dot{\xi}_\star/\xi_\star)e_\star)$$) render the stability analysis amenable to Lyapunov design. Theoretical guarantees—validated in multiple papers—include:

• All estimation errors (attitude, position, landmark, velocity bias) converge to arbitrarily small neighborhoods of zero, subject to initial conditions outside a measure-zero unstable set (typically for attitude $R$ not at a 180°-misorientation).
• Lyapunov or stochastic Lyapunov arguments (in observers with noise adaptation) yield semi-global uniform ultimate boundedness (SGUUB) and/or exponential prescribed convergence rates.
• Observability is inherently up to a global translation and a global rotation about the gravity axis (in practice, estimates converge modulo these four unobservable SLAM directions).

5. Implementation and Computational Considerations

All proposed observers (notably (Hashim et al., 2020, Hashim et al., 2021, Hashim, 2021)) are formulated for both continuous and discrete time, with efficient O($n$) complexity per step (in the number of landmarks $n$), substantially outperforming O($n^3$) EKF-SLAM in computational cost. Discrete-time implementation proceeds via integration over time steps: $$\hat T_{k+1} = \hat T_k \exp\left([U_m[k] - \hat b_U[k] - W_U[k]]_\wedge\,\Delta t\right)$$

$$\hat p_{i}[k+1] = \hat p_{i}[k] - k_1\Delta t \, e_i[k] + \Delta t\, \hat R_k [y_i[k]]_\times W_\Omega[k]$$

Bias and innovation updates are executed at each step, requiring no matrix inversion or linearization of kinematic models. Quaternion-form observers are directly supported via the appropriate quaternion kinematics and bias correction.

Typical real-time operating conditions (e.g., IMU at 200 Hz, vision at 20 Hz) were demonstrated to be tractable on modest computational hardware. Parameter tuning involves gains for PPF dynamics, adaptive bias learning rates, and innovation weighting. Initialization procedures must supply an initial guess for pose and at least rough landmark positions; convergence is robust to large initial errors except at singularities (180° attitude offsets).

6. Experimental Validation and Performance

LI-SLAM methods were validated on the EuRoC “V1_02_medium” quadrotor dataset, incorporating ground-truth 6 DoF motion capture, synthetic landmark placements, significant injected IMU bias, and measurement noise. Results from (Hashim et al., 2020) report:

• Position estimation errors below 0.1 m (sub-decimeter) and near-exact convergence of landmark position errors to their true values.
• Attitude estimation errors decay within user-prescribed performance bands.
• Online adaptation of velocity bias estimates ($\hat b_\Omega$, $\hat b_V$) to the true underlying biases.
• No direct baseline (e.g., EKF-SLAM) comparison is included, but the performance under substantial noise and bias validates observer robustness.
• All theoretical convergence claims (including satisfaction of prescribed performance and stochastic mean-square bounds) are empirically observed.

7. Context, Impact, and Prospects

LI-SLAM establishes a symmetry-preserving, physically consistent framework for fusing IMU and direct landmark measurements at the filter level, in contrast to extended Kalman or particle filter approaches that linearize or sample over local models. The geometric design enables global or almost-global convergence (outside singular attitude configurations), guaranteed bias adaptation, and rigorous transient/steady-state optimality under prescribed user specifications. Implementation overhead is minimal, scaling linearly with landmark count, and observers are amenable to real-time deployment on embedded platforms.

Extensions remain active:

• Incorporation of projective camera models and range-only bearing (suitable for monocular vision) is an open direction.
• Explicit handling of time-varying and dynamic landmarks, multiple IMU modalities, and closed-form observability recovery.
• Integration with higher-level factor-graph frameworks and consistency monitoring for large-scale or multi-session scenarios.

The field of LI-SLAM advances SLAM robustness in environments where only partial or noisy observability is available, with mathematics that guarantee deterministic or mean-square error bounds and computational architectures suitable for resource-limited real-world robotic systems (Hashim et al., 2020).