The “ ” Paige in Kalman Filtering K. E. Schubert.
-
Upload
stewart-powell -
Category
Documents
-
view
213 -
download
0
Transcript of The “ ” Paige in Kalman Filtering K. E. Schubert.
![Page 1: The “ ” Paige in Kalman Filtering K. E. Schubert.](https://reader035.fdocuments.us/reader035/viewer/2022070401/56649f165503460f94c2cdf1/html5/thumbnails/1.jpg)
The “” Paige in Kalman Filtering
K. E. Schubert
![Page 2: The “ ” Paige in Kalman Filtering K. E. Schubert.](https://reader035.fdocuments.us/reader035/viewer/2022070401/56649f165503460f94c2cdf1/html5/thumbnails/2.jpg)
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.)
![Page 3: The “ ” Paige in Kalman Filtering K. E. Schubert.](https://reader035.fdocuments.us/reader035/viewer/2022070401/56649f165503460f94c2cdf1/html5/thumbnails/3.jpg)
Why Filtering?
State (xi) is not directly known
Must observe through minimum measurements
Observer Equation
kkkkkk vuDxCy Want to reconstruct the state vector
![Page 4: The “ ” Paige in Kalman Filtering K. E. Schubert.](https://reader035.fdocuments.us/reader035/viewer/2022070401/56649f165503460f94c2cdf1/html5/thumbnails/4.jpg)
Random Variables
Process and observation noise
Independent, white Gaussian noise
v
w
RNvp
RNwp
,0
,0
~
~
2,~ Nxp
y=ax+b
22,~ aa bNyp
![Page 5: The “ ” Paige in Kalman Filtering K. E. Schubert.](https://reader035.fdocuments.us/reader035/viewer/2022070401/56649f165503460f94c2cdf1/html5/thumbnails/5.jpg)
Complete Problem
Control and estimation are independent
Concerned only with observer
Obtain estimate:
kkkkkk
kkkkkk
vuDxCy
wuBxAx
1
kkx̂
![Page 6: The “ ” Paige in Kalman Filtering K. E. Schubert.](https://reader035.fdocuments.us/reader035/viewer/2022070401/56649f165503460f94c2cdf1/html5/thumbnails/6.jpg)
Predictor-Corrector
1ˆ kkx
Measurements
Predict(Time Update)
Correct(Measurement Update)
kkx̂
![Page 7: The “ ” Paige in Kalman Filtering K. E. Schubert.](https://reader035.fdocuments.us/reader035/viewer/2022070401/56649f165503460f94c2cdf1/html5/thumbnails/7.jpg)
To Err Is Kalman!
How accurate is the estimate?
kkkkk
kkkkk
xxe
xxe
ˆ
ˆ11
What is its distribution?
T
kkkkkk
Tkkkkkk
eeEP
eeEP
111
![Page 8: The “ ” Paige in Kalman Filtering K. E. Schubert.](https://reader035.fdocuments.us/reader035/viewer/2022070401/56649f165503460f94c2cdf1/html5/thumbnails/8.jpg)
Predictor-Corrector
11ˆ kkkk Px
Measurements
Predict(Time Update)
Correct(Measurement Update)
kkkk Px̂
![Page 9: The “ ” Paige in Kalman Filtering K. E. Schubert.](https://reader035.fdocuments.us/reader035/viewer/2022070401/56649f165503460f94c2cdf1/html5/thumbnails/9.jpg)
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
![Page 10: The “ ” Paige in Kalman Filtering K. E. Schubert.](https://reader035.fdocuments.us/reader035/viewer/2022070401/56649f165503460f94c2cdf1/html5/thumbnails/10.jpg)
Correct
kkkkkk Kxx 1ˆˆ
Kalman Gain
1
11
vTkkkk
Tkkkk RCPCCPK
1 kkkkkk PCKIP
Innovations (What’s New)
1ˆ kkkkk xCy
Oblique Projection
![Page 11: The “ ” Paige in Kalman Filtering K. E. Schubert.](https://reader035.fdocuments.us/reader035/viewer/2022070401/56649f165503460f94c2cdf1/html5/thumbnails/11.jpg)
System 1 (Basic Example)
X 2,
Companion Form
Nice but not perfect numerics and stability
01
9.1.
10
c
A
![Page 12: The “ ” Paige in Kalman Filtering K. E. Schubert.](https://reader035.fdocuments.us/reader035/viewer/2022070401/56649f165503460f94c2cdf1/html5/thumbnails/12.jpg)
System 1
![Page 13: The “ ” Paige in Kalman Filtering K. E. Schubert.](https://reader035.fdocuments.us/reader035/viewer/2022070401/56649f165503460f94c2cdf1/html5/thumbnails/13.jpg)
System 1
![Page 14: The “ ” Paige in Kalman Filtering K. E. Schubert.](https://reader035.fdocuments.us/reader035/viewer/2022070401/56649f165503460f94c2cdf1/html5/thumbnails/14.jpg)
System 1
![Page 15: The “ ” Paige in Kalman Filtering K. E. Schubert.](https://reader035.fdocuments.us/reader035/viewer/2022070401/56649f165503460f94c2cdf1/html5/thumbnails/15.jpg)
System 1
![Page 16: The “ ” Paige in Kalman Filtering K. E. Schubert.](https://reader035.fdocuments.us/reader035/viewer/2022070401/56649f165503460f94c2cdf1/html5/thumbnails/16.jpg)
System 1 (Again)
X 2,
Companion Form
Nice but not perfect numerics and stability
01
9.1.
10
c
A
![Page 17: The “ ” Paige in Kalman Filtering K. E. Schubert.](https://reader035.fdocuments.us/reader035/viewer/2022070401/56649f165503460f94c2cdf1/html5/thumbnails/17.jpg)
System 1
![Page 18: The “ ” Paige in Kalman Filtering K. E. Schubert.](https://reader035.fdocuments.us/reader035/viewer/2022070401/56649f165503460f94c2cdf1/html5/thumbnails/18.jpg)
System 1
![Page 19: The “ ” Paige in Kalman Filtering K. E. Schubert.](https://reader035.fdocuments.us/reader035/viewer/2022070401/56649f165503460f94c2cdf1/html5/thumbnails/19.jpg)
System 1
![Page 20: The “ ” Paige in Kalman Filtering K. E. Schubert.](https://reader035.fdocuments.us/reader035/viewer/2022070401/56649f165503460f94c2cdf1/html5/thumbnails/20.jpg)
System 1
![Page 21: The “ ” Paige in Kalman Filtering K. E. Schubert.](https://reader035.fdocuments.us/reader035/viewer/2022070401/56649f165503460f94c2cdf1/html5/thumbnails/21.jpg)
System 2 (Stiffness)
X 2,
Large Eigenvalue Spread
Condition number around 109
Large sampling time (big steps)
01
98.000000001.
10
c
A
![Page 22: The “ ” Paige in Kalman Filtering K. E. Schubert.](https://reader035.fdocuments.us/reader035/viewer/2022070401/56649f165503460f94c2cdf1/html5/thumbnails/22.jpg)
System 2
![Page 23: The “ ” Paige in Kalman Filtering K. E. Schubert.](https://reader035.fdocuments.us/reader035/viewer/2022070401/56649f165503460f94c2cdf1/html5/thumbnails/23.jpg)
System 2
![Page 24: The “ ” Paige in Kalman Filtering K. E. Schubert.](https://reader035.fdocuments.us/reader035/viewer/2022070401/56649f165503460f94c2cdf1/html5/thumbnails/24.jpg)
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
![Page 25: The “ ” Paige in Kalman Filtering K. E. Schubert.](https://reader035.fdocuments.us/reader035/viewer/2022070401/56649f165503460f94c2cdf1/html5/thumbnails/25.jpg)
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
![Page 26: The “ ” Paige in Kalman Filtering K. E. Schubert.](https://reader035.fdocuments.us/reader035/viewer/2022070401/56649f165503460f94c2cdf1/html5/thumbnails/26.jpg)
Our Square Roots
fTw
fww
ifv
ifTvv
Tkkkk
kTkkk
RRR
RRR
SSP
UUP
1
1
1
![Page 27: The “ ” Paige in Kalman Filtering K. E. Schubert.](https://reader035.fdocuments.us/reader035/viewer/2022070401/56649f165503460f94c2cdf1/html5/thumbnails/27.jpg)
State Error
IuuxSxS
IuuSxx
IuuSe
SSPe
kkkkkkk
kkkkkk
kkkkk
Tkkkkkk
,0~,ˆ
,0~,ˆ
,0~,
,0,0~
11/
1
1/
1
1/1
![Page 28: The “ ” Paige in Kalman Filtering K. E. Schubert.](https://reader035.fdocuments.us/reader035/viewer/2022070401/56649f165503460f94c2cdf1/html5/thumbnails/28.jpg)
Observations
kkk
ifvk
ifv
kifvk
vxCRyR
IvRv~
,0~~
![Page 29: The “ ” Paige in Kalman Filtering K. E. Schubert.](https://reader035.fdocuments.us/reader035/viewer/2022070401/56649f165503460f94c2cdf1/html5/thumbnails/29.jpg)
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
![Page 30: The “ ” Paige in Kalman Filtering K. E. Schubert.](https://reader035.fdocuments.us/reader035/viewer/2022070401/56649f165503460f94c2cdf1/html5/thumbnails/30.jpg)
Measurement Update
Then, by definition
kkk
k
k
kkk
ifv
kk
kifv
kkkk
xU
r
b
xCR
ST
yR
xST
0
11/
1
![Page 31: The “ ” Paige in Kalman Filtering K. E. Schubert.](https://reader035.fdocuments.us/reader035/viewer/2022070401/56649f165503460f94c2cdf1/html5/thumbnails/31.jpg)
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
![Page 32: The “ ” Paige in Kalman Filtering K. E. Schubert.](https://reader035.fdocuments.us/reader035/viewer/2022070401/56649f165503460f94c2cdf1/html5/thumbnails/32.jpg)
Error Part 2
k
fwkkkkkk
kkf
wk
kkkkkkkkkk
kkkkk
kkkkk
wRuUAxx
IwwRw
wxxAuUAxA
uUxx
uxxU
~~ˆ
,0~~,~
~ˆ
~ˆ
~ˆ
111
11
1
![Page 33: The “ ” Paige in Kalman Filtering K. E. Schubert.](https://reader035.fdocuments.us/reader035/viewer/2022070401/56649f165503460f94c2cdf1/html5/thumbnails/33.jpg)
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
![Page 34: The “ ” Paige in Kalman Filtering K. E. Schubert.](https://reader035.fdocuments.us/reader035/viewer/2022070401/56649f165503460f94c2cdf1/html5/thumbnails/34.jpg)
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
![Page 35: The “ ” Paige in Kalman Filtering K. E. Schubert.](https://reader035.fdocuments.us/reader035/viewer/2022070401/56649f165503460f94c2cdf1/html5/thumbnails/35.jpg)
System 3 (Fun Problem)
X 20,
Known difficult matrix that was scaled to be stable
00125
200
1
125
2
0125
1
c
A
![Page 36: The “ ” Paige in Kalman Filtering K. E. Schubert.](https://reader035.fdocuments.us/reader035/viewer/2022070401/56649f165503460f94c2cdf1/html5/thumbnails/36.jpg)
System 3
![Page 37: The “ ” Paige in Kalman Filtering K. E. Schubert.](https://reader035.fdocuments.us/reader035/viewer/2022070401/56649f165503460f94c2cdf1/html5/thumbnails/37.jpg)
System 3
![Page 38: The “ ” Paige in Kalman Filtering K. E. Schubert.](https://reader035.fdocuments.us/reader035/viewer/2022070401/56649f165503460f94c2cdf1/html5/thumbnails/38.jpg)
System 3
![Page 39: The “ ” Paige in Kalman Filtering K. E. Schubert.](https://reader035.fdocuments.us/reader035/viewer/2022070401/56649f165503460f94c2cdf1/html5/thumbnails/39.jpg)
System 3
![Page 40: The “ ” Paige in Kalman Filtering K. E. Schubert.](https://reader035.fdocuments.us/reader035/viewer/2022070401/56649f165503460f94c2cdf1/html5/thumbnails/40.jpg)
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