ee 570: location and navigation: theory &...

EE 570: Location and Navigation: Theory & Practice

Navigation Sensors and INS Mechanization

Tuesday 5 March 2013 NMT EE 570: Location and Navigation: Theory & Practice Slide 1 of 13

Navigation Sensors and INS Mechanization Navigation Equations – Case 3: Nav Mechanization

Tuesday 5 March 2013 NMT EE 570: Location and Navigation: Theory & Practice

• CASE 3: Nav Frame Mechanization

Determine the Position, Velocity, and Attitude of the Body frame with respect to the Navigation Frame

• Determine our PVA wrt the Nav frame

Position: Typically described in curvilinear coordinates: 𝐿𝑏 , 𝜆𝑏 , ℎ𝑏

𝑇

Velocity: Typically the velocity of the body wrt the earth frame resolved in navigation frame coords: 𝑣 𝑒𝑏

𝑛

Attitude: Typically the orientation of the body described in the nav frame: 𝐶𝑏

𝑛

Slide 2 of 13


• Determine our PVA wrt the Nav frame

ex

ey

ez

Inertial Frame

ECEF Frame

ix

iy

iz

?

ie

nx

ny

nz

?

en

Nav Frame

bx

by

bzBody Frame

?

nb


Body & Nav frame have the same origin

Inertial & Earth frame have the same origin

Slide 3 of 13

? ? ? ?

ib ie en nb


1. Attitude Update: Method A

Note that:

Now

nb ib ie

b

n

b

e

b b

n n b

b b nbC C

Measured by the gyro

cos( )

cos( ) 0

* * * 0

* * * 0 0

(in in) ( )

n

ie e ie

b

b b b

n e

T

ie

ie

C

L

L s L s L

See next slide


ib i

b b

e en nb

b b

n b n b n b

b ib b bie enC C C

ib i

n b n

e

n n

b benC C

ib

n b b b

eb ie nC

TSk C C Sk C

C Sk Sk C C

Slide 4 of 13


e e n

n n enC C

Last term: 𝛺𝑒𝑛𝑛 = 𝑆𝑘 𝜔𝑒𝑛

𝑛

0 sin( ) -

-sin( ) 0 -cos( )

cos( ) 0

b b b

b b b b

b b b

L L

L L

L L

Courtesy of Mathematica

,

n

en x

,

n

en y

,

n

en z

cos( )

-

-sin( )

b b

en b

b

n

b

L

L

L

,

,

,

Cos( )

n

n

eb N

N b

b

eb E

b

b E b

b

e

n

b D

R hL

L R hh

v

v

v

From handout #2

,

,

,

-

tan( )-

eb E

E b

eb N

en

N b

b e

n

b

n

E

n

E

n

b

v

R h

R h

L

R h

v

v

( ) ( )n n n

b b bC C tC


( ) ( )ib ie en

n b n n n

b bC I t C t

T

n e e

nen nC C

Slide 5 of 13


1. Attitude Update: Method B

Note that: 𝛺𝑛𝑏𝑏 = 𝛺𝑖𝑏

𝑏 − 𝛺𝑖𝑒𝑏 − 𝛺𝑒𝑛

𝑏

Hence, 𝐶𝑏𝑛(+) = 𝐶𝑏

𝑛(−)𝑒𝛺𝑛𝑏𝑏 𝛥𝑡

nb i

b b

b ie en

b b


2( ) ( ) sin( ) 1 cos( )n n

b bC C I K K

bnb t

e e K

b b n n b n

ib ie en

n

n b n bC C C C

TSk C C Sk C

ie

b

e

b n

n iSk Sk C i

b

n be

n nC Sk C

Slide 6 of 13


1. Attitude Update:

High Fidelity

Lower Fidelity


ˆb

nb t k

( ) ( ) ( )ib ie

n n b n n n

b b n beC C I t C t

2( ) ( ) sin( ) 1 cos( )n n

b bC C I K K

nb i

b b

b ie en

b b ib ie

b b n

nn e

b n

nC C

Slide 7 of 13


2. Specific Force Transformation:

Simply coordinatize the specific force

( )n n

ib i

b

b bf C f



3. Velocity Update

Note that: 𝑣 𝑒𝑏𝑛 = 𝐶𝑒

𝑛 𝑣 𝑒𝑏𝑒

Finally,

TSk C C Sk C

2n n n n n

ib b e ebn ief g v

eb eb

n n e

eb

n e

e ev C v C v

2n n e n e e e e

ne e e i ib ebb b eeC v C f g v

2n n n n n e e

ib b en e ebieebf g v C v

2n n n n n n e

ib b en ie ebeebf g v C v


( ) ( ) 2 ( )n n n n n n n

ib beb e ieeb ebnv v t f g v

C Sk Sk C C

a 2e e e e e e

eb eb ib b ie ebv f g v

Slide 9 of 13


4. Position Update

Recalling the relationship between 𝑣 𝑒𝑏𝑛 and the curvilinear

coordinates (see handout #2)

,( ) ( ) ( )n

b b eb Dh h t v

, ( )( ) ( )

eb

n

b b

N

N

b

vL L t

R h

, ( )

( ) ( )cos( )

eb E

n

b

E

b

b b

vt

R h L

,

,

,

Cos( )

n

n

eb N

N b

b

eb E

b

b E b

b

e

n

b D

R hL

L R hh

v

v

v



( )eb

nv

( )n

bC

b

ib

1. Attitude Update

( )

( )

( )b

b

b

h

L

( ) ( ) ( )ib ie

n n b n n n

b b n beC C I t C t

2. SF Transform

b

ibf

n

ibf

( )n n

ib i

b

b bf C f

Grav

Model

3. Velocity Update

n

bg

( )eb

nv

( ) ( )

2 ( )

n n

n n n n n

ib b

eb

e ie

eb

ebn

v v

t f g v

3. Position Update

( )n

bC ( ), ( ), ( )b b bh L

,( ) ( ) ( )n

b b eb Dh h t v

, ( )( ) ( )

eb

n

b b

N

N

b

vL L t

R h

, ( )

( ) ( )cos( )

eb E

n

b

E

b

b b

vt

R h L



• In continuous time notation:

Attitude: 𝐶 𝑏𝑛 = 𝐶𝑏

𝑛 𝛺𝑖𝑏𝑏 − 𝛺𝑖𝑒

𝑛 + 𝛺𝑒𝑛𝑛 𝐶𝑏

𝑛

Velocity: 𝑣 𝑒𝑏𝑛 = 𝑓 𝑖𝑏

𝑛 + 𝑔 𝑏𝑛 − 𝛺𝑒𝑛

𝑛 + 2𝛺𝑖𝑒𝑛 𝑣 𝑒𝑏

𝑛

Position:

𝐿 𝑏𝜆 𝑏ℎ 𝑏

=

𝑣𝑒𝑏,𝑁𝑛

𝑅𝑁+ℎ𝑏

𝑣𝑒𝑏,𝐸𝑛

Cos(𝐿𝑏) 𝑅𝐸+ℎ𝑏−𝑣 𝑒𝑏,𝐷

𝑛



• Combining into a state-space equation:

,

,

,

C

2

os( )

n

n

nn

n n n n nn

ib b

eb N

N bb

eb Eb

b E bb

eb Deb

eb

ib i

en ieb

n b n n n

b be en

v

v

vv

R hL

L R h

vC

C C

h

f g


ee 570: location and navigation: theory &...

Documents