Keywords

1 Introduction

In recent years, the marine navigation systems have developed rapidly, following the development in measurement technology, and control systems and algorithms [12]. The accuracy and reliability of those systems have visibly increased, which allows them to be used to control such offshore operations as drilling, loading and offloading, or pipe-laying at increasing depths and during heavy environmental disturbances [11].

The main component of the marine navigation system is the dynamic positioning (DP) system [4, 5]. It is, generally, designed to increase the safety of navigation and to allow performing precise offshore operations. The DP system has two basic goals, which are: keeping the ship on the position, and following the reference trajectory.

The DP system performs ship positioning in three degrees of freedom: surge (forward motion), sway (transverse motion), and yaw (rotation about the vertical axis). It usually consists of: thruster control system, power management system, computer system, and sensors.

The thruster control system takes commands from the DP computer system and performs control and allocation of the thrusters. This system has been the subject of scientific analyses [15, 17].

The power management system is responsible for turning on/off electrical generators to meet ship electricity demands, and to prevent the blackout on the ship. The ship power management issues were the subject of a number of publications [18].

The computer system is the main power of each DP system. This element manages the whole system of the DP vessel. Its tasks include data collection and processing, and distributing relevant information over the entire DP system. This topic has also raised the interest of a number of research workers [2, 3].

The sensors are responsible for measuring position, velocity, acceleration, heading, and other ship motion parameters. The data collected by the sensor system are passed to the computer system, where data fusion algorithms process them to calculate the required values [9].

It frequently occurs that some parameter values cannot be obtained directly from measurements. In those cases the computer system makes use of a special observer algorithm to calculate the missing values [6]. For instance, the DP system requires the information about the ship position and velocity, but when the ship keeps the desired position, its velocity is very low and immeasurable. That is why systems of this type have an element which filters the measured position and the observation data of immeasurable ship velocity. In this paper, the velocities are calculated from the position data received from noisy GPS signals. There are two approaches to design filters and observers for DP systems. The first approach makes use of the mathematical model of the ship. The more accurate the model is, the better state observers/ filters for these systems may be designed. However, creating an accurate ship model is a difficult task, due to the action of environmental disturbances such as wind, wave, and sea currents. These disturbances introduce additional variables to the nonlinear ship model. The second approach, which is adopted in the article, makes use of a more complicated algorithm, which takes into account disturbances and ship models in a simplified way to create a filter/observer.

Common estimators used in the DP systems are: Nonlinear Observer (NO), Extended Kalman Filter (EKF) or, very rarely, Particle Filter (PF). The Kalman Filter was the first to be used in an advance control system. Then it was developed to the Extended Kalman Filter, which is used until today in many marine applications. It has been the object of study in numerous publications [4, 5, 10, 14]. The next estimator was the Nonlinear Observer proposed by Fossen and Strand [13]. This observer makes use of a nonlinear model of the ship and reveals very good quality of estimation. It was used in many marine applications [8, 13]. The last estimator, the Particle Filter [1, 8, 10] is not often used in control systems, due to relatively high structural complexity and resultant difficulties concerning real-time computer application.

In this paper, the idea of the Particle Kalman Filter (PKF) [7, 16], is developed. So far, filters of this type have never been applied in the DP system. The PKF is a combination of the Particle Filter and the Kalman Filter. The main task of filtering in the DP system is to observe ship velocities with high accuracy. The PKF algorithms enables to estimate the unmeasured states with a very small error with respect to the real state, what will be seen in the simulation results. The goal of this paper is to analyse the PKF making use of different ship models. The first model takes only into account ship kinematics, while the second one considers both kinematics and dynamics of the moving ship. The second model is more accurate, but both filters give almost similar results. In the simulations, the ship model and the white noise were used to generate the GPS signal. Estimating the ship position requires the information on propeller forces, which was taken in this case from position regulators.

2 Ship Model

The vessel model consists of two parts, which model ship kinematics and dynamics, respectively. These two parts make use of different reference frames. The position vector is defined in the earth-fixed frame as \( \varvec{\eta }=[x, y, \psi ]^T \) where x and y are ship position coordinates and \(\psi \) is the heading with reference to the X axis directed to the North. The velocity vector is defined in the coordinate system fixed to the vessel as \(\varvec{\upsilon }=[u, v, r]^T \). Here u and v are the surge and sway velocities, and r is the angular velocity. The relation between the velocity vector in the Earth frame and the body-fixed frame is expressed by the kinematic equation.

$$\begin{aligned} \dot{\varvec{\eta }}= & {} R(\psi )\varvec{\upsilon } \end{aligned}$$
(1)
$$\begin{aligned} R(\psi )= & {} \begin{bmatrix} cos(\psi )&-sin(\psi )&0\\ sin(\psi )&cos(\psi )&0 \\ 0&0&1 \end{bmatrix} \end{aligned}$$
(2)

The transformation is performed by using matrix \(R(\psi )\) which describes the rotation around the Z axis by \(\psi \) angle.

For dynamic positioning, the dynamic model at low velocities is given by:

$$\begin{aligned} M\dot{\varvec{\upsilon }}+D\varvec{\upsilon } = \varvec{\tau }_{control}+\varvec{\tau }_{wave}+\varvec{\tau }_{wind} \end{aligned}$$
(3)

where M is the inertia matrix. It contains the added mass coefficients which depend on the hull shape and body rigidity factor. D is the damping matrix caused by the linear wave drift damping and the laminar skin friction damping \(\varvec{\tau }_{control}\), \(\varvec{\tau }_{wave}\), \(\varvec{\tau }_{wind}\) are the force vectors generated by ship propellers, wind and wave, respectively [4].

3 Environmental Disturbances Model

The environmental forces generated by wind and waves are given by nonlinear equations [4].

The wind force \(\varvec{\tau }_{wind}=[X_{wind},Y_{wind},N_{wind}]^T\) is given by the following equations:

$$\begin{aligned} X_{wind}= & {} \frac{1}{2}C_{X}(\gamma _{r}) \varsigma _{a}V_{r}^{2}A_{T}\end{aligned}$$
(4)
$$\begin{aligned} Y_{wind}= & {} \frac{1}{2}C_{Y}(\gamma _{r}) \varsigma _{a}V_{r}^{2}A_{L}\end{aligned}$$
(5)
$$\begin{aligned} N_{wind}= & {} \frac{1}{2}C_{N}(\gamma _{r}) \varsigma _{a}V_{r}^{2}A_{L}L \end{aligned}$$
(6)

where \(C_X (\gamma _r)\), \(C_Y (\gamma _r)\), \(C_N (\gamma _r)\) are the nonlinear coefficients depending on the wind direction relative to the vessel. \(A_T\) and \(A_L\) are, respectively, the transverse ship surface and the side ship surface. The \(\varsigma _a \) is the air density, and L is the ship length. \(V_r\) is the wind velocity.

The wave forces \(\varvec{\tau }_{wave}=[X_{wave},Y_{wave},N_{wave}]^T\) is given by the following equations:

$$\begin{aligned} X_{wave}= & {} \sum { \varsigma _{w}gBLT_wcos \beta _{w} s_{i}(t)} \end{aligned}$$
(7)
$$\begin{aligned} Y_{wave}= & {} \sum { -\varsigma _{w}gBLT_wsin \beta _{w} s_{i}(t)} \end{aligned}$$
(8)
$$\begin{aligned} N_{wave}= & {} \sum { \frac{1}{24} \varsigma _{w}gBL(L^{2}-B^{2})cos \beta _{w} s_{i}(t)} \end{aligned}$$
(9)

where B is the ship breadth, \(T_w\) is the average draft of the ship considered as a parallelepiped, \(\varsigma _w\) is the sea water density, \(s_i (t)\) is the wave angle, \( \beta _w\) is the angle between the ship course and the wave direction, and \(s_i (t)\) is the density function of the i-th component \( \omega _{ei} \) of the meeting frequency. It is calculated from:

$$\begin{aligned} s_{i} (t ) =A_{i}k_{i} sin(\omega _{ei}t) \end{aligned}$$
(10)

where \(\omega _i\) is the amplitude of the harmonic wave of a given frequency. The frequency range \(\varDelta \omega \) represents the relationship:

$$\begin{aligned} A_{i} = \sqrt{2S(\omega _{i}) \varDelta \omega } \end{aligned}$$
(11)

The spectral wave density \(S(\omega _i)\) is calculated using the Pierson-Mostkowitz spectrum:

$$\begin{aligned} S(\omega _{i}) = \frac{4 \pi ^{2}H_{s}^{2}}{(0.710 T_{0})^{4}\omega _{i}^{5} }*exp (\frac{-16 \pi ^{2}}{(0.710 T_{0})^{4}\omega _{i}^{4} }) \end{aligned}$$
(12)

where \(T_0\) is modal period and \(H_s\) is the significant wave height.

The wave number is calculated from

$$\begin{aligned} k_{i}=\frac{\omega _{i}}{g} \end{aligned}$$
(13)

where g is the acceleration of gravity. The factor changing the meeting frequency \(\omega _{ei}\) with which the waves interact with the ship hull also depends on the speed. This frequency is given by the following formula:

$$\begin{aligned} \omega _{ei} = \omega _{i} - \frac{ \omega _{i}^{2} V cos(\beta _{w})}{g} \end{aligned}$$
(14)

where V is the resultant ship velocity vector.

4 Ship Kinematics Model for PKF

In the real world, not all vessels have access to the model of dynamics. In this situation, the observers can make use only of simple kinematics models. The first type of the Particle Kalman Filter (PKF 1) analysed in this paper makes use of the ship model which does not include such external disturbances as wind and waves.

The simplest model of kinematics of an arbitrary ship is given by:

$$\begin{aligned} {{\varvec{x}}}(k+1)= & {} A_{PKF1}{{\varvec{x}}}(k)+{{\varvec{w}}}\end{aligned}$$
(15)
$$\begin{aligned} {{\varvec{y}}}(k+1)= & {} C_{PKF1}{{\varvec{x}}}(k+1)+{{\varvec{v}}}\end{aligned}$$
(16)
$$\begin{aligned} A_{PKF1}= & {} \begin{bmatrix} \ 1&0&0&T&0&0&T^2/2&0&0 \\ 0&1&0&0&T&0&0&T^2/2&0 \\ 0&0&1&0&0&T&0&0&T^2/2 \\ 0&0&0&1&0&0&T&0&0 \\ 0&0&0&0&1&0&0&T&0 \\ 0&0&0&0&0&1&0&0&T \\ 0&0&0&0&0&0&1&0&0 \\ 0&0&0&0&0&0&0&1&0 \\ 0&0&0&0&0&0&0&0&1 \end{bmatrix}\end{aligned}$$
(17)
$$\begin{aligned} C_{PKF1}= & {} [I_{6x6},0_{6x3}] \end{aligned}$$
(18)

Here \({{\varvec{x}}}(k)=[x(k),y(k),\psi (k),\dot{x}(k),\dot{y}(k),\dot{\psi }(k),\ddot{x}(k),\ddot{y}(k),\ddot{\psi }(k)]\) and x(k), y(k), \(\psi (k)\) are the ship position and heading coordinates in the Earth-fixed reference frame, \(\dot{x}(k)\), \(\dot{y}(k)\), \( \dot{\psi }(k)\) are the longitudinal, transverse and rotational velocity, and \(\ddot{x}(k)\), \(\ddot{y}(k)\), \(\ddot{\psi }(k)\) are the ship acceleration components. The disturbances are added as \({{\varvec{w}}}\) and \({{\varvec{v}}}\).

5 Ship Dynamics Model for PKF

In the second approach, the ship dynamics model is assumed to be known, which allows to implement a more complicated model of the ship to the observer algorithm [4, 5]. The Particle Kalman Filter with the model of ship kinematics and dynamics (PKF 2) makes use of two coordinate systems, described as:

$$\begin{aligned} \dot{\varvec{\eta }}= & {} R (\psi )\varvec{\upsilon }\end{aligned}$$
(19)
$$\begin{aligned} M \dot{\varvec{\upsilon }} +D \varvec{\upsilon }= & {} \varvec{\tau }_{control} \end{aligned}$$
(20)

When the system (19) is used, for which \( R(\psi )^{T} = R(\psi )^{-1}\), it is possible to obtain \(\varvec{\upsilon }=R(\psi )\dot{\varvec{\eta }}\), subsequently: \(\dot{\varvec{\upsilon }} = R(\psi ) ^{T} \varvec{\ddot{\eta }}+R(\psi )^{T}\dot{\varvec{\eta }} \). Finally we get:

$$\begin{aligned} J(\varvec{\eta })\varvec{\ddot{\eta }}+C(\varvec{\eta },\varvec{\dot{\eta }})+F(\varvec{\eta })\varvec{\dot{\eta }} = \varvec{\tau }^{*} \end{aligned}$$
(21)

where the vessel model parameters are defined as

$$\begin{aligned} J(\varvec{\eta })= & {} R(\psi ) M R(\psi )^{T}\end{aligned}$$
(22)
$$\begin{aligned} F(\varvec{\eta })= & {} R(\psi )DR(\psi )^{T}\end{aligned}$$
(23)
$$\begin{aligned} C(\varvec{\eta },\varvec{\dot{\eta }})= & {} R(\psi ) M\dot{R}(\psi )^{T} \end{aligned}$$
(24)
$$\begin{aligned} \varvec{\tau }^{*}= & {} R(\psi ) \varvec{\tau }_{control} \end{aligned}$$
(25)

In the simulation, the state-space model of the ship was used, where \({{\varvec{x}}}= [\varvec{\eta };\dot{\varvec{\eta }}] \) are the state-space vectors. The generalized ship state-space model is:

$$\begin{aligned} \ddot{\varvec{\eta }}+J({\varvec{\eta }})^{-1}[C(\varvec{\eta },\dot{\varvec{\eta }})+F(\varvec{\eta })]\dot{\varvec{\eta }}= J^{-1}(\varvec{\eta })\varvec{\tau }^{*} \end{aligned}$$
(26)

The general ship model takes the form:

$$\begin{aligned} \dot{{{\varvec{x}}}}= & {} A_{PKF2}({{\varvec{x}}}){{\varvec{x}}}+B_{PKF2}({{\varvec{x}}}){{\varvec{u}}}+{{\varvec{w}}}\end{aligned}$$
(27)
$$\begin{aligned} {{\varvec{y}}}= & {} C_{PKF2}{{\varvec{x}}}+{{\varvec{v}}}\end{aligned}$$
(28)
$$\begin{aligned} A_{PKF2}({{\varvec{x}}})= & {} \begin{bmatrix} 0_{3x3}&I_{3x3} \\ 0_{3x3}&-J^{-1}({{\varvec{x}}})[C({{\varvec{x}}},\dot{{{\varvec{x}}}})+F({{\varvec{x}}})] \\ \end{bmatrix}\end{aligned}$$
(29)
$$\begin{aligned} B_{PKF2}({{\varvec{x}}})= & {} [0_{3x3} \ J^{-1}({{\varvec{x}}}) ]^T \end{aligned}$$
(30)
$$\begin{aligned} C_{PKF2}= & {} [I_{6x6}] \end{aligned}$$
(31)

where \({{\varvec{w}}}\) is the process noise sequence, and \({{\varvec{v}}}\) is the measurement noise sequence. The \({{\varvec{y}}}=(x,y,\psi )\) is the output state vector.

The above equations represent the continuous models of the ship. To implement it into the algorithm, the continuous model require discretisation. To obtain discrete samples for the state estimation algorithm, the nonlinear model (27), (28) is expanded into the Taylor series around the point \(\hat{{{\varvec{x}}}}\).

$$\begin{aligned} f[{{\varvec{x}}}(t),{{\varvec{u}}}(t)] = f[\hat{{{\varvec{x}}}}(t),{{\varvec{u}}}(t)] + \frac{\partial f[{{\varvec{x}}}(t),{{\varvec{u}}}(t)]}{\partial x(t)}|_{x=\hat{x}} +[{{\varvec{x}}}(t)-\hat{{{\varvec{x}}}}(t)] \end{aligned}$$
(32)

The discrete model of the ship without external disturbances takes the form:

$$\begin{aligned} {{\varvec{x}}}_{k+1} = A^D_{PKF2} {{\varvec{x}}}_{k} + {{\varvec{w}}}\end{aligned}$$
(33)
$$\begin{aligned} {{\varvec{y}}}_{(k+1)} = C_{PKF2}^{D} {{\varvec{x}}}_{k+1} +{{\varvec{v}}} \end{aligned}$$
(34)

where:

$$\begin{aligned} A^{D}_{PKF2}= & {} I_{6x6} + T\frac{\partial (A_{PKF2}({{\varvec{x}}}){{\varvec{x}}} + B_{PKF2}({{\varvec{x}}}) {{\varvec{u}}})}{\partial {{\varvec{x}}}}|_{x=\hat{x}}\end{aligned}$$
(35)
$$\begin{aligned} C^{D}_{PKF2}= & {} I_{6x6} \end{aligned}$$
(36)

In these equations T is the sampling time, and \(I_{n \times n}\) is the identity matrix of size \(n \times n\).

6 Particle Kalman Filter

The Particle Kalman Filter (PKF) algorithm is a combination of the Monte Carlo (MC) method with the Kalman Filter. The Particle Filter (PF) is used to filter the noisy states and the Kalman Filter (KF) is used to observe the non-measurable states. In order to develop the details of the algorithm we introduce the nonlinear function \(f_k\) of states which draws the state values to the next time step with normal distribution. \(h_k\) is the updated nonlinear function of measurements.

$$\begin{aligned} {{\varvec{x}}}_k^{i}= & {} f_k({{\varvec{x}}}_{k-1}^{i},{{\varvec{u}}}_{k-1}^{i},{{\varvec{w}}}_{k-1}^{i})\end{aligned}$$
(37)
$$\begin{aligned} {{\varvec{y}}}_k^{i}= & {} h_k ({{\varvec{x}}}_k^{i}) \end{aligned}$$
(38)

In the above equations, \({{\varvec{x}}}_k^{i}\) is the vector of state variables, \({{\varvec{y}}}_k^{i}\) is the vector of measurement, and \({{\varvec{w}}}_{k-1}^{i}\) are the noise estimations. Each sample with weight \(\{{{\varvec{x}}}_{0:k}^i,\varvec{\omega }_k^i \}_{i=1}^{N_s }\) depends on the random measure of the posterior probability density function, \(p({{\varvec{x}}}_{0:k}|{{\varvec{y}}}_{1:k}^{m})\) where \(\{{{\varvec{x}}}_{0:k}^i,i=0,\dots ,N_s\}\) is the set of support points with associated weights \(\{\varvec{\omega }_k^i,i=0,\dots ,N_s\}\), and \({{\varvec{x}}}_{0:k}=\{x_j,j=0,\dots ,k\} \) represents the state-space in time. \(N_s\) is the number of the sample.

$$\begin{aligned} p({{\varvec{x}}}_{0:k}|{{\varvec{y}}}_{1:k}^{m}) = \sum _{i=1}^{N_{s}} \varvec{\omega }_k^i({{\varvec{x}}}_{0:k}-{{\varvec{x}}}_{0:k}^i) \end{aligned}$$
(39)

Here \({{\varvec{x}}}_{0:k}\) is the sample state from 0 to k, \({{\varvec{x}}}_{0:k}^i\) is the i-th drawn sample at the sample time 0 : k. Using normalized weights \(\sum \omega _k^i=1 \) the \({{\varvec{y}}}_k\) is given by the equation:

$$\begin{aligned} {{\varvec{y}}}_k=\sum _{i=1}^{N}\varvec{\omega }_k^ih({{\varvec{x}}}_k^i) \end{aligned}$$
(40)

In this algorithm the weights are calculated from normal distribution:

$$\begin{aligned} \varvec{\omega }_k^i= \frac{1}{\sqrt{2\pi x_N}}exp(\frac{(-{{\varvec{y}}}^{{\varvec{m}}}_{{\varvec{k}}}-{{\varvec{y}}}^{{\varvec{i}}}_{{\varvec{k}}})^2}{4}) \end{aligned}$$
(41)

where \(x_N\) is the variance measure, \({{\varvec{y}}}^m_k\) is the variable measure, and \({{\varvec{y}}}_k^i\) is the calculated output from the i-th sample at time k. The measurement vector is \( {{\varvec{y}}}_k^m=(x_k^m,y_k^m,\psi _k^m)\), where \(x_k^m\), \(y_k^m\) are the simulated measures of coordinates obtained from GPS, \(\psi _k^m\) is the simulated measurement of the vessel course.

Due to sample degradation, the resampling algorithm is implemented. The first step of resampling is weight normalization \(\sum \varvec{\omega }_k^i=1\). The algorithm draws samples with high weights from all particles [1]. Then the particles with low weights are replaced. The output weight of all samples is set equal to \(1/N_s\). The final values from the PF part are calculated using (40). These values are then used as the input values to the KF part.

The KF algorithm works in a loop and performs the following calculations:

$$\begin{aligned} K_k= & {} P_kH^T(HP_kH^T+R_k)^{-1}\end{aligned}$$
(42)
$$\begin{aligned} \hat{{{\varvec{x}}}}_k= & {} {{\varvec{x}}}_k + K_k({{\varvec{z}}}_k-H{{\varvec{x}}}_k)\end{aligned}$$
(43)
$$\begin{aligned} \hat{P}_{k}= & {} P_k - K_k HP_k \end{aligned}$$
(44)
$$\begin{aligned} P_{k+1}= & {} A \hat{P}_kA^T+ Q_k \end{aligned}$$
(45)
$$\begin{aligned} {{\varvec{x}}}_{k+1}= & {} A \hat{{{\varvec{x}}}}_k \end{aligned}$$
(46)

There are two stages in the loop: prediction and correction. In the above formulas, \(K_k\) is the Kalman gain matrix, \({{\varvec{x}}}_k\) is the state estimate update, \(P_k\) is the error covariance update, \(P_{k+1}\)is the error covariance propagation, \(\hat{{{\varvec{x}}}}_k\) is the state estimate propagation, and \({{\varvec{z}}}_k\) is the output from PF. A is the discrete model of state transition. \(Q_k\) is the covariance matrix of the process, and \(R_k\) is the covariance matrix of the measurement. In the simulation, the resampling algorithm described in [1] was used.

The pseudo-code of the proposed algorithm is shown below. The numbers on the right side refer to the equations used in the algorithms.

figure a

In the algorithms, H is equal to (18). The sampling period T of both algorithms is equal to 0.01 s.

7 Simulation and Experimental Results

This section presents the results of two simulation tests:

  1. 1.

    In the first simulation test, the velocities were set equal to zero and the states were observed while position keeping.

  2. 2.

    In the second simulation test, the velocities were set from 0 to 2 m/s and the states were observed while manoeuvring.

Table 1. The mean square error of the estimator with respect to the real value
Full size table

As can be seen from Table 1, both filters, with and without ship dynamics, return similar results of position filtering. Table 1 compares the mean square errors received for both filters in tests 1 and 2. As can be noticed, the filter with ship dynamics gives sometimes better results, but not in all cases. The values observed during the tests are almost the same in both cases. As shown in Figs. 1, 2, 3, and 4, the observers produce overshoots during manoeuvres in test 2, while in test 1 the overshoots are small. This is connected with the fact that the filters have slow dynamics. Figures 5 and 6 show filtering capabilities of both filters. Despite high interference, the output values from the filters reveal good quality and have small errors with respect to the real values.

Fig. 1.
figure 1

Longitudinal velocity: real and observed from PKF1, test 1

Full size image
Fig. 2.
figure 2

Longitudinal velocity: real and observed from PKF2, test 1

Full size image
Fig. 3.
figure 3

Longitudinal velocity: real and observed from PKF1, test 2

Full size image
Fig. 4.
figure 4

Longitudinal velocity: real and observed from PKF2, test 2

Full size image
Fig. 5.
figure 5

Measured, real and observed position along the X axis, test 1

Full size image
Fig. 6.
figure 6

Measured, real and observed position along the X axis, test 2

Full size image

8 Conclusion

The paper presents the Particle Kalman Filter designed in two variants: with and without dynamic equations. As can be seen, both filters return similar results of position filtering and velocity observation, therefore it seems possible to use the proposed filter without dynamics for observation of objects moving at low velocity. The possibility for more precise modelling and improving the dynamics of the filter during manoeuvres (with respect to the overshoot and velocity of the observed signal) lies in changing the window width to search the variables and in changing the number of samples. In the here reported simulation, it took about 40 [s] for the computer to simulate the ship behaviour covering a period of 300 [s]. This means that the filter can effectively work as a real-time algorithm. The simulations were performed for 100 samples of variable signals. The algorithm is scalable and can be adapted to the computing power, as the running time of the algorithm depends linearly on the number of samples per one iteration.