# Simultaneous Localization and Mapping Based on Kalman Filter and Extended Kalman Filter

Authors: Inam Ullah; Xin Su; Xuewu Zhang; Dongmin Choi
Journal: Wireless Communications and Mobile Computing
 (2020)

---

## Abstract

For more than two decades, the issue of simultaneous localization and mapping (SLAM) has gained more attention from researchers and remains an influential topic in robotics. Currently, various algorithms of the mobile robot SLAM have been investigated. However, the probability-based mobile robot SLAM algorithm is often used in the unknown environment. In this paper, the authors proposed two main algorithms of localization. First is the linear Kalman Filter (KF) SLAM, which consists of five phases, such as (a) motionless robot with absolute measurement, (b) moving vehicle with absolute measurement, (c) motionless robot with relative measurement, (d) moving vehicle with relative measurement, and (e) moving vehicle with relative measurement while the robot location is not detected. The second localization algorithm is the SLAM with the Extended Kalman Filter (EKF). Finally, the proposed SLAM algorithms are tested by simulations to be efficient and viable. The simulation results show that the presented SLAM approaches can accurately locate the landmark and mobile robot.


---

## Body

## 1. Introduction

Wireless sensor networks (WSNs) grasp the potential of various new applications in the area of management and control. Examples of such applications include detection, target tracking, habitation monitoring, catastrophe management, and climate management such as temperature and humidity. The key technology that drives the development of sensor applications is the quick growth of digital circuit mixing. In the recent future, these applications will provide a small, cheap, and efficient sensor node. Localization is also crucial for various applications in WSNs. In both universal computing and WSNs, there has been considerable consideration of localization [1, 2]. Characteristically, the WSN system offers the range and/or bearing angle measurements between each landmark and vehicle.Mobile robot localization is also one of the attractive researches that support a truly self-governing mobile robot performance. Various independently working robots can accomplish tasks more rapidly in many situations. However, there is a possibility of even better productivity gains if robots can work cooperatively. The capability to collaborate is dependent on the robot’s capability to connect and communicate with each other’s. In this work, the authors consider the procedure of simultaneous localization and mapping (SLAM). For this purpose, a linear Kalman Filter (KF) with SLAM and Extended Kalman Filter (EKF) with SLAM are applied [3, 4].SLAM plays a key role in the field of robotics and especially in a mobile robot system. The key objective of SLAM is to jointly measure the position of the robot as well as the model of the surrounding map [5–7]. For the safe interaction of robots within the operation area, this information is important. A variety of the SLAM algorithm has been presented over the last decade. Most of the early algorithms for SLAM used a laser rangefinder [8] which works as the core sensor node, and visual sensor nodes are the most used option currently, whichever is active or passive [9, 10]. In contrast to a laser rangefinder, currently, small, light, and affordable cameras can offer higher determination data and virtually unrestricted estimation series. These cameras work as passive sensor nodes and, therefore, do not affect one another while deploying in similar operation areas. Distinct in the designed light range sensor nodes, cameras are also able to apply for both interior and exterior situations. Therefore, such features can make the camera the best choice for mobile robotic platforms and SLAM.In SLAM, the need for using the environment map is twofold or double [11, 12]. The first one is the map often essential to support or back up other responsibilities; for example, a map can notify a track arrangement or offer an initiative imagining for a worker. Secondly, the map or plot follows in restraining the fault performed in measuring the state of the robot. While without a map, the dead reckoning would rapidly point energetically. On the other hand, by using a map, for example, a set of distinct landmarks, the robot can reorganize its localization error by reentering the known areas. Therefore, SLAM applications are more useful in such situations in which a preceding plan is not existing and require to be constructed. In some aspects of the robots, a set of landmark location is known prior. For example, a robot is operational on the floor of a workshop that can be supplied with a physically assembled chart of artificial guidelines in the operation area. Alternatively, in another case, in which the robot has admittance to the global positioning system (GPS), the GPS satellite can be chosen as a moving beacon at a prior known position. In this case, the SLAM may not be needed if the localization is done consistently concerning the prior known landmark of the robot. Through the development of indoor localization uses of mobile robots, the popularity of SLAM is increased. Most of the indoor procedures rule out the practice of GPS to assure the error of localization. The SLAM algorithm also provides an interesting substitute to the maps which is built by the user, which represents that the process of the robot is also conceivable in the nonappearance of ad hoc networks for localization [13].KF derivatives are concerned with the first branch of those methods which apply a filter [14, 15]. The KFs assume that Gaussian noises affect data, which is not inevitably accurate in our case. KFs are planned to solve the problems of linear systems in their basic form and are rarely used for SLAM, although they have great convergence properties. On the other hand, in the nonlinear filtering systems such as in SLAM, the EKF is a common tool. EKF introduces a step of linearization for the nonlinear systems, and a first-order Taylor expansion performs linearization around the current estimate. The optimality of EKF’s is shown as long as linearization is performed around the state vector’s exact value. It is the value to estimate in practice and is therefore not usable, and this can lead to problems of accuracy. Nonetheless, estimates are close enough to the reality, for the most part, to allow the EKF to be used.KF is Bayes filters which signify posteriors by using the Gaussians [16], for example, the distributions of unimodal multivariate that can be denoted efficiently by a minor sum of parameters. The KF SLAM is based on the hypothesis that the transformation and estimation functions are linear with the introduction of Gaussian noise. In state-of-the-art SLAM, KF has two main variations. The first one is the EKF, and the second one is the information filtering (IF) or EIF. The EKF is usually applicable for the nonlinear functions by approximating the mobile robot motion model by means of linear functions. A variety of the SLAM algorithms use the EKF and IF applied by propagating the state error covariance inverse [17–19]. IF is more advantageous as compared to the KF. Initially, the information is filtered out by summing the vector and matrices of information which resultantly give a more precise estimate. Next, the IF is steadier than the KF. Lastly, the EKF is comparatively slow while estimating the maps of having dimensions, because the measurement of every vehicle normally affects the Gaussian parameters. Consequently, the updates need prohibitive times when faced with a situation having several landmarks.In recent years, the SLAM and autonomous mobile robot combinations play an important role in the controlling disaster field. Particularly, the autonomous robots are widely used for the maintenance and rescue operations in the disaster controlling such as radioactivity leaks. Since the area is unreachable, simultaneous mapping of the environment and the robot localization is crucial to determine the exact source spot [20–23]. Therefore, SLAM has been an important issue as the localization degree hangs on active mapping. However, the SLAM implementation by using the EKF is pretty exciting because of the approximation of the sensor noises and real-time stochastic system as Gaussian. Therefore, inappropriate alteration of the noise covariance may result in filter divergence over time, resulting in the complete system becoming unstable. The researchers presented some alternate methods that are moderately straightforward but severe computationally which have the benefit to accommodate the noise model other than the Gaussian such as UKF, FastSLAM, and Monte Carlo localization [24–26].


### 1.1. Contributions

In the above paragraphs, the authors investigated the SLAM with KF and EKF. The performance of such models under localization is not yet well-thought-out. Therefore, in this work, the authors analyzed SLAM by suing linear KF and EKF. The basic contribution of this work included one dimensional (1D) SLAM using a linear KF (a) motionless robot with absolute measurement, (b) moving vehicle with absolute measurement, (c) motionless robot with relative measurement, (d) moving vehicle with relative measurement, and (e) moving vehicle with relative measurement while the robot location is not detected. Furthermore, the authors analyzed the localization performance of SLAM with EKF. The proposed SLAM-based algorithms are evaluated and compared with each other and also with other algorithms regarding SLAM. More precisely, the proposed SLAM algorithms present good accuracy while maintaining a sensible computational complication.


### 1.2. Organization of the Paper

The structure of this paper is as follows: Section2 demonstrates the work related to SLAM and Section 3 demonstrates the proposed SLAM algorithms. The subsections of Section 3 are SLAM with KF and SLAM with EKF, respectively. Section 4 demonstrates the comparison of the proposed and other algorithms. Finally, Section 5 demonstrates the conclusion and future direction of the proposed algorithms.


## 1.1. Contributions

In the above paragraphs, the authors investigated the SLAM with KF and EKF. The performance of such models under localization is not yet well-thought-out. Therefore, in this work, the authors analyzed SLAM by suing linear KF and EKF. The basic contribution of this work included one dimensional (1D) SLAM using a linear KF (a) motionless robot with absolute measurement, (b) moving vehicle with absolute measurement, (c) motionless robot with relative measurement, (d) moving vehicle with relative measurement, and (e) moving vehicle with relative measurement while the robot location is not detected. Furthermore, the authors analyzed the localization performance of SLAM with EKF. The proposed SLAM-based algorithms are evaluated and compared with each other and also with other algorithms regarding SLAM. More precisely, the proposed SLAM algorithms present good accuracy while maintaining a sensible computational complication.


## 1.2. Organization of the Paper

The structure of this paper is as follows: Section2 demonstrates the work related to SLAM and Section 3 demonstrates the proposed SLAM algorithms. The subsections of Section 3 are SLAM with KF and SLAM with EKF, respectively. Section 4 demonstrates the comparison of the proposed and other algorithms. Finally, Section 5 demonstrates the conclusion and future direction of the proposed algorithms.


## 2. Related Work

Before presenting the proposed SLAM algorithms, it would be better to present some background knowledge and related work on SLAM algorithms. In this section, the authors present a detailed description of the SLAM that forms the basis of the proposed SLAM algorithms. Compared to the current solutions, many people still do not have highly accurate instruments; they still have challenging piloting capabilities and can solve the SLAM problem. Their mapping, therefore, depends on the toughness policy of acting as a replacement for the accurate world definition. With linear KF, this approach is a new research concept for SLAM.Regarding the SLAM, readers may not be familiar with the origin and its derivation may refer to the standard and current work on SLAM [27, 28]. For the SLAM problem, the first method was introduced between 1986 and 1991. Smith and Chesseman [29] published a paper in 1986 for the solution of SLAM problems. They present the EKF to solve this problem. In that paper, they established a numerical basis for explaining the relation between landmarks and operating the geometric uncertainty.Several other researchers have worked on various SLAM issues. For example, in [30–32], the authors presented a new architecture that applies one monocular SLAM system for the tracking of unconstraint motion of the mobile robot. The improved oriented FAST and rotated BRIEF (ORB) characteristics show the landmarks to design a network feature procedure of detection. An enhanced matching feature system has enhanced function matching strength. The updated EKF measures the free-moving visual sensor’s multiple dimensional states rather than the standard EKF. Furthermore, in [33, 34], the authors address the issue of the applications of SLAM for navigation problems. For the solution of high-accuracy problems, an EKF or particle filter (PF) algorithm [35] is frequently applied to the processing of data. The PF algorithm, which is often applied for the G-mapping SLAM technique, is well-matched for the nonlinear system’s investigation. Though, PF computational dimensions are larger than those of EKF. Therefore, EKF and PF also have some disadvantages in the process of navigation. The Gaussian smoothing filter and its modification are used which is based on the distributed computing scheme. This algorithm is meaningfully better to the EKF and PF algorithms regarding the computational speed.For the reduction of the linearization error of KF algorithms, the authors presented three techniques and their viability and efficiency are assessed by SLAM [36]. In the derivative-based approaches of the KF system, the linearization error is undetectable owing to the practice of the Taylor expansion for the linearization of the nonlinear motion process. The presented three techniques reduce the error of linearization by substituting the Jacobian observation matrix with new formulations. Similarly, in [37], a SLAM with limited sensing by applying EKF is proposed. The robot’s problem with creating a map of an unidentified atmosphere while adjusting its particular location which is the basis on a similar map and sensor information is called SLAM. Because sensor accuracy plays a major part in this issue, most of the planned schemes comprise the use of high-priced laser sensor nodes and comparatively innovative and inexpensive RGB-D cameras. These sensors are too costly for some applications, and RGB-D cameras consume much power, CPU, or communication specifications for on-board or PC processing of data. Thus, the authors tried to model an uncertain setting using a low-cost device, EKF, and dimensional features such as walls and furniture.Usually, the typical filter uses the scheme model and former stochastic info to approximate the subsequent robot state. Though in the real-time condition, the sound statistics possessions are comparatively unidentified, and the system is imprecisely demonstrated. Therefore, the filter deviation might arise in the incorporation scheme. Furthermore, the predictable precision might be stimulating to be grasped due to the nonappearance of the receptive time-varying of mutually the process and measurement noise statistic. So, the outdated approach desires to be upgraded pointing to deliver an aptitude to guesstimate those belongings. To solve this problem, the new adaptive filter is proposed in [38] named as an adaptive smooth variable structure filter (ASVSF). The upgraded SVSF is consequential and executed; the process and measurement noise statistics are appraised by using the maximum a posteriori creation and the weighted exponent concept. The authors applied ASVSF to overwhelm the SLAM problem of a self-directed mobile robot; hereafter, it is shortened as an ASVSF-SLAM algorithm.In [39], the authors presented a 3D cooperative SLAM for a joint air grounded robotic system which is intended to succeed an indoor quadrotor flying done composed with a Mecanum-wheeled omnidirectional robot (MWOR) in indoor unidentified and no GPS environments. Moreover, an Oriented Fast and Rotated BRIEF- (ORB-) SLAM 2.0 method is applied to yield a 3D chart and determine concurrently the location of the indoor quadrotor, and a particle-filter SLAM (FastSLAM 2.0) method is applied to shape the 2D chart of the universal atmosphere for the MWOR. An additional accurate 3D quadrotor location estimation technique for the quadrotor is planned with the help of the MWOR. A cooperative SLAM applying fuzzy Kalman filtering is presented to fuse the productions of the ORB-SLAM 2.0, FastSLAM 2.0, and quadrotor location estimation methods, in order to localize the quadrotor further precisely. Mutually, SLAM methods, quadrotor position estimation method, and cooperative SLAM have been executed in the robotic operation system atmosphere.Recent work on SLAM [40] attempted to address the issue of SLAM landmarks [41]. The authors presented an AUV vision-based SLAM, in which the submerged nonnatural landmarks are utilized for visual sensing of onward and down cameras. The camera can also estimate the AUV location data, along with several navigation sensor nodes such as depth sensor node, Doppler velocity log (DVL), and an inertial measurement unit (IMU). The landmark detection algorithm is organized in a framework of conventional EKF SLAM to measure the landmark and robot status. Furthermore, partial observability of mobile robot based on EKF is explored in [42, 43] to find the answer that can avoid erroneous measurements. When considering only certain environmental landmarks, the computational costs of mobile robots can be minimized, but with an increase in device uncertainties. The fuzzy logic methodology is presented to guarantee that the calculation has attained the desired output even though some of the landmarks have been omitted for reference purposes. For the measurement invention of KF, fuzzy logic is used to exact the location of the mobile robots and any sensed landmarks all throughout the process of observations.Researchers have proposed several algorithms for SLAM; some of which are already discussed in the above pages. Most of them focused on the landmark’s estimation, performance, accuracy, and effectiveness of the SLAM algorithm. However, there are still some important and fundamental issues that need to be addressed, such as an optimal solution for SLAM, active SLAM for SLAM development, SLAM failure detection, SLAM front end robust algorithm, and SLAM algorithm that considers various aspects at once. Therefore, in this paper, the authors attempted to propose a modified SLAM algorithm by applying KF and EKF. The authors presented SLAM algorithms that consider several aspects of the SLAM such as velocity, distance, coverage area, maximum range, and localization time. The notations used in this work are listed in Table1.Table 1
Index of notation.
NotationDescriptionXkCurrent stateXk−1Previous stateQkCovariance matrix of predictionRkCovariance matrix of observationXk+1Estimated state vectorPk+1/kCovariance matrix for predictionQProcess noise matrixRMeasurement noise matrixwk∼N0,QkProcess noisevk∼N0,RkObservation noiseK+1Time instantZk+1Estimated measurement vector∇Fx, ∇FuJacobian matrices of the functionfB, FState transition matrixUInput of the process noiseW, VVectors of state measurement noisevMobile robot velocitypLandmark positionJFJacobian of state equationKk+1Kalman gainXk+1/k+1Updated estimatePk+1/k+1Updated covarianceHMeasurement Jacobian or linearization matrixdtGlobal timeTTimetInitial timeLMLandmarkPkNew state covariance matrix


## 3. Proposed Simultaneous Localization and Mapping Algorithms

This section presents the proposed SLAM algorithms based on KF and EKF. The proposed algorithms are analyzed and evaluated in the next subsections.


### 3.1. Simultaneous Localization and Mapping with Kalman Filter

In the following section, the authors presented the theory of SLAM which results in efficient localization and mapping in WSNs. Specifically, the author presents the analysis of the operating environment and finally discussed the proposed algorithm and compared it with other SLAM algorithms. In the existence of Gaussian white noise, the KF provides a well-designed and statically optimum explanation for the linear systems. It is a technique that uses linear estimation associated with the states and error covariance matrixes for the purpose to produce gain stated to as the Kalman gain. Such benefit is added to the estimation of a preceding condition, thereby generating an estimate of a posteriori [44]. The below equations define the dynamic model of the system and the measuring model used for the linear state approximation in general which consists of two f and h functions.
(1)Xk+1=fXk,Uk,Wk,(2)Zk+1=hXk,Vk,which administrate state proliferation and state measurements, where U is the input of the process, W and V are the vectors of state and measurement noise, while k represents the discrete-time. In the above equations, f and h are typically based on a set of discretized difference equations that govern the dynamics and observation from the method.
(3)X˘k+1/k=Fk×X˘k/k,(4)Pk+1/k=Fk×Pk/k×FkT+Ak×Qk×AkT,(5)Kk+1=Pk+1/k×HkTHk+1×Pk+1/k×Hk+1T+Rk−1,(6)X˘k+1/k+1=X˘k+1/k+Kk+1Zk+1−Hk+1×X˘k+1/k,(7)Pk+1/k+1=I−Kk+1×Hk+1×Pk+1/k.Such equations from the KF-based method are used iteratively in conjunction with Equations (1) and (2). Equation (3) generalizes the prior state estimate, and Equation (4) represents the equivalent state covariance error. The gain of Kalman can be estimated by Equation (5) which is applied to update the state approximation and covariance error, defined by Equations (6) and (7), correspondingly. EKF is practically comparable to the iterative KF method, and sometimes, it is used for the nonlinear systems. By applying the Jacobian, which is a first-order partial derivative, the measurement Hk+1 and nonlinear system Fk matrices are linearized.A one-dimensional SLAM with KF is applied for a motionless robot, and the measurement is considered an absolute measurement. A 1-DoF mobile robot is used which is motionless in a fixed position of a straight line. The mobile robot is used for detecting the motionless/stationary landmarks. The mobile robot velocityv and position p of the landmarks are calculated by applying SLAM with linear KF. Ten numbers of landmark positions are considered. For the real trajectory, the robot is motionless at a given position which is v=0m/s. The landmark distance is relative to the mobile robot’s location/position which had a moderate measurement noise as shown in Figure 1. The state vector is the diagonal of those that correspond to the robot’s present state by projecting the next one. The vector used for the control is null; it shows that there are no exterior inputs to vary the mobile robot’s state; i.e, the velocity and position of the robot are constant. The initial matrix of covariance is not prevalent; it is characterized by a broad diagonal ambiguity in both the robot’s landmark location and state and equal ambiguity/uncertainty. The result of mobile robot localization with absolute measurement is shown in Figure 2.Figure 1
SLAM with motionless robot and absolute measurement while having a moderate measurement noise.Figure 2
SLAM with motionless robot and absolute measurement.Next, a one-dimensional SLAM with KF is applied for a moving vehicle and the measurement is considered an absolute measurement. A 1-DoF mobile robot is traveling on a straight path. The mobile robot is sensing the motionless/stationary landmarks. The robot velocityv and the landmark position/velocity p are calculated by applying SLAM using a linear KF, and in this case, all the measurements are absolute, see Figure 3. The landmark positions are the same as the previous one.Figure 3
SLAM with moving vehicle and absolute measurement.Furthermore, a one-dimensional SLAM with KF is applied for a motionless robot, and the measurement is considered a relative measurement. Here, a 1-DoF mobile robot is used in a motionless and fixed position of a straight lane that detects the motionless/stationary landmarks. The robot velocity and the position/location landmarks are calculated by using the SLAM with a KF, see Figure4. The fourth one is a one-dimensional SLAM with linear KF. In this case, a moving vehicle is considered with a relative measurement and a 1-DoF robot is traveling on a straight line that detects the motionless/stationary landmarks. The robot position/location, velocity, and landmark position are calculated through SLAM with linear KF. Here, all the measures are comparative to the position/location of the mobile robot, see Figure 5.Figure 4
SLAM with motionless robot and relative measurement.Figure 5
SLAM with moving vehicle and relative measurement.The last one is almost different from the previous four SLAM algorithms. In this case, a one-dimensional SLAM with linear KF is considered and the vehicle is moving with a relative/comparative motion. The position/location of the mobile robot is not observed in this case. A mobile robot is traveling on a straight line that detects the landmarks which are motionless as shown in Figure6. The velocity of the robot and its landmark are calculated by applying SLAM with linear KF. As mentioned before, the position is not observed and all the measurements are relative/comparative to the mobile robot position/location.Figure 6
SLAM with moving vehicle and relative measurement while the position of the robot is not observed.


## 3.1. Simultaneous Localization and Mapping with Kalman Filter

In the following section, the authors presented the theory of SLAM which results in efficient localization and mapping in WSNs. Specifically, the author presents the analysis of the operating environment and finally discussed the proposed algorithm and compared it with other SLAM algorithms. In the existence of Gaussian white noise, the KF provides a well-designed and statically optimum explanation for the linear systems. It is a technique that uses linear estimation associated with the states and error covariance matrixes for the purpose to produce gain stated to as the Kalman gain. Such benefit is added to the estimation of a preceding condition, thereby generating an estimate of a posteriori [44]. The below equations define the dynamic model of the system and the measuring model used for the linear state approximation in general which consists of two f and h functions.
(1)Xk+1=fXk,Uk,Wk,(2)Zk+1=hXk,Vk,which administrate state proliferation and state measurements, where U is the input of the process, W and V are the vectors of state and measurement noise, while k represents the discrete-time. In the above equations, f and h are typically based on a set of discretized difference equations that govern the dynamics and observation from the method.
(3)X˘k+1/k=Fk×X˘k/k,(4)Pk+1/k=Fk×Pk/k×FkT+Ak×Qk×AkT,(5)Kk+1=Pk+1/k×HkTHk+1×Pk+1/k×Hk+1T+Rk−1,(6)X˘k+1/k+1=X˘k+1/k+Kk+1Zk+1−Hk+1×X˘k+1/k,(7)Pk+1/k+1=I−Kk+1×Hk+1×Pk+1/k.Such equations from the KF-based method are used iteratively in conjunction with Equations (1) and (2). Equation (3) generalizes the prior state estimate, and Equation (4) represents the equivalent state covariance error. The gain of Kalman can be estimated by Equation (5) which is applied to update the state approximation and covariance error, defined by Equations (6) and (7), correspondingly. EKF is practically comparable to the iterative KF method, and sometimes, it is used for the nonlinear systems. By applying the Jacobian, which is a first-order partial derivative, the measurement Hk+1 and nonlinear system Fk matrices are linearized.A one-dimensional SLAM with KF is applied for a motionless robot, and the measurement is considered an absolute measurement. A 1-DoF mobile robot is used which is motionless in a fixed position of a straight line. The mobile robot is used for detecting the motionless/stationary landmarks. The mobile robot velocityv and position p of the landmarks are calculated by applying SLAM with linear KF. Ten numbers of landmark positions are considered. For the real trajectory, the robot is motionless at a given position which is v=0m/s. The landmark distance is relative to the mobile robot’s location/position which had a moderate measurement noise as shown in Figure 1. The state vector is the diagonal of those that correspond to the robot’s present state by projecting the next one. The vector used for the control is null; it shows that there are no exterior inputs to vary the mobile robot’s state; i.e, the velocity and position of the robot are constant. The initial matrix of covariance is not prevalent; it is characterized by a broad diagonal ambiguity in both the robot’s landmark location and state and equal ambiguity/uncertainty. The result of mobile robot localization with absolute measurement is shown in Figure 2.Figure 1
SLAM with motionless robot and absolute measurement while having a moderate measurement noise.Figure 2
SLAM with motionless robot and absolute measurement.Next, a one-dimensional SLAM with KF is applied for a moving vehicle and the measurement is considered an absolute measurement. A 1-DoF mobile robot is traveling on a straight path. The mobile robot is sensing the motionless/stationary landmarks. The robot velocityv and the landmark position/velocity p are calculated by applying SLAM using a linear KF, and in this case, all the measurements are absolute, see Figure 3. The landmark positions are the same as the previous one.Figure 3
SLAM with moving vehicle and absolute measurement.Furthermore, a one-dimensional SLAM with KF is applied for a motionless robot, and the measurement is considered a relative measurement. Here, a 1-DoF mobile robot is used in a motionless and fixed position of a straight lane that detects the motionless/stationary landmarks. The robot velocity and the position/location landmarks are calculated by using the SLAM with a KF, see Figure4. The fourth one is a one-dimensional SLAM with linear KF. In this case, a moving vehicle is considered with a relative measurement and a 1-DoF robot is traveling on a straight line that detects the motionless/stationary landmarks. The robot position/location, velocity, and landmark position are calculated through SLAM with linear KF. Here, all the measures are comparative to the position/location of the mobile robot, see Figure 5.Figure 4
SLAM with motionless robot and relative measurement.Figure 5
SLAM with moving vehicle and relative measurement.The last one is almost different from the previous four SLAM algorithms. In this case, a one-dimensional SLAM with linear KF is considered and the vehicle is moving with a relative/comparative motion. The position/location of the mobile robot is not observed in this case. A mobile robot is traveling on a straight line that detects the landmarks which are motionless as shown in Figure6. The velocity of the robot and its landmark are calculated by applying SLAM with linear KF. As mentioned before, the position is not observed and all the measurements are relative/comparative to the mobile robot position/location.Figure 6
SLAM with moving vehicle and relative measurement while the position of the robot is not observed.


## 4. Simulation Results and Discussion

The proposed SLAM algorithm is evaluated by simulation. In this simulation, the author evaluates the SLAM algorithm by conducting a different experiment with different landmarks. The simulation is divided into five steps, such as a motionless robot with absolute measurement, a moving vehicle with absolute measurement, a motionless robot with relative measurement, a moving vehicle with relative measurement, and a moving vehicle with relative measurement while the robot location is not detected. The number of time-stamps is 1200 with the map of dimension [180]. The landmark positions are set to beLM=10 which are denoted by p. For the real trajectory, the velocity and position are v=1.0m/s and p=40, respectively, at state xk=xk−1 and vk=vk−1, i.e., motionless at a given position having a moderate measurement noise as shown in Figure 1. The process pn and measurement noise r is added, and the landmark distance is relative to the robot position, see Figure 2. The state equation is a diagonal of those, which ensures that the next state’s estimate or prediction is equal to the present state. The control vector is null; it shows that there are no exterior inputs that vary the state of the robot because, as we stated earlier, the velocity v and position p are constant. Also, the primary covariance matrix is well-defined by a higher diagonal uncertainty mutually in the position of the landmark and the robot state and by a comparable uncertainty, which means that none prevails over the other.The process noise matrix represented byQ and the measurement noise matrix represented by R are computed in which the landmarks are motionless. For the next state prediction, the measurement is done at the prediction position, and for observation, it is measured at the right position/location xk+1, vk+1, and p. The landmark positions are similar for all five methods. However, for this case, a vehicle is considered with constant velocity v=0.02m/s and the position are p=30. Also, in this case, the landmark distance is absolute. In the third case, the robot is motionless and the measurement is relative at a given velocity and position v=1.0m/s and p=40, respectively. The fourth one is the SLAM with linear KF in which the vehicle is moving and the measurement is relative. The landmark distance is relative to robot position and a vehicle with a constant velocity of v=0.02m/s and at the position, see Figure 5, the red line denotes the position. The last one is the SLAM with linear KF and a vehicle is moving, and the measurement is relative. Note, in this case, the position is not observed as the previous. The constant velocity of the vehicle is set to be v=−0.2m/s and the position is 20, as can be seen in Figure 6. The abovementioned algorithms for SLAM with KF are evaluated in deep detail. The authors considered a variety of aspects regarding the SLAM localization. The offered SLAM algorithms present a high level of accuracy in various conditions and perform well in terms of velocity, distance, coverage area, etc.


### 4.1. Simultaneous Localization and Mapping with Extended Kalman Filter

EKF is well-known as a widespread resolution to the SLAM problem for mobile robot localization. In this section, the authors realized the EKF SLAM-based algorithm for a mobile robot that follows a specific trajectory. EKF SLAM relies on present elements of the navigation system known as landmarks to change the location of the robot. EKF SLAM for a mobile robot is executed in a defined field with a specific feature. The authors considered two basic mathematical models such as the EKF state and observation model that are represented below.(8)Xk+1=fXk,Uk,wk,(9)Zk+1=hXk+1,υk+1,where wk∼N0,Ok and vk∼N0,Rk which characterize the process and observation noise. Here, Xk+1 denotes the estimated state vector at time k+1. The time is the discrete time for a known input Uk assuming all noise to be wk. In Equation (9), Zk+1 represents the estimated measuring vector at the time instant k+1, where vk is the observation noise. Qk and Rk denote the covariance matrix of prediction and observation, respectively. EKF offers an approximation of the optimal state estimate. The EKF-SLAM objectives are to estimate recursively the landmark state Xk as stated by the Zk+1 measurement. EKF is basically divided into several steps which are represented as at the initial state, the state vector X˘k+1 will become
(10)X˘k+1=fX˘k,Uk+1+∇Fx×Xk−X˘kk.In the prediction stage, the covariance matrix for predictionPk+1/k can be represented as
(11)Pk+1/k=∇Fx×Pk/k×∇FxT+∇Fu×Qk×∇FuT.In the above equation, the∇Fx and ∇Fu denote the Jacobian matrices of the function f concerning the state vector which is Xk+1. The state transition matrix is denoted by B, and F is the state equation which can be represented as follows:
(12)B=dt×cosx30dt×cosx300dt(13)F=100010001Therefore, the Jacobian of the state equation will become(14)JF=00−dt×u1×sinx300dt×u1×cosx3000,and the global initialization Jacobian G can be written as follows:
(15)G=−δ1−δ20δ1δ2δ2−δ1−1−δ2δ1In the observation and update phase, the observation modelZ˘k+1 at X˘k+1/k can be represented as
(16)Z˘k+1=hX˘k+1/k+Hk+1×X˘k+1/k−Xk+1.To apply the KF update cycle, i.e.,X˘k+1/k and P˘k+1/k, the KF gain can be computed.
(17)Kk+1=Pk+1/k×Hk+1T×Hk+1×Pk+1/k×Hk+1T+Rk−1,where Kk+1 is the Kalman gain. With measurement of Zk+1, the updated estimate can be
(18)K˘k+1/k+1=X˘k+1/k+Kk+1×Zk+1−Hk+1×X˘k+1/k.If theZk of measurement is available, EKF calculates the matrix of Kalman gain and integrates the invention of measurement to obtain the approximate state Xk, accompanied by the update of the state error matrix. Therefore, the measurement updated step from the above equation will become
(19)X˘k=X˘k+Kk×Zk−hX˘k,(20)Pk=I−Kk×Hk×Pk.Therefore, the update covariancePk+1/k+1 can be represented as
(21)Pk+1/k+1=I−Kk+1×Hk+1×Pk+1/k,where Kk is the Kalman gain and Pk is the new state covariance matrix. H is the measurement Jacobian or linearization matrix and X˘k denotes the state vector Xk estimate.After evaluating EKF in deep detail, the authors conclude that the EKF also has some disadvantages that is if the process and measurement noise are not accurately displayed, the robot will diverge from its route which resultantly give a contradiction. Particularly, in the case of the robot velocity, the robot is sensitive to the velocity as by varying the velocity the robot is diverging from its route as shown in Figure7 . However, in the first case, the velocity is v=1.0m/s as shown in Figure 8.Figure 7
The SLAM EKF performance for variousv values: (a) v=0.9m/s, (b) `, (c) v=0.7m/s, and (d) v=0.6m/s, where the green line represents the dead reckoning calculation of the existing location, which is the outcome of the displacement function applied for the previous location. The black mark is the ground truth information that is necessary for real-time localization. The red mark is the EKF-SLAM. The blue asterisks represent the true landmarks, and the black circle is the estimated landmarks.
(a)(b)(c)(d)Figure 8
Performance of SLAM with Extended Kalman Filter.


## 4.1. Simultaneous Localization and Mapping with Extended Kalman Filter

EKF is well-known as a widespread resolution to the SLAM problem for mobile robot localization. In this section, the authors realized the EKF SLAM-based algorithm for a mobile robot that follows a specific trajectory. EKF SLAM relies on present elements of the navigation system known as landmarks to change the location of the robot. EKF SLAM for a mobile robot is executed in a defined field with a specific feature. The authors considered two basic mathematical models such as the EKF state and observation model that are represented below.(8)Xk+1=fXk,Uk,wk,(9)Zk+1=hXk+1,υk+1,where wk∼N0,Ok and vk∼N0,Rk which characterize the process and observation noise. Here, Xk+1 denotes the estimated state vector at time k+1. The time is the discrete time for a known input Uk assuming all noise to be wk. In Equation (9), Zk+1 represents the estimated measuring vector at the time instant k+1, where vk is the observation noise. Qk and Rk denote the covariance matrix of prediction and observation, respectively. EKF offers an approximation of the optimal state estimate. The EKF-SLAM objectives are to estimate recursively the landmark state Xk as stated by the Zk+1 measurement. EKF is basically divided into several steps which are represented as at the initial state, the state vector X˘k+1 will become
(10)X˘k+1=fX˘k,Uk+1+∇Fx×Xk−X˘kk.In the prediction stage, the covariance matrix for predictionPk+1/k can be represented as
(11)Pk+1/k=∇Fx×Pk/k×∇FxT+∇Fu×Qk×∇FuT.In the above equation, the∇Fx and ∇Fu denote the Jacobian matrices of the function f concerning the state vector which is Xk+1. The state transition matrix is denoted by B, and F is the state equation which can be represented as follows:
(12)B=dt×cosx30dt×cosx300dt(13)F=100010001Therefore, the Jacobian of the state equation will become(14)JF=00−dt×u1×sinx300dt×u1×cosx3000,and the global initialization Jacobian G can be written as follows:
(15)G=−δ1−δ20δ1δ2δ2−δ1−1−δ2δ1In the observation and update phase, the observation modelZ˘k+1 at X˘k+1/k can be represented as
(16)Z˘k+1=hX˘k+1/k+Hk+1×X˘k+1/k−Xk+1.To apply the KF update cycle, i.e.,X˘k+1/k and P˘k+1/k, the KF gain can be computed.
(17)Kk+1=Pk+1/k×Hk+1T×Hk+1×Pk+1/k×Hk+1T+Rk−1,where Kk+1 is the Kalman gain. With measurement of Zk+1, the updated estimate can be
(18)K˘k+1/k+1=X˘k+1/k+Kk+1×Zk+1−Hk+1×X˘k+1/k.If theZk of measurement is available, EKF calculates the matrix of Kalman gain and integrates the invention of measurement to obtain the approximate state Xk, accompanied by the update of the state error matrix. Therefore, the measurement updated step from the above equation will become
(19)X˘k=X˘k+Kk×Zk−hX˘k,(20)Pk=I−Kk×Hk×Pk.Therefore, the update covariancePk+1/k+1 can be represented as
(21)Pk+1/k+1=I−Kk+1×Hk+1×Pk+1/k,where Kk is the Kalman gain and Pk is the new state covariance matrix. H is the measurement Jacobian or linearization matrix and X˘k denotes the state vector Xk estimate.After evaluating EKF in deep detail, the authors conclude that the EKF also has some disadvantages that is if the process and measurement noise are not accurately displayed, the robot will diverge from its route which resultantly give a contradiction. Particularly, in the case of the robot velocity, the robot is sensitive to the velocity as by varying the velocity the robot is diverging from its route as shown in Figure7 . However, in the first case, the velocity is v=1.0m/s as shown in Figure 8.Figure 7
The SLAM EKF performance for variousv values: (a) v=0.9m/s, (b) `, (c) v=0.7m/s, and (d) v=0.6m/s, where the green line represents the dead reckoning calculation of the existing location, which is the outcome of the displacement function applied for the previous location. The black mark is the ground truth information that is necessary for real-time localization. The red mark is the EKF-SLAM. The blue asterisks represent the true landmarks, and the black circle is the estimated landmarks.
(a)(b)(c)(d)Figure 8
Performance of SLAM with Extended Kalman Filter.


## 5. Simulation Results and Discussion

The proposed SLAM EKF algorithm is evaluated through simulation. In this simulation, the author evaluates the SLAM EKF algorithm by performing simulation with various factors. Firstly, the time ist=0, end time is t=60sec, while the global time is dt=0.1sec. In this simulation, the state vector is considered xEst=000T in which the xTrue=xEst, while at the dead reckoning state xd=xTrue. The landmark coordinates are [xy], i.e., LM=015;100;1520. The maximum range is set to be 20 at the initial stage and parameter α=1. For the input parameters, the time is set to be T=10sec, the velocity is v=1.0m/s, and yawrate=5deg/sec. At the initial stage, the velocity is limited to v=1.0m/s as can be seen in Figure 8; however, in the next stage, the velocity is varying.In the case of varying the velocities as can be seen in Figure7, the velocities are set to be v=0.9m/s, v=0.8m/s, v=0.7m/s, and v=0.6m/s. By varying the velocity of the robot, the robot is diverging from its route and, therefore, reduces the coverage area as can be seen in Figure 7(a)-7(d). Also, the error between the true landmark and predicted landmark is increasing. On the other hand, for higher velocities (more than v=1.0m/s), the proposed algorithm is not applicable, because in the SLAM, the robot is following the prior defined map and the robot keeps communication with the surrounding. However, in our previous study, we mentioned the higher velocities for the robot, in the case of EKF, UKF, and PF, the coverage area, and localization were increasing by increasing the velocity. Furthermore, the maximum range was set to be 20 as shown in Figure 6, but by modifying the maximum range to 30 or above, in this case also, the robot diverges from its route of localization as shown in Figure 9. Resultantly, the authors conclude that the proposed algorithm is more suitable for constant velocity which presents a high level of accuracy.Figure 9
Performance of SLAM with Extended Kalman Filter in case of higher range.


## 6. Comparison of the Proposed and Other Algorithms

In the above sections, the authors investigated and evaluated well about the proposed SLAM algorithms. However, to demonstrate the effectiveness and better performance of the planned algorithms, the authors present a brief comparison of the proposed algorithms with other algorithms in this section.In [45], the authors presented a neurofuzzy-based adaptive EKF method. The purpose of this method is to estimate the right value of matrix R at every stage. They plan an adaptive neurofuzzy EKF to lessen the variance among the theoretical and actual covariance matrices. The parameters for this technique are then skilled offline by using a particle swarm optimization method. A mobile robot steering with a number of landmarks under two situations is assessed. The technique is applied that the adaptive neurofuzzy EKF provides development in performance effectiveness.An EKF-based SLAM system for a mobile robot with sensor bias estimation is presented in [46]. The authors proposed an improved method for EKF which is practical to the issue of mobile robot SLAM which has taken into consideration the sensor bias issue. Mobile robot Pioneer 3-AT is taken as the model for studying the theoretical derivation and the authentication of the investigation in this work. At first, the kinematic model of Pioneer 3-AT mobile robot is introduced; then, the improved EKF method, taking into account the issue of bias estimation and compensation, is anticipated to increase the precision of the location estimation. In addition, a study explores the autonomous location and atmosphere mapping of stirring substances under the dust and low lighting situations in underground underpasses. The typical EKF algorithm has a problem that machine noise and the prior statistical characteristics of the observed noise cannot be predicted accurately. Thus, the authors presented an enhanced EKF algorithm to accomplish a fuzzy adaptive SLAM [45, 47, 48]. Therefore, to predict the position, a laser matching is applied to the EKF prediction process, and the weighted average location is used as the final location of the predicted component. The machine noise and the weighted value of experiential noise become fuzzily recognizable by observing the variation of mean value and covariance. The improved filtering algorithm is applied to a SLAM simulation study and measure the impact on position estimation of four dissimilar landmark measurements.A recent approach strong tracking second-order (STSO) central difference SLAM is presented in [49] which it is based on the tracking second-order central difference KF. The proposed procedure gathers the second-order central differential filter (SOCDF), strong tracking filter (STF), and PF. Using Cholesky decomposition, the algorithm uses the Sterling Interpolation second-order method to solve a nonlinear system problem. This methodology transmits directly in the probabilistic estimation of SLAM by adding the covariance square root factor. Furthermore, in [50], a visual-inertial SLAM feedback mechanism is presented for the real-time motion assessment of the SLAM map. The main aspect of this mechanism is that the front-end and the back-end can support each other in the VISLAM. The output from the back-end is fed to the KF-based front-end to decrease the motion estimation error produced by the linearization of the KF estimator. On the other hand, this more accurate front-end motion estimation will improve back-end optimization as it provides the back-end with an exact primary state. Similarly, the EKF-based SLAM approaches are presented in [33, 51, 52] which focus on the performance and effectiveness of the SLAM. Each algorithm presents well in its domain, but the proposed SLAM algorithms perform well compared to the other SLAM algorithms.


## 7. Conclusion and Future Directions

In this work, the SLAM algorithm is proposed in two different methods such as SLAM with linear KF and SLAM with EKF. Firstly, SLAM with linear KF is implemented in five different methods such as the motionless robot with absolute measurement, moving vehicle with absolute measurement, a motionless robot with relative measurement, moving vehicle with relative measurement, and moving vehicle with relative measurement while the robot location is not detected. The landmark position was set to be 10 for all five cases. The mobile robot position or velocity and landmark position are calculated by applying SLAM using a linear KF. Secondly, the SLAM with EKF is implemented and an analytical expression for the EKF-based SLAM algorithm is derived and their presentation is evaluated. The SLAM algorithm with EKF is evaluated in various scenarios, and several iterations are applied to explain the performance of EKF-based SLAM well. The proposed algorithm is simulated for varying velocities, and their performance is presented in Figure8. Each process of localization is effective in its domain. In this analysis, many localization factors such as velocity, coverage area, localization time, and cross section area are taken into consideration. The proposed SLAM-based algorithm performance is intensively assessed by executing numerous iterations as can be seen in the figures above. The planned SLAM-based algorithms present a high precision while preserving realistic computational complexity. The simulation outcomes indicate that the planned SLAM algorithms can accurately locate the landmark and mobile robot.Future research will use more simulation and tests to show the robustness of the SLAM in different scenarios and landmarks. We will try to make a robot pilot more originally and also apply SLAM with UKF and PF algorithms. In addition, improving the development of some standards for estimating SLAM approaches, particularly for large-scale SLAM in dynamic situations, is also important to make the results of SLAM algorithms more valuable.



---

*Source: 2138643-2020-06-08.xml*