Date post: | 04-Jan-2016 |
Category: |
Documents |
Upload: | stewart-powell |
View: | 213 times |
Download: | 0 times |
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