L ECTURE 22: M AP B UILDING ( UNKNOWN # OF LANDMARKS ) EKF SLAM I - - PowerPoint PPT Presentation

SLIDE 1

16-311-Q INTRODUCTION TO ROBOTICS FALL’17

LECTURE 22:

MAP BUILDING (UNKNOWN # OF LANDMARKS) EKF SLAM

INSTRUCTOR: GIANNI A. DI CARO

SLIDE 2

2

PROBLEM: WHAT IF THE NUMBER OF LANDMARKS IS UKNOWN?

The state vector ξk must be incrementally expanded by two new coordinate components [𝝁ix 𝝁iy ] each time a new landmark 𝝁i is observed Usually, both the locations and the number M of the landmarks existing in the environment are not known a priori

Where(?) the M(?) landmarks?

State vector ξ of the EKF: (Coordinates of the M landmarks)

Formally, the expansion of the state vector ξk amounts to the following process dynamics (before process dynamics was stationary), implemented through the use of the auxiliary (non linear!) function q():

ξ∗

k+1 = q(ξk, zk+1; xk, yk, θk) =

" ξk g(xk, yk, θk, zk+1) # = 2 6 6 4 ξk xk + ρk cos(θk + βk) yk + ρk sin(θk + βk) 3 7 7 5 = 2 6 6 4 ξk λi

kx

λi

ky

3 7 7 5

If ξk is 2n × 1, n < M ⇒ ξ∗

k+1 is 2(n + 1) × 1.

The order of the landmarks in the state vector depends on the order of observation

SLIDE 3

3

THE STATE EXPANSION FUNCTION

ξ∗

k+1 = q(ξk, zk+1; xk, yk, θk) =

" ξk g(xk, yk, θk, zk+1) # = 2 6 6 4 ξk xk + ρk cos(θk + βk) yk + ρk sin(θk + βk) 3 7 7 5 = 2 6 6 4 ξk λi

kx

λi

ky

3 7 7 5

• The vector function g(x, y, θ, z), which is the inverse of the observation function

l(), gives the measured coordinates (in the world frame) of the observed landmark.

• Inputs: known vehicle pose [︎x y θ]T , sensor observation z = [︎ρ β]T at time step k.
• g() gets the local robot measure and outputs the world coordinates of the
• bserved landmark:

g(x, y, θ, z) =   x + ρ cos(θ + β) y + ρ sin(θ + β)  

SLIDE 4

4

EXPANDING BOTH STATE AND COVARIANCE MATRIX

• After the new landmark i has been observed, at step k + 1, the expanded state vector:

ξ∗

k+1 ←

⇥ ξk λi⇤T = ⇥ λt

x

λt

y

λs

x

λs

y

λj

x

λj

y

. . . . . . λi

x

λi

y

⇤T

• Without losing generality, let’s assume that the landmarks are ordered incrementally in

the state vector → landmark i is the i-th discovered landmark: ξ∗

k+1 ←

⇥ ξk λi⇤T = ⇥ λ1

x

λ1

y

λ2

x

λ2

y

λ3

x

λ3

y

. . . . . . λi

x

λi

y

⇤T

• The new expanded covariance matrix, of dimension 2i × 2i:

P ∗

k+1|k =

               σλ1

xλ1 x

σλ1

xλ1 y

σλ1

xλ2 x

σλ1

xλ2 y

. . . σλ1

xλi x

σλ1

xλi y

σλ1

y λ1 x

σλ1

y λ1 y

σλ1

y λ2 x

σλ1

y λ2 y

. . . σλ1

y λi x

σλ1

y λi y

σλ2

xλ1 x

σλ2

xλ1 y

σλ2

xλ2 x

σλ2

xλ2 y

. . . σλ2

xλi x

σλ2

xλi y

σλ2

y λ1 x

σλ2

y λ1 y

σλ2

y λ2 x

σλ2

y λ2 y

. . . σλ2

y λi x

σλ2

y λi y

. . . . . . . . . . . . . . . . . . . . . σλi

xλ1 x

σλi

xλ1 y

σλi

xλ2 x

σλi

xλ2 y

. . . σλi

xλi x

σλi

xλi y

σλi

y λ1 x

σλi

y λ1 y

σλi

y λ2 x

σλi

y λ2 y

. . . σλi

y λi x

σλi

y λi y

               =         Σλ1λ1 Σλ1λ2 . . . Σλ1λi Σλ2λ1 Σλ2λ2 . . . Σλ2λi . . . . . . . . . . . . Σλiλ1 Σλiλ2 . . . Σλiλi        

SLIDE 5

5

T H E E X PA N D E D C O VA R I A N C E M AT R I X

SLIDE 6

6

P R E D I C T I O N U P D AT E F O L L O W I N G A S TAT E E X PA N S I O N

• The process dynamics defined through q() is not linear → the Jacobian of q()

needs to be computed to define the EKF process updating equations based on the expanded state and covariance

SLIDE 7

7

J A C O B I A N M AT R I X

Qk(λk, zk+1, w ρ

k , w β k ; xk, yk, θk) =

h rqλ1

kx

rqλ1

ky

. . . rqλn

kx

rqλn

ky

rgkx rgky iT

SLIDE 8

8

J A C O B I A N S F O R L I N E A R I Z AT I O N O F q k( )

Said n = (i − 1), the number of landmarks before the current new observation, the dimension of the covariance matrix before the expansion is 2n × 2n, while the dimension of the Jacobian Q is (2n + 2) × (2n + 3 + 2) Qkξ is evaluated at the current observation zk+1, therefore is also referred to as Qzk+1 The Jacobian Qkξ of qk() for the linearization of the state dynamics in the case of state expansion can be also expressed in a more compact form: Qkξ = " I2n×2n 0 02n×2 Gξ Gz #

ˆ ξk|k ,zk+1,xk ,yk ,θk ,w=0

Gξ = ∂gk ∂ξk = " 0 . . . 0 0 . . . 0 # Gz = ∂gk ∂zk = " cos(θk + βk) −ρk sin(θk + βk) sin(θk + βk) ρk cos(θk + βk) # The Jacobian Qkξ allows to linearly transform (expand) the covariance matrix Pk when adding a new landmark to the state vector: P ∗

k|k+1 = Qkξ

" Pk|k 02n×2 02×2n Wk # QT

kξ =

2 4 Pk|k 02n×2 02×2n GzWkGT

z

3 5 which results in an expanded 2(n + 1) × 2(n + 1) matrix

SLIDE 9

9

E K F I N C L U D I N G S TAT E E X PA N S I O N

The complete EKF equations: When a new landmark i is observed: ˆ ⇠∗

k+1|k = qk(ˆ

k|k, zk+1, xk, yk, θk) P ∗

k+1|k =

2 4 Pk|k GzWkGT

z

3 5 At every time step k + 1 a landmark i is observed ˆ ⇠k+1 = ˆ ⇠k+1|k + Gk+1(zk+1 − `k(ˆ k+1|k, 0 0; xk, yk, θk)) Pk+1 = Pk+1|k − Gk+1Lk⇠Pk+1|k Gk+1 = Pk+1|kLk⇠T S−1

k+1

Sk+1 = Lk⇠Pk+1|kLk⇠T + LkwWk+1Lkw T The Jacobians Lk⇠ and Lkw of `k(), computed before, accounts for the linearization of the

• bservation model when an already discovered landmark is observed (xk and yk are

parameters): Lk⇠ = 2 6 6 6 4 0 0 . . . λi

kx − xk

r i

k

λi

ky − yk

r i

k

. . . 0 0 0 0 . . . − λi

ky − yk

(r i

k)2

λi

kx − xk

(r i

k)2

. . . 0 0 3 7 7 7 5

ˆ ⇠k+1|k ,0

Lkw = " 1 0 0 1 #

SLIDE 10

SOLVING LANDMARK MAPPING AND ROBOT LOCALIZATION TOGETHER

The previous derivation was based on the fact that robot’s localization was perfect Since this is not (usually) the case, let’s solve the joint problem: Simultaneous Localization and Mapping (SLAM)

10

S O LV I N G L O C A L I Z AT I O N A N D M A P P I N G T O G E T H E R

The state vector needs to include also robot’s coordinates, since also robot’s pose needs to be estimated ξ = ⇥ x, y, θ, λ1

x λ1 y λ2 x λ2 y . . . λM x

λM

y

⇤T ξ has now dimension (2M + 3) × 1, and P is a (2M + 3) × (2M + 3) matrix

SLIDE 11

THE PROBLEM IN A NUTSHELL

Looking for absolute robot pose Looking for absolute landmark positions But only relative measurements of landmarks are available

11

T H E S L A M C H A L L E N G E I N A N U T S H E L L

SLIDE 12

12

W H Y S L A M I S A H A R D P R O B L E M

• Robot path and the landmark

map are both unknown

• Errors in map and pose estimates

are correlated

• Mapping between observations

and landmarks is unknown

• Data Association: Selecting

wrong data association can be disastrous (divergence)

SLIDE 13

13

S TAT E V E C T O R A N D C O VA R I A N C E M AT R I X

The state vector and the covariance matrix: the state vector includes both the robot’s generalized coordinates and landmarks’ coordinates; said n = (i − 1) the number of discovered landmarks at step k: ξk = ⇥ xk yk θk λ1

k λ2 k . . . λn k

⇤T = h ξRk ξλn

k

iT ⇥ ⇤ and the resulting covariance matrix (omitting the index k) is: P(3+2n)×(3+2n) = 2 6 6 6 6 6 6 6 6 6 6 6 6 6 6 6 6 6 4 σxx σxy σxθ σxλ1

x

σxλ1

y

. . . σxλn

x

σxλn

y

σyx σyy σyθ σyλ1

x

σyλ1

y

. . . σyλn

x

σyλn

y

σθx σθy σθθ σθλ1

x

σθλ1

y

. . . σθλn

x

σθλn

y

σλ1

x x σλ1 x y σλ1 x θ σλ1 x λ1 x σλ1 x λ1 y . . . σλ1 x λn x σλ1 x λn y

σλ1

y x σλ1 y y σλ1 y θ σλ1 y λ1 x σλ1 y λ1 y . . . σλ1 y λn x σλ1 y λn y

. . . . . . . . . . . . . . . . . . . . . . . . σλn

x x σλn x y σλn x θ σλn x λn x σλn x λn y . . . σλn x λn x σλn x λn y

σλn

y x σλn y y σλn y θ σλn y λn x σλn y λn y . . . σλn y λn x σλn y λn y

3 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 5 = " ΣRR ΣRλ ΣT

Rλ Σλλ

#

SLIDE 14

Update predictions for robot pose and landmark’s position

14

T H E S L A M M O V I E

Robot’s pose prediction using the

• dometry model

Observe the environment while moving:

1 Get a landmark observation 2 Perform a measurement prediction

using the map (being) created Compare predicted and observed measure to perform data association (is a new landmark?) and compute innovation

SLIDE 15

4 State expansion after observing a previously unseen landmark: EKF based on the linearization

• f the process dynamics using the Jacobian Qkz to reduce the process equation to a linear

form of the type ξk+1 = Akξk + νk → EKF where non linearity refers to system evolution

15

E K F E Q U AT I O N S F O R S L A M ?

Combination of all equations from previous cases!

2 Robot’s pose prediction correction after the sensory observation of a landmark, whose

position is reported on a map: EKF based on the linearization of the observation equations using the Jacobians Hkξ and Hkw to reduce the observation equation to a linear form of the type zk+1 = Ckξk + wk → EKF where non linearity refers to measurement process

1 Robot’s pose prediction using the odometry model: EKF based on the linearization of the

motion equations using the Jacobians Fkξ and Fkv computed previously, and the new (constant) Jacobian Fkλ, to reduce the motion equation to a linear form of the type ξk+1 = Akξk + νk → EKF where non linearity refers to process dynamics

3 Landmark’s position estimation using robot’s sensory data and known pose (map building):

EKF based on the linearization of the observation equations using the Jacobians Lkξ and Lkw to reduce the observation equation to a linear form of the type zk+1 = Ckξk + wk → EKF where non linearity refers to measurement process

SLIDE 16

4 5 Observation model, which is unchanged from the previous scenario, but now λi

kx, λi ky, xk, yk, θk are variables, such that zk+1 can be seen as the overlapping of the

functions hk() and `k() that have been considered for the localization-only and mapping-only cases before: zk+1 = " ρi

k+1

βi

k+1

# = 2 4 q (λi

kx − xk)2 + (λi ky − yk)2

arctan ⇣ (λi

ky − yk)/(λi kx − xk)

⌘ − θk 3 5 + "w ρ

k

w β

k

# = ok(⇠k, wk)

16

S TAT E A N D O B S E R VAT I O N E Q U AT I O N S F O R S L A M

State equation for robot motion, including also the in the state vector: At each step k: ⇠k+1 = 2 6 6 6 6 6 6 6 6 6 6 6 6 4 xk yk θk 1

k

· · · n

k

3 7 7 7 7 7 7 7 7 7 7 7 7 5 + 2 6 6 6 6 6 6 6 6 6 6 6 4 (∆Sk + νs

k) cos(θk + ∆θk 2

+ νθ

k)

(∆Sk + νs

k) sin(θk + ∆θk 2

+ νθ

k)

∆θk + νθ

k

· · · 3 7 7 7 7 7 7 7 7 7 7 7 5 = fk(⇠k, ⌫k; ∆Sk, ∆θk) 4 5 4 5 State equation for state expansion, when a previously unseen landmark i is observed: ⇠∗

k+1 =

" ⇠k gk(xk, yk, θk, zk) # = 2 6 6 4 ⇠k xk + ρk cos(θk + βk) yk + ρk sin(θk + βk) 3 7 7 5 = qk(⇠k, zk+1)

SLIDE 17

17

J A C O B I A N S

The Jacobians Fkξ, and Fkν of fk(), to be evaluated in (ξk = ˆ ξk|k, νk = 0), for the linearization of the motion dynamics, which is robot’s motion since landmarks do not move (Jacobians are (re)computed because of the new sub-matrix Fkξλn in the Fkξ Jacobian):

fkx = xk + (∆Sk + νs

k) cos(θk + ∆θk 2

+ νθ

k)

fky = yk + (∆Sk + νs

k) sin(θk + ∆θk 2

+ νθ

k)

fkθ = θk + ∆θk + νθ

k

fkλ1 = λ1

k

· · · · · · fkλn = λn

k

⇒

2 6 6 6 6 6 6 6 6 6 6 6 6 6 6 6 6 6 6 6 6 6 6 6 6 6 4 ∂fkx ∂xk ∂fkx ∂yk ∂fkx ∂θk ∂fkx ∂λ1

k

· · · fkx ∂λn

k

∂fkx ∂νs

k

∂fkx ∂νθ

k

∂fky ∂xk ∂fky ∂yk ∂fky ∂θk ∂fky ∂λ1

k

· · · fky ∂λn

k

∂fky ∂νs

k

∂fky ∂νθ

k

∂fkθ ∂xk ∂fkθ ∂yk ∂fkθ ∂θk ∂fkθ ∂λ1

k

· · · fkθ ∂λn

k

∂fkθ ∂νs

k

∂fkθ ∂νθ

k

∂fkλ1 ∂xk ∂fkλ1 ∂yk ∂fkλ1 ∂θk ∂fkλ1 ∂λ1

k

· · · ∂fkλ1 ∂λn

k

∂fkλ1 ∂νs

k

∂fkλ1 ∂νθ

k

· · · · · · · · · · · · · · · · · · · · · · · · ∂fkλn ∂xk ∂fkλn ∂yk ∂fkλn ∂θk ∂fkλn ∂λ1

k

· · · ∂fkλn ∂λn

k

∂fkλn ∂νs

k

∂fkλn ∂νθ

k

3 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 5 =

⇥FkξR Fkξλn Fkν ⇤

SLIDE 18

18

J A C O B I A N S F O R R O B O T P O S E

2 6 6 6 6 6 6 6 6 6 6 6 6 6 6 6 6 6 6 6 6 6 6 6 6 6 4 ∂fkx ∂xk ∂fkx ∂yk ∂fkx ∂θk ∂fkx ∂λ1

k

· · · fkx ∂λn

k

∂fkx ∂νs

k

∂fkx ∂νθ

k

∂fky ∂xk ∂fky ∂yk ∂fky ∂θk ∂fky ∂λ1

k

· · · fky ∂λn

k

∂fky ∂νs

k

∂fky ∂νθ

k

∂fkθ ∂xk ∂fkθ ∂yk ∂fkθ ∂θk ∂fkθ ∂λ1

k

· · · fkθ ∂λn

k

∂fkθ ∂νs

k

∂fkθ ∂νθ

k

∂fkλ1 ∂xk ∂fkλ1 ∂yk ∂fkλ1 ∂θk ∂fkλ1 ∂λ1

k

· · · ∂fkλ1 ∂λn

k

∂fkλ1 ∂νs

k

∂fkλ1 ∂νθ

k

· · · · · · · · · · · · · · · · · · · · · · · · ∂fkλn ∂xk ∂fkλn ∂yk ∂fkλn ∂θk ∂fkλn ∂λ1

k

· · · ∂fkλn ∂λn

k

∂fkλn ∂νs

k

∂fkλn ∂νθ

k

3 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 5 =

⇥FkξR Fkξλn Fkν ⇤

4 5 Fkξ = 2 6 6 6 6 6 6 6 4 1 0 −∆Sk sin(θk + ∆θk

2 )

0 1 ∆Sk cos(θk + ∆θk

2 )

03×2n 0 0 1 02n×3 I2n×2n 3 7 7 7 7 7 7 7 5

ˆ ξk|k ,ν=0

Fkν = 2 6 6 6 6 6 4 cos(θk + ∆θk

2 ) −∆Sk sin(θk + ∆θk 2 )

sin(θk + ∆θk

2 )

∆Sk cos(θk + ∆θk

2 )

1 02n×2 3 7 7 7 7 7 5

ˆ ξk|k ,ν=0

Fkξ

Fkν

SLIDE 19

19

J A C O B I A N S F O R T H E O B S E R VAT I O N M O D E L S ( L O C A L I Z AT I O N A N D M A P P I N G )

Ok(xk, yk, θk, λk, w ρ

k , w β k ) =

⇥ rhkρ rhkβ ⇤T

• kρ =

q (λi

kx − xk)2 + (λi ky − yk)2 + w ρ k

• kβ =

arctan ⇣ (λi

yx − yk)/(λi kx − xk)

⌘ − θk + w β

k

The Jacobians Okξ and Okw of ok() for the linearization of the observation model for both localization and mapping, to be evaluated in (ξk = ˆ ξk+1|k, wk = 0). The Jacobians are derived from the combination of the Jacobians Hkξ and Lkξ computed for the localization-only and mapping-only cases: 2 3

Ok = 2 6 6 6 6 4 ∂okρ ∂xk ∂okρ ∂yk ∂okρ ∂θk ∂okρ ∂λ1

kx

∂okρ ∂λ2

ky

. . . ∂okρ ∂λn

kx

∂okρ ∂λn

ky

∂okρ ∂w ρ

k

∂okρ ∂w β

k

∂okβ ∂xk ∂okβ ∂yk ∂okβ ∂θk ∂okβ ∂λ1

kx

∂okβ ∂λ2

ky

. . . ∂okβ ∂λn

kx

∂okβ ∂λn

ky

∂okβ ∂w ρ

k

∂okβ ∂w β

k

3 7 7 7 7 5 = ⇥ OkξR Okξλn Okw ⇤

localization-only and mapping-only cases: Okξ = 2 6 6 6 4 λi

kx xk

r i

k

• λi

ky yk

r i

k

0 0 . . . λi

kx xk

r i

k

λi

ky yk

r i

k

. . . 0 0 λi

ky yk

(r i

k)2

λi

kx xk

(r i

k)2

1 0 0 . . . λi

ky yk

(r i

k)2

λi

kx xk

(r i

k)2

. . . 0 0 3 7 7 7 5 Okw = " 1 0 0 1 # r i

k =

q (λi

kx xk)2 + (λi ky yk)2,

Okξ is sparse: Okξ = h HkξR 0 0 · · · 0 0 Lkξλi 0 0 · · · 0 iT

Okξ

SLIDE 20

20

J A C O B I A N S F O R L A N D M A R K S TAT E E X PA N S I O N

q h i The Jacobian Qkξ of qk() for the linearization of the state dynamics when a new landmark is

• bserved (state expansion with landmark initialization); Qkξ is obtained as in the

mapping-only case + the fact that now Gξ = ⇥ GξR Gξλ ⇤ is non-zero since ξR is part of the state vector, making GξR 6= 0 0: Qkξ = " I(3+2n)⇥(3+2n) 0 0(3+2n)⇥2 GξR 02⇥2n Gz #

ˆ ξk|k ,zk ,w=0

GξR = ∂gk ∂ξR = " 1 0 ρk sin(θk + βk) 0 1 ρk cos(θk + βk) # Gz = ∂gk ∂z = " cos(θk + βk) ρk sin(θk + βk) sin(θk + βk) ρk cos(θk + βk) #

qk(ξk|k, zk+1, xk, yk, θk) = " ξk|k g(xk, yk, θk, zk+1) # = 2 6 6 4 ξk|k xk + ρk cos(θk + βk) yk + ρk sin(θk + βk) 3 7 7 5 = 2 6 6 6 6 6 6 6 6 6 6 6 6 6 6 6 6 6 6 6 4 λ1

kx

λ1

ky

λ2

kx

λ2

ky

· · · λi−1

kx

λi−1

ky

gk

x

gk

y

3 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 5

SLIDE 21

EKF FOR SIMULTANEOUS LOCALIZATION AND MAPPING

The SLAM EKF equations: At every time step k: ˆ ξk+1|k = fk(ˆ ξk|k, 0 0; ∆Sk, ∆θk) Pk+1|k = FkξPkFkξT + FkνVkFkνT When a new landmark i is observed: ˆ ξ∗

k+1|k = qk(ˆ

ξk|k, zk+1) P ∗

k+1|k = Qkξ

" Pk|k Wk # QkξT At every time step k + 1 a landmark i is observed ˆ ξk+1 = ˆ ξk+1|k + Gk+1(zk+1 − ok(ˆ ξk+1|k, 0 0)) Pk+1 = Pk+1|k − Gk+1OkξPk+1|k Gk+1 = Pk+1|kOkξT S−1

k+1

Sk+1 = OkξPk+1|kOkξT + OkwWk+1Okw T The Kalman gain matrix G multiplies innovation from the landmark observation, a 2-vector, so as to update every element of the state vector: the pose of the vehicle and the position of every map feature.

21

E K F E Q U AT I O N S F O R S L A M

SLIDE 22

SLAM PERFORMANCE

22

S L A M P E R F O R M A N C E I N S I M U L AT I O N

SLIDE 23

SLAM PERFORMANCE: COVARIANCE VS. TIME

23

C O VA R I A N C E V S . T I M E