Vision Based Navigation for Debris Removal Missions

HAL Id: hal-00750610

https://hal.inria.fr/hal-00750610

Submitted on 11 Nov 2012

HAL is a multi-disciplinary open access archive for the deposit and dissemination of sci-entific research documents, whether they are pub-lished or not. The documents may come from teaching and research institutions in France or abroad, or from public or private research centers.

L’archive ouverte pluridisciplinaire HAL, est destinée au dépôt et à la diffusion de documents scientifiques de niveau recherche, publiés ou non, émanant des établissements d’enseignement et de recherche français ou étrangers, des laboratoires publics ou privés.

Vision Based Navigation for Debris Removal Missions

Keyvan Kanani, Antoine Petit, Eric Marchand, Thomas Chabot, Bernard

Gerber

To cite this version:

Keyvan Kanani, Antoine Petit, Eric Marchand, Thomas Chabot, Bernard Gerber. Vision Based Navigation for Debris Removal Missions. 63rd International Astronautical Congress, 2012, Naples, Italy. �hal-00750610�

!"#$%&'(")'(*'(+'(,%-+..( !"#$( !"#$%&'(")'(*'(+'(,%-+..( (

!"#"$%&'(#)*&+(,"-(."$%&/$0&1)20"#&3)4$,(5&6"##"$%#&

( 7)8,(%&7(%(%"& "/01234(560788207/'(916:;7'(<7=>6:?<6:6:2@6/01234?76A/?:70( (

":0B2:7(C7020%'(D12;(E61;F6:A%'(GFB46/(#F6HB0&(6:A(I71:61A(J71H71&( (

!:$B1H20( /71>2;2:K'( 6:A( 7/L7;2688=( 861K7( A7H12/( 174B>68'( FB8A/( 4B17( 6:A( 4B17( 0F7( 6007:02B:( BM( /L6;7( ;B443:20=?(!:(0F2/(L6L71(N7(L17/7:0(6(>2/2B:(H6/7A(/B8302B:(NF2;F(7:6H87/(0B(6//7//(0F7(LB/7(O600203A7( 6:A(LB/202B:P(BM(6(061K70(3/2:K(6(4B:B;3861(;64716?(GF7(L1BLB/7A(246K7(L1B;7//2:K(68KB120F4(6;F27>7/( QR( A707;02B:( 6:A( 016;<2:K( BM( 0F7( 061K70'( 178=2:K( B:( M1647$0B$M1647( 016;<2:K( 6:A( 016;<2:K( H=( A707;02B:( 07;F:2S37/?( GF7( 462:( L12:;2L87( 2/( 0B( 682K:( 0F7( L1BT7;02B:( BM( 0F7( QR( 4BA78( BM( 0F7( 016;<7A( A7H12/( N20F( BH/71>602B:(46A7(2:(0F7(246K7(6:A(L1B>2A7/(0F7(;B4L8707(178602>7(LB/202B:(6:A(600203A7(0F6:</(0B(:B:$ 82:761( BL0242U602B:( L1B;7//?( V3::2:K( 60( >2A7B( 1607'( 0F7( L1BLB/7A( 68KB120F4( ;6:( F6:A87( >71=( ;B4L87,( 061K70/'(3:A71(A7K16A7A(;B:A202B:/'(2?7?(N20F(D610F(H6;<K1B3:A'(B;;83/2B:(B1(BL02;68(L7;382612027/(O/3;F(6/( LBB1(07,0317(B1(/L7;3861(17M87;02B:/P?(

GF7(016;<2:K(68KB120F4(F6/(L17>2B3/8=(H77:(07/07A(B:(1768(246K7/(0B(A74B:/01607(20/(1BH3/0:7//(0B(1768( A606?( !:( 0F7( L17/7:0( L6L71( N7( MB;3/( B:( F76>=( A7H12/( 016;<2:K'( 82<7( /60788207/( B1( 863:;F71( 06:</?( 57>7168( 07/0/( B:( /2438607A( 246K7/( F6>7( H77:( ;61127A( B30( 0B( S36:020602>78=( /FBN( 0F7( L71MB146:;7/( 6:A( 1BH3/0:7//(BM(HB0F(016;<2:K(6:A(6//B;2607A(:6>2K602B:?( (

%" %&'()*+,'-)&#

( GF7( 6;02>7( 174B>68( BM( F76>=( /L6;7( A7H12/( O0=L2;688=( 861K71( 0F6:( %...<KP( F6/( H77:( 2A7:02M27A( 6/( 6( <7=( A7>78BL47:0( 0B( ;B:01B8( 0F7( K1BN0F(2:(0F7(A7H12/(LBL38602B:(6:A(0B(82420(0F7( 12/<(MB1(6;02>7(/60788207/?(D31BL76:(5L6;7("K7:;=( 6/( N788( 6/( W"5"( L1BLB/7/( /7>7168( /03A27/( B1( 7,L71247:0/( A7682:K( N20F( A7H12/( 174B>68( B1( 4B17( K7:71688=( 0B( B1H20( /71>2;2:K( OX1H2068( D,L17//( 7,L71247:0( Y%Z'( #876:5L6;7( L1BK164( Y&Z[P?( !:( 0F60( ;B:07,0'( "/01234( 560788207/( F6/( H77:(NB1<2:K(B:(BL0242U602B:(6:A(24L8747:0(BM( /7:/B1/( 6:A( :6>2K602B:( /B8302B:/( B:HB61A( 6( R7H12/( V74B>68( \7F2;87( :647A( ]GF7( R7H120B1^( N20F( 0F7( 462:( BHT7;02>7( 0B( 7:/317( F2KF( /6M70=( L1B,2420=( 46:73>71/?( !:( L6102;3861'( /L7;268( 6007:02B:( F6/( H77:( L62A( 0B( 0F7( A7/2K:( BM( 630B:B4B3/'( >2/2B:$H6/7A( :6>2K602B:( /B8302B:( MB1( 3:;BBL71602>7( 17:A7U>B3/( N20F( /L6;7( A7H12/?( GF7( L1BLB/7A( /B8302B:( 7:6H87/( 0F7( 7/024602B:( BM( ;F6/71( /0607/( N20F( 17/L7;0( 0B( % (!WV!"$_6K6A2;'(916:;7( & ("/01234(560788207/'(916:;7(

!"#$%&'(")'(*'(+'(,%-+..( !"#.( A7H12/'(3/2:K(6(/2:K87(;64716(O:B(6;02>7(/7:/B1/(

B1( /0717B( A7>2;7( 617( ;B:/2A717AP( 6:A( <:BN2:K( 0F7( QR( 4BA78( BM( 0F7( 061K70'( NF2;F( ;6:( H7( ;BBL71602>7( B1( :B0?( !M( 0F2/( /B8302B:( F6/( H77:( A7>78BL7A(MB1(A7H12/(174B>68'(20(2/(6LL82;6H87(0B( 861K71(M278A(N20F2:(0F7(2:$B1H20(/71>2;7/(AB462:?( ( !:( B1A71( 0B( 7/024607( 0F7( LB/7( BM( 6( ;64716( N20F( 17/L7;0( 0B( 6( /L7;2M2;( /;7:7'( ;B44B:( 4BA78$ H6/7A( 6LL1B6;F7/( 3/7( 720F71( LB2:0( YQZ'( 7AK7( M760317/( Y-'*')Z( B1( 6( ;B4H2:602B:( BM( HB0F( Y`'aZ?( DAK7( M760317/( BMM71( 6( KBBA( 2:>6126:;7( 0B( 288342:602B:( ;F6:K7/( B1( 246K7( :B2/7( 6:A( 617( L6102;38618=( /3206H87( N20F( LBB18=( 07,0317A( /;7:7/'( NF70F71( 0F7( /;7:7( 2/( 2:( 2:A3/01268'( B30ABB1( B1( 2:ABB1( 7:>21B:47:0/?( 9B1( /3;F( ;86//( BM( 6LL1B6;F7/'( 0F7( LB/7( ;B4L30602B:( 2/( 6;F27>7A( H=( 42:242U2:K( 0F7( A2/06:;7( H70N77:( 0F7( L1BT7;07A( 7AK7/( BM( 0F7( QR( 4BA78( 6:A( 0F7( ;B117/LB:A2:K( 7AK7( M760317/( 2:( 0F7( 246K7'( 7,016;07A( 0F6:</( 0B( 6( %R( /761;F( MB1( K16A27:0( 46,246(68B:K(0F7(4BA78(7AK7(:B1468/?((

(

5B8302B:( L17/7:07A( 2:( 0F2/( L6L71( F6/( H77:( A7>78BL7A( H=( !WV!"( 6:A( /L7;2682U7A( 0B( 17:A7U>B3/( L1BH874602;( ( 2:( ;B886HB1602B:( N20F( "/01234( 560788207/?( !0( H78B:K/( 0B( 7AK7( H6/7A( 016;<71/?( !0( 624/( 60( M38M2882:K( /7>7168( ;F6887:K7/'( 2:( 0714( BM( 6;;316;='( 1BH3/0:7//( 0B( B:$B1H20(

;B:A202B:/( OD610F( H6;<K1B3:A'( A217;02B:68(

288342:602B:'( ;B:/0162:7A( 016T7;0B127/[P( 6:A( B:$ HB61A(24L8747:0602B:(O1768$0247(7/L7;2688=P?(( ( !:( 6( M21/0( /7;02B:'( 0F7( /B8302B:( 2/( A7/;12H7A?( !:( 6( /7;B:A'(0=L2;68(L71MB146:;7/(617(L17/7:07A?( (

%%" /0#1)*23#4562*#!)62#26'-15'-)&#

( C1BLB/7A( /B8302B:( 2/( A2>2A7A( 2:0B( 0NB( /07L/b( 2:202682U602B:( 6:A( 016;<2:K?( IB0F( /07L/( 3/7( 0F7( <:BN:( QR( 4BA78( BM( 0F7( 061K70?( GF2/( 4BA78( ;6:( H7(0F7(#"R(4BA78(B1(6(/24L82M27A(B:7?( GF2/(/7;02B:(L1B>2A7/(0F7(B3082:7(BM(0F7(/B8302B:?( 9B1( 6( 4B17( A706287A( A7/;12L02B:'( L876/7( 17M71( 0B( Y+Z?(( (

%%"$" %&-'-53-75'-)&#

( !:202682U602B:( 624/( 60( A707;02:K( 0F7( 061K70( 2:( 6:( 246K7( /7S37:;7( 6:A( 60( L1B>2A2:K( 0F7( 016;<2:K( N20F(6:(2:20268(K37//(BM(0F7(061K70(LB/7'(N20FB30( 6:=(L12B1(2:MB14602B:(B:(0F7(LB/7?(!0(;B:/2/0/(2:( 460;F2:K( OA707;02B:c460;F2:K( /06K7P( 0F7( 246K7( ;B:0B31/( N20F( 6( A606H6/7( BM( >27N/( H3280( H7MB17( 0F7(42//2B:(O8761:2:K(/06K7P?(( ( 9//5"%)& 5)(0%"%-?( "( F27161;F2;68( 4BA78( >27N( K16LF( 876A2:K( 0B( L1B0B0=L7( >27N/( !"( BM( 0F7( 4BA78( 2/( H3280?( D6;F( :BA7( BM( 0F7( >27N( K16LF( ;B:062:/( 6:( 246K7( L1BT7;02B:( BM( 0F7( 061K70( ;B:0B31/( 60( 6( L6102;3861( LB2:0( BM( >27N?( GF7( LB2:0/( BM( >27N( 617( /64L87A( B:( 6( /LF717( O/77( 92K317(%P?(GF7(/64L82:K(BM(0F7(>27N/(2/(BL0242U7A( 0B(82420(0F7(474B1=(/2U7(BM(0F7(A606H6/7(6:A(0B( 2:/317( 0F7( NFB87( ;B>716K7( BM( 0F7( /L6;7( BM( LB//2H87(>27N/?( ( 9%5"%)&.(0-).&*).):."$%?(528FB37007(BM(0F7(061K70( 2/( 7,016;07A( 2:( 0F7( 246K7( 3/2:K( H286=71( /7K47:0602B:( 07;F:2S37/?( GF2/( 470FBA( ;B:/2/0/( 2:( 42:242U2:K( 6:( 7:71K=( M3:;02B:( ;B4H2:2:K(

!"#$%&'(")'(*'(+'(,%-+..( !"#/( 4B02B:( 6:A( ;B8B1'( 68B:K( N20F( 074LB168( 6:A( /L60268( L12B1/?( !0( 688BN/( A2/02:K32/F2:K( 0F7( MB17K1B3:A(/F6L7(M1B4(0F7(H6;<K1B3:A(6:A(F6/( 0F7(6A>6:06K7(0B(H7(1768$0247?( (

9%5"%)& 4(.:;"%-& (%*& <$#)& "%"."(5"=(."$%?( GF7( >27N( K16LF( 2/( 0F7:( 7,L8B17A( 0B( M2:A( 0F7( L1B0B0=L7(>27N(NFB/7(;B:0B31/(;B117/LB:A(0F7( 4B/0( 0B( 0F7( 7,016;07A( /28FB37007?( GF7( 3/7A( /24286120=( 47012;( A712>7/( M1B4( Y%.Z?( !0( ;B:/2A71/( HB0F( 0F7( A2/06:;7( 6:A( 0F7( B127:0602B:( BM( 7AK7/( 0B(460;Fb(

( )

_!

( )

( )

=

+

=

M k i k ori j i k j

c

d

c

d

M

j

i

s

1

1

_"

₍ d20F(

( )

2 2 ] ... 0 [

min

i i k i l i k n l i k j

cog

c

c

c

c

d

!

!

=

" (

( )

i k j c d (K2>7/(0F7(476:(A2/06:;7(MB1(76;F(;B:0B31( LB2:0( i k c (0B(0F7(;8B/7/0(B:7(2:(L1B0B0=L7(>27N(!"?( GF7( 47012;( 2/( :B14682U7A( H=( 0F7( A2/06:;7( H70N77:( i k

c ( 6:A( 0F7( ;7:01BeA( #$%&( BM( 0F7( /F6L7(

7,016;07A(M1B4(!"(0B(;BL7(N20F(/;687(;F6:K7/?( (

( )

( )

(

j

₍

_{( )}

₎

)

c d i k i k ori j i k j

c

ori

c

ori

c

d

arg

!

=

( GF2/(A2/06:;7(/06:A/(MB1(0F7(A2MM717:;7(H70N77:( 0F7( B127:0602B:( $'&( BM( 0F7( ;B:0B31( LB2:0( i k c ( 6:A( 0F7( B127:0602B:( BM( 0F7( ;8B/7/0( ;B:0B31( LB2:0(

( )

(

)

j c dj ki

c

arg (2:(!"?( ( X:;7( 0F7( ;8B/7/0( L1B0B0=L7( >27N( 2/( MB3:A'( 20/( 6//B;2607A(LB/7(2/(;B:/2A717A(6/(2:202682U602B:(BM( 0F7(061K70(LB/7?( GF7( 460;F2:K( /06K7( ;6:( H7( 160F71( 0247( ;B:/342:K?( GB( ;BL7( N20F( 1768( 0247'( 6( I6=7/26:( M1647NB1<(2/(/70(0B(/L176A(0F7(2:202682U602B:(B>71( /7>7168( 246K7/( O074LB168( 2:202682U602B:P?( !0( 7:6H87/( 0B( L1B>2A7( 6:( 3L( 0B( A607( LB/7( 2:202682U602B:(0B(0F7(JW#(/=/074?( ( >"-?0)&@&A&B0"%:"<5)&$/&"%"."(5"=(."$%C&)*+*',-(+&*./($0( ,(/12*'*(,'*(/*-*#3*4(3$(1'$45#*(1'$3$361*(+&*./( /3$'*4(&0(,(2&*','#2&#,-(7$4*-(+&*.(%',128(9,'%*3(&/( 32*0(*:3',#3*4(;6(/*%7*03,3&$0(,04(7,3#2*4($+*'( /5##*//&+*(<',7*/(.&32(#-$/*/3(1'$3$361*8( (

%%"." 8(5,9-&:#

( X:;7( 0F7( 061K70( F6/( H77:( A707;07A( 2:( 246K7'( 6:A( 20/( LB/7( F6/( H77:( 2:202682U7A'( 6( M1647( 0B( M1647(7AK7(016;<2:K(2/(L71MB147A?( _2<7(2:202682U602B:'(016;<2:K(2/(QR(4BA78(H6/7A?(!0( 624/( 60( M2:A2:K( 0F7( 061K70( LB/7( NF2;F( 46<7/( H7/0(460;F(0F7(L1BT7;02B:(BM(0F7(QR(4BA78(N20F( 0F7( 246K7( 7AK7/?( G16;<2:K( 6:A( LB/7( 7/024602B:( 617(0F3/(/243806:7B3/?(GF7=(617(L71MB147A(2:(6( M6/0( 2071602>7( N6='( H=( 42:242U602B:( BM( MB88BN2:K( ;1207126b(

( )

(

)

(

)

!

"

=

#

i i i

r

x

l

d

_,

_'

$

(

!"#$%&'(")'(*'(+'(,%-+..( !"#;( NF717(!( 2/( 6( 1BH3/0( 7/02460B1( 3/7A( 0B( 17T7;0(

B308271/(OG3;<7=(7/02460B1P'(6:A(

d

!

(

l

i

( )

r

,

x

i

'

)

(2/(

0F7( A2/06:;7( H70N77:( 6( LB2:0( :&=( 6:A( 0F7( ;B117/LB:A2:K(82:7(-&>'??( ( ( >"-?0)&D&E&6$,"%-&)*-)&<0"%:"<5)C&@'$7(32*(&0&3&,-( 1$/*('AB(CD(/*,'#2(,-$0%(32*(1'$"*#3*4(#$03$5'( 504*'-6&0%(32*(7*,/5'*7*03(1$&038(D&/3,0#*($<(,( 1$&03(:A&(3$(,(#$''*/1$04&0%(-&0*(-&(>'?(.&32&0(32*( 7&0&7&E,3&$0(1'$#*//8( ( ( ( (

O6P( OHP( O;P(

( ( (

OAP( O7P( OMP(

>"-?0)&F&A&B0"%:"<5)&$/&.0(:G"%-?(9',#F&0%(&/( 1*'<$'7*4(5/&0%(,(<',7*(>,?(,04(32*(GD(7$4*-($<(32*( 3,'%*3(>;?8(92*(/,-&*03(*4%*/($<(32*(3,'%*3(,'*( *:3',#3*4(>#?(,04(1'$"*#3*4(&03$(&7,%*(>4?B(%&+*0(,0( &0&3&,-(1$/*8(H$/*(&/(&3*',3&+*-6('*<&0*4(>*?(3$(7,F*( 1'$"*#3&$0(*4%*/(7,3#2(.&32(&7,%*(*4%*/(><?8( f:82<7( 2:202682U602B:'( 0F7( 7AK7( 460;F2:K( 2/( 8B;68?( "/(6(;B:/7S37:;7'(016;<2:K(13:/(2:(1768(0247(H30( 2/(87//(1BH3/0(0B(F2KF(A2MM717:;7/(H70N77:(7AK7/'( 476:2:K( 0F60( L17A2;07A( 061K70( LB/7( /F688( H7( ;8B/7(7:B3KF(O07:/(BM(L2,78/P(0B(1768(B:7?( (

%%%" <2(=)(15&,2#56626612&'#

( C17/7:07A( /B8302B:( F6/( H77:( L17>2B3/8=( 07/07A( B:(/60788207(4B;<3L(6:A(1768(246K7/(Y%%Z?(GF7/7( 07/0/( 7:6H87A( 0B( A74B:/01607( 0F60( 061K70( A707;02B:(6:A(016;<2:K(NB1<(B:(1768(246K7/'(H30( 0F7=( A2A( :B0( 7:6H87( 0B( 6//7//( S36:020602>7( L71MB146:;7/'(6/(:B(K1B3:A(0130F(N6/(6>6286H87?( GB( M288( 0F7( K6L'( L1BLB/7A( /B8302B:( F6/( H77:( 07/07A(B:(/=:0F702;(246K7/?(GF7(17/380/(BM(0F7/7( 07/0/(617(L17/7:07A(2:(0F2/(/7;02B:?( (

%%%"$" %15:2#6-1+35'-)&#

( 5=:0F702;( 246K7/( BM( A7H12/( 617( /2438607A( 3/2:K( 6:( 2:$FB3/7( 0BB8( ;6887A( 53117:A71g?( GF2/( 17:A7171( 2/( H6/7A( B:( ;86//2;68( 17:A712:K( M3:;02B:/( O16/0712U602B:'( 16=$016;2:K[P'( 6A6L07A( 0B( L7;3826120=( BM( /L6;7( 7:>21B:47:0( OM7N( BHT7;0/'( 8B:K( A2/06:;7/P( 6:A( A7H12/( L1BL71027/( OE_!(07,0317'(F2KF(/L7;386120=(BM(/31M6;7[P?(( 53117:A71g( 2/( ;B3L87A( N20F( 6( /24L82M27A( /L6;7( A=:642;/(/243860B1(0B(/2438607(016T7;0B127/(6:A( /60788207(A=:642;/(/3MM2;27:08=(17682/02;(MB1(07/02:K( L31LB/7?( ( 9B1( 0F7( L71MB146:;7( 6//7//47:0'( 0NB( 061K70/( 617( /2438607Ab( "126:7( -( 3LL71( /06K7( 6:A( 6( 5LB0( !071602B:( B:(C'(h(

!"#$%&'(")'(*'(+'(,%-+..( !"#>( M6428=( /60788207?( G=L2;68( ;8B/7( 16:K7( A2/06:;7/'(

M1B4(%..4(0B(%.4'(6:A(034H82:K(1607'(M1B4(.(0B( &( A7Kc/( O0=L2;68( 1607( BM( 861K7( A7H12/P'( 617( ;B:/2A717A?( GF7/7( L61647071/( F6>7( H77:( ;FB/7:( 6/( 0F7=( ;B117/LB:A( 0B( LB07:0268( M30317( 42//2B:(BM(A7H12/(174B>68?( (

%%%"." 826'6#(26+3'6#

( E2/;7886:7B3/( 07/0/( F6>7( H77:( ;61127A( B30( 0B( 6//7//( 0F7( L71MB146:;7/( BM( 0F7( /B8302B:( 6:A( 20/( 1BH3/0:7//( 0B( A2MM717:0( ;B:A202B:/( OL17/7:;7( BM( H6;<K1B3:A'(061K70(;B4L87,20=[P?( !:(0F2/(/7;02B:(N7(MB;3/(B:(0F177(17L17/7:0602>7( 07/0/b( 0NB( B:( 5LB0( 6:A( B:7( B:( "126:7( -( 3LL71( /06K7?(( ( 0 100 200 300 400 500 600 700 800 900 5 10 15 20 25 30 35 40 45 50 55 frame Po s it io n ( m ) retrieved truth ( -10 100 200 300 400 500 600 700 800 900 -0.5 0 0.5 1 1.5 2 frame ! P o s iti o n ( m ) X Y Z ( 0 100 200 300 400 500 600 700 800 900 -200 -150 -100 -50 0 50 100 150 200 frame Eu le r a n g le s ( d e g ) retrieved truth ( -200 100 200 300 400 500 600 700 800 900 -15 -10 -5 0 5 10 15 20 frame ! E u le r a n g le s ( d e g ) X Y Z ( >"-?0)&H&A&I<$.&(<<0$(:;&A&<$#)&)#."4(."$%8(>3$1( -*<3?(I/3&7,3&$0($<(E(#$$'4&0,3*(>-&0*($<(/&%23?($<( 3,'%*3(&0(#,7*',(<',7*8(>3$1('&%23?(I''$'($0(1$/&3&$08( >;$33$7(-*<3?('*3'&*+*4(I5-*'(,0%-*/8(>;$33$7('&%23?( *''$'($0(I5-*'(,0%-*/8( ( 921/0(07/0(;B:/2/0/(2:(6(/24L87(016:/8602B:(0BN61A/( 6( 1B0602:K( 5LB0( O&A7Kc/P'( N20F( 0F7( D610F( 2:( H6;<K1B3:A'(6/(/FBN:(B:(92K317(*?(!0(7:6H87/(0B( L1B>2A7( 0=L2;68( L71MB146:;7/( BM( 016;<2:K( 68KB120F4?(( ( ( ( ( 91647(.( 91647(--.( 91647(a`*( >"-?0)&J&A&I<$.&(<<0$(:;&E&"4(-)#C&J'**0(#$03$5'/( #$''*/1$04(3$(1'$"*#3*4(GD(7$4*-8(K<(#$03$5'/(,'*( ,-&%0*4(.&32(/1$3(#$03$5'/(&0(&7,%*B(1$/*(&/(.*--( */3&7,3*48( ( 92K317( -( /FBN/( 0F7( 7/024607A( LB/7( 2:( ;B4L612/B:( 0B( K1B3:A( 0130F?( D11B1( B:( LB/202B:( A7;176/7/( L1BLB102B:688=( 0B( A2/06:;7( 0B( 061K70'( M1B4( *.;4( 0B( *;4'( MB1( 6( A2/06:;7( 16:K2:K( M1B4( *.4(0B(*4?(!0(/FBN/'(6/(7,L7;07A'(0F60(711B1(B:( LB/202B:( 2/( A217;08=( L1BLB102B:68( 0B( 246K7( 17/B8302B:?( D/024602B:( 711B1( B:( 600203A7( 2/( 4B17( ;B:/06:0'( >61=2:K( 462:8=( H70N77:( i.?*A7K'( N20F( 6( 42:2434( 711B1( 176;F7A( NF7:( 0F7( /60788207( B;;3L=(0F7(NFB87(246K7'(N20FB30(/L2882:K(B30(20?( 921/0(07/0(612/7/(68/B(6(F61A(;B:M2K31602B:'(NF7:( /B861(6116=/(6:A(5LB0(HBA=(617(L71L7:A2;3861(0B( 246K7( L86:7( O/77( M1647( --.( BM( 92K317( *( 6:A( 6//B;2607A(711B1/(B:(92K317(-P?(( ( !:( 6( /7;B:A( 07/0'( 6( 1B0602:K( 5LB0( O&A7Kc/P( 2/( 016;<7A( A312:K( 6( 8B:K( B1H20( L712BA?( 92K317( )( 2883/01607/( 0F7( LB/7( 2:202682U602B:( BM( 0F2/( 07/0?( !46K7( 2/( M21/08=( /7K47:07A( 6:A( 2071602>78=( M2007A( N20F( L1B0B0=L7( >27N/( BM( F27161;F2;68( K16LF?( GF7( 2:20268( LB/7( ;B117/LB:A/( 0B( 0F7( LB/7( BM( 0F7( ;8B/7/0(L1B0B0=L7(>27N?(

!"#$%&'(")'(*'(+'(,%-+..( !"#?( ( O6P( ( OHP( ( O;P( ( OAP( ( O7P( >"-?0)&K&A&I<$.&5$%-&.0(:G"%-&E&"%"."(5"=(."$%C&K7,%*( /*%7*03,3&$0($0(#$-$'(,04(7$3&$0(&/(1*'<$'7*4(>,?8( L,3#2&0%(.&32(1'$3$361*(+&*./($<(+&*.(%',12(&/(32*0( 1'$#*//*4($0(/*+*',-(#$0/*#53&+*(<',7*/(>;(3$(*?8( ( ( 57;B:A( 07/0( /FBN/( 0F60( 016;<2:K( ;6:( H7( 1BH3/0( B>71( 6( 8B:K( 016;<2:K( /7S37:;7'( NF717( /3:( 288342:602B:(6:A(H6;<K1B3:A(OD610F(B1(:B0P(>61=( 6(8B0(O/77(92K317(`P?( ( C71MB146:;7/( BM( LB/7( 7/024602B:( 617( /FBN:( 2:( 92K317(a?(D11B1/(B:(LB/7(7/024602B:(617(S3207(0F7( /647(6/(MB1(07/0(B:7b(LB/202B:(7/024602B:(61B3:A( *.;4(MB1(6(061K70(H70N77:(&.4(6:A(`.4(OM1B4( -..L,(0B(%*.L,P(6:A(600203A7(711B1(61B3:A(%A7K?( CB/7( 7/024602B:( 2/( 711B:7B3/( NF7:( LB2:0( BM( >27N(2/(;8B/7(0B(0F7(F61A(;B:M2K31602B:(A7/;12H7A( MB1(M21/0(07/0?( ( ( ( ( ( ( >"-?0)&L&E&I<$.&5$%-&.0(:G"%-&E&"4(-)#C&J'**0( #$03$5'/(#$''*/1$04(3$(1'$"*#3*4(GD(7$4*-( ( ( 0 500 1000 1500 10 20 30 40 50 60 70 80 frame Po s it io n ( m ) retrieved truth ( -10 500 1000 1500 -0.5 0 0.5 1 1.5 2 2.5 3 frame ! P o s iti o n ( m ) X Y Z ( 0 500 1000 1500 -200 -150 -100 -50 0 50 100 150 200 frame Eu le r a n g le s ( d e g ) retrieved truth ( -200 500 1000 1500 -15 -10 -5 0 5 10 15 20 frame ! E u le r a n g le s ( d e g ) X Y Z ( >"-?0)&M&E&I<$.&5$%-&.0(:G"%-&<$#)&)#."4(."$%8(>3$1( -*<3?(I/3&7,3&$0($<(E(#$$'4&0,3*(>-&0*($<(/&%23?($<( 3,'%*3(&0(#,7*',(<',7*8(>3$1('&%23?(I''$'($0(1$/&3&$08( >;$33$7(-*<3?('*3'&*+*4(I5-*'(,0%-*/8(>;$33$7('&%23?( *''$'($0(I5-*'(,0%-*/8(

!"#$%&'(")'(*'(+'(,%-+..( !"#@( ( "( 0F21A( 07/0( ;B:/2A71/( "126:7( -( 3LL71( /06K7( 2:/076A(BM(5LB0(/60788207?(G16;<2:K(L71MB146:;7/( 617( S3207( 0F7( /647( A74B:/01602:K( 0F60( B31( /B8302B:( 2/( 1BH3/0( 0B( 061K70( /F6L7?( 92K317( +( /FBN/( 6:( 7,64L87( BM( 3LL71( /06K7( 246K7/?( D>7:( 2M( 07,0317( BM( /06K7( /31M6;7( /FB38A( A2MM71( M1B4( /2438607A( 246K7/'( 0F2/( 07/0( 46<7/( 3/( ;B:M2A7:0( B:( 0F7( 6H2820=( BM( B31( /B8302B:( 0B( 016;<( 863:;F71( 3LL71(/06K7/?(( ( ( ( >"-?0)&N&A&O0"(%)&H&?<<)0&#.(-)&.0(:G"%-C& (

%%%"/" A)1!+'5'-)&53#,)6'6#

X31( /B8302B:( 2/( ;BA7A( 2:( #jj( 6:A( 3/7/( JCf( M3:;02B:682027/?( X:( 6( /06:A61A( ;B4L3071'( 0F7( NFB87( 68KB120F4( O2:202682U602B:( j( 016;<2:KP( 2/( 0=L2;688=(L1B;7//7A(60(61B3:A(%*(ML/?(( W7>710F787//'( ;B4L30602B:( ;B/0( A7L7:A/( F2KF8=( B:(0F7(QR(4BA78(;B4L87,20='(0F7(246K7(/2U7(6:A( 0F7(2:071$M1647/(4B02B:?(

%B" A)&,3+6-)&#

( !:( 0F2/( L6L71'( 6( >2/2B:( H6/7A( /B8302B:( 0B( 6//7//( 0F7(LB/7(O600203A7(j(LB/202B:P(BM(6:(BHT7;0(2:(0F7( ;64716(M1647(F6/(H77:(L17/7:07A?(GF7(/B8302B:( :77A/( 4B:B;3861( ;64716( 246K7/( 6:A( 0F7( QR(

4BA78( O;B4L8707( B1( /24L82M27AP( BM( 0F7( BHT7;0( 6/( 2:L30?( B)0/$04(%:)#& 57>7168( 07/0/( F6>7( H77:( ;61127A( B30( B:( /2438607A( 246K7/?( GF7=( ;B:/2A71( 5LB0( /60788207( 6:A("126:7(-(3LL71(/06K7/(6/(061K70/?(G7/0/(/FBN( 0F7( KBBA( L71MB146:;7/( BM( LB/7( 7/024602B:?( G=L2;68(6:(711B1(B:(LB/202B:(2/(61B3:A(%k(BM(0F7( A2/06:;7( 0B( 061K70( OM1B4( %..4( 0B( *4( N20F( *%&,*%&( 246K7/P( 6:A( 6:( 711B1( B:( 600203A7( 87//( 0F6:(%A7K?(

(

9<)0(."$%(5&(<<5":(."$%&

GF7( 24L8747:0602B:( BM( 0F7( /B8302B:( 2/( 60( 6:( 6A>6:;7A( L1B0B0=L7( 87>78( 6:A( ;B38A( H7( 76/28=( 016:/M7117A( 0B( 6( K1B3:A$/7K47:0( 0B( 6//7//( BMM82:7( OH30( 2:( 1768( 0247P( 0F7( LB/7( BM( /L6;7( A7H12/(BH/71>7A(H=(6:(B:$B1H20(>7F2;87?(( X:(HB61A(6:A(;8B/7$8BBL(24L8747:0602B:(NB38A( :77A( 4B17( NB1<'( H30( 2/( /712B3/8=( MB17/77:( H=( "/01234( 560788207/( MB1( M30317( A7H12/( 174B>68( B1( B0F71(2:$B1H20(/71>2;2:K(42//2B:/?(( (

#

C2=2(2&,26#

( Y%Z((V?(I?(9127:A?(X1H2068(D,L17//(C1BK164( 534461=(6:A(E2//2B:(X>71>27N'( C1B;77A2:K/(BM(5C!D'(>B8?()+*a'(O&..aP'(LL?( )+*a.Q$%$)+*a.Q$%%?( ( Y&Z((#876:(5L6;7(N7H/207b( F00LbccNNN?7/6?2:0cGD#c#876:l5L6;7c( ( YQZ((J?(I87/71'(m?(C6/0614B>'(6:A(R?(5012;<71?( V768$0247(QA(;64716(016;<2:K(MB1(2:A3/01268( 63K47:07A(176820=(6LL82;602B:/?(nB31:68(BM( d5#J'(L6K7/(-`o*-'(&..*?(

!"#$%&'(")'(*'(+'(,%-+..( !"#D( ( Y-Z((R?J?(_BN7?(92002:K(L616470712U7A( 0F177A247:/2B:68(4BA78/(0B(246K7/?(!DDD( G16:/?(B:(C60071:(":68=/2/(6:A(E6;F2:7( !:07882K7:;7'(%QO*Pb--%o-*.'(E6=(%++%?( ( Y*Z((G?(R1344B:A(6:A(V?(#2LB886?(V768$0247( >2/368(016;<2:K(BM(;B4L87,(/013;0317/?(!DDD( G16:/?(B:(C60071:(":68=/2/(6:A(E6;F2:7( !:07882K7:;7'(&-O`Pb+Q&o+-)'(n38=(&..&?( ( Y)Z(("?!?(#B4LB10'(D?(E61;F6:A'(E?(C17//2KB30'( 6:A(9?(#F6347007?(V768$0247(461<7187//( 016;<2:K(MB1(63K47:07A(176820=b(0F7(>210368( >2/368(/71>B2:K(M1647NB1<?(!DDD(G16:/?(B:( \2/3682U602B:(6:A(#B4L3071(J16LF2;/'( %&O-Pb)%*o)&a'(n38=(&..)?( ( Y`Z((_?(\6;;F7002'(\?(_7L7020'(6:A(C?(936?( #B4H2:2:K(7AK7(6:A(07,0317(2:MB14602B:(MB1( 1768$0247(6;;31607(QA(;64716(016;<2:K?(!:( "#Ec!DDD(!:0?(5=4L?(B:(E2,7A(6:A( "3K47:07A(V76820='(!5E"Vp.-'(L6K7/(-ao*`'( "182:K0B:'(\"'(WB>74H71(&..-?( ( YaZ((D?(VB/07:(6:A(G?(R1344B:A?(93/2:K(LB2:0/( 6:A(82:7/(MB1(F2KF(L71MB146:;7(016;<2:K?(!:( !DDD(!:0?(#B:M?(B:(#B4L3071(\2/2B:'(>B8347( &'(L6K7/(%*.ao%*%*'(I72T2:K'(#F2:6'(&..*?( Y+Z(("?(C7020'(D?(E61;F6:A'(q?(q6:6:2?( \2/2B:$H6/7A(R707;02B:(6:A(G16;<2:K(MB1( 5L6;7(W6>2K602B:(2:(6(V7:A7U>B3/(#B:07,0?( !:071:602B:68(5=4LB/234(B:("102M2;268( !:07882K7:;7'(VBHB02;/(6:A("30B4602B:(2:( 5L6;7(O2$5"!V"5P'(57L074H71(&.%&( ( Y%.Z(#F12/026:(V72:H6;F71'(E600F26/(V370F71'( 6:A(rB1/0(I2/;FBM?(CB/7(7/024602B:(BM( <:BN:(BHT7;0/(H=(7MM2;27:0(/28FB37007( 460;F2:K?(!:(&.0F(!:071:602B:68(#B:M717:;7( B:(C60071:(V7;BK:202B:(O!#CVP'(&.%.?( ( Y%%Z("?(C7020'(W?(R7/L1s'(D?(E61;F6:A'(q?(q6:6:2'( 9?( #F6347007'( 5?( C1B>B/0'( J?( 986:A2:?( QR( EBA78$H6/7A( \2/368( 016;<2:K( MB1( 5L6;7( "30B:B4B3/( V7:A7U>B3/?( a0F( !:071:602B:68( D5"( #B:M717:;7( B:( J32A6:;7( t( W6>2K602B:( #B:01B8(5=/074/?(&.%%(