Square-Root Unscented Information Filter and Its Application in SINS/DVL Integrated Navigation

Abstract

To address the problem of low accuracy for the regular filter algorithm in SINS/DVL integrated navigation, a square-root unscented information filter (SR-UIF) is presented in this paper. The proposed method: (1) adopts the state probability approximation instead of the Taylor model linearization in EKF algorithm to improve the accuracy of filtering estimation; (2) selects the most suitable parameter form at each filtering stage to simply the calculation complexity; (3) transforms the square root to ensure the symmetry and positive definiteness of the covariance matrix or information matrix, and then to enhance the stability of the filter. The simulation results indicate that the estimation accuracy of SR-UIF is higher than that of EKF, and similar to UKF; meanwhile the computational complexity of SR-UIF is lower than that of UKF.

1. Introduction

Most underwater or surface navigation applications employ a Strapdown Inertial Navigation System (SINS) as their main navigation sensor, since SINS is a standalone system that can provide all of the required navigation data: position, velocity and orientation [1,2,3]. However, even with high precision SINS, the navigation solutions drift in time due to measurement errors of its inertial sensors. The Doppler velocity log (DVL) is a good acoustic-based device in marine applications, which can provide three-dimensional velocities to mitigate the errors of marine SINS [4,5,6]. Therefore, the integrated SINS and DVL navigation system is a common navigation method for underwater or surface navigation during long voyages [7,8,9]. The SINS system error model is nonlinear, and a nonlinear filtering algorithm is generally used for state estimation. The earliest nonlinear filtering method used in engineering was the Extend Kalman Filter (EKF) algorithm [10,11]. Its core idea is to approximate a linear expansion of the current nonlinear state equation (namely a Taylor series expansion, truncating high-order terms, retaining first-order terms) to apply the rules of EKF. However, the EKF algorithm is only applicable to weakly nonlinear systems. The stronger the nonlinearity of the estimated object is, the larger the estimation error will be, and it will even cause filter divergence.

Some scholars have proposed a probabilistic approximation of the nonlinear filtering construction idea [12], that is, using a deterministic sampling method to replace the Taylor series expansion linearization of the system model in EKF algorithm, and to approach the mean and variance of the Gauss state distribution by utilizing deterministic sampling points through a nonlinear system equation transformation propagation. The Unscented Kalman Filter (UKF) algorithm proposed by Uhlman is the first nonlinear algorithm to practice this idea [13,14,15]. It adopts an Unscented Transformation (UT) to obtain 2n + 1 Sigma sampling points with different weights, and uses the abovementioned Sigma sampling points to generate new points after transforming the nonlinear system equation for estimating the mean and the variance of the system states at the next moment. The theoretical deduction proves that the estimation accuracy of UKF algorithm can reach the third-order terms of Taylor series expansion for the nonlinear system [16,17,18].

The information filtering algorithm realizes a state estimation by transforming information parameters (including information matrix and information vector) [19,20]. It is equivalent to a series of Kalman filter algorithms which pass the moment parameters (covariance matrix and state vector) [21,22]. The whole filtering process can be divided into two processes: time updating and measurement updating. The time updating process involves the calculation of marginal probability, and the retrieval of the moment parameter form is relatively simple. The measurement updating process involves conditional probability, and the information parameter form is more effective. Therefore, in order to optimize the performance of the algorithm, the Square-Root Unscented Information Filter (SR-UIF) algorithm is proposed in this paper, which is applied to a nonlinear integrated SINS/DVL navigation system. This algorithm adopts the form of moment parameters in the process of time updating according the characteristic of the parameter form, respectively, the form of information parameters in the process of measurement updating; and ensures the symmetry and positive definite of the information matrix or covariance matrix by propagating their square root, and alleviates the problems such as divergence and data precision degradation caused by rounding errors in the filtering process.

The remainder of this paper is organized as follows: Section 2 studies the duality of Gaussian distribution; Section 3 focuses on the theoretical derivation of the SR-UIF algorithm; Section 4 briefly introduces the SINS/DVL model used in this paper; Section 5 verifies our findings through simulations and the conclusions are given in Section 6.

2. Duality of a Gaussian Distribution

We set X as the state vector of the integrated navigation system, which obeys a Gaussian distribution of multi-dimensional random variables, namely $p(\mathit{X})\sim N(\widehat{\mathit{X}},\mathit{P})$, $\widehat{\mathit{X}}$ is its mean value and P is its variance. Through the expansion of the Gauss distribution index term, it can be obtained that:

$$\begin{array}{ll}p(\mathit{X})& =N(\widehat{\mathit{X}},\mathit{P})\\ & =\frac{1}{\sqrt{\left|2\pi \mathit{P}\right|}}\exp \left\{-\frac{1}{2}{\left(\mathit{X}-\widehat{\mathit{X}}\right)}^T{\mathit{P}}^{-1}\left(\mathit{X}-\widehat{\mathit{X}}\right)\right\}\\ & =\frac{e^{-\frac{1}{2}{\mathit{y}}^T{\mathit{Y}}^{-1}\mathit{y}}}{\sqrt{\left|2\pi {\mathit{Y}}^{-1}\right|}}\exp \left\{-\frac{1}{2}{\mathit{X}}^T\mathit{Y}\mathit{X}+{\mathit{y}}^T\mathit{X}\right\}\\ & =N^{-1}(\mathit{y},\mathit{Y})\end{array}$$

(1)

where $\mathit{Y}$ is the information matrix, and $\mathit{y}$ is the information vector. It is usually called $\widehat{\mathit{X}}$ and P as the moment parameter form, $\mathit{y}$ and $\mathit{Y}$ as the information parameter form, and their transformation relations are:

$$\{\begin{array}{c}\mathit{Y}={\mathit{P}}^{-1}\\ \mathit{y}=\mathit{Y}\widehat{\mathit{X}}={\mathit{P}}^{-1}\widehat{\mathit{X}}\end{array}$$

(2)

From Equations (1) and (2), the information parameter form is also an expression of the Gaussian distribution, which is equivalent to the representation of the moment parameter form, and the two forms can be converted to each other. These are collectively referred to as the duality of the Gaussian distribution. The whole filtering process can be divided into time updating and measurement updating:

(1)

the time updating process involves the calculation of marginal probability:

$$\rho ({\mathit{X}}_k/{\mathit{U}}^{k-1},{\mathit{Z}}^{k-1})={\displaystyle \int \rho ({\mathit{X}}_k,{\mathit{X}}_{k-1}/{\mathit{U}}^{k-1},{\mathit{Z}}^{k-1})}d{\mathit{X}}_{k-1}$$

(2)

The measurement updating process involves the calculation of conditional probability:

$$\rho ({\mathit{X}}_k/{\mathit{U}}^{k-1},{\mathit{Z}}^k)=\frac{\rho ({\mathit{Z}}_k/{\mathit{X}}_k)\rho ({\mathit{X}}_k/{\mathit{U}}^{k-1},{\mathit{Z}}^{k-1})}{{\displaystyle \int \rho ({\mathit{Z}}_k/{\mathit{X}}_k)\rho ({\mathit{X}}_k/{\mathit{U}}^{k-1},{\mathit{Z}}^{k-1})}d{\mathit{X}}_k}$$

Suppose that X and Z satisfy the Gauss distribution as follows:

$$\rho (\mathit{X},\mathit{Z})=N\left(\left[\begin{array}{c}{\widehat{x}}_{\mathit{X}}\\ {\widehat{x}}_{\mathit{Z}}\end{array}\right];\left[\begin{array}{cc}{P}_{\mathit{X},\mathit{X}}& P_{\mathit{X},\mathit{Z}}\\ P_{\mathit{Z},\mathit{X}}& P_{\mathit{Z},\mathit{Z}}\end{array}\right]\right)=N^{-1}\left(\left[\begin{array}{c}{\widehat{y}}_{\mathit{X}}\\ {\widehat{y}}_{\mathit{Z}}\end{array}\right];\left[\begin{array}{cc}{Y}_{\mathit{X},\mathit{X}}& Y_{\mathit{X},\mathit{Z}}\\ Y_{\mathit{Z},\mathit{X}}& Y_{\mathit{Z},\mathit{Z}}\end{array}\right]\right)$$

The condition probability and marginal probability of the moment parameter form and information parameter are as shown in

Table 1. Expressions for the condition probability and marginal probability of the different form.

Parameter Form	Marginal Probability	Conditional Probability
Moment parameter	$\begin{array}{l}\widehat{x}={\widehat{x}}_{\mathit{X}}\\ P=P_{\mathit{X},\mathit{X}}\end{array}$	$\begin{array}{l}\widehat{x}={\widehat{x}}_{\mathit{X}}+P_{\mathit{X},\mathit{Z}}{P}_{\mathit{Z},\mathit{Z}}^{-1}\left(\mathit{Z}-{\widehat{x}}_{\mathit{Z}}\right)\\ P=P_{\mathit{X},\mathit{X}}-P_{\mathit{X},\mathit{Z}}{P}_{\mathit{Z},\mathit{Z}}^{-1}{P}_{\mathit{Z},\mathit{X}}\end{array}$
Information parameter	$\begin{array}{l}\widehat{y}={\widehat{y}}_{\mathit{X}}-Y_{\mathit{X},\mathit{Z}}{Y}_{\mathit{Z},\mathit{Z}}^{-1}{\widehat{y}}_{\mathit{Z}}\\ Y=Y_{\mathit{X},\mathit{X}}-Y_{\mathit{X},\mathit{Z}}{Y}_{\mathit{Z},\mathit{Z}}^{-1}{Y}_{\mathit{Z},\mathit{X}}\end{array}$	$\begin{array}{l}\widehat{y}={\widehat{y}}_{\mathit{X}}-Y_{\mathit{X},\mathit{Z}}\mathit{Z}\\ P=Y_{\mathit{X},\mathit{X}}\end{array}$

below. It can be seen when using the moment parameters and information parameters to calculate the marginal probability and conditional probability, the two equivalent expressions have completely different calculation characteristics. It is relatively simple to calculate the marginal probability in the moment parameter form. On the contrary, when calculating conditional probability, it is relatively effective to calculate the conditional probability in the information parameter form.

3. Square-Root Unscented Information Filter (SR-UIF) Algorithm

Square-Root Unscented Information Filter (SR-UIF) algorithm is a nonlinear filtering algorithm based on probabilistic approximation, which has the same structure as the well known UKF algorithm. It adopts an Unscented Transformation (UT) to obtain 2n + 1 Sigma sampling points with different weights, and uses the abovementioned Sigma sampling points to generate new points after transforming the nonlinear system equation for estimating the mean and the variance of the system states at the next moment. However, compared with the UKF algorithm, the SR-UIF algorithm selects the most suitable expression form at each filtering stage, that is, using the moment parameter form in the time updating, using the information parameter form in the measurement updating, for simplifying the computational complexity. Meanwhile, SR-UIF algorithm ensures the symmetry and positive definiteness of the information matrix or covariance matrix by propagating their square root, for improving the stability of the algorithm.

The discretized integrated navigation system model is:

$$\{\begin{array}{c}{\mathit{X}}_k=f({\mathit{X}}_{k-1})+{\mathit{G}}_{k-1}{\mathit{W}}_{k-1}\\ {\mathit{Z}}_k=h\left({\mathit{X}}_k\right)+{\mathit{V}}_k\end{array}$$

(3)

The following is analyzed for the implementation steps of SR-UIF algorithm. First, it is assumed that the initial filter estimation state is as follows:

$$\{\begin{array}{c}{\widehat{\mathit{X}}}_0=E[{\mathit{X}}_0]\\ {\mathit{P}}_0=E[({\mathit{X}}_0-{\widehat{\mathit{X}}}_0){({\mathit{X}}_0-{\widehat{\mathit{X}}}_0)}^T]\end{array}$$

(4)

Combined with the duality of Gaussian distribution, the initial information parameters can be obtained:

$$\{\begin{array}{c}{\mathit{y}}_0={\mathit{Y}}_0{\widehat{\mathit{X}}}_0\\ {\mathit{Y}}_0^{-1}={\mathit{P}}_0={\left\{E[({\mathit{X}}_0-{\widehat{\mathit{X}}}_0){({\mathit{X}}_0-{\widehat{\mathit{X}}}_0)}^T]\right\}}^{-1}\end{array}$$

(5)

3.1. Time Updating

The calculating of the edge probabilities involved in the time updating is simpler by using the moment parameter form. Therefore, the mean and variance are applied as the iterative factors in the time updating process of SR-UIF algorithm. The specific process is as follow:

(1)

Decompose the covariance matrix ${\widehat{\mathit{P}}}_{k-1}$ at $k-1$ time by using the Cholesky algorithm:

$${\widehat{\mathit{P}}}_{k-1}={\mathit{S}}_{k-1}{\mathit{S}}_{k-1}^T$$

(6)

where ${\mathit{S}}_{k-1}$ is the Cholesky factor of the covariance matrix ${\widehat{\mathit{P}}}_{k-1}$, and is the lower triangular matrix.

(2)

Calculate the Sigma point sets and their weights according to the Unscented Transformation (UT):

$$\{\begin{array}{ll}{\mathit{\xi }}_i=0,{\mathit{W}}_i={\mathit{W}}_i^m={\mathit{W}}_i^c=\frac{\kappa }{n+\kappa }& ,i=0\\ {\mathit{\xi }}_i=\sqrt{\frac{n+\kappa }{2}}{[1]}_i,{\mathit{W}}_i={\mathit{W}}_i^m={\mathit{W}}_i^c=\frac{1}{2\left(n+\kappa \right)}& ,i=1,2,\cdots ,2n\end{array}$$

(7)

where n is the system dimension; $\kappa$ is a free parameter; ${\mathit{W}}_i^m$ is the weighted value corresponding to each Sigma point, ${\mathit{W}}_i^c$ is the weighted value corresponding to variance matrix, and satisfies ${\displaystyle \sum {\mathit{W}}_i^c}={\displaystyle \sum {\mathit{W}}_i^m}=1$; ${[1]}_i$ indicates the i column or line of identity matrix $[1]$. Further, the state sample points at $k$ time can be obtained as follows:

$${\mathit{X}}_{i,k-1}={\mathit{S}}_{k-1}{\xi }_i+{\widehat{\mathit{x}}}_{k-1}$$

(8)

(3)

One-step state prediction augmented sample points ${\mathit{X}}_{i,k|k-1}^{\ast }$ can be obtained through a nonlinear transformation:

$${\mathit{X}}_{i,k|k-1}^{\ast }=F({\mathit{X}}_{i,k-1})$$

(9)

Then their mean and covariance matrix at $k$ time are

$${\widehat{\mathit{x}}}_{k|k-1}={\displaystyle \sum _{i=0}^m{\mathit{W}}_i^m{\mathit{X}}_{i,k|k-1}^{\ast }}$$

(10)

$${\mathit{P}}_{k/k-1}={\displaystyle \sum _{i=0}^m{\mathit{W}}_i^c{\mathit{X}}_{i,k|k-1}^{\ast }{\mathit{X}}_{i,k|k-1}^{\ast T}-{\widehat{\mathit{x}}}_{k/k-1}{\widehat{\mathit{x}}}_{k/k-1}^T}+{\mathit{Q}}_{k-1}$$

(11)

where ${\mathit{Q}}_{k-1}$ is the variance matrix of system noise.

3.2. Measurement Updating

As it is relatively simple to calculate the conditional probabilities by utilizing the information parameter form, the information vector and information matrix are applied as the iterative factors in the measurement updating process of SR-UIF algorithm, for achieving the optimal design of the algorithm. According to the theoretically deduced knowledge in the Appendix, it can be seen that the update equation of the information matrix and the information vector at $k$ time:

$$\{\begin{array}{c}{\widehat{\mathit{Y}}}_k={\mathit{Y}}_{k/k-1}+{\mathit{I}}_k\\ {\widehat{\mathit{y}}}_k={\widehat{\mathit{y}}}_{k/k-1}+{\mathit{i}}_k\end{array}$$

(12)

where ${\mathit{Y}}_{k/k-1}$ is the one-step prediction information matrix, and ${\mathit{Y}}_{k/k-1}={\mathit{P}}_{k/k-1}^{-1}$; ${\widehat{\mathit{y}}}_{k/k-1}$ is the one-step prediction information vector, and ${\widehat{\mathit{y}}}_{k/k-1}={\mathit{Y}}_{k/k-1}{\widehat{\mathit{x}}}_{k/k-1}$; ${\mathit{I}}_k$ and ${\mathit{i}}_k$ are the contributions of the information matrix and the information parameters, respectively, and:

$$\{\begin{array}{c}{\mathit{I}}_k=\left({\mathit{Y}}_{k/k-1}{\mathit{P}}_{xz,k/k-1}\right){\mathit{R}}_k^{-1}{\left({\mathit{Y}}_{k/k-1}{\mathit{P}}_{xz,k/k-1}\right)}^T\\ {\mathit{i}}_k=\left({\mathit{Y}}_{k/k-1}{\mathit{P}}_{xz,k/k-1}\right){\mathit{R}}_k^{-1}\left({\mathit{z}}_k-h\left({\widehat{\mathit{x}}}_{k/k-1}\right)+{\mathit{P}}_{xz,k/k-1}^T{\widehat{\mathit{y}}}_{k/k-1}\right)\end{array}$$

(13)

${\mathit{R}}_k^{}$ is the variance matrix of measurement noise, ${\mathit{P}}_{xz,k/k-1}$ is the cross-covariance matrix between $\mathit{x}$ and $\mathit{z}$.

It can be seen from Equation (13) that the measurement update states require the known ${\mathit{P}}_{xz,k/k-1}$. Now the specific solution process of ${\mathit{P}}_{xz,k/k-1}$ is given as follows:

(1)

Decompose the covariance matrix ${\mathit{P}}_{k/k-1}$ by using the Cholesky algorithm again,

$${\mathit{P}}_{k/k-1}={\mathit{S}}_{k/k-1}{\mathit{S}}_{k/k-1}^T$$

(14)

where ${\mathit{S}}_{k/k-1}$ is the Cholesky factor of the covariance matrix ${\mathit{P}}_{k/k-1}$, and is the lower triangular matrix.

(2)

Calculates the one-step prediction state sample points:

$${\mathit{X}}_{i,k|k-1}={\mathit{S}}_{k|k-1}{\xi }_i+{\widehat{\mathit{x}}}_{k|k-1}$$

(15)

(3)

One-step measurement prediction augmented sample points can be obtained through nonlinear transformation:

$${\mathit{Z}}_{i,k|k-1}=h\left({\mathit{X}}_{i,k|k-1}\right)$$

(16)

then their mean at $k$ time is:

$${\widehat{\mathit{z}}}_{k|k-1}={\displaystyle \sum _{i=0}^m{\mathit{W}}_i^m{\mathit{Z}}_{i,k|k-1}}$$

(17)

and the cross-covariance matrix is:

$${\mathit{P}}_{xz,k/k-1}={\displaystyle \sum _{i=0}^m{\mathit{W}}_i^c{\mathit{X}}_{i,k|k-1}^{\ast }{\mathit{Z}}_{i,k|k-1}^{\ast T}-{\widehat{\mathit{x}}}_{k/k-1}{\widehat{\mathit{z}}}_{k/k-1}^T}$$

(18)

In order to facilitate the experiment recording and observation, the filtering finally needs to transform the information parameter form into the moment parameter form. However, the state quantities involved in the inversion of large information matrix are undoubtedly huge, which makes the algorithm difficult to process. For this, this paper utilizes the Cholesky decomposition to deal with it, that is, firstly:

$${\widehat{\mathit{Y}}}_k={\mathit{L}}_k{\mathit{L}}_k^T$$

(19)

where ${\mathit{L}}_k$ is the Cholesky factor of the information matrix ${\widehat{\mathit{Y}}}_k$.

Since the Cholesky factor ${\mathit{L}}_k$ is a lower triangular matrix and its upper half elements are all zero, the computational complexity of the inverse operation of ${\mathit{L}}_k$ is much less than that of ${\widehat{\mathit{Y}}}_k$ when both ${\mathit{L}}_k$ and ${\widehat{\mathit{Y}}}_k$ are the same dimension. It can realize the optimization design of the algorithm for using Cholesky factor ${\mathit{L}}_k$ to solve the state vector estimate at k time:

$${\widehat{\mathit{x}}}_k={({\mathit{L}}_k^T)}^{-1}\mathit{f}$$

(20)

where $\mathit{f}$ is the forward vector of Cholesky, and $\mathit{f}={\mathit{L}}_k^{-1}{\widehat{\mathit{y}}}_k$.

Equation (6) shows that the covariance matrix ${\widehat{\mathit{P}}}_k$ needs to be decomposed during the next time updating process, and Equation (19) has been decomposed to the information matrix ${\widehat{\mathit{Y}}}_k$ at the next moment, which is a duplicate calculation. In view of this, the paper applies Equation (19) instead of Equation (6) to achieve further reduce the amount of the filtering calculation according to the transformation relation between ${\widehat{\mathit{P}}}_k$ and ${\widehat{\mathit{Y}}}_k$. The specific operation is as follows:

(1)

According to the transformation relation between ${\widehat{\mathit{P}}}_k$ and ${\widehat{\mathit{Y}}}_k$, the equation relationship between ${\mathit{S}}_k$ and ${\mathit{L}}_k$ is:

$$\{\begin{array}{c}{\mathit{L}}_k={\left({\mathit{S}}_k^T\right)}^{-1}\\ {\mathit{L}}_k^T={\mathit{S}}_k^{-1}\end{array}$$

(21)

(2)

Then ${\mathit{L}}_k$ is used to instead of ${\mathit{S}}_k$ in Equation (7) for iteration:

$${\mathit{X}}_{i,k}={\left({\mathit{L}}_k^T\right)}^{-1}{\xi }_i+{\widehat{\mathit{x}}}_k$$

(22)

3.3. Summary

Based on the above analysis, the frame diagram of SR-UIF algorithm is shown in the following Figure 1.

4. Nonlinear SINS/DVL Integrated Navigation Model

Set the local geographic coordinate frame t as the navigation frame of SINS, the error propagation equation about misalignment angles ${\mathit{\phi }}^t$ are constructed as:

$${\dot{\mathit{\phi }}}^t=\delta {\mathit{\omega }}_{ie}^t+\delta {\mathit{\omega }}_{et}^t-\left({\mathit{\omega }}_{ie}^t+{\mathit{\omega }}_{et}^t\right)\times {\mathit{\phi }}^t+{\mathit{C}}_b^t{\mathit{\epsilon }}^b$$

(23)

where e, b, and i denote the Earth, body, and inertial frames, respectively; ${\mathit{C}}_b^t$ is the attitude transformation matrix from the body coordinate frame b to frame t; ${\mathit{\epsilon }}^b$ is constant gyro drift in body frame; ${\mathit{\omega }}_{ie}^t$ is the earth rotational angular rate in frame t, and $\delta {\mathit{\omega }}_{ie}^t$ is its calculation error; ${\mathit{\omega }}_{et}^t$ is the rotational angular rate from frame t to frame e in frame t, and $\delta {\mathit{\omega }}_{et}^t$ is it calculation error.

Meanwhile, the vector expression of velocity equation is:

$$\delta {\dot{\mathit{v}}}^t={\mathit{f}}^t\times {\mathit{\phi }}^t-\left(2{\mathit{\omega }}_{ie}^t+{\mathit{\omega }}_{et}^t\right)\times \delta {\mathit{v}}^t-\left(2\delta {\mathit{\omega }}_{ie}^t+\delta {\mathit{\omega }}_{et}^t\right)\times {\mathit{v}}^t+{\mathit{C}}_b^t{\mathbf{\nabla }}^b$$

(24)

where $\delta {\mathit{v}}^t$ is the velocity error vector; ${\mathit{f}}^t$ is the specific force measured by the accelerometer in frame t; ${\mathbf{\nabla }}^b$ is the errors of accelerometers in body frame.

Ignoring the height channel, the equations describing position errors are two equations as follow:

$$\left[\begin{array}{c}\delta \dot{L}\\ \delta \dot{\lambda }\end{array}\right]=\left[\begin{array}{c}\frac{\delta v_y}{R+h}\\ \frac{\delta v_x}{R+h}\sec L+\frac{\delta v_x}{R+h}\sec L\tan L\delta L\end{array}\right]$$

(25)

where $\delta L$ and $\delta \lambda$ are the latitude and longitude error, respectively.

In this paper, a four-beam phased array DVL is used. The measurement error includes the velocity offset error $\delta V_d$, drift angle error $\delta \Delta$, and calibration coefficient error $\delta C$. Assuming that $\delta C$ is a random constant, $\delta V_d$ and $\delta \Delta$ are represented by the first-order Markov process, the error equations of DVL are expressed as:

$$\{\begin{array}{c}\delta {\dot{V}}_d=-{\beta }_d\delta V_d+{\omega }_d\\ \delta \dot{\Delta }=-{\beta }_{\Delta }\delta \Delta +{\omega }_{\Delta }\\ \delta \dot{C}=0\end{array}$$

(26)

where ${\beta }_d^{-1}$, ${\beta }_{\Lambda }^{-1}$ are expressed as the correlation time of DVL velocity offset error and bias angle error, respectively; ${\omega }_d$ and ${\omega }_{\Delta }$ are their Gauss white noise.

The system vector is defined as $\mathit{X}=\left[{\begin{array}{cccccccccc}\delta L& \delta \lambda & \delta v_e& \delta v_n& {\phi }_e& {\phi }_n& {\phi }_u& \delta V_d& \delta \Delta & \delta C\end{array}}^T\right]$, and the model of the system state equation is applied in this paper:

$$\dot{\mathit{X}}=f(\mathit{X})+\mathit{G}\mathit{W}$$

(27)

where $\mathit{W}={\left[\begin{array}{cccccccccc}0& 0& {\omega }_{ae}& {\omega }_{an}& 0& 0& 0& {\omega }_d& {\omega }_{\Delta }& 0\end{array}\right]}^T$ is the process noise sequence; the specific expressions of and refer to Equations (23)–(26).

Assuming that the DVL measurement is the ground velocity $V_d^{\prime }$, then the components of $V_d^{\prime }$ in the east and north directions are:

$$\{\begin{array}{c}{V}_{de}^{\prime }=\left(1+\delta C\right)\left(V_d+\delta V_d\right)\sin \left(K_d+{\varphi }_u+\delta \Delta \right)\\ V_{dn}^{\prime }=\left(1+\delta C\right)\left(V_d+\delta V_d\right)\cos \left(K_d+{\varphi }_u+\delta \Delta \right)\end{array}$$

(28)

where ${\varphi }_u$ is the azimuth misalignment angle; $K_d$ is the heading angle added to the drift angle. The Taylor series expansion is performed on $x=K_d$ for Equation (28):

$$\{\begin{array}{c}{V}_{de}^{\prime }\approx V_e+V_n\left({\varphi }_u+\delta \Delta \right)+\delta V_d\sin K_d+\delta C{V}_e\\ V_{dn}^{\prime }\approx V_n-V_e\left({\varphi }_u+\delta \Delta \right)+\delta V_d\cos K_d+\delta C{V}_n\end{array}$$

(29)

Meanwhile, the computing velocity of SINS can be expressed as:

$$\{\begin{array}{c}{V}_{se}^{}=V_e+\delta V_e\\ V_{sn}^{}=V_n+\delta V_n\end{array}$$

(30)

The difference between the SINS computing velocity and the DVL measurement velocity is taken as the measurement vectors, namely

$$\{\begin{array}{c}\delta V_e^{}=\delta V_e-V_n\left({\varphi }_u+\delta \Delta \right)-\delta V_d\sin K_d-\delta C{V}_e\\ \delta V_n^{}=\delta V_n+V_e\left({\varphi }_u+\delta \Delta \right)-\delta V_d\cos K_d-\delta C{V}_n\end{array}$$

(31)

Then the measurement equation is set up as follows:

$$\mathit{Z}=\mathit{H}\mathit{X}+\mathit{V}$$

(32)

where the measurement noise is taken as $\mathit{V}={\left[\begin{array}{cc}{v}_e& v_n\end{array}\right]}^T$; $\mathit{H}$ is the measurement matrix, and

$$\mathit{H}=\left[\begin{array}{cccccccccc}0& 0& 1& 0& 0& 0& -V_n& -\sin K_d& -V_n& -V_e\\ 0& 0& 0& 1& 0& 0& V_e& -\cos K_d& V_e& -V_n\end{array}\right]$$

5. Results

In the simulation experiments, two classical motion models are designed: uniform linear motion and uniform circular motion. First of all, the accuracy of position and velocity for purely inertial navigation, filtering with the SR-UIF algorithm, UKF algorithm, and EKF algorithm are compared and analyzed in these two modes of motion. Then, the computational complexity of SR-UIF algorithm and UKF algorithm are also compared and analyzed, which is determined by the average time consumed by a once filtering operation and the total elapsed time of each simulation experiment. Finally, the filtering performance of 1 h off-line data is used to further analyze the performance advantages of SR-UIF algorithm in terms of estimation accuracy and computational complexity. Finally, the filtering performance of 1 h off-line data is used to further analyze the performance advantages of SR-UIF algorithm in terms of estimation accuracy and computational complexity.

5.1. Simulation Analysis

Set the simulation conditions: initial latitude ${\phi }_0={45.7796}^{\circ }$, and initial longitude ${\lambda }_0={126.6705}^{\circ }$; initial position error $\delta L=\delta \lambda =100/R\mathrm{rad}$, initial velocity error $0.01\mathrm{m}/\mathrm{s}$; initial misalignment angles ${\varphi }_{e0}={\varphi }_{n0}={\varphi }_{u0}=1^{\circ }$; the gyro constant drifts along three axes of body frame are ${0.01}^{\circ }/\mathrm{h}$ with white noise ${0.005}^{\circ }/\mathrm{h}$; the accelerometer biases along three axes of body frame are $1\times {10}^{-4} \mathrm{g}$ with white noise $0.5\times {10}^{-4} \mathrm{g}$; for DVL, the velocity offset error $\delta V_d=0.01\mathrm{m}/\mathrm{s}$, the drift angle error $\delta \Delta =1^{\prime }$, the calibration coefficient error $\delta C=0.001$, the correlation time of DVL velocity offset error and bias angle error ${\beta }_d^{-1}=5\min$, ${\beta }_{\Delta }^{-1}=15\min$. The measurement data are obtained from IMU at a rate of 100 Hz and from DVL at a rate of 1 Hz. The filtering period is 1 s and the simulation time is 6284 s.

The initial parameters are set as:

$${\widehat{\mathit{X}}}_0={\left[\begin{array}{cccccccccc}0& 0& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]}^T$$

$${\mathit{P}}_0=diag\left\{\begin{array}{cccccccccc}{\left(100/R\right)}^2& {\left(100/R\right)}^2& {\left(0.1\mathrm{m}/\mathrm{s}\right)}^2& {\left(0.1\mathrm{m}/\mathrm{s}\right)}^2& {\left(1^{\circ }\right)}^2& {\left(1^{\circ }\right)}^2& {\left(1^{\circ }\right)}^2& {\left(0.005\mathrm{m}/\mathrm{s}\right)}^2& {\left(1^{\prime }\right)}^2& {\left(0.001\right)}^2\end{array}\right\}$$

$${\mathit{Q}}_0=diag\left\{\begin{array}{cccccccccc}0& 0& {\left(50\mathrm{ug}\right)}^2& {\left(50\mathrm{ug}\right)}^2& {\left({0.005}^{\circ }/\mathrm{h}\right)}^2& {\left({0.005}^{\circ }/\mathrm{h}\right)}^2& {\left({0.005}^{\circ }/\mathrm{h}\right)}^2& q\delta V_d& q\delta \Delta & 0\end{array}\right\}$$

$${\mathit{R}}_0^{}=diag\left\{\begin{array}{cc}{\left(0.01\mathrm{m}/\mathrm{s}\right)}^2& {\left(0.01\mathrm{m}/\mathrm{s}\right)}^2\end{array}\right\}$$

where $q\delta V_d={\left(0.005\mathrm{m}/\mathrm{s}\right)}^2\left(1-e^{-2{\beta }_{d}T}\right)$, $q\delta \Delta ={\left(1^{\prime }\right)}^2\left(1-e^{-2{\beta }_{\Delta }T}\right)$.

5.1.1. Uniform Linear Motion

The system is in uniform linear motion at 10 m/s with an initial heading angle of 45°, and the simulation time is 24 h. Figure 2, Figure 3, Figure 4 and Figure 5 show the position and velocity estimation error curves for purely inertial navigation, filtering with EKF, UKF, SR-UIF.

Table 2. Comparison of position and velocity errors for purely inertial navigation, filtering with EKF, UKF, SR-UIF in uniform linear motion.

Filtering Method	Maximum Latitude Error (deg)	Maximum Longitude Error (deg)	Maximum East Velocity Error (m/s)	Maximum North Velocity Error (m/s)
purely SINS	0.1518	0.1981	1.801	−2.517
EKF	9.294 ×10−3	1.194 × 10−2	0.03296	0.03177
UKF	3.211 × 10−3	8.888 × 10−3	0.0326	0.03048
SR-UIF	3.211 × 10−3	8.879 × 10−3	0.0326	0.03048

gives the estimation error values.

From the above simulation results in the uniform linear motion, it can be seen that:

(1)

The position and velocity errors for purely inertial navigation output diverge with time. The maximum latitude error and longitude error after 24 h are 0.1518° and 0.1981°, meaning 16,888 m and 23,000 m. Meanwhile, the maximum east velocity error and north velocity error reach 1.801 m/s and −2.517 m/s, respectively.

(2)

The position estimation errors in SINS/DVL integrated navigation are suppressed after filtering. The maximum latitude error and longitude error after EKF filtering are 9.294 × 10⁻³⁰ and 1.194 × 10⁻²⁰, meaning 720 m and 1322 m. The position estimation accuracy of UKF and SR-UIF algorithm is approximately the same, both higher than that of EKF algorithm. Specifically, the maximum latitude error and longitude error after SR-UIF filtering are 3.211 × 10⁻³⁰ and 8.879 × 10⁻²⁰, meaning 249 m and 986 m.

(3)

The velocity estimation errors in SINS/DVL integrated navigation are also suppressed after filtering. The maximum velocity error can be maintained in 10⁻² order of magnitude, and the velocity estimation accuracy of UKF or SR-UIF algorithm is slightly higher than that of EKF.

5.1.2. Uniform Circular Motion

The system has a radius of 10 km, a velocity of 10 m/s, an angular rate of $\omega ={10}^{-3}\mathrm{rad}/\mathrm{s}$, and performs a uniform circular motion counterclockwise. The simulation results are shown in the following figure. Figure 6, Figure 7, Figure 8 and Figure 9 show the position and velocity estimation error curves for purely inertial navigation, filtering with EKF, UKF, SR-UIF.

Table 3. Comparison of position and velocity errors for purely inertial navigation, filtering with EKF, UKF, SR-UIF in uniform circular motion.

Filtering Method	Maximum Latitude Error (deg)	Maximum Longitude Error (deg)	Maximum East Velocity Error (m/s)	Maximum North Velocity Error (m/s)
purely SINS	0.2792	−0.2703	−7.544	12.88
EKF	−3.594 × 10−4	−5.332 × 10−4	0.0178	0.2058
UKF	9.561 × 10−5	1.3254 × 10−5	−0.003045	0.003076
SR-UIF	9.561 × 10−5	1.3235 × 10−5	−0.003043	0.003071

gives the estimation error values.

From the above simulation results, it can be seen that:

(1)

Compared to the uniform linear motion, the uniform circular motion is more complex. Then the accuracy of pure inertial navigation is lower in the uniform circular motion for a short time, of which the maximum latitude error and longitude error are 0.2792° and −0.2684°, meaning 31,056 m and 30,302 m; and the maximum east velocity error and north velocity error reach −7.477 m/s and 12.88 m/s, respectively.

(2)

The position accuracy is obviously improved after filtering, the maximum latitude error and longitude error after EKF filtering are −3.594 × 10⁻⁴⁰ and −5.332 × 10⁻⁴⁰, meaning 28 m and 59 m. The position estimation accuracy of UKF and SR-UIF algorithm is approximately the same, which can be controlled within the same order of magnitude and both higher than that of EKF algorithm.

(3)

The velocity error can converge to a small range. The maximum velocity error after EKF filtering can be maintained in 10⁻² order of magnitude, and the initial filtering stage has obvious oscillatory process. The velocity estimation accuracy of UKF and SR-UIF algorithm advance by an order of magnitude compared to that of EKF, which can be maintained in 10⁻³ order of magnitude and the entire filtering process is smooth.

5.1.3. Performance Analysis

In order to analyze the superiority of SR-UIF algorithm performance, the simulation effect diagram of the time consumed by once filtering operation with SR-UIF and UKF in uniform linear motion or uniform circular motion is given in Figure 10.

We simulate multiple uniform linear motion and uniform circular motion experiments to calculate the average time consumed by once filtering operation with SR-UIF and UKF, and the total elapsed time of each simulation experiment. The results are shown in the following

Table 4. Comparison of the average time consumed by once filtering operation, and the total elapsed time of each simulation experiment with UKF, SR-UIF in uniform linear motion or uniform circular motion.

Filtering Method	Uniform Linear Motion (24 h)		Uniform Circular Motion (1.67 h)
	Average Time Consumed by Once Filtering Operation (s)	Total Elapsed Time of Each Simulation Experiment (s)	Average Time Consumed by Once Filtering Operation (s)	Total Elapsed Time of Each Simulation Experiment (s)
UKF	6.413 × 10−4	55.40832	6.396 × 10−4	4.0083732
SR-UIF	6.212 × 10−4	53.67168	6.195 × 10−4	3.8824065

.

From the above simulation results, it can be seen that whether the system is in a uniform linear motion or a uniform circular motion, the average time consumed by once filtering operation and the total elapsed time of each simulation experiment with SR-UIF algorithm are slightly smaller than with the UKF algorithm, which indicates that in view of the face that SR-UIF algorithm adopts the most suitable parameter form in each filtering stage, the computation amount of filtering is less than that of UKF algorithm.

5.2. Measured Data Analysis

Based on a ship-borne experiment using laboratory fiber optic gyro (FOG) strapdown inertial navigation equipment and a RDI Workhorse-type DVL instrument, the accuracy of the SR-UIF algorithm is analyzed. Figure 11 shows the velocity and position error curves of SINS/DVL integrated navigation system using SR-UIF algorithm and EKF algorithm. It can be seen that in the one-hour data analysis process, the latitude error and longitude error only reach −1.5 × 10⁻⁴⁰ and −1 × 10⁻⁴⁰ with SR-UIF algorithm, while these errors reach −2.8 × 10⁻⁴⁰ and −2.3 × 10⁻⁴⁰ with EKF algorithm. Meanwhile, the velocity error with SR-UIF algorithm has converged to within 0.02 m/sand remains stable, while within 0.1 m/s with EKF algorithm. The above results show that the positioning accuracy and velocity accuracy of SR-UIF algorithm are higher than that of EKF algorithm.

The estimation accuracy and the time consumed for data analysis of SR-UIF algorithm and UKF algorithm are given below in different data collection time, as shown in

Table 5. Performance comparison of SR-UIF and UKF in one hour collection data.

Filtering Method	Maximum Latitude Error (deg)	Maximum Longitude Error (deg)	Maximum East Velocity Error (m/s)	Maximum North Velocity Error (m/s)	Total Elapsed Time of Data Analysis (s)
UKF	−2.707 × 10−5	1.081 × 10−4	−0.02059	0.05312	1.841076
SR-UIF	−2.693 × 10−4	9.264 × 10−5	−0.02052	0.05304	1.834968

,

Table 6. Performance comparison of SR-UIF algorithm and UKF algorithm in one three collection data.

Filtering Method	Maximum Latitude Error (deg)	Maximum Longitude Error (deg)	Maximum East Velocity Error (m/s)	Maximum North Velocity Error (m/s)	Total Elapsed Time of Data Analysis (s)
UKF	−1.055 × 10−3	1.103 × 10−3	0.02574	0.05312	3.294395
SR-UIF	−9.494 × 10−4	1.0843 × 10−3	0.02569	0.05304	3.267851

and

Table 7. Performance comparison of SR-UIF algorithm and UKF algorithm in one five collection data.

Filtering Method	Maximum Latitude Error (deg)	Maximum Longitude Error (deg)	Maximum East Velocity Error (m/s)	Maximum North Velocity Error (m/s)	Total Elapsed Time of Data Analysis (s)
UKF	−1.888 × 10−3	1.501 × 10−3	−0.03409	0.05312	4.7329875
SR-UIF	−1.795 × 10−3	1.446 × 10−3	−0.03272	0.05304	4.7236778

.

As can be seen from the data in Table 5, Table 6 and Table 7, the positioning accuracy and velocity accuracy of the SR-UIF algorithm are nearly identical as with the UKF algorithm, but for the total elapsed time in the same collection time, the SR-UIF algorithm is significantly lower than the UKF algorithm.

5.3. Discussion

The simulation results and the measured data analysis results show that:

(1)

The position and velocity estimation accuracy of the nonlinear filtering based on the probabilistic approximation (such as UKF algorithm and SR-UIF algorithm) is higher than that of EKF algorithm based on the model Taylor series expansion.

(2)

SR-UIF algorithm and UKF algorithm are different expressions of the same filtering algorithm based on the probabilistic approximation, and they are equivalent in the filtering estimation value due to the duality of Gaussian distribution. The difference is that SR-UIF algorithm adopts the most suitable parameter form in each filtering stage, making its computational complexity lower than that of UKF algorithm. The simulation results are reflected that the average time consumed by once filtering operation and the total elapsed time of each experiment with SR-UIF algorithm are slightly smaller than UKF algorithm.

6. Conclusions and Future Works

In this paper, a square-root unscented information filter (SR-UIF) algorithm is applied to a SINS/DVL integrated navigation system. It is shown that: (1) the algorithm based on probability approximation has a relatively high estimation accuracy, and (2) the performance advantage of the algorithm is optimized by utilizing the most suitable parameter form in each filtering stage, and 3) using the square root of the variance matrix as the iterative factor to ensure the symmetry and positive definite of the information matrix or covariance matrix and thereby enhance the stability of the filtering. Finally, the simulation experiments show that: (1) the positioning and velocity measurement accuracy of SR-UIF algorithm are obviously higher than that of EKF algorithm, equaling to that of UKF algorithm; (2) and the computational complexity of SR-UIF algorithm is slightly lower than that of UKF algorithm. The simulation results provide a good theoretical basis and solution for popularizing the application of SINS/DVL integrated navigation filtering algorithm.

What we touched on in this paper is just the beginning, and there are many places where SR-UIF algorithm can wait for mining applications. The SR-UIF algorithm adopts the form of moment parameters in the process of time updating and the form of information parameters in the process of measurement updating, which makes it convenient to realize distributed or decentralized design of measurement data structure. The potential benefits of this data structure are: (1) improving the fault tolerance of the algorithm by detecting the accuracy of data measured from different sensors; (2) fusing the time unsynchronized measurement information by processing the distributed data. At the same time, the application scene of SR-UIF algorithm can also be transferred from underwater navigation of SINS/DVL to the cooperative navigation system on the ground or in the air, which will also become a direction of the future SR-UIF extension application.