Covariance Intersection Algorithm for Formation …islab.isr.ist.utl.pt/htdocs/workshop4/presentations/...intersection algorithm that estimates the full state at each spacecraft in

INSTITUTODE SISTEMASE ROBÓTICA

Measurement

Vector y1

CI EKF

1

CI EKF

2

Measurement

Vector y2

CI EKF

3

Measurement

Vector y3

State vector

estimate

331ˆ v÷z +=

State vector

estimate 223

ˆ v÷z +=

State vector

estimate 332

ˆ v÷z +=

x1

z1

y1

-8000

-6000

-4000

-2000

0

2000

4000

6000

8000

12250

13250

14250

15250

16250

17250

18250

time [s]

x1, z1

, y1 [m

]

x1

z1

y1

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

q Overview

q Introduction

q State vector

q R/F subsystem antennas

q Full-order Decentralized Filter

q Covariance Intersection and Kalman Filter

q 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/FSubsytem

• Estimator out ofthe control loopYET!Measurement

Vector y1

EKFCI

1

CI EKF

2

MeasurementVector y2

EKF CI

3

MeasurementVector y3

State vector estimate

State vector estimate

State vector estimate23

÷ vz +=31

÷ vz +=

12÷ vz +=


ISR/IST FF NAVIGATION → state vector

ij!r

TFi

TFj

LVLH0

TFk

zk̂

yk̂

ki!r

jk!r

i!r

k!r

j!r

ij!r

i j

[ ]Tijijijij zzyyxx !!!="r

[ ]Tjjji zyx=!r

[

]Tyyzzxx

yyzzx x

yyzzxxX

'''

'''

'''

333333

222222

111111=


ISR/IST FF NAVIGATION → R/F measurements

jiji

multipath

ji

ji

j

i ETTc,,)( !""" ++#+#=

rr

zkr

• RelativeMeasurements

Transmitterspacecraft i

Receiverspacecraft j LVLH

BF

0

0

xk

r

ykr

1,Rj

i!

3,Rj

i!

2,Rj

i!

i!r

j!r

)( jiTT ! - 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

!!!

!!!rrr

rrr

"=

"=

kii!!rr

=

kjj !!rr

=

( ) ( ) ( ) ( ) ( ) ( )[ ]

[ ]

[ ]31c32

c

12

cPC

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

rrrrrr

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

""

""

""

rr

rr

rr

Transmitterspacecraft i

Receiverspacecraft j LVLH

BF

0

0

zkr

xk

r

ykr

1,Rj

i!

3,Rj

i!2,Rj

i!

R3

zk̂

xk̂

yk̂

1!ijz

jT

0

ijy

ijx

ISR/IST FF NAVIGATION → R/F subsystem antennas


( )

( )

( )

( )

( )

( ) 13132

13

2

13

2

13

,1

3

13132

12

2

13

2

13

,1

3

13132

13

2

13

2

13

,1

3

12122

12

2

12

2

12

,1

2

12122

12

2

12

2

12

,1

2

12122

12

2

12

2

12

,1

2

1000

1000

1000

1000

1000

1000

3

2

1

3

2

1

vTczyx

vTczyx

vTczyx

vTczyx

vTczyx

vTczyx

R

R

R

R

R

R

+!+"++=

+!++"+=

+!+++"=

+!+"++=

+!++"+=

+!+++"=

#

#

#

#

#

#

( )ijjijiji

multipah

ijTTcTc and Ev !="+= ,,

#$

Measurements considering spacecraft 1

where

1

2!

1

3!

ISR/IST FF NAVIGATION → R/F subsystem antennas


ISR/IST FF NAVIGATION → Observation Matrix

!!!!!!!!!!!!

"

#

$$$$$$$$$$$$

%

&

'

'

'

'

'

'

'

'

'

'

'

'

'

'

'

'

=

OMM

K

K

KMM

K

K

32

1

3

12

1

3

32

1

3

12

1

3

32

1

2

12

1

2

32

1

2

12

1

2

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

412

13

2

13

2

13

13

2

13

2

13

2

13

13

2

13

2

13

2

13

13

61

,1

3

412

12

2

13

2

13

13

2

12

2

13

2

13

13

2

12

2

13

2

13

13

61

,1

3

412

13

2

13

2

13

13

2

13

2

13

2

13

12

2

13

2

13

2

13

13

61

,1

3

511512

12

2

12

2

12

12

2

12

2

12

2

12

12

2

12

2

12

2

12

12

,1

2

511512

12

2

12

2

12

12

2

12

2

12

2

12

12

2

12

2

12

2

12

12

,1

2

511512

12

2

12

2

12

12

2

12

2

12

2

12

12

2

12

2

12

2

12

2

12

,1

2

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,1

2

xh

12,1

2

yh 12,1

2

yh



• The same for each spacecraft 2 and 3

!!!!!!!!!!!!

"

#

$$$$$$$$$$$$

%

&

'

'

'

'

'

'

'

'

'

'

'

'

'

'

'

'

=

OMM

K

K

KMM

K

K

32

,2

3

12

,2

3

32

,2

3

12

,2

3

32

,2

1

12

,2

1

32

,2

1

12

,2

1

2

22

11

22

11

÷÷

÷÷

÷÷

÷÷

H

RR

RR

RR

RR

((

((

((

((

!!!!!!!!!!!!

"

#

$$$$$$$$$$$$

%

&

'

'

'

'

'

'

'

'

'

'

'

'

'

'

'

'

=

OMM

K

K

KMM

K

K

32

,3

2

12

,3

2

32

,3

2

12

,3

2

32

,3

1

12

,3

1

32

,3

1

12

,3

1

3

22

11

22

11

÷÷

÷÷

÷÷

÷÷

H

RR

RR

RR

RR

((

((

((

((



ISR/IST FF NAVIGATION → Full-order decentralized filter

( )( ) )1

ˆ(ˆ

1

1

1

1111

!

+

!!

+

!

+

!

+

!

+

+

+

!+

+=

kvk

kkkk

zP

xPPx

"

"

1,11,11 !+!++ += ikikk vxz

( ) ( )( )1

11

1

1 1!!

+

!

+

!+

+ !+=vkkk PPP ""

EKF

CI

to fuse estimates from another

spacecraft whose correlation is unkownSpacecraft i

++

+

+++

#

#

kk

kk

PP

xx

1

1

Compute Kalman Gain:1

1111111 )(!

++

!

+++

!

++ +=k

T

kkk

T

kkk RHPHHPK

Update estimate from measurement

)(ˆ111111

!

++++

!

+

+

+ !+=kkkkkk xHyKxx))

Project ahead:+!

+ $=kkk xx)

1ˆ

k

T

kkkk QPP +$$= +!

+1

Compute Kalman Gain:

T

kkk

T

kkkkkk

KRK

HKIPHKIP

111

111111 )()(

+++

++

!

+++

+

+

+

+!!= +

+1k̂x

1+ky

( )( ) )1

ˆ(ˆ

1

1

1

1111

!

+

!!

+

!

+

!

+

!

+

+

+

!+

+=

kvk

kkkk

zP

xPPx

"

"

1,11,11 !+!++ += ikikk vxz

( ) ( )( )1

11

1

1 1!!

+

!

+

!+

+ !+=vkkk PPP ""

EKF

CI



++

+

+++

#

#

kk

kk

PP

xx

1

1


1111111 )(!

++

!

+++

!

++ +=k

T

kkk

T

kkk RHPHHPK


)(ˆ111111

!

++++

!

+

+


Project ahead:+!

+ $=kkk xx)

1ˆ

k

T

kkkk QPP +$$= +!

+1


T

kkk

T

kkkkkk

KRK

HKIPHKIP

111

111111 )()(

+++

++

!

+++

+

+

+

+!!= +

+1k̂x

1+ky

++

+

+++

#

#

kk

kk

PP

xx

1

1

++

+

+++

#

#

kk

kk

PP

xx

1

1


1111111 )(!

++

!

+++

!

++ +=k

T

kkk

T

kkk RHPHHPK1

1111111 )(!

++

!

+++

!

++ +=k

T

kkk

T

kkk RHPHHPK


)(ˆ111111

!

++++

!

+

+


)(ˆ111111

!

++++

!

+

+


Project ahead:+!

+ $=kkk xx)

1ˆ

+!

+ $=kkk xx)

1ˆ

k

T

kkkk QPP +$$= +!

+1 k

T

kkkk QPP +$$= +!

+1


T

kkk

T

kkkkkk

KRK

HKIPHKIP

111

111111 )()(

+++

++

!

+++

+

+

+

+!!=

T

kkk

T

kkkkkk

KRK

HKIPHKIP

111

111111 )()(

+++

++

!

+++

+

+

+

+!!= +

+1k̂x+

+1k̂x

1+ky 1+ky

Estimationerror ofeach

S/C kalmanfilter


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 minimizetrace(Pzz)

Intersection of thecovariance ellipsoidsof Pxx and Pyy gives

the covarianceellipsoid of the

Maximum Likehoodestimator for different

cross-correlations.


ISR/IST FF NAVIGATION → Covariance Intersection

CI provides an estimate and acovariance matrix whoseellipsoid encloses theintersection region withoutprevious knowledge ofcross-covariance, Pxy


• Sensor Observation

Linearize local observation model

to obtain local observation matrix

)(kyi

• State and covariance estimate from predecessor spacecraft

Compute

( ) )(1/ˆ)( 11kvkkkz

ii

PC

i !! +!= "

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

!!+!=+ kkPwkkPwkkPiii

( ) ( )( ( )( ) ))()1/(1)1/(ˆ)1/()1/(1/ˆ111kzkkPwkk÷kkPwkkPkk÷

iii

PC

iii

PC

!!!

!!+!!!=!

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

Tiiii +!=

1))()(()1/()( !!= kSkHkkPkK

iiii

)))1/(ˆ()()(()1/(ˆ)1/(ˆ !!+!=+ kk÷HkykKkk÷kk÷i

PC

iiii

PC

i

PC

( ) ( ) ( )TiiiTiiiiiikKkRkkkHkKIkkPkHkKIkkP )()()()()()1/()()()1/( +!!!=+

Entire fleet state in the ith S/C


FILTERING





( )( ) )(1/)( kvkkhkyii

PC

ii +!= "

( )( )1/ˆ !kkHi

PC

i "

Compute

• State and covariance estimate from predecessor spacecraft

Compute

( ) )(1/ˆ)( 11kvkkkz

ii

PC

i !! +!= "

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

!!+!=+ kkPwkkPwkkPiii

( ) ( )( ( )( ) ))()1/(1)1/(ˆ)1/()1/(1/ˆ111kzkkPwkk÷kkPwkkPkk÷

iii

PC

iii

PC

!!!

!!+!!!=!

( ) ( ) ( ) ( ) ( ) ( )[ ]T31

c

32

c

12

c

TTTTTTi

PCxxx÷ ˆˆˆˆˆˆˆˆˆˆ

313212313212!!!!!! """=Entire fleet state in the ith S/C:


Local innovation covariance matrix

Kalman Gain matrix

FILTERING


Compute

( ) )()()1/()()( kRkHkkPkHkSTiiii +!=

1))()(()1/()( !!= kSkHkkPkK

iiii


PC

iiii

PC

i

PC


• State and covariance estimate from predecessorspacecraft

Compute

( )( ) )(1/)( kvkkhkyii

PC

ii +!= "

( )( )1/ˆ !kkHi

PC

i "

( ) ( ) ( ) ( ) ( ) ( )[ ]T31

c

32

c

12

c

TTTTTTi


313212313212!!!!!! """=Entire fleet state in the ith S/C





FILTERING


Compute

( ) )()()1/()()( kRkHkkPkHkSTiiii +!=

1))()(()1/()( !!= kSkHkkPkK

iiii


PC

iiii

PC

i

PC


• State and covariance estimate from predecessorspacecraft

Compute

( )( ) )(1/)( kvkkhkyii

PC

ii +!= "

( )( )1/ˆ !kkHi

PC

i "

( ) ( ) ( ) ( ) ( ) ( )[ ]T31

c

32

c

12

c

TTTTTTi







FILTERING


ISR/IST FF NAVIGATION → Prediction

FILTERING( ) ( ) ( ) ( ) ( ) ( )[ ]T31

c

32

c

12

c

TTTTTTi



PREDICTION


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(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 Navigationalgorithm(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:

?

( )( ) )1

ˆ(ˆ

1

1

1

1111

!

+

!!

+

!

+

!

+

!

+

+

+

!+

+=

kvk

kkkk

zP

xPPx

"

"

1,11,11 !+!++ += ikikk vxz

( ) ( )( )1

11

1

1 1!!

+

!

+

!+

+ !+=vkkk PPP ""

EKF

CI



++

+

+++

#

#

kk

kk

PP

xx

1

1


1111111 )(!

++

!

+++

!

++ +=k

T

kkk

T

kkk RHPHHPK


)(ˆ111111

!

++++

!

+

+


Project ahead:+!

+ $=kkk xx)

1ˆ

k

T

kkkk QPP +$$= +!

+1


T

kkk

T

kkkkkk

KRK

HKIPHKIP

111

111111 )()(

+++

++

!

+++

+

+

+

+!!= +

+1k̂x

1+ky

( )( ) )1

ˆ(ˆ

1

1

1

1111

!

+

!!

+

!

+

!

+

!

+

+

+

!+

+=

kvk

kkkk

zP

xPPx

"

"

1,11,11 !+!++ += ikikk vxz

( ) ( )( )1

11

1

1 1!!

+

!

+

!+

+ !+=vkkk PPP ""

EKF

CI



++

+

+++

#

#

kk

kk

PP

xx

1

1


1111111 )(!

++

!

+++

!

++ +=k

T

kkk

T

kkk RHPHHPK


)(ˆ111111

!

++++

!

+

+


Project ahead:+!

+ $=kkk xx)

1ˆ

k

T

kkkk QPP +$$= +!

+1


T

kkk

T

kkkkkk

KRK

HKIPHKIP

111

111111 )()(

+++

++

!

+++

+

+

+

+!!= +

+1k̂x

1+ky

++

+

+++

#

#

kk

kk

PP

xx

1

1

++

+

+++

#

#

kk

kk

PP

xx

1

1


1111111 )(!

++

!

+++

!

++ +=k

T

kkk

T

kkk RHPHHPK1

1111111 )(!

++

!

+++

!

++ +=k

T

kkk

T

kkk RHPHHPK


)(ˆ111111

!

++++

!

+

+


)(ˆ111111

!

++++

!

+

+


Project ahead:+!

+ $=kkk xx)

1ˆ

+!

+ $=kkk xx)

1ˆ

k

T

kkkk QPP +$$= +!

+1 k

T

kkkk QPP +$$= +!

+1


T

kkk

T

kkkkkk

KRK

HKIPHKIP

111

111111 )()(

+++

++

!

+++

+

+

+

+!!=

T

kkk

T

kkkkkk

KRK

HKIPHKIP

111

111111 )()(

+++

++

!

+++

+

+

+

+!!= +

+1k̂x+

+1k̂x

1+ky 1+kyR/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̂

PXt,ˆ

PXt,ˆ

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

PXt,ˆ

!

5050 ..,ˆ

+

!

+ ttPX

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

50

3

50 ..,ˆ

!!=ttPXz

SC2

ttPX ,ˆ

ttPX ,ˆ

ttPX ,ˆ

Com

SC2

Com

unic

atio

n

ttPX ,ˆ

tX̂


ISR/IST FF NAVIGATION ALGORITHM – MODULARITY

Each filter is different due to the linearized observation matrix.

!!!!!!!!!!!!

"

#

$$$$$$$$$$$$

%

&

'

'

'

'

'

'

'

'

'

'

'

'

'

'

'

'

=

OMM

K

K

KMM

K

K

32

1

3

12

1

3

32

1

3

12

1

3

32

1

2

12

1

2

32

1

2

12

1

2

1

22

11

22

11

÷÷

÷÷

÷÷

÷÷

H

RR

RR

RR

RR

,,

,,

,,

,,

((

((

((

((

!!!!!!!!!!!!

"

#

$$$$$$$$$$$$

%

&

'

'

'

'

'

'

'

'

'

'

'

'

'

'

'

'

=

OMM

K

K

KMM

K

K

32

,2

3

12

,2

3

32

,2

3

12

,2

3

32

,2

1

12

,2

1

32

,2

1

12

,2

1

2

22

11

22

11

÷÷

÷÷

÷÷

÷÷

H

RR

RR

RR

RR

((

((

((

((

!!!!!!!!!!!!

"

#

$$$$$$$$$$$$

%

&

'

'

'

'

'

'

'

'

'

'

'

'

'

'

'

'

=

OMM

K

K

KMM

K

K

32

,3

2

12

,3

2

32

,3

2

12

,3

2

32

,3

1

12

,3

1

32

,3

1

12

,3

1

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 ofrelative distances state variables.

• Attitude estimation, which implies to include Startracker 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


Measurement

Vector y1

CI EKF

1

CI EKF

2

Measurement

Vector y2

CI EKF

3

Measurement

Vector y3

State vector

estimate

331ˆ v÷z +=

State vector

estimate 223

ˆ v÷z +=

State vector

estimate 332

ˆ v÷z +=

x1

z1

y1

-8000

-6000

-4000

-2000

0

2000

4000

6000

8000

12250

13250

14250

15250

16250

17250

18250

time [s]

x1, z1

, y1 [m

]

x1

z1

y1

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

Documents

Covariance Intersection Algorithm for Formation …islab.isr.ist.utl.pt/htdocs/workshop4/presentations/...intersection algorithm that estimates the full state at each spacecraft in