Inertial Navigation Systems
Muhammad Ushaq
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
Wander Azimuth Frame w w wx y z
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
Wander Azimuth Frame w w wx y z
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
Wander Azimuth Frame w w wx y z
gp g x
ipz igz
M
ie ie
VSin Sin Tan
R h
g
x
M
VTan Sin
R h
Muhammad Ushaq 9
Wander Azimuth Frame w w wx y z
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
Wander Azimuth Frame w w wx y z
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
Coordinate Frames in Wander Azimuth Navigation
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
Coordinate Frames in Wander Azimuth Navigation
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
Updating Position Matrix weC
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
Updating Position Matrix weC
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
Updating Position Matrix weC
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
Updating Position Matrix weC
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
Updating Position Matrix weC
( )
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
Updating Position Matrix weC
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
Position Update from weC
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
Position Update from weC
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
Position Update from weC
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
Position Update from weC
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
Computation of Transport Rate wew
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
Computation of Transport Rate wew
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
Computation of Transport Rate wew
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
Computation of Transport Rate wew
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
r rq q q q q q q q 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
r q q q q q q q q q q q q r
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
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
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
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
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
Updating Attitude Matrix Using Quaternion
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
Updating Attitude Matrix Using Quaternion
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
Updating Attitude Matrix Using Quaternion
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
Updating Attitude Matrix Using Quaternion
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 q
C 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
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
q q q q q q q q q q q q
C 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
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