mechanization of inertial navigation systems in local level north pointing navigation frame
TRANSCRIPT
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
Transformation from ECI Frame to ENU Frame
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
Transformation from ECI Frame to ENU Frame
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
Sin CosC Sin Cos Sin Sin Cos
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
Sin CosC Sin Cos Sin Sin Cos
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
Position update from Updated Velocity Components
ω 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
Position update from Updated Velocity Components
( ) ( )φ φ φ+∆
+ ∆ = + ∫ 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
Updating Attitude DCM bgC
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
Updating Attitude DCM bgC
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
Updating Attitude DCM bgC
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
Updating Attitude DCM bgC
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
Updating Attitude DCM bgC
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
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
Updating Attitude DCM Using Quaternion Updating
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
Updating Attitude DCM Using Quaternion Updating
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
Updating Attitude DCM Using Quaternion Updating
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