html5-img
1 / 50

Introduction to Kalman Filter and SLAM

Introduction to Kalman Filter and SLAM. Ting-Wei Hsu 08/10/30. What is Kalman Filter? (cont.). 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

ruby
Télécharger la présentation

Introduction to Kalman Filter and SLAM

An Image/Link below is provided (as is) to download presentation Download Policy: Content on the Website is provided to you AS IS for your information and personal use and may not be sold / licensed / shared on other websites without getting consent from its author. Content is provided to you AS IS for your information and personal use only. Download presentation by click this link. While downloading, if for some reason you are not able to download a presentation, the publisher may have deleted the file from their server. During download, if you can't get a presentation, the file might be deleted by the publisher.

E N D

Presentation Transcript

  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.)

More Related