Abstract
A new moving state marine initial alignment method of strap-down inertial navigation system (SINS) is proposed based on high-degree cubature Kalman filter (CKF), which can capture higher order Taylor expansion terms of nonlinear alignment model than the existing third-degree CKF, unscented Kalman filter and central difference Kalman filter, and improve the accuracy of initial alignment under large heading misalignment angle condition. Simulation results show the efficiency and advantage of the proposed initial alignment method as compared with existing initial alignment methods for the moving state SINS initial alignment with large heading misalignment angle.
1. Introduction
It is well known that the attitude update of strap-down inertial navigation system (SINS) is achieved based on numerical integration [1]. Therefore, it is necessary to know initial navigation parameters including position, velocity, and attitude for navigation calculation. The procedure of estimating initial navigation parameters is initial alignment, and the accuracy of estimation of these initial navigation parameters, especially the estimation accuracy of attitude, is very important to subsequent navigation operation, since the initial attitude errors (or misalignment angles) will seriously degrade the performance of SINS and cause positioning and attitude errors [2]. Thus, it is important to estimate initial attitude and reduce misalignment angles. Initial alignment of SINS is usually accomplished in stationary mode [3]. However, a moving state initial alignment is necessary to maintain high navigation accuracy. Generally, after initial alignment, the resulting navigation state errors grow up because of the initialization errors and cumulative sensor inaccuracies [4]. Consequently, in large navigation errors, due to the growing sensor error and the poor orientation, SINS often requires to be realigned, and the initialization needs the ship to stop at the initial position for at least 5 to 10 minutes [4, 5]. However, it is inconvenient and impractical that there is not enough time to stop at the initial position. Therefore, a moving state initial alignment of SINS is necessary to enable the ship to start instantly [5]. Besides, in some applications, such as carrier-launched aircraft, it is necessary to achieve an accurate moving state (or in-motion) initial alignment of host SINS [6]. As the host carrier may be sailing while aligning the SINS of a carrier-launched aircraft, aiding information from host SINS will be used to accomplish the alignment, then a moving state alignment should be implemented to realign SINS for vessel in sail [6].
In moving state initial alignment of SINS, heading misalignment angle may be large since there is no reference to indicate current heading, especially for integrated alignment, and error model of SINS with large heading misalignment angle is nonlinear, which means linear estimation methods are not suitable for SINS initial alignment with large heading misalignment angle [7]. In order to solve the problem of moving state initial alignment with large heading misalignment angle, Kong et al. proposed an initial alignment method based on extended Kalman filter (EKF) [8]. However, it has low alignment accuracy and slow alignment speed. In order to improve the alignment accuracy and alignment speed, Zhou et al. proposed an initial alignment method based on unscented Kalman filter (UKF), which can at least capture the posterior mean and covariance to the second order of the Taylor series of any nonlinearity [9]. To improve the computational efficiency of UKF method, Chang et al. proposed an initial alignment method based on marginalized UKF [10]. To further improve the accuracy of UKF method, Long et al. proposed an initial alignment method based on central difference Kalman filter (CDKF), which can provide better covariance estimation than UKF [11]. To improve the numerical stability of UKF method, Sun proposed an initial alignment method based on cubature Kalman filter (CKF) [12], which is a special case of UKF with better numerical stability [13].
However, all moving state initial alignment methods mentioned above have limited alignment accuracy and alignment speed because they cannot capture the fifth order Taylor expansion terms of nonlinear alignment model. In order to improve alignment accuracy and alignment speed, a new moving state initial alignment method based on the fifth-degree CKF (5th-CKF) is proposed in this paper. For moving state initial alignment of SINS with large heading misalignment angle, the 5th-CKF addresses the strong nonlinearity problem better than existing methods because it can capture the fifth order Taylor expansion terms of nonlinear alignment model. As will be seen in our simulation results, the proposed initial alignment method outperforms existing initial alignment methods in terms of alignment accuracy and alignment speed.
The remainder of this paper is organized as follows. The nonlinear error model of moving state marine SINS initial alignment is presented in Section 2. The 5th-CKF method is formulated in Section 3. Section 4 focuses on the application of the 5th-CKF to the nonlinear estimation problem of moving state initial alignment of SINS and compares the proposed initial alignment method with existing initial alignment methods for the moving state SINS initial alignment with large heading misalignment angle. Concluding remarks are drawn in Section 5.
2. Marine SINS Initial Alignment Nonlinear Error Model
Initial alignment is a process to precisely determine initial values of strap-down matrix between the vehicle’s body frame and the reference frame so that the navigation computer can start with exact initial conditions. Initial alignment is a key technique in SINS. The alignment accuracy and alignment speed will influence the performance of SINS navigation. Next we will firstly introduce nomenclature used in inertial technology and then formulate marine SINS nonlinear error model in moving state initial alignment.
2.1. SINS Nonlinear Error Model for Moving State Marine Initial Alignment
In this paper, we choose as state vector in initial alignment, where and are errors of latitude and longitude (note we ignore the altitude error for marine application), and are velocity errors in east and north directions, , , and are rolling, pitching, and heading misalignment angles, respectively, and are constant bias of specific force in b frame, and , , and are constant drifts of gyro in b frame. If we denote vectors , , , and , we will have the following SINS nonlinear error model for moving state marine initial alignment [10]: with where is the meridian radius of curvature and is the transverse radius of curvature, is the computed geographic latitude, and is the transformation matrix from true navigation frame ( frame) to erroneously computed navigation frame ( frame) which is formulated aswhere is the computed attitude matrix, is the specific force measured by accelerometers in the body frame, and is the specific force error vector in the body frame, which can be formulated as where is a random white noise vector in the body frame and is a constant bias vector in the body frame. is the gyro error vector in the body frame, which can be formulated as where is a random white noise vector in the body frame and is a constant drift vector in the body frame. is the angular rate of the earth frame with respect to the inertial frame. is the computed value of , and is the computational error of . , , and can be formulated as follows: where is the angular rate of the navigation frame with respect to the earth frame. is the computed value of , and is the computational error of . , , and can be formulated as where , , are true velocity values in east, north, and up direction.
According to the definitions of , , and , they can be formulated as
We choose the velocity and position differences between SINS and external sensors, such as GPS or other higher accuracy SINSs as measurement vector , which can be formulated as where , , , and are measured latitude, longitude, velocity in east, and north directions, respectively.
Note that the process model of moving state initial alignment introduced in (1) is a continuous model and we must transform it into discrete form. Given the sample time , the propagations of position error, velocity error, and misalignment angles are discretized by using the fourth-degree Runge-Kutta method, and all the parts related to noise are discretized by using first-degree Runge-Kutta method. Based on (1) and (9), the discrete state equation and observation equation for state estimation can be formulated as where , is the Gaussian random process noise with mean and covariance and is the Gaussian random measurement noise with mean and covariance . Equation (10) formulates the nonlinear error model for moving state marine SINS initial alignment.
It is clear to see from (1) that the state equation of the error model of moving state marine SINS initial alignment is typically nonlinear. Thus, nonlinear filtering algorithms are necessary to estimate the state vector from which misalignment angles can be obtained to finish initial alignment. Next we will introduce high degree CKF method.
3. High Degree CKF
3.1. Brief Introduction of CKF
The heart of Gaussian filter is to compute multidimensional Gaussian-weighted integral [13, 14]. Different Gaussian approximate filters can be obtained when different integral rules are used. The third-degree CKF (3rd-CKF) is obtained when the third-degree spherical-radial cubature rule is used, and the third-degree spherical-radial cubature rule can be formulated as [13] where is an -dimensional Gaussian random vector with mean and covariance and is the square root matrix of ; that is, , and denotes a unit vector to the direction of coordinate axis .
The heart of the 3rd-CKF is the third-degree spherical-radial cubature rule in (11), which makes it possible to numerically compute multivariate moment integrals encountered in nonlinear Bayesian filter. The 3rd-CKF provides a systematic solution for high-dimensional nonlinear filtering problems. In addition, the 3rd-CKF is more stable and more principled in mathematics than sigma point approaches [13]. However, the accuracy of the 3rd-CKF is limit. To improve the accuracy of the 3rd-CKF, the 5th-CKF is proposed, which can capture higher order Taylor expansion terms of nonlinear function than the 3rd-CKF, thus higher accuracy can be obtained [14]. Next we will introduce the 5th-CKF method.
3.2. 5th-CKF Method
CKF is a recursive filtering method. We assume the posterior probability density of has been already known in the previous update . Firstly we calculate the Cholesky decomposition of as follows: The first class cubature-point and its weight are calculated as follows: The second class cubature-points and their weights are calculated as follows: where denotes a unit vector to the direction of coordinate axis .
The third class cubature-points and their weights are calculated as follows: where Sample points are obtained by propagating the above cubature-points through state equation in (10) as follows: One-step state prediction is then obtained as weighted linear combination of sample points One-step state prediction error covariance is updated as follows: Next the measurement update is performed. Cholesky decomposition of is performed firstly: The first class cubature-point and its weight are calculated as follows: Then the second class cubature-points and their weights are calculated as follows: The third class cubature-points and their weights are calculated as follows: Sample points are obtained by propagating the above cubature-points through observation equation as follows: One-step measurement prediction is then obtained as weighted linear combination of sample points: Autocorrelation covariance matrix is obtained as follows: Cross-correlation covariance matrix is calculated as follows: The Kalman filter gain is calculated as follows: State estimation is calculated as follows: The state estimation error covariance is calculated as follows:
and will be used in the next iteration. From the estimated state vector we can obtain estimated misalignment angles , with which the strap-down matrix between vehicle’s body frame and the reference frame can be determined, and the navigation computer can start with exact initial conditions. can be used to evaluate the accuracy of estimation. Next simulations will be performed to show the advantage of the proposed initial alignment method based on the 5th-CKF as compared with existing methods in marine initial alignment.
4. Simulations
Three simulations are performed with different parameter sets under different moving states of ship. In the first simulation, the ship is on the mooring. In the second simulation, the ship sails with constant speed m/s and m/s. In the third simulation, the ship accelerates with and and initial velocity of m/s and m/s. In addition, initial values of process noise covariance matrix and state and measurement noise covariance matrix in simulations are set as , , , respectively. Other parameters used in simulations are shown in Table 1.
To compare the performance of existing initial alignment methods based on the 3rd-CKF, UKF, CDKF, and the proposed initial alignment method based on the 5th-CKF, we choose the absolute value of estimation error of misalignment angles as performance metric. For a fair comparison, we make 500 independent Monte Carlo runs. Simulation results of existing methods and the proposed method are shown in Figures 1, 2, and 3 and Tables 2, 3, and 4, which corresponds to simulation 1, simulation 2, and simulation 3, respectively. Besides, a comparison of computational complexity between the proposed method and existing methods is shown in Table 5.
It is seen from Figures 1–3 that the proposed initial alignment method has faster alignment speed than existing initial alignment methods under large heading misalignment angle conditions. From Tables 2–4, we also can see that the proposed initial alignment method outperforms existing initial alignment methods in terms of alignment accuracy under large heading misalignment angle conditions. As shown in Table 5, although the proposed initial alignment method has higher computational complexity than existing initial alignment methods, its computation requirement is acceptable for practical marine navigation application.
Theoretically, as discussed in Section 2, the initial alignment model is nonlinear for the case of large heading misalignment angle, and all nonlinear filtering algorithms only can achieve suboptimal estimation of initial misalignment angles. However, the 5th-CKF can capture higher order Taylor expansion terms of nonlinear initial alignment model than the 3rd-CKF, UKF, and CDKF. Thus, the proposed initial alignment method based on the 5th-CKF is superior to existing methods based on the 3rd-CKF, UKF, and CDKF in terms of alignment accuracy and alignment speed under large heading misalignment angle. Theoretical analysis agrees with simulation results.
5. Conclusion
In this paper, a new moving state initial alignment method is proposed based on the 5th-CKF. Three simulations are performed for marine SINS initial alignment under different conditions, including mooring, moving with constant speed, and moving with constant acceleration. Simulation results show that the proposed marine SINS initial alignment method is superior to existing methods in terms of alignment accuracy and alignment speed for the moving state SINS initial alignment with large heading misalignment angle. It is more suitable for applications where fast and accurate alignment is necessary.
Nomenclatures
| Frame: | Inertial frame |
| Frame: | Earth frame |
| Frame: | True navigation frame (“east-north-up” local geographic frame) |
| Frame: | Frame fixed to the vehicle (right-front-up) |
| Frame: | Erroneously computed navigation frame |
| Misalignment angle vector : | Euler angles between frame and frame |
| : | Direction cosine matrix from frame to frame. |
Conflict of Interests
The authors declare that there is no conflict of interests regarding the publication of this paper.
Acknowledgments
This work was supported by the National Natural Science Foundation of China under Grant nos. 61001154, 61201409 and 61371173, China Postdoctoral Science Foundation no. 2013M530147, Heilongjiang Postdoctoral Fund LBH-Z13052 and the Fundamental Research Funds for the Central Universities of Harbin Engineering University no. HEUCFX41307.