Extended Kalman Filter Sensor Fusion in Practice for Mobile Robot Localization

Self-driving vehicles and autonomously guided robots could be very beneficial to today's civilization. However, the mobile robot's position must be accurately known, which referred as the localization with the task of tracking the dynamic position, in order for the robot to be active and useful. This paper presents a robot localization method with a known starting location by a real-time reconstructed environment model that represented as an occupancy grid map. The extended Kalman filter (EKF) is formulated as a nonlinear model-based estimator for fuse Odometry and a LIDAR range finder sensor. Because the occupancy grid map for the area is provided, just the inaccuracies of the LIDAR range finder will be considered. The experimental results on the “turtlebot” robot using robot operating system (ROS) show a significant improvement in the pose of the robot using the Kalman filter compared with sample Odometry. This paper also establishes the framework for using a Kalman filter for state estimation, providing all relevant mathematical equations for differential drive robot, this technique can be used to a variety of mobile robots. Keywords—Autonomous navigation; kalman filter; self-driving vehicle; simultaneous localization and mapping; occupancy grid map; ROS


I. INTRODUCTION
Recently, mobile robots have been used to perform specialized tasks in a number of industries, including services, rescue, military, disaster relief, unmanned defense vehicles, and so on. Localization of mobile robots is a hard problem that many academics are seeking to address via the development of innovative techniques. Researchers have added more sources in order to build a powerful localization approach [1].Odometry is one of the most important techniques to tackle the posture tracking problem, it uses encoder data to track the motion progress from a specified beginning position. This technique tracks motion from a known beginning position using encoder data, the encoded data is sent to the central processor, which uses a geometric equation to update the robot's position [2,3]. Due to a variety of factors such as wheel slippage, ground roughness, and varying wheel diameters, this approach has accumulative errors. So, under severe conditions, solely utilizing Odometry for localization virtually never results in an accurate state, and it becomes more active when other sources of sensing are used. Stereo, LIDAR range finder, sonar, compass, gyro, and GPS are the most often utilized extra sensors [4]. Building a robot from the scratch is expensive and time consuming, so working in an environment that guarantees theoretical study and practical implementation will be quiet helpful. ROS is an open source meta operating system that provides hardware abstraction, control implementation of commonly used functionalities, tools and libraries for building, writing, and running code for simulated and real robots after installing necessary drivers. The significance of this research rests in the framework it provides for fusing several sensors with a Kalman filter for robot localization, with the experimental results emphasizing notable reduction of errors in robot position. The paper is structured as follow: in section III the motion model of two wheeled robot was derived. Section IV presents Kalman filter pose tracking design. Section V discusses Kalman filter implementation results, followed by conclusion in section VI.

II. LITERATURE REVIEW
Iraj Hassanzadeh and Mehdi Abedinpour implemented an augmented unscented and extended Kalman filter for position tracking using a differential drive mobile robot with encoder readings, assuming real measurements are available. The work showed an improvement in pose tracking using this technique with the unscented filter outperforming the extended one [5]. Jaeyong Park and Sukgyu Lee investigated a mobile robot SLAM (simultaneous localization and mapping) technique based on EKF extended Kalman filter, with an additional extended Kalman filter used to enhance robot heading accuracy, because the robot's kinematic model was unclear due to the rough surface, its heading was deviated as it drove across uneven terrain. They proposed a method for correcting uncertain robot postures utilizing an extra extended Kalman filter on a simulation-based test [6]. The Kalman filter was applied on a Pioneer 2DX mobile robot by Edouard Ivanjko, Mario Vasak, and Ivan Petrovic, combining data from wheel encoders and a sonar sensor. The experimental result indicates that the kalman filter reduces posture tracking errors when compared to using Odometry alone [7]. Yusuke Misono and Yoshitaka Goto have implemented outdoor SLAM using Kalman filter in an outdoor environment using electric wheelchair mobile robot provided by LIDAR range finder and GPS the experimental results reveal a significant improvement of self-localization vehicle estimate by the SLAM algorithm compared with dead reckoning [8]. On a laser range finderequipped robot platform, Angga Rusdinar, and Sungshin Kim investigated simultaneous localization and mapping with a particle filter. The experiment's findings showed that the suggested particle filter can improve map building performance and mobile robot localization accuracy inside indoor buildings [9]. Mohammed Faisal, Mansour Alsulaiman, and Ramdane Hedjar Hassan developed a localization method to reduce error accumulation in the dead-reckoning approach, since dead-reckoning is reliant on encoder information, they use an additional sensing source, the proposed localization *Corresponding Author. www.ijacsa.thesai.org system uses the extended Kalman filter in combination with infrared sensors to enhance the mobility robots' localization, by rectifying errors in the robot's position and address the issue of dead-reckoning, with the working area's walls serving as references (landmarks) [10]. Yi-Xiang Wang and Ching-Lung Chang investigated SLAM under the robot operating system (ROS) utilizing a laser range finder (LIDAR), an Inertial Measurement Unit (IMU), an odometer, and an Ultrawideband (UWB), and fused all of the above sensors using Extended Kalman filter. Experiment findings demonstrate that the mobile robot's average error distance in the system is restricted to 10cm [11]. For city navigation, Zanwu Xia and Si Tang developed a new technique to improve the accuracy of high definition (HD) maps in order to improve the localization of self-driving cars. The research focused on extracting the factors that have a high impact on the global map, such as feature sufficiency, layout, local similarity, and map representation quality. The Kalman filter was used to combine data from LIDAR, IMU, and GNSS systems. The experimental results show a reduction in accumulative errors [12].
In this study, we focus on the problem of indoor robot localization, which involves determining the position of the robot x, y, and its orientation Ɵ. The idea of the Kalman filter is to reduce the errors in both the mechanical model of the robot and the sensor readings. Kalman Filter is designed to deal with linear systems, but most nontrivial systems are nonlinear. Therefore, a new modified technique called extended Kalman filter (EKF) has been developed. This paper aims to deal with the uncertainties of a mobile robot by fusing Odometery and LIDAR range finder in the dead-reckoning method. Three steps are encoded in the method proposed here; the prediction step depends on the motion equation of the robot platform. Data acquisition by measurement sensors in the workspace, which are used to correct the robot position calculated in the motion prediction step. Update step that corrects the sum of motion uncertainty and measurement uncertainty, Fig. 1.

III. MOTION MODEL
Regardless of whether robot position has to be adjusted or not, a kinematic model of robot motion exists that is dependent on the degree of freedom available to it. For example, a wheeled mobile robot without a manipulating arm (our robot platform) has three degrees of freedom (displacement along X-axis, displacement along Y-axis, and the orientation around Z-axis). Fly robot for instance has six degree of freedom (beside to displacement along orthogonal axis's X Y Z, there is orientation around each axis Ɵ x Ɵ y Ɵ z ).In the experiment, a two-wheeled mobile robot was used. Each wheel has encoders placed, besides the passive caster wheel for stability. The driving wheels are individually controlled. The following relationships define the mobile robot's kinematic model (Fig. 2). (1) Where and are the center gravity of robot platform ; travel distance between two successive time interval k+1 and k ; mobile robot's translational velocity ; T is the sampling time ; the robot's heading with the X-axis; rotational angle of robot between k+1 and k time steps ; and the left and right wheel's respective linear velocities ; and the left and right wheel's respective angular velocities ; The two driving wheels have a radius of R ; b axle length or robot. The radius of both driving wheels, in sampling, is assumed to be equal. We add three more variables to (6) and (7) to account for sampling errors caused by not knowing the exact radius and axle length. The uncertainty of the exact wheel radius is represented by k 1 and k 2 , while the uncertainty of the exact axle length is represented by k 3 . In [13,14,15,16] the systematic error correction approach is described in depth, as well as how the parameter values were calculated. Equations (1) to (7) describe the fundamental concept of Odometry position tracking.

IV. LOCATION TRACKING WITH KALMAN FILTER
Although there are many specific techniques for estimating the state of the system from, a set of measurements, most of these do not explicitly consider the noisy nature of measurements. This noise is typically described by statistics, which leads to have to use stochastic methods to tackle the problem [17,18]. This section describes the stochastic state estimation process, initially, the basics of Bayesian filtering are presented, providing a brief mathematical derivation of how it is possible to make an estimate of the state. A mathematical treatment of the Kalman Algorithm for localization is then presented.
The difficulty in utilizing sensor fusion to localize a mobile robot is balancing the uncertainty of the state (x, y and Ɵ) with the LIDAR range measurement (robot's output) to achieve an optimum estimation of the posture. Kalman filter requires that the state random variables have Gaussian probability distributions that are adequately characterized by the mean and covariance [19,20]. Time Update and Measurement Update are the two major stages in calculating the optimum state estimate. The state prediction is generated using the motion model based on the previous value and the control input value. The output forecasts are calculated using the measurement model based on the outcomes of the time update. The anticipated state mean and covariance are then adjusted by reducing the state covariance using the difference between Expected and measured output.

A. Bayesian Filtering
A Bayesian filter is a mathematical tool that estimates the development of the system's state given the available data [21]. This tool necessitates: The analytical knowledge of the transition function f t and the stochastic knowledge of the noise of the state  The analytical knowledge of the output function h t and the stochastic knowledge of the observation noise  The realization of the output of the system z 1: t at time t.
Having this data, the Bayesian filter is able to estimate the function probability density. Once the state estimation problem has been formalized as a Baysian filtering problem, we have to find a mathematical formulation that allows us to return to the probability density function using our system knowledge and observations. First, we have to model the system's internal evolution as well as how it manifests itself through its observable outputs. Two conditional distributions are introduced for this purpose. The first is observation model , represents the density of the measurement given the system state .The second function is an evolution model that represents how the system develops over time. Using the Bayes rule and the Markovian chain assumption, it is possible to obtain an a posteriori probability density of the state incrementally. Because data arrives in stages over time, a recursive formulation of Bayes' rule known as the Bayes Filter is used. The a posteriori probability of the state is gradually refined in this formulation as measurements arrive.
(10) (11) If we are interested in the iterative distribution the equation becomes: Where η is a normalization factor to ensure that equation (15) represents a probability density function. Usually, the evaluation of this equation takes place in two steps. In the prediction phase, the x t + 1 state is calculated starting from the x t state, through the application of the transition model. Subsequently, the z t observation is incorporated into the previously calculated probability density function, through the updating phase. The relationship between the present state and the prior state is depicted by the motion model. The location of the mobile robot in a global coordinate frame is defined as the state vector; , where, k indicates the sampling moment. The probability distribution is assumed to be Gaussian so that the state variable is fully described by a 3 * 3 covariance matrix P k and the state expected value ̂ (Mean estimated value). The control input u k represents the motion command that moves the mobile robot from step k to step k + 1, since u k can be expressed as; denotes translation by distance d k followed by a rotation angle . The state vector is calculated using the current state vector and the current control input by the state transition function f(.); . Since depicts process noise (also Gaussian with zero mean) , according to equations (1), (2), and (3) the state transition function is obtained.
Two independent error sources, translational and angular, were used to model the process noise covariance Q k . The expression for Q k is: Where and are the variances of translation d k and rotation respectively.

B. Measurement Model
Measurement function determines the distance between the obstacles and robot's l RPLIDAR (Fig. 3). In the world model, p i = (x i , y i ) represents the point (occupied cell identified by the LIDAR). The LIDAR model utilizes distance readings that are linked to the causative obstacle.
The measurement noise is represented by . Because all distance measurements are utilized in parallel for the distance measurement value, the distance measurements are a single measurement vector. And and components of the measurement form a diagonal matrix R k .

C. EKF-based Pose Tracking
This paper describes EKF, as sensor fusion-based method for robot posture tracking. More a thorough discussion of the EKF localization method can be found in [22]. At time k the values of the control input vector u k-1 generated by wheel encoder are supplied to the equations that will be mentioned in this section, and the first-time update is performed to get the prediction estimate, and when fresh LIDAR measurements are received, these predictions are adjusted. The prior mean ̂ is calculated by using the nonlinear Odometry function to propagate the predicted state.
The anticipated state covariance is calculated by propagating the state covariance through a linearized system form. (20) ̂ }) denotes the Jacobian of function f(.) with respect to the state which can be computed as follows. ̂ }) is the Jacobian of (.) with respect to the control input u. It is important to note that when (9) and (10) are employed, the mean and covariance are only true to the first order of the associated Taylor expansion [23]. If no fresh LIDAR measurements are available at time k, or if all are discarded, there is no measurement update, and the estimated mean and covariance are assigned the anticipated values.
Otherwise, a measurement update occurs, in which the initial forecasts of the approved LIDAR readings in ̂ with the i-th component are as follows: In time step k, the state estimate and covariance are calculated as follows: Since Z k reflects actual LIDAR readings, where is the Jacobian matrix and we can obtain by calculating the derivative of measurement function with respect to state : Since: And K k is the Kalman filter which can be computed as follows: By implementation of previous equations, we can see the diverges in robot location in case of using Kalman filter (blue color) compared with sample Odometry (red color), as shown in Fig. 4.
After we have seen the diverges in robot pose using kalam filter compared with sample odmetry and deriving the necessary equations, We will go further, to see the effect of using kalman filter in reducing estimated error the following figure (Fig. 5) depicts the implementation of EKF on a differential drive mobile robot (turtelbot) using ROS. www.ijacsa.thesai.org

V. DISCUSSION
In Fig. 5 estimates after the prediction step are shown as red arrows; estimates after the update step are shown as green arrows. Blue dots represent (static) landmarks and the red lines indicate the robot's field of view. Robot state covariance increases during the prediction step and decreases during the update step. The algorithm benefits more from the correction step when the motion covariance is large, as otherwise the prediction step already yields fairly precise estimates. A larger number of landmarks result in a more precise position estimate. Loop closing, e.g., observing landmarks that have already been observed at an earlier stage greatly decreases the covariance of both the robot state and landmark position. This effect is clearly visible in Fig. 5, where loop closure occurs twice: first around step 9 and then around step 20. Both times, the squared estimation error decreased. It's worth noting that the findings displayed above were obtained using ROS in simulated settings for obstacles, LIDAR measurements, linear and angular velocities; hence, with ROS, we can safely utilize the same software for actual robots, as shown in Fig. 6.

VI. CONCLUSION
Extended Kalman filters are developed and tested as mobile robot posture monitoring methods. Odometry and sensor fusion LIDAR-based sensors are shown to significantly improve mobile robot localization, and the experimental results reveal substantial differences when utilizing sample Odometry and the EKF method. The algorithm is tested with different motion and measurement noise covariance, generated trajectories and a varying number of landmarks. This technique produces significant results in a limited environment and with a small number of Landmarks. However, it is clear that this approach is insufficient in the case of a large number of Landmarks, which would definitely result in an increase in the size of the state space.