ekf, ukf - university of washington · ekf, ukf pieter abbeel uc berkeley eecs many slides adapted...
TRANSCRIPT
![Page 1: EKF, UKF - University of Washington · EKF, UKF Pieter Abbeel UC Berkeley EECS Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics TexPoint fonts used in EMF](https://reader033.vdocuments.mx/reader033/viewer/2022042323/5f0d89767e708231d43ad97b/html5/thumbnails/1.jpg)
EKF, UKF
Pieter Abbeel UC Berkeley EECS
Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics
TexPoint fonts used in EMF. Read the TexPoint manual before you delete this box.: AAAAAAAAAAAAA
![Page 2: EKF, UKF - University of Washington · EKF, UKF Pieter Abbeel UC Berkeley EECS Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics TexPoint fonts used in EMF](https://reader033.vdocuments.mx/reader033/viewer/2022042323/5f0d89767e708231d43ad97b/html5/thumbnails/2.jpg)
n Kalman Filter = special case of a Bayes’ filter with dynamics model and sensory model being linear Gaussian:
Kalman Filter
2 -1
![Page 3: EKF, UKF - University of Washington · EKF, UKF Pieter Abbeel UC Berkeley EECS Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics TexPoint fonts used in EMF](https://reader033.vdocuments.mx/reader033/viewer/2022042323/5f0d89767e708231d43ad97b/html5/thumbnails/3.jpg)
n At time 0:
n For t = 1, 2, …
n Dynamics update:
n Measurement update:
Kalman Filtering Algorithm
![Page 4: EKF, UKF - University of Washington · EKF, UKF Pieter Abbeel UC Berkeley EECS Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics TexPoint fonts used in EMF](https://reader033.vdocuments.mx/reader033/viewer/2022042323/5f0d89767e708231d43ad97b/html5/thumbnails/4.jpg)
4
Nonlinear Dynamical Systems
n Most realistic robotic problems involve nonlinear functions:
n Versus linear setting:
![Page 5: EKF, UKF - University of Washington · EKF, UKF Pieter Abbeel UC Berkeley EECS Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics TexPoint fonts used in EMF](https://reader033.vdocuments.mx/reader033/viewer/2022042323/5f0d89767e708231d43ad97b/html5/thumbnails/5.jpg)
5
Linearity Assumption Revisited
y y
x
x
p(x)
p(y)
![Page 6: EKF, UKF - University of Washington · EKF, UKF Pieter Abbeel UC Berkeley EECS Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics TexPoint fonts used in EMF](https://reader033.vdocuments.mx/reader033/viewer/2022042323/5f0d89767e708231d43ad97b/html5/thumbnails/6.jpg)
6
Linearity Assumption Revisited
y y
x
x
p(x)
p(y)
![Page 7: EKF, UKF - University of Washington · EKF, UKF Pieter Abbeel UC Berkeley EECS Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics TexPoint fonts used in EMF](https://reader033.vdocuments.mx/reader033/viewer/2022042323/5f0d89767e708231d43ad97b/html5/thumbnails/7.jpg)
7
Non-linear Function
“Gaussian of p(y)” has mean and variance of y under p(y)
y y
x
x
p(x)
p(y)
![Page 8: EKF, UKF - University of Washington · EKF, UKF Pieter Abbeel UC Berkeley EECS Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics TexPoint fonts used in EMF](https://reader033.vdocuments.mx/reader033/viewer/2022042323/5f0d89767e708231d43ad97b/html5/thumbnails/8.jpg)
8
EKF Linearization (1)
![Page 9: EKF, UKF - University of Washington · EKF, UKF Pieter Abbeel UC Berkeley EECS Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics TexPoint fonts used in EMF](https://reader033.vdocuments.mx/reader033/viewer/2022042323/5f0d89767e708231d43ad97b/html5/thumbnails/9.jpg)
9
EKF Linearization (2)
p(x) has high variance relative to region in which linearization is accurate.
![Page 10: EKF, UKF - University of Washington · EKF, UKF Pieter Abbeel UC Berkeley EECS Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics TexPoint fonts used in EMF](https://reader033.vdocuments.mx/reader033/viewer/2022042323/5f0d89767e708231d43ad97b/html5/thumbnails/10.jpg)
10
EKF Linearization (3)
p(x) has small variance relative to region in which linearization is accurate.
![Page 11: EKF, UKF - University of Washington · EKF, UKF Pieter Abbeel UC Berkeley EECS Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics TexPoint fonts used in EMF](https://reader033.vdocuments.mx/reader033/viewer/2022042323/5f0d89767e708231d43ad97b/html5/thumbnails/11.jpg)
11
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 12: EKF, UKF - University of Washington · EKF, UKF Pieter Abbeel UC Berkeley EECS Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics TexPoint fonts used in EMF](https://reader033.vdocuments.mx/reader033/viewer/2022042323/5f0d89767e708231d43ad97b/html5/thumbnails/12.jpg)
n Numerically compute Ft column by column:
n Here ei is the basis vector with all entries equal to zero, except for the i’t entry, which equals 1.
n If wanting to approximate Ft as closely as possible then ² is chosen to be a small number, but not too small to avoid numerical issues
EKF Linearization: Numerical
![Page 13: EKF, UKF - University of Washington · EKF, UKF Pieter Abbeel UC Berkeley EECS Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics TexPoint fonts used in EMF](https://reader033.vdocuments.mx/reader033/viewer/2022042323/5f0d89767e708231d43ad97b/html5/thumbnails/13.jpg)
n Given: samples {(x(1), y(1)), (x(2), y(2)), …, (x(m), y(m))}
n Problem: find function of the form f(x) = a0 + a1 x that fits the samples as well as possible in the following sense:
Ordinary Least Squares
![Page 14: EKF, UKF - University of Washington · EKF, UKF Pieter Abbeel UC Berkeley EECS Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics TexPoint fonts used in EMF](https://reader033.vdocuments.mx/reader033/viewer/2022042323/5f0d89767e708231d43ad97b/html5/thumbnails/14.jpg)
n Recall our objective:
n Let’s write this in vector notation:
n , giving:
n Set gradient equal to zero to find extremum:
Ordinary Least Squares
(See the Matrix Cookbook for matrix identities, including derivatives.)
![Page 15: EKF, UKF - University of Washington · EKF, UKF Pieter Abbeel UC Berkeley EECS Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics TexPoint fonts used in EMF](https://reader033.vdocuments.mx/reader033/viewer/2022042323/5f0d89767e708231d43ad97b/html5/thumbnails/15.jpg)
n For our example problem we obtain a = [4.75; 2.00]
Ordinary Least Squares
a0 + a1 x
![Page 16: EKF, UKF - University of Washington · EKF, UKF Pieter Abbeel UC Berkeley EECS Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics TexPoint fonts used in EMF](https://reader033.vdocuments.mx/reader033/viewer/2022042323/5f0d89767e708231d43ad97b/html5/thumbnails/16.jpg)
n More generally:
n In vector notation:
n , gives:
n Set gradient equal to zero to find extremum (exact same derivation as two slides back):
Ordinary Least Squares
0 10 20 30 40
0 10 20 30 20 22 24 26
![Page 17: EKF, UKF - University of Washington · EKF, UKF Pieter Abbeel UC Berkeley EECS Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics TexPoint fonts used in EMF](https://reader033.vdocuments.mx/reader033/viewer/2022042323/5f0d89767e708231d43ad97b/html5/thumbnails/17.jpg)
n So far have considered approximating a scalar valued function from samples {(x(1), y(1)), (x(2), y(2)), …, (x(m), y(m))} with
n A vector valued function is just many scalar valued functions and we can approximate it the same way by solving an OLS problem multiple times. Concretely, let then we have:
n In our vector notation:
n This can be solved by solving a separate ordinary least squares problem to find each row of
Vector Valued Ordinary Least Squares Problems
![Page 18: EKF, UKF - University of Washington · EKF, UKF Pieter Abbeel UC Berkeley EECS Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics TexPoint fonts used in EMF](https://reader033.vdocuments.mx/reader033/viewer/2022042323/5f0d89767e708231d43ad97b/html5/thumbnails/18.jpg)
n Solving the OLS problem for each row gives us:
n Each OLS problem has the same structure. We have
Vector Valued Ordinary Least Squares Problems
![Page 19: EKF, UKF - University of Washington · EKF, UKF Pieter Abbeel UC Berkeley EECS Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics TexPoint fonts used in EMF](https://reader033.vdocuments.mx/reader033/viewer/2022042323/5f0d89767e708231d43ad97b/html5/thumbnails/19.jpg)
n Approximate xt+1 = ft(xt, ut)
with affine function a0 + Ft xt
by running least squares on samples from the function:
{( xt(1), y(1)=ft(xt(1),ut), ( xt(2), y(2)=ft(xt(2),ut), …, ( xt(m), y(m)=ft(xt(m),ut)}
n Similarly for zt+1 = ht(xt)
Vector Valued Ordinary Least Squares and EKF Linearization
![Page 20: EKF, UKF - University of Washington · EKF, UKF Pieter Abbeel UC Berkeley EECS Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics TexPoint fonts used in EMF](https://reader033.vdocuments.mx/reader033/viewer/2022042323/5f0d89767e708231d43ad97b/html5/thumbnails/20.jpg)
n OLS vs. traditional (tangent) linearization:
OLS and EKF Linearization: Sample Point Selection
traditional (tangent)
OLS
![Page 21: EKF, UKF - University of Washington · EKF, UKF Pieter Abbeel UC Berkeley EECS Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics TexPoint fonts used in EMF](https://reader033.vdocuments.mx/reader033/viewer/2022042323/5f0d89767e708231d43ad97b/html5/thumbnails/21.jpg)
n Perhaps most natural choice:
n
n reasonable way of trying to cover the region with reasonably high probability mass
OLS Linearization: choosing samples points
![Page 22: EKF, UKF - University of Washington · EKF, UKF Pieter Abbeel UC Berkeley EECS Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics TexPoint fonts used in EMF](https://reader033.vdocuments.mx/reader033/viewer/2022042323/5f0d89767e708231d43ad97b/html5/thumbnails/22.jpg)
n Numerical (based on least squares or finite differences) could give a more accurate “regional” approximation. Size of region determined by evaluation points.
n Computational efficiency:
n Analytical derivatives can be cheaper or more expensive than function evaluations
n Development hint:
n Numerical derivatives tend to be easier to implement
n If deciding to use analytical derivatives, implementing finite difference derivative and comparing with analytical results can help debugging the analytical derivatives
Analytical vs. Numerical Linearization
![Page 23: EKF, UKF - University of Washington · EKF, UKF Pieter Abbeel UC Berkeley EECS Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics TexPoint fonts used in EMF](https://reader033.vdocuments.mx/reader033/viewer/2022042323/5f0d89767e708231d43ad97b/html5/thumbnails/23.jpg)
n At time 0:
n For t = 1, 2, …
n Dynamics update:
n Measurement update:
EKF Algorithm
![Page 24: EKF, UKF - University of Washington · EKF, UKF Pieter Abbeel UC Berkeley EECS Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics TexPoint fonts used in EMF](https://reader033.vdocuments.mx/reader033/viewer/2022042323/5f0d89767e708231d43ad97b/html5/thumbnails/24.jpg)
35
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 25: EKF, UKF - University of Washington · EKF, UKF Pieter Abbeel UC Berkeley EECS Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics TexPoint fonts used in EMF](https://reader033.vdocuments.mx/reader033/viewer/2022042323/5f0d89767e708231d43ad97b/html5/thumbnails/25.jpg)
36
Linearization via Unscented Transform
EKF UKF
![Page 26: EKF, UKF - University of Washington · EKF, UKF Pieter Abbeel UC Berkeley EECS Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics TexPoint fonts used in EMF](https://reader033.vdocuments.mx/reader033/viewer/2022042323/5f0d89767e708231d43ad97b/html5/thumbnails/26.jpg)
37
UKF Sigma-Point Estimate (2)
EKF UKF
![Page 27: EKF, UKF - University of Washington · EKF, UKF Pieter Abbeel UC Berkeley EECS Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics TexPoint fonts used in EMF](https://reader033.vdocuments.mx/reader033/viewer/2022042323/5f0d89767e708231d43ad97b/html5/thumbnails/27.jpg)
38
UKF Sigma-Point Estimate (3)
EKF UKF
![Page 28: EKF, UKF - University of Washington · EKF, UKF Pieter Abbeel UC Berkeley EECS Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics TexPoint fonts used in EMF](https://reader033.vdocuments.mx/reader033/viewer/2022042323/5f0d89767e708231d43ad97b/html5/thumbnails/28.jpg)
UKF Sigma-Point Estimate (4)
![Page 29: EKF, UKF - University of Washington · EKF, UKF Pieter Abbeel UC Berkeley EECS Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics TexPoint fonts used in EMF](https://reader033.vdocuments.mx/reader033/viewer/2022042323/5f0d89767e708231d43ad97b/html5/thumbnails/29.jpg)
n Assume we know the distribution over X and it has a mean \bar{x}
n Y = f(X)
n EKF approximates f by 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 30: EKF, UKF - University of Washington · EKF, UKF Pieter Abbeel UC Berkeley EECS Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics TexPoint fonts used in EMF](https://reader033.vdocuments.mx/reader033/viewer/2022042323/5f0d89767e708231d43ad97b/html5/thumbnails/30.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 2 <n
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 31: EKF, UKF - University of Washington · EKF, UKF Pieter Abbeel UC Berkeley EECS Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics TexPoint fonts used in EMF](https://reader033.vdocuments.mx/reader033/viewer/2022042323/5f0d89767e708231d43ad97b/html5/thumbnails/31.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 32: EKF, UKF - University of Washington · EKF, UKF Pieter Abbeel UC Berkeley EECS Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics TexPoint fonts used in EMF](https://reader033.vdocuments.mx/reader033/viewer/2022042323/5f0d89767e708231d43ad97b/html5/thumbnails/32.jpg)
[Table 3.4 in Probabilistic Robotics]
![Page 33: EKF, UKF - University of Washington · EKF, UKF Pieter Abbeel UC Berkeley EECS Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics TexPoint fonts used in EMF](https://reader033.vdocuments.mx/reader033/viewer/2022042323/5f0d89767e708231d43ad97b/html5/thumbnails/33.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!