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