Introduction to Kalman Filter and SLAM

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

2. What is Kalman Filter? (cont.)

3. What is Kalman Filter? (cont.) • 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. z1 z2 z3 z4 z5 z6 z7 x1, σ1 x2, σ2 x3, σ3 Kalman Filter Model

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

11. Extend to System Model

12. F z6 x2, σ2 F z7 x3, σ3 Extend to System Model z1 z2 z3 z4 z5 x1, σ1

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

14. Prediction to Correction

15. System Model • Fk state transition model • wk is the process noise which is assumed to be drawn from a zero mean multivariate normal distribution with covariance Qk • Observation model: • vk is the observation noise which is assumed to be zero mean Gaussian white noise with covariance Rk

16. System Model z1 z2 z3 z4 z5 x1, σ1 z6 F x2, σ2 z7 F x3, σ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.) Measurement Noise Covariance Process 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 • Fk state transition model • wk is the process noise which is assumed to be drawn from a zero mean multivariate normal distribution with covariance Qk • Observation model: • vk is the observation noise which is assumed to be zero mean Gaussian white noise with covariance Rk

29. The Matrix • The system state: x • xr, yr , thetar for robot • x1,y1…xn, yn position of each landmark.

30. The Matrix • Covariance Matrix P 3x3 3x2 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. The Matrix • 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 • Prr 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 xN yN]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] • st robot post at time t • st = s1, s2, s3…st • zt , nt : measurement feature n at time t • ut : control of vehicle

45. Fast SLAM Algorithm • zt depend only on st, nt, θnt

46. Particle Filter in FastSLAM

47. Step 1. Extend the Path Posterior by Sampling New Poses • . st robot pose ut contorl

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

49. Step 3. Resampling

50. Step 3. Resampling (cont.)