Covariance Intersection Algorithm for Formation...
Transcript of Covariance Intersection Algorithm for Formation...
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
INSTITUTODE SISTEMASE ROBÓTICA
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
INSTITUTODE SISTEMASE ROBÓTICA
ISR/IST FORMATION FLYING GNC
Guidance Control FF S/C
Navigation)(
inittx
)
0.5severy
tx )()
)(tx)(tdesx
perigee
apogee
INSTITUTODE SISTEMASE ROBÓTICA
ISR/IST FORMATION FLYING SIMULATOR ← DEIMOS Lda
INSTITUTODE SISTEMASE 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/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 +=
INSTITUTODE SISTEMASE ROBÓTICA
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=
INSTITUTODE SISTEMASE ROBÓTICA
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
INSTITUTODE SISTEMASE ROBÓTICA
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
INSTITUTODE SISTEMASE ROBÓTICA
( )
( )
( )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
INSTITUTODE SISTEMASE ROBÓTICA
( )
( )
( )
( )
( )
( ) 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
INSTITUTODE SISTEMASE ROBÓTICA
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
INSTITUTODE SISTEMASE ROBÓTICA
( )
( ) ( ) ( )
( )
( )
( ) ( )
( ) ( )
( )
( )
( )
( ) ( ) ( )
( )
( )
( ) ( )
( ) ( )
( )
( ) !!
"
#
$$
%
&
'++
'
'++'++=
(
(
!!
"
#
$$
%
&
+'++'+
'
+'+=
(
(
!!
"
#
$$
%
&
++'++'++'
'=
(
(
!!
"
#
$$
%
&
'++
'
'++'++=
(
(
!!
"
#
$$
%
&
+'++'+
'
+'+=
(
(
!!
"
#
$$
%
&
++'++'++'
'=
(
(
))
))
))
))
))
))
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
ISR/IST FF NAVIGATION → Observation Matrix
INSTITUTODE SISTEMASE ROBÓTICA
• 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 → Observation Matrix
INSTITUTODE SISTEMASE ROBÓTICA
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
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
++
+
+++
#
#
kk
kk
PP
xx
1
1
++
+
+++
#
#
kk
kk
PP
xx
1
1
Compute Kalman Gain:1
1111111 )(!
++
!
+++
!
++ +=k
T
kkk
T
kkk RHPHHPK1
1111111 )(!
++
!
+++
!
++ +=k
T
kkk
T
kkk RHPHHPK
Update estimate from measurement
)(ˆ111111
!
++++
!
+
+
+ !+=kkkkkk xHyKxx))
)(ˆ111111
!
++++
!
+
+
+ !+=kkkkkk xHyKxx))
Project ahead:+!
+ $=kkk xx)
1ˆ
+!
+ $=kkk xx)
1ˆ
k
T
kkkk QPP +$$= +!
+1 k
T
kkkk QPP +$$= +!
+1
Compute Kalman Gain:
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
INSTITUTODE SISTEMASE ROBÓTICA
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.
INSTITUTODE SISTEMASE ROBÓTICA
ISR/IST FF NAVIGATION → Covariance Intersection
CI provides an estimate and acovariance matrix whoseellipsoid encloses theintersection region withoutprevious knowledge ofcross-covariance, Pxy
INSTITUTODE SISTEMASE ROBÓTICA
• 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
ISR/IST FF NAVIGATION → Full-order decentralized filter
FILTERING
INSTITUTODE SISTEMASE ROBÓTICA
• Sensor Observation
Linearize local observation model
to obtain local observation matrix
( )( ) )(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:
ISR/IST FF NAVIGATION → Full-order decentralized filter
Local innovation covariance matrix
Kalman Gain matrix
FILTERING
INSTITUTODE SISTEMASE ROBÓTICA
Compute
( ) )()()1/()()( kRkHkkPkHkSTiiii +!=
1))()(()1/()( !!= kSkHkkPkK
iiii
)))1/(ˆ()()(()1/(ˆ)1/(ˆ !!+!=+ kk÷HkykKkk÷kk÷i
PC
iiii
PC
i
PC
( ) ( ) ( )TiiiTiiiiiikKkRkkkHkKIkkPkHkKIkkP )()()()()()1/()()()1/( +!!!=+
• State and covariance estimate from predecessorspacecraft
Compute
( )( ) )(1/)( kvkkhkyii
PC
ii +!= "
( )( )1/ˆ !kkHi
PC
i "
( ) ( ) ( ) ( ) ( ) ( )[ ]T31
c
32
c
12
c
TTTTTTi
PCxxx÷ ˆˆˆˆˆˆˆˆˆˆ
313212313212!!!!!! """=Entire fleet state in the ith S/C
ISR/IST FF NAVIGATION → Full-order decentralized filter
• Sensor Observation
Linearize local observation model
to obtain local observation matrix
FILTERING
INSTITUTODE SISTEMASE ROBÓTICA
Compute
( ) )()()1/()()( kRkHkkPkHkSTiiii +!=
1))()(()1/()( !!= kSkHkkPkK
iiii
)))1/(ˆ()()(()1/(ˆ)1/(ˆ !!+!=+ kk÷HkykKkk÷kk÷i
PC
iiii
PC
i
PC
( ) ( ) ( )TiiiTiiiiiikKkRkkkHkKIkkPkHkKIkkP )()()()()()1/()()()1/( +!!!=+
• State and covariance estimate from predecessorspacecraft
Compute
( )( ) )(1/)( kvkkhkyii
PC
ii +!= "
( )( )1/ˆ !kkHi
PC
i "
( ) ( ) ( ) ( ) ( ) ( )[ ]T31
c
32
c
12
c
TTTTTTi
PCxxx÷ ˆˆˆˆˆˆˆˆˆˆ
313212313212!!!!!! """=Entire fleet state in the ith S/C
ISR/IST FF NAVIGATION → Full-order decentralized filter
• Sensor Observation
Linearize local observation model
to obtain local observation matrix
FILTERING
INSTITUTODE SISTEMASE ROBÓTICA
ISR/IST FF NAVIGATION → Prediction
FILTERING( ) ( ) ( ) ( ) ( ) ( )[ ]T31
c
32
c
12
c
TTTTTTi
PCxxx÷ ˆˆˆˆˆˆˆˆˆˆ
313212313212!!!!!! """=Entire fleet state in the ith S/C
PREDICTION
INSTITUTODE SISTEMASE 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 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
INSTITUTODE SISTEMASE ROBÓTICA
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
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
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
++
+
+++
#
#
kk
kk
PP
xx
1
1
++
+
+++
#
#
kk
kk
PP
xx
1
1
Compute Kalman Gain:1
1111111 )(!
++
!
+++
!
++ +=k
T
kkk
T
kkk RHPHHPK1
1111111 )(!
++
!
+++
!
++ +=k
T
kkk
T
kkk RHPHHPK
Update estimate from measurement
)(ˆ111111
!
++++
!
+
+
+ !+=kkkkkk xHyKxx))
)(ˆ111111
!
++++
!
+
+
+ !+=kkkkkk xHyKxx))
Project ahead:+!
+ $=kkk xx)
1ˆ
+!
+ $=kkk xx)
1ˆ
k
T
kkkk QPP +$$= +!
+1 k
T
kkkk QPP +$$= +!
+1
Compute Kalman Gain:
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
INSTITUTODE SISTEMASE ROBÓTICA
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
INSTITUTODE SISTEMASE ROBÓTICA
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̂
INSTITUTODE SISTEMASE ROBÓTICA
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
INSTITUTODE SISTEMASE 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 ofrelative distances state variables.
• Attitude estimation, which implies to include Startracker and R/F susbsytem.
ISR/IST FF NAVIGATION ALGORITHM – WORK TO GO
INSTITUTODE SISTEMASE 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
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