1. INSTITUTO DE SISTEMAS E ROBÓTICA 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. INSTITUTO DE SISTEMAS E ROBÓTICA Outline  Overview  Introduction  State vector  R/F subsystem antennas  Full-order Decentralized Filter  Covariance Intersection and Kalman Filter  Details of the Navigation Algorithm ISR/IST FORMATION FLYING NAVIGATION work

3. INSTITUTO DE SISTEMAS E ROBÓTICA ISR/IST FORMATION FLYING GNC Guidance Control FF S/C Navigation perigee apogee

4. INSTITUTO DE SISTEMAS E ROBÓTICA ISR/IST FORMATION FLYING SIMULATOR ← DEIMOS Lda

5. INSTITUTO DE SISTEMAS E ROBÓTICA 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. FAC mode only Sensors: R/F Subsytem Estimator out of the control loop YET! Measurement Vector y 1 EKF CI 1 EKF 2 Measurement Vector y 2 EKF CI 3 Measurement Vector y 3 State vector estimate

6. INSTITUTO DE SISTEMAS E ROBÓTICA ISR/IST FF NAVIGATION → state vector TF i TF j LVLH 0 TF k i j

7. INSTITUTO DE SISTEMAS E ROBÓTICA ISR/IST FF NAVIGATION → R/F measurements Relative Measurements Transmitter spacecraft i Receiver spacecraft j LVLH BF 0 0 - Time Bias - pseudo-range measurement noise due to receiver thermal noise - Multipath error

8. INSTITUTO DE SISTEMAS E ROBÓTICA ISR/IST FF NAVIGATION → state vector TF i TF j LVLH 0 State vector: Relative variables

9. INSTITUTO DE SISTEMAS E ROBÓTICA Transmitter spacecraft i Receiver spacecraft j LVLH BF 0 0 R3R3 0 ISR/IST FF NAVIGATION → R/F subsystem antennas

10. INSTITUTO DE SISTEMAS E ROBÓTICA Measurements considering spacecraft 1 where ISR/IST FF NAVIGATION → R/F subsystem antennas

11. INSTITUTO DE SISTEMAS E ROBÓTICA ISR/IST FF NAVIGATION → Observation Matrix Linearization

12. INSTITUTO DE SISTEMAS E ROBÓTICA ISR/IST FF NAVIGATION → Observation Matrix

13. INSTITUTO DE SISTEMAS E ROBÓTICA The same for each spacecraft 2 and 3 ISR/IST FF NAVIGATION → Observation Matrix

14. INSTITUTO DE SISTEMAS E ROBÓTICA ISR/IST FF NAVIGATION → Full-order decentralized filter Estimation error of each S/C kalman filter

15. INSTITUTO DE SISTEMAS E ROBÓTICA ISR/IST FF NAVIGATION → Covariance Intersection Considering variables x and y and z, such that z=W x x+W y If P xy is known → Maximum Likehood estimates minimize trace(P zz ) Intersection of the covariance ellipsoids of P xx and P yy gives the covariance ellipsoid of the Maximum Likehood estimator for different cross- correlations.

16. INSTITUTO DE SISTEMAS E ROBÓTICA 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, P xy

17. INSTITUTO DE SISTEMAS E ROBÓTICA Sensor Observation Linearize local observation model to obtain local observation matrix State and covariance estimate from predecessor spacecraft Compute Entire fleet state in the i th S/C ISR/IST FF NAVIGATION → Full-order decentralized filter FILTERING

18. INSTITUTO DE SISTEMAS E ROBÓTICA Sensor Observation Linearize local observation model to obtain local observation matrix Compute State and covariance estimate from predecessor spacecraft Compute Entire fleet state in the i th S/C: ISR/IST FF NAVIGATION → Full-order decentralized filter Local innovation covariance matrix Kalman Gain matrix FILTERING

19. INSTITUTO DE SISTEMAS E ROBÓTICA Compute State and covariance estimate from predecessor spacecraft Compute Entire fleet state in the i th S/C ISR/IST FF NAVIGATION → Full-order decentralized filter Sensor Observation Linearize local observation model to obtain local observation matrix FILTERING

20. INSTITUTO DE SISTEMAS E ROBÓTICA Compute State and covariance estimate from predecessor spacecraft Compute Entire fleet state in the i th S/C ISR/IST FF NAVIGATION → Full-order decentralized filter Sensor Observation Linearize local observation model to obtain local observation matrix FILTERING

21. INSTITUTO DE SISTEMAS E ROBÓTICA ISR/IST FF NAVIGATION → Prediction FILTERING Entire fleet state in the i th S/C PREDICTION

22. INSTITUTO DE SISTEMAS E ROBÓTICA Ptant=MATRIZ_COV_SC_ANTERIOR(u); TetaNow=convertTimeTeta(time-0.5); TetaAhead=convertTimeTeta(time); TetaStep=TetaAhead-TetaNow; [z,Pant]=PROJECTAHEAD(TetaNow,TetaStep,ztant,Ptant); [X,P]=COV_INTERSECTION(P,Pant,X,z); end %BEGIN -PROJECT AHEAD - 0.5 SECONDS % TETA[rad] -> Corresponde a T=0.5 segundos; TetaNow=convertTimeTeta(time); TetaAhead=convertTimeTeta(time+0.5); TetaStep=TetaAhead-TetaNow; [Xnew,Pnew]=PROJECTAHEAD(TetaNow,TetaStep,X,P); %18 variaveis de estado + 171 da matrix de covariancia %o 1º elemeno e a flag saida=[measurements 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)]; sys=[saida]; X=Xnew; P=Pnew; case {1,2,4,9} sys=[]; end;%switch end; %case ISR/IST FF NAVIGATION – INTERFACE with SIMULATOR Details about some problems concerning Navigation algorithm(s) % This function estimates relative position from R/F Subsystem % % Institute for Systems and Robotics % IST / Lisbon - Portugal % %last changed in: %11/11/2004 % function [sys,x0,str,ts]=RFestimates(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 sizes=simsizes; sizes.NumContStates=0; sizes.NumDiscStates=0; sizes.NumOutputs=190; % 108 (X[18]+ P18*18/2+diagonal=171+19) sizes.NumInputs=224; ;%32+191 sizes.DirFeedthrough=1; sizes.NumSampleTimes=1; sys=simsizes(sizes); %sys=[0 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 P=eye(18); X=[1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1]'; case 3 % RF medidas% Y=MEDIDAS_SENSOR_RF(u(1:32)); time=u(224) if (time-floor(time))==0 measurements=1; elseif (time-floor(time))>0 measurements=0; end %elements of previous S/C estimate %measurement update ou FILTERING %medidas dos sensores if measurements==1 [X,P]=KALMAN_FILTER(P,X,Y); end if measurements==0 %medidas de outros satelites que nao vem com a prediction ztant=VECTOR_ESTADO_SC_ANTERIOR(u); Time Sampling Sycronization Prediction Modularity

23. INSTITUTO DE SISTEMAS E ROBÓTICA ISR/IST FF NAVIGATION ALGORITHM→ TIME SAMPLING Simulator time: ? S/C sampling: ? R/F sampling: 1s

24. INSTITUTO DE SISTEMAS E ROBÓTICA ISR/IST FF NAVIGATION ALGORITHM→ SYCRONIZATION control KF Navigation SC1 RF (each 1 second) CI KF SC2 RF CI KF SC3 RF CI (each 0.5 second) (each 1 second) (each 0.5 second) (each 1 second) (each 0.5 second) (each 1 second) comunication control

25. INSTITUTO DE SISTEMAS E ROBÓTICA ISR/IST FF NAVIGATION ALGORITHM – PREDICTION KF Navigation SC1 RF CI control Prediction SC3 CI If RF If not RF Output=state estimate prediction KF control RF (each 1 second) (each 0.5 second) SC2 Com SC2 Comunication

26. INSTITUTO DE SISTEMAS E ROBÓTICA 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. INSTITUTO DE SISTEMAS E ROBÓTICA 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. ISR/IST FF NAVIGATION ALGORITHM – WORK TO GO

28. INSTITUTO DE SISTEMAS E ROBÓTICA 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 ISR/IST FF NAVIGATION ALGORITHM – CHALLENGES

29. INSTITUTO DE SISTEMAS E ROBÓTICA 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