instituto de sistemas e robÓtica covariance intersection algorithm for formation flying spacecraft...

INSTITUTO DE SISTEMAS E ROBÓTICA

Covariance Intersection Algorithm for Formation Flying

Spacecraft NAVIGATIONfrom RF Measurements

4 ISLAB WORKSHOP12 November 2004

Sónia Marques

Formation Estimation Methodologies for Distributed Spacecraft

ESA (European Space Agency) 17529/03/NL/LvH/bj


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


ISR/IST FORMATION FLYING GNC

Guidance Control FF S/C

Navigation)( inittx

0.5severy

tx )(

)(tx)(tdesx

perigee

apogee


ISR/IST FORMATION FLYING SIMULATOR ← DEIMOS Lda


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 y1

EKFCI

1

CI EKF

2

MeasurementVector y2

EKF CI

3

MeasurementVector y3

State vector estimate



23 χ vz 31 χ vz

12 χ vz


ISR/IST FF NAVIGATION → state vector

ij

TFi

TFj

LVLH0

TFk

zk̂

yk̂

ki

jk

i

k

j

iji j

Tijijijij zzyyxx

Tjjji zyx

Tyyzzxx

yyzzx x

yyzzxxX

'''

'''

'''

333333

222222

111111


ISR/IST FF NAVIGATION → R/F measurements

jijimultipath

jiji

ji ETTc ,,)(

zk

• Relative Measurements

Transmitter spacecraft i

Receiver spacecraft j LVLH

BF

0

0

xk

yk

1,Rji

3,Rji

2,Rji

i

j

)( ji TT - Time Bias

ji , - pseudo-range measurement noise due to receiver thermal noiseji

multipathE ,- Multipath error


ISR/IST FF NAVIGATION → state vector

TFi

TFj

LVLH0

kjkiji

jiji

kii

kjj

31

c32c

12cPC

313212313212

TTTTTTT

TTTTTTT

xxxχχ

z

y

x

z

y

x

z

y

x

z

y

x

z

y

x

z

y

x

χ

31

31

31

32

32

32

12

12

12

31

31

31

32

32

32

12

12

12

313212313212

State vector:

Relative variables


222

222

222

1000

1000

1000

3

2

1

ijijij

R

ji

ijijij

R

ji

ijijij

R

ji

zyx

zyx

zyx

Transmitter spacecraft i

Receiver spacecraft j LVLH

BF

0

0

zk

xk

yk

1,Rji

3,Rji

2,Rji

R3

zk̂

xk̂

yk̂1ijz jT

0

ijy

ijx

ISR/IST FF NAVIGATION → R/F subsystem antennas


1313213

213

213

,13

1313212

213

213

,13

1313213

213

213

,13

1212212

212

212

,12

1212212

212

212

,12

1212212

212

212

,12

1000

1000

1000

1000

1000

1000

3

2

1

3

2

1

vTczyx

vTczyx

vTczyx

vTczyx

vTczyx

vTczyx

R

R

R

R

R

R

ijjijijimultipah

ij TTcTc and Ev ,,

Measurements considering spacecraft 1

where

12

13

ISR/IST FF NAVIGATION → R/F subsystem antennas


ISR/IST FF NAVIGATION → Observation Matrix

32

13

12

13

32

13

12

13

32

12

12

12

32

12

12

12

1

22

11

22

11

χχ

χχ

χχ

χχ

H

RR

RR

RR

RR

,,

,,

,,

,,

Linearization


001000

1000

100010000

0010001000

1000

10000

00100010001000

10000

001000

1000

10001000

0010001000

1000

1000

00100010001000

1000

41213

213

213

13

213

213

213

13

213

213

213

1361

,13

41212

213

213

13

212

213

213

13

212

213

213

1361

,13

41213

213

213

13

213

213

213

12

213

213

213

1361

,13

51151212

212

212

12

212

212

212

12

212

212

212

12,12

51151212

212

212

12

212

212

212

12

212

212

212

12,12

51151212

212

212

12

212

212

212

12

212

212

212

212

,12

3

2

1

3

2

1

czyx

z

zyx

y

zyx

x

χ

czyx

z

zyx

y

zyx

x

χ

czyx

z

zyx

y

zyx

x

χ

czyx

z

zyx

y

zyx

x

χ

czyx

z

zyx

y

zyx

x

χ

czyx

z

zyx

y

zyx

x

χ

R

R

R

R

R

R

12,12

xh 12,12

yh 12,12

yh



• The same for each spacecraft 2 and 3

32

,23

12

,23

32

,23

12

,23

32

,21

12

,21

32

,21

12

,21

2

22

11

22

11

χχ

χχ

χχ

χχ

H

RR

RR

RR

RR

32

,32

12

,32

32

,32

12

,32

32

,31

12

,31

32

,31

12

,31

3

22

11

22

11

χχ

χχ

χχ

χχ

H

RR

RR

RR

RR



ISR/IST FF NAVIGATION → Full-order decentralized filter

Estimation error of each

S/C kalman filter


ISR/IST FF NAVIGATION → Covariance Intersection

Considering variables x and y and z, such that z=Wx x+Wy

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.


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

111 1 yyxxzz PwwPP

yPwxwPPz yyxxzz11 1 )(


• Sensor Observation

Linearize local observation model

to obtain local observation matrix

)(ky i

)(1/)( kvkkhky iiPC

ii

1/ˆ kkH iPC

i

• State and covariance estimate from predecessor spacecraft

Compute

)(1/ˆ)( 11 kvkkkz iiPC

i

1111)1/(1)1/()1/(

kkPwkkPwkkP iii

)()1/(1)1/(ˆ)1/()1/(1/ˆ111

kzkkPwkkχkkPwkkPkkχ iiiPC

iiiPC

Compute )()()1/()()( kRkHkkPkHkS

Tiiii

1))()(()1/()( kSkHkkPkK iiii

)))1/(ˆ()()(()1/(ˆ)1/(ˆ kkχHkykKkkχkkχ iPC

iiiiPC

iPC

TiiiTiiiiii kKkRkkkHkKIkkPkHkKIkkP )()()()()()1/()()()1/(

T31c

32c

12c

TTTTTTiPC xxxχ ˆˆˆˆˆˆˆˆˆˆ

313212313212 Entire fleet state in the ith S/C


FILTERING





)(1/)( kvkkhky iiPC

ii

1/ˆ kkH iPC

i

Compute

)()()1/()()( kRkHkkPkHkSTiiii 1))()(()1/()( kSkHkkPkK iiii


iiiiPC

iPC



Compute


i

1111)1/(1)1/()1/(

kkPwkkPwkkP iii

)()1/(1)1/(ˆ)1/()1/(1/ˆ111

kzkkPwkkχkkPwkkPkkχ iiiPC

iiiPC

T31c

32c

12c

TTTTTTiPC xxxχ ˆˆˆˆˆˆˆˆˆˆ 313212313212 Entire fleet state in the ith S/C:


Local innovation covariance matrix

Kalman Gain matrix

FILTERING


Compute

)()()1/()()( kRkHkkPkHkSTiiii 1))()(()1/()( kSkHkkPkK iiii


iiiiPC

iPC



Compute


i

1111)1/(1)1/()1/(

kkPwkkPwkkP iii

)1/(ˆ)1/()1/(1/ˆ1

kkχkkPwkkPkkχ iPC

iiiPC

)()1/(111 kzkkPw ii

)(1/)( kvkkhky iiPC

ii

1/ˆ kkH iPC

i

T31c

32c

12c

TTTTTTiPC xxxχ ˆˆˆˆˆˆˆˆˆˆ 313212313212 Entire fleet state in the ith S/C





FILTERING


ISR/IST FF NAVIGATION → Prediction

FILTERING T31

c32c

12c

TTTTTTiPC xxxχ ˆˆˆˆˆˆˆˆˆˆ 313212313212 Entire fleet state in the ith S/C

PREDICTION

))/(ˆ,(/ˆ 111 KKχKfkkχ iPC

iPC

))/(ˆ/ˆ 111 KKχkkχ iPCk

iPC

Q)1)((k/kPkkP Tk

ik

i 11 /


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(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% <sonia>%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 Pglobal 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


ISR/IST FF NAVIGATION ALGORITHM→ TIME SAMPLING

Simulator time: ?

S/C sampling:

?R/F sampling:

1s


ISR/IST FF NAVIGATION ALGORITHM→ SYCRONIZATION

control

KF

Navigation

SC1

RF(each 1 second)

CI

KFSC2

RF

CI

KFSC3

RF

CI (each 0.5 second)

(each 1 second)

(each 0.5 second)

(each 1 second)

(each 0.5 second)

(each 1 second)com

unic

atio

nco

mun

icat

ion

tX̂

tX̂

tX̂

PX t ,ˆ

PX t ,ˆ

control

control


ISR/IST FF NAVIGATION ALGORITHM – PREDICTION

KF

Navigation SC1RF

CI

tX̂control

Prediction SC3

CI

If RF

If not RF

Output=state estimate

prediction

KFcontrol

PX t ,ˆ

5050 .. ,ˆ

tt PX

RF(each 1 second)(each 0.5 second)3

503

50 .. ,ˆ tt PXz

SC2

tt PX ,ˆ

tt PX ,ˆ

tt PX ,ˆ

Com

SC2

Com

unic

atio

n

tt PX ,ˆ

tX̂


ISR/IST FF NAVIGATION ALGORITHM – MODULARITY

Each filter is different due to the linearized observation matrix.

32

13

12

13

32

13

12

13

32

12

12

12

32

12

12

12

1

22

11

22

11

χχ

χχ

χχ

χχ

H

RR

RR

RR

RR

,,

,,

,,

,,

32

,23

12

,23

32

,23

12

,23

32

,21

12

,21

32

,21

12

,21

2

22

11

22

11

χχ

χχ

χχ

χχ

H

RR

RR

RR

RR

32

,32

12

,32

32

,32

12

,32

32

,31

12

,31

32

,31

12

,31

3

χχ

χχ

χχ

χχ

22

11

22

11

RR

RR

RR

RR

H

Possible solution towards modularity:

Using a mask as a selector and all possibilities,

Pre-stored in the algorithm code

321 ,, HHH


• 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


• 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


Covariance Intersection Algorithm for Formation Flying

Spacecraft NAVIGATIONfrom RF Measurements

4 ISLAB WORKSHOP12 November 2004

Sónia Marques

Formation Estimation Methodologies for Distributed Spacecraft

ESA (European Space Agency) 17529/03/NL/LvH/bj

instituto de sistemas e robÓtica covariance intersection algorithm for formation flying spacecraft...

Documents

spacecraft navigation

navigation work slide

i j slide

nllvhbj slide

p xy slide

covariance matrix

i th sc isrist ff navigati

sc kalman filter slide