Robust LocalizationKalman Filter

& LADAR Scans

State Space RepresentationContinuous State Space Model

Commonly written

Discrete State Space ModelCommonly written

2y2

1y1

1s

Integrator3

1s

Integrator2

1s

Integrator1

1s

Integrator

-K-

Gain9

C9

Gain8

C8

Gain7 C7

Gain6

C6

Gain5C5

Gain4

C4

Gain3

C3

Gain2

C2

Gain1

C1

Gain

2U2

1U1

Discrete State Space Observer or Estimator

Find L to meet your Design NeedsIf system is Observable, poles of F-LH can

be placed anywhere*. *Very fast poles amplify noise issues

Overview

• System model• Problem statement• Sensor model• State estimator• Code

A 1-Dimensional Sensor Fusion Problem

• Given two measurements of the same state x, find the “best” value to assign to x and a measure of confidence in that new x value.

• Use Normal distributions to define our measurements and “best” estimate of our states. N(mean, variance). The mean is the value for state x and variance is our trust in this value where smaller variance indicates larger trust.

1-D ExampleFor this simple example, using our robot, let’s assume that we apply

the same control effort to both motors and in doing so the robot travels in a straight line. We then can form a kinematic State Space model of the distance the robot is away from the front wall:

x is the robot’s distance from the wall, v is the robot’s velocity and q is the system uncertainty or noise. After driving the robot many times up to this front wall and collecting and analyzing the data, you find that the variance of the state estimation is 0.5. Doing a similar run of tests using the ultrasonic sensor and the LADAR you find that the variance of the ultrasonic distance measurement is 1.0 and the variance of the LADAR measurement is 0.1. Then picking one point in time of the robot’s travel to the front wall you find that the model gives you a reading of 2 and the ultrasonic sensor gives you a reading of 4 and the LADAR gives you a reading of 5.With out knowing anything about Kalman filtering, how would you “fuse” this data at this point in time?

1-D Example• First “fuse” the model’s prediction with the

Ultrasonic data and come up with a new “best” distance and value of trust.

• Second “fuse” the new “best” distance and value of trust with the LADAR data.

• Since we trust the system model twice as much as the Ultrasonic measurement how could we combine the two?

1-D Example

Doing some algebra and organizing into a nice form:

where

Show ProbExample.m in Matlab

1-D Example in Kalman Filter Form

S =

Prediction Step usually happens many more times and much faster then the Correction Step but does not have to.

Prediction Step

Correction Step Innovation

System Model

2

(2),

,

k k

k k

xSE y

v

x x

u u

System State (pose)

Control Inputs

Derivation of control inputs

tvk

tk1kx kx

System Evolution

State update equation:System

Hx z

u ++

F

B

Z-1

1,

1

1

1

1

1

1

cossin

fk k k

k k k

k k k k

k

k

k

k

xk

y

kk k

k

k

b

x v ty v t

xy

t

qqq

x x ux q

x

Objective

2

ˆ

ˆ

min [ ]k

k k k

kE

x

x x x

x

• Minimize the current expected squared error

• At all times, have a state estimate close to the true state

Dead Reckoning

Robot DeadReckoned

Path

Expected Error

2 2

2

[ ]

ˆˆ [( ) ] [( ) ][ ] [ ]0 ( )

k k

k k kk k k

k

E

E EE EVar

X X

X X XX X XX

The optimal estimate of is Though the expected error is zero, the expected variance is nonzero and increases with time

Sensor Model

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

State Estimation – Observers Without Probability –

• Often we have fewer sensors than states or sensors that do not return our state directly

Observer

Correction Factor

True System

Hx z

u ++

F

B

H++

F

Bx

– + L

z

Z-1

Z-1

y ~

+

Kalman Filter

1

1|

1 1 1

1 1 1

ˆ ˆ

ˆ ˆ

kk k k k

Tk k kk k

k k k k

k k k k

x Fx Bu

P FP F Q

x x Ky

P I - KH P

Two Steps :1. Predict the state and covariance matrix using motion model

2. Correct the state and covariance based on sensor data

whe

1 1

11

11

ˆk k k

Tkk k

Tk k

y z Hx

S HP H R

K P H S

re

Application Specific Kalman Filter

1

1|

1 1 1

1 1 1

ˆ ˆ

ˆ ˆ

kk k k k

k k kk k

k k k k

k k k k

x x Bu

P P Q

yx x Ky

P I - K P

Two Steps :1. Predict the state and covariance matrix using motion model

2. Correct the state and covariance based on sensor data

where

1 1

11

11

ˆk k k

kk k

k k

z x

S P R

K P S

Kalman Filter Video

DeadReckoned

Path

KalmanFiltered

Path

LADARMeasurements

ConfidenceEllipse

Kalman FilterBlindfolding the Robot

DeadReckoned

Path

KalmanFiltered

Path

LADARMeasurements

ConfidenceEllipse

Code Review

• Corner detection• Kalman filter

Extended Kalman Filter – Dealing With Nonlinearities

• The Kalman filter is the optimal linear estimator

• The robotic system is nonlinear– System can be linearized– We will still have the best linear

estimator at the estimated operating point

EKF algorithm