Comparison of the SLAM algorithms: Hangar experiments

. This study purposes to compare two known algorithms in an application scenario of simultaneous localization and mapping (SLAM) and to present issues related with them as well. Mostly used SLAM algorithms Extended Kalman Filter (EKF) and Unscented Kalman Filter (UKF) are compared with respect to the point of accuracy of the robot states, localization and mapping. Because of considering the most implementations in the previous studies, the simulation environments are chosen as big as possible to provide reliable results. In this study, two different hangar regions are tried to be simulated. According to the outcomes of the applications, UKF-based SLAM algorithm has superior performance over the EKF-based one, apart from elapsed time.


Introduction
Autonomous mobile robots should perform the behaviours or any given tasks with a high degree of autonomy.It is vital for guiding the robot in a desired manner.Therefore, a robot has to know answers of two critical questions: "Where am I?", "What does the world look like?".The first question addresses to the problem which is known as a localization problem.According to the localization problem, a mobile robot needs to know its position (location, orientation) to achieve the goal [1].On the other hand, the second question is related with the environmental information.The robot seeks the answer of what is the world around its.This problem is known as a mapping phenomenon, in the literature.In this problem, a robot tries to build a map or a plan of its environment with an acquired sensor data [2].
In addition to the above mentioned parts, it is good to specify that robot positions and environment information are known for mapping and localization problem, respectively.With these thought, simultaneous localization and mapping (SLAM) problem addresses to the problem that lacking of position and surrounding data.Accordingly, SLAM by a mobile robot is concerned with the problem of creating a map of an unknown environment.The robot has its on-board sensors while simultaneously determining its location using the created map.Because of the fact that, truly autonomy system needs to location and position of features of robot and environment, SLAM problem solution is seen a "holy grail" for the mobile robotic community [3].The background of SLAM problem is based upon the paper of Smith et al. [4] by using stochastically approach to the problem.After that, SLAM problem has attracted distinct attention from the many researches and mobile robotics communities in the past twenty five years [3,5].
From emerging to now SLAM has been found widely usage area.Early studies, still ongoing, of SLAM are performed by using EKF [6], then particle filter based solutions are shown up [7], after that using of the information-state approaches have come up [8].Kalman filter based solutions have been still remained on the researchers' agenda.On the other hand, visual SLAM based approaches have been aroused attention from many studies on this field [9].
For the solution of SLAM problem, plenty of techniques has been applied, but among them Bayesian based approaches have founded the most useful and successful ones.According to the Bayesian method, sensor data observed from the robot are expressed thorough a conditional probability distribution.Then, Bayesian filter uses these distributions as an input and performs two phases that are prediction and correction steps to solve the problem.In the prediction phase, robot motion model is taken into consideration and next state of the robot is predicted.After that, related observations are taken by sensors and used to correct the states.
The structure of the paper is as follows: First, the mathematical probabilistic definitions on SLAM problem is described to show the bases of it in section II.Thereafter, in section III, used robot dynamics, which is based upon Ackermann Modell, are shown.Moreover, EKF and UKF based SLAM algorithms' foundation are elucidated in depth and state equations have been revealed.Finally, implementation of the algorithms is realized using the thought simulation environments and Root Mean Squared Error (RMSE) and true-estimated paths are pointed out.

SLAM problem
SLAM is an exploration of environment while simultaneously tracking the robot's pose.In order to overcome the problem, probabilistic approaches have been applied successively, up to now.SLAM problem is defined in two manners, which are online SLAM (eq. 1) and full SLAM (eq.2).
‫ݔ(ܲ‬ , ݉|ܼ ଵ: , ܷ ଵ: ) (1) ‫ݔ(ܲ‬ ଵ: , ݉|ܼ ଵ: , ܷ ଵ: ) Full SLAM can be distinguished from the online SLAM only if the posterior are calculated over the entire path ‫ݔ‬ ଵ: along with the map.In the equations, x represents the location and orientation of the robot, ܷ is a control vector, ݉ denotes the i th feature location and ܼ is an observation taken via robot's sensor.
As a result of Bayes' theorem and assumptions that the robot motion model is Markov and the landmarks are time invariant in the environment.With this thought, SLAM formula can be reproduced as in the eq. 3 [10,3].
ߟ is a normalizing constant which comes from assumptions stage.From the point of probabilistic view, calculation of the probabilistic equations are in need of motion and observation models that possesses the robot (eq.4-5).The motion model is obtained from the robot's dynamic equation while the observation one is derived according to the sensor readings.

‫ݔ(ܲ‬ ‫ݔ|‬
The eq. 3 tells that two stages are implemented to find SLAM problem solution: time update (prediction), prior state estimation and robot motion model are combined; measurement update (correction), this calculation is corrected and associated with the sensor observation and finally result is normalized to appropriate interval.

Robot model and Algorithms
In this section, Ackermann steering geometry that is used for the robot model and EKF-UKF algorithms are explained in detail.

Robot Model
In the study, used robot motion model has Ackermann steering geometry.It is generally accepted having poor kinematic model but also is a good representation for slow motions [11,12].Following equations point out the motion and observation models (( 6), ( 7)), respectively.
The equations above are summarized with the following definitions: The eq. 6 is called velocity motion model and the other one, 7, represents the range-bearing observation model (Fig. 1 and Fig. 2).

EKF-based SLAM
Extended Kalman Filter (EKF) is the oldest and one of the mostly used techniques to implement SLAM problem, in the literature.EKF-based SLAM algorithm is a result of Bayesian Filter and extension of Kalman Filter (KF) for the form of nonlinear cases.The main goal of the EKF is to find posterior distribution of the mean and covariance ((8), ( 9)) by applying the prediction and correction steps [13,3,14].
In the time update or prediction step, robot motion model and robot's mean-covariance are used to predict the mean and covariance, at time k-1.At this step, used robot motion model ( ‫ݔ(ܲ‬ ‫ݔ|‬ ିଵ , ܷ )) is described in the eq.10.
According to this step, predicted mean and covariance are evaluated with the following equations (( 11), ( 12)).
The most prominent part of the EKF is ∇݂ , which means Jacobian of ݂ calculated at time k-1.
The second stage is a measurement update or correction that provides with the estimation of the prediction via obtained measurements and measurement model.Measurement model of the robot ‫ݖ(ܲ(‬ ‫ݔ|‬ , ݉)) can be thought as an equation in 13.
In the equation h (•) denotes the geometry of the measurement, which is similar to motion model, and ‫ݒ‬ is an additive, zero mean Gaussian disturbance ‫ݒ(‬ ~ࣨ(0 , ܴ ) ). Predicted measurement are calculated from the (13) after that the important points that are innovation and its covariance are obtained with respect to real measurements and predicted values (( 14), ( 15)).
As in the ∇݂, ∇h is a Jacobian of the ℎ evaluated at preceding step.After from all these steps, estimation is finished (( 16), ( 17)), states are corrected and if necessary next state can be repeated using the same steps.
K is a Kalman gain and calculated as the following (18).

UKF-based SLAM
Unscented Kalman Filter (UKF) is an alternative way to implement SLAM problem.UKF is mainly based on unscented transform.The main idea on the UKF is to express the probability distribution with random variables, so called sigma points.Instead of using nonlinear function to define probability distribution by Taylor expansion, it is chosen 2݊ + 1 sigma points from the Gaussian (19). ߯ where λ is defined, α (usually set to 1݁ − 4 ≤ ߙ ≤ 1 ) and k (generally chosen 0) are the scaling factors that show the spreads of sigma points from the mean.
In the light of unscented transform information, mentioned sigma points are replacement in the motion model ( 21) and predicted mean and covariance are calculated according to the equations ( 22), (23), respectively.
(26) is called innovation and ( 27) is an innovation covariance.After the measurement, corrected mean and covariance at time k-1 are evaluated with respect to ( 16) and (17) as in the same EKF-based SLAM.In the above equations, K is a Kalman gain (28) and is ܲ ௫௭ cross correlation matrix (29).
As it is seen in the formulas UKF-based SLAM differs from the EKF one that is not include Jacobian calculation which generally provides an advantage of easy calculation [15,16].

Simulation results
The proposed SLAM algorithms both EKF and UKF are simulated using the MATLAB program.In the simulation, the robot dynamic is thought as Ackermann model.The robot wheel base is set to 4 meters, speed is 3 m/s.In addition to this, sensor maximum distance is accepted as a 30 meters and time interval, ∇t, between k and k-1 is supposed to be 0.025 seconds.
In the study, two different scenarios are implemented that are hangar_1 (square low) and hangar_2 (cycloid).For the fair comparison, each algorithm is run ten times and average RMSE criteria results are indicated in the table 1 and 2, for each experiment.

Conclusion
EKF is one of the best and classical algorithm to the solution of SLAM problem.Although its easy implementation and effectiveness are verified various studies, new solution to SLAM problem are required.Besides this, UKF is one of the mostly used techniques and powerful solution to the SLAM problem.In this study, both EKF and UKF algorithms are applied to the big size environment to see the effectuality of the algorithms.First of all, simulation environments are generated taking consideration to the square low and cyclical hangar plans.After that, landmarks are established nearly each ten meters and robot waypoints are determined to optimal vision of sensor.Finally, produced data files are loaded to the workspace and algorithms are tested navigating the robot through waypoints.Both position error and heading error evaluation results show the superiority of the UKF to EKF (table 1, 2).While the RMSE of the robot pose is superior from the point of the UKF, elapsed time to complete the route is not as good as in EKF.
The simulated maps are indicated in the figures, 3 and 4. According to the simulation, hangar environments are nearly chosen in the size of 200 by 200 meters to see the effectiveness of the algorithms in a big size region.It is approximately placed a special feature, called also landmark, in each ten meters.

Figure 5
Figure 5 and 6 are the results of the algorithms' estimated path versus true robot path.In addition to previous results these figures also show the supremacy of the UKF-based algorithm, which is the closest one to the true path.