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. The recently developed H∞CKF provides nonlinear filtering good robustness, and Sage-Husa estimator could timely modify the statistical properties of noise. The novel algorithm is superior to H∞CKF in accuracy by combining Sage-Husa estimator with the H∞CKF while ensuring robustness. The effectiveness of the novel AH∞CKF is verified by lake experiment and simulation.
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 . 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 . In addition, the dead reckoning system (DR) composed of Doppler Velocity Log (DVL) and compass is also a common system of underwater navigation , 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. The integrated navigation system estimates the current position and other information by fusing the measurement information provided by each component. Thereinto, 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 . 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 . 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 , which shows good performance in accuracy, even if model dimension increases. The 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]. Therefore, 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. Therefore, robustness is required in the navigation system to deal with the problems above. H∞ ﬁlter was proposed to minimize the effect of the worst disturbances on the estimation errors, which is the reason for H∞ ﬁlter guarantee outstanding robustness in case of outliers [11,12]. However, H∞ filter can only be applied in linear system. The 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 .
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 . An adaptive UKF by combining the maximum likelihood principle (MLP) and moving horizon estimation (MHE) was proposed to estimate the system noise statistic . 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 process-modeling error in . A method combined of CKF and the Mahalanobis distance criterion was proposed to deal with model uncertainty in . A robust UKF based on innovation orthogonality (IO-RUKF) was presented to resist the disturbance of measurement errors . 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. The 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 , 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. The 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.
2. The H∞ Filter and H∞CKF
The H∞ filter is a special form of the Kalman filter. The principle of the H-infinity filter is to minimize the estimation error under the condition of interference maximization .
The system equation, measurement equation, and estimated equation can be represented as follows:where is a filtering epoch, is the state vector at epochs and is the measurement vector at time respectively. and are the system dynamics and the observation functions, respectively. and denote system noise terms and measurement noise terms, respectively, whose statistical properties are unknown. The statistics of noise terms and may be unknown or deterministic.
Instead of estimating the state directly, one can estimate a linear combination of states:where is the signal to be estimated. is a known matrix and is usually set to the identity matrix. Thus, the state vector can be directly estimated.
The design idea of H∞ filter is to find the method to minimize the cost function when , , and reach the upper bound, where and denote the covariance matrices of and respectively. represents the initial estimation error covariance matrix preset according to specific problems, which denotes how close is to the initial estimate . The cost function  is given bywhere the expression denotes . Estimation vector should be found to minimize the cost function . However, it is difficult to achieve an analytical solution for the optimal H-infinity filter. Thus, a suboptimal algorithm has been commonly treated in the literature. is bounded by a given threshold under a prescribed disturbance tolerance level:wherein is the error attenuation parameter. To apply H∞ filter in the nonlinear system, an algorithm that combines H∞ with cubature Kalman filter was proposed. The H∞ cubature filter (H∞CKF)  algorithm is summarized as follows： Prediction:(1)Calculate cubature points: where 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 is a unit matrix. is crucial for the existence of the cubature H∞ filter. With the increase of , 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 must guarantee the existence of filter.
The H∞CKF minimizes the estimation error in the worst case, which makes it more robust than the cubature Kalman filter.
3. A Novel Adaptive H-Infinity Filtering Algorithm
3.1. The 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 . 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. The estimator combined with cubature Kalman filter is shown aswhere . is used to weight the new and old noise information in the estimator. The impact of old information is reduced or eliminated by weighting new and old noise information. is the forgetting factor, which is usually 0.95 to 0.99 on experience. limits the memory range of the filter. The larger the is, the stronger the impact of the latest measurement data on the current estimation is. If the noise statistics changes frequently, should be set to be a larger value. In contrast, should take a smaller value if the noise statistics changes infrequently.
The estimator can be used to deal with the noise of time-varying 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.
3.2. 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. Thus, we combine H∞ cubature Kalman filter with Sage-Husa noise statistics estimator to improve the accuracy of H∞CKF. The Sage-Husa noise statistics estimator can be embedded into the H∞CKF directly. The Novel Adaptive H-Infinity filtering algorithm is given in Figure 1.
The 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.
4. 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. The 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 diﬀerent situations for comparison .
The state vector is defined as . The mathematical motion model of the mobile two-wheel driving robot is shown as follows:where and denote displacement velocity and angular velocity of the carrier, respectively. means the sampling time, denotes the position on the X-axis, denotes the position on the Y-axis, denotes the azimuth, denotes the state noise, and .
The observation model is given as follows:where denotes the measurement noise vector and . The simulation parameters are set as follows. The initial states of the carrier are . The sampling interval . The actual initial state is assumed to be . The velocities of the right and left wheels are set to be and respectively. The distance error , and the angular error .
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. The 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 45th, 90th, and 135th 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 40th second to the 80th 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 45th, 90th, and 135th seconds. The covariance of statement noise is , and the covariance of observation noise is .
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 45th, 90th, and 135th 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 90th 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. The covariance of statement noise and observation noise is still and in the first 40 seconds. Both are already known. From the 40th second to the 80th second, the covariance of the real observation noise is set to be . In this period, the change of the real covariance is unknown. Both have returned to original value since the 80th 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. The 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 algorithms before the 40th second, while RMSE of AH∞CKF is obviously lower than that of H∞CKF and AH∞CKF from the 40th to 80th 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. The 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. The 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.
Figures 9 and 10 show the comparisons of the three algorithms on the trajectory, where 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.
The 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.
5. 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. The GPS receiver JNS100 is set as a reference station. The positioning accuracy of GPS receiver is 10 mm + 1.5 ppm (2DRMS). The accuracy of Doppler log is 0.5%. The accuracy of HMR3000 log is 0.5°RMS. The 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. We can see that the values of the position errors in the X-axis of the AH∞CKF algorithm are within 4 m, while those of CKF and H∞CKF are within 14 m and 7 m. Similarly, the values of the position estimation errors in the Y-axis of the AH∞CKF algorithm are within 8 m, while those of CKF and H∞CKF are within 33 m and 30 m in the same condition. We can see that the filtering results of CKF and H∞CKF are divergent, which is because of the error accumulation of the INS. Nevertheless, errors of AH∞CKF keep stable and show no divergence. In 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. This 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 116th 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.
Figure 13 shows the actual trajectory of the vehicle.
6. 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. Then, simulations were carried out in two different cases to verify the new algorithm. The 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. The 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.
The data supporting the conclusions of the study all come from our simulations and lake experiment and are included within the article.
Conflicts of Interest
The authors declare that there are no conflicts of interest regarding the publication of this paper.
G. D. Watt, A. R. Roy, J. Currie et al., “A concept for docking a UUV with a slowly moving submarine under wave0073,” IEEE Journal of Oceanic Engineering, vol. 41, no. 2, pp. 471–498, 2016.View at: Google Scholar
N. Y. Ko and S. Jeong, “Attitude estimation and DVL based navigation using low-cost MEMS AHRS for UUVs,” in Proceedings of the International Conference on Ubiquitous Robots & Ambient Intelligence, IEEE, Kyoto, Japan, June 2015.View at: Google Scholar
G. Hu, S. Gao, and Y. Zhong, “A derivative UKF for tightly coupled INS/GPS integrated navigation,” ISA Transactions, vol. 56, pp. 135–144, 2015.View at: Google Scholar
F. Gustafsson, “Particle filter theory and practice with positioning applications,” IEEE Aerospace and Electronic Systems Magazine, vol. 25, no. 7, pp. 53–82, 2010.View at: Google Scholar
H. Poveda, E. Grivef, G. Ferre et al., “Kalman vs. H∞ filter in terms of convergence and accuracy: application to carrier frequency offset estimation,” in Proceedings of the Signal Processing Conference, IEEE, Istanbul, Turkey, June 2012.View at: Google Scholar
G. Hu, L. Ni, B. Gao, X. Zhu, W. Wang, and Y. Zhong, “Model predictive based unscented kalman filter for hypersonic vehicle navigation with INS/GNSS integration,” IEEE Access, vol. 8, pp. 4814–4823, 2020.View at: Google Scholar
G. Hu, W. Wang, Y. Zhong, B. Gao, and C. Gu, “A new direct filtering approach to INS/GNSS integration,” Aerospace Science and Technology, vol. 77, pp. 755–764, 2018.View at: Google Scholar
B. Gao, S. Gao, Y. Zhong, G. Hu, and C. Gu, “Interacting multiple model estimation-based adaptive robust unscented kalman filter,” International Journal of Control, Automation and Systems, vol. 15, no. 5, pp. 2013–2025, 2017.View at: Google Scholar
B. Gao, S. Gao, G. Hu, Y. Zhong, and C. Gu, “Maximum likelihood principle and moving horizon estimation based adaptive unscented kalman filter,” Aerospace Science and Technology, vol. 73, pp. 184–196, 2018.View at: Google Scholar
B. Gao, G. Hu, S. Gao, Y. Zhong, and C. Gu, “Multi-sensor optimal data fusion based on the adaptive fading unscented kalman filter,” Sensors, vol. 18, no. 2, p. 488, 2018.View at: Google Scholar
C. Jiang, S. Zhang, and Q.-Z. Zhang, “A new adaptive h-infinity filtering algorithm for the gps/ins integrated navigation,” Sensors (Basel, Switzerland), vol. 16, no. 12, 2016.View at: Google Scholar
L. Zhao and X. Wang, “An adaptive UKF with noise statistic estimator,” in Proceedings of the Industrial Electronics and Applications, 2009, IEEE, Xian, China, June 2009.View at: Google Scholar
M. L. Rodrigues, L. F. M. Vieira, and M. F. M. Campos, “Mobile robot localization in indoor environments using multiple wireless technologies,” in Proceedings of the 2012 Brazilian Robotics Symposium and Latin American Robotics Symposium, pp. 79–84, Fortaleza, Brazil, October 2012.View at: Google Scholar
S. G. Tzafestas, “Mobile robot control and navigation: a global overview,” Journal of Intelligent & Robotic Systems, vol. 91, no. 1, pp. 35–58, 2018.View at: Google Scholar
A. Lobo, R. Kadam, S. Shajahan, K. Malegam, K. Wagle, and S. Surve, “Localization and tracking of indoor mobile robot with beacons and dead reckoning sensors,” in Proceedings of the 2014 IEEE Students’ Conference on Electrical, Electronics and Computer Science, pp. 1–4, Bhopal, India, March 2014.View at: Google Scholar