^{1}

^{1}

^{2}

^{*}

^{2}

This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution license (http://creativecommons.org/licenses/by/3.0/).

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 modern marine navigation, the strapdown inertial navigation systems (SINS) is widely used due to its advantages of being more compact and autonomous. However, accumulated navigation errors are inevitable in SINS and may become considerably conspicuous in the long-term. Consequently, it is often aided with other sensors, e.g., global positioning system (GPS) and Doppler velocity log (DVL)

One outstanding feature of BD is that it is an active inquiry-response positioning system. The user's position information is sent to the ground central control system through two satellites and then processed by the ground central control system. Then, the processed information is sent back to the satellites, and finally the estimated user's position is sent to the user by the satellites [

To solve the multi-sensor asynchronous problem, a SINS/Beidou/STAR integrated navigation system based on the federal filtering algorithm was built up [

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.

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 [

In this paper, _{z}_{x}_{y}_{x} ϕ_{y} ϕ_{z}^{T}_{i}_{i}_{i}_{i}

The nonlinear attitude error equation of SINS can be derived as follows:
^{b}_{ω}

The SINS velocity error equation is given by:
^{b}^{b}^{n}^{n}^{n}

Suppose that ^{b}^{b}^{n}

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 _{M}_{N}_{x}_{y}_{x}_{y}

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 [_{t}_{δ}

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 [

In _{d}_{z}_{d}_{d}_{z}

From

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}_{d}_{d}_{Δ} are the corresponding white noises.

Consider the following discrete-time nonlinear state-space model:
_{k}_{k}_{k}_{−1} and _{k}_{k}_{−1} and _{k}

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 2

A set of 2_{i}_{i}_{i}_{i}

Under the assumption that the posterior density at time

Time-update:

Measurement-update:

With the new measurement vector _{k}_{k}_{∣}_{k}_{k}_{∣}_{k}_{k}

CKF uses cubature rule and 2_{i}_{i}

The nonlinear model for a SINS/BD/DVL integrated navigation system is established under the large azimuth misalignment angle in this paper. Considering the following error states: the longitude error _{x}_{y}_{x}_{y}_{z}_{x}_{y}_{x}_{y}_{z}_{t}_{d}

The corresponding state equation is written as:

The state function _{ax}_{ay}_{gx}_{gy}_{gz}_{δ}_{d}_{Δ} are the white noises of _{d}

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 [_{i}_{i}_{c}_{i}_{1}, _{i}_{2}, _{i}_{3}, are the direction cosine from the user to the _{1,}_{iρ}_{1,}_{iρ̇}

The velocity error measurements between the SINS and the DVL are as follows:
_{2,}_{x}_{2,}_{y}

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. Now, a new sampling principle is presented. If the sampling interval of SINS, BD and DVL are _{1}, _{2} and _{3} respectively, the greatest common divisor (GCD) of _{1}, _{2} and _{3} is denoted as _{1},_{2},_{3}). Thus, the sampling interval of the integrated navigation system Δ

Using this sampling criterion Δ

If only the measurements from SINS and BD are available at time _{1} and its covariance matrix _{1}, the state vector of the SINS/BD/DVL integrated navigation system at time

Similarly, if only the measurements from SINS and DVL are available at time _{2} and its covariance matrix _{2}, the state vector of the SINS/BD/DVL integrated navigation system at time

If all measurements from SINS, BD and DVL are available at time _{1}, _{2} and their covariance matrixes _{1}, _{2}, and then combine the locally estimated state vectors by sensor nodes:
_{1} and _{2} are the corresponding weighting matrices for both of the subsystem: SINS/BD and SINS/DVL. Suppose that the sensors are independent, the individual suboptimal estimations of the state vectors can be obtained. After the minimum variance principle, the weighting matrices can be determined, which is explained in details in [

If no measurement is available at time

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 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:
_{m}_{m}_{m}_{θ}_{γ}_{ψ}_{k}_{k}_{k}

The initial conditions of different sensors are presented as follows:

The initial latitude and longitude:

The initial velocity components: _{x}_{y}_{x}_{y}

The acceleration due to the gravity: _{0} = 9.7805 ^{2};

The initial misalignment angles: _{x}_{y}_{z}

The SINS gyro constant drifts: _{x}_{y}_{z}

The SINS gyro random noises: _{gx}_{gy}_{gz}

The SINS accelerometer constant biases: ∇_{x}_{y}^{−4} _{0};

The SINS accelerometer random noises: _{ax}_{ay}^{−5} _{0};

The constant bias of the BD clock error: Δ

The frequency drift of the BD clock error: _{t}

The correlation time:

The DVL velocity drift error: _{d}

The DVL scale factor error: ^{−4};

The DVL drift angle error:

The correlation times of _{d}

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

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 [

As can be seen from _{i}_{i}

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.

The authors would like to thank Yonggang Zhang, Qian Sun and other reviewers for their helpful comments. This work was supported in part by the National Natural Science Foundation of China (51179039, 61203225) and the Fundamental Research Funds for Central Universities (No. heucf041420).

The authors declare no conflict of interest.

The schematic of the velocity errors measured by the DVL.

The sampling principle of SINS/BD/DVL integrated navigation system.

Flow chart of novel algorithm based on CKF.

Sailing track of the marine vehicle.

(

(

Motion states of the marine vehicle.

^{2}) | ||
---|---|---|

1. Mooring | 0–300 | _{x}_{y} |

2. Accelerated motion | 300–620 | _{x}_{y} |

3. Uniform motion | 620–1,620 | _{x}_{y} |

4. Accelerated motion | 1,620–2,100 | _{x}_{y} |

5. Uniform motion | 2,100–3,100 | _{x}_{y} |

6. Accelerated motion | 3,100–3,700 | _{x}_{y} |

7. Uniform motion | 3,700–5,200 | _{x}_{y} |

8. Accelerated motion | 5,200–6,200 | _{x}_{y} |

9. Uniform motion | 6,200–10,800 | _{x}_{y} |

Simulation Results with different sensor data.

| |||
---|---|---|---|

SINS/BD | 275.1 | −183.4 | 219.8 |

SINS/DVL | −314.5 | −185.9 | 202.3 |

SINS/BD/DVL | −118.6 | −98.7 | 109.1 |

Simulation results with different filters.

| |||
---|---|---|---|

EKF | −384.4 | −255 | 284.5 |

CKF | −118.6 | −98.7 | 109.1 |