The “” Paige in Kalman Filtering

K. E. Schubert

Kalman’s Interest

State Space (Matrix Representation)

Discrete Time (difference equations)

kkkkkk wuBxAx 1

Optimal Control

Starting at x0 Go to xG

Minimize or maximize some quantity (time, energy, etc.)

Why Filtering?

State (xi) is not directly known

Must observe through minimum measurements

Observer Equation

kkkkkk vuDxCy Want to reconstruct the state vector

Random Variables

Process and observation noise

Independent, white Gaussian noise

v

w

RNvp

RNwp

,0

,0

~

~

2,~ Nxp

y=ax+b

22,~ aa bNyp

Complete Problem

Control and estimation are independent

Concerned only with observer

Obtain estimate:

kkkkkk

kkkkkk

vuDxCy

wuBxAx

1

kkx̂

Predictor-Corrector

1ˆ kkx

Measurements

Predict(Time Update)

Correct(Measurement Update)

kkx̂

To Err Is Kalman!

How accurate is the estimate?

kkkkk

kkkkk

xxe

xxe

ˆ

ˆ11

What is its distribution?

T

kkkkkk

Tkkkkkk

eeEP

eeEP

111

Predictor-Corrector

11ˆ kkkk Px

Measurements

Predict(Time Update)

Correct(Measurement Update)

kkkk Px̂

Predict

wTkkkkkk

kkkkk

RAPAP

xAx

1

1ˆˆ

No random variableYou don’t know it

Eigenvalues must be <1(For convergence)

Distribution does effect error covariance

wTkkkkkk

kkkkkk

RAPAP

weAe

1

1

Correct

kkkkkk Kxx 1ˆˆ

Kalman Gain

1

11

vTkkkk

Tkkkk RCPCCPK

1 kkkkkk PCKIP

Innovations (What’s New)

1ˆ kkkkk xCy

Oblique Projection

System 1 (Basic Example)

X 2,

Companion Form

Nice but not perfect numerics and stability

01

9.1.

10

c

A

System 1

System 1

System 1

System 1

System 1 (Again)

X 2,

Companion Form

Nice but not perfect numerics and stability

01

9.1.

10

c

A

System 1

System 1

System 1

System 1

System 2 (Stiffness)

X 2,

Large Eigenvalue Spread

Condition number around 109

Large sampling time (big steps)

01

98.000000001.

10

c

A

System 2

System 2

Trouble in Paradise

Inversion in the Kalman gain is slow and generally not stable

A is usually in companion formnumerically unstable (Laub)

Covariance are symmetric positive definiteCalculation cause P to become unsymmetric then lose positivity

n

knktikit

iiin

xax

aaa

I

0,,1

,0,1,

0

Square Root Filters

Kailath suggested propegating the square root rather than the whole covariance

Not really square root, actually Choleski Factor

rTr=R

Use on Rw, Rv, P

Our Square Roots

fTw

fww

ifv

ifTvv

Tkkkk

kTkkk

RRR

RRR

SSP

UUP

1

1

1

State Error

IuuxSxS

IuuSxx

IuuSe

SSPe

kkkkkkk

kkkkkk

kkkkk

Tkkkkkk

,0~,ˆ

,0~,ˆ

,0~,

,0,0~

11/

1

1/

1

1/1

Observations

kkk

ifvk

ifv

kifvk

vxCRyR

IvRv~

,0~~

Measurement Equation

Iv

uT

r

u

r

ux

U

r

b

Iv

u

v

uTx

CR

ST

yR

xST

k

kk

k

k

k

kk

k

k

k

k

k

k

kkk

kifv

kk

kifv

kkkk

,0~~

~,

~

0

,0~~,~

11/

1

Iv

u

v

ux

CR

S

yR

xS

k

k

k

kk

kifv

k

kifv

kkk ,0~~,~

11/

1

Measurement Update

Then, by definition

kkk

k

k

kkk

ifv

kk

kifv

kkkk

xU

r

b

xCR

ST

yR

xST

0

11/

1

Updating for Free?

U k xk k

bk

Uk xk k

Uk xk ˜ u k

U k xk k

xk ˜ u k

UkP

k kU

k

T I Pk k

1 Uk

TUk

Error Part 2

k

fwkkkkkk

kkf

wk

kkkkkkkkkk

kkkkk

kkkkk

wRuUAxx

IwwRw

wxxAuUAxA

uUxx

uxxU

~~ˆ

,0~~,~

~ˆ

~ˆ

~ˆ

111

11

1

Time Updating

1111

1111

111

11

11

111

~0

~

~~~

,0~~

~,~

~

~~

kkkkk

kkkkk

k

kkkk

k

kTkk

fwkkkk

k

k

k

kfwkkkk

kf

wkkkkkk

uSxx

uSxx

u

rSx

w

uTTRUAx

Iw

u

w

uRUAx

wRuUAxx

Paige’s Filter

kkk

kkkkk

kkf

wkk

PfactorS

xAx

STRUA

11

1

11 0

~

1

1/11

0

kkk

kkkk

k

kk

kifk

kkk

kifk

kk

PfactorU

bxU

r

bU

yR

xS

CR

ST

System 3 (Fun Problem)

X 20,

Known difficult matrix that was scaled to be stable

00125

200

1

125

2

0125

1

c

A

System 3

System 3

System 3

System 3

Conclusions

Called Paige’s filter but really Paige and Saunders developed

O(n3) and about 60% faster than regular square root

Current interests: faster, special structures, robustness