Prediction error method estimation for Simultaneous Localisation and Mapping

This paper presents a batch estimation method for Simultaneous Localization and Mapping (SLAM) using the Prediction Error Method (PEM). The estimation problem considers landmarks as parameter while treating dynamics using state space models. The gradient needed for parameter estimation is computed recursively using an Extended Kalman Filter (EKF). Results using simulations with a monocular camera and inertial sensors are presented and compared to a Nonlinear Least-Squares (NLS) estimator. The presented method produce both lower RMSE's and scale better to the batch length.

I. Introduction

This paper investigates the use of the Prediction Error Method (PEM), see e.g., [1], as a way to utilize the particular structure of Nonlinear Least-Squares (NLS) problems encountered in Simultaneous Localization And Mapping (SLAM). The aim in SLAM is to estimate a moving platform's position and orientation while mapping the observed environment, [2], [3]. A strong trend in SLAM algorithm research is (incremental) batch optimisation which usually solves some form of NLS problem [4]–[8]. These often suffers from poor complexity scaling, usually quadratic in batch length as both motion of the platform and landmarks are considered as parameters. The idea behind using PEM is to model the landmarks as (static) parameters included in the dynamic model used for the platform's motion which is modeled as dynamic states. This is a classic system identification setup and PEM is one, rather standard and successful, way of estimating the parameters of the system [1]. The main advantage of this approach is that problem is split into an optimisation part, where landmarks are estimated, and predictor part, where the output of the system (measurements) is predicted. This is done utilizing the time series nature of data through a filter which also output a complete state trajectory estimate. This property allows also for the gradient of the predictor w.r.t. parameters to be calculated recursively (for a particular choice of the predictor) so that no numerical gradients are necessary. This approach is different from both the standard EKF-SLAM approach, [2], where both the platform's motion and the landmarks are considered as states and NLS approaches where everything is considered as parameters. This split into two parts allows for computation complexity reduction, the optimisation problem is smaller (in the number of parameters) than NLS problem, and the prediction problem is smaller than usual EKF-SLAM problem since the number of states is constant. Basically, the optimisation problem will scale with the number of parameters (landmarks) and the predictor part will scale linearly with the batch length (time steps). This complexity reduction is the main motivation behind using PEM as an estimation method for SLAM, and this will be demonstrated with the empirical experiments comparing PEM and NLS complexities. Since both PEM and NLS approaches are optimisation ones while EKF-SLAM is a filtering approach without gradient or Jacobian calculation and no comparison with it is done.