104
INS and GPS integration Casper Ebbesen Schultz Kgs. Lyngby 2006 IMM-M.Sc-2006-60

INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

  • Upload
    others

  • View
    1

  • Download
    0

Embed Size (px)

Citation preview

Page 1: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

INS and GPS integration

Casper Ebbesen Schultz

Kgs. Lyngby 2006

IMM-M.Sc-2006-60

Page 2: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

Technical University of Denmark

Informatics and Mathematical Modelling

Building 321, DK-2800 Lyngby, Denmark

Phone +45 45253351, Fax +45 45882673

[email protected]

www.imm.dtu.dk

ISSN 1601-233X

Page 3: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

!" #$

% #% #%&$

#' # $$$

% # # % (# ')&%

* $&

')&#%# '+

% !" ,&% #%

$ $#% $ ')&%

#$ $& #$ '

$$$&$ %#%

##%&%%'

-$##'

$.$$ $#

%# $ '

$ &$ #

# #% $ #% #$

'- $/ $ . $

$$ '

&** #%0'+ ' $##

#1 #$$'-*

$ $ # $$

Page 4: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

!"2%$3 *!32"%#&#1

%'

#*& ##$ 4 #3# $

5 2%$6 6#78 & &

# $'5#3'9 4 # #$ ##

4 #: $&3'*%# #

&: $&#% &$#

# & *$('4 *

##$ 66 7;'< #&&

$'

6!2%$6 "3 *

!2%$3 *4 "$ &

$#%&'&*#756,#'

=$

4 3 *

6#5.>;?

Page 5: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

7

3

!"%@ % #A %

* %$ #'3% # @ **$*

$'*' A % $ B 1#@ @ * A '

* * %**1 @ *% A

B 1 '%$*** %%% #A #'

% !"% %1 % %*

$ # ' $1

$B # '$1 A % # #A '

$% $$*% A @ $1$#A #

A $ ##

# '5%*$ $#

A '3#1* $$$* A $

A B #@ #A # #%

%@ $$ '

C$% $ *$B 1 $

# %%**

'5%*$$#B

* $*% $* '

%** %10'+ ' $B %1

$ *#1* #A '5%%1 A **$

$ **#A $ * * !"3 ***

Page 6: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

=

2%!32"1# #1*%*$ $ **

A '

5#** A A $*$4 ##%3# $ 5

2%$6 6 1 78 A % $A '

#*3'9 4 # % $$A #4

## : # 3'*%1# %*B :

*$$ *B 1# %$%#B A

' ** %1 ** $ 7;#A 66'D%

$* 1 A %%$A $ '

3#1*%A 6!#A 2%$6 " 3 *!3 *

**2%4 "$ 1 1 A $

'1*%1 @ #A 756,#'

C##1#A *'

Page 7: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

E

$6

' 31% ''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''' ' 3'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''7

' #''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''= ' #$ '''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''' '7 #$% ''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''7 '= $#& '''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''> 'E 5($ '''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''77 ' .( '''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''7? '? '''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''= '> % $'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''=>

7' '''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''=; 7' '''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''E7 7'7 3$$ '''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''E= 7'= '''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''E 7'E % $ '''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''E;

=' 5 $ '''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''' =' 5 $, '''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''7 =' C ''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''E

!" E' ''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''' E' 4 % $$ '''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''? E'7 . ''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''?

# $% & $'( ) ' 1%/# ''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''?= ' '''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''?

) * + ) ?' : '''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''?; ?' ''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''> ?'7 '''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''> ?'= '''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''> ?'E D '''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''; ?' ''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''; ?'? $/ $ '''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''';7 ?'> #'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''';E

, -

. ! ,

$( (

Page 8: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

+ /

3# %$$

$$ F $

% &#'

0 &

G %

G , 1/$$#

/ G //

G #$ %

/ G #//

G

G #

# G ##

G !' ' #"

G *

G %*

G $$#

$ G #$$%

G %%

G #

# G #

( G (

G #%

Page 9: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

?

/ G $//

G

% G %%

$% G %#$$

& G #

/ G %

. G %%

0 ( &

- G /

G $$ /

G # /

G /

) G /

H GH /

G $%

G $%# %

G #

( ) G & IJ%IJ

G % /

C G $5

G % /

C G //! C K L,/ C K M,/ 7C K N,/"

C G /$ $ IJ$ IJ

0 1

α G& O#$

Page 10: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

>

β G #$

γ G %%

( )δ • G $( (((( ))))••••

ε G

η G #

G

λ G

G&%

ν G %(

G %#$$% %

ξ G

ρ G !" &%

σ G %

τ G

ϕ G

ψ G .

ω G5!K E'= OK ?';P,EO"

ω G %%$$ IJ%$ IJ/#$ IJ

0 ( 1

(((( ))))∆ •••• G $( (((( ))))••••

G $$&( (((( ))))••••

∆θ G %$$ IJ%$ IJ/#$ IJ

φ G #

G /

φ G3##

Ω G *& $ $ %%ω

Page 11: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

;

2

G %

∇ G %

( )• G ( (((( ))))•••• $ IJ

( )• G ( (((( ))))•••• #

( )Q• G %$( (((( ))))••••

(((( ))))•••• G % $( (((( ))))••••

(((( ))))•••• G %%( (((( ))))••••

(((( ))))R•••• G ! #"%( (((( ))))••••

(((( ))))•••• G $( (((( ))))••••

( )−• G ( (((( ))))•••• $#

( )+• G ( (((( ))))•••• $#

( )!"• G ( (((( ))))•••• $$

( )∂

∂ • G #%%&# (((( ))))••••

$ +

$$ $ #$&'

$& '$& #

'

G,#$

6O G6,(

62 G62#

3 G3$$

33 G3# $3$

3+ G3+ $

3C G3C*

Page 12: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

565- G5,6,5,-/

5H- G5/H -

G

G%

2 G 2

G %

G

4 03 G 4 ( 31

4 H- G 4 .H -

5 G5,

3+ G3+ $

C G,C

C GC(

G%%

)2 G ) 2

56 G 56

26 G2%6

<>= G< ;>=

<4 G<

N2 G ND2#

Page 13: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

&# %#$

&#$$$ '#$ % 1

$ ##!%7"'$##&%$

$& &S

' 6% ! $#%"

' #%#*#%

#%%

% '

.% # #%$#!

"' $ $

%$% '

%&##'% $

$ (, %&*%

,7 $% !"'$

(# '5##$ $ $$

%&, # $ !%

$ F"1$& $ !5, "'

% #

$&# 'C$ $&# #$ $

$$ ( #$ #$# '

Page 14: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

* / 3

1%$&.#$ $ '

# &%#4 0'#

%$ .(#$

# &'

$ #%#$

& $' ##&

$ $ '

$&#$$$ ($$

'

&*$, 4 0# &# 1%

'-#$ $&$'

. 1%$$S

' #. $

& #'

' $$ ($$

'

7' %#$&# #$ &

=' .#$ $#% #

&% # &$

%'

E' . #% $ $/ $

&$$$ '

Page 15: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

7

*

;###/ .&'

- ( 41%$ %#$'

- ( 43& #.# #

% O $ '

- ( 4%#$ '

- ( 4$$ ($ , #$

$'

- ( 43$$ $'

% $$ .# $ '

- ( 41%$$&# %$ #

(&'

- ( )4%.#$ : #$

$$$ ' #$%.'

- ( ,4 .1%$'

- ( 44 $'

$( ( 5 $4&$ &'

Page 16: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

=

%

###$% '-

#'&T&

% ' %$&%(

#$ $$ *#%'

- $% !"% &

&% 1# , '

# (

#$% $1 ##'

$1###

# ' $#&%

&7 #&$ '!5, "'

C

( )

( )#

0$

5$

N

L

M

L

N

M

Page 17: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

E

$% ( ) %#%$ T$

5,6,5,-/$ !565-$15$ "'

% $ %&$

# ' $ % /$ $

5$ ( )C ' $ $ %

#% ( )# %!5, "

( ) ( ) ( ) # C = + ⋅ !5(''"

#% /$$ % %'

%%$$ % %

%# $$( /#

$1#'$#'E

($ '

3 $% %( /#

('% $('

$% /# % '

#$ $ % 1 '

% $& #$ % %'

I* J!3C" #$ *& $

#%# $$ %'

& I#$/ J #$ *& $*&$

#'$/ ' ' !"O%

!"' $#7'

Page 18: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

( !

##$% &T&

I $J!&"I

## $J!&"!5, "'*

1#&*# $1*& $

&1 % & $

*&'& $$$1*& $

1*&'$ %

# &#%' $$

' ' % '!5, "

$#% ' &

%# % %

$' #!

"&&'

#$ $ % *&

')$ $$'$$ $/#

! $$/", , %$,

$$ /' $1 !' '5"$

$$ '#'7'7'

$

#$ *#&$ #$# '.

#$ &#$#( #'5

%/&$ '' /

&#$ # /' % $#$

Page 19: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

?

##$$!&T&" $#

$ ( #& % /'

! ! ! "

#$#%$ $ %

$ ' '%'- %$!&

%$%/"#$ &##%$

%/*$ '7'0$' '%#%

#$ %%## '

# $ % & ' % ( )*% ! ! + "

$,

5( #

U

Page 20: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

>

-% &#$ '#

$ !%" %#$

$'&#$ %

$ $ #S

$ = − !5(''"

< &#$ !%" $

#, %$!#$$"

%'

3$$#$ %%$ *&

& &%5 # &!?=,>?"?>7'

#*&I##JI#J '$$

& #$$'+ ## #$ # $

( #!*$ '"&# $*##$

( #'

$&%$$ ' $

#$ , !5"

& ' %' '

$%# $'/#%

$ #$ ##*'+ #$ /#V OW

$#$$V OW$% %#$$'

Page 21: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

;

+ (

#!$1 " '&*

&###$%$ &$%>E

-#4 -!>;,>>"'$ #&# &*

&!# "#/'& &

! "$& ' %$7 $$

#*#&&'3$

& % #*# &&'- '=&#

$ #'

3

0 #

6*&*&

, % ! ! ! "

+ # #! #$,# #"& % #%

%#%$ ..$ #'#$

# # % #

#$ ##$ #'##$$,#

# #$&& $

' $##%*&

*&'$$,# # % & &

!&&*"$$ O$

1'#$% %'

Page 22: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

& $

$$% ' ## 5

#$$ # #

#$ '4 * #$ # &#

$ '# /#V OOW$ %V

W$ '

$ #1'$&

$1 $%#$ $

S

' #&#' $#% %

#% % %$# #%#$

#%'$$'0 /#V O W

$ V OOW$ #'

! $ ##'&

/#V## W!## "$ #'4 *$$

$#% %#' + $

/# $'$$%#' '

#&$# #'

0!$"$ '

$' ',//,#

' $ # *&# ' '

%&*& #'

Page 23: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

. ( / + $# $ #%

'/#V O W$ V OOW$ #'

. %$# % %#%

%'/#V O W$ V OOW$ #'

/ + % #& #' %

'/#V O W$ V OOW$ #'

" # %

' $ *%#'%

# &*$#&.

% &($ ' &*#

&*$ ##& &

' &* /#V /X W' $

$$

#'

#$ #$7 *#

'5 + ! 5 "&$

& #$ #'/

('3$$ &$

/ #$$$(' '!'"

'#'7' %$$ #

$$ '

Page 24: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

( ! /

#$& $

&# $5 %$'

% & #$5 %'$

%& # &

' &$ &&&

$'0 #&%

# $& !5, "'

5$ S 5$ #S

%

δ

δ

= ⋅= ⋅= ⋅= ⋅

= ⋅ ⋅= ⋅ ⋅= ⋅ ⋅= ⋅ ⋅

7

%

δ

δ

= ⋅ ⋅ ⋅= ⋅ ⋅ ⋅= ⋅ ⋅ ⋅= ⋅ ⋅ ⋅

= ⋅ ⋅ ⋅= ⋅ ⋅ ⋅= ⋅ ⋅ ⋅= ⋅ ⋅ ⋅ !5(''7"

< %δ δ %#'

& # /#$$&

#$#'&,##$$$

ω &

% #$$$%∆

∆θ '

# ##!#'E"

#$ $&$#$$ #!E#';"'

D $ #$$S $ S*

*

$ *

% $ ∆

−−−−

==== *

*

*

∆θ ω

−−−−

==== !5(''="

< * * ∆ ++++= −= −= −= − &&% * * ++++ '

Page 25: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

7

( !

% #$$##% $%

# % '$%#% (

#$$$ '- $

&#$$ ' #

#% $&# $$ '#%

!"$ #$ #

#$'

/ ( 0 +

# &$$&#'$! "

#$ & #$ '

#$ *# #$% $ #%#$

$ #%%##$%

$ '#$ ! "% &

$ #'

#& & #

' '%' T*# #$%

#$ $ % $ '

#$#$ # $

$ !$,$ "#$% $ '

&$#&

$ % ##& $/'$

#$ #& #$

Page 26: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

=

*%$'$ $ #

$' ' '

- ! ! +

$$$# $'

## !"&#&$ *

%'$#$ #$' '

$ !2"'-$#$ 2

# % #%#&

% !"'& %&

#$ 2#'$$&&$ 'E'

" + 6" 7

# 8 6# 8 7

$ / + 6$7

$ V OW! #"

$ V W! #"

$ + (

$ V OW! #$

$"

$ V W! #$

$"

V W 9 + V OW $ V W

"

-. /*0 /1 2 /3 *

Page 27: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

E

$$# $

'' # #$$$$ $

%$##/ #$$$ ' $

#$$$#$2&%SY$)&4 C-,Y

$4 4 ;,Y7$)&)?Y=$&,

562'

$ " % $ -

#

Z 7! O" [=!* O" [=!* O" !* O " 7!* O "

' 'E[' '[ \ 70V OW , '7YO'Y Y7 =Y= ,

, E[E [E O ,$V## W

, YOEY EY7 Y= ,

, '['E '['E O ,

#

!C<"V OOX ).W , , 'EY7 ?Y= ,

E[E E[ \ =0V] W , EYOEY Y7 EY= ,

, [ =[ O ,$V## W

, EYOEY 7Y7 Y= ,

, E[ [= O ,

V] OOX ).W , , Y7 7?Y= ,

##/ #\ 2^

E[2^

[E2^

Z 2^

Z 2^

##0

% # $

O !&#"

$ * /1 2 *% ! ! ( )*% ! ! + 4 ! ! # 5 % ! ! + "

. ! !

#$$$ '$$$$

&'

! !,$ " $5/&, &

#$/'/#&$& S

Page 28: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

• N,/##/$5

• L,/# & %(/

• M,/ LN/ # ,$

$ $$$$ '

: : :!5 ! !,$ "% $5

/ &'/#&$& S

• N,/##/$5

• L,/# & $&

• M,/ LN/ # ,$

,,,$/$ $ #$ ##$$

'

' + ! !,$ " $2/

&%$ '/#&$& S

• L,/# & $%

• M,/# &$$%

• N,/ LM/ # ,$

C/# $%'

" ! !,$ "% $2/

&5'/#&$& S

• L,/# &#! "

• M,/# &#! "

• N,/# #& #

#$% $ *&,,#!52" '+ &$

/,,&!53" '% $ $

Page 29: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

?

! !,$ "$; < ! !,$ " #

% $ &LM,/# #'

= < ! !&,$ "% $ & $2

!#"$% $ %'

M,/&#& ,& % #&

N,/'%M,/&$

#',& % &$

N&,/&&. & M&,/'

!5, '="/$#&$& S

• N&,/# #& #

• M&,/%#& α $ &&

• L&,/ MN/ # ,$

- '&$$$ $ /' N N=

L

M

&

5(#

ω

L

M

ML

N

6$5

&%(/

ϕλ

6$2

L

N

M

M

L#

&M &L

M

L

#

6$2

α

) O.

C

+ $ % ( % % "

Page 30: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

>

$ ! ( 0 +

+ $ # #& #

&$ % $ '$ *

'# #

$* '

* /' '%$ $ '

% # /# / C '-

# C #$

% $ '& $$ &#

$$$2&#% $ '#*& ' '

&#' %#$

&#'6#'E# .('

##$ $&# %

# # ' % $ #!$ '?"

#!&*& $ %%" #

# $ $ ##!&*& $

5"' # %5$

E?';E −⋅ OE'=? O! " %

2#$ # # '4 & %

2/ %

5!'"' $$

# $&'

$& #$$ &

$ ' * !, " '

Page 31: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

;

-

6 # % #

# % '$ ,

%% $ # !& $5

%"!5, " $ '?'

N N=

N,/'

M,/'

L,/'

M M=

L L=4 %#

M

M,/'

N,/'

,%#

N

L

#η = ξ =

γ γ

N

L,/'

6 * )

&$ '? ,% $L,/ ξ & ,

% $M,/ # η ' %% γ *&

# $$% &% /%∆

#!%7"

/%

∆ξ

γ ∆−−−− = −= −= −= − ⋅⋅⋅⋅

%

∆η

γ ∆−−−− ==== ⋅⋅⋅⋅

!5(''E"

< ∆ % &&% γ

% ! "γ γ==== ' γ %$$ ϕ #

Page 32: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

7

!5, "'&

%$##'' '!<';;"'

=

7 = E ! " ! " γ ϕ ϕ ϕ= ⋅ + ⋅ + ⋅ + + ⋅ ⋅ + ⋅= ⋅ + ⋅ + ⋅ + + ⋅ ⋅ + ⋅= ⋅ + ⋅ + ⋅ + + ⋅ ⋅ + ⋅= ⋅ + ⋅ + ⋅ + + ⋅ ⋅ + ⋅ !5(''"

< −−−− %

K ;'?>7??E = K ,'7>?;>;

K 'E?;== E K '=7;??7

7 K '7?> K '?

<# % & ∆θ

.$ !%7"'

C ! " C ! "∆θ η ξ ∆θ= − ⋅ − ⋅= − ⋅ − ⋅= − ⋅ − ⋅= − ⋅ − ⋅ !5(''?"

(((( ))))(((( ))))

L

M

∆θ

ψ∆θ

−−−−

= −= −= −= −

!5(''>"

/&$ % $ $!"

7 C C ! " C ! " C ! "ψ η ξ= ⋅ − ⋅ −= ⋅ − ⋅ −= ⋅ − ⋅ −= ⋅ − ⋅ − !5('';"

< C C 7C LMN,/ %' '! *"'

-&,2/5('?'>

#' / !' ' #

"'$ !

"'

Page 33: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

7

$ % $

' $%ν #$$% $

% ω !0 ;?"'

$ν = ×ω !5(''"

%$ / C &$ % $ %!

"# '% &#$$

$ % & ω

# & %% γ !$ 5(''"5 ω

ϕ $&'

( )( )

( )

$

C

$

− ϕ γ ω ⋅ ϕ − = ⋅ ω γ ⋅ω ⋅ ϕ − ×ω γ

!5(''"

- ## /&$ % $

% %7, !#'='7"') % /

% '&2

%$$ $ ' '& '

$#$ $ $ &$

#'=''

6 * 7, '$ T&

#'5/$&$ (

'%% % * $

Page 34: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

7

%%##/ #$&/!L,/"' $$

%##&$#%%%&

%' !E"'-

##ξ #η ψ $. !

E"'

ξ ≈ °

( ) ( )2

5

%

% %

η = +

5

%

%−

ψ =

!5(''"

< 5 O O2% ,%%##%'

$ %!" %%

') %%&

%# E O!"'

$2& #*&5/H

-!5H-"& !4 )2" #$

'$ !E"'

>

% $% $ /$

$$ '

5/H -!5H-"& !)2"'

## $ #$ 5H-'H&(

$%$ ' '.%#!N2"#!62"

!%7"'&$#=H $'

Page 35: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

77

$

%#$. % %

#'#$. δ %##&

# ϕ # &*C<') &

% . !5, "

C<

! " δ

ω ϕ====

⋅ ⋅⋅ ⋅⋅ ⋅⋅ ⋅

C<

! " ω ϕ δ

==== ⋅ ⋅⋅ ⋅⋅ ⋅⋅ ⋅ !5(''7"

!

&$ $% ($ '$

$$$( $ & '

#'#% %% % /C $$

% %' %

%% %!5, "

(((( )))) / % C ==== !5(''="

<# %%IJ#$ %$ '

/&$ $ $

# ω %$ !5, "'

/ % % /# .

# !5 "'

%$ $$$ '%

($ % $ %%$#% %%

% / C .'

Page 36: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

7=

#%% $ /# $%

! ϕ λ # "!"'

(((( )))) ϕ λ==== !5(''E"

%!52"% $ /# # 52%

/!#"%!

"

(((( ))))5

2

%

% %

%

ϕϕ λ

++++ = = + ⋅ ⋅= = + ⋅ ⋅= = + ⋅ ⋅= = + ⋅ ⋅

−−−−

!5(''"

<$% # % %$$ ,

1/ $$#! *"

(((( ))))

(((( ))))

7 O

ϕ

⋅ −⋅ −⋅ −⋅ −====

− ⋅− ⋅− ⋅− ⋅

(((( ))))O

ϕ====

− ⋅− ⋅− ⋅− ⋅ !5(''?"

%%$#$/#!5, "

5

2

%

% 3 %! "

%

ϕλ

ϕ−−−−

++++

= = ⋅ == = ⋅ == = ⋅ == = ⋅ = + ⋅+ ⋅+ ⋅+ ⋅ −−−−

!5(''>"

%%$%% $ %$ #$

$ 5 ω $$%

Page 37: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

7E

$ &#5 ω 5 %$ γ '#$$

$ $ % $ !5, "'

$ C $= ⋅= ⋅= ⋅= ⋅ !5('';"

5 ω % $ $$ %

%%!5, "'

C

λ λω ω ϕ λ ϕ λ ϕ ω ϕ

ϕ λ ϕ λ ϕ ω ω ϕ

−−−− = ⋅ = − ⋅ − ⋅ ⋅ = ⋅= ⋅ = − ⋅ − ⋅ ⋅ = ⋅= ⋅ = − ⋅ − ⋅ ⋅ = ⋅= ⋅ = − ⋅ − ⋅ ⋅ = ⋅

⋅ ⋅ ⋅⋅ ⋅ ⋅⋅ ⋅ ⋅⋅ ⋅ ⋅

!5(''"

$$% $ &#5 ω %

%%!5, "'

5

5

%

%

%

ϕω λ ϕ

λ ϕ ϕ

−−−− ++++−−−− = ⋅ == ⋅ == ⋅ == ⋅ = ++++ ⋅⋅⋅⋅ ⋅⋅⋅⋅ ++++

!5(''"

5 %$ % % $ ('

γγ

==== −−−−

!5(''"

* % $ %%$%

$/#!5, "'

Page 38: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

7

(((( )))) % C $ %Ω Ω γ = ⋅ − ⋅ + × += ⋅ − ⋅ + × += ⋅ − ⋅ + × += ⋅ − ⋅ + × + !5(''7"

< Ω

Ω *& $%5(''''

- %%$ //#$& $$(

!5, "'

(((( )))) C C CΩ Ω Ω= ⋅ = ⋅ −= ⋅ = ⋅ −= ⋅ = ⋅ −= ⋅ = ⋅ − !5(''="

%% $ $&!5, "'

3 %

/ % C $ ! " %

C C ! "

Ω Ω

Ω Ω

−−−− ⋅⋅⋅⋅ = = ⋅ − ⋅ + × += = ⋅ − ⋅ + × += = ⋅ − ⋅ + × += = ⋅ − ⋅ + × +

⋅ −⋅ −⋅ −⋅ −

!5(''E"

< / 3−−−− %

3 ! "

ϕ−−−−

++++ ==== + ⋅+ ⋅+ ⋅+ ⋅

−−−−

!5(''"

%( $ ω $

#'

&%$ 5$ !5,

"'($ % $ #''

Page 39: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

7?

# <

($ .($$#'

6#''[''7 .(% $ '-

#$ .($ !

5, *"'

? 0 ( ! 0

$*&

'#$/,

#''7'

& # /# ∆θ #$

# #$ /. !"'

(((( )))) /

.

O! "

O! "

O! "

∆θ ∆θ ∆

++++

= + ⋅ − ⋅= + ⋅ − ⋅= + ⋅ − ⋅= + ⋅ − ⋅ ++++

!5(''?"

< * * ∆ ++++= −= −= −= − &&% * * ++++

& /#% #$$$%∆

#$ $ /. !"'

(((( ))))/

$$

.

O! "

% O! " %

O! "

∆ ∆ ∆

++++

= + ⋅ − ⋅= + ⋅ − ⋅= + ⋅ − ⋅= + ⋅ − ⋅ ++++

!5(''>"

&#E % #'

Page 40: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

7>

$ (

$$ $ # / C '5

('0$$(

#$$!5, "

( '& % *' '! *"'

( $ $ $

$$ %' $ %

(/#!"'

(((( )))) 7 =( ( ( ( (==== !5('';"

$($ %!"'

7 =( ( ( ( + + + = !5(''7"

$ T$$( .!"'

(R(( (

=⋅ !5(''7"

$ &(( / C %!"

7 7 77

7 7 77

7 77

= 77

'E !C C " C C C(

'E !C C " C C C((

( 'E !C C " C C C( 'E C C C

⋅ − ⋅ + + +⋅ − ⋅ + + +⋅ − ⋅ + + +⋅ − ⋅ + + + ⋅ − ⋅ + + +⋅ − ⋅ + + +⋅ − ⋅ + + +⋅ − ⋅ + + + = == == == = ⋅ − ⋅ + + +⋅ − ⋅ + + +⋅ − ⋅ + + +⋅ − ⋅ + + + ⋅ + + +⋅ + + +⋅ + + +⋅ + + +

!5(''7"

Page 41: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

7;

(((( )))) (((( )))) (((( ))))(((( )))) (((( )))) (((( ))))(((( )))) (((( )))) (((( ))))

7 = 7 = 7 =

7 = 7 = 7 =

7 = 7 = 7 =

( ( ( ( ( ( ( ( ( ( ( (

C ( ( ( ( ( ( ( ( ( ( ( (

( ( ( ( ( ( ( ( ( ( ( (

− − + ⋅ ⋅ − ⋅ ⋅ ⋅ + ⋅− − + ⋅ ⋅ − ⋅ ⋅ ⋅ + ⋅− − + ⋅ ⋅ − ⋅ ⋅ ⋅ + ⋅− − + ⋅ ⋅ − ⋅ ⋅ ⋅ + ⋅ = ⋅ ⋅ + ⋅ − − + ⋅ ⋅ − ⋅= ⋅ ⋅ + ⋅ − − + ⋅ ⋅ − ⋅= ⋅ ⋅ + ⋅ − − + ⋅ ⋅ − ⋅= ⋅ ⋅ + ⋅ − − + ⋅ ⋅ − ⋅ ⋅ ⋅ − ⋅ ⋅ ⋅ + ⋅ − − +⋅ ⋅ − ⋅ ⋅ ⋅ + ⋅ − − +⋅ ⋅ − ⋅ ⋅ ⋅ + ⋅ − − +⋅ ⋅ − ⋅ ⋅ ⋅ + ⋅ − − +

!5(''77"

< 1C !1" $ C '

%%$($,$$(!"

( )

. /

. /

/ .

/ .

( ( (

ω −ω ω

−ω ω ω = ⋅Ω ω ⋅ = ⋅ ⋅ ω −ω ω −ω −ω −ω

!5(''7="

< ( ) / .ω = ω ω ω %$'

$ /# ∆ω 5T %

$& $$$(5(''7=!5, "'

( )

( ( ( + = + ⋅Ω ω ⋅ ⋅∆ ⇔

. /

. /

7 7 7 / .

= = = / .

( ( ( ( ( (

( ( (

( ( (

∆θ ∆θ ∆θ

∆θ ∆θ ∆θ

∆θ ∆θ ∆θ

∆θ ∆θ ∆θ++++

⋅ − ⋅ ⋅⋅ − ⋅ ⋅⋅ − ⋅ ⋅⋅ − ⋅ ⋅ − ⋅ ⋅ ⋅− ⋅ ⋅ ⋅− ⋅ ⋅ ⋅− ⋅ ⋅ ⋅ = + ⋅ ⋅= + ⋅ ⋅= + ⋅ ⋅= + ⋅ ⋅ ⋅ − ⋅ ⋅⋅ − ⋅ ⋅⋅ − ⋅ ⋅⋅ − ⋅ ⋅ − ⋅ − ⋅ − ⋅− ⋅ − ⋅ − ⋅− ⋅ − ⋅ − ⋅− ⋅ − ⋅ − ⋅

!5(''7E"

< / . ! " C ! " ∆θ ∆θ ∆θ ∆θ ∆θ ∆θ ∆θ ω ω ∆= = − = − ⋅ + ⋅= = − = − ⋅ + ⋅= = − = − ⋅ + ⋅= = − = − ⋅ + ⋅ '

C ω

ω

$ #%# / %%'$$

# (((( ))))(((( )))) O ∆θ= ⋅ −= ⋅ −= ⋅ −= ⋅ − (((( )))) O O ∆θ ∆θ= ⋅= ⋅= ⋅= ⋅ & / .∆θ ∆θ ∆θ ∆θ= + += + += + += + + '

< ( ++++ 5(''77# /!5, "'

Page 42: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

=

% ! !( ! ! + @(

$ $ % #$$ $%∆ %

$ / C $$#!"'

( ).

$ . / $0$ #

/

'E 'E

% C 'E 'E %

'E 'E

− ⋅∆θ ⋅ ∆θ

∆ = ⋅ ⋅ ∆θ − ⋅ ∆θ ⋅∆ − ⋅∆θ ⋅ ∆θ

!5(''7"

( ).

$ . / $$ #

/

'E 'E

% C 'E 'E %

'E 'E

⋅ ∆θ − ⋅ ∆θ

∆ = ⋅ − ⋅ ∆θ ⋅ ∆θ ⋅∆ ⋅∆θ − ⋅∆θ

!5(''7?"

< /.∆θ %5(''7E'

$$ % $6 %

%%$%$ 5(''7!"'

$ % % ! " % ∆ ∆ ω ω ∆ γ ∆= − + × ⋅ + ⋅= − + × ⋅ + ⋅= − + × ⋅ + ⋅= − + × ⋅ + ⋅ !5(''7>"

#%% $ U$ # !"'

% % %+ += + ∆ !5(''7;"

$&##$ C ,H

'C ,H/# ##/ $

'+ (' '5T !$"$

C ,H !$"#$ '

!5, *"'

Page 43: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

=

## %!"

(((( ))))

(((( ))))

% %

ϕ

∆+ ++ ++ ++ +

+ ⋅+ ⋅+ ⋅+ ⋅

= + ⋅ ⋅ + ⋅= + ⋅ ⋅ + ⋅= + ⋅ ⋅ + ⋅= + ⋅ ⋅ + ⋅ ++++ −−−−

!5(''="

.$ .($ '>'

,#

,

2$

C

ω +

ω

$ +

C

ω

%∆

% #

γ

+

( ) %⋅ω + ω ×

%

ϕ λ

6 # ω

ϕ λ

ω

ω

+

ϕ λ

+ ω

6 # C

5

ω

7$ % /3 *%

.(#$ # ,$

&%# ' ,

$ ## 5(''7E'7;'='

$ $ .($ !5, "'

Page 44: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

=

) "

($ .(#%# %

$ $ '#$ 2#

%'&$ 2&&

#$ .(') ($

$$$'

% / #%%% /

% /δ #%!5, "

( ) ( ) 5 2/ % C % % %δ = δ δ δ = δϕ δλ δ δ δ δ δη δξ δψ !5(''="

< ( ) δ = δϕ δλ δ #%

( ) 5 2% % % %δ = δ δ δ 5$% %% $

( )Cδ = δη δξ δψ /

C '

# & #$*&

#''

%'!5, "%$

( ) / % C $δ = δ δ δ δω δ ⇔ !5(''="

( ) 5 2/ . / ./ % % % $ $ $δ = δϕ δλ δ δ δ δ δη δξ δψ δω δω δω δ δ δ

< ( )/ .δω = δω δω δω # ( )/ .$ $ $ $δ = δ δ δ

'% $

$$('%, &

%$$$(.'

Page 45: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

=7

)

%%$#&$5(''>'#$$

&*& # R $$(

$# /###/

!5, "'

(((( ))))5

2

%R %

! " %

δ δ δϕ

++++

∂ ∂∂ ∂∂ ∂∂ ∂ = − = == − = == − = == − = = + ⋅+ ⋅+ ⋅+ ⋅∂ ∂∂ ∂∂ ∂∂ ∂ −−−−

!5(''=7"

$$$&!5, "'

3 % 3 3 − −δ = δ + δ !5(''=="

< 3− %5('' 3 %!5, "

55

%%

%

3

− ϕ + = +

!5(''=E"

-&5(''==&!5, "'

(((( ))))

5 5 5

2

% %% %

% ! " ! " ! "

%

δ δϕϕδ δ δλ

ϕ ϕ ϕδ δ

−−−− ++++++++ ⋅ −⋅ −⋅ −⋅ − = += += += + + ⋅ + ⋅+ ⋅ + ⋅+ ⋅ + ⋅+ ⋅ + ⋅ + ⋅+ ⋅+ ⋅+ ⋅ −−−−

!5(''="

&% # ( )5 % % %$%

# %% !5, "'

Page 46: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

==

) 9 +

%%$%&$5(''7' &#

%$$&*&% % #%

R% '$$($% /#

##/ !5, "'

(((( )))) (((( ))))(((( ))))

R% % % % % C $ % %% %

δ δ Ω Ω γ δ ∂ ∂∂ ∂∂ ∂∂ ∂= − = = ⋅ − ⋅ + × += − = = ⋅ − ⋅ + × += − = = ⋅ − ⋅ + × += − = = ⋅ − ⋅ + × +

∂ ∂∂ ∂∂ ∂∂ ∂ !5(''=?"

$$$&!5, "'

(((( )))) (((( )))) % C $ % % C $δ δ Ω Ω δ δΩ δΩ δγ δ = − ⋅ + − ⋅ + + += − ⋅ + − ⋅ + + += − ⋅ + − ⋅ + + += − ⋅ + − ⋅ + + + !5(''=>"

(((( )))) (((( )))) % - C % D δ ε Ω Ω δ δΩ δΩ δγ = − + − ⋅ + − ⋅ + += − + − ⋅ + − ⋅ + += − + − ⋅ + − ⋅ + += − + − ⋅ + − ⋅ + + !5(''=;"

< - *& /$#$$% $ ( ) 5 2ε = ε ε ε

$$ &$ % $

( ) 7 = $ $δ D *& /$

%% % δγ %'

$%$5(''=;& %!5, "

!"')&%$& %!5, "'

5 2 /

2 5

5 .

$ $ $

% $ $ C $ '''

$ $ $

− δη δ δ = − δξ + δ +

− δψ δ

Page 47: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

=E

( ) ( )( )

( ) ( )( )

( )

5 2

5

5

5

2 5 5

25

% % % %

%''' %

%

C

% % % %

%% %

%

ω ϕ + ω ϕ + + ϕ δϕ − ω ϕ − δλ + + ϕ δ

γ − ω ϕ

− ϕ+ ϕ ω ϕ + − ω ϕ −+ + + +

ϕ− ω ϕ − − −+ + +

ω ϕ +

5

25

%

%

%%

δ δ δ

+ +

!5(''E"

& &% # ( )5 2% % % %$%

# %% % !5, "'

) $

$$($ /$ /#

##/ !5, "'

C ε = −Ω ε − δω + !5(''E"

< ( ) 7 = #$ δω ' $%$5(''E&

%!5, "!"'0

$& %!5, "'

5 /

2 .

%

% C '''

%

+ δ δω

− ε = δ + δω + + δ δω − ϕ +

Page 48: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

=

( )

( )

( ) ( )

5

55

5 5

5

5

%

%'''

% %

% %

% %

% %

− + δϕ ω ϕ δλ + + δ ϕ −ω ϕ − + ϕ +

ϕ ω ϕ + −ω ϕ − + + δη ϕ −ω ϕ − − δξ + + δψ

ω ϕ + + +

!5(''E"

%& &% # ( )5 2% % % %

$% # %% !5,

"'

) + ( /

#$ #! $$$

" $,*%#&$$(!5,

"'

/

7.

& &

−α = −α + = −α +

−α

!5(''E7"

<α #$ τ & ,*%#'

,*% ,*% #%! & "

$ $$&'

$,*%# % !

"'

Page 49: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

=?

) $ /

#$ $,

*%#&$$(!5, "'

/

7.

& &

−β = −β + = −β +

−β

!5(''E="

<β #$ τ & ,*%#

$ 5(''E7'

)

6 5(''='=;'E'E7'E= %$$$($

% $ !5, "'

( )( ) ( )

3 %

- C % D %

/ C

&

&

− δ δ − ε + − ⋅Ω + Ω δ − ⋅δΩ + δΩ + δγ δ = = −Ω ε − δω +ε

−α +

−β +

!5('EE"

))

<( %5(''EE## $ #$

'+ $ # $ %!5, "

( ) ( ) ( ) ( ) ( )/ - / & = + !5(''E"

< ( )/ ( )- / ( ) $$ /

!# /" ( )& #% ' #E'

Page 50: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

=>

, $ !"

%##%#$% #%%

$ & $$ % '+

# #$$ &

.(( #%

% $ '< %

$$$ !"'

)&% ## #

# & #%

'#&$ /&%

$ & & %#'

&## ( $%

##%'-& & &

% ' # % $

&% $#*' $ $&

!5, "'

./ % .'

3$$#$# ##%

'$ ##

!#7"'+ %#$ &##

$ #'

& $ /

$$$$ ('H $ %

$ & '&#=E'

Page 51: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

=;

7

###$ !"& #

% '- %&$&

$$$$ /#'

+

DC!D & C [ "

&&&,% %#23# $

3$!33" ;##%;?7!'"';?>$

##& &$# ;;E!4 #E"'

& $ ,%# %%#

,%# & ' #

$ *& $ '

# * #$ %#

.*!'"'

$ &$ 7''

# *% 5 4 *

Page 52: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

E

(

# $=! %"# $%#

&$##/ '* %5'

/#&$EE_%(#%

$E> !4 #E"'5#$ #

*! " ' &

!, " . $&*E$

;>''2%6 !26" $

$#&*! &"'26,

/!#"5, T$$&

& , =$ !'4 #E"'

-

$ $ ! /

"# ' *

*$ 6#

2'' *#$

!'"'

8

$%!

%"$$%,# '#$

$ %$#$ '

$#%%

## #$%'*' '%#,

% % % '!'

"'

Page 53: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

E

5 &$$$(!4 K E?E'=).

4 K ?')."' &,C !C"

% $ # *'

$C6,(!6O," 4 &$($

'7).# '!,"

4 4 &$($'7).' $%&*'

,#M, ##'$

,#$ !"!4 #E"'- $$#$

# # 3##

!%7"'

$ %$ C&#$

% # '<%&

$ $ # '#

&% *&%*

!$* %$ & "'%

($# /#!

4 #E"'

# # = ρ + ⋅ − ⋅ + − + + !5('7'"

<# # $ ρ &

% #$ % * %*

# # ##

!' ' #"'&#7'E'

-$ # % *

' '!'3 '"'

Page 54: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

E

- (

$ %$ #$ $('

$ #$ %

# ' !*& $&&% &

%"*&% %%#$##

!' '4 03 &!4 #E""'$#&*$!*&

#"& '%($

# /#!3 '"'

# φ = ρ − λ + ⋅ − ⋅ + − + + !5('7'"

<φ $ # λ #&%

$% 5('7''&

#7'E'-$ # %

*' '!'3 '"'

) * ( (

$# %$ $ $#

*&3##$'$$ #

&% !#4 "%($ 3##

/#!4 #E"'

# φ = ρ + ⋅ − ⋅ + − + + !5('7'7"

<φ $ 3## ρ &

% #$ % *$

%*$ $

#$ #

##$ $!' ' #"'

$ %'

Page 55: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

E7

$/

<# &*&#

*& $#''- %

!#7''=" $# # *&$*

'0$# # % '

# *' '!E"'$# #$

#, $ #,### '

3# #$# %$ #$#&*

$ '%!"!$

%" *#,## %&!#SOO '1#'' %"'

##&$ %!/#

5$ " &# %!3 '"'

( ) ( ) ( ) L L N N N N = − + − + − + ⋅ !5('7'="

<I J # $ #

( )L O M O N *& ( )L O M O N *&

$ # # #$ % *&

%*' %5('7'7'!#

7'7" #%*$ 5('7'='

. (&'

5('7'=$*&% &$

*&# '$ $ $$

*&& ((

#$ $*&# !3 '"'

Page 56: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

E=

* !!

$ #('$$

& $&%'+ $%

! "#&*&#&!%" %'

*

$# $$(13'

& %# %%!"*

# # '33 #$$%

'& %$$ #

&$!3 '"'< %

( . # !#7'="'$&

% !&%"

#% ' #*&

# #'$$$ $

# 5('7'')

$# % '

# #$% 5('7'=!4 #

E"'

. (

$# (%# '%

$ 3& * $%($ 5('7'

$$ ' $$&&

%(' # *$$&&%%

'$$ *

%'< #

### #

%'$$ ,$$&!4 #E"'

Page 57: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

EE

( ) ( )

C C C C #

C C C C #

C C C C C C C C C

φ = ρ − λ + ⋅ − ⋅ + − + +

φ = ρ − λ + ⋅ − ⋅ + − + +

φ = φ − φ = ρ − ρ − λ − + ⋅ − +

!5('7'E"

<#ICJICJ$&$$%%

#%('$&$$ $& $$$$$

,$$'%* $&$$

%,$$&!4 #E"'

( ) ( )

( ) ( )

( )

C C C C C C C

C C C C C C C

C C C C C C C C C C C

φ = ρ − ρ − λ − + ⋅ − +

φ = ρ − ρ − λ − + ⋅ − +

φ = φ − φ = ρ − ρ − ρ + ρ − λ − − + +

!5('7'"

<#IJIJ$&$$%

#%('-$&$$ $&,$$$&$$

# & % #'

#,$$&!4 #E"'

( ) ( ) ( ) ( ) ( ) ( )( ) ( ) ( ) ( ) ( ) ( )

( ) ( ) ( )

( ) ( )

C C C C C C C C C

C C C C C C C C C

C C C

C C

φ = ρ − ρ − ρ + ρ − λ − − + +

φ = ρ − ρ − ρ + ρ − λ − − + +

φ = φ − φ

φ = ρ

( ) ( ) ( ) ( ) ( ) ( ) ( ) C C C C C C C − ρ − ρ + ρ − ρ + ρ + ρ − ρ

$ #$ !#7''",

$$'#,$$$*!3 '"

!5('7'?"

Page 58: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

E

$$$ #%#$&

$$ #' %#

#*###'

+ , , %#

#%* #!"'

1

*$$$*&#, '

* $

!#7''"'#$&

&% '$ %$$!'"'

2 /

!*&# "#$#

%$ # ##'& %

# '#$&

$ % $$'4 &$

% $$ ! &%

"# $ & '0 %!'

" $ #$#$ $$$E

'

(

#$ % #'

#$ E* %!"$

.!$"'< %

. #$ % #$ $#

%'/# 56!56"%

Page 59: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

E?

# #' '!'"'#

# # % $ %

$$!3 ''"'$#$ $$$

#E $ $#* #

#%!"'

% ( (

####$ 5$#E

* %'4 *#&$

# # ###'

%& #' #;` $

## '& #$

$$#%' '&#

!"'## ##$

$&#$ $$O ' #

$' $ ' $$$!'

"'3 #$&% ($

!"'

. 1

%*$$$%*&#, '

$ % % & '

( $ $$($5('7'=

%$ $$5('7''%%$ #$

# # ($%!"'

# (

## [$ $ %

!$ "[$#$$#$ $

Page 60: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

E>

' #$

!"'%#

%$$'#$$$(%%#

. $ $% ') % *

$ #'& $ !E

!3 '"" ## !7 !""$

$ % $ # $ '

) "

%# '' '

% .,

$$'&%

#!"'#$ $$ % $

/#$%#$!" #'%

'E'E $ #$ $# !'

"'

,

$# #'

##$ *& % #

'/# $$3+ $!3+ "%'-

# #3+ !3+ "'#%$

% #$! $&

#"%! "'$3+ =%

##'$3+ %# *& $

# #$ $

!3 '"'

Page 61: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

E;

, +

-#%# . $

$ ' .#$ 7''

8 ( $! !!

* ,7

+ ,7 a

# , 'E,'

## 'E,E ',

C%* ,

# ,E $$!,E "

!" 'E,'E $$!'E,'E "

% + a ,E ,7

$ # ( . % ! ! 1 ! ! 5 % ! ! + "

$ !

2 &#%#

'0$ && $

*' ' '- %$

5$ $& $$

%&/#!#$ % "'

5 /$$ ' ' *

%.$ ' $

% %$' '$$'

)#% #, '

&! "$$% ' $

&'$& &#$

#& $ '- $$

& * #$$ ! *"'

Page 62: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

= 5 (

5 $ *&# $ $%'

$$ (# $%%

& '+ $ # ((

(& $ %')&%$*& $

% % $# !

"' % %, #$

.$ ! '="'# (

$'$(#E'

! + +

$ # %5(''E'

# #'

/ - / &= + !5('='"

## $% / & *& $%$

H $'% $#

##& $%!"'

H $ # !%7"'

/ / &+ += Φ + !5('='"

< +Φ /$ * $ #U

%5(''E'

Page 63: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

/ # %&& #

U ! " %/#$!;?="'

( ) ( )

7 -

- - - '''

F 7F∆

+∆ ∆

Φ = = + ∆ + + + !5('='7"

< / ∆ %&& #U'+ $

& !5, "'

H $%*&

# ' $ #&!%7"'

. ) /= + η !5('='="

< . % / ) / η &

% / C '

<& #% & η .

&(&.# $%%

$!%7"'

R R/ / H+ −= + ν !5('='E"

( ) H ) + −= − !5('='"

<I,IIUJ$$ ##% H H

ν %( %'

H H %!%7"'

Page 64: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

( ) H ) ) ) C

−− −= + !5('='?"

%( ν $$&% . #

% R. %!"'

R R. . . ) /−ν = − = − !5('='>"

)%( $&$ '$

&$ #%(

$&#% R/+ (# R/

− '+ $

$$&#% &#$

' !5('='=" %

$#& 'H /$

& $& $&$ #

# $%'$ % C

H & %$

% H & '

#%%$& $!%7"'

R R/ /− ++ += Φ !5('=';"

b− ++ + += Φ Φ + !5('='"

< +Φ / %5('='7 b # / %

!"'

( ) ( ) ( )

6 6 6

b b +

+ += Φ Φ !5('='"

Page 65: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

7

< 6 +Φ , /&& #U 6b

, # /$ & ( ) $5(''E'

6 5('='E='='?=';='##H $ $ =''

,*8 %

%H $&# 5('='

5('='= ')&% ,

# , ').($

., H $ $ ='

!"'

! : +

,# %!%7"'

( ) / $ / &= + !5('='"

( "

R R/ /− ++ += Φ

"%

b− ++ + += Φ Φ +

8 ( ( "6 #H

( ) H ) ) ) C

−− −+ + + + + + += +

"2# &

( ) R R R/ / H . ) /+ − −+ + + + + += + −

7"2#% ( ) H ) + −

+ + + += −

R/

$( !

C b ) + +Φ

Page 66: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

=

( ) . / = + η !5('='7"

<$,$# %$#

&%#%'

.5('='='7 % Q/ !"'

Q / / /= + δ !5('='="

< /δ #$ %%'

$ #$ % $

/#$5('='='7#$ %!

"'

/ / &+δ = Φ δ + !5('='E"

. ) /δ = δ + η !5('='"

$# /#%!5('='='="5('

='E='$ & /#% /δ $$

&# .δ !"'

$. %%H $

4 .H -!4 H-"')&%$. #

/ − H $5/H -!5H-"!%7"'

$.H $ $ $ ='

/δ 5('='=% / !"'

Page 67: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

E

. / +

# #%$

!" # '$1

#$ % $%(!$$&

#%5('='>"'$ $ ' '

' ' ## !#7"'

$ 5('='=!

%7"'

0 %(&$ &%

(#$ &. %!"'

( ) νν σ !5('='?"

< νσ %$%( %!"'

) ) C−

νσ = + !5('='>"

<%#%'%(&

$& %!"'

( ) νν ∇ σ !5('=';"

< / # ∇ % '

6,(#$ $&#!$

"& $$ ( $!%7"'

$6,('

!%7"'

Page 68: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

E $

$% '#

$&#$$% $$

.# '

$ $2#%$

1&%# ' $

$( $ ! [=).$

[).$"! *"'%# &$ &

$(#!"&&' $$&

$&' ' '& $&

%$#$'- #%$*

$ #%#$ &T!"'

!%7"

&& '& &

$'

&

&$$H $#%$'

+ $ !H $"$

!H $"'&$$#E''E'''

# $& #! *"'

Page 69: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

?

" $& H $

#%$ ( ) % '

" $& ( ) %∆θ ∆ .(

#%$ ( ) % '

7" 2$#%$ "#H $'$*

$$&#%$ "" ( ) %∆ ∆

$#% ( ) %δ δ # ( )ε '

=" 2 $ 7"##%$ " $

% ( ) % C '

&$ E''

-9

% $ # #

$$!"$ %

'OD'O'

2

.(

H $

H $

∆θ%∆

%

%

+

− ∆%∆

+

% %

C

%δ εδ

Page 70: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

>

!E"'+ % $ # $

%!%7"'$

# #, # $ #'

% ##% #$ $

#%!$"'$

$$& $

#!"'3&$$H $

$. ! *"'

%

H $!OH $"'

$$#E''7'#

$& #!"

" $& ( ) %∆θ ∆ .(

#%$ ( ) % '

" 2&# $ #%$ "#

# 3## ( ) φ φ '

7" 2$## 3## $ "#O

H $'$*$$&# 3##

$ "&# 3## ( ) φ φ

$#% ( ) %δ δ # ( )ε '

=" 2 $ 7"##%$ " $

% ( ) % C '

Page 71: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

;

% $ % #% !$

"# $#$ '$#&

# 3## !E"' $

& $&'

% %. H $&

# !%7"'3

!"H $ $

. ! *"'

&$ E''

-$ %

2

+ $ I#JI#O, J

'$ # $%&

'OD'O'

2

.(

3##

OH $

∆θ%∆

%

φ +

∆φ

+

%

% C

5#

3##

φ

φ

φ

∆φ

δ %δε

Page 72: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

?

'<%#%

$ ' %$*$ * .

(#%&*# #&

! *"' %$ ,

% !%7"'

/ &!$ ,

"&#$ %&'(

%$ &$ $$!%7"'

(

&$$& # '+ I

#JI$*JI##JI$$&J'I#J

# $ H $! "O

H $! " .# ' .

($## !"'- & H $

$$ ###'

$$*&#*& #!#

''"'

I##J *

.(&#'&$ & #

$ .($ $ #

!"'I+ ##J # $#$

'

Page 73: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

?

I#J # & ' '&5,

2'I#J # &$ E'7'+ %#$

&&$ E'E''

-# : %

" ? !

#%# $$$H $

'&$

' '!%7"'

& !!

2 ##%$2

$$$ $'% $$!"'

*& $%$ $2 *&'

2 % # ## $ ! ,$ "

% %!5, "'

'OD'O'

2

.(

H $

∆θ%∆ +

% %

C

%δ εδ

OH $

%δ εδ

+

A - ( A

A - ( A

%δ εδ

%δ εδ

Page 74: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

?

2

2

2

L L

M M

N N

= − −

!5('E'L"

< % % ## $ ( ) 2 OLMN $

2#% % ## $ '5 /&

$ ## $ C /&% $ ##

$ C *&#$2!"'

( ) 2

C

+ = − + ϕ

!5('E'L"

< 2 2 %$2#%/#

% $ '%!"'

( ) 2 % % C C= + Ω + Ω − Ω

< 2 2 %$2#%!52"

% $ ( ) Ω Ω *& $%5('''

Ω *& /$ # '

# + <

2 $$ 2#%

#/#& '$

2 $ &&$

Page 75: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

?7

%!' '$"#$ $).2!5, "'5%

$& %!' '"#O/#'

# &2 $$

#$ !"

* *

* *

* * * *

− −

− −= +− −

!5('E'L"

* *

* *

* * * *

% % %

− −

− −= +− −

!5('E'L"

< #$ #$ E'='

-,/1 2 5 4 *

$ % ( '24 ,

#'4 , #' '!"'-

$2T &, .'

$## $ #

2 !5, "'

2

* * +* −* −

Page 76: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

?=

4 0#

.#$ $ #%

$## &4 0'

/ 3 5 (

1%$# & #, '

## &$ &%&* $$ $ 2

# &%'$ %&%!

&#3## "% !

# # "'+ % $ %&'#$

& $& $ S

2 / 6 7 0$C5L'$

" 6 7 0$C5L%'$

# 8 0$/$

$ + /

/$&# &&!$

#"&( $ !$ %"'

C5L$ $$ &&&

# #$ '-%&&

#&$$ $&# & &%'#

$$C5L$ # & $$

%/$$ '

Page 77: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

?E

/#&## ' & %

$ & # $'

# '- %#

&#$# $#&$$

#'

$#%# $$ ' #

$#/#$& #'

' 6$ # $ % $'

' 6!" &

$ '*&$ !"'

7' 6 & #

# $ %$' &

% $$%$ # # #

'

=' 6$$$& $ '7'

E' 6 &% #

# $ %$! #$7'"'

' $ =' $ E'

?' 6#$% $ '

( '

/#%# .(

#''0$ . %#$%

/&$ % $ '

$$* $. $$&&%

%#'#$$$' 'E O'- $*

Page 78: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

?

$ #$

#'='#$ %%

&. &*$ #'

< / # .(

&,%#%'$$&

%$ H $! #E''"

$#% & '#

,%#%!$ "&

/&&$ &'

/

<# $# '$&

$ & !##$* FF"'

& $$#$# %

&T$' /##$ $

+ $$!+ "&$$#$# '<%

% &#+ #/#$

# '

# & .(& # '##

& %#'# & *

$%$ &$' &

%#&& #%#'+ $&

!##&/#$&$ %"

& 1&#%#!" #T

Page 79: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

??

*##&%'$ '&&

%1$ %'

+ $ ;% % 5 4 * /3 * ;% % % <"

$&T*&%1$ '&**$'0

&1&/# !#% %"

&& '-**%#1*#$& &

'0$ ' $&1#

%# '*$2

#$ %1$ '

&$ * # #&'2$

&$!& # & &

%1"'&$ & .(

$'

Page 80: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

?>

+ =% % ;+

3$ $# .

'3 6 # #

$ 2%$6 $$# ' *&% #

$ #?'-#

& ## &## *'

! - *

$# $$63$&

'# $ '' 4 0 ,$

I' J'*$63 I' J

& '

# $ I ' JI.5(' J% '

Page 81: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

?;

? 3

###$ #$ #'

1% #$ #

% $$'3$

#$$& $#%

'- & $/$ #$&

$ # # '

4 0# $ !#"

&$# '6UU: !

% "$&&'

) $" % B

: &%#74 % !4 " #

3# $ 5 2%$6 2%$6 '

$&#$ $ # $

#%, #, '&

#, '

$&& #$&$$#

#$$$# % ! #"'

+ #$&$# ## '$#

&%$#$&%'$ $$#$.

$& '

Page 82: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

>

) %

&-E#$O#1'

&&$6 # 0*#&

#% % $#&&*&'

# $1$ &

: ' & #$ &$&

7 $* '* &##$%#$, &

.%#'.%#&'- ?'&

$1, & *& N2

& #$ ' * #&

#$ ,,#!52"$ & #

& &'# 0*%$

1&$ ?'?''

6 ;

Page 83: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

>

6 =% ;

& 1& $&

% '& $$ #$ '3

$& ,&, & *0

#$ % &'&&. &&'

) #

2&$& )&)

?%+ 5=,$(%&%'2

& &, $ .&/#

!%7"'0 %& % %

'2$%& $/&&

$$%$ 2%$6 '% &2&

&!"'#$$2?''

*> 5 6 ! ! $ + (

0!c " '] O

$!c " 7## E##

C &*! /" ' OOX 'E OX

$ 6 *> > 5 6 ! ! /1 2 4 ! ! ! # 5 % ! ! + "

Page 84: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

>

) $ +

#$ &$$1&'$ #

$1&$% $& #1&

! "'$$&

&1 $ &$&&

O& #'

) (

0$# $$# %& #$'

$# &*#$/ # &# &

. ##$ '&T&

## # %$$

# &#!& $# $

## *"'?'& ## % #$'

- (

4 ?

6 VW ;

- VW

5% *V W

26 M

6%V W 'E

24 3## M

4 3##%V OW '7

4 #%V W '7

4 #%V W 'E

- / (

4 @

4 O<4

O O4

$ 6 : % % *0 /3 $ A 4 ! ! # 5 % ! ! + "

?'$# 4 <4 ! $

4 4 "O & # '6 #

& %.#?'='

Page 85: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

>7

) . ! 3 +

$1&#$

('+ $& # $ ?'=4 &'

&/#4 <4 &

% #% !$"# $

#$ !%7"'

$ ?'7%& '%

$ *& ? $&

%'2 $ #$ $

$* '

6 # *

Page 86: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

>=

-/ &$$/#$!"

*# '##/'# &&

% # % $/%

,,$'0&$& # #

&/# '

-$ % &&='?* $

?'$$$&$/#

###%#!#7'="'

$% % %

$$ $##%#

%$ #&?'7' % $&

$1 $#&&*%

$1'

9 + $

( E ( '> O 4 % '=

3 % ='? 3 % 'E O '7

2 7'7 2 '> O 0 % '=

# . ?' # . '= O ,

$ 6 # 1 */3 * 5 4 * ;

, !73"C! (" %$#

/#$1#$ 73#

'73% 7 O$&$

1 #& /'-#

%$/ &$ $

#&% $4 <4 !%7"'

Page 87: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

>E

)

.#$ $ $%$$

! #"& ' #& #$$ $%

&#=$ #'$ #&$ ?'='

6 ,*

#&$ ?'=$= #'$ #

& $$ $ $$

$ !#7'="'

$ #&% &$$%$ #&'

# #& ' '&$ $

&%!&"&# #&

')% *&E_ . $

Page 88: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

>

$&%'&$ #'<

& &! $

#E''OH $&

$&$&&"' $% # #&$ #

# #'

) +

#$ &C

&#$$1#$ 'C

&$% # .$$$ $ #

$% '$1& $

#!#?''"#$ &/#

$C')C& $#$ $

%'-%$$C& #'

##& /&.73&&

/#%'C&$$ &'

) - ( / 0 ( (

C$ ## # 4 #

$ ?'E?'!/# "'&$ ?'E&#

&($ $ $ # '&

/##& $!#''="'

C73 $= #' %

# #$'- 7'&&

$$$&#$ ')$

& %$$=$ # #$

$$'$$& ## #'-

# # $$($ %

'- #&&% # #'

Page 89: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

>?

6 -1 * 9 % %

6 + 1 * 9 % %

Page 90: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

>>

) - ( / 0

$ # ## #$4

'- ?'?&C$ # # 4 #

' $$$&$

?'? #$ ?'E'&/#$$&

& '$$$# #&

$ $ '5%

&% # #'$=

$# #C#'?E $

#%'3 % % ' '&$ $

##!$%" $

&#$ #'

6 6 1 * 9 %

Page 91: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

>;

6 71 * 9 %

) - ( / 0 & = & (

- #$# 4 #

<4 !<4 "! $4 4 # ".'

- ?';?'!/# "&C# ## #

<4 ' #$ ?'E?''3 &% $<4 !'>;

#'; $4 !%7""&/# &

$ <4 && 4 '#$#

#&$$& &%#'2 4

$<4 $&#$ $# #'

+ %&C#E $ # $

#% #'- # $$'&

&#$ $4 &#$ <4 '

#&$# #$$1$&% '

Page 92: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

;

6 B 1 * 9 % %

6 ! 1 * 9 %

Page 93: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

;

) 9 + +

*$ #&#%$#

$%'$ ?'?'!/# "&%C

& ##C'$$

.$#$$ ##

&'C% # #&$4 #

? O$=7'E O$# #$

'* $ ?'

7 #' #$

% $ ##% % '

6 1 * 9 % %

- $% $ $#

&$ '

Page 94: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

;

6 1 * 9 % %

) $ +

-$ ?'7!/# "'##

#. C' &$ $ &

. '$ ?'7. $

$ #'-. C & #

'7 $ # ##=&T' $

#'$& #%&#$$ #$ '! O

& ' ="#$ $#

&$. '

Page 95: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

;7

6 # 1 * 9 % %

)) % !5 / + !

#%#$ . #$

## #' # %$ #

$/ ' # ##$

#% $ !%

7"'- ?'=!/# "&% $/ $ # #&

$$$ ' & $$,

%##$/ '$ ##$/

$& ' % &$

'- ?'E!/# "& $/ $$ #

! #"' % $ &

$ #' $$$$ #'

$ $ #E & $ #'

Page 96: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

;=

Average time to fix L1 ambiguities after complete gaps

0

20

40

60

80

100

120

140

160

180

200

Gap 1 Gap 2 Gap 3 Gap 4 Gap 5

Tim

e to

fix

[s]

Loose coupling

Loose coupling w/seeding

Tight coupling

6 ,0 ? 9 %

Time to fix L1 ambiguities after data gap 1

0

25

50

75

100

125

150

175

200

2 4 6 8 10 15 20 25 30 35 40

Duration of gap 1 [s]

Tim

e to

fix

[s]

Loose couplingLoose coupling w/seedingTight coupling

6 -0 ? 9 %

Page 97: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

;E

), $ + ( /

$#$* # #$

: '&$#!%"& $

#$ $ '?'E& #% $$

#$$% #' $$$&

#'% ##$='

$$& $ '

$N, #! &. $

%%"#'E O$ #!7 "'

#$$2 #& O!?'"'-

$#&% $#$%'

= ( = ( ( % #V OW

(

(

! ,! (

C) ' 'EE '7 '7 '7? 'EE

D) '? '>;; '= ' ' '>;;

=) 'E 'E;E 'E77 'E=> 'EE 'E;

$ 6 -0 % % 9 % %

% $ $$#&

?'' % &

= #' #$$2 &]

#%&'

= ( = ( ( % V] W

(

(

! ,! (

C) 7=? E;7 EE EE EE E;7

D) =7 E? ;? ;? ;? E>

=) ?; ?E ?E; ?E; ?E; ?E

$ 6 + 0 % % 9 % %

Page 98: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

;

> 6

3$$&#' &

&$$ ('&

%# $&# &#$ $

& '

# $% & $'(

4 0# %#$ & #$!

$ "'&#$ #*##&'

&# ## & '

# &&# &(*&*'*&

%3 *#'

* +

1%$& #$

#% $$'

#&#$ &#$#% %

#$ # # # ='!%7"&

/ $$ 2'3 # ##$ &

#%$'E,'&$ $

# #%#$# #'-$4 #<4

&% & <4 4 &#$'

)$!$ 2"& 4 #

& %$$=$ # #

$$$'

Page 99: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

;?

-% && $

4 $<4 '-.

! ##"% % $$'

$ % $/%$ &

% '&$ $ $/

$ # $ #&'$

& #

'

$$

#$ $% #&$

*$#'-% $ #

$$#$2 %! "$&%

$$$% ##'

Page 100: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

;>

; C$

$&&## T

'

' C? . 6)7

I% J<,&M*

* C? # & < 6 7

IJ B 0B 3 *'

: + C" 6 7

I(O3 J5+ 763# $

5 2%$6 6'

/ C$6)7

I##+ # 5 J$ 2'

C6 7

I$ 5%$4 &65,02 <$4 D

% ##J3# $ 5 2%$

6 6265C#7;'

D 1 C- 6 7

I% &##J<2'

Page 101: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

;;

D CD D ? 8 6 7

I2- ,4 5 J'$555D';'7='

& ( C 6 7

I%##J5+ E63# $

5 2%$6 6'

# C 6 7

I [ $ J , 2'

C# 6 7

IC, $,2$) ,

% J'3'3# $ 5 2%$6

6265C#?7'

C :;6 7

I #% $4 &6O$4 ##J3# $

5 2%$6 6265C#E'

C :;6 7

I5 ($4 &,6% J'3'3# $

5 2%$6 6265C#;'

C& 6 7

I$ % $ * J$*

$ HB %2%3 *

Page 102: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

% C* ; D & = 6 7

#&% 555C% ?'

% CD ':E 6 7

I- $ C%J5' <d 2'

= C# ? 0 < 6 7

I#& 2 5,-/6- J $$

% ;;D'7?''

= CD & * ;% 6 7

I% ##J5d 6

5 #'

Page 103: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

##

Page 104: INS and GPS integration - imm.dtu.dk · Technical University of Denmark Informatics and Mathematical Modelling Building 321, DK-2800 Lyngby, Denmark Phone +45 45253351, Fax +45 45882673

$ $ / ! ! 3 + +