Comparison of EKF based SLAM and Optimization
based SLAM Algorithms
Yanhao Zhang, Teng Zhang, Shoudong Huang
Center for Autonomous Systems, University of Technology Sydney, Sydney, Australia
Yanhao.Zhang@student.uts.edu.au, Teng.Zhang@student.uts.edu.au, Shoudong.Huang@uts.edu.au
Abstract—This paper compares the recent developed state-
of-the-art extended Kalman filter (EKF) based simultaneous
localization and mapping (SLAM) algorithm, namely, invariant
EKF SLAM, with the nonlinear least squares optimization based
SLAM algorithms. Simulations in 1D, 2D, and 3D are used to
evaluate the invariant EKF SLAM algorithm. It is demonstrated
that in most 2D/3D scenarios with practical noise levels, the
accuracy of invariant EKF is very close to that of nonlinear
least squares optimization based SLAM. In the simple 1D case,
the Kalman filter results and the linear least squares results are
exactly the same (for any noise levels) due to the linear motion
model and linear observation model involved.
Index Terms—EKF, optimization, SLAM, Lie group, perfor-
mance.
I. INTRODUCTION
SLAM problem [1] asks whether it is possible for a robot to
build a map of an unknown environment and simultaneously
work out its own location within the map, using informa-
tion gathered from sensors mounted on the robot. Reliable
solutions to SLAM underpin successful robot deployment in
many application domains especially when an external location
reference such as a global positioning system (GPS) is not
available. In point feature based SLAM, the map is represented
by a sparse set of point features, and the solution to SLAM is
an estimate of the observed point features and the latest robot
pose, together with the associated estimate uncertainty.
Both EKF [1] and least squares optimization [2] have been
used extensively in SLAM research in the past. Earlier SLAM
research has used EKF where the state vector contains the
latest robot pose and the positions of the features observed.
However, it has been shown that EKF SLAM could result in
inconsistent estimate [3] [4], here inconsistency means that
the estimated covariance from the algorithm can violate the
theoretical achievable lower bounds [1] [5].
Optimization based SLAM uses a state vector containing
all the robot poses and all the features observed. Because re-
linearization is performed during each iteration step, there is
no inconsistency issue in optimization based SLAM and thus
the quality of the estimate is higher than that of EKF SLAM.
However, when the robot trajectory is very long, the dimension
of the state vector in optimization based SLAM is very high.
Recently, in [6], a 2D invariant EKF SLAM algorithm was
proposed and it was demonstrated to perform much better than
traditional EKF SLAM. Furthermore, in [7], some basic con-
vergence properties of the 3D invariant EKF SLAM algorithm
were proved, without the requirement that the Jacobians were
evaluated at the ground truth as in [5]. It is also pointed out that
the invariant EKF SLAM satisfies two necessary conditions
that a consistent SLAM algorithm should satisfy, namely,
(i) the estimate (relative position and orientation) value is
invariant to the initial robot pose, and (ii) the estimate value is
invariant to the initial robot pose uncertainty. This new version
of EKF SLAM appears to overcome the fundamental limitation
of the traditional EKF SLAM algorithms.
Although optimization based SLAM algorithms are becom-
ing popular recently due to the high quality performance
and the efficiency of the modern sparse solvers [8] [9], they
may not always be the best algorithm to use in practice
due to the accumulated length of the state vector, especially
for the scenarios when a robot continuously operates in an
environment with fixed size. Thus it is interesting to compare
the performance between the state-of-the-art invariant EKF
SLAM with optimization based SLAM algorithm for different
scenarios.
This paper focuses on the accuracy comparison between
invariant EKF SLAM and optimization based SLAM. We
started from the 1D linear case using Kalman filter (KF) and
linear least squares (LLS), then we analyzed 2D/3D nonlinear
cases using right invariant EKF (RI-EKF) method [7] and
non-linear least squares (NLLS) method. We numerically
confirmed that the KF and LLS results for 1D SLAM are
always exactly the same no matter how big the noise level is
and no matter what operation scenario is. For 2D/3D cases,
in most practical scenarios with reasonable noise levels, the
RI-EKF results are very close to that of the NLLS results.
This paper is organized as follows. Section II discusses
some related work. In Section III, the motion model and
observation model of SLAM problem in 1D/2D/3D are s-
tated, and the RI-EKF SLAM algorithm and least squares
optimization based SLAM algorithm are briefly reviewed. In
Section IV, the RI-EKF SLAM and optimization based SLAM
are compared using 1D/2D/3D simulations for different noise
levels and different simulation environments. Finally, Section
V concludes the paper and outlines the future work.
Throughout this paper, bold lower-case and upper-case
letters are reserved for vectors and matrices, respectively. R
represent the set of real numbers. SO(2), SO(3) are the
special orthogonal groups. The square of Euclidean norm is
denoted by ·
2
. The square of weighted Euclidean norm
of vector e with a positive definite matrix W is denoted by
e
2
W
:= e
W
−1
e.
1308
978-1-5386-3758-6/18/$31.00
c
2018 IEEE