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 Economics Navigation ……

Basic Idea

z[n] = A + u[n]

Measure1:

State:

Basic Idea

Measure2:

State = ?

Basic Idea (cont.)

Measure from 1 & 2

Kalman Filter Model

z1 z2 z3 z4 z5

x1, σ1

z6

x2, σ2

z7

x3, σ3

Extend to System Model

x = Hθ+w

y = θ

Estimate from Two Distributions If x and y are distributed according to Gaussian PD

F with [E(x) E(y)]T

And covariance matrix

))(()var(

),cov()()|( xEx

x

yxyExyE

)var(

),(cov)var()|var(

2

x

yxyxy

Extend to System Model

Extend to System Model

Extend to System Model

z1 z2 z3 z4 z5

x1, σ1

F z6

x2, σ2

F z7

x3, σ3

Pre-limit of Kalman Filter

Linear dynamical system Markov Chain Zero mean Gaussian noise

Prediction to Correction

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 m

ean Gaussian white noise with covariance Rk

kkkk wxFx 1

kkkk vxHz

System Model

z1 z2 z3 z4 z5

x1, σ1

F

z6

x2, σ2

F

z7

x3, σ3

Predict and Update

Predict Predicted state Predicted estimate covariance

Update innovation or measurement residual

Innovation (or residual) covariance

Predict and Update (cont.)

Update Optimal Kalman gain

Updated state estimate

Updated estimate covariance

http://en.wikipedia.org/wiki/Kalman_filter

kTkkkk

Tkkk

RHPH

HP

1|

1|

Example: 2D PV Model Position-velocity model

u(n): change in velocity

v(n): measurement error

Example: 2D PV Model (cont.)

Process Noise Covariance

Measurement Noise Covariance

EKF-Extended Kalman Filter

Processes to be estimate or measurement is non-linear.

Model:

Predict:

EKF-Extended Kalman Filter

Update:

Transition and observation matrix

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

SLAM

Simultaneous localization and mapping

Technique used by robots and autonomous vehicles to build up a map within an unknown environment.

SLAM Problem

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.

Spring Network Analogy

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 m

ean Gaussian white noise with covariance Rk

kkkk wxFx 1

kkkk vxHz

The Matrix

The system state: x

xr, yr , thetar for robot

x1,y1…xn, yn position of each landmark.

The Matrix

Covariance Matrix P

3x3 3x2

2x3

The Matrix

Measurement model : H

The Matrix

Jacobian of H of robot

The Matrix

H for SLAM EKF as landmark number two observed.

The Matrix

The Matrix

Prediction model : A

The Matrix

The SLAM specific Jacobians

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

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

TT

T

VRVHPH

PHK

The Matrix

Process noise

Measure noise c, d represent the

accuracy of measure device

=

Step 3: Add New Landmarks to Current State X = [X xN yN]T

FastSLAM

Integrates particle filter and extend Kalman Filter.

Cope with non-linear robot models better.

FastSLAM Robot Trajectory

Factoring the SLAM Posterior

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

Fast SLAM Algorithm

zt depend only on st, nt, θnt

Particle Filter in FastSLAM

Step 1. Extend the Path Posterior by Sampling New Poses .

st robot pose

ut contorl

Step 2 Updating the Observed Landmark Estimate

zt sensor measurement

θlandmark

Step 3. Resampling

Step 3. Resampling (cont.)