INTRODUCTION
In a Strapdown Inertial Navigation System (SINS) used in superspeed vehicles,
inertial instruments (threegyros and threeaccelerometers) are rigidly attached
to the vehicle, which suffer from the high dynamics environment of vehicle movement
directly. In order to keep tracking of body attitude and velocity accurately
in severe vibratory environment, the ability of the strapdown system’s
attitude and velocity algorithm may be the critical factor in determining its
performance, if accurate navigation is to be achieved (Lee
et al., 1990). Therefore, the realtime implementation of attitude
and velocity updating algorithm has played a critical role in strapdown inertial
navigation systems.
Nowadays, commonly used attitude updating algorithms for most modernday strapdown
inertial navigation systems was the quaternion method based on the rotation
vector. The quaternion method was quite popular used in reality due to the advantages
of its nonsingularity, simplicity and computational efficiency. It has been
proven that the quaternion method based on the rotation vector can effectively
suppress and even eliminate the noncommutativity error, which is one of the
major error sources in attitude updating algorithms. Ignagni
(1996) proposed the concept of distance between the cross products leading
to optimal accuracy characteristics and minimum computational throughput required
in coning compensation algorithm. The algorithms utilizing the enhancement concept
have the simplest form in generating many distinct gyrosample cross products
and optimal coefficients to minimize the coning compensation error (Ignagni,
1996; Lee et al., 1990).
Lee et al. (1990) proposed the generic equivalency
between the coning integrals in attitude algorithm and the sculling integrals.
The equivalency allows us to convert a coning compensation approximation algorithm
in an attitude updating algorithm to its sculling approximation algorithm counterpart
in velocity updating algorithm using a simple mathematical formula. In this
study, a general scheme for implementation of attitude and velocity SINS algorithm
in realtime environment was presented. This scheme of realtime implementation
was formulated in a recursive form using two timerate executions and using
only integer arithmetic.
CONING AND SCULLING COMPENSATION APPROXIMATION ALGORITHMS
In the development of the coning and sculling compensation algorithms, it is usual that each major interval is divided into a number of minor intervals, each in turn being divided into a number of subminor sample intervals over which the gyro incremental angle and accelerometer incremental velocity are measured, as shown in Fig. 1. This is referred to as two timerate structure that updates the coning/sculling compensation in every minor interval, whereas the attitude and velocity of vehicle are updated only once in every major interval.
The coning compensation ΔΦ_{m} over the minor interval from
τ_{m1} to τ_{m} is defined as the integral of the
noncommutativity rate vector over the interval:

Fig. 1: 
Intervals associated with coning and sculling compensation,
ΔT: Data sample interval 
Where:
and ω is the angular rate vector, the components of which are sensed by
a set of three gyros. According to the concept of distance between the cross
products, cross products with equal distance behave exactly the same for coning
inputs (Lee et al., 1990; Park
et al., 1999). Taking advantage of this property, a generalized algorithmic
approximation to the coning integral defined by Eq. 1 consists
of the sum of all possible cross products. Those products are formed from the
gyro incremental angle for the n subminor sample intervals that make up the
minor computational interval of rotation vector (Ignagni,
1996) such as:
where, n is the number of gyro incremental angle sample for mth minor interval; p is the number of gyro incremental angle sample for (m1)th minor interval; Δθ_{m}(i) is ith gyro incremental angle sample for mth minor interval; Δθ_{m1}(j) is jth gyro incremental angle sample for (m1)th minor interval and k_{ni} and k_{2nj} are the constant coefficients of Δθ_{m}(i) and Δθ_{m1}(j) for any combinations of n and p, respectively.
The sculling compensation Δv_{sculm} over the minor interval from τ_{m1 }to τ_{m} is defined as follows:
Where:
By comparing the sculling compensation algorithm in Eq. 3
with the coning compensation algorithm in Eq. 1, the equivalency
between the two was demonstrated (Ignagni, 1998; Roscoe,
2001). So, using a simple mathematical formula that converts the coning
compensation approximation algorithm in Eq. 2 to the sculling
compensation approximation algorithm counterpart, a generalized algorithmic
approximation to the sculling compensation algorithm defined by Eq.
3 is derived as follows:
where Δv_{m}(i) is ith accelerometer incremental velocity sample for mth minor interval, Δv_{m1}(j) is jth accelerometer incremental velocity sample for (m1)th minor interval and the other as what defined in Eq. 5. Especially, the constant coefficients of Δv_{m}(i) and Δv_{m1}(j) are the same as the constant coefficients of Δθ_{m}(i) and Δθ_{m1}(j), respectively.
OPTIMAL COEFFICIENTS OF CONING/SCULLING COMPENSATION ALGORITHM
Because the coefficients of coning compensation approximation algorithm in Eq. 2 are the same as coefficients of sculling compensation algorithm in Eq. 4, so we only need to derive the coefficients of coning compensation approximation algorithm. In order to derive the coefficients of coning compensation approximation algorithm in Eq. 2, a pure coning motion is selected as base motion of vehicle and is defined by the angular rate vector as:
where, I and J are the unit vectors along two orthogonal axes of the vehicle. Applying the coning motion in Eq. 5 to Eq. 1, we obtain:
where, T is the minor interval. For the coning environment defined by Eq. 5, the gyro incremental angular vector over a subminor sample interval of duration ΔT from t_{k1} to t_{k} is:
Taken over by different subminor sample intervals, the cross product of two incremental angular vectors Δθ(i)xΔθ(j) becomes:
where λ = Ω·ΔT. It can be seen from Eq.
8 that the value of the cross product of two incremental angular vectors
is independent of the absolute time but depends only on a multiple of the subminor
sample interval ΔT (Lee et al., 1990; Park
et al., 1999).
Substituting Eq. 8 into Eq. 2 and expanding each term by the Taylor series, the algorithmic approximation to coning compensation over the minor interval is obtained as:
where, A_{ij} is the constant coefficient and is defined as:
Expanding each term in Eq. 6 using the Taylor series and substituting ΩT = nΩΔT = nλ into Eq. 6, we obtain:
In order to find the optimal coefficients of coning compensation algorithm
k_{i} (i = 1, 2þ, n+p1) in Eq. 1, let the corresponding
items of Eq. 9 and 10 equal and let:
so the simultaneous equation for k_{i} can be defined as follows:
where, N is the number of total gyro incremental angular samples in order to
compute k_{i}, namely N = n+p. So the coefficients vector
is obtained as follows:
REALTIME IMPLEMENTATION OF ATTITUDE/VELOCITY UPDATING ALGORITHM
After the coning compensation approximation term
computed by Eq. 2, the rotation vector Φ_{m}
over the mth minor interval is computed as follows (Park
et al., 1999):
where, θ_{m} is the sum of the gyro incremental angular samples over the mth minor interval as follows:
According to the twospeed structure of the algorithm (Lee
et al., 1990; Ignagni, 1996), the rotation vector
Φ_{m} over the major attitude updating interval is computed as
follows:
Similar to the rotation vector computation over the major interval, the velocity updating computation over the major interval is as follows. The velocity rotation compensation term Δv_{rotm} of velocity updating algorithm is:
Where:
and
So the velocity compensation term Δv_{SFm} solving for an integral that represents the change in velocity (in body frame coordinate) over the mth minor interval caused by specific force acceleration is as follows:
And the velocity compensation Δv_{G/Corm} solving for an integral
that represents the change in velocity (in body frame coordinate) over the mth
minor interval caused by nocuous acceleration (namely gravity and Coriolis acceleration)
is as follows:
Because g^{n}, ω^{n}_{in} and ω^{n}_{en} even vary smoothly in highly dynamic environments (due to the small size of the earth angular rates and the smooth vary of body velocity), they can be approximated by their average values over the mth minor interval, so the approximation for Δv_{G/Corm} is as follows:
where, m1/2 is the parameter value midway over the mth minor interval. Because g^{n}_{m1/2} is not explicitly available in Eq. 16, so in reality the linear extrapolation is usually used to approximate g^{n}_{m1/2} as follows:
So the velocity at τ_{m} time is as follows:
where, v_{m1} is the body velocity at τ_{m1} time and C^{n}_{b(m1)} is the body attitude matrix at τ_{m1 }time. Similar to the twospeed structure of above attitude updating algorithm, the velocity vector v_{M }over the major interval is computed as follows:
where, v_{0} and C^{n}_{b(0)} are the velocity and attitude matrix at last major interval, respectively.
Considering a realtime implementation of the attitude/velocity updating algorithm in a digital computer (such as DSP or FPGA), two problems need to be solved, namely floatingpoint arithmetic and recursive calculation. The attitude updating algorithm as well as the velocity updating algorithm requires floatingpoint arithmetic. In addition, for recursive calculation, coning compensation and quaternion update in attitude updating algorithm and sculling compensation and velocity update in velocity updating algorithm have to be carried out in each update interval.
Lee et al. (1990) and Li
et al. (2010), a general scheme that could ease implementing the
attitude algorithms in a recursive form using highrate and lowrate executions
and using integer arithmetic only. In this study, we generalized this general
scheme to be used in the implementation of attitude and velocity updating algorithm.
In the attitude and velocity updating algorithm, it is desirable that the coning/sculling compensation be updated in every minor interval, whereas the quaternion and velocity are updated only once in every major interval. In this way, the time to compute the quaternion and velocity updating can be reduced while the coning/sculling compensation is updated at a high rate. To make it possible, an algorithm has to be obtained to directly update the coning/sculling compensation instead of using the quaternion and velocity.
To obtain a recursive form of attitude and velocity updating algorithm, now
we separate Eq. 14 and 18 into two parts:
(1) high rate and (2) low rates as shown in Eq. 19 and 20:
where, the rotation vector Φ calculated at every major interval is used
to obtain the corresponding the updating quaternion over the major interval.
And so we can use the updating quaternion to obtain the attitude matrix C^{n}_{b}
of vehicle. Attitude matrix C^{n}_{b(M1)} in Eq.
20b is the attitude matrix of vehicle at last major interval calculated
by rotation vector Φ. The coefficients k_{i} (i = 1,2,...,N1)
in Eq. 19c and 19d can be calculated
by Eq. 12 for any combinations of n and p.
CONCLUSIONS
In this study, a general scheme to implementation of strapdown inertial navigation attitude and velocity algorithm in realtime environment was presented. This scheme of realtime implementation was formulated in a recursive form using two timerate executions and using integer arithmetic only.
The important aspect of the proposed realtime implementation of strapdown inertial navigation attitude and velocity algorithm was divided into two timerate computations. The highrate algorithm that updated the coning/sculling compensation can be performed entirely in integer arithmetic. The timeconsuming of attitude and velocity updating was performed at a lower rate, resulting in much savings in the realtime implementation of strapdown inertial navigation system.
Considering the coding of the algorithm in a navigation computer (such as DSP
or FPGA), it was showed that the general realtime implementation of strapdown
attitude and velocity algorithm can be used in strapdown inertial navigation
system for any combinations of n and p.
ACKNOWLEDGMENTS
This study is supported in part by the Aviation Science Foundation (20110776001, National Key Laboratory of Science and Technology on Aircraft Control, Xi’An Flight Automatic Control Research Institute), the Zhejiang Provincial Natural Science Foundation (Y1100696 and R1090052), the Fundamental Research Funds for the Central Universities (2011QNA4021) and the National Nature Science Foundation of China under grant No. 61070003 and 61071128.