Introduction to Kalman Filter and SLAM - PowerPoint PPT Presentation

1 / 50
About This Presentation
Title:

Introduction to Kalman Filter and SLAM

Description:

Introduction to Kalman Filter and SLAM Ting-Wei Hsu 08/10/30 – PowerPoint PPT presentation

Number of Views:216
Avg rating:3.0/5.0
Slides: 51
Provided by: archerEe6
Category:

less

Transcript and Presenter's Notes

Title: 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.)
  • Whats 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
  • zn A un
  • Measure1
  • State

5
Basic Idea
  • Measure2
  • State ?

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

7
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
Extend to System Model
z1
z2
z3
z4
z5
x1, s1
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, s1
z6
F
x2, s2
z7
F
x3, s3
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,y1xn, 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 yNT

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
  • T MAP, consists of collection of features?0
    ?1?n
  • st robot post at time t
  • st s1, s2, s3st
  • 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.)
Write a Comment
User Comments (0)
About PowerShow.com