A Novel Adaptive Factor-Based H∞ Cubature Kalman Filter for Autonomous Underwater Vehicle

Abstract

In the navigation of an autonomous underwater vehicle (AUV), the positioning accuracy and stability of the navigation system will decrease due to uncertainties such as mobility, inaccuracy of a priori process noise characteristic, and simplification of a dynamic model. In order to solve the above problems, a new, adaptive factor-based H∞ cubature Kalman filter based on a fading factor (AF-H∞CKF) is proposed in this paper. On the one hand, the H∞ game theory provides AF-H∞CKF good robustness in the worst case; on the other hand, the fading factor makes the innovation orthogonal and inflates the predicted error covariance and the Kalman gain, which avoids a decrease in estimation precision in the case of model uncertainty. The simulation and experiment results show that the AF-H∞CKF filter can deal with AUV navigation better than other existing algorithms in the presence of outliers and model uncertainty, which confirms its effectiveness and superiority.

1. Introduction

AUV integrates a variety of advanced technologies, such as underwater communication, multi-sensor fusion, data processing, etc., and has been widely used in the fields of mine clearance, oceanographic survey, and ocean sounding data acquisition [1]. The navigation system provides the motion data for an AUV to reach the destination and successfully complete the task. Due to the advantages of autonomy and good accuracy in short time, the inertial navigation system (INS) is widely used in AUV navigation [2]. However, inertial navigation cannot meet the demands of long range because of its accumulation of measurement error [3]. Thus, the dead reckoning (DR), comprised of Doppler velocity log (DVL) and magnetic compass, is often combined with inertial navigation (INS), which ensures high precision in a short time and suppresses the influence of error accumulation [4].

In the practical application of AUV navigation, data fusion and interference suppression need to be realized by the filtering algorithm [5,6]. Kalman filter (KF), which is widely used in navigation systems, is a practical real-time optimal estimation method [7]. However, KF cannot meet the requirements of nonlinear systems, and its estimation results are susceptible to model uncertainty [8].

The improved extended Kalman filter (EKF) is suitable for nonlinear systems. However, the model error will be generated and accumulated over time in the EKF linearization transformation [9]. The unscented Kalman filter (UKF) [10,11] and the cubature Kalman filter (CKF) can solve the above problems. In addition, CKF shows higher accuracy than UKF when the model dimension is beyond three [12]. However, due to the influence of water depth and flow in AUV navigation, partial measurement systems may stop working, resulting in system instability [13]. The H∞ filter can ensure good robustness in the presence of outliers, whose theory is to minimize the influence of outliers on the estimated output in the presence of outliers; however, it only satisfies the linear system [14]. The combination of H∞ filter and cubature Kalman filter can not only extend the application range to nonlinear system, but also ensure robustness [15].

Although the H∞CKF shows good robustness in regard to extreme noises, the accuracy still needs to be improved in the case of uncertainty. Therefore, various adaptive methods were proposed. The Sage–Husa noise estimator [16] was presented to modify the real-time noise properties through using the measurements, which decreases the influence caused by noise property uncertainty. Yang proposed an adaptive H-Infinity cubature Kalman filter based on the Sage–Husa estimator (AH∞CKF), which combines H∞CKF and the Sage–Husa estimator to revise noise properties [17]. Gao proposed the model predictive unscented Kalman filter (MP-UKF) [18] to correct the dynamic model by comparing the estimation outputs with the measurements. The Huber method can also compensate the process’s uncertainty [19]. Nevertheless, the variance of the process and measurement errors cannot be estimated by these algorithms concurrently. However, a fading factor adaptive algorithm based on innovation orthogonality [20] can suppress the effects of model uncertainty without estimating the process and measurement errors.

In the practical navigation of AUVs, there always exists maneuverability, unknown noise properties, and simplified motion model, all of which bring about instability and low precision. Hence, a novel filter named AF-H∞CKF, combining H∞CKF with a fading factor adaptive algorithm, is proposed to modify the predicted error covariance and improve the filter accuracy for the navigation of AUVs. In this novel filter, H∞ theory can ensure the robustness in the case of extreme error, and the adaptive factor can keep high accuracy when the model is uncertain.

The rest of this manuscript is as follows. In Section 2, the H∞ theory and H∞CKF are introduced. Then, an adaptive factor algorithm and the AF-H∞CKF are introduced in Section 3. In Section 4, the proposed filter is verified through simulations. To further validate the efficacy of the novel filter, the four filters are applied in a lake experiment and simulations in Section 5. Lastly, we obtain a conclusion in Section 6.

2. H∞ Theory and H∞CKF

In a nonlinear system, the measurement equation and estimated equation can be given by:

$$\{\begin{array}{ll}{a}_{x+1}& =f(a_x)+q_x\\ b_x& =h(b_x)+r_x\end{array}$$

(1)

The $x$ shown in the Equation (1) indicates the filtering epoch; $a_x\in R^n$ denotes the state vector; $b_x{R}^m$ denotes the measurement vector; $f$ is the dynamic model function; $h$ is the observation function; and $q_x$ and $r_x$ represent the process noise and measurement noise, respectively. The covariance of $q_x$ and $r_x$ is represented by $Q_x$ and $R_x$, respectively.

The principle of the H∞ theory is that the estimation error can be minimized when the external interference reaches the maximum value [21]. A linear combination of states is used in the estimation as follows:

$$c_x=l_x{a}_x$$

(2)

where $c_x\in R^n$ is exactly the vector that needs to be estimated; $l_x$ is a known linear matrix without additional calculation. Additionally, $l_x$ is generally regarded as an identity matrix to make the estimation easier.

The design principle of H∞ filter is based on game theory, i.e., the specific cost function will be minimized when $P_0$, $Q_x$, and $R_x$ arrive at the upper limits. In addition, $P_0$, mentioned above, represents the initial predicted error covariance that is presented on the basis of the actual conditions, and it denotes the closeness between the initial estimate ${\hat{a}}_0$ and actual initial state vector $a_0$. The cost function [22] is shown below:

$$J=\frac{{\displaystyle \sum _{x=1}^N\left|\right|c_x-{\widehat{c}}_x|{|}_2^2}}{{\left|\right|c_0-{\widehat{c}}_0\left|\right|}_{P_0^{-1}}^2+{\displaystyle \sum _{x=1}^N(|\left|q_x\right|{|}_{Q_x^{-1}}^2+\left|\right|r_x|{|}_{R_x^{-1}}^2)}}$$

(3)

where ${\widehat{c}}_x$ represents the optimal estimation vector, which needs to be found for the sake of minimizing the cost function $J$. In addition, the expression ${\Vert c_0-\stackrel{\wedge }{c_0}\Vert }_{P_0^{-1}}^2$ represents ${({\mathrm{c}}_0-{\widehat{c}}_0)}^T{P}_0^{-1}(c_0-{\widehat{c}}_0)$. Nevertheless, obtaining an optimal analytical solution through Equation (3) is very difficult. Thus, a suboptimal solution has been commonly adopted in the literature [23]. A threshold is defined as limiting the function $J$ to make sure $J$ is under a predetermined disturbance tolerance level:

$$\max J <{\gamma }^2$$

(4)

where $\gamma$ is the threshold mentioned above. To apply H∞ theory to nonlinear systems, the H∞CKF algorithm which combines H∞ with CKF framework is presented in [15]. The formula of H∞CKF is given below:

Prediction:

(1)

Obtain the cubature points:

$$P_{x|x}=S_x{S}_x^T$$

(5)

$$a_x^k=S_x{\delta }_k+\widehat{a_k},k=1,2,\cdots ,n,\dots ,2n$$

(6)

$${\delta }_k=\sqrt{n}{[1]}_k=\sqrt{n}\{\left(\begin{array}{l}1\hfill \\ 0\hfill \\ ⋮\hfill \\ 0\hfill \end{array}\right),\left(\begin{array}{l}0\hfill \\ 1\hfill \\ ⋮\hfill \\ 0\hfill \end{array}\right),\cdots ,\left(\begin{array}{l}0\hfill \\ 0\hfill \\ ⋮\hfill \\ 1\hfill \end{array}\right),\left(\begin{array}{l}-1\hfill \\ 0\hfill \\ ⋮\hfill \\ 0\hfill \end{array}\right),\left(\begin{array}{l}0\hfill \\ -1\hfill \\ ⋮\hfill \\ 0\hfill \end{array}\right),\cdots ,\left(\begin{array}{l}0\hfill \\ 0\hfill \\ ⋮\hfill \\ -1\hfill \end{array}\right)\}$$

(7)

where ${\delta }_k$ is a cubature point.

(2)

Propagate the cubature points:

$$a_{x+1|x}^k=f(a_x^k)$$

(8)

(3)

Predict the state vector and calculate the predicted covariance:

$$\begin{array}{l}{\hat{a}}_{x+1|x}=\frac{1}{2n}{\displaystyle \sum _{k=1}^{2n}{a}_{x+1|x}^k}\\ P_{x+1|x}=\frac{1}{2n}{\displaystyle \sum _{k=1}^{2n}{a}_{x+1|x}^k}{(a_{x+1|x}^k)}^T-{\widehat{a}}_{x+1|x}{({\hat{a}}_{x+1|x})}^T+{\widehat{Q}}_x\end{array}$$

(9)

Time update:

(1)

Calculate cubature points:

$$\begin{array}{l}{P}_{x+1|x}=S_{x+1|x}{S}_{x+1|x}^T\\ A_{x+1|x}^k=S_{x+1|x}{\delta }_k+{\hat{a}}_{x+1|x}\end{array}$$

(10)

(2)

Obtain the cubature points:

$$b_{x+1}^k=h(A_{x+1|x}^k)+{\widehat{r}}_{x+1}$$

(11)

(3)

Calculate the predicted observation vector:

$${\widehat{b}}_{x+1|x}=\frac{1}{2n}{\displaystyle \sum _{k=1}^{2n}{b}_{x+1|x}^k}$$

(12)

(4)

The innovation covariance $P_{cc,x+1|x}$ and the cross-covariance $P_{ac,x+1|x}$ are calculated by:

$$\begin{array}{l}{P}_{\mathrm{cc},x+1|x}=\frac{1}{2n}{\displaystyle \sum _{k=1}^{2n}{b}_{x+1|x}^k}{(b_{x+1|x}^k)}^T-{\widehat{b}}_{x+1|x}{({\widehat{b}}_{x+1|x})}^T+{\widehat{R}}_{x+1}\\ P_{ac,x+1|x}=\frac{1}{2n}{\displaystyle \sum _{k=1}^{2n}{A}_{x+1|x}^k}{(b_{x+1|x}^k)}^T-{\widehat{a}}_{x+1|x}{({\widehat{b}}_{x+1|x})}^T\end{array}$$

(13)

(5)

Calculate the gain matrix and the updated state:

$$\begin{array}{l}{K}_{x+1}=P_{ac,x+1|x}{(P_{cc,x+1|x})}^{-1}\\ {\widehat{a}}_{x+1} = {\widehat{a}}_{x+1|x}+K_{x+1}(b_{x+1}-{\widehat{b}}_{x+1})\end{array}$$

(14)

(6)

Calculate $P_{x+1|x+1}$, the covariance of the estimation vector:

$$\begin{array}{l}{P}_{x+1|x+1}^{-1}=P_{x+1|x}^{-1}+P_{x+1|x}^{-1}{P}_{\mathrm{ac},x+1|x}^{}{R}_{x+1}^{-1}{(P_{ac,x+1|x}^{})}^T{(P_{x+1|x}^{-1})}^T-{\gamma }^{-2}{I}_n\\ P_{x+1|x+1}=P_{x+1|x+1}^{-1}\backslash I_n\end{array}$$

(15)

where $I_n$ is an identity matrix. The threshold $\gamma$ is of vital importance to ameliorate the performance of the H∞ cubature filter. As the $\gamma$ decreases, the sensitivity of the filtering algorithm to model errors and outliers also decreases; however, its robustness would generally enhance. Nevertheless, with $\gamma$ decreasing, the covariance of the estimated state vector would increase, while the estimation accuracy would decrease. In the meantime, the filter may be invalid when the threshold $\gamma$ becomes very small, which means the minimum of $\gamma$ must be able to ensure the stability of the filter.

Overall, the H∞CKF guarantees good robustness of the H∞ theory.

3. A Fading Factor Adaptive Filter and the Proposed AF-H∞CKF

H∞CKF provides better robustness compared to that of CKF in terms of extreme disturbance. Although the H∞ theory ensures the robustness of H∞CKF in the condition of the worst disturbance, the accuracy of H∞CKF still declines when it comes to unknown external noises and an inaccurate dynamic model. The predicted covariance needs to be adjusted according to measurements. Therefore, a fading factor adaptive filter is introduced to be combined with H∞CKF in this section.

3.1. The Fading Factor Adaptive Filter

To ameliorate the filter algorithm’s accuracy and tracking capability when there is an uncertain system model, Donghua Zhou and others put forward the concept of an EKF-based fading factor adaptive filter. The innovation sequence should be irrelevant when the filter works well, which can be interpreted as effective information being fully utilized. Thus, the orthogonality of the innovation sequence can be regarded as an index to measure the filtering performance. The main idea of the fading factor adaptive filter is to modify the predicted error covariance $P_{x+1|x}$ by way of satisfying the following equations:

$$E[(a_x-{\hat{a}}_{x|x}){(a_x-{\hat{a}}_{x|x})}^T]=\min$$

(16)

$$E[{\alpha }_{x+j}{\alpha }_x{}^T]=0,x=0,1,2\dots ,j=1,2\dots$$

(17)

where ${\alpha }_{x+1}=c_{x+1}-{\widehat{c}}_{x+1|x}$. Equation (16) denotes the performance criteria of the adaptive filter and Equation (17) denotes that the innovation sequence should be irrelevant. An adaptive factor is introduced to ensure the position accuracy and strong tracking characteristics of the filter. The predicted covariance is modified by this factor. By adjusting the predicted covariance and gain matrix online, the innovation is coerced to be orthogonal to ensure the tracking ability. The steps of the algorithm in [20] are as follows:

$${\hat{a}}_{x+1|x}=f({\hat{a}}_{x|x})$$

(18)

$$P_{x+1|x}={\lambda }_{x+1}{F}_{x+1|x}{P}_{x|x}{F}_{x+1|x}^T+Q_x$$

(19)

$${\widehat{c}}_{x+1|x}=h({\hat{a}}_{x+1|x})$$

(20)

$$K_{x+1}=P_{x+1|x}{H}_{x+1}^T{(H_{x+1}{P}_{x+1|x}{H}_{x+1}^T+R_{x+1})}^{-1}$$

(21)

$${\hat{a}}_{x+1|x+1}={\hat{a}}_{x+1|x}+K_{x+1}(c_{x+1}-{\widehat{c}}_{x+1|x})$$

(22)

$$P_{x+1|x+1}=(I-K_{x+1}{H}_{x+1})P_{x+1|x}$$

(23)

where $F_{x+1|x}=\frac{\partial f(\widehat{a_x})}{\partial a_x}|{}_{a_x={\widehat{a}}_{x|x}}$, $H_{x+1}=\frac{\partial h(a_{x+1})}{\partial a_{x+1}}|{}_{a_{x+1}={\widehat{a}}_{{}_{x+1|x}}}$, and ${\lambda }_{x+1}$ denote the fading factor, which is derived as follows:

$${\lambda }_{x+1}=\{\begin{array}{l}{\lambda }_0,{\lambda }_0\ge 1\\ 1,{\lambda }_0<1\end{array}$$

(24)

$${\lambda }_0=\frac{tr(M_{x+1})}{tr(N_{x+1})}$$

(25)

$$M_{x+1}=V_{x+1}-H_{x+1}{Q}_x{H}_{x+1}^T-\beta R_{x+1}$$

(26)

$$N_{x+1}=H_{x+1}{F}_{x+1|x}{P}_{x|x}{F}_{x+1|x}^T{H}_{x+1}^T$$

(27)

$$V_{x+1}=\{\begin{array}{l}{\alpha }_0{\alpha }_0{}^T,x=0\\ \frac{\rho V_x+{\alpha }_{x+1}{\alpha }_{x+1}{}^T}{1+\rho }\end{array},x\ge 1$$

(28)

where $\rho (0<\rho \le 1)$ denotes the forgetting factor. $\beta (\beta \ge 1)$ is a weakening factor selected previously by experience, which is added to avoid possible over-regulation. If ${\lambda }_x\ge 1$, the filter predicted covariance will also be inflated, leading to an increase in the weight of the newly measured data in the filter estimation, achieving the purpose of restraining the filter divergence and improving the filter accuracy.

3.2. The Proposed AF-H∞CKF

The defect of low filtering accuracy always exists in the H∞CKF algorithm introduced above, due to the uncertainty of the model. Therefore, the adaptive filter mentioned above would be applied in H∞CKF and improve the tracking performance of H∞CKF.

Then, the fading factor ${\lambda }_{x+1}$ can be used to update the predicted error covariance $P_{x+1|x}$, which is represented as follows:

$$P_{x+1|x}={\lambda }_{x+1}[\frac{1}{2n}{\displaystyle \sum _{k=1}^{2n}{a}_{x+1|x}^k}{\left(a_{x+1|x}^k\right)}^T-{\hat{a}}_{x+1|x}{({\hat{a}}_{x+1|x})}^T]+{\widehat{Q}}_x$$

(29)

Equation (24) denotes the calculation of factor ${\lambda }_{x+1}$. However, in CKF, the $M_{x+1}$ and $N_{x+1}$ cannot be derived as Equation (25) because there is no Jacobian matrix. Thus, the $M_{x+1}$ and $N_{x+1}$ that can be applied in CKF need to be derived.

Before the fading factor is added, the predicted covariance $P_{x+1|x}$, the innovation covariance matrix $P_{cc, x+1|x}$, and the cross-covariance matrix $P_{ac,x+1|x}$ are expressed as:

$$P_{x+1|x}=E[(a_{x+1}-{\hat{a}}_{x+1|x}){(a_{x+1}-{\hat{a}}_{x+1|x})}^T]=F{P}_{x|x}{F}^T+Q_x$$

(30)

$$P_{cc,x|x+1}=E[(c_{x+1}-{\widehat{c}}_{x+1|x}){(c_{x+1}-{\widehat{c}}_{x+1|x})}^T]=H_{x+1|x}{P}_{x+1|x}{H}_{x+1|x}^T+R_{x+1}$$

(31)

$$P_{ac,x+1|x}=E[(a_{x+1}-{\hat{a}}_{x+1|x}){(c_{x+1}-{\widehat{c}}_{x+1|x})}^T]=P_{x+1|x}{H}_{x+1|x}^T$$

(32)

According to Equation (32), we can obtain the following formula:

$$H_{x+1|x}=[P_{ac,x+1|x}]{[P_{x+1|x}]}^{-1}$$

(33)

Put Equations (30), (31), and (33) into Equation (25), and the $M_{x+1}$ and $N_{x+1}$ can be derived as following:

$$\begin{gathered} M_{x+1}=V_{x+1}-{[P_{ac,x+1|x}]}^T{[P_{x+1|x}]}^{-1}{Q}_x{[P_{ac,x+1|x}]}^{-1}{[P_{x+1|x}]}^T-\beta R_{x+1} \\ {N}_{x+1}=P_{cc,x+1|x}-V_{x+1}+M_{x+1} \end{gathered}$$

(34)

According to the above derivation, the core AF-H∞CKF algorithm structure is shown in Figure 1.

The detailed steps of the AF-H∞CKF algorithm are as follows:

(1)

Initialization;

(2)

Obtain cubature points $a_x^k$ and propagate points using Equations (5)–(8);

(3)

Predict the state vector ${\hat{a}}_{x+1|x}$ and obtain the covariance $P_{x+1|x}$ using Equation (9);

(4)

Calculate the predicted measurement vector ${\widehat{b}}_{x+1|x}$ with the updated cubature points using Equations (10)–(12);

(5)

Compute the covariance $P_{cc,x+1|x}$ and $P_{ac,x+1|x}$ using Equation (13);

(6)

Calculate the fading factor ${\lambda }_{x+1}$ using Equations (24) and (34);

(7)

Update the predicted error covariance $P_{x+1|x}$ using Equation (29);

(8)

Update the covariance $P_{cc,x+1|x}$ and $P_{ac,x+1|x}$ with the updated $P_{x+1|x}$ using Equation (13);

(9)

Compute matrix $K_{x+1}$ and updated state ${\hat{a}}_{x+1}$ using Equation (14);

(10)

Calculate the corresponding covariance $P_{x+1|x+1}$ using Equation (15).

Aimed at the uncertainty of the system model, the AF-H∞CKF proposed in this thesis is used to increase the accuracy and tracking ability of the system.

4. Simulations and Analysis

In order to verify the robustness and accuracy of the new filter, AF-H∞CKF is simulated and compared with CKF, H∞CKF and AH∞CKF, in which AH∞CKF is also an adaptive robustness CKF algorithm. These algorithms can be well-verified by the CA model below [24].

The position, velocity, acceleration, and other information output by the navigation system are used to construct the error equation of the system. In this paper, six state parameters are selected to construct the state vector $a_x$, which are the position $e_x$, velocity $v_{e,x}$, and acceleration $a_{e,x}$ in the east, and the position $n_x$, velocity $v_{n,x}$, and acceleration $a_{n,x}$ in the north. See Formula (35).

$$a_x=[e_x,v_{e,x},a_{e,x},n_x,v_{n,x},a_{n,x}]$$

(35)

The mathematical dynamic model [25] is shown as:

$$a_{x+1}={\mathsf{\varphi }}_{x+1|x}{a}_x+q_x$$

(36)

In the above formula, $q_x$ represents the process noise vector, which satisfies $q_x~N(0,Q_x)$, ${\mathsf{\varphi }}_{x+1|x}$ represents the transformation matrix, and ${\mathsf{\varphi }}_{x+1|x}$ is defined as follows:

$$\begin{gathered} {\mathsf{\varphi }}_{x+1|x}=diag[{\mathsf{\varphi }}_e,{\mathsf{\varphi }}_n] \\ {\mathsf{\varphi }}_e={\mathsf{\varphi }}_n=\left(\begin{array}{ccc}1& t& t^2/2\\ 0& 1& t\\ 0& 0& 1\end{array}\right) \end{gathered}$$

(37)

where $t$ is the sampling time.

The observation model is given by:

$$b_x=\left[\begin{array}{l}{e}_{INS,x}\\ n_{INS,x}\\ {\theta }_x\\ e_x\end{array}\right]=\left[\begin{array}{l}{e}_x\\ n_x\\ \arctan [\frac{v_{e,x}}{v_{n,x}}]\\ t\sqrt{v_{e,x}^2+v_{n,x}^2}\end{array}\right]+r_x$$

(38)

where $r_x$ denotes the observation noise vector at epoch $x$ and satisfies $v_x~N(0,R_x)$; $e_{INS,x}$, $n_{INS,x}$, ${\theta }_x$, and $s_x$ denote the eastern position, northern position, azimuth, and distance, respectively, in the sampling time.

This simulation sets up two different situations that will occur in AUV navigation; one is adding abnormal noise, and the other is a situation where the model is inaccurate. In Case 1, anomalous noise was added in the 150th, 300th, and 450th second to verify the robustness of the novel algorithm. In Case 2, the noise property from the 300th second to the 400th second was unknown and different from other parts during the simulation process to verify the accuracy.

In addition, the initial parameters of simulations are given as follows. The initial state vector is $a_0={[0,10,0,0,10,0]}^T$; the sampling interval is $t=1s$; the process noise covariance is $Q_x=diag[0,0,{0.03}^2,0,0,{0.03}^2]$; and the measurement noise covariance is $R_x=diag[3^2,3^2,{0.1}^2,{0.1}^2]$.

On the basis of the above terms, twenty Monte Carlo simulations were put into effect to assess the performance of CKF, H∞CKF, AH∞CKF, and the proposed AF-H∞CKF in two cases.

Moreover, the root mean square error (RMSE) is an utterly common method to evaluate whether filtering is the most suitable algorithm for the system. Hence, this paper also uses the RMSE to compare the performance of the four filters mentioned above, which can be written as:

$$RMS{E}_x=\sqrt{\frac{1}{N}{\displaystyle \sum _{k=1}^N{({\hat{a}}_x^k-a_x)}^2}}$$

(39)

where N is the Monte Carlo run.

4.1. Case 1: Outliers

In Case 1, the noise characteristics at other times are known, except for adding abnormal noise at the 150th, 300th, and 450th second. The outlier vectors $(30,0,0,30,0,0)$ and $(3,3,0,20)$ are respectively, added in the state vector and measurement vector at these time points. Finally, four filters are simulated in this condition. The simulation results are shown in Figure 2, Figure 3, Figure 4 and Figure 5.

Figure 2 and Figure 3 describe the RMSE position in both the X-axis and Y-axis obtained by CKF, H∞CKF, AH∞CKF, and AF-H∞CKF, respectively. It is obvious that the RMSE values of the four filters are all very small when the outliers are not added. However, the RMSE of CKF and H∞CKF increase sharply at the time point of adding outliers, while AH∞CKF and AF-H∞CKF show good robustness because the H∞ theory is adopted and adaptiveness is considered. For example, it can be concluded from Figure 2 that the RMSE of AF-H∞CKF decreased by 82.76% and 75.61%, respectively, compared with CKF and H∞CKF, where the RMSE of AH∞CKF decreased by 70.69% and 58.54% compared with the first two algorithms at the 150th second. Although both AH∞CKF and AF-H∞CKF show good robustness, AF-H∞CKF performs better when no outliers exist. Figure 4 indicates the trajectory obtained by the four filters, and Figure 5 shows the partial enlargement of the rectangular region in Figure 4. It can be clearly indicated that the trajectory precision of the proposed AF-H∞CKF is much higher than that of the other three filters.

In summary, the new AF-H∞CKF ensures better robustness than the others in the case of outliers.

4.2. Case 2: Inaccurate Model

In order to evaluate the effectiveness of the novel AF-H∞CKF in terms of an inaccurate model, the process noise suddenly becomes unknown during the time period $(300 \mathrm{s},400 \mathrm{s})$. The actual process noise is unknown in this period, which is $w_x~N(10,diag[{20}^2,0,{0.03}^2,{20}^2,0,{0.03}^2])$. The noise properties are returned to the initiate property at the beginning of the 400th second. The simulation results are shown in Figure 6, Figure 7, Figure 8 and Figure 9.

Figure 6 and Figure 7 describe the RMSE position in both the X-axis and Y-axis obtained by CKF, H∞CKF, and AF-H∞CKF, respectively. The RMSE of CKF, H∞CKF, and AH∞CKF in Figure 7 is about 14 m, 13 m, and 6 m, respectively, while AF-H∞CKF is no more than 5 m in Figure 7 when process noise suddenly becomes unknown. It can be derived that the positioning accuracy of CKF and H∞CKF is very poor during the time interval (300 s, 400 s) due to the unknown noise, while AF-H∞CKF can still provide high accuracy and the error of AH∞CKF does not increase sharply; it is still larger than that of AF-H∞CKF. Figure 8 indicates the trajectory obtained through the four filters. Figure 9 shows the partial enlargement of the rectangular region in Figure 8. Figure 8 and Figure 9 indicate that the trajectory obtained by simulation of AF-H∞CKF is closest to the real trajectory among the four different filters.

Table 1. Comparison of the four algorithms in Case 2.

	CKF	H∞CKF	AH∞CKF	AF-H∞CKF
Mean error in the X-axis (m)	1.831	1.640	1.649	1.357
Mean error in the Y-axis (m)	2.633	2.167	2.237	1.381
Running time (s)	0.049	0.067	0.078	0.082

describes the comparison between the four filters on mean error and running time. It can be calculated that the X-axis error decreases by 26%, 17%, and 18% and the Y-axis error decreases by 48%, 36%, and 38%, respectively, compared with the former three algorithms, which further proves the superiority of this AF-H∞CKF. On the other hand, we must realize that a longer running time for the proposed AF-H∞CKF is needed, compared to the other three algorithms.

In summary, the AF-H∞CKF introduced in this paper offers the best accuracy to the navigation system among the four filters in the case of model uncertainty.

5. Experiments and Analysis

In addition to the above simulation results, we conducted an experiment on Taihu Lake to obtain the actual navigation data and further verify the effectiveness of the algorithm.

In this experiment, we collected navigation data by carrying a navigation system on a ship. The navigation system was composed of satellite navigation and autonomous navigation including dead reckoning (DR) and inertial navigation. The hardware of this contained a magnetic compass HMR3000, a Doppler log, a strapdown inertial navigation system, and a GPS receiver JNS100. The GPS receiver was used as a reference station. The location accuracy of the GPS receiver was 10 mm + 1.5 ppm (2DRMS); the precision of the Doppler log was 0.5%; the precision of the HMR3000 log was 0.5°RMS; and the gyro zero drift stability was 0.05°/h.

By matching the data collected by the above sensors with Google Maps, we could obtain the actual navigation track of the ship, which is shown in Figure 10. The blue part of Figure 10 shows Taihu Lake on the map, and the red line is the actual navigation track. Then, the test data were applied to the CA model to verify the performance of the AF-H∞CKF.

Figure 11 and Figure 12 describe the filtering errors of position in both east and north obtained by CKF, H∞CKF, AH∞CKF, and AF-H∞CKF. As shown in Figure 11, we can see that the error of AF-H∞CKF is mostly less than 0.5 m, while CKF and H∞CKF are close to 4.6 m and 2.3 m, respectively. Due to the influence of unknown noise, the position accuracy of CKF and H∞CKF is far lower than AF-H∞CKF and AH∞CKF in most of the whole experiment, while AF-H∞CKF maintains higher accuracy than AH∞CKF. Figure 11 also indicates that the errors of CKF and H∞CKF are divergent, which can be explained by the INS measurement error accumulation. Nevertheless, the error of AF-H∞CKF shows no divergence. In addition, it can be seen from Figure 12 that the error of the AF-H∞CKF algorithm is within 0.7 m, while that of CKF and H∞CKF reach 6 m and 3.6 m, respectively, due to the appearance of outliers.

In

Table 2. Comparisons of the four algorithms.

	CKF	H∞CKF	AH∞CKF	AF-H∞CKF
Mean error in the X-axis (m)	1.577	0.578	0.342	0.145
Mean error in the Y-axis (m)	0.739	0.485	0.281	0.151
Running time (s)	0.029	0.039	0.044	0.047

, the mean error in the X-axis of the AF-H∞CKF is 91%, 75%, and 69% lower than that of CKF, H∞CKF, and AH∞CKF, respectively, and the mean error in the Y-axis of the AF-H∞CKF is 80%, 69%, and 46% lower than that of CKF, H∞CKF, and AH∞CKF. On the other hand, a longer running time for the proposed AF-H∞CKF is needed, compared to other algorithms.

According to the above analysis, we find that the experimental results are consistent with the simulation results. In summary, the proposed AF-H∞CKF ensures good robustness and high accuracy for its adjusting fading factor at the cost of increased running time.

6. Conclusions

In this paper, a new adaptive factor-based H∞ cubature Kalman filter (AF-H∞CKF) is proposed to solve the problem of AUV navigation, including low estimation accuracy and poor robustness. This paper introduces the improved cubature Kalman filter algorithm in detail. The H∞ theory adopted in AF-H∞CKF can ensure good robustness in the worst case. Moreover, the adaptive fading factor adopted makes the innovation orthogonal and modifies the predicted covariance, which greatly improves the accuracy in case of model uncertainty. The performance of AF-H∞CKF is compared with CKF, H∞CKF, and AH∞CKF in a lake experiment and simulations. The experiment and simulations indicate that H∞CKF provides better robustness than CKF; AH∞CKF provides better accuracy than H∞CKF in the case of model uncertainty; and AF-H∞CKF guarantees both good robustness and high precision, and performs better than the other three filters in the cases of outliers and model uncertainty.