TH

16 ERU SYMPOSIUM, 2010: FACULTY OF ENGINEERING, UNIVERSITY OF MORATUWA, SRI LANKA

Stochastic Self Localization of a Mobile Robot

Abstract
This paper describes a solution to the localization problem based on the Extended Kalman filter. Estimation of
the location and path of a Nonholonomic robot in a given environment is a key problem which needs to be
overcome for successful mobile robot navigation. In this work, a differential drive vehicle model is used and
evolution of the vehicle motion is modeled using vehicle frame translation derived from successive dead
reckoned poses as a control input. The nearest neighbor algorithm is used for the purpose of data association.
The localization algorithm is simulated using matlab. The results show that the proposed approach localizes
the robot with the necessary accuracy.

1. Introduction
localization algorithm, unlike the sophisticated systems
with SICK laser scanners found in test beds.

The self localization and mapping (SLAM) [1] [2]

problem is often recognized in robotics literature as one
of the key challenges in building autonomous capabilities
for mobile vehicles. The goal of an autonomous vehicle
performing SLAM, is to start from an unknown location
in an unknown environment and build a map of its
environment incrementally by using the information
extracted by its sensors, while simultaneously using that
map to localize itself with respect to a reference
coordinate frame and navigate in real time.
A vehicle capable of performing SLAM using naturally
occurring environmental features and capable of running
autonomously for days in a completely unknown and
unstructured environment will indeed be invaluable in
several key areas of robotics. These include driver
assistance systems, mining, surveying, autonomous under
water exploration and autonomous planetary exploration.
In this paper we focus in finding a solution to the
localization problem which is the first step solving the
SLAM problem.
The first solution to the SLAM problem was proposed
by Smith et al. [3]. They emphasized the importance of
map and vehicle correlation in SLAM and introduced the
extended Kalman filter (EKF) based stochastic mapping
framework, which estimated the vehicle pose and the
landmark positions in an augmented vector using second
order statistics. The next solution proposed by Smith et al.
[3] and Durrant-Whyte [4] was based on the study of an
estimation-theoretic solution. The key point established
here was to show that a high degree of correlation exists
between estimates of location of different land marks in a
map, and these correlation would grow to unity following
increase in number of observations.
Smith and Cheeseman [5] showed that the relative
observations of landmarks and the estimates of these
landmarks are all correlated with each other because of
the common error associated with the estimated vehicle
position.
However the EKF based stochastic mapping approach
is considered as the primary framework in most
stochastic localization algorithms. Our work is also based
on this approach. The differentiating factor of our work
from most common SLAM implementations is in that,
we use a low cost platform with spurious sensor data and
low data acquisition rates for implementation of the

2. Methodology

Figure 1: Overview of the experimental setup

2.1 Key definitions
In the EKF-SLAM [1] [2] formulation the pose of the
vehicle () and the known landmark locations M
(known as the map) at the time k are represented by
absolute coordinates with reference to a global coordinate
frame. The localization solution is stated for a vehicle
travelling a 2-D terrain with only point features (land
marks) in the environment.
The state vector () ( vehicle-map composite) at time
k is thus
() = [ () ]

(1)

Where () = [ () () () ] is the vehicle

pose and () ,() and () denotes vehicle positions
x coordinate, y coordinate and vehicle orientation.
= [1 1, 2 2, 3 . , ] is the vector of landmark
coordinates, where [ , ] , i=1,2,3 ,N denote the
feature coordinates with respect to the world coordinate
frame.

16TH ERU SYMPOSIUM, 2010: FACULTY OF ENGINEERING, UNIVERSITY OF MORATUWA, SRI LANKA
2.2 Process model
The differential drive used for the robot has a nonlinear motion model.

correct landmark association , the innovation is calculated

to be
( + 1) = ( + 1) ( + 1|)
And the associated innovation covariance matrix is
given by
( + 1) = () ( + 1|) () + ( + 1) (5)

dx
d

2.5 State update

Using the definitions stated above, the state update
equations [1] [2] are given by,
( + 1 | + 1) = ( + 1|) + ( + 1) ( + 1)
(6)

x
c

( + 1 | + 1) = ( + 1|) ( + 1)
( + 1) ( + 1)

dy

Where the gain matrix ( + 1) is given by

( + 1) = ( + 1|) ()1 ( + 1)

Figure 2: Differential drive motion model

(7)

(8)

At each time instance k , the equations are iteratively

used and the state is updated to reflect the current state.

(1 cos )
=

3. Implementation

We can represent the vehicle motion model by

This section describes the implementation of the
localization algorithm on a mini robot of circular
construction with a radius of 15 cm and height of 6 cm. It
is equipped with two IR sensors mounted on a servo to act
as a 2D scanner. The implementation is aimed at
demonstrating key properties of the analyzed approach to
localization, such as convergence, consistency and
boundedness of the errors. It also shows how to
incorporate non-linear vehicle and observation models in
the algorithm and how the data association problem is
practically solved. However the implementation described
here is only a first step in the realization of a fully
autonomous navigation system, as a complete SLAM
solution is required for navigation when prior knowledge
of the map is not available.

( + 1)
()
cos
(()) sin
(()) 0 ( + 1)
( + 1) = () + sin
(()) cos
(()) 0 ( + 1)
( + 1)
()
0
0
1 ( + 1)

It can be represented by the discrete time vehicle

motion model by
( + 1) = () () + ( + 1) + ( + 1)
Where () is the control input at the kth time step,
and the noise associated with the control input is a zero
mean uncorrelated noise sequence with covariance matrix
() , () is the control input distribution matrix.
Now as the landmarks are static
( + 1) = ()

2.3 Observation model

The observation model is represented by
(1)
() = () + ()
where () is a vector of temporally uncorrelated
observation errors with zero mean and variance ().
Matrix is the observation matrix and relates the output
of the sensor () to the state vector () when
observing the ith landmark. The covariance matrix of the
composite state vector () which is known as the
composite covariance matrix , is denoted by ().The
observation model used by us, for a specific landmark
is given by
() = ( ())2 + ( ())2 + ()
yi yr (k)
() = arctan
() + w (k)
xi xr (k)

3.1
Experimental setup
Fig 3 shows the test platform, The scanner has an angular
accuracy of 5o with 360o range, a linear accuracy of 10 cm
, and range of 100 cm. The frequency of data acquisition
is 0.25 Hz . Two hall sensors connected to the drive
wheels gives the angular displacement of each wheel.

2.4 State prediction

The observation prediction equations [1] [2] used are
( + 1|) = () (|) + ()
(2)
(3)
( + 1|) = ()( + 1|)
(4)
( + 1|) = () (|) () + ()

Figure 3: Test Robot

The vehicle is driven by signals sent from the control
program running on the remote PC. The encoder readings

Following the prediction , an observation ( + 1) of

the ith landmark of the true state is made. Assuming

16TH ERU SYMPOSIUM, 2010: FACULTY OF ENGINEERING, UNIVERSITY OF MORATUWA, SRI LANKA
and the range bearing readings are sent through RF
signals to the PC from the robot after each scan of the
environment.
3.2 Experimental results
Table 1: Results of the statistical analysis (500 data
samples) of the data acquisition system
Actual
data (r,)

Mean value (r,)

Standard deviation

40,45o

38.85,52.40

4.12,1.720

70,1800

67.93,182.50

0.82,00

30,2700

26.33,270.110

1.63,1.840

60,3150

57.79,321.70

1.74,1.670

Figure 4: Estimated path of the robot

This result justifies using a measurement model error of

[5cm,100] in the simulation.
Simulation results are shown in Fig. 4, and Fig. 5. It can
be seen that the vehicle localization is perfectly formed,
and that the error covariances of the vehicle state lies
well within the confidence limit (2), thus this algorithm
generates vehicle location estimates which are consistent,
stable and have bounded errors.

Figure 5: Error covariance of the state

References

4. Conclusion

[1] Durrant-Whyte, H. and Bailey, T., Simultaneous

localization and mapping: Part I, IEEE Robotics and
Automaton Magazine, Vol.13, No.2 , pp. 99-108,
(2006)

The main contribution of this paper is to prove that the

non- divergent estimation theoretic solution can be
practically implemented to solve the localization problem
in an experimental setup. The work is to be extended to
solving a complete SLAM problem where both Map and
location are unknown. However this does not address
practical issues such as landmark extraction, data
association and maintenance and validation of the
localization algorithm. The implementation and
deployment of a large-scale SLAM system, capable of
vehicle localization and map building over large areas,
will require further development to address these practical
issues.

[2] Bailey, T. and Durrant-Whyte H., Simultaneous

localization and mapping (SLAM): Part II, IEEE
Robotics and Automaton Magazine, Vol. 13, No.3 pp.
108-117 , (2006)
[3] Smith, R., and Cheeseman, P., A stochastic map for
uncertain spatial relationships, Proc. 4th Int. Symp.
Robot. Res., (1987), pp. 467-474.
[4] Leonard, J.J. and Durrant-Whyte, H.F., Mobile robot
localization by tracking geometric beacons, IEEE
Trans. On Robotics and Automaton, Vol.7 , No.3 ,
pp. 376-382, (1991)
[5] Smith, R., Cheeseman, P., Estimating uncertain spatial
relationships in robotics, Autonomous Robot
Vehicles, I.J. Cox and G.T. Wilfon, Eds, New York:
Springer Verlag, pp. 167-193 .