Improvement of Robot's Simultaneous Localization and Mapping Using an Effective Transformation to Achieve Linear Model

Abstract: Nowadays mobile robots have wide engineering applications. Simultaneous localization and mapping (SLAM) is an important task of these robots. The major and common algorithms used for this task are based on extended Kalman filter (EKF). One of the main problems in EKF-based SLAM is its divergence. The nonlinearity of motion and observation models and linearization error are the main reasons for the divergence. There have been some efforts to address this problem with limited success. In this paper, by applying a simple compass and using an effective transformation, we transform the non-linear state space model into a linear model. Then, by applying the original KF to this model, we reach a new method, which is called LMKF SLAM. We show that the LMKF SLAM is significantly superior to the state-of-the-art methods, especially EKF-based SLAMs, both in accuracy, convergence, and computational complexity. The proposed method is also more stable with respect to the uncertainty of sensors values and changes in system parameters. Experimental results verify these points.

• The paper introduces LMKF SLAM, which transforms the state space by eliminating direct orientation, enabling direct linearization of motion and observation models.
• The paper demonstrates that LMKF SLAM outperforms EKF-based methods with lower RMSE, enhanced stability, and reduced computational complexity in varied environments.
• The paper shows that the approach achieves provable convergence and minimal sensitivity to sensor uncertainties, marking a significant shift in SLAM filtering strategies.

Linearizing SLAM for Enhanced Consistency and Performance: LMKF SLAM

Background and Motivation

Simultaneous Localization and Mapping (SLAM) remains a fundamental challenge in mobile robotics, particularly for real-time deployment in complex and uncertain environments. Filtering-based SLAM approaches, notably those leveraging the Extended Kalman Filter (EKF), Extended Information Filter (EIF), Fast SLAM (based on particle filters), cubature Kalman filter (CKF), and improved CKF (ICKF), contend with the intrinsic nonlinearity of robot motion and observation models. These nonlinearities, when approximated via linearization, induce errors that manifest as filter inconsistency and potential divergence, especially as environmental complexity and sensor uncertainty scale.

Prior analyses (e.g., Julier et al. 2001, Bailey et al. 2006, Huang et al. 2008-2010, Barrau et al. 2014-2015) have extensively characterized the mechanisms of EKF-SLAM divergence and explored partial remedies. These have included FEJ-EKF, OC-EKF, and invariant filters, but they rely on sophisticated and often computationally expensive interventions to mitigate linearization-induced inconsistency.

Proposed Solution: LMKF SLAM

The paper introduces the Linear Model Kalman Filter (LMKF) SLAM, which fundamentally restructures the SLAM state space through the application of an effective transformation. By leveraging a simple compass sensor to directly measure the robot's orientation, the process eliminates $\theta_k$ from the state vector and enables direct linearization of both motion and observation models with respect to state variables. Noise terms associated with sensor measurements are handled through first-order linearization, allowing the Kalman filter to be applied directly without further approximations.

Motion Model Linearization

The conventional nonlinear robot motion model, involving trigonometric dependencies on $\theta_k$ and steering angle $G_k$, is reduced to a linear form by excluding orientation as a state variable. The resultant motion model expresses next-state positions as a deterministic function plus a linear function of process noise:

$$\mathbf{x}_{k+1} = \mathbf{x}_k + \mathbf{u}_k + \mathbf{G}_k \mathbf{w}_k$$

where $\mathbf{u}_k$ and $\mathbf{G}_k$ are derived from linearization about measured sensor values.

Observation Model Linearization

Observation is similarly linearized. Range and bearing sensor measurements, together with compass-supplied orientation, allow landmarks to be represented in terms of linear relations with the robot's position. The final form links observation $\boldsymbol{\zeta}_k$ as:

$$\boldsymbol{\zeta}^i_k = \mathbf{H}^i_k \mathbf{x}_k + \mathbf{S}^i_k \boldsymbol{\vartheta}^i_k$$

with $\mathbf{H}^i_k$ as a sparse observation matrix and $\mathbf{S}^i_k$ encoding linearized noise effects.

Theoretical Guarantees

Unlike nonlinear EKF-SLAM, the LMKF SLAM admits direct convergence guarantees as per classical Kalman filter theory. The proof of convergence for linear SLAM models (Dissanayake et al. 2001) applies unaltered, establishing bounded absolute error for both robot and landmark positioning regardless of the number of landmarks or environment size. This is a significant improvement over EKF-based SLAMs, which can diverge even in stationary scenarios under certain noise conditions.

Numerical Results and Empirical Evaluation

The LMKF SLAM was evaluated in both artificial environments and on the Sydney Victoria Park real-world dataset, with thorough comparison against EKF, UnFS-SLAM, and ICKF. Key findings are as follows:

• RMSE Robustness: LMKF demonstrates superior robustness to changes in robot speed ($\theta_k$0), sensor uncertainties ($\theta_k$1, $\theta_k$2, $\theta_k$3), and path topology (open-loop vs closed-loop). RMSE growth is negligible with parameter variation, in contrast to EKF, ICKF, and UnFS, which exhibit rapid error accumulation with increasing uncertainty.
• Error Variance: The standard deviation of RMSE for LMKF remains significantly lower than for competing methods, evidencing enhanced reliability and stability under repeated trials (up to 50 runs per setting).
• Sensor Dependence: Unlike EKF and UnFS, which are sensitive to steering angle uncertainty ($\theta_k$4), LMKF depends primarily on compass angle uncertainty ($\theta_k$5) and exhibits low sensitivity to this parameter.
• Path Structure: Performance degradation between closed-loop and open-loop paths is minimal for LMKF, with positioning error increases capped at 20% versus dramatic losses in EKF, ICKF, and UnFS.
• Computational Complexity: LMKF achieves superior localization accuracy and mapping consistency with lower computational demands and execution time compared to EKF, UnFS, and ICKF. On the Victoria Park dataset, LMKF delivers RMSE of 1.2m and maximum absolute error of 3.46m, substantially outperforming competitors despite using imprecise reconstructed compass data.

Practical and Theoretical Implications

The LMKF SLAM's reparameterization and reliance on direct orientation measurement represent a paradigm shift for SLAM filtering strategies. Practically, robots equipped with basic electronic compasses can exploit LMKF SLAM for accurate, robust, and computationally efficient mapping and localization across large-scale or feature-dense environments. The model's stability with respect to sensor uncertainty and its minimal requirements on steering sensor input extend its operational versatility to cost-sensitive and less sensor-rich platforms.

From a theoretical perspective, LMKF SLAM illustrates how sensor fusion and judicious state space transformations can circumvent fundamental issues of filter inconsistency in nonlinear stochastic systems. This approach can inform future developments in SLAM, multi-agent state estimation, and real-time spatial mapping in robotics, with potential generalization to high-dimensional or multimodal sensor configurations.

Progress in this direction may enable further simplifications of state representations, expanded applicability to three-dimensional environments, and robust SLAM frameworks for heterogeneous sensor suites. Additionally, the approach directs attention to sensor calibration and measurement reliability, rather than algorithmic complexity, as a key driver of filter performance.

Conclusion

The LMKF SLAM methodology exemplifies how effective transformation and orientation measurement can linearize SLAM dynamics, enabling provable convergence and substantially improving robustness, accuracy, and computational complexity over contemporary filtering-based SLAM methods. As empirical results indicate, LMKF SLAM is highly stable and reliable, with minimal sensitivity to sensor uncertainty and environmental structure, positioning it as a practical and theoretically sound approach for future robotic mapping and navigation systems (2606.28475).