Kalman Filtering Pieter Abbeel UC Berkeley EECS Many slides adapted from Thrun, Burgard and Fox,...
-
date post
22-Dec-2015 -
Category
Documents
-
view
217 -
download
0
Transcript of Kalman Filtering Pieter Abbeel UC Berkeley EECS Many slides adapted from Thrun, Burgard and Fox,...
![Page 1: Kalman Filtering Pieter Abbeel UC Berkeley EECS Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics TexPoint fonts used in EMF. Read.](https://reader035.fdocuments.in/reader035/viewer/2022062407/56649d7a5503460f94a5e448/html5/thumbnails/1.jpg)
Kalman Filtering
Pieter AbbeelUC Berkeley EECS
Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics
![Page 2: Kalman Filtering Pieter Abbeel UC Berkeley EECS Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics TexPoint fonts used in EMF. Read.](https://reader035.fdocuments.in/reader035/viewer/2022062407/56649d7a5503460f94a5e448/html5/thumbnails/2.jpg)
Kalman Filter = special case of a Bayes’ filter with dynamics model and sensory model being linear Gaussian:
Above can also be written as follows:
Overview
2 -1
Note: I switched time indexing on u to be in line with typical control community conventions (which is different from the probabilistic robotics book).
![Page 3: Kalman Filtering Pieter Abbeel UC Berkeley EECS Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics TexPoint fonts used in EMF. Read.](https://reader035.fdocuments.in/reader035/viewer/2022062407/56649d7a5503460f94a5e448/html5/thumbnails/3.jpg)
Time update
Assume we have current belief for :
Then, after one time step passes:
Xt+1Xt
![Page 4: Kalman Filtering Pieter Abbeel UC Berkeley EECS Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics TexPoint fonts used in EMF. Read.](https://reader035.fdocuments.in/reader035/viewer/2022062407/56649d7a5503460f94a5e448/html5/thumbnails/4.jpg)
Now we can choose to continue by either of (i) mold it into a standard multivariate Gaussian
format so we can read of the joint distribution’s mean and covariance
(ii) observe this is a quadratic form in x_{t} and x_{t+1} in the exponent; the exponent is the only place they appear; hence we know this is a multivariate Gaussian. We directly compute its mean and covariance. [usually simpler!]
Time Update: Finding the joint
![Page 5: Kalman Filtering Pieter Abbeel UC Berkeley EECS Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics TexPoint fonts used in EMF. Read.](https://reader035.fdocuments.in/reader035/viewer/2022062407/56649d7a5503460f94a5e448/html5/thumbnails/5.jpg)
We follow (ii) and find the means and covariance matrices in
Time Update: Finding the joint
[Exercise: Try to prove each of these without referring to this slide!]
![Page 6: Kalman Filtering Pieter Abbeel UC Berkeley EECS Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics TexPoint fonts used in EMF. Read.](https://reader035.fdocuments.in/reader035/viewer/2022062407/56649d7a5503460f94a5e448/html5/thumbnails/6.jpg)
Time Update Recap
Assume we have
Then we have
Marginalizing the joint, we immediately get
Xt+1Xt
![Page 7: Kalman Filtering Pieter Abbeel UC Berkeley EECS Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics TexPoint fonts used in EMF. Read.](https://reader035.fdocuments.in/reader035/viewer/2022062407/56649d7a5503460f94a5e448/html5/thumbnails/7.jpg)
Generality!
Assume we have
Then we have
Marginalizing the joint, we immediately get
WV
![Page 8: Kalman Filtering Pieter Abbeel UC Berkeley EECS Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics TexPoint fonts used in EMF. Read.](https://reader035.fdocuments.in/reader035/viewer/2022062407/56649d7a5503460f94a5e448/html5/thumbnails/8.jpg)
Observation update Assume we have:
Then:
And, by conditioning on (see lecture slides on Gaussians) we readily get:
Zt+1
Xt+1
![Page 9: Kalman Filtering Pieter Abbeel UC Berkeley EECS Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics TexPoint fonts used in EMF. Read.](https://reader035.fdocuments.in/reader035/viewer/2022062407/56649d7a5503460f94a5e448/html5/thumbnails/9.jpg)
At time 0:
For t = 1, 2, … Dynamics update:
Measurement update:
Often written as:
Complete Kalman Filtering Algorithm
(Kalman gain)
“innovation”
![Page 10: Kalman Filtering Pieter Abbeel UC Berkeley EECS Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics TexPoint fonts used in EMF. Read.](https://reader035.fdocuments.in/reader035/viewer/2022062407/56649d7a5503460f94a5e448/html5/thumbnails/10.jpg)
10
Kalman Filter Summary
Highly efficient: Polynomial in measurement dimensionality k and state dimensionality n: O(k2.376 + n2)
Optimal for linear Gaussian systems!
![Page 11: Kalman Filtering Pieter Abbeel UC Berkeley EECS Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics TexPoint fonts used in EMF. Read.](https://reader035.fdocuments.in/reader035/viewer/2022062407/56649d7a5503460f94a5e448/html5/thumbnails/11.jpg)
Nonlinear systems
Extended Kalman Filter, Unscented Kalman Filter
Very large systems with sparsity structure
Sparse Information Filter
Very large systems with low-rank structure
Ensemble Kalman Filter
Kalman filtering over SE(3)
How to estimate At, Bt, Ct, Qt, Rt from data (z0:T, u0:T)
EM algorithm
How to compute (note the capital “T”)
Smoothing
Forthcoming Extensions
![Page 12: Kalman Filtering Pieter Abbeel UC Berkeley EECS Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics TexPoint fonts used in EMF. Read.](https://reader035.fdocuments.in/reader035/viewer/2022062407/56649d7a5503460f94a5e448/html5/thumbnails/12.jpg)
Square-root Kalman filter --- keeps track of square root of covariance matrices --- equally fast, numerically more stable (bit more complicated conceptually)
If At = A, Qt = Q, Ct = C, Rt = R
If system is “observable” then covariances and Kalman gain will converge to steady-state values for t -> 1
Can take advantage of this: pre-compute them, only track the mean, which is done by multiplying Kalman gain with “innovation”
System is observable if and only if the following holds true: if there were zero noise you could determine the initial state after a finite number of time steps
Observable if and only if: rank( [ C ; CA ; CA2 ; CA3 ; … ; CAn-1]) = n
Typically if a system is not observable you will want to add a sensor to make it observable
Kalman filter can also be derived as the (recursively computed) least-squares solutions to a (growing) set of linear equations
Things to be aware of that we won’t cover