Monte Carlo Hidden Markov Models - People | MIT CSAILpeople.csail.mit.edu/rplatt/thrun_tracking_lecture.pdfStanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors

Stanford CS223B Computer Vision, Winter 2007

Lecture 12 Tracking Motion

Professors Sebastian Thrun and Jana Košecká

CAs: Vaibhav Vaish and David Stavens

Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007

Overview

The Tracking Problem Bayes Filters Particle Filters Kalman Filters Using Kalman Filters


The Tracking Problem

Can we estimate the position of the object? Can we estimate its velocity? Can we predict future positions?

Image 4Image 1 Image 2 Image 3


The Tracking Problem

Given Sequence of Images Find center of moving object Camera might be moving or stationary

We assume: We can find object in individual images.

The Problem: Track across multiple images.

Is a fundamental problem in computer vision


Methods

Bayes Filter

Particle Filter

Uncented Kalman Filter

Kalman Filter

Extended Kalman Filter


Further Reading…


Example: Moving Object


Kalman Filter Tracking


Particle Filter Tracking


Mixture of KF / PF (Unscented PF)


Overview



SlowDown!

Example of Bayesian Inference

?

Environment priorp(staircase) = 0.1

Bayesian inferencep(staircase | image) p(image | staircasse) p(staircase) p(im | stair) p(stair) + p(im | no stair) p(no stair) = 0.7 • 0.1 / (0.7 • 0.1 + 0.2 • 0.9) = 0.28

Sensor modelp(image | staircase) = 0.7p(image | no staircase) = 0.2p(staircase)

= 0.28

Cost modelcost(fast walk | staircase) = $1,000cost(fast walk | no staircase) = $0cost(slow+sense) = $1

Decision TheoryE[cost(fast walk)] = $1,000 • 0.28 = $280E[cost(slow+sense)] = $1

=


Bayes Filter Definition

Environment state xt

Measurement zt

Can we calculate p(xt | z1, z2, …, zt) ?


Bayes Filters Illustrated


Bayes Filters: Essential Steps

Belief: Bel(xt)

Measurement update: Bel(xt) ← Bel(xt) p(zt|xt)

Time update: Bel(xt+1) ← Bel(xt) ⊗ p(xt+1|ut,xt)

∝


x = statet = timez = observationu = actionη = constant

Bayes Filters

),|()( 00 tttt uzxpxBel =

),,,|(),,,,|( 011011 zzuxpzzuxzp ttttttt −−−−= η

),,,|()|( 011 zzuxpxzp ttttt −−= η

1011011 ),,|(),,,|()|( −−−−−∫= tttttttt dxzuxpzuxxpxzp η

1111 )(),|()|( −−−−∫= ttttttt dxxBeluxxpxzpη

),,,,|( 011 uzuzxp tttt −−=

Markov

Bayes

Markov

1021111 ),,|(),|()|( −−−−−−∫= ttttttttt dxuuzxpuxxpxzp η


Bayes Filtersx = statet = timez = observationu = action

1111 )(),|()|()( −−−−∫= tttttttt dxxBeluxxpxzpxBel η


Bayes Filters Illustrated


Bayes Filters

Initial Estimate of State Iterate

– Receive measurement, update your belief (uncertainty shrinks)

– Predict, update your belief (uncertainty grows)


Methods

Bayes Filter

Particle Filter

Uncented Kalman Filter

Kalman Filter

Extended Kalman Filter


Overview



Particle Filters: Basic Idea

)(xp

tx

)|()( ...1 tttt zxpXxp ≈∈ (equality for )∞↑n

set of n particles Xt


Particle Filter Explained


11...11...111...1...1 ),|(),|()|(),|( −−−−−∫= tttttttttttt dxuzxpxuxpxzpuzxp η

Basic Particle Filter Algorithm

),|()( ...1...1 ttttt uzxpXxp ≈∈

Initialization:

X0 ← n particles x0 [i] ~ p(x0)

particleFilters(Xt −1 ){ for i=1 to n xt

[i] ~ p(xt | xt −1

[i]) (prediction) wt

[i] = p(zt | xt

[i]) (importance weights)

endfor for i=1 to n include xt

[i] in Xt with probability ∝ wt[i] (resampling)

}


Weight samples: w = f / g

Importance Sampling


Particle Filter

By Frank Dellaert


Case Study: Track moving objects from Helicopter

1. Harris Corners

2. Optical Flow (with clustering)

3. Motion likelihood function

4. Particle Filter

5. Centroid Extraction

David Stavens, Andrew Lookingbill, David Lieb, CS223b Winter 2004


1. Harris Corner Extraction



2. Optical Flow + Motion Detection



3. Motion Likelihood Function



4. Particle Filters



5. Extract Centroid



More Particle Filter Tracking



Some Robotics Examples

Tracking Hands, People Mobile Robot localization People localization Car localization Mapping


Examples Particle Filter

Siu Chi Chan McGill University


Another Example

Mike Isard and Andrew Blake


Tracking Fast moving Objects

K. Toyama, A.Blake


Particle Filters: IllustrationWith: Wolfram Burgard, Dieter Fox, Frank Dellaert

),|()( ...1...1 ttttt uzxpXxp ≈∈


Particle Filters (1)


Particle Filters (2)


Particles = Robustness


Tracking People from Moving Platform

• robot location (particles)• people location (particles)• laser measurements (wall) With Michael Montemerlo


Tracking People from Moving Platform

With Michael Montemerlo

• robot location (particles)• people location (particles)• laser measurements (wall)


Particle Filters for Tracking Cars

With Anya Petrovskaya


Mapping Environments (SFM)

With Dirk Haehnel


Overview



Tracking with KFs: Gaussians!

updateinitial estimte

x

y

x

y

prediction

x

y

measurement

x

y


Kalman Filters

),(~)( ΣµNxp

−Σ−−∝ − )()(

2

1exp)( 1 µµ xxxp T


Kalman Filters

prior

Measurementevidence

posterior

)()|()|( xpxzpzxp ∝

dxxpxxpxp )()|'()'( ∫=


A Quiz

prior

Measurementevidence

)()|()|( xpxzpzxp ∝

posterior?


Gaussians

2

2)(

2

1

2

2

1)(

:),(~)(

σµ

σπ

σµ

−−=

x

exp

Nxp

-σ σ

µUnivariate

)()(2

1

2/12/

1

)2(

1)(

:)(~)(

μxΣμx

Σx

Σμx

−−− −

=t

ep

,Νp

dπ

µ

Multivariate


),(~),(~ 22

2

σµσµabaNY

baXY

NX+⇒

+=

Properties of Univariate Gaussians

++

++

⋅⇒

−− 22

21

222

21

21

122

21

22

212222

2111 1

,~)()(),(~

),(~

σσµ

σσσµ

σσσ

σµσµ

NXpXpNX

NX


Measurement Update Derived( ) ( )

−−−−⋅=⋅⇒

22

22

21

21

212222

2111

2

1

2

1expconst.)()(

),(~

),(~

σµ

σµ

σµσµ xx

XpXpNX

NX

( ) ( )) new(for 0

2

1

2

12

2

22

1

12

2

22

21

21 µ

σµ

σµ

σµ

σµ =−+−=

−−−−

∂∂ xxxx

x

( ) ( ) 0212

221 =−+− σµµσµµ

212

221

22

21 )( σµσµσσµ +=+

22

21

212

221

σσσµσµµ

++=

( ) ( ) 22

212

2

22

21

21

2

2

2

1

2

1 −− +=

−−−−

∂∂ σσ

σµ

σµ xx

x

22

21

1−− +

=σσ

σ


We stay in the “Gaussian world” as long as we start with Gaussians and perform only linear transformations.

),(~),(~ TAABANY

BAXY

NXΣ+⇒

+=Σ

µµ

Properties Multivariate GaussiansEssentially the same as in the 1-D case, but with more general notation

( )112

1121

12112

12121

222

111 )(,)()(~)()(),(~

),(~ −−−−− Σ+ΣΣΣ+Σ+ΣΣ+Σ⋅⇒

ΣΣ

µµµµ

NXpXpNX

NX


Linear Kalman Filter

tttttt uBxAx ε++= −1

tttt xCz δ+=

Estimates the state x of a discrete-time controlled process that is governed by the linear stochastic difference equation

with a measurement


Components of a Kalman Filter

tε

Matrix (n × n) that describes how the state evolves from t to t+1 without controls or noise.

tA

Matrix (n × i) that describes how the control ut changes the state from t to t+1.tB

Matrix (k × n) that describes how to map the state xt to an observation zt.tC

tδ

Random variables representing the process and measurement noise that are assumed to be independent and normally distributed with covariance Rt and Qt respectively.


Kalman Filter Algorithm

1. Algorithm Kalman_filter( µt-1, Σt-1, ut, zt):

2. Prediction:3. 4.

5. Correction:6. 7. 8.

9. Return µt, Σt

ttttt uBA += −1µµ

tTtttt RAA +Σ=Σ −1

tttt CKI Σ−=Σ )(

1)( −+ΣΣ= tTttt

Tttt QCCCK

)( tttttt CzK µµµ −+=


Kalman Filter Updates in 1D

old belief

measurement

belief

new belief

belief

mea

sure

men

t



1)(with )(

)()( −+ΣΣ=

Σ−=Σ−+=

= tTttt

Tttt

tttt

ttttttt QCCCK

CKI

CzKxbel

µµµ

2,

2

2

22 with )1(

)()(

tobst

tt

ttt

tttttt K

K

zKxbel

σσσ

σσµµµ

+=

−=−+=

=

old belief

new belief

mea

sure

men

t



+Σ=Σ+=

=−

−

tTtttt

tttttt RAA

uBAxbel

1

1)(µµ

+=+=

= −2

,2221)(

tactttt

tttttt a

ubaxbel

σσσµµ

old belief

new belief

mea

sure

men

t

new belief

newest belief


Kalman Filter Updates

belief

latest belief

belief

measurement

belief


The Prediction-Correction-Cycle

+Σ=Σ+=

=−

−

tTtttt

tttttt RAA

uBAxbel

1

1)(µµ

+=+=

= −2

,2221)(

tactttt

tttttt a

ubaxbel σσσ

µµ

Prediction



1)(,)(

)()( −+ΣΣ=

Σ−=Σ−+=

= tTttt

Tttt

tttt

ttttttt QCCCK

CKI

CzKxbel

µµµ

2,

2

2

22 ,)1(

)()(

tobst

tt

ttt

tttttt K

K

zKxbel

σσσ

σσµµµ

+=

−=−+=

=

Correction



1)(,)(

)()( −+ΣΣ=

Σ−=Σ−+=

= tTttt

Tttt

tttt

ttttttt QCCCK

CKI

CzKxbel

µµµ

2,

2

2

22 ,)1(

)()(

tobst

tt

ttt

tttttt K

K

zKxbel

σσσ

σσµµµ

+=

−=−+=

=

+Σ=Σ+=

=−

−

tTtttt

tttttt RAA

uBAxbel

1

1)(µµ

+=+=

= −2

,2221)(

tactttt

tttttt a

ubaxbel σσσ

µµ

Correction

Prediction


Kalman Filter Summary

Highly efficient: Polynomial in measurement dimensionality k and state dimensionality n: O(k2.376 + n2)

Optimal for linear Gaussian systems!

Most robotics systems are nonlinear!


Overview



Let’s Apply KFs to Tracking Problem

Image 4Image 1 Image 2 Image 3


Kalman Filter with 2-Dim Linear Model

Linear Change (Motion)

Linear Measurement Model

),0( QNDCxz ++=What is A, B, Q?

=

2

2

0

0

σσ

Q

=

0

0D

=

10

01C

=

0

0B

=

10

01A

),0(' RNBAxx ++=What is C, D, R?

=

2

2

0

0

ρρ

R


Kalman Filter Algorithm

1. Algorithm Kalman_filter( µt-1, Σt-1, ut, zt):

2. Prediction:3. 4.

5. Correction:6. 7. 8.

9. Return µt, Σt

ttttt uBA += −1µµt

Ttttt RAA +Σ=Σ −1

1)( −+ΣΣ= tTttt

Tttt QCCCK

)( DCzK tttttt −−+= µµµtttt CKI Σ−=Σ )(

=

2

2

0

0

σσ

Q

=

0

0D

=

10

01C

=

0

0B

=

10

01A

=

2

2

0

0

ρρ

R


Kalman Filter in Detail

Measurements Change Prediction Measurement Update

),0( RNxz +=

),0(' QNxx +=

( ))'('''''

'''1

111

µµµ −Σ+=+Σ=Σ

−

−−−

zR

R

µµ =+Σ=Σ '' Q

updateinitial position

x

y

x

y

prediction

x

y

measurement

x

y


Can We Do Better?


Kalman, Better!

initial position prediction measurement

x

x

x

x

x

x

next

pre

dici

ton

x

x

update

x

x


We Can Estimate Velocity!

past measurements

prediction


Kalman Filter For 2D Tracking

Linear Measurement model (now with 4 state variables)

Linear Change

=

2

2

000

000

0000

0000

q

qQ

=

2

2

0

0

r

rR),0(

0010

0001RN

y

x

y

x

y

x

obs

obs +

=

),0(

1000

0100

010

001

'

'

'

'

QN

y

x

y

x

t

t

y

x

y

x

+

∆

∆

=


Putting It Together Again

Measurements

Change

Prediction

Measurement Update

),0(0010

0001RNxz +

=

),0(

1000

0100

010

001

' QNxt

t

x +

∆

∆

=

( ))(''

'1

111

µµµ AzRA

ARAT

T

−Σ+=+Σ=Σ

−

−−−

µµ C

CQC T

=+Σ=Σ

'

'


Summary Kalman Filter Estimates state of a system

– Position– Velocity– Many other continuous state variables possible

KF maintains– Mean vector for the state– Covariance matrix of state uncertainty

Implements– Time update = prediction– Measurement update

Standard Kalman filter is linear-Gaussian– Linear system dynamics, linear sensor model– Additive Gaussian noise (independent)– Nonlinear extensions: extended KF, unscented KF: linearize

More info: – CS226– Probabilistic Robotics (Thrun/Burgard/Fox, MIT Press)


Summary Kalman Filter Estimates state of a system

– Position– Velocity– Many other continuous state variables possible

KF maintains– Mean vector for the state– Covariance matrix of state uncertainty

Implements– Time update = prediction– Measurement update

Standard Kalman filter is linear-Gaussian– Linear system dynamics, linear sensor model– Additive Gaussian noise (independent)– Nonlinear extensions: extended KF, unscented KF: linearize

More info: – CS226– Probabilistic Robotics (Thrun/Burgard/Fox, MIT Press)

Particle

and discrete

set of particles(example states)

= predictive sampling= resampling, importance weights

fully nonlinear

easy to implement


Summary


Documents

Monte Carlo Hidden Markov Models - People | MIT CSAILpeople.csail.mit.edu/rplatt/thrun_tracking_lecture.pdfStanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors