gaussian (or normal) distribution - the new age of discoverysleonard/review.pdfgaussian white noise...

600.436/600.636G.D. Hager

S. Leonard

Gaussian (or Normal) Distribution

-s s

m

Univariate

Multivariate

𝑝 𝑥 ~𝑁 𝜇, 𝜎2

𝑝 𝑥 =1

2𝜋𝜎𝑒−12(𝑥−𝜇)2

𝜎2

𝑝 𝒙 ~𝑁 𝝁, Σ

𝑝 𝒙 =1

(2𝜋)𝑑/2 Σ 1/2𝑒−

12𝒙−𝝁 𝑇Σ−1(𝒙−𝝁)

600.436/600.636G.D. Hager

S. Leonard

Properties of Gaussians

• We stay in the “Gaussian world” as long as we start with

Gaussians and perform only linear transformations.

• Same holds for multivariate Gaussians

𝑋~𝑁 𝜇, 𝜎2

𝑌 = 𝑎𝑋 + 𝑏⟹ 𝑌~𝑁(𝑎𝜇 + 𝑏, 𝑎2𝜎2)

𝑋1~𝑁 𝜇1, 𝜎1

𝑋2~𝑁 𝜇2, 𝜎2⟹ 𝑋1 + 𝑋2~𝑁(𝜇1 + 𝜇2, 𝜎1 + 𝜎2 )

2

22 2

600.436/600.636G.D. Hager

S. Leonard

Covariance, covariance, covariance

• Know what’s a covariance

matrix

– What it does, what it means,

why it matters, how you can

compute one, how you can

visualize them

𝒙 =1

𝑁

𝑖=0

𝑁

𝒙𝑖

𝒆𝑖 = 𝒙𝑖 − 𝒙

Σ = E[𝒆𝑖𝒆𝑖𝑇]

600.436/600.636G.D. Hager

S. Leonard

Kalman Filter

• Recursive solution for discrete linear filtering problems

– A state x Rn

– A measurement z Rm

– Discrete (i.e. for time t = 1, 2, 3, … )

– Recursive process (i.e. xt = f ( xt-1 ) )

– Linear system (i.e xt = A xt-1 )

• The system is defined by:

1) linear process model

xt = A xt-1 + B ut-1 + wt-1

2) linear measurement model

zt = H xt + vt

control

Input

(optional)

Gaussian

white

noise

state

transitionGaussian

white

noise

observation

model

How a state transitions into another state How a state relates to a measurement

600.436/600.636G.D. Hager

S. Leonard

Kalman Filter

• Recipe:

1. Start with an initial guess

𝒙0, Σ02. Compute prior (prediction)

𝒙𝑡′ = 𝐴 𝒙𝑡−1 + 𝐵𝒖𝑡−1

Σ𝑡′ = 𝐴Σ𝑡−1𝐴

𝑇 + 𝑄

3. Compute posterior (correction)

𝐾𝑡 = Σ𝑡′𝐻𝑇 𝐻Σ𝑡

′𝐻𝑇 + 𝑅 −1

𝒙𝑡 = 𝒙𝑡′ + 𝐾𝑡 𝒛𝑡 − 𝐻 𝒙𝑡

′

Σ𝑡 = 1 − 𝐾𝑡𝐻 Σ𝑡′

Time update

(predict)

Measurement

update

(correct)RE

PE

AT

Minimizes the sum of the expected

errors: 𝑒𝑖2

600.436/600.636G.D. Hager

S. Leonard

Initial

𝒙0, Σ0

16-735, Howie Choset with slides

from G.D. Hager and Z. Dodds

Predict 𝒙1′ from 𝒙0 and 𝒖0

𝒙1′ = 𝐴 𝒙0 + 𝐵𝒖0

Σ1′ = 𝐴Σ0𝐴

𝑇 + 𝑄

Correction using 𝒛1𝐾1 = Σ1

′𝐻𝑇 𝐻Σ1′𝐻𝑇 + 𝑅 −1

𝒙1 = 𝒙1′ + 𝐾1 𝒛1 − 𝐻 𝒙1

′

Σ1 = 1 − 𝐾1𝐻 Σ1′

Predict 𝒙2′ from 𝒙1 and 𝒖1

𝒙2′ = 𝐴 𝒙1 + 𝐵𝒖1

Σ2′ = 𝐴Σ1𝐴

𝑇 + 𝑄

600.436/600.636G.D. Hager

S. Leonard

Kalman Filter Limitations

• Assumptions:

– Linear process model 𝒙𝑡+1 = 𝐴𝒙𝑡 + 𝐵𝒖𝑡 +𝒘𝑡

– Linear observation model 𝒛𝑡 = 𝐻𝒙𝑡 + 𝒗𝑡– White Gaussian noise 𝑁(0, Σ )

• What can we do if system is not linear?

– Non-linear state dynamics 𝒙𝑡+1 = 𝑓 𝒙𝑡 , 𝒖𝑡 , 𝒘𝑡

– Non-linear observations 𝒛𝑡 = ℎ 𝒙𝑡 , 𝒗𝑡

600.436/600.636G.D. Hager

S. Leonard

• Extended Kalman Filter Recipe:

– Given

– Prediction

– Measurement correction

Extended Kalman Filter

• Kalman Filter Recipe:

– Given

– Prediction

– Measurement correction

600.436/600.636G.D. Hager

S. Leonard

EKF for Range-Bearing Localization

• State 𝒔𝑡 = 𝑥𝑡 𝑦𝑡 𝜃𝑡𝑇 2D position and orientation

• Input 𝒖𝑡 = 𝑣𝑡 𝜔𝑡𝑇 linear and angular velocity

• Process model

𝒇 𝒔𝑡 , 𝒖𝑡 , 𝒘𝑡 =

𝑥𝑡−1 + (∆𝑡)𝑣𝑡−1cos(𝜃𝑡−1)𝑦𝑡−1 + (∆𝑡)𝑣𝑡−1sin(𝜃𝑡−1)

𝜃𝑡−1 + (∆𝑡)𝜔𝑡−1

+

𝑤𝑥𝑡𝑤𝑦

𝑤𝜃𝑡

• Given a map, the robot sees N landmarks with coordinates

𝒍1 = 𝑥𝑙1 𝑦𝑙1 𝑇 , ⋯ , 𝒍𝑁 = 𝑥𝑙𝑁 𝑦𝑙𝑁 𝑇

The observation model is

𝒛𝑡 =𝒉1(𝒔𝑡, 𝒗1)

⋮𝒉𝑁(𝒔𝑡, 𝒗𝑁)

𝒉𝑖 𝒔𝑡, 𝒗𝑡 =𝑥𝑡 − 𝑥𝑙𝑖

2+ 𝑦𝑡 − 𝑦𝑙𝑖

2

tan−1𝑦𝑡−𝑦𝑙𝑖𝑥𝑡−𝑥𝑙𝑖

− 𝜃𝑡

+𝑣𝑟𝑣𝑏

x

xtyt

y lN

l1

r1

b1

600.436/600.636G.D. Hager

S. Leonard

Kalman Filters and SLAM• Localization: state is the location of the robot

• Mapping: state is the location of 2D landmarks

• SLAM: state combines both

• If the state is

then we can write a linear observation system

– note that if we don’t have some fixed landmarks, our system is unobservable(we can’t fully determine all unknown quantities)

• Covariance S is represented by http://ais.informatik.uni-freiburg.de

600.436/600.636G.D. Hager

S. Leonard

• State 𝒔𝑡 = 𝑥𝑡 𝑦𝑡 𝜃𝑡 𝒍1𝑇 ⋯ 𝒍𝑁

𝑇 position/orientation of

robot and landmarks coordinates

• Input 𝒖𝑡 = 𝑣𝑡 𝜔𝑡𝑇 forward and angular velocity

• The process model for localization is

𝒔𝑡′ =

𝑥𝑡−1𝑦𝑡−1𝜃𝑡−1

+

∆𝑡𝑣𝑡−1cos(𝜃𝑡−1)∆𝑡𝑣𝑡−1sin(𝜃𝑡−1)

∆𝑡𝜔𝑡−1

This model is augmented for 2N+3 dimensions to

accommodate landmarks. This results in the process equation𝑥𝑡′

𝑦𝑡′

𝜃𝑡′

𝒍1𝑡′

⋮𝒍𝑁𝑡′

=

𝑥𝑡−1𝑦𝑡−1𝜃𝑡−1

𝒍1𝑡−1⋮

𝒍𝑁𝑡−1

+

1 0 00 1 00 0 10 0 0⋮ ⋮ ⋮0 0 0

∆𝑡𝑣𝑡−1cos(𝜃𝑡−1)∆𝑡𝑣𝑡−1sin(𝜃𝑡−1)

∆𝑡𝜔𝑡−1

EKF Range Bearing SLAM

Prior State Estimation

Landmarks don’t depend

on external input

600.436/600.636G.D. Hager

S. Leonard

EKF Range Bearing SLAM

Prior Covariance UpdateWe assume static landmarks. Therefore, the function

f(s,u,w) only affects the robot’s location and not the

landmarks.

Σ𝑡′ = 𝐴𝑡Σ𝑡−1𝐴𝑡

𝑇 +𝑊𝑡𝑄𝑊𝑡𝑇

Jacobian of the

robot motion

The motion of the robot does not

affect the coordinates of

the landmarks

Jacobian of the

process model

2Nx2N identity

𝐴 =

𝜕𝑥

𝜕𝑥

𝜕𝑥

𝜕𝑦

𝜕𝑥

𝜕𝜃𝜕𝑦

𝜕𝑥

𝜕𝑦

𝜕𝑦

𝜕𝑦

𝜕𝜃𝜕𝜃

𝜕𝑥

𝜕𝜃

𝜕𝑦

𝜕𝜃

𝜕𝜃

0

0 𝐼

600.436/600.636G.D. Hager

S. Leonard

Bayes Filter

• Given a sequence of measurements z1, …, zk

• Given a sequence of commands u0, …, uk-1

• Given a sensor model P(zk | xk)

• Given a dynamic model P(xk | xk-1)

• Given a prior probability P(x0)

• Find P(xk | z1:k, u0:k-1 )

x0xk-

2

xk-

1xk

zk-2 zk-1 zk

u0 uk-2 uk-1

600.436/600.636G.D. Hager

S. Leonard

Bayesian Localization

• Recall Bayes Theorem:

𝑃 𝑥 𝑧) =𝑃 𝑧 𝑥 𝑃(𝑥)

𝑃(𝑧)

• Also remember conditional independence

• Think of x as the state of the robot and z as the data we know

𝑃(𝒙𝑘|𝒖0:𝑘−1, 𝒛1:𝑘) Posterior Probability Distribution

𝑃 𝒙𝑘 𝒖0:𝑘−1, 𝒛1:𝑘 =𝑃 𝒛𝑘 𝒙𝑘 , 𝒖0:𝑘−1, 𝒛1:𝑘−1 𝑃(𝒙𝑘|𝒖0:𝑘−1, 𝒛1:𝑘−1)

𝑃(𝒛𝑘|𝒖0:𝑘−1, 𝒛1:𝑘−1)

= 𝜂𝑘𝑃(𝒛𝑘|𝒙𝑘)

𝒙𝑘−1

𝑃 𝒙𝑘 𝒖𝑘−1, 𝒙𝑘−1 𝑃 𝒙𝑘−1 𝒖0:𝑘−2, 𝒛1:𝑘−1 𝑑𝒙𝑘−1

observation recursionstate prediction

600.436/600.636G.D. Hager

S. Leonard

Predict Motion (Prior Distribution)

• Suppose we have 𝑃 𝒙𝑘• We have 𝑃(𝒙𝑘+1|𝒙𝑘 , 𝒖𝑘)

• Put together

𝑃 𝒙𝑘+1 = 𝒙𝑘

𝑃(𝒙𝑘+1|𝒙𝑘 , 𝒖𝑘)𝑃 𝒙𝑘 𝑑𝒙𝑘

What is the probability distribution for xk+1 given the command uk and all

the previous states xk?

600.436/600.636G.D. Hager

S. Leonard

System Model

State space X = { 1, 2, 3, 4}

P(xk+1 | xk,

uk)

Xk+1=

1

Xk+1=

2

Xk+1=

3

Xk+1=

4

Xk=1 0.25 0.5 0.25 0

Xk=2 0 0.25 0.5 0.25

Xk=3 0 0 0.25 0.75

Xk=4 0 0 0 1

xk

P(xk)

1 2 3 4

0.5

Prior probability

distribution 𝑃(𝑥𝑘)

Compute 𝑃 𝑥𝑘+1 = 𝑥𝑘𝑃(𝑥𝑘+1|𝑥𝑘 , 𝑢𝑘)𝑑𝑥𝑘

Transition matrix: The probability P(j|i) of

moving

from i to j is given by Pi,j. Each row must sum

to 1.

600.436/600.636G.D. Hager

S. Leonard

Observation Model

• How likely is the measurement zk given a state xk?

• The measurement zk is what we get. So we have to stick by it

• However, not all states will likely produce the measurement

• We’re not interested in finding the most likely state. We want the whole distribution

X

X

𝑃(𝑧𝑘|𝑥𝑘)

zk

xk

Doesn’t have to be Gaussian

600.436/600.636G.D. Hager

S. Leonard

Observation Model

Beam Model

• Mixing all these cases together we get

𝑃 𝑧𝑘 𝒙𝑘 , 𝑚 =

𝑤hit𝑤short𝑤max𝑤rand

𝑇 𝑃hit(𝑧𝑘|𝒙𝑘, 𝑚)

𝑃short(𝑧𝑘|𝒙𝑘 , 𝑚)

𝑃max(𝑧𝑘|𝒙𝑘, 𝑚)𝑃rand(𝑧𝑘|𝒙𝑘, 𝑚)

where the weights are parameters

𝑤hit + 𝑤short + 𝑤max + 𝑤rand = 1

Probabilistic Robotic

600.436/600.636G.D. Hager

S. Leonard

Likelihood Field Model

𝑃hit 𝑧𝒌 𝒙𝑘 , 𝑚 = 𝜀𝜎ℎ𝑖𝑡2 (𝑑2)

Probabilistic Robotic

600.436/600.636G.D. HagerS. Leonard

Discrete Bayes Filter Algorithm Algorithm Discrete_Bayes_filter( u0:k-1, y1:k, P(x0) )

1. P(x) = P(x0) (if you don’t know: uniform distribution)

2. for i=1:k

3. for all states x X

4.

5. end for

6. h=0 Normalization constant


8.

9. h = h + P(x)

10. end for


12. P(x) = P(x) / h

13. end for

14. end for

Xx

xPxuxPxP )(),|()(

)()|()( xPxzPxP

Prediction given prior dist. and command

Update using measurement

Normalize to 1

600.436/600.636G.D. Hager

S. Leonard

Particle Filter

• Computing 𝑃(𝒛𝑘|𝒙𝑘 , 𝑚) and 𝑃(𝒙𝑘+1|𝒙𝑘 , 𝒖𝑘) is not easy

• In practice, it is never directly computable

• Need to propagate an entire conditional distribution, not just

one state like we did with Kalman,

• Represent probability distribution by random samples

• Estimation of non-Gaussian, nonlinear processes

600.436/600.636G.D. Hager

S. Leonard

Particle Filter

600.436/600.636G.D. Hager

S. Leonard

Particle Filter Algorithm Algorithm Particle_filter( u0:k-1, y1:k, P(x0), set of N samples M = {xj, wj} )

1. for i=1:k

2. for j=1:N

3. compute a new state x by sampling according to P( x | ui-1, xj )

4. xj = x

5. end

6. h=0

7. for j=1:N

8. wj = P( zi | xj )

9. h += wj

10. end

11. for j=1:N

12. wj = wj / h

13. end

14. resample (M ) according to weights wj

15. end

gaussian (or normal) distribution - the new age of discoverysleonard/review.pdfgaussian white noise...

Documents