View
215
Download
0
Tags:
Embed Size (px)
Citation preview
AIAA GNC, 11 Aug. 2009
Mark L. Psiaki,
Sibley School of Mechanical & Aerospace EngineeringCornell University
Kalman Filtering & Smoothing to Estimate Real-Valued States & Integer Constants
GNC/Aug. ‘09 2 of 21
Goal Improve estimation algorithms for systems that have
integer measurement ambiguities CDGPS with double-differenced integer ambiguities Systems using carrier-phase measurements of TDMA
signals
Use SRIF/LAMBDA-type formulation to deal with mixed real/integer problem
Develop optimal & suboptimal Kalman filter & smoother algorithms Optimal: keep all ambiguities & treat as integers Suboptimal: retain integers in a finite time window
Strategies
GNC/Aug. ‘09 3 of 21
Outline of TalkI. Related research
II. Problem definition
III. Mixed real/integer Kalman filter Optimal, retains all past integers Suboptimal, retains finite window of past integers
IV. Mixed real/integer fixed-interval smoother Optimal, retains all integers of fixed interval Suboptimal, retains finite window of past & future integers
relative to each time point
V. Truth-model simulation & results
VI. Conclusions
GNC/Aug. ‘09 4 of 21
Related Research: Batch estimation w/integer ambiguities
The LAMBDA method, Teunissen (1995) & follow-ons Other methods, e.g., Chen & Lachapelle (1995) SRIF LAMBDA-like method, Psiaki & Mohiuddin (2007)
Kalman filtering w/integer ambiguities Standard Covariance EKF, Kroes et al. (2005) SRIF-based EKF, Mohiuddin & Psiaki (2008) Sub-optimal dropping of each integer ambiguity immediately
after its last occurrence in a measurement Smoothing w/integer ambiguities
Nothing
GNC/Aug. ‘09 5 of 21
Dynamics ModelReal-state dynamics:
Partitioning of integer states by affected measurement sample times (past, past & present, past, present & future):
kkkkkk ΓΦ wxx 1
k
kk n
nn 1
Growth of integer state with sample number
kk
k
kΠΔ
Δ
n
nn
n
1 Or dynamic re-partitioning
k
kk
k
k Π~
nn
nn
1
1
GNC/Aug. ‘09 6 of 21
Measurement Model
11
111
~
ykk
knkkwkkxkk HHH
nn
wxy
… using integer vector partitions
… using full integer vector
1111 ykknkkwkkxkk HHH nwxy
111 000000~
knknk ΠI
IHH
GNC/Aug. ‘09 7 of 21
Example Sensitivities of Different Measurement Types to Different Integers
0 1000 2000 3000 4000 5000 6000 7000 8000 90001
2
3
4
5
6
7
8
9
10
11x 10
-3
Time (sec)
Am
big
uity
Se
nsi
tiviti
es,
htil
de
(m
)
GNC/Aug. ‘09 8 of 21
Kalman Filtering/Smoothing Problem find: x0, …, xk+1, w0, …, wk, & nk+1 = [n0; …; nk] to minimize:
subject to: xj+1 = jxj + jwj + j for j = 0, 1, 2, ..., k nk+1 is an integer-valued vector
][][ 000T
00021
xxxxxx ˆRˆRJ zxzx
k
jjwwjjwwj RR
0
T21 ][][ ww
k
jj
jnjjwjjxj HHH
0
T1
0
121 ][{ y
n
nwx
}][ 1
0
1
j
jnjjwjjxj HHH y
n
nwx
GNC/Aug. ‘09 9 of 21
Stage-k a posterior info:
Combined information eqs. w/dynamics substitution for xk:
New stage-(k+1) a posterior info after QR factorization:
Optimal SRIF Kalman Filterxkxkkxnkkxxk RR ˆˆˆ znx
nknkknnkR ˆˆ zn
111
1
1
1
111
11
ˆˆ
ˆ
ˆˆ0
]0,ˆ[00
]0,ˆ[ˆˆ00
yk
nk
xk
wk
kkxkk
nk
kkxxkxk
k
k
k
nkkxkkkxkwk
nnk
xnkkxxkkkxxk
wwk
ΦH
ΦR
HΦHΓΦHH
R
RΦRΓΦR
R
y
zz
nxw
1
1
1
1
1
1
1
11
11
11
ˆˆˆ
ˆˆˆ
000
ˆ00
ˆˆ0
ˆˆˆ
rk
nk
xk
wk
rk
nk
xk
wk
k
k
k
nnk
xnkxxk
wnkwxkwwk
R
RR
RRR
zzzz
nxw
GNC/Aug. ‘09 10 of 21
Measurement Update via Integer Linear Least-Squares Solution Solve integer linear least-squares problem to
determine integer a posteriori estimate
Back-substitute to compute real-valued states:
]ˆˆ[]ˆˆ[ 111T
11121
nkknnknkknnk RR znzn
1ˆ
kn
]ˆˆˆ[ˆˆ 1111
11
kxnkxkxxkk RR nzx
)(min 1kJ n
GNC/Aug. ‘09 11 of 21
Suboptimal KF Retention of Exact Integers within a Window of Samples
0 1000 2000 3000 4000 5000 6000 7000 8000 90001
2
3
4
5
6
7
8
9
10
11x 10
-3
Time (sec)
Am
big
uity
Se
nsi
tiviti
es,
htil
de
(m
)
Measurements used in tk = 3000 sec
sub-optimal filter
tk = 3000 sec +0/- i*deltat window
for considering exact integers
k
k
i,k
k Δ
Δ
nn
n
m )1(max
GNC/Aug. ‘09 12 of 21
Stage-k a posterior info:
Combined information eqs. w/dynamics substitution for xk & mk
New stage-(k+1) a posterior info after QR factorization:
Suboptimal SRIF Kalman Filter xkxkkxmkkxxk RR ˆˆˆ zmx
mkmkkmmkR ˆˆ zm
111
1
1
1
111
11
ˆˆ
ˆ
ˆˆ0
0
ˆ00ˆ
ˆˆˆˆ000
yk
mk
xk
wk
kkxkk
mk
kkxxkxk
k
k
k
ik
mkkxkkkxkwk
mmbkmmak
xmbkkxxkkkxxkxmak
wwk
ΦH
ΦRΔ
HΦHΓΦHH
RR
RΦRΓΦRR
R
y
zz
mxwn
1
1
1
1
1
1
1
1
1
11
11
11
ˆˆˆˆ
ˆˆˆˆ
0000
ˆ000
ˆˆ00
ˆˆˆ0
ˆˆˆˆ
rk
mk
xk
wk
Δnk
rk
mk
xk
wk
Δnk
k
k
k
ik
mmk
xmkxxk
wmkwxkwwk
ΔnmkΔnxkΔnwkΔnΔnkΔ
R
RR
RRR
RRRR
zzzzz
mxwn
GNC/Aug. ‘09 13 of 21
Terminal sample K initialization:
1-sample backwards recursion starts w/filtered wk & smoothed xk+1 info. eqs. & uses dynamics to get
QR factorize to isolate smoothed xk info. eq.
Optimal RTS Smoother in SRIF Form
,ˆ K*K nn ,RR xxK
*xxK KxnKxK
*xK ˆRˆ nzz K
*K xx i.e.,
*xk
wk
k*xxk
*xk
*kwnkkwxkwk
k
k
k*xxkk
*xxk
kwxkkwxkwwk ˆ
R
RRˆ
RR
RRR
111
111
11
11
z
nzxw
*xk
*wk
*xk
*wk
k
k*xxk
*wxk
*wwk
R
RR
z
zxw
0 )]0 [
known(with
1*k
*k I nn
GNC/Aug. ‘09 14 of 21
Suboptimal RTS Smoother Retention of Exact Integers within a Window of Samples
0 1000 2000 3000 4000 5000 6000 7000 8000 90001
2
3
4
5
6
7
8
9
10
11x 10
-3
Time (sec)
Am
bigu
ity S
ensi
tiviti
es, h
tilde
(m
)
Additional measurements used intk = 3000 sec sub-optimal smoother
Measurements used in tk = 3000 sec
sub-optimal filter & smoother
tk = 3000 sec +/- i*deltat range
for considering exact integers
)11( ik,Kmin
k
k
k
n
nm
l
GNC/Aug. ‘09 15 of 21
Terminal sample K initialization:
1-sample backwards recursion starts w/filtered wk & nk-i & smoothed xk+1 & lk+1 info. eqs. & uses dynamics & integer permutation/partitions to get
Suboptimal RTS Smoother (1 of 2)
,RR xxK*xxK
,ˆ xK*xK zz
,RR xmK*xlK ,RR mmK
*llK
mK*lK zz
*lk
*xk
wk
Δnk
*lk
k*xxk
*xk
kwxkwk
kΔnxkΔnk
k
k
k
ik
*llak
*llbk
*xlakk
*xxkk
*xxk
*xlbk
wlkkwxkkwxkwwk
ΔnlkkΔnxkkΔnxkΔnwkˆˆ
R
Rˆ
Rˆ
RR
RRΓRR
RRΓRR
RRΓRR
1
1
1
11
1
1
11
11
11
00
0
0
z
z
z
z
lxwn
GNC/Aug. ‘09 16 of 21
Suboptimal RTS Smoother (2 of 2) New stage-k smoothed xk & lk square-root information
equations after QR factorization
is the integer vector that minimizes
The real part of the state is determined by back substitution:
*lk
*xk
*wk
*nk
*lk
*xk
*wk
*nk
k
k
k
ik
*llk
*xlk
*xxk
*wlk
*wxk
*wwk
*nlk
*nxk
*nwk
*nkn
R
RR
RRR
RRRR
z
z
z
z
lxwn
000
00
0
*kl
][][)( T *lkk
*llk
*lkk
*llkk RRJ zlzll
)()( 1 *k
*xlk
*xk
*xxk
*k RR lzx
GNC/Aug. ‘09 17 of 21
Example 1-Dimensional CDGPS-Type Problem with 3rd-Order Dynamics Dynamics:
Measurements:
kkk
k
kkk
kk
k ΔtΔt
Δt
qΔtΔtΔtΔt
wxx
01/331/3/5
00)34/(4/5
000)52/(
10010
0.5122
1
k
kk
kk
s
s
kΔtΔt
ΔtΔt
kkr
kxy
2
2
10.1255.01
0.1255.01
)/1(0000
00)/1( {1
1
3205
1921
1283
1920531
3205
1921
1283
1920531
2 }11
0000
00
yks
s
s,k
s,k
kkk
kkr
k
kkr
k
n
n
h~
h~
qΔtΔt w
GNC/Aug. ‘09 18 of 21
x1 Errors for Three Kalman Filters
0 1000 2000 3000 4000 5000 6000 7000 8000 9000
-0.1
-0.05
0
0.05
0.1
Time (sec)
x 1 fi
lter
erro
r (m
)
OptimalSuboptimal, i = 40Suboptimal, no integers
GNC/Aug. ‘09 19 of 21
x1 Errors for Three Smoothers
0 1000 2000 3000 4000 5000 6000 7000 8000 9000-0.015
-0.01
-0.005
0
0.005
0.01
0.015
Time (sec)
x 1 s
moo
ther
err
or (
m)
OptimalSuboptimal, i = 40Suboptimal, no integers
GNC/Aug. ‘09 20 of 21
Integer-Part Computational Cost of Optimal & Suboptimal Algorithms
0 1000 2000 3000 4000 5000 6000 7000 8000 90000
0.5
1
1.5
2
2.5
3
3.5
4x 10
4
Time (sec)
Cum
ulat
ive
L tot IL
LS E
xecu
tion
Cos
t Met
ric
Optimal KFSuboptimal KF, i = 40Suboptimal KF & Smoother, i = 40
GNC/Aug. ‘09 21 of 21
Summary & Conclusions Developed optimal & suboptimal Kalman filters & fixed-
interval smoothers for mixed real/integer estimation problems Constant integer ambiguities enter only measurements Optimal algorithms consider all integers in data batch Suboptimal algorithms drop integers that affect measurements
only in remote past or future Tested using data from truth-model simulation
Optimal & suboptimal filter achieve modest accuracy gains vs. all-reals approximate filter
Filter accuracy gains may be greater for different problem Optimal & suboptimal smoother significantly more accurate
than all-reals smoother Suboptimal smoother nearly as accurate as optimal smoother Suboptimal algorithms reduce required processing by at least
65% through reductions in dimensions of measurement update integer linear least-squares problems