Initial Alignment of Large Azimuth Misalignment Angle in SINS based on Reduced Multiple Fading Factors Strong Tracking CKF

Aiming at the problem that Cubature Kalman Filter(CKF) has low accuracy and robustness under the condition of strapdown inertial navigation system(SINS) initial alignment due to model error and external disturbance, Reduced Multiple Strong Tracking Cubature Kalman Filter(RMSTCKF) is proposed, and the algorithm flow and suboptimal solution of multiple fading factor are derived. Multiple fading factor can improve tracking ability under each state according to the degree of uncertainty of different states, having stronger adaptability and robustness. Applying RMSTCKF to large azimuth misalignment angle error function described by Euler platform error angle(EPEA), carrying out the simulation under two different conditions, namely noise mismatch and the base is disturbed, and making contrast between RSTCKF and RCKF, the simulation results show that the filter accuracy and convergence rate of RMSTCK when system noise mismatches with true noise are obviously better than RSTCKF and RCKF, having better practical value in engineering.


INTRODUCTION
Practical working environment of SINS is usually complex and uncertain, the result of SINS coarse alignment is large misalignment, thus making linear error model and KF algorithm inapplicable based on the assumption of small misalignment. Article [1] uses the nonlinear error model and nonlinear filter algorithms described by EPEA to solve the problem, but system will be effected by model error, noise and external disturbance, so nonlinear filter sill have low robustness and tracking capability [2,3]. Article [4] proposes strong tracking filter theory, STF can maintain stronger robustness and tracking capability even when facing model uncertainty, noise and big external disturbance [3,4,5]. Article [3] applies STFUKF to initial alignment, but uncertainty of each state vector is different, so it proves that single fading factor can't track each state vector perfectly. MSTUKF is proposed in article [7], which doesn't need to calculate complex Jacobi matrix. Current theory has proved that the accuracy of UKF and CKF can reach the second-order and the third-order respectively, meanwhile, CKF needn't more sampling points and adjustable parameters, having shorter filter time and higher numerical stability [8,9,10].
This article derives reduced CKF on the basis that the distribution characteristics of cubature points in the third-order spherical radial cubature principle and the measurement equation is linear, combing STF with RCKF framework, introducing 2 multiple fading factor matrix, and proposing RMSTCKF that is appropriate to initial alignment. RMSTCKF only needs one cubature transformation without calculating Jacobi matrix, and multiple fading factor can improve tracking ability correspondingly according to the degree of uncertainty in different states. The SINS nonlinear error function described by EPEA in article [1] is applied in this article, and through the simulations done under the condition that noise mismatch and the base is disturbed, it's effective and practical for RMSTCKF.

MULTIPLE FADING FACTOR STF
Formula (a) is the performance index that makes the filter meet the condition of minimum variance estimation. Formula (b) forces the residual sequence of filter to keep orthogonal moment by moment, conquering the problem that the performance of filer decreases when state changes suddenly or the model is unsure [4,5], thus making STF have stronger robustness and tracking capability.

Flow of RMSTCKF
Set the initial state and covariance matrix of the system are 00 | X and 0 P , and apply the third-order spherical radial cubature principle to the algorithm. The time update and measurement update based on numerical integration from article [6] are shown as following.

Flow of RMSTCKF
① calculate cubature points [1]  ii n ξ denotes set of cubature points, [1]  n i R denotes the ith element of completely symmetric point set [1] .
④ calculate predicted error covariance matrix And use 2 multiple fading factor matrix introduced in to replace single fading factor, the form is: can be expressed as:  (1), completing the whole STF afterwards.

Sub-optimal solution of multiple fading factor
The sub-optimal solution of multiple fading factor is [2]: T 11

Large azimuth misalignment angle error model
Set inertial coordinate system is i-frame, earth coordinate system is e-frame, carrier coordinate system is b-frame. Select local geography coordinate system to be navigation coordinate system n-frame, the navigation coordinate system calculated by SINS is p-frame. Due to the influence of different error origin of SINS, the p and n calculated by navigation computer have misalignment angle error , which denotes a group of euler angle that rotate from n-frame to p-frame, the rotation order is defined as z  (27), ignore the second-order and higher-order value, and SINS attitude error function is expressed as: Where

SINS nonlinear filter model
Where the coefficient matrix 1  C  can be approximated as:

Simulation
Apply RCKF, RSTCKF and RMSTCKF to large azimuth misalignment angle model to carry on I nitial alignment. The position of SINS is 45  N, 108  E, height equals to 100m, the initial state of system is 0 = X 0 , and the parameters of inertial measurement unit are shown in table 1.

Initial alignment simulation on stationary base
Set the initial misalignment angle of E-direction, Ndirection and S-direction is   T 1 1 30    , apply above 3 algorithms to initial alignment simulation for 600s, the azimuth misalignment angle error is shown in figure 1. From the aspect of convergence, there's little difference between RCKF, RSTCKF and RMSTCKF. Through 100 times Monte-Carlo simulation, the accuracy of initial alignment is measured by the RMSE of azimuth misalignment angle at last 100s, and the specific results are shown in table 2. From the aspect of convergence speed, RMSTCKF converges around limited accuracy at 120s, faster than RSTCKF and RCKF. System isn't affected by model error and external disturbance in ideal environment, residual sequence keeps orthogonal moment by moment, and the above 3 algorithms can keep better filter capability.  Other simulation conditions are invariant, carry on the initial alignment simulation that last 600s with above 3 algorithms. Misalignment angle error curve at Sdirection is shown in figure 3, figure 3 (a) and figure 3 (b) are enlarged detail where is disturbed. It can be seen from figure 3 (a) that RMATCKF and RSTCKF have stronger adjustable ability and shorter convergence time than RCKF when system is disturbed. RMSTCKF converges to limited accuracy around 150s, RSTCKF converges to limited accuracy around 220s, and RCKF converges to limited accuracy around 500s.
It can be seen from figure 3 (b) that the filter value of 3 algorithm have different degree of wave due to the disturbance during 40s~50s. However, the disturbance wave of RMSTCKF is obviously smaller than the other 2 algorithms. The disturbance wave of RSTCKF is bigger than RMSTCKF but smaller than RCKF, and the disturbance wave of RCKF is the biggest that filter performance of RCKF is obviously decreasing, being disable to meet the alignment requirements of SINS. From above analysis, it's obvious that the proposed RMSTCKF has better adaptability and higher filter accuracy, equipping with stronger robustness and tracking capability when state changes suddenly.

Conclusion
An improved algorithm RMSTCKF which is applicable for initial alignment of large azimuth misalignment angle