mechanization of inertial navigation systems in local level north pointing navigation frame

Inertial Navigation Systems

Muhammad [email protected]

Mechanization of Inertial Navigation System in Local Level North Pointing Navigation Frame

Muhammad Ushaq 2

In designing an inertial navigation system, the navigation engineer mustdefine from the onset a coordinate system where the velocity and positionintegrations must be performed

The particular INS coordinate frame selected will be used in the onboardnavigation computer in order to keep track of the vehicle velocity, position,and attitude estimates

Three types of INS configurations are commonly employed, depending on the application:1. Local-level mechanization (Torqued)

a. North-slaved (or north-pointing)b. Free azimuthc. Wander azimuth

2. Space-stabilized mechanization (Un-torqued)3. Strapdown (gimbal-less or analytic) mechanization (Un-torqued)

COORDINATE FRAMES FOR INS MECHANIZATION

Muhammad Ushaq 3

I. Geographic

II. Wander-azimuth

III. Space-stable

COORDINATE FRAMES FOR INS MECHANIZATION

Muhammad Ushaq 4

Coordinate Frames

Ye (λ=900)

Xi

Zi , Ze

Xe λ=00

φ =00

Yi

Polaris

h

Earth Reference Ellipsoid

Inertial Ref Meridian

Greenwich Meridian

Earth Rotation Vehicle Position

ωieΔt

λ

Local Meridian

Equatorial Plane

Xg

Yg

cφ φ

Zg,

*

pole

Ω

Greenwich meridian

Xi

Ye

O

Ωt

Yi

Xe

ZeZi

P

N

L

U

E

equatorial plane

local meridian

plane

Local geographic Frame axes

Earth axes

Inertial axes

Muhammad Ushaq 5

Coordinate Frames

Ob

zb

xb

yb

Muhammad Ushaq 6

Coordinate Frames

Earth centered Inertial Frame (i)

Earth Centered Earth Fixed Frame (e)

Local level Geographic Frame (g) Xg Yg Zg = E N U

Body Frame (b) (X=right wing, Y=longitudinal-fwd, Z=up)

Platform FrameThe platform frame's axes are parallel to the nominal input axes of

accelerometers

True FrameThis frame corresponds to the ideal or error-free orientation of the inertial

platform; at the vehicle’s actual position. This frame is mechanization-

dependent.

Muhammad Ushaq 7

Coordinate Frames

Computer Frame

The computer frame is defined as the frame in which the navigation equation

mechanization actually occurs. This reference frame is specified by the

navigation system outputs of position and velocity. Due the certain errors, this

frame will not be the same is the true frame in which the equations are

nominally mechanized.

Tangent Plane

This is a local-level system and is fixed at one point on the earth only

Strapdown (or body frame)

The coordinate axes are fixed to the vehicle.

Muhammad Ushaq 8

Coordinate Frames

Accelerometer FrameThis frame is a non-orthogonal frame defined by the input or sensitive axes ofthe instruments mounted on the inertial platform.

Gyroscope FrameThis frame, like the accelerometer frame, is a non-orthogonal frame defined bythe input or sensitive axes of the instruments mounted on the inertial platform.

Muhammad Ushaq 9

Coordinate Frames

The true frame platform frame, and computer frame are usually coincident,however, because of inertial navigation system errors, the system designer mustaccount for small-angle misalignments between these frames.How to choose a frame for Mechanization

Individual choice can be made on the basis of:a. Mission requirements

b. Measuring Instruments available

c. System complexity and Interface with other avionic subsystems (e.g., flightcontrol, weapon delivery)

d. System environmental constraints

e. Worldwide navigation capability

f. Onboard navigation computer computation speed

g. Storage capacity

h. Ease of implementation of the navigation system equations and algorithms

i. Algorithm complexity, computational burden

Muhammad Ushaq 10

Characteristics of an Optimum Navigation Algorithm

a. Autonomy

b. Accuracy

c. In-flight adaptability

d. Mission flexibility

e. Reliability

f. Computational efficiency

g. Robustness

Muhammad Ushaq 11

Characteristics of local level mechanization

Locally level system allows the two horizontal gyroscopes to maintain

their output axes vertical and their input axes in the horizontal plane. In

this arrangement, the g-sensitive drift is due mainly to the mass unbalance

about the output axis of the gyroscope, and as a result it is virtually absent

for the horizontal gyroscopes in this mechanization.

Gyroscope drift-induced position errors for this mechanization are caused

predominantly by the horizontal gyroscopes.

The vertical gyroscope is subjected to g-sensitive drift.

local-level system will have a bounded oscillatory latitude error and a growing longitude error in response to a constant gyroscope drift.

An inertial system, on the other hand, will have unbounded latitude and

longitude errors in response to the same drift.

Muhammad Ushaq 12

Local Level (ENU) Mechanization

The vehicle position is defined by a coordinate system such that the x axispoints east, the y-axis points north, and the z-axis up.

pole

Ω

Greenwich meridian

Xi

Ye

O

Ωt

Yi

Xe

ZeZi

P

N

L

U

E

equatorial plane

local meridian

plane

Local geographic navigation

axes

Earth axes

Inertial axes

VehiclePosition

Muhammad Ushaq 13

Transformation from ECI Frame to ENU Frame

This mechanization is achieved from following angular rotation starting

from ECI frame

/e

90 90 ( ) Z axis Z axis X axis

λ φΩ + −→ →→o o

iei i i e e e g g g

tX Y Z X Y Z X Y Z ENUabout about about

From earth centered inertial frame to earth centered earth fixed frame

Z axisΩ→ie

i i i e e etX Y Z X Y Z

about

( ) in( ) 0( ) ( ) 00 0 1

Ω Ω = − Ω Ω

ie ieei ie ie

Cos t S tC Sin t Cos t

The geographic frame (ENU) has its origin at the location of the inertial navigation system. This is a local-level frame with its ,g gx y , axes in a plane tangent to the reference ellipsoid and gz perpendicular to that ellipsoid.

Muhammad Ushaq 14


From Earth fixed frame to Navigation to ENU

/e

90 90 ( ) Z axis X axis

λ φ+ −→→o o

e e e g g gX Y Z X Y Z ENUabout about

1 0 0 ( 90) ( 90) 00 (90 ) (90 ) ( 90) ( 90) 00 (90 ) (90 ) 0 0 1

λ λφ φ λ λφ φ

+ + = − − − + +

− − −

ge

Cos SinC Cos Sin Sin Cos

Sin Cos

(90 ) (90) ( ) (90) ( ) ( )(90 ) (90) ( ) (90) ( ) ( )( 90) (90) ( ) (90) ( ) ( )( 90) (90) ( ) (90) ( ) ( )

φ φ φ φφ φ φ φ

λ λ λ λλ λ λ λ

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

Cos Cos Cos Sine Sin SinSin Sin Cos Cos Sin CosCos Cos Cos Sin Sin SinSin Sin Cos Cos Sin Cos

Muhammad Ushaq 15


geC gets simplified as:

1 0 0 00 00 0 0 1

λ λφ φ λ λφ φ

− = − −

−

ge

Sin CosC Sin Cos Cos Sin

Cos Sin

0λ λφ λ φ λ φφ λ φ λ φ

− = − −

ge

Sin CosC Sin Cos Sin Sin Cos

Cos Cos Cos Sin Sin

geC is called position matrix

Muhammad Ushaq 16

Updating Latitude and Longitude from 𝐶𝐶𝑒𝑒𝑔𝑔

geC is also called position matrix for the geodetic frame mechanization and

position of navigation system (latitude and longitude ) can be computed from the

updated solution of geC as follows:

3,31sin ( )φ φ −= = g

em C

3,21

3,1

tanλ −

=ge

m ge

CC

31

31

31

0180 0 , 0180 0 , 0

λλ λ λ

λ λ

>= + < <

− < >

m

m m

m m

if Cif C andif C and

0λ λφ λ φ λ φφ λ φ λ φ

− = − −

ge


Cos Cos Cos Sin Sin

Muhammad Ushaq 17

Updating 𝐶𝐶𝑒𝑒𝑔𝑔

Position matrix may be updated by solving following differential

= −Ω

g g ge eg eC C

00

0

ω ωω ωω ω

−Ω = −

−

g gegz egy

g g geg egz egx

g gegy egx

Muhammad Ushaq 18

Orthogonalization of 𝐶𝐶𝑒𝑒𝑔𝑔

Orthogonalization algorithm is included to ensure that the geC rows and

columns remains normal and orthogonal. If geC is to be orthogonal then

following relationship should hold:

111 2 [( ) ]−+ = +T

k k kX X XWhere 0 (0)= g

eX C , k=0,1,2,3.. and T represents transpose of matrix

Muhammad Ushaq 19

Transformation from ENU Frame to Body Frame

Body frame (b) is defined as:X: right wingY: longitudinal (forward)Z: Vertical (Up)

g Z axis axis axisψ θ γ− ′ ′ ′ ′′ ′′ ′′→ → →

′ ′′g g g g g g g g g b b bg g

X Y Z X Y Z X Y Z X Y Zabout about X about Y

0 1 0 0 00 1 0 0 0

0 0 0 0 1

γ γ ψ ψθ θ ψ ψ

γ γ θ θ

− − =

−

bg

Cos Sin Cos SinC Cos Sin Sin Cos

Sin Cos Sin Cos

( )

γ ψ γ θ ψ γ ψ γ θ ψ γ θ

θ ψ θ ψ θ


+ − + −

=

− − −

bg

Cos Cos Sin Sin Sin Cos Sin Sin Sin Cos Sin Cos

Cos Sin Cos Cos Sin

Cos Cos Cos Sin Sin Sin Sin Cos Sin Cos Cos Cos

C

ψ : heading

θ : pitch

γ : roll

Muhammad Ushaq 20

Earth Spin Rate in ENU Frame

Earth spin rate expressed (coordinatized) in earth fixed frame is given as

00

ωω ω

ω

= = Ω

X

Y

Z

eie

e eie ie

eieie

Earth Spin rate can be transformed into ENU frame by pre-multiplying this rate with the transformation matrix g

eC

0 00

λ λφ λ φ λ φφ λ φ λ φ

ωω ω

ω

− = = − − Ω

X

Y

Z

eie

g g eie e ie

eieie


Cos Cos Cos Sin Sin

Muhammad Ushaq 21

Earth Spin Rate in ENU Frame

0CosCos

φφ

ω = Ω Ω

gie ie

ie

Ω =ie Earth’s rate= 57.29115 10 [ / ]−× rad s

Muhammad Ushaq 22

Vehicle Transport Rate

The turn rate (the rate caused by movement of vehicle w.r.t. earth) of the ENU

Frame with respect to the Earth is called transport rate and is obtained from

φ

ω

− +

=

+

+

gy

Mg

g xeg

Ng

x

M

VR hV

R h

V TanR h

ω gegy

ω gegz

ω gegx

Muhammad Ushaq 23

Analytical Precession Command (Spatial Rate)

Gyroscope torqueing ratesω gigx ,ω g

igy and ω gigz with respect to inertial space are

ω ω ω= +g g gig ie eg

0φφ

ω ωω

=

gie ie

ie

CosSin

0φ φφ

φ φ φ

ω ω ω ω ωω

ω

− −

+ + = + = + = + + + + + +

g gy y

M Mg g

g g g x xig ie eg ie

N Nie g g

x x

M M

ie

ie

V VR h R hV VCos Cos

R h R hSin

V VTan Sin TanR h R h

ω gig is also be used in updating of b

gC or equivalent quaternion

Muhammad Ushaq 24

Gyro Torqueing Rates for ENU Frame

The two rates ω gigx and ω g

igy represent the level angular

rates of the platform required to maintain the platform level

ω gigz , represents the platform azimuth rate required to

maintain the desired platform orientation to north; that is

ω gigz , defines a north-pointing system mechanization

Muhammad Ushaq 25

Updating Velocity and Altitude

( 2 )ω ω= − + × −

g g g geg ie

g ge ef gV V

Here

= =g g bib b ib

g f C ff

bibf is the output of Accelerometer triad in body frame

and ( ) ( )1−= =

Tg b bb g gC C C

0 (2 ) (2 ) 0(2 ) 0 (2 ) 0

(2 ) (2 ) 0

ω ω ω ωω ω ω ωω ω ω ω

+ − += − − + + +

+ − + −

g g g g g g gex x iez egz iey egy exg g g g g g g

ey y iez egz iex egx eyg g g g g gg

z iey egy iex egx ezez

V f VV f V

f V gV

Muhammad Ushaq 26

Updating Velocity and Altitude

By Integrating we get the velocity vector in the reference frame as

1

1( ) ( )+

+ = + ∫ n

n

n n

tg g g

e e et

V t V t V dt

To get updated altitude, integrate the third component of

the velocity g

eV vector as

1

1( ) ( )+

+ = + ∫n

n

n n

tg

ezt

h t h t V dt

Ground velocity is given by following equation

( ) ( )2 2+= g g

ground ex eyV VV

Muhammad Ushaq 27

Position update from Updated Velocity Components

ω gegy

ω gegz

ω gegx

φ =+

gy

M

VR h

( )λ

φ=

+

gx

N

VR h Cos

= gzh V

Vertical channel of INS being unstable

altitude can be updated by barometric

correction as:

1( )= + −

gz Bh V K h h

Muhammad Ushaq 28


ω gegy

ω gegz

ω gegx

Here gyV , g

xV and gzV are north, east and

up/vertical components of velocity of vehicle

with respect to the earth, respectively.

NR is the east-west radius of curvature of

earth or constant latitude radius of

curvature of earth. It is given as follows

2R 1( )φ= +N eR SineMR is referred to as the meridian radius or

curvature for north-south motion. It is also

called as constant longitude radius of

curvature of earth. 21 2 3( )φ= − +M eR R e Sine

Muhammad Ushaq 29


( ) ( )φ φ φ+∆

+ ∆ = + ∫ t t

t

t t t dt

( ) ( )λ λ λ+∆

+ ∆ = + ∫ t t

t

t t t dt

( ) ( )+∆

+ ∆ = + ∫t t

zt

h t t h t V dt

Muhammad Ushaq 30

Updating Gravitation Field Vector

Gravity can be updated using following

2 -69.783 0.051799 0.94 10φ= + − ×Sin hg

For geographic frame we have

g

0g 0

= g

Muhammad Ushaq 31

Strapdown Implementation (Computation of Attitude)

Following sequence of transformation is made to reach from navigation

(g) frame to strapdown (body) frame

g Z axis axis axisψ θ γ− ′ ′ ′ ′′ ′′ ′′→ → →

′ ′′g g g g g g g g g b b bg g

X Y Z X Y Z X Y Z X Y Zabout about X about Y

0 1 0 0 00 1 0 0 0

0 0 0 0 1

γ γ ψ ψθ θ ψ ψ

γ γ θ θ

− − =

−

bg

Cos Sin Cos SinC Cos Sin Sin Cos

Sin Cos Sin Cos

( )


θ ψ θ ψ θ


+ − + −

=

− − −

bg

Cos Cos Sin Sin Sin Cos Sin Sin Sin Cos Sin Cos

Cos Sin Cos Cos Sin

Cos Cos Cos Sin Sin Sin Sin Cos Sin Cos Cos Cos

C

Muhammad Ushaq 32

Updating Attitude DCM bgC

Let us denote transformation matrix from the navigation to the body frame at time t

as ( )bgC t and that at time ( )+ ∆t t as ( )+ ∆b

gC t t . Let body frame at time t is

denoted by ( )b b bX Y Z t and that at time ( )+ ∆t t it is denoted by ( )+ ∆b b bX Y Z t t. Let during this very small span of time following rotations take place in body frame.

' ''b b b

( ) ( ) X Y Z

θθ θ∆∆ ∆→→→ +∆yx zb b b b b bX Y Z t X Y Z t t

about about about

Hence the corresponding transformation matrix at time ( )+ ∆t t will be given as follows

( ) ( ) 0 ( ) 0 ( ) 1 0 0( ) ( ) 0 0 1 0 0 ( ) ( )0 0 1 ( ) 0 ( ) 0 ( ) ( )

( ) ( )θ θ θ θθ θ θ θ

θ θ θ θ

∆ ∆ ∆ − ∆

− ∆ ∆ ∆ ∆

∆ ∆ − ∆ ∆

+ ∆ =

z z y y

z z x x

y y x x

b bg g

Cos Sin Cos SinSin Cos Cos Sin

Sin Cos Sin CosC t t C t

Muhammad Ushaq 33


As θ∆ x , θ∆ y θ∆ z are very small (because Δt is assumed to be very short time) so

Cosines of these angles are equal to unity and Sines are equal to the angles

themselves. Using this trigonometric identity we have following

1 0 1 0 1 0 0( ) 1 0 0 1 0 0 1 ( )

0 0 1 0 1 0 1

θ θθ θ

θ θ

∆ −∆ + ∆ = −∆ ∆ ∆ −∆

z yb bg z x g

y x

C t t C t

0( ) ( ) 0 ( )

0

θ θθ θθ θ

∆ −∆ + ∆ = + −∆ ∆ ∆ −∆

z yb b bg g z x g

y x

C t t C t C t

Muhammad Ushaq 34


0( ) ( ) 0 ( )

0

θ θθ θθ θ

∆ −∆ + ∆ − = −∆ ∆ ∆ −∆

z yb b bg g z x g

y x

C t t C t C t

Dividing this equation by ∆t and taking limit 0∆ →t , we have following

0 0

0 0 0

0 0

0 lim lim

( ) ( )lim lim 0 lim ( )

lim lim 0

θθ

θθ

θ θ

∆ → ∆ →

∆ → ∆ → ∆ →

∆ → ∆ →

−∆ ∆ ∆ ∆

+ ∆ − ∆−∆ = ∆ ∆ ∆ ∆ −∆ ∆ ∆

yzt t

b bg g bxz

t t t g

y xt t

t tC t t C t

C tt t t

t t

Muhammad Ushaq 35

Updating Attitude DCM

Hence we have

bgC

0

( ) ( )lim ( )∆ →

+ ∆ −=

∆

b bg g b

t g

C t t C tC t

t

0 0

0 0

0 0

0 lim lim

( ) lim 0 lim ( )

lim lim 0

θθ

θθ

θ θ

∆ → ∆ →

∆ → ∆ →

∆ → ∆ →

−∆ ∆ ∆ ∆

∆−∆ = ∆ ∆ ∆ −∆ ∆ ∆

yzt t

b bxzg t t g

y xt t

t t

C t C tt t

t t

Muhammad Ushaq 36


It can be conceptualized that,

0 0 0lim , lim and limθθ θ

∆ → ∆ → ∆ →

∆∆ ∆∆ ∆ ∆

yx zt t tt t t

are the components of

angular rate of body frame with respect to navigation frame during the time from

t to + ∆t t

0lim θ ω∆ →

∆=

∆bx

t gbxt

0limθ

ω∆ →

∆=

∆y b

t gbyt

0lim θ ω∆ →∆

=∆

bzt gbzt

Muhammad Ushaq 37


So we can write

0( ) 0 ( )

0

ω ωω ωω ω

− = − −

b bgbz gby

b b b bg gbz nbx g

b bgby gbx

C t C t

0( ) 0 ( )

0

ω ωω ωω ω

− = − − −

b bgbz gby

b b b bg gbz gbx g

b bgby gbx

C t C t ( ) ( )= −Ω

bg

b bgb gC t C t

Where Ωbgb is the skew symmetric matrix corresponding to .ωb

gb

This equation can be updated using Runge Kutta Integration method as follows

Muhammad Ushaq 38


1 ( ) ( )= −Ωb bgb gk t C t

t2 22 ( ) ( ) 1∆ ∆= −Ω + + tb b

gb gk t C t k

t2 23 ( ) ( ) 2∆ ∆= −Ω + + tb b

gb gk t C t k

4 ( ) ( ) 3= −Ω + ∆ + ∆b bgb gk t t C t tk

( ) ( ) ( 1 2 2 2 3 4)6∆

+ ∆ = + + + +b bg g

tC t t C t k k k k

Here ( )Ωbgb t is the skew symmetric matrix corresponding to ωb

gb at the start of the

navigation cycle whereas t2( )∆Ω +b

gb t is the skew symmetric matrix corresponding

to t2( )ω ∆+b

gb t at the mid of navigation cycle. And ( )Ω + ∆bgb t t is the skew

symmetric matrix corresponding to ( )ω + ∆bgb t t at the end of the navigation cycle.

Muhammad Ushaq 39

Computation of ωbgb

As

ω ω ω= +b bib ig

bgb

ω ωω∴ = −b bib ig

bgb

ω ωω = −b gib ig

b bggb C

ωbib is the output of gyroscopes and ω

gig is the analytical

precession command

Muhammad Ushaq 40

Quaternion Updating

Quaternion method based on the attitude representation with respect to

navigation frame based on four parameter representation. The

transformation is achieved through a single rotation about a defined

vector. Quaternion is represented as:

0

1

2

3

cos( / 2)( / )sin( / 2)( / )sin( / 2)( / )sin( / 2)

µµ µ µµ µ µµ µ µ

= =

x

y

z

qq

qqq

Whereas , ,µ µ µx y z are the components of the vector µ in x,y and z directions；µ is the magnitude of the rotation vector µ .

Muhammad Ushaq 41

Quaternion Updating

Quaternion can also use four parameters with complex number descriptions, which uses a real number 0q and three imaginary components 1q , 2q ， 3q represented as:

0 1 2 3= + + +

q q q i q j q k

Whereas i•i=-1, i•j=k, j•i=-k.

In addition, the quaternion can also be used in trigonometric form as:

(cos sin ) (0 )θ θ θ π= + ≤ ≤

q h I

2 2 2 20 1 2 3= = + + +h N q q q q

2 2 2 1 2 30 1 2 3 2 2 2

1 2 3

cos sinθ θ + += = + + Ι =

+ +

q i q j q kq q q qq q q

Muhammad Ushaq 42

Quaternion Updating

0 1 2 3 0

1 0 3 2 1

2 3 0 1 2

3 2 1 0 3

− − − − =

− −

q q q q pq q q q p

qpq q q q pq q q q p

Muhammad Ushaq 43

Updating Attitude DCM Using Quaternion Updating

At the beginning of navigation, initial quaternion is calculated from initial bgC

(which is obtained by initial alignment) as follows:

[ ]0 1 2 3= TQ q q q q

0 ( ) ( ) ( ) ( ) ( ) ( )2 2 2 2 2 2ψ θ γ ψ θ γ

= +q Cos Cos Cos Sin Sin Sin

1 ( ) ( ) ( ) ( ) ( ) ( )2 2 2 2 2 2ψ θ γ ψ θ γ

= +q Cos Sin Cos Sin Cos Sin

2 ( ) ( ) ( ) ( ) ( ) ( )2 2 2 2 2 2ψ θ γ ψ θ γ

= −q Cos Cos Sin Sin Sin Cos

3 ( ) ( ) ( ) ( ) ( ) ( )2 2 2 2 2 2ψ θ γ ψ θ γ

= −q Cos Sin Sin Sin Cos Cos

Muhammad Ushaq 44


The dynamic equation of Quaternion is as follows 12

( )ω= bgbQ M Q Or *1

2( )ω= b

gbQ M Q

0ω ω ω ω = T

x y zb b bgb gb gb

bgb is a quaternion.

Whereas [ ]0 1 2 3= TQ q q q q

0 1 2 3

1 0 3 2

2 3 0 1

3 2 1 0

( )

− − − − =

− −

q q q qq q q q

M Qq q q qq q q q

0 1 2 3

1 0 3 2

2 3 0 1

3 2 1 0

*( )

− − − − =

− −

q q q qq q q q

M Qq q q qq q q q

Muhammad Ushaq 45


0 0

1 1

2 2

3 3

001

020

ω ω ωω ω ωω ω ωω ω ω

− − −

−=

−

−

b b bgbx gby gbz

b b bgbx gbz gbyb b bgby gbz gbxb b bgbz gby gbx

q qq qq qq q

From the updated quaternion, Strapdown DCM ( bgC ) is

calculated using following relation.

2 2 2 20 1 2 3 1 2 0 3 1 3 0 2

2 2 2 21 2 0 3 0 1 2 3 2 3 0 1

2 2 2 21 3 0 2 2 3 0 1 0 1 2 3

2( ) 2( )2( ) 2( )2( ) 2( )

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

− + − − +

gb

q q q q q q q q q q q qC T q q q q q q q q q q q q

q q q q q q q q q q q q

Muhammad Ushaq 46


2 2 2 211 0 1 2 3 12 1 2 0 3

13 1 3 0 2 21 1 2 0 3

2 2 2 222 0 1 2 3 23 2 3 0 1

31 1 3 0 2 32 2 3 0 1

2 2 2 233 0 1 2 3

, 2( )2( ) , 2( )

, 2( )2( ) , 2( )

= + − − = +

= − = −

= − + − = +

= + = −

= − − +

C q q q q C q q q qC q q q q C q q q q

C q q q q C q q q qC q q q q C q q q q

C q q q q

11 12 13

21 22 23

31 32 33

=

bg

C C CC C C C

C C C

Muhammad Ushaq 47

Attitude Computation from Updated DCM

Actual values of attitude angles are adjusted as follows

1 21

22

( )ψ −=gCtanC

1 13

33

( )γ −= −gCtanC

123( )θ −=g Sin C

Heading

If 22C >0 and ψ g >0 then ψ =ψ g

Else if 22C >0 and ψ g <0 then ψ =ψ g +2π

Else if 22C <0 then ψ =ψ g +π

Muhammad Ushaq 48

Attitude Computation from Updated DCM

Roll

If 33C >0 then γ γ= g

Else if 33C <0 and γ g <0 then γ γ= g +π

Else if 33C <0 and γ g >0 then γ γ= g +π

Pitch

θ θ= g

Muhammad Ushaq 49

Muhammad Ushaq 50

Transformation

bf gf UpdateVelocity

gxV

gzV

gyV Compute

Positionω p

epUpdate Position Matrix

Comp Grnd Vel

V

CompPositon

φλ

ψ

AttitudeComp

θγ

4τ

g Calculation

h

ComputeDCM

NormalizeQuaternion

UpdateQuaternion

1τ

3τ

ωbgb

ComputeAttitude Rates

ωbibAngular

Rate

1τ

Accelerome

ter

Gyro

ExternalAltitude

Infogh

AltitudeComp h

ComputeEarth Rate

ωieω g

ie

2τ

φ

Block Diagram of SINS Mechanization in Geographic Frame

mechanization of inertial navigation systems in local level north pointing navigation frame

Documents