Mechanization of Inertial Navigation in Wander Azimuth Navigation Frame

Inertial Navigation Systems

Muhammad Ushaq

[email protected]

Mechanization of Inertial Navigation System in Wander Azimuth Frame

Muhammad Ushaq 2

Coordinate Frames Employed in W Azimuth Mechanization

Ob

zb

xb

yb

Greenwich meridian

Inertial reference

meridianiet

ix

ex

eziz

ie

Local meridian

iy

gx

gywxwy

N

0( 90 )ey E

S

00

00

Equatorial plane

gzwz

c

Muhammad Ushaq 3

Free Azimuth Frame F F Fx y z

The vertical platform axis in this system is not torqued

This frame is inertially non-rotating along the vertical or z-axis ( 0p

ipz ),

which in effect eliminates the torquing error associated with vertical

gyroscope

The platform axes then diverge in azimuth from the geographic axes as

p g

ipz igz

As 0p

ipz and

gg xigz

M

ie

VSin Tan

R h

gg xigz

M

ie

VSin Tan

R h

Muhammad Ushaq 4

Free Azimuth Frame F F Fx y z

os

g

x

M

ie

VSin Sin

R h C

( )

g

x

N

V

R h Cos

ieSin Sin

ie Sin

The vertical gyroscope being not torqued can be considered as an

advantage, since the vertical gyroscope exhibits worse drift rate

characteristics than either of the horizontal gyroscopes.

Muhammad Ushaq 5

Wander Azimuth Frame w w wx y z

( w w wx y z ) has its wy , axis pointing degrees from

north

wx axis pointing degrees from east

wz , axis is perpendicular to the surface of the reference

ellipsoid ( 0h )

The horizontal axes w wx y , are displaced

from the cast and north axes by the wander

angle .

The wander angle is taken to be positive

west of true north.

Yg

Yw

Xw

Xg

North

East

Muhammad Ushaq 6


When the latitude ( ), longitude ( ) and wander angle ( ) are zero, the

w w wx y z axes are aligned with e e ex y z of the earth-fixed frame

When wander angle ( ) is zero, the w w wx y z axes are

aligned with ( )g g g ENUx y z of the earth-fixed frame

As the vehicle moves over the surface of the earth. The platform

coordinate system diverges by an angle , called the wander

angle., from the geographic or navigation frame.

The divergence rate is due to

I. The vertical component of the earth's rotation

II. The vertical component of transport rate due to vehicle

motion.

Muhammad Ushaq 7


In the wander azimuth system, the vertical platform axis is torqued to

compensate only for the vertical component of the earth rate i.e.

p

ipz ieSin , and not the vertical component of the aircraft transport

rate i.e. Sin or

g

x

M

VTan

R h

This system finds extensive use in many of today's aircraft navigation

system mechanizations, since no singularity exists at the poles.

The platform axes (p=w) diverge in azimuth from the geographic axes as:

p g

ipz igz

0

0g

gw

Muhammad Ushaq 8


gp g x

ipz igz

M

ie ie

VSin Sin Tan

R h

g

x

M

VTan Sin

R h

Muhammad Ushaq 9


S/No Mechanization Vertical Gyro

Torquing p

ipz Divergence from

North ( )

1 North Pointing

(ENU) ie Sin 0

2 Free Azimuth 0 ie Sin

3 Wander Azimuth ieSin Sin

The wander azimuth design eliminates variation of platform azimuth

with time, simplifying the necessary coordinate transformations

Muhammad Ushaq 10


The transformation from geographic frame (g) to Wander Azimuth Frame

(w) is defined by the following Transformation Matrix.

( ) ( ) 0

( ) ( ) 0

0 0 1

w

g

Cos Sin

C Sin Cos

This is a positive right handed rotation about z-axis (vertically

upward axis)

The rotation rate of the w-frame with respect to the e-frame,

resolved in the w-frame, is

w w g

ew g ewC

Muhammad Ushaq 11

Transformation of velocity b/w ENU and W Frame

G

G

gxwx

bx

gyby wy

Yg

Yw

Xw

Xg

( ) ( ) 0

( ) ( ) 0

0 0 1

w g

x x

w g

y y

w g

z z

V Cos Sin V

V Sin Cos V

V V

w g g

x x yV V Cos V Sin

w g g

y x yV V Sin V Cos

w g

z zV V

w g

x x

w g

y y

V VCos Sin

V VSin Cos

Muhammad Ushaq 12

Transformation b/w ENU and W Frame

Yg

Yw

Xw

Xg

( ) ( ) 0

( ) ( ) 0

0 0 1

g w

x x

g w

y y

g w

z z

V Cos Sin V

V Sin Cos V

V V

g w

x x

g w

y y

V VCos Sin

V VSin Cos

g w w

x x yV V Cos V Sin

g w w

y x yV V Sin V Cos

g w

z zV V

Muhammad Ushaq 13

Why we need mechanization in Wander Azimuth Frame

In north pointing navigation scheme the rate for longitude is given by:

( )

g

x

N

V

R h Cos

At North or South Pole when latitude is 90 o, the rate of longitude becomes

The vertical component of precession command given by the equation

g

y

M

gg g g xig ie eg

N

g

x

M

ie

ie

V

R h

VCos

R h

VSin Tan

R h

Muhammad Ushaq 14

Wander Azimuth Frame Mechanization

A completely general solution of the all-earth navigation problem can be

achieved by the wander-azimuth mechanization.

The local-level, wander-azimuth implementation allows a true worldwide

capability and is utilized in many major inertial navigation systems.

In a wander-azimuth, local-level mechanization, the platform is aligned so

that it is perpendicular to the local geodetic vertical.

The gyroscope that senses rotation about the vertical is left un-torqued and

will not maintain a particular terrestrial heading reference.

Muhammad Ushaq 15

Wander Azimuth Frame Mechanization

Local-level, wander-azimuth navigation systems are used for navigating

beyond latitudes 70o.

In general, the local-level frame will have an azimuth rotation relative to

north. This angle, which we defined as the wander angle, varies as the vehicle

moves over the earth from its initial position. For a north-pointing system, the

wander angle 0 ).

Torques are applied to the vertical channel in order to cancel the gyroscope

bias error.

Muhammad Ushaq 16

Wander Azimuth Frame Conventions & Assumptions

The wander azimuth mechanization will be carried out for a local-vertical,

geodetic frame

The wander angle is defined to be positive counterclockwise from north

The ( , ,w w wx y z ) frame to coincide with the computational and platform

frames

The positive direction of the z-axis is up directed along the geodetic latitude,

The positive direction of the y-axis for 0 is north, while the x-y axes form

a plane that is locally level forming the gyroscope x and y-axes respectively

Muhammad Ushaq 17

Coordinate Frames in Wander Azimuth Navigation

Greenwich meridian

Inertial reference

meridian

iet

ix

ex

eziz

ie

Local meridian

iy

gx

gy

wxwy

N

0( 90 )ey E

S

00

00

Equatorial plane

gzwz

c

Ob

zb

xb

yb

Muhammad Ushaq 18


The Earth fixed frame (e-frame, e e ex y z ): It is the earth fixed coordinate frame

used for position location definition. Its ez axis is coincident with the Earth’s polar

axis while the other two axes are fixed to the Earth within the equatorial plane.

The geographical frame (g-frame, g g gx y z ): It is a local geographic coordinate

frame; gz axis is parallel to the upward vertical at the local earth surface

referenced position location. gx -axis points towards east and gy points towards

north.

The inertial frame (i-frame, i i ix y z ): It is the non-rotating inertial coordinate

frame. Angular measurements are taken in this frame. Its iz points along Earth’s

polar axis, ix and iy complete the right-hand orthogonal axes set.

Muhammad Ushaq 19


The body frame (b-frame, b b bx y z ): It is the strapdown inertial sensor

coordinate frame with bx , by and bz axes pointing along vehicle’s pitch, roll and

yaw axes respectively.

The wander azimuth frame (w-frame, w w wx y z ): This frame is used to avoid

the singularities in the computations that occur at the poles of the navigation

frame. Like geographic frame, it is locally level but is rotated through the wander

angle about the local vertical.

Muhammad Ushaq 20

Position Matrix

The transformation matrix

defining the position of navigation

frame with respect to earth frame

will be obtained from following

sequence of rotations.

/

e g

90 90( )

Z axis X axis Z (U)

o o

e e e g g g w w wX Y Z X Y Z ENU X Y Zabout about about

According to this sequence of rotation weC will be formed as follows

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

( ) ( ) 0 0 (90 ) (90 ) ( 90) ( 90) 0

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

we

Cos Sin Cos Sin

Sin Cos Cos Sin Sin Cos

Sin Cos

C

090

wx' ''( )e e gy y x

ey''

ex

'

ex

ex

'' ( )e gy ywy'( )e ez z'' ( )e g wz z z

o

Muhammad Ushaq 21

Position Matrix

0 0

0

0 0 1

w

e

Cos Sin Sin Cos

C Sin Cos Sin Cos Sin Sin Cos

Cos Cos Cos Sin Sin

we

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

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

Cos Cos Cos Sin Sin

C

Muhammad Ushaq 22

Updating Position Matrix weC

Let us denote positional Matrix at time t as ( )w

eC t and that at time ( ) t t as

( ) w

eC t t . Let navigation (wander azimuth) frame at time t is denoted by

( )w w wX Y Z t and that at time ( ) t t is denoted by ( ) w w wX Y Z t t . Let during

this small span of time following angular displacements take place in navigation

frame.

' ''

w w w

( ) ( ) X Y Z

yx z

w w w w w wX Y Z t X Y Z t tabout about about

The matrix will undergo through following transformations:

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

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

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

z z y y

w w

e z z x x e

y y x x

Cos Sin Cos Sin

C t t Sin Cos Cos Sin C t

Sin Cos Sin Cos

Muhammad Ushaq 23


Considering x , y z as very small angles, 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 y

w w

e z x e

y x

C t t C t

1 0 0 0

( ) 0 1 0 0 ( )

0 0 1 0

z y

w w

e z x e

y x

C t t C t

1

( ) 1 ( )

1

z y

w w

e z x e

y x

C t t C t

Muhammad Ushaq 24


0

( ) ( ) 0 ( )

0

z y

w w w

e e z x e

y x

C t t C t C t

0

( ) ( ) 0 ( )

0

z y

w w w

e e z x e

y x

C t t C t C t

Muhammad Ushaq 25


0 0

0 0 0

0 0

0 lim lim

( ) ( )( ) lim lim 0 lim ( )

lim lim 0

yzt t

w ww we e xze t t t e

y xt t

t t

C t t C tC t C t

t t t

t t

0 0

0 0

0 0

0 lim lim

( ) lim 0 lim ( )

lim lim 0

yzt t

w wxze t t e

y xt t

t t

C t C tt t

t t

Muhammad Ushaq 26


Where

0 0 0lim ,lim and lim

yx zt t t

t t t are the components of

angular rate of Wander Azimuth with respect to earth fixed frame during the time

from (t) to (t+Δt).so we can write:

!

( )! !

0

0 ( )

0

w

e

w wewz ewy

w w wewz ewx e

w wewy ewx

nC t

r n rC t

wxewx

t

y wewy

t

wzewz

t

Muhammad Ushaq 27


( )

0

0 ( )

0

w

e

w wewz ewy

w w wewz ewx e

w wewy ewx

C t C t

( ) ( ) w

e

w wew eC t C t

0

0

0

w wewz ewy

w w wew ewz ewx

w wewy ewx

Muhammad Ushaq 28


In wander azimuth scheme as 0 Z

wew therefore above equation reduces to

( )

0 0

0 0 ( )

0

w

e

wew y

w wewx e

w wew y ewx

C t C t

Position Matrix is updated by solving this Equation

Muhammad Ushaq 29

Position Update from weC

we

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

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

Cos Cos Cos Sin Sin

C

From the updated weC position (latitude, longitude, and wander angle) are

calculated by following relations

(3,3)1

2 2

(3,1) (3,2)

( )( ) ( )

w

e

mw w

e e

Ctan

C C

(3,2)1

(3,1)

( )

w

e

m w

e

Ctan

C

(1,3)1

(2,3)

( )

w

e

m w

e

Ctan

C

Muhammad Ushaq 30


Note:

Range of is 0o to 180o

Range of is 0o to 180o

Range of is -90o to +90o

1Sin

1Cos

1Tan

Muhammad Ushaq 31


Calculated

Values Range

Adjusted

Values Range

m 90 90 o o 90 90 o o

m 90 90 o o 180 180 o o

m 90 90 o o 0 360o o

Muhammad Ushaq 32


Adjustment in Longitude

Sign of (3,1)

w

eC Sign of m Calculated Range

of Adjusted value of

+ + 0 90o o m

90 180o o 180 o

m

+ 180 90 o o 180 o

m

+ 90 0 o o m

Muhammad Ushaq 33


Adjustment in Wander Angle

Sign of

(2,3)

w

eC Sign of m

Calculated Range

of Practical value of

+ + 0 90o o m

90 180o o 180 o

m

+ 180 270o o 180 o

m

+ 270 360o o 360 o

m

Muhammad Ushaq 34

Computation of Transport Rate wew

We know that wander azimuth frame is obtained by an angular rotation of

geographic frame about vertical axis through an angle . This rotation is governed

by following transformation matrix

0

0

0 0 1

w

g

Cos Sin

C Sin Cos

w gewx eg x

w gew y eg y

Cos Sin

Sin Cos

g

eg y

g

Meg x

g geg y eg x

N

V

R h

V

R h

Muhammad Ushaq 35


10

10

g g

Meg x eg x

g g

eg y eg y

N

R h V

V

R h

g

eg y

g

Meg x

g geg y eg x

N

V

R h

V

R h

10

10

w g

Mewx eg x

w g

ew y eg y

N

R h VCos Sin

VSin Cos

R h

Muhammad Ushaq 36


g w

eg x ewx

g w

eg y ew y

V VCos Sin

V VSin Cos

We have already evaluated

10

10

w w

Mewx ewx

w w

ew y ew y

N

R h VCos Sin Cos Sin

VSin Cos Sin Cos

R h

w w

N Mewx ewx

w w

ew y ew y

N M

Sin Cos

R h R h VCos Sin

VCos Sin Sin Cos

R h R h

Muhammad Ushaq 37


2 2

2 2

1 1( )

1 1( )

w w

M N M Newx ewx

w w

ew y ew y

M N M N

Cos SinSin Cos

R h R h R h R h V

VSin CosSin Cos

R h R h R h R h

Note 0 w

ewz

1 1

1 1

w wywewx ewx

w w

ew y ew y

xw

R V

V

R

Muhammad Ushaq 38


While1

,

1

xwR and

1

ywR are given by following equations

2 21

xw M N

Sin Cos

R R R

2 21

yw M N

Cos Sin

R R R

1 1 1( )

M N

Sin CosR R

Muhammad Ushaq 39

Computation of Spatial Rate wiw

w w w

iw ie ew

0g

ie ie

ie

Cos

Sin

0 ( ) ( ) 0 0

( ) ( ) 0

0 0 1

w w g w

ie g ie g ie ie

ie ie

Cos Sin

C C Cos Sin Cos Cos

Sin Sin

Muhammad Ushaq 40

Computation of RN and RM and Gravity Vector

21 ( )( ) N eR R eSin

21 2 3 ( )( ) M eR R e eSin

2 -69.7803267 0.051799 0.94114 10 g Sin h

wg 0 0 T

g

Muhammad Ushaq 41

Velocity Update

( ) ( )

t t

w w w

ew ew ew

t

V t t V t V dt

( 2 ) w w w w w w

ew ib ew ie ewV f V g

Here

w w b

ib b ibf C f

b

ibf is the output of Accelerometer triad in body frame

and 1

T

w b b

b w wC C C

Muhammad Ushaq 42

Velocity Update

0 (2 ) (2 ) 0

(2 ) 0 (2 ) 0

(2 ) (2 ) 0

w w w w w w w

ewx ib x ie z ewz ie y ew y ewx

w w w w w w w

ewy ib y ie z ewz ie x ewx ew y

w w w w w w w

ewz ib z ie y ew y ie x ewx ewz

V f V

V f V

V f V g

Note 0 w

ewz

0 2 (2 ) 0

2 0 (2 ) 0

(2 ) (2 ) 0

w w w w w w

ewx ib x ie z ie y ew y ewx

w w w w w w

ewy ib y ie z ie x ewx ew y

w w w w w w w

ewz ib z ie y ew y ie x ewx ewz

V f V

V f V

V f V g

Muhammad Ushaq 43

Velocity Update

( 1) ( ) w w

ewx ewx

w

ewxV V Vk k t

( 1) ( ) w w

ew y ew y

w

ewyV V Vk k t

( 1) ( ) w w

ewz ewz

w

ewzV V Vk k t

Muhammad Ushaq 44

Altitude Update

( ) ( )

z

t t

w

ew

t

h t t h t V dt

z

w

ewh V

Muhammad Ushaq 45

Body rate with respect to Wander Azimuth frame bwb

b b

ib iw

bwb

b b

ib iw

bwb

b w

ib iw

b bwwb C

b

ib is the output of gyroscopes and w

iw is spatial rate computed earlier

Muhammad Ushaq 46

Transformation from Wander Azimuth to Body Frame

Body frame (b) is defined as:

X: right wing

Y: longitudinal (forward)

Z: Vertical (Up)

w Z axis axis axis

G

w w w w w w w w w b b b

w w

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

G : heading

: pitch

: roll G

G

gxwx

bx

gyby wy

G

Muhammad Ushaq 47

Transformation from Wander Azimuth to Body Frame

cos cos sin sin sin cos sin sin sin cos sin cos

cos sin cos cos sin

sin cos cos sin sin sin sin cos sin cos cos cos

G G G G

G G

G G G G

b

wC

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

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

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

G G

G G

bw

Cos Sin Cos Sin

Cos Sin Sin Cos

Sin Cos Sin Cos

C

1( ) ( ) w b b Tw wbC C C

Where and are the pitch and roll angles respectively, and G = heading angle

between the wander and body frame = , where is the heading angle between

geographic and body frame and is the wander angle.

Muhammad Ushaq 48

Quaternions, Review

0 1 2 3 Q q q i q j q k

2 2 2 ii i jj j kk k

ij ji k

jk kj i

ki ik j

Conjugate of quaternion

*

0 1 2 3 Q q q i q j q k

Norm or length of a quaternion

* 2 2 2 2

0 1 2 30 1 2 3 0 1 2 3 N Q QQ q q i q j q k q q i q j q k q q q q

Muhammad Ushaq 49

Quaternions, Review

Inverse of a Quaternion

*

1 , 0

QQ N Q

N Q

Unit Quaternion

* 2 2 2 2

0 1 2 3 1 N Q QQ q q q q

If norm is equal to 1 we have

1 * if N 1 Q QQ

A unit quaternion is a quaternion of norm one. Dividing a non-zero quaternion q

by its norm produces a unit quaternion

Muhammad Ushaq 50

Quaternions, Review

Vector Transformation Using Quaternion

Let a vector gr is transformed into the body frame as

br :

*

g bR Q R Q

Any vector x y zr r i r j r k can be expressed as quaternion form with a

zero scaler term as 0 x y zR r i r j r k . Similarly the vector in body

frame can be expressed as quaternion as:

0 g g g g

x y zR r i r j r k

0 b b b b

x y zR r i r j r k

Muhammad Ushaq 51

Quaternions, Review

Conversely any quaternion can be expressed as the sum of a scalar and a vector

0 Q q q

Aforementioned in view the equation for transformation *

n bR Q R Q

can be written as:

*

b nR Q R Q

0 1 2 3 0 1 2 30 b g g g

x y zR r i r j r kq q i q j q k q q i q j q k

2 2 2 2

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

2 2 2 2

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

2 2 2 2

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

2( ) 2( )

2( ) 2( )

2( ) 2( )

b g g g

x y z

g g g

x y z

g g g

x y z

R i r q q q q r q q q q r q q q q

j r q q q q r q q q q r q q q q

k r q q q q r q q q q r q q q q

Muhammad Ushaq 52

Quaternions, Review

2 2 2 2

0 1 2 3

2 2 2 2

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

2 2 2 2

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

2 2 2 2

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

0 00 0 0

0 2( ) 2( )

0 2( ) 2( )

0 2( ) 2( )

b g

x x

b g

y y

b g

z z

q q q q

r rq q q q q q q q q q q q



2 2 2 2

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

2 2 2 2

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

2 2 2 2

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

2( ) 2( )

2( ) 2( )

2( ) 2( )

b g

x x

b g

y y

b g

z z

r q q q q q q q q q q q q r



b b n

nr C r

Muhammad Ushaq 53

Quaternions, Review

2 2 2 2

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

2 2 2 2

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

2 2 2 2

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

2( ) 2( )

2( ) 2( )

2( ) 2( )

n b

n b

n b

x q q q q q q q q q q q q x

y q q q q q q q q q q q q y

z q q q q q q q q q q q q z

2 2 2 2

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

2 2 2 2

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

2 2 2 2

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

2( ) 2( )

2( ) 2( )

2( ) 2( )

b

n

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

C q q q q q q q q q q q q


2 2 2 2

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

2 2 2 2

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

2 2 2 2

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

2( ) 2( )

2( ) 2( )

2( ) 2( )

n

b


C q q q q q q q q q q q q


Muhammad Ushaq 54

Quaternions, Review

Quaternion is also expressed as:

0

1

2

3

cos( / 2)

( / )sin( / 2)

( / )sin( / 2)

( / )sin( / 2)

x

y

z

q

qq

q

q

Whereas , , x y z are the components of the vector (rotation vector) in x,y

and z directions； is the magnitude of the rotation vector .

x i ,

yj and

z k are unit vectors along x , y and z directions.

Muhammad Ushaq 55

Quaternions, Review

Initial Quaternion from W frame to b frame

w Z axis X axis Y axisG

w w w b b bX Y Z X Y Zabout about about

Following the above-mentioned sequence of rotations we get initial

quaternion as:

2 2 2 2 2 2(0) (cos sin ) (cos sin ) (cos sin ). .G GQ k i j

Muhammad Ushaq 56

Quaternions, Review

Initial Quaternion from W frame to b frame

0 1 2 3

2 2 2 2 2 2

2 2 2 2 2 2

2 2 2 2 2 2

2 2 2 2 2 2

+

cos cos cos sin sin sin

(cos sin cos sin cos sin )

(cos cos sin sin sin cos )

(cos sin sin sin cos cos )

G G

G G

G G

G Gk

Q

i

q q i q j q k

j

G

G

gxwx

bx

gyby wy

G

bz

wy

bxwx

Gwy

wz( )w wz z

( )w wx x

( )b wy y

Muhammad Ushaq 57

Updating Attitude Matrix Using Quaternion

0

1

2

3

cos( / 2) cos( / 2)

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

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

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

x

y

z

q

q iq

q j

q k

w Z axis X axis Y axisG

w w w b b bX Y Z X Y Zabout about about

Following the above-mentioned sequence of rotations we get initial

quaternion as:

2 2 2 2 2 2(cos sin ) (cos sin ) (cos sin ). .G GQ k i j

Muhammad Ushaq 58


2 2 2 2 2 2

2 2 2 2 2 2

2 2 2 2 2 2

2 2 2 2 2 2

cos cos cos sin sin sin

(cos sin cos sin cos sin )

(cos cos sin sin sin cos )

(cos sin sin sin cos cos )

G G

G G

G G

G Gk

Q

i

j

2 2 2 2 2 2(cos sin ) (cos sin ) (cos sin ). .G GQ k i j

Rotation about Z-axis = Yaw = cos𝜓𝐺

2+ 𝑘 sin

𝜓𝐺

2

Rotation about X-axis = Pitch = cos𝜃

2+ 𝑖 sin

𝜃

2

Rotation about Y-axis = Roll = cos𝛾

2+ 𝑗 sin

𝛾

2

Muhammad Ushaq 59


Quaternion can also be updated as follows:

b b

ib iw t

b b

x ibx iwx

b b

y iby iwy

b b

z ibz iwz

t

Or

22 2

x y zN

Muhammad Ushaq 60


0

0

0

0

x y z

x z y

y z x

z y x

M

0 0

1 1

2 2

3 3

01 0 0 0Sin

020 1 0 0Cos

00 0 1 02

00 0 0 1

q t qN

q t qNM

q t qN

q t q

Muhammad Ushaq 61


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


C T q q q q q q q q q q q q


0 0

1 1

2 2

3 3

0

01

02

0

b b b

wbx wby wbz

b b b

wbx wbz wby

b b b

wby wbz wbx

b b b

wbz wby wbx

q q

q q

q q

q q

Muhammad Ushaq 62

Attitude Computation

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


C T q q q q q q q q q q q q


cos cos sin sin sin cos sin sin sin cos sin cos

cos sin cos cos sin

sin cos cos sin sin sin sin cos sin cos cos cos

G G G G

G G

G G G G

b

wC

1 21

22

( )m

Ctan

C

1 13

33

( )

m

Ctan

C

1

23( ) m Sin C

Muhammad Ushaq 63

Attitude Computation

Heading

If 22C >0 and m >0 then G m

Else if 22C >0 and m <0 then 2 G m

Else if 22C <0 then G m

G

Roll

If 33C >0 then m

Else if 33C <0 and m>0 then m+π

Else if 33C <0 and m<0 then m

Pitch

m

Heading Range

Range from 0 to 360

0=North

90=East

180=South

270=West.

Pitch Range

Range from -90 to +90

0 = Horizon

+90 = straight up

–90 = straight down.

Roll Range

Range From-180 to +180

0 = Horizon

+90 = Full roll right

–90 = Full roll left.

Muhammad Ushaq 64

Block Diagram of SINS in Wander Azimuth Scheme

b bib wbSample: ,then calculate:

Calculate : , thenCalculate wbq C

Calculate : , ,hV V

Calculate : ,w wew eC

Calculate : , , , , , , , , (0) ;

(0) , (0) , (0) , (0)

wG ie

w w b bew ew wb wb

and q q

V V h h

Continueyes

No

Stop

0 0 0 0 0Put in: , , , , V h

bInitialAlignment: C (0)w

Set : , , , ,ie o ef g R e

Samples : (0), (0)b bib f

Calculate : , , ( , )M NR R g h

Calculate : (0), (0), (0), (0)w w we ie ewC q

bS a m p le f

Muhammad Ushaq 65

Documents

Mechanization of Inertial Navigation in Wander Azimuth Navigation Frame