gps 9 kalman filtering

8/12/2019 Gps 9 Kalman Filtering

1/56

1

Kalman Filtering

It is an effective and versatile mathematical procedure for

combining noisy sensoroutputs to estimate the state of a

system with uncertain dynamics.

Kalman filtering is a relatively recent (1960) development in

filtering.

Kalman filtering has been applied in areas as diverse as

aerospace, tracking missiles, navigation, nuclear power

plant instrumentation, demographic modeling,

manufacturing, computer vision applications.

For Kalman filter the problem is formulated is state space

and is time varying.


2/56

2

What is Kalman filter

Some Applied Math

Noisy data Less noisy data

Delay is the price for filtering


3/56

3

Introduction

The Kalman filter is a linear, recursive estimator that

produces the minimum variance estimate in a least squares

sense under the assumption of white, Gaussian noise processes.

Consider the problem of estimating the variables of a system. In

dynamic systems (that is, systems which vary with time) the system

variables are often denoted by the term "state variables".

Since its introduction in 1960, the Kalman filter has become an

integral component in thousands of military and civilian navigation

systems.

This deceptively simple, recursive digital algorithm has been an

early-on favorite for conveniently integrating (or fusing) navigation

sensor data to achieve optimal overall system performance.


4/56

4

The Kalman filter is a multiple-input, multiple-output digital filter

that can optimally estimate, in real time, the states of a system

based on its noisy outputs.

The purpose of a Kalman filter is to estimate the state of a system

from measurements which contain random errors. An example is

estimating the positionand velocityof a satellite from radar data.

There are 3 components of position and 3 of velocity so there are atleast 6 variables to estimate. These variables are called state

variables. With 6 state variables the resulting Kalman filter is called

a 6-dimensional Kalmanfilter.

To provide current estimates of the system variables - such as

position coordinates - the filter uses statistical models to properly

weight each new measurement relative to past information.


5/56

5

Predict Correct Kalman filter operates by

Predicting the new state and its uncertainty

Correcting with the new measurement.

Predict Correct


6/56

6

Example: Two-dimensional Position Only

Process Model

kkkk

kkk

k

k

k

k

k

k

wxAx

wxAx

y

x

y

x

y

x

1

11

1

1

1

1

Or

~

~

10

01

Noise

NoseState State transition


7/56

7

Measurement Model

kv

kxH

kz

kv

ku

ky

kx

yH

xH

kv

ku

~

~

0

0

Noise

Nosemeasurement Measurement matrix


8/56

8

Preparation

CovarianceNoisetMeasuremen

0

0

CovarianceNoiseProcessyyQ0

0

TransitionState10

01

yyR

xxRTvvER

xxQ

TwwEQ

A


9/56

9

Initialization

0

0oP

ozHox


10/56

10

Predict

QTAkAPkP

kxAkx

1

1

Uncertainty

Transition


11/56

11

Correct

1

R

T

HkHP

T

HkPK

kPKHIkP

kxHkzKkxkxPredictedActual

Measurement space


12/56

12

Summary

QTAkAPkP

kxAkx

1

1

kPKHIkP

kxHkzKkxkx

RT

HkHPT

HkPK

)(

1


13/56

13

Review of State Model

noisetmeasuremen)1(

matrixoutput)(

t vectormeasuremen1)(

functionforcing)1(

matrixntransitiostate)(

at timevectorstate)(

1

mv

mmH

mz

nw

nnA

tnnx

vxHz

wxAx

k

k

k

k

k

kk

kkkk

kkkk White noise


14/56

14

The System

w

k

xk+1

z-1 xk

Hk

vk

zk

Ak

+

+ +

Measurement noise

State transition matrix


15/56

15

Kalman Predictor Update Equation

kkkkk xHzKxAxAx 1Prior estimate ofxk

system.actualthe

likemuchlooksthatsystemabeout toturnwillpredictorThe


16/56

16

The Predictor

kx

z

k

AkKz

k

Ak

-

+

H

Ak

Z-1

1kx


17/56

17

The Filter

kx

Zk+1 AkKz

k

Ak

-

+

H

Ak

Z-1

1kx


18/56

18

How Kalman Filter Works?

The Kalman filter maintains two types of variables:

Estimate State Vector: The components of the estimated statevector include the following:

The variables of interest (what we want or need to know, such asposition and velocity).

Nuisance variables that may be necessary to the estimation process.

The Kalman filter state variables for a specific application must includeall those system dynamic variables that are measurable by the sensorsused in the application.

A Covariance Matrix: a measure of estimation uncertainty. Theequations used to propagate the covariance matrix (collectivelycalled the Riccati equation) model and manage uncertainty, takinginto account how sensor noise and dynamic uncertainty contribute touncertainty about the estimated system state.


19/56

19

By maintaining an estimate of its own estimation uncertainty and the

relative uncertainty in the various sensor outputs, the Kalman filter is

able to combine all sensor information optimally in the sense thatthe resulting estimate minimizes any quadratic loss function of

estimation, including the mean-squared value of any linear

combination of static estimation errors.

The Kalman gain is the optimal weighting matrix for combining newsensor data with a prior estimate to obtain a new estimate.


20/56

20

Lecture 1: The Start

For the Kalman filter, the problem is formulated in state space.

Consider a linear system: u(t) and y(t) could be scalars or vectors.

Each element of u(t) is a white noise. We want to model y(t) as the response of a linear system, where the system

input is the unity power spectrum white noise u(t). This implies E [x(t)] = 0.

Linear System y(t)u(t)

xy

ux

B

GFx


21/56

21

Suppose we have a fourth order system

Assume n = 4, then m = n -1 = 3

Based on the information given in Chapter 3 of the State Space we can

write the matrix equation

u(t) y(t)

oansna

ns

obmsmbmsmb

..11

..11

Forward path

Feedback path


22/56

22

40

31

22

13

40

31

22

13

012

23

34 01

2

2

3

3

1

)(

sasasasa

sbsbsbsb

asasasas

bsbsbsbsG

textbooktheof3.1ExamplevRead

U(s)

1 1/s 1/s 1/s 1/s bo

Y(s)x1x2x3x4

b1b2

b3

-a0-a1-a2

-a3

43

2

1

3210)(

)(

1

0

0

0

4

3

2

1

3-2-1-0-

1000

0100

0010

4

3

2

1

43322110)(

433221104

.

43

.;32

.;21

.

x

x

x

x

bbbbty

tu

x

x

x

x

aaaax

x

x

x

dt

d

xbxbxbxbty

uxaxaxaxax

xxxxxx


23/56

23

Discrete-Time State Space Model

In the Kalman filtering it is customary to writew(k) and y(k)

We will repeat the process as before

u(t) y(t)

onsn

ns

msmmsm

..11

..11

Forward path

Feedback path


24/56

24

40

31

22

131

40

31

22

13

012

23

34

01

2

2

3

3)(

ssss

ssss

ssssssssG

textbooktheof3.1ExamplevRead

U(s)

1 1/s 1/s 1/s 1/s bo

Y(s)x1x2x3x4

b1b2

b3

-a0-a1-a2

-a3

)(4

)(3

)(2

)(1

3210)(

)(

1

0

0

0

)(4

)(3

)(2

)(1

3-2-1-0-

1000

0100

0010

)1(4

)3(3

)2(2

)1(1

43322110)(

433221104

.

43.;32

.;21.

kx

kx

kx

kx

ty

tw

kx

kx

kx

kx

kx

kx

kx

kx

dt

d

xxxxty

uxxxxx

xxxxxx


25/56

25

Writing the former equations together using matrix notation we obtain

the controllable state variable

0]kE[X

matrixgainequationOutputkB

1kXkXformatrixntransitioStatekkat timevectorStatekX

kXkBkY

kWkXk1kX


26/56

26

Development of the Discrete Kalman Filter

There should be a discrete linear system.

The input is white noise. The observations are the system output plus a white noise called the

measurement noise.

The system input noise and the measurement noise are

uncorrelated to each other.

You should know: The state space model for the system.

The second order statistics of the input noise.

The second order statistic of the measurement noise.

The problem: Given the noisy observations of the output, find

estimates of the system state vector.


27/56

27

What Makes Kalman Filter Different?

It is kind of like a mathematical proof by induction.

Assume that we have obtained a prediction for the state vector at time k and

that this estimate is based on the first k-1 observations. In other words, assume that we have an estimate of Xk given Zk-3, Zk-2, Zk-1.

This is called a priori estimate or prior of Xk: the true state vector at time k.

In books or other resources you may see it as

error.vectorstatepredictedtheiske

-kX

-kXE-keEmeanwith

)predicted-kX

true;k(X-kX

kX-ke

is-kXpredictionin theerrorthesokX

-kX

-kX

EE


28/56

28

The Predicted State Vector Error Covariance Matrix

)-kX

kHk(ZkK-kX

kX

kZkK-kX

kHkK-IkX

T-kX

kX

-kX

-kXET

ke-keE

-kP


29/56

29

Lecture 2: State and Covariance Correction

The Kalman filter is a two-step process: prediction and

correction. The filter can start with either step but will begin by describing the

correction step first.

The correction step makes corrections to an estimate, based onnew information obtained from sensor measurements.

The Kalman gain matrixKis the crown jewel of Kalman filter. Allthe efforts of solving the matrix is for the sole purpose ofcomputing the optimal value of the gain materix K used forcorrection of an estimate x .

noisetmeasuremenonBased

)()(x gain

Hxz

)(xHzKx


30/56

30

There are two basic processes that are modeled by a Kalmanfilter. The first process

is a model describing how the errorstate vector changes in time. This model is the

system dynamics model. The second model defines the relationshipbetween the

error state vector and any measurements processed by the filter and is the

measurement model.

Time Update Measurement Update(Correct)


31/56

31

Intuitively, the Kalman filter sorts out information and

weights the relative contributions of the measurements and of

the dynamic behavior of the state vector.

The measurements and state vector are weighted by their

respective covariance matrices. If the measurements are inaccurate

(large variances) when compared to the state vector estimate, thenthe filterwill deweight the measurements.

On the other hand, if the measurements are very accurate (small

variances) when compared

to the state estimate, then the filter will tend to weight themeasurements heavily with the consequence that its previously

computed state estimate will contribute little to the latest

state estimate.


32/56

32

Gaussian Probability Density Function

PDFs are nonnegative integrable functions whose integral equals

unity. The density function of Gaussian probability distributions havethe form given. Where nis the dimension of P(nnmatrix), is themean of the distribution. The parameter Pis the covariance matrix of

the distribution.

])[2

1exp(

det)2(

1)( 1

xxxp

T

nP

P


33/56

Th f K l filt i t ti ll ti t th l f i bl d ibi


34/56

34

The purpose of a Kalman filter is to optimally estimate the values of variables describing

the state of a system from a multidimensional signal contaminated by noise

System: Unknown multiple state variables

+

Multiple

noiseinputs

Multiple noises

Sampled multiple output

Multidimensional signal plus noise

Kalman filter Multiple state

Variable estimates

Multiply noisy outputs


35/56

35

The following figure illustrates the Kalman filter algorithm itself.

Because the state (or signal) is typically a vector of scalar random

variables (rather than a single variable), the state uncertainty

estimate is a variance-covariance matrix-or simply, covariancematrix. Each diagonal term of the matrix is the variance of a scalar

random variable-a description of its uncertainty. The term is the

variable's mean squared deviation from its mean, and its square root

is its standard deviation. The matrix's off-diagonal terms are the

covariances that describe any correlation between pairs of variables.

The multiple measurements (at each time point) are also vectors

that a recursive algorithm processes sequentially in time. This

means that the algorithm iteratively repeats itself for each new

measurement vector, using only values stored from the previouscycle. This procedure distinguishes itself from batch-processing

algorithms, which must save all past measurements.


36/56

36

Starting with an initial predicted state estimate (as shown in thefigure) and its associated covariance obtained from past information,the filter calculates the weights to be used when combining thisestimate with the first measurement vector to obtain an updated"best" estimate. If the measurement noise covariance is muchsmaller than that of the predicted state estimate, the measurement'sweight will be high and the predicted state estimate's will be low.

Also, the relative weighting between the scalar states will be a

function of how "observable" they are in the measurement. Readilyvisible states in the measurement will receive the higher weights.Because the filter calculates an updated state estimate using thenew measurement, the state estimate covariance must also bechanged to reflect the information just added, resulting in a reduceduncertainty. The updated state estimates and their associated

covariances form the Kalman filter outputs.


37/56

37

Finally, to prepare for the next measurement vector, the filter mustproject the updated state estimate and its associated covariance tothe next measurement time.

The actual system state vector is assumed to change with timeaccording to a deterministic linear transformation plus anindependent random noise.

Therefore, the predicted state estimate follows only the deterministictransformation, because the actual noise value is unknown. Thecovariance prediction ac-counts for both, because the randomnoise's uncertainty is known.

Therefore, the prediction uncertainty will increase, as the stateestimate prediction cannot account for the added random noise.This last step completes the Kalman filter's cycle.


38/56

38

Compute weights from predicted states

Covariance and measurement noise covariance

Compute new covariance of updated state estimates

Predict state estimates and

Covariance to next time step

Update state estimates as

Weighted linear blend of predictedstate estimates & new measurement

Updated state estimates

New measurements each

cycle

Predictedinitial state

Estimate and covariance


39/56

39

Mathematical Definitions The varianceand the closely-related standard deviation

are measures of how spread out a distribution is. It is a

measure of estimation quality. The covarianceis a statistical measure of correlation of

the fluctuations of two different quantities. Intuitively,covariance is the measure of how much two variablesvary together.

Least squaresis a mathematical optimization techniquewhich, when given a series of measured data, attemptsto find a function which closely approximates the data (a"best fit"). It attempts to minimize the sum of the squaresof the ordinate differences (called residuals) betweenpoints generated by the function and correspondingpoints in the data. It is sometimes called least meansquares.


40/56

40

Simple Example of Process Model

A simple hypothetical example may help clarify the

Kalman concepts. Consider the problem of determining

the actual resistance of a nominal 100-ohm resistor by

making repeated ohmmeter measurements and

processing them in a Kalman filter.

First, one must determine the appropriate statistical

models of the state and measurement processes so that

the filter can compute the proper Kalman weights (or

gains). Here, only one state variable, the resistance, x is

unknown but assumed to be constant. So the state

process dynamics evolves with time as

Xk+1 = Xk. [1]


41/56

41

Note that no random noise corrupts the state process as it evolves

with time. The color code on a resistor indicates its precision, or

tolerance, from which one can deduce assuming that the populationof resistors has a Gaussian or normal histogram that the uncertainty

(variance) of the 100-ohm value is, say, (2 ohm)2. So our best

estimate of x, with no measurements, is x0/ = 100 with an

uncertainty of P0/= 4. Repeated ohmmeter measurement,

zk= xk+ vk [2] directly yield the resistance value with some measurement noise, vk

(measurement errors from turn-on to turn-on are assumed

uncorrelated). The ohmmeter manufacturer indicates the

measurement noise uncertainty to be Rk = (1 ohm)2 with an averagevalue of zero about the true resistance.

H i W k ?


42/56

42

How it Works?

Estimated State Vectorincluding the variables of interest, nuisance

variables, and the Kalman filter state variables for a specific

application.

A Covariance Matrix: A measure of estimation uncertainty. The

equations used to propagate the covariance matrix (called Riccatti

equation) model and manage uncertainty, taking into account how

sensor noise and dynamic uncertainty contribute to uncertaintyabout the estimated system state.

Kalman filter is able to combine all sensor information optimallyin

the sense that the resulting estimate minimizes any quadratic loss

function of estimation error.

The Kalman gain is the optimal weighting matrix for combining new

sensor data with a prior estimate to obtain a new estimate.


43/56

43

For our purpose in ELG4152

The noisy sensors may include speed sensors (wheel speeds of

land vehicles, water speed sensors for ships, air speed sensors for

aircraft, GPS receivers and inertia sensors, and time sensors.

The system state may include the position, velocity, acceleration,

attitude, and attitude rate of a vehicle on land, at sea, in the air, or in

the space.

Uncertain dynamics may include unpredictable disturbances of the

host vehicle, whether caused by a human operator or by the

medium (winds, surface currents, turns in the road, or terrain

changes). It might include also unpredictable changes in the sensorparameters.


44/56

44

The one dimensional Kalman Filter

Suppose we have a random variable x(t) whose value we want to

estimate at certain times t0,t1, t2, t3, etc. Also, suppose we know thatx(tk) satisfies a linear dynamic equation.

x(tk+1) = Fx(tk) + u(k) (the dynamic equation)

In the above equation Fis state transition matrix (in this example a

known number) that relates state at time step tkto time step tk+1. In

order to work through a numerical example let us assume F= 0.9.

Kalman assumed that u(k) is a random number. Suppose the

numbers are such that the meanof u(k) = 0 and the varianceof u(k)

is Q. For our numerical example, we will take Qto be 100.

u(k) is called white noise, which means it is not correlated with anyother random variables and most especially not correlated with past

values of u.


45/56

45

In later lessons we will extend the Kalman filter to cases where the

dynamic equation is not linear and where u is not white noise. But

for this lesson, the dynamic equation is linear and w is white noise

with zero mean.

Now suppose that at time t0someone came along and told you he

thought x(t0) = 1000 but that he might be in error and he thinks the

variance of his error is equal to P. Suppose that you had a great

deal of confidence in this person and were, therefore, convinced thatthis was the best possible estimate of x(t0). This is the initial

estimate of x. It is sometimes called the a priori estimate.

A Kalman filter needs an initial estimate to get started. It is like an

automobile engine that needs a starter motor to get going. Once it

gets going it does not need the starter motor anymore. Same with

the Kalman filter. It needs an initial estimate to get going. Then it will

not need any more estimates from outside.


46/56

46

We have an estimate of x(t0),which we will call xe. For our example

xe= 1000. The variance of the error in this estimate is defined by P

= E[(x(t0) -xe)2].

where Eis the expected value operator. x(t0) is the actual value of x

at time t0 and xe is our best estimate of x. Thus the term in the

parentheses is the error in our estimate. For the numerical example,

we will take P = 40,000.

Now we would like to estimate x(t1). Remember that the firstequation we wrote (the dynamic equation) was

x(tk+1) = Fx(tk) + u(k).

Therefore, for k = 0 we have x(t1) = Fx(t0) + u(0).

Dr. Kalman says our new best estimate of x(t1) is given by New xe= Fxe(Eq. 1)or in our numerical example 900.


47/56

47

We have no way of estimating u(0) except to use its mean value of

zero. How about Fx(t0). If our initial estimate of x(t0) = 1000 was

correct then Fx(t0) would be 900. If our initial estimate was high,then our new estimate will be high but we have no way of knowing

whether our initial estimate was high or low (if we had some way of

knowing that it was high than we would have reduced it). So 900 is

the best estimate we can make. What is the variance of the error of

this estimate? NewP= E [(x(t1)new xe)

2]

Substitute the above equations in for x(t1) and new xe and you get

New P= E [(Fx(t0) + u - Fxe)2]

= E [F2

(x(t0) - xe)2

] + E u2

+ 2F E (x(t0)- xe)*u] The last term is zero because u is assumed to be uncorrelated with

x(t0) and xe.


48/56

48

So, we are left with

New P = PF2+ Q (Eq. 2)

For our example, we have New P = 40,000 X .81 + 100 = 32,500

Now, let us assume we make a noisy measurement of x. Call the

measurement y and assume y is related to x by a linear equation.

(Kalman assumed that all the equations of the system are linear.

This is called linear system theory. y(1) = Mx(t1) + w(1)

where w is white noise. We will call the variance of w, "R".

M is some number whose value we know. We will use for our

numerical example M = 1 , R = 10,000 and y(1) = 1200

Notice that if we wanted to estimate y(1) before we look at the

measured value we would use


49/56

49

ye = M*new xe

for our numerical example we would have ye = 900

Dr. Kalman says the new best estimate of x(t1) is given by

Newer xe = new xe + K*(y(1) - M*new xe)

= new xe + K*(y(1) - ye) (Eq. 3)

where K is a number called the Kalman gain.

Notice that y(1) - ye is just our error in estimating y(1). For our

example, this error is equal to plus 300. Part of this is due to thenoise, w and part to our error in estimating x.

If all the error were due to our error in estimating x, then we would

be convinced that new xe was low by 300. Setting K = 1 would

correct our estimate by the full 300. But since some of this error is

due to w, we will make a correction of less than 300 to come up withnewer xe. We will set K to some number less than one.

What value of K should we use? Before we decide let us compute


50/56

50

What value of K should we use? Before we decide, let us compute

the variance of the resulting error

E (x(t1)newer xe)2= E [xnew xe - K(y - M new xe)]2

= E [(xnew xe - K(Mx + w - M new xe)]

2

= E [{(1 - KM) (xnew xe)2+Kw}]2

= new P(1 - KM)2+ RK2

The cross product terms dropped out because w is assumed

uncorrelated with x and new xe. The newer value of the variance is

Newer P = new P (1 - KM)2+ R(K2) (Eq. 5)

If we want to minimize the estimation error we should minimize

newer P. We do that by differentiating newer P with respect to K and

setting the derivative equal to zero and then solving for K. A little

algebra shows that the optimal K is given by K = M new P/ [new P(M2) + R] (Eq. 4)

For our example, K = .7647 ; Newer xe = 1129; newer P = 7647

Notice that the variance of our estimation error is decreasing.


51/56

51

These are the five equations of the Kalman filter. At time t2, we start

again using newer xe to be the value of xe to insert in equation 1

and newer P as the value of P in equation 2.

Then we calculate K from equation 4 and use that along with the

new measurement, y(2), in equation 3 to get another estimate of x

and we use equation 5 to get the corresponding P. And this goes on

computer cycle after computer cycle.

In the multi-dimensional Kalman filter, x is a column matrix withmany components. For example if we were determining the orbit of

a satellite, x would have 3 components corresponding to the position

of the satellite and 3 more corresponding to the velocity plus other

components corresponding to other random variables.

Equations 1 through 5 would become matrix equations and thesimplicity and intuitive logic of the Kalman filter becomes obscured.


52/56

52

Kalman Filter Applications: GPS/Automobile

Kalman Filter

Display

Map Match

Roadmap

Database

DGPS

Receiver

DGPS

Reference

Station

Magnetic

Compass

Wheel

Speed

Sensor

Radio

Modem

Receiver

Radio

Modem

Trans

Can be added to the system state vector

System state vector

could include 10 state

Variables: 3 position;

Three velocity; receiverClock bias;

wheel speed;

Magnetic Compass.

i

i

G S/


53/56

53

GPS/Land Vehicle Integration

State Vector:10 state variables

Measurement Vector:Inputs to the Kalman filter include the following:

Pseudoranges ito each satellite being tracked by the receiver. Integrated Doppler components i for each of the satellites.

From the wheel speed sensor: indicated vehicle speed.

From the magnetic compass: magnetic heading angle.

.variablesstatetheofvaluesestimatedat theevaluatedhofderivative

partialtheisvariablesfor thesematrixysensitivittmeasuremenThe

offset.factorscalesensorspeedwheelS

output;sensorspeedwheelbias;outputcompassmagneticangle;outputcompassmagnetic

noisesensorvcity;north veloity;east veloc

v)-cos()(1

)-sin()(1

v,,,h

wheelspeed

wheel

wheelwheelspeed

wheelwheelspeed

wheelspeedwheel

S

vv

SS

SS

SSv

v

NE

N

E

GPS / INS I t ti


54/56

54

GPS / INS Integration

Kalman filter provides a simple algorithm that can easily lend itself to

integrated systems and requires only adequate statistical models of

the state variables and associated noises for its optimalperformance.

Integrating GPS with an inertial navigation system (INS) and a

Kalman filter provides improved overall navigation performance.

Essentially, the INS supplies virtually noiseless outputs that slowlydrift off with time. GPS has minimal drift but much more noise. The

Kalman filter, using statistical models of both systems, can take

advantage of their different error characteristic to optimally minimize

their deterious traits.


55/56

C St d


56/56

56

Case Study

Write an article (case study) describing a consideration or

application based on Kalman filter. You may make use of the control

tool box of MATLAB.

gps 9 kalman filtering

Documents