1. INTRODUCTION
An inertial navigation system (INS) is a dead-reckoning based system, so initial navigational information needs to be provided to perform the subsequent navigational computation. The initial position and velocity can be easily obtained from GPS or other aiding sensors (Lee et al. Reference Lee, Wang, Rizos, Grejner-Brzezinska and Toth2002; Wang et al. Reference Wang, Lee, Hewitson and Hyung-Keun2003), while the acquisition of the initial attitude is not that straightforward and requires specific initial alignment algorithms.
The initial alignment of high-precision INS and low-cost INS should be treated using different methods, because the noise levels of the inertial sensors are quite different for these two types of systems. For high-precision INS, which sometimes can be further classified into strategic-grade INS and navigation-grade INS (Greenspan, Reference Greenspan1995), the initial alignment can be achieved through two stages: coarse alignment and fine alignment (Savage, Reference Savage2000). The coarse alignment process aims at making the unknown initial attitude errors converge to small angle errors, and is generally carried out analytically on a stationary base by comparing the measured gravity and Earth rate with the standard gravity and Earth rate (Savage, Reference Savage2000). Under the premise of small attitude errors, the fine alignment can be performed based on the INS error equations (Benson, Reference Benson1975). The fine alignment aims to eliminate the residual attitude errors, and can be carried out on a stationary base using the gyrocompassing theory (Savage, Reference Savage2000), or carried out on a dynamic moving base using in-motion alignment techniques (Baziw, Reference Baziw and Leondes1972a and Reference Baziw and Leondes1972b; Savage, Reference Savage2000; Shin, Reference Shin and El-Sheimy2004; Groves Reference Groves2008) with the aid of GPS or other sensors.
However, the above initial alignment approach for high-precision INS cannot be used in the initial alignment of low-cost INS. Generally, the low-cost INS uses gyros with drift rates of 1–10 deg/h or more, and accelerometers with biases of 0·1–1·0 mg or more (Greenspan, Reference Greenspan1995). Earth rate is about 15 deg/h, so the noise levels of the low-cost gyros are near or higher than Earth rate, which means that the low-cost gyros cannot measure the Earth rotation properly. So the coarse alignment technique based on the measurements of gravity and Earth rate for high-precision INS cannot be used in the initial alignment of the low-cost INS, and neither can the fine alignment technique based on the gyrocompassing theory be used. A possible solution for this problem is using the in-motion alignment technique. However, although the in-motion fine alignment methods have been intensively explored (Baziw, Reference Baziw and Leondes1972a and Reference Baziw and Leondes1972b; Savage, Reference Savage2000; Shin, Reference Shin and El-Sheimy2004; Groves Reference Groves2008), the in-motion coarse alignment is still a challenging issue.
Because gravity is a relatively large quantity, even low-cost accelerometers can measure it properly. So the coarse levelling to determine the pitch and roll of the low-cost INS can be carried out using gravity measurements on a stationary base prior to the in-motion coarse alignment (Savage, Reference Savage2000). The remaining task is to deal with unknown initial heading error using the in-motion coarse alignment techniques. Various approaches have been studied about this issue. Dmitriyev et al. (Reference Dmitriyev, Stepanov and Shepel1997) and Kong et al. (Reference Kong, Nebot and Durrant-Whyte1999) constructed a nonlinear INS error dynamics model to account for the unknown initial heading error. The benefit of the nonlinear method is that the in-motion coarse alignment is not necessary and a single Kalman Filter can complete the initial alignment, but that method suffers from the complexity of nonlinear operations. Rogers (Reference Rogers1997), Kelley et al. (Reference Kelley, Katz and Bedoya1990), and Pham (Reference Pham1992) developed a linear INS error dynamics model in the wander azimuth frame for the in-motion coarse alignment to deal with the unknown initial heading error. That method transferred the heading error into wander azimuth angle error and modelled the sine and cosine errors of the wander azimuth angle into the INS error equations. But the observability of the errors of sine and cosine of the wander azimuth angle are very weak, because the coupling coefficients between these trigonometric function errors and the position errors as well as the velocity errors are all small quantities. A quantitative analysis of this can be found in Section 2. Discussions on the observability concept can be found in e.g., Bar-Itzhack and Porat (Reference Bar-Itzhack and Porat1980), and Han and Wang (Reference Han and Wang2008).
Inspired by the work of Rogers (Reference Rogers1997), Kelley et al. (Reference Kelley, Katz and Bedoya1990), and Pham (Reference Pham1992), a new linear INS error dynamics model in the north pointing frame is developed in this research for the in-motion coarse alignment to deal with the unknown initial heading error. Instead of the sine and cosine errors of the wander azimuth angle, the sine and cosine of the platform heading misalignment are modelled into the INS error equations. In this new model, the observability of the sine and cosine of the platform heading misalignment are much better, because the coupling coefficients between these trigonometric functions of the platform heading misalignment and the velocity errors are much larger. The coefficients in the velocity dynamics are the elements of the specific force, which is generally a large quantity because, on the one hand, gravity is measured in the specific force, and, on the other hand, some intentional manoeuvres should be made during initial alignment of INS for land vehicle applications. Experimental tests show that this proposed model results in a faster and more accurate alignment.
The subject matter of this paper is presented as follows. Section 2 reviews the conventional approach for the initial alignment of low-cost INS and analyzes its drawbacks. To account for the unknown initial heading error, Section 3 develops a new error dynamics model and observation model for the in-motion coarse alignment of low-cost INS. Section 4 is devoted to the in-motion fine alignment, the error dynamics model and observation model which can be easily derived as a special case of the in-motion coarse alignment under the condition of small heading error. Section 5 discusses the Kalman Filtering correction scheme for both the in-motion coarse alignment and fine alignment. Section 6 illustrates the configuration and results of the experimental tests, and Section 7 presents the concluding remarks.
2. CONVENTIONAL APPROACH FOR THE INITIAL ALIGNMENT OF LOW-COST INS: REVIEW AND ANALYSIS
In Rogers (Reference Rogers1997), Kelley et al. (Reference Kelley, Katz and Bedoya1990), and Pham (Reference Pham1992), a two-stage initial alignment scheme for INS was proposed. In the first stage, a linear INS error dynamics model accounting for unknown initial heading error was developed in the wander azimuth frame to perform the in-motion coarse alignment and, in the second stage, a linear INS error dynamics model accounting for small initial heading error was developed in the wander azimuth frame to carry out the in-motion fine alignment. Although it was not specifically proposed for the initial alignment of the low-cost INS, it did provide an alternative solution to this problem. However, this conventional approach has two drawbacks: weak observability of the heading error and correlated driving noises of the velocity error dynamics.
The weak observability of the heading error arises from the fact that the coupling coefficients between the heading error and the position error as well as the velocity error in the INS error dynamics are small quantities. The heading error propagates as the velocity error and, as a result, the position error through the coupling coefficients. So, if these coefficients are not large enough, the velocity error and position error propagated from the heading error may be ‘submerged’ by the system noise, especially for low-cost systems and, under such circumstances, the heading error will become unobservable or weakly observable. The weak observability will affect the accuracy and increase the time of alignment.
The coupling coefficients for the in-motion coarse alignment and fine alignment using the conventional approach can be obtained from equations (19) and (22), respectively, in Rogers (Reference Rogers1997). The coupling coefficients are time-varying, and their expressions are listed in Table 1 for coarse alignment and Table 2 for fine alignment, respectively. From Table 1 and Table 2, it can be noted that the coupling coefficients between the heading error and the position error are determined by the rotation rate (
) of the navigation frame with respect to the Earth centred Earth fixed (ECEF) frame while the coupling coefficients between the heading error and the velocity error are determined by both the velocity of the navigation frame (
) with respect to the ECEF frame, and Earth rate (Ω). However, , , and Ω are all small quantities in the initial alignment of INS for land vehicle applications. As previously mentioned, Ω is about 15 deg/h, which is a small quantity. , cannot be very large during the initial alignment, because the land vehicle should perform some special manoeuvres to accelerate the initial alignment process. In our tests, which will be discussed in detail in Section 6, two kinds of manoeuvres were carried out: small ‘eight-like’ movements and small circular movements. These manoeuvres should be carried out in restricted areas to limit the maximum value of and, as a result, . Although the velocity cannot be very large during these manoeuvres the acceleration can; based on this analysis, this paper develops a new initial alignment scheme.
The coupling coefficients between the heading error and system error states in the in-motion coarse alignment using the conventional approach.
Notes: 1. All the symbols are kept the same as equation (19) in Rogers (Reference Rogers1997) for the convenience of comparison.
The coupling coefficients between the heading error and system error states in the in-motion fine alignment using the conventional approach.
Notes: 1. All the symbols are kept the same as equation (22) in Rogers (Reference Rogers1997) for the convenience of comparison.
Another problem of the conventional approach is that the INS velocity error dynamics are driven by correlated noises. According to equation (19) in Rogers (Reference Rogers1997), the horizontal velocity error dynamics for coarse alignment can be rewritten as follows:
From the above equations, it can be noted that the driving noise of equation (1a) consists of δf x, δg x, and −v yεz, and the driving noise of equation (1b) consists of δf y, δg y, and −v xεz. Obviously, −v yεz and −v xεz are correlated with each other. A similar situation can also be observed in the velocity error dynamics for the fine alignment. According to Kalman Filtering theory, the correlated driving noises will make the filter sub-optimal (Brown and Hwang Reference Brown and Hwang1997). This sub-optimality of the filter will also affect the accuracy of the alignment.
To avoid the aforementioned two drawbacks, Sections 3 to 5 propose the new initial alignment scheme, in which the coupling coefficients between the heading error and velocity error are enhanced and the problem of the correlated driving noises is removed. Experimental tests in Section 6 show impressive improvement in both the accuracy and the speed of the alignment by using this new scheme.
3. NEW APPROACH FOR IN-MOTION COARSE ALIGNMENT
Coarse alignment aims to estimate the initial heading angle and to make the heading misalignment converge to a small angle. For strategic-grade and navigation-grade INS, the coarse alignment can be achieved analytically on a stationary base by comparing the measured gravity and Earth rate with the standard gravity and Earth rate (Savage, Reference Savage2000). But for low-cost systems, the gyros used are generally too noisy to measure the Earth rotation, so the analytical approach cannot be used to obtain the initial heading angle. Under the assumptions of small tilt misalignment and unknown heading misalignment, this paper develops a new INS error dynamics model, which is used in the Kalman Filter to estimate the initial heading angle for low-cost INS.
3.1. The Development of INS Error Dynamics Model for Unknown Heading Error
INS error dynamics with small platform misalignments was discussed in Benson (Reference Benson1975). The position error dynamics with an unknown heading misalignment remains the same as that with a small heading misalignment, because the position error dynamics do not rely on small misalignment assumptions. The velocity error dynamics and platform misalignment dynamics are redeveloped in this paper.
The detailed derivation of the velocity error dynamics and the platform misalignment dynamics with unknown heading misalignment can be found in Appendix A. This development proceeds with the factorization of the direction cosine matrix C cp:
where c represents the computed geographic frame, p represents the platform frame which is a mathematically virtual platform for strapdown INS, and p′ represents a medium frame which is a local level frame, as is the c frame, and has the same heading misalignment as the p frame. The geometric relationship between the c frame and the p frame is shown in Figure 1.
Geometric Relationship between the c frame and the p′frame (ψz is the unknown heading misalignment.
So the direction cosine matrix C cp′ can be computed as follows:
![C_{c}^{p'} \equals \left[ {\matrix{ {\cos \psi _{z} } \tab {\sin \psi _{z} } \tab 0 \cr { \minus \sin \psi _{z} } \tab {\cos \psi _{z} } \tab 0 \cr 0 \tab 0 \tab 1 \cr} } \right]](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041008052-0960:S0373463310000214_eqn3.gif?pub-status=live)
Considering the tilt misalignment are small angles, define the tilt misalignment vector of the p frame with respect to the p′ frame as:
Because the p frame and the p′ frame have the same heading misalignment, so the direction cosine matrix C p′p can be computed as:
![C_{p}^{p'} \equals I \minus \left( {\psi _{xy} \times\hskip-3pt } \right) \equals \left[ {\matrix{ 1 \tab 0 \tab {\psi _{y} } \cr 0 \tab 1 \tab {\psi _{x} } \cr {\psi _{y} } \tab { \minus \psi _{x} } \tab 1 \cr} } \right]](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041008052-0960:S0373463310000214_eqn5.gif?pub-status=live)
where (ψxy×) represents the skew symmetric matrix of vector ψxy. According to equations (2), (3), and (5), the direction cosine matrix C cp can be computed as:
![C_{c}^{p} \equals \left[ {\matrix{ {\cos \psi _{z} } \tab {\sin \psi _{z} } \tab { \minus \psi _{y} } \cr {\sin \psi _{z} } \tab {\cos \psi _{z} } \tab \hskip11pt{\psi _{x} } \cr {\psi _{y} \cos \psi _{z} \plus \psi _{x} \sin \psi _{z} } \tab {\psi _{y} \sin \psi _{z} \plus \psi _{x} \cos \psi _{z} } \tab \hskip5pt 1 \cr} } \right]](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041008052-0960:S0373463310000214_eqn6.gif?pub-status=live)
When the heading misalignment is also a small angle, the following relationship holds:
where ψ is the small angle misalignment vector with elements ψx, ψy and ψz, and (ψ×) is the skew symmetric matrix of vector ψ. This correspondence will be used to derive the observation equation in Subsection 3.2. The resulting INS error dynamics with an unknown heading misalignment is summarized in equation (8).
![\eqalign{ \tab{d \over {dt}}\left[ {\matrix{ {\delta r_{x}^{c} } \cr {\delta r_{y}^{c} } \cr {\delta r_{z}^{c} } \cr {\delta v_{x}^{c} } \cr {\delta v_{y}^{c} } \cr {\delta v_{z}^{c} } \cr {\psi _{x} } \cr {\psi _{y} } \cr {\sin \psi _{z} } \cr {\cos \psi _{z} \minus 1} \cr} } \right] \cr\noalign{\vskip-6pt}\cr\equals \tab \left[ {\matrix{ \hskip-5pt 0 \tab\hskip-10pt {\omega _{ecz}^{c} } \tab \hskip-5pt { \minus \omega _{ecy}^{c} } \tab \hskip-5pt 1 \tab \hskip-5pt 0 \tab \hskip-5pt 0 \tab \hskip-5pt 0 \tab \hskip-5pt 0 \tab \hskip-5pt 0 \tab \hskip-5pt 0 \cr { \minus \omega _{ecz}^{c} } \tab \hskip-5pt 0 \tab \hskip-5pt {\omega _{ecx}^{c} } \tab \hskip-5pt 0 \tab \hskip-5pt 1 \tab \hskip-5pt 0 \tab \hskip-5pt 0 \tab \hskip-5pt 0 \tab \hskip-5pt 0 \tab \hskip-5pt 0 \cr \hskip-5pt {\omega _{ecy}^{c} } \tab \hskip-5pt \hskip-5pt { \minus \omega _{ecx}^{c} } \tab \hskip-5pt 0 \tab \hskip-5pt 0 \tab \hskip-5pt 0 \tab \hskip-5pt 1 \tab \hskip-5pt 0 \tab \hskip-5pt 0 \tab \hskip-5pt 0 \tab \hskip-5pt 0 \cr \hskip-5pt 0 \tab \hskip-5pt 0 \tab \hskip-5pt 0 \tab \hskip-5pt 0 \tab \hskip-5pt \eqalign{\tab\lpar 2\omega _{iez}^{c} \cr\tab\plus \omega _{ecz}^{c} \rpar} \tab \hskip-5pt { \minus \omega _{ecy}^{c} } \tab \hskip-5pt 0 \tab \hskip-5pt { \minus f_{z}^{c} } \tab \hskip-5pt {f_{y}^{c} } \tab \hskip-5pt {f_{x}^{c} } \cr \hskip-5pt 0 \tab \hskip-5pt 0 \tab \hskip-5pt 0 \tab \hskip-5pt \eqalign{\tab\minus \lpar 2\omega _{iez}^{c} \cr\tab\plus \omega _{ecz}^{c} \rpar} \tab \hskip-5pt 0 \tab \hskip-5pt \eqalign{\tab\lpar 2\omega _{iex}^{c} \cr\tab\plus \omega _{ecx}^{c} \rpar} \tab \hskip-5pt {f_{z}^{c} } \tab \hskip-5pt 0 \tab \hskip-5pt { \minus f_{x}^{c} } \tab \hskip-5pt {f_{y}^{c} } \cr \hskip-5pt 0 \tab \hskip-5pt 0 \tab \hskip-5pt 0 \tab \hskip-5pt {\omega _{ecy}^{c} } \tab \hskip-5pt \eqalign{\tab\minus \lpar 2\omega _{iex}^{c}\cr\tab \plus \omega _{ecx}^{c} \rpar} \tab \hskip-5pt 0 \tab \hskip-5pt { \minus f_{y}^{c} } \tab \hskip-5pt {f_{x}^{c} } \tab \hskip-5pt 0 \tab \hskip-5pt 0 \cr \hskip-5pt 0 \tab \hskip-5pt 0 \tab \hskip-5pt 0 \tab \hskip-5pt 0 \tab \hskip-5pt 0 \tab \hskip-5pt 0 \tab \hskip-5pt 0 \hskip-5pt \tab \eqalign{\tab\lpar \omega _{iez}^{c} \cr\tab\plus \omega _{ecz}^{c} \rpar} \tab \hskip-5pt { \minus \omega _{ecy}^{c} } \tab \hskip-5pt \eqalign{\tab\minus \lpar \omega _{iex}^{c} \cr\tab\plus \omega _{ecx}^{c} \rpar} \cr \hskip-5pt 0 \tab \hskip-5pt 0 \tab \hskip-5pt 0 \tab \hskip-5pt 0 \tab \hskip-5pt 0 \tab \hskip-5pt 0 \tab \hskip-5pt \eqalign{\tab\minus \lpar \omega _{iez}^{c} \cr\tab\plus \omega _{ecz}^{c} \rpar} \tab \hskip-5pt 0 \tab \eqalign{\tab\lpar \omega _{iex}^{c} \cr\tab\plus \omega _{ecx}^{c} \rpar }\tab \hskip-5pt { \minus \omega _{ecy}^{c} } \cr \hskip-5pt 0 \tab \hskip-5pt 0 \tab \hskip-5pt 0 \tab \hskip-5pt 0 \hskip-5pt \tab \hskip-5pt 0 \tab \hskip-5pt 0 \tab \hskip-5pt 0 \tab \hskip-5pt 0 \tab \hskip-5pt 0 \tab \hskip-5pt 0 \cr \hskip-5pt 0 \tab \hskip-5pt 0 \tab \hskip-5pt 0 \tab \hskip-5pt 0 \tab \hskip-5pt 0 \tab \hskip-5pt 0 \tab \hskip-5pt 0 \tab \hskip-5pt 0 \tab \hskip-5pt 0 \tab \hskip-5pt 0 \cr} } \right]\cr\noalign{\vskip-6pt}\cr\tab\times\left[ {\matrix{ {\delta r_{x}^{c} } \cr {\delta r_{y}^{c} } \cr {\delta r_{z}^{c} } \cr {\delta v_{x}^{c} } \cr {\delta v_{y}^{c} } \cr {\delta v_{z}^{c} } \cr {\psi _{x} } \cr {\psi _{y} } \cr {\sin \psi _{z} } \cr {\cos \psi _{z} \minus 1} \cr} } \right] \plus \left[ {\matrix{ 0 \cr 0 \cr 0 \cr {\nabla _{x} } \cr {\nabla _{y} } \cr {\nabla _{z} } \cr 0 \cr 0 \cr 0 \cr 0 \cr} } \right] \plus \left[ {\matrix{ 0 \cr 0 \cr 0 \cr {\delta g_{x} } \cr {\delta g_{y} } \cr {\delta g_{z} } \cr 0 \cr 0 \cr 0 \cr 0 \cr} } \right] \plus \left[ {\matrix{ 0 \cr 0 \cr 0 \cr 0 \cr 0 \cr 0 \cr {\varepsilon _{x} } \cr {\varepsilon _{y} } \cr 0 \cr 0 \cr} } \right] \cr}](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary-alt:20160710031429-44803-mediumThumb-S0373463310000214_eqn8.jpg?pub-status=live)
3.2. Observations for Coarse Alignment
To estimate the initial heading with a Kalman Filter, external observations should be provided. Here DGPS positions are used as the observations. However, the observation equation in this development is different from that used in conventional GPS/INS integration because of the unknown heading misalignment. This unknown heading misalignment will lead to different projection error from the body frame to the navigation frame of the lever arm.
The positions observed from GPS and computed from INS have the following relationship:
where l b is the lever arm between the centre of the IMU and the antenna phase centre of GPS. Impose a perturbation in both sides of equation (9), and then the following relationship can be obtained:
where δr INSc is the same as δr c in equation (8); and δl b is the error of the lever arm which can be estimated during Kalman Filtering; and δC bc is the perturbation of the attitude matrix, and its expression will be derived as follows.
The attitude matrix C bc fulfills the following equation:
Supposing the error of C bc is introduced only by the platform misalignment, so the perturbation of C bc can be obtained as follows:
For small heading misalignment, the perturbation of C pc can be calculated as follows (Goshen-Meskin and Bar-Itzhack, Reference Goshen-Meskin and Bar-Itzhack1992):
Insert equation (13) into equation (12) and expand C bp as the multiplication of C cp and C bc, and then the following relationship holds:
Considering the correspondence indicated by equation (7), the perturbation of the attitude matrix, δC bc, for unknown heading misalignment is:
Insert equation (15) into equation (10), and the observation equation for unknown heading misalignment can be formulated as follows:
Ignore the two nonlinear items in equation (6), and insert it into equation (16), the matrix form of the observation equation can be obtained as:
![\eqalign{\delta r_{GPS}^{c} \equals \tab\left[ {\matrix{ {\delta r_{GPS_{x} }^{c} } \cr {\delta r_{GPS_{y}}^{c} } \cr {\delta r_{GPS_{z} }^{c} } \cr} } \right] \equals \left[ {\matrix{ 1 \tab 0 \tab 0 \tab 0 \tab 0 \tab 0 \tab 0 \tab {l_{z}^{c} } \tab { \minus l_{y}^{c} } \tab { \minus l_{x}^{c} } \cr 0 \tab 1 \tab 0 \tab 0 \tab 0 \tab 0 \tab { \minus l_{z}^{c} } \tab 0 \tab {l_{x}^{c} } \tab { \minus l_{y}^{c} } \cr 0 \tab 0 \tab 1 \tab 0 \tab 0 \tab 0 \tab {l_{y}^{c} } \tab { \minus l_{x}^{c} } \tab 0 \tab 0 \cr} } \right]\cr\cr\tab \times \left[ {\matrix{ {\delta r_{x}^{c} } \cr {\delta r_{y}^{c} } \cr {\delta r_{z}^{c} } \cr {\delta v_{x}^{c} } \cr {\delta v_{y}^{c} } \cr {\delta v_{z}^{c} } \cr {\psi _{x} } \cr {\psi _{y} } \cr {\sin \psi _{z} } \cr {\cos \psi _{z} \minus 1} \cr} } \right] \plus \left[ {\matrix{ {\delta l_{x}^{c} } \cr {\delta l_{y}^{c} } \cr {\delta l_{z}^{c} } \cr} } \right] \plus \left[ {\matrix{ {w_{x} } \cr {w_{y} } \cr {w_{z} } \cr} } \right]}](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary-alt:20160710031429-62749-mediumThumb-S0373463310000214_eqn17.jpg?pub-status=live)
where w is the measurement noise vector.
4. IN-MOTION FINE ALIGNMENT
The coarse alignment discussed in Section 3 will make the heading misalignment converge to a small angle. Under the condition of small platform misalignment, including small tilt misalignment and small heading misalignment, the fine alignment can proceed.
The following two approximations hold under the assumption of the small heading misalignment:
The error dynamics of small heading misalignment, ψz, can be derived from equation (A.6) in Appendix A by using equations (18a) and (18b) and by ignoring the high order small terms:
Considering equations (18a), (18b) and (19), the INS error dynamics for small platform misalignment and the corresponding observation equation can be obtained from equations (8) and (17), respectively, as follows:
![\eqalign{\tab {d \over {dt}}\left[ {\matrix{ {\delta r_{x}^{c} } \cr {\delta r_{y}^{c} } \cr {\delta r_{z}^{c} } \cr {\delta v_{x}^{c} } \cr {\delta v_{y}^{c} } \cr {\delta v_{z}^{c} } \cr {\psi _{x} } \cr {\psi _{y} } \cr {\psi _{z} } \cr} } \right] \cr\noalign{\vskip-6pt}\cr\tab\equals \left[ {\matrix{ 0 \tab {w_{ecz}^{c} } \tab {w_{ecy}^{c} } \tab 1 \tab 0 \tab 0 \tab 0 \tab 0 \tab 0 \cr { \minus w_{ecz}^{c} } \tab 0 \tab {w_{ecx}^{c} } \tab 0 \tab 1 \tab 0 \tab 0 \tab 0 \tab 0 \cr {w_{ecy}^{c} } \tab { \minus w_{ecx}^{c} } \tab 0 \tab 0 \tab 0 \tab 1 \tab 0 \tab 0 \tab 0 \cr 0 \tab 0 \tab 0 \tab 0 \tab \eqalign{\tab\lpar 2w_{iez}^{c} \cr\tab\plus w_{ecz}^{c} \rpar} \tab { \minus w_{ecy}^{c} } \tab 0 \tab { \minus f_{z}^{c} } \tab {f_{y}^{c} } \cr 0 \tab 0 \tab 0 \tab \eqalign{\tab\lpar 2w_{iez}^{c} \cr\tab\plus w_{ecz}^{c} \rpar} \tab 0 \tab \eqalign{\tab\lpar 2w_{iex}^{c} \cr\tab\plus w_{ecx}^{c} \rpar} \tab {f_{z}^{c} } \tab 0 \tab { \minus f_{x}^{c} } \cr 0 \tab 0 \tab 0 \tab {w_{ecy}^{c} } \tab \eqalign{\tab\lpar 2w_{iex}^{c} \cr\tab\plus w_{ecx}^{c} \rpar} \tab 0 \tab { \minus f_{y}^{c} } \tab {f_{x}^{c} } \tab 0 \cr 0 \tab 0 \tab 0 \tab 0 \tab 0 \tab 0 \tab 0 \tab \eqalign{\tab\lpar w_{iez}^{c} \cr\tab\plus w_{ecz}^{c} \rpar} \tab { \minus w_{ecy}^{c} } \cr 0 \tab 0 \tab 0 \tab 0 \tab 0 \tab 0 \tab \eqalign{\tab\lpar w_{iez}^{c} \cr\tab\plus w_{ecz}^{c} \rpar} \tab 0 \tab \eqalign{\tab\lpar w_{iex}^{c} \cr\tab\plus w_{ecx}^{c} \rpar} \cr 0 \tab 0 \tab 0 \tab 0 \tab 0 \tab 0 \tab {w_{ecy}^{c} } \tab \eqalign{\tab\lpar w_{iex}^{c} \cr\tab\plus w_{ecx}^{c} \rpar} \tab 0 \cr} } \right]\noalign{\vskip-6pt}\cr\cr\tab\times \left[ {\matrix{ {\delta r_{x}^{c} } \cr {\delta r_{y}^{c} } \cr {\delta r_{z}^{c} } \cr {\delta v_{x}^{c} } \cr {\delta v_{y}^{c} } \cr {\delta v_{z}^{c} } \cr {\psi _{x} } \cr {\psi _{y} } \cr {\psi _{z} } \cr} } \right] \plus \left[ {\matrix{ 0 \cr 0 \cr 0 \cr {\nabla _{x} } \cr {\nabla _{y} } \cr {\nabla _{z} } \cr 0 \cr 0 \cr 0 \cr} } \right] \plus \left[ {\matrix{ 0 \cr 0 \cr 0 \cr {\delta g_{x} } \cr {\delta g_{y} } \cr {\delta g_{z} } \cr 0 \cr 0 \cr 0 \cr} } \right] \plus \left[ {\matrix{ 0 \cr 0 \cr 0 \cr 0 \cr 0 \cr 0 \cr {\varepsilon _{x} } \cr {\varepsilon _{y} } \cr {\varepsilon _{z} } \cr} } \right] \cr}](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary-alt:20160710031429-62058-mediumThumb-S0373463310000214_eqn20.jpg?pub-status=live)
![\eqalign{\delta r_{GPS}^{c} \equals \tab \left[ {\matrix{ {\delta r_{GPSx}^{c} } \cr {\delta r_{GPSy}^{c} } \cr {\delta r_{GPSz}^{c} } \cr} } \right] \hskip-2pt\equals \hskip-2pt \left[ {\matrix{ 1 \tab 0 \tab 0 \tab 0 \tab 0 \tab 0 \tab 0 \tab {l_{z}^{c} } \tab { \minus l_{y}^{c} } \cr 0 \tab 1 \tab 0 \tab 0 \tab 0 \tab 0 \tab { \minus l_{z}^{c} } \tab 0 \tab {l_{x}^{c} } \cr 0 \tab 0 \tab 1 \tab 0 \tab 0 \tab 0 \tab {l_{y}^{c} } \tab { \minus l_{x}^{c} } \tab 0 \cr} } \right] \left[ {\matrix{ {\delta r_{x}^{c} } \cr {\delta r_{y}^{c} } \cr {\delta r_{z}^{c} } \cr {\delta v_{x}^{c} } \cr {\delta v_{y}^{c} } \cr {\delta v_{z}^{c} } \cr {\psi _{x} } \cr {\psi _{y} } \cr {\psi _{z} } \cr} } \right] \hskip-2pt \plus \hskip-2pt \left[ {\matrix{ {\delta l_{x}^{c} } \cr {\delta l_{y}^{c} } \cr {\delta l_{z}^{c} } \cr} } \right] \hskip-2pt \plus \hskip-2pt \left[ {\matrix{ {w_{x} } \cr {w_{y} } \cr {w_{z} } \cr} } \right] }](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary-alt:20160710031429-53410-mediumThumb-S0373463310000214_eqn21.jpg?pub-status=live)
5. KALMAN FILTERING CORRECTIONS OF ALIGNMENT
Based on the INS error dynamics and the observations discussed in Sections 3 and 4, the Kalman Filter is used to achieve the coarse and fine alignments. After the INS errors are estimated by use of the Kalman Filter, they are used to correct the navigation results. The correction approaches of position and velocity are the same for coarse and fine alignments, but the attitude correction is treated separately by different methods for coarse and fine alignments.
The position correction can be achieved by the following steps:
• Change the position representation from longitude, latitude and height to the ECEF frame and change the position error representation from the c frame to the ECEF frame;
• Correct the position error in the ECEF frame by using the following equation:
where 
and 
are the position vectors before and after corrections in the ECEF frame, and 
is the position error vector resolved in the ECEF frame;
• Change the position representation from ECEF frame to longitude, latitude and height.
The velocity correction is more straightforward and can be achieved as follows:
where 
and 
are the velocity vectors before and after the corrections in the c frame, and 
is the velocity error vector in the c frame.
The attitude correction of coarse alignment can be achieved by using the following approach. From equation (8), it can be found that, ψx, ψy, sinψz and cos ψz−1 can be estimated during the coarse alignment Kalman Filtering, so the direction cosine matrix from the c frame to the p frame, C cp, can be computed according to equation (6). Then, the attitude matrix, C bc, can be computed as:
where Ĉ bc and C bc are the attitude matrix before and after corrections.
For the fine alignment, ψx, ψy, and ψz can be estimated from the alignment Kalman Filter. Because equation (7) holds under the condition of small platform misalignment, during fine alignment C pc can be computed as:
Finally, the attitude correction equation of fine alignment can be obtained by inserting equation (25) into equation (24):
6. EXPERIMENTAL TESTS
A field test was carried out to verify the performance of the proposed initial alignment approach. In the experiment, CMIGITS II IMU (Martin and Detterich, Reference Martin and Detterich1997) was used to collect inertial data and the noise parameters of the inertial sensors are listed in Table 3. A Leica Gx1230 receiver with a Leica Leiat502 antenna was used to record the rover GPS data, and the base station receiver was a Leica Mc500 receiver with a Leica Leiat504 Leis antenna located on the top of the Electrical Engineering building, UNSW.
CMIGITS II IMU Noise Parameters.
The field test was carried out in an open space without GPS signal blockages about 20 km away from the GPS base station. Two kinds of manoeuvres were carried out to excite the errors of the inertial sensors and to accelerate the alignment process: small ‘eight-like’ movement and small circular movement.
In the remaining part, the experimental results from the proposed algorithm and the conventional algorithm proposed in Rogers (Reference Rogers1997), Kelley et al. (Reference Kelley, Katz and Bedoya1990), and Pham (Reference Pham1992) are compared. The test trajectory is shown in Figure 2, where the computed trajectories from the conventional alignment algorithm, the proposed alignment algorithm, and the DGPS reference are compared.
Trajectory comparison using different approaches: DGPS reference, the conventional algorithm, and the proposed algorithm.
Figure 3 shows the position estimation errors during the alignment process. It can be noted that the position errors converged to less than 0·5 m in about 75 seconds for the proposed alignment algorithm, while it took 250 seconds for the conventional algorithm.
North position errors (top), and east position errors (bottom) comparison between the proposed and the conventional algorithms during the initial alignment.
Figure 4 shows the heading estimation errors during the alignment process. From the zero value initialization, the estimated heading converged to the true heading. For the proposed algorithm, it took about 150 seconds to converge to less than 20 arc minutes; while for the conventional algorithm, it took about 560 seconds to converge to the same error level.
Heading errors comparison between the proposed and the conventional algorithms during the initial alignment.
Figure 5 illustrates the tilt angles estimation errors. The tilt angles, namely the roll and pitch angles, are initialized before Kalman Filtering using the gravity measurements in stationary base. Then the remaining errors of the tilt angles are further eliminated by the coarse and fine alignments. From Figure 5, it can be noted that the convergences of the tilt angle errors by using the proposed algorithm are also much faster than that by using the conventional algorithm.
Roll errors (top), and pitch errors (bottom) comparison between the proposed and the conventional algorithms during the initial alignment.
Table 4 compares the alignment accuracy between the proposed alignment approach and the conventional alignment approach. The alignment performance of the proposed approach has obvious improvements when compared with that of the conventional approach.
Alignment accuracy of the proposed and conventional algorithms.
7. CONCLUDING REMARKS
In this paper, a new two-stage Kalman Filtering mechanism has been developed for the initial alignment of low-cost INS with the aid of GPS. Based on this framework, the following novel work has been completed:
• An INS error dynamics model accounting for unknown initial heading error has been developed for coarse alignment.
• The corresponding observation equation accounting for unknown initial heading error has been developed for coarse alignment.
• An attitude error correction method of Kalman Filtering for coarse alignment has been proposed.
Experimental test results have demonstrated that this novel alignment mechanism can accelerate the alignment process and improve the alignment accuracy.
APPENDIX A. INS ERROR DYNAMICS WITH UNKNOWN HEADING ERROR
A.1. Attitude Error Dynamics
According to Benson (Reference Benson1975), the torquing rate of the mathematical platform of a strapdown INS is:
where ε is the drift vector of gyros.
The rotation rate of the platform frame p relative to the computed frame c can be expressed as follows:
Insert equation (A.1) into equation (A.2), and then obtain:
where ωicp can be expressed as:
Considering equation (A.4), equation (A.3) can be transformed into:
According to equation (6), equation (A.5) can be expanded as:
![\eqalign{\left[ {\matrix{ {w_{cpx}^{p} } \cr {w_{cpy}^{p} } \cr {w_{cpz}^{p} } \cr} } \right] \equals \tab \left[ {\matrix{ {1 \minus \cos \psi _{z} } \tab {\sin \psi _{z} } \tab {\psi _{y} } \cr {\sin \psi _{z} } \tab {1 \minus \cos \psi _{z} } \tab { \minus \psi _{x} } \cr { \minus \psi _{x} \sin \psi _{z} \minus \psi _{y} \cos \psi _{z} } \tab { \minus \psi _{x} \cos \psi _{z} \plus \psi _{y} \sin \psi _{z} } \tab 0 \cr} } \right]\cr\tab\times\left[ {\matrix{ {w_{icx}^{c} } \cr {w_{icy}^{c} } \cr {w_{icz}^{c} } \cr} } \right] \minus \left[ {\matrix{ {\varepsilon _{x} } \cr {\varepsilon _{y} } \cr {\varepsilon _{z} } \cr} } \right]}](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041008052-0960:S0373463310000214_eqnA6.gif?pub-status=live)
Because the tilt misalignment angles are small angles, so the following relationship holds (Benson, Reference Benson1975):
Because ψz is an unknown angle, sin ψz and cosψz−1 may be large initially. But the change rate of sinψz and cosψz−1 are small, so the following relationships will hold:
Combining equations (A.7)~(A.10), the INS attitude error dynamics with unknown heading error can be obtained as follows:
![\hskip-6pt\eqalign{{d \over {dt}}\left[ {\matrix{ {\psi _{x} } \cr {\psi _{y} } \cr {\sin \psi _{z} } \cr {\cos \psi _{z} \minus 1} \cr} } \right] \equals \left[ {\matrix{ 0 \tab {w_{icz}^{c} } \tab { \minus w_{icy}^{c} } \tab {w_{icx}^{c} } \cr { \minus w_{icz}^{c} } \tab 0 \tab {w_{icx}^{c} } \tab { \minus w_{icy}^{c} } \cr 0 \tab 0 \tab 0 \tab 0 \cr 0 \tab 0 \tab 0 \tab 0 \cr} } \right] \left[ {\matrix{ {\psi _{x} } \cr {\psi _{y} } \cr {\sin \psi _{z} } \cr {\cos \psi _{z} \minus 1} \cr} } \right] \minus \left[ {\matrix{ {\varepsilon _{x} } \cr {\varepsilon _{y} } \cr 0 \cr 0 \cr} } \right] }](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041008052-0960:S0373463310000214_eqnA11.gif?pub-status=live)
From equation (A.11), it can be noted that sin ψz and cos ψz−1 are modelled as random constants during the coarse alignment.
A.2. Velocity Error Dynamics
According to Benson (Reference Benson1975), the INS velocity error dynamics coordinated in the c frame is as follows:
where f p can be expressed as:
Considering equation (A.13), equation (A.12) can be transformed into:
where ▿ represents the error vector of accelerometers and is equal to δf p, and C cp is computed in equation (6). Equation (A.14) is the INS velocity error dynamics with unknown heading error. When substituting (ψ×) for (I−C cp), equation (A.14) is exactly the same as the Psi velocity error dynamics (Benson, Reference Benson1975) with small heading error.
According to equation (6), equation (A.14) can be expanded as follows:
![\eqalign{ {d \over {dt}}\left[ {\matrix{ {\delta v_{x}^{c} } \cr {\delta v_{y}^{c} } \cr {\delta v_{z}^{c} } \cr} } \right] \equals \tab \left[ {\matrix{ {\cos \psi _{z} \minus 1} \tab {\sin \psi _{z} } \tab {\psi _{y} } \cr { \minus \sin \psi _{z} } \tab {\cos \psi _{z} \minus 1} \tab {\psi _{x} } \cr {\psi _{x} \sin \psi _{z} \plus \psi _{y} \cos \psi _{z} } \tab { \minus \psi _{x} \cos \psi _{z} \plus \psi _{y} \sin \psi _{z} } \tab 0 \cr} } \right]\left[ {\matrix{ {f_{x}^{c} } \cr {f_{y}^{c} } \cr {f_{z}^{c} } \cr} } \right] \cr \ \ \ \ \ \ \ \ \ \ \ \ \ \ \tab \minus \left[ {\matrix{ 0 \tab { \minus \left( {2w_{iez}^{c} \plus w_{ecz}^{c} } \right)} \tab {2w_{iey}^{c} \plus w_{ecy}^{c} } \cr {2w_{iez}^{c} \plus w_{ecz}^{c} } \tab 0 \tab { \minus \left( {2w_{iex}^{c} \plus w_{ecx}^{c} } \right)} \cr { \minus \left( {2w_{iey}^{c} \plus w_{ecy}^{c} } \right)} \tab {2w_{iex}^{c} \plus w_{ecx}^{c} } \tab 0 \cr} } \right]\cr\tab\times\left[ {\matrix{ {\delta v_{x}^{c} } \cr {\delta v_{y}^{c} } \cr {\delta v_{z}^{c} } \cr} } \right] \plus \left[ {\matrix{ {\nabla _{x} } \cr {\nabla _{y} } \cr {\nabla _{z} } \cr} } \right] \plus \left[ {\matrix{ {\delta g_{x} } \cr {\delta g_{y} } \cr {\delta g_{z} } \cr} } \right] \cr}](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary-alt:20160710031429-86600-mediumThumb-S0373463310000214_eqnA15.jpg?pub-status=live)
Although the equation for 
includes a nonlinear item, considering the facts that both ψx and ψy are small quantities, it can be approximated as:
Rearrange (A.15) by using (A.16), and then the INS velocity error dynamics with unknown heading error can be obtained as follows:
![\eqalign{ \tab {d \over {dt}}\left[ {\matrix{ {\delta v_{x}^{c} } \cr {\delta v_{y}^{c} } \cr {\delta v_{z}^{c} } \cr} } \right] \cr\tab\equals \left[ {\matrix{ 0 \tab { \minus \left( {2w_{iez}^{c} \plus w_{ecz}^{c} } \right)} \tab {2w_{iey}^{c} \plus w_{ecy}^{c} } \tab 0 \tab { \minus f_{z}^{c} } \tab {f_{y}^{c} } \tab {f_{x}^{c} } \cr {2w_{iez}^{c} \plus w_{ecz}^{c} } \tab 0 \tab { \minus \left( {2w_{iex}^{c} \plus w_{ecx}^{c} } \right)} \tab {f_{z}^{c} } \tab 0 \tab { \minus f_{x}^{c} } \tab {f_{y}^{c} } \cr { \minus \left( {2w_{iey}^{c} \plus w_{ecy}^{c} } \right)} \tab {2w_{iex}^{c} \plus w_{ecx}^{c} } \tab 0 \tab { \minus f_{y}^{c} } \tab {f_{x}^{c} } \tab 0 \tab 0 \cr} } \right] \cr \tab \times \left[ {\matrix{ {\delta v_{x}^{c} } \cr {\delta v_{y}^{c} } \cr {\delta v_{z}^{c} } \cr {\psi _{x} } \cr {\psi _{y} } \cr {\sin \psi _{x} } \cr {\cos \psi _{x} \minus 1} \cr} } \right] \plus \left[ {\matrix{ {\nabla _{x} \plus \delta g_{x} } \cr {\nabla _{y} \plus \delta g_{y} } \cr {\nabla _{z} \plus \delta g_{z} } \cr} } \right] \cr}](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary-alt:20160709104234-13342-mediumThumb-S0373463310000214_eqnA17.jpg?pub-status=live)
ACKNOWLEDGEMENTS
The first author is sponsored by the Chinese Scholarship Council for his studies in Australia.
