MOVING BASE INS ALIGNMENT WITH LARGE INITIAL HEADING ERROR

Paul G. Savage
Strapdown Associates, Inc.

SAI-WBN-14006
www.strapdownassociates.com
October 3, 2014

ABSTRACT

Prior to inertial navigation mode engagement, an inertial navigation system (INS)

executes Kalman filter attitude alignment operations, an estimation process based on
minimizing a linearized model of INS attitude errors. Kalman error model linearization
places a constraint on INS initial attitude before initiating Kalman alignment operations:
attitude errors at the start of Kalman alignment must be small enough that residual second
order error effects from linearized modeling have negligible impact on alignment
accuracy. Under stationary alignment conditions, linearized Kalman error models have
been commonly configured for large initial heading error with no second order heading
induced errors, by representing heading error as inaccuracy in estimating horizontal earth
rate components. For non-stationary dynamic alignment conditions, however, the earth
rate estimation alignment approach cannot be used directly. The difficulty arises when
forming the Kalman filter measurement input; a navigation data comparison between the
INS and the equivalent data provided by a reference navigation device (e.g., GPS derived
velocity). The data measurement comparison must be made in a common coordinate
frame which becomes problematic when INS heading is initially unknown. Based on a
recent INS alignment article for small initial heading error, this article shows how the
Kalman filter measurement can be formulated so that large initial INS heading
uncertainty generates negligible second order error impact on alignment accuracy under
dynamic motion conditions.

MATHEMATICAL NOTATION

V = Vector without specific coordinate frame designation. A vector is a

parameter that has length and direction. Vectors used in the paper are
classified as “free vectors”, hence, have no preferred location in coordinate
frames in which they are analytically described.

V or V = Magnitude of vector V.

V A = Column matrix with elements equal to the projection of V on coordinate

frame A axes. The projection of V on each frame A axis equals the dot
product of V with a unit vector parallel to that coordinate axis.

1
(VA ×) = Skew symmetric (or cross-product) form of V A represented by the
⎡ 0 −V ZA V YA ⎤
⎢ ⎥
square matrix ⎢ V ZA 0 −V XA ⎥ in which V XA, V YA, V ZA are the
⎢⎣ −V YA V XA 0 ⎥⎦
( )
components of V A . The matrix product of V A × with another A frame
vector equals the cross-product of V A with the vector in the A frame,
( )
i.e.: V A × W A = V A × W A .

A
CA 1 = Direction cosine matrix that transforms a vector from its coordinate frame
2
A2 projection form to its coordinate frame A1 projection form, i.e.:
A
V A1 = C A1 V A 2 . The columns of CA 12 are projections on A1 axes of unit
A2
A
vectors parallel to A2 axes. Conversely, the rows of CA 12 are projections on
A
A2 axes of unit vectors parallel to A1 axes. An important property of CA 12
is that it's inverse equals it's transpose.

ω A1 A 2 = Angular rotation rate of coordinate frame A2 relative to coordinate

frame A1. Conversely, the angular rotation rate of coordinate frame A1
relative to coordinate frame A2 is the negative of ω A1 A 2 , i.e.,:
ω A 2 A1 = − ω A1 A 2 .

. d( )
()= = Derivative with respect to time t.
dt

( ) = Computed value of parameter ( ) that, in contrast with the idealized error free
value ( ), contains errors.

( ) = Measured value of strapdown inertial sensor ( ) that, in contrast with the

idealized error free value ( ), contains errors.

δ = Designation for errors that are small compared with .

Δ = Designation for errors that can be as large as .

E = Expected value operator.

2
COORDINATE FRAMES

N = INS locally level navigation coordinate frame (with Z axis up) used for
attitude referencing and velocity/position integration operations. By
definition in this article, the initial heading of the N Frame is assumed to be
nominal, i.e., error free. Initial heading alignment of the N Frame relative to
another known reference frame (N*) is accounted for by defining the N
frame to be nominally misaligned from the N* frame.

N* = Locally level navigation coordinate frame (with Z axis up) used by a

reference aiding device to deliver position/velocity data to the INS being
aligned. The heading angle misalignment between the N and N* frames is
the means to account for initial heading error in the INS attitude data at the
start of alignment. As defined, the Z axis of the N* frame is parallel to the Z
axis of the N Frame.

B = Strapdown inertial sensor coordinates (“body frame”) with axes parallel to

nominal right handed orthogonal sensor input axes.

I = Non-rotating inertial coordinate frame used as a symbolic reference for gyro

angular rotation rate measurements.

E = Coordinate (earth) frame aligned with axes fixed to the earth.

PARAMETER DEFINITIONS

Analytic parameters used in this article are defined following equations where they
are first used.

INTRODUCTION

An important part of inertial navigation system (INS) operations is the initialization

process in which the INS navigation parameters (angular orientation - attitude, velocity,
and position) are initialized for integration functions to follow during inertial navigation.
Initialization typically consists of two phases; Coarse Alignment followed by Fine
Alignment, e.g., [3 - Chapt. 6]. During Coarse Alignment, the INS angular attitude is
initialized to an approximately correct value. Fine Alignment then ensues in which
attitude is converged to inertial navigation grade accuracy and velocity/position are
initialized, all generally implemented within a Kalman filter structure.

General Inertial Aiding Structure

Fine Alignment is a specialized application of Inertial Navigation Aiding (Figure 1), a

dynamic process in which INS computed navigation data is periodically compared with

3
equivalent reference navigation data (at cycle rate n), and used in feedback fashion to
update INS error parameters. Note in Figure 1 that all parameters are shown with a ( )
designation to indicate that they are computed estimates within the INS and reference
navigation device of actual equivalent ( ) parameters. The analytic details of the Figure 1
operations are provided in [1, 2, 3 - Chapt. 15, 4 - pp. 415 - 457].

Kalman Filter Operations

Kalman Gain Matrix

Calculation

INS Error
Kn
Corrections Linearized Dynamic
Error Model
.
INS Inertial ucn zRes n
Navigation x = Ax
tn .
∫tn-1 x dt + u cn
Integration
Operations xn = xn-1 +

xn(+) = xn(-) + Kn zRes n

xn
Linearized
Observation Model
Inertially (Measurement)
Computed
Navigation
zn = Hn xn
Data
zn
Reference Mn Calculate Measurement
Device Compare Residual
Navigation
Data
zRes n = M n - zn

Observation
(Non-Linearized
Measurement)

Figure 1 - Inertial Navigation Aiding

In Figure 1, the inertially-computed/reference-device navigation data comparison M

("observation") is input to the Kalman filter where it is compared against a linearized
estimate of M, known as the "measurement" z. The equation for z is based on estimates
of expected errors (embodied in an error state vector column matrix x) generated by a
linearized dynamic error model of inertial navigation and reference device operations,
and how they couple into the measurement (through the "measurement matrix" H). (The
error state dynamic matrix A in Figure 1 defines the dynamics of how x propagate from

4
the last n cycle to the current n cycle.) The difference between the observation M and
estimated measurement z (the "measurement residual" zRes), is multiplied by a Kalman
gain matrix Kn to generate corrections to the Kalman filter error estimates. The control
vector uc formed from INS error estimates in x (including provisions for x computation
delay) is used to correct the INS by subtraction from the equivalent INS parameter data.
To account for the uc corrections applied to the INS, the uc vector is also used to update
the Kalman x error model for the applied INS error correction.

The Kn Kalman gain matrix in Figure 1 is computed at each n cycle from a statistical
model of the expected uncertainty in the Figure 1 linearized updating process, a function
of the error state covariance matrix P:

tn . .
∫tn-1
T T
Pn = Pn-1 + P dt P = A P + P A + GP QP GP

T T T -1 (1)
Kn = Pn(-) Hn Hn Pn(-) Hn + GMn RM GMn
T T T
Pn(+) = I - Kn Hn Pn(-) I - Kn Hn + Kn GMn RM GMn Kn

T
The P covariance is analytically defined as E xUncrnty xUncrnty where E is the expected
value operator and xUncrnty is the uncertainty in the error state estimate x compared with
the true value x. The covariance matrix measures how initial uncertainties in x (at the
start of Fine Alignment) are progressively reduced by the Figure 1 dynamic
estimation/updating process, and how unaccounted for noise effects (in x propagation
between updates and measurement updating) delay the convergence process. Noise
parameters incorporated in the (1) gain determination operations are the QP process noise
matrix that accounts for random INS error buildup between n cycles, the GP matrix that
couples the process noise into error state uncertainty components, the measurement noise
matrix RM that accounts for random errors in the observation and in calculation of the
measurement residual, and the GM matrix that couples the measurement noise into the
measurement residual components [3 - Sect. 15.1] and [4 - pp. 428].

The success of the Figure 1 process depends on the accuracy by which the Kalman
filter linearized models match the actual operations in the INS and reference navigation
device. An important element in this regard is the impact of linearization on the
measurement residual. Second order components in the Figure 1 observation vector M
are ignored in the Kalman filter linearized models, hence, will appear in the measurement
residual zRes and modify x through the Kalman gains. Since the gains do not account for
second order errors, the result will add unknown errors to x. To minimize the impact of
second order errors on Kalman filter performance, it has been previous practice to assure
that Coarse Alignment attitude errors are small enough that second order residuals
become negligible. However, in some applications, residual second order Kalman filter

5
modeling errors can still produce mis-estimation of INS errors under particular dynamic
conditions [5, 6].

Application of Kalman Aiding To Velocity Matching INS Alignment

Applications of the Figure 1 inertial aiding concept to INS Fine Alignment commonly
use velocity for the INS/reference-device data comparison (so-called "velocity matching"
alignment). The associated inertial navigation integration operations in Figure 1
(between Kalman filter applied uc control updates) are versions of the following:

N N tn N N N B N N
CBn = CBn-1 + ∫ tn-1CB dt CB = CB ωIB × - ωIN CB
N N N
ωIN = ωIE + ωEN (2)
N N tn N N N B N N N N
vn = vn-1 + ∫ t n - 1v dt v = CB aSF + gP - ωIN + ωIE × v

where

n = Subscript indicating value of the designated parameter at the Kalman filter

update cycle time.

B
ωIB = Angular rate of the strapdown inertial sensor B frame relative to the non-
rotating inertial I frame (IB subscript) measured in the B frame (superscript)
by INS gyros

N
CB = Direction cosine matrix that transforms vectors from the sensor body B
frame to the locally level navigation N frame.

N
ωIE = Angular rate of the earth fixed E frame relative to the non-rotating I frame
(IE) projected on N frame axes.

N
ωEN = Angular rate of the N frame relative to the E frame (EN) projected on N
frame axes.

vN = Velocity vector relative to the earth projected on N Frame axes.

B
aSF = Specific force (non-gravitational) acceleration measured in the strapdown
sensor B frame by INS accelerometers.

6
 N
gP = Plumb-bob gravity in the N frame that equals the sum of earth's gravitational
mass attraction plus earth's rotation centripetal acceleration effect. Defined
as such because gP lies along the direction of a plumb-bob under zero
velocity conditions.

Under dynamic velocity matching trajectory conditions, [5] shows that residual
second order Kalman filter modeling errors can produce mis-estimation of small initial
heading errors under mild maneuvering conditions. Reference [5] shows how the second
order error effects can be mitigated as part of Kalman gain matrix determination by
modeling them into the Figure 1 measurement and process noise structure (within the
Equation (1) QP , GP , RM and GM matrices). As an alternate, [6] describes a Fine
Alignment approach whereby second order errors can be minimized by using a modified
form of the traditional velocity matching observation equation.

Under the special case of quasi-stationary conditions (e.g., stationary ground

alignment of an INS in an aircraft), [3 - Sects. 6.1.2 & 15.2.1] shows how a velocity
matching observation can be structured to make large initial heading errors observable
within a linearized structure that has no significant second-order errors. The concept is
based on (2) under zero velocity conditions:

N N tn N N N B N N
CBn = CBn-1 + ∫ tn-1CB dt CB = CB ωIB × - ωIE × CB
N N (3)
N N tn N N N
vHn = vHn-1 + ∫ tn-1vH dt vH0 = 0 vH = - gP Z N CB uZN H

where

H = Subscript indicating horizontal components of the designated vector.

N
uZN = Unit vector along the upward N Frame Z axis (ZN).

N
gP Z N = Component of computed plumb-bob gravity gP along the N frame upward
Z axis (of negative sign).

In forming (3) from (2) it has been recognized that under stationary conditions, the initial
N B N
value of velocity v0H is zero, aSF in (2) is vertical to balance plumb-bob gravity, and ωEN
N
in (2) is proportional to velocity, hence, not present in (3). The computed vH in (3) then
becomes the Figure 1 observation (i.e., compared with the true zero value). The (3) form
allows the initial N frame to be nominally defined at any arbitrary heading, then used
N
during Fine Alignment to estimate the N frame horizontal components of earth rate ωIE.
Using the principle that horizontal earth rate points north, the estimated earth rate

7
components are then used at Fine Alignment completion to determine the orientation of
the selected N Frame relative to true north.

Under moving-base INS alignment conditions, the (3) simplified quasi-stationary

earth rate estimation concept cannot be implemented directly because of the non-zero
N
velocity terms in the general (2) form. Although the magnitude of initial velocity v0 in
(2) is known (from reference velocity input data magnitude), its vector direction has
uncertainty in the N frame because of the initially unknown heading angle between the
INS N frame and the reference data input data coordinate frame (identified herein as N*).
N N
The v0 error in (2) thereby becomes part of the inertially computed velocity v ,
N N N N N
generating error in ωEN (a function of v ), in CB from ωEN, in adding to v error through
N B N N N
CB coupling of aSF into v , and by v input in (2) to the v Coriolis acceleration (the last
N
term in v ). The traditional method for implementing (2) has been to allow for a heading
N
error component in CB for estimation in the Kalman filter linearized error model (in
N
addition to CB tilt misalignment from vertical), thereby leading to the requirement for
small initial tilt/heading errors (to minimize residual second order error effects). In most
applications, Coarse Alignment to small tilt errors is readily achievable over a fairly short
time period (e.g., from roll/pitch attitude inputs from another device, or by using INS
accelerometers as the measure of attitude to vertical under controlled non-accelerating
trajectory conditions). In some applications, Coarse Alignment to small heading error is
not practical. To accommodate large initial heading uncertainties in such applications,
traditional Kalman alignment structures need to be reformulated for negligible second
order error impact under dynamic conditions.

This article extends the (3) quasi-stationary Fine Alignment large initial heading error
approach to dynamic moving base conditions by using the [6] revised
observation/measurement as a base. The article begins with the horizontal form of the [6]
revised observation equation. Non-linear error equations are then developed for the
observation and the (2) inertial navigation equations within a Figure 1 Fine Alignment
structure. From the non-linear error equations, a Kalman filter compatible linearized
error version is then developed. Lastly, the impact of linearization approximations on
Fine Alignment performance is assessed by comparing the non-linear observation and
linearized measurement equations forming the Figure 1 measurement residual. It is
shown that by virtue of the [6] revised observation approach, all significant residual
second order errors are vertical, (i.e., not present on the horizontal measurement
residual), hence, have no impact on Fine Alignment performance. The article includes a
summary of the equations developed to implement the large heading angle velocity
matching alignment approach, including initialization of the (1) covariance matrix for
Kalman gain calculations.

For simplicity and clarity in the remainder of this article, "Velocity Matching Fine
Alignment" will be referred to as "Alignment" or "Kalman Alignment".

8
OBSERVATION EQUATION

The Observation Equation described in this article for the Kalman Alignment
observation input in Figure 1 is the horizontal component of [6 - Eq. (28)]:

N N N N*
Mn = vn - CN*n vRefn H (4)
where

N
Mn = Observation vector in N frame navigation coordinates.

N*
vRef = Velocity relative to the earth provided in N* frame coordinates by the
reference navigation device (see Figure 1) to the Kalman filter observation
N
block (for reference comparison with v in (2)).

N
CN* = Direction cosine matrix that transforms vectors from the N* to the N
N
frame. From the definition of N*, CN* represents a rotation around the
upward vertical.

Only the horizontal components in (4) are used in anticipation of potentially large second
error propagation into the vertical observation, thereby mitigating potentially significant
Kalman filter estimation errors in Figure 1. Note - For simplicity in this article, (4) does
N N
not allow for differences in physical location between the v and vRef navigation points
that would normally be included in an actual system design (e.g., due to physical
separation between the INS and reference navigation device under vehicle angular
motion, i.e., so-called "lever arm" effects [3 - Sect. 15.2.2.2]).

N
Since CN* is a rotation transformation around the vertical, (4) is

N N N N*
Mn = vHn - CN*n vRefH/n (5)

N
The vH vector in (5) is calculated for this application using the following form of (2)
between Kalman updates.

N N tn N
vHn = vHn-1 + ∫ tn-1vH dt
N N B N N N N
(6)
N
vH = CB aSF + gP - ωIN + ωIE × vH + vZN uZ H

N N
with CB determined by CB integration in (2), and where

9
 vZN = Vertical component of vN.

Because the N and N* frame definitions make their Z vertical axes parallel, it is
expedient during Alignment to set vZN in (6) to

vZN = vZN*Ref (7)

where

N*
vZN*Ref = Component of vRef along the N* frame vertical Z axis.

N
Additionally, to simplify error modeling, the gP gravity vector in (4) is first calculated in
N* coordinates based on reference aiding device input position data, and then
transformed to the N frame:

N N N*
gP = CN* gP Ref (8)

where
N*
gP Ref = Plumb-bob gravity calculated in N* coordinates using standard INS
computation techniques (e.g., [3 - Sects. 5.4 & 5.4.1]), but based on N*
frame navigation data provided to the Kalman alignment process by the
reference navigation device.

Similarly, for the angular rate terms in (2) and (6):

N* N* N* N*
ωIE = ωIERef ωEN = ωENRef
N N* N N* (9)
N N
ωIE = CN* ωIE ωEN = CN* ωEN
where

N* N*
ωIERef, ωENRef = Angular rates ωIE, ωEN calculated in N* coordinates using
standard INS computation techniques (e.g., [3 - Sects. 4.1.1 & 5.3]), but
based on N* frame navigation data provided to the Kalman alignment
process by the reference navigation device.

N
The N relative to N* attitude in (5) represented by CN*, is a heading rotation (about
N
the vertical) from N* to N through angle β . Thus, from [1 - Eq. (3.2.2.1-4)], CN* in (5) is

N N N N
CN* = I + sin β uZN × + (1 - cos β) uZN × uZN × (10)

10
where

β = Constant angle measured positive around the upward defined N and N* frame
Z axes.

I = Identity matrix.

N
Note in (10) that CN* is represented by the two scalar parameters sin β and cos β . The
error in these parameters will form part of the errors to be estimated by the Kalman
alignment filter. This is directly analogous to the [3 - Sects. 6.1.2 & 15.2.1] stationary
alignment technique that uses horizontal earth rate components to represent heading,
equaling horizontal earth rate magnitude multiplied by the sine and cosine of N frame
heading angle from north.

Recognizing that the N frame selection maintains it at a fixed β alignment orientation

N
relative to the N* frame, the CN* matrix is constant, hence, between Figure 1 Kalman
updates,

d d
cos β = 0 sin β = 0 (11)
dt dt

It is also to be noted that a more general treatment would allow for differences between N
N
and N* angular rates by representing CN* as a heading rotation (about the vertical) from
N* to N through a time changing angle α followed by the previously defined constant
angle β . The α angle would be used to account for the difference in vertical rotation
rates that may exist between the N and N* frames (e.g., a GPS type geographic local
level east/north/up navigation frame implementation for frame N* versus a wander
azimuth local level navigation N frame implementation [3 - Sect. 2.2]). However, since
design of the Kalman alignment process allow selection of the N frame during alignment,
it is easily set to be the same as N*. This simplifies the analytical development, allowing
the N angular rate to be equated directly to the N* rate calculated from N* navigation
parameter inputs (as previously shown).

OBSERVATION AS A FUNCTION OF SYSTEM ERRORS

Kalman filter theory requires that observation (5) be an unbiased error measurement
so that the equivalent error free form of (5) is

N N N N*
Mn = vHn - CN*n vRefH/n = 0 (12)

Substituting for error definitions between (5) and (12) finds for (12) with (5):

11
 N N N N N N* N*
Mn = vHn - ΔvH - CN*n - ΔCN* vRefH/n - δvRef =0
n n H/n
N N N N* N N*
= vHn - ΔvH - CN*n vRefH/n + CN*n δvRef
n H/n (13)
N N* N N*
+ ΔCN* vRefH/n - ΔCN* δvRef
n n H/n
N N N N* N N* N N*
= Mn - ΔvH + CN*n δvRef + ΔCN* vRefH/n - ΔCN* δvRef
n H/n n n H/n

N N N* N N N*
in which the ΔCN*, ΔvH, and δvRefH errors in CN*, vH, and vRefH are defined as

N N N*
ΔCN* _ ΔvH _ δvRefH _
N N N N N* N*
= CN* - CN* = vH - vH = vRefH - vRefH (14)

N N
The Δ large error assignments to ΔCN* and ΔvH are made because of the large initially
N
unknown heading error between N and N*, and (as will be apparent subsequently), vH is
initialized at the start of Kalman alignment using N* frame velocity data.

Then, from (13):

N N N N* N N* N N*
Mn = ΔvHn - CN*n δvRefH/n - ΔCN*n vRefH/n + ΔCN*n δvRefH/n (15)

The linearized form of (15) (including linearized inputs) will form the basis for the
measurement equation used in forming z for Figure 1.

Inputs To Observation Equation (15)

N
The ΔCN* term in (15) is derived from the equivalent of (10) - (11) for the true value
N
of CN*:
N N N N
CN* = I + sin β uZN × + (1 - cos β) uZN × uZN × (16)

d d
cos β = 0 sin β = 0 (17)
dt dt

N
From (14), the ΔCN* error in (15) is the difference between (10) - (11) and (16) - (17):

N N N N N N
ΔCN* = CN* - CN* = Δsin β uZN × - Δcos β uZN × uZN ×
(18)
Δcos β _= cos β - cos β Δsin β _= sin β - sin β

12
with, between Figure 1 Kalman updates,

d d
Δcos β = 0 Δsin β = 0 (19)
dt dt

N
The ΔvH velocity error term in (15) includes Figure 1 control updates uc and,
between updates, the integral of the horizontal velocity error rate:

tn N
+ ∫ t ΔvH dt
N N
ΔvH = ΔvH (20)
n n-1 n-1

N N
in which ΔvH is the horizontal component of the Appendix A derived Δv general
velocity error rate:
N N B N N N
Δv ≈ CB δaSF + aSF × γ + δgP
N N N N N (21)
- ΔωIN + ΔωIE × v - ωIN + ωIE × ΔvN
1 N N N N B N N N
- aSF × γ × γ - CB δaSF × γ + ΔωIN + ΔωIE × ΔvN
2
in which
N N N N N N
ΔωIE _= ωIE - ωIE ΔωIN _= ωIN - ωIN
(22)
N N
δgP _= gP - gP δaSF _= aSF - aSF
N N N N

and where
N N
γ = Small angular error vector in the CB attitude matrix (defined analytically in
Appendix A).

The horizontal component of (21) for (20) is derived in Appendix B. The derivation
follows from (7) and (9), showing that for this Kalman alignment approach, the Δω and
Δv terms in (21) are horizontal:

N N N* N N N*
ΔωIEH = ΔCN* ωIEH ΔωENH = ΔCN* ωENH
N N N (23)
ΔωINH = ΔωIEH + ΔωENH
N
ΔvN = ΔvH

Applying (23) in (21), Appendix B then finds for horizontal velocity error rate in the (20)
integrand:

13
 N B N N N
CB δaSF + aSF × γ + δgP
N N N N N
ΔvH = + uZ × vZN ΔωINH + ΔωIEH - ωINZ N + ωIEZ N ΔvH (24)
1 N N N N B N
- aSF × γ × γ - CB δaSF × γ
2 H

N N N
Note that the second order ΔωIN + ΔωIE × Δv Coriolis term in (21) has vanished in
N N N
(24) because from (23), it equals ΔωINH + ΔωIEH × ΔvH which is vertical. This is a
N N
significant finding because, due to the large nature of its errors, ΔωIN + ΔωIE × ΔvN can
N N N
be as large as the ωIN + ωIE × v term in the (2) and (6) velocity rate equations.
Finding it to be vertical means that its large second order nature will not cause an error in
the linearized model of (24) used in the Figure 1 Kalman filter design. Thus, it will not
impact the Kalman filter measurement residual in Figure 1, and not lead to error mis-
estimation. This is also a primary reason for selecting the horizontal form for observation
Equation (4), i.e., to exclude the large second order vertical component of
N N
ΔωIN + ΔωIE × ΔvN in the velocity error rate integral.

N
Based on the error in (8), the δgP term in (24) becomes

N N N*
δgP = ΔCN* gP Ref/H (25)

N* N*
in which the minor gravity modeling error in gP Ref/H has been neglected. The gP Ref/H
vector in (25) would then be calculated using standard techniques (e.g., [3 - Sects. 5.4 &
N*
5.4.1] using reference input position data. Note that gP Ref/H is generally very small, on
the order of one micro-g per thousand feet of altitude - The dominant one g component of
N
gravity component is vertical. However, because of the potentially large value for ΔCN*
in this application, (25) is included in the (21) velocity error rate model (i.e., not
neglected) for Kalman alignment filter design, to properly account for its presence in the
Kalman filter observation equation input.

N
The γ attitude error term in (24) includes Figure 1 control updates uc and, between
N
updates, the integral of the γ attitude error rate derived in Appendix C:

N N B N N N 1 N B N N
γ ≈ - CB δωIB - ωIN × γ + ΔωINH + CB δωIB + ΔωINH × γ (26)
2
N N tn N N N
γn = γn-1 + ∫ t γ dt γ0 = γH0 (27)
n-1

14
where

0 = Subscript indicating the value of the designated parameter at the start of Kalman
alignment.

N
Note that the initial value for γ in (27) is horizontal (i.e., zero vertical component). This
is because the basic definition for the N Frame is to have a nominal initial heading, with
initial heading misalignment accounted for as misalignment of the N frame relative to the
N* frame.

SUMMARY OF EQUATIONS FOR KALMAN ALIGNMENT IMPLEMENTATION

The Figure 1 operations for Kalman alignment are or are derived from (1), (2), (5) -
(11), (15), (18) - (20), and (23) - (27) as summarized next.

The Figure 1 INS Inertial Navigation Integration Operations For Kalman alignment
between updates are (2) and (6) - (11), resequenced and renumbered next:

d d
cos β = 0 sin β = 0 (28)
dt dt

N N N N
CN* = I + sin β uZN × + (1 - cos β) uZN × uZN × (29)

N* N* N* N*
ωIE = ωIERef ωEN = ωENRef
N N* N N* (30)
N N
ωIE = CN* ωIE ωEN = CN* ωEN

N N N N N B N N
ωIN = ωIE + ωEN CB = CB ωIB × - ωIN CB
N N tn N (31)
CBn = CBn-1 + ∫ tn-1CB dt

vZN = vZN*Ref (32)

N N N*
gP = CN* gP Ref (33)

N N B N N N N N
vH = CB aSF + gP - ωIN + ωIE × vH + vZN uZ H
N N tn N (34)
vHn = vHn-1 + ∫ tn-1vH dt

15
 The Observation Equation For the Kalman alignment Compare operation in Figure 1
is (5):

N N N N*
Mn = vHn - CN*n vRefH/n (35)

with
N
CN*0 = I (36)

The Linearized Dynamic Error Model operations between Kalman updates in Figure
1 for Kalman alignment are the linearized forms of (18) - (20) and (23) - (27),
resequenced and renumbered next:

d d
Δcos β = 0 Δsin β = 0
dt dt (37)
N N N N
ΔCN* = Δsin β uZN × - Δcos β uZN × uZN ×

N N N* N N N*
ΔωIEH = ΔCN* ωIEH ΔωENH = ΔCN* ωENH
(38)
N N N
ΔωINH = ΔωIEH + ΔωENH

N N N
N B N (39)
γLin = - CB δωIB - ωIN × γLin + ΔωINH

N N N
tn
γLinn = γLinn-1 + ∫ tn-1γLin dt (40)

N N N*
δgP = ΔCN* gP Ref/H (41)

N B N N N
CB δaSF + aSF × γLin + δgP
N
ΔvHLin = N N
(42)
N N
+ uZ × vZN ΔωINH + ΔωIEH - ωINZ N + ωIEZ N ΔvHLin H

tn N
ΔvHLin/n = ΔvHLin/n-1 + ∫ tn-1ΔvHLin dt
N N
(43)

where

Lin = Subscript indicating linearized form of designated parameter.

16
 B B
The error parameters in (37), (39), (42), and for the δωIB and δaSF sensor error
components, constitute the estimated error state vector x in Figure 1. Reference [3 - Eqs.
(12.5-6) & (12.5-12)] show that the estimated sensor error components can be modeled as

B B B B
δωIB = δKScal/Mis ωIB + δKBias δaSF = δLScal/Mis aSF + δLBias (44)

where

δKScal/Mis, δLScal/Mis = Estimated gyro and accelerometer scale-factor-

error/misalignment error matrices.

δKBias, δLBias = Estimated gyro and accelerometer bias error vectors.

The error parameter coefficients in (37), (39), (42), and (44) constitute the error state
dynamic matrix A in Figure 1.

The measurement equation for the Kalman alignment Linearized Observation Model
operation in Figure 1 is the linearized form of (15):

N N N N* N N*
zn = ΔvHLin/n - CN*n δvRefH/n - ΔCN*n vRefH/n (45)

where

z = The Measurement (see Figure 1).

The error parameter coefficients in (45) constitute the measurement matrix H in Figure 1.

The Kalman gain matrix computations for the Kalman alignment Linearized
Observation Model operation in Figure 1 are from (1):

tn . .
∫tn-1
T T
Pn = Pn-1 + P dt P = A P + P A + GP QP GP

T T T -1 (46)
Kn = Pn(-) Hn Hn Pn(-) Hn + GMn RM GMn
T T T
Pn(+) = I - Kn Hn Pn(-) I - Kn Hn + Kn GMn RM GMn Kn

Note that the error parameters in (37) - (46) are shown to be estimated by the Kalman
filter (indicated with a ( ) notation), in contrast with the true ( ) values. Note also that
(37) - (46) are functions of error terms and parameters containing errors, the latter
available in the INS computer. This contrasts with the more typical error equations in

17
which the navigation parameters are represented by their ideal values. Equations (37) -
(46) are in the form that would actually be implemented in the INS for the Kalman filter
used to estimate the error parameters during Kalman alignment. This form arises because
of the general derivation process followed in this article (and in [5, 6]) by which idealized
error free equations were modified to be functions of computed minus error parameters.

KALMAN ALIGNMENT INITIALIZATION

Navigation Data Initialization

N
The estimated attitude matrix CB in (31) is initialized prior to Kalman alignment
initiation at a Coarse Alignment determined value with arbitrary heading relative to
north. Reference [3 - Sect. 6.1.1] illustrates a rapid Coarse Alignment method under
stationary conditions. A subsequent article will describe how the equivalent fast Coarse
Alignment can be performed under dynamic moving base conditions.

N
The estimated velocity vector vH in (34) is initialized at the horizontal velocity value
provided in N* coordinates by the reference navigation device:

N N*
vH0 = vRefH/0 (47)

where it is recognized that the heading angle β between the N and N* frames is initially
unknown, hence, assumed to be zero.

Estimated Error State Initialization

All error parameters in (37) - (45) are unknown at the start of Fine Alignment, hence,
their estimated values within the Figure 1 estimated error state vector x would be
initialized at zero.

Covariance Matrix Initialization

The estimated covariance matrix P in Figure 1 is initialized based on the uncertainty

in the x estimated error states:

χ _= x - x (48)

with the covariance matrix P defined as

_ T
P=E χχ (49)

18
where

χ = Uncertainty in x.

E = Expected value operator.

For the Δsin β heading error parameter in (37), the χ component derives from β being
initially unknown and equally likely to be plus or minus so that sin β 0 = 0. Then from the
(18) definition for Δsin β 0,

Δsin β 0 = sin β 0 - sin β 0 = - sin β 0 (50)

Since x0 = 0 as stated previously, the initial uncertainty in Δsin β is zero, and the
uncertainty in Δsin β is from (50):

χ Δsinβ0 = Δsin β 0 - Δsin β 0 = sin β 0 (51)

Similarly, for Δcos β ,

Δcos β 0 = - cos β 0 χΔcosβ0 = cos β 0 (52)

Assuming all values for β 0 have equal likelihood over the - π to + π range of N* to N
possible headings, the corresponding (51) - (52) initial covariance elements then become

1 π
PΔsinβ Δsinβ = E sin2β0 =
0
∫ - π sin2β0 dβ0 dt = 1
2π 2
1 π
PΔcosβ Δcosβ 0 = E cos2β0 = ∫ - π cos2β0 dβ0 dt = 1 (53)
2π 2
PΔsinβ Δcosβ = PΔcosβ Δsinβ = E sin β0 cosβ0
0 0
1 π
= ∫ - π sin β0 cosβ0 dβ0 dt = 0
2π
N
With velocity initialized as in (47), the initial uncertainty in vH0 becomes a function
N
of the unknown β 0 between N and N*. Recognizing the true value of vH0 to be
N N* N
CN*0 vRefH/0, the (14) error definition for ΔvH0 then finds with (47) and linearization:

19
 N N N N N* N N*
ΔvH0 = vH0 - vH0 = CN*0 vRefH/0 - CN*0 vRefH/0
N N N* N N* N*
= CN*0 + ΔCN*0 vRefH/0 - CN*0 vRefH/0 - δvRefH/0
N N* N N* (54)
= ΔCN*0 vRefH/0 + CN*0 δvRefH/0
N N* N N N*
= ΔCN*0 vRefH/0 + CN*0 - ΔCN*0 δvRefH/0
N N* N N*
≈ ΔCN*0 vRefH/0 + CN*0 δvRefH/0

N
Because all error state estimates are initially zero, CN*0 = I and
N N N N N
ΔCN*0 = CN*0 - CN*0 = I - CN*0 . Then (54) obtains for the linearized ΔvH0 uncertainty:

N N N N* N*
χ Δv H/0 = ΔvH - ΔvH = - I - CN* vRefH/0 - δvRef (55)
0 0 0 H/0

From (16),
N N N N
I - CN*0 = - sin β 0 uZN × - (1 - cos β 0) uZN × uZN × (56)

so that (55) becomes

N N* N* N*
χ Δv H/0 = sin β 0 uZN × vRefH/0 - (1 - cos β 0) vRefH/0 - δvRefH/0 (57)

With (57), the horizontal velocity error initial covariance then is

T
PΔv H/0 Δv H/0 = E χ Δv H/0 χ Δv H/0
N* N* T
= uN N 2
ZN × vRefH/0 uZN × vRefH/0 E sin β 0 (58)
N* N* T T
+ vRefH/0 vRefH/0 E 1 - cos β0 2 + E δvN* N*
RefH/0 δvRefH/0

N*
in which is has been assumed that β 0 and δvRefH/0 are uncorrelated. Over the - π to + π
range of N* to N equally likely β 0 headings,

E sin2β0 = 1 E 1 - cos β0 2 = E 1 - 2 cos β0 + cos2β0 = 3 (59)

2 2

Thus, (58) becomes

20
 1 N N* N N* T
PΔv H/0 Δv H/0 = uZN × vRefH/0 uZN × vRefH/0
2
T
(60)
3 N* N* N* N* T
+ vRefH/0 vRefH/0 + E δvRef δvRef
2 H/0 H/0

With (51) - (52) and (57), the initial cross-correlations between χ Δv H/0, χ Δsinβ0 , and
χ Δcosβ0 are obtained similarly:

PΔv H Δsinβ = E χ Δv H/0 χΔsin0 = E χ Δv H/0 sin β 0

0
N N* 1 N N*
= uZN × vRefH/0 E sin2β 0 = uZN × vRefH/0
2
T T
(61)
PΔsinβ ΔvH 0 = E χ Δsin0 χ Δv H/0 = E sin β 0 χ Δv H/0
T T
E sin2β0 = 1 uN
N N* N*
= uZN × vRefH/0 ZN × vRefH/0
2

PΔv H Δcosβ = E χ Δv H/0 χΔcos0 = E χ Δv H/0 cos β 0

0
N* 1 N*
= vRefH/0 E cos2β 0 = vRefH/0
2
T T
(62)
PΔcos Δv H 0 = E χ Δcos0 χ Δv H/0 = E cos β 0 χ Δv H/0
N* T 1 N* T
= vRefH/0 E cos2β 0 = vRefH/0
2
N
The initial uncertainty in attitude error γ recognizes that from the definition of the N
N N
frame, the vertical component of γ is zero, hence, the initial uncertainty in γ is
horizontal, and

N N N N N
χ γ 0 = γ0 - γ0 = γ0 - γH0 = - γH0
(63)
T N N T 2
Pγ 0 γ 0 = E χγ 0 χγ 0 =E γH0 γH0 = Ixy σγ H/0

where

Ixy = Identity matrix with zero for the lower diagonal element.

σγ H/0 = Standard deviation of the Coarse Leveling tilt uncertainty, assumed

uncorrelated between horizontal axes.

The remaining sensor error covariance elements are uncorrelated between axes.

21
NOISE PARAMETERS

The Kalman Alignment filter linearized dynamic error models in Figure 1 derive from
the general linear forms [3 - Sect. 15.1]:

x = A x + G P nP zn = Hn xn + GMn nMn (64)

compared to the Figure 1 estimation equivalents

x=Ax zn = Hn xn zResn = Mn - zn
(65)
xn(+) = xn(-) + Kn zResn

The uncertainty equations corresponding to (65) are derived from the (48) uncertainty
definition. For uncertainty propagation between Kalman filter n cycle updates (and
linearization):

χ = x - x = A x - A x - GP nP ≈ A x - A x - GP nP = A χ - GP nP (66)

For the error state updating operation (with linearization), at the Kalman n cycles:

zResn = Mn - zn ≈ zn - zn = Hn xn + GMn nMn - Hn xn(-)

(67)
≈ Hn xn + GMn nMn - Hn xn(-) = - Hn χ n(-) + GMn nMn
From (65),
xn(+) = xn(-) + Kn zResn
(68)
xn(+) - xn = xn(-) - xn + Kn zResn

so that with (67)

χn(+) = χn(-) + Kn zResn
= χn(-) + Kn - Hn χn(-) + GMn nMn (69)
= I - Kn Hn χn(-) + Kn GMn nMn

Summarizing for the (66) and (69) uncertainty propagation and update equations:

χ = A χ - GP nP
(70)
χ n(+) = I - Kn Hn χ n(-) + Kn GMn nMn

By applying covariance definition (49) to (70), the equivalent P and Pn(+) covariance
forms of (46) are obtained. The QP , RM process/measurement noise terms in (46) are,

22
respectively, the nP process noise density matrix, and the nM measurement noise
T
covariance matrix E nM nM , as discussed next.

Process Noise

For the Kalman Alignment application, the QP process noise matrix in (46) is
B
produced by noise components in the (23) and (33) gyro/accelerometer error vectors δaSF
B
and δωIB. From [3 - Eqs. (12.5-6) & (12.5-12)], the actual inertial sensor errors are
commonly modeled as:

B B
δωIB = δKScal/Mis ωIB + δKBias + δωRand
B B
(71)
δaSF = δLScal/Mis aSF + δLBias + δaRand

where

δωRand, δaRand = Gyro and accelerometer random noise error components.

The velocity and attitude error state uncertainties are

N N N N
χ Δv H = ΔvH - ΔvH χγ = γ - γ (72)

so that
N N N
N (73)
χ Δv H = ΔvH - ΔvH χγ = γ - γ

N N
Substituting (39) and (42) for γ , ΔvH and their equivalent true value form from the
linearized versions of (26) and (24), then finds from (73) with (44) and (71):

N N B
N B
χ γ = γ - γ = Aγ(woSens) χ + CB δωIB - δωIB
N
= Aγ χ + CB δωRand
N (74)
N N B B
χ Δv H = ΔvH - ΔvH = AΔv H(woSens) χ - CB δaSF - δaSF
N
= AΔv H χ - CB δaRand

For δωRand, δaRand typically modeled as white noise, the associated (46) elements in QP
would be along the diagonal and equal to the corresponding white noise densities [3 -
Sect. 15.1.2.1.1]. From (74), the corresponding GP process noise coupling matrix would

23
 N
be CB. Note, however, that because the transpose of a correctly computed direction
cosine matrix equals its transpose, and because QP is diagonal (uncorrelated process
noise elements in δωRand, δaRand), if the noise vector components have equal densities,
T
the GP QP GP term in (46) would reduce to GP .

It is also to be noted that sensor quantization noise present on the sensor outputs is
treated differently, predominantly modeled as random uncertainty in attitude (for gyro
quantization) and velocity (for accelerometer quantization) [3 - Sect. 12.5]. As such,
gyro quantization becomes predominantly a white noise input to the velocity error
uncertainty rate equation, modeled as a white noise density in the QP process noise
matrix [3 - Eq. (12.5-18)]. For accelerometer error, the comparable treatment models
quantization as white noise input to the position error uncertainty rate equation, not
implemented for the velocity matching alignment approach described herein. However,
accelerometer quantization noise does also become measurement noise for a velocity type
measurement (described in the next section). For the integrated velocity matching
alignment approach described in [3 - Sect. 15.2.2], accelerometer quantization would be
modeled as white process noise input to the integrated velocity matching measurement
integrator.

Measurement Noise

For the Alignment Kalman filter gain design, the measurement residual in Figure 1 is
from (45), (15), and [3 - Eq. (12.5-11)], with linearization

N N N N N N
zResn = Mn - zn ≈ zn - zn = - HwoΔvHn χ woΔvHn(-) - ΔvHn(-) - ΔvHn
(75)
N
= - Hn χ n(-) + CBn δυQuant
where
N
zn = Linearized form of MN N
n including linearized M n components.

Then, as in (69),
N
χ n(+) = χ n(-) + Kn zResn = χ n(-) + Kn - Hn χ n(-) + CBn δυQuant
N
(76)
= I - Kn Hn χ n(-) + Kn CBn δυQuant
where

δυQuant = accelerometer quantization noise.

For δυQuant in (76) typically modeled as a white vector sequence, the (76) covariance
equivalent in (46) sets the elements of measurement noise matrix RM equal to the

24
 T
δυQuant covariance: E δυQuant δυQuant . From (76), the corresponding GMn
N
measurement noise coupling matrix in (46) would be CB. Because the transpose of a
correctly computed direction cosine matrix equals its transpose, and because RM is
diagonal (uncorrelated measurement noise elements in δυQuant), if the noise vector
T
components have equal variances, the GMn RM GMn term in (46) would reduce to RM.

NAVIGATION MODE ENTRY AT ALIGNMENT COMPLETION

Kalman alignment completion is defined as the time when Δsin β is reduced to an

acceptable value (as manifested in the PΔsinβ Δsinβ covariance element of P). This
N
minimizes the error in CN* which then will accurately represent the heading orientation
of the N frame relative to the N* frame. Initialization of attitude for inertial navigation
N N
would proceed based on the values for CN* and CB at Kalman alignment completion.
N
For example, attitude for inertial navigation could be initialized by resetting the CB
matrix to have its Y axis parallel to the N* frame Y axis using:

N N T N
CB(+) = CN* AlnEnd CB(-) (77)

where
N N
CB(-) = CB at Kalman Alignment completion.

N N
CN* AlnEnd = CN* at Kalman Alignment completion.

N N N
CB(+) = CB to initiate inertial navigation after rotating CB(-) about the vertical so
that the N frame Y axis aligns with the N* frame Y axis.

N
Following navigation mode initialization at the end of alignment, the CB matrix would be
updated as in (2) using normal methods typically employed (e.g., with the N frame
N
defined as the azimuth wander type whereby the vertical component of ωIN would be set
to the vertical component of earth rate, as in [3 - Sect. 4.5]).

SECOND ORDER ERROR IMPACT ASSESSMENT

The impact of second order errors on the Kalman alignment process can be assessed
by analyzing the linear compared with the non-linear terms in the (15) observation
equation. To simplify the analysis, we will analyze (15) when the uc control vector is

25
zero. This converts the Kalman filter in Figure 1 to a pure estimator in which the errors
in the observation propagate in the normal manner without Kalman intervention. To
further simplify the analysis, sensor, reference input, and gravity error terms will be
neglected to obtain from (15), (19), (20), (24), (26), and (27) for the relevant equations:

N N N N 1 N N
γ ≈ - ωIN × γ + ΔωINH + ΔωINH × γ (78)
2

N t N
γ = γH0 + ∫ 0 γ dτ
N (79)

N N
aSF × γ
N N N N N
ΔvH ≈ + uZ × vZN ΔωINH + ΔωIEH - ωINZ N + ωIEZ N ΔvH (80)
1 N N N
- aSF × γ × γ
2 H
tn N
ΔvHn = ΔvH0 + ∫ 0 ΔvH dt
N N
(81)

d d
Δcos β = 0 Δsin β = 0 (82)
dt dt

N N N N*
Mn ≈ ΔvHn - ΔCN*0 vRefH/n (83)

N
Note in (83) that the initial value of ΔCN* is used based on (82) and the assumption of
analysis without zero error controls in Figure 1.

To analytically solve the equations, a first order Picard expansion approach is

N
employed in which the γ term in (78) is approximated by its initial value. Then (79)
integrates to

N t N N t N N t N
γ = γH0 + ∫ 0 γ dτ ≈ γH0 + ∫ 0 ΔωINH dτ + γH0 × ∫ 0 ωIN dτ
N

1 N t N (84)
- γH0 × ∫ 0 ΔωINH dτ
2

Using the first order Picard expansion approach in (80) finds with (84):

26
 N N
aSF × γ
N N N N N
ΔvH ≈ + uZ × vZN ΔωINH + ΔωIEH - ωZN + ωIE ΔvH0
1 N N N
- aSF × γH0 × γH0
2 H
N t N N t N
N
γH0 + ∫ 0 ΔωINH dτ + γH0 × ∫ 0 ωIN dτ
aSF × t
1 N N
- γH0 × ∫ 0 ΔωINH dτ
2
N N N N
(85)
= + uZ × vZN ΔωINH + ΔωIEH - ωINZ N + ωIEZ N ΔvH0
1 N N N
- aSF × γH0 × γH0
2 H
N t N N N t N
= aSFZ N uZ × γH0 + ∫ 0 ΔωINH dτ + aSF × γH0 × ∫ 0 ωIN dτ H
N

N N N N
+ uZ × vZN ΔωINH + ΔωIEH - ωINZ N + ωIEZ N ΔvH
0
1 N N t N 1 N N N
- aSF × γH0 × ∫ 0 ΔωINH dτ H - aSFH × γH0 × γH0
2 2

N N t N
The aSF × γH0 × ∫ 0 ωIN dt term in (85) expands as

N N t N
aSF × γH0 × ∫ 0 ωIN dτ
N N t N t
= aSFH + aSFZ N uZ × γH0 ×
N
∫0 ωINH dτ + ∫0 ωINZ N dτ uZ
N

N N t N N N t
= aSFH × γH0 × ∫ 0 ωINH dτ + aSFH × γH0 × ∫0 N
ωINZ N dτ uZ
N t N N t
+ aSFZ N uZ × γH0 × ∫ 0 ωINH dτ + aSFZ N uZ × γH0 × ∫0
N N N
ωINZ N dτ uZ
(86)
N N t N N N t
×∫ ∫ N
= aSFH × γH0 0 ωINH dτ + aSFH × γH0 × 0 ωINZ N dτ uZ
N t
+ aSFZ N uZ × γH0 ×
N
∫0 ωINZ N dτ uZ
N

N N t N N N t
= aSFH × γH0 × ∫ 0 ωINH dτ + aSFH × γH0 × ∫0 N
ωINZ N dτ uZ
t N
- aSFZ N ∫0 ωINZ N dτ γH0

so that
N N t N
aSF × γH0 × ∫ 0 ωIN dτ H
N (87)
N N t t N
= aSFH × γH0 × ∫ 0 ωINH dτ - aSFZ N ∫0 ωINZ N dτ γH0

27
 1 N N t N N
For the aSF × γH0 × ∫ 0 ΔωINH dt H term in (85), using (23) for ΔωINH and
2
N N
ΔCN* = ΔCN*0 due to no-control operation:

1 N N t N 1 N N t N*
aSF × γH0 × ∫ 0 ΔωINH dt H = aSFH × γH0 × ΔCN*0 ∫ 0 ωINH dτ
N
(88)
2 2
N N N N
Then using (87) - (88) in (85), with (23) for ΔωINH , ΔωIEH and ΔCN* = ΔCN*0 due to
no-control operation, the (81) integration yields
tn N
ΔvHn = ΔvH0 + ∫ 0 ΔvH dt
N N

t N
= ΔvH +
0
N
∫ 0n aSFZ N dt N
uZ × γH0
tn t N*
+ ΔCN*0 ∫ 0 aSFZ N uZ × ∫ 0 ΔωINH dτ dt
N N

tn N N t N tn t N (89)
+ ∫ 0 aSFH × γH0 × ∫ 0 ωINH dτ dt - ∫ 0 aSFZ N ∫0 ωINZ N dτ dt γH0
tn N* N*
+ uZ × ∫ 0 vZN ΔCN*0 ΔωINH + ΔωIEH - ωINZ N + ωIEZ N ΔvH0 dt
N N N

1 tn N N t N*
- ∫ 0 aSFH × γH0 × ΔCNN*0 ∫ 0 ωINH dτ dt
2
1 N tn N N
+ γH0 × ∫ 0 aSFH dt × γH0
2

Since the heading orientation between the N and N* frames is unknown at the start of
N N N
Kalman alignment, CN*0 = I, and without Figure 1 control updates, CN* = CN*0 = I.
tn N N N N N* N* N* N*
Thus, ∫ 0 aSFH = vHn - vH0 = CN*0 vRefH/n - vRefH/0 = vRefH/n - vRefH/0 and, with (54)
(neglecting reference velocity errors), (89) becomes

tn N
∫0
N N N* N
ΔvH = ΔCN* vRef + aSFZ N dt uZ × γH0
n 0 H/0
t t N*
+ ΔCN*
N
∫ n
0 0
aSFZ N uZ × ∫ 0 ΔωINH dτ dt
N

tn N N t N tn t N
+ ∫ 0 aSFH × γH0 × ∫ 0 ωINH dτ dt - ∫ 0 aSFZ N ∫0 ωINZ N dτ dt γH0
(90)
tn N* N*
+ uZ × ∫ 0 vZN ΔCN* ΔωINH + ΔωIEH - ωINZ N + ωIEZ N ΔvH dt
N N N
0 0
1 tn N N t N*
- ∫ 0 aSFH × γH0 × ΔCN*0 ∫ 0 ωINH dτ dt
N
2
1 N N* N* N
+ γH0 × vRefH/n - vRefH/0 × γH0
2

28
Finally, (90) is substituted into (83) to obtain for the observation:

N N N N*
Mn ≈ ΔvHn - ΔCN*0 vRefH/n
N* N* t N
N
= - ΔCN*0 vRefH/n - vRefH/0 + ∫ 0n aSFZ N dt N
uZ × γH0
t t N*
N
+ ΔCN* ∫ n
0 0
aSFZ N uZ × ∫ 0 ωINH dτ dt
N

tn N N t N tn t N
+ ∫ 0 aSFH × γH0 × ∫ 0 ωINH dτ dt - ∫0 aSFZ N ∫0 ωINZ N dτ dt γH0 (91)
tn N* N* N* N*
+ uZ × ∫ 0 vZN ΔCN*0 ωINH + ωIEH - ωINZ N + ωIEZ N ΔvH0 dt
N N N

1 tn N N t N
∫ ∫
N
- a SF H × γ H0 × ΔC N*0 0 ωINH dτ dt
2 0
1 N N* N* N
+ γH0 × vRefH/n - vRefH/0 × γH0
2

The last two terms in (91) constitute the non-linearities that are not accounted for in the
linearized Kalman filter equations. Their impact on performance can be assessed by
comparison with (91) linear terms having similar signatures.

2 N* N*
The last term in (91) is on the order of γH0 vRefH/n - vRefH/0 in magnitude. The
N N* N*
comparable linear term in (91) is ΔCN*0 vRefH/n - vRefH/0 . Clearly,
2 N* N* N* N*
γH0 vRefH/n - vRefH/0 is second order compared with ΔCN
N*0 vRefH/n - vRefH/0 , hence,
N N* N*
negligible in comparison. Similarly, compared with the linear ΔCN*0 vRefH/n - vRefH/0
term, the second to last term in (91) is on the order of
1 t N N* N*
γH0 ∫ 0 ωINH dτ ΔCN* vRefH/n - vRefH/0 in magnitude, hence, also negligibly small
N
2 0
t N
(particularly since ∫ 0 ωINH dτ is also small during Kalman alignment). Thus, the impact
of second order terms on the input observation should have negligible impact on
linearized Kalman alignment performance.

APPENDIX A - VELOCITY ERROR RATE EQUATION DERIVATION

The idealized error free form of the velocity rate in (2) is

N N B N N N
v = CB aSF + gP - ωIN + ωIE × vN (A-1)

The errors between the computed equivalents in (2) and (A-1) are defined as

29
 N N N N N N N
δCB _= CB - CB ΔωIE _= ωIE - ωIE ΔωIN _= ωIN - ωIN
N N
(A-2)
N N
δgP _= gP - gP δaSF _= aSF - aSF
N N N N

N N N
in which the errors in ωIE and ωIN are anticipated to be comparable in magnitude to ωIE
N
and ωIN due to large heading uncertainties in their (9) computation. In contrast, the error
N N
in gP is small relative to gP because it is primarily along the local vertical as is the
N
equivalent in N* frame coordinates, hence is largely unaffected by the CN*
transformation in its (8) computation. Based on the (A-2) definitions, (A-1) can be
expanded as:

N N N N B B
v - Δv = CB - δCB aSF - δaSF
N N N N N N N
+ gP - δgP - ωIN + ωIE - ΔωIN - ΔωIE × v - ΔvN
N B N B N N B N N
= CB aSF - δCB aSF - CB - δCB δaSF + gP - δgP (A-3)
N N N N N N
- ωIN + ωIE × v + ΔωIN + ΔωIE × v
N N N N
+ ωIN + ωIE × ΔvN - ΔωIN + ΔωIE × ΔvN

N
or with v from (2)

N N B N B N
Δv = δCB aSF + CB δaSF + δgP
N N N N N
- ΔωIN + ΔωIE × v - ωIN + ωIE × ΔvN (A-4)
N B N N
- δCB δaSF + ΔωIN + ΔωIE × ΔvN

N
The δCB error in (A-4) is derived from

N_ N N N NT N
δCB = CB - CB = I - CB CB CB (A-5)

N
Applying [3 - Eq. (3.5.2-8)] assigns the cause for the δCB error to misalignment of the N
frame from its nominal error free value. Identifying the misaligned N frame as N gives:

N N N N
CB = CB = CN CB (A-6)

or

30
 N N N
CB = CN CB (A-7)

hence, with (A-7) in (A-5),

N N N
δCB = I - CN CB (A-8)

N
Defining CN in terms of a rotation vector using the form of [3 - Eq. (19.1.3-3)] as a
model:
N N N N
CN = I + f1(γ) γ × + f2(γ) γ × γ ×
2 2 (A-9)
sin γ γ 1 - cos γ 1 γ
f1(γ) = =1- + f2(γ) = = - +
γ 3!
γ
2 2 4!
where
N N
γ = Rotation angle error vector associated with the CB matrix considering the N
frame to be misaligned, as projected on frame N axes.

Applying (A-9) in (A-8) yields with no approximations:

N N N N N
δCB = - f1 γ × + f2 γ × γ × CB (A-10)

N
Using (A-10) for δCB in (A-4) finds with no approximations:

N N B N B N N
Δv = CB δaSF + f1 CB aSF × γ + δgP
N N N N N
- ΔωIN + ΔωIE × v - ωIN + ωIE × ΔvN
(A-11)
N B N N N B N
- f2 CB aSF × γ × γ - f1 CB δaSF × γ
N N N B N N
+ ΔωIN + ΔωIE × ΔvN + f2 CB δaSF × γ × γ

Then substituting f1 and f2 from (A-9) and neglecting terms with third order error
products, (A-11) becomes:

N N B N N N
Δv ≈ CB δaSF + aSF × γ + δgP
N N N N N
- ΔωIN + ΔωIE × v - ωIN + ωIE × ΔvN (A-12)
1 N N N N B N N N
- aSF × γ × γ - CB δaSF × γ + ΔωIN + ΔωIE × ΔvN
2

31
Equation (A-12) is the error rate equation associated with the (2) velocity updating
process.

APPENDIX B - HORIZONTAL VELOCITY ERROR RATE EQUATION

DERIVATION

This appendix derives the horizontal form of the (A-12) general velocity error
expression for the Equation (20) integrand.

N N
The ΔωIE term in (A-12) is derived by expanding from the error free version of ΔωIE
in (9):
N N N* N N N N N* N*
ωIE = CN* ωIE = ωIE - ΔωIE = CN* - ΔCN* ωIE - δωIE
N N* N* N N*
(B-1)
N N
= CN* ωIE - ΔCN* ωIE - CN* - ΔCN* δωIE

N N*
Substituting ωIE from (9) in (B-1) and neglecting the minor δωIE error in the reference
derived data then finds

N N N*
ΔωIE = ΔCN* ωIE (B-2)

N N
which also shows the ωIE error to be of the large Δ type. Finally, because CN* is a
N
rotation around the vertical Z axis, ΔCN* has no Z axis components, and (B-2) simplifies
to
N N N N*
ΔωIE = ΔωIEH = ΔCN* ωIEH (B-3)

It can be shown similarly that

N N N N*
ΔωEN = ΔωENH = ΔCN* ωENH (B-4)

N N N N
Recognizing that ωIN satisfies ωIN = ωIE + ωEN (and similarly for the true value), from
N
(9) with (B-3) and (B-4), the error in ωIN for (A-12) is

N N N N N N
ΔωIN = ΔωIE + ΔωEN = ΔωIEH + ΔωENH = ΔωINH
N* N* N* (B-5)
N N
= ΔCN* ωIEH + ωENH = ΔCN* ωINH

32
 Neglecting the minor error in the reference data sets the error value in vertical
velocity equation (7) to zero so that ΔvN in (A-12) becomes

N
ΔvN = ΔvH (B-6)

Substituting the horizontal properties of (6) and (B-4) - (B-6) into (A-12), then finds
N
for ΔvH in (20):

N B N N N
CB δaSF + aSF × γ + δgP
N N N N N
ΔvH = + uZ × vZN ΔωINH + ΔωIEH - ωINZ N + ωIEZ N ΔvH (B-7)
1 N N N N B N
- aSF × γ × γ - CB δaSF × γ
2 H

Equation (B-7) is the horizontal velocity error rate integrated by (20) into the (15)
observation equation.

APPENDIX C - ATTITUDE ERROR RATE EQUATION DERIVATION

The idealized error free form of the attitude rate in (2) is

N N B N N
CB = CB ωIB × - ωIN × CB (C-1)

Substituting the (A-5) and (A-2) definitions into (C-1) yields:

N N N N B B N N N N
CB - δCB = CB - δCB ωIB - δωIB × - ωIN - ΔωIN × CB - δCB
N B N B N B N B
= CB ωIB × - CB δωIB × - δCB ωIB × + δCB δωIB × (C-2)
N N N N N N N N
- ωIN × CB + ωIN × δCB + ΔωIN × CB - ΔωIN × δCB

in which
B B B
δωIB _= ωIB - ωIB (C-3)

N
Substituting (2) for CB in (C-2) finds

N N B N B N B
δCB = CB δωIB × + δCB ωIB × - δCB δωIB ×
N N N N
(C-4)
N N
- ωIN × δCB - ΔωIN × CB + ΔωIN × δCB

33
 N N
The δCB term in (C-4) is the derivative of (A-10) with (2) for CB:

N N
N f1 γ × + f1 γ × N
δCB = - d N N N N CB
+ f2 γ × γ × + f2 γ × γ ×
dt
N N N N
- f1 γ × + f2 γ × γ × CB
(C-5)
N N
f1 γ × + f1 γ × N
=- d N N N N CB
+ f2 γ × γ × + f2 γ × γ ×
dt
N N N N B N N
- f1 γ × + f2 γ × γ × CB ωIB × - ωIN × CB

Then substituting (A-10) and (C-5) into (C-4) obtains after rearrangement:

N N
f1 γ × + f1 γ ×
d N N N N
+ f2 γ × γ × + f2 γ × γ ×
dt
N N N N
- f1 γ × + f2 γ × γ × ωIN × (C-6)
N B N N N N B
= - CB δωIB × - f1 γ × + f2 γ × γ × CB δωIB ×
N N N N N
- ωIN × f1 γ × + f2 γ × γ × + ΔωIN ×
N N N N
+ ΔωIN × f1 γ × + f2 γ × γ ×

From its definition, the cross-product matrix operator form of a vector is skew-
symmetric (i.e., the element in row i column j equals the negative of the element in row j
column i). It follows that the transpose of a cross-product operator matrix equals the
negative of the matrix. Based on this property, the transpose of (C-6) is:

N N
- f 1 γ × - f1 γ ×
d N N N N
+ f2 γ × γ × + f2 γ × γ ×
dt
N N N N
+ ωIN × - f1 γ × + f2 γ × γ × (C-7)
N B N B N N N
= CB δωIB × + CB δωIB × - f1 γ × + f2 γ × γ ×
N N N N N
+ - f1 γ × + f2 γ × γ × ωIN × - ΔωIN ×
N N N N
- - f1 γ × + f2 γ × γ × ΔωIN ×

34
Subtracting (C-7) from (C-6), dividing by 2, and rearranging obtains

N N B N
f1 γ × = - CB δωIB × + ΔωIN ×
N 1 N 1 N B N
+ f1 - ωIN + ΔωIN + CB δωIB × γ ×
2 2
N N 1 N 1 N B
- f1 γ × - ωIN + ΔωIN + CB δωIB × (C-8)
2 2
1 N N N N B
+ f2 γ × γ × ΔωIN - CB δωIB ×
2
1 N N B N N N
+ f2 ΔωIN - CB δωIB × γ × γ × - f1 γ ×
2

or, after applying [3 - (3.1.1-22)]:

N N B N
f1 γ × = - CB δωIB × + ΔωIN ×
N 1 N 1 N B N
+ f1 - ωIN + ΔωIN + CB δωIB × γ ×
2 2 (C-9)
1 N N N N B
+ f2 γ × γ × ΔωIN - CB δωIB ×
2
1 N N B N N N
+ f2 ΔωIN - CB δωIB × γ × γ × - f1 γ ×
2

But from (A-9) and (C-9),

2
γ 2 γ
f1 = 1 - + = order of γ f1 = - γ+ = order of γ ΔωIN (C-10)
3! 3

Hence,
N N B N
γ × = - CB δωIB × + ΔωIN ×
N 1 N 1 N B N
+ - ωIN + ΔωIN + CB δωIB × γ × (C-11)
2 2
2 2
+ order of γ ΔωIN + order of γ δωIB

Therefore,

N N B N 1 N 1 N B N N
γ = - CB δωIB - ωIN - ΔωIN - CB δωIB × γ + ΔωIN + (C-12)
2 2

or with (B-5):

35
 N N B N N N 1 N B N N
γ ≈ - CB δωIB - ωIN × γ + ΔωINH + CB δωIB + ΔωINH × γ (C-13)
2
Equation (C-13) is the error rate equation associated with the (2) attitude updating
processes performed in the INS computer.

REFERENCES

[1] Gelb, A., Applied Optimal Estimation, The MIT Press, Cambridge Mass., London,
England, 1978.

[2] Brown, G.B. and Hwang, P.Y.C., Introduction To Random Signals And Applied
Kalman Filtering, John Wiley & Sins, Inc., 1997.

[3] Savage, P.G., Strapdown Analytics, Second Edition, Strapdown Associates, Inc.,
2000, available for purchase at www.strapdownassociates.com.

[4] Savage, P.G., Introduction To Strapdown Inertial Navigation Systems, Strapdown

Associates, Inc., 1981 - 2010, available for purchase at
www.strapdownassociates.com.

[5] Savage, P.G., " Mitigating Second Order Error Effects In Linear Kalman Filters Using
Adaptive Process And Measurement Noise", SAI-WBN-14001, Strapdown
Associates, Inc., May 16, 2014, free access available at
www.strapdownassociates.com.

[6] Savage, P.G., " Modifying The Kalman Filter Measurement To Mitigate Second
Order Error Amplification In INS Velocity Matching Alignment Applications ",
SAI-WBN-14005, Strapdown Associates, Inc., July 15, 2014, free access
available at www.strapdownassociates.com.

36