Kalman Filter

•Born 1930 in Hungary•Studied at MIT / Columbia•Developed filter in 1960/61•Based on Recursive Bayesian 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 one way to do that reliably

Noise

This is the actual path of our UGV(Unmanned Ground Vehicle) as it follows unwinding road.

This is the data we got from GPS.

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

Ok . How about if we just average out our GPS reading?

That’s 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 state cannot be measured directly• Need to estimate “optimally” from measurements

Measuring Devices Estimator

MeasurementError Sources

System State (desired but not known)

External Controls

Observed Measurements

Optimal Estimate of System State

SystemError Sources

System

Black Box

Sometimes the system state and the measurement may be two different things

Error in GPS(Global Positioning System)-HISTOGRAM

GPS ERRORS• 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…

If we were to plot just the error of GPS on graph, we would see a plot like this

Error

This is really a Good GPS RECEIVER

As a review, Histogram gets taller the more measurements are at a particular value

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

4m

So the Noise is this random bits of stuff here at the bottom

So we could make a filter that would get rid of this noise.BUT we can’t measure our own error while we are moving

Moving Robot – Estimating Error

Path followed by robot according to law of motion

Sensor Readings

Sensor Readings doesn’t follow the Law of Physics or Equilibrium of Motion which is

Y=x + vt + 1/2at2

Here’s the real path of UGV over the ground with boxes showing 1second interval

Now here’s the GPS reading we received at 1 HzSo what if we used the

physical motion of the vehicle to estimate where the next measurement should be and use that for the error?

Moving Robot – Estimating Error

Equilibrium of Motion which isY=x + vt + 1/2at2

X=positionV=velocity(meters per second)a= acceleration(meters per second squaredT=time interval

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

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 function20 mph

20 mph

20 mph

Kalman Filter Concept

• We can’t 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

x̌� k-1|k-1

Prediction StepBased on Example Physical Model

Pk|k-1

x̌� k|k-1

Update StepCompare Prediction to Measurements

Measurements ykPk|k

x̌� k|k

Next Time stepk=k+1

Output Estimate of State

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-k≡APk-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 CovariancePk=(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

measurementsP-

k≡APk-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 CovariancePk=(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

Trash

WALL-E

GPS

Robot

Trash

Robot Picks up Trash

What is a Kalman Filter?

0 0.5 1 1.5 2 2.5 3 3.5 40

0.5

1

1.5

2

2.5

3

3.5

Probability Where Robot IsPosition Where Robot is SentWhere GPS THINKS Robot isCombination Where Robot Actually is

Time

Positi

on

Xt

µ-Motor Command

Xt-1

What is a Kalman Filter?STATE PREDICTION:Xt=Axt-1+Bµt+€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• Žt =Predicted Measurement

What is a Kalman Filter?STATE PREDICTION MODEL:Xt=Axt-1+Bµt+€t

Where €t is Gaussian Error. It is a Linear Function Based on Rule of Physics. Pt=Pt-1+Vt-1.t+1/2 att2

Vt=Vt-1+att

)(

)(

VVelocity

PPositionx

kt

T

t

t

t

tt a

Tv

pt

v

px

2

2

10

1

1

1

What is a Kalman Filter?STATE PREDICTION MODEL:Pt=Pt-1+Vt-1.t+1/2 att2

Vt=Vt-1+att

Measurement Prediction:Zt=Cxt +€t

k

tTav

pv

px

t

t

t

tt

1

t1-t1

0

Ta 1/2.TV 2

tt

tt

V

PP

1

10 1