An Improved Coarse Alignment Algorithm for Odometer-Aided SINS Based on the Optimization Design Method

Abstract

An improved coarse alignment (ICA) algorithm is proposed in this paper with a focus on improving alignment accuracy of odometer-aided strapdown inertial navigation system (SINS) under variable velocity and variable acceleration condition. In the proposed algorithm, the outputs of inertial sensors and odometer in a sampling interval are linearized rather than assumed to be a constant, which improves the accuracy of the vector observations and the precision of coarse alignment. Simulation and field test results illustrate that, under variable velocity and variable acceleration condition, the proposed algorithm can obtain a better alignment performance than conventional coarse alignment method.

1. Introduction

[-15]Strapdown inertial navigation system (SINS) can autonomously, continuously and comprehensively provide the position, velocity, and attitude of the carrier [1,2,3,4]. The performance of SINS depends on the accuracy and rapidity of the initial alignment process, which can be divided into coarse alignment and fine alignment [5,6]. Coarse alignment is important since it provides a rapidly alignment result for the fine alignment. The existing algorithms of the coarse alignment mainly include: analytic coarse alignment [7,8,9], inertial frame coarse alignment (IFCA) [10,11], and coarse alignment based on Davenport’s q method [12,13,14,15,16]. The analytic coarse alignment can only be used on static base. In order to solve moving base coarse alignment problem, some IFCA algorithms have been proposed. In [13], Li et al. proposes a fast SINS initial alignment method based on IFCA with the aid of external reference velocity. However, this algorithm has the problem of large random errors. In [15,17,18], the optimization-based alignment (OBA) method with the aid of external reference information provided by Global Navigation Satellite System (GNSS) is proposed. The OBA algorithm obtains optimal attitude matrix through the q method to reduce random errors of attitude angles, however, these algorithms in [15,17] are not suitable for odometer-aided SINS, since the odometer can only provide the velocity in body frame. To solve this problem, the OBA algorithm with the aid of external velocity provided by odometer is reported in [19,20,21]. However, the outputs of inertial sensors and odometer in one sampling interval are assumed to be a constant, which may result in errors of coarse alignment when the velocity and acceleration change.

In order to improve the alignment performance under changeable velocity and acceleration, an improved coarse alignment (ICA) algorithm for SINS aided by odometer is proposed in this paper. The main contribution is that the outputs of inertial sensors and odometer that are assumed to be a constant in a sampling interval in [15,17,19] are replaced by their linear approximations to improve the accuracy of the integral formulae of vector observations. Experimental results show that the proposed ICA algorithm has better accuracy than the OBA algorithm and the traditional IFCA algorithm especially when the velocity and acceleration change, which is more suitable for coarse alignment of odometer-aided SINS.

This paper is organized as follows. Section 2 introduces the OBA algorithm for odometer-aided SINS. Section 3 derives the proposed ICA algorithm with linear approximation of inertial sensors and odometer. Simulation results and field test results are reported in Section 4. Finally, conclusions are drawn in Section 5.

2. OBA Algorithm for Odometer-Aided SINS

The coarse alignment aims at determining a coordinate transformation matrix ${\mathit{C}}_b^n\left(t\right)$ in a short time, where n denotes the local level navigation frame and b denotes the body frame, respectively. In this paper, we denote by i the inertial frame and e the earth frame. The kinematic equations of navigation are known as [1,15,19]

$${\dot{\mathit{C}}}_b^n={\mathit{C}}_b^n{\mathit{\omega }}_{nb}^b\times$$

(1)

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

(2)

where ${\mathit{v}}^n$ denotes the ground velocity in the navigation frame, ${\mathit{\omega }}_{ie}^n$ is the earth rotation angular velocity, ${\mathit{\omega }}_{en}^n$ denotes the angular rate of the navigation fame with respect to the earth frame, and ${\mathit{f}}^b$ is the specific force measured by accelerometers. ${\mathit{\omega }}_{ib}^b$ is the body angular rate measured by gyroscope, and ${\mathit{\omega }}_{nb}^b={\mathit{\omega }}_{ib}^b-{\mathit{C}}_n^b({\mathit{\omega }}_{ie}^n+{\mathit{\omega }}_{en}^n)$ represents the angular rate of the body frame with respect to the navigation frame. Moreover, ${\mathit{g}}^n$ denotes the local gravity acceleration, and $(·\times )$ denotes the cross product matrix. According to the chain rule, the attitude transformation matrix can be decomposed as

$${\mathit{C}}_b^n\left(\tau \right)={\mathit{C}}_{n\left(0\right)}^{n\left(\tau \right)}{\mathit{C}}_b^n\left(0\right){\mathit{C}}_{b\left(\tau \right)}^{b\left(0\right)}$$

(3)

where ${\mathit{C}}_{b\left(\tau \right)}^{b\left(0\right)}$ and ${\mathit{C}}_{n\left(\tau \right)}^{n\left(0\right)}$ denote the change of body frame and navigation frame from time 0 to $\tau$, respectively. They are calculated by the following differential equations

$${\dot{\mathit{C}}}_{b\left(\tau \right)}^{b\left(0\right)}={\mathit{C}}_{b\left(\tau \right)}^{b\left(0\right)}{\mathit{\omega }}_{ib}^b\times$$

(4)

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

(5)

where ${\mathit{\omega }}_{in}^n={\mathit{\omega }}_{ie}^n+{\mathit{\omega }}_{en}^n$ denotes the angular rate of the navigation frame with respect to the inertial frame.

After obtaining the attitude matrixes ${\mathit{C}}_{b\left(\tau \right)}^{b\left(0\right)}$ and ${\mathit{C}}_{n\left(\tau \right)}^{n\left(0\right)}$ by Equations (4) and (5), the key problem to determine ${\mathit{C}}_b^n\left(\tau \right)$ is to obtain the constant matrix ${\mathit{C}}_b^n\left(0\right)$. The specific force Equation (2) is used as the measurement equation in the OBA algorithm with the aid of velocity in navigation frame, such as GNSS, and it can be rewritten as follows if the velocity is provided in body frame, such as the odometer

$$\begin{array}{c}\hfill {\mathit{C}}_b^n\left(\tau \right)\left[{\dot{\mathit{v}}}^b\left(\tau \right)-{\mathit{f}}^b\left(\tau \right)+\left({\mathit{\omega }}_{ib}^b\left(\tau \right)+{\mathit{\omega }}_{ie}^b\left(\tau \right)\right)\times {\mathit{v}}^b\left(\tau \right)\right]={\mathit{g}}^n\end{array}$$

(6)

Substituting (3) into (6) and integrating the specific force equation from time interval 0 to t, we have

$$\begin{array}{c}\hfill {\mathit{C}}_b^n\left(0\right)\left[{\mathit{C}}_{b\left(t\right)}^{b\left(0\right)}{\mathit{v}}^b\left(t\right)-{\mathit{v}}^b\left(0\right)-{\int }_0^t{\mathit{C}}_{b\left(\tau \right)}^{b\left(0\right)}\left({\mathit{f}}^b\left(\tau \right)-{\mathit{\omega }}_{ie}^b\left(\tau \right)\times {\mathit{v}}^b\left(\tau \right)\right)\mathrm{d}\tau \right]={\int }_0^t{\mathit{C}}_{n\left(\tau \right)}^{n\left(0\right)}{\mathit{g}}^n\mathrm{d}\tau \end{array}$$

(7)

Equation (7) can also be rewritten in a compact form as

$${\mathit{C}}_b^n\left(0\right){\mathit{\alpha }}_{\Delta v}\left(t\right)={\mathit{\beta }}_{\Delta v}\left(t\right)$$

(8)

where

$$\begin{array}{ccc}\hfill {\mathit{\alpha }}_{\Delta v}\left(t\right)& \triangleq & {\mathit{C}}_{b\left(t\right)}^{b\left(0\right)}{\mathit{v}}^b\left(t\right)-{\mathit{v}}^b\left(0\right)-{\int }_0^t{\mathit{C}}_{b\left(\tau \right)}^{b\left(0\right)}{\mathit{f}}^b\left(\tau \right)\mathrm{d}\tau +{\int }_0^t{\mathit{C}}_{b\left(\tau \right)}^{b\left(0\right)}\left({\mathit{\omega }}_{ie}^b\left(\tau \right)\times {\mathit{v}}^b\left(\tau \right)\right)\mathrm{d}\tau \hfill \end{array}$$

(9)

$$\begin{array}{ccc}\hfill {\mathit{\beta }}_{\Delta v}\left(t\right)& \triangleq & {\int }_0^t{\mathit{C}}_{n\left(\tau \right)}^{n\left(0\right)}{\mathit{g}}^n\mathrm{d}\tau \hfill \end{array}$$

(10)

The next step is to calculate the vector observations ${\mathit{\alpha }}_{\Delta v}\left(t\right)$ and ${\mathit{\beta }}_{\Delta v}\left(t\right)$. In [15,17,19], the output of accelerometer ${\mathit{f}}^b$, the output of gyroscope ${\mathit{\omega }}_{ib}^b$ and the output of odometer ${\mathit{v}}^b$ are all assumed to be constants in a sampling interval, which may result in errors in ${\mathit{\alpha }}_{\Delta v}\left(t\right)$ when velocity and acceleration are changing. Next we will propose an ICA algorithm by linearizing these outputs in sampling intervals.

3. ICA Algorithm for Odometer-Aided SINS

In the following, T represents the time period of the update interval $[t_k,t_{k+1}]$, $k=1,...,M-1$, and the current time is $t=t_M=MT$, where M is the sampling number. Ignoring the change of gravity acceleration ${\mathit{g}}^n$, the vector observation ${\mathit{\beta }}_{\Delta v}\left(t\right)$ is written as

$${\mathit{\beta }}_{\Delta v}\left(t\right)={\int }_0^t{\mathit{C}}_{n\left(\tau \right)}^{n\left(0\right)}{\mathit{g}}^n\mathrm{d}\tau =\sum _{k=1}^{M-1}{\mathit{C}}_{n\left(t_k\right)}^{n\left(0\right)}{\int }_{t_k}^{t_{k+1}}{\mathit{C}}_{n\left(\tau \right)}^{n\left(t_k\right)}{\mathit{g}}^n\mathrm{d}\tau$$

(11)

Since the angular rate of navigation frame with respect to inertial frame changes slowly, ${\mathit{C}}_{n\left(\tau \right)}^{n\left(t_k\right)}$ can be approximated as [15]

$${\mathit{C}}_{n\left(\tau \right)}^{n\left(t_k\right)}={\mathit{I}}_{3\times 3}+\frac{sin\left(\parallel {\mathit{\phi }}_n\parallel \right)}{\parallel {\mathit{\phi }}_n\parallel }({\mathit{\phi }}_n\times )+\frac{1-cos(\parallel {\mathit{\phi }}_n\parallel )}{\parallel {\mathit{\phi }}_n{\parallel }^2}{({\mathit{\phi }}_n\times )}^2\approx {\mathit{I}}_{3\times 3}+{\mathit{\phi }}_n\times$$

(12)

where ${\mathit{\phi }}_n$ denotes the navigation frame rotation vector from $t_k$ to $\tau$, and ${\mathit{\phi }}_n$ is approximated as

$${\mathit{\phi }}_n\approx {\int }_{t_k}^{\tau }{\mathit{\omega }}_{in}^n\left(t\right)\mathrm{d}t\approx (\tau -t_k){\mathit{\omega }}_{in}^n\left(t_k\right)$$

(13)

Substituting Equations (12) and (13) into Equation (11) yields

$${\mathit{\beta }}_{\Delta v}\left(t_M\right)=\sum _{k=1}^{M-1}{\mathit{C}}_{n\left(t_k\right)}^{n\left(0\right)}(T{\mathit{I}}_{3\times 3}+\frac{T^2}{2}{\mathit{\omega }}_{in}^n\left(t_k\right)\times ){\mathit{g}}^n$$

(14)

The first integral term in Equation (9) can be written as

$$\begin{array}{ccc}\hfill {\mathit{\alpha }}_{\Delta v1}\left(t_M\right)& =& {\int }_0^t{\mathit{C}}_{b\left(\tau \right)}^{b\left(0\right)}{\mathit{f}}^b\left(\tau \right)\mathrm{d}\tau \hfill \\ & =& \sum _{k=1}^{M-1}{\mathit{C}}_{b\left(t_k\right)}^{b\left(0\right)}{\int }_{t_k}^{t_{k+1}}{\mathit{C}}_{b\left(\tau \right)}^{b\left(t_k\right)}{\mathit{f}}^b\left(\tau \right)\mathrm{d}\tau \hfill \\ & \approx & \sum _{k=1}^{M-1}{\mathit{C}}_{b\left(t_k\right)}^{b\left(0\right)}{\int }_{t_k}^{t_{k+1}}\left({\mathit{I}}_{3\times 3}+\left({\int }_{t_k}^{\tau }{\mathit{\omega }}_{ib}^b\left(t\right)\mathrm{d}t\right)\times \right){\mathit{f}}^b\left(\tau \right)\mathrm{d}\tau \hfill \end{array}$$

(15)

In order to improve the accuracy of the vector observations, we make a linear approximation for the outputs ${\mathit{f}}^b$ and ${\mathit{\omega }}_{ib}^b$ of SINS. Suppose that ${\mathit{f}}^b$ changes linearly during the period of $[t_k,t_{k+1}]$ and can be approximated as

$${\mathit{f}}^b\left(t\right)={\mathit{a}}_f·t+{\mathit{b}}_f$$

(16)

Then ${\mathit{a}}_f$ and ${\mathit{b}}_f$ can be calculated by using Equations (17) and (18)

$$\begin{array}{ccc}\hfill {\mathit{a}}_f& =& \frac{{\mathit{f}}^b\left(t_{k+1}\right)-{\mathit{f}}^b\left(t_k\right)}{T}\hfill \end{array}$$

(17)

$$\begin{array}{ccc}\hfill {\mathit{b}}_f& =& {\mathit{f}}^b\left(t_k\right)-\frac{{\mathit{f}}^b\left(t_{k+1}\right)-{\mathit{f}}^b\left(t_k\right)}{T}{t}_k\hfill \end{array}$$

(18)

Similarly, ${\mathit{\omega }}_{ib}^b$ can be approximated as a linear form

$${\mathit{\omega }}_{ib}^b\left(t\right)={\mathit{a}}_{\omega }·t+{\mathit{b}}_{\omega }$$

(19)

where

$$\begin{array}{ccc}\hfill {\mathit{a}}_{\omega }& =& \frac{{\mathit{\omega }}_{ib}^b\left(t_{k+1}\right)-{\mathit{\omega }}_{ib}^b\left(t_k\right)}{T}\hfill \end{array}$$

(20)

$$\begin{array}{ccc}\hfill {\mathit{b}}_{\omega }& =& {\mathit{\omega }}_{ib}^b\left(t_k\right)-\frac{{\mathit{\omega }}_{ib}^b\left(t_{k+1}\right)-{\mathit{\omega }}_{ib}^b\left(t_k\right)}{T}{t}_k\hfill \end{array}$$

(21)

Substituting Equations (16) and (19) into Equation (15), we obtain

$$\begin{array}{cc}{\mathit{\alpha }}_{\Delta v1}\left(t_M\right)& =\sum _{k=1}^{M-1}{\mathit{C}}_{b\left(t_k\right)}^{b\left(0\right)}{\int }_{t_k}^{t_{k+1}}\left[{\mathit{I}}_{3\times 3}+\left({\int }_{t_k}^{\tau }{\mathit{a}}_{\omega }t+{\mathit{b}}_{\omega }\mathrm{d}t\right)\times \right]({\mathit{a}}_f\tau +{\mathit{b}}_f)\mathrm{d}\tau \hfill \\ & =\sum _{k=1}^{M-1}{\mathit{C}}_{b\left(t_k\right)}^{b\left(0\right)}[\frac{T}{2}\left({\mathit{f}}^b\left(t_{k+1}\right)+{\mathit{f}}^b\left(t_k\right)\right)+\left(\frac{T^2}{24}{\mathit{\omega }}_{ib}^b\left(t_{k+1}\right)+\frac{T^2}{8}{\mathit{\omega }}_{ib}^b\left(t_k\right)\right)\times {\mathit{f}}^b\left(t_k\right)\hfill \\ & +\left(\frac{T^2}{8}{\mathit{\omega }}_{ib}^b\left(t_{k+1}\right)+\frac{5}{24}{T}^2{\mathit{\omega }}_{ib}^b\left(t_k\right)\right)\times {\mathit{f}}^b\left(t_{k+1}\right)]\end{array}$$

(22)

The output ${\mathit{v}}^b$ of the odometer contained in the second integral in (9) can also be approximated as

$${\mathit{v}}^b\left(t\right)={\mathit{a}}_v·t+{\mathit{b}}_v$$

(23)

Then ${\mathit{a}}_v$ and ${\mathit{b}}_v$ can be calculated through Equations (24) and (25)

$$\begin{array}{ccc}\hfill {\mathit{a}}_v& =& \frac{{\mathit{v}}^b\left(t_{k+1}\right)-{\mathit{v}}^b\left(t_k\right)}{T}\hfill \end{array}$$

(24)

$$\begin{array}{ccc}\hfill {\mathit{b}}_v& =& {\mathit{v}}^b\left(t_k\right)-\frac{{\mathit{v}}^b\left(t_{k+1}\right)-{\mathit{v}}^b\left(t_k\right)}{T}{t}_k\hfill \end{array}$$

(25)

The term ${\mathit{\omega }}_{ie}^b$ is calculated as follows

$${\mathit{\omega }}_{ie}^b\left(t_{k+1}\right)={\mathit{C}}_n^b\left(t_k\right){\mathit{\omega }}_{ie}^n\left(t_{k+1}\right)$$

(26)

Substituting Equations (19), (23) and (26) into Equation (9), the second integral term in (9) can be approximated as

$$\begin{array}{cc}{\mathit{\alpha }}_{\Delta v2}\left(t_M\right)& ={\int }_0^t{\mathit{C}}_{b\left(\tau \right)}^{b\left(0\right)}\left({\mathit{\omega }}_{ie}^b\left(\tau \right)\times {\mathit{v}}^b\left(\tau \right)\right)\mathrm{d}\tau \hfill \\ & \approx \sum _{k=1}^{M-1}{\mathit{C}}_{b\left(t_k\right)}^{b\left(0\right)}{\int }_{t_k}^{t_{k+1}}\left[{\mathit{I}}_{3\times 3}+\left({\int }_{t_k}^{\tau }{\mathit{a}}_{\omega }t+{\mathit{b}}_{\omega }\mathrm{d}\mathrm{t}\right)\times \right]\left({\mathit{\omega }}_{ie}^b\left(t_{k+1}\right)\times ({\mathit{a}}_v\tau +{\mathit{b}}_v)\right)\mathrm{d}\tau \hfill \\ & =\sum _{k=1}^{M-1}{\mathit{C}}_{b\left(t_k\right)}^{b\left(0\right)}[\frac{T}{2}\left({\mathit{C}}_n^b\left(t_k\right){\mathit{\omega }}_{ie}^n\left(t_{k+1}\right)\right)\times \left({\mathit{v}}^b\left(t_{k+1}\right)+{\mathit{v}}^b\left(t_k\right)\right)\hfill \\ & +\left(\frac{T^2}{24}{\mathit{\omega }}_{ib}^b\left(t_{k+1}\right)+\frac{T^2}{8}{\mathit{\omega }}_{ib}^b\left(t_k\right)\right)\times \left(\left({\mathit{C}}_n^b\left(t_k\right){\mathit{\omega }}_{ie}^n\left(t_{k+1}\right)\right)\times {\mathit{v}}^b\left(t_k\right)\right)\hfill \\ & +\left(\frac{T^2}{8}{\mathit{\omega }}_{ib}^b\left(t_{k+1}\right)+\frac{5}{24}{T}^2{\mathit{\omega }}_{ib}^b\left(t_k\right)\right)\times \left(\left({\mathit{C}}_n^b\left(t_k\right){\mathit{\omega }}_{ie}^n\left(t_{k+1}\right)\right)\times {\mathit{v}}^b\left(t_{k+1}\right)\right)]\hfill \end{array}$$

(27)

The vector observation ${\mathit{\alpha }}_{\Delta v}\left(t_M\right)$ is abbreviated as

$${\mathit{\alpha }}_{\Delta v}\left(t_M\right)={\mathit{C}}_{b\left(t_M\right)}^{b\left(0\right)}{\mathit{v}}^b\left(t_M\right)-{\mathit{v}}^b\left(0\right)-{\mathit{\alpha }}_{\Delta v1}\left(t_M\right)+{\mathit{\alpha }}_{\Delta v2}\left(t_M\right)$$

(28)

Then the discrete measurement equation can be rewritten in a compact form

$${\mathit{C}}_b^n\left(0\right){\mathit{\alpha }}_{\Delta v}\left(t_M\right)={\mathit{\beta }}_{\Delta v}\left(t_M\right),(M=1,2,3,\dots )$$

(29)

After obtaining the vector observations, the measurement Equation (29) can be solved by the $\mathit{q}$ method. The constant matrix ${\mathit{C}}_b^n\left(0\right)$ can be formulated by its corresponding quaternion ${\mathit{q}}_b^n=\mathit{q}={\left[\begin{array}{cc}s& {\mathit{\eta }}^T\end{array}\right]}^T$, subject to ${\mathit{q}}^T\mathit{q}=1$

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

(30)

where s is scalar part and $\mathit{\eta }$ is the vector part. The vector observations can be written in the form of quaternion

$$\begin{array}{ccc}\hfill {\mathit{\alpha }}_{\Delta V}\left(t_M\right)& =& {\left[\begin{array}{cc}0& {\mathit{\alpha }}_{\Delta v}{\left(t_M\right)}^T\end{array}\right]}^T\hfill \end{array}$$

(31)

$$\begin{array}{ccc}\hfill {\mathit{\beta }}_{\Delta V}\left(t_M\right)& =& {\left[\begin{array}{cc}0& {\mathit{\beta }}_{\Delta v}{\left(t_M\right)}^T\end{array}\right]}^T\hfill \end{array}$$

(32)

It is convenient to make Equation (29) equivalent to ${\mathit{\beta }}_{\Delta V}\left(t_M\right)=\mathit{q}\circ {\mathit{\alpha }}_{\Delta V}\left(t_M\right)\circ \mathit{q}\ast$, where ∘ denotes the quaternion multiplication and $\mathit{q}\ast$ is the conjugate quaternion of $\mathit{q}$. The quaternion multiplication matrix is defined as

$$\stackrel{+}{\mathit{q}}\triangleq \left[\begin{array}{cc}s& -{\mathit{\eta }}^T\\ \mathit{\eta }& s{\mathit{I}}_{3\times 3}+(\mathit{\eta }\times )\end{array}\right],\overline{\mathit{q}}\triangleq \left[\begin{array}{cc}s& -{\mathit{\eta }}^T\\ \mathit{\eta }& s{\mathit{I}}_{3\times 3}-(\mathit{\eta }\times )\end{array}\right]$$

(33)

Then we have the equivalent measurement equation [12,15]

$$\left[[\stackrel{+}{{\mathit{\beta }}_{\Delta V}\left(t_M\right)}]-[\stackrel{-}{{\mathit{\alpha }}_{\Delta V}\left(t_M\right)}]\right]\mathit{q}=0.$$

(34)

Thus, the attitude quaternion can be determined by solving the following optimization problem

$$J\triangleq \underset{q}{min}{\mathit{q}}^T\mathit{K}\mathit{q}$$

(35)

where

$$\mathit{K}\triangleq \sum _M{\left[[\stackrel{+}{{\mathit{\beta }}_{\Delta V}\left(t_M\right)}]-[\stackrel{-}{{\mathit{\alpha }}_{\Delta V}\left(t_M\right)}]\right]}^T\left[[\stackrel{+}{{\mathit{\beta }}_{\Delta V}\left(t_M\right)}]-[\stackrel{-}{{\mathit{\alpha }}_{\Delta V}\left(t_M\right)}]\right]$$

(36)

It can be proved that J in Equation (35) will be minimized if $\mathit{q}$ is chosen to be the eigenvector corresponding to the smallest eigenvalue of $\mathit{K}$, which is the optimal quaternion related to ${\mathit{C}}_b^n\left(0\right)$ [15,18]. The block diagram of the ICA algorithm is shown in Figure 1. In Figure 1, LA denotes the process of linear approximation for the outputs of inertial sensors and odometer, AUb and AUn denote update processes of attitude matrix ${\mathit{C}}_{b\left(t_{k+1}\right)}^{b\left(0\right)}$ and attitude matrix ${\mathit{C}}_{n\left(t_{k+1}\right)}^{n\left(0\right)}$, respectively, and the q method is used to calculate the attitude matrix ${\mathit{C}}_b^n\left(0\right)$. According to the chain rule, the attitude matrix ${\mathit{C}}_b^n\left(t_{k+1}\right)$ can be updated in real time by Equation (3). In conclusion, the attitude matrix ${\mathit{C}}_b^n\left(t_M\right)$ can be calculated through the ICA algorithm in real time by Equations (3)–(5) and (29).

Figure 1. The block diagram of the proposed ICA algorithm.

4. Simulation and Test

4.1. Simulation Results

We first give simulation results under variable velocity and acceleration condition for coarse alignment of odometer-aided SINS using the proposed ICA algorithm. In order to verify the performance of the ICA algorithm, the in-motion model with severe maneuvering was built. The swing motion are set as sine functions

$$\begin{array}{ccc}\hfill \theta & =& A_{\theta }sin(2\pi t/T_{\theta }+P{h}_{\theta })+{\theta }_p\hfill \\ \hfill \gamma & =& A_{\gamma }sin(2\pi t/T_{\gamma }+P{h}_{\gamma })+{\gamma }_r\hfill \\ \hfill \phi & =& A_{\phi }sin(2\pi t/T_{\phi }+P{h}_{\phi })+{\phi }_h\hfill \end{array}$$

(37)

where $A_{\theta }$, $A_{\gamma }$ and $A_{\phi }$ are the amplitudes and set as ${10}^{\circ }$, ${11}^{\circ }$, and ${12}^{\circ }$, respectively. $T_{\theta }$, $T_{\gamma }$ and $T_{\phi }$ are swing periods, which are set as $10\mathrm{s}$, $9\mathrm{s}$ and $8\mathrm{s}$, respectively. $P{h}_{\theta }$, $P{h}_{\gamma }$ and $P{h}_{\phi }$ are initial phases and they are random values in the range from 0 radian to $2\pi$ radian, ${\theta }_p$, ${\gamma }_r$ and ${\gamma }_h$ are initial attitude angles and they are random values in the range from $0^{\circ }$ to ${50}^{\circ }$. The initial parameters of the simulation are shown in Table 1. As is shown in Table 1, the random error of the gyroscope and accelerometer are set as Gaussian white noise, and the standard deviation of the gyroscope noise and accelerometer noise are $0.{05}^{\circ }/\mathrm{h}/\sqrt{\mathrm{HZ}}$ and ${10}^{-4}\mathrm{g}/\sqrt{\mathrm{Hz}}$, respectively.

Table 1. Initial parameters.

Parameter Name	Parameter Value
Initial longitude $\lambda$	${120}^{\circ }\mathrm{E}$
Initial latitude L	${30}^{\circ }\mathrm{N}$
Fiber-optic gyroscope drift	$0.{005}^{\circ }/\mathrm{h}$
Fiber-optic gyroscope noise	$0.{05}^{\circ }/\mathrm{h}/\sqrt{\mathrm{HZ}}$
Accelerometer bias	${10}^{-4}\mathrm{g}$
Accelerometer noise	${10}^{-4}\mathrm{g}/\sqrt{\mathrm{Hz}}$
odometer noise	$0.02\mathrm{m}/\mathrm{s}$

In order to simulate the condition of variable velocity and variable acceleration, the velocity of the vehicle is set as a sine-cosine function in the navigation frame, and the velocity of the vehicle is shown in Figure 2.

Figure 2. The true velocity profile of the simulation test.

The first simulation lasts about 150 s under the condition of variable velocity and variable acceleration, and the simulation results of the OBA algorithm and the proposed ICA algorithm are shown in Figure 3. Figure 3 shows that the errors of the roll angle and pitch angle are all less than $0.1^{\circ }$ after 10 s, and the heading error obtained by the proposed ICA algorithm is less than $2^{\circ }$ after 80 s. Compared with the OBA algorithm, the heading angle calculated by the ICA algorithm can obtain more accurate results with faster convergence speed.

Figure 3. The attitude angle error of the simulation.

To further show the advantage of the proposed algorithm, 50 coarse alignment trials are performed. Each coarse alignment lasts 100 s, and the final attitude angles are chosen as the alignment results. Figure 4 gives the results of the three kinds of typical coarse alignment algorithms. The black line denotes the results of the proposed ICA algorithm, the green line denotes the results of the OBA algorithm aided by odometer [19], and the red line denotes the results of the traditional IFCA method (TIFCA) aided by odometer [13]. The statistics of the attitude angle errors through three algorithms are shown in Table 2.

Figure 4. The attitude angle errors of 50 coarse alignments. The abscissa denotes the order $\mathrm{N}$ of the alignments, and the ordinate denotes the errors of the pitch angle, roll angle and heading angle, respectively.

Table 2. The simulation result of 50 coarse alignments.

Attitude Angle	Method	Maximum Error	Mean Error	Standard Deviation
Pitch	TIFCA	$0.{0157}^{\circ }$	$0.{0002}^{\circ }$	$0.{0066}^{\circ }$
	OBA	$-0.{0145}^{\circ }$	$-0.{0021}^{\circ }$	$0.{0068}^{\circ }$
	ICA	$0.{0103}^{\circ }$	$-0.{00006}^{\circ }$	$0.{0055}^{\circ }$
Roll	TIFCA	$-0.{0333}^{\circ }$	$0.{0018}^{\circ }$	$0.{0161}^{\circ }$
	OBA	$0.{0156}^{\circ }$	$-0.{0015}^{\circ }$	$0.{0087}^{\circ }$
	ICA	$0.{0140}^{\circ }$	$0.{0016}^{\circ }$	$0.{0067}^{\circ }$
Heading	TIFCA	$-5.{391}^{\circ }$	$0.{2396}^{\circ }$	$2.{3105}^{\circ }$
	OBA	$4.{984}^{\circ }$	$0.{5555}^{\circ }$	$2.{8391}^{\circ }$
	ICA	$-1.{67}^{\circ }$	$-0.{1393}^{\circ }$	$0.{6220}^{\circ }$

Figure 4 and Table 2 show that the errors of the level attitude angle, pitch angle and roll angle, are all less than $0.{05}^{\circ }$. However, the TIFCA algorithm and the OBA algorithm have a poor performance on heading alignment, and we can see from Table 2 that the heading error of the two algorithms are more than $4^{\circ }$ and standard deviation is greater than $2^{\circ }$. The heading error of the proposed ICA algorithm is less than $1.{67}^{\circ }$ at 100 s, and the standard deviation reduces to $0.{622}^{\circ }$. The standard deviation of the heading angel error adopting ICA algorithm is obviously smaller than the other two algorithms. Both pitch and roll angles can be accurate and quickly obtained by all algorithms, and there is no big difference in the results. The difficulty of coarse alignment is the estimation of the heading angle, especially on variable velocity and variable acceleration condition. As is shown in simulation results, compared with the TIFCA algorithm and OBA algorithm, the proposed algorithm has a better alignment performance under the condition of variable velocity and variable acceleration.

4.2. Test Results

In order to verify the validity of the proposed ICA algorithm in practice, we carried out a field test to verify the performance of the ICA algorithm. As is shown in Figure 5, the self-made fiber-optic-SINS (FSINS) and photonics inertial navigation system (PHINS) are installed together on a reference platform inside the car, and the GPS antenna is installed outside on the top of the car. The position of the experiment is $126.{67}^{\circ }\mathrm{E}$ and $45.{78}^{\circ }\mathrm{N}$. The initial position for each coarse alignment is provided by GPS, and the ground velocity ${\mathit{v}}^b$ in the body frame is provided by the odometer. The specific force $f^b$ and the body angler rate ${\omega }_{ib}^b$ are provided by FSINS, and the FSINS is equipped with gyroscopes (drift $0.{01}^{\circ }/\mathrm{h}$, noise $0.{03}^{\circ }/\mathrm{h}/\sqrt{\mathrm{Hz}}$) and accelerometers (bias ${10}^{-4}\mathrm{g}$, noise ${10}^{-5}\mathrm{g}/\sqrt{\mathrm{Hz}}$). The attitude angles provided by the GPS/PHINS integrated navigation system are used as the attitude reference, and the specifications of GPS/PHINS integrated navigation system are listed in Table 3. The car was moving severely with the change of velocity and acceleration. We carried out four coarse alignments, and each alignment data segment lasts 100 s with the feature of variable velocity and variable acceleration. Figure 6 shows the velocity changes of four segments.

Figure 5. The FSINS and PHINS for the experiment.

Table 3. Specifications of the GPS/PHINS integrated navigation system.

Index	Accuracy
Heading, Roll, Pitch resolution	$0.{001}^{\circ }$
Heading, Roll, Pitch dynamic accuracy	$0.{01}^{\circ }$
Saturation of the speed	$41.66\mathrm{m}/\mathrm{s}$
Speed accuracy	$0.1\mathrm{m}/\mathrm{s}$
The position error of GPS receiver	≤10 m

Figure 6. Velocity profile for four segments.

The coarse alignment results of three algorithms are listed in Table 4, Table 5 and Table 6, in which final attitude angle errors of four coarse alignments are defined as the differences between final attitude angles of three algorithms and the outputs of the reference. As is shown in Table 4 and Table 5, the level attitude errors of three algorithms all reduce to values less than $0.{05}^{\circ }$ in $100\mathrm{s}$, which fulfill the accuracy requirement of coarse alignment. Table 6 shows the final heading angle errors of four segments. As is shown in Table 6, the heading angle errors of the four segments obtained by the proposed ICA algorithm are all smaller than the other two algorithms. In conclusion, under the variable velocity and variable acceleration condition, the ICA algorithm has a better performance in coarse alignment of odometer-aided SINS, and the test results coincide with the simulation results.

Table 4. The final pitch angle error of four segments.

Data	TIFCA	OBA	ICA
Segment 1	$0.{008}^{\circ }$	$-0.{006}^{\circ }$	$-0.{006}^{\circ }$
Segment 2	$0.{009}^{\circ }$	$-0.{007}^{\circ }$	$-0.{003}^{\circ }$
Segment 3	$-0.{004}^{\circ }$	$0.{004}^{\circ }$	$0.{003}^{\circ }$
Segment 4	$0.{013}^{\circ }$	$0.{013}^{\circ }$	$0.{011}^{\circ }$

Table 5. The final roll angle error of four segments.

Data	TIFCA	OBA	ICA
Segment 1	$-0.{035}^{\circ }$	$-0.{033}^{\circ }$	$-0.{035}^{\circ }$
Segment 2	$-0.{012}^{\circ }$	$-0.{011}^{\circ }$	$-0.{014}^{\circ }$
Segment 3	$-0.{015}^{\circ }$	$-0.{008}^{\circ }$	$-0.{009}^{\circ }$
Segment 4	$0.{036}^{\circ }$	$0.{038}^{\circ }$	$0.{038}^{\circ }$

Table 6. The final heading angle error of four segments.

Data	TIFCA	OBA	ICA
Segment 1	$-1.{117}^{\circ }$	$-0.{677}^{\circ }$	$-0.{065}^{\circ }$
Segment 2	$1.{376}^{\circ }$	$1.{024}^{\circ }$	$0.{210}^{\circ }$
Segment 3	$-2.{029}^{\circ }$	$-1.{698}^{\circ }$	$-0.{685}^{\circ }$
Segment 4	$-1.{498}^{\circ }$	$-2.{354}^{\circ }$	$0.{944}^{\circ }$

5. Conclusions

To overcome the performance degradation of the existing TIFCA and OBA algorithms under variable velocity and variable acceleration conditions, a novel improved coarse alignment algorithm for odometer-aided SINS is proposed in this paper. By constructing the vector observations with a linear approximation of sensors’ outputs, the proposed algorithm is able to obtain better accuracy than existing TIFCA and OBA algorithms. Simulation results and field tests verified the performance that the proposed algorithm can obtain smaller heading angle errors, which is more suitable for odometer-aided SINS coarse alignment.