A Coarse Alignment Based on the Sliding Fixed-Interval Least Squares Denoising Method

: The observation vectors in traditional coarse alignment contain random noise caused by the errors of inertial instruments, which will slow down the convergence rate. To solve the above problem, a real-time noise reduction method, sliding fixed-interval least squares (SFI-LS), is devised to depress the noise in the observation vectors. In this paper, the least square method, improved by a sliding fixed-interval approach, is applied for the real-time noise reduction. In order to achieve a better-performed coarse alignment, the proposed method is utilized to de-noise the random noise in observation vectors. First, the principles of proposed SFI-LS algorithm and coarse alignment are devised. A simulation test and turntable experiment were executed to demonstrate the availability of the designed method. It is indicated that, from the results of the simulation and turntable tests, the designed algorithm can effectively reduce the random noise in observation vectors. Therefore, the proposed method can enhance the performance of coarse alignment availably.

short time [Gao, Lu and Yu (2015); Xu, Xu, Zhang et al. (2017); Chang and Hu (2018)]. The initial alignment generally consists of two phases: the coarse alignment and the fine alignment [Qin, Yan, Gu et al. (2005); Wu, Wu, Hu et al. (2011);Xu, He, Qin et al. (2017)]. The coarse alignment performed first can quickly resolve a rough initial attitude. The output of coarse alignment is utilized as the initial value of fine alignment [Wu, Wu, Hu et al. (2011)]. The fine alignment can further improve the accuracy of the initial attitude [Xu, He, Qin et al. (2017)]. Traditional coarse alignment algorithms usually use the outputs of gyroscopes, accelerometers and other sensors to construct observation vector models, and then solve the rough initial attitude matrix. Qin et al. [Qin, Yan, Gu et al. (2005)] designed a method to complete the coarse alignment, which divides the initial attitude matrix into several sub-matrices by introducing the idea of inertial frame. However, the attitude determination method used in Qin et al. [Qin, Yan, Gu et al. (2005)] only utilizes the vectors at two point in the alignment process, which makes the convergence rate of the method slow. Wu et al. [Wu, Wu, Hu et al. (2011)] proposed a method named OBA to achieve a better-performed coarse alignment. The attitude determination method in the OBA algorithm constructs K matrix using the observation vectors, which chooses the eigenvector of the K matrix belong to the smallest eigenvalue as the optimal attitude quaternion. However, the OBA method does not dispose the random noise in the observation vectors caused by the measurement errors of the inertial sensors, which slows down the convergence speed of the alignment method. In summary, to achieve a better-performed coarse alignment on the swing base, attention needs to be paid on dealing with the random noise in the observation vectors. Least squares method has an important application in noise processing [Dong, Haynes and Atluri (2015); Yu, Zhao, de Lamare et al. (2019)]. But it needs to be improved if it is to be used in real-time processing of observation vectors. In this paper, a real-time noise reduction method, sliding fixed-interval least squares (SFI-LS) method, is proposed for the random noise in the observation vectors. The framework of this paper is designed as below: the theory of coarse alignment with inertial frame is stated in the part 2. Section 3 is devoted to introducing the noise reduction method and the attitude determination algorithm. Section 4 gives the simulation test and turntable experiment. Finally, conclusions are drawn in part 5.
2 The principle of coarse alignment 2.1 Definition of the coordinate frame Some frames utilized in this paper are defined as follows: ① i-frame: inertial frame; ② n-frame: navigation frame, this paper chooses local level geographic frame (East-North-Up, ENU); ③ n0-frame: the n-frame at the initial moment, remains static with the i-frame; ④ b-frame: the body frame for SINS (Right-Forth-Up, RFU); ⑤ b0-frame: the b-frame at the initial moment, remains static with the i-frame; The diagram of coordinate frames introduced above is shown in the Fig. 1.

Coarse alignment based on the inertial frame
The attitude quaternion at any time, corresponding to the attitude matrix , can be resolved into several sub-quaternions as where, the vector ( ) represents the attitude quaternion at time t. The vector 0 0 is the initial attitude quaternion, which is a constant attitude quaternion. The vector 0 ( ) is the mutative attitude quaternion in the n-frame. The vector ( ) 0 is the mutative attitude quaternion in the b-frame. The two time-varying quaternions can be solved by the following formula.

�̇(
where, the quaternions ( ) 0 and ( ) 0 are all unit vector [1 0 0 0 ] at the initial time. ∈ ℝ 3×1 is the carrier rotation angular velocity measured directly by gyroscopes. ∈ ℝ 3×1 is the rotation angular speed of the n-frame relative to the i-frame.
Through the above analysis, once the constant attitude quaternion 0 0 is solved, the attitude quaternion ( ) could be calculated according to Eq. (1). It is noticed that the quaternion 0 0 is always a constant attitude quaternion throughout the process of the coarse alignment, which is significant for the improvement of the alignment accuracy. Therefore, the problem of coarse alignment is converted to the attitude determination by introducing the idea of inertial frame. Next, the vector information needed for attitude determination will be constructed.
The force equation of SINS in the n-frame is as follow: ] is the force vector in the n-frame. When the carrier is swing, the velocity of the carrier is zero. Then the Eq. (3) can be expressed as indicates interference acceleration. The force vector described in n-frame can be converted into the b-frame by the next equation: where, is the force vector described in b-frame, measured directly by accelerometers. Substituting Eq. (1) and Eq. (5) into Eq. (4), then Reorganizing the above equation Then, the observation vectors and the reference vectors are defined respectively as follows: Eq. (7) can be transformed into the observation vectors-based measurement model for 0 0 as 0 0 ⨂ ⨂ 0 0 * = (9)

Noise reduction of vectors
It can be seen from the formula that the reference vector is only related to the angular speed of the earth's rotation and the gravity vector of the earth . Therefore, it can be considered that there is no error in the reference vectors. The observation vector is computed by the outputs of the accelerometers and the gyroscopes on the swing base. Because both the output of the accelerometers and the gyroscopes contains constant error and random error, the observation vector also contains error. If the observation vectors with errors are directly used, the attitude determination method will converge slowly. The traditional least squares method is a mathematical optimization technique, which is proved having better optimization effect than other methods. This method has widely applied in curve fitting. The specific principle of the least squares is introduced as follows.
Firstly, the mathematical model of observation vector is established as follows.
where, i = 1,2,3, j = 1,2, ⋯ , n The vectorization of the Eq. (10) is expressed as: Normally, the system of equations has no exact solution. In order to get the approximate solution of the equation, the residual sum function S(b) is introduced.
The aim of the least square method is to find an optimal vector ̂ to minimize the value of S(b).
Multiplying the transposition of matrix on both sides of Eq. (11): = (14) If the matrix is nonsingular, then vector has a unique solution. = ( ) −1 (15) The traditional least square method treats all the measurement data as a whole, gives parameter estimation by batch processing. But this method also has obvious shortcomings. On the one hand, when the measuring data is large, lager storage space is needed. On the other hand, this method is mostly used for post-processing, which is not suitable for realtime processing system. Therefore, in some practical projects, this method is not applicable. In view of the slow convergence rate of coarse alignment due to random noise in observation vectors, a real-time noise reduction method, sliding fixed-interval least squares (SFI-LS) method, is designed to reduce the noise in observation vectors. The specific ideas of the proposed algorithm are as follows. First, the fixed-interval of the proposed method is set as N. After calculating the observation vector at , the least square curve fitting is performed by using all the observation vectors from − to (M>N). Then, the observation vector at after noise reduction is computed by the solved coefficients. Finally, attitude determination is carried out using the observation vector after noise reduction. The schematic diagram of the real-time noise reduction method designed in this paper is shown in the following figure. It should be noted that the length of the sliding fixed-interval and the order of least squares fitting have great influence on the performance of the algorithm, which are two important parameters.

Attitude determination method
After constructing observation vectors, reference vectors and denoising the observation vectors, the constant attitude matrix can be solved by attitude determination algorithm. The specific process of the attitude determination method is as follows: The following two matrices are constructed using Eq. (9): The Eq. (9) is converted to [ ( ) − ( )] 0 0 = (18) According to the optimal attitude determination method, the above attitude determination problem can be transformed into the following optimal solution problem. where, . The constraint condition is The eigenvector of the matrix K corresponding to its minimum eigenvalue is the sought attitude quaternion 0 0 .
For more clarity, the structure of inertial alignment coarse alignment method based on vector noise reduction is listed as follows. Step 2: Step 3: Step 4: Compute using by = − 0 ⨂ ⨂ 0 * ; Step 5: Compute using by = 0 ⨂� + �⨂ 0 * ; Step 6: De-noise the vector by the proposed least squares denoising method of Sliding window; Step 7: Step 8: Determine the initial attitude quaternion by Calculate the normalized eigenvector of belong to the smallest eigenvalue; Step 9: Obtain the attitude matrix at current time Step 10: Go back to Step 1 until the end.

Simulation test and turntable test
A simulation experiment and turntable test are executed, using the sinusoidal motion model, to demonstrate the availability of the proposed algorithm on the swing base. The model of the simulation test is set as Eq. (20). The quantity A is the amplitude, the parameter f represents the frequency, the parameter φ represents the initial phase of the swing motion, the quantity θ is the swing center of the swing motion. The traditional coarse alignment method using the original observation vectors is utilized to compare with the proposed method in the paper. The performance of the two methods are compared and analyzed from the two aspects of convergence speed and convergence accuracy.

Simulation test
In the simulation test, the setting of the swing parameters are listed in Tab. 2. The simulation time is 200 s. The errors setting of the inertial measurement units in the simulation test are set as follows. As described before, the length of the sliding fixed-interval and the order of least squares fitting have great influence on the results of the algorithm. Next, two experiments were conducted to verify the effect of parameters on the performance of the designed method. First, the coarse alignment with different intervals of the SFI-LS algorithm are carried out to verify the influence of the length of the sliding fixed-interval on the performance of the algorithm. Specifically, the following five coarse alignment methods are listed as Tab. 4. The alignment results of the three attitudes for the above several methods are showed in Figs. 3-5.  In the above coarse alignment methods, the method1 is the traditional coarse alignment method without denoising the observation vectors. Method 2 to Method 5 adopted the proposed SFI-LS algorithm with different intervals of the SFI-LS algorithm, but they all adopt second-order model to process the noise in observation vectors. Fig. 3 and Fig. 4 show that there is no difference in horizontal angle attitude errors between these methods. All these methods can achieve good horizontal angle accuracy. Emphasis should be placed on the accuracy of the yaw angle. As can be seen from Fig. 5, when the order of SFI-LS method is fixed, the alignment performance of the SFI-LS method with interval of 5000 (Method 5) is the best. The convergence rate of Method 5 has been obviously improved compared with that of the traditional method (Method 1). Second, the coarse alignment with different order models of the SFI-LS algorithm are executed to demonstrate the influence of the order model of the least square method on the performance of the algorithm. Specifically, the following three coarse alignment methods are listed as Tab. 5. The alignment results of the three attitudes for the above several methods are showed in Figs. 6-8.  In the above coarse alignment methods, the Method 1 is the traditional coarse alignment method without denoising the observation vectors. Method 5 and Method 6 adopted the proposed SFI-LS algorithm with different order models of the SFI-LS algorithm, but the interval of the SFI-LS algorithm used in the above two methods are all set as 5000. Fig. 6 and Fig. 7 show that all these methods have a good performance on the horizontal angle. The difference between these methods is mainly reflected in the heading angle error. As can be seen from Fig. 8, when the interval of the SFI-LS method is fixed, the alignment performance of the SFI-LS method with the second-order model (Method 5) is the best.
After the above analysis, on the one hand, compared with traditional method, the design coarse alignment method has better performance. On the other hand, after comparing the several methods, the SFI-LS method (Method 5) with the interval length of 5000 and the second-order model has achieved the best performance of the coarse alignment.

Turntable test
The turntable experiment is carried out to verify the validity of this method in real environment. The physical diagram of the turntable experiment is shown in Fig. 9. The IMU is roughly installed in the center of the three-axis turntable. The performance indexes of the IMU are listed as Tab. 6. The swing motion of the IMU is simulated by controlling the rotation of the turntable. In the turntable test, the setting of the swing parameters is set as Tab. 7. The attitude of the turntable is taken as the reference value of the experiment. As shown in Fig. 9, the IMU is not accurately installed in the center of the three-axis turntable.
There is an installation error and arm error between the IMU and the turntable. These two errors have been calibrated and compensated before the turntable experiment is executed.   Through the analysis and the results of the simulation experiments, it is found that the Method 5 has the best performance than other methods. Therefore, in the turntable experiment, the Method 5 is utilized to compare with the traditional coarse alignment method. Figs. 10-12 show the attitude error results of the two coarse aliment methods in the turntable experiment. Fig. 13 shows the comparison of observation vectors in turntable experiment before and after noise reduction.  As described in Tab. 5, Method 1 represents the traditional coarse alignment method, while Method5 represents the proposed coarse alignment method based on noise reduction. Consistent with the simulation results, the horizontal angle errors of Method 5 and method1 are basically the same. However, the yaw angle errors of the two algorithms are quite different. Because the observation vectors in the Method 5 are de-noised, the convergence rate of Method 5 is faster and the oscillation amplitude is smaller. Fig. 13 shows the curves of the observation vectors, which have been normalized in the turntable experiment. The blue curve in Fig. 13 is the curves of the observation vectors without processing. As the observation vectors in the Method1 contain the random errors of the inertial sensors, the noise distribution of the blue curve is obvious. After real-time denoising, the noise of the observation vectors is much smaller, as shown in the red curve. Obviously, the performance of coarse alignment has been greatly improved after reducing the random errors in the observation vectors, which is embodied in the improvement of convergence rate and the decrease of oscillation amplitude.

Conclusion
In view of the slow convergence rate of coarse alignment due to random noise in observation vectors, a real-time noise reduction method, sliding fixed-interval least squares (SFI-LS) method, is devised to depress the noise in the observation vectors. In order to introduce the designed method in this paper clearly, the principle of real-time noise reduction method is deduced. The simulation and turntable experiments are executed to demonstrate the availability of the designed method. It is indicated that the designed algorithm can effectively improve the convergence rate of the coarse alignment, from the results of the simulation test and the turntable test. Most importantly, this method can be used for real-time noise reduction of observation vectors. Therefore, this method has important value in engineering application of rough alignment.