Abstract
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 extraction method 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.
1. 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–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 two-speed 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–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–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.
2. 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 are, respectively, an attitude direction cosine matrix at the end of attitude updating cycle and cycle , and which used to update from are, respectively, an updating attitude direction cosine matrix and an updating rotation vector from the ending time of cycle 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 to time , and denotes the coning correction.
3. 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, begins at time , 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 , 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 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-increment-synchronized 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 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 : where s are gyro sensed angular rate samples which are adjacent, spaced sequentially forward in time, and time interval-equivalent and s are gyro sensed angular rate subsamples which are selected from s, also adjacent, spaced sequentially forward in time, and time interval-equivalent; both and lie at time (i.e., ; both and lie at time ) and both and lie at time (i.e., ; both and lie at time ; both and lie at time ); s are angular increment subsamples which are approximately generated from s, also adjacent, spaced sequentially forward in time, and time interval-equivalent ( and , resp., end at time 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 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).
4. 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 given by (A.3) in Appendix A over the time interval with ; 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 , have the relationships:
According to the relationships given by (14), (13) can be rewritten as
According to the Taylor-series expansion of and 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 term coefficients of Taylor series of (17) to zeros which gives the following system of linear equations: 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 is a by square matrix whose th row and th column component is , is a by one column matrix formed from components s, and is by one column matrix formed from components s, s, and s.
Solving matrix (21) results in which gives the optimized coning correction coefficients s, s, and s applicable to the compressed coning correction structure form defined by (5).
On the basis of the solution of (22), the optimized coning correction coefficients s, s, s, and s applicable to the uncompressed coning correction structure form defined by (3) or (4) can be gained from some relationships defined by (5) as
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 .
A Taylor series truncated with the number of terms for angular rate in time around time point is given by By setting , then (24) can be rewritten as Or where is the time increment of time relative to time and are the coefficients of series in (26). Based on (25) and (26), the angular rate samples s in time with can be given by And the angular increment samples s over the time interval with 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 , and is a by one column matrix whose th row component is . Solving matrix equation (30) gives the following result: where 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 and , 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 With recognizing that , , , and , comparing (37) to (3) or (4) can get Setting that and will get 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
6. 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.
6.2. Maneuver Performance Evaluation
Under the maneuver environment defined by (26), setting that 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 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)).
7. 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 Taylor-series 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) , , (two sequential s samples in the calculation), and ms (i.e., attitude updating time interval equals to ms) and (2) , , (four sequential s samples in the calculation), and ms (i.e., attitude updating time interval equals to 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 as in Figure 3, where each of five denotations indicates both versions of its compressed and uncompressed algorithms.
The coefficients , , , , , , , , and in (47), (48), and (49) are, respectively, calculated with corresponding algorithm coefficients from Table 1 to Table 6.
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 Taylor-series 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 adequate coning accuracy, the FSIR algorithm would have more adequate coning accuracy.
By analyzing Table 7 which is associated to (47), (48), and (49), it is found that is the main attribution to maneuver error of all concerned compressed algorithms (FSIRc, TSIRc, FSIc, TSIc, and FSRc); , , , and are, respectively, the main attribution to maneuver error of FSIRuc, that of TSIRuc, that of FSIuc, and that of TSIuc, and , , and, 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 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.
8. 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.
Appendices
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 in (2) is given by Then taking the cross product of the angular increment vector with the angular rate vector gives
Under the coning environment defined by (A.1), defining the angular rate vector at time as , with , gives And defining the angular increment over the time interval beginning at as gives Then the cross products , , and are evaluated as
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-direction-cosine-matrix formula [1, 2, 10] as where is a direction cosine attitude matrix at time equivalent to a rotation vector , is the magnitude of vector , is the cross-product antisymmetry matrix composed of components, and is the three by three unit matrix.
Setting in (B.1) gets Substituting for and in (B.1) from (B.3) and (B.4) can easily find where , , and are the diagonal components of attitude matrix . Solving for magnitude from term (B.5) then gives where is the trace of matrix .
Equation (B.6) gives an ideal analysis formula to calculate magnitude . While, if is far smaller than one, there is an approximation for as
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 gets , which needs more than twenty significant digits for , while double type variable generally gives sixteen significant digits in computer software). To resolve this problem, we need to investigate (B.1) again. Given , , and are small, recognize that
Equation (B.1) may become an approximate form as follows: Substituting for and in (B.10) from (B.3) and (B.4) can get the following approximate equation term: where is the 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 .
Conflict of Interests
The authors declare that there is no conflict of interests regarding the publication of this paper.
Acknowledgments
This work was supported in part by the National Natural Science Foundation of China (no. 51375087), the Specialized Research Fund for the Doctoral Program of Higher Education (no. 20110092110039), the Ocean Special Funds for Scientific Research on Public Causes (no. 201205035-09), the Program Sponsored for Scientific Innovation Research of College Graduate in Jiangsu Province, China (no. CXZZ12_0097), and the Scientific Research Foundation of Graduate School of Southeast University (no. YBJJ1349).