2. UNSCENTED INTEGRATION METHOD

Due to nonlinearity of the dynamic equations of navigation, the general form of a continuous time non-linear state space model (Simon, Reference Simon2006) may expressed as:

(1)$${\dot {\bf x}} = {\bf f}({\bf x}\comma\ {\bf u}) + {\bf Gw}\comma\quad {\bf w} \sim {\bf N}(0\comma\ {\bf Q}_{\bf c} )$$

Where x is the total state vector of the system which is given as follows:

(2)$${\bf x} = \left[ {\matrix{ {{\bf p}^n} \hfill \cr {{\bf v}^n} \hfill \cr \Theta \hfill \cr {{\bf b}_a} \hfill \cr {{\bf b}_g} \hfill \cr}} \right]$$

${\bf p}^n = [L\comma\ l\comma\ Z]^T $ is the position vector of the vehicle in the navigation frame which includes the latitude, longitude, and the depth of the vehicle relative to the water surface. ${\bf v}^n = [v_N\comma\ v_E\comma\ v_D ]^T $ is the velocity vector resolved in the navigation frame which is included in the north velocity, east velocity, and down velocity. Θ = [ϕ, θ, ψ]^T is the orientation vector of the body frame with respect to navigation frame (North, East, and Down directions). ${\bf b}_a = [b_{ax}\comma\ b_{ay}\comma\ b_{az} ]^T $ is the vector of the accelerometer bias and ${\bf b}_g = [b_{gx}\comma\ b_{gy}\comma\ b_{gz} ]^T $ is the vector of the gyroscope bias. The inertial sensors' biases are modelled as a random walk plus a random constant (Maybeck, Reference Maybeck1979; Farrell, Reference Farrell2008):

(3)$${\dot {\bf b}}_a = {\bf \eta} _a\comma \quad {\bf \eta} _a \sim {\rm N}\left( {{\bf 0}\comma\ \sigma _{b_a} ^2 {\bf I}} \right)$$

(4)$${\dot{\bf b}}_g = {\bf \eta} _g\comma \quad {\bf \eta} _g \sim {\rm N}\left( {{\bf 0}\comma\ \sigma _{b_g} ^2 {\bf I}} \right)$$

Where η_a and η_g are Gaussian white noise vectors with variances $\sigma _{b_a} ^2 {\bf I}$ and $\sigma _{b_g} ^2 {\bf I}$, respectively. Throughout, the symbol N represents the normal distribution and I is the identity matrix.

f is the system nonlinear function which describes the dynamic behaviour of the system. G is the dynamic noise distribution. The control input u is given by:

(5)$${\bf u} = \left[ {\matrix{ {{\bf f}^b} \cr {{\bf \omega} _{ib}^b} \cr}} \right]$$

Where ${\bf f}^b = [\,f_x\comma\ f_y\comma\ f_z ]^T $ and ${\bf \omega} _{ib}^b = [\,p\comma\ q\comma\ r]^T $ are the specific force and angular rate of the IMU relative to inertial frame, resolved in the IMU frame respectively, measured by the inertial sensors (Farrell, Reference Farrell2008; Groves, Reference Groves2008). w, in Equation (1), is the process noise due to uncertainty in the control inputs and is modelled in the KF as a Gaussian white noise vector with zero mean and a Power Spectral Density (PSD) ${\bf Q}_c $, which may be expressed as follows:

(6)$${\bf w} = \left[ {\matrix{ {{\bf w}_a} \cr {{\bf w}_g} \cr {{\bf \eta} _a} \cr {{\bf \eta} _g} \cr}} \right]$$

Where ${\bf w}_a = [w_{ax}\comma\ w_{ay}\comma\ w_{az} ]^T $ and ${\bf w}_g = [w_{gx}\comma\ w_{gy}\comma\ w_{gz} ]^T $ are random noise components of the accelerometers and gyroscopes in x, y and z directions of the IMU axes, respectively and ${\bf Q}_c $ is a diagonal matrix as follows:

(7)$${\bf Q}_c = \left( {\matrix{ {\zeta _a^2 {\bf I}_3} & {{\bf 0}_3} & {{\bf 0}_3} & {{\bf 0}_3} \cr {{\bf 0}_3} & {\zeta _g^2 {\bf I}_3} & {{\bf 0}_3} & {{\bf 0}_3} \cr {{\bf 0}_3} & {{\bf 0}_3} & {\zeta _{b_a} ^2 {\bf I}_3} & {{\bf 0}_3} \cr {{\bf 0}_3} & {{\bf 0}_3} & {{\bf 0}_3} & {\zeta _{b_g} ^2 {\bf I}_3} \cr}} \right)$$

Where ζ _a, ζ _g, $\zeta _{b_a} $ and $\zeta _{b_g} $ are the root power spectral density of the accelerometers' noise, gyroscopes' noise, accelerometers' bias and gyroscopes' bias, respectively. I₃ and 0₃ are the 3 × 3 unit and zero matrices, respectively. Assuming that the noise characteristics of all accelerometers and all gyroscopes in x, y and z directions of the IMU axes are equal (Groves, Reference Groves2008; Niu et al., Reference Niu, Nasser, Goodall and El-Sheimy2007):

(8)$$\zeta _a = \sigma _a \sqrt {dt}\comma\ \zeta _g = \sigma _g \sqrt {dt} $$

(9)$$\zeta _{b_a} = \sigma _{b_a} \sqrt {\tau _{b_a}}\comma\ \zeta _{b_g} = \sigma _{b_g} \sqrt {\tau _{b_g}}$$

Where σ _a is the standard deviation of the accelerometer's noise, σ _g is the standard deviation of the gyroscope's noise, dt is the sampling time of inertial sensors, $\sigma _{b_a} $ and $\sigma _{b_g} $ are the standard deviations of the accelerometer and gyroscope bias, respectively and $\tau _{b_a} $ and $\tau _{b_g} $ are their correlation time. In this paper, the specifications of the inertial sensors provided by manufacturers are used to determine the system noise PSDs (Groves, Reference Groves2008).

In the proposed approach, the auxiliary measurements of the system are a nonlinear combination of the states which are corrupted with noise. This can be given in terms of the total states of the system by the following equation:

(10)$${\bf y} = {\bf h}({\bf x}) + {\bi v}\comma\quad {\bi v} \sim {\rm N}({\bf 0}\comma\ {\bf R})$$

Where y is the measurement vector of the system that is defined as follows:

(11)$${\bf y} = \left[ {\matrix{ {Z_m} \cr {{\bf v}_m^b} \cr {\Theta _m} \cr}} \right]$$

Where Z _m is the auxiliary signal of depth, ${\bf v}_m^b = [v_x\comma\ v_y\comma\ v_z ]^T $ are the measurements of the DVL in the body frame, and Θ_m = [ϕ _m, θ _m, ψ _m]^T are the auxiliary signals of orientation. The roll and pitch measurements are computed by the accelerometer signals and heading measurement is provided by a compass.

h, in Equation (6), is the measurement model function and v represents the measurement noise which is assumed to be a zero mean, Gaussian white-noise process with known covariance matrix R.

The discrete-time form of the Equations (1) may be expressed by:

(12)$${\bf x}_k = {\bf f}({\bf x}_{k - 1}\comma\ {\bf u}_{k - 1} ) + {\bf q}_{k - 1}\comma \quad {\bf q}_{k - 1} \sim {\rm N}({\bf 0}\comma\ {\bf Q}_{k - 1} )$$

Where x_k and u_k are the total state vector of the system and the control input at discrete time k, respectively. q_k is the process noise with covariance matrix Q_k at time step k (Jekeli, Reference Jekeli2001; Farrell, Reference Farrell2008):

(13)$${\bf Q}_{k - 1} \approx {\bf G}_{k - 1} {\bf Q}_c\, dt{\bf G}_{k - 1}^T $$

The discrete-time form of the Equations (10) may be given by the following equation:

(14)$${\bf y}_k = {\bf h}({\bf x}_k ) + {\bf r}_k\comma \quad {\bf r}_k \sim {\rm N}({\bf 0}\comma\ {\bf R}_k )$$

Where y_k is the measurement vector of the system at discrete time k. r_k is the measurement noise vector with covariance matrix R_k at time step k. In many navigation applications, due to being independent, the individual components of the measurement noise vector, R_k are considered as a diagonal matrix (Groves, Reference Groves2008; Grewal and Andrews, Reference Grewal and Andrews2008).

The unscented filter can be derived as an approximation of the general Gaussian filter. In order to construct the Gaussian filter, the moment matching approximations are used and also the filtering distribution is assumed as Gaussian (Arasaratnam, Reference Arasaratnam2009; Sarkka, Reference Sarkka2013).

(15)$$p\left( {{\bf x}_k \vert {\bf y}_k} \right) \cong {\rm N}\left( {{\bf x}_k \vert {\hat{\bf x}}_k\comma\ {\bf P}_k} \right)$$

Where ${\hat{\bf x}}_k $ and P_k are the mean and the covariance of the Gaussian distribution, respectively.

The prediction stage of the Gaussian filter with additive noise is given as follows:

(16)$$\eqalign{{\hat{\bf x}}_k^ - & = {\rm E}\left[ {{{\bf x}_k}\vert{{\bf y}_{1:k - 1}}} \right] \cr & = {\rm E}\left[ {{\bf f}\left( {{{\bf x}_{k - 1}}\comma\ {{\bf u}_{k - 1}}} \right) + {{\bf q}_{k - 1}} \vert {{\bf y}_{1:k - 1}}} \right]} $$

Since q_k−1 is zero-mean Gaussian, we get

(17)$$\eqalign{{\hat{\bf x}}_k^ - & = {\rm E}\left[ {{\bf f}\left( {{{\bf x}_{k - 1}}\comma\ {{\bf u}_{k - 1}}} \right)\vert{{\bf y}_{1:k - 1}}} \right] \cr & = \int {{\bf f}\left( {{{\bf x}_{k - 1}}\comma\ {{\bf u}_{k - 1}}} \right)} p\left( {{{\bf x}_{k - 1}}\vert{{\bf y}_{1:k - 1}}} \right)d{{\bf x}_{k - 1}} \cr & = \int {{\bf f}\left( {{{\bf x}_{k - 1}}\comma\ {{\bf u}_{k - 1}}} \right)} {\rm N}\left( {{{\bf x}_{k - 1}}\vert{\hat{\bf x}}_{k - 1}^ + \comma\ {\hat{\bf P}}_{k - 1}^ + } \right)d{{\bf x}_{k - 1}}}$$

(18)$$\eqalign{{\bf P}_k^ - & = {\rm E}\left[ {\left( {{{\bf x}_k} - {\hat{\bf x}}_k^ - } \right)\left. {{{\left( {{{\bf x}_k} - {\hat{\bf x}}_k^ - } \right)}^T}} \right\vert{{\bf y}_{1:k - 1}}} \right] \cr & = {\int {\left( {{\bf f}\left( {{{\bf x}_{k - 1}}\comma\ {{\bf u}_{k - 1}}} \right) - {\hat{\bf x}}_k^ - } \right)\left( {{\bf f}\left( {{{\bf x}_{k - 1}}\comma\ {{\bf u}_{k - 1}}} \right) - {\hat{\bf x}}_k^ - } \right)} ^T}{\rm N}\left( {{{\bf x}_{k - 1}}\vert{\hat{\bf x}}_{k - 1}^ + \comma\ {\hat{\bf P}}_{k - 1}^ + } \right)d{{\bf x}_{k - 1}} + {{\bf Q}_{k - 1}}} $$

Where E is the expectation operator, ${\hat{\bf x}}_k^ - $ and ${\bf P}_k^ - $ are the predicted mean and covariance of the Gaussian filter at the time step k.

Similarly to the prediction stage, the correction stage of the Gaussian filter is expressed by:

(19)$${\hat {\bf z}}_k = \ {\int} {{\bf h}({\bf x}_k ){\rm N}\left( {{\bf x}_k \vert {\hat{\bf x}}_k^ -\comma\ {\hat{\bf P}}_k^ -} \right)} d{\bf x}_k$$

(20)$${\bf P}_{yy\comma\ k} = \ {\int} {\left( {{\bf h}({\bf x}_k ) - {\hat {\bf z}}_k} \right)\left( {{\bf h}({\bf x}_k ) - {\hat {\bf z}}_k} \right)^T {\rm N}\left( {{\bf x}_k \vert {\hat{\bf x}}_k^ -\comma\ {\hat{\bf P}}_k^ -} \right)} d{\bf x}_k + {\bf R}_k$$

(21)$${\bf P}_{xy\comma\ k} = {\int} {\left( {{\bf x}_k - {\hat{\bf x}}_k^ -} \right)\left( {{\bf h}({\bf x}_k ) - {\hat {\bf z}}_k} \right)^T {\rm N}\left( {{\bf x}_k \vert {\hat{\bf x}}_k^ -\comma\ {\hat{\bf P}}_k^ -} \right)} d{\bf x}_k$$

(22)$${\bf K}_k = {\bf P}_{xy\comma\ k} {\bf P}_{yy\comma\ k}^{ - 1} $$

(23)$${\hat {\bf x}}_k^ + = {\hat {\bf x}}_k^ - + {\bf K}_k ({\bf y}_k - {\hat {\bf z}}_{\bf k} ) $$

(24)$${\bf P}_k^ + = {\bf P}_k^ - + {\bf K}_k {\bf P}_{yy\comma\ k} {\bf K}_k^T $$

Where ${\hat {\bf z}}_k $ is the predicted measurement, P_yy,k is the innovation covariance, P_xy,k is the cross-covariance matrix, K_k is the filter gain matrix and ${\hat {\bf x}}_k^ + $ and ${\bf P}_k^ + $ are the corrected mean and covariance of the Gaussian filter at the time step k, respectively.

The moment matching formulation makes possible the utilisation of the numerical approaches like the cubature integration methods for computing the multi-dimensional integrals of the form ${\int} {{\bf g}({\bf x}){\rm N}\left( {{\bf x}\vert{\hat {\bf x}}\comma\ {\bf P}} \right)d{\bf x}} $, where g(x) is an arbitrary nonlinear function.

It can be shown that the computation of the aforementioned integral may be accomplished using the third order spherical cubature rule as follows (Arasaratnam, Reference Arasaratnam2009; Sarkka, Reference Sarkka2013):

• • Calculate the 2n cubature or sigma points by:

  (25)$$\chi ^{(i)} = \left\{ {\matrix{ {\sqrt n {\bf e}_i \comma\quad} & {i = 1\comma \ldots\comma\, n} \cr { - \sqrt n {\bf e}_{i - n} \comma\quad} & {i = n + 1\comma \ldots\comma\, 2n} \cr}} \right.$$
  Where n is the dimension of the state vector and e_i is an n-component unit vector in the direction of coordinate axis i. In general, χ ⁽ⁱ⁾ is generated as follows:
  (26)$$\chi ^{(i)} = \left\{ {\left( {\matrix{ {\sqrt n} \hfill \cr 0 \hfill \cr 0 \hfill \cr \vdots \hfill \cr 0 \hfill \cr}} \right)\comma\left( {\matrix{ 0 \hfill \cr {\sqrt n} \hfill \cr 0 \hfill \cr \vdots \hfill \cr 0 \hfill \cr}} \right)\comma\ldots\comma \left( {\matrix{ 0 \hfill \cr 0 \hfill \cr 0 \hfill \cr \vdots \hfill \cr {\sqrt n} \hfill \cr}} \right)\comma\left( {\matrix{ { - \sqrt n} \hfill \cr 0 \hfill \cr 0 \hfill \cr \vdots \hfill \cr 0 \hfill \cr}} \right)\comma\left( {\matrix{ 0 \hfill \cr { - \sqrt n} \hfill \cr 0 \hfill \cr \vdots \hfill \cr 0 \hfill \cr}} \right)\comma\ldots\comma \left( {\matrix{ 0 \hfill \cr 0 \hfill \cr 0 \hfill \cr \vdots \hfill \cr { - \sqrt n} \hfill \cr}} \right)} \right\}$$

• • Compute the integral by:

  (27)$${\int} {{\bf g}({\bf x}){\rm N}\left( {{\bf x} \vert {\hat {\bf x}}\comma\ {\bf P}} \right)d{\bf x} \approx \displaystyle{1 \over {2n}}\sum\limits_{i = 1}^{2n} {{\bf g}\left( {{\hat {\bf x}} + \sqrt {\bf P} \chi ^{(i)}} \right)}} $$

Where the square root of the matrix P($\sqrt {\bf P} = {\bf C}$) can be calculated by solving the Cholesky decomposition equation ${\bf P} = {\bf CC}^T $ for a symmetric, nonnegative definite matrix (Grewal and Andrews, Reference Grewal and Andrews2008).

3. INTEGRATED NAVIGATION SYSTEM EQUATIONS

In this section, the system and measurement equations for the total states of navigation are derived and the procedure of the designed system is described.

3.1. The system equations

The system nonlinear functions f described in Equation (1) are given as follows (Farrell, Reference Farrell2008; Britting, Reference Britting1971):

(28)$${\dot {\bf p}}^n = {\rm Y}{\bf v}^n $$

(29)$${\dot{\bf v}}^n = {\bf C}_b^n \left( {{\bf f}^b - {\bf b}_a} \right) - \left( {2{\bf \omega} _{ie}^n + {\bf \omega} _{en}^n} \right) \times {\bf v}^n + {\bf g}^n $$

(30)$$\dot \Theta = \Lambda ^{ - 1} {\bf \omega} _{nb}^b $$

(31)$${\dot{\bf b}}_a = 0$$

(32)$${\dot{\bf b}}_g = 0$$

Where

(33)$${\rm Y} = \left( {\matrix{ {\displaystyle{1 \over {R_N - Z}}} & 0 & 0 \cr 0 & {\displaystyle{{\sec L} \over {R_E - Z}}} & 0 \cr 0 & 0 & 1 \cr}} \right)$$

(34)$${\bf f}^b = \left[ {\,f_x\comma\ f_y\comma\ f_z} \right]^T $$

(35)$${\bf \omega} _{ie}^n = \left[ {\Omega \cos L\comma\ 0\comma - \Omega \sin L} \right]^T $$

(36)$${\bf \omega} _{en}^n = \left[ {\displaystyle{{v_E} \over {R_E - Z}}\comma\ \displaystyle{{v_N} \over {R_N - Z}}\comma\ - \displaystyle{{v_E \tan L} \over {R_E - Z}}} \right]^T $$

(37)$${\bf g}^n = \left[ {0\comma\ 0\comma\ g} \right]^T $$

(38)$$\Lambda = \left( {\matrix{ 1 & 0 & { - \sin \theta} \cr 0 & {\cos \phi} & {\sin \phi \cos \theta} \cr 0 & { - \sin \phi} & {\cos \phi \cos \theta} \cr}} \right)$$

(39)$${\bf \omega} _{nb}^b = \left[ {\omega _x\comma\ \omega _y\comma\ \omega _z} \right]^T = \left( {{\bf \omega} _{ib}^b - {\bf b}_g} \right) - {\bf C}_b^n \left[ {{\bf \omega} _{ie}^n + {\bf \omega} _{en}^n} \right]$$

(40)$$R_N = \displaystyle{{R\left( {1 - e^2} \right)} \over {\left( {1 - e^2 \sin ^2 \left( L \right)} \right)^{1\cdot5}}} $$

(41)$$R_E = \displaystyle{R \over {\left( {1 - e^2 \sin ^2 \left( L \right)} \right)^{0\cdot5}}} $$

The variables R _N, R_E, R, e and Ω represent the meridian radius of curvature, the transverse radius of curvature, the length of the semi-major axis, the major eccentricity of the ellipsoid of the Earth, and the Earth's angular rate, respectively (Titterton and Weston, Reference Titterton and Weston2004). ${\bf C}_b^n $ is the transformation matrix from body to navigation axes which may be expressed by the following equation:

(42)$${\bf C}_b^n = \left( {\matrix{ {\cos \theta \cos \psi} & { - \cos \varphi \sin \psi + \sin \varphi \sin \theta \cos \psi} & {\sin \varphi \sin \psi + \cos \varphi \sin \theta \cos \psi} \cr {\cos \theta \sin \psi} & {\cos \varphi \cos \psi + \sin \varphi \sin \theta \sin \psi} & { - \sin \varphi \cos \psi + \cos \varphi \sin \theta \sin \psi} \cr { - \sin \theta} & {\sin \varphi \cos \theta} & {\cos \varphi \sin \theta} \cr}} \right)$$

f^b represents the specific force in body axes. ${\bf \omega} _{ie}^n $ is the angular rate of the Earth expressed in the navigation frame. ${\bf \omega} _{en}^n $ represents the angular rate of the navigation frame with respect to the Earth-fixed frame. gⁿ and g are the gravity vector in the navigation frame and the Earth gravity constant, respectively. ${\bf \omega} _{nb}^b $ is the angular rate of the vehicle with respect to the navigation frame. ${\bf \omega} _{ib}^b = [p\comma\ q\comma\ r]^T $ is the angular rate of the vehicle. The gravity g is computed by the following equation (Titterton and Weston, Reference Titterton and Weston2004):

(43)$$g = \displaystyle{{g_0} \over {1 - Z/R_0}} $$

Where

(44)$$g_0 = 9\!\cdot\!780318 \times \left( {1 + 5\!\cdot\!3024 \times 10^{ - 3} \sin ^2 L - 5\!\cdot\!9 \times 10^{ - 6} \sin ^2 2L} \right)$$

(45)$$R_0 = \sqrt {R_N R_E} $$

3.2. The measurement equations

In this section, the measurement equations used in this paper, including the depth, velocity, roll, pitch, and heading are derived.

The measurement equation of the depth meter and its measurement noise variance are defined by:

(46)$$Z_m = Z + \nu _z $$

(47)$$R_z = \sigma _z^2 $$

Where ν _z and $\sigma _z^2 $ is the measurement noise and the measurement noise variance of the depth meter, respectively.

The relationship between the DVL's measurements and the states of the velocity may be expressed by the following nonlinear equation:

(48)$${\bf v}_m^b = {\bf C}_n^b {\bf v}^n + {\bf \nu} _v $$

This equation can be expressed in component form as follows:

(49)$$v_x = C_{11} v_N + C_{21} v_E + C_{31} v_D + \nu _{v_x} $$

(50)$$v_y = C_{12} v_N + C_{22} v_E + C_{32} v_D + \nu _{v_y} $$

(51)$$v_z = C_{13} v_N + C_{23} v_E + C_{33} v_D + \nu _{v_z} $$

Where C _ij is the element in the ith row and the jth column of the transformation matrix from body frame to navigation frame (i.e. ${\bf C}_n^b $), [v _N, v _E, v _D] is the north, east and down velocity components and $\nu _v = \left[ {\nu _{v_x}\comma\ \nu _{v_y}\comma\ \nu _{v_z}} \right]$ is the measurement noise vector of the DVL.

It can be shown that the covariance matrix of the DVL measurements caused by instrumental noise is given by (Gilcoto et al., Reference Gilcoto, Jones and Farina-Busto2009):

(52)$${\bf R}_{dvl} = \left( {\matrix{ {\displaystyle{{\sigma _{b_1} ^2 + \sigma _{b_2} ^2} \over {4\sin ^2\beta}}} & 0 & {\displaystyle{{\sigma _{b_1} ^2 - \sigma _{b_2} ^2} \over {8\sin \beta \cos \beta}}} \cr 0 & {\displaystyle{{\sigma _{b_{3}} ^2 + \sigma _{b_4} ^2} \over {4\sin ^2 \beta}}} & {\displaystyle{{\sigma _{b_4} ^2 - \sigma _{b_3} ^2} \over {8\sin \beta \cos \beta}}} \cr {\displaystyle{{\sigma _{b_1} ^2 - \sigma _{b_2} ^2} \over {8\sin \beta \cos \beta}}} & {\displaystyle{{\sigma _{b_4} ^2 - \sigma _{b_3} ^2} \over {8\sin \beta \cos \beta}}} & {\displaystyle{{\sigma _{b_1} ^2 + \sigma _{b_2} ^2 + \sigma _{b_3} ^2 + \sigma _{b_4} ^2} \over {16\cos ^2 \beta}}} \cr}} \right)$$

Where $\sigma _{b_i} ^2 $ is the variance of the radial velocity along the beam i (i = 1,2,3,4). It may be assumed that the radial velocity variances are equal which yields:

(53)$$if\;\sigma _{b_1} ^2 = \sigma _{b_2} ^2 = \sigma _{b_3} ^2 = \sigma _{b_4} ^2 \Rightarrow {\bf R}_{dvl} = \left( {\matrix{ {\sigma _{v_x} ^2} & 0 & 0 \cr 0 & {\sigma _{v_y} ^2} & 0 \cr 0 & 0 & {\sigma _{v_z} ^2} \cr}} \right)$$

Where $\sigma _{v_x} ^2 $, $\sigma _{v_y} ^2 $, and $\sigma _{v_z} ^2 $ are the variance of the DVL's measurements along its body frame.

The measurement model of the roll and pitch may be given by:

(54)$$\eqalign{& \phi _m = \phi + \nu _\phi \cr & \theta _m = \theta + \nu _\theta} $$

Where ϕ _m and θ _m are the auxiliary signals of the roll and pitch computed from the accelerometer's signals. Under circumstances where the vehicle is stationary or moves at a constant velocity (or cruising conditions), the only acceleration acting on the vehicle is the acceleration due to gravity (Grenon et al., Reference Grenon, An, Smith and Healey2001; Groves, Reference Groves2008; Farrell, Reference Farrell2008). Therefore, the roll and pitch signals may be computed by the following equations:

(55)$$\theta _m = \displaystyle{{\sin ^{ - 1} (\,f_x )} \over g}$$

(56)$$\varphi _m = \displaystyle{{\sin ^{ - 1} (\,f_y )} \over {g\cos (\theta _m )}}$$

Where f _x and f _y are the accelerations acting on the vehicle along the forward and starboard directions measured by the accelerometers. When the following logical condition is true, the roll and pitch measurements are used in the correction stage of the integration filter (Farrell, Reference Farrell2008):

(57)$$\left( {\Vert {{\bf f}^b} \Vert - g \lt T_f} \right){\rm and}\left( {\Vert {{\bf \omega} _{ib}^b} \Vert \lt T_\omega} \right)$$

Where T _f and T _ω are determined by trial and error. The symbol $\Vert\cdot\Vert$ denotes the norm of a vector.

Although, in theory, the roll and pitch signals are correlated to each other, as stated in Equation (56), one can assume that these signals are independent for small pitch angles:

(58)$${\rm for}\,{\rm small}\,{\rm pitch}\,{\rm angles} \Rightarrow \cos (\theta _m ) \approx 1 \Rightarrow \left\{ {\matrix{ {\theta _m = \displaystyle{{\sin ^{ - 1} (\,f_x )} \over g}} \cr {\phi _m \approx \displaystyle{{\sin ^{ - 1} (\,f_y )} \over g}} \cr}} \right.$$

This type of manoeuvre is performed in many marine applications, including the test conducted for this paper. Therefore, the measurement noise matrix for the signals of the roll and pitch is assumed to be diagonal as follows:

(59)$${\bf R}_\Theta = \left( {\matrix{ {\sigma _\varphi ^2} & 0 \cr 0 & {\sigma _\theta ^2} \cr}} \right)$$

Where $\sigma _\varphi ^2 $ and $\sigma _\theta ^2 $ are the variance of the signals of the roll and pitch, respectively. These quantities are calculated in terms of the variance of the acceleration signals according to Equation (58).

The measurement equation of the compass and its measurement noise variance are defined by:

(60)$$\psi _m = \psi + \nu _\psi $$

(61)$$R_\psi = \sigma _\psi ^2 $$

Where $\sigma _\psi ^2 $ is the variance of the compass measurement.

3.3. Data integration

3.3.1. The prediction stage

In the prediction stage of the unscented filter, for each time step of inertial sensors, the total state of the system ${\hat {\bf x}}^ - $ and covariance of the state ${\bf P}^ - $ are predicted using Equations (12) and (13) and the unscented based integration method given by Equations (20)–(22).

In order to execute the prediction stage, the system equations can be expressed in component form as follows:

(62)$${\dot L} = \displaystyle{{v_N} \over {R_N + Z}}$$

(63)$$ {\dot l} = \displaystyle{{v_E \sec (L)} \over {R_E + Z}}$$

(64)$${\dot Z} = v_D $$

(65)$${\dot v}_N = f_N - v_E \left( {2\Omega + {\dot l}}\right)\sin (L) + v_D {\dot L}$$

(66)$${\dot v}_E = f_E - \left( {2\Omega + {\dot l}}\right)\left( {v_N \sin \left( L \right) + v_D \cos \left( L \right)} \right)$$

(67)$${\dot v}_D = f_D - v_E \left( {2\Omega + {\dot l}}\right)\cos \left( L \right) + v_N {\dot L} + g$$

(68)$${{\dot \phi}} = \left( {\omega_y \sin \phi + \omega_x \cos \phi} \right)\tan \theta + \omega _x $$

(69)$${\dot \theta}= \left( {\omega_y \cos \phi - \omega _z \sin \phi} \right)$$

(70)$${\dot \psi} = \left( {\omega _y \sin \phi + \omega _z \cos \phi} \right)\sec \theta $$

Where

(71)$$f_N = C_{11} (\,f_x ) + C_{12} (\,f_y ) + C_{13} (\,f_z )$$

(72)$$f_E = C_{21} (\,f_x ) + C_{22} (\,f_y ) + C_{23} (\,f_z )$$

(73)$$f_D = C_{31} (\,f_x ) + C_{32} (\,f_y ) + C_{33} (\,f_z )$$

(74)$$\omega _x = \left( p \right) + \left( {C_{11} \omega _N + C_{21} \omega _E + C_{31} \omega _D} \right)$$

(75)$$\omega _y = \left( q \right) + \left( {C_{12} \omega _N + C_{22} \omega _E + C_{32} \omega _D} \right)$$

(76)$$\omega _z = \left( r \right) + \left( {C_{13} \omega _N + C_{23} \omega _E + C_{33} \omega _D} \right)$$

And

(77)$$\omega _N = {\it \Omega} \cos L + \displaystyle{{v_E} \over {R_E - Z}}$$

(78)$$\omega _E = - \displaystyle{{v_D} \over {R_N - Z}}$$

(79)$$\omega _D = - {\it \Omega} \sin L - \displaystyle{{v_E \tan L} \over {R_E - Z}}$$

Where C _ij is the element in the ith row and the jth column of the transformation matrix that is given by Equation (35).

3.3.2. The correction stage

Since auxiliary sensors are asynchronous or sampled at different rates, the correction stage is also executed asynchronously. Operating in asynchronous mode means that when measurements from one or more of the auxiliary sensors are received at time step k, the measurement parameters associated with those sensors (y_k and R_k) are formed, and then the correction equations of the unscented filter are executed. In the event that none of the auxiliary sensors are available, the correction stage will not be executed. In this case, the filter continues to operate without correction using the prediction stage. The total state of the navigation and its covariance matrix are predicted with sampling frequency of inertial sensors according to Equations (12) and (13) with or without the correction stage.

On receiving of a new auxiliary signal y_k, the navigation state ${\hat {\bf x}}^ + $ and its associated covariance matrix ${\bf P}^ + $ at time step k are corrected using Equations (14)–(19) and the unscented-based integration method expressed by Equations (20)–(22).

In the best case, where all of the auxiliary sensors are available, the measurement vector y and the matrix R are given as follows:

(80)$${\bf y} = \left[ {Z_m\comma\ v_x\comma\ v_y\comma\ v_z\comma\ \phi _m\comma\ \theta _m\comma\ \psi _m} \right]^T $$

(81)$${\bf R} = \left[ {\matrix{ {\sigma _z^2} \hfill & 0 \hfill & 0 \hfill & 0 \hfill & 0 \hfill & 0 \hfill & 0 \hfill \cr 0 \hfill & {\sigma _{v_x} ^2} \hfill & 0 \hfill & 0 \hfill & 0 \hfill & 0 \hfill & 0 \hfill \cr 0 \hfill & 0 \hfill & {\sigma _{v_y} ^2} \hfill & 0 \hfill & 0 \hfill & 0 \hfill & 0 \hfill \cr 0 \hfill & 0 \hfill & 0 \hfill & {\sigma _{v_z} ^2} \hfill & 0 \hfill & 0 \hfill & 0 \hfill \cr 0 \hfill & 0 \hfill & 0 \hfill & 0 \hfill & {\sigma _\phi ^2} \hfill & 0 \hfill & 0 \hfill \cr 0 \hfill & 0 \hfill & 0 \hfill & 0 \hfill & 0 \hfill & {\sigma _\theta ^2} \hfill & 0 \hfill \cr 0 \hfill & 0 \hfill & 0 \hfill & 0 \hfill & 0 \hfill & 0 \hfill & {\sigma _\psi ^2} \hfill \cr}} \right]$$

The dimensions of the correction parameters are determined based on the number of received auxiliary sensors. When one of the sensors is not received, the matrices associated with that sensor, which was given in Section 3.2, are removed from the correction parameters. The measurement noise matrix, R, is determined by stationary measurements of the auxiliary sensors. However, due to external factors such as environmental vibration, the effective noise levels may be changed. Therefore, in order to obtain the optimal estimates, it is required to tune R (Groves, Reference Groves2008). The flowchart of the unscented based integrated navigation system is illustrated in Figure 3.

Figure 3.

Strap-down integrated navigation system based on unscented based integration method.

4. EXPERIMENTAL RESULTS

In order to evaluate the performance of the proposed system, a lake test was accomplished. An instrumented vessel was utilised for the experiments as shown in Figure 4. The instruments used in the test include a fibre optic gyro IMU assembled by Sepahan Electronic Inc. with model Sarmad2, a DVL made by Jay Technology Inc., model IR/BQN_01 and a GPS receiver (NovAtelOEMV-1) installed on the vessel according to Figure 5. The GPS receiver is a single-frequency GPS receiver and its measurements were used only for position initialisation and reference purposes.

Figure 4.

Instrumented vessel for experiments.

Figure 5.

Instruments mounted on the vessel.

To evaluate the proposed approach, the inertial signals are incorporated with measurements of the DVL, depth meter, roll and pitch angles. The position estimated by the proposed approach is compared with a position reference. In order to have an accurate position reference, two similar GPS receivers were used as a simple differential GPS. The first GPS receiver was installed at the origin point and the second one on the vessel and the position reference was calculated by subtracting smoothed data of the two GPS receivers. Our measurements showed that the accuracy of the calculated position reference was lower than 2 metres. As shown in Figure 5, a stand was used for holding the DVL under the water. The specifications of the instruments used can be found in Table 1 (Novatel, 2010).

Table 1.

Instrument specifications.

In this paper, a trajectory composed of both the pseudo-sinusoid manoeuvres and the approximately straight track is used to show the performance of the designed system. The signals measured by the instruments are logged by a computer carried by the vessel and then post-processing of the collected data is carried out in the laboratory. To verify the accuracy of the proposed system, its estimated position is compared with that of the ESKF-based integrated navigation system (Lee et al., Reference Lee, Jun, Kim, Lee, Aoki and Hyakudome2007; Miller et al., Reference Miller, Farrell, Zhao and Djapic2010; Shabani et al., Reference Shabani, Gholami, Davari and Emami2013).

According to Equations (8) and (9) and parameters given in Table 1, the matrix Q_c is considered as follows:

(82)$${\bf Q}_c = \left( {\matrix{ {\left( {9\!\cdot\!8 \times 10^{ - 5}} \right)^2 {\bf I}_3} & {{\bf 0}_3} & {{\bf 0}_3} & {{\bf 0}_3} \cr {{\bf 0}_3} & {\left( {2\!\cdot\!91 \times 10^{ - 4}} \right)^2 {\bf I}_3} & {{\bf 0}_3} & {{\bf 0}_3} \cr {{\bf 0}_3} & {{\bf 0}_3} & {\left( {2\!\cdot\!45 \times 10^{ - 4}} \right)^2 {\bf I}_3} & {{\bf 0}_3} \cr {{\bf 0}_3} & {{\bf 0}_3} & {{\bf 0}_3} & {\left( {4\!\cdot\!85 \times 10^{ - 7}} \right)^2 {\bf I}_3} \cr}} \right)$$

The measurement covariance matrix is also considered as follows:

(83)$${\bf R} = \left[ {\matrix{ {\left( {0\!\cdot\!1} \right)^2} & 0 & 0 & 0 & 0 & 0 & 0 \cr 0 & {\left( {0\!\cdot\!01} \right)^2} & 0 & 0 & 0 & 0 & 0 \cr 0 & 0 & {\left( {0\!\cdot\!01} \right)^2} & 0 & 0 & 0 & 0 \cr 0 & 0 & 0 & {\left( {0\!\cdot\!01} \right)^2} & 0 & 0 & 0 \cr 0 & 0 & 0 & 0 & {\left( {0\!\cdot\!001} \right)^2} & 0 & 0 \cr 0 & 0 & 0 & 0 & 0 & {\left( {0\!\cdot\!001} \right)^2} & 0 \cr 0 & 0 & 0 & 0 & 0 & 0 & {\left( {0\!\cdot\!1} \right)^2} \cr}} \right]$$

In Figure 6, the estimated positions in the horizontal (e.g. north and east) direction are shown. Figure 7 also shows the absolute error curves. Absolute error is expressed as the absolute value of the difference between the reference and estimated positions. In Figures 8 and 9, the curves of the estimated position absolute errors in the directions of the east and north are depicted, respectively. These curves are defined as the absolute value of the difference between the estimated position in the east (or north) direction and the GPS position in that direction.

Figure 6.

Estimated positions.

Figure 7.

Estimation error.

Figure 8.

Estimated position errors in the east direction.

Figure 9.

Estimated position errors in the north direction.

Based on these results, the positions estimated by both approaches pursue the reference position with a bounded error. However, due to utilising the more accurate integration method in the proposed navigation system, its estimation error is less than that of the ESKF approach.

In this paper, the Root Mean Square Error (RMSE) and the relative error criteria are used for quantitatively evaluating the performance of the system which is calculated as follows:

(84)$$\eqalign{&{SE_{\rm k} = ( {R_{xk} - E_{xk}} )^2 + ( {R_{yk} - E_{yk}} )^2 + ( {R_{zk} - E_{zk}} )^2} \cr & \qquad\quad {{\rm RMSE} = {\rm sqrt}[ {{\rm mean}( {SE_k} )} ]}} $$

(85)$${\rm Relative}\,{\rm RMSE} = \displaystyle{{{\rm RMSE}} \over {{\rm Travelled}\,{\rm Distance}}} \times 100$$

Where R _i and E _i are the reference and estimated position along the corresponding axis. In this test where the vessel travelled approximately 4897 metres for nearly 42 minutes, the RMSE values of the position estimated by the proposed system and ESKF approach are 4·3 m and 6·5 m and relative errors are 0·09% and 0·13% of the travelled distance, respectively. The maximum position error for the proposed system is 9·3 m, while this value for the ESKF approach is 16·1 m. The RMSE values of the estimated position in the directions of the east and north for the proposed system are 3·26 m and 2·8 m, whereas these values for ESKF approach are 3·95 m and 5·2 m. Therefore, in accordance with the obtained results, the unscented-based integrated navigation system estimates the vehicle position more accurately than the conventional ESKF approach. The results obtained from the practical test for travelled trajectory are summarised in Table 2.

Table 2.

Performance comparison of the proposed system and conventional ESKF approach.