Initial Alignment of Large Azimuth Misalignment Angles in SINS Based on Adaptive UPF

Abstract

The case of large azimuth misalignment angles in a strapdown inertial navigation system (SINS) is analyzed, and a method of using the adaptive UPF for the initial alignment is proposed. The filter is based on the idea of a strong tracking filter; through the introduction of the attenuation memory factor to effectively enhance the corrections of the current information residual error on the system, it reduces the influence on the system due to the system simplification, and the uncertainty of noise statistical properties to a certain extent; meanwhile, the UPF particle degradation phenomenon is better overcome. Finally, two kinds of non-linear filters, UPF and adaptive UPF, are adopted in the initial alignment of large azimuth misalignment angles in SINS, and the filtering effects of the two kinds of nonlinear filter on the initial alignment were compared by simulation and turntable experiments. The simulation and turntable experiment results show that the speed and precision of the initial alignment using adaptive UPF for a large azimuth misalignment angle in SINS under the circumstance that the statistical properties of the system noise are certain or not have been improved to some extent.

1. Introduction

The initial alignment is a key technology in SINS, and the alignment precision and the alignment time are two important indexes which affect the overall system performance. With the application field of navigation systems continuing to expand, most application environments cannot meet the condition that the initial misalignment angle is a large angle and the noise is a Gaussian white noise, so continuing to use the traditional linear navigation system model and Kalman Filter (KF) will produce a greater model error and estimation error, which make the navigation parameters unbelievable [1]. According to this situation, research is mainly divided into two aspects; one is the research on a nonlinear model of the inertial navigation system [2,3,4,5,6,7,8,9,10], the initial alignment is usually divided into two stages of coarse alignment and fine alignment, as the strict mathematical error model of the inertial navigation system is a set of nonlinear differential equations, there must be some modeling error when using a linear model to approximate a nonlinear model. The small misalignment angle linear model is satisfied only under the condition of assuming that the various error sources are minor. However, the actual initial misalignment angles are large in many cases and, therefore, directly adopting a nonlinear model can reflect more truly the error propagation characteristics. In [3], the initial alignment error model of an inertial navigation system is established with the use of a disturbance approximation method, in which the azimuth misalignment angle is large and the horizontal misalignment angles are small, however, the model limits that the horizontal misalignment angles are small, affecting its scope of application. Reference [4] unified two processes of coarse alignment and fine alignment, and the universal SINS initial alignment nonlinear error models are established by using three status to describe the $\text{Ψ}$ angle; and in [5] a three misalignment angles non-linear error mode expressed by multiplicative quaternion is derived, where the derivation process is without linearization to ensure the accuracy of the model. The other is the study of non-linear filters [11,12,13]. In the commonly-used methods of nonlinear filtering, there are the extended Kalman Filter (EKF), unscented Kalman Filter (UKF), particle filter (PF), and UKF-PF (UPF). The most famous algorithm to solve the problem of nonlinear filtering is the extended Kalman Filter (EKF) [1]. This filter, based upon the principle of linearizing the process and observation models using Taylor series expansions, has been successfully implemented in some nonlinear problems. Since the high-order terms above the second-order term are discarded during the linear process, the EKF can only be suitable for the estimation of poor nonlinear objects, and the stronger the nonlinear characteristics of the estimated object are, the greater the estimation error is, which can even cause divergence. Unlike EKF, UKF is based on the covariance matrix of the estimation vector and the measurement vector to determine the optimum gain matrix, the covariance matrix is calculated based on the reproducible double sigma sample points, where these sample points are determined according to the system equation and the measurement equation. Therefore, during the process of calculating the optimum gain matrix, no additional conditions are imposed on the system equation and the measurement equation in UKF, so the algorithm is not only suitable for linear objects, but also for non-linear objects. However, UKF is an approximate form of linear minimum variance estimation, while the standard Kalman filter is a precise linear minimum variance estimation, so only under nonlinear conditions can UKF fully reflect its superiority. The conditional mean is calculated by PF directly according to the probability density, and the probability density is determined by the EKF or UKF approximately, the estimated value ${\widehat{X}}_k$ at $K^{th}$ time is determined by the weighted average value of multitudinous sample values (particles) that have different distributions. EKF or UKF must be completed once when each particle is calculated, so PF is suitable for the condition that the system and measurement are nonlinear, and the estimated accuracy is higher than the accuracy of using EKF or UKF alone, but the calculation level is much higher than EKF and UKF and it can be invalid when the sensor is very accurate or the data experience abrupt changes. The core of PF is choosing a reasonably recommended probability density, so the closer the recommended density selected is to the true density, the better the filter effect is, otherwise it will be worse, or even divergent. If we combine PF with UKF, the recommended density is determined by UKF, which can not only solve the problem of the degradation of particles, but also enable particles to get the latest a posteriori information of the measurement vector when they update, which is helpful for particles to move toward the area with higher likelihood ratio. PF combined with UKF is called UPF. For the condition that the treated object is non-linear and the white noise does not obey the Gaussian distribution, the highest estimation accuracy is achieved by UPF, then PF, UKF follows, and EKF follows it again. Above all, to a certain extent, the use limitations of UPF are less, and the filtering result is better than that of the other kinds of algorithms.

This paper, based on the UPF filtering algorithm, aims at improving the deficiencies of UPF. An approach based on a covariance matching criterion is adopted to judge the convergence and divergence situation of the filter, the covariance of the prediction error is revised and the filter gain is adjusted by an approach of introducing an adaptive attenuation factor, then achieving the goal that restrains and eliminates the divergence phenomenon in the filter and further improves the filter capability of fast tracking. To some extent, it reduces the influence on the system due to system simplification, the uncertain statistical properties of the noise, meanwhile, better overcome the UPF particle degradation phenomenon.

The rest of this paper is organized as follows: the nonlinear error model of SINS based on Euler platform error angles is established in Section 2. Then the detailed adaptive UPF filtering algorithm is designed in Section 3. In Section 4, the factors that influence the adaptive UPF filter are analyzed. In Section 5, two kinds of filtering algorithms are used for the simulation experiment. In Section 6, a turntable experiment for the proposed method with a certain type of SINS is carried out by contrast with that of initial alignment of large azimuth misalignment angle in SINS based on UPF. Finally, conclusions are drawn in Section 7.

2. Nonlinear Error Model of SINS

For the case of SINS with a large misalignment angle, the error caused by the rotation order cannot be ignored and the error model of SINS must be re-established according to the large misalignment situation. Euler platform error angles are used to indicate the misalignment angle between the ideal navigation coordinate and calculated navigation coordinate, and the rotation order of the group error angles should be considered. The corresponding nonlinear error model of SINS is established.

This paper follows the coordinate system selection:

• i frame—geocentric inertial coordinate, the origin is at the center of the Earth , the x_i axis points at equinox, the z_i axis is along the Earth’s axis of rotation, the y_i axis and the x_i axis, the z_i axis constitute the right-handed coordinate system;
• e frame—the Earth coordinate, the origin is at the center of the Earth, the x_e axis passes through the intersection of the prime meridian and the equator, the z_e axis passes through the North Pole of the Earth, and the y_e axis passes through the intersection of the eastern longitude 90° meridian and the equator;
• n frame—the navigation coordinate, here we select the “East-North-Up (ENU)” geographic coordinate system as the navigation coordinate;
• b frame—“Right-Front-Up” coordinate for the SINS coordinate.

n frame has followed through three Euler angles rotation to b frame, the three Euler angles are denoted by the heading angle $\text{Ψ}\in (-\pi \pi ]$, pitch angle $\theta \in [-\pi /2\pi /2]$, and roll angle $\gamma \in (-\pi \pi ]$, and the rotation transformation relationship between the n frame and b frame can be described by the attitude matrix $C_b^n$ [10,14].

2.1. Attitude Error Equation

In the actual navigation system there exist various disturbances and measurement errors, so a rotational error usually exists between the SINS calculation platform coordinate (n' frame) and the ideal navigation coordinate (n frame). The n frame requires one to rotate three angles successively in a certain order and then it can coincide with the n' frame, and now it is assumed that three rotations are successively rotated around the z-axis, x-axis, y-axis, and the turned angles denoted as ${\phi }_z$, ${\phi }_x$ and ${\phi }_y$, so their vector expression form is $\phi ={[{\phi }_x{\phi }_y{\phi }_z]}^T$. Three rotations corresponding with the attitude transformation matrix follow as $C_{{\phi }_z}$, $C_{{\phi }_x}$ and $C_{{\phi }_y}$, so the transformation matrix from the n frame to the n' frame can be expressed as:

$$C_n^{n^{\prime }}=C_{{\phi }_y}{C}_{{\phi }_x}{C}_{{\phi }_z}$$

(1)

where $C_{{\phi }_z}=\left[\begin{array}{ccc}\cos {\phi }_z& \sin {\phi }_z& 0\\ -\sin {\phi }_z& \cos {\phi }_z& 0\\ 0& 0& 1\end{array}\right]$, $C_{{\phi }_x}=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos {\phi }_x& \sin {\phi }_x\\ 0& -\sin {\phi }_x& \cos {\phi }_x\end{array}\right]$ and $C_{{\phi }_y}=\left[\begin{array}{ccc}\cos {\phi }_y& 0& -\sin {\phi }_y\\ 0& 1& 0\\ \sin {\phi }_y& 0& \cos {\phi }_y\end{array}\right]$

If it is assumed that the angular velocity of the n' frame relative to the n frame is ${\omega }_{n{n}^{\prime }}^{n^{\prime }}$, the differential equation for the Euler platform error angles is:

$$\dot{\phi }=C_{\omega }^{-1}{\omega }_{n{n}^{\prime }}^{n^{\prime }}$$

(2)

where $C_{\omega }^{-1}=\frac{1}{\cos {\phi }_x}\left[\begin{array}{ccc}\cos ({\phi }_y)\cos ({\phi }_x)& 0& \sin ({\phi }_y)\cos ({\phi }_x)\\ \sin ({\phi }_y)\sin ({\phi }_x)& \cos ({\phi }_x)& -\cos ({\phi }_y)\sin ({\phi }_x)\\ -\sin ({\phi }_y)& 0& \cos ({\phi }_y)\end{array}\right]$

From [10] the SINS attitude error equation can be obtained as follows:

$$\dot{\phi }=C_{\omega }^{-1}[(I-C_n^{n^{\prime }}){\widehat{\omega }}_{in}^n+C_n^{n^{\prime }}\delta {\omega }_{in}^n-C_b^{n^{\prime }}\delta {\omega }_{ib}^b]$$

(3)

2.2. Velocity Error Equation

In the navigation frame, the velocity differential equation of SINS is [10,14,15]:

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

(4)

However, the velocity differential equation contains errors in the actual system, so now the SINS velocity differential equations should be as follows:

$$\dot{\tilde{v}}=C_b^{n^{\prime }}{\tilde{f}}_{sf}^b-(2{\tilde{\omega }}_{ie}^n+{\tilde{\omega }}_{en}^n)\times {\tilde{v}}^n+{\tilde{g}}^n$$

(5)

where ${\tilde{v}}^n=v^n+\delta v^n$, ${\tilde{f}}_{sf}^b=f_{sf}^b+\delta f_{sf}^b$, ${\tilde{\omega }}_{ie}^n={\omega }_{ie}^n+\delta {\omega }_{ie}^n$, ${\tilde{\omega }}_{en}^n={\omega }_{en}^n+\delta {\omega }_{en}^n$, ${\tilde{g}}^n=g^n+\delta g^n$, and $\delta f_{sf}^b$ is the accelerometer measurement error. The velocity error equation of SINS can be obtained directly by means of Equation (5) minus Equation (4):

$$\begin{array}{l}\delta {\dot{v}}^n=[I-{(C_n^{n^{\prime }})}^T]C_b^{n^{\prime }}{\tilde{f}}_{sf}^b+{(C_n^{n^{\prime }})}^T{C}_b^{n^{\prime }}{f}_{sf}^b-(2\delta {\omega }_{ie}^n+\delta {\omega }_{en}^n)\times {\tilde{v}}^n\\ -(2{\tilde{\omega }}_{ie}^n+{\tilde{\omega }}_{en}^n)\times \delta v^n+(2\delta {\omega }_{ie}^n+\delta {\omega }_{en}^n)\times \delta v^n+\delta g^n\end{array}$$

(6)

The calculation parameters error $\delta {\omega }_{ie}^n$ and $\delta {\omega }_{en}^n$ in Equations (3) and (6) can be specifically expressed as:

$$\delta {\omega }_{ie}^n={\widehat{\omega }}_{ie}^n-{\omega }_{ie}^n=\left[\begin{array}{c}0\\ {\omega }_{ie}[\cos \tilde{L}-\cos (\tilde{L}-\delta L)]\\ {\omega }_{ie}[\sin \tilde{L}-\sin (\tilde{L}-\delta L)]\end{array}\right]$$

(7)

$$\delta {\omega }_{en}^n={\tilde{\omega }}_{en}^n-{\omega }_{en}^n=\left[\begin{array}{c}-\delta L\\ \dot{\tilde{\lambda }}\cos \tilde{L}-(\dot{\tilde{\lambda }}-\delta \dot{\lambda })\cos (\tilde{L}-\delta L)\\ \dot{\tilde{\lambda }}\sin \tilde{L}-(\dot{\tilde{\lambda }}-\delta \dot{\lambda })\sin (\tilde{L}-\delta L)\end{array}\right]$$

(8)

where $\tilde{L}=L+\delta L$ and $\tilde{\lambda }=\lambda +\delta \lambda$.

2.3. Initial Alignment Error Model of Large Azimuth Misalignment Angle in SINS

It is assumed that the two horizontal misalignment angles are both small angles; assuming the gyro measurement errors $\delta {\omega }_{ib}^b$ are mainly composed of the constant drift error ${\epsilon }^b$ and zero mean Gaussian white noise $w_g^b$, the accelerometer measurement error $\delta f_{sf}^b$ are mainly the constant bias error ${\nabla }^b$ and zero mean Gaussian white noise $w_a^b$, the gravity error term $\delta g^n$ is ignored, ${\tilde{v}}^n=\delta v^n$ holds under the static base, then the state equation of the initial alignment filtering model is obtained [10]:

$$\{\begin{array}{l}{\dot{\phi }}_i=C_{\omega }^{-1}[(I-C_n^{n^{\prime }}){\tilde{\omega }}_{in}^n+C_n^{n^{\prime }}\delta {\omega }_{in}^n-C_b^{n^{\prime }}\delta {\omega }_{ib}^b](i=x,y,z)\hfill \\ \delta {\dot{v}}_i^n=[I-{(C_n^{n^{\prime }})}^T]C_b^{n^{\prime }}{\tilde{f}}_{sf}^b+{(C_n^{n^{\prime }})}^T{C}_b^{n^{\prime }}{\nabla }^b-(2{\tilde{\omega }}_{ie}^n+{\tilde{\omega }}_{en}^n)\times \delta v^n+{(C_n^{n^{\prime }})}^T{C}_b^{n^{\prime }}{w}_a^b(i=x,y,z)\hfill \\ {\dot{\epsilon }}_i^b=0(i=x,y,z)\hfill \\ {\dot{\nabla }}_i^b=0(i=x,y,z)\hfill \end{array}$$

(9)

In this paper the state error vector can be expressed as $X={\left[\begin{array}{llllllllll}{\phi }_x\hfill & {\phi }_y\hfill & {\phi }_z\hfill & \delta v_x^n\hfill & \delta v_y^n\hfill & {\epsilon }_x^b\hfill & {\epsilon }_y^b\hfill & {\epsilon }_z^b\hfill & {\nabla }_x^b\hfill & {\nabla }_y^b\hfill \end{array}\right]}^T$, and the noise vector $W=\left[\begin{array}{cccccccccc}{w}_{gx}^b& w_{gy}^b& w_{gz}^b& w_{ax}^b& w_{ay}^b& 0& 0& 0& 0& 0\end{array}\right]$. Establishing the filtering state model, and making the velocity error of SINS $Z=\delta v^n$ as the observation equation:

$$\{\begin{array}{l}\dot{X}=f(X)+G(X)W\hfill \\ Z=HX+V\hfill \end{array}$$

(10)

The specific expressions of $f(X)$ and $G(X)$ can refer to Equation (9), and the matrix H is given as $H=\left[\begin{array}{ccc}{0}_{2\times 3}& I_{2\times 2}& 0_{2\times 5}\end{array}\right]$, and V is the measurement noise.

3. UPF and Adaptive UPF

3.1. UPF Algorithm

Suppose the discrete form of the system equation and the observation equation is as follows:

$$\{\begin{array}{l}{X}_k=f(X_{k-1},u_{k-1})+W_{k-1}\hfill \\ Z_k=h(X_k)+V_k\hfill \end{array}$$

(11)

where ${\mathit{W}}_k$ and $V_k$ are the uncorrelated white Gaussian noise, whose mean value is zero and the variance matrix is ${\mathit{Q}}_k$ and ${\mathit{R}}_k$, then the specific steps of the UPF algorithm are as follows [13,14]:

• Initialization: k = 0; Suppose the initial state variable $x_0~p(x_0)$, the covariance matrix is $P_0$, we sample particles ${\chi }_0^{(i)}(i=1,2,3\dots ,N)$ from the initial probability distribution $p(x_0)$, for the simplified calculation, let ${\chi }_0^{(i)}~N({\overline{\chi }}_0^{(i)},P_0^i)$, where ${\overline{\chi }}_0^{(i)}={\chi }_0^{(i)}$ and $P_0^i=P_0$;
• The forecast and sampling of the weighted particles: k = 1, 2, …; make use of the unscented Kalman filtering for particles to forecast, and calculate σ sampling points:

  $${\chi }_{(0)k-1}^{(i)}={\overline{\chi }}_{k-1}^{(i)}$$

  (12)

  $${\chi }_{(j)k-1}^{(i)}={\overline{\chi }}_{k-1}^{(i)}+\gamma {(\sqrt{P_{k-1}^{(i)}})}_{(j)},j=1,2\cdot \cdot \cdot ,n$$

  (13)

  $${\chi }_{(j)k-1}^{(i)}={\overline{\chi }}_{k-1}^{(i)}+\gamma {(\sqrt{P_{k-1}^{(i)}})}_{(j-n)},j=n+1,n+2\cdot \cdot \cdot ,2n$$

  (14)

  where $\gamma =\sqrt{n+\lambda }$, $\lambda ={\alpha }^2(n+\kappa )-n$, ${10}^{-4}\le \alpha \le 1$, $\kappa =3-n$, n is status dimension;
  Time updating is:

  $$\{\begin{array}{l}{\chi }_{k/k-1}^{(i)}=f({\chi }_{k-1}^{(i)},u_{k-1})\hfill \\ {\overline{\chi }}_{k/k-1}^{(i)}={\displaystyle \sum _{j=0}^{2n}{W}_j^{(m)}{\chi }_{(j)k/k-1}^{(i)}}\hfill \\ P_{k/k-1}^{(i)}={\displaystyle \sum _{j=0}^{2n}{W}_j^{(c)}({\chi }_{(j)k/k-1}^{(i)}-{\chi }_{k/k-1}^{(i)}){({\chi }_{(j)k/k-1}^{(i)}-{\chi }_{k/k-1}^{(i)})}^T+Q_{k-1}}\hfill \\ Z_{(j)k/k-1}^{(i)}=h({\chi }_{(j)k/k-1}^{(i)})\hfill \\ {\overline{Z}}_{k/k-1}^{(i)}={\displaystyle \sum _{j=0}^{2n}{W}_j^{(m)}{Z}_{(j)k/k-1}^{(i)}}\hfill \end{array}$$

  (15)

  where:

  $$W_0^{(m)}=\frac{\lambda }{n+\lambda }$$

  (16)

  $$W_0^{(c)}=\frac{\lambda }{n+\lambda }+1-{\alpha }^2+\beta$$

  (17)

  $$W_j^{(m)}=W_j^{(c)}=\frac{1}{2(n+\lambda )},j=1,2,\cdot \cdot \cdot ,2n$$

  (18)

  $\lambda ={\alpha }^2(n+\kappa )-n$, ${10}^{-4}\le \alpha \le 1$, $\kappa =3-n$, and for the normal distribution, $\beta =2$;
  Measuring updating is:

  $$\{\begin{array}{l}{P}_{(zz)k/k-1}^{(i)}={\displaystyle \sum _{j=0}^{2n}{W}_j^{(c)}(Z_{(j)k/k-1}^{(i)}-{\overline{Z}}_{k/k-1}^{(i)}){(Z_{(j)k/k-1}^{(i)}-{\overline{Z}}_{k/k-1}^{(i)})}^T+R_k}\hfill \\ P_{(xz)k/k-1}^{(i)}={\displaystyle \sum _{j=0}^{2n}{W}_j^{(c)}({\chi }_{(j)k/k-1}^{(i)}-{\overline{\chi }}_{k/k-1}^{(i)}){(Z_{(j)k/k-1}^{(i)}-{\overline{Z}}_{k/k-1}^{(i)})}^T}\hfill \\ K_k^{(i)}=P_{(xz)k/k-1}^{(i)}{(P_{(zz)k/k-1}^{(i)})}^{-1}\hfill \\ {\overline{\chi }}_k^{(i)}={\chi }_{k/k-1}^{(i)}+K_k^{(i)}(Z_k-{\overline{Z}}_{k/k-1}^{(i)})\hfill \\ P_k^{(i)}=P_{k/k-1}^{(i)}-K_k^{(i)}{P}_{(zz)k/k-1}^{(i)}{(K_k^{(i)})}^T\hfill \end{array}$$

  (19)

  Using the particle ${\chi }_k^{(i)}$ generation according to the recommended density function $q(X_k^{(i)}/(X_0^k(i),Z_0^k))\approx N({\overline{\chi }}_k^{(i)},P_k^{(i)})$ as the second sampling original particles;
• According to the weight value updating formula $w_k^{(i)}=w_{k-1}^{(i)}\frac{p(Z_k/{\chi }_k^{(i)})p({\chi }_k^{(i)}/{\chi }_{k-1}^{(i)})}{q[{\chi }_k^{(i)}/({\chi }_0^k(i),Z_0^k)]}$, the corresponding weight values of N particles are calculated and the normalization processing is performed;
• The original particles ${\chi }_k^{(i)}(i=1,2,\cdot \cdot \cdot ,N)$ are re-sampled by re-sampling algorithms to generate the second sampled particles ${\chi }_k^{(j)}(j=1,2,\cdot \cdot \cdot ,N)$ and their weight values are calculated;
• The optimal estimation of state variables and the corresponding covariance matrix of each particle are calculated according to ${\widehat{X}}_k=w_k^{(j)}{\displaystyle \sum _{j=1}^N{\chi }_k^{(i)}}$;
• The particles ${\chi }_k^{(j)}$ after re-sampling in step (4) and ${\tilde{P}}_k^{(i)}$ calculated in step (5) are substituted in step (2) for the iterative calculation.

3.2. The Adaptive UPF Algorithm in This Paper

The time-varying gradually fading factors are used to weaken the influence that the obsolete data on the current filtering value based on the idea of strong tracking filter, the covariance of state prediction error, and the corresponding gain matrix are adjusted in real-time to achieve this purpose. To some extent, the adaptive UPF can judge the convergence of the system in real time and improve the correction of the current information error to the system filter by introducing memory attenuation factor, it can also slow down the degradation of UPF particle and accelerate the convergence rate of the particle filter.

The adaptive measure taken in this paper is to judge the covariance of the filter, the specific formula is as follows [14,15]:

$${\tilde{Z}}_k^T{\tilde{Z}}_k\le S*tr(E({\tilde{Z}}_k{\tilde{Z}}_k^T))$$

(20)

where S is the setting adjustment coefficients, and generally S > 1; ${\tilde{Z}}_k$ is the residual error array of the system, ${\tilde{Z}}_k=Z_k-h({\overline{X}}_{k|k-1})$.

When Equation (14) is not true, $P_{k/k-1}^{(i)}$ is needed to carry out the effective amendment, the approach in this paper is the introduction of the adaptive weighting coefficient ${\lambda }_k$ of the attenuation memory factors, the specific definition can be expressed as:

$${\lambda }_k=\{\begin{array}{l}{\lambda }_0,{\lambda }_0\ge 1\hfill \\ 1,{\lambda }_0<1\hfill \end{array}$$

(21)

The correction formula is:

$$P_{k/k-1}^{(i)}={\lambda }_k{\displaystyle \sum _{j=0}^{2n}{W}_j^{(c)}({\chi }_{(j)k/k-1}^{(i)}-{\overline{\chi }}_{k/k-1}^{(i)}){({\chi }_{(j)k/k-1}^{(i)}-{\overline{\chi }}_{k/k-1}^{(i)})}^T+Q_{k-1}}$$

(22)

where:

$${\lambda }_0=\frac{tr(C_{0,k}-R)}{tr({\displaystyle \sum _{j=0}^{2n}{W}_j^{(c)}({\chi }_{(j)k/k-1}^{(i)}-{\overline{\chi }}_{k/k-1}^{(i)})}{(Z_{(j)k/k-1}^{(i)}-{\overline{Z}}_{k/k-1}^{(i)})}^T)}$$

(23)

$$C_{0,k}=\{\begin{array}{l}{\tilde{Z}}_k{{\tilde{Z}}_k}^T,k=1\hfill \\ \frac{\rho C_{0,k-1}+{\tilde{Z}}_k{{\tilde{Z}}_k}^T}{1+\rho }, k>1\hfill \end{array}$$

(24)

where $0<\rho \le 1$, its main role is to enhance quick tracking capability of the filter to the system state, the larger it is, the larger the assigned weight value of the current information is, the impact residual error of the current information on the estimate of the system is also more prominent. In order to ensure that the system has the ability of strong tracking with slowly changing circumstances and mutational status, $\rho =0.95$ in this paper.

In this paper, the implementation steps of the adaptive UPF are as follows:

• Initialization: k = 0; we sample particles ${\chi }_0^{(i)}(i=1,2,3\cdot \cdot \cdot ,N)$ from the initial probability distribution $p(x_0)$, for the simplified calculation, let ${\chi }_0^{(i)}~N({\overline{\chi }}_0^{(i)},P_0^i)$, where ${\overline{\chi }}_0^{(i)}={\chi }_0^{(i)}$, $P_0^i=P_0$;
• Forecast updating:
  According to Equations (15) and (19), ${\overline{\chi }}_{k/k-1}^{(i)}$ and ${\overline{Z}}_{k/k-1}^{(i)}$ are obtained. Then the specific covariance is as follows:

  $$P_{k/k-1}^{(i)}={\displaystyle \sum _{j=0}^{2n}{W}_j^{(c)}({\chi }_{(j)k/k-1}^{(i)}-{\overline{\chi }}_{k/k-1}^{(i)}){({\chi }_{(j)k/k-1}^{(i)}-{\overline{\chi }}_{k/k-1}^{(i)})}^T+Q_{k-1}}$$

  (25)
• Judge whether Equation (21) is satisfied or not; if satisfied, skip to the fifth step, otherwise correct $P_{k/k-1}^{(i)}$ in accordance with Equations (22), (23) and (24);
• Measurement updating:

  $${\overline{x}}_k^{(i)}={\overline{x}}_{k,k-1}^{(i)}+K_k(z_k-{\overline{z}}_{k,k-1}^{(i)})$$

  (26)

  $$P_k^{(i)}=P_{k,k-1}^{(i)}-K_k{P}_{zz}{K}_k^T$$

  (27)
• According to the weight value updating formula $w_k^{(i)}=w_{k-1}^{(i)}\frac{p(Z_k/{\chi }_k^{(i)})p({\chi }_k^{(i)}/{\chi }_{k-1}^{(i)})}{q[{\chi }_k^{(i)}/({\chi }_0^k(i),Z_0^k)]}$, the corresponding weight values of N particles are calculated and normalized;
• The original particles ${\chi }_k^{(i)}(i=1,2,\cdot \cdot \cdot ,N)$ are re-sampled by re-sampling algorithms to generate the second sampled particles ${\chi }_k^{(j)}(j=1,2,\cdot \cdot \cdot ,N)$ and their weight values are calculated;
• The optimal estimation of state variables and corresponding covariance matrix of each particle are calculated according to ${\widehat{X}}_k=w_k^{(j)}{\displaystyle \sum _{i=1}^N{\chi }_k^i}$;
• The particles ${\chi }_k^{(j)}$ after re-sampling in the sixth step and ${\tilde{P}}_k^{(j)}$ calculated in the seventh step are substituted in the second step for the iterative calculation.

4. Adaptive UPF Filter Influence Factors Analysis

4.1. The Influence of the Importance Probability Density Function on the Accuracy of Adaptive UPF

The selection of the importance probability density function embodies in the weight updating section of particles, namely [16,17]:

$$w_k^{(i)}=w_{k-1}^{(i)}\frac{p(Z_k/{\chi }_k^{(i)})p({\chi }_k^{(i)}/{\chi }_{k-1}^{(i)})}{q[{\chi }_k^{(i)}/({\chi }_0^k(i),Z_0^k)]}$$

(28)

The ideal importance probability density function can ensure $Var(w_k^j)=0$, namely the degradation of the particles can be completely eliminated. However, the optimum importance probability density function requires sampling from $p({\chi }_k^{(i)}/{\chi }_{k-1}^{(i)},Z_k)$, which is very difficult to carry out under the circumstance that the posterior probability density is a non-Gaussian case. In this paper, the specific method is:

$$p(Z_k/{\chi }_k^{(i)})=e^{[-0.5*{(Z_k-Z_{k/k-1})}^T*{(R)}^{-1}*(Z_k-Z_{k/k-1})]}$$

(29)

$$p({\chi }_k^{(i)}/{\chi }_{k-1}^{(i)})=e^{[-0.5*{(Z_k-Z_{k/k-1})}^T*{(Q)}^{-1}*(Z_k-Z_{k/k-1})]}$$

(30)

$$q(X_k^{(i)}/(X_0^k(i),Z_0^k)={(\sqrt{\left|P_k\right|})}^{-1}*e^{[-0.5*{(Z_k-Z_{k/k-1})}^T*{(P_k)}^{-1}*(Z_k-Z_{k/k-1})]}$$

(31)

where $p(Z_k/{\chi }_k^{(i)}), p({\chi }_k^{(i)}/{\chi }_{k-1}^{(i)}), q(X_0^k(i),Z_0^k)$ can be modified according to the actual situation, but the modification is intended to achieve making the weight value of a particle smaller when the difference between the predicted particle and the actual status is larger, when the predicted particle is in good agreement with the actual status, the weight value of the particle should be larger.

4.2. Influence of Re-Sampling Algorithm on the Filtering Accuracy

Re-sampling is raised against the degradation issue of the particle weight value. Its purpose is to remove the particles with small weight value, increase the particles with large weight value, while the total number of particles is maintained constant. There are four kinds of commonly-used and representative re-sampling strategies, namely polynomial re-sampling, stratified re-sampling, system re-sampling, and residual error re-sampling [2,18]. Merwe et al. [18] pointed out that, regardless of the re-sampling method you choose, the impact on the PF calculation method is not very large [2]. In this paper, simulation is done to analyze three kinds of methods: system re-sampling, remainder re-sampling, and polynomial re-sampling.

5. Simulation and its Analysis

5.1. Simulation Conditions

Under the condition of static base, gyro constant drift is 0.01°/h, random drift is 0.001°/$\sqrt{h}$; accelerometer zero bias is 100 µg (g = 9.8 m²/s), random deviation is 50 µg; the local geographic latitude is 32.37°, longitude is 118.22°. Simulation time is 2000 s.

5.2. Simulation Results and Analysis

5.2.1. The First Experiment

In accordance with the large azimuth misalignment angle error model, we partly use two kinds of filtering algorithms for the simulation experiment in the case where noise statistical properties are determined. Now we select the initial misalignment angle as $\phi (0)={\left[\begin{array}{ccc}1°& 1°& 10°\end{array}\right]}^T$, the feedback correction is not performed during the simulation process in both cases; the simulation results of the alignment error are shown in Figure 1 and Figure 2.

Figure 1. The alignment errors of large azimuth misalignment angle ($\phi (0)={\left[\begin{array}{ccc}1°& 1°& 10°\end{array}\right]}^T$): (a) Pitch errors; (b) Roll errors; (c) Heading errors.

Figure 1. The alignment errors of large azimuth misalignment angle ($\phi (0)={\left[\begin{array}{ccc}1°& 1°& 10°\end{array}\right]}^T$): (a) Pitch errors; (b) Roll errors; (c) Heading errors.

Table 1. Determining the statistical properties of the noise are the statistical results of the alignment.

	Mean/°			Variance/°
	Pitch Error	Roll Error	Heading Error	Pitch Error	Rollerror	Heading Error
UPF	0.0209	0.0189	1.0983	0.0524	0.0567	2.3819
Adaptive UPF	−0.0181	0.0074	−0.8609	0.0555	0.0423	1.3958

Table 1. Determining the statistical properties of the noise are the statistical results of the alignment.

	Mean/°			Variance/°
	Pitch Error	Roll Error	Heading Error	Pitch Error	Rollerror	Heading Error
UPF	0.0209	0.0189	1.0983	0.0524	0.0567	2.3819
Adaptive UPF	−0.0181	0.0074	−0.8609	0.0555	0.0423	1.3958

Figure 1 shows that when the initial misalignment angle is $\phi (0)={\left[\begin{array}{ccc}1°& 1°& 10°\end{array}\right]}^T$, and we use the adaptive UPF, the horizontal alignment time required less than 50 s, the azimuth alignment time required less than 300 s; using UPF, the horizontal alignment time needs about 300 s, and the azimuth alignment time is about 500 s based on SINS error model with large azimuth misalignment angle. The alignment time of the adaptive UPF is obviously superior to UPF, but alignment accuracy of both is considerable. It can be seen that to some extent the alignment accuracy using adaptive UPF is higher than UPF from Table 1.

5.2.2. The Second Experiment

In order to verify the filtering performance of the two kinds of filtering methods under the uncertain noise situation, we specifically add noise whose variances are all 0.02 to the acceleration of three directions to do experimental analysis. Take $\phi (0)={\left[\begin{array}{ccc}1°& 1°& 10°\end{array}\right]}^T$. As it can be seen from Figure 2, when the measurement noise increases, the alignment of the horizontal direction will have a substantial shock after using UPF and the system may have been affected to some extent. The horizontal alignment time is about 700 s, the azimuth alignment time is about 900 s. After using the adaptive UPF, the level alignment time is significantly better than UPF, about 50 s, and the error curve is smooth. The azimuth alignment needs 450 s, so the alignment time is much shorter than UPF. From the statistical results of Table 2, the alignment accuracy using the adaptive UPF is significantly better than UPF.

Table 2. The statistical properties of uncertain noise are the statistical results of alignment.

	Mean/°			Variance/°
	Pitch Error	Roll Error	Head Error	Pitch Error	Roll Error	Head Error
UPF	0.0454	0.0278	2.8174	0.0648	0.0634	4.9038
Adaptive UPF	0.0105	0.0122	−0.7304	0.0629	0.0425	2.2557

Table 2. The statistical properties of uncertain noise are the statistical results of alignment.

	Mean/°			Variance/°
	Pitch Error	Roll Error	Head Error	Pitch Error	Roll Error	Head Error
UPF	0.0454	0.0278	2.8174	0.0648	0.0634	4.9038
Adaptive UPF	0.0105	0.0122	−0.7304	0.0629	0.0425	2.2557

Figure 2. The alignment errors of large misalignment angles ($\phi (0)={\left[\begin{array}{ccc}1°& 1°& 10°\end{array}\right]}^T$): (a) Pitch errors; (b) Roll errors; (c) Heading errors.

Figure 2. The alignment errors of large misalignment angles ($\phi (0)={\left[\begin{array}{ccc}1°& 1°& 10°\end{array}\right]}^T$): (a) Pitch errors; (b) Roll errors; (c) Heading errors.

5.2.3. The Third Experiment

This paper also analyzes the influence of re-sampling algorithms on the filtering effect of the adaptive UPF by experiment. Figure 3 shows the results of residual re-sampling, the systematic re-sampling, multinomial re-sampling applied with the adaptive UPF.

Figure 3. The influence of different re-sampling algorithms on alignment accuracy: (a) Pitch errors; (b) Roll errors; (c) Heading errors.

Figure 3. The influence of different re-sampling algorithms on alignment accuracy: (a) Pitch errors; (b) Roll errors; (c) Heading errors.

Figure 3 shows the alignment accuracy of the three re-sampling algorithms is comparable after the alignment is finished but, initially, using a multinomial re-sampling algorithm has larger and longer time jitter in the horizontal and heading direction, so we can select a different re-sampling algorithm based on the system.

6. Turntable Experiment

The initial alignment of a large azimuth misalignment angle in SINS based on adaptive UPF proposed in this paper has been verified by a turntable experiment. The experiment was run as a semi-physical simulation with data collected from the turntable and SINS.

6.1. Experiment Setup

6.1.1. Turntable and SINS

The turntable used in this experiment is shown in Figure 4. In the turntable, the rate controlling accuracy is ±0.0005°/s and angle measuring accuracy is ±0.0001°. In addition, angle information can be provided via a serial communication port as a response to the external time-synchronization signal. In the experiment, the inner, intermediate, and outer frames are used to simulate the ship’s roll, pitch, and yaw respectively.

The strapdown inertial navigation system used in this experiment which is developed by Casic33s is shown in Figure 4. Fiber optic gyros and quartz accelerometers are used in this type of SINS. The sensor precision of the SINS is provided in Table 3. The update frequency of the turntable data and SINS sensor data are 100 Hz.

Figure 4. The turntable and SINS.

Figure 4. The turntable and SINS.

Table 3. Sensor precision of SINS.

Gyro		Accelerometer
Constant errors	0.006°/h	Constant errors	50 µg
Random errors	0.006°/$\sqrt{h}$	Random errors	50 µg

Table 3. Sensor precision of SINS.

Gyro		Accelerometer
Constant errors	0.006°/h	Constant errors	50 µg
Random errors	0.006°/$\sqrt{h}$	Random errors	50 µg

The constant drift, scale factors, cross coupling coefficient, installation error angle, and so on, can be calculated and compensated by the exact calibration according to [16,19], so these errors all can be ignored in the calibration.

6.1.2. Construction of the Experimental Environment

As shown in Figure 5, the experimental environment consists of a turntable, IMU (FOSN), computer, time-synchronization signal generator, data acquisition card, local area network, serial communication port, and so on.

Figure 5. Experimental environment.

Figure 5. Experimental environment.

A time-synchronization signal of 100 Hz is introduced to make the IMU data and turntable data synchronous. In this experimental environment, once the time-synchronization signal is active, the current angle information of the turntable and the information of the IMU will be sent to the navigation computer via the serial communication port, then the current sensor data will be collected for navigation solution and data fusion and, finally, navigation parameters will be sent back to the recording data computer at 1 Hz by the navigation computer. Additionally, data from the SINS and the turntable should be stored in the recording data computer in order to evaluate different alignment algorithms.

6.2. Experimental Results and Analysis

During the experiment, the inner and intermediate frames of the turntable are constantly kept in a level status. The outer frame rotates to 10° (this value can be selected randomly); then we can consider the carrier theoretical attitude value as ${\left[\begin{array}{ccc}0°& 0°& 10°\end{array}\right]}^T$. Two alignment schemes based on UPF and adaptive UPF are compared through the semi-physical simulation. The experiment lasts for 2000 s. Estimation error curves for the misalignment are shown as Figure 6.

From Figure 6, it can be seen that, when using the adaptive UPF, the horizontal alignment time requires less than 50 s, the azimuth alignment time requires about 500 s; using UPF, the horizontal alignment time needs about 300 s, and the azimuth alignment time is about 700 s, based on the SINS error model with a large azimuth misalignment angle. The alignment time of the adaptive UPF is obviously superior to UPF.

Figure 6. Estimation error curves for misalignment angles: (a) Pitch errors; (b) Roll errors; (c) Heading errors.

Figure 6. Estimation error curves for misalignment angles: (a) Pitch errors; (b) Roll errors; (c) Heading errors.

7. Conclusions

Based on the established nonlinear state equation of SINS with a large azimuth misalignment error, the initial alignment adopts UPF and the adaptive UPF under the circumstance of the large azimuth misalignment in SINS, where the statistical noise characteristics are fixed or not. The simulation shows that when the noise statistical properties are certainly determined, using the adaptive UPF has faster alignment speed than UPF, while the alignment accuracy advantage is not obvious. When the noise statistical properties are uncertainly determined, comparing the adaptive UPF with the normal UPF, the speed and the accuracy of alignment has obvious advantages. In addition, it is found that the system error caused by a residual error re-sampling algorithm is larger than the systematic re-sampling and polynomial re-sampling by choosing different re-sampling algorithms, which cannot be ignored in the high-precision navigation systems. Turntable experiments were done to certify the feasibility and superiority of the initial alignment of large azimuth misalignment angle in SINS base on adaptive UPF. They provide theoretical evidence and a calculation method for the initial alignment of large azimuth misalignment angles on a static base of SINS based on adaptive UPF in engineering, and at the same time provides a new idea for the initial alignment on a moving base of SINS based on adaptive UPF.