A Strapdown Interial Navigation System/Beidou/Doppler Velocity Log Integrated Navigation Algorithm Based on a Cubature Kalman Filter

The integrated navigation system with strapdown inertial navigation system (SINS), Beidou (BD) receiver and Doppler velocity log (DVL) can be used in marine applications owing to the fact that the redundant and complementary information from different sensors can markedly improve the system accuracy. However, the existence of multisensor asynchrony will introduce errors into the system. In order to deal with the problem, conventionally the sampling interval is subdivided, which increases the computational complexity. In this paper, an innovative integrated navigation algorithm based on a Cubature Kalman filter (CKF) is proposed correspondingly. A nonlinear system model and observation model for the SINS/BD/DVL integrated system are established to more accurately describe the system. By taking multi-sensor asynchronization into account, a new sampling principle is proposed to make the best use of each sensor's information. Further, CKF is introduced in this new algorithm to enable the improvement of the filtering accuracy. The performance of this new algorithm has been examined through numerical simulations. The results have shown that the positional error can be effectively reduced with the new integrated navigation algorithm. Compared with the traditional algorithm based on EKF, the accuracy of the SINS/BD/DVL integrated navigation system is improved, making the proposed nonlinear integrated navigation algorithm feasible and efficient.

In [18], the local UKF was used to estimate the nonlinear integrated system and the federated Kalman filter was used to fuse the predictions of local filters, but in high-dimensional systems, the computation load is still heavy, thus, the filter converges slowly. In 2009, Arasaratnam and Haykin [19] proposed a more accurate nonlinear filtering solution based on a Cubature transform named Cubature Kalman filter (CKF) which can avoid linearization of the nonlinear system by using Cubature point sets to approximate the mean and variance. The third-order accuracy of the system can be achieved with this method. Because of its high accuracy and low calculation load, the CKF is widely used in attitude estimation and navigation [20][21][22].
In this paper a novel asynchronous algorithm for the SINS/BD/DVL integrated navigation system is proposed on the basis of CKF. Meantime, new nonlinear system and measurement models are also established for the measurements from SINS, BD and DVL. Taking multi-sensor asynchronization into account, a new sampling principle is proposed to make the best use of individual measurements. Even better, CKF can not only reduce the computational complexity, but also improve the accuracy of the navigation solution. The results from simulations showed that the proposed algorithm is superior to the conventional one. The rest of the paper is organized as follows. The description of the error differential equations of the SINS/BD/DVL integrated navigation system and the nonlinear filter named CKF are presented in Section 2. Section 3 shows the new sampling principle and the new asynchronous integrated navigation algorithm. Numerical examples along with specfic analysis are given in Section 4. Section 5 concludes this manuscript.

Nonlinear Error Model of SINS
Traditional linear differential equations are obtained under the assumption that the misalignment angles are small, so modeling errors are inevitable due to the nonlinearity of the true error model [3]. To improve the accuracy of the system model, a nonlinear error model of large azimuth misalignment angle for SINS is considered in this paper.
In this paper, i , b , e , n and n denote the inertial coordinate system, the body coordinate system, the earth coordinate system, the navigation coordinate system, and the calculation coordinate system of SINS, respectively. Suppose that n can be transformed to n by turning z  , The nonlinear attitude error equation of SINS can be derived as follows: The SINS velocity error equation is given by:   is ignored, Equation (4) can be rewritten as follows: Because both of the gyro and accelerometer errors are composed of a constant error vector and a zero-mean Gaussian white noise vector, their differential equations are: The position error equations comprise the longitude error  and the latitude error  : tan sec sec wherein M R and N R are the Earth's radii of the meridian circle and the prime vertical circle, respectively;  and  are the longitude and latitude of a point of interest; x v and y v are the east and north velocities with their velocity errors x v  and y v  , respectively.

Error Model of BD
The location information can be received directly from BD. The major error sources which affect the measurement accuracy of BD are the error of the BD receiver, the track error and the multi-path effect. To focus on the asynchronicity problem of multi-sensor systems, only the clock error of a BD receiver is taken into account here, including the clock bias and the clock frequency drift [6]. Despite the fact that the clock bias consists of constant and random components, only the constant bias is taken into account here for simplification. Normally, one uses t  and t  to denote the clock constant bias and the clock frequency drift. So the shaping filter of the BD receiver's clock error is described as follows: wherein  is the correlation time and W  is the white noise.

Error Model of DVL
The DVL functions as a sensor that measures the frequency shift of an acoustic signal, either transmitted or received by a moving object, which is proportional to the velocity of the moving object [2,23]. It can not only provide high accuracy absolute velocity, but aslo have satisfactory anti-interference performance, hence, DVL is widely deployed in marine navigation systems. The working principle of a DVL is based on the Doppler effect and the principle is described in Figure 1.
In Figure 1, K means the true heading, d K is the heading with the drift angle  , the drift of the angle error is denoted by  , and z  indicates the azimuth misalignment angle. By using d V  to denotes the velocity vector measured by the DVL, the following velocity equations are satisfied: From Figure 1, one can also obtain: According to the working principle of the DVL, one can obtain the velocity and the drift angle relative to the seafloor. Thus, the measurement errors include the velocity drift error d V  , the scale factor error C  and the drift angle error  [2,4]. The DVL error model is as follows: and  respectively; d W , W  are the corresponding white noises.

Cubature Kalman Filter
Consider the following discrete-time nonlinear state-space model: wherein k x and k z are the state vector and the measurement vector at time k , respectively;   f  and   h  are specific known nonlinear functions; and 1 k W  and k  are the noise vectors from two independent zero-mean Gaussian processes with their covariance matrices 1 k Q  and k R , respectively. CKF is proposed to solve this nonlinear filtering problem on the basis of the spherical-radial cubature criterion. CKF first approximates the mean and variance of probability distribution through a set of 2N (N is the dimension of the nonlinear system) Cubature points with the same weight, propagates the above cubature points through the nonlinear functions, and then calculates the mean and variance of the current approximate Gaussian distribution by the propagated cubature points [19].
A set of 2N Cubature points are given by  Under the assumption that the posterior density at time -1 k is known, the steps involved in the timeupdate and the measurement-update of CKF are summarized as follows [19]: Time-update:  to compute the mean and variance of probability distribution without any linearization of a nonlinear model. Thus, the modeling can reach the third-order or higher. Furthermore, this filtering solution does not demand Jacobians and Hessians so that the computational complexity will be alleviated to a certain extent.

Nonlinear Model of SINS/BD/DVL
The nonlinear model for a SINS/BD/DVL integrated navigation system is established under the large azimuth misalignment angle in this paper.
The corresponding state equation is written as: The state function   f  can be obtained from Equations (1)-(13) and [3]. Futher, the process noise vector is given by: To solve the problem of asynchronism, a new method is proposed to establish the measurement equations. The multi-sensor measurements can be pre-processed separately. Then, the central fusion blends all of the pre-processed data to obtain the optimal state vector. Here, the measurements are divided into two groups: pseudo-ranges and pseudo-range rates as the measurements for the SINS/BD filter, and the velocity errors as measurements for the SINS/DVL filter.
The measurement equation for the SINS/BD filter is [8]: wherein 2,x  , 2,y  are the DVL measurement noises.

Nonlinear Integration Navigation Algorithm Based on CKF
In this subsection, a CKF-based novel nonlinear algorithm is structured to solve the asynchronicity problem. In general, the smaller the sampling interval one uses, the higher system accuracy one can achieved, but accompanied with a larger calculation burden. A proper sampling interval should be designed accordingly.   The solution accuracy of the SINS/BD/DVL integrated navigation system can be improved enormously via CKF whilst the asynchronous problem is solved by this method. Besides, the computational cost of the BD control system of the ground center can also be reduced by using this sampling principle.

Simulations and Results
Simulations were performed in this work. Their results are presented in this section. Suppose that the swing dynamic model of a marine vehicle is given by: where  ,  and  are pitch, roll and yaw angles, respectively; the swing amplitudes were set up as  Table 1. The total time of each simulation was 10,800 s, and the sailing track of the vehicle is shown as in Figure 4.   Under the same simulation conditions, the nonlinear algorithm based on CKF was used to estimate state vectors for the SINS/BD/DVL integrated navigation system. The solution was compared with the CKF solution only using the measurements from SINS/BDor from SINS/DVL. Assume the sampling intervals of BD and DVL are 0.5 s and 0.1 s, respectively, while the sampling interval of the fusion center is 0.05 s. First, the alignment lasted 15 min, and then the navigation was performed. The simulation results are presented in Figure 5 and Table 2. Here the north position error, the east position error and the position error are used to describe the performance of the simulation results in which the location error is as follows: position error north position error east position error  Figure 5 and Table 2 show that the north position error, the east position error and the position error from the SINS/BD/DVL integrated solution were much smaller than the errors from the subsystems: SINS/BD and SINS/DVL respectively. Besides, the position error converged rapidly with the proposed algorithm. By using the redundant and complementary measurements from the SINS/BD/DVL integrated navigation system, the novel algorithm can reduce the impact of the asynchronous problem. Thus, the position error can be decreased availably, and the navigation accuracy can be increased significantly. Since it was assumed that all sensors were independent in this research, the estimation results were suboptimal. The equipment errors, such as the gyro drifts, the accelerometer biases, and the misalignment angles, can also bring errors to the navigation solution. Considering the above reasons, the delivered results are acceptable and reasonable.  To prove the superiority of the proposed nonlinear asynchronous fusion algorithm based on CKF, another simulation was carried out with the traditional fusion algorithm based on EKF introduced in [11]. The simulation conditions were the same as indicated above. The simulation results are given in Figure 6 and Table 3.  As can be seen from Figure 6 and Table 3, compared with the traditional nonlinear fusion method based on EKF, the north position error, the east position error and the position error of the SINS/BD/DVL integrated navigation system are smaller with the new algorithm based on CKF. With the traditional method based on EKF the maximal position error was about 284 m as the one with the proposed integration algorithm was nearly 109 m. That is, the position error was decreased by 61.6%.
As CKF uses cubature rule and 2N cubature point sets   , ii  to compute the mean and variance of probability distribution without any linearization of a nonlinear model, the filtering accuracy can be improved significantly. Hence, the higher navigation accuracy can be obtained based on CKF.

Conclusions
In this manuscript, a novel nonlinear integrated navigation algorithm based on CKF was proposed in order to solve the multi-sensor asynchronicitybproblem and reduce the high calculation load of the SINS/BD/DVL integrated navigation system. The main focus of this work was on establishing of a nonlinear system model and proposing of a new sampling principle to take multi-sensor asynchronism into account. The superiority of CKF was analyzed theoretically for the situation with the nonlinear system and measurement models. To verify the new navigation algorithm, numerical simulations were carried out. The results showed that the proposed nonlinear fusion algorithm based on CKF cannot only solve the asynchronicity problem of the SINS/BD/DVL integrated navigation system, but also significantly improve the navigation accuracy of the nonlinear system without imposing any additional calculation burden. However, under the assumption made in this study that all sensors in the integrated system were independent, the fusion results were suboptimal. Our future work will focus on a fusion algorithm that is suitable for multi-sensor asynchronous systems with the correlated noises.