A Generalized Coning Correction Structure for Attitude Algorithms

A new coning correction structure is presented for attitude update coning correction. Different from the previous rate-based and increment-based coning correction structures, the new structure contains cross-product of angular rates, cross-product of angular increments, and cross-product of angular rate and increment (an angular increment may be approximated from angular rate samples). Two types of optimization methods including time Taylor-series method and frequency Taylor-series method were utilized to design the structure coefficients including the uncompressed and the compressed. Two types of algorithm error models including one applicable to coning environments and the other two applicable to maneuver environments were defined and used for analyzing or evaluating the algorithm performance.The derivation procedure of a rotation vector magnitude extractionmethod is included. Analysis and simulation results indicate that the new structure-based algorithm with the compressed coefficients designed by using frequency Taylor-series method gives a superior algorithm performance in coning environments and maneuver environments.


Introduction
The so-called two-stage algorithm structure for modern strapdown inertial navigation attitude updating was introduced by Jordan [1] and Bortz [2] in 1969.One stage is based on an exact updating formula to update the attitude in an attitude updating cycle, and the other stage to be imported into the former is based on an approximate computation formula to calculate the rotation vector including an integrated gyro sensed angular rate and a coning correction calculated from gyro data.Currently there exist two classes of essentially equivalent structures for rotation vector computation: two-speed structure and single-speed structure.Because of the low-speed arithmetic capability of early computers, the coning correction was generally based on the two-speed structure [3][4][5] where there are some cycles executed for coning correction in a rotation vector computing cycle (or an attitude updating cycle), and this processing way allows lower attitude updating rate and computation load when algorithm accuracy holds.As the rise of arithmetic capability of modern computers, the single-speed structure [5] where there are only cycle executed for coning correction in a rotation vector computing cycle may be applicable to the coning correction substituted for the two-speed structure, and this way can meet the demand of high attitude updating rate but makes bigger computation load.
In the latest several decades, attitude algorithm study activity has concentrated on designing an efficient coning correction structure and achieving the optimized structure coefficients for computing coning correction under the aforementioned rotation vector computation structures.The earliest optimization method (time Taylor-series method) for the coning correction coefficient design uses truncated Taylor series of time to approximate gyro sensed angular rate under the assumption that the body is undergoing a motion of angular rate of finite order power series law.In 1983, Miller [6] introduced the method (coning frequency Taylor-series method) of using truncated Taylor series of coning frequency to approximate coning correction error for the coning correction coefficient design under a constrained coning motion condition.In 1990 and 1996, on the basis of Miller's idea, Ignagni [3,4] discussed several classes of coning correction algorithm structures under the twospeed structure and realized the coning correction coefficient optimization design under an unconstrained coning motion condition.In 2010, Savage [5] further expanded Miller's idea and raised an idea of optimizing coning correction algorithms under the single-speed structure based on least squares principle in a given composite coning environment and achieving a balanced algorithm performance over a coning frequency range.In 2013, Song et al. [7] renew the time Taylor-series method combined with Miller's and Savage's methods to achieve a superior maneuver accuracy, as well as retaining the optimal coning motion performance.Additionally, in 2009, Ben et al. [8] developed an angular rate-based coning correction structure similar to the angular increment-based coning correction structure used in [3][4][5][6] and introduced Miller's idea into coning algorithm design for coning correction using cross product of angular rates rather than cross product of angular increments used in algorithms referred in [3][4][5][6].
For modern strapdown inertial navigation systems, single type of angular increment gyros or angular rate gyros is generally used for constructing gyro sensor assembly [9].Along with the advancement of gyro technology, high precise rate gyros and increment gyros may be jointly used as a portion for constructing multi type sensor assembly.This paper brings a new class of coning correction structure under the single-speed structure.The new structure contains cross product of angular rates, cross product of angular increments, and cross product of angular rate and increment for coning correction and needs both inputs of angular rate and angular increment which may be also approximated from gyro sensed angular rate (it means single type of angular rate gyros can also meet the demand of the new coning correction structure).When angular rate input is overlooked, the new structure can be simplified as some class of angular increment-based structure.In addition, the new structure includes uncompressed coning correction structure and compressed one based on coning motion characteristics.So the new structure has high flexibility and applicability which allow pure angular rate input, pure angular increment input, or composite angular rate and increment input.Two types of aforementioned coning correction optimization methods: time Taylor-series method and coning frequency Taylor-series method, were utilized to design the coning correction coefficients of the new structure.The algorithm performance is evaluated for the new structure under coning environments and maneuver environments.

Attitude Algorithm Structure
The classical attitude updating numerical computing formula in modern strapdown inertial navigation systems is given by [1,2,5] where  is a navigation coordinate frame,  is a body coordinate frame,   () and   (−1) are, respectively, an attitude direction cosine matrix at the end of attitude updating cycle  and cycle  − 1,  (−1)

𝑏(𝑙)
and   which used to update   () from   (−1) are, respectively, an updating attitude direction cosine matrix and an updating rotation vector from the ending time of cycle  − 1 to the ending time of cycle , |  | is the magnitude of vector   , and   × is the cross-product antisymmetry matrix composed of   components.
The updating rotation vector   is generally computed by using a simple structure to approximate the integral of the rotation vector differential equation.A commonly used single-speed structure [1,2,5] is given by where  is a time,   is the integral of the gyro sensed angular rate  from time  −1 to time   , and   denotes the coning correction.

Coning Correction Structure
For the requirement of strapdown system with multitypes of gyros, the new generalized computation algorithm formula for updating rotation vector   with the form of the integrated angular rate   and the coning correction  φ is proposed as where each Δ is an angular increment sample over a fixed time interval, the Δs are adjacent and spaced sequentially forward in time, Δ −+1 begins at time  −1 , Δ  ends at time   , the s are some angular rate samples which are adjacent, spaced sequentially forward in time, and time interval-equivalent,   lies at time   ,   s,   s, and   s are coefficients depending on the coning correction structure,  is the number of angular increment samples selected to compute   in cycle , , selected to be equal to or greater than , is the number of angular increment samples selected to compute  φ in cycle , , selected to be equal to or greater than , is the number of angular rate samples selected to compute  φ in cycle , and   is the angular rate sample time interval set to compute  φ in cycle .Here, it is alternative that  can be configured to equal to  + 1, and the angular increment sample interval can be configured to   .Right now, a sketch map, as Figure 1 where   equivalent to the product   , is a time interval from time  −1 to time   or the attitude updating cycle and the other signs are defined as those in (3), which has described clearly the distribution relationship of the angular increment sample series Δs and the angular rate sample series s against time.This paper will mainly study the class of coning correction structure with the parameters set above, which is called the angular rate-and-incrementsynchronized structure.
For the attitude updating algorithm with pure angular rate inputs, the integrated angular rate   in (2) can be approximated by using an integrator to integrate the gyro sensed angular rate samples from time  −1 to time   , and the coning correction  φ can be computed with the same structure as the angular rate-and-increment-synchronized structure used in (3).This angular rate-based computation algorithm formula for updating rotation vector has the form of the integrated angular rate   and the coning correction  φ : s, also adjacent, spaced sequentially forward in time, and time interval-equivalent (Δ 1 and Δ  , resp., end at time   − ( − 1)  and   ),   s are angular rate-to-increment approximation coefficients (how to determine   s will not be discussed),   s,   s,   s, , , and  are defined as those in (3) above,   is the angular rate subsample time interval (also the angular increment subsample time interval) set to compute  φ in cycle ,  is the angular rate sample time interval, and  is the whole number ratio of   to . Figure 2 has described clearly the distribution relationship of the angular rate sample series   s, the angular rate subsample series s, and the angular increment subsample series Δs against time.
Under the pure coning environment defined by (A.1), on the basis of those coning motion properties characterized by (A.6), (A.7), and (A.8), a compressed algorithm structure form equivalent to the uncompressed structure form for coning correction  φ in (3) can be given by where   is the coning correction coefficient equivalent to the sum of  −, s from (3),   is the coning correction coefficient equivalent to the sum of  −, s from (3), and   is the coning correction coefficient equivalent to the sum of  −+1, −  +1,−+1 s from (3).
Setting each   and each   in (3) equivalent to zero and marking each   as   will get the traditional computation algorithm formula for updating rotation vector with angular increment inputs which has the form of the integrated angular rate   and the coning correction  φ [4,5]: where   s are coefficients depending on the coning correction structure, other signs are defined as those in (3).This form is well known and here called uncompressed  subsample algorithm form with angular increments.
Based on the coning motion property, a compressed algorithm structure form equivalent to the uncompressed structure form for coning correction  φ in (6) can be given by where   is the coning correction coefficient equivalent to the sum of  −, s from (6).
Setting each   and each   in (3) equivalent to zero and marking each   as   will get the traditional coning correction structure with angular rate input [8]: where   s are coefficients depending on the algorithm form of ( 8), other signs are defined as those in (3).
Based on the coning motion property, the compressed coning correction algorithm form equivalent to the form as ( 8) can be given by where   is the coning correction coefficients equivalent to the sum of  −, s in (8).

Algorithm I: Coning Correction Coefficient Design by Coning Frequency Taylor-Series Method
The following derives a general formula for calculating the coning correction coefficients defined in ( 3), (4), and ( 5) by using the so-called coning frequency Taylor-series method.
Under the coning environment defined by (A.1) in Appendix A, integrate the cross product (,  −1 )×() given by (A.3) in Appendix A over the time interval [ −1 ,   ] with   =  −1 +   =  −1 +   ; then substituting the integral into (2) gives the value of coning correction   defined by (2) as Additionally, substituting for   ×   , Δ  × Δ  , and   × Δ  in (5) from (A.6), (A.7), and (A.8) in Appendix A gives the value of coning correction  φ defined by (5) as With recognizing that the -axis component of the coning correction  φ or   is only nonzero and dependent on the time intervals, define the -axis component of the difference between  φ and   as the coning correction error (): Substituting for  φ and   in (12) from ( 10) and ( 11) and carrying out simple derivation then give where Φ(Ω) is the total rotation rate around -axis, () is the error normalized by the time interval   , and  is a coning frequency parameter relevant to   .According to the trigonometry formula sin 2 (/2) = (1 − cos )/2, have the relationships: According to the relationships given by ( 14), (13) can be rewritten as According to the Taylor-series expansion of sin  and cos  in variable  around zero point, consider the following: The coning correction error () in (15) can be reformed as a Taylor-series expansion form in parameter  around zero point: The coning correction optimization design objective should be to minimize the absolute value of the coning correction error (), which can be achieved by setting the first  + 2 − 2 term coefficients of Taylor series of (17) to zeros which gives the following system of linear equations: Mathematical Problems in Engineering Which can be simplified as Define the following terms: Then the system of linear equations ( 19) can be rewritten as the following matrix form: where which gives the optimized coning correction coefficients   s,   s, and   s applicable to the compressed coning correction structure form defined by (5).

Algorithm II: Coning Correction Coefficient Design by Time Taylor-Series Method
Following derives a general formula for calculating the coning correction coefficients defined in (3), (4), and ( 5) by using Taylor-series expansion in time  for angular rate  around time point  0 .A Taylor series truncated with the number of terms + for angular rate () in time  around time point  0 is given by By setting  =  −  0 , then (24) can be rewritten as Or where  is the time increment of time  relative to time  0 and   are the coefficients of series in (26).Based on (25) and ( 26), the angular rate samples   s in time  0 +   with   = ( −  + )  can be given by And the angular increment samples Δ  s over the time interval [ 0 +   ,  0 +   +   ] with   = ( −  +  − 1)  can be given by Define the following terms: Then equation terms ( 27) and (28) can be transformed to a matrix form as follows: where Γ is a  +  by  +  square matrix whose th row th column component is   ,  is a  +  by one column matrix whose th row component is  ,1 , and  is a  +  by one column matrix whose th row component is  ,1 .Solving matrix equation (30) gives the following result: where Γ −1 is a  +  by  +  square matrix whose th row th column component is   .Matrix equation (31) can be rewritten in the equation term form as follows: Further, with   =   −  0 and  −1 =  −1 −  0 , the coning correction   in (2) is rewritten as Substituting for () in (33) from (26) gives Additionally, using (32) can get Then the coning correction   can be derived from (33), (35), and (36) as Setting that  −1 =  0 and   =  −1 +   will get  −1 = 0 and   =   ; then correction coefficients   ,   , and   in (38) can be simplified as When used in a coning environment, the coning correction  φ can be with the compressed form of (5).So (39) can be rewritten as (40)

Algorithm Performance Evaluation
6.1.Coning Performance Evaluation.In a coning environment, the algorithm error for evaluating the performance of the algorithm with the form of (5) was defined as the average rotation rate ė  () equivalent to () in (13) divided by   (  =   ) as follows: where Φ (Ω) is the total rotation rate used to evaluate coning algorithm performance and Ḟ () is the normalized error equivalent to () in (13) divided by   .Note that Φ (Ω) is different from Φ(Ω) which can be used to design coning correction coefficients [5].
Setting each   and each   in (41) equivalent to zero and marking each   as   will get an algorithm error applicable to evaluating the algorithm with the form of (7) as follows: where   s are coefficients designed based on the algorithm structure of (7).
Setting each   and each   in (41) equivalent to zero and marking each   as   will get an algorithm error applicable to evaluating the algorithm with the form of (9) as follows: where   s are coefficients designed based on the algorithm structure of (9).Each uncompressed algorithm has the same coning accuracy as its compressed version, because each uncompressed algorithm is equivalent to its compressed version under coning conditions.

Maneuver Performance Evaluation.
Under the maneuver environment defined by (26), setting that  −1 = 0 and   =   to (37) will get In addition, cross products   ×  , Δ  ×Δ  , and   ×Δ  are respectively derived from ( 27) and (28) as Substituting for   ×   , Δ  × Δ  , and   × Δ  in (3) or ( 4) from (45) gets In a maneuver environment, the algorithm error for analyzing the performance of the algorithm with the form of (3) or (4) was defined as the difference of  φ of (46) and   of (44) with   ×   = 0 and   ×   = −  ×   as follows: Setting , each   , and each   in (48) equivalent to zero will get an algorithm error applicable to analyzing the algorithm with the form of ( 6) as follows: Setting , each   , and each   in (48) equivalent to zero will get an algorithm error applicable to analyzing the algorithm with the form of (8) as follows: In a maneuver environment, the algorithm error for evaluating the performance of all concerned algorithms was defined as the rotation vector magnitude error   () equivalent to the direction cosine attitude matrix error as where () is the direction cosine attitude matrix error at time ,   () is the th row and th column component of matrix (),   () is (1) computed direction cosine attitude matrix at time  using those investigated algorithms, and   () is the true direction cosine attitude matrix at time .In this paper, an approximated   () obtained by using (1) at a more than 100 kHz rotation vector updating rate was used instead of a true   () (see Appendix B for the derivation of ( 50)).

Algorithm Example and Performance Evaluation
To show the properties of several algorithms, evaluated results computed by using the optimized coning correction coefficients designed by using coning frequency Taylorseries method and time Taylor-series method were produced, compared, and analyzed under coning environments and maneuver conditions.To analyze and compare conveniently, signs  and  in (3), (4), and (5) were, respectively, rewritten as   and   (the number of subsamples in the new structure is   +   ), sign  in ( 6) and ( 7) was rewritten as   , and sign  in ( 8) and ( 9) was rewritten as   .Two terms of parameters were set: (1)   = 3,   = 2,  = 2 (two sequential Δs samples in the   calculation), and   = 10 ms (i.e., attitude updating time interval   equals to 10 ms) and (2)   = 5,   = 5,  = 4 (four sequential Δs samples in the   calculation), and   = 5 ms (i.e., attitude updating time interval   equals to 20 ms).
(1) FSIRc indicates the coning algorithm based on the compressed form of (5) taking the coefficients designed by using frequency Taylor-series method (see Algorithm I).
(2) TSIRc indicates the coning algorithm based on the compressed form of (5) taking the coefficients designed by using time Taylor-series method (see Algorithm II).
(3) FSIc indicates the coning algorithm based on the compressed form of (7) taking the coefficients designed by using frequency Taylor-series method (see [5]).
(4) TSIc indicates the coning algorithm based on the compressed form of ( 7) taking the coefficients designed by using time Taylor-series method (derived from TSIuc and ( 7)).
(5) FSRc indicates the coning algorithm based on the compressed form of (9) taking the coefficients designed by using frequency Taylor-series method (see [7]).
(6) FSIRuc indicates the coning algorithm based on the uncompressed form of (3) or (4) taking the coefficients designed by using frequency Taylor-series method (see Algorithm I).
(7) TSIRuc indicates the coning algorithm based on the uncompressed form of (3) or ( 4) taking the coefficients designed by using time Taylor-series method (see Algorithm II).(8) FSIuc indicates the coning algorithm based on the uncompressed form of ( 6) taking the coefficients designed by using frequency Taylor-series method (derived from FSIc and ( 7)).
(9) TSIuc indicates the coning algorithm based on the uncompressed form of ( 6) taking the coefficients designed by using time Taylor-series method (see [5]).
(10) FSRuc indicates the coning algorithm based on the uncompressed form of (8) taking the coefficients designed by using frequency Taylor-series method (see [7]).
Tables 1, 2, 3, 4, 5, and 6 give the coefficients of the ten concerned algorithms above.The coefficients in all tables were given in the form of whole number ratio approximating the decimals of limited significant digits (the number of significant digits is sixteen in this paper) in order to both ignore the numerical truncation error effect on the algorithm performance and simplify writing.
Under coning environments with Φ (Ω) set to unity over all frequencies, computing algorithm error ė  () in (41) with FSIRc and TSIRc coefficients in Table 1, algorithm error ė  () in (42) with FSIc and TSIc coefficients in Table 2, and algorithm error ė  () in (43) with FSRc coefficients in Table 3 results in a plot of normalized algorithm error ė  () versus coning frequency /(2) as in Figure 3, where each of five denotations indicates both versions of its compressed and uncompressed algorithms.
In addition, the maneuver environment set for algorithm performance evaluation was the extreme 2s angular rate profile pictured in Figure 4. Then the rotation vector magnitude error   () in (50) was computed over Figure 4's maneuver profile by using corresponding coefficients from Table 1 to Table 6.Maxima of rotation vector magnitude errors of ten algorithms over the 2s maneuver profile are presented in Table 8. Figure 3 shows the relative coning accuracy of ten concerned algorithms, rather than its absolute coning accuracy over the 1 Hz∼100 Hz coning frequency range.The new FSIR algorithm has almost the best coning performance over the most presented frequencies.All algorithms (FSIR, FSI, and FSR) designed by using coning frequency Taylorseries method are more accurate than those (TSIR and TSI) designed by using time Taylor-series over the most presented frequencies.If the traditional FSI algorithm had  7 which is associated to (47), (48), and (49), it is found that  13 is the main attribution to maneuver error of all concerned compressed algorithms (FSIRc, TSIRc, FSIc, TSIc, and FSRc);  14 ,  16 ,  13 , and  12 are, respectively, the main attribution to maneuver error of FSIRuc, that of TSIRuc, that of FSIuc, and that of TSIuc, and  12 ,  14 , and,  23 are the main attributions to maneuver error of FSRuc.Under the general fact that one low-order power series term is bigger than one high-order, the power series terms with the main attributions supply the main maneuver error of concerned algorithms.According to the values of the main attributions in Table 7, the FSIRc algorithm has the best maneuver performance compared to other compressed algorithms; each uncompressed algorithm except FSIuc has an much evident maneuver accuracy improvement compared with its compressed algorithm version.
Table 8 indicates that the maximum-error of FSIRc is the smallest in those of all compressed algorithms, the maximum-error of each uncompressed algorithm except FSIuc is smaller about 1 to 2 orders than its compressed algorithm version.The simulation results in Table 8 are basically consistent with the analytic conclusions above.According to the application experience that the algorithm maneuvering error less than 1% of 21.3 rad from sensors is compatible with the overall INS accuracy requirement of 0.01 deg/h [5], the 6.46−1 rad maximum-error of FSIRc algorithm is less than 1% of 200 rad and compatible with the overall INS accuracy requirement of 0.1 deg/h (a lower system accuracy), whereas the maximum-errors of other compressed algorithms are not.All uncompressed algorithms except FSIuc have much adequate maneuver accuracy for the overall requirement of 0.1 deg/h and are just compatible with the overall requirement of 0.01 deg/h.

Conclusions
The new rate-and-increment-synchronized coning correction structure is presented for achieving higher coning correction performance in coning and maneuver environments.As the traditional increment-based structure, the correction coefficient of the new structure permits to be designed using the frequency Taylor-series method and time Taylor-series method.Compared with the traditional increment-based and rate-based compressed algorithms, the new algorithm with the compressed coefficients optimized by using frequency Taylor-series method gives a superior coning performance, as well as a higher maneuver accuracy.Uncompressed algorithms based on the new structure are much higher accurate than its compressed version under a maneuver environment.

A. Several Relationships under Coning Motion
This appendix firstly defines the unconstrained coning motion and then derives several useful relationships for the coning correction structure compression and coefficient optimization.
Assume that the body is undergoing the pure coning motion defined by the angular rate vector as follows: where  is a time, () is an angular rate vector in the body frame at time ,  and  are amplitudes of the angular oscillations in two orthogonal axes of the body, Ω is frequency associated with the angular oscillations, and [⋅]  is the transposition of [⋅].Under the coning environment defined by (A.1), the angular increment vector (,  −1 ) in ( 2  The values of all cross products from (A.3), (A.6), (A.7), and (A.8) are independent of absolute time but depend only on the time intervals.

B. Rotation Vector Magnitude Computation
This appendix derives a rotation-vector-magnitude extraction formula like (41) from a rotation-vector-to-directioncosine-matrix formula [1,2,10]  According to (B.7), we find that it is not applicable to compute |()| from () by using (B.6), owing to the deficiency of the number of significant digits (e.g., setting |()| = 1.0 − 10 gets 3 − tr(()) ≈ 1.0 − 20, which needs more than twenty significant digits for tr(()), while double type variable generally gives sixteen significant digits in computer software).To resolve this problem, we need to investigate (B.

Figure 1 :
Figure 1: Distributions of Δ series and  series against time.

Table 1 :
Algorithm coefficients for the compressed form of (5).

Table 3 :
Algorithm coefficients for the compressed form of (9).

Table 5 :
Algorithm coefficients for the compressed form of (6).

Table 6 :
Algorithm coefficients for the compressed form of (8).

Table 8 :
Maximum of rotate vector magnitude errors in the set maneuver environment.
1) again.Given   ,   , and   are small, Substituting for ()× and [()×] 2 in (B.10) from (B.3) and (B.4) can get the following approximate equation term:   () is the th column and th raw component of matrix ().With denoting   () by using   , solving for   ,   , and   from (B.11) then gives Substituting for   ,   , and   in (B.2) from (B.12) finally gets where  is an approximation of magnitude |()| and  is the main error from magnitude |()| to .