“EKF Localization and EKF

SLAM incorporating prior

information”

Final Report

ME-410

Samuel Castaneda

ID: 113155

1. Abstract

In the context of mobile robotics, before any motion planning or navigation algorithm can be

executed, it is necessary for the robot to have a globally consistent map of the environment and

its own location in it. The problem of simultaneously learning the global map and the location of

the robot is commonly known as the Simultaneous Localization and Mapping (SLAM) problem.

One important result about most SLAM problems is that since objects in the map have unknown

positions, seen a new object doesn’t help the robot to get a better estimate of its position nor the

other objects poses. It’s not until the robot sees a previously known landmark the loop can be

closed and the estimation can really be accurate (achieve global consistency).

A novel SLAM approach was presented in [1] to achieve global consistency by utilizing information

extracted from aerial images of the environment as prior information to the SLAM problem. In this

work, the SLAM problem is built-up step by step, starting with a robot and sensor model,

generating and environment and first building the simple Localization problem using and Extended

Kalman Filter (EKF), then the solution is expanded to cover the online SLAM problem, and Finally

prior information is added to prove that this can actually grant global consistency without needing

to close the loop.

2. Introduction

The motivation for this project comes from how the aerial images from outdoor environments

can be easily accessed by any person with a web connection and the growing importance of

robotics in the academic and commercial worlds.

o Related Work

The idea of this work comes from the approach proposed in [1]. This approach the authors

extract features from the global aerial images to have some initial information about the

landmarks in the environment. Then as the robot moves and takes 3D laser scans of the

environment the map is built using a Graph-based formulation to solve the SLAM problem.

It is important to notice that the approach proposed in [1] includes more than just the

SLAM solution since first the aerial images needs to be processed to extract the desired

features (this process can be seen in Figure 2.1). Then for every step of the robot the

problem starts with extracting a 2D projection from the 3D laser scans (see Figure 2.2).

After obtaining this, the association problem has to made to match every measurement

with the corresponding landmark (Known as Association Problem), and finally after this is

done is when the SLAM algorithm can be applied.

Figure 2.1. Image processing: (a) Google Earth image, (b) Result of edge extraction, (c) Resultant likelihood field. [1]

Figure 2.2. Shows: (a) 3D laser scan image of the area, (b) Google Earth image, (c) Edges extraction of aerial image,

(d) Bird-eye projection of the 3D laser scan, (e) Positions extracted from (d). [1]

o Scope

For the purpose of this work, the goal is to achieve the same results given in [1] but with a

simpler version in the sense that the focus will be only in the SLAM part, correspondences

and prior information will be considered given for the purpose of this so no Image

Processing nor Association problem will be solved. Also instead of taking 3D laser scans

with the sensor and computing the 2D bird-view projection, the sensor information will

consider to be directly measuring the 2D distances.

With respect to the SLAM problem, instead of Graph-SLAM formulation used in [1], this

work will use an EKF formulation which will be build up step by step starting from a simple

Localization problem, to then extend this to solve the Online SLAM problem and finally

add the prior information.

Table 2.1 shows the comparison between work done in [1] against the work done in this

project.

Table 2.1. The Proposed work for this paper against the Original work in [1].

3. The Model

o Motion model

The model considered for the robot motion takes into account three parameters that

locates a robot in a 2D space, this parameters that constitute the state are: {x , y, θ}. The

control or input consider two variables which are ,f, T-, where “f” describes a linear

velocity command, and T defines an angular velocity command.

( 2 )

( 1 )

( 4 )

( 5 )

( 6 )

( 7 )

The model used is called a “Velocity Motion Model” and as shown in *2+ it can be

derived as follows:

Consider a robot moving among a circular trajectory generated by applying a linear

velocity (“f”) and an angular velocity (“T”) to the robot (See Figure 3.1). The radius of this

circle is given by:

If the robot position is note by {x, y} and the center of the circle is denoted by {xc, yc}:

( )

( )

( )

( )

Now, if “f” and “T” remains constant over a time interval Δt, the new position of the

robot will still be on the circle but shifted by an angle of T. Δt, solving from (2) and (3):

( )

( )

( )

( )

( ) ( )

Mixing the previous equations in a state space representation we get the motion model

Note that this model doesn’t work if the angular velocity command is zero (T=0), to solve

this issue, since the main use for the model will be to estimate the Sate and not to actually

generate a straight trajectory we will just consider a very small value for T when the input

command is zero

( 3 )

.

Figure 3.1. Robot movement described by a circular trajectory. [2]

Linearization:

The EKF that will be applied requires the use of a linearized version of the model

for certain steps. This will be done according to Taylor expansion.

Let X(k+1) = g(U, X) from equation (7) with “X(t) = {x(t), y(t), θ(t)}” been the state

and “U = {f(t), T(t)}” the input.

The Jacobian of g with respect to X is given by

(

)

where “gi” stands for the ith row of g.

Then:

By evaluating the “G” at the current state a linearized model is obtained.

Motion noise:

The EKF formulation requires Additive White Gaussian Noise (AWGN). The noise is

assumed to come in the control space as follows:

When converted into state space, the noise will come as:

To compute the covariance matrix of the noise in state space “Rk”:

where Vk is the Jacobian of g with respect to U

o Sensor Model

As sensor model we will be using a range bearing model, it measures the distance

(“rk”) and angle (“θk”) of a landmark “j” with respect to the robot state at a given

moment “k”. From basic trigonometry:

Zk(j) is the measurement of the jth landmark as a given step “k” of the robot motion, it

is computed with the function “h” that depends on the robot state at step k (“Xk”) and

the global position of the jth landmark (“Mj”). Note that for the localization problem

the landmarks positions are fixed, this will not be the case for the SLAM problem.

Zk (j) h(X(k), M(j))

Linearization:

Again, the EKF formulation requires a linearized version of the model, this time for the

sensor model. In this case the Jacobian “H” is given by:

Sensor noise:

As before the noise will be AWGN, and we will assume the range noise to be

independent to the angular noise.

4. Localization Problem

The localization problem consists on a robot moving on an environment with a known

map expressed as landmarks, the goal is to give the best possible approximation of the robot’s

location according to the sensing of the landmarks. Figure 4.1 shows the Localization problem

Set-up.

Figure 4.1. A robot located in space with known landmarks, the measurements of the landmarks will provide the

information needed to locate the robot as it moves around the environment.

2 0 2 4 6 8 10 122

0

2

4

6

8

10

12

o EKF formulation

A Kalman Filter bases its estimation on considering a linear system to keep track of

the expected value and covariance of the state as it propagates through a linear

process. The EKF is just to make the Kalman Filter track a non-linear system by using

linear approximations of the model computed around the present state for each point

(or best approximation of the present state).

The estimation process consists in two main steps: Prediction, corresponding to the

upper section of Figure 4.2; and Filtering, corresponding to the lower section of Figure

4.2.

Prediction

Also known as A-priori estimation. It uses the input and the motion model to

propagate the expected value and covariance of the state. This gives a first estimation

of the state at the next time step.

The equations for the prediction step are:

where μk1 and Σk1 represents the expected value of state, and the covariance matrix

at a time k+1; μk and Σk are the expected value of the state, and the covariance matrix

at a time k; Uk is the input at time k. Linearization for Gk and Rk are made around the

best estimation (A-posteriori estimation) of the state at time k.

Filtering

Also known as A-posteriori estimation. The measurement from the sensor has

arrived and now this information is compared with the prediction stage results to give

a refined estimation of the state.

The equations for the filtering step are:

where μk and Σk are initialized as the A-priori estimation. Linearization for Hj

(previously refereed as H) is done around the value μk. (Note that the meaning of “k”

and “k1” is different from the one used before in the sense that they don’t refer to

time “k” and time “k+1” but just to belief and corrected estimation, they are both for

time “k+1”).

This filtering stage works with one landmark “j” at a time, in case more than one

landmark is observed by the sensor, then the filter will run as many times as the

number of landmarks in range, this way each of the measurements contributes to

correct the estimation. In case no landmark is in range the sensor will not give any

measurement and this step will be skipped, leaving the A-priori as the best possible

estimation.

Figure 4.2. Extended Kalman Filter Overview.

5. SLAM Problem

The SLAM problem is in essence very similar to the Localization Problem but now the map of

the environment is also unknown, and the job now is to simultaneously estimate both, robot and

landmark coordinates.

To include the landmarks into the estimation the basic idea is just to create an extended state:

( ) ( ( ) ( )

)

Note that the map “M” now becomes “M(k)”.

Additionally for this part, the definition of each landmark will be expanded to:

where the parameter “ms” is called the “signature” and can be used to send information about

the landmark which for our simulation will be the identification “j”.

o From EKF Localization to EKF SLAM

As stated before, the state is now expanded, but the idea of the EKF formulation remains.

In regarding to the estimation of the Robot parameters, everything will be the same except for

that now instead of the filter using known knowledge of the map to evaluate h(X(k),M(j)) and

H, now it will use the best available estimation of those values (given by the A-priori or a

filtering stage for the previous landmark), is important to notice that the matrix H has to be

modified because partial derivatives now have to be taken with respect to the landmark

coordinates too, so now:

For the map we will assume static maps. The estimation of the map follows the same

formulation than estimating the state but with some important considerations to take into

account:

1) Since the map is considered static, the Prediction step should not modify the map

estimation which is the same as considering the map state space dynamics as just an

identity matrix ( M(k+1) = I.M(k) ).

2) When a landmark is seen for the first time, its estimated position will be set according

to the estimation of the robot’s position plus the sensor measurement. And naturally

the uncertainty (reflected by the covariance matrix) will come as a product of the

robot’s and sensor’s uncertainty.

3) At the start of the simulation the landmarks have completely unknown positions and

the state vector M(0) and covariance matrix Σ(0) should be initialized to reflect this. A

good way to do it is proposed in [2] by just setting the coordinates of the landmarks at

the origin and the covariance matrix for the section corresponding to the landmarks as

Diag(∞).

The full estimation will work by setting the robot’s and map’s estimators in parallel and

while each of them working by its own, they are highly coupled because each of them takes

the estimation of the other one in its formulation.

After all the considerations discussed the problem now reduces in how to set the formulas

for Prediction and Filtering correctly to work both estimations in parallel.

The method proposed in *2+ relies in declaring an auxiliary matrix “Fx” to handle this.

where “N” is the number of landmarks.

By taking Fx as proposed we can now rewrite the prediction step to satisfy the conditions

discussed above as:

The function g1 is similar to the full model “g”. And is given by:

It is easy to see that with formulation the prediction stage fulfills the conditions discussed.

Similarly, for the Filtering step, a matrix Fxj is proposed as follows:

The filtering equations can be written as:

3N

3j-3 3N-3j

Before these filtering equations it is important to check if the landmark has been

previously observed and if not, initialize it at the coordinates where it is observed. This is

done with the following equations just before each landmark processed in the filtering

step:

6. SLAM with prior information

One important result of the SLAM problem in order to obtain a globally consistent map and

robot estimation, the trajectory of the robot needs to be such that it comes back to a previously

observed landmark with small uncertainty, this will allow the global uncertainty of all landmarks

and specially the robot’s position to converge and have a reliable estimation, this property can be

observed from the results of the SLAM problem in section 7.

Having a limitation such as relaying in the robot to describe a loop like trajectory is very restrictive

for some mobile robotics applications and some other solution needs to be found to achieve

global consistency of the estimation.

If we analyze what happens in the EKF algorithm is that once a landmark is first observed, it’s

expected value and covariance matrix are modified for the first time, in the case of the loop-

closing trajectory, when the landmark is observed again, the filtering step uses the original

previous information about the landmark to correct the estimation of the robots position and with

that it corrects all other estimations for the landmarks. But what if instead of initializing the

landmark information as if they are completely unknown, we use some prior information about

the landmarks?, logically assuming there is such prior information. Well the behavior the first time

that landmark is seen will be very similar as if there was a loop closing there (some initial

information and some new information mixing for the filtering).

This idea is the one to be demonstrated by using the prior in the EKF. The way to do this is actually

very simple is the prior is given, just instead of having Y(0)={0,0,...,0} and Σ(0) as previously

described, use the prior information, Figure 6.1 shows the initialization with no prior and Figure

6.2 shows the initialization including prior information in the third and fifth landmarks.

Figure 6.1 State and covariance matrix initialization for the SLAM problem with 5 landmarks to consider, no prior

information available.

Figure 6.2 State and covariance matrix initialization for the SLAM problem with 5 landmarks to consider, prior information

added.

7. Simulation Results

RESULTS FOR LOCALIZATION PROBLEM

As we can observe from figure 7.1, the robot moves with a relatively high

uncertainty in the model, shown by the A-priori estimation ellipses, but when the sensor

detects some landmarks the filtering allows the A-posteriori estimation to be much more

certain, then as the robot moves away from the landmarks the uncertainty grows, and

only one ellipse is observed, this is because they are actually overlapped since no

landmark detection means A-priori and A-posteriori are the same. And when again just

when the robot detects more landmarks the estimation becomes certain again as seen in

section (d) of Figure 7.1, the A-priori had large uncertainty but the A-posteriori (resulting

from filtering) is much more certain.

2 0 2 4 6 8 10 122

0

2

4

6

8

10

12

2 0 2 4 6 8 10 122

0

2

4

6

8

10

12

2 0 2 4 6 8 10 122

0

2

4

6

8

10

12

2 0 2 4 6 8 10 122

0

2

4

6

8

10

12

Figure 7.1 EKF Localization Problem Simulation.

Blue Rectangle – Robot Blue dots – Landmarks Dotted line – Sensor range Blue ellipse – Apriori uncertainity Red ellipse – Aposteriori uncertainty

( a ) ( b )

( c ) ( d )

RESULTS FOR SLAM PROBLEM

2 0 2 4 6 8 10 122

0

2

4

6

8

10

12

2 0 2 4 6 8 10 122

0

2

4

6

8

10

12

2 0 2 4 6 8 10 122

0

2

4

6

8

10

12

2 0 2 4 6 8 10 122

0

2

4

6

8

10

12

2 0 2 4 6 8 10 122

0

2

4

6

8

10

12

2 0 2 4 6 8 10 122

0

2

4

6

8

10

12

( a ) ( b )

( d ) ( c )

( f ) ( e )

Figure 7.2 EKF SLAM Problem Simulation.

ROBOT Rectangles: Black– Real position. Green- Noiseless trajectory. Red – Apriori estimation. Blue – Aposteriori estimation. Blue ellipse – Apriori uncertainty Red ellipse – Aposteriori uncertainty Dotted line – Sensor range

LANDMARKS Blue dots – Real positions Red crosses – Estimated positions. Blue ellipses – Uncertainty of landmark.

As shown in Figure 7.2 both landmarks and robot position start at the origin. As

the robot moves its position becomes more and more uncertain, affecting with it the

uncertainty of the landmarks it observes on the way, also, the real and estimated

trajectories diverge more and more from each other.

It is not until the robot closes the loop with a more certain landmark in part (d) of Figure

7.2 when the filtering can make significant corrections, this can be seen by comparing A-

priori and A-posteriori estimations, the first one is very close to the path that the noiseless

trajectory would follow and the second one very close to the actual position of the robot

following the real trajectory; and also by a significant reduction in all uncertainty ellipses.

Finally, after one more step where the robot is still able to see the landmark (Section (f) of

Figure 7.2), all estimations becomes even more accurate.

RESULTS FOR SLAM PROBLEM WITH PRIOR INFORMATION

The experiment again starts with the robot at the origin but some landmarks are

initialized according to prior information, this can be seen as two of the landmarks in

Figure 7.3 (a) have already their estimated position and uncertainty.

Then again as in the previous case the robot’s and landmarks uncertainty grows as the

simulation advances, it can be observed a slight correction in the robot’s estimation when

the third landmark is observed, but the interesting result comes when the fifth landmark is

observed because just one step before in Figure 7.3 (e), uncertainty is high for both the

robot and the fourth landmark, but in Figure 7.3 (f) the robot observes the fifth landmark

and because of the prior knowledge, the robot is able to again get a very good estimation

of its location and also the fourth landmark’s estimation gets better.

This proves that the prior knowledge about a landmark behaves as a closing loop system

without having to close the loop.

2 0 2 4 6 8 10 122

0

2

4

6

8

10

12

2 0 2 4 6 8 10 122

0

2

4

6

8

10

12

2 0 2 4 6 8 10 122

0

2

4

6

8

10

12

2 0 2 4 6 8 10 122

0

2

4

6

8

10

12

2 0 2 4 6 8 10 122

0

2

4

6

8

10

12

2 0 2 4 6 8 10 122

0

2

4

6

8

10

12

( a ) ( b )

( d ) ( c )

( e ) ( f )

Figure 7.3 EKF SLAM with prior information Problem Simulation.

8. Conclusions

With the conclusion of this project, the Localization and SLAM techniques are clearly

understood as well as the EKF setup and operation.

The EKF approach can be effectively applied to solve both localization and SLAM problems and can

also be extended to incorporate initial knowledge about the landmarks in the case of SLAM with

prior information.

When prior information about the landmarks is available the SLAM problem can achieve global

consistency without having to close the loop.

This EKF approach can be easily extended to applications outside the robotics field, as long as the

system can be modeled and the sensing device is sufficient to grant an observable process and the

noise is AWGN.

By comparing the results obtained from this work with the ones from [1] it’s possible to affirm that

Graph-Slam and EKF formulations can both effectively solve the SLAM problem and after applying

prior information, both can give similar results, the choice of which is better comes down to the

kind of noise, computational resources available, and number of landmarks to be processed.

References

[1] Kummerle, Kleiner, etal. “Large Scale Graph-based SLAM using Aerial Images as Prior

Information”.

[2] Thrun, S; Burgard, W; Fox,D “Probabilistic Robotics”, Cambridge, Mass. : MIT Press, c2006.