cs 287 lecture 12 (fall 2019) kalman filtering
TRANSCRIPT
![Page 1: CS 287 Lecture 12 (Fall 2019) Kalman Filtering](https://reader033.vdocument.in/reader033/viewer/2022051813/628306070927d038a9577186/html5/thumbnails/1.jpg)
CS 287 Lecture 12 (Fall 2019)Kalman Filtering
Lecturer: Ignasi ClaveraSlides by Pieter Abbeel
UC Berkeley EECS
Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics
![Page 2: CS 287 Lecture 12 (Fall 2019) Kalman Filtering](https://reader033.vdocument.in/reader033/viewer/2022051813/628306070927d038a9577186/html5/thumbnails/2.jpg)
Outline
n Gaussians
n Kalman filtering
n Extend Kalman Filter (EKF)
n Unscented Kalman Filter (UKF) [aka “sigma-point filter”]
![Page 3: CS 287 Lecture 12 (Fall 2019) Kalman Filtering](https://reader033.vdocument.in/reader033/viewer/2022051813/628306070927d038a9577186/html5/thumbnails/3.jpg)
Outline
n Gaussians
n Kalman filtering
n Extend Kalman Filter (EKF)
n Unscented Kalman Filter (UKF) [aka “sigma-point filter”]
![Page 4: CS 287 Lecture 12 (Fall 2019) Kalman Filtering](https://reader033.vdocument.in/reader033/viewer/2022051813/628306070927d038a9577186/html5/thumbnails/4.jpg)
Multivariate Gaussians
![Page 5: CS 287 Lecture 12 (Fall 2019) Kalman Filtering](https://reader033.vdocument.in/reader033/viewer/2022051813/628306070927d038a9577186/html5/thumbnails/5.jpg)
Multivariate Gaussians
(integral of vector = vector of integrals of each entry)
(integral of matrix = matrix of integrals of each entry)
![Page 6: CS 287 Lecture 12 (Fall 2019) Kalman Filtering](https://reader033.vdocument.in/reader033/viewer/2022051813/628306070927d038a9577186/html5/thumbnails/6.jpg)
§ µ = [1; 0]§ S = [1 0; 0 1]
§ µ = [-.5; 0]§ S = [1 0; 0 1]
§ µ = [-1; -1.5]§ S = [1 0; 0 1]
Multivariate Gaussians: Examples
![Page 7: CS 287 Lecture 12 (Fall 2019) Kalman Filtering](https://reader033.vdocument.in/reader033/viewer/2022051813/628306070927d038a9577186/html5/thumbnails/7.jpg)
n µ = [0; 0]
n S = [1 0 ; 0 1]§ µ = [0; 0]§ S = [.6 0 ; 0 .6]
§ µ = [0; 0]§ S = [2 0 ; 0 2]
Multivariate Gaussians: Examples
![Page 8: CS 287 Lecture 12 (Fall 2019) Kalman Filtering](https://reader033.vdocument.in/reader033/viewer/2022051813/628306070927d038a9577186/html5/thumbnails/8.jpg)
§ µ = [0; 0]§ S = [1 0; 0 1]
§ µ = [0; 0]§ S = [1 0.5; 0.5 1]
§ µ = [0; 0]§ S = [1 0.8; 0.8 1]
Multivariate Gaussians: Examples
![Page 9: CS 287 Lecture 12 (Fall 2019) Kalman Filtering](https://reader033.vdocument.in/reader033/viewer/2022051813/628306070927d038a9577186/html5/thumbnails/9.jpg)
§ µ = [0; 0]§ S = [1 0; 0 1]
§ µ = [0; 0]§ S = [1 0.5; 0.5 1]
§ µ = [0; 0]§ S = [1 0.8; 0.8 1]
Multivariate Gaussians: Examples
![Page 10: CS 287 Lecture 12 (Fall 2019) Kalman Filtering](https://reader033.vdocument.in/reader033/viewer/2022051813/628306070927d038a9577186/html5/thumbnails/10.jpg)
§ µ = [0; 0]§ S = [1 -0.5 ; -0.5 1]
§ µ = [0; 0]§ S = [1 -0.8 ; -0.8 1]
§ µ = [0; 0]§ S = [3 0.8 ; 0.8 1]
Multivariate Gaussians: Examples
![Page 11: CS 287 Lecture 12 (Fall 2019) Kalman Filtering](https://reader033.vdocument.in/reader033/viewer/2022051813/628306070927d038a9577186/html5/thumbnails/11.jpg)
Partitioned Multivariate Gaussiann Consider a multi-variate Gaussian and partition random vector into (X, Y).
![Page 12: CS 287 Lecture 12 (Fall 2019) Kalman Filtering](https://reader033.vdocument.in/reader033/viewer/2022051813/628306070927d038a9577186/html5/thumbnails/12.jpg)
Partitioned Multivariate Gaussian: Dual Representation
n Precision matrix
n Straightforward to verify from (1) that:
n And swapping the roles of Sigma and Gamma:
(1)
![Page 13: CS 287 Lecture 12 (Fall 2019) Kalman Filtering](https://reader033.vdocument.in/reader033/viewer/2022051813/628306070927d038a9577186/html5/thumbnails/13.jpg)
Marginalization: p(x) = ?
We integrate out over y to find the marginal:
Hence we have:
Note: if we had known beforehand that p(x) would be a Gaussian distribution, then we could have found the result more quickly. We would have just needed to find and , which we had available through
![Page 14: CS 287 Lecture 12 (Fall 2019) Kalman Filtering](https://reader033.vdocument.in/reader033/viewer/2022051813/628306070927d038a9577186/html5/thumbnails/14.jpg)
If
Then
Marginalization Recap
![Page 15: CS 287 Lecture 12 (Fall 2019) Kalman Filtering](https://reader033.vdocument.in/reader033/viewer/2022051813/628306070927d038a9577186/html5/thumbnails/15.jpg)
Self-quiz
![Page 16: CS 287 Lecture 12 (Fall 2019) Kalman Filtering](https://reader033.vdocument.in/reader033/viewer/2022051813/628306070927d038a9577186/html5/thumbnails/16.jpg)
Conditioning: p(x | Y = y0) = ?
We have
Hence we have:
• Conditional mean moved according to correlation and variance on measurement• Conditional covariance does not depend on y0
![Page 17: CS 287 Lecture 12 (Fall 2019) Kalman Filtering](https://reader033.vdocument.in/reader033/viewer/2022051813/628306070927d038a9577186/html5/thumbnails/17.jpg)
If
Then
Conditioning Recap
![Page 18: CS 287 Lecture 12 (Fall 2019) Kalman Filtering](https://reader033.vdocument.in/reader033/viewer/2022051813/628306070927d038a9577186/html5/thumbnails/18.jpg)
Outline
n Gaussians
n Kalman filtering
n Extend Kalman Filter (EKF)
n Unscented Kalman Filter (UKF) [aka “sigma-point filter”]
![Page 19: CS 287 Lecture 12 (Fall 2019) Kalman Filtering](https://reader033.vdocument.in/reader033/viewer/2022051813/628306070927d038a9577186/html5/thumbnails/19.jpg)
n Kalman Filter = special case of a Bayes’ filter with dynamics and sensory models linear Gaussians:
Kalman Filter
2 -1
![Page 20: CS 287 Lecture 12 (Fall 2019) Kalman Filtering](https://reader033.vdocument.in/reader033/viewer/2022051813/628306070927d038a9577186/html5/thumbnails/20.jpg)
Time updaten Assume we have current belief for :
n Then, after one time step passes:Xt+1Xt
![Page 21: CS 287 Lecture 12 (Fall 2019) Kalman Filtering](https://reader033.vdocument.in/reader033/viewer/2022051813/628306070927d038a9577186/html5/thumbnails/21.jpg)
n Now we can choose to continue by either of n (i) mold it into a standard multivariate Gaussian format so we can read of
the joint distribution’s mean and covariance
n (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 22: CS 287 Lecture 12 (Fall 2019) Kalman Filtering](https://reader033.vdocument.in/reader033/viewer/2022051813/628306070927d038a9577186/html5/thumbnails/22.jpg)
n We follow (ii) and find the means and covariance matrices in
[Exercise: Try to prove each of these without referring to this slide!]
Time Update: Finding the joint
![Page 23: CS 287 Lecture 12 (Fall 2019) Kalman Filtering](https://reader033.vdocument.in/reader033/viewer/2022051813/628306070927d038a9577186/html5/thumbnails/23.jpg)
Time Update Recapn Assume we have
n Then we have
n Marginalizing the joint, we immediately get
Xt+1Xt
![Page 24: CS 287 Lecture 12 (Fall 2019) Kalman Filtering](https://reader033.vdocument.in/reader033/viewer/2022051813/628306070927d038a9577186/html5/thumbnails/24.jpg)
Generality!n Assume we have
n Then we have
n Marginalizing the joint, we immediately get
WV
![Page 25: CS 287 Lecture 12 (Fall 2019) Kalman Filtering](https://reader033.vdocument.in/reader033/viewer/2022051813/628306070927d038a9577186/html5/thumbnails/25.jpg)
Observation updaten Assume we have:
n Then:
n And, by conditioning on (see lecture slides on Gaussians) we readily get:
Zt+1
Xt+1
![Page 26: CS 287 Lecture 12 (Fall 2019) Kalman Filtering](https://reader033.vdocument.in/reader033/viewer/2022051813/628306070927d038a9577186/html5/thumbnails/26.jpg)
n At time 0:
n For t = 1, 2, …
n Dynamics update:
n Measurement update:
n Often written as:
Complete Kalman Filtering Algorithm
(Kalman gain)“innovation”
![Page 27: CS 287 Lecture 12 (Fall 2019) Kalman Filtering](https://reader033.vdocument.in/reader033/viewer/2022051813/628306070927d038a9577186/html5/thumbnails/27.jpg)
Kalman Filter Summary
n Highly efficient: Polynomial in measurement dimensionality kand state dimensionality n:
O(k2.376 + n2)
n Optimal for linear Gaussian systems!
![Page 28: CS 287 Lecture 12 (Fall 2019) Kalman Filtering](https://reader033.vdocument.in/reader033/viewer/2022051813/628306070927d038a9577186/html5/thumbnails/28.jpg)
Outline
n Gaussians
n Kalman filtering
n Extend Kalman Filter (EKF)
n Unscented Kalman Filter (UKF) [aka “sigma-point filter”]
![Page 29: CS 287 Lecture 12 (Fall 2019) Kalman Filtering](https://reader033.vdocument.in/reader033/viewer/2022051813/628306070927d038a9577186/html5/thumbnails/29.jpg)
Nonlinear Dynamical Systemsn Most realistic robotic problems involve nonlinear functions:
n Versus linear setting:
![Page 30: CS 287 Lecture 12 (Fall 2019) Kalman Filtering](https://reader033.vdocument.in/reader033/viewer/2022051813/628306070927d038a9577186/html5/thumbnails/30.jpg)
Linearity Assumption Revisitedy
y
x
x
p(x)
p(y)
![Page 31: CS 287 Lecture 12 (Fall 2019) Kalman Filtering](https://reader033.vdocument.in/reader033/viewer/2022051813/628306070927d038a9577186/html5/thumbnails/31.jpg)
Linearity Assumption Revisitedy
y
x
x
p(x)
p(y)
![Page 32: CS 287 Lecture 12 (Fall 2019) Kalman Filtering](https://reader033.vdocument.in/reader033/viewer/2022051813/628306070927d038a9577186/html5/thumbnails/32.jpg)
Non-linear Function
“Gaussian of p(y)” has mean and variance of y under p(y)
yy
x
x
p(x)
p(y)
![Page 33: CS 287 Lecture 12 (Fall 2019) Kalman Filtering](https://reader033.vdocument.in/reader033/viewer/2022051813/628306070927d038a9577186/html5/thumbnails/33.jpg)
EKF Linearization (1)
![Page 34: CS 287 Lecture 12 (Fall 2019) Kalman Filtering](https://reader033.vdocument.in/reader033/viewer/2022051813/628306070927d038a9577186/html5/thumbnails/34.jpg)
EKF Linearization (2)
p(x) has HIGH variance relative to region in which linearization is accurate.
![Page 35: CS 287 Lecture 12 (Fall 2019) Kalman Filtering](https://reader033.vdocument.in/reader033/viewer/2022051813/628306070927d038a9577186/html5/thumbnails/35.jpg)
EKF Linearization (3)
p(x) has LOW variance relative to region in which linearization is accurate.
![Page 36: CS 287 Lecture 12 (Fall 2019) Kalman Filtering](https://reader033.vdocument.in/reader033/viewer/2022051813/628306070927d038a9577186/html5/thumbnails/36.jpg)
n Dynamics model: for xt “close to” μt we have:
n Measurement model: for xt “close to” μt we have:
EKF Linearization: First Order Taylor Series Expansion
![Page 37: CS 287 Lecture 12 (Fall 2019) Kalman Filtering](https://reader033.vdocument.in/reader033/viewer/2022051813/628306070927d038a9577186/html5/thumbnails/37.jpg)
n At time 0:
n For t = 1, 2, …
n Dynamics update:
n Measurement update:
EKF Algorithm
![Page 38: CS 287 Lecture 12 (Fall 2019) Kalman Filtering](https://reader033.vdocument.in/reader033/viewer/2022051813/628306070927d038a9577186/html5/thumbnails/38.jpg)
EKF Summary
n Highly efficient: Polynomial in measurement dimensionality k and state dimensionality n:
O(k2.376 + n2)
n Not optimal!n Can diverge if nonlinearities are large!n Works surprisingly well even when all assumptions are
violated!
![Page 39: CS 287 Lecture 12 (Fall 2019) Kalman Filtering](https://reader033.vdocument.in/reader033/viewer/2022051813/628306070927d038a9577186/html5/thumbnails/39.jpg)
Outline
n Gaussians
n Kalman filtering
n Extend Kalman Filter (EKF)
n Unscented Kalman Filter (UKF) [aka “sigma-point filter”]
![Page 40: CS 287 Lecture 12 (Fall 2019) Kalman Filtering](https://reader033.vdocument.in/reader033/viewer/2022051813/628306070927d038a9577186/html5/thumbnails/40.jpg)
Linearization via Unscented Transform
EKF UKF
![Page 41: CS 287 Lecture 12 (Fall 2019) Kalman Filtering](https://reader033.vdocument.in/reader033/viewer/2022051813/628306070927d038a9577186/html5/thumbnails/41.jpg)
UKF Sigma-Point Estimate (2)
EKF UKF
![Page 42: CS 287 Lecture 12 (Fall 2019) Kalman Filtering](https://reader033.vdocument.in/reader033/viewer/2022051813/628306070927d038a9577186/html5/thumbnails/42.jpg)
UKF Sigma-Point Estimate (3)
EKF UKF
![Page 43: CS 287 Lecture 12 (Fall 2019) Kalman Filtering](https://reader033.vdocument.in/reader033/viewer/2022051813/628306070927d038a9577186/html5/thumbnails/43.jpg)
UKF Sigma-Point Estimate (4)
![Page 44: CS 287 Lecture 12 (Fall 2019) Kalman Filtering](https://reader033.vdocument.in/reader033/viewer/2022051813/628306070927d038a9577186/html5/thumbnails/44.jpg)
n Assume we know the distribution over X and it has a mean \bar{x}
n Y = f(X)
n EKF approximates f to first order and ignores higher-order terms
n UKF uses f exactly, but approximates p(x).
UKF intuition why it can perform better[Julier and Uhlmann, 1997]
![Page 45: CS 287 Lecture 12 (Fall 2019) Kalman Filtering](https://reader033.vdocument.in/reader033/viewer/2022051813/628306070927d038a9577186/html5/thumbnails/45.jpg)
n Picks a minimal set of sample points that match 1st, 2nd and 3rd moments of a Gaussian:
n \bar{x} = mean, Pxx = covariance, i à i’th column, x in Rn
n κ : extra degree of freedom to fine-tune the higher order moments of the approximation; when x is Gaussian, n+κ = 3 is a suggested heuristic
n L = \sqrt{P_{xx}} can be chosen to be any matrix satisfying:
n L LT = Pxx
Original Unscented Transform
[Julier and Uhlmann, 1997]
![Page 46: CS 287 Lecture 12 (Fall 2019) Kalman Filtering](https://reader033.vdocument.in/reader033/viewer/2022051813/628306070927d038a9577186/html5/thumbnails/46.jpg)
n Dynamics update:
n Can simply use unscented transform and estimate the mean and variance at the next time from the sample points
n Observation update:n Use sigma-points from unscented transform to compute the covariance
matrix between xt and zt. Then can do the standard update.
Unscented Kalman filter
![Page 47: CS 287 Lecture 12 (Fall 2019) Kalman Filtering](https://reader033.vdocument.in/reader033/viewer/2022051813/628306070927d038a9577186/html5/thumbnails/47.jpg)
[Table 3.4 in Probabilistic Robotics]
![Page 48: CS 287 Lecture 12 (Fall 2019) Kalman Filtering](https://reader033.vdocument.in/reader033/viewer/2022051813/628306070927d038a9577186/html5/thumbnails/48.jpg)
UKF Summary
n Highly efficient: Same complexity as EKF, with a constant factor slower in typical practical applications
n Better linearization than EKF: Accurate in first two terms of Taylor expansion (EKF only first term) + capturing more aspects of the higher order terms
n Derivative-free: No Jacobians needed
n Still not optimal!
![Page 49: CS 287 Lecture 12 (Fall 2019) Kalman Filtering](https://reader033.vdocument.in/reader033/viewer/2022051813/628306070927d038a9577186/html5/thumbnails/49.jpg)
n How to estimate At, Bt, Ct, Qt, Rt from data (z0:T, u0:T)
n EM algorithm
n How to compute (= smoothing) (note the capital “T”)
Forthcoming
![Page 50: CS 287 Lecture 12 (Fall 2019) Kalman Filtering](https://reader033.vdocument.in/reader033/viewer/2022051813/628306070927d038a9577186/html5/thumbnails/50.jpg)
n Square-root Kalman filter --- keeps track of square root of covariance matrices --- equally fast, numerically more stable (bit more complicated conceptually)
n Very large systems with sparsity structure
n Sparse Information Filter
n Very large systems with low-rank structure
n Ensemble Kalman Filter
n Kalman filtering over SE(3)
n How to estimate At, Bt, Ct, Qt, Rt from data (z0:T, u0:T)
n EM algorithm
n How to compute (= smoothing) (note the capital “T”)
Things to be aware of (but we won’t cover)
![Page 51: CS 287 Lecture 12 (Fall 2019) Kalman Filtering](https://reader033.vdocument.in/reader033/viewer/2022051813/628306070927d038a9577186/html5/thumbnails/51.jpg)
n If At = A, Qt = Q, Ct = C, Rt = R
n If system is “observable” then covariances and Kalman gain will converge to steady-state values for t -> 1n Can take advantage of this: pre-compute them, only track the mean, which is done by multiplying Kalman gain with
“innovation”
n 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
n Observable if and only if: rank( [ C ; CA ; CA2 ; CA3 ; … ; CAn-1]) = n
n Typically if a system is not observable you will want to add a sensor to make it observable
n 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 (but we won’t cover)
![Page 52: CS 287 Lecture 12 (Fall 2019) Kalman Filtering](https://reader033.vdocument.in/reader033/viewer/2022051813/628306070927d038a9577186/html5/thumbnails/52.jpg)
n If system is observable (=dual of controllable!) then Kalman filter will converge to the true state.
n System is observable if and only if:
O = [C ; CA ; CA2 ; … ; CAn-1] is full column rank (1)
Intuition: if no noise, we observe y0, y1, … and we have that the unknown initial state x0 satisfies:
y0 = C x0
y1 = CA x0
...
yK = CAK x0
This system of equations has a unique solution x0 iff the matrix [C; CA; … CAK] has full column rank. B/c any power of a matrix higher than n can be written in terms of lower powers of the same matrix, condition (1) is sufficient to check (i.e., the column rank will not grow anymore after having reached K=n-1).
Kalman filter property