Dr Abubakr - - PowerPoint PPT Presentation

9-1

SA-1

EE-5 6 2 : Robot Motion Planning

Simultaneous Localization & Mapping (SLAM)

۵۶۲ تابور ِى ءء

ى روا م و

Dr Abubakr Muhammad Assistant Professor Electrical Engineering, LUMS Director, CYPHYNETS Lab http: / / cyphynets.lums.edu.pk

9-2

Resources

Course material from

• Stanford CS-226 (Thrun) [ slides]
• KAUST ME-410 (Abubakr, 2011)
• LUMS EE-662 (Abubakr, 2013)

http: / / cyphynets.lums.edu.pk/ index.php/ Teaching

Textbooks

• Probabilistic Robotics by Thrun et al.
• Principles of Robot Motion by Choset et al.

9-3

SLAM OVERVI EW

Part 1.

9-4

Recall: W hat is Localization?

• Definition. Calculation of a

mobile robot’s position /

• rientation relative to an

external reference system

• Usually world coordinates serve as

reference

• Basic requirement for several robot

functions:

• approach of target points, path

following

• avoidance of obstacles, dead-ends
• autonomous environment mapping

Requires accurate maps !!

9-5

Recall: W hat is Mapping?

• Objective: Store information
• utside of sensory horizon
• Map provided a-priori or can

be online

• Types
• world-centric maps

navigation, path planning

• robot-centric maps

pilot tasks (e. g. collision avoidance)

• Problem: inaccuracy due to

sensor systems

Requires accurate localization!!

9-6

Given

• The robot’s controls
• Observations of nearby

features

Estim ate

• Map of features
• Path of the robot

The SLAM Problem

A robot is exploring an unknown, static environment.

SLAM: Simultaneous Localization And Mapping

9-7

Chicken-or-Egg?

• SLAM is a chicken-or-egg problem
• A map is needed for localizing a robot
• A good pose estimate is needed to build a map
• Thus, SLAM is regarded as a hard problem

in robotics

• A variety of different approaches to

address the SLAM problem have been presented

• Probabilistic methods outperform most
• ther techniques

9-8

W hy is SLAM a hard problem ?

SLAM: robot path and map are both unknown Robot path error correlates errors in the map

9-9

W hy is SLAM a hard problem ?

• In the real world, the mapping between
• bservations and landmarks is unknown
• Picking wrong data associations can have

catastrophic consequences

• Pose error correlates data associations

Robot pose uncertainty

9-10

SLAM:

Sim ultaneous Localization and Mapping

• Full SLAM:
• Online SLAM:

Integrations typically done one at a time

) , | , (

: 1 : 1 : 1 t t t

u z m x p

1 2 1 : 1 : 1 : 1 : 1 : 1

... ) , | , ( ) , | , (

−

∫∫ ∫

=

t t t t t t t

dx dx dx u z m x p u z m x p 

Estimates most recent pose and map! Estimates entire path and map!

9-11

Graphical Model of Full SLAM: ) , | , (

: 1 : 1 : 1 t t t

u z m x p

9-12

Graphical Model of Online SLAM:

1 2 1 : 1 : 1 : 1 : 1 : 1

... ) , | , ( ) , | , (

−

∫∫ ∫

=

t t t t t t t

dx dx dx u z m x p u z m x p 

9-13

Victoria Park Data Set Vehicle

[ courtesy by E. Nebot]

9-14

Data Acquisition

[ courtesy by E. Nebot]

9-15

Victoria Park Data Set

[ courtesy by E. Nebot]

9-16

Raw Odom etry ( no SLAM)

Odometry GPS (for reference)

9-17

Estim ated Trajectory

[ courtesy by E. Nebot]

9-18

EKF SLAM Application

[ courtesy by J. Leonard]

9-19

EKF SLAM Application

• dometry

estimated trajectory

[ courtesy by John Leonard]

9-20

PROBABI LI STI C APPROACH TO ROBOTI CS

Part 2.

9-21

Graphical Model of Full SLAM: ) , | , (

: 1 : 1 : 1 t t t

u z m x p

Action:

1 1

( | , )

t t t

p x x u

− −

9-22

Motion Models

• In general the configuration of a robot can be

described by six parameters.

• Three-dimensional cartesian coordinates plus

three Euler angles pitch, roll, and tilt.

• Throughout this section, we consider robots
• perating on a planar surface.

The state space of such systems is three- dimensional (x,y,θ).

9-23

Odom etry Model

2 2

) ' ( ) ' ( y y x x

trans

− + − = δ θ δ − − − = ) ' , ' ( atan2

1

x x y y

rot 1 2

'

rot rot

δ θ θ δ − − =

Robot moves from to . Odometry information .

θ , , y x ' , ' , ' θ y x

trans rot rot

u δ δ δ , ,

2 1

=

trans

δ

1 rot

δ

2 rot

δ

θ , , y x ' , ' , ' θ y x

9-24

Reasons for Motion Errors

bump ideal case different wheel diameters carpet and many more …

9-25

Robot Motion

9-26

Start

Error Propagation

9-27

Noise Model for Odom etry

• The measured motion is given by the

true motion corrupted with noise.

| | | | 1 1

2 1 1

ˆ

trans rot

rot rot δ α δ α

ε δ δ

+

+ =

| | | | 2 2

2 2 1

ˆ

trans rot

rot rot δ α δ α

ε δ δ

+

+ =

| | | |

2 1 4 3

ˆ

rot rot trans

trans trans δ δ α δ α

ε δ δ

+ +

+ =

9-28

Typical Distributions for Probabilistic Motion Models

2 2 2

2 1 2

2 1 ) (

σ σ

πσ ε

x

e x

−

=      − > =

2 2 2

6 | | 6 6 | x | if ) (

2

σ σ σ εσ x x

Normal distribution Triangular distribution

9-29

Effect of Distribution Type

9-30

Graphical Model of Full SLAM: ) , | , (

: 1 : 1 : 1 t t t

u z m x p

Sensing: (

| , )

t t

p z x m

9-31

Sensors for Mobile Robots

• Contact sensors: Bumpers, Whiskers
• Internal sensors
• Accelerometers (spring-mounted masses)
• Gyroscopes (spinning mass, laser light)
• Compasses, inclinometers (earth magnetic field, gravity)
• Proximity sensors
• Sonar (time of flight)
• Radar (phase and frequency)
• Laser range-finders (triangulation, tof, phase)
• Infrared (intensity)
• Stereo
• Visual sensors: Cameras, Stereo
• Satellite-based sensors: GPS

9-32

Ultrasonic Sensors

• Signal profile [ Polaroid]

9-33

Laser Range Scanner

9-34

Sources of Error

• Opening angle
• Crosstalk
• Specular reflection

9-35

Typical Ultrasound Scan

9-36

Typical Measurem ent Errors of an Range Measurem ents

• 1. Beams reflected by
• bstacles
• 2. Beams reflected by

persons / caused by crosstalk

• 3. Random

measurements

• 4. Maximum range

measurements

9-37

Proxim ity Measurem ent

• Measurement can be caused by …
• a known obstacle.
• cross-talk.
• an unexpected obstacle (people, furniture, …

).

• missing all obstacles (total reflection, glass, …

).

• Noise is due to uncertainty …
• in measuring distance to known obstacle.
• in position of known obstacles.
• in position of additional obstacles.
• whether obstacle is missed.

9-38

Beam -based Proxim ity Model

Measurement noise

zexp zmax

b z z hit

e b m x z P

2 exp )

( 2 1

2 1 ) , | (

− −

= π η       < =

−

• therwise

z z m x z P

z

e ) , | (

exp unexp λ

λ η

Unexpected obstacles

zexp zmax

9-39

Beam -based Proxim ity Model

Random measurement Max range

max

1 ) , | ( z m x z P

rand

η =

small

z m x z P 1 ) , | (

max

η =

zexp zmax zexp zmax

9-40

Resulting Mixture Density

              ⋅               = ) , | ( ) , | ( ) , | ( ) , | ( ) , | (

rand max unexp hit rand max unexp hit

m x z P m x z P m x z P m x z P m x z P

T

α α α α

How can we determine the model parameters?

9-41

Raw Sensor Data

Measured distances for expected distance of 300 cm.

Sonar Laser

9-42

Approxim ation

• Maximize log likelihood of the data
• Search space of n-1 parameters.
• Hill climbing
• Gradient descent
• Genetic algorithms
• …
• Deterministically compute the n-th

parameter to satisfy normalization constraint.

) | (

exp

z z P

9-43

Approxim ation Results

Sonar Laser

300cm 400cm

9-44

BAYES FI LTERI NG

Part 3.

9-45

Sim ple Exam ple of State Estim ation

• Suppose a robot obtains measurement z
• What is P(open| z)?

9-46

Causal vs. Diagnostic Reasoning

• P(open| z) is diagnostic.
• P(z| open) is causal.
• Often causal knowledge is easier to
• btain.
• Bayes rule allows us to use causal

knowledge:

) ( ) ( ) | ( ) | ( z P

• pen

P

• pen

z P z

• pen

P =

9-47

Exam ple

• P(z|open) = 0.6

P(z|¬open) = 0.3

• P(open) = P(¬open) = 0.5

67 . 3 2 5 . 3 . 5 . 6 . 5 . 6 . ) | ( ) ( ) | ( ) ( ) | ( ) ( ) | ( ) | ( = = ⋅ + ⋅ ⋅ = ¬ ¬ + = z

• pen

P

• pen

p

• pen

z P

• pen

p

• pen

z P

• pen

P

• pen

z P z

• pen

P

z raises the probability that the door is

• pen.

9-48

Com bining Evidence

• Suppose our robot obtains another
• bservation z2.
• How can we integrate this new

information?

• More generally, how can we estimate

P(x| z1...zn )?

9-49

Recursive Bayesian Updating

) , , | ( ) , , | ( ) , , , | ( ) , , | (

1 1 1 1 1 1 1 − − −

=

n n n n n n

z z z P z z x P z z x z P z z x P    

Markov assumption: zn is independent of z1,...,zn-1 if we know x.

) ( ] ) | ( [ ) , , | ( ) | ( ) , , | ( ) , , | ( ) | ( ) , , | (

... 1 ... 1 1 1 1 1 1 1 1

x P x z P z z x P x z P z z z P z z x P x z P z z x P

n i i n n n n n n n n

∏

= − − −

= = = η η    

9-50

Exam ple: Second Measurem ent

• P(z2|open) = 0.5

P(z2|¬open) = 0.6

• P(open|z1)=2/3

625 . 8 5 3 1 5 3 3 2 2 1 3 2 2 1 ) | ( ) | ( ) | ( ) | ( ) | ( ) | ( ) , | (

1 2 1 2 1 2 1 2

= = ⋅ + ⋅ ⋅ = ¬ ¬ + = z

• pen

P

• pen

z P z

• pen

P

• pen

z P z

• pen

P

• pen

z P z z

• pen

P

z2 lowers the probability that the door is

• pen.

9-51

Actions

• Often the world is dynam ic since
• actions carried out by the robot,
• actions carried out by other agents,
• or just the tim e passing by

change the world.

• How can we incorporate such

actions?

9-52

Typical Actions

• The robot turns its w heels to move
• The robot uses its m anipulator to grasp

an object

• Plants grow over tim e…
• Actions are never carried out w ith

absolute certainty.

• In contrast to measurements, actions

generally increase the uncertainty.

9-53

Modeling Actions

• To incorporate the outcome of an

action u into the current “belief”, we use the conditional pdf P( x| u,x’)

• This term specifies the pdf that

executing u changes the state from x’ to x.

9-54

Exam ple: Closing the door

9-55

State Transitions P(x| u,x’) for u = “close door”: If the door is open, the action “close door” succeeds in 90% of all cases.

• pen

closed 0.1 1 0.9

9-56

I ntegrating the Outcom e of Actions

∫

= ' ) ' ( ) ' , | ( ) | ( dx x P x u x P u x P

∑

= ) ' ( ) ' , | ( ) | ( x P x u x P u x P

Continuous case: Discrete case:

9-57

Exam ple: The Resulting Belief

) | ( 1 16 1 8 3 1 8 5 10 1 ) ( ) , | ( ) ( ) , | ( ) ' ( ) ' , | ( ) | ( 16 15 8 3 1 1 8 5 10 9 ) ( ) , | ( ) ( ) , | ( ) ' ( ) ' , | ( ) | ( u closed P closed P closed u

• pen

P

• pen

P

• pen

u

• pen

P x P x u

• pen

P u

• pen

P closed P closed u closed P

• pen

P

• pen

u closed P x P x u closed P u closed P − = = ∗ + ∗ = + = = = ∗ + ∗ = + = =

∑ ∑

9-58

Bayes Filters: Fram ew ork

• Given:
• Stream of observations z and action data u:
• Sensor model P(z| x).
• Action model P(x| u,x’).
• Prior probability of the system state P(x).
• W anted:
• Estimate of the state X of a dynamical system.
• The posterior of the state is also called Belief:

) , , , | ( ) (

1 1 t t t t

z u z u x P x Bel  =

1 1

{ , , , }

t t

u z u z 

9-59

Bayes Filter Exam ple

9-60

Dynam ic Bayesian Netw ork for Controls, States, and Sensations

9-61

Markov Assum ption

Underlying Assumptions

• Static world
• Independent noise
• Perfect model, no approximation errors

) , | ( ) , , | (

1 : 1 : 1 1 : 1 t t t t t t t

u x x p u z x x p

− −

=

) | ( ) , , | (

: 1 : 1 : t t t t t t

x z p u z x z p =

9-62

1 1 1

) ( ) , | ( ) | (

− − −

∫

=

t t t t t t t

dx x Bel x u x P x z P η

Bayes Filters

) , , , | ( ) , , , , | (

1 1 1 1 t t t t t

u z u x P u z u x z P   η =

Bayes z = observation u = action x = state

) , , , | ( ) (

1 1 t t t t

z u z u x P x Bel  =

Markov

) , , , | ( ) | (

1 1 t t t t

u z u x P x z P  η =

Markov 1 1 1 1 1

) , , , | ( ) , | ( ) | (

− − −

∫

=

t t t t t t t t

dx u z u x P x u x P x z P  η

1 1 1 1 1 1 1

) , , , | ( ) , , , , | ( ) | (

− − −

∫

=

t t t t t t t t

dx u z u x P x u z u x P x z P   η

Total prob. Markov 1 1 1 1 1 1

) , , , | ( ) , | ( ) | (

− − − −

∫

=

t t t t t t t t

dx z z u x P x u x P x z P  η

9-63

Bayes Filter Algorithm

1.

Algorithm Bayes_ filter( Bel(x),d ): 2. η=0 3. If d is a perceptual data item z then 4. For all x do 5. 6. 7. For all x do 8. 9. Else if d is an action data item u then 10. For all x do 11. 12. Return Bel’(x)

) ( ) | ( ) ( ' x Bel x z P x Bel = ) ( ' x Bel + =η η ) ( ' ) ( '

1

x Bel x Bel

−

=η ' ) ' ( ) ' , | ( ) ( ' dx x Bel x u x P x Bel

∫

=

1 1 1

) ( ) , | ( ) | ( ) (

− − −

∫

=

t t t t t t t t

dx x Bel x u x P x z P x Bel η

9-64

Bayes Filters are Fam iliar!

• Kalman filters
• Particle filters
• Hidden Markov models
• Dynamic Bayesian networks
• Partially Observable Markov Decision

Processes (POMDPs)

1 1 1

) ( ) , | ( ) | ( ) (

− − −

∫

=

t t t t t t t t

dx x Bel x u x P x z P x Bel η

9-65

Bayes Filters in Localization

1 1 1

) ( ) , | ( ) | ( ) (

− − −

∫

=

t t t t t t t t

dx x Bel x u x P x z P x Bel η

9-66

Sum m ary

• Bayes rule allows us to compute

probabilities that are hard to assess

• therwise.
• Under the Markov assumption,

recursive Bayesian updating can be used to efficiently combine evidence.

• Bayes filters are a probabilistic tool

for estimating the state of dynamic systems.

9-67

PARTI CLE FI LTERI NG

Part 5.

9-68

Bayes Filters in Localization

1 1 1

) ( ) , | ( ) | ( ) (

− − −

∫

=

t t t t t t t t

dx x Bel x u x P x z P x Bel η

9-69

6 9

Histogram = Piecewise Constant

9-70

Piecew ise Constant Representation

) , , ( > =< θ y x x Bel

t

9-71

Discrete Bayes Filter Algorithm

1.

Algorithm Discrete_ Bayes_ filter( Bel(x),d ): 2. η=0 3. If d is a perceptual data item z then 4. For all x do 5. 6. 7. For all x do 8. 9. Else if d is an action data item u then 10. For all x do 11. 12. Return Bel’(x)

) ( ) | ( ) ( ' x Bel x z P x Bel = ) ( ' x Bel + =η η ) ( ' ) ( '

1

x Bel x Bel

−

=η

∑

=

'

) ' ( ) ' , | ( ) ( '

x

x Bel x u x P x Bel

9-72

I m plem entation ( 1 )

• To update the belief upon sensory input and to carry out

the normalization one has to iterate over all cells of the grid.

• Especially when the belief is peaked (which is generally the

case during position tracking), one wants to avoid updating irrelevant aspects of the state space.

• One approach is not to update entire sub-spaces of the

state space.

• This, however, requires to monitor whether the robot is

de-localized or not.

• To achieve this, one can consider the likelihood of the
• bservations given the active components of the state

space.

9-73

I m plem entation ( 2 )

• To efficiently update the belief upon robot motions, one typically

assumes a bounded Gaussian model for the motion uncertainty.

• This reduces the update cost from O(n2) to O(n), where n is the

number of states.

• The update can also be realized by shifting the data in the grid

according to the measured motion.

• In a second step, the grid is then convolved using a separable

Gaussian Kernel.

• Two-dimensional example:

1/ 4 1/ 4 1/ 2 1/ 4 1/ 2 1/ 4

+ ≅

1/ 16 1/ 16 1/ 8 1/ 8 1/ 8 1/ 4 1/ 16 1/ 16 1/ 8

Fewer arithmetic operations Easier to implement

9-74

Markov Localization in Grid Map

9-75

Grid-based Localization

9-76

• Set of weighted samples

Mathem atical Description

• The samples represent the posterior

State hypothesis Importance weight

9-77

• Particle sets can be used to approximate functions

Function Approxim ation

• The more particles fall into an interval, the higher

the probability of that interval

• How to draw samples form a function/ distribution?

9-78

• Let us assume that f(x)<1 for all x
• Sample x from a uniform distribution
• Sample c from [ 0,1]
• if f(x) > c

keep the sample

• therwise

reject the sampe

Rejection Sam pling

c x f(x) c’ x’ f(x’)

OK

9-79

• We can even use a different distribution g to

generate samples from f

• By introducing an importance weight w, we can

account for the “differences between g and f ”

• w = f / g
• f is often called

target

• g is often called

proposal

• Pre-condition:

f(x)>0  g(x)>0

I m portance Sam pling Principle

9-80

I m portance Sam pling w ith Resam pling: Landm ark Detection Exam ple

9-81

Distributions

9-82

Distributions

Wanted: samples distributed according to p(x| z1, z2, z3)

9-83

This is Easy!

We can draw samples from p(x| zl) by adding noise to the detection parameters.

9-84

I m portance Sam pling

) ,..., , ( ) ( ) | ( ) ,..., , | ( : f

• n

distributi Target

2 1 2 1 n k k n

z z z p x p x z p z z z x p

∏

= ) ( ) ( ) | ( ) | ( : g

• n

distributi Sampling

l l l

z p x p x z p z x p = ) ,..., , ( ) | ( ) ( ) | ( ) ,..., , | ( : w weights Importance

2 1 2 1 n l k k l l n

z z z p x z p z p z x p z z z x p g f

∏

≠

= =

9-85

I m portance Sam pling w ith Resam pling

Weighted samples After resampling

9-86

Particle Filters

9-87

) | ( ) ( ) ( ) | ( ) ( ) | ( ) ( x z p x Bel x Bel x z p w x Bel x z p x Bel α α α = ← ←

− − −

Sensor I nform ation: I m portance Sam pling

9-88

∫

←

−

' d ) ' ( ) ' | ( ) (

,

x x Bel x u x p x Bel

Robot Motion

9-89

) | ( ) ( ) ( ) | ( ) ( ) | ( ) ( x z p x Bel x Bel x z p w x Bel x z p x Bel α α α = ← ←

− − −

Sensor I nform ation: I m portance Sam pling

9-90

Robot Motion

∫

←

−

' d ) ' ( ) ' | ( ) (

,

x x Bel x u x p x Bel

9-91

Particle Filter Algorithm

• Sample the next generation for particles using the

proposal distribution

• Compute the importance weights :

weight = target distribution / proposal distribution

• Resampling: “Replace unlikely samples by more

likely ones”

9-92

Particle Filter Algorithm

, = ∅ = η

t

S n i  1 = } , { > < ∪ =

i t i t t t

w x S S

i t

w + =η η

i t

x ) , | (

1 1 − − t t t

u x x p

) ( 1 i j t

x −

1 − t

u ) | (

i t t i t

x z p w = n i  1 = η /

i t i t

w w =

9-93

draw xi

t−1 from Bel(xt−1)

draw xi

t from p(xt | xi t−1,ut−1)

Importance factor for xi

t:

) | ( ) ( ) , | ( ) ( ) , | ( ) | (

• n

distributi proposal

• n

distributi target

1 1 1 1 1 1 t t t t t t t t t t t t i t

x z p x Bel u x x p x Bel u x x p x z p w ∝ = =

− − − − − −

η

1 1 1 1

) ( ) , | ( ) | ( ) (

− − − −

∫

=

t t t t t t t t

dx x Bel u x x p x z p x Bel η

Particle Filter Algorithm

9-94

Resam pling

• Given: Set S of weighted samples.
• W anted : Random sample, where the

probability of drawing xi is given by wi.

• Typically done n times with replacement to

generate new sample set S’.

9-95

w2 w3 w1 wn Wn-1

Resam pling

w2 w3 w1 wn Wn-1

Roulette wheel Binary search, n log n Stochastic universal sampling Systematic resampling Linear time complexity Easy to implement, low variance

9-96

• 1. Algorithm systematic_resampling(S,n):

2.

• 3. For

Generate cdf 4. 5. Initialize threshold

• 6. For

Draw samples … 7. While ( ) Skip until next threshold reached 8. 9. Insert 10. Increment threshold

• 11. Return S’

Resam pling Algorithm

1 1

, ' w c S = ∅ = n i  2 =

i i i

w c c + =

−1

1 ], , ] ~

1 1

=

−

i n U u n j  1 =

1 1 − +

+ = n u u

j j i j

c u >

{ }

> < ∪ =

−1

, ' ' n x S S

i

1 + = i i

Also called stochastic universal sampling

9-97

Mobile Robot Localization

• Each particle is a potential pose of the robot
• Proposal distribution is the motion model of the

robot (prediction step)

• The observation model is used to compute the

importance weight (correction step)

9-98

Start

Motion Model

9-99

Proxim ity Sensor Model

Laser sensor Sonar sensor

9-100

9-101

9-102

9-103

9-104

9-105

9-106

9-107

9-108

9-109

9-110

9-111

9-112

9-113

9-114

9-115

9-116

9-117

9-118

I nitial Distribution

9-119

After I ncorporating Ten Ultrasound Scans

9-120

After I ncorporating 6 5 Ultrasound Scans

9-121

Estim ated Path

9-122

Using Ceiling Maps for Localization

[Dellaert et al. 99]

9-123

Vision-based Localization

P(z|x) h(x) z

9-124

Under a Light

Measurement z: P(z|x):

9-125

Next to a Light

Measurement z: P(z|x):

9-126

Elsew here

Measurement z: P(z|x):

9-127

Global Localization Using Vision

9-128

Sum m ary – Particle Filters

• Particle filters are an implementation of

recursive Bayesian filtering

• They represent the posterior by a set of

weighted samples

• They can model non-Gaussian

distributions

• Proposal to draw new samples
• Weight to account for the differences

between the proposal and the target

• Monte Carlo filter, Survival of the fittest,

Condensation, Bootstrap filter

9-129

Sum m ary – Monte Carlo Localization

• In the context of localization, the

particles are propagated according to the motion model.

• They are then weighted according to

the likelihood of the observations.

• In a re-sampling step, new particles

are drawn with a probability proportional to the likelihood of the

• bservation.

9-130

KALMAN FI LTERI NG BASI CS

Part 6.

9-131

Techniques for Generating Consistent Maps

• Scan matching
• EKF SLAM
• FastSLAM
• Probabilistic mapping with a single

map and a posterior about poses Mapping + Localization

• GraphSLAM, SEIF

9-132

Graphical Model of Online SLAM:

1 2 1 : 1 : 1 : 1 : 1 : 1

... ) , | , ( ) , | , (

−

∫∫ ∫

=

t t t t t t t

dx dx dx u z m x p u z m x p 

9-133

Kalm an Filter Algorithm

1.

Algorithm Kalm an_ filter( µt-1, Σt-1, ut, zt): 2. Prediction: 3. 4. 5. Correction: 6. 7. 8. 9. Return µt, Σt

t t t t t

u B A + =

−1

µ µ

t T t t t t

R A A + Σ = Σ

−1 1

) (

−

+ Σ Σ =

t T t t t T t t t

Q C C C K ) (

t t t t t t

C z K µ µ µ − + =

t t t t

C K I Σ − = Σ ) (

1 1 1 1

) ( ) , | ( ) | ( ) (

− − − −

∫

=

t t t t t t t t

dx x Bel u x x p x z p x Bel η

9-134

                                              =

2 2 2 2 2 2 2 1

2 1 2 2 2 1 2 2 2 1 2 1 1 1 1 1 2 1 2 1 2 1

, ) , (

N N N N N N N N N N N

l l l l l l yl xl l l l l l l yl xl l l l l l l yl xl l l l y x yl yl yl y y xy xl xl xl x xy x N t t

l l l y x m x Bel σ σ σ σ σ σ σ σ σ σ σ σ σ σ σ σ σ σ σ σ σ σ σ σ σ σ σ σ σ σ σ σ σ σ σ σ θ

θ θ θ θ θ θ θ θ θ θ θ

             

• Map with N landmarks: (3+ 2N)-dimensional

Gaussian

• Can handle hundreds of dimensions

( E) KF-SLAM

9-135

EKF-SLAM

Map Correlation matrix

9-136

EKF-SLAM

Map Correlation matrix