1. Introduction to Kalman Filter and SLAM Ting-Wei Hsu 08/10/30

2. What is Kalman Filter? (cont.)

3. What’s used for ?  Tracking missiles  Tracking heads/heads  Extracting lip motion from video  Fitting Bezier patches to points data  Lots of computer vision  Economics  Navigation  ……

4. Basic Idea z[n] = A + u[n] Measure1: State:

5. Basic Idea Measure2: State = ?

6. Basic Idea (cont.) Measure from 1 & 2

7. Kalman Filter Model z1z1 z2z2 z3z3 z4z4 z5z5 x 1, σ 1 z6z6 x 2, σ 2 z7z7 x 3, σ 3

8. Extend to System Model x = Hθ+w y = θ

9. Estimate from Two Distributions If x and y are distributed according to Gaussian PDF with [E(x) E(y)] T And covariance matrix

10. Extend to System Model

12. z1z1 z2z2 z3z3 z4z4 z5z5 x 1, σ 1 F z6z6 x 2, σ 2 F z7z7 x 3, σ 3

13. Pre-limit of Kalman Filter Linear dynamical system Markov Chain Zero mean Gaussian noise

14. Prediction to Correction

15. System Model  F k state transition model  w k is the process noise which is assumed to be drawn from a zero mean multivariate normal distribution with covariance Q k  Observation model:  v k is the observation noise which is assumed to be zero mean Gaussian white noise with covariance R k

16. System Model z1z1 z2z2 z3z3 z4z4 z5z5 x 1, σ 1 F z6z6 x 2, σ 2 F z7z7 x 3, σ 3

17. Predict and Update Predict  Predicted state  Predicted estimate covariance Update  innovation or measurement residual  Innovation (or residual) covariance

18. Predict and Update (cont.) Update  Optimal Kalman gain  Updated state estimate  Updated estimate covariance http://en.wikipedia.org/wiki/Kalman_filter

19. Example: 2D PV Model Position-velocity model u(n): change in velocity v(n): measurement error

20. Example: 2D PV Model (cont.) Process Noise Covariance Measurement Noise Covariance

21. EKF-Extended Kalman Filter Processes to be estimate or measurement is non-linear. Model: Predict:

22. EKF-Extended Kalman Filter Update: Transition and observation matrix

23. Disadvantage of the Extended Kalman Filter Use only first level Taylor series. If the initial estimate of the state is wrong, the filter may quickly diverge. Solution: Unsented Kalman filter

24. SLAM Simultaneous localization and mapping Technique used by robots and autonomous vehicles to build up a map within an unknown environment.

25. SLAM Problem

26. Overview of the Process 1.Update the current state estimate using the odometry data. 2.Update the estimated state from re- observing landmarks. 3.Add new landmarks to the current state.

27. Spring Network Analogy

28. System Model  F k state transition model  w k is the process noise which is assumed to be drawn from a zero mean multivariate normal distribution with covariance Q k  Observation model:  v k is the observation noise which is assumed to be zero mean Gaussian white noise with covariance R k

29. The Matrix The system state: x  x r, y r, theta r for robot  x 1,y 1 …x n, y n position of each landmark.

30. The Matrix Covariance Matrix P 3x33x2 2x3

31. The Matrix Measurement model : H

32. The Matrix Jacobian of H of robot

33. The Matrix H for SLAM EKF as landmark number two observed.

34. The Matrix

35. Prediction model : A

36. The Matrix The SLAM specific Jacobians

37. Step 1: Update current state using the odometry data Update current state using odometry data P rr is the top left 3 by 3 matrix of P Update the robot to feature correlation

38. Step 2.Update the Estimated State from Re-observing Landmarks X = X + K*(z-h)

39. The Matrix Process noise Measure noise  c, d represent the accuracy of measure device =

40. Step 3: Add New Landmarks to Current State X = [X x N y N ] T

41. FastSLAM Integrates particle filter and extend Kalman Filter. Cope with non-linear robot models better.

42. FastSLAM Robot Trajectory

43. Factoring the SLAM Posterior

44. Symbol Θ MAP, consists of collection of features[θ 0 θ 1 …θ n ] s t robot post at time t s t = s 1, s 2, s 3 …s t z t, n t : measurement feature n at time t u t : control of vehicle

45. Fast SLAM Algorithm z t depend only on s t, n t, θ n t

46. Particle Filter in FastSLAM

47. Step 1. Extend the Path Posterior by Sampling New Poses. s t robot pose u t contorl

48. Step 2 Updating the Observed Landmark Estimate z t sensor measurement θlandmark

49. Step 3. Resampling

50. Step 3. Resampling (cont.)