Covariance Intersection Algorithm

1
Covariance Intersection Algorithm for Formation
Flying Spacecraft NAVIGATION from RF
Measurements 4 ISLAB WORKSHOP 12 November
2004 Sónia Marques Formation Estimation
Methodologies for Distributed Spacecraft ESA
(European Space Agency) 17529/03/NL/LvH/bj
2
ISR/IST FORMATION FLYING NAVIGATION work

• Outline
• Overview
• Introduction
• State vector
• R/F subsystem antennas
• Full-order Decentralized Filter
• Covariance Intersection and Kalman Filter
• Details of the Navigation Algorithm

3
ISR/IST FORMATION FLYING GNC
apogee
perigee
4
ISR/IST FORMATION FLYING SIMULATOR ? DEIMOS Lda
5
ISR/IST FF NAVIGATION ? introduction
For the formation state estimator, we are using
a covariance intersection algorithm that
estimates the full state at each spacecraft in a
decentralized manner.
Measurement Vector y3

• FAC mode only
• Sensors R/F Subsytem
• Estimator out of the control loop YET!

3
EKF
CI
State vector estimate
State vector estimate
Measurement Vector y2
2
CI
EKF
Measurement Vector y1
1
State vector estimate
EKF
CI
6
ISR/IST FF NAVIGATION ? state vector
TFi
LVLH
TFk
0
TFj
7
ISR/IST FF NAVIGATION ? R/F measurements
Transmitter spacecraft i
0
BF
Receiver spacecraft j
LVLH
0

• Relative Measurements

- Time Bias
- pseudo-range measurement noise due to receiver
thermal noise
- Multipath error
8
ISR/IST FF NAVIGATION ? state vector
State vector Relative variables
9
ISR/IST FF NAVIGATION ? R/F subsystem antennas
Transmitter spacecraft i
0
R3
BF
Receiver spacecraft j
0
LVLH
0
10
ISR/IST FF NAVIGATION ? R/F subsystem antennas
Measurements considering spacecraft 1
where
11
ISR/IST FF NAVIGATION ? Observation Matrix
Linearization
12
ISR/IST FF NAVIGATION ? Observation Matrix
13
ISR/IST FF NAVIGATION ? Observation Matrix

• The same for each spacecraft 2 and 3

14
ISR/IST FF NAVIGATION ? Full-order decentralized
filter
Estimation error of each S/C kalman filter
15
ISR/IST FF NAVIGATION ? Covariance Intersection
Considering variables x and y and z, such that
zWx xWy If Pxy is known ? Maximum Likehood
estimates minimize trace(Pzz)
Intersection of the covariance ellipsoids of Pxx
and Pyy gives the covariance ellipsoid of the
Maximum Likehood estimator for different
cross-correlations.
16
ISR/IST FF NAVIGATION ? Covariance Intersection
CI provides an estimate and a covariance matrix
whose ellipsoid encloses the intersection region
without previous knowledge of cross-covariance,
Pxy
17
ISR/IST FF NAVIGATION ? Full-order decentralized
filter
Entire fleet state in the ith S/C
FILTERING

• Sensor Observation
• Linearize local observation model
• to obtain local observation matrix

Compute

• State and covariance estimate from predecessor
  spacecraft
• Compute

18
ISR/IST FF NAVIGATION ? Full-order decentralized
filter
Entire fleet state in the ith S/C
FILTERING

• Sensor Observation
• Linearize local observation model
• to obtain local observation matrix

Compute
Local innovation covariance matrix
Kalman Gain matrix

• State and covariance estimate from predecessor
  spacecraft
• Compute

19
ISR/IST FF NAVIGATION ? Full-order decentralized
filter
Entire fleet state in the ith S/C
FILTERING

• Sensor Observation
• Linearize local observation model
• to obtain local observation matrix

Compute

• State and covariance estimate from predecessor
  spacecraft
• Compute

20
ISR/IST FF NAVIGATION ? Full-order decentralized
filter
Entire fleet state in the ith S/C
FILTERING

• Sensor Observation
• Linearize local observation model
• to obtain local observation matrix

Compute

• State and covariance estimate from predecessor
  spacecraft
• Compute

21
ISR/IST FF NAVIGATION ? Prediction
Entire fleet state in the ith S/C
FILTERING
PREDICTION
22
ISR/IST FF NAVIGATION INTERFACE with SIMULATOR
PtantMATRIZ_COV_SC_ANTERIOR(u)
TetaNowconvertTimeTeta(time-0.5)
TetaAheadconvertTimeTeta(time)
TetaStepTetaAhead-TetaNow
z,PantPROJECTAHEAD(TetaNow,TetaStep,ztant,Ptant
) X,PCOV_INTERSECTION(P,Pant,X,z)
end BEGIN -PROJECT AHEAD - 0.5 SECONDS
TETArad -gt Corresponde a T0.5 segundos
TetaNowconvertTimeTeta(time)
TetaAheadconvertTimeTeta(time0.5)
TetaStepTetaAhead-TetaNow Xnew,PnewPROJECTA
HEAD(TetaNow,TetaStep,X,P) 18 variaveis de
estado 171 da matrix de covariancia o 1º
elemeno e a flag saidameasurements X(1) X(2)
X(3) X(4) X(5) X(6) X(7)
X(8) X(9) X(10) X(11) X(12)
X(13) X(14) X(15) X(16) X(17)
X(18),... P(1,1) P(1,2) P(1,3)
P(1,4) P(1,5) P(1,6) P(1,7) P(1,8)
P(1,9) P(1,10) P(1,11) P(1,12) P(1,13)
P(1,14) P(1,15) P(1,16) P(1,17) P(1,18)
P(2,2),... P(2,3) P(2,4) P(2,5)
P(2,6) P(2,7) P(2,8) P(2,9) P(2,10)
P(2,11) P(2,12) P(2,13) P(2,14) P(2,15)
P(2,16) P(2,17) P(2,18) P(3,3) P(3,4)
P(3,5),... P(3,6) P(3,7) P(3,8)
P(3,9) P(3,10) P(3,11) P(3,12) P(3,13)
P(3,14) P(3,15) P(3,16) P(3,17) P(3,18)
P(4,4) P(4,5) P(4,6) P(4,7) P(4,8)
P(4,9),... P(4,10) P(4,11) P(4,12)
P(4,13) P(4,14) P(4,15) P(4,16) P(4,17)
P(4,18) P(5,5) P(5,6) P(5,7) P(5,8)
P(5,9) P(5,10) P(5,11) P(5,12) P(5,13)
P(5,14),... P(5,15) P(5,16) P(5,17)
P(5,18) P(6,6) P(6,7) P(6,8) P(6,9)
P(6,10) P(6,11) P(6,12) P(6,13) P(6,14)
P(6,15) P(6,16) P(6,17) P(6,18) P(7,7)
P(7,8),... P(7,9) P(7,10) P(7,11)
P(7,12) P(7,13) P(7,14) P(7,15) P(7,16)
P(7,17) P(7,18) P(8,8) P(8,9) P(8,10)
P(8,11) P(8,12) P(8,13) P(8,14) P(8,15)
P(8,16),... P(8,17) P(8,18) P(9,9)
P(9,10) P(9,11) P(9,12) P(9,13) P(9,14)
P(9,15) P(9,16) P(9,17) P(9,18) P(10,10)
P(10,11) P(10,12) P(10,13) P(10,14) P(10,15)
P(10,16),... P(10,17) P(10,18) P(11,11)
P(11,12) P(11,13) P(11,14) P(11,15) P(11,16)
P(11,17) P(11,18) P(12,12) P(12,13) P(12,14)
P(12,15) P(12,16) P(12,17) P(12,18) P(13,13)
P(13,14),... P(13,15) P(13,16) P(13,17)
P(13,18) P(14,14) P(14,15) P(14,16) P(14,17)
P(14,18) P(15,15) P(15,16) P(15,17) P(15,18)
P(16,16) P(16,17) P(16,18) P(17,17) P(17,18)
P(18,18) syssaida XXnew PPnew case
1,2,4,9 sys endswitch end case
This function estimates relative position from
R/F Subsystem Institute for
Systems and Robotics IST / Lisbon -
Portugal ltsoniagt last changed
in 11/11/2004 function sys,x0,str,tsRFesti
mates(t,x,u,flag) variaveis que sao
reconhecidas tanto na inicializacao (case 0) como
no (case 3). global P global X switch flag
case 0 sizessimsizes sizes.NumContStates
0 sizes.NumDiscStates0
sizes.NumOutputs190 108 (X18
P1818/2diagonal17119) sizes.NumInputs224
32191 sizes.DirFeedthrough1
sizes.NumSampleTimes1 syssimsizes(sizes)
sys0 0 189 228 0 1 1 189 saidas 228
entradas str No state ordering x0
No continuous states ts-1 1 -1
inherited sample time INITIALIZATION
Peye(18) X1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 1' case 3 RF medidas
YMEDIDAS_SENSOR_RF(u(132)) timeu(224) if
(time-floor(time))0 measurements1
elseif (time-floor(time))gt0
measurements0 end elements of
previous S/C estimate measurement update ou
FILTERING medidas dos sensores if
measurements1 X,PKALMAN_FILTER(P,X,Y)
end if measurements0 medidas de outros
satelites que nao vem com a prediction
ztantVECTOR_ESTADO_SC_ANTERIOR(u)
Details about some problems concerning
Navigation algorithm(s)

• Time Sampling
• Sycronization
• Prediction
• Modularity

23
ISR/IST FF NAVIGATION ALGORITHM? TIME SAMPLING
Simulator time ?
S/C sampling ?
R/F sampling 1s
24
ISR/IST FF NAVIGATION ALGORITHM? SYCRONIZATION
Navigation SC1
RF(each 1 second)
(each 1 second)
KF
control
comunication
CI
(each 0.5 second)
RF
SC2
KF
(each 1 second)
control
comunication
CI
(each 0.5 second)
RF
SC3
KF
(each 1 second)
control
CI
(each 0.5 second)
25
ISR/IST FF NAVIGATION ALGORITHM PREDICTION
Navigation SC1
RF
control
KF
CI
RF(each 1 second)
(each 0.5 second)
SC2
Comunication
If RF If not RF Outputstate
estimate prediction
KF
control
Prediction SC3 CI
Com SC2
26
ISR/IST FF NAVIGATION ALGORITHM MODULARITY
Each filter is different due to the linearized
observation matrix.
Possible solution towards modularity Using a
mask as a selector and all possibilities,
Pre-stored in the algorithm code
27
ISR/IST FF NAVIGATION ALGORITHM WORK TO GO

• Simulations with the CONTROL-in-the-loop
• Include others sensors Divergent laser, Sun
  Sensor
• Include communications errors
• Include carrier-phase signals and/or single
  difference tecniques to improve the accurancy of
  relative distances state variables.
• Attitude estimation, which implies to include
  Star tracker and R/F susbsytem.

28
ISR/IST FF NAVIGATION ALGORITHM CHALLENGES

• Sycronization
• All SC must be sycronized still!
• Communication failure
• R/F subsystem fails or R/F susbsystem backup
• No sensors to compute relative positions
• Propagation of the state and covariance matrix
• Suggestions ? Use of a camera?!?!
• S/C total fail
• ?Bye Bye

29
Covariance Intersection Algorithm for Formation
Flying Spacecraft NAVIGATION from RF
Measurements 4 ISLAB WORKSHOP 12 November
2004 Sónia Marques Formation Estimation
Methodologies for Distributed Spacecraft ESA
(European Space Agency) 17529/03/NL/LvH/bj