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

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 z1z1 z2z2 z3z3 z4z4 z5z5 x 1, σ 1 z6z6 x 2, σ 2 z7z7 x 3, σ 3

Extend to System Model x = Hθ+w y = θ

Estimate from Two Distributions If x and y are distributed according to Gaussian PDF with [E(x) E(y)] T And covariance matrix

Extend to System Model

z1z1 z2z2 z3z3 z4z4 z5z5 x 1, σ 1 F z6z6 x 2, σ 2 F z7z7 x 3, σ 3

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

Prediction to Correction

System Model  F k state transition model  w k is the process noise which is assumed to be drawn from a zero mean multivariate normal distribution with covariance Q k  Observation model:  v k is the observation noise which is assumed to be zero mean Gaussian white noise with covariance R k

System Model z1z1 z2z2 z3z3 z4z4 z5z5 x 1, σ 1 F z6z6 x 2, σ 2 F z7z7 x 3, σ 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

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  F k state transition model  w k is the process noise which is assumed to be drawn from a zero mean multivariate normal distribution with covariance Q k  Observation model:  v k is the observation noise which is assumed to be zero mean Gaussian white noise with covariance R k

The Matrix The system state: x  x r, y r, theta r for robot  x 1,y 1 …x n, y n position of each landmark.

The Matrix Covariance Matrix P 3x33x2 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

Prediction model : A

The Matrix The SLAM specific Jacobians

Step 1: Update current state using the odometry data Update current state using odometry data P rr 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)

The Matrix Process noise Measure noise  c, d represent the accuracy of measure device =

Step 3: Add New Landmarks to Current State X = [X x N y N ] 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 ] s t robot post at time t s t = s 1, s 2, s 3 …s t z t, n t : measurement feature n at time t u t : control of vehicle

Fast SLAM Algorithm z t depend only on s t, n t, θ n t

Particle Filter in FastSLAM

Step 1. Extend the Path Posterior by Sampling New Poses. s t robot pose u t contorl

Step 2 Updating the Observed Landmark Estimate z t sensor measurement θlandmark

Step 3. Resampling

Step 3. Resampling (cont.)