“2-second” filter: software development...
TRANSCRIPT
1
“2-second” Filter:Software Development Review
M.Heifetz, J.Conklin
2
Outline Fundamentals of 2-sec Filter
Modular Software Structure
Schedule of Tests
…
3
Four Cornerstones of Filter Development
Estimation Algorithms: Numerical Techniques
EstimationTheory
Gyroscope Motion: Torque Model(s)
SQUID Readout Signal Structure: Measurement Model(s)
Algebraic Method Machinery: Development and Experience
GP-B DataAnalysis
Experience
4
SQUID Readout Signal Model
noisebias
ttst
ttsttCtZ
rEWEW
rNSNSgSQUID
++
+!"+
+!"=
)})(sin()]()([
))(cos()]()(){[()(
#$%
#$%SQUIDData
!!"
#$$%
&=
EW
NS
'
''r
• Estimation performed for the data collected during Guide Star Valid (GSV) mode
• Pointing
intpoorbGSparbend
ann
Aber
orb
Aber!""""""rrrrrrr
+++++=
Orbitaldata
EarthEphemerides
known Estimated (?)
Torb= 24.648770 days
Pointing Error Compensation:Telescope data + scale factor
matching
5
1. Use TFM scale factor variations as is (simplest)
• Algebraic filter will estimate constant CgLM only
2. Use γCg model without TFM prior information (symmetric phase)
• Algebraic filter estimates full set of γCg coefficients ank, bnk and CgLM
3. Use TFM scale factor and estimate correction via γCg• Algebraic filter estimates subset of γCg coefficients amn, bmn, and CgLM
Scale Factor (Cg) – 4 Approaches
)()( tCCtCTF
g
LM
gg +=
( )
)()(
)(
)2/)(tan()(),()(
,])(sin()(cos()(1[)(
01
0001
00
1000
tb
a
tb
ta
tttata
tmbtmataCtC
nN
nmn
mn
m
m
nN
nn
M
mmm
LM
g
cg
cg
cg
L
!
"!!
# $$%
&''(
)=$$%
&''(
)
=#=
# *+*++=
=
=
=
6
4. Ideal Approach: Exact Polhode Phase• γCg Model using exact polhode phase φp
– Algebraic filter will estimate CgLM, update TFM estimates of amn, bmn
( )
)2/)(tan()(),()(
)(
,])(sin()(cos(1[)(
2
0
0
tttb
a
tb
ta
tmbtmaCtC
nmN
nmn
mn
m
m
M
mpmpm
LM
g
cg
cg
L
!"" =# $$%
&''(
)=$$%
&''(
)
# *+*+=
+
=
=
7
Gyroscope Motion: Torque Model
)]cos()()sin()([))((
)]sin()()cos()([))((
rrNSNSEW
EW
rrEWEWNS
NS
tctcstkrdt
ds
tctcstkrdt
ds
!++!++""=
!+"!++"+=
+"
+"
##$
##$
)](sin)()(cos)([)(0020
001
tmktmktkm
kM
m
m!+!="
=
##
k
mn
kN
n
mn
mn
m
m
Mmtk
k
k
k,...,1,0),(01
00
2
1
2
1
=!!"
#$$%
&=!!"
#$$%
&'+
=
( )*
c
nncN
n
mn
mn
m
m
Mmtc
c
c
c,...,1,0),(01
00
2
1
2
1
=! ""#
$%%&
'=""#
$%%&
'(+
=±
±
±
±
)*
)](sin)()(cos)([)( 0
02
0
001
tmctmctcp
mp
cM
mm
!+!=±
=
±± " ##
)2/tan(00!" =
Models for :!
;2/!" =1.
))()(
)()(arctan()(
tst
tstt
NSNS
EWEW
!
!=
"
"#2.
TFM
Misalignment Torque Roll-resonance TorqueRelativity
8
• Explicit solution for orientation (Alex S.)
• Explicit computation of as a part of Jacobian computation !x
s
!
!
sr
),,,,,()(
)(00!" ##=$$
%
&''(
)r
EW
NS
txfts
ts rr
,...]),,(),,(),,(,,[002121!"
WENSmnmnmnmnEWNS
sscckkrrx±±
=
• - state vector (constant parameters)x
• No need for numerical ODE integration !
• Allows explicit computation of the Jacobian !x
h
!
!
9
Pointing Error Compensation (matching)
!+
!+
+
!=
wss
wssN
• Normalized Pointing signal (per axis, per telescope side)
• Pointing Error ( per axis / per telescope side): matching model
])(1[ 2
3,
NcNcTTp
yx
+=!
2 Telescope sides (A,B)
2 axes (x,y) 2 axes (x,y)
2 signals / axis),( !+
ss
2 signals / axis),( !+
ss
• Gyroscopes 1 and 3:
Gyroscopes 2 and 4:x
!
y!
• - part of state vector(per gyro, per telescope side)
),,(3
T
cwcT
,1
T
c
10
GP-B Data Analysis: Nonlinear Filtering Problem
Nk ,...2,1= - number of data points
kkkkuxhz !+= ),(
SQUIDData
Model:Nonlinear in x
Noise statistics
Two main approaches:
• Iterative Extended Kalman Filter (IEKF)- widely used in post-flight data analysis- drawbacks: linearization and potentially biased state-vector estimate
• Sigma Point Filter (SPF)- recently developed by the aero-astro community for spacecraftattitude estimation, nonlinear aerodynamic parameter estimation, andtracking applications
- claims that performance is better than EKF/IEKF
- drawbacks: more computationally intensive than EKF
11
Iterative Extended Kalman Filter (IEKF)• Iterative linearization process
x
Px,ˆ - Current estimate of the state-vector and its covariance matrix)1(~ !nx
Linearization about current estimate:
!+"+"#
#+= )ˆ()ˆ(|),ˆ(
ˆxxoxx
x
huxhZ
xk
)( nN ! matrix in batch caseCompute Jacobian:
Form Innovations: ).,ˆ( uxhzz !="
x
x
hJ
ˆ|
!
!=
xxx ˆ!="Define correction vector:
k
xxJz !"" += ,...)ˆ(• Linear structure:
(1)
(2)
(3)
(4)
12
Output: and),(
;ˆˆˆ
xx
new
x
new
PPfP
xxx
!
!
=
+=
• Apply linear least-squares estimator (e.g. square-root information filter):
x̂!x
P!
Iteration process repeats until the cost function reaches plateau (or )0ˆ!x"
SQUID Data(GSV)
SQUID Model(GSV)
+
-LSQ
Estimator x̂!
xxxnew
ˆˆˆ !+=Jacobian
• Difficulty: Jacobian computation- analytic- numerical
• Analytic solution for clears the way for the analytic Jacobiancomputation
s
13
Module-based Functional Block Diagram
-state vector
Module
gC
Module
s
Module
GSV
!
x
h
!
!
hModule
h-Jacobian
Module
Z
-z! Module
IEKF
x̂!
P
x
Cg
!
!x
s
!
!
x!
!"gC !s
x
GSVZ
Module
GSI!
GSIZ
RelativityEstimate
GSI!
GSI!TFM
Data
SQUIDData
TelescopeData
AberrationData
!" ˆ
RelativityEstimate
uncertainty
RollPhaseData
ModuleResidualAnalysis
- KACST
ModuleTruth Model
ModuleOptimization
14
List of Modules
• Module Data preparation:
- Calibration signal removal- Grades- Bandpass filter (roll ± orbit)
Input: SQUID signal (sampling rate: 2sec)Data grades
Output: SQUID signal
Algorithms: T.Holmes (30%), K.Stahl (30%) Code: K.Stahl Readiness: 100% (for current set of Data Grades)
)(tZ
Z !" += ,...),,()( sChtZgGSV
15
• Module 4 methods (see above) Input: Cg parameters (Cg
LM, ank, bnk ) Cg
TF, polhode phase and angle
Output:
Algorithms: M.Heifetz, A.Silbergleit, J.Conklin, V.Solomonik
Code: V.Solomonik, J.ConklinReadiness: 80 % for methods 1 and 2, 50% for others (4 weeks)
Comments:
gC
,gC
x
Cg
!
!
List of Modules – cont.
•Code for all methods exist and have been vetted•Must be packaged into a single function with option to select method•For Cg with exact polhode phase (method 4), φp, γp should be written to L3(and L3 speedread) to drastically reduce execution time
16
• Module Input: s-parameters – part of state vector (relativity, torque coefficients)
Pointing (both GSV and GSI)
Roll Phase, Polhode Phase and Angle
Output: orientation
Jacobian
Method: Explicit solution Numerical integration (back-up) Sub-module Misalignment torque (MT)
Misalignment torque model(s) Sub-module Roll-resonance torque (RT) Roll-resonance torque model(s)
Algorithms: A.Silbergleit, M.Heifetz, J.Conklin Code: V.Solomonik Readiness: numerical integrator 100% (back-up), analytic 20% (4 weeks)
!
x
s
!
!
,...),( txss =
s
)(),( tstsEWNS
List of Modules – cont.
17
• Module Input:
- Aberrations (orbital, annual), starlight bending, parallax;- Telescope signals;- Telescope scale factor coefficients (part of state vector)
Output: - Pointing
- Jacobian
- Pointing error estimate (Gyro/Telescope matching)
Algorithms: T.Holmes (20%), M.Heifetz, V.SolomonikCode: V.Solomonik, T.Holmes (20%)Readiness: 80% (2 weeks)
GSV
!
GSV
!
x!
!"
List of Modules – cont.
18
List of Modules – cont.
• Module h-Jacobian Input:
-
- as a part of the state vector- Parts of Jacobian (from corresponding modules):
Output:- Model
- Jacobian
Algorithm: M.Heifetz, A.Silbergleit, V.Solomonik, J. Conklin Code: V.Solomonik Readiness: 50% (3 weeks)
)(th
x
h
!
!
,x
s
!
!,
x
Cg
!
!
x!
!"
),(tCg
)(),(),(),(),( ttttstsrEWNSEWNS
!""
!" ˆ
19
• Module IEKF (Primary method) Input: Z(t), ,
Output: State vector estimate, covariance matrix, P Method: IEKF (uses Bierman library) Algorithm: T.Holmes (20%), V.Solomonik, M.Heifetz, J. Conklin Code: V.Solomonik Readiness: 0% (1 month)
List of Modules – cont.
),( txhx
h
!
!
x̂
• Module Optimization Input: Z(t), ,
Output: State vector estimate,
Method: Nonlinear least-squares fit Algorithm: A. Bradley (Stanford Optimization Lab) Code: K. Stahl, KACST Readiness: 0% (3 months)
),( txhx
h
!
!
x̂
20
List of Modules – cont.
• Module SPF (for Phase 3) Input: , (no Jacobian required)
Output: State vector estimate , covariance matrix
Method: Sigma-point filter
Algorithm: T.Holmes (20%), M.Heifetz, J. Conklin, KACST
Code: V.Solomonik, KACST
Readiness: 0% (4 months)
)(tZ
Px̂
),( txh
21
Two interwoven loops• Guide Star Valid Data Loop (full mission)
– State vector parameters estimation:• Relativity (rNS, rEW)• Gyro scale factor coefficients (Cg
LM, ank , bnk)• Roll phase offset (δφ)• Telescope scale factor coeffs. (Gyro/Telescope Matching) (cT
i)• Roll-resonance torque parameters (c±
1mn, c±2mn)
• Misalignment torque parameters (k1mn, k2mn)• Initial orientation (sNS0, sWE0)
• Guide Star Invalid Data Loop (full mission)– Pointing determination
Pointing is needed for s-propagationAdvantage of redundancy: 4 sources of information (4 Gyros)for determining 2 components
EWNS!! ,
),(EWNS!!!
r
22
• Module Truth Model Algorithm: M.Heifetz, KACST Code: KACST Readiness: 0%
List of Modules – cont.
• Module Compute and update based on SQUID data (GSI) and estimated parameters
Initial estimate from B. Clarke, J. Conklin exists
Algorithms: M.Heifetz, T.Holmes, J.Conklin, M.Adams, KACST
Code: KACST, M.Adams
Readiness: 0%
GSI
!GSI
!
23
• Module Geometric Method Integration Purpose: Apply Geometric Method to s(t) with Roll-Resonance torque removed Algorithm: M.Keiser, J.Conklin, K. Stahl Code: K. Stahl Readiness: 0%
List of Modules – cont.• Module Residuals Analysis Goodness-of-fit tests, Residual structure identification Algorithms: T.Holmes, M.Heifetz, KACST Code: KACST Readiness: 0%
24
10 Data Segments interrupted by anomalous events
1) September 13, 2004 – September 23 (11 days)2) September 25 – November 10 (47 days)3) November 12 – December 04 (23 days)4) December 05 – December 09 (5 days)5) December 10 – January 20, 2005 (42 days)6) January 21 – March 04 (43 days)7) March 07 – March 15 (9 days)8) March 16 – March 18 (3 days)9) March 19 – May 27 (70 days)
10) May 31 – July 23 (54days)
Data Segmentation
307 days of science data available
Segmentsto analyze
first
25
Schedule of TestsSchedule of Tests
• Phase 1: Test of baseline configuration March - April- Data: Segment 5 (or 6)- Module : Mode 1 ( from TFM);- Module : Initial profile, no iterative update;- Matching based on known telescope scale factors (no update);
gC
GSI! g
C
• Phase 2: Test of extended baseline configuration April-June
- Data: Segment 5 + 6- Module : Mode 2 (Estimated parameters);- Module : Initial profile, no iterative update;- Matching: estimation of telescope scale factors
gC
gC
GSI!
• Phase 3: Full Mission Analysis Test July - August
26
Modules where KACST can contribute
• Module Residuals Analysis Goodness-of-fit tests, Residual model identification Algorithms: T.Holmes, M.Heifetz, KACST Code: KACST
• Module Compute and update spacecraft pointing during GSI based on SQUID data and estimated parameters
Algorithms: M.Heifetz, T.Holmes, J.Conklin, M.Adams, KACST
Code: KACST, M.Adams
GSI
!GSI
!
27
• Module Truth Model
Simulate SQUID data and test Estimation Methods Algorithms: M.Heifetz, KACST Code: KACST
• Module Optimization Interface between optimization package and GP-B data analysis software
Study optimization package that will be used as a part of estimation process;This package exploits subroutines written in C and/or Fortran, and GP-Banalysis software is written in Matlab: therefore some interface is neededfor communication between various modules
Algorithm: A. Bradley (Stanford Optimization Lab), J.ConklinCode: K. Stahl, KACST
28
• Module SPF (for Phase 3) Investigate alternative nonlinear estimation techniques: Sigma-point filters
Algorithms: T.Holmes, M.Heifetz, J. Conklin, KACST
Code: KACST