Optimization-based alignment for inertial navigation systems: Theory and algorithm

Aerospace Science and Technology 15 (2011) 1–17

Contents lists available at ScienceDirect

Aerospace Science and Technology www.elsevier.com/locate/aescte

Optimization-based alignment for inertial navigation systems: Theory and algorithm ✩ Meiping Wu, Yuanxin Wu ∗ , Xiaoping Hu, Dewen Hu Laboratory of Inertial Technology, Department of Automatic Control, College of Mechatronics and Automation, National University of Defense Technology, Changsha, Hunan 410073, PR China

a r t i c l e

i n f o

a b s t r a c t

Article history: Received 8 June 2008 Received in revised form 4 May 2010 Accepted 7 May 2010 Available online 15 May 2010 Keywords: Alignment Eigenvector Global observability Inertial navigation system

Inertial navigation system (INS) necessitates an alignment stage to determine the initial attitude at the very start. A novel alignment approach is devised by way of an optimization method, in contrast to the existing alignment methods, e.g., gyrocompassing and filtering techniques. This paper shows that the INS attitude alignment can be equivalently transformed into a “continuous” attitude determination problem using infinite vector observations. It reveals an interesting link between these two individual problems that has been studied in parallel for several decades. The INS alignment is heuristically established as an optimization problem of finding the minimum eigenvector. Sensitivity analysis with respect to sensor biases is made and explicit error equations are obtained for a special stationary case. Simulation studies and experiment tests favorably demonstrate its rapidness, accuracy and robustness. The proposed approach is inherently able to cope with any large angular motions, as well as highfrequency translational motions. By inspecting the constant initial Euler angles, it could alternatively be used to detect the existence of significant sensor biases. © 2010 Elsevier Masson SAS. All rights reserved.

1. Introduction Inertial navigation system (INS) is a system of calculating velocity by integration of the total acceleration and computing position by integration of the resultant velocity. As a dead-reckoning navigation method, the INS necessitates an alignment stage to determine the initial conditions prior to navigation operation. Initial alignment is vitally important for any INS, because the performance of the INS is largely decided by the accuracy and rapidness of the alignment process. The term “alignment” usually implies attitude alignment since initial velocity and position can be easily identified, e.g., using the global satellite system (GPS) [6,26]. The existing alignment methods are typically composed of two steps, viz., a coarse alignment stage using the analytic method, followed by a fine alignment stage by way of gyrocompass or optimal estimation schemes [6,8,9,25,26]. The analytic method approximately regards the vehicle attitude (time-varying due to ubiquitous disturbing) as constant. It is coarse and needs to be refined by the relatively complicated gyrocompass or Kalman filtering methods. The gyrocompassing and Kalman filtering techniques are founded on the simplified linear system models [6,26], so are their theoretical analyses [1–4,7,12,14,16,20–22]. As a result of model simplification, the alignment performance is compromised, e.g., slow convergence and vulnerability to disturbances. This paper is dedicated to a novel optimization-based alignment (OBA) approach. It is an enlightened product by the global observability analysis [24,28,30,31] and distinctive from previous methods in principle. The INS alignment is heuristically established as an optimization problem of finding the minimum eigenvector. No model simplification has been made throughout the derivation and the novel OBA approach has such benefits as rapidness, accuracy and robustness. It does not need a parameter-tuning process that is inevitable for either gyrocompassing (pole placement) or Kalman filtering (error covariance predetermination and small initial value) method. The main contribution of the paper is a new way of thought to address the INS alignment, besides the gyrocompassing and filtering methods. Interestingly, the OBA approach also provides insights into attitude alignment’s connection to the attitude determination problem [5,27] that evolves independently on a parallel road. The attitude determination problem using multiple vector observations was formulated ✩ This work was supported in part by National Natural Science Foundation of China (60604011) and Foundation for the Author of National Excellent Doctoral Dissertation of PR China (FANEDD 200897). Corresponding author. Tel./fax: +86 0731 4531122. E-mail addresses: [email protected] (M. Wu), [email protected] (Y. Wu), [email protected] (X. Hu), [email protected] (D. Hu).

*

1270-9638/$ – see front matter doi:10.1016/j.ast.2010.05.004

© 2010

Elsevier Masson SAS. All rights reserved.

2

M. Wu et al. / Aerospace Science and Technology 15 (2011) 1–17

by Wahba [27] as a constrained least square estimation of a constant direction cosine matrix (DCM). Davenport re-parameterized DCM by quaternion and devised the equivalent q-method to attack the so-called single-frame (or constant-attitude) attitude determination problem [23]. In contrast, the INS alignment requires determining the non-constant attitude in real time. It is shown that the INS alignment can be transformed into a problem of determining the initial attitude at the beginning of alignment that is right a constant quantity. It thus allows us to solve the INS alignment problem using the single-frame q-method algorithm in the attitude determination community. It founds the main idea of the OBA approach. The content is organized as follows. Section 2 presents the main principle in mathematical language and poses the alignment problem as a minimization procedure. Sensitivity analysis is made with respect to sensor biases and explicit error equation is obtained for a special case. Section 3 gives the algorithmic description of OBA and incorporates practical considerations as well. Simulation studies and experiment tests are performed for both stationary and angular/translational swaying alignment in Section 4. Discussions and conclusions are made in Section 5. 2. Optimization-based alignment: Modeling and analysis 2.1. Mathematical formulation The estimability of the gyroscope/accelerometer biases depends on maneuvers [3,21,30], specifically they are unobservable for a stationary INS [2,30], so we do not take the sensor biases into account in the sequel. Without the loss of generality, the local-level frame N is selected as the navigation reference frame, with x directing north, y upward vertical and z east. Denote by B the INS body frame, by E the Earth frame and by I some chosen inertial frame. Using the gyroscopes/accelerometers outputs of an INS at some fixed location (latitude L and height h), the ground velocity v n = [ v N v U v E ] T (north, up and east) and the body attitude matrix with respect to the navigation frame C nb can be obtained from the kinematic equations as [13,26]

C˙ bn = C bn v˙ = n

C bn









b b b n , ωnb × , ωnb = ωib − C nb ωnie + ωen

(1)

b

(2)



n ie

n en

f − 2ω + ω



n

n

×v +g ,

b b where ωnb is angular rate of the body frame with respect to the navigation frame, expressed in the body frame, ωib is the body angular n T rate measured by gyroscopes in the body frame, ωie = [Ω cos L Ω sin L 0] is the Earth rotation rate in the navigation frame with Ω being n = [ v E /( R E + h) v E tan L /( R E + h) − v N /( R N + h)] T is angular rate of the navigation frame with respect to the Earth the Earth rate, ωen frame, expressed in the navigation frame, R E and R N are respectively the transverse and meridian radii of curvature, h is the altitude, f b is the specific force measured by accelerometers in the body frame, g n is the gravity vector in the navigation frame. The 3 × 3 skew symmetric matrix (·×) is so defined that the cross product satisfies a × b = (a×)b for arbitrary two vectors. Obviously it is satisfied that (a×) T = −(a×). Note that an attitude matrix (or DCM) belongs to SO(3) manifold, i.e., special orthogonal matrix (3 × 3) with unit determinant. Consider a finite time interval [0, t f ]. The INS alignment problem is equivalent to the unique determination of current attitude using gyroscope and accelerometer outputs. Suppose that the latitude and the height are known by survey or GPS, etc. For many cases where the ground velocity is known or able to be measured to high accuracy, e.g., it is identically zero when the INS is at rest, we will show that the alignment could be solved in an optimization way. Next we seek the analytic form of C nb , which is intractable starting from (1) since C nb also appears on the right side. Designate an n inertial frame I to be the local-level frame at t 0 , i.e., I  N (0). Noticing that ωnie , ωen and g n are all known quantities using the measured velocity and known position, we have

C˙ bi = C bi C˙ ni = C ni





b ωib × ,





ωnin × ,

(3)

solutions to which are

C ib = φb (0, t )C ib (0) = φb (0, t )C nb (0), C in = φn (0, t )C in (0) = φn (0, t ).

(4)

b In the above, φb (0, t ) and φn (0, t ) are known attitude matrices as functions of ωib and ωnin . They encode the attitude changes of the body frame and the navigation frame from time 0 to t, respectively. The analytic form of the current body attitude matrix is thus given by

C nb = C ib C ni = φb (0, t )C nb (0)φnT (0, t ).

(5)

Substituting (5) into (2) and reorganizing the terms yield

β(t ) = C bn (0)α (t ),

t ∈ [0, t f ],

(6)

where









n × v n − gn . α (t )  φbT (0, t ) f b and β(t )  φnT (0, t ) v˙ n + 2ωnie + ωen

Right multiplying as

(7)

α T (t ) and integrating over the whole time interval, (6) gives a least-square solution to the initial body attitude matrix

M. Wu et al. / Aerospace Science and Technology 15 (2011) 1–17

 t f

t f C bn (0)

T

=

− 1 T

β(t )α (t ) dt ·

α (t )α (t ) dt

0

3

(8)

,

0

tf

 tf

when the 3 × 3 matrix 0 α (t )α (t ) dt is nonsingular. According to Lemma A.1 in Appendix A, 0 α (t )α T (t ) dt is nonsingular if and only if α (t ) on the interval [0, t f ] spans the whole three-dimensional space. For a stationary alignment case, the condition is naturally satisfied as the Earth rotates with time and α (t ) depicts a cone in inertial space; for dynamic cases, it would be satisfied much more easily. The current attitude matrix C bn is readily obtained using (8) and (5). Since α (t ) and β(t ) are known vectors in (6), the INS alignment problem is equivalent to a “continuous” attitude determination problem using vector observations [5,27] in that the reference and observation vectors are infinite over the continuous time interval [0, t f ]. This reveals a fundamental link between the two seemingly independent problems that have been studied in parallel for several decades. However, the closed-form solution (8) is apt to error disturbances, as a result of which C bn (0) thus obtained usually deviates from being an attitude matrix. Quaternion is a simpler parameterization of rotation than attitude matrix [26,29]. Next we turn to use a four-element unit quaternion q = [s η T ] T , where s is the scalar part and η is the vector part, to encode the initial body attitude matrix C nb (0). The relationship between these two rotation parameters is T





C bn (0) = s2 − η T η I + 2ηη T − 2s(η×).

(9)

Eq. (6) can be rewritten as

β(t ) = q ◦ α (t ) ◦ q∗ ,

t ∈ [0, t f ],

(10)

where, with a little abuse of notation, α (t ) and β(t ) here are two vector quaternions with zero scalar parts. Superscript ∗ denotes the conjugate quaternion operator and ◦ denotes quaternion multiplication defined as



q1 ◦ q2 = s1 s2 − η1T η2

( s 1 η 2 + s 2 η 1 + η1 × η2 ) T

T

(11)

.

Express (11) in the matrix form +



q1 ◦ q2 = [q1 ]

s2

η2

+





−

= [q2 ]

s1

η1

 (12)

.

−

The two matrices, [q] and [q], are respectively defined by



+

[q] 

s

−η T

η sI + (η×)



−



[q] 

,

−η T

s

η sI − (η×)

 (13)

.

Eq. (10) is a quadratic equation in attitude quaternion, which can be transformed into a linear equation as in [29]. Multiplying both sides from right by q yields

β(t ) ◦ q − q ◦ α (t ) = 0,

t ∈ [0, t f ].

(14)

Using (12), the matrix form produces a linear equation in q as +  −  β(t ) − α (t ) q = 0,



t ∈ [0, t f ].

(15)

Following the famous q-method algorithm for the optimal attitude determination [17,23], we could pose the alignment problem as a minimization procedure, i.e.,

t f min q

 +  −  2 β(t ) − α (t ) q dt = min q T

t f

q

0



+  −   +  −  T β(t ) − α (t ) β(t ) − α (t ) dt q  min q T K q, q

(16)

0

subject to

q T q = 1.

(17)

The definition of the 4 × 4 real symmetric matrix K is obvious. Therefore the alignment problem is equivalent to determining an unit quaternion that minimizes (16) and the current attitude matrix is thus obtained using (5), (9) and the optimal quaternion. It can be proved that the optimal quaternion is exactly the normalized eigenvector of K belonging to the smallest eigenvalue, see e.g. [23]. A simple explanation is provided below for easy reference. The optimal quaternion satisfying (16) and (17) can be solved through the Lagrange multiplier method. Define the following gain function





L (q) = q T K q − λ q T q − 1 ,

(18)

where λ is a real scalar. Then the original constrained problem is transformed to an unconstrained optimization. Differentiating (18) with respect to q at the extremes gives

4

M. Wu et al. / Aerospace Science and Technology 15 (2011) 1–17

( K − λ I )q = 0

(19)

indicating that the optimal quaternion must be an normalized eigenvector and λ is the corresponding eigenvalue of K . Substitute into (18), we have

L (q) = λ.

(20)

Therefore the minimization will be achieved if only q is chosen to be the normalized eigenvector corresponding to the smallest eigenvalue of K . Determined by the inherent property of the alignment problem, the smallest eigenvalue is unique. 2.2. Sensitivity analysis When sensor errors exist, matrix K will deviate from the true value, so are the eigenvectors [15]. It is illuminating to investigate the change or sensitivity of K  s minimum eigenvector as a function of sensor errors, e.g., gyroscope/accelerometer biases. The analysis is somewhat straightforward but lengthy. Here the main result is briefly reported and readers are referred to Appendix A.3 for the details. Consider a stationary alignment for a navigation-grade INS and assume the body frame coincides with the navigation frame. The error Euler angles for the initial body attitude matrix C nb (0) are given up to one order of time by

⎡

⎤

⎡ εx ⎤ ⎢ εz sec L ∇z tan L ⎥ ⎢ ε2 ⎥ y ⎥ ⎣ δψ(0) ⎦ = ⎢ ⎣ Ω − g ⎦ − ⎣ 2 ⎦ t. ∇x δθ(0) 0 ⎡

δφ(0)

⎤

− ∇gz

(21)

g

The error Euler angles for the current body attitude matrix C nb are given by

⎡

δφ

⎤

⎡

− ∇gz

⎢ ⎣ δψ ⎦ = ⎢ εz sec L − ∇z tan L g ⎣ Ω ∇x δθ g

⎤

⎡ εx ⎤ ⎥ ⎢ ε2 ⎥ ⎥ + ⎣ y ⎦ t. ⎦ 2

(22)

0

We see that (22) is a bit different from the well-known error equation, see, e.g., [26], which, irregardless of sign difference, is exactly the zero-order term above. Additionally, it includes a term linearly increasing with time. Same characteristics are also observed in (21) that has an opposite slope. For general cases where the body frame is not aligned with the navigation frame, the error equations would be a linear combination of the sensor biases with much more complex form [26]. 3. Algorithm implementation 3.1. Recursive algorithm Now an efficient algorithm could be readily constructed to solve the alignment problem in a recursive manner. Note that the matrix K can be recursively computed using its previous value. Suppose T is the time interval indexed by k (= 0, 1, 2, . . .), then the current time t = kT . The algorithm is summarized below. Initialization: Set k = 0. Let φb (0, t ) = φn (0, t ) = I and K = 04×4 ; Step 1: Set k = k + 1; b Step 2: Update φb (0, (k − 1) T ) to φb (0, kT ) using the gyroscope outputs ωib and φn (0, (k − 1) T ) to φn (0, kT ) using ωnin (see (4));

Step 3: Compute α (kT ) and β(kT ) using the accelerometer outputs f b and the velocity measurements (see (7)); Step 4: Update K ((k − 1) T ) to K (kT ) with the resultant α (kT ) and β(kT ) according to (13) and (16), i.e.,

K (kT ) =

kT 

+  −   +  −  T β(t ) − α (t ) β(t ) − α (t ) dt

0

   +  − T  +  −  β(kT ) − α (kT ) ; ≈ K (k − 1) T + T · β(kT ) − α (kT )

(23)

Step 5: Determine the initial attitude quaternion q by calculating the normalized eigenvector of K (kT ) belonging to the smallest eigenvalue; Step 6: Obtain the attitude matrix at current time using (5) and (9); Step 7: Go to Step 1 until the end. The transformations between attitude quaternion and attitude matrix could be saved if we use only one parameterization, e.g., quaternion, throughout the paper. The modifications to the algorithm accordingly are straightforward. The usage of mixed parameterizations is just for easy presentation. 3.2. Practical considerations We have so far assumed known ground velocity of the INS and it is unpractical in many cases. The above solution becomes in principle invalid when the INS undergoes considerable translational motions, e.g., onboard a ship on the sea. In such a case, the ground velocity cannot be measured (with enough accuracy).

M. Wu et al. / Aerospace Science and Technology 15 (2011) 1–17

5

Fig. 1. Alignment result for a completely stationary INS. Dotted line is for KALMAN, dashed for OBA and solid for OBAUF.

Fortunately, the solution is able to be upgraded to isolate unknown but high-frequency translational motions with the help of a lown pass filter. In specific, if the two components of β(t ) in (7), namely φnT (0, t )( v˙ n + (2ωnie + ωen ) × v n ) and φnT (0, t ) g n , are separable in frequency domain (the latter is very slowly changing), we could use a well-designed low-pass filter to filter out the unknown quantity n φnT (0, t )( v˙ n + (2ωnie + ωen ) × v n ). Similar ideas were adopted in [10,18]. Denote the low-pass filter by F (·) and apply it to the both sides of (6),

         n ˜ t )  F −φnT (0, t ) g n = F φnT (0, t ) v˙ n + 2ωn + ωen × v n + F −φnT (0, t ) g n β( ie     = F β(t ) = C bn (0) F α (t )  C bn (0)α˜ (t ), t ∈ [0, t f ],

(24)

˜ t ) are obvious. The rest of the algorithm keeps the same. ˜ (t ) and β( where the definitions of α 4. Simulations and experiments In this section, we carry out simulations and experiments to evaluate the optimization-based alignment algorithms, with and without using a low-pass filter. The low-pass filter is selected for illustration to be a 100-order equiripple finite impulse filter (FIR) with the pass frequency at 0.01 Hz and the stop frequency at 0.5 Hz. As a reference, we also implement the well-known indirect Kalman alignment method as described in the classical textbooks, e.g., [26]. It is here composed of five states, namely, three angular errors and two horizontal velocity errors. The coarse alignment lasts 20 seconds followed by the Kalman fine process. 4.1. Simulation study We design simulations mimicking typical alignment conditions: the first two for a stationary INS, the other one for a swaying INS without translations and another one for a swing INS with high-frequency translational motions. The INS is assumed to be located at latitude 28.2204 deg and zero height. It has gyroscopes with drift rate 0.01 deg/h and accelerometers with bias 5 × 10−5 m/s2 . The sampling rate is 100 Hz. The noise standard deviations are 0.005 deg/h and 2.5 × 10−5 m/s2 for gyroscopes and accelerometers, respectively. The initial three Euler angles from the reference frame to the body frame are respectively 20 deg (roll), 50 deg (yaw) and 30 deg (pitch). The rotation sequence is first around y axis (yaw, denoted by ψ ), followed by z axis (pitch, denoted by θ ) and then x axis (roll, denoted by φ ), that is to say,

⎡

1

C nb = ⎣ 0 0

0 cos φ

0

⎤⎡

cos θ

sin φ ⎦ ⎣ − sin θ

− sin φ cos φ

0

⎤⎡

sin θ

0

cos θ

0⎦⎣

0

1

cos ψ

0

− sin ψ

0

1

0

sin ψ

0

cos ψ

⎤ ⎦.

(25)

Firstly, consider the INS to be completely stationary. Fig. 1 presents the alignment result of three angles for the first 100 s, by the Kalman method (KALMAN), the OBA method and the optimization-based alignment method using a filter (OBAUF), respectively. As expected, the difference between OBA and OBAUF is negligible. Two level angles, roll and pitch, reach their respective true values almost instantaneously. The OBAUF needs a couple of seconds to accumulate data for the 100-order filter and the Kalman method spends 20 seconds in the coarse stage. The yaw angle stabilizes at 50.02 deg in 10 seconds for OBA and OBAUF but 50 seconds for KALMAN. Because of the weak observability, it takes longer time for the yaw angle to converge. The slowly climbing trends in level angles as well as the biased yaw are owed to the existence of gyroscope/accelerometer biases that has not been taken into account in the algorithms. Note that KALMAN also has a similar behavior.

6

M. Wu et al. / Aerospace Science and Technology 15 (2011) 1–17

Fig. 2. Euler angle estimates for a stationary case with zero initial angles.

Fig. 3. Initial Euler angle estimates.

In specific, we carry out a special stationary case with zero initial Euler angles, to validate the theoretical sensitivity analysis in Section 2. Fig. 2 gives the results for OBAUF and KALMAN for 2000 s, along with the analytic error curve as given in (22). Here (22) is instantiated as

⎡

⎤ ⎡ ⎤ 1.3838 × 10−6 −2.9233 × 10−4 ⎣ δψ ⎦ = ⎣ ⎦ + ⎣ 1.3983 × 10−6 ⎦ t . 0.0430 δφ δθ

⎤

⎡

2.9233 × 10−4

(26)

−2.0889 × 10−9

The OBAUF error agrees nicely with the analysis result. Similar phenomenon is also observed for KALMAN. In other words, the analytic equation (22) could potentially be used to characterize the error behavior of Kalman alignment method.1 It was unavailable before because

1 The underlying reason may be owed to fact that Kalman is in fact a recursive least square method [11], namely, a kind of unconstrained optimization in contrast to constrained one in this paper.

M. Wu et al. / Aerospace Science and Technology 15 (2011) 1–17

7

Fig. 4. Estimates along with the true values of three angles for the first 100 s when the INS performs angular swinging with no translations.

Fig. 5. Estimate errors of Euler angles for a swaying INS with no translations.

the Kalman method itself does not have analytic error formulation at all. Fig. 3 plots the initial Euler angles by OBAUF, as well as the analytic curve in (21)

⎡

⎤ ⎡ ⎤ 1.3839 × 10−6 −2.9233 × 10−4 ⎣ δψ(0) ⎦ = ⎣ ⎦ − ⎣ 1.3983 × 10−6 ⎦ t . 0.0430 δφ(0)

⎤

⎡

2.9233 × 10−4

δθ(0)

(27)

0

Nice agreement is also observed. Note that (26) and (27) have almost the same slope with opposite sign. For our parameter settings, the conditions for (63)–(64) are satisfied and the slope is approximately

εx ± 2

εy 2





0 = ± 1.3889 × 10−6

1.3889 × 10−6

0



(28)

which implies that we might instead estimate εx and ε y using the actual slopes. For instances, excluding the transient period (0–10 s), the fitted slope of roll error in Fig. 3 is −1.3214 × 10−6 , or equivalently, εx = 2 × 1.3214 × 10−6 × 3600 = 0.0095 deg/h; the fitted slope of yaw error is 1.5695 after eliminating the transient period (0–1000 s), so ε y = 2 × 1.5695 × 10−6 × 3600 = 0.0113 deg /h. Of course, we are

8

M. Wu et al. / Aerospace Science and Technology 15 (2011) 1–17

Fig. 6. Estimates of the initial Euler angles for OBA for a swaying INS with no translations.

Fig. 7. The incremental angles and incremental velocities in one time interval when the INS is swaying with angular and translational motions.

able to derive similar information from Fig. 2, but it will be impossible when angular motion is significant because the trend is submerged in and cannot be distinguished from the real angular changes. Fortunately, it is another story for the initial Euler angles in Fig. 3 as they are always constant quantities whether angular motion exists or not. Secondly, we consider the INS angularly swaying without translational motions. Keep the initial angles unchanged and set the three angle rates to 0.05 sin(0.1π t ), 0.05 sin(0.1π t + 2π /3) and 0.05 sin(0.1π t + 4π /3) in radian per second, respectively. The angle estimates along with the true value are plotted in Fig. 4 for the first 100 seconds. We see that the true attitude angles are varying as large as 15 deg in magnitude. The curves for OBA and OBAUF are too close with the true value to discriminate. The yaw angle of KALMAN converges comparatively rather slowly. Fig. 5 gives the estimate errors for 200 seconds by subtracting the true value from the estimates. Similar characteristics with those in the stationary case can be identified for OBA and OBAUF because they are inherently not influenced by any angular motions. In specific, the two level angles errors climb slowly due to the existence of sensor biases; the yaw error fluctuates about 0.02 deg with reduced magnitude as time going. It is quite different for KALMAN in that its yaw error is not well below 0.1 deg until the end. Comparing Fig. 5 with Fig. 1 in the yaw angle implies that KALMAN is sensitive to angular motions while OBA and OBAUF are both

M. Wu et al. / Aerospace Science and Technology 15 (2011) 1–17

9

Fig. 8. Alignment results for the INS with both angular and translational motions.

Fig. 9. Alignment results for the INS with both angular and translational motions.

not. Estimates of the constant initial Euler angles by OBA are plotted in Fig. 6. It indicates that the new approach could alternatively serve as an evaluation tool of INS calibration in that the existence of significant biases would lead to apparent climbing trends in the estimates of initial Euler angles. Note that the initial yaw angle also has a bias of 0.02 deg. Thirdly, the INS is swaying with both angular and translational motions. Keep the above angular motions unchanged and additionally add vibrating translations. We set zero initial ground velocity and the three ground velocity rates to 0.05 sin(4π t ), 0.05 sin(4π t + 2π /3) and 0.05 sin(4π t + 4π /3) meter per squared second, respectively. The incremental angles and incremental velocities are presented in Fig. 7, the alignment results are given in Fig. 8 and the angle errors are plotted in Fig. 9. The translation imposes considerable vibrating effect on the OBA method because the zero ground velocity assumption is not valid any more. OBAUF yields satisfactory result with the aid of the low-pass filter. The yaw angle error reduces to 0.02 deg well before 300 s for OBAUF, but is still about 0.05 deg for OBA and KALMAN at 300 s. The initial attitude angles by OBA and OBAUF are given Fig. 10. The climbing trends of two level angles are apparent and the yaw behavior is similar with Fig. 9.

10

M. Wu et al. / Aerospace Science and Technology 15 (2011) 1–17

Fig. 10. Initial angle estimates by OBA and OBAUF for the INS with both angular and translational motions.

Fig. 11. Raw data: incremental angles (rad) of gyroscopes and incremental velocities (m/s) of accelerometers when the INS is rotating on a three-axis table.

The above observations show that KALMAN is apt to disturbance of angular motions and OBA is sensitive to translational motions. OBAUF could cope with both large angular swinging and high-frequency translational swinging and yields satisfactory alignment estimates, especially the yaw angle. 4.2. Experimental results This section reports our experimental result by examining a real INS system consisting of three ring laser gyroscopes with drift rate 0.02 deg/h (1σ ) and three quartz accelerometers with bias 5 × 10−4 m/s2 (1σ ) at output rate 100 Hz. The experiment data was collected as such. Let an INS stand still on a three-axis table for about 1000 seconds and then rotate the three axes simultaneously at about 0.8 Hz for 2400 seconds. The alignment experiment was carried out from 1500 s to 2500 s. The incremental angles and incremental velocities between 1500 s and 1510 s are plotted in Fig. 11.

M. Wu et al. / Aerospace Science and Technology 15 (2011) 1–17

11

Fig. 12. The computed attitude by inertial navigation.

Fig. 13. The horizontal ground velocity of INS is not zero due to the lever arm with respect to the rotating table, as indicated by the oscillating lines after 220 s. The solid line is for north velocity and dotted line is for east velocity.

Because the INS true attitude is unknown, we took an initial alignment between 0–800 s followed by inertial navigation and used as an approximate reference the computed attitude (see Fig. 12). Since the INS center is never perfectly coincident with the intersecting point of three table axes, the ground velocity of the INS is not zero. Fig. 13 gives the computed velocity by inertial navigation for the first 300 seconds. Fig. 14 gives the alignment errors by subtracting the approximate attitude reference from the estimates. KALMAN performs unexpectedly very poorly, 0.01–0.02 deg in level angles and 0.2 deg in yaw angle at 1000 s. Fig. 15 presents more details of OBA and OBAUF. Fig. 16 plots the initial attitude angles, in which the effect of the oscillating ground velocity on OBA is very clear. The yaw error is within 0.06 deg in 200–1000 s as for OBAUF. Apparent climbing trends show unsatisfactory calibration of the INS system.

5. Discussions and conclusions This paper devises a novel INS alignment approach that derives itself as a byproduct of the nonlinear global observability analysis [24,28,30,31]. In specific, it is shown that the alignment problem in the navigation community can be equivalently transformed into an attitude determination problem using infinite vector observations over a continuous time interval. It essentially builds up an interesting link between these two problems that has been studied in parallel for several decades. Then by referring to the q-method that is widely used to attack the attitude determination problem, the INS alignment is heuristically established as an optimization problem of finding the minimum eigenvector. Sensitivity analysis of the approach with respect to sensor biases has been rigorously made and explicit error equations are obtained for a special stationary case (C nb = I ). By inspecting the constant initial Euler angles, the alignment approach could alternatively act as a detection tool of significant sensor biases. The analysis is well validated by simulations and experiments.

12

M. Wu et al. / Aerospace Science and Technology 15 (2011) 1–17

Fig. 14. Attitude errors for the rotating INS on the three-axis table.

Fig. 15. Attitude errors for the rotating INS on the three-axis table by OBA and OBAUF.

Interestingly, the Kalman filtering alignment method also possesses error behaviors that can be well explained by the analytic result. This is a new finding that has never been reported in the previous literature. Simulations and experiment studies favorably demonstrate rapidness, accuracy and robustness of the novel approach. In specific, OBA and OBAUF are inherently not influenced by angular motions. OBA is sensitive to translational motions because the zero ground velocity assumption becomes invalid. With the help of a low-pass filter, OBAUF is able to satisfactorily cope with angular/translational motions in both our simulations and experiment. We also find that the traditional KALMAN is apt to be disturbed by angular motions and performs very poorly in the experiment though acceptable in simulations. The proposed algorithms have been successfully applied to real INS systems and work well in several engineering projects. A disadvantage of the proposed algorithm is that it does not estimate the sensor biases. For a swaying INS alignment, the inertial sensor biases should be estimated because they can be made globally observable by moderate maneuvers [30]. An advanced optimization method considering sensor biases is under way and hopefully will be reported in the near future.

M. Wu et al. / Aerospace Science and Technology 15 (2011) 1–17

13

Fig. 16. Initial attitude angles for the rotating INS on the three-axis table by OBA and OBAUF.

Acknowledgements We gracefully thank Junxiang Lian and Yonggang Tang for many inspiring talks and Zhuangzhi Yang and Hongliang Zhang for experiment support. Hongliang Zhang also contributes in part to the proof of Lemma A.1. Appendix A. Two useful lemmas and sensitivity analysis details A.1. Lemma A.1 Lemma A.1. Let m(t ) be a n-dimensional vector varying on the interval [t 0 , t 1 ], then

 t1

 T

rank

m(t )m (t ) dt

   = dim span m(t ), t ∈ [t 0 , t 1 ] .

(29)

t0

Proof. If a time-invariant vector x ∈ {span{m(t ), t ∈ [t 0 , t 1 ]}}⊥ , i.e.,

m T (t )x = 0,

t ∈ [t 0 , t 1 ].

(30)

Left multiplying m(t ) and integrating over [t 0 , t 1 ], (30) yields

t1 m(t )m T (t ) dt · x = 0.

(31)

t0

 t1

So x ∈ null(

t0

m(t )m T (t ) dt ).

 t1

On the other hand, if a time-invariant vector x ∈ null(

t0

m(t )m T (t ) dt ),

t1 m(t )m T (t ) dt · x = 0.

(32)

t0

Left multiplying x T , we get

t1 T

t1 T

x · t0



2

m T (t )x

m(t )m (t ) dt · x =

dt = 0

⇐⇒

m T (t )x = 0,

t ∈ [t 0 , t 1 ].

t0

 t1

This means x ∈ {span{m(t ), t ∈ [t 0 , t 1 ]}}⊥ . Therefore, null(

t0

m(t )m T (t ) dt ) = {span{m(t ), t ∈ [t 0 , t 1 ]}}⊥ , so

(33)

14

M. Wu et al. / Aerospace Science and Technology 15 (2011) 1–17

 t1

 T

rank

m(t )m (t ) dt t0



 t1

 T

= n − dim null

m(t )m (t ) dt t0

  ⊥ = n − dim span m(t ), t ∈ [t 0 , t 1 ]    = dim span m(t ), t ∈ [t 0 , t 1 ] .

(34)

2

The proof ends. A.2. Lemma A.2

Lemma A.2. For a n × n matrix A, λi and ζi (i = 1, 2, . . . , n) are the n pairs of eigenvalues and corresponding eigenvectors. Then for any unrepeated eigenvalues λi , A − λi I + ζi ζiT is nonsingular. Proof. Suppose A − λi I + ζi ζiT is singular, then there exists a nonzero vector y =







A − λi I + ζi ζiT y = A − λi I + ζi ζiT

n 



n 

a jζ j =

j =1

(λ j − λi )a j ζ j +

n

j =1 a j ζ j

n 

j =1, j =i

satisfying



a j ζiT ζ j ζi = 0.

(35)

j =1

Since {ζ j }nj=1 are linear independent vectors, all the coefficients must be equal to zero. That is to say, (λ j − λi )a j = 0 ( j = 1, 2, . . . , n, j = i ) and

n

T j =1 a j ζi ζ j

= 0. That λi is a unique eigenvalue indicates that a j = 0, j = 1, 2, . . . , n. Contradiction. The proof ends here. 2

A.3. Sensitivity analysis with respect to sensor biases Left multiplying (19) by q T , it yields

λ = q T K q.

(36)

The matrix K , eigenvalue λ and eigenvector q are functions of the inevitable sensor error, denoted by a scalar are omitted for brevity below. Taking derivatives of (17) and (19) with respect to μ respectively yield

qT and



dq dμ

dK dμ

μ. Their dependences on μ

=0

−

(37)

dλ dμ

 I q + (K − λI )

dq dμ

= 0.

(38)

Taking transpose of (19), we get q T ( K − λ I ) = 0. Then left multiplying (38) by q T and using (17) give



dK

qT

dμ

−

dλ dμ



I q + qT (K − λI )

dq dμ

= qT

dK dμ

q−

dλ dμ

= 0,

(39)

i.e.,

dλ dμ

= qT

dK dμ

q.

(40)

Left multiplying (37) by q and adding the resultant to (38), we obtain



K − λ I + qq T

 dq dμ

    dK dλ dK dK =− − I q=− − qT q · I q. dμ

dμ

dμ

(41)

dμ

Because ( K − λ I + qq T ) is invertible for a unrepeated λ (see Lemma A.2), the rate of eigenvector with respect to any scalar sensor error is obtained as

dq dμ

    −1 dK −1 dK   dK dK = − K − λ I + qq T − qT q · I q = − K − q T K q · I + qq T − qT q · I q. dμ

dμ

dμ

(42)

dμ

With (13) and (16), the explicit form of matrix K is

t f  K= 0

(α (t ) − β(t ))T (α (t ) − β(t ))

2β T (t )(α (t )×)

−2(α (t )×)β(t )

(α (t ) − β(t ))(α (t ) − β(t ))T − ((α (t )×) + (β(t )×))2

of which the derivative with respect to the parameter

dK dμ

=

t f  2(α (t )−β(t ))T dα(t ) dμ

0

−2( ddαμ(t ) ×)β(t )

 dt ,

(43)

μ is 2β T (t )(

dα (t ) ×) dμ

dα (t ) (α (t )−β(t )) T +(α (t )−β(t ))( ddαμ(t ) ) T −( ddαμ(t ) ×)((α (t )×)+(β(t )×))−((α (t )×)+(β(t )×))( ddαμ(t ) ×) dμ

 dt .

(44)

M. Wu et al. / Aerospace Science and Technology 15 (2011) 1–17

So the attitude quaternion q(μ) in the presence of sensor error

 dq(μ)   q(μ) = q(0) + dμ 

μ=0

15

μ can be approximated to the first order as

· μ.

(45)

Eqs. (42)–(45) constitute the fundamental eigenvector perturbation equation that is nontrivial to solve for a general case. Next we consider the stationary alignment case. Change of the navigation frame due to the constant angular velocity ωnin = ωnie can be described as [19]

φn (0, t ) = I −

sin( ωnie t ) 

ω

n ie



ωnie × +

1 − cos( ωnie t ) 

ω

n 2 ie

2

ωnie × .

(46)

The alignment procedure usually takes no more than several hundred seconds, so ωnie t = Ω t 1 (Ω = 7.292115 × 10−5 rad/s) and (46) can be reduced to

  t 2  n 2 φn (0, t ) ≈ I − t ωnie × + ωie × .

(47)

2

Suppose the gyro bias is denoted by ε  [εx ε y εz ] T and accelerometer bias by ∇  [∇x ∇ y ∇z ] T . The measured body rotation rate in b = C nb ωnie + ε and the measured specific force f b = −C nb g n + ∇ . The change of the body frame is described by the inertial frame ωib

φb (0, t ) = I −

b sin( ωib t )

ω

b ib

(ωib ×) +

b 1 − cos( ωib t ) 

ω

b 2 ib

2

b ωib × .

(48)

b b For a navigational grade system, ωib  ε and ωib t = C nb ωnie + ε t ≈ Ω t 1. Eq. (48) is approximated as

   t 2  b n  2 φb (0, t ) ≈ I − t C nb ωnie + ε × + C n ωie + ε × .

(49)

2

Substituting (47) and (49) into (7) gives





 

α (t ) = I + t C nb ωnie + ε × +

t 2  2

  2 

C nb ωnie + ε ×

−C nb g n + ∇



    t 2  n 2 n β(t ) = − I + t ωnie × + ωie × g .

and

2

(50)

ε y = εz = ∇x = ∇ y = ∇z = 0, α (t ) is a function of the scalar parameter εx and    b n    b n    b n t2  (e 1 ×) C n ωie × + C n ωie × (e 1 ×) C n g = − t (e 1 ×) + (51)

Firstly, consider the effect of gyroscope biases. Let

 dα (t )   dε  x

ε x =0

2

in which e i represents the three-dimensional natural base with the ith element being 1 and other elements zeros. Similarly, we have

    b n    b n    b n t2   ( × + × ( = − t ( e ×) + e ×) C ω C ω e ×) Cn g , 2 2 2 n n ie ie dε y ε y =0 2     b n    b n    b n dα (t )  t2   ( × + × ( = − t ( e ×) + e ×) C ω C ω e ×) Cn g . 3 3 3 n ie n ie dε  2 dα (t ) 

z

Then it comes to the influence of accelerometer biases. Let

 dα (t )   d∇  x

(52)

ε z =0

∇ x =0

εx = ε y = εz = ∇ y = ∇z = 0, α (t ) is a function in the parameter ∇x and





   t 2  b n  2 = I + t C nb ωnie × + C n ωie × e1 .

(53)

2

Similarly, we have

    b n   t 2  b n  2  = I + t C n ωie × + C n ωie × e2 , d∇ y ∇ y =0 2     b n   t 2  b n  2 dα (t )   × + × = I + t C ω C ω e3 . n ie n ie d∇  2 dα (t ) 

z

(54)

∇ z =0

Substituting (51)–(54), it yields

q(εx , ε y , εz , ∇x , ∇ y , ∇z ) = q(0, 0, 0, 0, 0, 0) +

+

  

dq(∇x )  d ∇x

∇ x =0

  

dq(εx ) 

∇x +

d εx

ε x =0

  

εx +

dq(∇x )  d∇ y

∇ y =0

  

dq(εx )  dε y

∇y +

ε y =0

εy +

  

dq(∇x )  d∇ z

∇ z =0

  

dq(εx )  dε z

ε z =0

εz

∇z

+ − 1      T  = I − K − q T K q · I + qq T ε =∇=0 · Γ − q Γ q · I q  [δ q] q,

(55)

16

M. Wu et al. / Aerospace Science and Technology 15 (2011) 1–17

where δq denotes the multiplicative error attitude quaternion and

      dK  dK  dK  dK  dK        Γ = εx + εy + εz + ∇x + ∇y + ∇z d ε x  ε x =0 dε y ε y =0 d ε z  ε z =0 d∇x ∇x =0 d ∇ y  ∇ y =0 d∇z ∇z =0   dK    · μ.  dK 

μ=εx ,ε y ,εz ,∇x ,∇ y ,∇z

(56)

d μ μ=0

Eq. (55) is the explicit eigenvector perturbation equation with respect to the gyro bias ε and the accelerometer bias ∇ . Now in order to arrive at a significant result, further assume the body frame coincides with the navigation frame, i.e., C nb = I . When ε = ∇ = 0, α (t ) = β(t ) and the matrices K and ddKμ are respectively simplified as

t f

K=

0



0

dt

0 −4(β(t )×)2

0

(57)

and

dK dμ

t f  =2



β T (t )( ddαμ(t ) ×)

0

−( ddαμ(t ) ×)β(t ) −( ddαμ(t ) ×)(β(t )×) − (β(t )×)( ddαμ(t ) ×)

0

dt .

(58) +

The minimum eigenvalue λ of K is zero and the corresponding eigenvector q = [1 0 0 0] T . Using (57) and (58), [δq] in (55) is reduced to



+

[δq] = I − K + qq

=I−

=



  T −1 

1 0 −4

ε =∇=0 · Γ = I −

t 0

1 0

− 1

0

(β(t )×)2 dt

−4

t 0

− 1

0

(β(t )×)2 dt

 μ=εx ,ε y ,εz ,∇x ,∇ y ,∇z

t f

2 0

Γ

  dt  ·μ −( ddαμ(t ) ×)β(t ) ∗ μ=0 0

∗



 ∗     . t t − 12 ( 0 (β(t )×)2 dt )−1 μ=εx ,ε y ,εz ,∇x ,∇ y ,∇z 0 f ( ddαμ(t ) ×)β(t ) dt μ=0 · μ ∗ 1

(59)

According to the definition in (13), the first column is right the multiplicative error attitude quaternion, of which the vector part is approximately composed of one half of error Euler angles. After a tedious but straightforward derivation,2 the error Euler angles (see Section 4 for definition) for the initial body attitude matrix C nb (0) are given up to one order of time by

⎡

⎤

⎡ g εx −Ω∇x sin L ⎤ 2g ⎢ εz sec L ∇z tan L ⎥ ⎢ ⎥ − ⎣ g ε y +Ω∇x cos L ⎥ ⎣ δψ(0) ⎦ = ⎢ − ⎦ t. g ⎣ Ω ⎦ 2g ∇ x δθ(0) 0 ⎡

δφ(0)

⎤

− ∇gz

(60)

g

Using (5), (47) and (49), the error attitude matrix at current time is obtained to the first order of time as

I − C nb = I − φb (0, t )C nb (0)φnT (0, t )

⎛ ⎤ ⎞ ⎡ δφ(0)    2       n  t 2  n 2 t 2 b n b n ⎝ ⎦ ⎣ ⎠ = I − I − t C n ωie + ε × + C n ωie + ε × I − δψ(0) × I + t ωie × + ωie × 



2

⎡

∗

≈ ⎣ δθ

∗ ∗

∗

δθ(0)

⎤

∗⎦

2

(61)

−δψ δφ ∗ in which error Euler angles at current time are

⎡

δφ

⎤

⎡

− ∇gz

⎤

⎡

g εx −Ω∇x sin L −2ε y ∇x 2g

⎤

⎥ ⎢ ⎥ ⎢ ⎣ δψ ⎦ = ⎢ εz sec L − ∇z tan L ⎥ + ⎢ g ε y +Ω∇x cos L +2εx ∇x ⎥ t . g ⎦ ⎣ ⎦ ⎣ Ω 2g ∇x δθ εz sec L ∇z tan L −( Ω − g )εx g

2

The tedious job could be accomplished with the help of symbolic computation software, for example, Mathematica.

(62)

M. Wu et al. / Aerospace Science and Technology 15 (2011) 1–17

17

Careful examination reveals that if g εx  Ω∇x and g εx  ε y ∇x , then

g εx − Ω∇x sin L 2g

≈

g εx − Ω∇x sin L − 2ε y ∇x 2g

≈

εx 2

(63)

,

and if g ε y  Ω∇x and g ε y  εx ∇x ,

g ε y + Ω∇x cos L 2g

≈

g ε y + Ω∇x cos L + 2εx ∇x 2g

≈

εy 2

.

(64)

References [1] I.Y. Bar-Itzhack, Minimal order time sharing filters for INS in-flight alignment, Journal of Guidance, Control, and Dynamics 5 (1982) 396–402. [2] I.Y. Bar-Itzhack, N. Bermant, Control theoretic approach to inertial navigation systems, Journal of Guidance, Control, and Dynamics 11 (1988) 237–245. [3] I.Y. Bar-Itzhack, B. Porat, Azimuth observability enhancement during inertial navigation system in-flight alignment, Journal of Guidance, Control, and Dynamics 3 (1980) 337–344. [4] I.Y. Bar-Itzhack, D. Serfaty, Y. Vitekt, Doppler-aided low-accuracy strapdown inertial navigation system, Journal of Guidance, Control, and Dynamics 5 (1982) 236–242. [5] H.D. Black, A passive system for determining the attitude of a satellite, Journal of Guidance, Control, and Dynamics 2 (1964) 1350–1351. [6] K.R. Britting, Inertial Navigation Systems Analysis, John Wiley & Sons, Inc., 1971. [7] J.H. Dambeck, Observability and controllability analysis for a strapdown inertial navigation system, in: 3rd International Workshop on High Precision Navigation, University of Stuttgart, Bonn, 1995. [8] J.J. Deyst, A.A. Sutherland, Strapdown inertial system alignment using statistical filters: A simplified formulation, AIAA Journal 11 (1973) 452–456. [9] J. Fang, D. Wan, A fast initial alignment method for strapdown inertial navigation system on stationary base, IEEE Trans. on Aerospace and Electronic Systems 32 (1996) 1501–1505. [10] T. Gaiffe, Y. Cottreau, N. Faussot, G. Hardy, P. Simonpietri, H. Arditty, Highly compact fiber optic gyrocompass for applications at depths up to 3,000 meters, in: The 2000 International Symposium on IEEE/Underwater Technology, Tokyo, Japan, 2000. [11] A. Gelb, Applied Optimal Estimation, MIT Press, Cambridge, MA, 1974. [12] D. Goshen-Meskin, I.Y. Bar-Itzhack, Observability analysis of piece-wise constant systems – Part I: Theory, IEEE Trans. on Aerospace and Electronic Systems 28 (1992) 1056–1067. [13] P.D. Groves, Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems, Artech House, 2008. [14] S. Hong, M.H. Lee, H.-H. Chun, S.-H. Kwon, J.L. Speyer, Observability of error states in GPS/INS integration, IEEE Trans. on Vehicular Technology 54 (2005) 731–743. [15] M.S. Jankovic, Exact nth derivatives of eigenvalues and eigenvectors, Journal of Guidance, Control, and Dynamics 17 (1994) 136–144. [16] Y.F. Jiang, Y.P. Lin, Error estimation of INS ground alignment through observability analysis, IEEE Trans. on Aerospace and Electronic Systems 28 (1992) 92–97. [17] Keat, Analysis of least-squares attitude determination routine, DOAOP, Computer Sciences Corp., Report CSC/TM-77/6034, 1977. [18] J. Lian, Y. Wu, W. Wu, M. Wu, D. Hu, X. Hu, A novel strapdown ins alignment method for swaying vehicles, in: 13th Saint Petersburg International Conference on Integrated Navigation Systems, Russia, 2006. [19] J.M. McCarthy, Introduction to Theoretical Kinematics, MIT Press, Cambridge, MA, 1990. [20] J.G. Park, J.G. Lee, J. Kim, C.G. Park, Observability analysis of SDINS/GPS in-flight alignment, in: ION GPS, Salt Lake City, UT, 2000. [21] B. Porat, I.Y. Bar-Itzhack, Effect of acceleration switching during INS in-flight alignment, Journal of Guidance, Control, and Dynamics 4 (1981) 385–389. [22] I. Rhee, M.F. Abdel-Hafez, J.L. Speyer, Observability of an integrated GPS/INS during maneuvers, IEEE Trans. on Aerospace and Electronic Systems 40 (2004) 526–535. [23] M.D. Shuster, S.D. Oh, Three-axis attitude determination from vector observations, Journal of Guidance, Control, and Dynamics 4 (1981) 70–77. [24] Y. Tang, Y. Wu, M. Wu, W. Wu, X. Hu, L. Shen, INS/GPS integration: Global observability analysis, IEEE Trans. on Vehicular Technology 58 (2009) 1129–1142. [25] E.H. Thompson, J.L. Farrell, J.W. Knight, Alignment methods for strapdown inertial systems, Journal of Spacecraft 3 (1966) 1432–1434. [26] D.H. Titterton, J.L. Weston, Strapdown Inertial Navigation Technology, 2nd ed., IEE, 2004. [27] G. Wahba, A least squares estimate of spacecraft attitude, SIAM Review 7 (1965) 409. [28] Y. Wu, C. Goodall, N. El-Sheimy, Self-calibration for IMU/odometer land navigation: Simulation and test results, in: ION International Technical Meeting, San Diego, CA, USA, 2010. [29] Y. Wu, D. Hu, M. Wu, X. Hu, T. Wu, Observability analysis of rotation estimation by fusing inertial and line-based visual information: A revisit, Automatica 46 (2006) 1809–1812. [30] Y. Wu, H. Zhang, M. Wu, X. Hu, D. Hu, Observability of SINS alignment: A global perspective, IEEE Trans. on Aerospace and Electronic Systems, submitted for publication under 2nd review, 2008. [31] Y. Wu, M. Wu, X. Hu, D. Hu, Self-calibration for land navigation using inertial sensors and odometer: Observability analysis, in: AIAA Guidance, Navigation and Control Conference, Chicago, Illinois, USA, 2009.