An Adaptive Unscented Kalman Filter for Tracking Sudden Environmental Forces Changes in Dynamic Positioning System

DING Hao-han,FENG Hui,XU Hai-xiang

(a.Key Laboratory of High Performance Ship Technology of Ministry of Education;b.School of Transportation,Wuhan University of Technology,Wuhan 430063,China)

An Adaptive Unscented Kalman Filter for Tracking Sudden Environmental Forces Changes in Dynamic Positioning System

DING Hao-hana,b,FENG Huia,b,XU Hai-xianga,b

(a.Key Laboratory of High Performance Ship Technology of Ministry of Education;b.School of Transportation,Wuhan University of Technology,Wuhan 430063,China)

Unscented Kalman filter was widely used in dynamic positioning system since it can reconstruct unmeasured states as well as remove white and colored noise from the state estimates.This paper aims to solve the problem that unscented Kalman filter is unable to track sudden changes of the states of a vessel when it faces the extreme sea environment,while the dynamic positioning system requires to estimate these states accurately and instantaneously.By identifying the instant of a sudden change and appropriately adjusting the estimated covariance matrix,an adaptive unscented Kalman filter was proposed,which is able to track the states of vessel and reduce the deviation of the low-frequency position of the vessel efficiently.Numerical simulations show the effectiveness of the proposed scheme.

dynamic positioning system;adaptive unscented Kalman filter; environment sudden changes

0 Introduction

In dynamic positioning system,it is essential to estimate a system’s state when deciding upcoming control actions.Since most systems have a dual nature,including hidden states and measured variables,estimation is especially necessary to get full state knowledge.Due to the nonlinear property of vessel’s motion,one of the most common approaches for estimating the state of vessel has been the extended Kalman filter(EKF)[1-2].In this approach,each nonlinear step is replaced by its linearized equivalent to deal with the nonlinear model.Although the EKF is very simple,in practice,it has some well-known demerits:if the time step intervals are not sufficiently small,the filter’s performance would be highly unstable which is resulted from linearization,meanwhile,the derivation of Jacobian matrices is very hard and often leads to significant implementation difficulties.Another approach that has been effectively used to deal with the nonlinear terms is unscented Kalman filter(UKF)which was first suggested byJulier et al[3].This filter produces predictions of states and covariance which are provably more accurate than those of the EKF but without the need to calculate Jacobian matrices.Thus,UKF has been used in many different applications[4-8].However,the capability of this filter in tracking sudden changes is insufficient,which will have a great deteriorating effect on the estimation procedure when the system parameters of the vessel have a sudden change.To solve this problem,Bisht et al[9]presented an adaptive unscented Kalman filter(AUKF)for tracking sudden stiffness changes.This approach can identify and track sudden changes in the stiffness values of structural members that could possibly occur in structural system.Yang et al[10-12]proposed several adaptive tracking techniques which were based on the least-squares estimation approach for tracking sudden and gradual changes in system parameters as well.

Dynamic positioning system is a complex nonlinear system,the wind,wave and current will have a great impact on the positioning accuracy while high precision is required in some occasions.State estimation is crucial for dynamic positioning system since it can estimate the state of a nonlinear dynamic system from a series of noisy measurements.The environment of ocean changes unpredictably and the sudden change of the environmental force always leads the vessel deviate the working position.However,the UKF is not geared to track sudden parameter changes,which will deteriorate the positioning accuracy.The main contribution of this paper is that proposing an adaptive unscented Kalman filter in dynamic positioning system to track the sudden changes in environmental forces caused by wind,wave and current.By detecting the sudden changes and adjusting the estimated covariance matrix to track the change effectively.

The paper is organized as follows:Chap.2 gives the mathematical modelling of ships.The review of unscented Kalman filter is presented briefly in Chap.3.In Chap.4,the adaptive unscented Kalman filter in dynamic positioning system is proposed.Chap.5 is the simulation results and discussion.The paper ends with the conclusion.

1 Mathematical modelling of ships

1.1 Kinematic equations of motion

We often use earth-fixed frame XEYEZEand vesselfixed frame XYZ to describe the motion of the vessel[13-14]. In earth-fixed frame,position（x,y）and heading ψ of the vessel are expressed in vector form by η=[x,y,ψ]T.In vessel-fixed frame,the decomposing velocities are represented by the state vector ν=[u,v,r]Tand the origin of this frame is located at the vessel center line in a distance xGfrom the center of gravity,see Fig.1.The transformation between the vessel-fixed and the earth-fixed velocity vectorsis:

Noticeably,R（ψ）is non-singular for all ψ and it is clear that R-1（ψ）=RT（ψ）.

1.2 Ship modelling

The low frequency motion of a large class of vessels at low speed can be described by the following model:

where τ∈R3is a vector consisting of control forces and moment and b∈R3containing all unmodeled forces and moments caused by wind,currents and waves.u∈R3is the control inputs and B∈R15×3is a constant matrix describing the actuator configuration.The form of inertia matrix M∈R3×3is:

where m is the mass of vessel,Izis the moment of inertia about the vessel-fixed z-axis.Xu˙,Yυ˙, Yr˙,Nυ˙and Nr˙are added mass and inertia terms.The linear damping matrix D∈R3×3can be written as

where the cruise speed u0=0 in DP and u0＞0 when moving forward.Generally,the damping forces will be nonlinear.However,linear damping is a good assumption for DP and cruising at low constant speed.

1.3 First-order wave-induced model

Generally,a linear wave frequency model of order p can be described as:

where ξ∈R3×p,ww=［ww1,ww2,ww3］Tis a zero-mean Gaussian white noise and Aw,Ew,Cware constant matrices of appropriate dimensions.ηw=［xw,yw,ψw］Tis the first-order wave-induced motion.The total ship motion is the sum of ηwand η.Here,the first-order wave-induced motions are approximated by a second-order wave model and the state space realization of sec-ond-order wave model in 3 degrees of freedom can be written as

where ξ1∈R3,ξ2∈R3and:

where ω0i（i=1,2,3） is the dominating wave frequency,ζi（i=1,2,3）is the relative damping ratio and σi（i=1,2,3）is a parameter related to the wave intensity.

1.4 Slowly-varying environmental disturbances

It is assumed that the environmental forces caused by wind,waves and currents are slowly varying and frequently modeled as a first-order Markov process:

where b∈R3is the slowly-varying environmental forces and moment,wb∈R3is a zero-mean Gaussian white noise,T∈R3×3is a diagonal matrix of positive time constants and Eb∈R3×3is a diagonal matrix scaling the amplitude of wb.

1.5 Measurement system

It is assumed that for conventional ships only position and heading measurements are available,leading to:

where νy∈R3is zero-mean Gaussian white noise.

1.6 Total system model

The total system model can be written as

The corresponding state-space form is denoted by:

where

2 Unscented Kalman filter

Before introducing the adaptive unscented Kalman filter we first describe the traditional unscented Kalman filter to provide a better understanding of the proposed modifications in adaptive unscented Kalman filter.The basic steps of traditional unscented Kalman filter can be depicted as follows[15]:

(1)Initialization:given the initial value of state vector X0and estimated covariance matrix P0.

(2)The calculation of sigma points as well as their corresponding weight:

where n is the dimension of the state vector x,λ=ξ2（n+κ）-n is the scale factor,in which ξ is the range of the sigma point and κ=0 in general.For a Gaussian random variable,gives η=2.

(3)Time-update:

where f（·）represents the nonlinear function and Qk∈Rn×nis the covariance matrix of process noise.

(4)Measurement-update:

where the Zkis the measurement at time step k and R∈R3×3is the covariance matrix of measurement noise.Kkis the Kalman gain which is defined in terms of the cross-covariance Pxk,zkand the covariance of the error,Pzk,zk.

3 Adaptive unscented Kalman filter

The equations defined in Chap.3 show that the conventional unscented Kalman filter have the ability to track the states of the system even if it is a nonlinear process.However,it is unable to track the sudden changes on parameter values because it can not adjust itself to estimate the impact of the sudden change on the system.However,it is common that the environmental forces change suddenly when the vessel encounters the extreme sea environment and this change will deteriorate the accuracy of the dynamic positioning system.So,we need to track this change and it is noticeable that if we readjust the estimated covariance matrix properly,the estimated value can converge to the correct value in a shorter period.To do that, it is necessary to detect the sudden changes as soon as possible,and then adjust the estimated covariance matrix to hasten the change tracking.

3.1 Sudden change detection

To identify the sudden change of environmental forces,we need to select a threshold△0and when△＞△0means a sudden change.For one thing,we select the maximum value of delta in 1 000 periods which did not have sudden change in environmental forces to ensure that fewer wrong identifications of sudden change.For another,to avoid the estimated covariance matrix is increased many times in several consecutive time steps,we set 100 periods as the convergence period in which the estimated values are reaching the real values.Hence,when the estimated covariance matrix enlarge,it will not be increased again within 100 periods.

3.2 Enhanced covariance matrix

When the sudden change has been identified,we modify the diagonal value of the estimated covariance matrix corresponding to the environmental forces to allow the filter algorithm to track this change.We call the modified covariance matrix as enhanced covariance matrix. From the Eq.(14)we can notice that if we increase the estimated covariance matrix, the Kalman filter gains Kkwill increase accordingly,which means that we want to rely more on the precision of the measurements but less on the model,vice versa.Therefore,when the environmental forces change,we need to enlarge the gains Kkto rely more on the measurements since the model is unable to track the change adaptively.In addition,from Refs.[16-17]we can get some guidance about the minimum and maximum limits of the gains in RLS approach.Herein,aproper value should be detemined by which the covariance values calculated just before the change are multiplied.

Fig.2△values for 1 000 periods when environmental forces have a sudden change in time step 400

Fig.3 The overall estimation scheme for the adaptive unscented Kalman filter

3.3 Integration scheme

The adaptive unscented Kalman filter for dynamic positioning system can be summarized as follows:

(1)Given the initial value of state vector x0and estimated covariance matrix P0and set k0=0.

(3)Calculating△,if△＞△0and k+1-k0＞100 go to step 4,else k=k+1,go to step 2.

(4)Modifying the diagonal value of the estimated covariance matrix Pk+1corresponding to the environmental forces and set k0=k+1.

(5)k=k+1,Go to 2.

The overall scheme of adaptive unscented Kalman filter can be seen in Fig.3.

4 Simulation results

Consider that sometimes the environmental forces will last a long time(named as case 1) while sometimes it only continues a short time(denoted by case 2),we perform simulations on these two cases respectively.

In case 1,the environmental forces suddenly change from[0,0,0]to[5,-5,5]in the 400th period and last to the end of the simulation while in case 2,sudden change on the environmental forces is observed from[0,0,0]to[5,-5,5]in the 400th period and is vanished in the 700th period.The initial position is[0,0,0]and the set position is[5,5,15°] and the vessel will arrive at the set position at 300th period.We choose the step size as 0.5 s. In this model,△0=8.3 was used.The simulation use a model of a platform supply vessel and the main dimensions of this ship model are displayed in Tab.1.

The inertia matrix and damping matrix of this vessel are:

Tab.1 The main dimensions of this ship model

It is assumed that the process noise and measurement noise covariance matrices are:Q= diag(0,0,0,0.01,0.01,0.000 1,0,0,0,0,0,0,0.000 1,0.0001,0.000 1),R=diag(0.001, 0.001,0.000 1).The time constants matrix is T=diag(1 000,1 000,500).Further,the domi-nating wave frequency,relative damping ratio and wave intensity are chosen as ω01=ω02=ω03= 0.8,ζ1=ζ2=ζ3=0.3 and σ1=σ2=σ3=1.At last,we add a PID algorithm after the filter algorithm to provide the control forces in each time step.

Fig.4 Environmental force of sway in case 1

Fig.5 Environmental force of sway in case 2

Fig.6 Position of sway using UKF in case 1

Fig.7 Position of sway using AUKF in case 1

Fig.8 Position of sway using UKF in case 2

Fig.9 Position of sway using AUKF in case 2

Fig.4 and Fig.5 compared the estimation of environmental force in sway between UKF and AUKF in case 1 and case 2.The low-frequency position,estimated position and measured position in sway between UKF and AUKF in case 1 are shown in Fig.6 and Fig.7 while the low-frequency position,estimated position and measured position in sway between UKF and AUKF in case 2 are given in Fig.8 and Fig.9.It is clear that the deviation of the low-frequency position using AUKF is less than that using UKF due to the convergence rate of AUKF isfaster than that of UKF.It is because the controller will have a better response if the estimator can track the sudden changes in environmental forces effectively.Thus,the simulation results show the validity of the adaptive unscented Kalman filter and prove that this scheme is suitable to the dynamic positioning system.

5 Conclusions

In this paper,an adaptive unscented Kalman filter was introduced for the dynamic positioning system in order to identify and track sudden changes in environmental forces that could happen in complex ocean environment.The scheme first detects the sudden change through the definition of the indicator delta,and then adjusts the estimated covariance matrix to track the change.The validity of this scheme is illustrated by simulations of two cases.The presented simulation results clearly show that this scheme can accurately track the short term or long term sudden changes of environmental forces in dynamic positioning system.

[1]Jouffroy J,Fossen I.A tutorial on incremental stability analysis using contraction theory[J].Modeling,Identification and Control,2010,31(3):93-106.

[2]Fossen T I.Handbook of marine craft hydrodynamics and motion control[M].John Wiley&Sons,2011.

[3]Julier S J,Uhlmann J K,Durrant-Whyte H F.A new approach for filtering nonlinear systems[C].American Control Conference,Proceedings of the 1995.IEEE,1995,3:1628-1632.

[4]Julier S J,Uhlmann J K.Unscented filtering and nonlinear estimation[J].Proceedings of the IEEE,2004,92(3):401-422.

[5]Zhang S C,Hu G D,Liu S H.Target tracking for maneuvering reentry vehicles with reduced sigma points unscented Kalman filter[C]//Systems and Control in Aerospace and Astronautics,2006.ISSCAA 2006.1st International Symposium on.IEEE,2006:4-202.

[6]Sun F,Hu X,Zou Y,et al.Adaptive unscented Kalman filtering for state of charge estimation of a lithium-ion battery for electric vehicles[J].Energy,2011,36(5):3531-3540.

[7]Cheng J,Chen D,Landry Jr R,et al.An adaptive unscented Kalman filtering algorithm for MEMS/GPS integrated navigation systems[J].Journal of Applied Mathematics,2014:.

[8]Hajiyev C,Soken H E.Robust adaptive unscented Kalman filter for attitude estimation of pico satellites[J].International Journal of Adaptive Control and Signal Processing,2014,28(2):107-120.

[9]Bisht S S,Singh M P.An adaptive unscented Kalman filter for tracking sudden stiffness changes[J].Mechanical Systems and Signal Processing,2014,49(1):181-195.

[10]Yang J N,Lin S.On-line identification of non-linear hysteretic structures using an adaptive tracking technique[J].International Journal of Non-Linear Mechanics,2004,39(9):1481-1491.

[11]Yang J N,Lin S.Identification of parametric variations of structures based on least squares estimation and adaptive tracking technique[J].Journal of Engineering Mechanics,2005:.

[12]Yang J N,Huang H,Lin S.Sequential non-linear least-square estimation for damage identification of structures[J].International Journal of Non-linear Mechanics,2006,41(1):124-140.

[13]Fossen T I,Strand J P.Passive nonlinear observer design for ships using Lyapunov methods:Full-scale experiments with a supply vessel[J].Automatica,1999,35(1):3-16.

[14]Snijders J G,van der Woude J W,Westhuis J.Nonlinear observer design for dynamic positioning[C].Dynamic Positioning Conference,2000.

[15]Shi X,Sun X,Fu M,et al.An Unscented Kalman Filter based wave filtering algorithm for dynamic ship positioning[C]. Automation and Logistics(ICAL),2011 IEEE International Conference on.IEEE,2011:399-404.

[16]Landau I D,Lozano R,M'Saad M,et al.Adaptive control[M].Berlin:Springer,1998.

[17]Lin J W,Betti R,Smyth A W,et al.On-line identification of non-linear hysteretic structural systems using a variable trace approach[J].Earthquake Engineering&Structural Dynamics,2001,30(9):1279-1303.

動力定位中跟蹤環(huán)境力突變的自適應(yīng)無跡卡爾曼濾波

丁浩晗a，b，馮輝a，b，徐海祥a，b
（武漢理工大學(xué)a．高性能船舶技術(shù)教育部重點實驗室；b．交通學(xué)院，武漢430063）

無跡卡爾曼濾波可以在狀態(tài)估計中濾去噪聲干擾，已經(jīng)被廣泛應(yīng)用于動力定位系統(tǒng)中。針對復(fù)雜海洋情況下動力定位系統(tǒng)需要準(zhǔn)確、及時地估計當(dāng)前時刻的狀態(tài)而無跡卡爾曼濾波無法跟蹤狀態(tài)突變的問題，為此文章提出了一種自適應(yīng)無跡卡爾曼濾波。通過及時判斷狀態(tài)值突變并適當(dāng)調(diào)整后驗均方差矩陣，可有效地跟蹤船舶狀態(tài)并減小實際位置與定點位置的偏差。仿真實驗證明了算法的有效性。

動力定位；自適應(yīng)無跡卡爾曼濾波；環(huán)境力突變

U662.9

:A

國家自然科學(xué)基金項目資助（61301279;51479158）

丁浩晗（1992-），男，武漢理工大學(xué)交通學(xué)院船舶與海洋工程專業(yè)碩士；

U662.9

:A

10.3969/j.issn.1007-7294.2017.06.006

1007-7294（2017）06-0711-11

馮輝（1981-），男，武漢理工大學(xué)交通學(xué)院副教授；

date：2017-01-15

Supported by the National Natural Science Foundation of China(61301279,51479158)

Biography：DING Hao-han(1992-),male,master candidate;FENG Hui(1981-),male,Ph.D.,associate professor, corresponding author,E-mail:wuyun8210@163.com;XU Hai-xiang(1975-),male,Ph.D.,professor.

徐海祥（1975-），男，武漢理工大學(xué)交通學(xué)院教授。