1. EKF, UKF TexPoint fonts used in EMF.
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

2. Kalman Filter
Kalman Filter = special case of a Bayes’ filter with dynamics model and sensory model being linear Gaussian: 2 -1
Should add off-set to linear system --- make it affine
Then “linearization” will work out more cleanly for nonlinear systems to directly map into “linear (affine) systems”

3. Kalman Filtering Algorithm
At time 0:
For t = 1, 2, …
Dynamics update:
Measurement update:

4. Nonlinear Dynamical Systems
Most realistic robotic problems involve nonlinear functions:
Versus linear setting:

5. Linearity Assumption Revisitedx
p(x)
p(y)

6. Non-linear Function y p(y) p(x) x
“Gaussian of p(y)” has mean and variance of y under p(y)

7. EKF Linearization (1)

8. EKF Linearization (2)
p(x) has high variance relative to region in which linearization is accurate.

9. EKF Linearization (3)
p(x) has small variance relative to region in which linearization is accurate.

10. EKF Linearization: First Order Taylor Series Expansion
Dynamics model: for xt “close to” ¹t we have:
Measurement model: for xt “close to” ¹t we have:

11. EKF Linearization: Numerical
Numerically compute Ft column by column:
Here ei is the basis vector with all entries equal to zero, except for the i’t entry, which equals 1.
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

12. Ordinary Least Squares
Given: samples {(x(1), y(1)), (x(2), y(2)), …, (x(m), y(m))}
Problem: find function of the form f(x) = a0 + a1 x that fits the samples as well as possible in the following sense:

13. Ordinary Least Squares
Recall our objective:
Let’s write this in vector notation:
, giving:
Set gradient equal to zero to find extremum:
(See the Matrix Cookbook for matrix identities, including derivatives.)

14. Ordinary Least Squares
For our example problem we obtain a = [4.75; 2.00]
a0 + a1 x

15. Ordinary Least Squares10 20 30 40 22 24 26
More generally:
In vector notation:
, gives:
Set gradient equal to zero to find extremum (exact same derivation as two slides back):

16. Vector Valued Ordinary Least Squares Problems
So far have considered approximating a scalar valued function from samples {(x(1), y(1)), (x(2), y(2)), …, (x(m), y(m))} with
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:
In our vector notation:
This can be solved by solving a separate ordinary least squares problem to find each row of

17. Vector Valued Ordinary Least Squares Problems
Solving the OLS problem for each row gives us:
Each OLS problem has the same structure. We have

18. Vector Valued Ordinary Least Squares and EKF Linearization
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)}
Similarly for zt+1 = ht(xt)

19. OLS and EKF Linearization: Sample Point Selection
OLS vs. traditional (tangent) linearization:
OLS
traditional (tangent)

20. OLS Linearization: choosing samples points
Perhaps most natural choice:
reasonable way of trying to cover the region with reasonably high probability mass

21. Analytical vs. Numerical Linearization
Numerical (based on least squares or finite differences) could give a more accurate “regional” approximation. Size of region determined by evaluation points.
Computational efficiency:
Analytical derivatives can be cheaper or more expensive than function evaluations
Development hint:
Numerical derivatives tend to be easier to implement
If deciding to use analytical derivatives, implementing finite difference derivative and comparing with analytical results can help debugging the analytical derivatives

22. EKF Algorithm At time 0: For t = 1, 2, … Dynamics update:
Measurement update:

23. EKF Algorithm Extended_Kalman_filter( mt-1, St-1, ut, zt): Correction:
Prediction:
Correction:
Return mt, St

24. Localization
“Using sensory information to locate the robot in its environment is the most fundamental problem to providing a mobile robot with autonomous capabilities.” [Cox ’91]
Given
Map of the environment.
Sequence of sensor measurements.
Wanted
Estimate of the robot’s position.
Problem classes
Position tracking
Global localization
Kidnapped robot problem (recovery)

25. Landmark-based Localization

26. EKF_localization ( mt-1, St-1, ut, zt, m): Prediction:
Jacobian of g w.r.t location
Jacobian of g w.r.t control
Motion noise
Predicted mean
Predicted covariance

27. EKF_localization ( mt-1, St-1, ut, zt, m): Correction:
Predicted measurement mean
Jacobian of h w.r.t location
Pred. measurement covariance
Kalman gain
Updated mean
Updated covariance

28. EKF Prediction Step

29. EKF Observation Prediction Step

30. EKF Correction Step

31. Estimation Sequence (1)

32. Estimation Sequence (2)

33. Comparison to GroundTruth

34. EKF Summary
Highly efficient: Polynomial in measurement dimensionality k and state dimensionality n: O(k n2)
Not optimal!
Can diverge if nonlinearities are large!
Works surprisingly well even when all assumptions are violated!

35. Linearization via Unscented Transform
EKF
UKF

36. UKF Sigma-Point Estimate (2)
EKF
UKF

37. UKF Sigma-Point Estimate (3)
EKF
UKF

38. UKF Sigma-Point Estimate (4)

39. UKF intuition why it can perform better
[Julier and Uhlmann, 1997]
Assume we know the distribution over X and it has a mean \bar{x}
Y = f(X)
EKF approximates f by first order and ignores higher-order terms
UKF uses f exactly, but approximates p(x).

40. Self-quiz When would the UKF significantly outperform the EKF? y x
Analytical derivatives, finite-difference derivatives, and least squares will all end up with a horizontal linearization
 they’d predict zero variance in Y = f(X) y
When the f(mu) is significantly mismatched with E(f(x)) --- e.g., f concave or convex  KEF gives systematic over/underestimate x

41. A crude preliminary investigation of whether we can get EKF to match UKF by particular choice of points used in the least squares fitting
Beyond scope of course, just including for completeness.

42. Original unscented transform
Picks a minimal set of sample points that match 1st, 2nd and 3rd moments of a Gaussian:
\bar{x} = mean, Pxx = covariance, i  i’th column, x 2 <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
L = \sqrt{P_{xx}} can be chosen to be any matrix satisfying:
L LT = Pxx
[Julier and Uhlmann, 1997]

43. Unscented Transform Sigma points Weights
Pass sigma points through nonlinear function
Recover mean and covariance

44. Unscented Kalman filter
Dynamics update:
Can simply use unscented transform and estimate the mean and variance at the next time from the sample points
Observation update:
Use sigma-points from unscented transform to compute the covariance matrix between xt and zt. Then can do the standard update.

45. [Table 3.4 in Probabilistic Robotics]

46. UKF Summary
Highly efficient: Same complexity as EKF, with a constant factor slower in typical practical applications
Better linearization than EKF: Accurate in first two terms of Taylor expansion (EKF only first term) + capturing more aspects of the higher order terms
Derivative-free: No Jacobians needed
Still not optimal!

47. Unscented Transform Sigma points Weights
Pass sigma points through nonlinear function
Recover mean and covariance

48. UKF_localization ( mt-1, St-1, ut, zt, m):
Prediction:
Motion noise
Measurement noise
Augmented state mean
Augmented covariance
Sigma points
Prediction of sigma points
Predicted mean
Predicted covariance

49. UKF_localization ( mt-1, St-1, ut, zt, m):
Correction:
Measurement sigma points
Predicted measurement mean
Pred. measurement covariance
Cross-covariance
Kalman gain
Updated mean
Updated covariance

50. EKF_localization ( mt-1, St-1, ut, zt, m): Correction:
Predicted measurement mean
Jacobian of h w.r.t location
Pred. measurement covariance
Sigma – k h sigma = sigma – k s k
Kalman gain
Updated mean
Updated covariance

51. UKF Prediction Step

52. UKF Observation Prediction Step

53. UKF Correction Step

54. EKF Correction Step

55. Estimation Sequence
EKF PF UKF

56. Estimation Sequence
EKF UKF

57. Prediction Quality
EKF UKF

58. Kalman Filter-based System
[Arras et al. 98]:
Laser range-finder and vision
High precision (<1cm accuracy)
[Courtesy of Kai Arras]

59. Multi- hypothesis Tracking

60. Localization With MHT Belief is represented by multiple hypotheses
Each hypothesis is tracked by a Kalman filter
Additional problems:
Data association: Which observation corresponds to which hypothesis?
Hypothesis management: When to add / delete hypotheses?
Huge body of literature on target tracking, motion correspondence etc.

61. MHT: Implemented System (1)
Hypotheses are extracted from LRF scans
Each hypothesis has probability of being the correct one:
Hypothesis probability is computed using Bayes’ rule
Hypotheses with low probability are deleted.
New candidates are extracted from LRF scans.
[Jensfelt et al. ’00]

62. MHT: Implemented System (2)
Courtesy of P. Jensfelt and S. Kristensen

63. MHT: Implemented System (3) Example run
# hypotheses
P(Hbest)
Map and trajectory
#hypotheses vs. time
Courtesy of P. Jensfelt and S. Kristensen