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