An Optimization-Based Initial Alignment and Calibration Algorithm of Land-Vehicle SINS In-Motion

Abstract

For a running freely land-vehicle strapdown inertial navigation system (SINS), the problems of self-calibration and attitude alignment need to be solved simultaneously. This paper proposes a complete alignment algorithm for the land vehicle navigation using Inertial Measurement Units (IMUs) and an odometer. A self-calibration algorithm is proposed based on the global observability analysis to calibrate the odometer scale factor and IMU misalignment angle, and the initial alignment and calibration method based on optimal algorithm is established to estimate the attitude and other system parameters. This new algorithm has the capability of self-initialization and calibration without any prior attitude and sensor noise information. Computer simulation results show that the performance of the proposed algorithm is superior to the extended Kalman filter (EKF) method during the oscillating attitude motions, and the vehicle test validates its advantages.

1. Introduction

The strapdown inertial navigation system (SINS) is an autonomous navigation system that uses inertial measurement units (IMUs) and initial navigation information to determine the attitude, position and velocity [1,2]. SINS is widely used in aviation, marine, land vehicle navigation and positioning because of its advantage of complete autonomy [3,4]. The SINS capability depends on the accuracy and rapidity of initial alignment process which is one of the key technologies in SINS [5,6]. The research about static base initial alignment has been done very well [7,8], however, how to complete the initial alignment of the inertial navigation system on moving base becomes an urgent problem to be solved.

Unlike the alignment on a static base, the alignment on a moving base usually requires the carrier motion information provided by some external device, for example, global positioning system (GPS), cameras, odometers and Doppler laser radars [9,10,11]. SINS/GPS integrated navigation is a commonly used integrated navigation mode [12,13,14]. GPS signals are vulnerable to interference or shielding, and the poor adaptability of GPS-assisted initial alignment system limits its application in the military field. Cameras are also a promising choice despite their tight dependence on easily-identified features with known positions on the path. Odometers are a kind of cost-effective and conveniently-deployed sensor for land vehicles, and odometer aided in-motion alignment is widely used because of its fully self-contained characteristics [15].

The Kalman filter is widely used in initial alignment [16,17]. In [18,19], Kalman filter-based initial alignment for SINS/Doppler velocity log (DVL) integration is studied. However, the Kalman filter requires knowledge of the noise statistics and a roughly known initial attitude that is hardly achieved when the vehicle running freely. If not properly initiated, DVL aided SINS initial alignment based on Kalman filtering would fail.

By way of observability analysis, Wu systematically proposed a versatile strategy for self-contained land vehicle navigation using an IMU and an odometer [20,21]. In these papers, the INS attitude alignment is transformed into a “continuous” attitude determination problem using infinite vector observations, but the initial alignment is still implemented by the Kalman filter other than the optimal estimate algorithm. The coarse alignment algorithm based on optimal estimation for odometer aided SINS is studied in [22,23], in which the integration form of the velocity update equation in the body frame is used to give a rough initial attitude.

The optimization-based alignment (OBA) method with the aid of external velocity and position information provided by Global Navigation Satellite System (GNSS) is proposed in [24,25,26]. The OBA algorithm obtains an optimal attitude matrix through the q method to reduce random errors of attitude angles. However, the algorithms are not suitable for the IMU/odometer system because of the information provided by an odometer is different from the GNSS. In this paper, an optimization-based initial alignment and calibration algorithm of INS/odometer system is proposed, in which the attitude and the associated parameters including the odometer scale factor, lever arm, IMU misalignment angle and inertial sensor biases are estimated. The numerical and vehicle test results show that the performance of the proposed algorithm is superior to the extended Kalman filter (EKF) method during the angular motion.

The contents are organized as follows: Section 2 presents the frame definition and the SINS/odometer system model and conducts the observability analysis of system states. In Section 3, the numerical integration algorithm is derived and the joint estimation problem is posed as a unit quaternion-constrained optimization. Section 4 reports simulation and experiment results of the algorithm. Conclusions are finally drawn in Section 5.

2. Formulate Problem

2.1. System Description

In order to better understand and deduce the initial alignment and calibration algorithm, it is necessary to explain the related coordinate systems, that is, the Earth frame (e-frame), the navigation frame (n-frame), the vehicle frame (a-frame), the SINS frame (b-frame), and the odometer frame (m-frame). The relationship among a-frame, b-frame and m-frame is shown in Figure 1. These frames are defined in detail as follows.

e-frame: It is a frame fixed to the Earth and the origin is at its center; the z_e axis goes along Earth polar axis pointing to the North Pole; the x_e axis points to the intersection of the prime meridian and the equator; the y_e axis and the x_e, z_e axis form a right-hand coordinate frame. The e-frame rotates around the Earth’s rotation with angular rate ω_ie.

n-frame: The local geographic frame (east-north-up, ENU) is selected as the navigation frame. Its origin is the centroid of the vehicle; the z_n axis goes upward along the local geodetic vertical; the y_n axis and x_n axis horizontal north and east respectively.

a-frame: Its origin is the centroid of the vehicle, the point P as shown in Figure 1; the x_a axis shifts rightward along the vehicle’s transverse axis, the y_a axis forward along the longitudinal axis; the z_a axis upward.

b-frame: Its origin coincides with the origin of the vehicle frame; the axes aligned with the directions of the configuration of the three gyroscopes/accelerometers, which misalign the a-frame axes in attitude.

m-frame: Its origin is the center of the front axle, the point Q as shown in Figure 1; its coordinate axes are parallel to the three coordinate axes of b-frame; it is translated l^b from the b-frame.

The navigation (attitude, velocity and position) rate equations in the reference n-frame are well known as [27]:

$${\dot{\mathit{C}}}_b^n={\mathit{C}}_b^n\left({\mathit{\omega }}_{nb}^b\times \right),{\mathit{\omega }}_{nb}^b={\mathit{\omega }}_{ib}^b-{\mathit{b}}_g-{\mathit{C}}_n^b\left({\mathit{\omega }}_{ie}^n+{\mathit{\omega }}_{en}^n\right),$$

(1)

$${\dot{\mathit{v}}}^n={\mathit{C}}_b^n\left({\mathit{f}}^b-{\mathit{b}}_a\right)-\left(2{\mathit{\omega }}_{ie}^n+{\mathit{\omega }}_{en}^n\right)\times {\mathit{v}}^n+{\mathit{g}}^n,$$

(2)

where ${\mathit{C}}_b^n$ denotes the attitude matrix from b-frame to n-frame, ${\mathit{\omega }}_{nb}^b$ the body angular rate with respect to n-frame, ${\mathit{\omega }}_{ib}^b$ the body angular rate measured by gyroscopes in b-frame, ${\mathit{b}}_g$ the gyroscope bias, ${\mathit{\omega }}_{ie}^n$ denotes the e-frame rotation rate with respect to the inertial frame, ${\mathit{\omega }}_{en}^n$ the angular rate of the e-frame with respect to n-frame, ${\mathit{v}}^n$ the velocity relative to n-frame measured by SINS, ${\mathit{f}}^b$ the specific force measured by accelerometers in b-frame, ${\mathit{b}}_a$ the accelerometer bias, and ${\mathit{g}}^n$ the gravity vector. The 3 × 3 skew symmetric matrix (.×) is defined so that the cross product satisfies $\mathit{a}\times \mathit{b}=\left(\mathit{a}\times \right)\mathit{b}$ for arbitrary two vectors. The gyroscope bias ${\mathit{b}}_g$ and the accelerometer bias ${\mathit{b}}_a$ are taken as random constants, i.e., ${\dot{\mathit{b}}}_g=0$, ${\dot{\mathit{b}}}_a=0$. All the quantities herein are functions of time and, if not stated, their time dependences are omitted for brevity.

Denote the IMU misalignment angle as $\mathit{\alpha }={\left[\begin{array}{ccc}{\alpha }_x& {\alpha }_y& {\alpha }_z\end{array}\right]}^{\mathrm{T}}$, that the misalignment angle between b-frame and m-frame, then the installation error matrix ${\mathit{C}}_b^m$ can be expressed as [27]:

$${\mathit{C}}_b^m=\left[\begin{array}{ccc}\cos {\alpha }_y\cos {\alpha }_z-\sin {\alpha }_y\sin {\alpha }_x\sin {\alpha }_z& \cos {\alpha }_y\sin {\alpha }_z+\sin {\alpha }_y\sin {\alpha }_x\cos {\alpha }_z& -\sin {\alpha }_y\cos {\alpha }_x\\ -\cos {\alpha }_x\sin {\alpha }_z& \cos {\alpha }_x\cos {\alpha }_z& \sin {\alpha }_x\\ \sin {\alpha }_y\cos {\alpha }_z+\cos {\alpha }_y\sin {\alpha }_x\sin {\alpha }_z& \sin {\alpha }_y\sin {\alpha }_z-\cos {\alpha }_y\sin {\alpha }_x\cos {\alpha }_z& \cos {\alpha }_y\cos {\alpha }_x\end{array}\right].$$

(3)

The misalignment angle is considered as constant, i.e., ${\dot{\alpha }}_x={\dot{\alpha }}_y={\dot{\alpha }}_z=0$.

Taking the odometer scale factor k and the lever arm ${\mathit{l}}^b$ into account, the speed at the odometer measurement point can be expressed as:

$$y_{od}=k\cdot e_2{}^{\mathrm{T}}{\mathit{C}}_b^m\left({\mathit{C}}_n^b{\mathit{v}}_s^n+{\mathit{\omega }}_{nb}^b\times {\mathit{l}}^b\right),$$

(4)

where, ${\mathit{v}}_s^n$ is the velocity of the IMU measurement point expressed in n-frame, and ${\mathit{\omega }}_{nb}^b$ is the body angular rate with respect to the navigation frame, expressed in the b-frame. The odometer scale factor k and the lever arm ${\mathit{l}}^b$ are considered as constants, i.e., $\dot{k}=0,{\dot{\mathit{l}}}^b=0$.

For land-vehicle, the velocity in the plane perpendicular to the moving direction is assumed as zero, which is regarded as “virtual measurements”, i.e.:

$${\mathit{y}}_{nc}=\left[\begin{array}{l}0\\ 0\end{array}\right].$$

(5)

Merging Equations (4) and (5), the measurement equation is obtained as:

$$\mathit{y}=diag\left\{\left[\begin{array}{ccc}1& k& 1\end{array}\right]\right\}{\mathit{C}}_b^m\left({\mathit{C}}_n^b{\mathit{v}}_s^n+{\mathit{\omega }}_{nb}^b\times {\mathit{l}}^b\right).$$

(6)

2.2. The System Observability Analysis

A system is said to be observable if the initial state could be derived from the known measurement and input information in finite time [28]. The observable state can be estimated by designed observer. In this section, authors investigate the observability of some system states directly from the basic observability concept.

Substituting Equation (3) into (6):

$${\mathit{C}}_m^b\left[\begin{array}{c}0\\ y_{od}/k\\ 0\end{array}\right]=\frac{y_{od}}{k}\left[\begin{array}{c}-\cos {\alpha }_x\sin {\alpha }_z\\ \cos {\alpha }_x\cos {\alpha }_z\\ \sin {\alpha }_x\end{array}\right]={\mathit{C}}_n^b{\mathit{v}}_s^n+{\mathit{\omega }}_{nb}^b\times {\mathit{l}}^b.$$

(7)

It is obviously that the roll angle ${\alpha }_y$ has no effect on $y_{od}$, and it is unobservable.

We rewrite Equation (2) as:

$${\dot{\mathit{v}}}^n+\left(2{\mathit{\omega }}_{ie}^n+{\mathit{\omega }}_{en}^n\right)\times {\mathit{v}}^n-{\mathit{C}}_b^n\left({\mathit{f}}^b-{\mathit{b}}_a\right)={\mathit{g}}^n.$$

(8)

The time derivative of ${\mathit{v}}^n={\mathit{C}}_b^n{\mathit{v}}^b$ is:

$${\dot{\mathit{v}}}^n={\mathit{C}}_b^n\left({\dot{\mathit{v}}}^b+{\mathit{\omega }}_{nb}^b\times {\mathit{v}}^b\right).$$

(9)

Substituting Equation (9) into Equation (8):

$${\mathit{C}}_b^n\left({\dot{\mathit{v}}}^b+\left({\mathit{\omega }}_{ib}^b-{\mathit{b}}_g+{\mathit{\omega }}_{ie}^b\right)\times {\mathit{v}}^b-{\mathit{f}}^b+{\mathit{b}}_a\right)={\mathit{g}}^n.$$

(10)

Rewrite Equation (6) as:

$${\mathit{v}}^b={\mathit{C}}_m^{b}diag{\left\{\left[\begin{array}{ccc}1& k& 1\end{array}\right]\right\}}^{-1}\mathit{y}-{\mathit{\omega }}_{nb}^b\times {\mathit{l}}^b.$$

(11)

Note $\mathit{K}=diag{\left\{\left[\begin{array}{ccc}1& k& 1\end{array}\right]\right\}}^{-1}$, then we have

$${\mathit{v}}^b={\mathit{C}}_m^b\mathit{Ky}-{\mathit{\omega }}_{nb}^b\times {\mathit{l}}^b.$$

(12)

The derivative on both sides of Equation (12) is

$${\dot{\mathit{v}}}^b={\mathit{C}}_m^b\mathit{K}\dot{\mathit{y}}-{\dot{\mathit{\omega }}}_{nb}^b\times {\mathit{l}}^b.$$

(13)

Substituting Equation (13) into Equation (10), we have

$${\mathit{C}}_b^n\left[{\mathit{C}}_m^b\mathit{K}\dot{y}-{\dot{\mathit{\omega }}}_{nb}^b\times {\mathit{l}}^b+\left({\mathit{\omega }}_{ib}^b-{\mathit{b}}_g+{\mathit{\omega }}_{ie}^b\right)\times \left({\mathit{C}}_m^b\mathit{K}y-{\dot{\mathit{\omega }}}_{nb}^b\times {\mathit{l}}^b\right)-{\mathit{f}}^b+{\mathit{b}}_a\right]={\mathit{g}}^n.$$

(14)

Rewrite Equation (14) as

$$\left[{\mathit{C}}_m^b\mathit{K}\dot{y}+\left({\mathit{\omega }}_{ib}^b-{\mathit{b}}_g+{\mathit{\omega }}_{ie}^b\right)\times {\mathit{C}}_m^b\mathit{K}y\right]-\left[{\dot{\mathit{\omega }}}_{nb}^b+\left({\mathit{\omega }}_{ib}^b-{\mathit{b}}_g+{\mathit{\omega }}_{ie}^b\right)\times {\mathit{\omega }}_{nb}^b\right]\times {\mathit{l}}^b={\mathit{C}}_n^b{\mathit{g}}^n+{\mathit{f}}^b-{\mathit{b}}_a.$$

(15)

If the carrier has no attitude maneuver, then ${\mathit{\omega }}_{nb}^b=0$, $\Vert {\mathit{\omega }}_{ib}^b-{\mathit{b}}_g\Vert \approx \Vert {\mathit{\omega }}_{ie}^b\Vert \approx 7.3\times {10}^{-5}\mathrm{rad}/\mathrm{s}$ are small amounts. Equation (15) can be simplified as [19]:

$${\mathit{C}}_m^b\mathit{K}\dot{\mathit{y}}={\mathit{C}}_n^b{\mathit{g}}^n+{\mathit{f}}^b-{\mathit{b}}_a.$$

(16)

Make derivatives with respect to time on both sides:

$${\mathit{C}}_m^b\mathit{K}\ddot{\mathit{y}}={\dot{\mathit{f}}}^b.$$

(17)

Take the mode of both sides:

$$k=\pm \Vert \ddot{\mathit{y}}\Vert /\Vert {\dot{\mathit{f}}}^b\Vert .$$

(18)

In general, the odometer scale factor is positive

$$k=\Vert \ddot{\mathit{y}}\Vert /\Vert {\dot{\mathit{f}}}^b\Vert .$$

(19)

Rewrite Equation (17) into the following form

$$\frac{{\ddot{y}}_{odo}}{k}\left[\begin{array}{c}-\cos {\alpha }_x\sin {\alpha }_z\\ \cos {\alpha }_x\cos {\alpha }_z\\ \sin {\alpha }_x\end{array}\right]={\dot{\mathit{f}}}^b.$$

(20)

It is obviously that the odometer scale factor $k$ and IMU misalignment angle ${\alpha }_x$, ${\alpha }_z$ can be calculated from Equations (19) and (20) respectively, and the self-calibration algorithm will be designed in the next section.

3. Self-Calibration & Initial Alignment Algorithm

3.1. Self-Calibration Algorithm

According to the results of observability analysis in the previous section, it is feasible to construct an ideal observer to estimate the odometer scale factor and IMU misalignment angle based on Equations (19) and (20). Integrating Equation (20) twice over the subinterval $\left[t_{0}t\right]$

$${\mathit{C}}_m^b\mathit{K}\alpha \left(t\right)=\mathit{\beta }\left(t\right),$$

(21)

where, $\mathit{\alpha }\left(t\right)\triangleq \mathit{y}\left(t\right)-\mathit{y}\left(t_0\right)-\dot{\mathit{y}}\left(t_0\right)\left(t-t_0\right)$, $\mathit{\beta }\left(t\right)={\displaystyle {\int }_{t_0}^t{\mathit{f}}^{b}d\tau -}{\mathit{f}}^b\left(t_0\right)\left(t-t_0\right)$. Compared with Equation (20), this kind of integral form decline the effect of measurement noise. This equation is applied to all the segments that the vehicle has no attitude maneuver and the acceleration is not zero. And the calculation algorithms of the odometer scale factor and IMU misalignment angle are shown as below:

$$k=\Vert \mathit{\alpha }\left(t\right)\Vert /\Vert \mathit{\beta }\left(t\right)\Vert ,$$

(22)

$$\frac{\mathit{\alpha }\left(t\right)}{k}\left[\begin{array}{c}-\cos {\alpha }_x\sin {\alpha }_z\\ \cos {\alpha }_x\cos {\alpha }_z\\ \sin {\alpha }_x\end{array}\right]=\mathit{\beta }\left(t\right).$$

(23)

3.2. Initial Alignment and Calibration Algorithm

The aim of this section is to figure out the initial alignment and calibration method based on the known odometer scale factor and IMU misalignment angle.

According to the content of last section, ${\mathit{C}}_m^b\mathit{Ky}$ is a known parameter, denoted as:

$${\mathit{v}}_{od}^b={\mathit{C}}_m^b\mathit{Ky}.$$

(24)

The frozen e-frame at the beginning of the initial alignment is defined as the inertial frame, i.e., i-frame. By the chain rule of the attitude matrix, ${\mathit{C}}_n^b\left(t\right)$ at any time satisfies:

$${\mathit{C}}_n^b\left(t\right)={\mathit{C}}_{n\left(t\right)}^{b\left(t\right)}={\mathit{C}}_{b\left(0\right)}^{b\left(t\right)}{\mathit{C}}_{n\left(0\right)}^{b\left(0\right)}{\mathit{C}}_{n\left(t\right)}^{n\left(0\right)}={\mathit{C}}_{b\left(0\right)}^{b\left(t\right)}{\mathit{C}}_n^b\left(0\right){\mathit{C}}_{n\left(t\right)}^{n\left(0\right)},$$

(25)

where, ${\mathit{C}}_n^b\left(0\right)$ is the initial attitude matrix from n-frame to b-frame, ${\mathit{C}}_{b\left(0\right)}^{b\left(t\right)}$ and ${\mathit{C}}_{n\left(t\right)}^{n\left(0\right)}$ encode the attitude changes of the b-frame and n-frame from time 0 to t respectively. Their rate equations are:

$${\dot{\mathit{C}}}_{b\left(t\right)}^{b\left(0\right)}={\mathit{C}}_{b\left(t\right)}^{b\left(0\right)}\left({\mathit{\omega }}_{ib}^b-{\mathit{b}}_g\right)\times ,$$

(26)

$${\dot{\mathit{C}}}_{n\left(t\right)}^{n\left(0\right)}={\mathit{C}}_{n\left(t\right)}^{n\left(0\right)}\left({\mathit{\omega }}_{in}^n\times \right),$$

(27)

where, ${\mathit{\omega }}_{in}^n$ denotes the angular velocity of n-frame with respect to the inertial frame, i.e., ${\mathit{\omega }}_{in}^n={\mathit{\omega }}_{ie}^n+{\mathit{\omega }}_{en}^n$.

Substituting Equations (24) and (25) into Equation (15), and rewriting as:

$${\mathit{C}}_{b\left(t\right)}^{b\left(0\right)}\left({\dot{\mathit{v}}}_{od}^b+\left({\mathit{\omega }}_{ib}^b-{\mathit{b}}_g+{\mathit{\omega }}_{ie}^b\right)\times {\mathit{v}}_{od}^b-\left[{\dot{\mathit{\omega }}}_{nb}^b+\left({\mathit{\omega }}_{ib}^b-{\mathit{b}}_g+{\mathit{\omega }}_{ie}^b\right)\times {\mathit{\omega }}_{nb}^b\right]\times {\mathit{l}}^b-\left({\mathit{f}}^b-{\mathit{b}}_a\right)\right)={\mathit{C}}_n^b\left(0\right){\mathit{C}}_{n\left(t\right)}^{n\left(0\right)}{\mathit{g}}^n.$$

(28)

Make integration with respect to time on both sides of Equation (28):

$$\begin{array}{l}{\displaystyle {\int }_0^t{\mathit{C}}_{b\left(t\right)}^{b\left(0\right)}\left[{\dot{\mathit{v}}}_{od}^b+\left({\mathit{\omega }}_{ib}^b-{\mathit{b}}_g+{\mathit{\omega }}_{ie}^b\right)\times {\mathit{v}}_{od}^b\right]dt}\\ -{\displaystyle {\int }_0^t{\mathit{C}}_{b\left(t\right)}^{b\left(0\right)}\left[{\dot{\mathit{\omega }}}_{nb}^b+\left({\mathit{\omega }}_{ib}^b-{\mathit{b}}_g+{\mathit{\omega }}_{ie}^b\right)\times {\mathit{\omega }}_{nb}^b\right]\times dt{\mathit{l}}^b}\\ -{\displaystyle {\int }_0^t{\mathit{C}}_{b\left(t\right)}^{b\left(0\right)}\left({\mathit{f}}^b-{\mathit{b}}_a\right)dt}\\ ={\mathit{C}}_n^b\left(0\right){\displaystyle {\int }_0^t{\mathit{C}}_{n\left(t\right)}^{n\left(0\right)}{\mathit{g}}^{n}dt}\end{array},$$

(29)

where:

$${\displaystyle {\int }_0^t{\mathit{C}}_{b\left(t\right)}^{b\left(0\right)}{\dot{\mathit{v}}}_{od}^{b}dt}={\mathit{C}}_{b\left(t\right)}^{b\left(0\right)}{\mathit{v}}_{od}^b-{\mathit{v}}_{od}^b\left(0\right)-{\displaystyle {\int }_0^t{\mathit{C}}_{b\left(t\right)}^{b\left(0\right)}\left({\mathit{\omega }}_{ib}^b-{\mathit{b}}_g\right)\times {\mathit{v}}_{od}^{b}dt},$$

(30)

$${\displaystyle {\int }_0^t{\mathit{C}}_{b\left(t\right)}^{b\left(0\right)}\left({\dot{\mathit{\omega }}}_{nb}^b\times \right)dt}={\mathit{C}}_{b\left(t\right)}^{b\left(0\right)}\left({\mathit{\omega }}_{nb}^b\times \right)-\left({\mathit{\omega }}_{nb}^b\left(0\right)\times \right)-{\displaystyle {\int }_0^t{\mathit{C}}_{b\left(t\right)}^{b\left(0\right)}\left({\mathit{\omega }}_{ib}^b-{\mathit{b}}_g\right)\times {\mathit{\omega }}_{nb}^b\times dt}.$$

(31)

Substituting the above equations into Equation (29)

$$\begin{array}{l}{\mathit{C}}_{b\left(t\right)}^{b\left(0\right)}{\mathit{v}}_{od}^b-{\mathit{v}}_{od}^b\left(0\right)+{\displaystyle {\int }_0^t{\mathit{C}}_{b\left(t\right)}^{b\left(0\right)}{\mathit{\omega }}_{ie}^b\times {\mathit{v}}_{od}^{b}dt}\\ -\left({\mathit{C}}_{b\left(t\right)}^{b\left(0\right)}\left({\mathit{\omega }}_{nb}^b\times \right)-\left({\mathit{\omega }}_{nb}^b\left(0\right)\times \right)+{\displaystyle {\int }_0^t{\mathit{C}}_{b\left(t\right)}^{b\left(0\right)}{\mathit{\omega }}_{ie}^b\times {\mathit{\omega }}_{nb}^b\times dt}\right){\mathit{l}}^b\\ -{\displaystyle {\int }_0^t{\mathit{C}}_{b\left(t\right)}^{b\left(0\right)}\left({\mathit{f}}^b-{\mathit{b}}_a\right)dt}\\ ={\mathit{C}}_n^b\left(0\right){\displaystyle {\int }_0^t{\mathit{C}}_{n\left(t\right)}^{n\left(0\right)}{\mathit{g}}^{n}dt}\end{array}.$$

(32)

During the attitude maneuvering, ${\mathit{\omega }}_{ie}^b$ is much smaller amount than ${\mathit{\omega }}_{nb}^b$. Equation (32) can be simplified as:

$${\mathit{C}}_{b\left(t\right)}^{b\left(0\right)}{\mathit{v}}_{od}^b-{\mathit{v}}_{od}^b\left(0\right)-\left({\mathit{C}}_{b\left(t\right)}^{b\left(0\right)}{\mathit{\omega }}_{nb}^b-{\mathit{\omega }}_{nb}^b\left(0\right)\right)\times {\mathit{l}}^b-{\displaystyle {\int }_0^t{\mathit{C}}_{b\left(t\right)}^{b\left(0\right)}\left({\mathit{f}}^b-{\mathit{b}}_a\right)dt}={\mathit{C}}_n^b\left(0\right){\displaystyle {\int }_0^t{\mathit{C}}_{n\left(t\right)}^{n\left(0\right)}{\mathit{g}}^{n}dt},$$

(33)

where, ${\mathit{C}}_{b\left(t\right)}^{b\left(0\right)}$ can be simplified in the first order as:

$$\begin{array}{ll}{\mathit{C}}_{b\left(t\right)}^{b\left(0\right)}& ={\mathit{C}}_{b\left(\mathrm{T}\right)}^{b\left(0\right)}\cdots {\mathit{C}}_{b\left(M\mathrm{T}\right)}^{b\left(\left(M-1\right)\mathrm{T}\right)}\approx {\displaystyle \prod _{i=1}^M\left(\mathbf{I}+{\mathsf{\Theta }}_i-\mathrm{T}{\mathit{b}}_g\times \right)}\\ & \approx {\tilde{\mathit{C}}}_{b\left(t_M\right)}^{b\left(0\right)}-\mathrm{T}{\displaystyle \sum _{i=1}^M\left[{\displaystyle \prod _{j=1}^{i-1}\left(\mathbf{I}+{\mathsf{\Theta }}_j\right)}\right]}\times \left({\mathit{b}}_g\times \right)\left[{\displaystyle \prod _{j=i+1}^M\left(\mathbf{I}+{\mathsf{\Theta }}_j\right)}\right]\\ & \approx {\tilde{\mathit{C}}}_{b\left(t_M\right)}^{b\left(0\right)}-M\mathrm{T}{\mathit{b}}_g\times \end{array},$$

(34)

where, ${\tilde{\mathit{C}}}_{b\left(t_M\right)}^{b\left(0\right)}$ denotes the error-contaminated body matrix computed by ${\mathit{\omega }}_{ib}^b$ and ${\mathsf{\Theta }}_i$ is the skew symmetric matrix formed by the error-contaminated incremental rotation vector during the update interval [t_i−1 t_i]. For notational brevity, the ${\mathit{C}}_{b\left(t_M\right)}^{b\left(0\right)}$ is used instead of ${\tilde{\mathit{C}}}_{b\left(t_M\right)}^{b\left(0\right)}$ in the later sections. M is the current sequence, and T is the integral period.

Substituting Equation (34) into the left integral of Equation (33):

$$\begin{array}{ll}{\displaystyle {\int }_0^t{\mathit{C}}_{b\left(t\right)}^{b\left(0\right)}\left({\mathit{f}}^b-{\mathit{b}}_a\right)dt}& ={\displaystyle \sum _{k=0}^{M-1}{\displaystyle {\int }_{t_k}^{t_{k+1}}{\mathit{C}}_{b\left(t\right)}^{b\left(0\right)}\left({\mathit{f}}^b-{\mathit{b}}_a\right)}dt}\\ & ={\displaystyle \sum _{k=0}^{M-1}{\mathit{C}}_{b\left(t_k\right)}^{b\left(0\right)}{\displaystyle {\int }_{t_k}^{t_{k+1}}{\mathit{C}}_{b\left(t\right)}^{b\left(t_k\right)}\left({\mathit{f}}^b-{\mathit{b}}_a\right)}dt}\\ & ={\displaystyle \sum _{k=0}^{M-1}\left({\mathit{C}}_{b\left(t_k\right)}^{b\left(0\right)}-k\mathrm{T}{\mathit{b}}_g\times \right){\displaystyle {\int }_{t_k}^{t_{k+1}}\left(\mathbf{I}+\left({\displaystyle {\int }_{t_k}^t\left({\mathit{\omega }}_{ib}^b-{\mathit{b}}_g\right)d\tau }\right)\times \right)\left({\mathit{f}}^b-{\mathit{b}}_a\right)}dt}\end{array},$$

(35)

where, the incremental integral above can be approximated using the two-sample correction by:

$$\begin{array}{l}{\displaystyle {\int }_{t_k}^{t_{k+1}}\left(\mathbf{I}+\left({\displaystyle {\int }_{t_k}^t\left({\mathit{\omega }}_{ib}^b-{\mathit{b}}_g\right)d\tau }\right)\times \right)\left({\mathit{f}}^b-{\mathit{b}}_a\right)}dt\\ ={\displaystyle {\int }_{t_k}^{t_{k+1}}\left(\mathbf{I}+\left({\displaystyle {\int }_{t_k}^t{\mathit{\omega }}_{ib}^{b}d\tau }\right)\times \right)f^b}dt-{\displaystyle {\int }_{t_k}^{t_{k+1}}\left(\mathbf{I}+\left({\displaystyle {\int }_{t_k}^t{\mathit{\omega }}_{ib}^{b}d\tau }\right)\times \right)}dt{\mathit{b}}_a\\ -{\mathit{b}}_g\times {\displaystyle {\int }_{t_k}^{t_{k+1}}\left(t-t_k\right)\left({\mathit{f}}^b-{\mathit{b}}_a\right)}dt\\ =\Delta {\mathit{v}}_1+\Delta {\mathit{v}}_2+\frac{1}{2}\left(\Delta {\mathit{\theta }}_1+\Delta {\mathit{\theta }}_2\right)\times \left(\Delta {\mathit{v}}_1+\Delta {\mathit{v}}_2\right)+\frac{2}{3}\left(\Delta {\mathit{\theta }}_1\times \Delta {\mathit{v}}_2+\Delta {\mathit{v}}_1\times \Delta {\mathit{\theta }}_2\right)\\ -\left[T\mathbf{I}+\frac{T}{6}\left(5\Delta {\mathit{\theta }}_1+\Delta {\mathit{\theta }}_2\right)\times \right]{\mathit{b}}_a+\left[\frac{T}{6}\left(\Delta {\mathit{v}}_1+5\Delta {\mathit{v}}_2\right)-\frac{T^2}{2}{\mathit{b}}_a\right]\times {\mathit{b}}_g\end{array},$$

(36)

where, $\Delta {\mathit{v}}_1$ and $\Delta {\mathit{v}}_2$ are the first and the second samples of the incremental velocity measured by accelerometers, $\Delta {\mathit{\theta }}_1$ and $\Delta {\mathit{\theta }}_2$ are the first and the second samples of the incremental angle measured by gyroscopes during the update interval $\left[t_k{t}_{k+1}\right]$, respectively. Substituting Equation (36) into (35) and rewriting the equation by neglecting those products of IMU biases higher than the first order, we have:

$$\begin{array}{l}{\displaystyle {\int }_0^t{\mathit{C}}_{b\left(t\right)}^{b\left(0\right)}\left({\mathit{f}}^b-{\mathit{b}}_a\right)dt}\\ \approx {\displaystyle \sum _{k=0}^{M-1}{\mathit{C}}_{b\left(t_k\right)}^{b\left(0\right)}\left[\Delta {\mathit{v}}_1+\Delta {\mathit{v}}_2+\frac{1}{2}\left(\Delta {\mathit{\theta }}_1+\Delta {\mathit{\theta }}_2\right)\times \left(\Delta {\mathit{v}}_1+\Delta {\mathit{v}}_2\right)+\frac{2}{3}\left(\Delta {\mathit{\theta }}_1\times \Delta {\mathit{v}}_2+\Delta {\mathit{v}}_1\times \Delta {\mathit{\theta }}_2\right)\right]}\\ -{\displaystyle \sum _{k=0}^{M-1}{\mathit{C}}_{b\left(t_k\right)}^{b\left(0\right)}\left[T\mathbf{I}+\frac{T}{6}\left(5\Delta {\mathit{\theta }}_1+\Delta {\mathit{\theta }}_2\right)\times \right]{\mathit{b}}_a}+{\displaystyle \sum _{k=0}^{M-1}{\mathit{C}}_{b\left(t_k\right)}^{b\left(0\right)}\left[\frac{T}{6}\left(\Delta {\mathit{v}}_1+5\Delta {\mathit{v}}_2\right)\right]\times {\mathit{b}}_g}\\ +{\displaystyle \sum _{k=0}^{M-1}\left[\Delta {\mathit{v}}_1+\Delta {\mathit{v}}_2+\frac{1}{2}\left(\Delta {\mathit{\theta }}_1+\Delta {\mathit{\theta }}_2\right)\times \left(\Delta {\mathit{v}}_1+\Delta {\mathit{v}}_2\right)+\frac{2}{3}\left(\Delta {\mathit{\theta }}_1\times \Delta {\mathit{v}}_2+\Delta {\mathit{v}}_1\times \Delta {\mathit{\theta }}_2\right)\right]}\times kT{\mathit{b}}_g\end{array}.$$

(37)

Discretize the integral on the right of the Equation (28) as:

$${\displaystyle {\int }_0^t{\mathit{C}}_{n\left(t\right)}^{n\left(0\right)}{\mathit{g}}^{n}dt}={\displaystyle \sum _{k=0}^{M-1}{\displaystyle {\int }_{t_k}^{t_{k+1}}{\mathit{C}}_{n\left(t\right)}^{n\left(0\right)}{\mathit{g}}^n}dt}={\displaystyle \sum _{k=0}^{M-1}{\mathit{C}}_{n\left(t_k\right)}^{n\left(0\right)}{\displaystyle {\int }_{t_k}^{t_{k+1}}{\mathit{C}}_{n\left(t\right)}^{n\left(t_k\right)}{\mathit{g}}^n}dt}.$$

(38)

The n-frame rate ${\mathit{\omega }}_{in}^n$ changes much slower than the body rate ${\mathit{\omega }}_{ib}^b$, so ${\mathit{C}}_{n\left(t\right)}^{n\left(t_k\right)}$ can be approximated as

$$\begin{array}{ll}{\mathit{C}}_{n\left(t\right)}^{n\left(t_k\right)}& =\mathbf{I}+\frac{\sin \left(\Vert {\phi }_n\Vert \right)}{\Vert {\phi }_n\Vert }\left({\phi }_n\times \right)+\frac{1-\cos \left(\Vert {\phi }_n\Vert \right)}{{\Vert {\phi }_n\Vert }^2}{\left({\phi }_n\times \right)}^2\\ & \approx \mathbf{I}+\left({\phi }_n\times \right)\end{array},$$

(39)

where, ${\phi }_n\approx {\displaystyle {\int }_{t_k}^t{\mathit{\omega }}_{in}^{n}dt\approx \left(t-t_k\right)}{\mathit{\omega }}_{in}^n$ denotes the rotation vector of n-frame from t_k to the current time t. The integral can be approximated by:

$$\begin{array}{l}{\displaystyle {\int }_0^t{\mathit{C}}_{n\left(t\right)}^{n\left(0\right)}{\mathit{g}}^{n}dt}\approx {\displaystyle \sum _{k=0}^{M-1}{\mathit{C}}_{n\left(t_k\right)}^{n\left(0\right)}{\displaystyle {\int }_{t_k}^{t_{k+1}}\left(\mathbf{I}+\left(t-t_k\right){\mathit{\omega }}_{in}^n\times \right){\mathit{g}}^n}dt}\\ ={\displaystyle \sum _{k=0}^{M-1}{\mathit{C}}_{n\left(t_k\right)}^{n\left(0\right)}\left(T\mathbf{I}+\frac{T^2}{2}{\mathit{\omega }}_{in}^n\times \right){\mathit{g}}^n}\end{array}.$$

(40)

Substituting Equations (36), (37) and (40) into the left integral of Equation (33):

$${\mathit{\alpha }}_M+{\mathit{\gamma }}_M{\mathit{l}}^b+{\mathit{\chi }}_M{\mathit{b}}_a+{\mathit{\lambda }}_M{\mathit{b}}_g={\mathit{C}}_n^b\left(0\right){\mathit{\beta }}_M,$$

(41)

where the symbols in Equation (41) are defined as follows:

$$\Delta \mathit{v}=\Delta {\mathit{v}}_1+\Delta {\mathit{v}}_2+\frac{1}{2}\left(\Delta {\mathit{\theta }}_1+\Delta {\mathit{\theta }}_2\right)\times \left(\Delta {\mathit{v}}_1+\Delta {\mathit{v}}_2\right)+\frac{2}{3}\left(\Delta {\mathit{\theta }}_1\times \Delta {\mathit{v}}_2+\Delta {\mathit{v}}_1\times \Delta {\mathit{\theta }}_2\right),$$

$${\mathit{\alpha }}_M={\mathit{C}}_{b\left(t_M\right)}^{b\left(0\right)}{\mathit{v}}_{od}^b-{\mathit{v}}_{od}^b\left(0\right)-{\displaystyle \sum _{k=0}^{M-1}{\mathit{C}}_{b\left(t_k\right)}^{b\left(0\right)}}\Delta \mathit{v},$$

$${\mathit{\chi }}_M={\displaystyle \sum _{k=0}^{M-1}{\mathit{C}}_{b\left(t_k\right)}^{b\left(0\right)}\left[T\mathbf{I}+\frac{T}{6}\left(5\Delta {\mathit{\theta }}_1+\Delta {\mathit{\theta }}_2\right)\times \right]},$$

$${\mathit{\lambda }}_M=MT\left({\mathit{v}}_{od}^b\times \right)-{\displaystyle \sum _{k=0}^{M-1}\left({\mathit{C}}_{b\left(t_k\right)}^{b\left(0\right)}\left[\frac{T}{6}\left(\Delta {\mathit{v}}_1+5\Delta {\mathit{v}}_2\right)\times \right]+kT\Delta \mathit{v}\times \right)},$$

$${\mathit{\gamma }}_M=-\left({\mathit{C}}_{b\left(t\right)}^{b\left(0\right)}{\mathit{\omega }}_{ib}^b\times -{\mathit{\omega }}_{ib}^b\left(0\right)\times \right),$$

$${\mathit{\beta }}_M={\displaystyle \sum _{k=0}^{M-1}{\mathit{C}}_{n\left(t_k\right)}^{n\left(0\right)}\left(T\mathbf{I}+\frac{T^2}{2}{\mathit{\omega }}_{in}^n\times \right){\mathit{g}}^n}.$$

3.3. Optimization-Based Attitude and Parameter Estimation

In this section, Equation (41) will be posed as a constrained minimization problem to estimate the attitude and other parameters. The attitude matrix ${\mathit{C}}_b^n\left(0\right)$ is replaced by the four-element unit quaternion $\mathit{q}={\left[s\mathit{\eta }\right]}^{\mathrm{T}}$, where $s$ is the scalar part and $\mathit{\eta }$ is the vector part. The relationship between the unit quaternion and the attitude matrix is [1]:

$${\mathit{C}}_b^n\left(0\right)=\left(s^2-{\mathit{\eta }}^{\mathrm{T}}\mathit{\eta }\right)\mathbf{I}+2\mathit{\eta }{\mathit{\eta }}^{\mathrm{T}}+2s\left(\mathit{\eta }\times \right).$$

(42)

Define the quaternion multiplication as:

$${\mathit{q}}_1\circ {\mathit{q}}_2=\stackrel{+}{\left[{\mathit{q}}_1\right]}\left[\begin{array}{l}{s}_2\\ {\mathit{\eta }}_2\end{array}\right]=\stackrel{-}{\left[{\mathit{q}}_2\right]}\left[\begin{array}{l}{s}_1\\ {\mathit{\eta }}_1\end{array}\right],$$

(43)

where, $\stackrel{+}{\left[\mathit{q}\right]}=\left[\begin{array}{cc}s& -{\mathit{\eta }}^{\mathrm{T}}\\ \mathit{\eta }& s\mathbf{I}+\left(\mathit{\eta }\times \right)\end{array}\right]$, $\stackrel{-}{\left[\mathit{q}\right]}=\left[\begin{array}{cc}s& -{\mathit{\eta }}^{\mathrm{T}}\\ \mathit{\eta }& s\mathbf{I}-\left(\mathit{\eta }\times \right)\end{array}\right]$.

Then Equation (41) is equivalent to:

$${\mathit{\alpha }}_M+{\mathit{\gamma }}_M{\mathit{l}}^b+{\mathit{\chi }}_M{\mathit{b}}_a+{\mathit{\lambda }}_M{\mathit{b}}_g={\mathit{q}}^{\ast }\circ {\mathit{\beta }}_M\circ \mathit{q}.$$

(44)

Multiply both sides by $\mathit{q}$

$$\mathit{q}\circ \left({\mathit{\alpha }}_M+{\mathit{\gamma }}_M{\mathit{l}}^b+{\mathit{\chi }}_M{\mathit{b}}_a+{\mathit{\lambda }}_M{\mathit{b}}_g\right)={\mathit{\beta }}_M\circ \mathit{q}.$$

(45)

According to Equation (43), Equation (45) can be rewritten as:

$$\mathbf{0}=\left(\stackrel{-}{\left[{\mathit{\alpha }}_M\right]}-\stackrel{+}{\left[{\mathit{\beta }}_M\right]}\right)\mathit{q}+\stackrel{+}{\left[\mathit{q}\right]}\left({\mathit{\gamma }}_M{\mathit{l}}^b+{\mathit{\chi }}_M{\mathit{b}}_a+{\mathit{\lambda }}_M{\mathit{b}}_g\right)\triangleq \mathit{\pi }.$$

(46)

Since the magnitude of the unit quaternion is 1, we can pose the problem as a unit quaternion-constrained optimization [21,29]:

$$\underset{\mathit{q},{\mathit{l}}^b,{\mathit{b}}_a,{\mathit{b}}_g}{\min }{\displaystyle \sum _M{\mathit{\pi }}^{\mathrm{T}}\mathit{\pi }},s.t.{\mathit{q}}^{\mathrm{T}}\mathit{q}=1.$$

(47)

Ignoring the IMU biases and the lever arm, it is reduced to the Wahba problem [30], which is famous in attitude determination:

$$\underset{\mathit{q}}{\min }{\displaystyle \sum _M{\mathit{q}}^{\mathrm{T}}{\left(\stackrel{-}{\left[{\mathit{\alpha }}_M\right]}-\stackrel{+}{\left[{\mathit{\beta }}_M\right]}\right)}^{\mathrm{T}}\left(\stackrel{-}{\left[{\mathit{\alpha }}_M\right]}-\stackrel{+}{\left[{\mathit{\beta }}_M\right]}\right)\mathit{q}},s.t.{\mathit{q}}^{\mathrm{T}}\mathit{q}=1.$$

(48)

Denote $\mathit{K}={\displaystyle \sum _M{\left(\stackrel{-}{\left[{\mathit{\alpha }}_M\right]}-\stackrel{+}{\left[{\mathit{\beta }}_M\right]}\right)}^{\mathrm{T}}\left(\stackrel{-}{\left[{\mathit{\alpha }}_M\right]}-\stackrel{+}{\left[{\mathit{\beta }}_M\right]}\right)}$, the solution of Wahba problem is the eigenvectors belonging to the minimum eigenvalues of the matrix $\mathit{K}$. And the solution will be taken as the initial angle of the following Newton-Lagrange method.

The iterative Newton-Lagrange method is chosen to tackle the nonlinearly constrained optimization problem, and the Lagrangian equation for the problem (47) is defined as [31]:

$$L\left(\mathit{x},\mu \right)={\displaystyle \sum _M{\mathit{\pi }}^{\mathrm{T}}\mathit{\pi }}+\mu \left({\mathit{q}}^{\mathrm{T}}\mathit{q}-1\right),$$

(49)

where, $\mathit{x}\triangleq {\left[\begin{array}{cccc}{\mathit{q}}^{\mathrm{T}}& {\mathit{l}}^b{}^{\mathrm{T}}& {\mathit{b}}_a{}^{\mathrm{T}}& {\mathit{b}}_g{}^{\mathrm{T}}\end{array}\right]}^{\mathrm{T}}$, $\mu$ is the Lagrange multiplier. The iterative algorithm is given as below:

$$\left[\begin{array}{l}{\mathit{x}}_{k+1}\\ {\mu }_{k+1}\end{array}\right]=\left[\begin{array}{l}{\mathit{x}}_k\\ {\mu }_k\end{array}\right]+\left[\begin{array}{l}\Delta \mathit{x}\\ \Delta \mu \end{array}\right],$$

(50)

where, $\Delta \mathit{x}$ and $\Delta \mu$ are calculated by:

$${\nabla }^{2}L\left(\mathit{x},\mu \right)\left[\begin{array}{l}\Delta \mathit{x}\\ \Delta \mu \end{array}\right]=-\nabla L\left({\mathit{x}}_k,{\mu }_k\right).$$

(51)

Namely:

$$\left[\begin{array}{cc}{\nabla }_{xx}^{2}L\left({\mathit{x}}_k,{\mu }_k\right)& {\nabla }_{x\mu }^{2}L\left({\mathit{x}}_k,{\mu }_k\right)\\ {\nabla }_{x\mu }^{2}L{\left({\mathit{x}}_k,{\mu }_k\right)}^{\mathrm{T}}& 0\end{array}\right]\left[\begin{array}{l}\Delta \mathit{x}\\ \Delta \mu \end{array}\right]=-\left[\begin{array}{c}{\nabla }_{x}L\left({\mathit{x}}_k,{\mu }_k\right)\\ {\mathit{q}}^{\mathrm{T}}\mathit{q}-1\end{array}\right].$$

(52)

The first and the second derivatives of $L\left({\mathit{x}}_k,{\mu }_k\right)$ are expressed as follows:

$${\nabla }_{x}L\left({\mathit{x}}_k,{\mu }_k\right)={\displaystyle \sum _M\mathit{J}}-2\mu {\left[\begin{array}{cccc}{\mathit{q}}^{\mathrm{T}}& {\mathbf{0}}_{1\times 3}& {\mathbf{0}}_{1\times 3}& {\mathbf{0}}_{1\times 3}\end{array}\right]}^{\mathrm{T}},$$

$${\nabla }_{x\mu }^{2}L\left({\mathit{x}}_k,{\mu }_k\right)=-2{\left[\begin{array}{cccc}{\mathit{q}}^{\mathrm{T}}& {\mathbf{0}}_{1\times 3}& {\mathbf{0}}_{1\times 3}& {\mathbf{0}}_{1\times 3}\end{array}\right]}^{\mathrm{T}},$$

$${\nabla }_{xx}^{2}L\left({\mathit{x}}_k,{\mu }_k\right)={\displaystyle \sum _M\mathit{H}}-2\mu diag\left\{{\left[\begin{array}{cc}{\mathbf{1}}_{1\times 4}& {\mathbf{0}}_{1\times 9}\end{array}\right]}^{\mathrm{T}}\right\},$$

where, $\mathit{J}\triangleq {\left[\begin{array}{cccc}{\mathit{J}}_1^{\mathrm{T}}& {\mathit{J}}_2^{\mathrm{T}}& {\mathit{J}}_3^{\mathrm{T}}& {\mathit{J}}_4^{\mathrm{T}}\end{array}\right]}^{\mathrm{T}}$ is the Jacobian matrix of ${\mathit{\pi }}^{\mathrm{T}}\mathit{\pi }$ with:

$$\begin{array}{l}{\mathit{J}}_1=2{\left(\stackrel{-}{\left[{\mathit{\alpha }}_M\right]}-\stackrel{+}{\left[{\mathit{\beta }}_M\right]}\right)}^{\mathrm{T}}\left(\stackrel{-}{\left[{\mathit{\alpha }}_M\right]}-\stackrel{+}{\left[{\mathit{\beta }}_M\right]}\right)\mathit{q}\\ +2\left(\stackrel{-}{{\left[{\mathit{\gamma }}_M{\mathit{l}}^b+{\mathit{\chi }}_M{\mathit{b}}_a+{\mathit{\lambda }}_M{\mathit{b}}_g\right]}^{\mathrm{T}}}\left(\stackrel{-}{\left[{\mathit{\alpha }}_M\right]}-\stackrel{+}{\left[{\mathit{\beta }}_M\right]}\right)+{\left(\stackrel{-}{\left[{\mathit{\alpha }}_M\right]}-\stackrel{+}{\left[{\mathit{\beta }}_M\right]}\right)}^{\mathrm{T}}\stackrel{-}{\left[{\mathit{\gamma }}_M{\mathit{l}}^b+{\mathit{\chi }}_M{\mathit{b}}_a+{\mathit{\lambda }}_M{\mathit{b}}_g\right]}\right)\mathit{q}\end{array},$$

$${\mathit{J}}_2=2{\mathit{\gamma }}_M^{\mathrm{T}}\stackrel{+}{{\left[\mathit{q}\right]}^{\mathrm{T}}}\left(\stackrel{-}{\left[{\mathit{\alpha }}_M\right]}-\stackrel{+}{\left[{\mathit{\beta }}_M\right]}\right)\mathit{q}+2{\mathit{\gamma }}_M^{\mathrm{T}}\left({\mathit{\gamma }}_M{\mathit{l}}^b+{\mathit{\chi }}_M{\mathit{b}}_a+{\mathit{\lambda }}_M{\mathit{b}}_g\right),$$

$${\mathit{J}}_3=2{\mathit{\chi }}_M^{\mathrm{T}}\stackrel{+}{{\left[\mathit{q}\right]}^{\mathrm{T}}}\left(\stackrel{-}{\left[{\mathit{\alpha }}_M\right]}-\stackrel{+}{\left[{\mathit{\beta }}_M\right]}\right)\mathit{q}+2{\mathit{\chi }}_M^{\mathrm{T}}\left({\mathit{\gamma }}_M{\mathit{l}}^b+{\mathit{\chi }}_M{\mathit{b}}_a+{\mathit{\lambda }}_M{\mathit{b}}_g\right),$$

$${\mathit{J}}_4=2{\mathit{\lambda }}_M^{\mathrm{T}}\stackrel{+}{{\left[\mathit{q}\right]}^{\mathrm{T}}}\left(\stackrel{-}{\left[{\mathit{\alpha }}_M\right]}-\stackrel{+}{\left[{\mathit{\beta }}_M\right]}\right)\mathit{q}+2{\mathit{\lambda }}_M^{\mathrm{T}}\left({\mathit{\gamma }}_M{\mathit{l}}^b+{\mathit{\chi }}_M{\mathit{b}}_a+{\mathit{\lambda }}_M{\mathit{b}}_g\right),$$

and $\mathit{H}\triangleq \left[\begin{array}{cccc}{\mathit{H}}_{11}& {\mathit{H}}_{12}& {\mathit{H}}_{13}& {\mathit{H}}_{14}\\ {\mathit{H}}_{12}^{\mathrm{T}}& {\mathit{H}}_{22}& {\mathit{H}}_{23}& {\mathit{H}}_{24}\\ {\mathit{H}}_{13}^{\mathrm{T}}& {\mathit{H}}_{23}^{\mathrm{T}}& {\mathit{H}}_{33}& {\mathit{H}}_{34}\\ {\mathit{H}}_{14}^{\mathrm{T}}& {\mathit{H}}_{24}^{\mathrm{T}}& {\mathit{H}}_{34}^{\mathrm{T}}& {\mathit{H}}_{44}\end{array}\right]$ is the Hessian matrix of ${\mathit{\pi }}^{\mathrm{T}}\mathit{\pi }$ with:

$$\begin{array}{ll}{\mathit{H}}_{11}& =2{\left(\stackrel{-}{\left[{\mathit{\alpha }}_M\right]}-\stackrel{+}{\left[{\mathit{\beta }}_M\right]}\right)}^{\mathrm{T}}\left(\stackrel{-}{\left[{\mathit{\alpha }}_M\right]}-\stackrel{+}{\left[{\mathit{\beta }}_M\right]}\right)\\ & +2\left(\stackrel{-}{{\left[{\mathit{\gamma }}_M{\mathit{l}}^b+{\mathit{\chi }}_M{\mathit{b}}_a+{\mathit{\lambda }}_M{\mathit{b}}_g\right]}^{\mathrm{T}}}\left(\stackrel{-}{\left[{\mathit{\alpha }}_M\right]}-\stackrel{+}{\left[{\mathit{\beta }}_M\right]}\right)+{\left(\stackrel{-}{\left[{\mathit{\alpha }}_M\right]}-\stackrel{+}{\left[{\mathit{\beta }}_M\right]}\right)}^{\mathrm{T}}\stackrel{-}{\left[{\mathit{\gamma }}_M{\mathit{l}}^b+{\mathit{\chi }}_M{\mathit{b}}_a+{\mathit{\lambda }}_M{\mathit{b}}_g\right]}\right)\end{array},$$

$${\mathit{H}}_{12}=2\left({\left(\stackrel{-}{\left[{\mathit{\alpha }}_M\right]}-\stackrel{+}{\left[{\mathit{\beta }}_M\right]}\right)}^{\mathrm{T}}\stackrel{+}{\left[\mathit{q}\right]}-\stackrel{+}{\left[\left(\stackrel{-}{\left[{\mathit{\alpha }}_M\right]}-\stackrel{+}{\left[{\mathit{\beta }}_M\right]}\right)\mathit{q}\right]}\right){\mathit{\gamma }}_M,$$

$${\mathit{H}}_{13}=2\left({\left(\stackrel{-}{\left[{\mathit{\alpha }}_M\right]}-\stackrel{+}{\left[{\mathit{\beta }}_M\right]}\right)}^{\mathrm{T}}\stackrel{+}{\left[\mathit{q}\right]}-\stackrel{+}{\left[\left(\stackrel{-}{\left[{\mathit{\alpha }}_M\right]}-\stackrel{+}{\left[{\mathit{\beta }}_M\right]}\right)\mathit{q}\right]}\right){\mathit{\chi }}_M,$$

$${\mathit{H}}_{14}=2\left({\left(\stackrel{-}{\left[{\mathit{\alpha }}_M\right]}-\stackrel{+}{\left[{\mathit{\beta }}_M\right]}\right)}^{\mathrm{T}}\stackrel{+}{\left[\mathit{q}\right]}-\stackrel{+}{\left[\left(\stackrel{-}{\left[{\mathit{\alpha }}_M\right]}-\stackrel{+}{\left[{\mathit{\beta }}_M\right]}\right)\mathit{q}\right]}\right){\mathit{\lambda }}_M,$$

$${\mathit{H}}_{22}=2{\mathit{\gamma }}_M^{\mathrm{T}}{\mathit{\gamma }}_M,{\mathit{H}}_{23}=2{\mathit{\gamma }}_M^{\mathrm{T}}{\mathit{\chi }}_M,{\mathit{H}}_{24}=2{\mathit{\gamma }}_M^{\mathrm{T}}{\mathit{\lambda }}_M,$$

$${\mathit{H}}_{33}=2{\mathit{\chi }}_M^{\mathrm{T}}{\mathit{\chi }}_M,{\mathit{H}}_{34}=2{\mathit{\chi }}_M^{\mathrm{T}}{\mathit{\lambda }}_M,{\mathit{H}}_{44}=2{\mathit{\lambda }}_M^{\mathrm{T}}{\mathit{\lambda }}_M.$$

4. Simulation and Experiment

To verify the performance of the proposed optimization-based initial alignment algorithm, simulations and experiments are performed in this section.

4.1. Simulation and Analysis

The vehicle is assumed to be located at medium latitude 45° and equipped with a triad of gyroscopes (drift 0.02°/h, noise 0.002°/h/$\sqrt{\mathrm{Hz}}$) and accelerometers (bias 100 μg, noise 10 μg/$\sqrt{\mathrm{Hz}}$) at a sampling rate 100 Hz. The IMU misalignment angle is [20′ 10′ 30′]^T. The odometer is displaced from the IMU by the lever arm ${\mathit{l}}^b={\left[13.2-0.5\right]}^{\mathrm{T}}$ in meters and the odometer scale factor error is 0.002. White noise of velocity (standard variance 0.02 m/s) is simulated in odometer measurements. The initial attitude error is [1° 1° 10°]^T, the initial position error is 10m for each direction in latitude, longitude and height.

Firstly, the simulations are designed to mimic the typical motions of a land vehicle, the vehicle trajectory is designed as follows. The total simulation time is 1000 s. The maneuver mode includes the accelerating, turning, pitching and slowing down. The vehicle’s running trajectory is shown in Figure 2, the outputs of IMU are shown in Figure 3.

According to the results of observability analysis in the previous section, the self-calibration algorithm is applied to all the segments that the vehicle has no attitude maneuver and the acceleration is not zero. The trajectory data of the first 100 s are used for the simulation. As shown in Figure 4, the odometer scale factor is effectively estimated at about 55 s after the vehicle accelerating. And the IMU misalignment angle in Figure 5 is also effectively estimated once the vehicle starts to move at 30 s. As expected, the estimated result deviates from the truth value after the vehicle turning at 70 s, because the applicable conditions of the algorithm are not satisfied. The estimated values k = 1.00198,${\alpha }_x=20.02^{\prime }$, ${\alpha }_z=29.85^{\prime }$ will be considered to be known states in the following simulations.

Next, an extended Kalman filter (EKF) is implemented as a comparison of the proposed optimization-based alignment (OBA) method. Figure 6 presents the alignment result of attitude error by EKF (the blue dashed line) and OBA (the rad solid line). Roll and pitch gradually converge after 70 s (turning), and the convergence accuracy is better than 0.01′. And due to the large initial error setting, the convergence of the yaw is relatively slow and accuracy is about 0.1′. For the estimation of attitude error, the proposed OBA method is relatively better than EKF as shown in

Table 1. The estimate results of typical motion simulation.

Estimate Error	EKF	OBA
Attitude (min)	[0.0032 0.0042 0.0949]T	[−0.0023 −0.0023 0.0413]T
Lever arm (m)	[−0.0071 0.0347 −0.0387]T	[0.0010 0.0037 −0.0032]T

. The estimation of the lever arm is shown in Figure 7. For horizontal arm (x axis and y axis), the EKF method is expected to converge rapidly after the course turn, and the vertical arm (z axis) converges gradually after pitching. Compared with EKF, the convergence of OBA method is more quickly and the estimation accuracy is much higher. To present the estimate results clearly, the estimate errors are listed in Table 1.

Then, we designed a trajectory with alternating yawing and pitching motion, and the IMU outputs are shown in Figure 8. The estimate results of attitude are shown in Figure 9. It can be seen that the estimation of OBA is similar to the last simulation, but the estimate results of EKF vary obviously with the oscillation amplitude about 0.4′. The estimation of lever arm is shown in Figure 10, and the estimate result of EKF has obvious oscillations with the attitude motion too. The estimate errors of EKF and OBA are listed in

Table 2. The estimate results of angular motion simulation.

Estimate Error	EKF	OBA
Attitude (min)	[0.0134 0.0107 −0.2233]T	[−0.0130 −0.0087 0.0878]T
Lever arm (m)	[−0.0242 0.0308 0.0028]T	[−0.0033 0.0005 −0.0004]T

. It is clearly that the EKF is susceptible to disturbance of angular motions, while OBA is hardly affected. The OBA algorithm can track attitude motion and it is inherently not influenced by any angular motions.

4.2. Experiment and Analysis

A vehicle test was conducted to validate the actual performance of the proposed algorithm. The SINS/odometer system parameters are the same with the simulation condition. A high-precision GPS equipment was chosen as position reference, with the position accuracy less than 3 m and the velocity accuracy 0.1 m/s. And the attitude reference was given by SINS/GPS integrated navigation system. The vehicle test trajectory is shown in Figure 11, and the velocity measured by odometer is shown in Figure 12.

The odometer scale factor and IMU misalignment angle estimated by the OBA are shown in Figure 13 and Figure 14. The figures shows that the proposed method can correctly estimate the OD scale factor error, and the SINS installation angle error can be estimated after the vehicle turning as we expected. The estimate results of attitude error are shown in Figure 15. As can be seen from Figure 15, the heading error can reach an accuracy of 5′ within 200 s, and the two-level misalignment angles can reach an accuracy of 1′.

5. Conclusions

This paper has proposed a novel algorithm for the joint estimation of SINS/odometer attitude and associated parameters including the odometer scale factor, lever arm, IMU misalignment angle and inertial sensor biases. The global observability analysis of INS/odometer system is conducted at first. Then, based on the observability analysis results, an integration algorithm for identifying odometer scale factor and IMU misalignment angle was designed, and the initial alignment and calibration algorithm based on optimal algorithm is established. Later on, the initial alignment and calibration problem is posed as a unit quaternion-constrained optimization on attitude, lever arm, accelerometer bias and gyroscope drift, and the Newton-Lagrange algorithm is derived to solve the problem. Finally, simulation and experiment studies show that this new technique has the capability of self-initialization and calibration without any prior attitude and sensor noise information, and the performance of OBA method is superior to the EKF method during the angular motion.