# Novel SINS Initial Alignment Method under Large Misalignment Angles and Uncertain Noise Based on Nonlinear Filter

Authors: Bo Yang; Xiaosu Xu; Tao Zhang; Jin Sun; Xinyu Liu
Journal: Mathematical Problems in Engineering
 (2017)

---

## Abstract

For the SINS initial alignment problem under large misalignment angles and uncertain noise, two novel nonlinear filters, referred to as transformed unscented quadrature Kalman filter (TUQKF) and robust transformed unscented quadrature Kalman filter (RTUQKF), are proposed in this paper, respectively. The TUQKF sets new deterministic sigma points to address the nonlocal sampling problem and improve the numerical accuracy. The RTUQKF is the combination ofH∞ technique and TUQKF. It improves the accuracy and robustness of state estimation. Simulation results indicate that TUQKF performs better than traditional filters when misalignment angles are large. Turntable and vehicle experiments results indicate that, under the condition of uncertain noise, the performances of RTUQKF are better than other filters and more robust. These two methods can effectively further increase precision and convergence speed of SINS initial alignment.


---

## Body

## 1. Introduction

Initial alignment is one of the critical and difficult problems for inertial navigation system (INS). The essential purpose of initial alignment for strapdown INS (SINS) is to determine the attitude matrix between body frame and navigation frame [1, 2]. The Kalman filter (KF) is the mostly used technique to solve the problem of initial alignment. However, it can only deal with initial alignment under small misalignment angles [3]. Large misalignment angles and uncertain noise are two main problems existing in initial alignment in different application environments [4, 5]. The nonlinear model of SINS and nonlinear methods are developed to solve the alignment problem. In [6], a modeling method of nonlinear model of SINS was proposed. The widely used nonlinear filtering method in engineering is extended KF (EKF). The principle of EKF is simple and it has high computationally efficient [7]. However, the performance of EKF would decrease if the system has strong nonlinear characters [8]. The unscented KF (UKF) and cubature KF (CKF) were developed to overcome this problem [9, 10]. These two algorithms were applied to SINS initial alignment and got better effects than EKF [11, 12]. Although the derivation method of CKF is different from standard UKF, it is virtually a special case of the UKF [13]. It has pointed out that CKF has better precisions than UKF for high-dimensional problem [10]. However, it has proved that CKF suffers from nonlocal sampling problem which will lead to estimation errors in high-dimensional and strong nonlinear situations [14]. From [6] and Section 5, it is shown that nonlinear model of SINS includes a lot of trigonometric operations and the state dimensions are more than 10, so the SINS initial alignment problem under large misalignment angles is the high-dimensional problem coupled with strong nonlinear model. Therefore, in [14], new set of sigma points was designed to solve this problem and the novel algorithm is known as transformed unscented KF (TUKF). In this paper, in order to further increase accuracy of SINS initial alignment, the transformed unscented quadrature KF (TUQKF) was proposed based on TUKF. TUQKF is an extended version of TUKF. It is proved that, under single Gauss-Laguerre quadrature rule, TUQKF degenerates to TUKF and TUQKF exhibits better numerical characteristics than TUKF when using high-order Gauss-Laguerre rule.For the other problem, SINS initial alignment with uncertain noise, if environment noise is not Gaussian white noise, the traditional nonlinear filtering method mentioned above will produce a greater estimation error. One solution for this problem is to use theH∞ technique to improve the robustness of filter. The H∞ filter is more robust and has better precision than standard KF under uncertain noise [15, 16]. However, standard H∞ filter can only fulfill the linear filtering problems. In the recent two decades, H∞ filter was extended to nonlinear problems by combing with nonlinear filters. In [17], the H∞ filter was combined with EKF and the new algorithm known as robust extended KF (REKF) was proposed. In this framework, the H∞ filter was combined with UKF and CKF. These two new algorithms are known as robust unscented KF (REKF) and cubature H∞ filter (CH∞F) [16, 18]. In [18], the RUKF was applied to the SINS initial alignment with colored noise and achieved better accuracy than UKF. In this paper, in order to further increase the performance of filter for SINS initial alignment, H∞ filter has been combined with TUQKF and a novel algorithm named as robust transformed unscented quadrature KF (RTUQKF) was proposed. The proposed TUQKF and RTUQKF were used for SINS initial alignment, respectively, and compared with the square-root cubature KF (SRCKF) in mathematical simulation, turntable experiment, and vehicle experiment.The rest of paper is organized as follows. Section2 reviews the conventional nonlinear filters. Section 3 derives the TUQKF algorithm. The RTUQKF algorithm is derived in Section 4. The nonlinear model of SINS is established in Section 5. Three experiments are conducted in Section 6 to verify the effectiveness of TUQKF and RTUQKF. Finally, Conclusions are drawn in Section 7.


## 2. Conventional Nonlinear Filters

The general discrete nonlinear model is given by(1)xk=fxk-1+ωk-1zk=hxk+vk,where xk∈Rn and zk∈Rm are state and measurement, f(·) is nonlinear state function, and h(·) is nonlinear measurement function. ωk-1 and vk are uncorrelated zero-mean white Gaussian noise with variances Qk and Rk, respectively.The state vectorxk can be estimated by general Gaussian approximation filter. The heart of the Gaussian approximation filter is to evaluate the integral as follows [19]:(2)If=∫RnFx×Nx;x^,Pdxk,where F(x) is an arbitrary nonlinear function.Gauss–Hermite quadrature rule [20], unscented transformation method [8], and the cubature rule [10] are generally used to approximate the above integral:(3)∫RnFx×Nx;x^,Pdx=∫RnFSx+x^×Nx;0,Idx≈∑i=1NsWiFSβi+x^,where βi is quadrature points, Wi is corresponding weights, and S is square root of P; namely, P=SST.Therefore, the main purpose of evaluating integral (2) is to obtain quadrature points βi and weights Wi.Using Gauss–Hermite quadrature rule to approximate the integral (2), the total number of points increases exponentially with the dimension n and it leads to the curse of dimensionality problem [21]. So, it is not suitable for SINS initial alignment which is the high-dimensional problem. The unscented transformation method and cubature rule also bear the problem of accuracy decline for high-dimensional or strong nonlinear problems [10, 14]. To solve these problems, the TUKF selects novel sigma points to approximate the integral (2); the βi and Wi are given by [14](4)Υi=γi,1,γi,2,…,γi,nT;i=1,2,…,2n,βi=nΥi;i=1,2,…,2n,Wi=12n;i=1,2,…,2n,where(5)γi,2r-1=2ncos⁡2r-1iπnγi,2r=2nsin2r-1iπn,where i=1,2,…,2n, r=1,2,…,[n/2], and [n/2] denotes the greatest integer less than n/2. ri,n=(-1)i-1 when n is odd.In this paper, we propose a new sigma point selection strategy based on cubature rule and TUKF to further improve the performance of filter for high-dimensional problem.


## 3. Transformed Unscented Quadrature Kalman Filter

### 3.1. Cubature Rule and Transformed Cubature Points

Putting Gaussian probability density function into (2) yields(6)If=1P2πn∫Rnfxe-x-μx-μT/2Pdx,where μ and P are the mean and variance of state vector x, respectively.Equation (6) can be transformed by spherical-radial cubature rule [10, 22]:(7)If=12πn∫r=0∞∫UnfSrZ+μdsZrn-1e-r2/2dr,where x=SrZ+μ, S denotes Cholesky decomposition of P, namely P=SST, Z=1, and Un denotes surface of unit hypersphere.First, compute integral(8)∫UnfSrZ+μdsZ.Integral (8) can be approximated by [10](9)∫UnfSrZ+μdsZ≈∑i=1kfSrαi+μw,where αi are space points and αi=1, k denotes number of space points, and w denotes corresponding weights.It can be seen that the computational precision of integral (7) depends on αi and w. Performance of filters is also influenced by αi and w.Theαi can be selected by(10)αi=αi,1,αi,2,…,αi,nT,where(11)αi,2r-1=2ncos⁡2r-1iπn,αi,2r=2nsin⁡2r-1iπn,where i=1,2,…,2n and r=1,2,…,[n/2]. If n is odd, ri,n=(-1)i-1.In [14], it has been proved that α=α1,α2,…,α2n is an orthogonal matrix, so αi=1.Thew can be calculated by [10](12)w=2πn2nΓn/2.


### 3.2. Gauss-Laguerre Quadrature Rule

Bringing (9) and (12) into (6) and letting λ=r2/2 result in(13)If≈12nΓn/2∫λ=0∞∑i=12nfS2λαi+μλn/2-1e-λdλ.Defineg(λ) as(14)gλ=∑i=12nfS2λαi+μ.Computing integral (6) is transformed into computing integral (13). First, calculate the integral(15)∫λ=0∞gλλn/2-1e-λdλ.Gauss-Laguerre quadrature rule can be used to approximate integral (15) [10, 22]:(16)∫x=0∞gxxγe-xdx≈∑i=1mAigλi,where λi is quadrature point and Ai is corresponding weight.λ i can be calculated from(17)Lmγλ=-1mλ-γeλdmdλmλγ+me-λ=0.The weights can be determined as(18)Ai=m!Γγ+m+1λiL˙mγλ2.The precision of the integral depends on the number of quadrature points. The higher the number of quadrature points is the better the precision would be [22]. In addition, the quadrature points and weights of Gauss-Laguerre quadrature are independent of nonlinear function f(x), so quadrature points and weights always exist and can be computed and stored offline.Bringing approximate results of integral (15) into (13) yields(19)If≈∑j=1m∑i=12nfSξj+μWj,where(20)ξj=2λjαi,Wj=Aj2nΓn/2,where ξj is sigma points and Wj is corresponding weight of TUQKF.If integral (15) is solved with m order Gauss-Laguerre quadrature rule, the TUQKF is named as m-order TUQKF (TUQKF-m).


### 3.3. Analyses of TUQKF

If using first-order Gauss-Laguerre quadrature rule, quadrature point and weight areλ1=n/2 and A1=Γ(n/2). Putting λ1 and A1 into (20) yields(21)ξi=nαi;i=1,2,…,2n,Wi=12n;i=1,2,…,2n.Comparing (4) and (21), it can be seen that the sigma points and weights of the TUKF and the TUQKF-1 are the same. This means TUQKF-1 coincides with TUKF. So, TUKF is also derived in this paper from another point of view and TUQKF is an extended version of the TUKF. The precision of the filter is influenced by the order of Gauss-Laguerre rule. The higher the number of quadrature points is the higher the calculation precision of integral (2) would be. The higher the calculation accuracy of integral (2) the better the accuracy of the filter would be. So, mathematically, the performance of the TUQKF-m is better than the TUKF when m>1.Compared to EKF, TUQKF does not need to calculate Jacobian matrices and is more suitable for the system which has strong characters of nonlinear. Compared to UKF, TUQKF maintains the positive definiteness of the weight and the covariance matrix for high-dimensional problem. Compared to CKF, TUQKF also solves the nonlocal sampling problem. So, TUQKF is suitable for high-dimensional problem coupled with strong nonlinear model.Finally, discuss the computational efficiency. TUQKF-m requires M=2nm sigma points, so it is computationally more efficient than the Gauss–Hermite filter which requires mn sigma points. However, the computational cost of TUQKF-m is slightly higher than CKF and TUKF when m>1.When using the TUQKF-m, the balance between the accuracy and the computational cost should be considered. If the dimension of state vector is high, m should not be chosen very large. In addition, in practice, the assumption that the posterior density functions obey the Gauss distribution is hardly met. So, a large value of m would cause the problems such as “over fitting.” Considering the above problems, m was set to 2 in this paper.


### 3.4. TUQKF Algorithm Flow

#### 3.4.1. Initial Filter

(i)
Initialx^0 and P0 by(22)x^0=Ex0,P0=Ex^0-x0x^0-x0T. (ii)
Compute sigma pointξi and weight Wi (i=1,2,…,M).


#### 3.4.2. Time Update

(i)
Factorize(23)Pk-1∣k-1=Sk-1∣k-1Sk-1∣k-1T. (ii)
Evaluate and update transformed unscented quadrature points(24)Xi,k-1∣k-1=Sk-1∣k-1ξi+x^k-1∣k-1i=1,2,…,M,Xi,k-1∣k-1∗=fXi,k-1∣k-1i=1,2,…,M. (iii)
Estimate predicted state and error covariance(25)x^k∣k-1=∑i=1MWiXi,k-1∣k-1∗,Pk∣k-1=∑i=1MWiXi,k-1∣k-1∗Xi,k-1∣k-1∗T-x^k∣k-1x^k∣k-1T+Qk-1.


#### 3.4.3. Measurement Update

(i)
Factorize(26)Pk∣k-1=Sk∣k-1Sk∣k-1T. (ii)
Evaluate and update transformed unscented quadrature points(27)Xi,k∣k-1=Sk∣k-1ξi+x^k-1∣k-1i=1,2,…,M,Zi,k∣k-1=hXi,k-1∣k-1i=1,2,…,M. (iii)
Estimate predicted measurement(28)z^k∣k-1=∑i=1MWiZi,k-1∣k-1. (iv)
Estimate innovation covariance and cross-covariance matrix(29)Pzz,k∣k-1=∑i=1MWiZi,k-1∣k-1Zi,k-1∣k-1T-z^k∣k-1z^k∣k-1T,Pxz,k∣k-1=∑i=1MWiXi,k-1∣k-1Zi,k-1∣k-1T-x^k∣k-1z^k∣k-1T. (v)
Estimate the gain(30)K=Pxz,k∣k-1Pzz,k∣k-1+Rk-1. (vi)
Estimate updated state and corresponding error covariance(31)x^k∣k=x^k∣k-1+Kzk-z^k∣k-1,(32)Pk∣k=Pk∣k-1-KPzz,k∣k-1+RkKT.Measurement function described in (1) can be simplified as the following linear function:(33)zk=Hkxk+vk,where Hk is the measurement matrix.Measurement-update steps (26)~(29) can be simplified as(34)z^k∣k-1=Hkx^k∣k-1,(35)Pzz,k∣k-1=HkPk∣k-1HkT,(36)Pxz,k∣k-1=Pk∣k-1HkT.


## 3.1. Cubature Rule and Transformed Cubature Points

Putting Gaussian probability density function into (2) yields(6)If=1P2πn∫Rnfxe-x-μx-μT/2Pdx,where μ and P are the mean and variance of state vector x, respectively.Equation (6) can be transformed by spherical-radial cubature rule [10, 22]:(7)If=12πn∫r=0∞∫UnfSrZ+μdsZrn-1e-r2/2dr,where x=SrZ+μ, S denotes Cholesky decomposition of P, namely P=SST, Z=1, and Un denotes surface of unit hypersphere.First, compute integral(8)∫UnfSrZ+μdsZ.Integral (8) can be approximated by [10](9)∫UnfSrZ+μdsZ≈∑i=1kfSrαi+μw,where αi are space points and αi=1, k denotes number of space points, and w denotes corresponding weights.It can be seen that the computational precision of integral (7) depends on αi and w. Performance of filters is also influenced by αi and w.Theαi can be selected by(10)αi=αi,1,αi,2,…,αi,nT,where(11)αi,2r-1=2ncos⁡2r-1iπn,αi,2r=2nsin⁡2r-1iπn,where i=1,2,…,2n and r=1,2,…,[n/2]. If n is odd, ri,n=(-1)i-1.In [14], it has been proved that α=α1,α2,…,α2n is an orthogonal matrix, so αi=1.Thew can be calculated by [10](12)w=2πn2nΓn/2.


## 3.2. Gauss-Laguerre Quadrature Rule

Bringing (9) and (12) into (6) and letting λ=r2/2 result in(13)If≈12nΓn/2∫λ=0∞∑i=12nfS2λαi+μλn/2-1e-λdλ.Defineg(λ) as(14)gλ=∑i=12nfS2λαi+μ.Computing integral (6) is transformed into computing integral (13). First, calculate the integral(15)∫λ=0∞gλλn/2-1e-λdλ.Gauss-Laguerre quadrature rule can be used to approximate integral (15) [10, 22]:(16)∫x=0∞gxxγe-xdx≈∑i=1mAigλi,where λi is quadrature point and Ai is corresponding weight.λ i can be calculated from(17)Lmγλ=-1mλ-γeλdmdλmλγ+me-λ=0.The weights can be determined as(18)Ai=m!Γγ+m+1λiL˙mγλ2.The precision of the integral depends on the number of quadrature points. The higher the number of quadrature points is the better the precision would be [22]. In addition, the quadrature points and weights of Gauss-Laguerre quadrature are independent of nonlinear function f(x), so quadrature points and weights always exist and can be computed and stored offline.Bringing approximate results of integral (15) into (13) yields(19)If≈∑j=1m∑i=12nfSξj+μWj,where(20)ξj=2λjαi,Wj=Aj2nΓn/2,where ξj is sigma points and Wj is corresponding weight of TUQKF.If integral (15) is solved with m order Gauss-Laguerre quadrature rule, the TUQKF is named as m-order TUQKF (TUQKF-m).


## 3.3. Analyses of TUQKF

If using first-order Gauss-Laguerre quadrature rule, quadrature point and weight areλ1=n/2 and A1=Γ(n/2). Putting λ1 and A1 into (20) yields(21)ξi=nαi;i=1,2,…,2n,Wi=12n;i=1,2,…,2n.Comparing (4) and (21), it can be seen that the sigma points and weights of the TUKF and the TUQKF-1 are the same. This means TUQKF-1 coincides with TUKF. So, TUKF is also derived in this paper from another point of view and TUQKF is an extended version of the TUKF. The precision of the filter is influenced by the order of Gauss-Laguerre rule. The higher the number of quadrature points is the higher the calculation precision of integral (2) would be. The higher the calculation accuracy of integral (2) the better the accuracy of the filter would be. So, mathematically, the performance of the TUQKF-m is better than the TUKF when m>1.Compared to EKF, TUQKF does not need to calculate Jacobian matrices and is more suitable for the system which has strong characters of nonlinear. Compared to UKF, TUQKF maintains the positive definiteness of the weight and the covariance matrix for high-dimensional problem. Compared to CKF, TUQKF also solves the nonlocal sampling problem. So, TUQKF is suitable for high-dimensional problem coupled with strong nonlinear model.Finally, discuss the computational efficiency. TUQKF-m requires M=2nm sigma points, so it is computationally more efficient than the Gauss–Hermite filter which requires mn sigma points. However, the computational cost of TUQKF-m is slightly higher than CKF and TUKF when m>1.When using the TUQKF-m, the balance between the accuracy and the computational cost should be considered. If the dimension of state vector is high, m should not be chosen very large. In addition, in practice, the assumption that the posterior density functions obey the Gauss distribution is hardly met. So, a large value of m would cause the problems such as “over fitting.” Considering the above problems, m was set to 2 in this paper.


## 3.4. TUQKF Algorithm Flow

### 3.4.1. Initial Filter

(i)
Initialx^0 and P0 by(22)x^0=Ex0,P0=Ex^0-x0x^0-x0T. (ii)
Compute sigma pointξi and weight Wi (i=1,2,…,M).


### 3.4.2. Time Update

(i)
Factorize(23)Pk-1∣k-1=Sk-1∣k-1Sk-1∣k-1T. (ii)
Evaluate and update transformed unscented quadrature points(24)Xi,k-1∣k-1=Sk-1∣k-1ξi+x^k-1∣k-1i=1,2,…,M,Xi,k-1∣k-1∗=fXi,k-1∣k-1i=1,2,…,M. (iii)
Estimate predicted state and error covariance(25)x^k∣k-1=∑i=1MWiXi,k-1∣k-1∗,Pk∣k-1=∑i=1MWiXi,k-1∣k-1∗Xi,k-1∣k-1∗T-x^k∣k-1x^k∣k-1T+Qk-1.


### 3.4.3. Measurement Update

(i)
Factorize(26)Pk∣k-1=Sk∣k-1Sk∣k-1T. (ii)
Evaluate and update transformed unscented quadrature points(27)Xi,k∣k-1=Sk∣k-1ξi+x^k-1∣k-1i=1,2,…,M,Zi,k∣k-1=hXi,k-1∣k-1i=1,2,…,M. (iii)
Estimate predicted measurement(28)z^k∣k-1=∑i=1MWiZi,k-1∣k-1. (iv)
Estimate innovation covariance and cross-covariance matrix(29)Pzz,k∣k-1=∑i=1MWiZi,k-1∣k-1Zi,k-1∣k-1T-z^k∣k-1z^k∣k-1T,Pxz,k∣k-1=∑i=1MWiXi,k-1∣k-1Zi,k-1∣k-1T-x^k∣k-1z^k∣k-1T. (v)
Estimate the gain(30)K=Pxz,k∣k-1Pzz,k∣k-1+Rk-1. (vi)
Estimate updated state and corresponding error covariance(31)x^k∣k=x^k∣k-1+Kzk-z^k∣k-1,(32)Pk∣k=Pk∣k-1-KPzz,k∣k-1+RkKT.Measurement function described in (1) can be simplified as the following linear function:(33)zk=Hkxk+vk,where Hk is the measurement matrix.Measurement-update steps (26)~(29) can be simplified as(34)z^k∣k-1=Hkx^k∣k-1,(35)Pzz,k∣k-1=HkPk∣k-1HkT,(36)Pxz,k∣k-1=Pk∣k-1HkT.


## 3.4.1. Initial Filter

(i)
Initialx^0 and P0 by(22)x^0=Ex0,P0=Ex^0-x0x^0-x0T. (ii)
Compute sigma pointξi and weight Wi (i=1,2,…,M).


## 3.4.2. Time Update

(i)
Factorize(23)Pk-1∣k-1=Sk-1∣k-1Sk-1∣k-1T. (ii)
Evaluate and update transformed unscented quadrature points(24)Xi,k-1∣k-1=Sk-1∣k-1ξi+x^k-1∣k-1i=1,2,…,M,Xi,k-1∣k-1∗=fXi,k-1∣k-1i=1,2,…,M. (iii)
Estimate predicted state and error covariance(25)x^k∣k-1=∑i=1MWiXi,k-1∣k-1∗,Pk∣k-1=∑i=1MWiXi,k-1∣k-1∗Xi,k-1∣k-1∗T-x^k∣k-1x^k∣k-1T+Qk-1.


## 3.4.3. Measurement Update

(i)
Factorize(26)Pk∣k-1=Sk∣k-1Sk∣k-1T. (ii)
Evaluate and update transformed unscented quadrature points(27)Xi,k∣k-1=Sk∣k-1ξi+x^k-1∣k-1i=1,2,…,M,Zi,k∣k-1=hXi,k-1∣k-1i=1,2,…,M. (iii)
Estimate predicted measurement(28)z^k∣k-1=∑i=1MWiZi,k-1∣k-1. (iv)
Estimate innovation covariance and cross-covariance matrix(29)Pzz,k∣k-1=∑i=1MWiZi,k-1∣k-1Zi,k-1∣k-1T-z^k∣k-1z^k∣k-1T,Pxz,k∣k-1=∑i=1MWiXi,k-1∣k-1Zi,k-1∣k-1T-x^k∣k-1z^k∣k-1T. (v)
Estimate the gain(30)K=Pxz,k∣k-1Pzz,k∣k-1+Rk-1. (vi)
Estimate updated state and corresponding error covariance(31)x^k∣k=x^k∣k-1+Kzk-z^k∣k-1,(32)Pk∣k=Pk∣k-1-KPzz,k∣k-1+RkKT.Measurement function described in (1) can be simplified as the following linear function:(33)zk=Hkxk+vk,where Hk is the measurement matrix.Measurement-update steps (26)~(29) can be simplified as(34)z^k∣k-1=Hkx^k∣k-1,(35)Pzz,k∣k-1=HkPk∣k-1HkT,(36)Pxz,k∣k-1=Pk∣k-1HkT.


## 4. Robust Transformed Unscented Quadrature Kalman Filter

The discrete nonlinear model can be expressed as(37)xk=fxk-1+ωk-1,zk=hxk+vk,yk=Lkxk,where xk∈Rn and zk∈Rm are the state and measurement. f(·) is nonlinear state function, and h(·) is nonlinear measurement function. ωk-1 and vk are noise with uncertain statistical properties. yk∈Rn is the signal to be estimated and Lk is known.Nonlinear filters discussed above are derived under the condition of zero-mean Gaussian white noise. However, statistical properties ofωk-1 and vk are uncertain in (37). We can assume that the noise is zero-mean Gaussian white noise with variances Q and R. However, if the assumption is false, the nonlinear filters discussed above will produce greater estimation errors. Combining nonlinear filters with H∞ technique can improve the performance of filters under the different noise with different statistical properties.


### 4.1. REKF Algorithm

The robust extended Kalman filter (REKF) is the combination ofH∞ filter and EKF [17]. It contains the following two steps:Time update:(38)x^k∣k-1=fx^k-1∣k-1,Pk∣k-1=Fk-1Pk∣k-1Fk-1T+Qk-1,where Fk-1 is Jacobian matrix of f(·) computed at x^k-1∣k-1.Measurement update:(39)x^k∣k=x^k∣k-1+Kzk-hx^k∣k-1,(40)K=Pk∣k-1HkTHkPk∣k-1HkT+Rk-1,(41)Pk∣k=Pk∣k-1-Pk∣k-1HkTLkTRe,k-1HkTLkTTPk∣k-1,(42)Re,k=R00-γ2I+HkTLkTTPk∣k-1HkTLkT,where Hk is Jacobian matrix of h(·) computed at x^k∣k-1. γ is the bound which can be chosen by designer.If, and only if, the condition is satisfied,(43)Pk∣k-γ-2LkTLk>00≤k≤N,the filter can be designed to achieve the desired accuracy by iterating γ to arrive at a suboptimal solution [23, 24].It can be easily shown that REKF reverts to EKF withγ→∞. Thus, γ can be thought as a tuning parameter to control the trade-off between performance of H∞ and minimum variance performance [17, 25]. Equation (43) will be satisfied easily with a large value of γ, but the large value of γ reduces the robustness of the filter. If we set γ to a small value, there is no guarantee that (43) will be satisfied during the process of filtering. So, the selection of the parameter γ is very important. One method to choose parameter γ will be given in the next part.


### 4.2. Robust Transformed Unscented Quadrature Kalman Filter

In this part, the RTUQKF algorithm will be derived based on REKF framework.Equations (38) in timing update of REKF can be replaced by the (23)~(25) in timing update of TUQKF and the Jacobian matrix Hk can be replaced by the method based on the following two approximations [26]:(44)Pzz,k∣k-1≈HkPk∣k-1HkT,Pxz,k∣k-1≈Pk∣k-1HkT.For SINS initial alignment, the signal which needs to be estimated is statexk, so Lk can be set to I. Putting Lk=I, (44) into (41)~(42), results in(45)Pk∣k=Pk∣k-1-Pk∣k-1HkTPk∣k-1Re,k-1HkPk∣k-1Pk∣k-1≈Pk∣k-1-Pxz,k∣k-1Pk∣k-1Re,k-1Pxz,k∣k-1TPk∣k-1Re,k=R00-γ2I+HkPk∣k-1HkTHkPk∣k-1Pk∣k-1HkTPk∣k-1≈R+Pzz,k∣k-1Pxz,k∣k-1TPxz,k∣k-1-γ2I+Pk∣k-1.P z z , k ∣ k - 1 and Pxz,k∣k-1 can be calculated by (29) or (35)~(36). The RTUQKF algorithm can be summarized as follows:(1)
Initialx^0 and P0 by (22).(2)
Time update: compute (23)~(25).(3)
Measurement update: compute (26)~(31) and (45).If measurement function described in (37) can be simplified as linear function (33), the measurement-update steps (26)~(29) can be simplified as (34)~(36).IfH∞ filter is combined with TUQKF-m, the proposed RTUQKF is named as m-order RTUQKF (RTUQKF-m).One way to choose parameterγ was proposed in [27].Applying the matrix inversion lemma for (41) and Pk∣k should maintain the positive definiteness during the process of filtering, so it can be easily shown that(46)Pk∣k-1=Pk∣k-1-1+HkTR-1Hk-γ2I>0.Thus,γ can be selected as(47)γ2=αmax⁡eigPk∣k-1-1+HkTR-1Hk-1,where max⁡eigA-1 denotes the maximum eigenvalue of the matrix A-1 and α is a scalar larger than one.Putting (44) into (47), γ in RTUQKF can be selected as(48)γ2=αmax⁡eigPk∣k-1-1+Pk∣k-1-1Pxz,k∣k-1R-1Pk∣k-1-1Pxz,k∣k-1T-1.


### 4.3. A Comparison of TUQKF with RTUQKF

Comparing RTUQKF and TUQKF, it can be found that their essential distinction is thatPk∣k is different. The parameter γ is added in the RTUQKF and K is adjusted by changing the value of Pk∣k. So, RTUQKF improves the robustness of system by the parameter γ. Similarly, the RTUQKF reverts to TUKF with γ→∞.TUQKF can also be used under the condition of uncertain noise, although its performance may not be better than RTUQKF. However, RTUQKF is more complex than TUQKF and its computational cost is also slightly higher than TUQKF. In particular, a false parameterγ would lead to filter divergence. So, in this paper, these two new methods were proposed and in practice, one of them can be selected according to demand and actual situation.


## 4.1. REKF Algorithm

The robust extended Kalman filter (REKF) is the combination ofH∞ filter and EKF [17]. It contains the following two steps:Time update:(38)x^k∣k-1=fx^k-1∣k-1,Pk∣k-1=Fk-1Pk∣k-1Fk-1T+Qk-1,where Fk-1 is Jacobian matrix of f(·) computed at x^k-1∣k-1.Measurement update:(39)x^k∣k=x^k∣k-1+Kzk-hx^k∣k-1,(40)K=Pk∣k-1HkTHkPk∣k-1HkT+Rk-1,(41)Pk∣k=Pk∣k-1-Pk∣k-1HkTLkTRe,k-1HkTLkTTPk∣k-1,(42)Re,k=R00-γ2I+HkTLkTTPk∣k-1HkTLkT,where Hk is Jacobian matrix of h(·) computed at x^k∣k-1. γ is the bound which can be chosen by designer.If, and only if, the condition is satisfied,(43)Pk∣k-γ-2LkTLk>00≤k≤N,the filter can be designed to achieve the desired accuracy by iterating γ to arrive at a suboptimal solution [23, 24].It can be easily shown that REKF reverts to EKF withγ→∞. Thus, γ can be thought as a tuning parameter to control the trade-off between performance of H∞ and minimum variance performance [17, 25]. Equation (43) will be satisfied easily with a large value of γ, but the large value of γ reduces the robustness of the filter. If we set γ to a small value, there is no guarantee that (43) will be satisfied during the process of filtering. So, the selection of the parameter γ is very important. One method to choose parameter γ will be given in the next part.


## 4.2. Robust Transformed Unscented Quadrature Kalman Filter

In this part, the RTUQKF algorithm will be derived based on REKF framework.Equations (38) in timing update of REKF can be replaced by the (23)~(25) in timing update of TUQKF and the Jacobian matrix Hk can be replaced by the method based on the following two approximations [26]:(44)Pzz,k∣k-1≈HkPk∣k-1HkT,Pxz,k∣k-1≈Pk∣k-1HkT.For SINS initial alignment, the signal which needs to be estimated is statexk, so Lk can be set to I. Putting Lk=I, (44) into (41)~(42), results in(45)Pk∣k=Pk∣k-1-Pk∣k-1HkTPk∣k-1Re,k-1HkPk∣k-1Pk∣k-1≈Pk∣k-1-Pxz,k∣k-1Pk∣k-1Re,k-1Pxz,k∣k-1TPk∣k-1Re,k=R00-γ2I+HkPk∣k-1HkTHkPk∣k-1Pk∣k-1HkTPk∣k-1≈R+Pzz,k∣k-1Pxz,k∣k-1TPxz,k∣k-1-γ2I+Pk∣k-1.P z z , k ∣ k - 1 and Pxz,k∣k-1 can be calculated by (29) or (35)~(36). The RTUQKF algorithm can be summarized as follows:(1)
Initialx^0 and P0 by (22).(2)
Time update: compute (23)~(25).(3)
Measurement update: compute (26)~(31) and (45).If measurement function described in (37) can be simplified as linear function (33), the measurement-update steps (26)~(29) can be simplified as (34)~(36).IfH∞ filter is combined with TUQKF-m, the proposed RTUQKF is named as m-order RTUQKF (RTUQKF-m).One way to choose parameterγ was proposed in [27].Applying the matrix inversion lemma for (41) and Pk∣k should maintain the positive definiteness during the process of filtering, so it can be easily shown that(46)Pk∣k-1=Pk∣k-1-1+HkTR-1Hk-γ2I>0.Thus,γ can be selected as(47)γ2=αmax⁡eigPk∣k-1-1+HkTR-1Hk-1,where max⁡eigA-1 denotes the maximum eigenvalue of the matrix A-1 and α is a scalar larger than one.Putting (44) into (47), γ in RTUQKF can be selected as(48)γ2=αmax⁡eigPk∣k-1-1+Pk∣k-1-1Pxz,k∣k-1R-1Pk∣k-1-1Pxz,k∣k-1T-1.


## 4.3. A Comparison of TUQKF with RTUQKF

Comparing RTUQKF and TUQKF, it can be found that their essential distinction is thatPk∣k is different. The parameter γ is added in the RTUQKF and K is adjusted by changing the value of Pk∣k. So, RTUQKF improves the robustness of system by the parameter γ. Similarly, the RTUQKF reverts to TUKF with γ→∞.TUQKF can also be used under the condition of uncertain noise, although its performance may not be better than RTUQKF. However, RTUQKF is more complex than TUQKF and its computational cost is also slightly higher than TUQKF. In particular, a false parameterγ would lead to filter divergence. So, in this paper, these two new methods were proposed and in practice, one of them can be selected according to demand and actual situation.


## 5. Nonlinear Model of SINS

The auxiliary external information such as velocity reference is necessary when using nonlinear filters to solve the problem of SINS initial alignment. The velocity reference can be given by GPS in most case. If the navigation system is underwater, the velocity reference can be given by DVL. In this paper, the nonlinear model of SINS is established based on [6].


### 5.1. Subsection Establishment of the State Equation

In the model of SINS, the navigation frame (n frame) is usually chosen as the geographic coordinate system (ENU) and the SINS calculation platform coordinate can be described as n′ frame. The body frame (b frame) is fixed at the center of body, the right direction is defined as x-axis, and the front and the upward directions are defined as y-axis and z-axis, respectively. Three Euler angles, yaw, pitch, and roll, are denoted as ψ, θ, and γ, respectively. Misalignment angles can be denoted by ϕu, ϕe, and ϕn. Cbn is the rotation transformation matrix from n frame to b frame. The true attitude angles can be denoted as φ=θγψT. The true velocity information can be denoted as vsn=venvnnvunT. The attitude angles calculated by SINS can be denoted as φ~=θ~γ~ψ~T. The velocity information calculated by SINS can be denoted as v~sn=v~env~nnv~unT. Denote the vector ϕ=φ-φ~=ϕuϕeϕnT. The velocity error can be denoted as δvn=vsn-v~sn=δvenδvnnδvunT. The nonlinear error equations can be described by(49)ϕ˙=Cω-1I-Cnn′ω~inn+Cnn′δωinb-Cbn′εb+wgb,δv˙n=I-Cn′nCbn′f~b-2δωien+δωenn×v~n-2ω~ien+ω~enn×δvn+Cn′nCbn′∇b+wab,where εb=εxbεybεzb and wgb are the gyroscope constant errors and gyroscope random errors in b frame. ∇b=∇xb∇yb∇zbT and wab are the accelerometer constant errors and random errors in b frame. f~b is the specific force measured by accelerometer. ω~enn is the angular velocity of the rotation of a navigation coordinates frame relative to the earth. ω~ien is the rotation projection of the earth. δωinn,δωien,andδωenn are the slow variations of ω~inn,ω~ien,andω~enn, respectively. Cω-1 is the inverse matrix of Cω. With the large misalignment, Cω and Cnn′ can be described as(50)Cnn′=cos⁡ϕncos⁡ϕu-sin⁡ϕnsin⁡ϕesin⁡ϕucos⁡ϕnsin⁡ϕu+sin⁡ϕnsin⁡ϕesin⁡ϕu-sin⁡ϕncos⁡ϕe-cos⁡ϕesin⁡ϕucos⁡ϕecos⁡ϕusin⁡ϕesin⁡ϕncos⁡ϕu+cos⁡ϕnsin⁡ϕesin⁡ϕusin⁡ϕnsin⁡ϕu-cos⁡ϕnsin⁡ϕesin⁡ϕucos⁡ϕncos⁡ϕe,Cω=cos⁡ϕn0-sin⁡ϕncos⁡ϕe01sin⁡ϕesin⁡ϕn0cos⁡ϕncos⁡ϕe.The state vector is selected as(51)x=δvenδvnnϕeϕnϕu∇xb∇ybεxbεybεzb.On a moving base, the nonlinear state equation of SINS can be obtained as(52)ϕ˙=Cω-1I-Cnn′ω~inn+δωinn-Cbn′εb+wgn,δv˙n=I-Cn′nfn′-2δωien+δωenn×v~n-2ω~ien+ω~enn×δv~n+Cn′nCbn′∇b+wan,ε˙b=0,∇˙b=0.


### 5.2. Establishment of the Measurement Equation

In this paper, we choose the east velocity and north velocity to establish the measurement equation. The velocity differential equation of SINS can be described as(53)z=v~sn-Cnn′v~b=δvn-I-Cnn′TCbn′vb+v,where v~sn and vb are velocity calculated by SINS and the true value, respectively (only contains the east and north velocity components). v~b includes the east and north external velocity information. v is the measurement noise.If the external velocity information inn frame v~n is known, the measurement equation can be simplified as(54)z=v~sn-v~n=Hx+v,where v~n is the external velocity vector. H is the measurement matrix and H=I2×202×8.


## 5.1. Subsection Establishment of the State Equation

In the model of SINS, the navigation frame (n frame) is usually chosen as the geographic coordinate system (ENU) and the SINS calculation platform coordinate can be described as n′ frame. The body frame (b frame) is fixed at the center of body, the right direction is defined as x-axis, and the front and the upward directions are defined as y-axis and z-axis, respectively. Three Euler angles, yaw, pitch, and roll, are denoted as ψ, θ, and γ, respectively. Misalignment angles can be denoted by ϕu, ϕe, and ϕn. Cbn is the rotation transformation matrix from n frame to b frame. The true attitude angles can be denoted as φ=θγψT. The true velocity information can be denoted as vsn=venvnnvunT. The attitude angles calculated by SINS can be denoted as φ~=θ~γ~ψ~T. The velocity information calculated by SINS can be denoted as v~sn=v~env~nnv~unT. Denote the vector ϕ=φ-φ~=ϕuϕeϕnT. The velocity error can be denoted as δvn=vsn-v~sn=δvenδvnnδvunT. The nonlinear error equations can be described by(49)ϕ˙=Cω-1I-Cnn′ω~inn+Cnn′δωinb-Cbn′εb+wgb,δv˙n=I-Cn′nCbn′f~b-2δωien+δωenn×v~n-2ω~ien+ω~enn×δvn+Cn′nCbn′∇b+wab,where εb=εxbεybεzb and wgb are the gyroscope constant errors and gyroscope random errors in b frame. ∇b=∇xb∇yb∇zbT and wab are the accelerometer constant errors and random errors in b frame. f~b is the specific force measured by accelerometer. ω~enn is the angular velocity of the rotation of a navigation coordinates frame relative to the earth. ω~ien is the rotation projection of the earth. δωinn,δωien,andδωenn are the slow variations of ω~inn,ω~ien,andω~enn, respectively. Cω-1 is the inverse matrix of Cω. With the large misalignment, Cω and Cnn′ can be described as(50)Cnn′=cos⁡ϕncos⁡ϕu-sin⁡ϕnsin⁡ϕesin⁡ϕucos⁡ϕnsin⁡ϕu+sin⁡ϕnsin⁡ϕesin⁡ϕu-sin⁡ϕncos⁡ϕe-cos⁡ϕesin⁡ϕucos⁡ϕecos⁡ϕusin⁡ϕesin⁡ϕncos⁡ϕu+cos⁡ϕnsin⁡ϕesin⁡ϕusin⁡ϕnsin⁡ϕu-cos⁡ϕnsin⁡ϕesin⁡ϕucos⁡ϕncos⁡ϕe,Cω=cos⁡ϕn0-sin⁡ϕncos⁡ϕe01sin⁡ϕesin⁡ϕn0cos⁡ϕncos⁡ϕe.The state vector is selected as(51)x=δvenδvnnϕeϕnϕu∇xb∇ybεxbεybεzb.On a moving base, the nonlinear state equation of SINS can be obtained as(52)ϕ˙=Cω-1I-Cnn′ω~inn+δωinn-Cbn′εb+wgn,δv˙n=I-Cn′nfn′-2δωien+δωenn×v~n-2ω~ien+ω~enn×δv~n+Cn′nCbn′∇b+wan,ε˙b=0,∇˙b=0.


## 5.2. Establishment of the Measurement Equation

In this paper, we choose the east velocity and north velocity to establish the measurement equation. The velocity differential equation of SINS can be described as(53)z=v~sn-Cnn′v~b=δvn-I-Cnn′TCbn′vb+v,where v~sn and vb are velocity calculated by SINS and the true value, respectively (only contains the east and north velocity components). v~b includes the east and north external velocity information. v is the measurement noise.If the external velocity information inn frame v~n is known, the measurement equation can be simplified as(54)z=v~sn-v~n=Hx+v,where v~n is the external velocity vector. H is the measurement matrix and H=I2×202×8.


## 6. Experiments and Analysis

### 6.1. Mathematical Simulation

In this part, the navigation system is placed on a simulated swing base and the initial misalignment angles are set to large values:Δθ=10°, Δγ=10°, and Δψ=15°,20°,35°,40°, respectively. The simulation conditions of swing base and sensors are in Tables 1 and 2. Set system noise as zero-mean Gaussian white noise. Also assume that velocity of navigation system is 5 m/s and the external information only includes the velocity in b frame. The SRCKF, TUKF, and TUQKF-2 are used to solve this initial alignment problem. Simulation time is 700 s. A comparison of these three methods is given in detail.Table 1
Simulation conditions of swing base.
Roll Pitch Yaw Swaying amplitude (°) 9 12 14 Swaying oscillation cycles (s) 8 10 6Table 2
Simulation conditions of sensor precision.
Gyroscope Accelerometer Constant drift 0.0 1 ° / h Constant bias 50 ug Random drift 0.00 1 ° / h Random deviation 50 ugThe simulation results are shown in Table3 and the curve graphs of alignment errors with Δθ=10°, Δγ=10°, and Δψ=40° are shown in Figure 1.Table 3
Statistical properties of alignment errors in mathematical simulation.
Algorithm Δ ψ (°) Pitch error (°) Roll error (°) Yaw error (°) Mean Standard deviation Mean Standard deviation Mean Standard deviation SRCKF 15 0.0023 0.00031 −0.0017 0.00042 0.0945 0.0202 TUKF 0.0078 0.00047 −0.0026 0.0014 0.0594 0.0068 TUQKF 0.0035 0.00045 −0.0025 0.00064 0.0130 0.0090  SRCKF 20 0.0021 0.00032 −0.0011 0.00039 0.1264 0.0235 TUKF 0.0080 0.00071 −0.0041 0.0015 0.0538 0.0064 TUQKF 0.0034 0.00058 −0.0033 0.00066 0.0188 0.0096  SRCKF 35 0.0033 0.00063 0.0016 0.0006 0.2019 0.0359 TUKF 0.0075 0.00068 −0.0040 0.0014 −0.0713 0.0062 TUQKF 0.0043 0.00064 −0.0037 0.00079 −0.0090 0.0081  SRCKF 40 0.0026 0.00094 0.0035 0.0005 0.3270 0.0503 TUKF 0.0073 0.00074 −0.0043 0.0013 −0.0726 0.0070 TUQKF 0.0044 0.00072 −0.0042 0.00082 −0.0138 0.0070Figure 1
Alignment errors of mathematical simulation.Analyze the accuracy of initial alignment first. In Figure1, when Δθ=10°, Δγ=10°, and Δψ=40°, after 600 s, the error curves of horizontal alignment of 3 methods are closed and errors are less than 0.01°. The azimuth alignment errors of TUQKF-2 and TUKF are less than 0.05° and 0.1°, respectively. Their precisions are much better than SRCKF of which azimuth alignment error is more than 0.2°. So, in this simulation, the precision of TUQKF-2 is better than TUKF and much better than SRCKF.In Table3, the same results can be seen. For different azimuth misalignment angles, the average errors of horizontal alignment of 3 methods are all less than 0.008° and the average error of azimuth alignment of TUQKF-2 is better than TUKF and SRCKF. So, the TUQKF-2 algorithm can maintain the high accuracy of horizontal alignment and remarkably improve the accuracy of azimuth alignment.Next, analyze the convergence speed. In Figure1, the convergence speed of TUQKF-2 and TUKF are almost the same. The horizontal alignment convergence time is less than 200 s and the azimuth alignment convergence time is less than 500 s. Compared to these, the convergence speed of SRCKF is very slow.Owing to the convergence speed of azimuth alignment which is faster than horizontal alignment, we calculated the standard deviation of azimuth estimation errors in different periods to quantitatively analyze the convergence speed of azimuth alignment. The concrete data are shown in Table4.Table 4
Standard deviation of azimuth estimation errors in mathematical simulation.
Algorithm Δ ψ (°) Standard deviation of azimuth estimation errors in different periods (°) 100~200 s 200~300 s 300~400 s 400~500 s 500~600 s SRCKF 15 1.4951 0.3025 0.0806 0.0473 0.0366 TUKF 0.4951 0.2337 0.1326 0.0325 0.0152 TUQKF 0.9351 0.1279 0.0737 0.0122 0.0175  SRCKF 20 2.0072 0.3769 0.0893 0.0580 0.0378 TUKF 0.7823 0.1854 0.1191 0.0268 0.0144 TUQKF 1.1878 0.1286 0.0712 0.0129 0.0183  SRCKF 35 3.6417 0.8143 0.2009 0.1136 0.0630 TUKF 1.6296 0.1776 0.1107 0.0224 0.0142 TUQKF 1.1948 0.1486 0.0858 0.0143 0.0164  SRCKF 40 4.8857 1.0047 0.2880 0.1618 0.0878 TUKF 1.0020 0.1902 0.1270 0.0286 0.0153 TUQKF 1.1915 0.1572 0.0978 0.0167 0.0149From Table4, it can be seen that, in most of the time, the standard deviation of azimuth estimation error of TUQKF-2 is a bit smaller than TUKF and much smaller than SRCKF. So, the convergence speed of the azimuth of TUQKF-2 is a bit faster than TUKF and much faster than SRCKF.In a word, according to the mathematical simulation, the accuracy of horizontal alignment of 3 methods is almost same, less than0.01°. The precision of azimuth alignment of TUQKF-2 is better than TUKF and SRCKF. The convergence speed, especially the convergence speed of azimuth alignment of TUQKF-2, is a little better than TUKF and much better than the SRCKF.


### 6.2. Turntable Experiment

To verify the effectiveness of the RTUQKF, turntable experiment is designed. In this turntable experiment, one SINS named FOSN is placed on a three-axis turntable. FOSN can provide gyroscope data and accelerometer data. The sensor accuracy of FOSN is given in Table5. The precision of turntable is ±0.0001°, so it can provide the true attitude angles as a reference. In the experiment, the FOSN is connected with the turntable; the turntable is controlled by the computer and makes a fixed swaying motion.Table 5
The sensor accuracy of FOSN.
Gyroscope Accelerometer Constant drift 0.00 6 ° / h Constant bias 50 ug Random drift 0.00 6 ° / h Random deviation 50 ugIn this experiment, the data are collected from the real world, so the statistical properties of noise are uncertain and the velocity of SINS is zero, so (54) can be used as measurement equation.Two tests with different initial attitudes, different swaying amplitudes, and different oscillation cycles are made. The SRCKF, TUQKF-2, and RTUQKF-2 are used to solve the above initial alignment problem, respectively. The tests conditions and results are as follows.


#### 6.2.1. The First Turntable Test

In this test, 3 different kinds of initial attitude angles are selected asθ~=15°, γ~=10°, and ψ~=35° and θ~=15°, and γ~=10°, ψ~=30°, and θ~=10°, γ~=5°, and ψ~=30°. True initial attitude angles are θ=6.9443°, γ=-0.5263°, and ψ=-5.7903°, so initial misalignment angles are large. The swaying amplitudes of inner frame (roll), intermediate frame (pitch), and outer frame (yaw) are 10°, 12°, and 6°, respectively. The corresponding oscillation frequencies are 0.2 Hz, 0.125 Hz, and 0.15 Hz, respectively. Simulation time is also 700 s. The experiment results are shown in Table 6 and the curve graphs of alignment errors with θ~=15°, γ~=10°, and ψ~=35°are shown in Figure 2.Table 6
Statistical properties of alignment errors in first turntable test.
Algorithm θ ~ ,   γ ~ ,   ψ ~ (°) Pitch error (°) Roll error (°) Yaw error (°) Mean Standard deviation Mean Standard deviation Mean Standard deviation SRCKF 10, 5, 30 −0.0100 0.0039 −0.0322 0.0033 0.2472 0.0234 TUQKF −0.0101 0.0040 −0.0353 0.0033 0.0427 0.0221 RTUQKF −0.0079 0.0039 0.0327 0.0032 0.0257 0.0156  SRCKF 15, 10, 30 −0.0090 0.0039 0.0335 0.0032 0.2603 0.0238 TUQKF −0.0101 0.0039 0.0335 0.0033 −0.0560 0.0201 RTUQKF −0.0101 0.0038 0.0318 0.0033 0.0211 0.0162  SRCKF 15, 10, 35 −0.0059 0.0038 0.0295 0.0032 0.3515 0.0150 TUQKF −0.0093 0.0038 0.0311 0.0032 −0.0741 0.0182 RTUQKF −0.0096 0.0039 0.0318 0.0033 0.0344 0.0165Figure 2
The alignment errors of first turntable test.In Figure2, when θ~=15°, γ~=10°, and ψ~=35°, after 600 s, the initial errors of pitch and roll angles of 3 methods are also closed and less than about 0.04° and 0.05°. It is obviously that azimuth alignment error of RTUQKF-2 is better than TUQKF-2 and much better than SRCKF.In Table6, for different initial attitude angles, mean errors of horizontal alignment of 3 methods are all very small and in the same level. The average error of azimuth alignment of RTUQKF-2 is around 0.03° and much less than the TUQKF-2 and SRCKF if which the mean of azimuth alignment error is around 0.05° and 0.5°, respectively. So, the RTUQKF-2 algorithm can maintain the high accuracy of horizontal alignment and remarkably improve the accuracy of azimuth alignment when the misalignment angles are large and the noise is uncertain. In addition, the precision of TUQKF declines when the actual noise is uncertain and the RTUQKF is more robust than the TUQKF.Next, analyze the convergence speed. In Figure2, convergence speed of 3 methods is almost the same; only the convergence speed of azimuth alignment of SRCKF is slightly slower than other two methods. We also calculated the standard deviation of azimuth estimation errors in different periods to quantitatively analyze the convergence rate of alignment. The concrete data are shown in Table 7.Table 7
Standard deviation of azimuth estimation errors in first turntable test.
Algorithm θ ~ ,   γ ~ ,   ψ ~ (°) Standard deviation of azimuth estimation errors in different periods (°) 100~200 s 200~300 s 300~400 s 400~500 s 500~600 s SRCKF 10, 5, 30 1.4738 0.1286 0.0428 0.0225 0.0251 TUQKF 2.7423 0.1286 0.0528 0.0332 0.0196 RTUQKF 1.3892 0.1481 0.0492 0.0190 0.0145  SRCKF 15, 10, 30 2.7479 0.1361 0.0339 0.0231 0.0299 TUQKF 2.3790 0.1729 0.0452 0.0283 0.0157 RTUQKF 1.5035 0.1410 0.0448 0.0186 0.0140  SRCKF 15, 10, 35 1.5865 0.1704 0.0520 0.0200 0.0320 TUQKF 2.3692 0.1637 0.0449 0.0214 0.0139 RTUQKF 2.3244 0.1371 0.0453 0.0187 0.0137From Table7, the standard deviation of azimuth estimation errors of 3 methods are almost the same. It means that convergence speeds of 3 methods are all fast. However, when initial misalignment angles are very large, the standard deviation of azimuth misalignment angles estimation errors of RTUQKF-2 is the smallest. So, the convergence speed of RTUQKF-2 is better than other two methods with large misalignment angles and uncertain noise.


#### 6.2.2. The Second Turntable Test

In this test, initial misalignment angles are set to large values:Δθ=5°, Δγ=5°, and Δψ=25° and Δθ=5°, Δγ=5°, and Δψ=40°, and Δθ=10°, Δγ=5°, and Δψ=40°. The swaying amplitudes of inner frame (roll), intermediate frame (pitch), and outer frame (yaw) are 12°, 8°, and 6°, respectively. The corresponding oscillation frequencies are 0.2 Hz, 0.125 Hz, and 0.15 Hz, respectively. The corresponding rocking center is 6°, 8°, and 75°, respectively. Simulation time is 800 s. The experiment results are shown in Table 8 and the curve graphs of the alignment errors with Δθ=10°, Δγ=5°, and Δψ=40° are shown in Figure 3. The standard deviations of azimuth estimation errors in different periods are shown in Table 9.Table 8
Statistical properties of the alignment errors in second turntable test.
Algorithm Δ θ ~ ,   Δ γ ~ ,   Δ ψ ~ (°) Pitch error (°) Roll error (°) Yaw error (°) Mean Standard deviation Mean Standard deviation Mean Standard deviation SRCKF 5, 5, 25 −0.0056 0.0045 −0.0216 0.0028 0.2789 0.0098 TUQKF −0.0074 0.0046 −0.0209 0.0027 0.0411 0.0051 RTUQKF −0.0069 0.0046 −0.0207 0.0027 0.0141 0.0050  SRCKF 5, 5, 40 −0.0042 0.0044 −0.0258 0.0028 0.4730 0.0310 TUQKF −0.0065 0.0045 −0.0235 0.0028 0.0859 0.0098 RTUQKF −0.0058 0.0045 −0.0218 0.0027 0.0361 0.0042  SRCKF 10, 10, 35 −0.0049 0.0045 −0.0236 0.0028 0.4346 0.0189 TUQKF −0.0061 0.0045 −0.0235 0.0027 0.0836 0.0117 RTUQKF −0.0068 0.0045 −0.0217 0.0027 0.0362 0.0042Table 9
Standard deviation of azimuth estimation errors in second turntable test.
Algorithm Δ θ ~ ,   Δ γ ~ ,   Δ ψ ~ (°) Standard deviation of azimuth estimation errors in different periods (°) 100~200 s 200~300 s 300~400 s 400~500 s 500~600 s 600~700 s SRCKF 5, 5, 25 3.6450 0.2849 0.1276 0.0532 0.0316 0.0272 TUQKF 2.1568 0.2037 0.1623 0.0564 0.0264 0.0208 RTUQKF 2.1003 0.1842 0.1400 0.0512 0.0231 0.0191  SRCKF 5, 5, 40 6.2382 0.5436 0.0952 0.0681 0.0480 0.0513 TUQKF 3.5482 0.1717 0.1686 0.0709 0.0333 0.0288 RTUQKF 2.2171 0.1970 0.1509 0.0600 0.0219 0.0180  SRCKF 10, 10, 35 4.5488 0.3277 0.1279 0.0666 0.0403 0.0384 TUQKF 4.3622 0.2068 0.1619 0.0748 0.0414 0.0323 RTUQKF 1.6323 0.2280 0.1684 0.0612 0.0241 0.0191Figure 3
The alignment errors of second turntable test.From Figure3 and Table 8, the precisions of horizontal alignment of 3 methods are high and in the same level. The precision of azimuth alignment of the SRCKF is the lowest of the three methods. From Table 9, the convergence speed of TUQKF-2 is slightly better than other two filters.It can be found that the results of two turntable tests are equally. In a word, according to the turntable experiment, when the noise is uncertain, precision of horizontal alignment of 3 methods is all high and in the same level. The RTUQKF can effectively increase the precision of azimuth alignment. The convergence speed of azimuth alignment of RTUQKF is better than the other algorithms when the misalignment angles are sufficiently large. It can be seen that the RTUQKF is more robust than the other two algorithms with the uncertain noise.


### 6.3. Vehicle Experiment

In the last, to further verify the effectiveness of RTUQKF, a vehicle experiment is conducted with the FOSN and PHINS. In this experiment, IMU data is given by FOSN. PHINS is a high precision SINS and it can be integrated with the FlexPark 6 from NovAtel as a SINS/GNSS integrated navigation system. The navigation parameters from PHINS are used as reference.In the vehicle experiment, from start time to about 30 seconds, the vehicle is at the starting state and its velocity is zero. From about 30 seconds to about 780 seconds, the vehicle is in the moving state. After about 780 seconds, the vehicle stops moving. Alignment time is 800 s. Owing to the external vehicle reference inn frame which can be given, (54) can be used as measurement equation. In this experiment, the data are also collected from the real world, so the statistical properties of noise are uncertain.Select two different kinds of misalignment angles for navigation system:Δθ=10°, Δγ=10°, and Δψ=40° and Δθ=5°, Δγ=5°, and Δψ=30°. The SRCKF, TUQKF-2, and RTUQKF-2 are used to solve the above initial alignment problem, respectively. The vehicle experiment results are shown in Table 10 and the curve graphs of alignment errors with Δθ=10°, Δγ=10°, and Δψ=40° are shown in Figure 4.Table 10
Statistical properties of the alignment errors in vehicle experiment.
Algorithm Δ θ ~ ,   Δ γ ~ ,   Δ ψ ~ (°) Pitch error (°) Roll error (°) Yaw error (°) Mean Standard deviation Mean Standard deviation Mean Standard deviation SRCKF 5, 5, 30 0.0102 0.0035 −0.0134 0.0061 −0.3330 0.0118 TUQKF 0.0072 0.0033 −0.0123 0.0051 −0.2595 0.0116 RTUQKF 0.0073 0.0031 −0.0120 0.0050 −0.2258 0.0118  SRCKF 10, 10, 40 0.0144 0.0042 −0.0149 0.0074 −0.4795 0.0192 TUQKF 0.0086 0.0039 −0.0145 0.0059 −0.3736 0.0148 RTUQKF 0.0089 0.0034 −0.0133 0.0057 −0.2585 0.0131Figure 4
The alignment errors of vehicle experiment.In Figure4, when Δθ=10°, Δγ=10°, and Δψ=40°, after 700 s, the error curves of horizontal alignment of 3 methods are closed. It can also be seen obviously that error of azimuth alignment of RTUQKF-2 is better than TUQKF-2 and much better than SRCKF. The data from Table 10 show that, for two different misalignment angles, average errors of horizontal alignment of 3 methods are very small and the average error of azimuth alignment of RTUQKF-2 is around 0.25° and much less than the TUQKF-2 and SRCKF. This means that, for SINS in-motion alignment, RTUQKF-2 can maintain the high horizontal alignment accuracy and remarkably improve the accuracy of azimuth alignment when the misalignment angles are large and noise is uncertain.The convergence speed is also quantitatively analyzed by the standard deviation of azimuth estimation errors in different periods in Table11. From Figure 4 and Table 11, it can be seen that the standard deviation of azimuth misalignment angles estimation errors of SRCKF is larger than the other two methods. In most of the time, the standard deviation of azimuth misalignment angles estimation errors of RTUQKF-2 slightly is smaller than TUQKF-2. It means that convergence speed of azimuth alignment of RTUQKF is slightly faster than TUQKF and faster than SRCKF.Table 11
Standard deviation of azimuth estimation errors in vehicle experiment.
Algorithm Δ θ ~ ,   Δ γ ~ ,   Δ ψ ~ (°) Standard deviation of azimuth estimation errors in different periods (°) 100~200 s 200~300 s 300~400 s 400~500 s 500~600 s 600~700 s SRCKF 5, 5, 30 0.6754 0.0514 0.0846 0.0273 0.0291 0.0230 TUQKF 0.1251 0.0399 0.0363 0.0203 0.0230 0.0118 RTUQKF 0.1176 0.0361 0.0360 0.0207 0.0232 0.0119  SRCKF 10, 10, 40 1.6544 0.0676 0.1785 0.0367 0.0533 0.0620 TUQKF 0.3962 0.0554 0.0350 0.0322 0.0323 0.0227 RTUQKF 0.3592 0.0594 0.0357 0.0153 0.0309 0.0154The results of vehicle experiment are similar to turntable experiment. In conclusion, when misalignment angles are large and the noise is uncertain, the precision of the horizontal alignment of 3 methods is high. The superiority of RTUQKF embodies several aspects that this method can effectively increase the precision and convergence speed of azimuth alignment and this method improves the robustness of the algorithm when the noise is uncertain.


## 6.1. Mathematical Simulation

In this part, the navigation system is placed on a simulated swing base and the initial misalignment angles are set to large values:Δθ=10°, Δγ=10°, and Δψ=15°,20°,35°,40°, respectively. The simulation conditions of swing base and sensors are in Tables 1 and 2. Set system noise as zero-mean Gaussian white noise. Also assume that velocity of navigation system is 5 m/s and the external information only includes the velocity in b frame. The SRCKF, TUKF, and TUQKF-2 are used to solve this initial alignment problem. Simulation time is 700 s. A comparison of these three methods is given in detail.Table 1
Simulation conditions of swing base.
Roll Pitch Yaw Swaying amplitude (°) 9 12 14 Swaying oscillation cycles (s) 8 10 6Table 2
Simulation conditions of sensor precision.
Gyroscope Accelerometer Constant drift 0.0 1 ° / h Constant bias 50 ug Random drift 0.00 1 ° / h Random deviation 50 ugThe simulation results are shown in Table3 and the curve graphs of alignment errors with Δθ=10°, Δγ=10°, and Δψ=40° are shown in Figure 1.Table 3
Statistical properties of alignment errors in mathematical simulation.
Algorithm Δ ψ (°) Pitch error (°) Roll error (°) Yaw error (°) Mean Standard deviation Mean Standard deviation Mean Standard deviation SRCKF 15 0.0023 0.00031 −0.0017 0.00042 0.0945 0.0202 TUKF 0.0078 0.00047 −0.0026 0.0014 0.0594 0.0068 TUQKF 0.0035 0.00045 −0.0025 0.00064 0.0130 0.0090  SRCKF 20 0.0021 0.00032 −0.0011 0.00039 0.1264 0.0235 TUKF 0.0080 0.00071 −0.0041 0.0015 0.0538 0.0064 TUQKF 0.0034 0.00058 −0.0033 0.00066 0.0188 0.0096  SRCKF 35 0.0033 0.00063 0.0016 0.0006 0.2019 0.0359 TUKF 0.0075 0.00068 −0.0040 0.0014 −0.0713 0.0062 TUQKF 0.0043 0.00064 −0.0037 0.00079 −0.0090 0.0081  SRCKF 40 0.0026 0.00094 0.0035 0.0005 0.3270 0.0503 TUKF 0.0073 0.00074 −0.0043 0.0013 −0.0726 0.0070 TUQKF 0.0044 0.00072 −0.0042 0.00082 −0.0138 0.0070Figure 1
Alignment errors of mathematical simulation.Analyze the accuracy of initial alignment first. In Figure1, when Δθ=10°, Δγ=10°, and Δψ=40°, after 600 s, the error curves of horizontal alignment of 3 methods are closed and errors are less than 0.01°. The azimuth alignment errors of TUQKF-2 and TUKF are less than 0.05° and 0.1°, respectively. Their precisions are much better than SRCKF of which azimuth alignment error is more than 0.2°. So, in this simulation, the precision of TUQKF-2 is better than TUKF and much better than SRCKF.In Table3, the same results can be seen. For different azimuth misalignment angles, the average errors of horizontal alignment of 3 methods are all less than 0.008° and the average error of azimuth alignment of TUQKF-2 is better than TUKF and SRCKF. So, the TUQKF-2 algorithm can maintain the high accuracy of horizontal alignment and remarkably improve the accuracy of azimuth alignment.Next, analyze the convergence speed. In Figure1, the convergence speed of TUQKF-2 and TUKF are almost the same. The horizontal alignment convergence time is less than 200 s and the azimuth alignment convergence time is less than 500 s. Compared to these, the convergence speed of SRCKF is very slow.Owing to the convergence speed of azimuth alignment which is faster than horizontal alignment, we calculated the standard deviation of azimuth estimation errors in different periods to quantitatively analyze the convergence speed of azimuth alignment. The concrete data are shown in Table4.Table 4
Standard deviation of azimuth estimation errors in mathematical simulation.
Algorithm Δ ψ (°) Standard deviation of azimuth estimation errors in different periods (°) 100~200 s 200~300 s 300~400 s 400~500 s 500~600 s SRCKF 15 1.4951 0.3025 0.0806 0.0473 0.0366 TUKF 0.4951 0.2337 0.1326 0.0325 0.0152 TUQKF 0.9351 0.1279 0.0737 0.0122 0.0175  SRCKF 20 2.0072 0.3769 0.0893 0.0580 0.0378 TUKF 0.7823 0.1854 0.1191 0.0268 0.0144 TUQKF 1.1878 0.1286 0.0712 0.0129 0.0183  SRCKF 35 3.6417 0.8143 0.2009 0.1136 0.0630 TUKF 1.6296 0.1776 0.1107 0.0224 0.0142 TUQKF 1.1948 0.1486 0.0858 0.0143 0.0164  SRCKF 40 4.8857 1.0047 0.2880 0.1618 0.0878 TUKF 1.0020 0.1902 0.1270 0.0286 0.0153 TUQKF 1.1915 0.1572 0.0978 0.0167 0.0149From Table4, it can be seen that, in most of the time, the standard deviation of azimuth estimation error of TUQKF-2 is a bit smaller than TUKF and much smaller than SRCKF. So, the convergence speed of the azimuth of TUQKF-2 is a bit faster than TUKF and much faster than SRCKF.In a word, according to the mathematical simulation, the accuracy of horizontal alignment of 3 methods is almost same, less than0.01°. The precision of azimuth alignment of TUQKF-2 is better than TUKF and SRCKF. The convergence speed, especially the convergence speed of azimuth alignment of TUQKF-2, is a little better than TUKF and much better than the SRCKF.


## 6.2. Turntable Experiment

To verify the effectiveness of the RTUQKF, turntable experiment is designed. In this turntable experiment, one SINS named FOSN is placed on a three-axis turntable. FOSN can provide gyroscope data and accelerometer data. The sensor accuracy of FOSN is given in Table5. The precision of turntable is ±0.0001°, so it can provide the true attitude angles as a reference. In the experiment, the FOSN is connected with the turntable; the turntable is controlled by the computer and makes a fixed swaying motion.Table 5
The sensor accuracy of FOSN.
Gyroscope Accelerometer Constant drift 0.00 6 ° / h Constant bias 50 ug Random drift 0.00 6 ° / h Random deviation 50 ugIn this experiment, the data are collected from the real world, so the statistical properties of noise are uncertain and the velocity of SINS is zero, so (54) can be used as measurement equation.Two tests with different initial attitudes, different swaying amplitudes, and different oscillation cycles are made. The SRCKF, TUQKF-2, and RTUQKF-2 are used to solve the above initial alignment problem, respectively. The tests conditions and results are as follows.


### 6.2.1. The First Turntable Test

In this test, 3 different kinds of initial attitude angles are selected asθ~=15°, γ~=10°, and ψ~=35° and θ~=15°, and γ~=10°, ψ~=30°, and θ~=10°, γ~=5°, and ψ~=30°. True initial attitude angles are θ=6.9443°, γ=-0.5263°, and ψ=-5.7903°, so initial misalignment angles are large. The swaying amplitudes of inner frame (roll), intermediate frame (pitch), and outer frame (yaw) are 10°, 12°, and 6°, respectively. The corresponding oscillation frequencies are 0.2 Hz, 0.125 Hz, and 0.15 Hz, respectively. Simulation time is also 700 s. The experiment results are shown in Table 6 and the curve graphs of alignment errors with θ~=15°, γ~=10°, and ψ~=35°are shown in Figure 2.Table 6
Statistical properties of alignment errors in first turntable test.
Algorithm θ ~ ,   γ ~ ,   ψ ~ (°) Pitch error (°) Roll error (°) Yaw error (°) Mean Standard deviation Mean Standard deviation Mean Standard deviation SRCKF 10, 5, 30 −0.0100 0.0039 −0.0322 0.0033 0.2472 0.0234 TUQKF −0.0101 0.0040 −0.0353 0.0033 0.0427 0.0221 RTUQKF −0.0079 0.0039 0.0327 0.0032 0.0257 0.0156  SRCKF 15, 10, 30 −0.0090 0.0039 0.0335 0.0032 0.2603 0.0238 TUQKF −0.0101 0.0039 0.0335 0.0033 −0.0560 0.0201 RTUQKF −0.0101 0.0038 0.0318 0.0033 0.0211 0.0162  SRCKF 15, 10, 35 −0.0059 0.0038 0.0295 0.0032 0.3515 0.0150 TUQKF −0.0093 0.0038 0.0311 0.0032 −0.0741 0.0182 RTUQKF −0.0096 0.0039 0.0318 0.0033 0.0344 0.0165Figure 2
The alignment errors of first turntable test.In Figure2, when θ~=15°, γ~=10°, and ψ~=35°, after 600 s, the initial errors of pitch and roll angles of 3 methods are also closed and less than about 0.04° and 0.05°. It is obviously that azimuth alignment error of RTUQKF-2 is better than TUQKF-2 and much better than SRCKF.In Table6, for different initial attitude angles, mean errors of horizontal alignment of 3 methods are all very small and in the same level. The average error of azimuth alignment of RTUQKF-2 is around 0.03° and much less than the TUQKF-2 and SRCKF if which the mean of azimuth alignment error is around 0.05° and 0.5°, respectively. So, the RTUQKF-2 algorithm can maintain the high accuracy of horizontal alignment and remarkably improve the accuracy of azimuth alignment when the misalignment angles are large and the noise is uncertain. In addition, the precision of TUQKF declines when the actual noise is uncertain and the RTUQKF is more robust than the TUQKF.Next, analyze the convergence speed. In Figure2, convergence speed of 3 methods is almost the same; only the convergence speed of azimuth alignment of SRCKF is slightly slower than other two methods. We also calculated the standard deviation of azimuth estimation errors in different periods to quantitatively analyze the convergence rate of alignment. The concrete data are shown in Table 7.Table 7
Standard deviation of azimuth estimation errors in first turntable test.
Algorithm θ ~ ,   γ ~ ,   ψ ~ (°) Standard deviation of azimuth estimation errors in different periods (°) 100~200 s 200~300 s 300~400 s 400~500 s 500~600 s SRCKF 10, 5, 30 1.4738 0.1286 0.0428 0.0225 0.0251 TUQKF 2.7423 0.1286 0.0528 0.0332 0.0196 RTUQKF 1.3892 0.1481 0.0492 0.0190 0.0145  SRCKF 15, 10, 30 2.7479 0.1361 0.0339 0.0231 0.0299 TUQKF 2.3790 0.1729 0.0452 0.0283 0.0157 RTUQKF 1.5035 0.1410 0.0448 0.0186 0.0140  SRCKF 15, 10, 35 1.5865 0.1704 0.0520 0.0200 0.0320 TUQKF 2.3692 0.1637 0.0449 0.0214 0.0139 RTUQKF 2.3244 0.1371 0.0453 0.0187 0.0137From Table7, the standard deviation of azimuth estimation errors of 3 methods are almost the same. It means that convergence speeds of 3 methods are all fast. However, when initial misalignment angles are very large, the standard deviation of azimuth misalignment angles estimation errors of RTUQKF-2 is the smallest. So, the convergence speed of RTUQKF-2 is better than other two methods with large misalignment angles and uncertain noise.


### 6.2.2. The Second Turntable Test

In this test, initial misalignment angles are set to large values:Δθ=5°, Δγ=5°, and Δψ=25° and Δθ=5°, Δγ=5°, and Δψ=40°, and Δθ=10°, Δγ=5°, and Δψ=40°. The swaying amplitudes of inner frame (roll), intermediate frame (pitch), and outer frame (yaw) are 12°, 8°, and 6°, respectively. The corresponding oscillation frequencies are 0.2 Hz, 0.125 Hz, and 0.15 Hz, respectively. The corresponding rocking center is 6°, 8°, and 75°, respectively. Simulation time is 800 s. The experiment results are shown in Table 8 and the curve graphs of the alignment errors with Δθ=10°, Δγ=5°, and Δψ=40° are shown in Figure 3. The standard deviations of azimuth estimation errors in different periods are shown in Table 9.Table 8
Statistical properties of the alignment errors in second turntable test.
Algorithm Δ θ ~ ,   Δ γ ~ ,   Δ ψ ~ (°) Pitch error (°) Roll error (°) Yaw error (°) Mean Standard deviation Mean Standard deviation Mean Standard deviation SRCKF 5, 5, 25 −0.0056 0.0045 −0.0216 0.0028 0.2789 0.0098 TUQKF −0.0074 0.0046 −0.0209 0.0027 0.0411 0.0051 RTUQKF −0.0069 0.0046 −0.0207 0.0027 0.0141 0.0050  SRCKF 5, 5, 40 −0.0042 0.0044 −0.0258 0.0028 0.4730 0.0310 TUQKF −0.0065 0.0045 −0.0235 0.0028 0.0859 0.0098 RTUQKF −0.0058 0.0045 −0.0218 0.0027 0.0361 0.0042  SRCKF 10, 10, 35 −0.0049 0.0045 −0.0236 0.0028 0.4346 0.0189 TUQKF −0.0061 0.0045 −0.0235 0.0027 0.0836 0.0117 RTUQKF −0.0068 0.0045 −0.0217 0.0027 0.0362 0.0042Table 9
Standard deviation of azimuth estimation errors in second turntable test.
Algorithm Δ θ ~ ,   Δ γ ~ ,   Δ ψ ~ (°) Standard deviation of azimuth estimation errors in different periods (°) 100~200 s 200~300 s 300~400 s 400~500 s 500~600 s 600~700 s SRCKF 5, 5, 25 3.6450 0.2849 0.1276 0.0532 0.0316 0.0272 TUQKF 2.1568 0.2037 0.1623 0.0564 0.0264 0.0208 RTUQKF 2.1003 0.1842 0.1400 0.0512 0.0231 0.0191  SRCKF 5, 5, 40 6.2382 0.5436 0.0952 0.0681 0.0480 0.0513 TUQKF 3.5482 0.1717 0.1686 0.0709 0.0333 0.0288 RTUQKF 2.2171 0.1970 0.1509 0.0600 0.0219 0.0180  SRCKF 10, 10, 35 4.5488 0.3277 0.1279 0.0666 0.0403 0.0384 TUQKF 4.3622 0.2068 0.1619 0.0748 0.0414 0.0323 RTUQKF 1.6323 0.2280 0.1684 0.0612 0.0241 0.0191Figure 3
The alignment errors of second turntable test.From Figure3 and Table 8, the precisions of horizontal alignment of 3 methods are high and in the same level. The precision of azimuth alignment of the SRCKF is the lowest of the three methods. From Table 9, the convergence speed of TUQKF-2 is slightly better than other two filters.It can be found that the results of two turntable tests are equally. In a word, according to the turntable experiment, when the noise is uncertain, precision of horizontal alignment of 3 methods is all high and in the same level. The RTUQKF can effectively increase the precision of azimuth alignment. The convergence speed of azimuth alignment of RTUQKF is better than the other algorithms when the misalignment angles are sufficiently large. It can be seen that the RTUQKF is more robust than the other two algorithms with the uncertain noise.


## 6.2.1. The First Turntable Test

In this test, 3 different kinds of initial attitude angles are selected asθ~=15°, γ~=10°, and ψ~=35° and θ~=15°, and γ~=10°, ψ~=30°, and θ~=10°, γ~=5°, and ψ~=30°. True initial attitude angles are θ=6.9443°, γ=-0.5263°, and ψ=-5.7903°, so initial misalignment angles are large. The swaying amplitudes of inner frame (roll), intermediate frame (pitch), and outer frame (yaw) are 10°, 12°, and 6°, respectively. The corresponding oscillation frequencies are 0.2 Hz, 0.125 Hz, and 0.15 Hz, respectively. Simulation time is also 700 s. The experiment results are shown in Table 6 and the curve graphs of alignment errors with θ~=15°, γ~=10°, and ψ~=35°are shown in Figure 2.Table 6
Statistical properties of alignment errors in first turntable test.
Algorithm θ ~ ,   γ ~ ,   ψ ~ (°) Pitch error (°) Roll error (°) Yaw error (°) Mean Standard deviation Mean Standard deviation Mean Standard deviation SRCKF 10, 5, 30 −0.0100 0.0039 −0.0322 0.0033 0.2472 0.0234 TUQKF −0.0101 0.0040 −0.0353 0.0033 0.0427 0.0221 RTUQKF −0.0079 0.0039 0.0327 0.0032 0.0257 0.0156  SRCKF 15, 10, 30 −0.0090 0.0039 0.0335 0.0032 0.2603 0.0238 TUQKF −0.0101 0.0039 0.0335 0.0033 −0.0560 0.0201 RTUQKF −0.0101 0.0038 0.0318 0.0033 0.0211 0.0162  SRCKF 15, 10, 35 −0.0059 0.0038 0.0295 0.0032 0.3515 0.0150 TUQKF −0.0093 0.0038 0.0311 0.0032 −0.0741 0.0182 RTUQKF −0.0096 0.0039 0.0318 0.0033 0.0344 0.0165Figure 2
The alignment errors of first turntable test.In Figure2, when θ~=15°, γ~=10°, and ψ~=35°, after 600 s, the initial errors of pitch and roll angles of 3 methods are also closed and less than about 0.04° and 0.05°. It is obviously that azimuth alignment error of RTUQKF-2 is better than TUQKF-2 and much better than SRCKF.In Table6, for different initial attitude angles, mean errors of horizontal alignment of 3 methods are all very small and in the same level. The average error of azimuth alignment of RTUQKF-2 is around 0.03° and much less than the TUQKF-2 and SRCKF if which the mean of azimuth alignment error is around 0.05° and 0.5°, respectively. So, the RTUQKF-2 algorithm can maintain the high accuracy of horizontal alignment and remarkably improve the accuracy of azimuth alignment when the misalignment angles are large and the noise is uncertain. In addition, the precision of TUQKF declines when the actual noise is uncertain and the RTUQKF is more robust than the TUQKF.Next, analyze the convergence speed. In Figure2, convergence speed of 3 methods is almost the same; only the convergence speed of azimuth alignment of SRCKF is slightly slower than other two methods. We also calculated the standard deviation of azimuth estimation errors in different periods to quantitatively analyze the convergence rate of alignment. The concrete data are shown in Table 7.Table 7
Standard deviation of azimuth estimation errors in first turntable test.
Algorithm θ ~ ,   γ ~ ,   ψ ~ (°) Standard deviation of azimuth estimation errors in different periods (°) 100~200 s 200~300 s 300~400 s 400~500 s 500~600 s SRCKF 10, 5, 30 1.4738 0.1286 0.0428 0.0225 0.0251 TUQKF 2.7423 0.1286 0.0528 0.0332 0.0196 RTUQKF 1.3892 0.1481 0.0492 0.0190 0.0145  SRCKF 15, 10, 30 2.7479 0.1361 0.0339 0.0231 0.0299 TUQKF 2.3790 0.1729 0.0452 0.0283 0.0157 RTUQKF 1.5035 0.1410 0.0448 0.0186 0.0140  SRCKF 15, 10, 35 1.5865 0.1704 0.0520 0.0200 0.0320 TUQKF 2.3692 0.1637 0.0449 0.0214 0.0139 RTUQKF 2.3244 0.1371 0.0453 0.0187 0.0137From Table7, the standard deviation of azimuth estimation errors of 3 methods are almost the same. It means that convergence speeds of 3 methods are all fast. However, when initial misalignment angles are very large, the standard deviation of azimuth misalignment angles estimation errors of RTUQKF-2 is the smallest. So, the convergence speed of RTUQKF-2 is better than other two methods with large misalignment angles and uncertain noise.


## 6.2.2. The Second Turntable Test

In this test, initial misalignment angles are set to large values:Δθ=5°, Δγ=5°, and Δψ=25° and Δθ=5°, Δγ=5°, and Δψ=40°, and Δθ=10°, Δγ=5°, and Δψ=40°. The swaying amplitudes of inner frame (roll), intermediate frame (pitch), and outer frame (yaw) are 12°, 8°, and 6°, respectively. The corresponding oscillation frequencies are 0.2 Hz, 0.125 Hz, and 0.15 Hz, respectively. The corresponding rocking center is 6°, 8°, and 75°, respectively. Simulation time is 800 s. The experiment results are shown in Table 8 and the curve graphs of the alignment errors with Δθ=10°, Δγ=5°, and Δψ=40° are shown in Figure 3. The standard deviations of azimuth estimation errors in different periods are shown in Table 9.Table 8
Statistical properties of the alignment errors in second turntable test.
Algorithm Δ θ ~ ,   Δ γ ~ ,   Δ ψ ~ (°) Pitch error (°) Roll error (°) Yaw error (°) Mean Standard deviation Mean Standard deviation Mean Standard deviation SRCKF 5, 5, 25 −0.0056 0.0045 −0.0216 0.0028 0.2789 0.0098 TUQKF −0.0074 0.0046 −0.0209 0.0027 0.0411 0.0051 RTUQKF −0.0069 0.0046 −0.0207 0.0027 0.0141 0.0050  SRCKF 5, 5, 40 −0.0042 0.0044 −0.0258 0.0028 0.4730 0.0310 TUQKF −0.0065 0.0045 −0.0235 0.0028 0.0859 0.0098 RTUQKF −0.0058 0.0045 −0.0218 0.0027 0.0361 0.0042  SRCKF 10, 10, 35 −0.0049 0.0045 −0.0236 0.0028 0.4346 0.0189 TUQKF −0.0061 0.0045 −0.0235 0.0027 0.0836 0.0117 RTUQKF −0.0068 0.0045 −0.0217 0.0027 0.0362 0.0042Table 9
Standard deviation of azimuth estimation errors in second turntable test.
Algorithm Δ θ ~ ,   Δ γ ~ ,   Δ ψ ~ (°) Standard deviation of azimuth estimation errors in different periods (°) 100~200 s 200~300 s 300~400 s 400~500 s 500~600 s 600~700 s SRCKF 5, 5, 25 3.6450 0.2849 0.1276 0.0532 0.0316 0.0272 TUQKF 2.1568 0.2037 0.1623 0.0564 0.0264 0.0208 RTUQKF 2.1003 0.1842 0.1400 0.0512 0.0231 0.0191  SRCKF 5, 5, 40 6.2382 0.5436 0.0952 0.0681 0.0480 0.0513 TUQKF 3.5482 0.1717 0.1686 0.0709 0.0333 0.0288 RTUQKF 2.2171 0.1970 0.1509 0.0600 0.0219 0.0180  SRCKF 10, 10, 35 4.5488 0.3277 0.1279 0.0666 0.0403 0.0384 TUQKF 4.3622 0.2068 0.1619 0.0748 0.0414 0.0323 RTUQKF 1.6323 0.2280 0.1684 0.0612 0.0241 0.0191Figure 3
The alignment errors of second turntable test.From Figure3 and Table 8, the precisions of horizontal alignment of 3 methods are high and in the same level. The precision of azimuth alignment of the SRCKF is the lowest of the three methods. From Table 9, the convergence speed of TUQKF-2 is slightly better than other two filters.It can be found that the results of two turntable tests are equally. In a word, according to the turntable experiment, when the noise is uncertain, precision of horizontal alignment of 3 methods is all high and in the same level. The RTUQKF can effectively increase the precision of azimuth alignment. The convergence speed of azimuth alignment of RTUQKF is better than the other algorithms when the misalignment angles are sufficiently large. It can be seen that the RTUQKF is more robust than the other two algorithms with the uncertain noise.


## 6.3. Vehicle Experiment

In the last, to further verify the effectiveness of RTUQKF, a vehicle experiment is conducted with the FOSN and PHINS. In this experiment, IMU data is given by FOSN. PHINS is a high precision SINS and it can be integrated with the FlexPark 6 from NovAtel as a SINS/GNSS integrated navigation system. The navigation parameters from PHINS are used as reference.In the vehicle experiment, from start time to about 30 seconds, the vehicle is at the starting state and its velocity is zero. From about 30 seconds to about 780 seconds, the vehicle is in the moving state. After about 780 seconds, the vehicle stops moving. Alignment time is 800 s. Owing to the external vehicle reference inn frame which can be given, (54) can be used as measurement equation. In this experiment, the data are also collected from the real world, so the statistical properties of noise are uncertain.Select two different kinds of misalignment angles for navigation system:Δθ=10°, Δγ=10°, and Δψ=40° and Δθ=5°, Δγ=5°, and Δψ=30°. The SRCKF, TUQKF-2, and RTUQKF-2 are used to solve the above initial alignment problem, respectively. The vehicle experiment results are shown in Table 10 and the curve graphs of alignment errors with Δθ=10°, Δγ=10°, and Δψ=40° are shown in Figure 4.Table 10
Statistical properties of the alignment errors in vehicle experiment.
Algorithm Δ θ ~ ,   Δ γ ~ ,   Δ ψ ~ (°) Pitch error (°) Roll error (°) Yaw error (°) Mean Standard deviation Mean Standard deviation Mean Standard deviation SRCKF 5, 5, 30 0.0102 0.0035 −0.0134 0.0061 −0.3330 0.0118 TUQKF 0.0072 0.0033 −0.0123 0.0051 −0.2595 0.0116 RTUQKF 0.0073 0.0031 −0.0120 0.0050 −0.2258 0.0118  SRCKF 10, 10, 40 0.0144 0.0042 −0.0149 0.0074 −0.4795 0.0192 TUQKF 0.0086 0.0039 −0.0145 0.0059 −0.3736 0.0148 RTUQKF 0.0089 0.0034 −0.0133 0.0057 −0.2585 0.0131Figure 4
The alignment errors of vehicle experiment.In Figure4, when Δθ=10°, Δγ=10°, and Δψ=40°, after 700 s, the error curves of horizontal alignment of 3 methods are closed. It can also be seen obviously that error of azimuth alignment of RTUQKF-2 is better than TUQKF-2 and much better than SRCKF. The data from Table 10 show that, for two different misalignment angles, average errors of horizontal alignment of 3 methods are very small and the average error of azimuth alignment of RTUQKF-2 is around 0.25° and much less than the TUQKF-2 and SRCKF. This means that, for SINS in-motion alignment, RTUQKF-2 can maintain the high horizontal alignment accuracy and remarkably improve the accuracy of azimuth alignment when the misalignment angles are large and noise is uncertain.The convergence speed is also quantitatively analyzed by the standard deviation of azimuth estimation errors in different periods in Table11. From Figure 4 and Table 11, it can be seen that the standard deviation of azimuth misalignment angles estimation errors of SRCKF is larger than the other two methods. In most of the time, the standard deviation of azimuth misalignment angles estimation errors of RTUQKF-2 slightly is smaller than TUQKF-2. It means that convergence speed of azimuth alignment of RTUQKF is slightly faster than TUQKF and faster than SRCKF.Table 11
Standard deviation of azimuth estimation errors in vehicle experiment.
Algorithm Δ θ ~ ,   Δ γ ~ ,   Δ ψ ~ (°) Standard deviation of azimuth estimation errors in different periods (°) 100~200 s 200~300 s 300~400 s 400~500 s 500~600 s 600~700 s SRCKF 5, 5, 30 0.6754 0.0514 0.0846 0.0273 0.0291 0.0230 TUQKF 0.1251 0.0399 0.0363 0.0203 0.0230 0.0118 RTUQKF 0.1176 0.0361 0.0360 0.0207 0.0232 0.0119  SRCKF 10, 10, 40 1.6544 0.0676 0.1785 0.0367 0.0533 0.0620 TUQKF 0.3962 0.0554 0.0350 0.0322 0.0323 0.0227 RTUQKF 0.3592 0.0594 0.0357 0.0153 0.0309 0.0154The results of vehicle experiment are similar to turntable experiment. In conclusion, when misalignment angles are large and the noise is uncertain, the precision of the horizontal alignment of 3 methods is high. The superiority of RTUQKF embodies several aspects that this method can effectively increase the precision and convergence speed of azimuth alignment and this method improves the robustness of the algorithm when the noise is uncertain.


## 7. Conclusions

The SINS initial alignment problem is discussed in this paper. To solve the initial alignment problem under large misalignment angles, TUQKF algorithm is proposed. The TUQKF can maintain the positive definiteness of the weight and the covariance matrix in the filtering process and solve the nonlocal sampling problem for high-dimensional problem coupled with strong nonlinear model. In this paper, to solve the problem of uncertain noise in initial alignment, the RTUQKF algorithm is proposed. The RTUQKF is the combination ofH∞ filtering and TUQKF. This method maintains the high accuracy of filtering and further improves the robustness of filtering. The performance of TUQKF is compared with other filters in mathematical simulation. The results indicate that, compared to traditional filters such as SRCKF and TUKF, TUQKF can increase precision and convergence speed of initial alignment. By means of the turntable experiment and vehicle experiment, the performance of RTUQKF is analyzed and compared with other filters. The results show that when the noise is uncertain, RTUQKF can further increase the precision and convergence speed of azimuth alignment and it can also effectively improve the robustness of filter. Although the computational cost of TUQKF and RTUQKF is slightly higher than SRCKF and TUKF, their sigma points have a linear relationship to state dimension and the order of quadrature rule. These two algorithms will not surfer from the curse of dimensionality problem.



---

*Source: 5917917-2017-02-19.xml*