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

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

Problem Statement
Simultaneous Localization and Mapping (SLAM) is a fundamental task for mobile robots, yet standard implementations based on the Extended Kalman Filter (EKF) suffer from divergence and inconsistency. The primary causes of these failures are the nonlinearity inherent in robot motion and observation models, which necessitates linearization. This linearization introduces errors that accumulate over time, leading to incorrect state vector updates, jumps in estimated positions, and a reduction in the estimated covariance matrix that does not reflect true uncertainty. While various methods such as Fast SLAM, Unscented Fast SLAM (UnFS), and Cubature Kalman Filter (CKF) variants have been proposed to mitigate these issues, they often rely on complex approximations or still struggle with consistency in large environments or under high sensor uncertainty.

Methodology
The paper proposes a novel approach termed LMKF SLAM (Linear Model Kalman Filter SLAM). The core methodology involves transforming the standard nonlinear state-space model into a strictly linear model, thereby allowing the direct application of the standard Kalman Filter (KF) without the need for linearization approximations or Jacobian calculations.

1. State Space Transformation:

   • Motion Model: The authors assume the robot is equipped with a simple electronic compass to measure the robot's absolute angle ($\theta_k$) at each step. By incorporating this measurement, the state vector is reduced to exclude the orientation angle. The motion equations, originally nonlinear due to trigonometric functions of $\theta_k$, are transformed. The noise terms are linearized around zero, resulting in a linear motion model where the state update depends linearly on the previous state and control inputs.
   • Observation Model: Similarly, the observation model, which relates range ($r_k$) and bearing ($\phi_k$) to landmark positions, is transformed. By utilizing the measured robot angle $\theta_k$, the polar coordinates of the landmarks are converted into Cartesian coordinates relative to the global frame. This transformation ($\zeta_k$) linearizes the relationship between the observed data and the state vector (robot and landmark positions).

2. Filtering Algorithm:

   • With both motion and observation models linearized, the standard Kalman Filter is applied directly.
   • The algorithm employs sequential processing for landmark updates to increase computational speed, treating observations from different landmarks as independent.
   • The state vector consists of the robot's $(x, y)$ coordinates and the coordinates of all observed landmarks.

Key Contributions

• Linearization via Transformation: The paper introduces a simple transformation that converts nonlinear SLAM models into linear ones using only a standard electronic compass, eliminating the need for complex linearization techniques like EKF Jacobians or Unscented Transformations.
• Theoretical Convergence: Unlike EKF-based methods where convergence proofs are difficult due to nonlinearities, the proposed linear model allows for a rigorous proof of convergence, guaranteeing that the absolute positioning error of landmarks tends toward a lower bound.
• Robustness: The method is designed to be highly stable with respect to sensor uncertainties (speed, steering angle, and robot angle) and changes in system parameters.

Experimental Results
The authors evaluated LMKF SLAM against standard EKF, UnFS, and Improved CKF (ICKF) methods using both artificial simulations and the real-world Victoria Park dataset.

• Simulation (Artificial Environment):

  • Tests were conducted on both closed-loop and open-loop paths.
  • Accuracy: LMKF demonstrated significantly lower Root Mean Square Error (RMSE) for both robot and landmark positioning compared to EKF, UnFS, and ICKF.
  • Robustness: The performance of LMKF remained stable even when robot speed ($v$) and sensor noise ($\sigma_G, \sigma_\theta$) were increased. In contrast, the RMSE for EKF and ICKF grew rapidly with increased noise and speed.
  • Path Dependency: While other methods showed significant error reduction in closed-loop paths (due to loop closure correction), LMKF maintained high accuracy in open-loop paths, showing that its performance is less dependent on the existence of closed loops.
  • Stability: The standard deviation of the RMSE ($\sigma_{RMSE}$) was lowest for LMKF, indicating higher reliability across different noise sequences.

• Real-World Data (Victoria Park Dataset):

  • Using a dataset with a 2D laser scanner and GPS, the authors estimated the missing robot angle data using GPS and path information.
  • Despite the imprecise estimation of the robot angle (a disadvantage for the proposed method), LMKF achieved the lowest RMSE (1.2 m) and Maximum Absolute Error (3.46 m) compared to EKF (2.58 m), UnFS (2.1 m), and ICKF (2.33 m).
  • LMKF also required the least execution time (45.43 seconds) compared to the other methods.

Significance and Claims
The paper claims that the proposed LMKF SLAM method represents a significant improvement over state-of-the-art filtering methods. By achieving a linear model through a simple transformation, the method guarantees convergence and offers superior accuracy and stability. The authors emphasize that LMKF is particularly robust against sensor uncertainties and system parameter changes, outperforming EKF, UnFS, and ICKF even under conditions where the competing methods were theoretically favored (e.g., when simulation parameters were tuned to benefit the nonlinear models). The study concludes that this approach effectively addresses the divergence issues common in EKF-based SLAM without requiring complex computational overhead.