Vessel Dynamic Positioning Multi-sensor Fusion Based on Fuzzy Adaptive Unscented Kalman Filter

XU Shusheng,LI Juan

(College of Electromechanical Engineering,Qingdao Agricultural University,Qingdao,Shandong 266109,China)

Vessel Dynamic Positioning Multi-sensor Fusion Based on Fuzzy Adaptive Unscented Kalman Filter

XU Shusheng,LI Juan

(College of Electromechanical Engineering,Qingdao Agricultural University,Qingdao,Shandong 266109,China)

To improve the performance of multi-sensor fusion of vessel dynamic positioning system (DPS), a hierarchical fusion algorithm based on fuzzy adaptive unscented Kalman filter (UKF) is proposed. It combines the fuzzy adaptive filter algorithm established by using the covariance matching principle, the measurement quality control algorithm and the sub-system fault diagnosis algorithm established by using innovation. Then, the first-level parallel fusion algorithm is set up by using filter state parameters and the second-level fusion is established based on the mutually independent first-level fusion performance. In this way, a multi-sensor fusion algorithm is thus established based on fuzzy adaptive UKF, and the dynamic hierarchical fusion of multi-sensor is achieved. The numerical simulation results verifies the effectiveness of the proposed algorithm.

unscented Kalman filter; hierarchical fusion; fuzzy adaptive system; fault detection

Conventional UKF has no capability to adapt itself to changing conditions of the measurement system. The uncertainty of the process and measurement noise usually degrades the performance of the filter. Therefore, a robust algorithm must be introduced so that the filter can make itself insensitive to measurements in case of malfunctions and correct estimation process without affecting the good estimation behavior[1-2]. Based on multiple model and residual adaptive filter, algorithms can be built. In multiple-model-based adaptive estimation, more than one filter runs parallel under different models in order to satisfy the filter’s true statistical information[3]. In residual-based adaptive filter, the covariance matrices of the measurement and/or system noise are adjusted adaptively to overcome their uncertainties[4-6]. The fuzzy technique can be applied to adaptive filter to achieve the adjusting factor for noise covariance matrices[7-9].

To improve the local filter accuracy and robustness, the measurements must be inspected, and the subsystem fault must also be detected dynamically. Based on the filter innovation, different data quality control algorithms[10-11]as well as many fault detection and isolation methods[12-13]have been proposed. All the approaches have improved the filter performance and the robustness of the system.

In order to improve the fusion performance, the accurate credibility parameters of local filters are required to calculate the weighted factors for the global fusion. Many fuzzy methods have been developed by using several filter state parameters to obtain the credibility parameters of filters[3,9]. These approaches can improve the fusion accuracy.

In this paper, a fuzzy adaptive UKF and a multi-level hierarchical fusion algorithm are built. Based on the proposed fuzzy adaptive UKF, not only can the measurement noise covariance be adjusted, but the measurement data can be inspected and the sensor faults can be detected as well. According to the idea of multi-sensor data fusion, this paper proposes a multi-sensor hierarchical fusion algorithm using some state parameters of the filter. This approach can improve the fusion accuracy and the system fault-tolerance. The three redundancy heading measurement systems of vessel dynamic positioning are regarded as the research object to illustrate the algorithm. A semi-physical simulation system is to be carried out to evaluate the performance of the proposed algorithm.

Fig.1 The two-dimensional reference frame

1 Fuzzy adaptive UKF algorithm

1.1 The model of the vessel heading measurement system

The horizontal motion of a surface ship is usually described by the motion components in surge, sway, and yaw. To describe the motion of the vessel, the North-East-Down and the body-fixed reference frames need to be built[14]. The related two-dimensional reference frame is shown in Fig.1.

As discussed above, vectorsPEandVbaredefinedas:

PE=[xyψ]T,Vb=[uvr]T.

(1)

Where variablesx,yandψrepresent surge, sway, and yaw respectively, and theu,vandrrepresent the corresponding speed variables respectively.

Neglecting the elements corresponding to heave, roll and pitch finally, the approximate relationship between the two vectors is shown in Eq.(1)：

(2)

Where the conversion matrixJ(ψ)is:

The differential equation of the heading is shown as:

(3)

We define new state vectorX=[ψ,r]T, whereψrepresents the yaw angle andrrepresents the yaw rate. So the vessel heading system is described as:

(4)

The observation equation is written as：

(5)

WhereW(t)is the process noise vector, andV(t)is the measurement noise vector.

Considering the simulation system, the sampling period is set toT=0.1 s, and then the system models (4) and (5) in discrete-time equivalent form leads to

X(k+1)=ΦX(k)+ΓW(k)，

(6)

Z(k)=HX(k)+V(k).

(7)

WhereΦ=[1 0.1 0 1],Γ=[0 0.1],H=[0 1].

In Eq.(6) and Eq.(7), both the vectorsW(k)andV(k)are zero-mean Gaussian white sequences having zero cross correlation with each other:

WhereQkis the process noise covariance matrix, andRkis the measurement noise covariance matrix.

1.2 Fuzzy adaptive unscented Kalman filter

1.2.1 Unscented Kalman filter

The unscented Kalman filter(UKF) is a recursive estimator based on the optimal Gaussian approximate Kalman filter framework. A nonlinear dynamic system is defined by Eq.(6) and Eq.(7), the UKF algorithm is summarized[15].

For:k=1,…,∞。

1) Calculate sigma-points:

(8)

(9)

(10)

Whereλ=n(α2-1),0≤α≤1andαshould ideally be a small number to avoid sampling non-local effects when the nonlinearities are strong.

2) Time-update equations:

ξi,k|k-1=Φ(ξi,k-1)，

(11)

(12)

Zi,k|k-1=H(ξi,k|k-1)，

(13)

(14)

3) Measurement-update equations:

(15)

(16)

(17)

(18)

(19)

(20)

1.2.2 Measurement noise covariance adjusting algorithm

The degree of divergence (DOD) parameters of the UKF for reflecting the change of measurements can be determined. The DOD parameterβ(k) can be defined as the trace of innovation covariance matrix at present epoch[1]:

(21)

(22)

Fig.2 The flow chart of the proposed fuzzy adaptive UKF

Wheremis the length of the time window andΛ(k)is the filter residual.

If the mathematical model of the system is accurate enough, the innovation sequence will be zero-mean white noise, and thenβ(k)andρ(k)will be close to zero. If these parameters drift from zero for a long-time, then the measurement noise covariance matrixR(k)will be adjusted.

The parametersβ(k)andρ(k)are the inputs of a fuzzy inference system (FIS), and the adjusting coefficientS(k)can be obtained by the FIS, and then theR(k)will be adjusted.

R(k)=Sb(k)R(k-1).

(23)

WhereR(k)is the measurement noise covariance matrix at timek, andbis a factor for adjusting the response rate.

The proposed fuzzy UKF is shown in Fig.2, whereβ(k)is denoted byβkand others are similar.

1.2.3 Fuzzy logic system

As discussed above, a FIS can be proposed for adjusting the measurement noise covariance. The fuzzy set ofβ(k)is {Z (zero), S (small), L (large)}, and the domain is [0 0.5]. The fuzzy set ofρ(k)is {Z (zero), S (small), L (large)}, and the domain is [0 1]. The fuzzy set ofS(k)is {D (decrease), M (maintain), I (increase)}, and the domain is [0.7 1.3].

Tab.1 Rules of fuzzy control

According to theoretical and empirical researches, the definition of the fuzzy control rules is shown in Tab.1.

As discussed above, the fuzzy inference system has been established.

1.3 Measurement data quality control algorithm

Based on the residual of the UKF, a data quality control algorithm is proposed. The data quality control function is defined as:

(24)

Measurement data quality control rules are defined as:

q(k)>TD, the measurement is bad.

q(k)≤TD, the measurement is good.

The thresholdTDcan be set based on the different accuracy requirements and the actual motion of the system.

1.4 Subsystem fault detection algorithm

For the multi-sensor data fusion systems, filter algorithm must have a real-time fault detection method[12]. In this paper, a fault detection algorithm is proposed based on the residual of the UKF. The fault detection functionα(k)is defined as:

(25)

WhereUi(k) is the innovation of the UKF.

Two hypotheses are defined: no-fault (H0) and fault (H1) as:

H0:αi(k)≈1

H1:αi(k)≠1

Supposeεrepresents the preset deviation, a fault detection interval is defined as:

Ω=[1-ε1+ε]

Ifαi(k)∉Ω, the subsystemihas a fault, and the innovation of the UKF is set to zero. Ifαi(k)∈Ω, the subsystemihas no fault, and the fusion are normal.

The fault factorsMi(k) are defined as:

Mi(k)=1, no fault occurs,

Mi(k)=0, a fault occurs.

The fault factors will be applied to the fusion algorithm to realize faults isolation.

2 Hierarchical fusion algorithm

In general, the state covariances of the local filters are usually applied to calculate the weighted factors for the global fusion, but only one parameter is difficult to reflect the true local filter performance. In order to improve the fusion accuracy and fault-tolerance, this paper adopts two state parameters of the local filter to realize two independent first-level fusions, and then the global integration for the first-level fusions can be obtained. The algorithm increases small computation complexity, but it improves the fusion performance. The proposed fusion algorithm is shown in Fig. 3.

2.1 The first-level fusion algorithms

Assuming the state estimateXi(k), the state covariancePi(k), and the parameterβi(k)of the local UKFihave been obtained. In this paper, the parametersPi(k)andβi(k)are chosen for the first-level fusions. Assuming the state estimates of the first-level fusions areXΙ1(k),XΙ2(k)respectively, then the first-level algorithms can be derived as below.

Fig.3 The proposed hierarchical fusion algorithm

2.1.1 Fusion algorithm using parameterPi(k)

This algorithm is widely used for multi-sensor data fusion. The weighted factorξi(k)can be computed:

(26)

WhereNis the number of sensors of the system.

For the subsystemi, assuming the state estimates isXi(k), the fault factor isMi(k)respectively, and then the state estimationXΙ1(k)can be calculated:

(27)

2.1.2 Fusion algorithm using parameterβi(k)

As parameterPi(k), the parameterβi(k)can also be applied to the fusion approaches. A fusion algorithm based on the parameterβi(k)can be built.

The weighted factorζi(k)can be calculated usingβi(k):

(28)

WhereNis the number of sensors of the system.

Similarly, the other state estimationXΙ2(k)can be calculated:

(29)

WhereXi(k)is the state estimate andMi(k)is the fault factor of the subsystemi, respectively。

2.2 The global fusion algorithm

The state estimatesXΙ1(k)andXΙ2(k)of the hierarchical fusions have been obtained. The weighted factors can be calculated using the covariance of these estimates for the second-level fusion. Assuming that the state estimation of the global fusion isX(k), the initializationX(0)is defined as:

X(0)=(XΙ1(1)+XΙ2(1))/2.

(30)

(31)

Wherei=1,2,…,N,X(k-1)is the state estimation of the system at timek-1,Nis the number of the first-level fusions.

The weighted factors ϖi(k) will be calculated:

(32)

The state estimateX(k)can be obtained:

(33)

WhereNis the number of the first-level fusion.

3 Simulation experiments

Simulation experiments have been carried out to evaluate the performance of the proposed algorithm in comparison with that of the other methods for multi-sensor fusion. Simulation experiment was conducted by a semi-physical experiment system which is shown in Fig. 4.

Fig.4 The semi-physical simulation system

By the experiment system, we can simulate the station keeping and the tracking control of the DPS. The data coming from the experiments which simulating the station keeping and the tracking control of the DPS are denoted by data_1, data_2, respectively. To clearly show the simulation results, one part of the data and one part of the simulation figures are only selected.

The initial values of the process noise covariance matrix is setQ0=[0.4] and the measurement noise covariance matrix is setR0=[0.01]. The initial values of the stateX0and the estimation error covariance matrixP0can be obtained by calculating the average of the first five measurements.

3.1 Simulation on the fuzzy adaptive UKF algorithm

3.1.1 Simulation on the adjusting measurement noise covariance

The simulation experiments have been performed to evaluate the performance of the proposed fuzzy adaptive UKF algorithm in comparison with that of the conventional UKF. Two algorithms are denoted by alg-a and alg-b, respectively. The simulations using data_1 and data_2 are shown in Fig. 5 and Fig. 6. The variablese,xandtwhich are in the simulation figures represent fusion error, state estimate and time, respectively, and this definition is also applicable to the following sections.

Fig.5 Comparison of two algorithms with data-1

Fig.6 Comparison of two algorithms with data-2

The simulation of the algorithm alg-a is shown as dashed line, and the algorithm alg-b is shown as solid line. In Fig. 5, when the motion of vessel changes sharply, the state estimation error for the algorithm alg-a is smaller than that for the algorithm alg-b. When the motion of the vessel changes small, the performance of the algorithm alg-a is not better than that of the algorithm alg-b, as shown in Fig. 6. It demonstrates that the algorithm for adaptively adjusting measurement noise covariance can apply to the environment which the vessel motion changes sharply. On this situation, the measurement noise varies heavily and the measurement noise covariance will be adjusted. When the motion of the vessel changes small, the measurement noise will change little, so in this case the measurement noise covariance does not require adjusting frequently.

3.1.2 Simulation on the fault detection algorithm

Fig.7 Simulation of the data quality control and fault detection algorithms

To validate the performance of the proposed algorithm for data quality control and subsystem fault detection, the data_1 is added 1° arbitrarily and the random faults are set with the fault rate of 8.3%. A single sensor system is selected to evaluate the performance of the proposed detection algorithm.

The proposed detection algorithm is denoted by alg-1 which is shown as solid line, and the algorithm that the wild points will not be detected and the fault sensor will be removed directly is denoted by alg-2 which is shown as dashed line. The simulation is shown in Fig.7. The simulation result indicates that the proposed data quality control and the subsystem fault detection approaches are effective.

3.2 Simulation on the multi-level fusions algorithm

To assess the performance of the proposed multi-level fusion algorithm, two fusion algorithms that are based on the credibility of the UKF which will be evaluated by a fuzzy reasoning system[9]and the conventional fusion algorithm using the state estimates error covariance are selected. These three algorithms are denoted by alg-4, alg-5, alg-6, respectively. The simulations on them are shown as thick solid line, thin solid line and dashed line. The simulation is shown in Fig.8 and Fig.9.

Fig. 8 Comparison of three fusion algorithms with data_1

Fig.9 Comparison of three fusion algorithms with data_2

The simulation results indicate that the fusion error of the proposed algorithm is smaller than those of the algorithm alg-6. The difference of the fusion error between alg-4 and alg-5 is not significant. This is because both the approaches use two state parameters of the filter to perform the fusion. Although the two methods are different, they both make full use of the more accurate performance of the filter for fusion, so they all have higher fusion accuracy. However, the computational complexity of the algorithm alg-4 is smaller than that of the algorithm alg-5, but it is somewhat larger than that of the conventional algorithm with the state covariance matrices.

4 Conclusions

In this paper a multi-sensor hierarchical fusion algorithm based on fuzzy adaptive UKF has been presented. The proposed algorithm improves the fusion accuracy and fault-tolerance.

A fuzzy adaptive UKF for the case of measurement malfunctions is developed. By the use of defined adaptive factor, faulty measurements are taken into consideration with small weight and the estimations are corrected without affecting the characteristic of the accurate ones. The fuzzy adaptive UKF is performed only in the case of malfunctions in the measurement system. According to the feature of the residual, the data quality control and the subsystem fault detection approaches are also built. These algorithms will improve the accuracy of the filter and fault-tolerance.

A hierarchical fusion algorithm has been proposed. This method will increase the virtual redundancy of the system by the hierarchical fusions using several state parameters of the fuzzy adaptive UKF, so the performance of the system will be improved.

[1]TSENG C H, CHANG C W, JWO D J. Fuzzy adaptive multiple model nonlinear filter for integrated navigation sensor fusion[J]. Sensors, 2011, 11(2): 2090-2111.

[2]JWO D J, LAI S Y. Navigation integration using the fuzzy strong tracking unscented Kalman filter[J]. Journal of Navigation, 2009, 62(2): 303-322.

[3]LI J, SONG N, YANG G, et al. Fuzzy adaptive strong tracking scaled unscented Kalman filter for initial alignment of large misalignment angles[J]. Review of Scientific Instruments, 2016, 87(7): 075118.

[4]DONG N, XU Y J, LIU X D. An IMM-UKF with adaptive factor for GPS/BD-2 satellite navigation system[J]. Journal of Astronautics, 2015, 36(6): 676-683.

[5]HAJIYEW C. Robust adaptive unscented Kalman filter for attitude estimation of pico satellites[J]. International Journal of Adaptive Control & Signal Processing, 2014, 28(2): 107-120.

[6]WANG G Y, GUAN B L. Fuzzy adaptive variational Bayesian unscented Kalman filter[J]. Journal of Information Hiding & Multimedia Signal Processing, 2015, 6(4): 740-749.

[7]ZHANG T, XU X S. Information fusion technology in integrated navigation system based on fuzzy adaptive algorithm[J]. Journal of Chinese Inertial Technology, 2007, 15(2): 197-201.

[8]TSENG C H, LIN S F, JWO D J. Fuzzy adaptive cubature Kalman filter for integrated navigation systems[J]. Sensors, 2016, 16(8): 1167.

[9]GAO H U, GAO S S, YAN Z, et al. Novel adaptive UKF and its application in integrated navigation[J]. Journal of Chinese Inertial Technology, 2014, 22(3):357-361.

[10]QI J, SONG D, WU C, et al. KF-based adaptive UKF algorithm and its application for rotorcraft UAV actuator failure estimation[J]. International Journal of Advanced Robotic Systems, 2012, 9(4): 1.

[11]METHOD W A, METHOD L S, ESTIMATION L, et al. Adaptive fusion design using multiscale unscented Kalman filter approach for multisensor data fusion[J]. Mathematical Problems in Engineering, 2015, 2015(8): 1-10.

[12]SUN Y, LIANG Z. Fault diagnosis and fault tolerant control for non-Gaussian singular time-delayed stochastic distribution systems with disturbance based on the rational square-root model[M].Coliro: Hindawi Publishing Corporation, 2016.

[13] LI W, JIA Y. Consensus-based distributed multiple model UKF for jump Markov nonlinear systems[J]. IEEE Transactions on Automatic Control, 2012, 57(1): 227-233.

[14] FOSSEN T I. Handbook of marine craft hydrodynamics and motion control[M]. Hoboken: John Wiley & Sons, 2011.

[15]RUDOLPH V D M. Sigma-point Kalman filters for probabilistic inference in dynamic state-space models[M].Portland: Oregon Health & Science University, 2004.

(责任编辑：傅 游)

2017-01-20

国家自然科学基金项目(41674037，61374126，61379029)；山东省自然科学基金项目(ZR2013FM021)；青岛农业大学高层次人才科研基金项目(6631430)

徐树生(1966—)，男，山东淄博人，副教授，博士，主要从事水下机器人、智能检测与控制、多源信息融合研究，本文通信作者.E-mail:xuss0319@163.com 李 娟(1969—)，女，教授，博士，主要从事智能检测与控制、嵌入式系统应用、故障诊断和容错控制等应用研究.

基于模糊自适应UKF的船舶动力定位多传感器融合

徐树生，李 娟

(青岛农业大学 机电工程学院，山东 青岛 266109)

为了改善船舶动力定位系统多传感器融合性能，基于模糊自适应UKF建立了一种多传感器分级融合算法。利用协方差匹配原理建立模糊自适应算法，利用新息建立测量质量控制算法和子系统故障诊断算法，将这三种算法与UKF相结合构成模糊自适应UKF算法。利用滤波状态参数构建第一级相互平行的融合算法，再基于相互独立的第一级融合性能建立第二级融合，从而构建基于模糊自适应UKF的多传感器分级融合算法，实现多传感器的动态分级融合。仿真结果验证了所建算法的有效性。

UKF；分级融合；模糊自适应；故障诊断

U666.1

A

1672-3767(2017)05-0007-09

10.16452/j.cnki.sdkjzk.2017.05.002