Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun...
-
date post
19-Dec-2015 -
Category
Documents
-
view
219 -
download
3
Transcript of Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun...
![Page 1: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/1.jpg)
Stanford CS223B Computer Vision, Winter 2007
Lecture 12 Tracking Motion
Professors Sebastian Thrun and Jana Košecká
CAs: Vaibhav Vaish and David Stavens
![Page 2: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/2.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Overview
The Tracking Problem Bayes Filters Particle Filters Kalman Filters Using Kalman Filters
![Page 3: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/3.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
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
![Page 4: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/4.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
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
![Page 5: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/5.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Methods
Bayes Filter
Particle Filter
Uncented Kalman Filter
Kalman Filter
Extended Kalman Filter
![Page 6: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/6.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Further Reading…
![Page 7: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/7.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Example: Moving Object
![Page 8: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/8.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Overview
The Tracking Problem Bayes Filters Particle Filters Kalman Filters Using Kalman Filters
![Page 9: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/9.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
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
=
![Page 10: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/10.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Bayes Filter Definition
Environment state xt
Measurement zt
Can we calculate p(xt | z1, z2, …, zt) ?
![Page 11: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/11.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Bayes Filters Illustrated
Localization problem
![Page 12: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/12.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
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)
![Page 13: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/13.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
x = statet = timez = observationu = action = constant
Bayes Filters
),|()( 00 tttt uzxpxBel KK=
),,,|(),,,,|( 011011 zzuxpzzuxzp ttttttt KK −−−−=
),,,|()|( 011 zzuxpxzp ttttt K−−=
1011011 ),,|(),,,|()|( −−−−−∫= tttttttt dxzuxpzuxxpxzp KKη
1111 )(),|()|( −−−−∫= ttttttt dxxBeluxxpxzp
),,,,|( 011 uzuzxp tttt K−−=
Markov
Bayes
Markov
1021111 ),,|(),|()|( −−−−−−∫= ttttttttt dxuuzxpuxxpxzp Kη
![Page 14: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/14.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Bayes Filtersx = statet = timez = observationu = action
1111 )(),|()|()( −−−−∫= tttttttt dxxBeluxxpxzpxBel
![Page 15: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/15.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Bayes Filters Illustrated
![Page 16: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/16.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Bayes Filters
Initial Estimate of State Iterate
– Receive measurement, update your belief (uncertainty shrinks)
– Predict, update your belief (uncertainty grows)
![Page 17: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/17.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Methods
Bayes Filter
Particle Filter
Uncented Kalman Filter
Kalman Filter
Extended Kalman Filter
![Page 18: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/18.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Overview
The Tracking Problem Bayes Filters Particle Filters Kalman Filters Using Kalman Filters
![Page 19: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/19.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Particle Filters: Basic Idea
)(xp
tx
)|()( ...1 tttt zxpXxp ≈∈ (equality for )∞↑n
set of n particles Xt
![Page 20: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/20.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Particle Filter Explained
![Page 21: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/21.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
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)
}
![Page 22: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/22.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Weight samples: w = f / g
Importance Sampling
![Page 23: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/23.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Particle Filter
By Frank Dellaert
![Page 24: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/24.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
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
![Page 25: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/25.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
More Particle Filter Tracking
David Stavens, Andrew Lookingbill, David Lieb, CS223b Winter 2004
![Page 26: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/26.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Some Robotics Examples
Tracking Hands, People Mobile Robot localization People localization Car localization Mapping
![Page 27: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/27.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Examples Particle Filter
Siu Chi Chan McGill University
![Page 28: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/28.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Another Example
Mike Isard and Andrew Blake
![Page 29: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/29.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Tracking Fast moving Objects
K. Toyama, A.Blake
![Page 30: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/30.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Particle Filters (1)
![Page 31: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/31.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Particle Filters (2)
![Page 32: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/32.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Particles = Robustness
![Page 33: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/33.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Particle Filters for Tracking Cars
With Anya Petrovskaya
![Page 34: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/34.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Overview
The Tracking Problem Bayes Filters Particle Filters Kalman Filters Using Kalman Filters
![Page 35: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/35.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Tracking with KFs: Gaussians!
updateinitial estimate
x
y
x
y
prediction
x
y
measurement
x
y
![Page 36: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/36.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Kalman Filters
),(~)( ΣμNxp
⎭⎬⎫
⎩⎨⎧ −Σ−−∝ − )()(
2
1exp)( 1 μμ xxxp T
![Page 37: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/37.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Kalman Filters
prior
Measurementevidence
posterior
)()|()|( xpxzpzxp ∝
dxxpxxpxp )()|'()'( ∫=
![Page 38: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/38.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
A Quiz
prior
Measurementevidence
)()|()|( xpxzpzxp ∝
posterior?
![Page 39: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/39.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
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
![Page 40: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/40.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
),(~),(~ 22
2
σμσμ
abaNYbaXY
NX+⇒
⎭⎬⎫
+=
Properties of Univariate Gaussians
⎟⎟⎠
⎞⎜⎜⎝
⎛
+++
+⋅⇒
⎭⎬⎫
−− 22
21
222
21
21
122
21
22
212222
2111 1
,~)()(),(~
),(~
σσμ
σσ
σμ
σσ
σ
σμ
σμNXpXp
NX
NX
![Page 41: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/41.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Measurement Update Derived( ) ( )
⎭⎬⎫
⎩⎨⎧ −
−−
−⋅=⋅⇒⎭⎬⎫
22
22
21
21
212222
2111
2
1
2
1expconst.)()(
),(~
),(~
σ
μ
σ
μ
σμ
σμ xxXpXp
NX
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−− +
=σσ
σ
![Page 42: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/42.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
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 )(,)()(~)()(),(~
),(~ −−−−− Σ+ΣΣΣ+Σ+ΣΣ+Σ⋅⇒⎭⎬⎫
Σ
Σμμ
μ
μNXpXp
NX
NX
![Page 43: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/43.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
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
![Page 44: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/44.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
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.
![Page 45: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/45.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
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 μμμ −+=
![Page 46: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/46.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Kalman Filter Updates in 1D
old belief
measurement
belief
new belief
belief
mea
sure
men
t
![Page 47: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/47.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Kalman Filter Updates in 1D
1)(with )(
)()( −+ΣΣ=
⎩⎨⎧
Σ−=Σ−+=
= tTttt
Tttt
tttt
ttttttt QCCCK
CKICzK
xbelμμμ
2,
2
2
22 with )1(
)()(
tobst
tt
ttt
tttttt K
K
zKxbel
σσσ
σσμμμ
+=
⎩⎨⎧
−=−+=
=
old belief
new belief
mea
sure
men
t
![Page 48: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/48.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Kalman Filter Updates in 1D
⎩⎨⎧
+Σ=Σ+=
=−
−
tTtttt
tttttt RAA
uBAxbel
1
1)(μμ
⎩⎨⎧
+=+=
= −2
,2221)(
tactttt
tttttt a
ubaxbel
σσσ
μμ
old belief
new belief
mea
sure
men
t
new belief
newest belief
![Page 49: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/49.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Kalman Filter Updates
belief
latest belief
belief
measurement
belief
![Page 50: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/50.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
The Prediction-Correction-Cycle
⎩⎨⎧
+Σ=Σ+=
=−
−
tTtttt
tttttt RAA
uBAxbel
1
1)(μμ
⎩⎨⎧
+=+=
= −2
,2221)(
tactttt
tttttt a
ubaxbel
σσσ
μμ
Prediction
![Page 51: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/51.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
The Prediction-Correction-Cycle
1)(,)(
)()( −+ΣΣ=
⎩⎨⎧
Σ−=Σ−+=
= tTttt
Tttt
tttt
ttttttt QCCCK
CKICzK
xbelμμμ
2,
2
2
22 ,)1(
)()(
tobst
tt
ttt
tttttt K
K
zKxbel
σσσ
σσμμμ
+=
⎩⎨⎧
−=−+=
=
Correction
![Page 52: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/52.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
The Prediction-Correction-Cycle
1)(,)(
)()( −+ΣΣ=
⎩⎨⎧
Σ−=Σ−+=
= tTttt
Tttt
tttt
ttttttt QCCCK
CKICzK
xbelμμμ
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
![Page 53: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/53.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
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!
![Page 54: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/54.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Overview
The Tracking Problem Bayes Filters Particle Filters Kalman Filters Using Kalman Filters
![Page 55: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/55.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Let’s Apply KFs to Tracking Problem
Image 4Image 1 Image 2 Image 3
![Page 56: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/56.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
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
![Page 57: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/57.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
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
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
![Page 58: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/58.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
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
![Page 59: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/59.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Can We Do Better?
![Page 60: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/60.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Kalman, Better!
initial position prediction measurement
x&
x
x&
x
x&
x
next prediciton
x&
x
update
x&
x
![Page 61: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/61.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
We Can Estimate Velocity!
past measurements
prediction
![Page 62: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/62.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Kalman Filter For 2D Tracking
Linear Measurement model (now with 4 state variables)
Linear Change
⎟⎟⎟⎟⎟
⎠
⎞
⎜⎜⎜⎜⎜
⎝
⎛
=
2
2
000
000
0000
0000
q
⎟⎟⎠
⎞⎜⎜⎝
⎛=
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
+
⎟⎟⎟⎟⎟
⎠
⎞
⎜⎜⎜⎜⎜
⎝
⎛
⎟⎟⎟⎟⎟
⎠
⎞
⎜⎜⎜⎜⎜
⎝
⎛Δ
Δ
=
⎟⎟⎟⎟⎟
⎠
⎞
⎜⎜⎜⎜⎜
⎝
⎛
&
&
&
&
![Page 63: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/63.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Putting It Together Again
Measurements
Change
Prediction
Measurement Update
),0(0010
0001RNxz +⎟⎟
⎠
⎞⎜⎜⎝
⎛=
rr
),0(
1000
0100
010
001
' QNxt
t
x +
⎟⎟⎟⎟⎟
⎠
⎞
⎜⎜⎜⎜⎜
⎝
⎛Δ
Δ
=rr
( ))(''
'1
111
μμμ AzRA
ARAT
T
−Σ+=
+Σ=Σ−
−−−
μμ C
CQC T
=
+Σ=Σ
'
'
![Page 64: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/64.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
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)
![Page 65: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/65.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
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
![Page 66: Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.](https://reader035.fdocuments.net/reader035/viewer/2022081516/56649d3e5503460f94a168e3/html5/thumbnails/66.jpg)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Summary
The Tracking Problem Bayes Filters Particle Filters Kalman Filters Using Kalman Filters