Simultaneous Localization and Mapping · m(k) = z(k)− h(x$(k / k −1)) Sk=∇h xx kPkk−∇+hkR 12 ()() 0.95 a=

12 1

SLAMEduardo Nebot 1

Simultaneous Localization and Mapping

Eduardo Nebot

Australian Centre for Field Robotics [email protected]

http://acfr.usyd.edu.au/homepages/academic/enebot

SLAMEduardo Nebot 2

Overview

• The SLAM problem• Models • Standard EKF Implementation• Simplifications in the Prediction and Update Stages• Optimal Implementation of EKF SLAM• Relative Map Representation• Sub-optimal Filters• Exploration• Labs

12 2

SLAMEduardo Nebot 3

Basic Principle of SLAM

Vehicle Features

SLAMEduardo Nebot 4

System Description

RADAR

Steering angle

Wheel speed

MODEL

Gyro(INS)

Observation

Estimator

GPS

MAP states

Vehicle pose

Comparison

Objects Detection

Data Association

Additional Landmarks properties

Compass

LASER

12 3

SLAMEduardo Nebot 5

Kalman Implementation

• Prediction Stage

( / 1) ( ) ( 1/ 1) ( )Tx xP k k f k P k k f k Q− = ∇ − − ∇ +

x k f x k u k k v k( ) ( ( ), ( ), ) ( )= − − + −1 1 1

Can also model noise in the inputs

• This step is performed each time a set of inputs is available ( High Frequency information ).

• The uncertainty in the pose of the vehicle will grow according to the uncertainty of the model

SLAMEduardo Nebot 6

Kalman Implementation

• Update Stage– The update stage is performed once a feature is

associated to a known feature.

ˆ( ) ( ) ( ( / 1))k z k h x k kµ = − − ˆ( / ) ( / 1) ( ) ( )x k k x k k W k kµ= − +%

1( ) ( / 1) ( ) ( )

( / ) ( / 1) ( ) ( ) ( )

( ) ( ) ( / 1) ( )

Tx

T

Tx x

W k P k k h k S k

P k k P k k W k S k W k

S k h k P k k h k R

−= − ∇

= − −

= ∇ − ∇ +

12 4

SLAMEduardo Nebot 7

Vehicle Model

cos( )sin( )tan( )

c c

c c

c

x vy v

v

φφ

φ α

=

&&&

We translate the centre to the GPS antenna position (top of the Laser)

cos sinsin cos

v c

v c

x x a by y a b

φ φφ φ

+ − = + +

The velocity is measured with and encoder located in the back left wheel. The velocity Vc is then:

1 tan( )

ec

vv

HL

α=

−

SLAMEduardo Nebot 8

Vehicle Model

Discrete Model

( ) ( ) ( )( ) ( )

( ) ( ) ( )( ) ( )

( )

cos sin cos tan

sin cos sin tan

tan

cc

cc

c

vv a b

Lxv

y v a bL

vL

&&&

φ φ φ α

φ φ φ αφ

α

⋅ − ⋅ ⋅ + ⋅ ⋅ = ⋅ + ⋅ ⋅ − ⋅ ⋅ ⋅

( ) ( cos( ) tan( )( sin( ) cos( )))( 1)( 1) ( , ) ( ) ( sin( ) tan( )( cos( ) sin( )))( 1)

( ) tan( )

cc

cc

c

vx k T v a b

Lx kv

y k f x u y k T v a bL

k vk T

L

φ φ φ φ

φ φ φ φφ

φ α

+ ∆ − + +

+ = = + ∆ + + − +

+ ∆

Continuous Model

12 5

SLAMEduardo Nebot 9

Vehicle Model

Discrete Model

( ) ( cos( ) tan( )( sin( ) cos( )))( 1)( 1) ( , ) ( ) ( sin( ) tan( )( cos( ) sin( )))( 1)

( ) tan( )

cc

cc

c

vx k T v a b

Lx kv

y k f x u y k T v a bL

k vk T

L

φ φ φ φ

φ φ φ φφ

φ α

+ ∆ − + +

+ = = + ∆ + + − +

+ ∆

Jacobian

1 0 ( sin( ) tan ( cos( ) sin( )))

0 1 ( cos( ) tan ( sin( ) cos( )))

0 0 1

cc

cc

vT v a b

Lvf

T v a bX L

φ α φ φ

φ α φ φ

−∆ + −

∂ = ∆ − + ∂

SLAMEduardo Nebot 10

Observation Model

• Jacobians

2 2( ) ( )( ) ( )

atan( ) 2

L v L vr

L v

L v

x x y yz

h x y yzx x

βπ

φ

− + −

= = −− + −

[ ]

( ) ( )

2 2 2 2

2

( , , , , )

( , , , , )

1, ,0,0,0,.... , ,0,0....0

, , 1, 0,0,...., , ,0,0...0

rr

v v v L L

v v v L L

r

L v L v

zhx y x yh x

h zXx x y x y

withh

x y x yXh y x y xX

x x x y y y

x

β β

β

φ

φ

∂ ∂ ∂∂ ∂ = = ∂ ∂∂ ∂ ∂

∂= −∆ −∆ ∆ ∆

∂ ∆∂ ∆ ∆ ∆ ∆ = − − − ∂ ∆ ∆ ∆ ∆ ∆ = − ∆ = −

∆ = ∆ 2y+ ∆

12 6


Data Association

• Before the Update Stage we need to determine if the feature we are observing is: – An old feature – A new feature

• If there is a match with only one known Feature:– The Update stage is run with this Feature information– Not all the Feature need to be checked since the vehicle

pose is known with a given uncertainty (Selective Search)

( ) ( ) ( / 1) ( )Tx xS k h k P k k h k R= ∇ − ∇ +µ( ) ( ) ( $( / ))k z k h x k k= − −1

1 20.95( ) ( ) ( )T k S k kα µ µ χ−= <


New Features

• If there is no match then a potential new feature has been detected

• We do not want to incorporate an spurious observation as a new feature– It will not be observed again and will consume

computational time and memory– The features are assumed to be static. We don not want to

accept dynamic objects as features: cars, people etc.

12 7


Acceptance of New Features

• Get the feature in a list of potential features• Incorporate the feature once it has been observed for a

number of times

• Advantages:– Simple to implement– Appropriate for High Frequency external sensor

• Disadvantages:– Loss of information– Potentially a problem with sensor with small field of view: a feature

may only be seen very few times

• APPROACH 1


Acceptance of New Features

• The state vector is extended with past vehicle positions and theestimation of the cross-correlation between current and previous vehicle states is maintained. With this approach improved data association is possible by combining data form various points– J. J. Leonard and R. J. Rikoski. Incorporation of delayed decision making

into stochastic mapping – Stephan Williams, PhD Thesis, 2001, University of Sydney

• Advantages:– No Loss of Information– Absolutely necessary for Low frequency external sensors ( ratio between

vehicle velocity and feature rate information )

• Disadvantages:– The implementation is more complicated

• APPROACH 2

12 8


Incorporation of New Features

0 0, ,

0 0 0, ,

v v v m

m v m m

P PP

P P

=

• We have the vehicle sates and previous map

We observed a new feature and the covariance and cross-covariance terms need to be evaluated

0 0, ,

0 01 , ,

??

? ? ?

v v v m

m v m m

P PP P P

=


Incorporation of New Features

• Approach 1

0 0

0 00

00

0 0

vv vm

mv mm

P PP P P

A

=

With A very large

1( ) ( / 1) ( ) ( )

( ) ( ) ( / 1) ( )

( / ) ( / 1) ( ) ( ) ( )

Tx

Tx x

T

W k P k k h k S k

S k h k P k k h k R


−= − ∇

= ∇ − ∇ +

= − −

1 1 1

1 1 11

1 1 1

vv vm vn

mv mm mn

nv nm nn

P P PP P P P

P P P

=

• Easy to understand and implement

• Very large values of A may introduce numerical problems

12 9


Analytical Approach

0 0, ,

0 0 0, ,

v v v m

m v m m

P PP

P P

=

• We can also evaluate the analytical expressions of the new terms

0 0, ,

0 01 , ,

??

? ? ?

v v v m

m v m m

P PP P P

=


Analytical Approach

( , )

v v

m m

n v

X XX XX h X Z

=

( , ), ( , ), ( )

( , ), ( , ), ( )

( / , )

( / )v m

v m

v m X k k X k k Z k

z X k k X k k Z k

J F X X

J F Z

= ∂ ∂

= ∂ ∂

0 0 00 0 , 0

0 0z

IJ I J

ξ η

= =

0 0 0, , ,

0 0 00 , , ,

0 0 0, , ,

Tv v v m v v

T Tm v m m m v

Tv v v m v v

P P PJ P J P P P

P P P

ξξ

ξ ξ ξ ξ

=

0

0 0 00 0 00 0

Tz zJ P J

B

=

( , ), ( )( ( , ) / )vv x k k Z k

B Rh X Z z

η ηη

== ∂ ∂

• Assume the following System

We need P1

cos( / 2)( , )

sin( / 2)v v

v v

xh X Z

yϕ α πϕ α π

+ + − = + + −

1 0T T

z zP J P J J R J= +

12 10


Analytical Approach

0 0 0, , ,

0 0 01 , , ,

0 0 0, , ,

Tv v v m v v

Tm v m m m v

Tv v v m v v

P P PP P P P

P P P B

ξξ

ξ ξ ξ ξ

= +

( , , ) ( ( / ), ( ), ( ))

1 0 cos( / 2)/ ( , , )

0 1 sin( / 2)v v

vv v v

v r k k r k k

h x yϕ α ϕ α

ϕ α πϕ

ϕξ

α π=

+ − = ∂ ∂ = + −

( , ), ( )( ( , ) / )vv x k k Z k

Rh X

BZ z

η ηη

== ∂ ∂

• The analytical form of the covariance matrix for the new feature:

cos( / 2) sin( / 2)( ( , ) / ) / ( , )

sin( / 2) cos( / 2)v v

vv v

rh X Z z h r

rϕ α π ϕ α π

η αϕ α π ϕ α π

+ − − + − = ∂ ∂ = ∂ ∂ = + − + −


Now We know

• Structure of the Problem• Models• Kalman Filter Equations• Data Association• Feature Incorporation

Problem Solved ?

12 11


Computational Requirements

1

1

, ...

v

L

v L

N

N

XX

X

xx y

X y Xxy

φ

=

= =

1V TT

fJF x

IXI

∂ ∅ ∅ ∂ ∂= = ∅∂ ∅

3 3 31 , ,x xN NxNJ R R I R∈ ∅∈ ∈

The computational requirements for each update will be proportional to

3N


Simplifications ( Prediction Stage )

• In most navigation system the dead reckoning information is available at high frequency.

• The full error covariance matrix is only needed when a new set of observation is available

( ) ( )( )( ) ( ) ( )

1

1, , T

X k F X k

P k k J P k k J Q k

+ =

+ = ⋅ ⋅ +

) )

12 12



Considering the zeros in the Jacobian matrices.

11 121 1

21 22 2

3 3 3 2 2 21

3 3 3 2 2 211 12 21 12 22

, ,

, , ,

T TVT

T T

x x N Nx N

x x N T Nx N

P P QJ JJ P J Q

P PI I

J R R I R

P R P R P P P R

∅∅ ∅ ⋅ ⋅ + = ⋅ ⋅ + ∅ ∅∅ ∅

∈ ∅ ∈ ∈

∈ ∈ = ∈

( )

11 12 1 11 1 12 1 11 1 121

21 22 21 22 21 22

1 11 1 1 121 11 1 12 1 11 1 1 121

21 22 21 1 22 1 12 22

T

TTT TT

TT

P P J P J P J P J PJJ P

P P I P I P P PI

J P J J PJ P J P J P J J P IJJ P J

P P P J P II J P P

⋅ ⋅ ⋅ ⋅∅ ⋅ = ⋅ = = ⋅ ⋅∅

⋅ ⋅ ⋅⋅ ⋅ ⋅ ⋅ ⋅ ⋅∅ ⋅ ⋅ = ⋅ = = ⋅ ⋅∅ ⋅



( )

11 12 1 11 1 12 1 11 1 121

21 22 21 22 21 22

1 11 1 1 121 11 1 12 1 11 1 1 121

21 22 21 1 22 1 12 22

T

TTT TT

TT

P P J P J P J P J PJJ P

P P I P I P P PI

J P J J PJ P J P J P J J P IJJ P J

P P P J P II J P P

⋅ ⋅ ⋅ ⋅∅ ⋅ = ⋅ = = ⋅ ⋅∅

⋅ ⋅ ⋅⋅ ⋅ ⋅ ⋅ ⋅ ⋅∅ ⋅ ⋅ = ⋅ = = ⋅ ⋅∅ ⋅

( ) ( )( )( ) ( )

11 1 12

1 12 22

2, ,( 2 / )

, ,T

P k k G P k kP k k

G P k k P k k

+ ⋅ + =

⋅

( ) ( )( ) ( ) ( ) ( ) ( ) ( )( ) ( )

1 1 1

11 1 1 11 1 11 1

1

2, 1 , 1TT

G J k J k

P k k J k J k P k k J k Q k J k

= + ⋅

+ = + ⋅ ⋅ ⋅ + ⋅ +with

12 13



For n predictions:

( )( ) ( )

( )( ) ( )11 1 12

1 12 22

, ,,

, ,T

P k n k n G P k kP k n k

G P k k P k k

+ + ⋅ + =

⋅

( ) ( ) ( ) ( )1

1 1 1 1 10

, 1 ....n

i

G G k n J k i J k n J k−

=

= = + = + − ⋅ ⋅∏with

3 3 3 2 2 21

3 3 3 2 2 211 12 21 12 22

, ,

, , ,

x x N Nx N

x x N T Nx N

J R R I R

P R P R P P P R

∈ ∅ ∈ ∈

∈ ∈ = ∈

P11 is required at each step, P22 remains constant and P21

P12 are required only at the update stage


Simplification Update stage

The evaluation of the gain W requires P H’

Quadratic ( 4 * M * M )

1( ) ( / 1) ( ) ( )

( / ) ( / 1) ( ) ( ) ( )

( ) ( ) ( / 1) ( )

Tx

T

Tx x

W k P k k h k S k


S k h k P k k h k R

−= − ∇

= − −

= ∇ − ∇ +

12 14



( )( )

[ ]

( ) ( ) ( )

( ) ( ) ( )

21 1 2 2

2 31

2 22

1 2

, , , , ( 3)

, ,

,

, null matrixs .

xM

X X k

x

V X X k X X k

x

i i iX X k X X k

j

hH H k H H R M N

X

h hH R

X x y

h hH R

X x y

hj i

X

φ

=

= =

= =

∂= = = ∅ ∅ ∈ = +

∂

∂ ∂= = ∈

∂ ∂

∂ ∂= = ∈

∂ ∂

∂∅ ∅ = = ∅ ∀ ≠ ∂

The Jacobian of the Observation matrix H will normally have a large number of zeros since very few landmarks will be observed at a given time:



The operations can be simplified with

TP P W S W= − ⋅ ⋅Quadratic ( 4 * M * M )

1 1 2 2

3 21 2,

T T T

Mx Mx

P H P H P H

P R P R

⋅ = ⋅ + ⋅

∈ ∈

2*2

1 2

T

T Mx

S H P H R R

W P H S R−

= ⋅ ⋅ + ∈

= ⋅ ⋅ ∈

Still the main computational requirement is in the update of P ( order 4 M*M )

12 15


Experimental Results


Experimental Results

12 16


Estimated Trajectory using beacons

Partial TrajectoryFull trajectory-20 -15 -10 -5 0 5 10 15 20

-25

-20

-15

-10

-5

0

5

10

15

20

East Meters

Nor

th M

eter

s

Path

EstimatedEst. SampleEst. Beac.BeaconsGPS


Estimated error covariance

The error is in the order of 7 cm.

60 80 100 120 140 160 1800

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

Time(secs)

Des

viat

ion(

m)

Model Covariance

X var.Y var.Steer.

12 17


Innovation Sequence

Partial Trajectory60 80 100 120 140 160 180-0.5

-0.4

-0.3

-0.2

-0.1

0

0.1

0.2

0.3

0.4

0.5Zr Innovations

Time(secs)

Des

viat

ion(

m)

InnovationsSta. Desv. Inn. (95%)


Autocorrelation of Innovation Sequence

In this case the location of the beacon is not required0 50 100 150 200 250 300 350 400 450 500-0.4

-0.2

0

0.2

0.4

0.6

0.8

1Autocorrelation of Innovation Sequence

Var. ZrVar. Ztheta95% Confi. Bounds

12 18


Beacons Standard Deviations

The error is in the order of 7 cm.

60 80 100 120 140 160 1800

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

Time(secs)

Des

viat

ion(

m)

Beacons Covariances

Beac.1(east)Beac.1(north)Beac.3(east)Beac.3(north)Beac.5(east)Beac.5(north)Beac.7(east)Beac.7(north)


Efficient EKF implementation of SLAM

• For environment with large number of landmarks the standard Kalman Filter implementation is still very expensive N * N

12 19


Compressed Filter (CEKF)

Vehicle Features

• Key Concept: – When the vehicle navigates in a local area observing a

group of features the information gained is a function of only the observed features.

– This information can be saved and then transferred in one iteration to the rest of the map

A

Global Area


Compressed EKF (CEKF)

Assume a system with dynamic and observation models:

( ) ( ) ( )( )( ) ( )( )1 , , ,

, ,

n

m

X k F X k u k k X R

Y k G X k k Y R

+ = ∈

= ∈

With:

( )( ) ( ) ( )

( )( ) ( )

( )

1

,1 2

1

,

, , , ,

, [ , ]

, ,1, ,

1

, ,

, , /

i A Bi i

i i i i

i

ii

i i

i

A N N NA B A B

B

I

i i i ii

i AA

B B

i i

A

XX X R X R X R N N N

X

k k

f X u kX kF X u k

X k X k

y G X k h X

kk k k

k

X u

−=

= ∈ ∈ ∈ < <

Ω = Ω Ω =

+ = = +

≤ ≤

= =

∀

∪

12 20


Compressed EKF (CEKF)

When k=k1 1 1 1, 0 , 0k k kIφ ψ θ= = =

When k=k2 Global

update

( ) ( ) ( )

( ) ( ) ( ) ( ) ( )

( ) ( ) ( )

2 2 1

2 1 1 2 1

22 1 1

, 1 ,

, , , 1 ,

, , ,

ab k k ab k

bb k bb k ba k k ab k

kb k b k ba k

P P

P P P P

X X P

φ

ψ

θ

−

−

= ⋅

= − ⋅ ⋅

= − ⋅

Auxiliary ops. order Na

EKF order Na

( )

1 1 1

1, ,1

1 1 1 ,

standard EKF predicting for ,prediction step

, ,

standard EKF updating for ,

update step

aa a

k aa k k k k k

aa a

Tk a k k a kk k k

Tk k k k k k aa k k

P X

J

P X

H S HI

P

φ φ ψ ψ θ θ

βφ µ φψ ψ φ β φ µ β

− − −

−−

− − −

= ⋅ = =

= ⋅ ⋅= − ⋅

= + ⋅ ⋅ = ⋅1

1 2 , 1 1 1T T

k k k a k k kH S zθ θ φ −− − − − −

= + ⋅ ⋅ ⋅

k1 < k < k2 Each Prediction and

update

k=k2


Advantages of the CEKF Algorithm

Vehicle Features

• Constant Computational Requirements– Independent of the total number of features in the

global map

• Full use of High Frequency sensors

12 21


Compressed Filter

A

Global Area

• The computational cost of the SLAM proportional to 2Na * 2Na

– (Na landmarks in the local area)• Full update is only required when the vehicle leaves

the local Area A– Only the features in the new area need to be updated


General Compressed Filters

t 1 t 2 t 3 t4

t

( )( )

( )( )

( ) ( )

1

1

11

1 1

1

1

1

, ,1....

1

,

A

B

AA

B B

A

XX

X

X

f u kkk k

y k

X

h

X

X

X k

=

+ = +

=

( )( )

( )( )

( ) ( )

3

3

33

3 3

3

3

3

, ,1....

1

,

A

B

AA

B B

A

X

f u kkk k

XX

XX

y k

X X

kXh

=

+ = +

=

( )( )

( )( )

( ) ( )

2

2

22

2 2

2

2

2

, ,11

,

A

B

AA

B B

A

X

f u kkk k

XX

XXX

y k h k

X

X

=

+ = +

=

12 22


Compressed filter operation

t1 t2 t3 t4

high frequency internal updates high frequency internal updates

global update global update global update global update

•Internal estimator ( predictions and observations ) at high frequency. Estimator running on a reduced system

•External Estimator

•Global updates: low frequency full EKF update.

AX


*EKF

*”Unscented Filter”

*S.O.G. ….

Global Update (Large system estimation): EKF

t1 t2 t3 t4

t

Internal operation* high frequency

* Small system

Compressed filter operation

12 23


Global update

( )2 2

2 2SLAM

general case

case ( ) ( )A B

B A B

O N N

O L N O N N

⋅

⋅ ≤ ⋅

It is a low frequency event.

The reduced system estimator transfers the collected information to the full system doing an full EKF update.

If necessary, It performs a Gaussian Approximation of p(Xa(k)|Z(k))

Full EKF update

Computational Cost

An EKF / EKF combination (CEKF) gives identical result to the full EKF.


Compressed filters Properties:

*Internal estimator ( small system) can be more sophisticated.

* Sub optimal simplifications run at the global update (low frequency)

•High frequency.

•Good numerical stability.

•Adequate for non linear problems.

•It can be less conservative…if Sub Optimal simplifications are used.

12 24


EXAMPLE

When k=k1 1 1 1, 0 , 0k k kIφ ψ θ= = =

When k=k2 Global

update

( ) ( ) ( )

( ) ( ) ( ) ( ) ( )

( ) ( ) ( )

2 2 1

2 1 1 2 1

22 1 1

, 1 ,

, , , 1 ,

, , ,

ab k k ab k

bb k bb k ba k k ab k

kb k b k ba k

P P

P P P P

X X P

φ

ψ

θ

−

−

= ⋅

= − ⋅ ⋅

= − ⋅

Auxiliary ops. order Na

EKF order Na

( )

1 1 1

1, ,1

1 1 1 ,

standard EKF predicting for ,prediction step

, ,

standard EKF updating for ,

update step

aa a

k aa k k k k k

aa a

Tk a k k a kk k k

Tk k k k k k aa k k

P X

J

P X

H S HI

P

φ φ ψ ψ θ θ

βφ µ φψ ψ φ β φ µ β

− − −

−−

− − −

= ⋅ = =

= ⋅ ⋅= − ⋅

= + ⋅ ⋅ = ⋅1

1 2 , 1 1 1T T

k k k a k k kH S zθ θ φ −− − − − −

= + ⋅ ⋅ ⋅

k1 < k < k2 Each Prediction and

update

k=k2


Map Management

r

hysteresis region

Active sectors

Active landmark

Passive landmark

12 25


Map Management


Experimental Run

12 26


Outdoor Environment


SLAM

12 27


Map and Trajectory

Landmarks

Covariance


Landmark Covariance

12 28


CEKF–SLAM vs Full EKF SLAM

156 158 160 162 164 166 168 170 172 1740.18

0.19

0.2

0.21

0.22

0.23

0.24

0.25

0.26

0.27

Sta

ndar

d D

evia

tion

Time

Compression Filter - Full Slam Comparison

Compresed Filter

Full Slam

Full Update


Compressed Algorithm

240 260 280 300 320 340 360 380

-5

-4

-3

-2

-1

0

1

x 10-3 Compression Filter - Full Slam Comparison

Time

Cov

aria

nce

Diff

eren

ce

Full Update

12 29


Relative Representation


Landmark classification

Landmark types

•“A” Bases : Absolute

•“B” Bases : Semi-absolute

• Li normal landmarks: Relative

Base B

L 1

L 2

L 3

Base Base A

12 30


Constellation Map

B1A1

L1,1

L1,2

L1,3

L1,n

A2

B2

L2,n

L3,n

B3

A3

X

Y


SLAM Map and trajectory (Victoria Park)

12 31


Correlation Coefficients for the absolute representation

50 100 150 200 250 300

50

100

150

200

250

300

( )

( )( ) ( )

[ ]

,

,

, ,

0 , 1

c i j

P i j

P i i P j j

=

⋅

∈


50 100 150 200 250 300

50

100

150

200

250

300

constellations

bases and vehicle

( )

( )( ) ( )

[ ]

,

,

, ,

0 , 1

c i j

P i j

P i i P j j

=

⋅

∈

Correlation Coefficients for the absolute representation

12 32


Sub-Optimal SLAM


Sub-optimal Solutions: De-correlation Algorithms

• In the general case it is possible to de-correlate the covariance submatrices corresponding to two groups of states, Xa and Xb.

1

11 1

1

,

, , ,...

.. .... .. .. .... .. .. ..

.. ..

n x n m xm

T l xl

T T

m

n nm

A D E A R B RP D B F C R

E F C D E F

d d

D

d d

∈ ∈ = ∈

=

2

1 2

0

0T T

A E

P B FE F C

P P

α

β

+ = +

≤

12 33


De-correlation Procedure

0 00 0

, / 0

T T

T

C CP

C C

CC

α α α α α αβ β β β β β

αα β τ

β

+ − + = = − ≤ + − +

− = ≥ −

% % %% % %

%%% %

, ,1,

, , ,1 1 ,,

,

,/

0 ,

1,

/

0 ,

0 ,

m

i k i kki j

n n

i k i k k ik k k ii j

i k

c i j

i j

c c i j

i j

i k

κα α

κκβ β

κ

=

= =

⋅ == ≠

⋅ = ⋅ == ≠

> ∀

∑

∑ ∑

% %

% %% %

, , 1i k k iκ κ= =%

, , , ,1 1

,m n

i i i k j j k jk k

c cα β= =

= =∑ ∑%%


Selection of Passive and Active States

Active base landmarks

Active relative landmarks.

Passive close relative landmarks.

Passive far relative landmarks.

.

12 34


State Covariance

Matrix

Decorrelation Matrices

XaB

XaR

XbB

XbR1

C C

C

C

0

0

XbR200

0

0


Reduced Covariance Matrix

a

b

0

0

12 35


Experimental results


Compressed / Simplifications

12 36


Difference in position estimation


Difference in orientation estimation

12 37


Exploration


Map (section of Victoria Park)

12 38


Information maps


Example, using laser observations

( ) ( )

0

0

0 00 0 , 10000 0

, , det

MP M M

M

f x y P Pϕ

= =

= −∆

( ) ( ), , ,f x y F x yϕ =

Using omni-directional sensors

( 360o laser or two 180o lasers )

12 39


Original map and Information Map

-80 -60 -40 -20 0 20 40 60-30

-20

-10

0

10

20

30

40

50

60


Level sets

-60 -40 -20 0 20 40 60-20

-10

0

10

20

30

40

50

level lines (of fig 2)

12 40


SLAM Research

• Robust SLAM Implementation– Incorporation Absolute / Relative Information– Closing Large Loops– Link control with navigation– Extension to 3-D ( Inertial / 3D sensors )

• Multiple Vehicle problem• Environment Representation

– Non-feature based representation– Terrain Description– Generic definition of Terrain Features

• All terrain Navigation– Dead Reckoning from external information– Incorporation of 3D sensory information– Inertial aided with relative information


Labs

• SLAM code– GPS ( running for the first few seconds to obtain

heading )– Feature Incorporation ( validation )

• Victoria Data– ViewLsr ( Return range and bearing to trees )– Use same vehicle models

12 41


END

Documents

Simultaneous Localization and Mapping · m(k) = z(k)− h(x$(k / k −1)) Sk=∇h xx kPkk−∇+hkR 12 ()() 0.95 a=