Born 1930 in Hungary

Studied at MIT / Columbia

Developed filter in 1960/61
Based on Recursive Bayesian
Filter

Kalman Filter

Kalman Filters
A Kalman Filter is a more
sophisticated smoothing algorithm
that will actually change in real
time as the performance of Various
Sensors Change and become more
or less reliable
What we want to do is filter out
noise in our measurements and in
our sensors and Kalman Filter is

Noise
Notice it has the same
shape with some
random variations plus
and minus.

This is the actual path of our

UGV(Unmanned Ground Vehicle)
as it follows unwinding road.
Ok . How about if we
just average out our
GPS reading?

This is the data we got

from GPS.

Thats a bit better and smoothes out some of the

bumps but it is not useful since GPS is slow
already and averaging makes it much slower

Noise
Removing Noise from
Measurements by Sensor is the
purpose of Kalman Filter
When you average the sensor
signals or readings it will slow down
the system
Path of a
Robot
Data We Get from
SENSOR
Data after we average the Signals

Kalman Filter-Basic Block

Diagram
System
Error Sources
External
Controls

System

Black
Box

Sometimes the system

state and the measurement
may be two different things

System State
(desired but
not known)

Measurin
g Devices

Observed
Measurement
s

Estimato
r

Measurement
Error Sources

System state cannot be measured

directly

Optimal
Estimate of
System State

Error in GPS(Global Positioning

System)-HISTOGRAM
GPS ERRORS

If we were to
plot just the
error of GPS on
graph, we
would see a
plot like this
So the Noise is
this random
bits of stuff
here at the
bottom

We usually get GPS from manufacturers

with specification specifying some
percentage of Accuracy with in Some
meters. For example we may get 95%
accuracy with in 2 meters etc

As a review,
Histogram gets
taller the more
measurements
are at a
particular value
This is really a
Good GPS
RECEIVER
So we could make a

Most of the time GPS

is inside this error
range.
Notice that this
makes a particular
distribution of values
that fall in certain
range. Let's say that
as 4 meters

filter that would get rid

of this noise.BUT we
cant measure our own
error while we are

Erro
r

Moving Robot Estimating

Error
Heres the real path of
UGV over the ground
with boxes showing
1second interval
So what if we used
the physical motion
of the vehicle to
estimate where the
next measurement
should be and use
that for the error?

Path followed by robot

according to law of
motion

Sensor
Readings
Now heres the
GPS reading we
received at 1 Hz

Sensor Readings doesnt

follow the Law of Physics or
Equilibrium of Motion which
is
Y=x + vt + 1/2at2

Moving Robot Estimating

Error
With this sort of straight line motion the
state estimate can be very accurate as
the UGV is moving in a constant
direction and at constant speed.
So we might see something like this out
of a continuously updated estimation
function

20
mph
20
mph
Equilibrium of Motion which is
Y=x + vt + 1/2at2
X=position
V=velocity(meters per
second)
a= acceleration(meters per
second squared
T=time interval

20
mph
We also Know if we commanded the
vehicle to change course of Speed- a
steering input or throttle change

Kalman Filter Concept

We cant directly measure where the UGV(Unmanned
Ground Vehicle Robot) is- We have to use various
Sensors to make estimates.
Each of those Sensors has a Certain amount of
Accuracy and a Certain amount of NOISE.
We can use Equation of Motion to provide estimates
on where UGV may have moved and then see if Sensor
Readings makes sense given that estimate.
Then we can Update our Estimates with new Sensor
information and whole cycle starts over again.
Kalman filter is all done with matrix math

The Kalman Filter

Prior
Knowledge of
State

Pk-1|k-1
xxk-1|k-1

Next Time
step
k=k+1
Pk|k
xxk|k

Prediction
Step
Based on
Example
Physical Model
Pk|k-1
xxk|k-1
Update Step
Compare
Prediction to
Measurements

Output
Estimate of
State

Measureme
nts
yk

Set of Kalman filter Equations

in Detail
Prediction(Time Update)
1) Project the STATE
ahead
k-=Ayk-1+Buk
2) Project the Error
Covariance ahead
P-kAPk-1AT+Q

Correction (Measurement
Update)
1) Compute the KALMAN
GAIN
K=P-kHT(HP-kHT+R)-1
2) Update the estimate
with measurement zk
k=k- + K (zk-H k- )
3) Update the Error Covariance
Pk=(I-KH)P-k

Kalman Filter
1. Project the STATE ahead
k-=Ayk-1+Buk
. k is the predicted state of Vehicle at time k
. A is the model(equations of motion) that predict the new
state
. yk-1 is the state of Vehicle at previous time k-1
. B is the model that predicts what changes based on
commands to the vehicle increase in throttle or steering
. Uk is the commanded inputs at time k

Kalman Filter
2. Project the Error Covariance
ahead
. We want to predict how much noise will be in
our measurements
P-kAPk-1AT+Q
. A, same as first equation,Model of Motion
. P,the previous error value at time k-1
. AT,the model Transposed
. Q,the covariance of Error noise describes the distribution
of noise

Kalman Filter
1. Computing the KALMAN GAIN
K=P-kHT(HP-kHT+R)-1
. K, the Kalman gain(How much to trust this
sensor)
. P-k, as before, the Predicted Error Covariance
. H,the model of how Sensor readings reflect
the vehicle state a function to go from
Sensor Reading to STATE VECTOR
. R describes the noise in Sensor Measurement

Kalman Filter
Update the estimate with
measurements from Sensor
k=k- + K (zk-H k- )
k is the state at time k and the output of the filter
k- is the estimate of the state we did previously
K is the Kalman Gain
Zk is the measurements from Sensor
H is the model of how the measurements reflect the state
of the Vehicle

Kalman Filter
Update the Error Covariance
Pk=(I-KH)P-k
Pk is our new Error Covariance(description of the error
noise Gaussian Curve)
I is the Identity Matrix
K is the Kalman Gain
H is the measurement Model
P-k was the previous estimate of error noise

Problems with Kalman Filters

It is very difficult to compute the covariance
matrix of noise of various sensors and systems
It is possible to do some trial and error fitting
of error matrix to the problem to tune the
filter for performance
The filter needs to process several samples in
order to get enough iterations to produce
meaningful results You need to discard the
first 20 iterations or so when the filter first
starts.

THANK YOU!

What is a Kalman Filter?- Another Interpretation

P(H|D)=P(H) * P(D|H)
P(H)=Probability of Hypothesis
P(D)=Probability of Data
Kalman Filter is based on
P(Ht|Ht-1,Dt) => P(Ht|Ht-1,At,Dt) Where At =Action State

WALL-E
Robot

GPS

Trash

Robot Picks
up Trash
Trash

What is a Kalman Filter?

3.5

Xt

Xt-1

-Motor Command

2.5

2
Probability Where Robot Is

Position

Position Where Robot is Sent

1.5

Where GPS THINKS Robot is

Combination Where Robot Actually is

0.5

0
0

0.5

1.5

2
Time

2.5

3.5

What is a Kalman Filter?

STATE PREDICTION:
Xt=Axt-1+Bt+t
Where t is Gaussian Error. It is a Linear
Function Based on Rule of Physics.
SENSOR PREDICTION:
Zt=Cxt +t
Xest =Xt+K(Zt-t)
WHERE
Xt=Prediction
K=Kalman Gain
Zt =Actual Measurement

What is a Kalman Filter?

STATE PREDICTION MODEL:
Xt=Axt-1+Bt+t
Where t is Gaussian Error. It is a Linear
Function Based on Rule of Physics.
Position( P)
Pt=Pt-1+Vt-1.t+1/2 xatt 2Velocity (V )

Vt=Vt-1+att

pt 1 t
xt

vt 0 1

T2

pt 1

at k
2

vt 1 T

What is a Kalman Filter?

STATE PREDICTION MODEL:
Pt=Pt-1+Vt-1.t+1/2 att2
Vt=Vt-1+att

pt pt 1 Vt - 1.T 1/2 atT2

xt
k
0 vt 1 Ta t

vt

Measurement Prediction:
Zt=Cxt +t

Pt 1
Pt 1 0
t

Vt 1