2.1. State equation

The state equation is established as follows:

(2.1)\begin{equation}\boldsymbol{X^{\prime}} = \boldsymbol{FX} + \boldsymbol{GW}\end{equation}

where $\boldsymbol{X}$ is the state vector, $\boldsymbol{F}$ is the state transition matrix, $\boldsymbol{G}$ is the system noise matrix and $\boldsymbol{W}$ is the process noise vector. The state vector $\boldsymbol{X}$ is defined as (Hu et al., Reference González-García, Gómez-Espinosa, Cuan-Urquizo, García-Valdovinos, Salgado-Jiménez and Cabello2019):

(2.2)\begin{equation}\begin{array}{ccccc} \boldsymbol{X} & =\left[ {\begin{array}{* {20}{l}} {{\phi_x}}&{{\phi_y}}&{{\phi_z}}&{\delta V_E^n}&{\delta V_N^n}&{\delta V_U^n}&{\delta \lambda }&{\delta L}&{\delta h}&{{\varepsilon_x}} \end{array}} \right.\\ & {\left. {\begin{array}{* {20}{l}} {{\varepsilon_y}}&{{\varepsilon_z}}&{{\mathrm{\nabla }_x}}&{{\mathrm{\nabla }_y}}&{{\mathrm{\nabla }_z}}&{{K_d}} \end{array}} \right]^{^T}} \end{array}\end{equation}

where ${K_d}$ is the DVL scale factor, and the matrix $\boldsymbol{F}$, $\boldsymbol{G}$, and $\boldsymbol{W}$ are expressed as follows:

(2.3)\begin{equation}\boldsymbol{F} = \left[ {\begin{array}{* {20}{c}} {{\boldsymbol{F}_{aa}}}&{{\boldsymbol{F}_{av}}}&{{\boldsymbol{F}_{ap}}}&{ - \boldsymbol{C}_b^n}&{{\boldsymbol{0}_{3 \times 3}}}&{}\\ {{\boldsymbol{F}_{va}}}&{{\boldsymbol{F}_{vv}}}&{{\boldsymbol{F}_{vp}}}&{{\boldsymbol{0}_{3 \times 3}}}&{\boldsymbol{C}_b^n}&{{\boldsymbol{0}_{9 \times 1}}}\\ {{\boldsymbol{0}_{3 \times 3}}}&{{\boldsymbol{F}_{pv}}}&{{\boldsymbol{F}_{pp}}}&{\boldsymbol{C}_b^n}&{{\boldsymbol{0}_{3 \times 3}}}&{}\\ {}&{}&{{\boldsymbol{0}_{7 \times 15}}}&{}&{}&{} \end{array}} \right],\;\boldsymbol{G} = \left[ {\begin{array}{* {20}{c}} { - \boldsymbol{C}_b^n}&{{\boldsymbol{0}_{3 \times 3}}}\\ {{\boldsymbol{0}_{3 \times 3}}}&{\boldsymbol{C}_b^n}\\ {{\boldsymbol{0}_{1\textrm{0} \times \textrm{3}}}}&{{\boldsymbol{0}_{1\textrm{0} \times \textrm{3}}}} \end{array}} \right],\;\boldsymbol{W} = \left[ {\begin{array}{* {20}{c}} {\boldsymbol{w}_g^b}\\ {\boldsymbol{w}_a^b} \end{array}} \right]\end{equation}

where ${\boldsymbol{F}_{aa}},{\boldsymbol{F}_{av}},{\boldsymbol{F}_{ap}},{\boldsymbol{F}_{va}},{\boldsymbol{F}_{pv}},{\boldsymbol{F}_{vv}},{\boldsymbol{F}_{pp}},{\boldsymbol{F}_{vp}}$ are determined by the error equation of SINS, $\boldsymbol{C}_b^n$ is the transfer matrix from frame b to frame n, and $\boldsymbol{w}_g^b$and $\boldsymbol{w}_a^b$ are the error of gyroscope and accelerometer, respectively.

2.2. Measurement equation

The measurement equation is modelled as follows (Lee et al., Reference Huang and Zhang2005):

(2.4)\begin{equation}\boldsymbol{Z} = {\hat{\boldsymbol{V}}_b} - {\boldsymbol{V}_{bDVL}}= \boldsymbol{HX} + \boldsymbol{V}\end{equation}

where $\boldsymbol{Z}$ is the measured matrix, ${\hat{\boldsymbol{V}}_b}$ is the velocity calculated by SINS in the body frame, ${\boldsymbol{V}_{bDVL}}$ is the velocity provided by DVL, $\boldsymbol{H}$ is the measurement transfer matrix and $\boldsymbol{V}$ is the measurement information noise. The measurement transfer matrix is:

(2.5)\begin{equation}\boldsymbol{H} = \left[ {\begin{array}{* {20}{c}} {\begin{array}{* {20}{c}} {{\boldsymbol{C}_{31}}{V_N} - {\boldsymbol{C}_{21}}{V_U}}&{{\boldsymbol{C}_{11}}{V_U} - {\boldsymbol{C}_{31}}{V_E}}&{{\boldsymbol{C}_{21}}{V_E} - {\boldsymbol{C}_{11}}{V_N}}\\ {{\boldsymbol{C}_{32}}{V_N} - {\boldsymbol{C}_{22}}{V_U}}&{{\boldsymbol{C}_{12}}{V_U} - {\boldsymbol{C}_{32}}{V_E}}&{{\boldsymbol{C}_{22}}{V_E} - {\boldsymbol{C}_{12}}{V_N}}\\ {{\boldsymbol{C}_{33}}{V_N} - {\boldsymbol{C}_{23}}{V_U}}&{{\boldsymbol{C}_{13}}{V_U} - {\boldsymbol{C}_{33}}{V_E}}&{{\boldsymbol{C}_{23}}{V_E} - {\boldsymbol{C}_{13}}{V_N}} \end{array}}&{\boldsymbol{C}_n^b}&{{\boldsymbol{0}_{\textrm{3} \times \textrm{9}}}}&{\begin{array}{* {20}{c}} { - {V_{x\_bDVL}}}\\ { - {V_{y\_bDVL}}}\\ { - {V_{z\_bDVL}}} \end{array}} \end{array}} \right]\end{equation}

3.1. AKF with noise measurement estimator based on IAE and RAE

First, the dynamic system is modelled as:

(3.1)\begin{equation}\left\{ \begin{array}{l} {\boldsymbol{X}_k} = {{\bf \Phi }_{k,k - 1}}{\boldsymbol{X}_{k - 1}} + {{\bf \Gamma }_{k - 1}}{\boldsymbol{W}_{k - 1}}\\ {\boldsymbol{Z}_k} = {\boldsymbol{H}_k}{\boldsymbol{X}_k} + {\boldsymbol{V}_k} \end{array} \right.\end{equation}

where ${\boldsymbol{X}_k}$ is the state vector, ${{\bf \Phi }_{k,k - 1}}$ is the one-step transfer matrix from epoch k-1 to k, ${{\bf \Gamma }_{k - 1}}$ is the system noise matrix, ${\boldsymbol{W}_{k - 1}}$ is the system noise, ${\boldsymbol{Z}_k}$ is the measurement vector, ${\boldsymbol{H}_k}$ is the measurement matrix, and ${\boldsymbol{V}_k}$ is the measurement noise. ${\boldsymbol{W}_{k - 1}}$ and ${\boldsymbol{V}_k}$ are supposed to be uncorrelated zero-mean Gaussian white noise sequences with the statistical characteristics:

(3.2)\begin{equation}\left\{ \begin{array}{l} E[{{\boldsymbol{W}_k}} ]= {\boldsymbol{q}_k},Cov[{{\boldsymbol{W}_k},{\boldsymbol{W}_j}} ]= {\boldsymbol{Q}_k}{\boldsymbol{\delta }_{kj}}\\ E[{{\boldsymbol{V}_k}} ]= {\boldsymbol{r}_k},Cov[{{\boldsymbol{V}_k},{\boldsymbol{V}_j}} ]= {\boldsymbol{R}_k}{\boldsymbol{\delta }_{kj}}\\ Cov[{{\boldsymbol{W}_k},{\boldsymbol{V}_j}} ]= 0 \end{array} \right.\end{equation}

where ${\boldsymbol{q}_k}$ is the mean of the process noise, ${\boldsymbol{Q}_k}$ is the covariance of the process noise, ${\boldsymbol{r}_k}$ is the mean of the measurement noise, ${\boldsymbol{R}_k}$ is the covariance of the measurement noise and ${\boldsymbol{\delta }_{kj}}$ is the Kronecker data function. The conventional KF is described as follows:

(3.3)\begin{align}{\boldsymbol{X}_{k,k - 1}} &= {{\bf \Phi }_{k - 1}}{\hat{\boldsymbol{X}}_{k - 1}}\end{align}

(3.4)\begin{align}{\boldsymbol{P}_{k,k - 1}} &= {{\bf \Phi }_{k - 1}}{\boldsymbol{P}_{k - 1}}{\bf \Phi }_{k - 1}^T + {{\bf \Gamma }_{k - 1}}{\boldsymbol{Q}_{k - 1}}{\bf \Gamma }_{k - 1}^T\end{align}

(3.5)\begin{align}{\boldsymbol{K}_k} &= {\boldsymbol{P}_{k,k - 1}}{\textbf{H}}_k^T{({{{\textbf{H}}_k}{\boldsymbol{P}_{k,k - 1}}{\textbf{H}}_k^T + {\boldsymbol{R}_k}} )^{ - 1}}\end{align}

(3.6)\begin{align}{\hat{\boldsymbol{X}}_k} &= {\hat{\boldsymbol{X}}_{k,k - 1}} + {\boldsymbol{K}_k}({{{\bf Z}_k} - {\boldsymbol{H}_k}{{\hat{\boldsymbol{X}}}_{k,k - 1}}} )\end{align}

(3.7)\begin{align}{\boldsymbol{P}_k} &= ({\boldsymbol{I} - {\boldsymbol{K}_k}{\boldsymbol{H}_k}} ){\boldsymbol{P}_{k,k - 1}}\end{align}

where ${\boldsymbol{X}_{k,k - 1}}$ is the predicted state estimate, ${\boldsymbol{P}_{k,k - 1}}$ is the predicted estimate covariance, ${\boldsymbol{P}_k}$ is the updated estimate covariance and ${\boldsymbol{K}_k}$ is the Kalman gain matrix.

3.1.1. AKF with noise measurement estimator based on IAE

An AKF with measurement noise estimator based on IAE can be driven as follows:

Defining the innovation as:

(3.8)\begin{equation}{\boldsymbol{\varepsilon }_k} = {\boldsymbol{Z}_k} - {\boldsymbol{H}_k}{\hat{\boldsymbol{X}}_{k,k - 1}}\end{equation}

Using Equation (3.1), it can be directly driven that,

(3.9)\begin{equation}{\boldsymbol{\varepsilon }_k} = {\boldsymbol{H}_k}{\boldsymbol{X}_k} + {\boldsymbol{v}_k} - {\boldsymbol{H}_k}{\hat{\boldsymbol{X}}_{k,k - 1}} = {\boldsymbol{H}_k}({{\boldsymbol{X}_k} - {{\hat{\boldsymbol{X}}}_{k,k - 1}}} )+ {\boldsymbol{v}_k}\end{equation}

Calculating the covariance of innovation:

(3.10)\begin{align} E({\boldsymbol{\varepsilon }_k}{\boldsymbol{\varepsilon }_k}^T) & = E\Big\{{ {\Big\{{{\boldsymbol{H}_k}({{\boldsymbol{X}_k} - {{{\hat{X}}}_{k,k - 1}}} )+ {{v}_k}} \Big\}{{\Big\{{{{H}_k}\Big({{{X}_k} - {{{\hat{X}}}_{k,k - 1}}} \Big)+ {{v}_k}} \Big\}}^T}} \Big\}} \notag\\ & = E\Big\{{{{H}_k}\Big({{{X}_k} - {{{\hat{X}}}_{k,k - 1}}} \Big){{\Big({{{X}_k} - {{{\hat{X}}}_{k,k - 1}}} \Big)}^T}{{H}_k}^T + {{v}_k}{{v}_k}^T} \Big\}\notag\\ & = E\Big\{{{{H}_k}\Big({{{X}_k} - {{{\hat{X}}}_{k,k - 1}}} \Big){{\Big({{{X}_k} - {{{\hat{X}}}_{k,k - 1}}} \Big)}^T}{{H}_k}^T} \Big\}+ E\{{{{v}_k}{{v}_k}^T} \}\notag\\ & = {{H}_k}{{P}_{k,k - 1}}{{H}_k}^T + {{R}_k} \end{align}

Accordingly, the covariance of measurement noise can be described as:

(3.11)\begin{equation}{{R}_k} = E({{\varepsilon }_k}{{\varepsilon }_k}^T) - {{H}_k}{{P}_{k,k - 1}}{{H}_k}^T\textrm{ = }\frac{\textrm{1}}{k}\sum\limits_{i = 1}^k {\Big\{{{{\varepsilon }_i}{{\varepsilon }_i}^T} \Big\}} - {{H}_k}{{P}_{k,k - 1}}{{H}_k}^T\end{equation}

3.1.2. AKF with noise measurement estimator based on RAE

An AKF with measurement noise estimator based on RAE can be driven as follows:

Defining the residual as:

(3.12)\begin{equation}{{\gamma }_k} = {\boldsymbol{Z}_k} - {\boldsymbol{H}_k}{\hat{\boldsymbol{X}}_k}\end{equation}

Using Equations (3.6) and (3.8), it can be directly obtained that,

(3.13)\begin{equation}{\boldsymbol{\gamma }_k} = {\boldsymbol{Z}_k} - {\boldsymbol{H}_k}({\boldsymbol{\mathord{\buildrel{\lower3pt\hbox{$\scriptscriptstyle\frown$}} \over X} }_{k,k - 1}} + {\boldsymbol{K}_k}{\boldsymbol{\varepsilon }_k}) = {\boldsymbol{Z}_k} - {\boldsymbol{H}_k}{\boldsymbol{\mathord{\buildrel{\lower3pt\hbox{$\scriptscriptstyle\frown$}} \over X} }_{k,k - 1}} - {\boldsymbol{H}_k}{\boldsymbol{K}_k}{\boldsymbol{\varepsilon }_k} = (\boldsymbol{I} - {\boldsymbol{H}_k}{\boldsymbol{K}_k}){\boldsymbol{\varepsilon }_k}\end{equation}

Calculating the covariance of residual by the error propagation law (Wang, Reference Sabet, Mohammadi Daniali, Fathi and Alizadeh1999):

(3.14)\begin{equation}E({{\gamma }_k}{{\gamma }_k}^T) = {{R}_k} - {{H}_k}{{P}_k}{{H}_k}^T\end{equation}

Accordingly, the covariance of measurement noise can be described as:

(3.15)\begin{equation}{{R}_k} = E({{\gamma }_k}{{\gamma }_k}^T) + {{H}_k}{{P}_k}{{H}_k}^T = \frac{\textrm{1}}{k}\sum\limits_{i = 1}^k {\Big\{{{{\gamma }_i}{{\gamma }_i}^T} \Big\}} + {{H}_k}{{P}_k}{{H}_k}^T\end{equation}

By the way, Equation (3.15) can also be obtained via maximum likelihood criterion (Mohamed and Schwarz, Reference Lee, Lee, Hong, Kim and Seong1999). Comparing the two methods, subtraction in the IAE method inevitably results in negative definite R_k and then leads the filter to diverge. For this reason, the RAE method is more reliable and powerful in practical application. The measurement noise estimator in this paper is based on the RAE method.

3.2. AKF based on measurement noise estimator

It is noted that the measurement noise estimator based on IAE or RAE calculates R_k by the average of the innovations or residuals over all epochs. The average of the historical information may result in estimation latency. For example, when the measurement noise occurs in the hundredth epoch, the influence of the residual in the hundredth epoch to the system is merely 1% as the same as the epoch 1 to 99. The residuals which approach zero at the epoch 1 to 99 lead to sluggish and biased measurement noise estimation. From this point of view, it is necessary to reduce the weight of the historical epochs. Therefore, MW and EWMA are introduced.

The MW method employs a moving window to store the residuals and calculates the average of them for R_k. The EWMA method averages the residuals over all epochs with the exponential weight. The principles of the two methods are shown in Figure 2. The colour depth represents the average weight of the residual. The darker the colour, the greater the weight. It can be seen that R_k in the MW method is the average of the residuals in the moving window and in the EWMA method the closer to the current epoch, the greater the weight.

Figure 2.

(a) Diagram of MW method. (b) Diagram of EWMA method

3.2.1 Measurement noise estimator based on MW

The MW method estimates the measurement noise matrix through computing the average of the residuals in a moving window, which effectively eliminates the influence of the historical information. The general equation of MW is:

(3.16)\begin{equation}{\hat{\boldsymbol{R}}_k} = \frac{1}{m}\sum\limits_{i = k - m + 1}^k {\Big\{{{\boldsymbol{\gamma }_i}\boldsymbol{\gamma }_i^T\textrm{ + }{\boldsymbol{H}_i}{\boldsymbol{P}_i}\boldsymbol{H}_i^T} \Big\}}\end{equation}

where m is the width of the moving window, and k is the current epoch.

To enhance the operation efficiency, Equation (3.16) can be expressed as:

(3.17)\begin{equation}{\hat{\boldsymbol{R}}_k} = {\hat{\boldsymbol{R}}_{k - 1}} + \frac{1}{m}\Big\{{\Big({{\boldsymbol{\gamma }_k}\boldsymbol{\gamma }_k^T\textrm{ + }{\boldsymbol{H}_k}{\boldsymbol{P}_k}\boldsymbol{H}_k^T} \Big)- \Big({{\boldsymbol{\gamma }_{k - m}}\boldsymbol{\gamma }_{k - m}^T\textrm{ + }{\boldsymbol{H}_{k - m}}{\boldsymbol{P}_{k - m}}\boldsymbol{H}_{k - m}^T} \Big)} \Big\}\end{equation}

The optimal width of the moving window is determined by the dynamic characteristic of the environment. In highly dynamic circumstances, the width of the moving window is required to be small to detect changes sensitively. Conversely, in low dynamic circumstances, the width of the moving window is required to be great to improve the stability of the system.

3.2.2. Measurement noise estimator based on EWMA

R_k of the EWMA method is estimated with the exponential weighted average of the residuals over all epochs. The closer to the current epoch, the greater the weight. It effectively reduces the undesirable influence of the historical sequence. The general equation of EWMA is (Sun et al., Reference Narasimhappa, Nayak, Terra and Sabat2016; Narasimhappa et al., Reference Liu, Zhao, Sun, Wu, Zhu and Zhang2018):

(3.18)\begin{equation}\begin{array}{l} {{\hat{\boldsymbol{R}}}_k} = (1 - {d_k}){{\hat{\boldsymbol{R}}}_{k - 1}} + {d_k}({\boldsymbol{\gamma }_k}\boldsymbol{\gamma }_k^T\textrm{ + }{\boldsymbol{H}_k}{\boldsymbol{P}_k}\boldsymbol{H}_k^T)\\ {d_k} = \dfrac{{1 - b}}{{1 - {b^{k + 1}}}} \end{array}\end{equation}

where k is the time step, d_k is the forgetting factor, b is a constant, b∈(0⋅9,1].

It can be seen that the EWMA method estimates R_k by the residual of the current epoch and the calculated R_{k- 1} of the previous epoch. Without the space to store the residuals of historical epochs and complex computing, EWMA is more applicable to implementation. The principle of EWMA is illustrated as follows:

To calculate the measurement noise covariance matrix R_k in the k-th epoch with the exponential weighted average of the residuals over the whole historical epochs, the recursive formula is given:

(3.19)\begin{equation}{\hat{\boldsymbol{R}}_k} = ({1 - d} ){\hat{\boldsymbol{R}}_{k - 1}} + d{\hat{\boldsymbol{R}}'_k}\end{equation}

where $\hat{\boldsymbol{R}}^{\prime}_{k} = {\boldsymbol{\gamma }_k}\boldsymbol{\gamma }_k^T + {\boldsymbol{H}_k}{\boldsymbol{P}_k}\boldsymbol{H}_k^T$.

With the recursive formula, ${\hat{\boldsymbol{R}}_k}$ can be expanded as:

(3.20)\begin{equation}\begin{array}{l} {{\hat{\boldsymbol{R}}}_1} = ({1 - d} ){{\hat{\boldsymbol{R}}}_\textrm{0}} + d{{\hat{\boldsymbol{R}}}'_\textrm{1}} \\ {{\hat{\boldsymbol{R}}}_2} = ({1 - d} ){{\hat{\boldsymbol{R}}}_1} + d{{\hat{\boldsymbol{R}}}'_2} \textrm{ = }{({1 - d} )^2}{{\hat{\boldsymbol{R}}}_\textrm{0}} + ({1 - d} )d{{\hat{\boldsymbol{R}}}'_\textrm{1}} + d{{\hat{\boldsymbol{R}}}'_\textrm{2}} \\ {{\hat{\boldsymbol{R}}}_3} = ({1 - d} ){{\hat{\boldsymbol{R}}}_2} + d{{\hat{\boldsymbol{R}}}'_3} = {({1 - d} )^\textrm{3}}{{\hat{\boldsymbol{R}}}_\textrm{0}}\textrm{ + }{({1 - d} )^2}d{{\hat{\boldsymbol{R}}}'_1} + ({1 - d} )d{{\hat{\boldsymbol{R}}}'_2} + d{{\hat{\boldsymbol{R}}}'_3} \\ \ldots \\ {{\hat{\boldsymbol{R}}}_k} = ({1 - d} ){{\hat{\boldsymbol{R}}}_{k - 1}} + d{{\hat{\boldsymbol{R}}}'_k} = {({1 - d} )^k}{{\hat{\boldsymbol{R}}}_\textrm{0}} + {({1 - d} )^{k - \textrm{1}}}d{{\hat{\boldsymbol{R}}}'_\textrm{1}} + {({1 - d} )^{k - 2}}d{{\hat{\boldsymbol{R}}}'_2} + \cdots + d{{\hat{\boldsymbol{R}}}'_k} \end{array}\end{equation}

where ${\hat{\boldsymbol{R}}_\textrm{0}}$ is set from experience and d is the forgetting factor, d∈(0,0⋅1]. It can be seen in the last equation of Equation (3.20) that R_k is the exponential weighted average over the historical epochs. Regretably, the multiplier d for the exponential item is missed in the first coefficient. For clarity, the weight of historical epoch in the 200th epoch (k = 200) is shown in Figure 3(a). It can be seen that the weight increases exponentially, but there is a conspicuous bias in the initial epoch. Consequently, the weight of the initial epoch is bigger than others. To address this problem, bias correction is introduced:

(3.21)\begin{equation}{d_k} = \frac{{1 - b}}{{1 - {b^{k + 1}}}},\;b \in [0.9,1)\end{equation}

Figure 3.

(a) Weight in each epoch before bias correction. (b) d_k in bias correction. (c) Weight in each epoch after bias correction. (d) Relation between weight in each epoch and b.

The value of d_k is shown in Figure 3(b). As k increases, d_k decreases from a large value to 1–b. When k = 1, 1/ d_k is a small value. In consequence, the weight of the first epoch is reduced and the bias is corrected. The weight of each epoch with bias correction is shown in Figure 3(c). Compared with Figure 3(a), the bias is distinctly corrected.

Different b represents the different average performance. Figure 3(d) shows the relation of weight and b. It can be seen that the smaller b, the bigger the weight of the current epoch and the smaller the weight of the historical epochs. In highly dynamic circumstances, b is required to decrease to immediately detect changes. Conversely, in low dynamic circumstances, b is required to increase to improve the stability of the system. Therefore, an improved EWMA based on the adaptive b is proposed in the next section.

3.2.3. Measurement noise estimator based on IEWMA

In order to estimate the variable measurement noises in different dynamic environments and improve navigation accuracy, the measurement noise estimator based on IEWMA with adaptive forgetting factor is proposed in this paper.

Conceptually, the discrepancy between the residual covariance estimation and the theoretical residual covariance can be employed to evaluate the stability of the measurement noise estimator (Gao et al., Reference Gao, Gao, Hu, Zhong and Gu2015). The estimation convergence criterion can be represented as:

(3.22)\begin{equation}\Big|{{\boldsymbol{\gamma }^T}\boldsymbol{\gamma } - \kappa tr({{\hat{R}}_k} - {H_k}{P_{k,k - 1}}H_k^T)} \Big|\left\{ {\begin{array}{* {20}{l}} { \gt 0}&{\textrm{unstable}}\\ { = 0}&{\textrm{stable}} \end{array}} \right.\end{equation}

Where $\kappa$ is constant and determined by the empirical knowledge.

According to the analysis in section 3.2, when the measurement noise is stable, the forgetting factor b is required to be large, whereas when the measurement noise is unstable, the forgetting factor b is required to be small. When b = 0⋅9, R_k is approximately the average over the 10 epochs before the current epoch. When b = 0⋅98, R_k is approximately the average over the 50 epochs, and it is the average over the whole epochs when b approaches 1. Empirically, it is appropriate to calculate R_k according to the average over 10 more epochs. Therefore, $b \in [0.9,1)$ is supposed in the paper. Accordingly, in this paper the adaptive b is defined as:

(3.23)\begin{equation}b = 0.9 + 0.1{e^{ - |{{\boldsymbol{\gamma }^T}\boldsymbol{\gamma } - \kappa tr({{\hat{R}}_k} - {H_k}{P_{k,k - 1}}H_k^T)} |}}\end{equation}

where $\kappa$ is constant and $\kappa \textrm{ = 5}$.

Figure 4 shows the relation between b and the exponential term. It can be seen that, with the increasing exponential term, the value of b increases from 0⋅9 to 1. In high dynamic circumstances, the exponential term is large and b approaches 0⋅9. Conversely, in low dynamic circumstances, the exponential term goes to zero and b approaches 1.

Figure 4.

Relation between b and the exponential term

The superiority of the AKF based on IEWMA with the adaptive forgetting factor is listed as follows:

1. 1. It is adaptable for various dynamic characteristics.

2. 2. It can be applied to engineering practice without multiple experiences.

3. 3. The estimation result is more stable in low dynamic circumstances and more sensitive in highly dynamic circumstances.

4.1. Simulation

A trajectory lasting for about 1,000 s with three straight lines and two curves is simulated. The simulated trajectory and vehicle dynamic characteristics are shown in Figures 5 and 6. The start point is set as latitude $34^\circ \; \textrm{N}$ and longitude $108^\circ \; \textrm{E}$. The initial attitude is set as [0°, 0°, −90°]. The initial attitude error is set as [0⋅1°, 0⋅1°, 0⋅5°]. The drift bias and the random walk noise of the accelerometer are set as $50\,\mathrm{\mu g}$ and $50\,\mathrm{\mu g}/\sqrt {\textrm{Hz}} $, and those of the gyroscope are set as $0.01^\circ /\sqrt h $ and $0.01^\circ /\sqrt h $. The DVL scale factor is set as 0⋅005. The update rates of the IMU and DVL are set as 200 Hz and 1 Hz, respectively.

Figure 5.

Trajectory of the vehicle in simulation

Figure 6.

Dynamic characteristics of vehicle in simulation

In order to compare the performances of these methods, it is simulated that a graded noise with the sine function of 5 m/s occurs from 750 s to 900 s whereas the noise at other times is zero-mean white noise with standard deviation of 0⋅1 m/s. Fairly, the optimal window length was found to be 50, and the forgetting factor b = 0⋅97 after quite a few experiments to compare with the proposed method. Meanwhile, the conventional KF was also carried out to emphasise the performance of the proposed method. The comparisons of attitude, velocity and position errors of the four algorithms are shown in Figures 7–9, where the blue line, red line, green line and orange line represent KF, MW, EWMA and IEWMA, respectively. When noise occurs, the conventional KF immediately diverges. Conversely, with the measurement noise estimator, other methods are relatively stable. Looking at the enlarged drawing, we can find that the errors of IEWMA method are more stable and smaller than the others.

Figure 7.

Attitude errors of four methods in simulation

Figure 8.

Velocity errors of four methods in simulation

Figure 9.

Position errors of four methods in simulation

In order to compare intuitively the performances of the four methods, the horizontal position errors of KF and AKF with the measurement noise estimators of MW, EWMA and IEWMA are shown in Figure 10. It can be seen that the horizontal position error of IEWMA is more stable and smaller than the others.

Figure 10.

Horizontal position errors of four methods in simulation

Quantitative analysis was carried out on the navigation errors via the mean representing the size of the errors and the root mean square (RMS) representing the stability of the errors. The mean and RMS of the attitude, velocity and position errors are shown in Tables 1 and 2, respectively. The mean and RMS of horizontal position errors in the four methods are shown in Table 3.

Table 1.

Mean of attitude, velocity and position errors in four methods

Table 2.

RMS of attitude, velocity and position errors in four methods

Table 3.

Mean and RMS of horizontal position errors in four methods

From Table 1, we can see that the mean of velocity and position errors in IEWMA is smaller than the others. The differences of attitude error in MW, EWMA and IEWMA are negligible. Meanwhile, the RMS of all errors in the IEWMA method is smaller than the others in Table 2. It is also obvious that the mean and RMS of horizontal position error in IEWMA are smaller than the others in Table 3. Therefore, it is concluded that IEWMA outperforms MW and EWMA in both navigation accuracy and stability.

As mentioned, performance of the integrated system depends on the accuracy of the estimated R_k. The estimated R_k is shown in Figure 11. As anticipated, R_k of IEWMA is the nearest to the true value. On the one hand, R_k of IEWMA is more stable than the others in 0 s to 600 s. This is because, in cases where the measurement noise is stable, adaptive forgetting factor b becomes as small as possible. Accordingly, the weight of the current epoch decreases and the weight of historical sequence increases. On the other hand, R_k of IEWMA is more susceptible than others in 750 s to 900 s. Similarly, when noise occurs, adaptive forgetting factor b becomes as large as possible. Accordingly, the weight of the current epoch increases and the weight of the historical sequence decreases. The undesirable influence of historical sequence in IEWMA is eliminated. However, R_k of MW and EWMA is still influenced by the historical sequence and it is hysteretic to the volatile noise.

Figure 11.

Diagram of estimated R_k in simulation

4.2. Vehicle test

The proposed method was evaluated in land vehicle field testing to predict the feasibility of its operation in underwater environments. In the SINS/DVL navigation system used in the vehicle test, the inertial information is provided by the IMU and the velocity information of DVL is replaced by the PHINS which is developed by French firm IXBLU. PHINS transforms its own velocity from navigation frame to body frame using the true attitude information to provide the DVL information. A computer was utilised to perform a series of navigation operations. The installation structure is shown in Figure 12. The specifications of the IMU and PHINS are listed in Table 4.

Figure 12.

Installation structure for land trial

Table 4.

Specifications of IMU and PHINS

The land trial was conducted near 31° 88′N, 118° 82′E, on the campus of Southeast University. The vehicle trajectory, lasting for 1,200 s, is shown in Figure 13 and vehicle dynamic characters are shown in Figure 14.

Figure 13.

Trajectory of the vehicle in land trial

Figure 14.

Dynamic characteristics of vehicle in land trial

In the vehicle test, the graded noise with the sine function of 5 m/s is contrived from 800 s to 950 s to compare the performances of these methods. Fairly, the optimal width of the moving window for MW is set as 50 and the optimal forgetting factor b for EWMA is set as 0⋅98 after multiple experiences. The attitude, velocity and position errors of the four methods are shown in Figures 15–17. Meanwhile, horizontal position errors are shown in Figure 18. Similarly, the errors of the IEWMA method are the most stable and lowest in the four methods, which effectively verifies the feasibility of the proposed method.

Figure 15.

Attitude errors of four methods in land trial

Figure 16.

Velocity errors of four methods in land trial

Figure 17.

Position errors of four methods in land trial

Figure 18.

Horizontal position errors of four methods in land trial

Simultaneously, quantitative analysis is performed. The mean and RMS of attitude, velocity, individual position and horizontal position errors are shown in Tables 5–7. From Tables 5 and 6, we can see that the mean and RMS of errors for IEWMA is smaller than for KF, MW and EWMA. In Table 7, it is obvious that the mean and RMS of horizontal position error for IEWMA are smaller than the others. Therefore, the IEWMA method outperforms the other methods in both navigation accuracy and stability.

Table 5.

Mean of attitude, velocity and position errors in four methods

Table 6.

RMS of attitude, velocity and position errors in four methods

Table 7.

Mean and RMS of horizontal position errors in four methods

As mentioned, the performance of the integrated system depends on the accuracy of R_k. The estimated R_k is shown in Figure 19. It can be seen that R_k of IEWMA is more stable than MW and EWMA at the initial moments (0 s to 600 s). This is because the width of the moving window in MW and the forgetting factor b of EWMA cannot meet the requirements of the environment. Conversely, the adaptive forgetting factor b of IEWMA can decrease to adapt to the stable environment. Additionally, the estimated R_k of IEWMA is the closest to true value, while MW and EWMA are sluggish when noise occurs (800 s to 950 s). When the measurement noise occurs, the forgetting factor of IEWMA becomes smaller, and the weight of the residual in current time increases, which can be sensitive to changes. However, because of the fixed width of the moving window and forgetting factor b, MW and EWMA are still affected by the historical sequence and are too sluggish to estimate the noise accurately.

Figure 19.

Diagram of the estimated R_k in land trial