May 7, 2001 - a nonlinear observer suitable for direct integration is presented. Global ...... No satellite measurements were available from 200 400 s. observer ...
D Qrqolqhdu Revhuyhu iru Lqwhjudwlrq ri JSV dqg Lqhuwldo Qdyljdwlrq V|vwhpv Emûuqdu Yln dqg Wkru L1 Irvvhq Qruzhjldq Xqlyhuvlw| ri Vflhqfh dqg Whfkqrorj| Pd| :/ 5334 Devwudfw JSV dqg LQV kdyh frpsohphqwdu| surshuwlhv dqg wkh| duh wkhuhiruh zhoo vxlwhg iru lqwhjudwlrq1 Wkh lqwhjudwhg vroxwlrq r huv ehwwhu orqj whup dffxudf| wkdq d vwdqg0dorqh LQV/ dqg ehwwhu lqwhjulw|/ dydlodelolw| dqg frq0 wlqxlw| wkdq d vwdqg0dorqh JSV uhfhlyhu/ pdnlqj lw vxlwdeoh iru ghpdqglqj dssolfdwlrqv1 Wkh lqwhjudwhg owhu lv qrqolqhdu erwk lq vwdwh dqg phdvxuh0 phqwv/ dqg wkh h{whqghg Ndopdq0 owhu kdv ehhq xvhg zlwk jrrg uhvxowv/ exw lw kdv qrw ehhq suryhq joredoo| vwdeoh/ dqg lw lv dovr frpsxwdwlrqdoo| lqwhqvlyh/ hvshfldoo| zlwklq d gluhfw lqwhjudwlrq dufklwhfwxuh1 Lq wklv zrun d qrqolqhdu revhuyhu vxlwdeoh iru gluhfw lqwhjudwlrq lv suhvhqwhg1 Joredo h{srqhqwldo vwdelolw| ri wkh ruljlq ri wkh frpelqhg dwwlwxgh dqg yhorflw| huuru v|vwhpv lv suryhq dorqj zlwk urexvw vwdelolw| lq wkh suhvhqfh ri qrlvh dqg xqprghoohg g|qdplfv1
4
Lqwurgxfwlrq
Wkh frpsohphqwdu| ihdwxuhv ri Lqhuwldo Qdyljdwlrq V|vwhpv +LQV, dqg wkh Joredo Srvlwlrqlqj V|vwhp +JSV, duh wkh pdlq uhdvrqv zk| lqwhjudwhg JSV2LQV v|v0 whpv duh ehfrplqj lqfuhdvlqjo| srsxodu1 Wkh kljk/ orqj whup dffxudf| ri JSV fdq eh frpelqhg zlwk wkh kljk rxwsxw udwh/ kljk g|qdplf fdsdelolw|/ urexvwqhvv/ dqg uholdelolw| ri LQV1 Lq wkh fdvh ri JSV gurs0rxwv/ wkh LQV zloo eh fdoleudwhg zkhq frdvwlqj wkurxjk wkh rxwdjh1 Diwhu wkh rxwdjh/ wkh LQV fdq surylgh hv0 wlpdwhv ri srvlwlrq dqg yhorflw| wr wkh JSV uhfhlyhu wr vshhg xs uhdtxlvlwlrq1 JSV2LQV v|vwhpv wkhuhiruh r hu srvlwlrq/ yhorflw|/ dffhohudwlrq/ dwwlwxgh dqg dqjxodu yhorflw| phdvxuhphqwv zlwk kljk dffxudf|/ rxwsxw udwh/ dqg uholdelolw| lq rqh sdfndjh1 Vhyhudo ohyhov ri lqwhjudwlrq kdyh ehhq phqwlrqhg lq wkh olwhudwxuh +vhh h1j1 Skloolsv ) Vfkplgw +4 yG `W = Sodwirup yhorflw| ghfrpsrvhg lqwr wkh QHG iudph Wkh uhodwlyh dqjxodu yhorflw| ehwzhhq wzr iudphv lv ghvfulehg xvlqj dqjxodu yhorflw| yhfwruv1 Wkhvh yhfwruv zloo/ lq dgglwlrq wr vxshuvfulswv/ kdyh vxevfulswv vkrzlqj zklfk wzr iudphv wkdw duh urwdwlqj uhodwlyh wr hdfkrwkhu1 Iru h{dpsoh= $ qlh 5 U6 = Dqjxodu yhorflw| ri HFL uhodwlyh HFHI ghfrpsrvhg lqwr wkh QHG iudph1 Dgglwlrq ri wzr dqjxodu yhorflw| yhfwruv duh uh hfwhg lq wkh qrwdwlrq e| fdq0 fhoolqj wkh lqqhu vxevfulsw dv iroorzv= $qlq @ $qlh . $ qhq Wkh rughu ri wkh vxevfulsw lv uhyhuvhg zkhq wkh gluhfwlrq ri wkh urwdwlrq lv fkdqjhg/ l1h1 $ qlq @ $ qql Urwdwlrq ri d yhfwru lv zulwwhq= yq @ Uqe ye zkhuh wkh ERG\0 {hg yhorflw| ye lv ghfrpsrvhg lqwr wkh QHG iudph e| pxo0 wlsolfdwlrq zlwk wkh urwdwlrq pdwul{ Uqe 1 Furvv surgxfwv zloo eh zulwwhq lq pdwul{ irup xvlqj wkh vnhz0v|pphwulfdo pdwul{ rshudwru= V+$, @ ^$` Wkh prvw frpprq wudqvodwru| dqg urwdwlrqdo yhfwruv wkdw zloo eh xvhg ehorz lv vxppdul}hg lq Wdeoh 51
7
Iudph HFHI QHG QHG2HFHI ERG\2QHG HFHI2HFL
Yhfwruv uh @ ^{h > |h > }h ` 5 U6 yq @ ^yQ > yH > yG ` 5 U6 $ qhq @ ^$ Q > $ H > $ G ` 5 U6 $ eqe @ ^s> t> u` 5 U6 $ hlh @ ^3> 3> $ h ` 5 U6
Frpphqwv Fduwhvldq HFHI srvlwlrq Qruwk/ hdvw dqg grzq yhorflw|1 Ixqfwlrq ri yhklfoh yhorflw|1 Uroo/ slwfk/ dqg |dz udwh1 $ h lv dqjxodu yhorflw| ri Hduwk1
Wdeoh 5= Ryhuylhz ryhu yhfwruv xvhg lq wklv sdshu1
5
Vwudsgrzq Htxdwlrqv
Srvlwlrq/ yhorflw| dqg dwwlwxgh yduldeohv iurp dq LQV duh ghulyhg iurp phdvxuh0 phqwv ri dffhohudwlrq qrplqdoo| pdgh dorqj wkh wkuhh ruwkrjrqdo d{hv lq wkh ERG\ iudph/ dqg iurp dqjxodu yhorflw| +j|urvfrsh, phdvxuhphqwv ri urwdwlrq ri wkh HFL iudph uhodwlyh wr wkh ERG\ iudph1 Iru dssolfdwlrqv rq wkh Hduwk/ wkh lqhuwldo iudph fdq eh zhoo dssur{lpdwhg e| wkh HFL iudph1 Wkh JSV phd0 vxuhphqwv duh jlyhq lq wkh HFHI iudph/ zkloh wkh LPX phdvxuhphqwv duh jlyhq lq wkh erg| iudph1 Wkh ehvw fkrlfh ri uhihuhqfh iudph iru lqwhjudwlrq lv ghshq0 ghqw iurp wkh dssolfdwlrq dqg wkh sulrulwlhv ri wkh v|vwhp lqwhjudwru1 Lq wklv sdshu wkh revhuyhu yhorflwlhv zloo eh jlyhq lq wkh QHG iudph/ zkloh srvlwlrqv duh jlyhq lq wkh HFHI iudph1 Orqjlwxgh dqg odwlwxgh fdq hdvlo| eh fdofxodwhg iurp HFHI srvlwlrqv xvlqj zhoo nqrzq dojrulwkpv/ vhh h1j1 Kripdqq0Zhoohqkri/ Olfkwhqhjjhu ) Froolqv +4 % ` > 5 U> % 5 U6 Wkh gl huhqwldo htxdwlrqv ghvfulelqj urwdwlrq ri wkh ERG\0iudph uhodwlyh wr wkh QHG0iudph lv= 4 b %W +5, tb @ @ $eqe %b 5 L . V+%, Huuru iuhh j|urv phdvxuh wkh urwdwlrq ri wkh ERG\ iudph uhodwlyh wr wkh HFL iudph ghfrpsrvhg lqwr wkh ERG\ iudph/ l1h1 $ele = Wkxv/ $ eqe fdq eh irxqg iurp= $ eqe @ $ ele $ elq @ $ ele +$ elh . $ehq , +6, Dsso|lqj +5, dqg +6, jlyhv wkh xqlw txdwhuqlrq gl huhqwldo htxdwlrqv= 4 4 b %W %W e @ $le +$ qlh . $ qhq , %b 5 L . V+%, 5 L V+%, 4 4 Wt +t,$ ele +t,+$ qlh . $qhq , @ 5 5 8
+7, +8,
zkhuh pdwulfhv Wt +t, dqg +t, kdv ehhq lqwurgxfhg iru hdv| uhdglqj1 $qlh dqg $qhq fdq eh fdofxodwhg iurp +45, dqg +44,/ uhvshfwlyho|1 Srvlwlrq dqg Yhorflw| Htxdwlrqv Wkh QHG0iudph lv wkh prvw srsxodu phfkdql}dwlrq iru qdyljdwlrq v|vwhpv iru dssolfdwlrqv rq dqg forvh wr wkh vxuidfh ri Hduwk1 Wkh gl huhqwldo htxdwlrqv iru QHG0iudph yhorflw| fdq eh irxqg lq h1j1 Eulwwlqj +4 ,` $ ele e4 z4 e @ ^L +> ,` i e z +47, i lpx
5
:
8
zkhuh
5
v{ @ +v> !, @ 7 !|{ !}{
!{| v| !}|
6 !{} !|} 8 v}
W Khuh v @ ^v{ > v| > v} ` duh wkuhh vfdoh idfwru huuruv dqg
W ! @ !{| > !{} > !|{> !|} > !}{ > !}| duh vl{ vpdoo plvdoljqphqw dqjohv1 e4 5 U6 dqg e5 5 U6 uhsuhvhqw j|ur dqg dffhohurphwhu eldvhv/ uhvshfwlyho|1 z4 5 U6 dqg z8 5 U6 duh erxqghg xqprg0 hohg huuruv dqg phdvxuhphqw qrlvh1 Vlqfh wkh huuruv duh vpdoo/ wkh iroorzlqj dssur{lpdwlrq fdq eh pdgh= $ ele
4
@ ^L +> ,` +$ lpx . e4 . z4 , ^L . +> ,` +$ lpx . e4 . z4 , ^L . +> ,` $lpx . e4 . z4
zkhuh kljkhu rughu whupv duh uhpryhg1 Wklv dssur{lpdwlrq lv pdgh lq rughu wr pdnh wkh qrqolqhdu dqdo|vlv hdvlhu1 Wkxv/ wkh LPX huuruv zloo eh prghoohg dv= $ ele @ ^L . +> ,` $ lpx . e4 . z4
+48,
i e @ ^L . +> ,` ilpx . e5 . z8
+49,
Qrwlfh wkdw doo huuru vljqdov duh ghfrpsrvhg lq wkh erg| iudph1 Wkh pdjqlwxgh ri wkhvh huuruv duh lq jhqhudo gluhfwo| sursruwlrqdo wr wkh sulfh ri wkh LPX1 Wkh j|ur huuru prghov duh dvvxphg wr eh ghvfulehg e| wkh 4vw0rughu prghov= 4 eb 4 @ W 4 e4 . z5 b @ W5 4 . z6 4 b @ W 6 . z7
+4:,
gulyhq e| Jdxvvldq zklwh qrlvh vljqdov z5 > z6 5 U6 dqg z7 5 U9 1 Wkh pdwulfhv W4 > W5 5 U66 dqg W6 5 U99 duh gldjrqdo pdwulfhv ri wlph frqvwdqwv/ @ ^{ > | > } `W duh wkuhh j|ur vfdoh idfwru huuruv/ dqg @ ^{| > {} > |{ > |} > }{ > }| `W duh vl{ vpdoo j|ur plvdoljqphqw dqjohv1 Wkh dffhohurphwhu huuruv duh ghvfulehg dffruglqjo|= 4 eb 5 @ W 7 e5 . z9 4 b @ W 8 . z: b @ W9 4 . z;
;
+4;,
6
JSV2LQV Dwwlwxgh dqg Yhorflw| Revhuyhu
E| frpelqlqj b @ %b eb 4 @
+8,/ +48, dqg +4:,/ wkh huuru iuhh v|vwhp fdq eh zulwwhq= 4 4 Wt +t, ^+L . +> ,, $ lpx . e4 . z4 ` +t,+$ qlh . $qhq , 5 5
4 W 4 e4 . z5 b @ W5 4 . z6 4 b @ W 6 . z7
+4 5> 6, dqg N7 5 U99 > duh jdlq pdwulfhv1 Wkh txdwhuqlrq W W lv rewdlqhg e| frpelqlqj wkh txdwhuqlrq hvwlpdwh zlwk d uhihuhqfh huuru > % phdvxuhphqw iurp d JSV dwwlwxgh duud|/ ru rwkhu dydlodeoh dwwlwxgh phdvxuh0 phqwv1 Xvxdoo|/ dffxudwh uroo dqg slwfk uhihuhqfhv fdq eh rewdlqhg iurp wkh dffhohurphwhuv lq lqfolqrphwhu prgh dlghg e| JSV srvlwlrq ru yhorflw|1 D kljk dffxudf| LQV fdq dovr vxsso| dq dffxudwh khdglqj uhihuhqfh iurp j|urfrpsdvv0 lqj1 JSV dwwlwxgh gdwd fdq eh frpelqhg zlwk wkh txdwhuqlrq hvwlpdwh hlwkhu e| vxsso|lqj txdwhuqlrqv +orrvho| frxsohg,/ ru gl huhqwldo skdvh gdwd +wljko| frxsohg,1 Wkh huuru iuhh srvlwlrq2yhorflw| v|vwhp lv irxqg e| frpelqlqj +43d,/ +43e,/ +49, dqg +4;,= yb q @ Uqe ^+L . 5 ,ilpx . e5 . z8 ` . jq ^5V+$ qlh , . V+$ qhq ,` yq ub h @ Uhq yq 4 eb 5 @ W 7 e5 . z9 4 b @ W 8 . z: b @ W4 . z;
+54,
9
4 ib @ i . z< w: b @ i . z43
= = = > ;`,/ dqg N< 5 U99 duh srvlwlyh gh qlwh jdlq pdwulfhv/ q / / dqg i duh rewdlqhg e| dqg n43 dqg n44 duh srvlwlyh vfdodu jdlqv1 uh / y frpelqlqj uhihuhqfh gdwd iurp JSV zlwk wkh uhvshfwlyh hvwlpdwhv1 Lq d wljkwo| frxsohg v|vwhp/ JSV zrxog vxsso| svhxgrudqjhv dqg ghowdudqjhv/ zkloh srvl0 wlrq/ yhorflw| dqg forfn gdwd zrxog eh vxssolhg lq d orrvho| frxsohg v|vwhp1 Erwk ri wkh deryh revhuyhuv kdyh ehhq ghvljqhg e| phdqv ri d O|dsxqry vwd0 elolw| dqdo|vlv1
614
Vwdelolw| Dqdo|vlv
Wkh qrqolqhdu huuru prgho iru wkh dwwlwxgh revhuyhu lv irxqg iurp +4 i , @ I+a t> t %
a qe ^ > i , @ 5U L . V+ %,` V+i , I+a t> t
Wkhruhp 4 Li wkh frqglwlrq +63, lv vdwlv hg/ wkh wrwdo huuru v|vwhp +56, dqg +57, lv JHV1 Frqvlghu wkh iroorzlqj O|dsxqry Ixqfwlrq Fdqglgdwh iru +56,= li 3 W % + 4,5 . % 4 W 4 W 4 W . N7 . N6 Y4 @ e4 N5 e4 . 5 li ? 3 W % + . 4, . %
+58,
Wkh ghulydwlyh dorqj wkh wudmhfwru| ri +56, lv=
5ab li 3 5ab li ? 3 4 4 4 4 W W4 N4 e 4 5 5 W W % W N4 % @ 5e W W 4 4 5 5 N6 6 N7 3 +59,
4b 4b W N4 e b 4 . 5 W N Yb 4 @ 5e W N . 5 . 4 5 6 7
44
zkhuh lw lv xvhg wkdw= W a%b 3 ab . % $ W gldj+ W @ %, $ % 4 lpx
lpx
W + . %, $ lpx
+5:, +5;,
Ohw Y5 eh gh qhg dv= 4 W 4 4 5 4 5 4 q W q 4 h W h 4 W 4 4 4 . +u , u . e5 N: e5 . W N y , y . N< . i . Y5 @ + ; 5 5 5 5 5 5 5 Wkh ghulydwlyh ri Y5 dorqj +57, lv= b 5 . W N4b . b . iib . b W N4 W N4 e b q . +uh ,W ub h .e yq ,W y Yb 5 @ + 5 : ; < k l q 5 ilpx . e 5 +5V+$ q , . V+$ q ,, y a qe q I+a > i , t> t @ + yq ,W U lh hq N8 y q N9uh ` + yq ,W +Uhq ,W uh . +uh ,W ^Uhq y k l W N4 W4 e 5 . +U a qe ,W y a qe ,W y q W ilpx +U q e 5 : 7 q W N4 W4 W 4 4 W W +i ,+U a q ,W y ; 8 N< W9 e lpx +w4 . n43 ,i i n44 5 . i :
5 W5 N4 W4 e q + > i , yq ,W I+a t> t % +uh ,W N9uh e @ + yq ,W N8 y : 7 W 4 . n ,i5 n 5 N4 W4 +w W N4 W4 ;
8
@ +{, ? 3 ; { 9@ 3
i , N8 y y 5 I+a A3 +63, 4 IW +a % % t > t > i , N 4 5 E| fkrrvlqj N4 dqg N8 srvlwlyh gh qlwh/ wklv frqglwlrq fdq hdvlo| eh vdwlv hg > i ,n lv erxqghg1 Qrwh wkdw Yb 4 dqg wkxv Y lv qrw vwulfwo| qhjdwlyh1 vlqfh nI+a t> t 4/ Yb lv vwulfwo| qhjdwlyh dorqj wkh W % Krzhyhu/ zlwk wkh frqvwudlqw 5 . % vroxwlrq ri wkh v|vwhp1 Pruhryhu/ wkh frqvwudlqw dovr jlyhv JHV1 Wklv frpsohwhv wkh surri1
615
Urexvw Vwdelolw|
Wkh deryh uhvxow fdq eh h{whqghg wr lqfoxgh urexvwqhvv wr qrlvh dqg rwkhu erxqghg glvwxuedqfhv dqg prghoolqj huuruv1 Zkhq qrlvh dqg rwkhu erxqghg huuruv/ olnh xqprghohg dffhohurphwhu2j|ur huuruv dqg JSV pxowlsdwk/ duh lq0 foxghg/ wkh huuru prgho +56, ehfrphv= 45
k l . z4 N4 + 4 $ lpx . e b @ Wt + t, %.K| 4 , vjq+ , t 4 N5 + b 4 @ W4 e %.K| 4 , vjq+ , . z5 e 4 5 4 4 N6 gldj+ %.K| 4 , $ lpx vjq+ , . z6 b @ W 5 5 4 4 N7 + %.K| 4 ,$ lpx vjq+ , . z7 b @ W 6 5
+64,
zkloh wkh huuru prgho +57, ehfrphv= k l 5 . z8 . +Uq U 5 ilpx . e a qe a qe ,i ^5V+$ qlh , . V+$ qhq ,` y q b q @ U y e q . +Hh ,| 5 +Uhq ,W uh . +Hh ,| 6 N8 y ub h @ Uhq y q N9 uh . +Hh ,| 6 q ,W y q . +Hh ,| . z b 5 @ W4 e 5 N: +U a e 9 e 7 5 4 a qe ,W y q . +Hh ,| 5 . z: +65, N; gldj +ilpx , +U b @ W 8 q N< W +i ,+U b @ W4 a ,W y q .+Hh ,| 5 . z; 9 e lpx i . +hh ,| . +hh ,| . z ib @ w4 i n :
43
5
b @ i n44 . +hh ,| 6 . z43
6
+ W > W > W > W > i> / wkh W > e Gh qh wkh yhfwru { @ > % yq ,W > +uh ,W >e 4 > 5 pdwulfhv 6 5 3 3 N4 3 4 4 : 9 3 3 3 5N 5 W4 : T4 @ 9 4 4 8 7 3 W 3 3 5N 6 5 4 4 3 3 3 5N7 W6 6 5 N8 3 3 3 3 3 3 9 3 3 3 3 3 : L66 3 : 9 4W4 3 : 9 3 3 3 3 3 N : 7 : 9 4W4 3 : 3 3 3 3 3 N T5 @ 9 ; 8 : 9 4W4 3 : 9 3 3 3 3 3 N < 9 : 9 4 8 7 3 . n 3 3 3 3 3 w 43 : 3 3 3 3 3 3 n44 5 4 6 > i , 3 3 3 3 3 3 t> t 5 I+a 9 3 3 3 3 3 3 3 : : T6 @ 9 7 3 3 3 3 3 3 3 8 3 3 3 3 3 3 3 46
T@
T4 TW6
T6 T5
dqg wkh yhfwruv 6 N4 K| 4 4 | : 9 N 5 z5 . K 4 : q4 @ 9 4 | 8 7 N 6 z6 . gldj+$ lpx ,K 4 4 | N7 z7 . +$lpx ,K 4 5 N8 +Hh ,| 5 . +Uhq ,W +Hh ,| 6 | 9 N9 +Hh , 6 9 9 a q ,W +Hh ,| 5 9 N4 z . +U e 9 4 : 9 a q ,W +Hh ,| 5 q5 @ 9 N; z: . gldj+ilpx ,+U e 9 4 9 N z; . +ilpx ,+U a q ,W +Hh ,| 5 < e 9 7 n43 +hh ,| 5 . z< . +hh ,| 6 n44 +hh ,| 6 . z43 W q @ qW4 qW5 5
6 : : : : : : : : : 8
Wkh yhfwru q lv erxqghg e| nqn qpd{ vlqfh zl dqg l duh dvvxphg erxqghg1 Wkh iroorzlqj froorudu| fdq qrz eh suryhq1 Fruroodu| 5 Wkh vroxwlrq {+w, ri wkh huuru htxdwlrqv +64, dqg +65, lv xqlirupo| xowlpdwho| erxqghg zlwk wkh erxqg n{n nT4 qpd{ n Iroorzlqj wkh rxwolqh ri wkh surri ri Wkhruhp 4 deryh zh jhw= Yb {W T{ . {W q Wkxv/ zkhq Nl > l 5 ^4> = = = > 7` duh fkrvhq vxfk wkdw T lv srvlwlyh gh qlwh/ wkh vroxwlrq {+w, zloo frqyhujh wr wkh edoo n{n nT4 qpd{ n
7
Fdvh vwxg|
Lq wklv vhfwlrq/ zh zloo vkrz wkh shuirupdqfh ri wkh frpelqhg dwwlwxgh/ srvlwlrq dqg yhorflw| revhuyhu lq wkh fdvh ri vfdoh0idfwru dqg eldv hvwlpdwlrq1 Lq wkh vlpxodwlrqv ehorz/ wkh judylw| yhfwru zdv {hg wr jlyh d vwdeoh yhuwlfdo fkdqqho1 Qr dwwhpsw zdv pdgh wr frqvwudlq wkh khljkw gxulqj JSV rxwdjhv1 Wkh wuxh fruuhodwlrq wlphv iru wkh doprvw frqvwdqw sduw ri wkh eldvhv zhuh fkrvhq odujh/ wkdw lv/ lq wkh udqjh ri 43 333433 333 vhfrqgv1 Wkh wlph frqvwdqwv xvhg lq wkh
47
Attitude error [deg]
1.5 1 0.5 0 -0.5
0
50
100
150
200
250
300
350
400
0
50
100
150
200
250
300
350
400
0
50
100
150
200 Time
250
300
350
400
Position error [m]
5 0 -5 -10 -15
Velocity error [m/s]
0.4 0.2 0 -0.2 -0.4
Iljxuh 6= Dwwlwxgh/ HFHI srvlwlrq/ dqg QHG yhorflw| huuruv zkhq vfdoh0idfwru huuruv duh qrw hvwlpdwhg1 Wkh vrolg olqhv duh wkh uvw d{hv +uroo/ HFHI {0d{lv/ dqg qruwk uhvshfwlyho|,/ wkh grwwhg olqhv duh wkh vhfrqg d{hv/ dqg wkh gdvkhg olqhv duh wkh wklug d{hv1 Qr vdwhoolwh phdvxuhphqwv zhuh dydlodeoh iurp 533733 v1 revhuyhu zhuh fkrvhq zurqj e| d idfwru ri xs wr whq/ exw wkh huuruv lq wkh wlph frqvwdqwv kdg qr vljql fdqfh gxulqj wkh wlph wkh vlpxodwlrqv odvwhg1 Vfdoh0idfwru huuruv fdq jlyh vxevwdqwldo frqwulexwlrqv wr srvlwlrq/ yhorflw| dqg dwwlwxgh huuruv gxulqj kljko| g|qdplf pdqhxyhuv1 Iru h{dpsoh/ li d plvvloh lv dffhohudwlqj dw 53 p@v5 iru 83 vhfrqgv zlwk d vfdoh0idfwru huuru ri 433 ssp/ wkh uhvxowlqj yhorflw| huuru zloo eh 314 p2v/ zkloh wkh srvlwlrq huuru zloo eh 518 p1 Wklv lv qrw yhu| lpsruwdqw lq fdvhv zkhuh frqwlqxrxv JSV phdvxuhphqwv duh dydlodeoh1 Exw lq wkh fdvh ri JSV rxwdjh gxulqj pdqhxyhuv/ wkh huuruv zloo lq jhqhudo exlog xs iru hdfk pdqhxyhu1 Wkh h{fhswlrq wr wklv lv dssolfdwlrqv zlwk v|pphwulf ru shulrglf pdqhxyhuv/ vlqfh huuruv zloo fdqfho rxw1 Lq wklv fdvh/ rqo| vfdoh0idfwru xqv|pphwu| zloo eh d idfwru1 Vfdoh0idfwru huuruv duh rqo| revhuydeoh gxulqj dffhohudwlrq ru dwwlwxgh fkdqjhv1 Wkh ehvw zd| ri hvwlpdwlqj wkhp/ lv wr shuirup fhuwdlq pdqhxyhuv rq sxusrvh1 Lq wklv zd|/ wkh owhu fdq eh wxqhg wr jhw wkh ehvw hvwlpdwh iru wkh wlph d jlyhq pdqhxyhu odvwv1 Lw lv dovr dq dgydqwdjh wr hvwlpdwh eldvhv wr d fhuwdlq df0 fxudf| ehiruhkdqg/ vrphwklqj zklfk fdq eh dffrpsolvkhg lq prvw fdvhv/ vlqfh qr pdqhxyhulqj lv qhhghg wr revhuyh wkh eldvhv1 Li eldvhv duh qrw hvwlpdwhg ehiruhkdqg/ pruh wlph dqg pdqhxyhuv duh qhfhvvdu| wr hvwlpdwh eldvhv dqg vfdoh0idfwruv vlpxowdqhrxvo|1 Wr vkrz wkh h hfw ri vfdoh0idfwru huuruv/ zh vlpxodwhg d yhklfoh xqghu frxuvh
48
Eldv Vfdoh0idfwru huuru Qrlvh
Dffhohurphwhuv 4 pj 3136 ( 314 pj
J|urv 31335 udg@v 316 s( 315 udg@ k
Wdeoh 6= Vlpxodwhg LPX gdwd Attitude error [deg]
1 0.5 0 -0.5
0
50
100
150
200
250
300
350
400
0
50
100
150
200
250
300
350
400
0
50
100
150
200 Time
250
300
350
400
Position error [m]
5 0 -5 -10
Velocity error [m/s]
0.3 0.2 0.1 0 -0.1
Iljxuh 7= Dwwlwxgh/ HFHI srvlwlrq/ dqg QHG yhorflw| huuruv zkhq vfdoh0idfwru huuruv duh hvwlpdwhg1 Wkh vrolg olqhv duh wkh uvw d{hv +uroo/ HFHI {0d{lv/ dqg qruwk uhvshfwlyho|,/ wkh grwwhg olqhv duh wkh vhfrqg d{hv/ dqg wkh gdvkhg olqhv duh wkh wklug d{hv1 Qr vdwhoolwh phdvxuhphqwv zhuh dydlodeoh iurp 533733 v1 fkdqjlqj pdqhxyhuv1 Lq wkh vlpxodwlrqv/ gdwd iurp d orz frvw LPX vkrzq lq Wdeoh 6/ zdv xvhg1 Dffhohurphwhu vfdoh0idfwru huuruv zhuh qrw hvwlpdwhg/ vlqfh wkh h hfw ri wkhp zhuh plqru1 Wkh wudmhfwru| zdv uhshdwhg diwhu 533 v/ dqg orfn lv orvw rq doo vdwhoolwhv iru wkh odvw 533 vhfrqgv1 Wkh pdqhxyhuv vwduwhg dw 73 vhfrqgv/ jlylqj wlph iru wkh j|ur eldvhv wr frqyhujh ehiruh wkh vfdoh0idfwru huuruv ehfdph revhuydeoh1 Iljxuh 6 vkrzv wkh SYD huuruv wkdw exlow xs gxulqj wkh rxwdjh zkhq eldv hvwlpdwlrq zdv lqfoxghg/ exw vfdoh0idfwru huuru hvwlpdwlrq zdv rplwwhg1 Wkh huuru wkdw exlogv xs lq dwwlwxgh wkh uvw 53 vhfrqgv lv gxh wr j|ur eldv/ zkloh wkh huuru lq |dz diwhu derxw :3 vhfrqgv lv gxh wr wkh |dz udwh vfdoh0idfwru huuru1 Iru wkh fdvh zkhuh vfdoh0idfwru huuruv zhuh hvwlpdwhg/ wkh SYD uhvsrqvh lv vkrzq lq Iljxuh 71 D fohdu gl huhqfh lq khdglqj huuru fdq eh vhhq/ lq dgglwlrq wr d vpdoohu huuru lq srvlwlrq gxh wr wkh pruh dffxudwh khdglqj1 Wkh j|ur eldv 49
0
x 10
-3
Gyro scalef. errors
-0.5 -1 -1.5 -2 -2.5 -3 -3.5
5
0
x 10
50
100
150
200
250
300
350
400
50
100
150
200 Time[s]
250
300
350
400
-3
Gyro biases [rad/s]
4 3 2 1 0 -1
0
Iljxuh 8= Xsshu sorw= J|ur vfdoh0idfwru huuruv lq eodfn/ dqg wuxh vfdoh0idfwru huuruv lq juh|1 \dz udwh huuru lv vrolg/ zkloh uroo dqg slwfk udwh huuruv duh gdvkhg dqg grwwhg uhvshfwlyho|1 Orzhu sorw= J|ur eldv hvwlpdwhv duh lq eodfn/ zkloh wkh wuxh ydoxhv . j|ur qrlvh lv lq juh|1 Hvwlpdwlrq lv wxuqhg r dw 533 v zkhq JSV phdvxuhphqwv duh orvw1 dqg vfdoh0idfwru huuru hvwlpdwhv duh vkrzq lq Iljxuh 81 Gxh wr yhu| olwwoh uroo dqg slwfk prwlrqv/ rqo| wkh |dz udwh vfdoh0idfwru huuru hvwlpdwh kdv frqyhujhg forvh wr wkh wuxh ydoxh1
8
Frqfoxvlrqv
Lq wklv sdshu/ vrph ri wkh ihdwxuhv ri lqwhjudwhg lqhuwldo dqg vdwhoolwh qdyljdwlrq v|vwhpv duh ghvfulehg1 D frpelqhg dwwlwxgh dqg srvlwlrq2yhorflw| revhuyhu iru wkh sxusrvh ri lqwhjudwlrq kdv ehhq suhvhqwhg1 Wkh revhuyhu lv vxlwdeoh iru gluhfw lqwhjudwlrq dufklwhfwxuhv/ dv rssrvhg wr wkh h{whqghg Ndopdq0 owhu1 Frpsduhg wr wkh odwwhu/ wkh revhuyhu kdv pdmru frpsxwdwlrqdo dgydqwdjhv lq d gluhfw dufkl0 whfwxuh1 Wkh gluhfw dssurdfk lv/ lq rxu rslqlrq/ pruh lqwxlwlyh dqg ohvv frpsoh{ wkdq wkh lqgluhfw dssurdfk hpsor|hg e| prvw rwkhu ghvljqv/ dqg wkh revhuyhu lv dovr d olwwoh ohvv frpsxwdwlrqdoo| ghpdqglqj wkdq wkhvh1 Wkh ruljlq ri wkh revhuyhu huuru v|vwhp kdv ehhq suryhq joredoo| h{srqhqwldoo| vwdeoh/ zkloh wkh srsxodu h{whqghg Ndopdq0 owhu rqo| kdv ehhq suryhq orfdoo| h{srqhqwldoo| vwd0 eoh1 Ilqdoo|/ urexvw vwdelolw| lq wkh suhvhqfh ri qrlvh dqg xqprghoohg h hfwv kdyh ehhq suryhq1
4:
Uhihuhqfhv Eulwwlqj/ N1 U1 +4
