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

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.TheRTUQKF is the combination ofH∞ technique andTUQKF. It improves the accuracy and robustness of state estimation. Simulation results indicate that TUQKF performs better than traditional filters whenmisalignment 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.


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 2 Mathematical Problems in Engineering 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 the  ∞ technique to improve the robustness of filter.The  ∞ filter is more robust and has better precision than standard KF under uncertain noise [15,16].However, standard  ∞ filter can only fulfill the linear filtering problems.In the recent two decades,  ∞ filter was extended to nonlinear problems by combing with nonlinear filters.In [17], the  ∞ filter was combined with EKF and the new algorithm known as robust extended KF (REKF) was proposed.In this framework, the  ∞ filter was combined with UKF and CKF.These two new algorithms are known as robust unscented KF (REKF) and cubature  ∞ filter (C ∞ 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,  ∞ 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.Section 2 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.

Conventional Nonlinear Filters
The general discrete nonlinear model is given by where x  ∈   and z  ∈   are state and measurement, (⋅) is nonlinear state function, and ℎ(⋅) is nonlinear measurement function. −1 and k  are uncorrelated zero-mean white Gaussian noise with variances Q  and R  , respectively.
The state vector x  can be estimated by general Gaussian approximation filter.The heart of the Gaussian approximation filter is to evaluate the integral as follows [19]: where (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: where   is quadrature points,   is corresponding weights, and S is square root of P; namely, P = SS  .
Therefore, the main purpose of evaluating integral ( 2) is to obtain quadrature points   and weights   .
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.

Transformed Unscented
Quadrature Kalman Filter where  and P are the mean and variance of state vector x, respectively.
First, compute integral Integral ( 8) can be approximated by [10] ∫ where   are space points and ‖  ‖ = 1,  denotes number of space points, and  denotes corresponding weights.
It can be seen that the computational precision of integral (7) depends on   and .Performance of filters is also influenced by   and .
The  can be calculated by [10] 3.2.Gauss-Laguerre Quadrature Rule.Bringing (9) and ( 12) into (6) and letting  =  2 /2 result in Define () as Computing integral ( 6) is transformed into computing integral (13).First, calculate the integral Gauss-Laguerre quadrature rule can be used to approximate integral (15) [10,22]: where   is quadrature point and   is corresponding weight.  can be calculated from The weights can be determined as 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 (x), so quadrature points and weights always exist and can be computed and stored offline.
Bringing approximate results of integral (15) into (13) yields where where   is sigma points and   is corresponding weight of TUQKF.
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- is better than the TUKF when  > 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- requires  = 2nm sigma points, so it is computationally more efficient than the Gauss-Hermite filter which requires   sigma points.However, the computational cost of TUQKF- is slightly higher than CKF and TUKF when  > 1.
When using the TUQKF-, the balance between the accuracy and the computational cost should be considered.If the dimension of state vector is high,  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  would cause the problems such as "over fitting."Considering the above problems,  was set to 2 in this paper.

Time Update
(i) Factorize (ii) Evaluate and update transformed unscented quadrature points (iii) Estimate predicted state and error covariance

Measurement Update
(i) Factorize (ii) Evaluate and update transformed unscented quadrature points (iii) Estimate predicted measurement (iv) Estimate innovation covariance and cross-covariance matrix (v) Estimate the gain (vi) Estimate updated state and corresponding error covariance Measurement function described in (1) can be simplified as the following linear function: where H  is the measurement matrix.Measurement-update steps ( 26)∼(29) can be simplified as

Robust Transformed Unscented Quadrature Kalman Filter
The discrete nonlinear model can be expressed as where x  ∈   and z  ∈   are the state and measurement.(⋅) is nonlinear state function, and ℎ(⋅) is nonlinear measurement function. −1 and k  are noise with uncertain statistical properties.y  ∈   is the signal to be estimated and L  is known.Nonlinear filters discussed above are derived under the condition of zero-mean Gaussian white noise.However, statistical properties of  −1 and k  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  ∞ technique can improve the performance of filters under the different noise with different statistical properties.

REKF Algorithm.
The robust extended Kalman filter (REKF) is the combination of  ∞ filter and EKF [17].It contains the following two steps: Time update: x|−1 =  (x −1|−1 ) , where F −1 is Jacobian matrix of (⋅) computed at x−1|−1 .Measurement update: where H  is Jacobian matrix of ℎ(⋅) computed at x|−1 . is the bound which can be chosen by designer.If, and only if, the condition is satisfied, 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  ∞ 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.

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 H  can be replaced by the method based on the following two approximations [26]: For SINS initial alignment, the signal which needs to be estimated is state x  , so L  can be set to I. Putting L  = I, (44) into (41)∼(42), results in P ,|−1 and P ,|−1 can be calculated by (29) or (35)∼ (36).The RTUQKF algorithm can be summarized as follows: (1) Initial x0 and P 0 by (22).
One way to choose parameter  was proposed in [27].
Applying the matrix inversion lemma for (41) and P | should maintain the positive definiteness during the process of filtering, so it can be easily shown that Thus,  can be selected as where max {eig() −1 } denotes the maximum eigenvalue of the matrix  −1 and  is a scalar larger than one.Putting (44) into (47),  in RTUQKF can be selected as (48)

A Comparison of TUQKF with RTUQKF.
Comparing RTUQKF and TUQKF, it can be found that their essential distinction is that P | is different.The parameter  is added in the RTUQKF and K is adjusted by changing the value of P | .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.

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].

Subsection Establishment of the State Equation.
In the model of SINS, the navigation frame ( frame) is usually chosen as the geographic coordinate system (ENU) and the SINS calculation platform coordinate can be described as   frame.The body frame ( frame) is fixed at the center of body, the right direction is defined as -axis, and the front and the upward directions are defined as -axis and axis, respectively.Three Euler angles, yaw, pitch, and roll, are denoted as , , and , respectively.Misalignment angles can be denoted by   ,   , and   .C   is the rotation transformation matrix from  frame to  frame.The true attitude angles can be denoted as  = [  ] .The true velocity information can be denoted as  .The nonlinear error equations can be described by where , The state vector is selected as On a moving base, the nonlinear state equation of SINS can be obtained as ε  = 0, ∇ = 0. (52)

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 where k  and k  are velocity calculated by SINS and the true value, respectively (only contains the east and north velocity components).k includes the east and north external velocity information.k is the measurement noise.
If the external velocity information in  frame k is known, the measurement equation can be simplified as where k is the external velocity vector.H is the measurement matrix and H = [I 2×2 0 2×8 ].

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  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.
The simulation results are shown in Table 3 and the curve graphs of alignment errors with Δ = 10 ∘ , Δ = 10 ∘ , and Δ = 40 ∘ are shown in Figure 1.
Analyze the accuracy of initial alignment first.In Figure 1, 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 Table 3, 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 Figure 1, 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 Table 4.
From Table 4, 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 than 0.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.

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 Table 5.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.
In 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.In Figure 2, 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.

The First Turntable
In Table 6, 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 Figure 2, 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.
From Table 7, 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   9.
From Figure 3 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  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 in  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.
In Figure 4, 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 Table 11.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.
The 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.

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 of  ∞ 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.

Figure 2 :
Figure 2: The alignment errors of first turntable test.

Figure 3 :
Figure 3: The alignment errors of second turntable test.

Figure 4 :
Figure 4: The alignment errors of vehicle experiment.
. Denote the vector =  − φ = [      ] .The velocity error can be denoted ask  = k   − k  = [V   V   V   ] ] and w   are the gyroscope constant errors and gyroscope random errors in  frame.∇  = cos   cos   − sin   sin   sin   cos   sin   + sin   sin   sin   − sin   cos   − cos   sin   cos   cos   sin   sin   cos   + cos   sin   sin   sin   sin   − cos   sin   sin   cos   cos is the rotation projection of the earth.   ,    , and    are the slow variations of ω  , ω  , and ω  , respectively.C −1  is the inverse matrix of C  .With the large misalignment, C  and C    can be described as

Table 1 :
Simulation conditions of swing base.

Table 2 :
Simulation conditions of sensor precision.

Table 3 :
Statistical properties of alignment errors in mathematical simulation.

Table 4 :
Standard deviation of azimuth estimation errors in mathematical simulation.

Table 5 :
The sensor accuracy of FOSN.

Table 6 :
Statistical properties of alignment errors in first turntable test.

Table 7 :
Standard deviation of azimuth estimation errors in first turntable test.

Table 8 :
Statistical properties of the alignment errors in second turntable test.

Table 9 :
Standard deviation of azimuth estimation errors in second turntable test.
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.

Table 10 :
Statistical properties of the alignment errors in vehicle experiment.

Table 11 :
Standard deviation of azimuth estimation errors in vehicle experiment.