Strapdown Inertial Navigation Integration Algorithm Design Part 1: Attitude Algorithms

JOURNAL OF GUIDANCE, CONTROL, AND DYNAMICS
Vol. 21, No. 1, January– February 1998
Strapdown Inertial Navigation Integration

Algorithm Design Part 1: Attitude Algorithms
Paul G. Savage¤
Strapdown Associates, Inc., Maple Plain, Minnesota 55359
This series of two papers provides a rigorous comprehensive approach to the design of the principal software
algorithms utilized in modern-day strapdown inertial navigation systems: integration of angular rate into attitude,
acceleration transformation/integration into velocity, and integration of velocity into position. The algorithms
are structured utilizing the two-speed updating approach originally developed for attitude updating in which
an analytically exact equation is used at moderate speed to update the integration parameter (attitude, velocity,
or position) with input provided from a high-speed algorithm measuring dynamic angular rate/acceleration ef-
fects within the parameter update time interval [coning for attitude updating, sculling for velocity updating, and
Downloaded by SHANGHAI JIAO TONG UNIVERSITY on April 30, 2018 | http://arc.aiaa.org | DOI: 10.2514/2.4228
scrolling (writer’s terminology) for high-resolution position updating]. The algorithm design approach accounts
for angular rate/speci c force acceleration measurements from the strapdown system inertial sensors as well as
rotation of the navigation frame used for attitude referencing and velocity integration. This paper, Part 1, de nes
the overall design requirement for the strapdown inertial navigation integration function and develops direction
cosine and quaternion forms for the attitude updating algorithms. Part 2 [Savage, P. G., “Strapdown Inertial
Navigation Integration Algorithm Design Part 2: Velocity and Position Algorithms,” Journal of Guidance, Control,
and Dynamics (to be published)] deals with design of the velocity and position integration algorithms. Although
Parts 1 and 2 often cover fundamental inertial navigation concepts, the material presented is intended for use by
the practitioner who is already familiar with basic inertial navigation concepts.
Nomenclature ! A 1 A2 = angular rate of coordinate frame A 2 relative to

A; A1 ; A 2 ; A3 = arbitrary coordinate frames coordinate frame A 1 ; when A1 is the inertial I
C AA21 = direction cosine matrix that transforms a vector frame, ! A 1 A 2 is the angular rate measured by
from its A 2 frame projection form to its angular rate sensors mounted on frame A 2
A 1 frame projection form
I = identity matrix
q AA21 = attitude quaternion that transforms a quaternion
I. Introduction
A
vector from its A 2 frame component form to its
A 1 frame component form
A
I NERTIAL navigation is the process of calculating position by
integration of velocity and computing velocity by integration
of total acceleration. Total acceleration is calculated as the sum of
q A 21¤ = attitude quaternion q A 21 conjugate having the
same rst element as q AA21 but with the negative gravitationalacceleration,plus the accelerationproducedby applied
of elements 2 – 4 in q AA21 nongravitational forces (known as speci c force acceleration). An
q1 = identity quaternion having 1 for the rst inertial navigation system (INS) consists of a navigation computer
element and zero for the remaining three for the integration function, a precision clock for timing the inte-
V = vector without speci c coordinate frame gration operations, an accelerometer assembly for measuring the
designation speci c force acceleration, gravitation model software resident in
VA = column matrix with elements equal to the the navigation computer for calculating gravitational acceleration
projection of V on frame A axes as a function of calculated position, and an attitude reference for
.V A £/ = skew symmetric (or cross product) form of V A , de ning the angular orientation of the accelerometer triad as part
represented by the square matrix of the velocity calculation. In a modern day INS, the attitude refer-
ence is provided by a software integration function residing in the
2 3 INS computer using inputs from a three-axis set of inertial angu-
0 ¡VZ A VY A lar rate sensors. The angular rate sensor and accelerometer triads
4 VZ A 0 ¡V X A 5 are mounted to a common rigid structure within the INS chassis to
¡VY A VX A 0 maintain precision alignment between each inertial sensor. Such an
arrangement has been denoted as a strapdown INS because of the
rigid attachment of the inertial sensors within the chassis, hence, to
where VX A , VY A , V Z A are the components of
the vehicle in which the INS is mounted.
V A ; matrix product of .V A £/ with another
The primary functions executed in the INS computer are the an-
A-frame vector equals the cross product of V A
gular rate into attitude integrationfunction (denoted as attitude inte-
with the vector in the A frame
gration), use of the attitude data to transform measured acceleration
VqA = quaternion four vector equivalent to V A ,
into a suitable navigation coordinate frame where it is integrated
µ ¶ into velocity (denoted as velocity integration), and integration of
0 the navigation frame velocity into position (denoted as position in-
VA tegration). Thus, three integration functions are involved, attitude,
velocity,and position,each of which requireshigh accuracyto assure
negligible error compared to inertial sensor accuracy requirements.
Received July 7, 1997; revision received Sept. 11, 1997; accepted for
From a historical perspective, since the basic strapdown inertial
publication Sept. 15, 1997. Copyright °c 1997 by Strapdown Associates, navigation concept was originally formulated in the 1950s, strap-
Inc. Published by the American Institute of Aeronautics and Astronautics, down analystshave primarily focusedon the designof algorithmsfor
Inc., with permission. the attitude integration function. Invariably, the design approaches
¤ President. Member AIAA. were driven by the capabilities and limitations of contemporary
19
20 SAVAGE
ight computer technology. In the late 1950s and in the 1960s, two equations (assuming that Taylor series expansion for trigonometric
approaches were pursued by strapdown analysts (in various organi- coef cients is carried out to comparable accuracy order).
zations) for the attitude integration function1¡5 : high-speed attitude Most modern-day strapdown INSs for aircraft utilize attitude up-
updating, e.g., 10– 20 kHz, using rst-order digital algorithms, and dating algorithms based on a two-speed approach. The repetition
lower-speed attitude updating, e.g., 50– 100 Hz, using higher-order rate for the moderate-speed algorithm portion, e.g., 50– 200 Hz, is
algorithms. The high-speed approach was promoted as a means for typically designed, based on maximum angular rate considerations,
accurately accounting for high-frequency angular rate components to minimize power series truncation error effects in the moderate-
that can rectify into systematic three-dimensional attitude change; and high-speed algorithms. The repetition rate for the high-speed
however, computer technologyof that time period was only capable algorithm, e.g., 1– 4 kHz for an aircraft INS with 1 n mph 50 per-
of executing simpli ed rst-order equations of limited accuracy for centile radial position error rate, is designed, based on the antici-
the attitude updating algorithms. In contrast, the higher-order al- pated strapdown inertial sensor assembly vibration environment, to
gorithm proponents touted improved analytical accuracy compared accurately account for vibration-induced coning effects. Continu-
to rst-order algorithms; however, the improved accuracy was de- ing two-speed attitude algorithm development work has centered
graded due to the associated increase in executable operations per on variations for the high-speed integration function. Originally
attitude update cycle and, hence, a slower attitude update rate to conceived as a simple rst-order algorithm,6 today’s high-speed
satisfy contemporary computer throughput limitations. Tradeoffs attitude algorithms have taken advantage of increased throughput
between the two approaches were clouded by the emergence of the capabilities in modern-day computers and become higher order for
attitude quaternion as the preferred approach for the analytical form improved accuracy (Refs. 9 – 11 and 12, Sec. 7.1). While the atti-
of the computed attitude parameter (vs the traditional direction co- tude updating function has been evolving to its current form, very
sine matrix attitude representation). For the algorithms investigated little parallel work has been published on the development of the
during that time period, the quaternion showed improved accuracy companion strapdown INS algorithms for acceleration transforma-
in high-frequency angular rate environments. tion/velocity integration and position integration (the subject of the
In 1966, the writer proposed a new two-speed approach for the Ref. 13, Part 2, paper).
attitude integration function6 whereby the attitude updating oper- This paper, Part 1, de nes the overall design requirement for
ation is divided into two parts: a simple high-speed, rst-order the strapdown inertial navigation integration function and describes
algorithm portion coupled with a more complex moderate-speed, a comprehensive design process for developing the attitude inte-
higher-order algorithm portion whose input was provided by the gration algorithms based on the two-speed approach. The material
high-speedalgorithm. The simpli ed high-speedportion accounted presentedis a condensedversion of Ref. 12, Sec. 7.1 (which is an ex-
for high-frequency angular oscillations within the attitude update pansion of material in Ref. 9), emphasizing a more rigorous analyt-
cycle that can rectify into systematic attitude buildup (traditionally ical formulation and the use of exact closed-form equations, where
denoted as coning). Taken together, the combined accuracy of the possible, for ease in computer software documentation/validation
two-speed approach was equivalent to operating the higher-order (which is also consistent with modern-day ight computer technol-
algorithm at the high-speed rate (for improved accuracy); however, ogy). Included in the attitude algorithm design process is a rigorous
due to the simplicity of the high-speed algorithm, the combined treatmentof methodsfor accountingfor navigationcoordinateframe
computer throughput requirement was no greater than for original rotation during the attitude update time periods.
high-speed, rst-order attitude updating algorithms. The utility of The paper is organizedas follows. Section II providesbackground
the Ref. 6 two-speed algorithm design approach was limited by its material regarding coordinate frames and attitude parameters used.
basic analyticalformulationas a Picard-typerecursiveintegrationof Section III provides a complete set of typical strapdown inertial
the continuousform attitude rate differentialequation in which both navigation attitude, velocity, and position equations in continuous
the moderate- and high-speed algorithms were generated simulta- differential equation format, which serves as a framework for the
neously. The complexity of the analytical recursive integration de- equivalent algorithm design process. Section IV develops the two-
sign process limited expansion of the higher-order, moderate-speed speed attitude integration algorithm (for both direction cosine and
algorithm (to only second order in Ref. 6, which was considered quaternionformulationsincludingnavigationframe rotation effects)
acceptable at that time). in a generic form for the high-speed portion and describes a par-
In an unrelated design activity, Jordan7 in 1969 suggested a two- ticular form to illustrate the design of one of the classical high-
speed approach for the strapdown attitude updating function in speed, second-order coning computation algorithms. A tabular ref-
which the analyticalformulationat the onset was based on two sepa- erence summary of the attitude integration algorithms is presented
rately de ned calculations:a moderate-speed,classicalclosed-form, in Sec. V. Section VI provides a general discussion of the process
(exact) higher-orderattitude updating algorithm based on input atti- followed in selecting algorithms for a particular application and es-
tude change, and a simpli ed high-speed, second-order integration tablishingtheir execution rates. Concluding remarks are providedin
algorithm that measured the attitude change input for the moderate- Sec. VII.
speed algorithm. In 1971, Bortz8 extended the Jordan concept to Finally, it is important to recognize that although the original
have the high-speed calculation based on a differential equation intent of the two-speed approach was to overcome throughput lim-
that, when integrated, measures the exact attitude change input to itations of early computer technology (1965– 1975), that limitation
the exact attitude updating algorithm. The exact moderate-speedat- is rapidly becoming insigni cant with continuing rapid advances in
titude algorithm can be structured to any speci ed order of accuracy modern high-speed computers. This provides the motivation to re-
by truncation of two trigonometric coef cients. In practice, simpli- turn to a simpler single-speedalgorithm structure whereby all com-
ed forms of the Bortz attitude change differentialequation are used putations are executed at a repetition rate that is suf ciently high
for the high-speed function. References 7 and 8 thereby provided a to accurately account for multiaxis high-frequencyangular rate and
more general form of the two-speed attitude updating approach in accelerationrecti cation effects. The two-speed structure presented
which the moderate-speed, higher-order algorithm and high-speed, in both Parts 1 and 2 is compatible with compression into such a
simpli ed algorithm can be independently tailored to meet particu- single-speed format as explained in the particular sections where
lar applicationrequirements.(Interestingly,Ref. 8 proposed an ana- the algorithms are formulated.
log implementation for a simpli ed version of the high-speed algo-
rithm.) A secondarybene t derived from the Ref. 7 and 8 two-speed
approach (proposed using direction cosines for the exact moderate- II. Coordinate Frames and Attitude
speed attitude update operation) is that the moderate-speed por- Orientation Relationships
tion can also be formulated with an analytically exact, closed-form This section de nes the coordinate frames used in this paper and
quaternion updating algorithm using the identical high-speed input generically describes the properties of the direction cosine matrix,
applied for direction cosine updating. Thus, the new two-speed ap- the attitude quaternion, and the rotation vector, attitude parameters
proach has equal accuracy for either direction cosine or quaternion utilizedto representthe angularrelationshipbetween two coordinate
updating, both of which derive from analyticallyexact, closed-form frames.
SAVAGE 21
A. Coordinate Frame De nitions 12, Sec. 3.2.2.1) that the relationship between the rotation vector
A coordinate frame is an analytical abstraction de ned by three and the direction cosine matrix is given by
consecutively numbered (or lettered) unit vectors that are mutually µ ¶
perpendicular to one another in the right-hand sense. It can be vi- sin Á .1 ¡ cos Á/
C AA21 D I C .Á£/ sin Á C .Á£/2 (4)
sualized as a set of three perpendicularlines (axes) passing through Á Á2
a common point (origin) with the unit vectors emanating from the
origin along the axes. In this paper, the physical locations of the where Á and Á are the rotation vector and its magnitude. A unique
coordinate frame origins are arbitrary. A vector’s components (or property of the rotation vector is that it has identical components in
projections) in a particularcoordinateframe equal the dot productof the A 1 and A2 frames (Ref. 12, Sec. 3.2.2.1); hence, Á in Eq. (4)
the vector with the coordinate frame unit vectors. The vectors used represents either Á A 1 or Á A2 .
in this paper are classi ed as free vectors and, hence, have no pre- The attitude quaternion is a four vector, i.e., four components,
ferred location in coordinate frames in which they are analytically de ned as a function of the rotation vector (Refs. 4 and 9; 12, Sec.
described. 3.2.4; and 14, pp. 73– 76)
The coordinate frames are de ned as follows. 2 3
cos 0:5 Á
1) The E frame is the Earth xed coordinate frame used for
position location de nition. It is typically de ned with one axis q AA21 D 4 sin 0:5 Á 5 (5)
0:5 Á
parallel to the Earth polar axis with the other axes xed to the Earth 0:5 Á
and parallel to the equatorial plane.
2) The N frame is the navigation coordinate frame having its From Eq. (5), it is easilyveri ed that the sum of the squaresof the q AA21
Z axis parallel to the upward vertical at the local Earth surface elements is unity. The coordinate frame transformation equations
A
referenced position location. It is used for integrating acceleration associated with q A 21 are in quaternion algebra (Refs. 4, 9, and 12,
into velocity and for de ning the angular orientation of the local Sec. 3.2.4.1)
vertical in the E frame. A A A
3) The L frame is the locally level coordinateframe parallel to the VqA1 D q AA21 VqA 2 q A 21¤ ; VqA 2 D q A 21¤ VqA 1 q AA21 D q AA12 VqA 1 q A12¤ (6)
N frame but with Z axis parallel to the downward vertical, and X
and Y axes along N frame Y and X axes. It is used as the reference Equations (6) can be used to derive the attitude quaternion chain
for describing the strapdown sensor coordinate frame orientation. rule,
4) The B frame is the strapdown inertial sensor coordinate frame q AA13 D q AA23 q AA12 (7)
(body frame) with axes parallel to nominal right-handed orthogonal
sensor input axes.
C. Attitude Parameter Rate Equations
5) The I frame is the nonrotating inertial coordinate frame used
The rates of change of the Sec. II.B attitude parameters (Refs. 4,
as a reference for angular rotation measurements. Particular orien-
8, 9, and 12, Sec. 3.3) are given by
tations selected for the I frame are discussed in the sections where
¡ ¢ ¡ ¢
its orientation is pertinent to analytical operations. CP AA21 D C AA21 ! AI A2 2£ ¡ ! IAA1 1£ C AA21 (8)
B. Attitude Parameter De nitions qP AA21 D 12 q AA21 ! IAA2 2q ¡ 12 ! IAA1 1q q AA21 (9)
The direction cosine matrix is de ned as a square matrix whose
1
columns are an orthogonal set of unit vectors, each equal to a unit ÁP D ! AA11 A 2 C Á £ ! AA 11 A 2
vector along a coordinate axis of frame A 2 as projected onto the 2
axes of coordinate frame A 1 : µ ¶
1 Á sin Á ¡ ¢
£ ¤ C 1¡ Á £ Á £ ! AA11 A 2 (10)
C AA21 D u1AA1 2 A
u2A1 2
A
u3 A1 2 (1) Á 2 2.1 ¡ cos Á/
A
where ui A12 is the unit vector along A 2 frame axis i projected on III. Continuous Form Strapdown
coordinate frame A1 axes. Inertial Navigation Equations
From this basic de nition it can be demonstrated that the element The differential equations that de ne the primary operations typ-
in row i , column j of C AA21 equals the cosine of the angle between ically performed in a strapdown inertial navigation system (Refs. 9;
frame A 1 axis i and frame A2 axis j , that the transpose of C AA21 12, Chap. 4; and 15, pp. 77– 103 and 156– 177) are given as follows.
A
equals its inverse, the columns of C A 21 transposeequal frame A 1 axis Attitude rate
A
unit vectors projected on frame A2 axes, and the product of C A 21 ¡ ¢ ¡ ¢
with a vector projected on frame A 2 axes equals the components of CP BL D C BL ! IBB £ ¡ ! LIL £ C BL (11)
A
the vector projected on frame A 1 axes (and the converse for C A 21
transpose): or, alternatively,
A A
¡ ¢T A
qP BL D 12 q BL ! IBBq ¡ 12 ! LIL q q BL (12)
V A 1 D C A 21 V A 2 ; V A 2 D C A 21 V A 1 D C A 12 V A 1 (2)
Local level frame rotation rate
Equations (2) can be used to derive the directioncosine matrix chain ¡ ¢
rule, ! LIL D C NL ! NIE C ! NE N (13)
C AA13 D C AA23 C AA12 (3) ¡ ¢T

! NIE D C NE ! EIE (14)
The rotation vector de nes an axis of rotation and magnitude for ¡ ¢
rotation about the axis. Imagine frame A 1 being rotated from its ! NE N D FC u NZ N £ v N C ½ Z N u NZ N (15)
starting attitude to a new attitude by rotation about the rotation
vector through an angle equal to the rotation vector magnitude. Acceleration transformation
Now call frame A2 the new attitude of frame A 1 . By this de nition L
aSF D C BL aSF
B
(16)
of frame A 2 , an arbitrarily de ned rotation vector uniquely de nes
the attitude of frame A 2 relative to the original frame A 1 attitude. or, alternatively,
Conversely, for a given attitude of frame A2 relative to frame A 1 , a ¤
L
rotation vector can be de ned that is consistent with this attitude. aSFq
D q BL aSF
B
qL
q B
(17)
Thus, a rotation vector can be used to de ne the attitude of frame A 2
N
relative to frame A 1 . Analytically, it can be shown (Refs. 4, 9, and aSF D C LN aSF
L
(18)
22 SAVAGE
Velocity rate that Eq. (22) is exact for a rotating oblate Earth model. If vertical
¡ ¢¡ ¢ channel gravity/divergence control is to be incorporated to prevent
g NP N
Dg ¡ ! NIE £ ! NIE £ R N
(19) exponentially unstable vertical channel error growth, Eqs. (20) and
¡ ¢ (22) would include an additional vertical control term (Refs. 12,
vP N D aSF
N
C g NP ¡ ! NE N C 2! NIE £ v N (20) Sec. 4.4.1; 15, pp. 102– 103; and 18, Sec. 10.3).
Position rate IV. Attitude Update Algorithms
¡ ¢
CP NE D C NE ! NE N £ (21) In this section we develop algorithmic forms for direction cosine
matrix rate equation (11) and attitude quaternion rate equation (12)
hP D u NZ N ¢ v N (22) suitable for integration in a digital computer. The algorithms will be
structured using what is now the traditional two-speed approach6¡8
where in which analytically exact closed-form equations are applied for
the basic attitude update function using inputs from a higher speed
R = position vector from Earth’s center to the INS algorithmdesignedto measure attitudechangeover the basicattitude
v = velocity (rate of change of position) relative to the update cycle.
Earth de ned analytically as the time derivative of
R in the E frame
A. Attitude Direction Cosine Matrix
h = altitude above the Earth de ned as the distance from
The updating algorithm for the C BL direction cosine matrix is
the INS to the Earth surface measured along a line
designed to achieve the same numerical result at the attitude update
from the INS that is perpendicular to a tangent plane
times as would the formal continuousintegrationof the Eq. (11) CP BL

on the Earth’s reference geoid16 surface expression at the same time instant. The algorithm is constructedby
FC = curvature matrix (3 £ 3) that is a function of position envisioning the body B frame and local level L frame orientation
(C NE ; h) with elements 3,i and i ,3 equal to zero and the histories in the digital updating world [produced in Eq. (11) by ! BIB
remaining elements symmetrical about the diagonal. and ! LIL ] as being constructed of successive discrete orientations
For a spherical Earth model, the remaining elements relative to nonrotating inertial space (I ) at each update time instant.
are zero off the diagonal and the reciprocal of the To be completelygeneral, we also allow that C BL updatingoperations
radial distance from Earth center to the INS on the for L frame angular motion may not necessarily occur at the same
diagonal. For an oblate Earth model, the remaining time instant that C BL is updated for B frame motion, e.g., for a
terms represent the local curvature on the Earth multirate digital computation loop structure where C BL is updated
surface projected to the INS altitude (see Ref. 12, at a higher rate for B frame rotation than for L frame rotation. In
Sec. 5.3, for closed-form expression) the interests of minimizing computer throughput requirements, the
uZ N = unit vector upward along the geodetic vertical software architecture might have L frame updates occurring 5– 10
(the Z axis of the N frame) times slower than B frame updates. The nomenclature we adopt to
½Z N = vertical component of ! NE N ; the value selected for describe the coordinate frame orientation history is as follows:
½ Z N depends on the type of N frame utilized
(e.g., wander azimuth or free azimuth designed B I.m/ = discrete orientation of the body B frame in
to assure that ! NE N is nonsingular for all Earth nonrotating inertial space I at computer
locations (Refs. 12, Sec. 4.6, and 15, pp. 88– 99) update time tm
aSF = speci c force acceleration de ned as the acceleration m = computer cycle index for B frame angular
relative to nonrotating inertial space produced by motion updates to C BL
applied nongravitationalforces, measured by L I.n / = discrete orientation of the locally level L frame
accelerometers in nonrotating inertial space (I ) at computer
g = mass attraction gravitational acceleration or update time tn
gravitation (a function of R) n = computer cycle index for L frame
gP = plumb-bob gravity or gravity, which, for a stationary angular motion updates to C BL
INS, lies along the line of a plumb bob
With these de nitions, the general updating algorithm for C BL is
Analytical models for g can be found in Refs. 16; 17, Sec. 4.4; and
constructed as follows using the Eq. (3) direction cosine matrix
18, Sec. 6.3. See Ref. 12, Sec. 5.4.1, for N frame components of
product chain rule:
gP .
In performing the strapdown inertial navigation function, the LI LI BI
C B I .n ¡ 1/ D C B I .n ¡ 1/ C B I .m ¡ 1/ (23)
strapdown INS computer integrates the latter attitude rate, veloc- .m / .m ¡ 1/ .m /
ity rate, and position rate equations using suitable integration algo- LI LI LI
rithms. C B I .n/ D C L I .n / C B I .n ¡ 1/ (24)
The following points are worthy of note regarding the form of .m/ .n ¡ 1/ .m/
the latter navigation equations. Both direction cosine and quater- where
nion attitude forms are shown for the body attitude rate/acceleration
LI
transformation operations. Either can be used in practice with vir- C B I .n ¡ 1/ = C BL relating the B frame at time tm ¡ 1 to
tually identical results. The velocity is de ned relative to the Earth .m ¡ 1/
the L frame at time tn ¡ 1
LI
(E frame) and the velocity rate equation is written in the locally C B I .n/ = C BL relating the B frame at time tm to
level de ned N frame (for integration into velocity). This is typi- .m/
the L frame at time tn
BI
cal for many terrestrial navigation applications, e.g., aircraft INS. C B I .m ¡ 1/ = direction cosine matrix that accounts
Other coordinate frame options are also used for velocity de nition .m/
for B frame rotation relative to inertial
and the velocity rate equation, e.g., for tactical and strategic mis- space from its orientation at time tm ¡ 1
sile guidance.The position rate equations de ne position as altitude to its orientation at time tm
LI
plus the angular orientation of the N frame relative to the E frame C L I .n/ = direction cosine matrix that accounts for
[from which latitude and longitude can be extracted and R calcu- .n ¡ 1/
L frame rotation relative to inertial space
lated (Refs. 12, Secs. 4.5.1 and 4.5.3, and 15, pp. 88, 89)]. Position from its orientation at time tn ¡ 1
can also be de ned for the position rate equation as simply R [from to its orientation at time tn
which C NE and h can be calculated(Ref. 12, Sec. 4.5.4)]. Altitude rate
equation(22) appears trivial,but not necessarilywhen one considers The algorithm described by Eqs. (23) and (24) relates body B frame
a rotating oblate Earth model, a rotating N frame over the Earth, and and local-level L frame orientations at separate times and provides
the stated altitude de nition. Reference 12, Secs. 4.4 and 5.5, shows for B and L frame inertial angular motion updates to C BL at different
SAVAGE 23
update rates. Unlike the B frame (which can be rotating dynami- series expansion, the scalar multiplier of the Á £ .Á £ ! BIB / term
cally at 200– 300 deg/s), the inertial angular rate of the local level in Eq. (27) can be approximated as
L frame is generally small, equal to Earth’s rotation rate plus L ³ ´ ³ ´
frame angular rate relative to the Earth (transport rate, which is 1 Á sin Á 1 1 1
1¡ D 1 C Á2 C ¢ ¢ ¢ ¼ (29)
typically never larger than a few Earth rates). Consequently, the Á2 1 ¡ cos Á 12 60 12
L frame update can generally be performed at a lower rate than
the B frame update with comparable accuracy. Note the update hence, Eq. (27) to second order in Á is given by
rate requirement for B and L frame motion is based, in part, on ¡ ¢
minimizing errors in the approximate high-speed algorithm used to ÁP ¼ ! BIB C 12 Á £ ! BIB C 1
12
Á £ Á £ ! BIB (30)
measure attitude change (see Secs. IV.A.1 and IV.A.2). The B and
BI Through simulation and analysis (analytical expansion under hy-
L frame motion updates to C BL are performed by the C B I .m ¡ 1/ and pothesized analytically de nable angular motion conditions), it can
L I.n/ .m /
CL I terms in Eqs. (23) and (24), algorithms for which are derived be shown that to second-order accuracy in Á
.n ¡ 1/
separately next. 1
¡ ¢ 1
¡ ¢
2
Á £ ! IBB C 12
Á £ Á £ ! BIB ¼ 12 ® £ ! IBB (31)
1. Body Frame Rotation
where
Equation (23) updates the C BL attitude direction cosine matrix Z
BI t
using C B I .m ¡ 1/ to account for angular rate of the strapdown sen- ®.t / D ! BIB d¿ (32)
.m/
sor (body) B frame relative to nonrotating space ! BIB . The formal tm ¡ 1
BI
de nition for C B I .m ¡ 1/ is
.m/ Equation (31) is extremely signi cant because it enables Eq. (27) to
Z be simpli ed to second-order accuracy, i.e., in error to third order
tm
BI B I in Á, by retaining only rst-order terms. Thus, Eq. (27) becomes to
C B I .m ¡ 1/ D I C CP B.t.m/ ¡ 1/ dt (25) second-order accuracy
.m/
tm ¡ 1
ÁP ¼ ! BIB C 12 ® £ ! BIB (33)

where B.t / is the B frame attitude at an arbitrary time in the inter-
val tm ¡ 1 to tm . Substituting Eq. (33), Eq. (28) is given by
BI
The C B I .m ¡ 1/ matrix can also be expressed in terms of a rotation Z
.m/
tm £ ¡ ¢¤
vector de ning the frame B I.m/ attitude relative to frame B I.m ¡ 1/ . Ám D ! BIB C 1
2
®.t / £ ! IBB dt (34)
Applying Eq. (4) using Taylor series expansion for the coef cient tm ¡ 1
terms obtains
Finally, with Eq. (32) we obtain
B I.m ¡ 1/ sin Ám 1 ¡ cos Ám
C BI D IC .Ám £/ C .Ám £/.Ám £/ Ám D ®m C ¯ m (35)
.m/ Ám Ám2
sin Ám Á2 Á4 with
D 1 ¡ m C m ¡ ¢¢¢ (26) Z t
Ám 3! 5!
®.t / D ! IBB d¿; ®m D ®.tm /
.1 ¡ cos Ám / 1 Á2 Á4 tm ¡ 1
D ¡ m C m ¡¢¢¢ (36)
2
Ám 2! 4! 6! Z
1
tm ¡ ¢
¯m D ®.t / £ ! BIB dt
where Ám is the rotation vector de ning the frame B I.m/ attitude 2 tm ¡ 1
relative to frame B I.m ¡ 1/ at time tm . The Ám rotation vector can be
computed by treating Á as a general rotation vectorde ning the gen- where ¯ m is the coning attitude motion from tm ¡ 1 to tm . The ¯ m
eral B frame attitude relative to frame B I.m ¡ 1/ for time greater than term has been coined the coning term because it measures the ef-
tm ¡ 1 . Then Á is calculatedas the integral from time tm ¡ 1 of the gen- fects of coning motion components present in ! BIB . Coning motion
eral ÁP equation,with Á for Eq. (26) evaluatedas the integralsolution is de ned as the condition whereby an angular rate vector is itself
at time tm . Treating frame B I.m ¡ 1/ for Á de nition as the nonrotating rotating.For ! BIB exhibitingpure coningmotion (the ! IBB magnitude
inertial reference frame I , we obtain the following for the general being constant but the vector rotating), a xed axis in the B frame
ÁP expression by application of Eq. (10) with general frame A 2 re- that is approximately perpendicularto the plane of the rotating ! BIB
placed by body frame B and general frame A 1 replaced by inertial vector will generate a conical surface as the angular rate motion
frame I for angular rate description: ensues (hence, the term coning to describe the motion). Under con-
³ ´ ing angular motion conditions, B frame axes perpendicular to ! BIB
1 1 Á sin Á ¡ ¢ appear to oscillate (in contrast with nonconing or spinning angular
ÁP D ! BIB C Á £ ! IBB C 2 1 ¡ Á £ Á £ ! IBB motion in which axes perpendicularto ! BIB rotate around ! BIB ).
2 Á 2.1 ¡ cos Á/
For situations where ! BIB is not rotating, it is easily seen from
(27) Eq. (36) that ®.t / will be parallel to ! BIB ; hence, the cross product
in the ¯ m integrand will be zero and ¯ m will be zero. Under these
where Á is the rotation vector de ning the general attitude of frame conditions, Eq. (34) reduces to the simpli ed form
B relative to frame B I.m ¡ 1/ for time greater than tm ¡ 1 . Equation (27), Z tm
commonly referred to as the Bortz equation,8 relates the change in Ám D ! BIB dt (37)
B frame attitude to the B frame inertial angular rate ! BIB that would tm ¡ 1
be measured by strapdown angular rate sensors.
The attitude rotation vector Ám for Eq. (26) is then obtained as when ! IBB is not rotating. Note that Eq. (37) also applies to the
the integral of Eq. (27) from time tm ¡ 1 , evaluated at time tm exact Ám Eqs. (27) and (28) for a nonrotating ! BIB , i.e., without
Z t
approximation. This is readily veri ed by observing from Eq. (27)
P / dt ; P integration
that Á.t / will initially be aligned with ! BIB as the Á.t/
Á.t / D Á.¿ Ám D Á.tm / (28)
tm ¡ 1
begins and will then remain parallel to ! BIB because its cross prod-
P / expression will remain zero. Under these
ucts with Á.t / in the Á.t
where ¿ is the running integrationtime variable. To reduce the num- conditions, Eqs. (27) and (28) also reduce to Eq. (37).
ber of computationsinvolved in calculating ÁP with Eq. (27), simpli- Integrated angular rate and coning increment algorithms are dis-
fying assumptions are incorporated. For example, through a power cussed next. A discrete digital algorithm form of the ®m integrated
24 SAVAGE
rate and ¯m coning expressions in Eq. (36) can be developed by updateprocessarchitecture:attitude(C BL ) updateand coning(¯l ) up-
considering ¯ m to be the value at t D tm of the general function date (as discussed thus far) and sensor sampling for the coning
¯.t /, where from Eq. (36) update. Re nements on the latter technique10;11 utilize a general an-
Z gular rate pro le that is de ned directly in terms of its impact on
1
t ¡ ¢ the Eq. (42) integral as a sum of weighted cross products between
¯.t/ D ®.¿ / £ ! IBB d¿ (38)
2 tm ¡ 1
successive integrated angular rate increment sensor samples taken
over the tl ¡ tl¡1 time interval (similar to the approach presented in
Let us now consider the integration of Eq. (38) as divided into a Ref. 19 over the tm ¡ 1 to tm interval). The weighting coef cients in
portion up to and after a general time tl ¡ 1 within the tm ¡ 1 to tm the latter case are then optimized for best average performance in a
interval so that Eq. (38) is equivalently pure coning environment, i.e., ! BIB constantin magnitude,but rotat-
ing. Each of the latter design approaches are based on curve tting
¯.t / D ¯ l ¡ 1 C 1¯.t /; ¯ m D ¯.tm / techniques for an assumed angular rate pro le shape. Each resulting
Z tl (39) algorithm behaves differently in rate environments for which it was
1 ¡ ¢ not designed and in the presence of angular rate sensor quantization
1¯l D ®.t/ £ ! BIB dt
2 tl ¡ 1 noise. Selection of the preferred algorithm should include simula-
tion analysis to con rm acceptable performance under operational
where ¯l ¡ 1 is the value of ¯.t / at t D tl ¡ 1 and l is the computer rate environments and sensor noise characteristics.
cycle index for t D tl cycle times. Note that by its de nition, the We concludethis section by providingan example of an algorithm
l cycle index is faster than the m cycle index. We now de ne the for the Eq. (42) integral term based on the body rate term ! IBB being
next l cycle time point tl within the tm ¡ 1 to tm interval so that at tl , approximated to rst order by the truncated power series expansion
Eq. (39), including initial conditions, become
¯l D ¯l ¡ 1 C 1¯ l ; ¯ m D ¯ l .tl D tm / ! BIB ¼ A C B.t ¡ tl ¡ 1 /; A and B D const (43)
¯l D 0 at t D tm ¡ 1
(40) References 9– 11 and 12, Sec. 7.1.1.1.1, show that for the Eq. (43)
Z motion over the interval from tl ¡ 2 to tl
1
tl ¡ ¢
1¯l D ®.t/ £ ! BIB dt
2 tl ¡ 1 Z tl ¡ ¢ 1
1®.t / £ ! IBB dt D .1®l ¡ 1 C 1®l / (44)
Through a similar process, the ®.t / expression for Eq. (40) is ob- tl ¡ 1 12
tained by manipulation of ®.t / in Eqs. (36) as
Z t
Substituting Eq. (44) into Eq. (42) then yields
®.t/ D ®l ¡ 1 C 1®.t /; 1®.t / D ! BIB d¿
tl ¡ 1
1
¡ ¢
(41) 1¯ l D 2
®l ¡ 1 C 16 1®l ¡ 1 £ 1®l (45)
1®l D 1®.tl /; ®l D ®l ¡ 1 C 1®l
®m D ®l .tl D tm /; ®l D 0 at t D tm ¡ 1 Equation (45) has been classi ed as a second-orderalgorithmfor ¯ m

because it includes current and past l cycle 1® products in the 1¯ l
With Eqs. (41), Eqs. (40) are equivalently equation. From the analysis leading to Eq. (44), the l and l ¡ 1 1®
Z tl
product term in 1¯l , i.e., the 16 term, stems from the approximation
1 1 ¡ ¢ of linearly ramping angular rate in the tl ¡ 2 to tl time interval. If the
1¯ l D .®l ¡ 1 C 1®l / C 1®.t / £ ! BIB dt
2 2 tl ¡ 1
angular rate was approximated as a parabolically varying function
(42) of time, a third-order algorithm would result containing l, l ¡ 1,
¯l D ¯l ¡ 1 C 1¯ l ; ¯ m D ¯ l .tl D tm / and l ¡ 2 1® products. If the angular rate was approximated as a
constant over tl ¡ 1 to tl , the 16 term for 1¯l in Eq. (45) would van-
¯l D 0 at t D tm ¡ 1 ish, resulting in a rst-order algorithm for ¯ m . Finally, if angular
rates are slowly varying, we can approximate ¯ m as being equal
Equations (41) and (42) constitutethe constructof a digitalrecursive to zero. Alternatively (and more accurately), we can set the l cycle
algorithm at the l computer cycle rate for calculating®m and the ¯ m rate equal to the m cycle rate, which equates ¯ m in Eqs. (45) to
coning term as a summation of changes in ®, ¯ over the tl ¡ 1 to tl 1¯ l calculated once at time tm [and noting from the initial con-
interval. It remains to determine a digital equivalent for the Eq. (42) dition de nition in Eq. (41) that ®l ¡ 1 would be zero]. The latter
integral term in 1¯ l . algorithm was developed in Ref. 4. Note that setting the l and m
Continuing work in attitude algorithm development has centered rates equal can also be achieved by increasing the m rate to match
on the design of digital algorithms for evaluating the coning equa- the l rate. The result is a single, high-speed, higher-order algorithm
tion (42) integral term. In general, the methods utilized assume a with a simpler software architecture than the two-speed approach,
general analytical form for the angular rate pro le ! BIB in the tl ¡ 1 but requiring more throughput.Continuing advances in the speed of
to tl time interval, e.g., a truncated general polynomial in time. The modern-daycomputersmay make this the preferredapproachfor the
Eq. (42) integral is then analytically determined as a function of the future.
general rate pro le coef cients, e.g., the polynomial coef cients. The overall digital algorithm for ®m and ¯ m in Eq. (35) is deter-
Finally, the coef cients for the angular rate pro le are calculated to mined from the given results as a composite of Eqs. (41), (42), and
t successive integrated angular rate increment measurements. For (45)
the example that follows, the angular rate pro le is approximatedas
a constant plus a linear buildup in time with the constant and ramp- Z tl
ing coef cients calculated from the current and previous values of 1®l D d®; ®l D ®l ¡ 1 C 1®l
1®l . A more sophisticatedversion of this algorithm might include a tl ¡ 1
parabolic-with-timeterm in the assumed angular rate pro le, utiliz- (46)
ing the current, past, and past-past values of 1®l for coef cient de- ®m D ®l .tl D tm /; ®l D 0 at t D tm ¡ 1
termination.Recent work in this area10;11 calculates the angular rate
pro le coef cients from angular rate sensor measurements taken 1
¡ ¢
within the tl ¡ tl ¡ 1 time interval (an extension of the technique pro- 1¯l D 2
®l ¡ 1 C 16 1®l ¡ 1 £ 1®l ; ¯l D ¯l ¡ 1 C 1¯l
posed in Ref. 19 for single-speed algorithm enhancement), thereby (47)
incorporatinga third computation cycle rate into the overall attitude ¯ m D ¯ l .tl D tm /; ¯l D 0 at t D tm ¡ 1
SAVAGE 25
where navigation; however, it might not be negligible during initial align-

ment operations (prior to the start of inertial navigation) where the
d® = differential integrated angular rate increment, LI
C L I .n/ matrix is used to apply tilt updates to C BL (Refs. 12, Sec.
i.e., analytical representation of pulse output from .n ¡ 1/
strapdown angular rate sensors, ! BIB dt 6.1.2, and 15, pp. 120– 121). Initial tilt alignment corrections to C BL
1®l = summation of integrated angular rate output can be fairly large, e.g., 0.1– 1.0 deg, which can produce undesirable
increments from angular rate sensors errors in C BL during the initial alignment process if too simpli ed a
version of Eq. (49) is utilized. The closed-loop servo action of the
initial alignment operations would eventually correct the resulting
2. Local Level Frame Rotation
attitude error generated in C BL ; however, it could leave a residual or-
Equation (24) updates the C BL attitude direction cosine matrix us-
LI thogonality/normalityerrorin the C BL rows (and columns). The result
ing C L I .n/ to accountfor angularrateof the local-levelcoordinate L would be the requirementto include an orthogonality/normalization
.n ¡ 1/ LI
frame relative to nonrotating space ! LIL . The derivation for C L I .n/ correction algorithm (see Sec. IV.A.3) as an outer loop in the C BL
B I ¡ 1/ .n ¡ 1/
directly parallels that used to determine C B I .m in Sec. IV.A.1. The update processing.
LI
formal de nition for C L I .n / is
.m /
A discrete digital algorithm for the Eq. (50) ³ n integral can be
.n ¡ 1/ constructed by rst combining Eqs. (13) and (15) to obtain the ! LIL
Z tn
integrand and then approximating
LI
C L I .n/ DIC CP LL.t/ dt (48) µ ¶
.n ¡ 1/ I
.n ¡ 1/ ¡ ¢
tn ¡ 1 ! LIL ¼ C NL ! NIE 1 C ½ Z N n ¡ 1 u NZ N C FCn ¡ 1 u NZ N £v N
(52)
n¡ 2 2
2
where L.t/ is the L frame attitude at an arbitrary time in the interval
tn ¡ 1 to tn . where the subscript n ¡ 12 is the value for ( ) midway between times
LI
The C L I .n/ matrix can also be expressed in terms of the rotation tn ¡ 1 and tn . Using Eq. (52) in Eq. (50) then obtains
.n ¡ 1/
vector de ning the frame L I.n/ attitude relative to frame L I.n ¡ 1/ . µ ³ X
j ´¶
Applying Eq. (4) with Taylor series expansion for the coef cient ³n ¼ C NL ! NIE Tn C½ Z N 1 u NZ N Tn C FC 1 u NZ N £ 1RmN
terms obtains n¡ 1
2
n¡
2
n¡
2
LI sin ³n .1 ¡ cos ³n / (53)

C L I .n/ D I¡ .³ n £/ C .³ n £/.³ n £/ with ! NIE evaluated using Eq. (14) and
.n ¡ 1/ ³n ³n2
Z tm
sin ³n ³2 ³4 1RmN ´ v N dt (54)
D 1 ¡ n C n ¡ ¢¢¢ (49)
³n 3! 5! tm ¡ 1
.1 ¡ cos ³n / 1 ³2 ³4 where Tn is the computer n cycle update period tn ¡ tn ¡ 1 and j is the

D ¡ n C n ¡ ¢¢¢ number of computer m cycles over the tn ¡ 1 to tn n-cycle computer
³n2 2! 4! 6!
update period.
The subscripted n ¡ 12 terms in Eq. (53) are all functions of posi-
where ³ n is the rotation vector de ning the frame L I.n/ attitude at
tion, which (from Part 2, Ref. 13) is updated following the attitude
time tn relative to the frame L I.n ¡ 1/ attitude at time tn ¡ 1 . Note in
update at the n-cycle rate. Hence, to calculatethese terms in Eq. (52),
Eq. (49) that the sign for the [.sin ³n =³n /.³ n £/] term is negative in
BI an approximate extrapolation formula must be used based on previ-
contrast with the similar term in the Eq. (26) C B I .m ¡ 1/ expression. ously computed values of the ( ) parameters. For example, a linear
L I.n/ .m/
This is because the C L I matrix has the opposite phase sense extrapolation formula using the last two computed values for ( )
B I.m ¡ 1/ A .n ¡ 1/ L I
from C B I [or C A12 in Eq. (4)] in that C L I .n/ transforms vectors would be
.m/ BI .n ¡ 1/
from L I.n ¡ 1/ to L I.n/ , whereas C B I .m ¡ 1/ transforms vectors from B I.m /
L I.n / .m/ . /n¡ 1 ¼ . /n ¡ 1 C 12 [. /n ¡ 1 ¡ . /n ¡ 2 ] D 32 . /n ¡ 1 ¡ 12 . /n ¡ 2 (55)
to B I.m ¡ 1/ . As such, the C L I form in Eq. (49) is the transpose of 2
BI .n ¡ 1/
the Eq. (26) C B I .m ¡ 1/ expression form. In Part 2 (Ref. 13) we nd that the v N velocity update follows the
.m/
Because the tn ¡ 1 to tn update cycle is relatively short, ³ n will be attitude update. Therefore, current and past m-cycle values of v N
very small in magnitude.Because ! LIL is small and slowly changing are available for evaluating the Eq. (54) integral for 1RnN . Using a
over a typicaltn ¡ 1 to tn updatecycle (dueto small changesin velocity trapezoidal integration algorithm for Eq. (54) obtains
and position over this time period) the L frame rate vector ! LIL can ¡ ¢
be approximatedas nonrotating.The result is that ³ n for Eq. (49) can 1RmN ¼ 1
vmN C vmN ¡ 1 Tm (56)
2
be calculated as the integral of the simpli ed form of the Eq. (10)
rotation vector rate equation whereby the cross-product terms are where Tm is the computer m cycle update period tm ¡ tm ¡ 1 .
neglected, Part 2 (Ref. 13) also develops a high-resolution version of 1RmN
Z tn for precision position updating that accounts for dynamic angular
³n ¼ ! LIL dt (50) rates and accelerations within the m ¡ 1 to m cycle update interval.
tn ¡ 1
3. Normalization and Orthogonalization
We note in passing that Lbased on the smallness of ³ n as already From its basic de nition in Sec. II.B, the columns (and rows)
I
discussed,Eq. (49) for C L I .n/ can also be simpli ed. For example, of C BL represent orthogonal unit vectors, which, therefore, should
.n ¡ 1/
a second-order version (accurate to second order in ³ n ) is from be unity in magnitude (normality condition) and mutually orthog-
Eq. (49), onal to one another (orthogonality condition). In addition to the
basic C BL update algorithms already described, a normalization and
LI
C L I .n/ ¼ I ¡ .³ n £/ C 12 .³ n £/.³ n £/ (51) orthogonalization algorithm is frequently included to ensure that
.n ¡ 1/ the C BL rows and columns remain normal and orthogonal. Factors
that cause C BL orthogonality/normality error include C BL orthogo-
The computer memory/throughputadvantages of utilizing a simpli- nality/normality initialization error, software programming error,
LI
ed form of Eq. (49) for C L I .n/ [such as Eq. (51)] are trivial for roundoff error due to insuf cient computer wordlength for the to-
.n ¡ 1/
today’s modern computer technology compared to the disadvan- tal number of C BL algorithm update cycles expected, and insuf cient
tages of increased software validation/documentation complexity number of terms carriedin the Eqs. (26)and (49) Taylor series expan-
and loss in accuracy. The accuracy loss is generally minor during sions (truncation error). It is important to note (Ref. 12, Sec. 3.4.1)
26 SAVAGE
2 3
that orthogonality and normalization errors can only be produced cos 0:5 Ám
B I
from errors in the software implementation of Eqs. (23), (24), (26), q B I .m ¡ 1/ D 4 sin 0:5 Ám 5
and (49), not from errors in the algorithms feeding these equationsor .m/ 0:5Ám
0:5 Ám
from inertial sensorinput errors. The overall design/veri cation pro-
cess for the C BL integrationalgorithmsoftware must assureerror-free
programmingand acceptableroundoff/truncationerror for the angu- (60)
sin 0:5 Ám .0:5 Ám /2 .0:5 Ám /4
lar rate environment anticipated over the expected navigation time D1¡ C ¡ ¢¢¢
period, a readily achievable goal with today’s computer/software 0:5 Ám 3! 5!
development technology. Nevertheless, inclusion of a C BL orthog- .0:5 Ám /2 .0:5 Ám /4
onality/normality correction algorithm has been traditionally em- cos 0:5 Ám D 1 ¡ C ¡ ¢¢¢
ployed in many strapdown inertial navigation software packages 2! 4!
for enhanced accuracy and to relax the more stringent requirement The Ám rotation vector in Eq. (60) for attitude quaternion updating
of not allowing any orthogonality/normalization error in the ba- is identical to Ám used in Sec. IV.A.1 for C BL direction cosine matrix
sic C BL updating operations. The algorithms used for normaliza- updating and is calculated using the identical algorithm provided by
tion/orthogonalizationare based on the property that the transpose Eqs. (35), (41), and (42) or Eqs. (35), (46), and (47).
of a direction cosine matrix equals its inverse (see Sec. II.B); con-
sequently, the product of C BL with its transpose should be identity. 2. Local Level (L ) Frame Rotation
LI
Variations from this condition measure the orthogonality/normality Equation (58) updates the q BL attitude quaternion using q L I .n /
error, which can then be used by a control algorithm in iterative .n ¡ 1/
to account for angular rate of the local-level coordinate L frame
fashion for correction (Refs. 9; 12, Secs. 7.1.1.3; and 15, pp. 216– L
L I .n /
218). relative to nonrotating space ! I L . The formal de nition for q L I
.n ¡ 1/
is
Z tn
B. Attitude Quaternion LI
q L I .n/ D q1 C qP LL.n.t/¡ 1/ dt (61)
The updating algorithm for the q BL attitude quaternion is designed .n ¡ 1/
tn ¡ 1
to achieve the same numerical result at the attitude update times as
would the formal continuous integration of the Eq. (12) qP BL expres- with L.t / in Eq. (61) representingthe L frame attitude at an arbitrary
sion at the same time instant. The updating algorithm for the q BL time in the interval tn ¡ 1 to tn .
L
attitude quaternion is developed following the identical procedure The q L II .n/ attitude quaternion can also be expressed in terms
.n ¡ 1/
used for the C BL updating algorithm derivation in Sec. IV.A. Thus, of the rotation vector de ning the frame L I.n/ attitude relative to
using the Eq. (7) attitude quaternion chain rule, we write frame L I.n ¡ 1/ . Applying Eq. (5) with Taylor series expansion for the
integral terms yields
LI LI BI 2 3
q B I .n ¡ 1/ D q B I .n ¡ 1/ q B I .m ¡ 1/ (57) cos 0:5 ³n
.m/ .m ¡ 1/ .m/ L I.n/
qL I D 4 sin 0:5 ³n 5
LI LI LI .n ¡ 1/ ¡ 0:5 ³ n
q B I .n/ D q L I .n/
.n ¡ 1/
q B I .n ¡ 1/ (58) 0:5 ³n
.m / .m/
sin 0:5 ³n .0:5 ³n /2 .0:5 ³n /4

where D1¡ C ¡ ¢¢¢ (62)
0:5 ³n 3! 5!
LI
q B I .n ¡ 1/ = q BL relating the B frame at time tm ¡ 1 to the .0:5 ³n /2 .0:5 ³n /4
.m ¡ 1/
L frame at time tn ¡ 1 cos 0:5 ³n D 1 ¡ C ¡ ¢¢¢
qBI
L I.n /
= q BL relating the B frame at time tm to the 2! 4!
BI
.m/
L frame at time tn The negative sign on ³ n accounts for the opposite phase sense of
LI
q B I .m ¡ 1/ = attitude quaternion that accounts for B frame q L I .n/ , which describes the frame L I.n ¡ 1/ attitude relative to frame
.m/
rotation relative to inertial space from its .n ¡ 1/
orientation at time tm ¡ 1 to its orientation L I.n/ compared with the rotation vector ³ n phase sense, which de-
at time tm scribes the frame L I.n / attitude relative to frame L I.n ¡ 1/ . The ³ n
LI
q L I .n / = attitude quaternion that accounts for L frame rotation vector in Eqs. (62) is identical to ³ n used for C BL direc-
.n ¡ 1/
rotation relative to inertial space from its tion cosine matrix updating and is calculated using the identical
orientation at time tn ¡ 1 to its orientation at time tn computational algorithm described in Sec. IV.A.2 and provided by
Eqs. (53), (55), and (56).
BI LI An approximate form of Eqs. (62) that is comparable in accu-
The updates for q BL are performed by q B I .m ¡ 1/ and q L I .n/ in Eqs.
.m/ .n ¡ 1/ racy to direction cosine updating Eq. (51) is readily obtained by
(57) and (58), algorithms for which are derived separately next. substitution and truncation
³ ´
LI 1 ¡ 0:5.0:5 ³n /2
1. Body Frame Rotation q L I .n/ D (63)
BI .n ¡ 1/ ¡0:5 ³ n
Equation (57) updates the q BL attitude quaternion using q B I .m ¡ 1/
.m/
to account for angular rotation rate ! BIB of the strapdown sensor The comments in Sec. IV.A.2 regarding the advisability of using
(body) B frame relative to nonrotating space. The formal de nition the simpli ed Eq. (51) direction cosine local-level frame updating
B I ¡ 1/
for q B I .m is algorithmalso apply regardinguse of Eq. (63) for attitudequaternion
.m/
updating rather than the complete Eqs. (62) form.
Z tm
BI BI 3. Normalization
q B I .m ¡ 1/ D q1 C qP B.t/.m ¡ 1/ dt (59)
.m/
tm ¡ 1
To preserve the fundamental attitude quaternion normality char-
acteristic discussed in Sec. II.B, a normalization algorithm is fre-
quently incorporated as an outer-loop function in the q BL attitude
where B.t / is the B frame attitude at an arbitrary time in the inter- quaternionupdatingprocess.The discussionin Sec. IV.A.3 for direc-
val tm ¡ 1 to tm . tion cosinematricesregardingthe need for a normalization/orthogo-
BI
The q B I .m ¡ 1/ attitude quaternion can also be expressed in terms of nalization function is equally applicable for the attitude quaternion,
.m/
a rotation vector de ning the frame B I.m/ attitude relative to frame the only exception being that orthogonalization has no meaning
B I.m ¡ 1/ . Applying Eq. (5) with Taylor series expansion for the coef- in the de nition for the quaternion (as it does for the attitude di-
cient terms obtains rection cosine matrix); hence, the orthogonalization discussion in
SAVAGE 27
Table 1 Summary of strapdown INS attitude computation algorithms

Algorithm function Input Output Equation number
High-speed calculations
Integrated B frame angular rate increments 1®l ®l , ®m (41) or (46)
Coning increment 1®l , ®l ¯m (42) or (47)
Normal-speed calculations for Earth related parameters
N frame Earth rate components C NE ! NIE (14)
Vertical transport rate component CN E ½Z N Ref. 12, Sec. 4.6
Curvature matrix C NE ; h FC Ref. 12, Sec. 5.3
Normal-speed velocity calculations
N frame velocity update —— vN Part 2 (Ref. 13)
Normal-speed attitude calculations
B frame rotation vector ®m , ¯m Á (35)
B I m¡ 1/
B frame rotation matrix (for attitude direction Ám C B I .m (26)
cosine matrix updating) BI
.m /
¡ 1/
B frame rotation quaternion (for attitude Ám q B I .m (60)
quaternion updating) L I.n ¡ 1/ B I ¡ 1/
.m/
L I.n ¡ 1/
Attitude update for B frame rotation (direction CBI , C B I .m CBI (23)
.m ¡ 1/
cosine matrix form) L I ¡ 1/
.m/
B I ¡ 1/
.m/
L I.n ¡ 1/
Attitude update for B frame rotation q B I .n , q B I .m q BI (57)
.m ¡ 1/
(quaternion form) .m / .m /
N frame position increment vN 1RmN (56)

L frame rotation vector ! NIE , ½ Z N , FC , 1RmN ³n (53), (55)
LI
L frame rotation matrix for attitude direction ³n C L I .n/ (49)
.n ¡ 1/
cosine matrix updating (exact form) LI
L frame quaternion for attitude ³n q L I .n/ (62)
.n ¡ 1/
quaternion updating (exact form) L I .n ¡ 1/ LI L I .n /
Attitude update for L frame rotation C BI , C L I .n/ C BI (24)
.n ¡ 1/
(direction cosine matrix form) .m/
L I ¡ 1/ LI
.m/
L I.n /
Attitude update for L frame rotation q B I .n , q L I .n / q BI (58)
.n ¡ 1/
(quaternion form) .m/ .m/
L
Normalization and orthogonalization corrections CB C BL Sec. IV.A.3
(for attitude direction cosine matrix)
Normalization corrections q BL q BL Sec. IV.B.3
(for attitude quaternion)
Normal-speed position calculations
Position direction cosine matrix and altitude update —— C NE , h Part 2 (Ref. 13)
Sec. IV.A.3 does not apply. If a quaternion normalization algorithm used to predict high-speed coning algorithm error under speci ed
is to be utilized, it is based on comparing the magnitude of qBL with coning rates/amplitudes as a function of algorithm repetition rate
unity and using the variationfrom unity to iterativelyupdateq BL with (Refs. 9 – 11 and 12, Sec. 10). The coning rates/amplitudes must be
a control algorithm (Refs. 9; 12, Sec. 7.1.2.3; and 15, pp. 216– 218). derived either from empiricaldata or, more commonly,from analyti-
cal models of the sensor assembly mount imbalance and its response
V. Attitude Integration Algorithm Summary to external input vibration at particular frequencies (Ref. 12, Sec.
Table 1 summarizes the algorithms described for the strapdown 10). Frequency-domain simulators can be used to evaluate high-
inertial navigation attitude integration function listed in the order speed coning algorithm error under speci ed input vibration power
they would be executed in the navigation computer. Table 1 lists spectral density pro les and sensor assembly mount imbalance as
the algorithm function, input parameters, output parameters, and a function of algorithm repetition rate (Ref. 12, Sec. 10). For ex-
equation number. ample, the coning algorithm described by Eqs. (46) and (47) can
be shown by such simulators to have an error rate of 0.00037 deg/h
VI. Algorithm and Execution Rate Selection when operatedat a 2-kHz repetitionrate under exposureto 7.6 g rms
Faced with the multitude of potential strapdown inertial naviga- widebandrandomlinearinputvibration( at 0.04 g2 /Hz densityfrom
tion algorithms to choose from, the software designer must ulti- 20 to 1000 Hz, then decreasinglogarithmicallyto 0.01 g2 /Hz at 2000
mately choose one set for the application at hand. The algorithms Hz). The linear vibration generates a 0.0003-rad multiaxis angular
presented in this Part 1 and the subsequent Part 2 (Ref. 13) papers oscillation of the sensor assembly with a correspondingconing rate
are but one version of many similar algorithms developed over the of 9.9 deg/h due to the following typical sensor assembly mount
years by several authors. The process of selecting the algorithm set characteristics selected as simulator input parameters: 50-Hz linear
for a particular applicationshould consider the allowable algorithm vibration mode undamped natural frequency, 0.125 linear vibration
error under anticipated angular rates/accelerations/vibrations, the mode dampingratio, 71-Hz rotaryvibrationmode undamped natural
capability of the projected target navigation computer for the re- frequency, 0.18 rotary vibration mode damping ratio, 5% sensor as-
quired algorithm execution rate, and the complexity of the design sembly mount mechanical isolator spring and damping imbalance,
procedure for software validation/documentation with the selected and 1.4% sensor assembly center of mass offset from mechanical
algorithms. c.g. mount center (percent of distance between isolators).
Evaluation of candidate algorithm error characteristics is gen- The capabilitiesof modern-day computer and INS software tech-
erally performed using computerized time-domain simulators that nology make it reasonableto specify that the attitude algorithm error
exercise the algorithms in particular groupings at their selected rep- be no greater than 5% of the equivalent error produced by the INS
etition rates. The simulators generate simulated strapdown inertial inertial sensors (whose cost increases dramatically with accuracy
sensor angular rate/acceleration pro les for algorithm test input to- demands). For an INS with a 0.007-deg/h angular rate sensor bias
gether with known navigation parameter solutions for algorithm accuracy requirement (for a typical aircraft INS having 1 n mph
output comparison, e.g., Ref. 12, Sec. 11.2. For the attitude al- 50 percentile radial position error rate), the 0.00037-deg/h coning
gorithms discussed, simpli ed analytical error models can also be algorithm error rate satis es the 5% allowance.
28 SAVAGE
So long as the selected integration algorithm is analyticallyvalid, algorithm de nes a framework for design of the velocity/position
it can be improved in accuracy by increasing its repetition rate. integrationalgorithms in Part 2 (Ref. 13) to have similar characteris-
Continuing computer technology advances (increasing speed and tics: analytical exactness under constant angular rate/speci c force
decreasing program memory cost), therefore, tend to diminish any accelerationand using a small approximatehigh-speedcomputation
advantages one algorithm might have over another (usually mea- to measure deviations from the latter condition (denoted as sculling
sured, primarily, by accuracy for a given repetition rate and, sec- for the velocity algorithm and scrolling for the position algorithm).
ondarily, by required program memory). Excessively high repeti- A summary of the attitude integration algorithms developed is pro-
tion rates are to be avoided, however (even if computer throughput vided in Table 1 as a listing in the order they would be executed
allowances permit) to limit error buildup caused by computer - in the navigation computer. A similar table is provided in Part 2
nite wordlengtheffects and recti cation of high-frequencymultiaxis (Ref. 13) for the velocity/position integration algorithms.
sensor errors (high-frequency error output from one inertial sensor
that is frequency correlated with outputs from sensors in the other References
axes, denoted as pseudoconing error for the coning computation in 1 Minor, J. W., “Low-Cost Strapdown-Down Inertial Systems,” AIAA/
Part 1 and pseudosculling error for the sculling part of the veloc- ION Guidance and Control Conf., Aug. 1965.
2 Hills, F. B., “A Study of Coordinate-Conversion Errors in Strapped-
ity calculation in Ref. 13, Part 2). The nite computer wordlength
error effect is generally not a major factor with modern computer Down Navigation,” MIT Electronics Systems Lab., E SL-4-244, Mas-
technology, typically having 64-bit double precision oating point sachusetts Inst. of Technology, Cambridge, MA, Aug. 1965.
3 Turley, A. R., “A Solution for the Problems of the No-Gimbal Inertial
wordlengths. The pseudoconing/sculling issue must be resolved on
Navigator Concept,” Air Force Avionics Lab., AFAL-TR-64-307, Wright–
an individual design basis depending on the characteristicsof high- Patterson AFB, OH, Jan. 1965.
frequency error effects anticipatedfrom the inertial sensor assembly 4

Mckern, R. A., “A Study of Transformation Algorithms for Use in a
in its operationaldynamicenvironment.A generalgroundrule to fol- Digital Computer,” M.S. Thesis, Dept. of Aeronautics and Astronautics,
low in coning/sculling algorithm repetition rate selection is to run Massachusetts Inst. of Technology, Cambridge, MA, Jan. 1968.
the algorithms only as fast as required to accurately measure an- 5 United Aircraft Corp., “A Study of Critical Computational Problems
ticipated real multiaxis high-frequency angular rates/accelerations Associated with Strapdown Inertial Navigation Systems,” NASA CR-968,
that can potentially rectify into real attitude/velocity change, but April 1968.
6 Savage, P. G., “A New Second-Order Solution for Strapped-Down Atti-
no faster, to minimize the likelihood of rectifying high-frequency
tude Computation,” AIAA/JACC Guidance and Control Conf., Aug. 1966.
sensor output error into attitude/velocity error buildup. 7 Jordan, J. W., “An Accurate Strapdown Direction Cosine Algorithm,”
The ultimate selection of algorithms to be used in a particular NASA TN-D-5384, Sept. 1969.
application is generally made based on the previous experience of 8 Bortz, J. E., “A New Mathematical Formulation for Strapdown Inertial
the responsibledesign engineer.The author has had long experience Navigation,” IEEE Transactions on Aerospace and Electronic Systems, Vol.
with the algorithms described and feels comfortable adapting them AES-7, No. 1, 1971, pp. 61 – 66.
to any strapdown application. They are well de ned analytically, 9 Savage, P. G., “Strapdown System Algorithms,” Advances in Strapdown
can be programmed using a simple sequential software executive Inertial Systems, NATO AGARD Lecture Series No. 133, May 1984.
10 Ignagni, M. B., “Optimal Strapdown Attitude Integration Algorithms,”
structure, readily lend themselves to straightforwardvalidation pro-
cedures, and are easily adapted to the requirements and constraints Journal of Guidance, Control, and Dynamics, Vol. 13, No. 2, 1990, pp.
of particular applications. 363– 369.
11 Ignagni, M. B., “Ef cient Class of Optimized Coning Compensation
Algorithms,” Journal of Guidance, Control, and Dynamics, Vol. 19, No. 2,

VII. Concluding Remarks 1996, pp. 424– 429.
12 Savage, P. G., Strapdown Analytics, Strapdown Associates, Inc., Maple
We have de ned the overall requirement for the strapdown iner-
Plain, MN (to be published).
tial navigationintegrationfunction(in the form of continuousdiffer- 13 Savage, P. G., “Strapdown Inertial Navigation Integration Algorithm
ential equations) and developed the attitude integration algorithms Design Part 2: Velocity and Position Algorithms,” Journal of Guidance,
based on the two-speed updating approach: an exact algorithm for Control, and Dynamics (to be published).
moderate speed updating fed by a simpli ed high-speed algorithm. 14
Morse, P. M., and Feshbach, H., Methods of Theoretical Physics,
The high-speed algorithm contains a simple summing operation of McGraw – Hill, New York, 1953.
15
angular rate sensor inputs plus an approximate coning motion inte- Savage, P. G., Introduction to Strapdown Inertial Navigation Systems,
gration function. Under conditions where the angular rate vector is 8th printing, Strapdown Associates, Inc., Maple Plain, MN, 1997.
16
not rotating,i.e., zero coning,the coningterm becomeszero, the sim- “Department of Defense World Geodetic System 1984,” Defense Map-
ple summing operationbecomesan analyticallyexact representation ping Agency, Technical Rept. DMATR 8350.2, 2nd printing, Sept. 1987.
17
Britting, K. R., Inertial Navigation System Analysis, Wiley, New York,
of the attitude change, and the overall attitude update operation is 1971.
error free. Where computer throughput restrictions are not at issue, 18
Macomber, G. R., and Fernandes, M., Inertial Guidance Engineering,
the two-speed structure presented can be compressed into a single Prentice– Hall, Englewood Cliffs, NJ, 1962.
high-speed format by operating the moderate-speed algorithm at 19
Miller, R., “A New Strapdown Attitude Algorithm,” Journal of Guid-
the high-speed rate. This general form for the two-speed attitude ance, Control, and Dynamics, Vol. 6, No. 4, 1983, pp. 287– 291.

Strapdown Inertial Navigation Integration Algorithm Design Part 1: Attitude Algorithms

Uploaded by

Strapdown Inertial Navigation Integration Algorithm Design Part 1: Attitude Algorithms

Uploaded by

JOURNAL OF GUIDANCE, CONTROL, AND DYNAMICS

Vol. 21, No. 1, January– February 1998