A Novel Adaptive H-Infinity Cubature Kalman Filter Algorithm Based on Sage-Husa Estimator for Unmanned Underwater Vehicle

In the navigation of unmanned underwater vehicle (UUV), a filtering algorithm suitable for the working conditions is required. Due to the disturbance from the environment and maneuverability, outliers and noise with time-varying statistical properties always exist, which greatly affect the positioning accuracy and stability of the navigation system. In this paper, we present a novel nonlinear state estimation algorithm named AH∞CKF based on the combination of H∞CKF and Sage-Husa estimator. )e recently developed H∞CKF provides nonlinear filtering good robustness, and Sage-Husa estimator could timely modify the statistical properties of noise.)e novel algorithm is superior to H∞CKF in accuracy by combining Sage-Husa estimator with the H∞CKF while ensuring robustness. )e effectiveness of the novel AH∞CKF is verified by lake experiment and simulation.


Introduction
Unmanned underwater vehicle (UUV) is one of the most important platforms to explore and develop the ocean, which has great significance in the military. UUV integrates advanced technologies such as underwater acoustic communication, intelligent control, multisensor measurement, and information fusion and is crucial to ocean exploration and development with its advantages of good autonomy, flexibility, small size, and wide range of activities [1]. Underwater navigation system provides UUV with accurate position and attitude information, which is the key to determine whether the UUV can reach the predetermined location accurately and successfully complete the task. In general, the underwater navigation of the UUV is based on the inertial navigation system (INS), which may cause the measurement error to accumulate and enlarge as time goes by [2]. In addition, the dead reckoning system (DR) composed of Doppler Velocity Log (DVL) and compass is also a common system of underwater navigation [3], which calculates the position information according to the velocity information of DVL and the azimuth information of compass. In order to improve the navigation accuracy, the inertial navigation system is often combined with the DR system. e integrated navigation system estimates the current position and other information by fusing the measurement information provided by each component. ereinto, filtering algorithm is the key part of integrated navigation system. Nowadays, the most widely used fusion filtering algorithms in navigation system are based on Kalman Filter (KF) framework, which is derived according to the least square estimation of state vector and performs well in filtering [4]. To resolve the defect that Kalman filter can only be applied in linear estimation, Extended Kalman Filter (EKF) was proposed. In the process of linearized transformation in EKF, system error is generated, which may accumulate and even lead to divergence [5]. To resolve this defect, Julier and Uhlmann proposed an unscented transformation (UT) as a method to propagate mean and covariance information [6,7]. However, UKF performs worse when model dimension increases. Arasaratnam and Haykin proposed cubature Kalman filter (CKF) through a spherical-radial cubature rule [8], which shows good performance in accuracy, even if model dimension increases. e particle filter (PF), which approximates the densities through samples (particles), can be used to deal with nonlinear system, but variety of particle declines and high calculation cost is needed [9,10]. erefore, CKF is widely used in navigation system.
In practical application of UUV navigation, the stability of DVL will be affected by water depth and flow, and the strong maneuverability of UUV may bring about the instability of process noise. erefore, robustness is required in the navigation system to deal with the problems above. H∞ filter was proposed to minimize the effect of the worst disturbances on the estimation errors, which is the reason for H∞ filter guarantee outstanding robustness in case of outliers [11,12]. However, H∞ filter can only be applied in linear system. e H∞ Cubature Kalman Filter (H∞CKF) is proposed to keep the advantages of both CKF and H∞ filter and promises good robustness in case of extreme noise. H∞CKF can be applied in nonlinear system, and statistical properties of noises are not required for H∞CKF [13].
Although H∞CKF filter does not require Gaussian noise known prior, the accuracy still declines when prior noise model is inaccurate or the actual noise properties vary, which needs to be resolved. Strong tracking algorithms MP-UKF and RSTUKF were proposed by Hu to address the model uncertainty caused by carrier maneuverability [14,15]. Gao proposed an interacting multiple model (IMM) estimation-based adaptive robust UKF to deal with system model uncertainty [16]. An adaptive UKF by combining the maximum likelihood principle (MLP) and moving horizon estimation (MHE) was proposed to estimate the system noise statistic [17]. An optimal data fusion methodology was presented based on the adaptive fading unscented Kalman filter for multisensor nonlinear stochastic systems to refrain from the influence of processmodeling error in [18]. A method combined of CKF and the Mahalanobis distance criterion was proposed to deal with model uncertainty in [19]. A robust UKF based on innovation orthogonality (IO-RUKF) was presented to resist the disturbance of measurement errors [20]. Both [19,20] inflated covariance of predicted measurement and ensure good robustness.
In addition, the influence of dynamic errors can also be controlled by the Sage-Husa noise estimator, whose essence is to use the observation value to modify the prediction value continuously in the filtering process, and to modify the unknown or changing system parameters at the same time.
e Sage-Husa adaptive noise estimator is one of the most widely used adaptive filtering methods. Due to its advantages that the recursive formula is simple and easy to be implemented [21], Sage-Husa estimator is adopted in this paper to estimate unknown system noise.
In the navigation of UUV, the noise properties may be unknown or time-varying, which may decrease the navigation accuracy. In addition, worst-case disturbance may exist in the running system, which may greatly reduce the stability of the system. In this paper, H∞CKF is adopted to guarantee robustness of the UUV navigation system. Moreover, Sage-Husa adaptive noise estimator was combined with H∞CKF to modify noise statistical characteristics in real time and avoid larger error. e paper is organized as follows. In Section 2, a basic principle of the H-infinity filtering algorithms is introduced. In Section 3, the Sage-Husa noise estimator and the AH∞CKF are presented. In Section 4, the proposed algorithm is verified by simulations. To further verify the effectiveness of the proposed algorithm, the novel algorithm is applied in simulations and a lake experiment. Lastly, a brief conclusion is given in Section 6.

The H' Filter and H'CKF
e H∞ filter is a special form of the Kalman filter. e principle of the H-infinity filter is to minimize the estimation error under the condition of interference maximization [22]. e system equation, measurement equation, and estimated equation can be represented as follows: where k is a filtering epoch, x k ∈ R n is the state vector at epochs k and y k ∈ R m is the measurement vector at time k respectively. f and g are the system dynamics and the observation functions, respectively. w k and v k denote system noise terms and measurement noise terms, respectively, whose statistical properties are unknown. e statistics of noise terms w k and v k may be unknown or deterministic. Instead of estimating the state directly, one can estimate a linear combination of states: where z k ∈ R n is the signal to be estimated. L k is a known matrix and is usually set to the identity matrix. us, the state vector can be directly estimated. e design idea of H∞ filter is to find the method to minimize the cost function when P 0 , Q k , and R k reach the upper bound, where Q k and R k denote the covariance matrices of w k and v k respectively. P 0 represents the initial estimation error covariance matrix preset according to specific problems, which denotes how close x 0 is to the initial estimate x 0 . e cost function [23] is given by where the expression ‖z 0 − z 0 ‖ 2 . Estimation vector z k should be found to minimize the cost function J. However, it is difficult to achieve an analytical solution for the optimal H-infinity filter.
us, a suboptimal algorithm has been commonly treated in the literature. J is bounded by a given threshold under a prescribed disturbance tolerance level: wherein c is the error attenuation parameter. To apply H∞ filter in the nonlinear system, an algorithm that combines H∞ with cubature Kalman filter was proposed. e H∞ cubature filter (H∞CKF) [12] algorithm is summarized as follows： 2 Mathematical Problems in Engineering Prediction: (1) Calculate cubature points: where ζ i is the th cubature point: (2) Propagate the cubature points: (3) Calculate the predicted state vector and the predicted error covariance: Measurement update: (4) Calculate cubature points: (5) Propagate the cubature points: (6) Calculate the predicted measurement vector: (7) Calculate the innovation covariance matrix and the cross-covariance matrix: (8) Calculate the Kalman gain and the updated state: (9) Calculate the corresponding error covariance: where I n is a unit matrix. c is crucial for the existence of the cubature H∞ filter. With the increase of c, the filtering method would be more and more insensitive to the changes of system model error, initial state value, and noise statistical characteristics, and its robustness would decrease gradually. However, the variance of the estimated state is also reduced, and the estimation accuracy is increased. In the meantime, the minimum value of c must guarantee the existence of filter. e H∞CKF minimizes the estimation error in the worst case, which makes it more robust than the cubature Kalman filter.

A Novel Adaptive H-Infinity
Filtering Algorithm 3.1. e Adaptive Kalman Filter. In the standard CKF algorithm, it is assumed that the mean value of noise is zero, and the statistical characteristics of noise are accurately known. In practice, the statistical characteristics are often unknown and time-varying, which causes the decrease of filtering accuracy and even divergence [24]. To solve the above problems, we can estimate and modify the statistical characteristics of noise by adaptive algorithm in the process of filtering. According to Sage-Husa maximum posterior estimation algorithm, a suboptimal constant noise statistical estimator is obtained. e estimator combined with cubature Kalman filter is shown as where ε k � y k − y k|k−1 . d k is used to weight the new and old noise information in the estimator. e impact of old information is reduced or eliminated by weighting new and old noise information. b is the forgetting factor, which is usually 0.95 to 0.99 on experience. b limits the memory range of the filter. e larger the b is, the stronger the impact of the latest measurement data on the current estimation is. If the noise statistics changes frequently, b should be set to be a larger value. In contrast, b should take a smaller value if the noise statistics changes infrequently. e estimator can be used to deal with the noise of timevarying mean and covariance and improve filtering accuracy. It should be noted that the recursive formula above is related. In general, statement noise and measurement noise cannot be estimated at the same time; otherwise, filtering divergence will be caused. Users can choose to estimate only the statement noise or the measurement noise according to the actual situation of the system.

A Novel Adaptive H-Infinity Filtering Algorithm.
H∞CKF minimizes the estimation error in the case of interference and improves the robustness of the system. It has higher filtering accuracy and ensures that the filter can still work normally in case of serious abnormal noise.
However, the accuracy of H∞ filter will still decrease when the statistical characteristics of noise change.
us, we combine H∞ cubature Kalman filter with Sage-Husa noise statistics estimator to improve the accuracy of H∞CKF. e Sage-Husa noise statistics estimator can be embedded into the H∞CKF directly.
e Novel Adaptive H-Infinity filtering algorithm is given in Figure 1. e improved algorithm proposed above can be used when the noise statistical character of the system is absolutely or approximately unknown and ensure the accuracy and robustness of the filter.

Simulation and Analysis
To demonstrate the accuracy and robustness of AH∞CKF algorithm, the CKF, H∞CKF, and AH∞CKF algorithm are used to estimate the state of a nonlinear system. e algorithms will be verified by the following model, which obtains motion information through DVL, compass, and inertial navigation system (INS). We have performed simulations in two different situations for comparison [25].
Consider a carrier that can achieve the velocity and azimuth information, and obtain the position information by mark point M(x m , y m ) (Figure 2) [26,27]. e state vector is defined as [x e k , x n k , θ k ]. e mathematical motion model of the mobile two-wheel driving robot is shown as follows: where V k and ω k denote displacement velocity and angular velocity of the carrier, respectively. T means the sampling time, x e k denotes the position on the X-axis, x n k denotes the position on the Y-axis, θ k denotes the azimuth, w k � [w e k , w n k , w θ k ] denotes the state noise, and w k ∼ N(0, Q). e observation model is given as follows: where v k � [v r k , v ψ k ] denotes the measurement noise vector and v k ∼ N(0, R).
e simulation parameters are set as follows. e initial states of the carrier are [0, 0, 45°]. e sampling interval T � 1 s. e actual initial state is assumed to be [0, 0, 45°]. e velocities of the right and left wheels are set to be 0.4 m/s and 0.399 m/s respectively. e distance error σ r � 0.05 m, and the angular error σ ψ � 0.01 rad/s. Under the above terms, twenty Monte Carlo simulations were performed for the three filter algorithms, including CKF, H∞CKF, and AH∞CKF. And then the mean error and the root mean square errors (RMSE) of the estimated results were compared. e simulation will be carried out under two conditions of different noise. In Case 1, the noise model is accurately known and will not change in the simulation process except that abnormal noises were added at the 45 th , 90 th , and 135 th seconds. In Case 2, it is assumed that the prior statistical characteristics of the noise are accurately known, but the noise model changed from the 40 th second to the 80 th second during the simulation process and recovered after that.
In Case 1, the statistical property of noise is known before except that abnormal noise was added at the 45 th , 90 th , and 135 th seconds. e covariance of statement noise is Q � diag(0.015) 2 , (0.015) 2 , (0.001) 2 , and the covariance of observation noise is R � diag((0.05) 2 , (0.01) 2 ). Figures 3 and 4 show the comparisons among the CKF, H∞CKF, and AH∞CKF on RMSE in the X-axis and Y-axis, respectively. According to Figures 3 and 4, we can observe that the RMSE of CKF and H∞CKF increases obviously at the 45 th , 90 th , and 135 th seconds, while there is no significant growth in AH∞CKF. For example, the RMSE of CKF, H∞CKF, and AH∞CKF in X-axis at the 90 th second is 1.4 m, 0.8 m, and 0.3 m, respectively. Figures 5 and 6 show the comparisons of the several algorithms on trajectory, wherein Figure 6 is partial enlargement of the rectangle regions in Figure 5. Figures 3-6 show that the trajectory of AH∞CKF is closer to real trajectory than that of CKF and H∞CKF.
In Case 2, the statistical property of noise is accurately known at the beginning and changed from the 40th second to the 80th second during the simulation process. e covariance of statement noise and observation noise is still Q � diag ((0.015) 2 , (0.015) 2 , (0.001) 2 ) and R � diag((0.05) 2 , (0.01) 2 ) in the first 40 seconds. Both are already known. From the 40th second to the 80th second, the covariance of the real observation noise R is set to be diag((0.05) 2 , (0.18) 2 ). In this period, the change of the real covariance is unknown. Both have returned to original value since the 80 th second. Figures 7 and 8 show the comparisons of the CKF, H∞CKF, and AH∞CKF on RMSE in the X-axis and Y-axis, respectively.
e part of square frame shows the RMSE under the condition that the statistical property of noise is changed. According to Figures 7 and 8, we can find that there is no significant difference between RMSE of the three Calculate cubature points and propagate the points, using equations (5) and (7) Calculate the predicted state vector and the predicted error covariance, using equation (8) Update cubature points and propagate the points, using equations (9) and (10) Calculate the predicted measurement vector, using equation (11) Calculate the innovation covariance matrix and the cross-covariance matrix, using equation (12) Calculate the Kalman gain and the updated state, using equation (13) Calculate the corresponding error covariance, using equation (14) Update e noise statistic properties, using equation ( Mathematical Problems in Engineering algorithms before the 40 th second, while RMSE of AH∞CKF is obviously lower than that of H∞CKF and AH∞CKF from the 40 th to 80 th second. As an example, the peak RMSE of CKF in Figure 7 increases from 0.2 m to 1.4 m and that of H∞CKF increases from 0.2 m to 1.1 m when noise properties change. e peak RMSE of AH∞CKF in Figure 7 is about 0.3 m, which is obviously smaller than that of CKF and H∞CKF.
From Table 1, we can see that the mean error of the AH∞CKF algorithm is 0.129 m and 0.074 m in the X-axis and Y-axis, respectively, while that of the H∞CKF algorithm is 0.357 m and 0.369 m in the X-axis and Y-axis, respectively. e running time of the AH∞CKF algorithm is more than that of the H∞CKF and CKF algorithm. We can learn from Figures 7 and 8 and Table 1 that AH∞CKF algorithm provides better accuracy than H∞CKF algorithm when the statistical characteristics of noise change unpredictably at the cost of more cost calculation.  Figure 10 is partial enlargement of the square frame regions in Figure 9. Figures  9 and 10 show that the trajectory of AH∞CKF is closer to the real trajectory than that of CKF and H∞CKF. e above figures and tables show that the AH∞CKF algorithm proposed in this paper provides higher estimation accuracy than the CKF and H∞CKF algorithm, especially when statistical properties of noise change.

Experiments and Analysis
To further verify the performance of the AH∞CKF algorithm, an experiment was conducted in Taihu Lake. In this    experiment, we collected data through a boat equipped with GPS/INS integrated navigation system, which is composed of a magnetic compass HMR3000, a strapdown inertial navigation system, a Doppler log, and a GPS receiver. e GPS receiver JNS100 is set as a reference station. e positioning accuracy of GPS receiver is 10 mm + 1.5 ppm (2DRMS). e accuracy of Doppler log is 0.5%. e accuracy of HMR3000 log is 0.5°RMS. e gyro zero drift stability is 0.05°/h. Data was collected and transformed to be used in the mathematical model in the 4th chapter ( Figure 10). Figures 11 and 12 illustrate the estimation errors of the CKF, H∞CKF, and AH∞CKF algorithms. From the errors shown in Figures 11 and 12, error of AH∞CKF is smaller than that of CKF and H∞CKF in most of the experimental process.      Table 2, the mean error in the X-axis of the AH∞CKF algorithm is lower 67% than that of H∞CKF, and the mean error in the Y-axis of the AH∞CKF algorithm is lower 73% than that of H∞CKF. Figures 11 and 12 and Table 2 show that the error of AH∞CKF is smaller than that of CKF and H∞CKF. is is because the properties of noise are modified in real time to improve the filter precision through using the Sage-Husa noise estimator. Abnormal measurement appeared at the 116 th second in Figure 11. At this moment, estimation errors in the X-axis of the AH∞CKF algorithm are within 4 m, while those of CKF and H∞CKF are within 12 m and 10 m. So, we can verify that AH∞CKF provides better robustness than CKF. Table 2 shows that the running time of the AH∞CKF algorithm is 32% more than that of the H∞CKF, which means that higher calculation cost is needed for AH∞CKF than H∞CKF.    Mathematical Problems in Engineering Figure 13 shows the actual trajectory of the vehicle.

Conclusion and Future Work
In this paper, a novel nonlinear filter algorithm named AH∞CKF based on the H∞CKF and the Sage-Husa adaptive estimator was proposed to solve the problem that the noise statistic of the system is unknown or time-varying in engineering and to improve the accuracy of the estimation. Firstly, the H∞CKF algorithm was introduced. Secondly, we have proposed a new AH∞CKF algorithm through combining the advantages of the Sage-Husa adaptive estimator and the H∞CKF algorithm, and the novel algorithm given here can estimate and correct the statistical character of the noises in real time and reduce the estimated errors. en, simulations were carried out in two different cases to verify the new algorithm. e results showed that the proposed AH∞CKF algorithm can significantly improve the accuracy of the estimation when the noise distribution is unknown or time-varying. Lastly, to further verify the effectiveness of the new algorithm, the novel algorithm is verified by a lake experiment and simulation of its experiment data in Section 5. e AH∞CKF provides a high-precision and robust method for the navigation of mobile robot in an unknown environment.
In the following work, we would consider the adaptive filter with noise disturbance and unknown output functions, combined with the method mentioned in [28,29]. Moreover, the reliability of the tracking performance of the adaptive H∞ filter in the uncertain nonlinear system should be verified.
Data Availability e data supporting the conclusions of the study all come from our simulations and lake experiment and are included within the article.