Intelligent Industrial Systems

, Volume 1, Issue 2, pp 99–126 | Cite as

Distributed Control of Unmanned Surface Vessels Using the Derivative-free Nonlinear Kalman Filter

Original Paper

Abstract

Intelligence and autonomy is becoming a prerequisite for maritime transportation systems. In this paper a distributed control problem for unmanned surface vessels (USVs) is formulated as follows: there are N USVs which pursue another vessel (moving target). Each USV can be equipped with various sensors, such as IMU, cameras and non-imaging sensors such as sonar, radar and thermal signature sensors. At each time instant each USV can obtain measurements of the target’s cartesian coordinates. Additionally, each USV is aware of the target’s distance from a reference monitoring station (coastal or satellite monitoring units). The objective is to make the USVs converge in a synchronized manner towards the target, while avoiding collisions between them and avoiding collisions with obstacles in their motion plane. A distributed control law is developed for the USVs which enables not only convergence of the USVs to the goal position, but also makes possible to maintain the cohesion of the multi-USV system. Moreover, distributed filtering is performed, so as to obtain an estimate of the target vessel’s state vector. This provides the desirable state vector to be tracked by each one of the USVs. To this end, a new distributed nonlinear filtering method of improved accuracy and computation speed is introduced. This filtering approach, under the name Derivative-free distributed nonlinear Kalman filter is based on differential flatness theory and on an exact linearization of the target vessel’s dynamic/kinematic model. The performance of the proposed distributed filtering scheme is compared against the Extended and the Unscented Information filter.

Keywords

Distributed control Lyapunov stability Unmanned surface vessels Target tracking Distributed nonlinear Kalman filtering 

Introduction

Distributed control of unmanned surface and underwater vessels has received significant attention during the last years [1, 2, 3, 4, 5, 6]. In this paper a solution is developed for the problem of distributed control of cooperating unmanned surface vessels (USVs) which pursue a target vessel. The distributed control aims at achieving the synchronized convergence of the USVs towards the target and at maintaining the cohesion of the USVs swarm, while also avoiding collisions between the individuals USVs and collisions between the USVs and obstacles in their motion plane. To estimate the motion characteristics of the target vessel, distributed filtering is performed. Actually, each vessel is supplied with equipment which permits to measure the coordinates of the target vessel, such as IMU and cameras, as well as with sonar, radar and thermal signature sensors. Besides each USV receives information about the distance of the target vessel from a reference surface, the latter being provided by a coastal or a satellite-based measurement unit. By fusing the aforementioned measurements through a filtering procedure an estimate of the state vector of the target vessel is obtained. Next, to obtain an estimate of improved precision about the motion characteristics of the target vessel distributed filtering is performed which fuses the distributed state vector estimates into one single estimate.

To treat the distributed control problem for the cooperating USVs a Lyapunov theory-based method is introduced. Motion planning for the individual USVs is determined by the minimization of a Lyapunov function which comprises a quadratic term associated with the distance of each USV from the target vessel, as well as quadratic terms which are associated with the distance of the USVs between each other. By applying LaSalle’s theorem it is proven that the USVs will follow the target’s motion while remaining within a small area that encircles the target.

To treat the distributed filtering and state estimation in the multi-USV system one can apply established methods for decentralized state estimation, such as the extended information filter (EIF) and the Unscented Information Filter (UIF). EIF stands for the distributed implementation of the Extended Kalman Filter while UIF stands for the distributed implementation of the Unscented Kalman Filter. Moreover, to obtain a distributed filtering scheme in this paper the Derivative-free Extended Information Filter (DEIF) is implemented. This stands for the distributed implementation of a differential flatness theory-based filtering method under the name Derivative-free distributed nonlinear Kalman Filter [7, 8, 9, 10, 11]. The improved performance of DEIF comparing to the EIF and UIF is confirmed both in terms of higher estimation accuracy and in terms of elevated speed of computation.

The structure of the paper is as follows: In “Target Tracking Mobile Sensor Networks” section the problem of tracking of a target ship by multiple USVs is formulated. In “Distributed Motion Planning for the Multi-USV System” section a solution is provided to the problem of distributed control and distributed motion planning of the system of the multiple USVs. In “Distributed State Eestimation Using the Extended Information Filter” section the EIF is proposed as a solution of the problem of distributed state estimation for the multi-USVs system. In “Distributed State Estimation Using the Unscented Information Filter” section the UIF is analyzed and proposed as an alternative solution for the problem of distributed state estimation for the multi-USVs model. In “Filtering Using Differential Flatness Theory and Canonical Forms” section differential flatness theory and diffeomorphisms lead ing to canonical state space forms are used to develop a distributed filtering method of improved performance, under the name Derivative-free Extended Information filtering (DEIF). In “Simulation Tests” section simulation tests are provided to confirm the stability of the distributed control method for tracking of the target-ship by the fleet of the multiple UAVs and also to confirm the improved performance of DEIF against the EIF and UIF. Finally, in “Conclusions” section concluding remarks are stated.

Target Tracking in Mobile Sensor Networks

The Problem of Distributed Target Tracking

It is assumed that there are N USVs (unmanned surface vessels) with positions \(p_1,p_2,\ldots ,p_N \ \in R^2\) respectively, and a target with position \(x^{*} \in R^2\) moving on the sea surface (see Fig. 1). Each USV can be equipped with various sensors, such as IMU, cameras, sonar, radar and thermal signature sensors. The USVs can be considered as mobile sensors while the fleet of the autonomous vessels constitutes a mobile sensors network. The discrete-time target’s kinematic model is given by
$$\begin{aligned} x_t(k+1)= & {} \phi (x_t(k))+L(k)u(k)+w(k)\nonumber \\ z_t(k)= & {} \gamma (x_t(k))+v(k) \end{aligned}$$
(1)
Fig. 1

Distributed control for tracking of the target vessel by synchronized USVs

where \(x{\in }R^{m{\times }1}\) is the target’s state vector and \(z{\in }R^{p{\times }1}\) is the measured output, while w(k) and v(k) are uncorrelated, zero-mean, Gaussian zero-mean noise processes with covariance matrices Q(k) and R(k) respectively. The operators \(\phi (x)\) and \(\gamma (x)\) are defined as \(\phi (x)=[{\phi _1}(x),{\phi _2}(x),\ldots ,{\phi _m}(x)]^T\), and \(\gamma (x)=[{\gamma _1}(x),{\gamma _2}(x),\ldots ,{\gamma _p}(x)]^T\), respectively.

At each time instant each USV can obtain a measurement of the target’s position. Additionally, each USV is aware of the target’s distance from a reference surface measured in an inertial coordinates system (this can be a measurement provided by a coastal observation unit or equivalently by a satellite station). Finally, each USV can be aware of the positions of the rest \(N-1\) USVs. The objective is to make the USVs converge in a synchronized manner towards the target, while avoiding collisions between them and avoiding collisions with obstacles in the motion plane. To solve the overall problem, the following steps are necessary: (i) to perform distributed filtering, so as to obtain an estimate of the target’s state vector. This estimate provides the desirable state vector to be tracked by each one of the USVs, (ii) to design a suitable control law that will enable the mobile sensors not only convergence to the target’s position but will also preserve the cohesion of the USVs fleet (see Fig. 2).
Fig. 2

Distributed tracking of the target vessel (blue color) by N USVs (green color) through the fusion of distributed estimates about the target’s motion

The exact position and orientation of the target can be obtained through distributed filtering. Actually, distributed filtering provides a two-level fusion of the distributed sensor measurements. At the first level, local filters running at each USV provide an estimate of the target’s state vector by fusing the cartesian coordinates of the target with the target’s distance from a reference surface which is measured in an inertial coordinates system [14]. At a second level, fusion of the local estimates is performed with the use of the EIF and the UIF. It is also assumed that the time taken for communication between USVs is small, and that time delays, packet losses and out-of-sequence measurement problems in communication do not distort significantly the flow of the exchanged data.

Comparing to the traditional centralized or hierarchical fusion architecture, the network-centric architectures for the considered multi-USV system has the following advantages: (i) Scalability: since there are no limits imposed by centralized computation bottlenecks or lack of communication bandwidth, every USV can easily join or quit the system, (ii) Robustness: in a decentralized fusion architecture no element of the system is mission-critical, so that the system is survivable in the event of on-line loss of part of its partial entities (USVs), (iii) Modularity: every partial entity is coordinated and does not need to possess a global knowledge of the network topology. However, these benefits are possible only if the sensor data can be fused and synthesized for distribution within the constraints of the available bandwidth.

Tracking of the Reference Path by the Target

The complete kinematic and dynamic model of the target vessel, for both the case of fully actuated and underactuated ships, has been analyzed in [12, 13]. Here a simplified kinematic model of the target vessel is considered (without surge velocity), which is given by:
$$\begin{aligned} \dot{x}(t)= & {} v(t)cos(\theta (t)) \nonumber \\ \dot{y}(t)= & {} v(t)sin(\theta (t)) \nonumber \\ \dot{\theta }(t)= & {} \omega (t) \end{aligned}$$
(2)
In this model, (xy) are the cartesian coordinates of the vessel and \(\theta \) is its heading angle. The target is steered by a dynamic feedback linearization control algorithm which is based on differential flatness theory [15, 16, 17, 18]:
$$\begin{aligned} u_1= & {} \ddot{x}_d+K_{p_1}(x_d-x)+K_{d_1}(\dot{x}_d-\dot{x}) \nonumber \\ u_2= & {} \ddot{y}_d+K_{p_2}(y_d-y)+K_{d_2}(\dot{y}_d-\dot{y})\nonumber \\ \dot{\xi }= & {} {u_1}cos(\theta )+{u_2}sin(\theta )\nonumber \\ v= & {} \xi , \ \, \omega ={{u_2}cos(\theta )-{u_1}sin(\theta ) \over \xi } \end{aligned}$$
(3)
The linearized equivalent model of the target’s kinematics becomes
$$\begin{aligned} \ddot{x}= & {} u_1\nonumber \\ \ddot{y}= & {} u_2 \end{aligned}$$
(4)
A linearized model of similar structure is obtained in the case of the more elaborated vessel models as for example those described in [12, 13]. For the complete model of the vessel’s kinematics dynamics one arrives at a linearized description of the form \(x^{(4)}=u_1\) and \(y^{(4)}=u_2\). Therefore, the control and filtering methods to be developed in this manuscript are also applicable to more complicated vessel models.
In case of Eq. (4), the dynamics of the tracking error is given by
$$\begin{aligned} \ddot{e}_x+K_{{d_1}}{\dot{e}_x}+K_{{p_1}}{e_x}= & {} 0\nonumber \\ \ddot{e_y}+K_{{d_2}}{\dot{e}_x}+K_{{p_2}}{e_y}= & {} 0 \end{aligned}$$
(5)
where \(e_x=x-x_d\) and \(e_y=y-y_d\). The proportional-derivative (PD) gains are chosen as \(K_{p_1}\) and \(K_{d_1}\), for \(i=1,2\). The dynamic compensator of Eq. (3) has a potential singularity at \(\xi =v=0\), i.e. when the target is not moving. It is noted however that the occurrence of such a singularity is structural for non-holonomic systems. It is assumed that the target follows a smooth trajectory \(({x_d}(t),{y_d}(t))\) which is persistent, i.e. for which the nominal velocity \(v_d=(\dot{x}_d^2+\dot{y}_d^2)^{1/2}\) along the trajectory never goes to zero (and thus singularities are avoided). The following theorem assures avoidance of singularities in the proposed flatness-based control law [19]:

Theorem

Let \(\lambda _{11}, \lambda _{12}\) and \(\lambda _{21}, \lambda _{22}\) be respectively the eigenvalues of the two equations of the error dynamics, given in Eq. (5). Assume that for \(i=1,2\) it is \(\lambda _{i1},\lambda _{i2}<0\) (negative real eigenvalues), and that \(\lambda _{i2}\) is sufficiently small. If
$$\begin{aligned} min_{{t{\ge }0}} \left| \left| \left( \begin{pmatrix} \dot{x}_d(t) \\ \dot{y}_d(t) \end{pmatrix}\right) \right| \right| {\ge } \begin{pmatrix} \dot{\epsilon }_x^0 \\ \dot{\epsilon }_y^0 \end{pmatrix} \end{aligned}$$
(6)
with \(\dot{\epsilon _x^0}={\dot{\epsilon }_x}(0){\ne }0\) and \(\dot{\epsilon _y^0}={\dot{\epsilon }_y}(0){\ne }0\) then the singularity \(\xi =0\) is never met.

Distributed Motion Planning for the Multi-USV System

Kinematic Model of the Multi-USV System

The objective is to lead the fleet of N USVs, with different initial positions on the 2-D plane, to converge to the target’s position, and at the same time to avoid collisions between the USVs, as well as collisions with obstacles in the motion plane. An approach for doing this is the potential fields theory, in which the individual USVs are steered towards an equilibrium by the gradient of an harmonic potential [7, 20, 21, 22]. Variances of this method use nonlinear anisotropic harmonic potential fields which introduce to the USVs’ motion directional and regional avoidance constraints [23, 24, 25, 26, 27, 28]. In the examined coordinated target-tracking problem the equilibrium is the target’s position, which is not a-priori known and has to be estimated with the use of distributed filtering.

The position of each USV in the 2-D space is described by the vector \(x^i \ \in \ R^2\). The motion of the USVs is synchronous, without time delays, and it is assumed that at every time instant each USV i is aware about the position and the velocity of the other \(N-1\) USVs. The cost function that describes the motion of the i-th USV towards the target’s position is denoted as \(V(x^i): \ R^n \rightarrow R\). At the target’s position it holds \(\nabla _{x^i}V(x^i)=0\). The following conditions must hold:
  1. (i)

    The cohesion of the USVs ensemble should be maintained, i.e. the norm \(||x^i-x^j||\) should remain upper bounded \(||x^i-x^j||<\epsilon ^h\),

     
  2. (ii)

    Collisions between the USVs should be avoided, i.e. \(||x^i-x^j||>\epsilon ^l\),

     
  3. (iii)

    Convergence to the target’s position should be succeeded for each USV through the negative definiteness of the associated Lyapunov function \(\dot{V}^i(x^i)={\dot{e}^i(t)}^T{e^i(t)}<0\), where \(e=x-x^{*} \) is the distance of the i-th USV from the target’s position.

     
The interaction between the i-th and the j-th USV is
$$\begin{aligned}&g\left( x^i-x^j\right) =-\left( x^i-x^j\right) \nonumber \\&\quad \times \left[ g_a\left( ||x^i-x^j||\right) -g_r\left( ||x^i-x^j||\right) \right] \end{aligned}$$
(7)
where \(g_a()\) denotes the attraction term and is dominant for large values of \(||x^i-x^j||\), while \(g_r()\) denotes the repulsion term and is dominant for small values of \(||x^i-x^j||\). Function \(g_a()\) can be associated with an attraction potential, i.e. \({\nabla _{x_i}}{V_a}(||x^i-x^j||)=(x^i-x^j){g_a}(||x^i-x^j||)\). Function \(g_r()\) can be associated with a repulsion potential, i.e. \({\nabla _{x_i}}{V_r}(||x^i-x^j||)=(x^i-x^j){g_r}(||x^i-x^j||)\). A suitable function g() that describes the interaction between the USVs is given by [28, 29]
$$\begin{aligned} g\left( x^i-x^j\right) =-\left( x^i-x^j\right) \left( a-b{e^{{||x^i-x^j||^2} \over \sigma ^2}}\right) \end{aligned}$$
(8)
where the parameters ab and c are suitably tuned. It holds that \(g_a(x^i-x^j)=-a\), i.e. attraction has a linear behavior (spring-mass system) \(||x^i-x^j||g_a(x^i-x^j)\). Moreover, \(g_r(x^i-x^j)=b{e^{-||x^i-x^j||^2 \over \sigma ^2}}\) which means that \(g_r(x^i-x^j)||x^i-x^j|| \le b\) is bounded. Applying Newton’s laws to the i-th USV yields
$$\begin{aligned} \dot{x}^i=v^i, \ \ m^i{\dot{v}^i}=U^i \end{aligned}$$
(9)
where the aggregate force is \(U^i=f^i+F^i\). The term \(f^i=-{K_v}{v^i}\) denotes a friction-equivalent term, while the term \(F^i\) is the propulsion. Assuming zero acceleration \(\dot{v}^i=0\) one gets \(F^i={K_v}{v^i}\), which for \(K_v=1\) and \(m^i=1\) gives \(F^i=v^i\). Thus an approximate kinematic model for each USV is
$$\begin{aligned} \dot{x}^i=F^i \end{aligned}$$
(10)
According to the Euler-Langrange principle, the propulsion \(F^i\) is equal to the derivative of the total potential of each USV, i.e.
$$\begin{aligned} F^i= & {} -{\nabla _{x^i}}\left\{ V^i(x^i)+{1 \over 2}{\sum \limits _{i=1}^N}{\sum \limits _{j=1,j{\ne }i}^N}\left[ V_a(||x^i-x^j||\right. \right. \\&\left. \left. +\,V_r(||x^i-x^j||)\right] \right\} \Rightarrow \\ F^i= & {} -{\nabla _{x^i}}\left\{ V^i(x^i)\right\} +{\sum \limits _{j=1,j{\ne }i}^N}\left[ -{\nabla _{x^i}}{V_a}(||x^i-x^j||)\right. \\&\left. -{\nabla _{x^i}}{V_r}(||x^i-x^j||)\right] \Rightarrow \\ F^i= & {} -{\nabla _{x^i}}\{V^i(x^i)\}\\&+{\sum \limits _{j=1,j{\ne }i}^N}\left[ +(x^i-x^j){g_a}(||x^i-x^j||)\right. \\&\left. -\left( x^i-x^j\right) {g_r}\left( ||x^i-x^j||\right) \right] \Rightarrow \\ F^i= & {} -{\nabla _{x^i}}\left\{ V^i(x^i)\right\} -{\sum \limits _{j=1,j{\ne }i}^N}g\left( x^i-x^j\right) \end{aligned}$$
Substituting in Eq. (10) one gets in discrete-time form
$$\begin{aligned} x^i(k+1)= & {} x^i(k)+{\gamma ^i(k)}\left[ h(x^i(k))+e^i(k)\right] \nonumber \\&+{\sum _{j=1,j{\ne }i}^N}g(x^i-x^j), \quad i=1,2,\ldots ,M \end{aligned}$$
(11)
The term \(h(x(k)^i)=-{\nabla _{x^i}}V^i(x^i)\) indicates a local gradient algorithm, i.e. motion in the direction of decrease of the cost function \(V^i(x^i)={1 \over 2}{e^i(t)}^T{e^i(t)}\). The term \(\gamma ^i(k)\) is the algorithms step while the stochastic disturbance \(e^i(k)\) enables the algorithm to escape from local minima. The term \({\sum _{j=1,j{\ne }i}^N}g(x^i-x^j)\) describes the interaction between the i-th and the rest \(N-1\) stochastic gradient algorithms [30, 31, 32].

Stability of the Multi-USV System

The behavior of the multi-USV system is determined by the behavior of its center (mean of the vectors \(x^i\)) and of the position of each USV with respect to this center. The center of the multi-USV system is given by
$$\begin{aligned} \bar{x}= & {} E(x^i)={1 \over N}{\sum \limits _{i=1}^N}{x^i} \Rightarrow \dot{\bar{x}}={1 \over N}{\sum \limits _{i=1}^N}{\dot{x}^i} \Rightarrow \nonumber \\ \dot{\bar{x}}= & {} {1 \over N}{\sum \limits _{i=1}^N}\left[ {-\nabla _{x^i}}{V^i}(x^i)-{\sum \limits _{j=1,j{\ne }i}^N}\left( g(x^i-x^j)\right) \right] \end{aligned}$$
(12)
From Eq. (8) it can be seen that \(g(x^i-x^j)=-g(x^j-x^i)\), i.e. g() is an odd function. Therefore, it holds that \({\small {{1 \over N }({\sum _{j=1,j{\ne }i}^N}g(x^i-x^j))=0}}\), and
$$\begin{aligned} \dot{\bar{x}}={1 \over N}{\sum _{i=1}^N}\left[ -{\nabla _{x^i}}{V^i}(x^i)\right] \end{aligned}$$
(13)
Denoting the target’s position by \(x^*\), and the distance between the i-th USV and the mean position of the multi-USV system by \(e^i(t)=x^i(t)-\bar{x}\) the objective of distributed gradient for USV motion planning can be summarized as follows:
  1. (i)

    \(lim_{t \rightarrow \infty } \bar{x}=x^{*}\), i.e. the center of the multi-USV system converges to the target’s position,

     
  2. (ii)

    \(lim_{t \rightarrow \infty } {x^i}=\bar{x}\), i.e. the i-th USV converges to the center of the multi-USV system,

     
  3. (iii)

    \(lim_{t \rightarrow \infty } \dot{\bar{x}}=\dot{x}^{*}\), i.e. the center of the multi-USV system stabilizes at the target’s position.

     
If conditions (i) and (ii) hold then \(lim_{t \rightarrow \infty }{x^i}=x^{*}\). Furthermore, if condition (iii) also holds then all USVs will stabilize close to the target’s position.
It is known that the stability of local gradient algorithms can be proved with the use of Lyapunov theory [32]. A similar approach can be followed in the case of the distributed gradient algorithms given by Eq. (11). The following simple Lyapunov function is considered for each gradient algorithm [29]:
$$\begin{aligned} V_i={1 \over 2}{e^i}^T{e^i} \Rightarrow V_i={1 \over 2}||e_i||^2 \end{aligned}$$
(14)
Thus, one gets
$$\begin{aligned} \dot{V}^i= & {} {e^i}^T{\dot{e}^i} \Rightarrow \dot{V}^i=\left( {\dot{x}^i}-\dot{\bar{x}}\right) {e^i} \Rightarrow \\ \dot{V}^i= & {} \left[ -{\nabla _{x^i}{V^i}\left( x^i \right) }-{\sum \limits _{j=1,j{\ne }i}^N}{g\left( x^i-x^j \right) }\right. \\&\left. +\, {1 \over M}{\sum \limits _{j=1}^N}{\nabla _{x^j}{V^j}(x^j)}\right] {e^i}. \end{aligned}$$
Substituting \(g(x^i-x^j)\) from Eq. (8) yields
$$\begin{aligned} \dot{V}_i= & {} \left[ -{\nabla _{x^i}{V^i}(x^i)}-{\sum \limits _{j=1,j{\ne }i}^N}(x^i-x^j)a\right. \\&\quad +{\sum \limits _{j=1,j{\ne }i}^N}(x^i-x^j)g_r(||x^i-x^j||)\\&\quad \left. +\,{1 \over N}{\sum \limits _{j=1}^N}{\nabla _{x^j}{V^j}(x^j)}\right] {e^i} \end{aligned}$$
which gives,
$$\begin{aligned} \dot{V}_i= & {} -a\left[ {\sum \limits _{j=1,j{\ne }i}^N}(x^i-x^j)\right] {e^i}\\&+{\sum \limits _{j=1,j{\ne }i}^N}g_r(||x^i-x^j||){(x^i-x^j)^T}{e^i}\\&-\left[ {\nabla _{x^i}{V^i}(x^i)}-{1 \over N}{\sum \limits _{j=1}^M}{\nabla _{x^j}{V^j}(x^j)}\right] ^T{e^i} \end{aligned}$$
It holds that \({\sum _{j=1}^N}(x^i-x^j)=N{x^i}-N{1 \over N}{\sum _{j=1}^N}{x^j}=N{x^i}-N{\bar{x}}=N(x^i-\bar{x})=N{e^i}\), therefore
$$\begin{aligned} \dot{V}_i= & {} -aN||e^i||^2+{\sum _{j=1,j{\ne }i}^N}g_r(||x^i-x^j||){(x^i-x^j)^T}{e^i}\nonumber \\&-\left[ {\nabla _{x^i}{V^i}(x^i)}-{1 \over N}{\sum _{j=1}^N}{\nabla _{x^j}{V^j}(x^j)}\right] ^T{e^i} \end{aligned}$$
(15)
It assumed that for all \(x^i\) there is a constant \(\bar{\sigma }\) such that
$$\begin{aligned} ||\nabla _{x^i}{V^i}(x^i)|| \le \bar{\sigma } \end{aligned}$$
(16)
Eq. (16) is reasonable since for a USV moving on a 2-D plane, the gradient of the cost function \(\nabla _{x^i}{V^i}(x^i)\) is expected to be bounded. Moreover it is known that the following inequality holds:
$$\begin{aligned} {\sum \limits _{j=1,j{\ne }i}^N}{g_r}(x^i-x^j)^T{e^i} {\le } {\sum \limits _{j=1,j{\ne }i}^N}{b{e^i}} {\le } {\sum \limits _{j=1,j{\ne }i}^N}b||e^i||. \end{aligned}$$
Thus the application of Eq. (15) gives:
$$\begin{aligned}&\dot{V}^i {\le }-aN{||e^i||^2}+{\sum \limits _{j=1,j{\ne }i}^N}{g_r(||x^i\!-\!x^j||)||x^i\!-\!x^j||\cdot ||e^i||}\\&\qquad +\,||\nabla _{x^i}{V^i(x^i)} -{1 \over N}{\sum \limits _{j=1}^M{\nabla _{x^j}{V^j}(x^j)}}||||e^i||\\&\quad \Rightarrow \dot{V}^i \,{\le }-\, aN{||e^i||^2}+b(N-1)||e^i||+2{\bar{\sigma }||e^i||} \end{aligned}$$
where it has been taken into account that
$$\begin{aligned}&{\sum \limits _{j=1,j{\ne }i}^N}{g_r}(||x^i-x^j||)^T||{e^i}|| \le {\sum \limits _{j=1,j{\ne }i}^N}{b{||e^i||}} \\&\quad = b(N-1)||e^i||, \end{aligned}$$
and from Eq. (16),
$$\begin{aligned}&\left| \left| \nabla _{x^i}{V^i}(x^i)-{1 \over N}{\sum \limits _{j=1}^N}{\nabla _{x^i}{V^j(x^j)}}\right| \right| {\le } ||\nabla _{x^i}{V^i}(x^i)||\\&\quad +{1 \over N}\left| \left| {\sum \limits _{j=1}^N}{\nabla _{x^i}{V^j(x^j)}}\right| \right| \le {\bar{\sigma }}+{1 \over N}N{\bar{\sigma }} \le 2{\bar{\sigma }}. \end{aligned}$$
Thus, one gets
$$\begin{aligned} \dot{V}^i {\le } -{aN||e^i||}{\cdot }\left[ ||e^i||-{b(N-1) \over {aN}}-{2{\bar{\sigma } \over {aN}}}\right] \end{aligned}$$
(17)
The following bound \(\epsilon \) is defined:
$$\begin{aligned} \epsilon ={b(N-1) \over {aN}}+{2{\bar{\sigma }} \over {aN}}={1 \over {aN}}(b(N-1)+2\bar{\sigma }) \end{aligned}$$
(18)
Thus, when \(||e^i||>\epsilon , \dot{V}_i\) will become negative and consequently the error \(e^i=x^i-\bar{x}\) will decrease. Therefore the tracking error \(e^i\) will remain in an area of radius \(\epsilon \) i.e. the position \(x^i\) of the i-th USV will stay in the cycle with center \(\bar{x}\) and radius \(\epsilon \).

Convergence of the Mean of the Multi-USV System

The case of a convex quadratic cost function is examined, for instance
$$\begin{aligned} V^i(x^i)={A \over 2}||x^i-x^*||^2={A \over 2}(x^i-x^*)^T(x^i-x^*) \end{aligned}$$
(19)
where \(x^{*} \ {\in } \ R^{2}\) denotes the target’s position, while the associated Lyapunov function has a minimum at \(x^{*}\), i.e. \(V^i(x^{i}=x^{*})=0\). The distributed gradient algorithm is expected to converge to \(x^*\). The individual USVs will follow different trajectories on the 2-D plane and will end at the target’s position.
Using Eq.(19) yields \(\nabla _{x^i}{V^i}{(x^i)}=A(x^i-x^*)\). Moreover, the assumption \(\nabla _{x^i}{V^i}(x^i) \le {\bar{\sigma }}\) can be used, since the gradient of the cost function remains bounded. The USVs will concentrate round \(\bar{x}\) and will stay in a radius \(\epsilon \) given by Eq. (18). The motion of the mean position \(\bar{x}\) of the USVs is
$$\begin{aligned} \dot{\bar{x}}= & {} -{1 \over N}{\sum \limits _{i=1}^N}{\nabla _{x^i}{V^i(x^i)}} \Rightarrow \dot{\bar{x}}=-{A \over N}{\sum \limits _{i=1}^N}(x^i-x^*) \Rightarrow \nonumber \\ \dot{\bar{x}}= & {} -{A \over N}{\sum \limits _{i=1}^N}{x^i}+{A \over N}N{x^{*}} \Rightarrow \dot{\bar{x}}-{\dot{x}^*}=-A(\bar{x}-x^{*})-{\dot{x}^*}\nonumber \\ \end{aligned}$$
(20)
The variable \(e_{\sigma }=\bar{x}-x^*\) is defined, and consequently
$$\begin{aligned} {\dot{e}_{\sigma }}=-A{e_{\sigma }}-{\dot{x}^*} \end{aligned}$$
(21)
The following cases can be distinguished:
  1. (i)
    The target is not moving, i.e. \(\dot{x}^{*}=0\). In that case Eq. (21) results in an homogeneous differential equation, the solution of which is given by
    $$\begin{aligned} \quad {\epsilon _{\sigma }(t)}={\epsilon _{\sigma }(0)}e^{-At} \end{aligned}$$
    (22)
    Knowing that \(A>0\) results into \(lim_{t \rightarrow \infty }e_{\sigma }(t)=0\), thus \(lim_{t \rightarrow \infty }{\bar{x}}(t)=x^*\).
     
  2. (ii)
    the target is moving at constant velocity, i.e. \(\dot{x^{*}}=a\), where \(a>0\) is a constant parameter. Then the error between the mean position of the multi-USV formation and the target becomes
    $$\begin{aligned} \quad {\epsilon _{\sigma }(t)} = \left[ \epsilon _{\sigma }(0)+{a \over A}\right] e^{-At}-{a \over A} \end{aligned}$$
    (23)
    where the exponential term vanishes as \(t{\rightarrow }\infty \).
     
  3. (iii)

    the target’s velocity is described by a sinusoidal signal or a superposition of sinusoidal signals, as in the case of function approximation by Fourier series expansion. For instance consider the case that \(\dot{x^{*}}=b{\cdot }sin(at)\), where \(a,b>0\) are constant parameters. Then the nonhomogeneous differential equation Eq. (21) admits a sinusoidal solution. Therefore the distance \(\epsilon _{\sigma }(t)\) between the center of the multi-USV formation \(\bar{x}(t)\) and the target’s position \(x^{*}(t)\) will be also a bounded sinusoidal signal.

     
Fig. 3

LaSalle’s theorem: C: invariant set, \(E \subset C\): invariant set which satisfies \(\dot{V}(x)=0, M \subset E\): invariant set, which satisfies \(\dot{V}(x)=0\), and which contains the limit points of \(x(t) \in E, L^+\) the set of limit points of \(x(t) \in E\)

Convergence Analysis Using La Salle’s Theorem

From Eq. (17) it has been shown that each USV will stay in a cycle C of center \(\bar{x}\) and radius \(\epsilon \) given by Eq. (18). The Lyapunov function given by Eq. (14) is negative semi-definite, therefore asymptotic stability cannot be guaranteed. It remains to make precise the area of convergence of each USV in the cycle C of center \(\bar{x}\) and radius \(\epsilon \). To this end, La Salle’s theorem can be employed [29, 33] (Fig. 3).

La Salle’s Theorem

Assume the autonomous system \(\dot{x}=f(x)\) where \(f: D \rightarrow R^n\). Assume \(C \subset D\) a compact set which is positively invariant with respect to \(\dot{x}=f(x)\), i.e. if \(x(0) \in C \Rightarrow x(t) \in C \ \forall \ t\). Assume that \(V(x): D \rightarrow R\) is a continuous and differentiable Lyapunov function such that \(\dot{V}(x) \le 0\) for \(x \in C\), i.e. V(x) is negative semi-definite in C. Denote by E the set of all points in C such that \(\dot{V}(x)=0\). Denote by M the largest invariant set in E and its boundary by \(L^+\), i.e. for \(x(t) \in E: lim_{t \rightarrow \infty }x(t)=L^+\), or in other words \(L^+\) is the positive limit set of E. Then every solution \(x(t) \in C\) will converge to M as \(t \rightarrow \infty \).

La Salle’s theorem is applicable in the case of the multi-USV system and helps to describe more precisely the area round \(\bar{x}\) to which the USV trajectories \(x^i\) will converge. A generalized Lyapunov function is introduced which is expected to verify the stability analysis based on Eq. (17). It holds that
$$\begin{aligned} V(x)= & {} {\sum \limits _{i=1}^N}{V^i(x^i)}+{1 \over 2}{\sum \limits _{i=1}^N}{\sum \limits _{j=1,j{\ne }i}^N}\left\{ V_a(||x^i-x^j||\right. \\&\left. -\,V_r(||x^i-x^j||)\right\} \Rightarrow \\ V(x)= & {} {\sum \limits _{i=1}^N}{V^i(x^i)}+{1 \over 2}{\sum \limits _{i=1}^N}{\sum \limits _{j=1,j{\ne }i}^N}\left\{ a||x^i-x^j||\right. \\&\left. -\,V_r(||x^i-x^j||)\right\} \end{aligned}$$
and
$$\begin{aligned} {\nabla _{x^i}}V(x)= & {} \left[ {\sum \limits _{i=1}^N}{\nabla _{x^i}}{V^i(x^i)}\right] \\&+\,{1 \over 2}{\sum \limits _{i=1}^N}{\sum \limits _{j=1,j{\ne }i}^N}{\nabla _{x^i}}\{a||x^i-x^j||\\&-\,V_r(||x^i-x^j||)\} \Rightarrow \\ {\nabla _{x^i}}V(x)= & {} \left[ {\sum \limits _{i=1}^N}{\nabla _{x^i}}{V^i(x^i)}\right] \\&+{\sum \limits _{j=1,j{\ne }i}^N}(x^i-x^j)\{{g_a}(||x^i-x^j||)\\&-\,{g_r}(||x^i-x^j||)\} \Rightarrow \\ {\nabla _{x^i}}V(x)= & {} \left[ {\sum \limits _{i=1}^N}{\nabla _{x^i}}{V^i(x^i)}\right] \\&+{\sum \limits _{j=1,j{\ne }i}^N}(x^i-x^j)\{a-{g_r}(||x^i-x^j||)\} \end{aligned}$$
and using Eq. (11) with \(\gamma ^{i}(t)=1\) yields \({\nabla _{x^i}}V(x)=-\dot{x}^i\), and
$$\begin{aligned} \dot{V}(x)= & {} {{\nabla _{x}}V(x)^T}{\dot{x}}={\sum _{i=1}^N}{{\nabla _{x^i}}V(x)^T}{\dot{x}^i}\Rightarrow \dot{V}(x)\nonumber \\&=-{\sum _{i=1}^N}||\dot{x}^i||^2 \le 0 \end{aligned}$$
(24)
Therefore it holds \(V(x)>0\) and \(\dot{V}(x){\le }0\) and the set \(C=\{x: V(x(t))\le V(x(0))\}\) is compact and positively invariant. Thus, by applying La Salle’s theorem one can show the convergence of x(t) to the set \(M \subset C, \ M=\{x: \dot{V}(x)=0\} \Rightarrow \ M=\{x: \dot{x}=0\}\).

Distributed State Estimation Using the Extended Information Filter

Extended Kalman Filtering at Local Processing Units

To implement the previously analyzed distributed control scheme it is necessary to obtain accurate localization of the target and precise estimation of its motion characteristics. To this end, distributed filtering can be used, which actually performs fusion of estimates of the target’s state vector provided by filtering algorithms running locally at each USV [34, 35]. As mentioned, to obtain an accurate estimate of the target’s coordinates, fusion of the distributed sensor measurements can be performed either with the use of the EIF or with the use of the UIF. The distributed Extended Kalman Filter, also know as Extended Information Filter, performs fusion of the state estimates which are provided by local Extended Kalman Filters. Thus, the functioning of the local Extended Kalman Filters should be analyzed first. The following nonlinear state-space model is considered again [36, 37]:
$$\begin{aligned} x(k+1)= & {} \phi (x(k))+L(k)u(k)+w(k) \nonumber \\ z(k)= & {} \gamma (x(k))+v(k) \end{aligned}$$
(25)
where \(x{\in }R^{m{\times }1}\) is the system’s state vector and \(z{\in }R^{p{\times }1}\) is the system’s output, while w(k) and v(k) are uncorrelated, Gaussian zero-mean noise processes with covariance matrices Q(k) and R(k) respectively. The operators \(\phi (x)\) and \(\gamma (x)\) are \(\phi (x)=[{\phi _1}(x),{\phi _2}(x),\ldots ,{\phi _m}(x)]^T\), and \(\gamma (x)=[{\gamma _1}(x),{\gamma _2}(x),\ldots ,{\gamma _p}(x)]^T\), respectively. It is assumed that \(\phi \) and \(\gamma \) are sufficiently smooth in x so that each one has a valid series Taylor expansion. Following a linearization procedure, \(\phi \) is expanded into Taylor series about \(\hat{x}\):
$$\begin{aligned} \phi (x(k))=\phi (\hat{x}(k))+J_{\phi }(\hat{x}(k))[x(k)-\hat{x}(k)]+\cdots \end{aligned}$$
(26)
where \(J_{\phi }(x)\) is the Jacobian of \(\phi \) calculated at \(\hat{x}(k)\):
$$\begin{aligned} J_{\phi }(x)={\partial {\phi } \over \partial {x}}{\vert _{x=\hat{x}(k)}}= \begin{pmatrix} {\partial {\phi _1} \over \partial {x_1}} &{}\quad {\partial {\phi _1} \over \partial {x_2}} &{}\quad \cdots &{}\quad {\partial {\phi _1} \over \partial {x_m}} \\ {\partial {\phi _2} \over \partial {x_1}} &{}\quad {\partial {\phi _2} \over \partial {x_2}} &{}\quad \cdots &{} {\partial {\phi _2} \over \partial {x_m}} \\ \vdots &{}\quad \vdots &{}\quad \vdots &{}\quad \vdots \\ {\partial {\phi _m} \over \partial {x_1}} &{}\quad {\partial {\phi _m} \over \partial {x_2}} &{}\quad \cdots &{}\quad {\partial {\phi _m} \over \partial {x_m}} \end{pmatrix} \end{aligned}$$
(27)
Likewise, \(\gamma \) is expanded about \(\hat{x}^{-}(k)\)
$$\begin{aligned} \gamma (x(k))=\gamma (\hat{x}^{-}(k))+J_{\gamma }[x(k)-\hat{x}^{-}(k)]+\cdots \end{aligned}$$
(28)
where \(\hat{x}^{-}(k)\) is the estimation of the state vector x(k) before measurement at the k-th instant to be received and \({\hat{x}}(k)\) is the updated estimation of the state vector after measurement at the k-th instant has been received. The Jacobian \(J_{\gamma }(x)\) is
$$\begin{aligned} J_{\gamma }(x)={\partial {\gamma } \over \partial {x}} {\vert _{x=\hat{x}^{-}(k)}} = \begin{pmatrix} {\partial {\gamma _1} \over \partial {x_1}} &{}\quad {\partial {\gamma _1} \over \partial {x_2}} &{} \cdots &{}\quad {\partial {\gamma _1} \over \partial {x_m}} \\ {\partial {\gamma _2} \over \partial {x_1}} &{}\quad {\partial {\gamma _2} \over \partial {x_2}} &{}\quad \cdots &{}\quad {\partial {\gamma _2} \over \partial {x_m}} \\ \vdots &{} \vdots &{}\quad \vdots &{}\quad \vdots \\ {\partial {\gamma _p} \over \partial {x_1}} &{}\quad {\partial {\gamma _p} \over \partial {x_2}} &{}\quad \cdots &{}\quad {\partial {\gamma _p} \over \partial {x_m}} \end{pmatrix} \end{aligned}$$
(29)
The resulting expressions create first order approximations of \(\phi \) and \(\gamma \). Thus the linearized version of the system is obtained:
$$\begin{aligned} x(k+1)= & {} \phi (\hat{x}(k))+J_{\phi }(\hat{x}(k))[x(k)-\hat{x}(k)]+w(k) \nonumber \\ z(k)= & {} {\gamma }(\hat{x}^{-}(k))+J_{\gamma }(\hat{x}^{-}(k))[x(k)-\hat{x}^{-}(k)]+v(k)\nonumber \\ \end{aligned}$$
(30)
Now, the EKF recursion is as follows: First the time update is considered: by \(\hat{x}(k)\) the estimation of the state vector at instant k is denoted. Given initial conditions \(\hat{x}^{-}(0)\) and \(P^{-}(0)\) the recursion proceeds as:
  • Measurement update Acquire z(k) and compute:
    $$\begin{aligned} K(k)= & {} P^{-}(k)J^{T}_{\gamma }(\hat{x}^{-}(k)){\cdot }[J_{\gamma }(\hat{x}^{-}(k))P^{-}(k)J^T_{\gamma }(\hat{x}^{-}(k))\nonumber \\&+R(k)]^{-1}\nonumber \\ \hat{x}(k)= & {} \hat{x}^{-}(k)+K(k)[z(k)-{\gamma }(\hat{x}^{-}(k))] \nonumber \\ P(k)= & {} P^{-}(k)-K(k)J_{\gamma }(\hat{x}^{-}(k)){P^{-}(k)} \end{aligned}$$
    (31)
  • Time update Compute:
    $$\begin{aligned} P^{-}(k+1)= & {} J_{\phi }(\hat{x}(k))P(k){J_{\phi }^T(\hat{x}(k))}+Q(k)\nonumber \\ \hat{x}^{-}(k+1)= & {} {\phi }(\hat{x}(k))+L(k)u(k) \end{aligned}$$
    (32)
The schematic diagram of the EKF loop is given in Fig. 4.
Fig. 4

Schematic diagram of the EKF loop

Calculation of Local Estimations in Terms of EIF Information Contributions

Again the discrete-time nonlinear system of Eq. (25) is considered. The extended information filter (EIF) performs fusion of the local state vector estimates which are provided by the local extended Kalman filters, using the Information matrix and the Information state vector [38, 39, 40, 41]. The Information Matrix is the inverse of the state vector covariance matrix, and can be also associated to the Fisher Information matrix [42]. The Information state vector is the product between the Information matrix and the local state vector estimate
$$\begin{aligned} \textit{Y}(k)= & {} P^{-1}(k)=I(k)\nonumber \\ \hat{\textit{y}}(k)= & {} {P^{-}(k)}^{-1}\hat{x}(k)=\textit{Y}(k)\hat{x}(k) \end{aligned}$$
(33)
The update equation for the Information Matrix and the Information state vector are given by
$$\begin{aligned} Y(k)= & {} {P^{-}}(k)^{-1}+{J_{\gamma }^T}(k){R^{-1}(k)}{J_{\gamma }}(k)\nonumber \\= & {} \textit{Y}^{-}(k)+I(k)\end{aligned}$$
(34)
$$\begin{aligned} \hat{\textit{y}}(k)= & {} \hat{\textit{y}}^{-}(k)+{J_{\gamma }^T}{R(k)^{-1}}[z(k)-\gamma (x(k))+{J_\gamma }{\hat{x}^{-}}(k)]\nonumber \\= & {} \hat{\textit{y}}^{-}(k)+i(k) \end{aligned}$$
(35)
where
$$\begin{aligned} I(k)= & {} {J_{\gamma }^T}(k){R(k)^{-1}}{J_{\gamma }}(k) \ \text {is the associated information}\nonumber \\&\text {matrix and} \nonumber \\ i(k)= & {} {J_{\gamma }^T}{R(k)^{-1}}[(z(k)-\gamma (x(k)))+{J_\gamma }{\hat{x}^{-}}(k)]\nonumber \\&\text {is the information state contribution} \end{aligned}$$
(36)
The predicted information state vector and Information matrix are obtained from
$$\begin{aligned} \hat{\textit{y}}^{-}(k)= & {} {P^{-}(k)}^{-1}{\hat{x}^{-}}(k) \nonumber \\ \textit{Y}^{-}(k)= & {} {P^{-}(k)}^{-1}=[{J_{\phi }(k)}{P^{-}(k)}{J_{\phi }(k)}^T+Q(k)]^{-1} \end{aligned}$$
(37)
The EIF is next formulated for the case that multiple local sensor measurements and local estimates are used to increase the accuracy and reliability of the estimation of the target’s cartesian coordinates and bearing. It is assumed that an observation vector \(z^{i}(k)\) is available for N different sensor sites (USVs) \(i=1,2,\ldots ,N\) and each sensor observes a common state according to the local observation model, expressed by
$$\begin{aligned} z^{i}(k)=\gamma (x(k))+v^{i}(k), \quad i=1,2,\ldots ,N \end{aligned}$$
(38)
where the local noise vector \(v^{i}(k){\sim }N(0,R^{i})\) is assumed to be white Gaussian and uncorrelated between sensors. The variance of a composite observation noise vector \(v_k\) is expressed in terms of the block diagonal matrix
$$\begin{aligned} R(k)=diag\left[ R(k)^{1},\ldots ,R^{N}(k)\right] ^T \end{aligned}$$
(39)
The information contribution can be expressed by a linear combination of each local information state contribution \(i^{i}\) and the associated information matrix \(I^{i}\) at the i-th sensor site
$$\begin{aligned} i(k)= & {} {\!\sum \limits _{i=1}^N}{{J_\gamma ^{i}}^T(k)}{{R^{i}}(k)^{-1}}\!\left[ z^{i}(k)\!-\gamma ^{i}(x(k))+\!{J_\gamma ^{i}}(k){\hat{x}^{-}}(k)\!\right] \nonumber \\ I(k)= & {} {\sum \limits _{i=1}^N}{{J_\gamma ^{i}}^T(k)}{R^{i}}(k)^{-1}{J_\gamma ^{i}}(k) \end{aligned}$$
(40)
Using Eq. (40) the update equations for fusing the local state estimates become
$$\begin{aligned} \hat{\textit{y}}(k)= & {} \hat{\textit{y}}^{-}(k)+{\sum \limits _{i=1}^N}{{J_\gamma ^{i}}^T(k)}{{R^{i}}(k)^{-1}}\left[ z^{i}(k)-\gamma ^{i}(x(k))\right. \nonumber \\&\left. +\,{J_\gamma ^{i}}(k){\hat{x}^{-}}(k)\right] \nonumber \\ \textit{Y}(k)= & {} \textit{Y}^{-}(k)+{\sum \limits _{i=1}^N}{{J_\gamma ^{i}}^T(k)}{R^{i}}(k)^{-1}{J_\gamma ^{i}}(k) \end{aligned}$$
(41)
It is noted that in the EIF an aggregation (master) fusion filter produces a global estimate by using the local sensor information provided by each local filter.
Fig. 5

Fusion of the distributed state estimates of the target (obtained by the USVs) with the use of the extended information filter

As in the case of the extended Kalman filter the local filters which constitute the EIF can be written in terms of time update and a measurement update equation.

Measurement update acquire z(k) and compute
$$\begin{aligned}&Y(k)=P^{-}(k)^{-1}+{J_{\gamma }^T}(k){R(k)^{-1}}{J_{\gamma }(k)}\nonumber \\&\text {or} \ Y(k)=Y^{-}(k)+I(k) \ \text {where}\nonumber \\&I(k)={J_{\gamma }^T}(k){R^{-1}(k)}{J_{\gamma }(k)} \end{aligned}$$
(42)
Time update Compute
$$\begin{aligned} Y^{-}(k+1)= & {} {P^{-}(k+1)}^{-1}\nonumber \\= & {} [{J_{\phi }(k)}P(k){J_{\phi }(k)^T}+Q(k)]^{-1}\end{aligned}$$
(44)
$$\begin{aligned} y^{-}(k+1)= & {} {P^{-}(k+1)}^{-1}\hat{x}^{-}(k+1) \end{aligned}$$
(45)

Extended Information Filtering for State Estimates Fusion

In the EIF each one of the local filters operates independently, processing its own local measurements. It is assumed that there is no sharing of measurements between the local filters and that the aggregation filter (Fig. 5) does not have direct access to the raw measurements feeding each local filter. The outputs of the local filters are treated as measurements which are fed into the aggregation fusion filter [38, 39, 40]. Then each local filter is expressed by its respective error covariance and estimate in terms of information contributions given in Eq. (37) (Fig. 6)
$$\begin{aligned} {{P_{i}}^{-1}}(k)= & {} {{P_{i}^{-}(k)}^{-1}}+{J_{\gamma }^T}{R^(k)^{-1}}{J_{\gamma }}(k)\nonumber \\ {\hat{x}_{i}}(k)= & {} {P_i}(k)({P_i^{-}(k)^{-1}}{\hat{x}_{i}^{-}}(k))\nonumber \\&+\,{J_{\gamma }^T}{R(k)^{-1}}[z^{i}(k)-\gamma ^{i}(x(k))+{J_\gamma ^{i}}(k){\hat{x}_{i}^{-}}(k)]\nonumber \\ \end{aligned}$$
(46)
It is noted that the local estimates are suboptimal and also conditionally independent given their own measurements. The global estimate and the associated error covariance for the aggregate fusion filter can be rewritten in terms of the computed estimates and covariances from the local filters using the relations
$$\begin{aligned}&{J_{\gamma }^T(k)}{R(k)^{-1}}{J_{\gamma }}(k)={{P_{i}}(k)^{-1}}-{{P_{i}^{-}}(k)^{-1}}\nonumber \\&{J_{\gamma }^T(k)}{R(k)^{-1}}\left[ z^{i}(k)-\gamma ^{i}(x(k))+{J_\gamma ^{i}}(k){\hat{x}^{-}}(k)\right] \nonumber \\&\quad ={P_i(k)^{-1}}{\hat{x}_{i}}(k)-{P_i(k)^{-1}}{\hat{x}_{i}}(k-1) \end{aligned}$$
(47)
Fig. 6

Schematic diagram of the extended information filter loop

For the general case of N local filters \(i=1,\ldots ,N\), the distributed filtering architecture is described by the following equationsIt is noted that the global state update equation in the above distributed filter can be written in terms of the information state vector and of the information matrix
$$\begin{aligned} \hat{y}(k)= & {} {\hat{y}^{-}}(k)+{\sum _{i=1}^N}\Big ({\hat{y}_{i}}(k)-{\hat{y}_i^{-}}(k)\Big )\nonumber \\ \hat{Y}(k)= & {} {\hat{Y}^{-}}(k)+{\sum _{i=1}^N}\left( {\hat{Y}_{i}}(k)-{\hat{Y}_{i}^{-}}(k)\right) \end{aligned}$$
(49)
The local filters provide their own local estimates and repeat the cycle at step \(k+1\). In turn the global filter can predict its global estimate and repeat the cycle at the next time step \(k+1\) when the new state \(\hat{x}(k+1)\) and the new global covariance matrix \(P(k+1)\) are calculated. From Eq. (48) it can be seen that if a local filter (processing station) fails, then the local covariance matrices and the local state estimates provided by the rest of the filters will enable an accurate computation of the system’s state vector.

Distributed State Estimation Using the Unscented Information Filter

Unscented Kalman Filtering at Local Processing Units

It is also possible to estimate the cartesian coordinates and bearing of the target through the fusion of the estimates provided by local sigma-point Kalman filters. This can be succeeded using the distributed sigma-point Kalman filter, also known as unscented information filter (UIF) [38, 39]. First, the functioning of the local sigma-point Kalman filters will be explained. Each local sigma-point Kalman filter generates an estimation of the target’s state vector by fusing the estimate of the target’s coordinates and bearing obtained by each USV with the distance of the target from a reference surface, measured in an inertial coordinates system. Unlike EKF, in sigma-point Kalman filtering no analytical Jacobians of the system equations need to be calculated [43, 44, 45]. This is achieved through a different approach for calculating the posterior 1st and 2nd order statistics of a random variable that undergoes a nonlinear transformation. The state distribution is represented again by a Gaussian random variable but is now specified using a minimal set of deterministically chosen weighted sample points. The basic sigma-point approach can be described as follows:
  1. 1.

    A set of weighted samples (sigma-points) are deterministically calculated using the mean and square-root decomposition of the covariance matrix of the system’s state vector. As a minimal requirement the sigma-point set must completely capture the first and second order moments of the prior random variable. Higher order moments can be captured at the cost of using more sigma-points.

     
  2. 2.

    The sigma-points are propagated through the true nonlinear function using functional evaluations alone, i.e. no analytical derivatives are used, in order to generate a posterior sigma-point set.

     
  3. 3.

    The posterior statistics are calculated (approximated) using tractable functions of the propagated sigma-points and weights. Typically, these take on the form of a simple weighted sample mean and covariance calculations of the posterior sigma points.

     
It is noted that the sigma-point approach differs substantially from general stochastic sampling techniques, such as Monte-Carlo integration (e.g Particle filtering methods) which require significantly more sample points in an attempt to propagate an accurate (possibly non-Gaussian) distribution of the state. The simple sigma-point approach results in posterior approximations that are accurate to the third order for Gaussian inputs for all nonlinearities. For non-Gaussian inputs, approximations are accurate to at least the second-order, with the accuracy of third and higher-order moments determined by the specific choice of weights and scaling factors.
The unscented Kalman filter (UKF) is a special case of sigma-point Kalman filters. The UKF is a discrete time filtering algorithm which uses the unscented transform for computing approximate solutions to the filtering problem of the form
$$\begin{aligned} x(k+1)= & {} \phi (x(k))+L(k)U(k)+w(k)\nonumber \\ y(k)= & {} \gamma (x(k))+v(k) \end{aligned}$$
(50)
where \({x(k)}{\in }{R^n}\) is the system’s state vector, \({y(k)}{\in }{R^m}\) is the measurement, \(w(k){\in }{R^n}\) is a Gaussian process noise \(w(k){\sim }N(0,Q(k))\), and \({v(k)}{\in }{R^m}\) is a Gaussian measurement noise denoted as \({v(k)}{\sim }N(0,R(k))\). The mean and covariance of the initial state x(0) are m(0) and P(0), respectively.
Some basic operations performed in the UKF algorithm (Unscented Transform) are summarized as follows:
  1. 1)
    Denoting the current state mean as \(\hat{x}\), a set of \(2n+1\) sigma points is taken from the columns of the \(n{\times }n\) matrix \(\sqrt{(n+\lambda )P_{xx}}\) as follows:
    $$\begin{aligned} x^{0}= & {} \hat{x}\nonumber \\ x^{i}= & {} \hat{x}+\left[ \sqrt{(n+\lambda )P_{xx}}\right] _i, \quad i=1,\ldots ,n \nonumber \\ x^{i}= & {} \hat{x}-\left[ \sqrt{(n+\lambda )P_{xx}}\right] _i, \quad i=n+1,\ldots ,2n \end{aligned}$$
    (51)
    and the associate weights are computed:
    $$\begin{aligned} W_0^{(m)}= & {} {\lambda \over {(n+\lambda )}} \quad W_0^{(c)}={\lambda \over {{(n+\lambda )}+(1-\alpha ^2+b)}} \nonumber \\ W_i^{(m)}= & {} {1 \over {2(n+\lambda )}}, \quad W_i^{(c)}={1 \over {2(n+\lambda )}}\nonumber \\ \end{aligned}$$
    (52)
    where \(i=1,2,\ldots ,2n\) and \(\lambda =\alpha ^2(n+\kappa )-n\) is a scaling parameter, while \(\alpha , \beta \) and \(\kappa \) are constant parameters. Matrix \(P_{xx}\) is the covariance matrix of the state x.
     
  2. 2)
    Transform each of the sigma points as
    $$\begin{aligned} z^{i}=h(x^{i}) \quad i=0,\ldots ,2n \end{aligned}$$
    (53)
     
  3. 3)
    Mean and covariance estimates for z can be computed as
    $$\begin{aligned}&\hat{z}\,\simeq \, {\sum \limits _{i=0}^{2n}}{W_{i}^{(m)}}z^{i} \nonumber \\&P_{zz}={\sum \limits _{i=0}^{2n}}{W_{i}^{(c)}}(z^{i}-\hat{z})(z^{i}-\hat{z})^T \end{aligned}$$
    (54)
     
  4. 4)
    The cross-covariance of x and z is estimated as
    $$\begin{aligned} P_{xz}={\sum \limits _{i=0}^{2n}}{W_{i}^{(c)}}(x^{i}-\hat{x})(z^{i}-\hat{z})^T \end{aligned}$$
    (55)
    The matrix square root of positive definite matrix \(P_{xx}\) means a matrix \(A=\sqrt{P_{xx}}\) such that \(P_{xx}=A{A^T}\) and a possible way for calculation is SVD.
     
Next the basic stages of the unscented Kalman filter are given:

As in the case of the extended Kalman filter, the UKF also consists of prediction stage (time update) and correction stage (measurement update) [44, 45].

Time update: Compute the predicted state mean \(\hat{x}^{-}(k)\) and the predicted covariance \({P_{xx}}^{-}(k)\) as
$$\begin{aligned}{}[\hat{x}^{-}(k),{{P_{xx}^{-}}(k)}]= & {} UT(f_d,\hat{x}(k-1),{{P_{xx}}(k-1)}) \nonumber \\ {{P_{xx}^{-}}(k)}= & {} {P_{xx}}(k-1)+Q(k-1) \end{aligned}$$
(56)
Measurement update Obtain the new output measurement \(z_k\) and compute the predicted mean \(\hat{z}(k)\) and covariance of the measurement \({P_{zz}}(k)\), and the cross covariance of the state and measurement \({P_{xz}}(k)\)
$$\begin{aligned}{}[{\hat{z}}(k),{P_{zz}}(k),{{P_{xz}}(k)}]= & {} UT(h_d,\hat{x}^{-}(k),{P_{xx}^{-}}(k))\nonumber \\ {P_{zz}(k)}= & {} {P_{zz}(k)}+R(k) \end{aligned}$$
(57)
Then compute the filter gain K(k), the state mean \(\hat{x}(k)\) and the covariance \({P_{xx}}(k)\), conditional to the measurement y(k)
$$\begin{aligned}&K(k)={P_{xz}(k)}{{P_{zz}^{-1}}(k)}\nonumber \\&\hat{x}(k)=\hat{x}^{-}(k)+{K(k)}[z(k)-\hat{z}(k)]\nonumber \\&{P_{xx}(k)}={P_{xx}^{-}}(k)-{K(k)}{{P_{zz}(k)}}{K(k)^T} \end{aligned}$$
(58)
The filter starts from the initial mean m(0) and covariance \({P_{xx}}(0)\). The stages of state vector estimation with the use of the UKF algorithm are depicted in Fig. 7.
Fig. 7

Schematic diagram of the unscented Kalman filter loop

Unscented Information Filtering

The UIF performs fusion of the state vector estimates which are provided by local Unscented Kalman Filters, by weighting these estimates with local Information matrices (inverse of the local state vector covariance matrices which are again recursively computed) [38, 39, 40]. The UIF is derived by introducing a linear error propagation based on the unscented transformation into the EIF structure. First, an augmented state vector \(x_{\alpha }^{-}(k)\) is considered, along with the process noise vector, and the associated covariance matrix is introduced
$$\begin{aligned} {\hat{x}_{\alpha }^{-}}(k)= \begin{pmatrix} \hat{x}^{-}(k) \\ \hat{w}^{-}(k) \end{pmatrix}, \ \ {{P^{\alpha }}^{-}}(k)= \left( \begin{array}{ll} P^{-}(k) &{}\quad 0 \\ 0 &{}\quad Q^{-}(k) \end{array}\right) \end{aligned}$$
(59)
As in the case of local (lumped) Unscented Kalman Filters, a set of weighted sigma points \({X_{\alpha }^{i}}^{-}(k)\) is generated as
$$\begin{aligned} {X_{\alpha ,0}^{-}}(k)= & {} {\hat{x}_{\alpha }^{-}}(k) \nonumber \\ {X_{\alpha ,i}^{-}}(k)= & {} {\hat{x}_{\alpha }^{-}}(k)\!+\!\left[ \sqrt{(n_{\alpha }\!+\!\lambda ){P_{\alpha }^{-}}(k-1)}\right] _i\!\!, \quad i\!=\!1,\ldots ,n\nonumber \\ {X_{\alpha ,i}^{-}}(k)= & {} {\hat{x}_{\alpha }^{-}}(k)+\left[ \sqrt{(n_{\alpha }+\lambda ){P_{\alpha }^{-}}(k-1)}\right] _i,\nonumber \\&i=n+1,\ldots ,2n \end{aligned}$$
(60)
where \(\lambda =\alpha ^2(n_\alpha +\kappa )-n_\alpha \) is a scaling, while \(0{\le }\alpha {\le }1\) and \(\kappa \) are constant parameters. The corresponding weights for the mean and covariance are defined as in the case of the lumped Unscented Kalman Filter
$$\begin{aligned} W_0^{(m)}= & {} {\lambda \over {n_\alpha +\lambda }} \quad W_0^{(c)}={\lambda \over {(n_\alpha +\lambda )+(1-\alpha ^2+\beta )}}\nonumber \\ W_i^{(m)}= & {} {1 \over {2(n_\alpha +\lambda )}}, \quad i=1,\ldots ,2{n_\alpha } \quad W_i^{(C)}={1 \over {2(n_\alpha +\lambda )}}, \nonumber \\ i= & {} 1,\ldots ,2{n_\alpha } \end{aligned}$$
(61)
where \(\beta \) is again a constant parameter. The equations of the prediction stage (measurement update) of the information filter, i.e. the calculation of the information matrix and the information state vector of Eq. (37) now become
$$\begin{aligned} {\hat{y}^{-}}(k)= & {} Y^{-}(k){\sum \limits _{i=0}^{2n_\alpha }}W_i^{m}{X_i^x}(k) \nonumber \\ Y^{-}(k)= & {} {P^{-}(k)}^{-1} \end{aligned}$$
(62)
where \({X_i^x}\) are the predicted state vectors when using the sigma point vectors \(X_i^w\) in the state equation \({X_i^x}(k+1)=\phi ({X_i^w}^{-}(k))+L(k)U(k)\). The predicted state covariance matrix is computed as
$$\begin{aligned} {P^{-}(k)}={\sum _{i=0}^{2n_\alpha }}W_i^{(c)}[{X_i^x}(k)-{\hat{x}^{-}}(k)][{X_i^x}(k)-{\hat{x}^{-}}(k)]^T \end{aligned}$$
(63)
As noted, the equations of the EIF are based on the linearized dynamic model of the system and on the inverse of the covariance matrix of the state vector. However, in the equations of the UKF there is no linearization of the system dynamics, thus the UKF cannot be included directly into the EIF equations. Instead, it is assumed that the nonlinear measurement equation of the system given in Eq. (25) can be mapped into a linear function of its statistical mean and covariance, which makes possible to use the information update equations of the EIF. Denoting \(Y_i(k)=\gamma (X_i^x(k))\) (i.e. the output of the system calculated through the propagation of the i-th sigma point \(X^i\) through the system’s nonlinear equation) the observation covariance and its cross-covariance are approximated by
$$\begin{aligned} {P_{YY}^{-}}(k)= & {} E\left[ (z(k)-\hat{z}^{-}(k))(z(k)-\hat{z}^{-}(k))^T\right] \nonumber \\&\quad {\simeq }{J_{\gamma }}(k){P^{-}(k)}{J_{\gamma }}(k)^T\end{aligned}$$
(64)
$$\begin{aligned} {P_{XY}^{-}}(k)= & {} E\left[ (x(k)-\hat{x}(k)^{-})(z(k)-\hat{z}(k)^{-})^T\right] \nonumber \\&\quad {\simeq }{P^{-}(k)}{J_{\gamma }}(k)^T \end{aligned}$$
(65)
where \(z(k)=\gamma (x(k))\) and \({J_{\gamma }}(k)\) is the Jacobian of the output equation \(\gamma (x(k))\). Next, multiplying the predicted covariance and its inverse term on the right side of the information matrix Eq. (36) and replacing \({P(k)}{J_{\gamma }}(k)^T\) with \({P_{XY}^{-}}(k)\) gives the following representation of the information matrix [38, 39, 40]
$$\begin{aligned} I(k)= & {} {{J_{\gamma }(k)}^T}{R(k)^{-1}}J_{\gamma }(k)\nonumber \\= & {} {{P^{-}(k)}^{-1}}P^{-}(k){{J_{\gamma }}(k)}^T{R(k)^{-1}}{J_{\gamma }^{-}}(k){P^{-}(k)}^T\nonumber \\&\times \left( {{P^{-}(k)}^{-1}}\right) ^{T}\nonumber \\= & {} {{P^{-}(k)}^{-1}}{P_{XY}(k)}{R(k)^{-1}}{P_{XY}(k)}^{T}\left( {{P^{-}(k)}^{-1}}\right) ^T\quad \end{aligned}$$
(66)
where \({{P^{-}(k)}^{-1}}\) is calculated according to Eq. (63) and the cross-correlation matrix \({P_{XY}(k)}\) is calculated from
$$\begin{aligned} {P_{XY}^{-}(k)}={\sum _{i=0}^{2n_\alpha }}W_i^{(c)}\left[ {X_i^x}(k)-{\hat{x}^{-}}(k)\right] \left[ {Y_i}(k)-{\hat{z}^{-}}(k)\right] ^T\nonumber \\ \end{aligned}$$
(67)
where \(Y_i(k)=\gamma ({X_i^x}(k))\) and the predicted measurement vector \(\hat{z}^{-}(k)\) is obtained by \(\hat{z}^{-}(k)={\sum _{i=0}^{2n_\alpha }}W_i^{(m)}Y_i(k)\). Similarly, the information state vector \(i_k\) can be rewritten as
$$\begin{aligned} i(k)= & {} {{J_\gamma }(k)^T}{R(k)^{-1}}\left[ z(k)-\gamma (x(k))+{J_\gamma }(k)^T{\hat{x}^{-}}(k)\right] \nonumber \\= & {} {P^{-}(k)^{-1}}{P^{-}(k)}{{J_\gamma }(k)^T}{R(k)^{-1}}\left[ z(k)-\gamma (x(k))\right. \nonumber \\&\left. +\,{{J_\gamma }(k)^T}(P^{-}(k))^{T}({P^{-}(k)^{-1}})^T{\hat{x}^{-}}(k)\right] \nonumber \\= & {} {P^{-}(k)^{-1}}{P_{XY}^{-}(k)}{R(k)^{-1}}\left[ z(k)-\gamma (x(k))\right. \nonumber \\&\left. +\,{P_{XY}^{-}(k)}({{P^{-}(k)}^{-1}})^T\hat{x}^{-}(k)\right] \end{aligned}$$
(68)
To complete the analogy to the information contribution equations of the EIF a “measurement” matrix \(H^T(k)\) is defined as
$$\begin{aligned} {H(k)}^{T}={{P^{-}(k)}^{-1}}{P_{XY}^{-}}(k) \end{aligned}$$
(69)
In terms of the “measurement” matrix H(k) the information contributions equations are written as
$$\begin{aligned} i(k)= & {} {H^T}(k){R(k)}^{-1}[z(k)-\gamma (x(k))+H(k){\hat{x}^{-}}(k)]\nonumber \\ I(k)= & {} {H^T}(k){R(k)}^{-1}H(k) \end{aligned}$$
(70)
The above procedure leads to an implicit linearization in which the nonlinear measurement equation of the system given in Eq. (25) is approximated by the statistical error variance and its mean
$$\begin{aligned} z(k)=\gamma (x(k)){\simeq }H(k)x(k)+\bar{u}(k) \end{aligned}$$
(71)
where \(\bar{u}(k)=\gamma (\hat{x}^{-}(k))-H(k){\hat{x}^{-}}(k)\) is a measurement residual term.

Calculation of Local Estimations in Terms of UIF Information Contributions

Next, the local estimations provided by distributed (local) Unscented Kalmans filters will be expressed in terms of the information contributions (information matrix I and information state vector i) of the UIF, which were defined in Eq. (70) [38, 39, 40]. It is assumed that the observation vector \(\bar{z}_i(k+1)\) is available from N different sensors, and that each sensor observes a common state according to the local observation model, expressed by
$$\begin{aligned} \bar{z}_i(k)=H_i(k)x(k)+\bar{u}_i(k)+v_i(k) \end{aligned}$$
(72)
where the noise vector \(v_i(k)\) is taken to be white Gaussian and uncorrelated between sensors. The variance of the composite observation noise vector \(v_k\) of all sensors is written in terms of the block diagonal matrix \(R(k)=diag[R_1(k)^T,\ldots ,R_N(k)^T]^T\). Then one can define the local information matrix \(I_i(k)\) and the local information state vector \(i_i(k)\) at the i-th sensor, as follows
$$\begin{aligned} i_i(k)= & {} {H_i^T}(k){{R_i(k)}^{-1}}\left[ z_i(k)-\gamma ^{i}(x(k))+H_i(k){\hat{x}^{-}}(k)\right] \nonumber \\ I_i(k)= & {} {H_i^T}(k){{R_i(k)}^{-1}}{H_i}(k) \end{aligned}$$
(73)
Since the information contribution terms have group diagonal structure in terms of the innovation and measurement matrix, the update equations for the multiple state estimation and data fusion are written as a linear combination of the local information contribution terms
$$\begin{aligned} \hat{y}(k)= & {} {\hat{y}^{-}}(k)+{\sum \limits _{i=1}^N}{i_i(k)}\nonumber \\ Y(k)= & {} Y^{-}(k)+{\sum \limits _{i=1}^N}{I_i}(k) \end{aligned}$$
(74)
Then using Eq. (62) one can find the mean state vector for the multiple sensor estimation problem.

As in the case of the Unscented Kalman Filter, the UIF running at the i-th measurement processing unit can be written in terms of measurement update and time update equations

Measurement update Acquire measurement z(k) and compute
$$\begin{aligned}&Y(k)=P^{-}(k)^{-1}+{H^T}(k){R^{-1}(k)}{H(k)}\nonumber \\&\quad \text {or} \ Y(k)=Y^{-}(k)+I(k) \ \text {where} \nonumber \\&I(k)={H^T}(k){R^{-1}(k)}{H(k)} \end{aligned}$$
(75)
$$\begin{aligned} \hat{y}(k)= & {} {\hat{y}^{-}}(k)+{H^T}(k){R^{-1}(k)}[z(k)-\gamma (\hat{x}(k))\nonumber \\&+\,H(k)\hat{x}^{-}(k)] \ \text {or} \ \hat{y}(k)={\hat{y}^{-}}(k)+i(k) \end{aligned}$$
(76)
Time update Compute
$$\begin{aligned}&Y^{-}(k+1)=(P^{-}(k+1))^{-1} \end{aligned}$$
$$\begin{aligned} \text {where} \ P^{-}(k+1)= & {} {\sum \limits _{i=0}^{2n{_\alpha }}}{W_i^{(c)}}\left[ X_i^x(k+1) -\hat{x}^{-}(k+1)\right] \nonumber \\&\left[ X_i^x(k+1)-\hat{x}^{-}(k+1)\right] ^T \end{aligned}$$
(77)
$$\begin{aligned} \hat{y}(k+1)=Y(k+1){\sum \limits _{i=0}^{2n_{\alpha }}}W_i^{(m)}{X_i^x}(k+1) \end{aligned}$$
$$\begin{aligned} \text {where} \ {X_i^x}(k+1)=\phi ({X_i^w}(k))+L(k)U(k) \end{aligned}$$
(78)

Distributed Unscented Information Filtering for State Estimates Fusion

It has been shown that the update of the aggregate state vector of the UIF architecture can be expressed in terms of the local information matrices \(I_i\) and of the local information state vectors \(i_i\), which in turn depend on the local covariance matrices P and cross-covariance matrices \(P_{XY}\). Next, it will be shown that the update of the aggregate state vector can be also expressed in terms of the local state vectors \(x_i(k)\) and in terms of the local covariance matrices \(P_i(k)\) and cross-covariance matrices \({P_{XY}^{i}}(k)\). It is assumed that the local filters do not have access to each other row measurements and that they are allowed to communicate only their information matrices and their local information state vectors. Thus each local filter is expressed by its respective error covariance and estimate in terms of the local information state contribution \(i_i\) and its associated information matrix \(I_i\) at the i-th filter site (Fig. 8). Then using Eq. (62) one obtains
$$\begin{aligned} {P_i(k)}^{-1}= & {} {P_i^{-}(k)}^{-1}+{{H_i^T}(k)}{{R_i(k)}^{-1}}{H_i(k)}\nonumber \\ {\hat{x}_{i}}= & {} {P_i}(k)({P_i^{-}}(k){\hat{x}_i^{-}}(k)+{H_i^T}(k){{R_i(k)}^{-1}}[z_i(k)\nonumber \\&-\,\gamma ^{i}(x(k))+H_i(k){\hat{x}^{-}}(k)]) \end{aligned}$$
(79)
Using Eq. (79), each local information state contribution \(i_i\) and its associated information matrix \(I_i\) at the i-th filter are rewritten in terms of the computed estimates and covariances of the local filters
$$\begin{aligned}&{{H_i^T}(k)}{{R_i(k})^{-1}}{H_i(k)}={P_i}^{-1}(k)-{P_i^{-}}(k)^{-1}\nonumber \\&{H_i^T}(k){{R_i(k)}^{-1}}[z_i(k)-\gamma ^{i}(x(k))+H_i(k){\hat{x}^{-}}(k)])\nonumber \\&\quad ={{P_i}(k)^{-1}}{\hat{x}_{i}}(k)-{{P_i^{-}}(k)^{-1}}{\hat{x}_i^{-}}(k) \end{aligned}$$
(80)
where according to Eq. (69) it holds \(H_i(k)={{P_i^{-}}(k)^{-1}}{P_{XY,i}^{-}(k)}\). Next, the aggregate estimates of the distributed unscented information filtering are derived for a number of N local filters \(i=1,\ldots ,N\) and sensor measurements, first in terms of covariances [38, 39, 40].
$$\begin{aligned} {P(k)}^{-1}= & {} {P^{-}(k)}^{-1}+{\sum \limits _{i=1}^N}\left[ {P_i(k)}^{-1}-{P_i^{-}(k)}^{-1}\right] \nonumber \\ \hat{x}(k)= & {} P(k)\left[ {P^{-}(k)}^{-1}{\hat{x}^{-}}(k)\right. \nonumber \\&\left. +\,{\sum \limits _{i=1}^N}({{P_i(k)}^{-1}}{\hat{x}_{i}}(k)-{{P_i^{-}(k)}^{-1}}{\hat{x}_i^{-}}(k))\right] \end{aligned}$$
(81)
and also in terms of the information state vector and of the information state covariance matrix
$$\begin{aligned} \hat{y}(k)= & {} {\hat{y}^{-}}(k)+{\sum \limits _{i=1}^N}({\hat{y}_{i}}(k)-{\hat{y}_i^{-}}(k))\nonumber \\ Y(k)= & {} Y^{-}(k)+{\sum \limits _{i=1}^N}[Y_i(k)-Y_i^{-}(k)] \end{aligned}$$
(82)
State estimation fusion based on the UIF is fault tolerant. From Eq. (81) it can be seen that if a local filter (processing station) fails, then the local covariance matrices and local estimates provided by the rest of the filters will enable a reliable calculation of the system’s state vector. Moreover, the UIF is computationally more efficient comparing to centralized filters and results in enhanced estimation accuracy.
Fig. 8

Schematic diagram of the unscented information filter loop

Filtering Using Differential Theory and Canonical Forms

Conditions for Applying the Differential Flatness Theory

Next, a new filter will be developed, in accordance to differential flatness theory. It will be shown that the filter can be efficiently used in the problem of multiple-USVs navigation and can be also the basis of a distributed filtering method. First, the generic class of nonlinear systems \(\dot{x}=f(x,u)\) (including MIMO systems) is considered. Such systems can be transformed to the form of an affine in-the-input system by adding an integrator to each input [10, 11]
$$\begin{aligned} \dot{x}=f(x)+{\sum \limits _{i=1}^m}g_i(x){u_i} \end{aligned}$$
(83)
The following definitions are now used [8, 9, 33]:
  1. (i)

    Lie derivative: \({L_f}h(x)\) stands for the Lie derivative \({L_f}h(x)=({\nabla }h)f\) and the repeated Lie derivatives are recursively defined as \({L_f^0}h=h \ \ \text {for} \ i=0, {L_f^i}h={L_f}{{L_f^{i-1}}h}=\nabla {L_f^{i-1}h}f \ \ \text {for} \ i=1,2,\cdots \).

     
  2. (ii)

    Lie Bracket: \({ad_f^i}g\) stands for a Lie Bracket which is defined recursively as \({ad_f^i}g=[f,{ad_f^{i-1}}g]\) with \({ad_f^0}g=g\) and \({ad_f}g=[f,g]={{\nabla }g}f-{{\nabla }f}g\).

     
If the system of Eq. (83) can be linearized by a diffeomorphism \(z=\phi (x)\) and a static state feedback \(u=\alpha (x)+\beta (x)v\) into the following form
$$\begin{aligned} \dot{z}_{i,j}= & {} z_{i+1,j} \ \text {for} \ 1{\le }j{\le }m \ \text {and} \ 1{\le }i{\le }v_j-1 \nonumber \\ \dot{z}_{v_{i,j}}= & {} {v_j} \end{aligned}$$
(84)
with \({\sum _{j=1}^m}{v_j}=n\), then \(y_j=z_{1,j}\) for \(1{\le }j{\le }m\) are the 0-flat outputs which can be written as functions of only the elements of the state vector x. To define conditions for transforming the system of Eq. (83) into the canonical form described in Eq. (84) the following theorem holds [11]

Theorem

For nonlinear systems described by Eq. (83) the following variables are defined: (i) \(G_0=\text {span}[g_1,\ldots ,g_m]\), (ii) \(G_1=\text {span}[g_1,\ldots ,g_m,ad_f{g_1},\ldots ,ad_f{g_m}], \cdots \) (k) \(G_k=\text {span}\{{ad_f^j}{g_i} \ \text {for} \ 0{\le }j{\le }k, \ 1{\le }i{\le }m \}\). Then, the linearization problem for the system of Eq. (83) can be solved if and only if: (1). The dimension of \(G_i, \ i=1,\ldots ,k\) is constant for \(x{\in }X{\subseteq }R^n\) and for \(1{\le }i{\le }n-1\), (2). The dimension of \(G_{n-1}\) if of order n, (3). The distribution \(G_k\) is involutive for each \(1{\le }k{\le }{n-2}\).

Transformation of MIMO Systems into Canonical Forms

It is assumed now that after defining the flat outputs of the initial MIMO nonlinear system (this approach will be also shown to hold for the kinematics of the target), and after expressing the system state variables and control inputs as functions of the flat output and of the associated derivatives, the system can be transformed in the Brunovsky canonical form:
$$\begin{aligned}&\dot{x}_1=x_2\nonumber \\&\cdots \nonumber \\&\dot{x}_{r_1-1}=x_{r_1} \nonumber \\&\dot{x}_{r_1}=f_1(x)+{\sum \limits _{j=1}^p}{g_{1_j}(x)}u_j+d_1\nonumber \\&\dot{x}_{r_1+1}=x_{r_1+2}\nonumber \\&\cdots \nonumber \\&\dot{x}_{p-1}=x_{p} \nonumber \\&\dot{x}_{p}=f_p(x)+{\sum \limits _{j=1}^p}{g_{p_j}(x)}u_j+d_p\nonumber \\&y_1=x_1\nonumber \\&\cdots \nonumber \\&y_p=x_{n-r_p+1} \end{aligned}$$
(85)
where \(x=[x_1,\ldots ,x_n]^T\) is the state vector of the transformed system (according to the differential flatness formulation), \(u=[u_1,\ldots ,u_p]^T\) is the set of control inputs, \(y=[y_1,\ldots ,y_p]^T\) is the output vector, \(f_i\) are the drift functions and \(g_{i,j}, \ i,j=1,2,\ldots ,p\) are smooth functions corresponding to the control input gains, while \(d_j\) is a variable associated to external disturbances. It holds that \(r_1+r_2+\cdots +r_p=n\). Having written the initial nonlinear system into the canonical (Brunovsky) form it holds
$$\begin{aligned} y_i^{(r_i)}=f_i(x)+{\sum \limits _{j=1}^p}g_{ij}(x)u_j+d_j \end{aligned}$$
(86)
Next the following vectors and matrices can be defined: \(f(x)=[f_1(x),\ldots ,f_n(x)]^T, g(x)=[g_1(x),\ldots ,g_n(x)]^T\), with \(g_i(x)\!=\![g_{1i}(x),\ldots ,g_{pi}(x)]^T, A\!=\!diag[A_1,\ldots ,A_p], B=diag[B_1,\ldots ,B_p], C^T=diag[C_1,\ldots ,C_p], d=[d_1,\ldots ,d_p]^T\), where matrix A has the MIMO canonical form, i.e. with block-diagonal elements
$$\begin{aligned} A_i= & {} \begin{pmatrix} 0 &{}\quad 1 &{}\quad \cdots &{}\quad 0 \\ 0 &{}\quad 0 &{}\quad \cdots &{}\quad 0 \\ \vdots &{}\quad \vdots &{}\quad \cdots &{}\quad \vdots \\ 0 &{}\quad 0 &{}\quad \cdots &{}\quad 1 \\ 0 &{}\quad 0 &{}\quad \cdots &{}\quad 0 \end{pmatrix}_{{r_i}\times {r_i}} \nonumber \\ B_i^T= & {} \begin{pmatrix} 0&\quad 0&\quad \cdots&\quad 0&\quad 1 \end{pmatrix}_{1{\times }{r_i}}\nonumber \\ C_i= & {} \begin{pmatrix} 1&\quad 0&\quad \cdots&\quad 0&\quad 0 \end{pmatrix}_{1{\times }{r_i}} \end{aligned}$$
(87)
Thus, Eq. (86) can be written in state-space form
$$\begin{aligned} \dot{x}= & {} Ax+Bv+B\tilde{d}\nonumber \\ y= & {} {C}x \end{aligned}$$
(88)
where the control input is written as \(v=f(x)+g(x)u\). The system of Eq. (87) and Eq. (88) is in controller and observer canonical form.

Canonical Forms for the USV Model

It is assumed now that the target’s velocity has to be estimated through the processing of the sequence of position measurements by a filtering algorithm. To this end the derivative-free Kalman Filter for MIMO nonlinear dynamical systems can been used. From the application of the differential flatness theory for transforming the initial target USV’s model into a linearized equivalent that is finally written in the Brunovsky form, one has Eq. (4) which means \(\ddot{x}=u_1\) and \(\ddot{y}=u_2\). Next, the state variables \(x_1=x, x_2=\dot{x}, x_3=y\) and \(x_4=\dot{y}\) are defined. Considering the state vector \(x{\in }R^{4\times 1}\), the following matrices are also defined
$$\begin{aligned} A= & {} \begin{pmatrix} 0 &{}\quad 1 &{}\quad 0 &{}\quad 0 \\ 0 &{}\quad 0 &{}\quad 0 &{}\quad 0 \\ 0 &{}\quad 0 &{}\quad 0 &{}\quad 1 \\ 0 &{}\quad 0 &{}\quad 0 &{}\quad 0 \end{pmatrix}, \ \ B= \begin{pmatrix} 0 &{}\quad 0 \\ 1 &{}\quad 0 \\ 0 &{}\quad 0 \\ 0 &{}\quad 1 \end{pmatrix} \nonumber \\ C= & {} \begin{pmatrix} 1 &{}\quad 0 &{}\quad 0 &{}\quad 0 \\ 0 &{}\quad 0 &{}\quad 1 &{}\quad 0 \end{pmatrix} \end{aligned}$$
(89)
Using the matrices of Eq. (89) one obtains the Brunovsky form of the MIMO model of the target \(\dot{x}=Ax+Bv\) and \(y=Cx\), where the new input v is given by \(v=[u_1(x,t),u_2(x,t)]^T\). This is a state-space model in the form of Eq. (88), for which state estimation can be performed using the standard Kalman Filter recursion.
Fig. 9

Distance of the target’s reference point i from the reference plane \(P^{j}\), measured in the inertial coordinates system OXY

Derivative-free Extended Information Filtering

As mentioned above, for the system of Eq. (89), state estimation is possible by applying the standard Kalman Filter. The system is first turned into discrete-time form using common discretization methods and then the recursion of the linear Kalman Filter described in Eq. (90) and Eq. (91) is applied.

Measurement update
$$\begin{aligned} K(k)= & {} {P^{-}(k)}{C^T}[C{\cdot }P^{-}(k){C^T}+R]^{-1} \nonumber \\ {\hat{x}}(k)= & {} {\hat{x}^{-}}(k)+K(k)[z(k)-C{\hat{x}^{-}}(k)] \nonumber \\ P(k)= & {} P^{-}(k)-K(k)CP^{-}(k) \end{aligned}$$
(90)
Time update
$$\begin{aligned} P^{-}(k+1)= & {} {A(k)}P(k)A^T(k)+Q(k)\nonumber \\ {\hat{x}^{-}}(k+1)= & {} A(k)\hat{x}(k)+B(k)u(k) \end{aligned}$$
(91)
If the derivative-free Kalman Filter is used in place of the Extended Kalman Filter then in the EIF equations the following matrix substitutions should be performed: \(J_{\phi }(k){\rightarrow }A_d, J_{\gamma }(k){\rightarrow }C_d\), where matrices \(A_d\) and \(C_d\) are the discrete-time equivalents of matrices A and C which have been defined Eq. (89) and which appear also in the measurement and time update of the standard Kalman Filter recursion, given above. Matrices \(A_d\) and \(C_d\) can be computed using established discretization methods. Moreover, the covariance matrices P(k) and \(P^{-}(k)\) are the ones obtained from the linear Kalman Filter update equations given in the previous Eq. (90) and Eq. (91).

Simulation Tests

Estimation of Target’s Position with the use of the Extended Information Filter

The number of USVs used for target tracking in the simulation experiments was \(N=10\). However, since the USVs fleet (mobile sensor network) is modular a larger number of USVs could have been also considered. It is assumed that each USV can obtain an estimation of the target’s cartesian coordinates and bearing, i.e. the target’s cartesian coordinates [xy] as well as the target’s orientation \(\theta \). To improve the accuracy of the target’s localization, the target’s coordinates and bearing are fused with the distance of the target from a reference surface measured in an inertial coordinates system (see Figs. 2, 9).

The inertial coordinates system OXY is defined. Furthermore the coordinates system \(O'X'Y'\) is considered (Fig.  9). \(O'X'Y'\) results from OXY if it is rotated by an angle \(\theta \) (Fig. 9). The coordinates of the center of the target vessel with respect to OXY are (xy), while the coordinates of the reference point i that on the target vessel (e.g. bridge), with respect to \(O'X'Y'\) are \(x_i^{'},y_i^{'}\). The orientation of the reference point with respect to \(OX'Y'\) is \(\theta _i^{'}\). Thus the coordinates of the reference point with respect to OXY are \((x_i,y_i)\) and its orientation is \(\theta _i\), and are given by:
$$\begin{aligned} x_i(k)= & {} x(k)+x_i^{\prime }sin(\theta (k))+y_i^{\prime }cos(\theta (k))\nonumber \\ y_i(k)= & {} y(k)-x_i^{\prime }cos(\theta (k))+y_i^{\prime }sin(\theta (k)) \nonumber \\ \theta _i(k)= & {} \theta (k)+\theta _i \end{aligned}$$
(92)
Each plane \(P^j\) in the USV’s environment can be represented by \(P_r^j\) and \(P_n^j\) (Fig. 9), where (i) \(P_r^j\) is the normal distance of the plane from the origin O, (ii) \(P_n^j\) is the angle between the normal line to the plane and the x-direction.
The target’s reference point i is at position \(x_i(k),y_i(k)\) with respect to the inertial coordinates system OXY and its orientation is \(\theta _i(k)\). Using the above notation, the distance of the reference point i, from the plane \(P^j\) is represented by \(P_r^j,P_n^j\) (see Fig. 9):
$$\begin{aligned} d_i^j(k)=P_r^j-x_i(k)cos\left( P_n^j\right) -y_i(k)sin\left( P_n^j\right) \end{aligned}$$
(93)
Assuming a constant sampling period \({\Delta }t_k=T\) the measurement equation is \(z(k+1)=\gamma (x(k))+v(k)\), where z(k) is the vector containing target’s coordinates and bearing estimates obtained from a mobile sensor and the measurement of the target’s distance to the reference surface, while v(k) is a white noise sequence \(\sim N(0,R(kT))\). The measure vector z(k) can thus be written as
$$\begin{aligned} z(k)= & {} \left[ x(k)+{v_1}(k), y(k)+{v_2}(k), {\theta }(k)\right. \nonumber \\&\left. +\,{v_3}(k),d_1^j(k)+v_4(k)\right] \end{aligned}$$
(94)
with \(i=1,2,\ldots ,n_s, d_i^j(k)\) to be the distance measure with respect to the plane \(P^j\) and \(j=1,\ldots ,n_p\) to be the number of reference surfaces. By definition of the measurement vector one has that the output function \(\gamma (x(k))\) is given by \(\gamma (x(k))=[x(k),y(k),\theta (k),d_1^{1}(k)]\).

To obtain the Extended Kalman Filter (EKF), the kinematic model of the target described in Eq. (2) is discretized and written in the discrete-time state-space form of Eq.(25) [16, 46].

The measurement update of the EKF is
$$\begin{aligned} K(k)= & {} P^{-}(k)J_{\gamma }^T(\hat{x}^{-}(k))\left[ J_{\gamma }(\hat{x}^{-}(k))P^{-}(k)J_{\gamma }^T(\hat{x}^{-}(k))\right. \\&\left. +\,R(k) \right] ^{-1}\\ \hat{x}(k)= & {} \hat{x}^{-}(k)+K(k)\left[ z(k)-\gamma \left( \hat{x}^{-}(k)\right) \right] \\ P(k)= & {} P^{-}(k)-K(k){J_{\gamma }^T}P^{-}(k) \end{aligned}$$
The time update of the EKF is
$$\begin{aligned} P^{-}(k+1)= & {} J_{\phi }(\hat{x}(k))P(k)J_{\phi }^{T}(\hat{x}(k))+Q(k) \\ {\hat{x}^{-}}(k+1)= & {} \phi (\hat{x}(k))+L(k)U(k) \end{aligned}$$
where
$$\begin{aligned} L(k)= \begin{pmatrix} T{cos(\theta (k))} &{}\quad 0 \\ T{sin(\theta (k))} &{}\quad 0 \\ 0 &{}\quad T \end{pmatrix} \end{aligned}$$
and
$$\begin{aligned} J_{\phi }(\hat{x}(k))= \begin{pmatrix} 1 &{}\quad 0 &{}\quad -v(k)sin(\theta )T \\ 0 &{}\quad 1 &{}\quad -v(k)cos(\theta )T \\ 0 &{}\quad 0 &{}\quad 1 \end{pmatrix} \end{aligned}$$
while \(Q(k)=diag[{\sigma ^2}(k),{\sigma ^2}(k),{\sigma ^2}(k)]\), with \({\sigma ^2}(k)\) chosen to be \(10^{-3}\) and \(\phi (\hat{x}(k))=[\hat{x}(k),\hat{y}(k),\hat{\theta }(k)]^T\), \(\gamma (\hat{x}(k))=[\hat{x}(k),\hat{y}(k),\hat{\theta }(k),d(k)]^T\), i.e.
$$\begin{aligned} \gamma (\hat{x}(k))= \begin{pmatrix} \hat{x}(k) \\ \hat{y}(k) \\ \hat{\theta }(k) \\ P_r^j-x_i(k))cos(P_n^j)-y_i(k)sin(P_n^j) \end{pmatrix} \end{aligned}$$
(95)
The vector of the control input is given by \(U(k)=[v(k),\omega (k)]^T\). Assuming one reference surface in the target’s neighborhood one gets
$$\begin{aligned}&J_{\gamma }^T(\hat{x}^{-}(k))\nonumber \\&\quad =\left[ {J_{\gamma }}_1(\hat{x}^{-}(k)),{J_{\gamma }}_2(\hat{x}^{-}(k)), {J_{\gamma }}_3(\hat{x}^{-}(k)),{J_{\gamma }}_4(\hat{x}^{-}(k))\right] ^T, \text {i.e.}\nonumber \\&J_{\gamma }^T(\hat{x}^{-}(k))\nonumber \\&\quad =\!\begin{pmatrix} 1 &{}\quad 0 &{}\quad 0 \\ 0 &{}\quad 1 &{}\quad 0 \\ 0 &{}\quad 0 &{}\quad 1 \\ -cos(P_n^j) &{}\quad -sin(P_n^j) &{}\quad \{x_i^{'}cos(\theta -P_n^j)-y_i^{'}sin(\theta -P_n^j)\} \\ \end{pmatrix}\nonumber \\ \end{aligned}$$
(96)
The use of EKF for fusing the target’s coordinates and bearing measured by each USV with the target’s distance from a reference surface measured in an inertial coordinates system provides an estimation of the state vector \([x(t),y(t),\theta (t)]\) and enables the successful tracking of the target’s motion by the individual USVs (mobile sensors).
Fig. 10

a EIF-based target tracking by a fleet of USVs when the target follows a circular trajectory in an obstacles-free motion space, b Aggregate estimation of the target’s position with the use of extended information filtering (continuous line) and target’s reference path (dashed line)

Fig. 11

a EIF-based target tracking by a fleet of USVs when the target follows an eight-shaped trajectory in an obstacles-free motion space, b Aggregate estimation of the target’s position with the use of extended information filtering (continuous line) and target’s reference path (dashed line)

Fig. 12

a EIF-based target tracking by a fleet of USVs when the target follows a curve-shaped trajectory in an obstacles-free motion space, b Aggregate estimation of the target’s position with the use of extended information filtering (continuous line) and target’s reference path (dashed line)

Fig. 13

a EIF-based target tracking by a fleet of USVs when the target follows a line path in a motion space with obstacles, b Aggregate estimation of the target’s position with the use of extended information filtering (continuous line) and target’s reference path (dashed line)

Fig. 14

a EIF-based target tracking by a fleet of USVs when the target follows a circular path in a motion space with obstacles, b Aggregate estimation of the target’s position with the use of extended information filtering (continuous line) and target’s reference path (dashed line)

The tracking of the target by the fleet of the USVs was tested in the case of several reference trajectories, both for motion in an environment without obstacles as well as for motion in a plane containing obstacles. The proposed distributed filtering scheme enabled accurate estimation of the target’s state vector \([x,y,\theta ]^T\) through the fusion of the measurements of the target’s coordinates and orientation obtained by each USV with the measurement of the distance from a reference surface in an inertial coordinates frame. The state estimates provided by the Extended Kalman Filters running at each mobile sensor were fused into one single state estimate using Extended Information filtering. The aggregate estimated coordinates of the target \((\hat{x}^{*},\hat{y}^{*})\) provided the reference setpoint for the distributed motion planning algorithm. Each mobile sensor was made to move along the path defined by \((\hat{x}^{*},\hat{y}^{*})\). The convergence properties of the distributed motion planning algorithm were described in “Distributed Motion Planning for the Multi-USV System” section. The tracking of the target’s trajectory by the USVs as well as the accuracy of the two-level sensor fusion-based estimation of the target’s coordinates is depicted in Figs. 10, 11, 12, 13 and 14. The target is marked as a thick-line rectangle and the associated reference trajectory is plotted as a thick line.

It is noted that using distributed EKFs and fusion through the EIF is more robust comparing to the centralized EKF since (i) if a local processing unit is subject to a fault then state estimation is still possible and can be used for accurate localization of the target, as well as for tracking of target’s trajectory by the individual mobile sensors (unmanned surface vessels), (ii) communication overhead remains low even in the case of a large number of distributed mobile sensors, because the greatest part of state estimation procedure is performed locally and only information matrices and state vectors are communicated between the local processing units, (iii) the aggregation performed on the local EKF also compensates for deviations in state estimates of local filters (which can be due to linearization errors).

Estimation of Target’s Position with the use of Unscented Information Filtering

Next, the estimation of the target’s state vector was performed using the UIF. Again, the proposed distributed filtering enabled precise estimation of the target’s state vector \([x,y,\theta ]^T\) through the fusion of measurements of the target’s coordinates and bearing obtained by each mobile sensor with the distance of the target from a reference surface measured in an inertial coordinates system. The state estimates of the local Unscented Kalman Filters running at each mobile sensor (USV) were aggregated into a single estimation by the UIF. The estimated coordinates \([\hat{x}^{*},\hat{y}^{*}]\) of the target were used to generate the reference path which was followed by the mobile sensors. The tracking of the target’s trajectory by the USVs ensemble as well as the accuracy of the two-level sensor fusion-based estimation of the target’s position is shown in Figs. 15, 16, 17, 18 and 19.

As previously analyzed, the UIF is a derivative-free distributed filtering approach in which the local Unscented Kalman Filters provide estimations of the target’s coordinates using the update in-time of a number of suitably chosen sigma-points. Therefore, unlike the EIF and the local Extended Kalman Filters contained in it, in the Unscented Information Filter there is no need to calculate Jacobians through the computation of partial derivatives. Additionally, unlike the case of local Extended Kalman Filters there is no truncation of higher order Taylor expansion terms and no linearization errors are introduced at the local estimators. In that sense the Unscented Information Filter provides a robust distributed state estimation and enables accurate tracking of the target by the mobile sensors (USVs).
Fig. 15

a UIF-based target tracking by a fleet of USVs when the target follows a circular trajectory in an obstacles-free motion space, b Aggregate estimation of the target’s position with the use of unscented information filtering (continuous line) and target’s reference path (dashed line)

Fig. 16

a UIF-based target tracking by a fleet of USVs when the target follows an eight-shaped trajectory in an obstacles-free motion space, b Aggregate estimation of the target’s position with the use of unscented information filtering (continuous line) and target’s reference path (dashed line)

Fig. 17

a UIF-based target tracking by a fleet of USVs when the target follows a curve-shaped trajectory in an obstacles-free motion space, b Aggregate estimation of the target’s position with the use of unscented information filtering (continuous line) and target’s reference path (dashed line)

Fig. 18

a UIF-based target tracking by a fleet of USVs when the target follows a line path in a motion space with obstacles, b Aggregate estimation of the target’s position with the use of unscented information filtering (continuous line)

Fig. 19

a UIF-based target tracking by a fleet of USVs when the target follows a circular path in a motion space with obstacles, b Aggregate estimation of the target’s position with the use of unscented information filtering (continuous line) and target’s reference path (dashed line)

Estimation of the Target’s Position with the Derivative-free Distributed Nonlinear Kalman Filter

The DEIF is also used to solve the problem of the synchronized USVs navigation based on distributed state estimation. In the latter case, local Derivative-free Kalman Filters perform fusion of the target’s coordinates measurements \((x_i,y_i)\) with the distance \(d_i\) of the target from a reference surface, as follows:

The target’s state vector that was written in the observer canonical form described by Eq. (89) is extended through the inclusion of additional state variables that describe the dynamics of the distance measurement d with respect to the reference surface \(P^{j}\). Thus the extended state vector of the system now becomes \(x_e=[x_1,x_2,x_3,x_4,x_5,x_6]^T\) with \(x_1=x, x_2=\dot{x}, x_3=y, x_4=\dot{y}, x_5=d\) and \(x_6=\dot{d}\). The extended output vector is written as \(y_e=[y_1,y_2,y_3]^T\), with \(y_1=x, y_2=y\) and \(y_3=d\) which means that measurements of the target’s cartesian coordinates (xy) and of the target’s distance d from the reference surface can be obtained. The distance measuring sensor is taken to coincide with the point defining the cartesian coordinates of the target (e.g. center of gravity). In that case, from Eq. (92) one has \(x_i=x\) and \(y_i=y\). The target’s kinematics is written in the new state-space form which is also an observer canonical form:
$$\begin{aligned} \dot{x}_e= & {} {A_e}{x_e}+{B_e}{v_e} \nonumber \\ y_e= & {} {C_e}{x_e} \end{aligned}$$
(97)
while the associated state-space matrices are
$$\begin{aligned} A_e= & {} \begin{pmatrix} 0 &{}\quad 1 &{}\quad 0 &{}\quad 0 &{}\quad 0 &{}\quad 0 \\ 0 &{}\quad 0 &{}\quad 0 &{}\quad 0 &{}\quad 0 &{}\quad 0 \\ 0 &{}\quad 0 &{}\quad 0 &{}\quad 1 &{}\quad 0 &{}\quad 0 \\ 0 &{}\quad 0 &{}\quad 0 &{}\quad 0 &{}\quad 0 &{}\quad 0 \\ 0 &{}\quad 0 &{}\quad 0 &{}\quad 0 &{}\quad 0 &{}\quad 1 \\ 0 &{}\quad 0 &{}\quad 0 &{}\quad 0 &{}\quad 0 &{}\quad 0 \end{pmatrix}\quad B_e= \begin{pmatrix} 0 &{}\quad 0 &{}\quad 0 \\ 1 &{}\quad 0 &{}\quad 0 \\ 0 &{}\quad 0 &{}\quad 0 \\ 0 &{}\quad 1 &{}\quad 0 \\ 0 &{}\quad 0 &{}\quad 0 \\ 0 &{}\quad 0 &{}\quad 1 \end{pmatrix}\end{aligned}$$
(98)
$$\begin{aligned} C_e= & {} \begin{pmatrix} 1 &{}\quad 0 &{}\quad 0 &{}\quad 0 &{}\quad 0 &{}\quad 0 \\ 0 &{}\quad 0 &{}\quad 1 &{}\quad 0 &{}\quad 0 &{}\quad 0 \\ 0 &{}\quad 0 &{}\quad 0 &{}\quad 0 &{}\quad 1 &{}\quad 0 \end{pmatrix} \end{aligned}$$
(99)
while the extended inputs vector is defined as \(u_e=[u_1,u_2,u_3]^T\) where \(u_1\) and \(u_2\) were defined in Eq. (4). Assuming that the incidence angle \(P_n\) does not vary in time (see Fig. (9)), one has
$$\begin{aligned} u_3=-{\ddot{x}_1}cos(P_n)-{\ddot{x}_3}sin(P_n) \end{aligned}$$
(100)
It is noted that knowing the orientation of the landmark surface in a cartesian coordinates system, the coordinates of the target at time instant \(t=k{T_s}\) and the coordinates of a reference point on the landmark surface, it is always possible to compute the incidence angle \(P_n\). Results about the performance of the DEIF in estimating the state vector of the target and about using the target’s localization procedure for implemented distributed control of the pursuer USVs is given in Figs.  20, 21, 22, 23 and 24. It can be noticed that, whilst computationally simpler, the DEIF provides very accurate estimates of the target’s state vector.
Fig. 20

Target following reference path 1 a tracking of the target by the USVs using DEIF b estimation of the target’s coordinates through sensor fusion and derivative-free distributed nonlinear Kalman filtering

Fig. 21

Target following reference path 2 a tracking of the target by the USVs using DEIF b estimation of the target’s coordinates through sensor fusion and derivative-free distributed nonlinear Kalman filtering

Fig. 22

Target following reference path 3 a tracking of the target by the USVs using DEIF b estimation of the target’s coordinates through sensor fusion and derivative-free distributed nonlinear Kalman filtering

Fig. 23

Target following reference path 4, amidst obstacles a tracking of the target by the USVs using DEIF b estimation of the target’s coordinates through sensor fusion and derivative-free distributed nonlinear Kalman filtering

Fig. 24

Target following reference path 5, amidst obstacles a tracking of the target by the USVs using DEIF b estimation of the target’s coordinates through sensor fusion and derivative-free distributed nonlinear Kalman filtering

Indicative results about the accuracy of estimation provided by the considered nonlinear filtering algorithms (i.e. EIF, UIF and DEIF), as well as about the accuracy of tracking succeeded by the associated state estimation-based control loop are given in Table 1. It can be noticed that the DEIF is significantly more accurate and robust than the Extended Information Filter. Its accuracy is comparable to the one of the Unscented Information Filter. Results on the total runtime and the cycle time of the aforementioned distributed filtering algorithms are given in Table 2 (using the Matlab platform on a PC with an Intel i7 processor at 1.6GHz).

Conclusions

Distributed control has been implemented to unmanned surface vessels (USVs) pursuing a target vessel. The objective of the control system was to achieve the synchronized convergence of the vessels to the target’s position while also avoiding collisions between them and collisions with obstacles in their motion plane. The problem of distributed motion planning for the individual USVs has been solved with the use of a suitable Lyapunov function which comprised quadratic terms associated with the distance of the USVs from the target’s position, as well as quadratic terms associated with the distance of the USVs to each other. By applying LaSalle’s theorem it has been proven that the individual USVs will track the target and actually they will remain in a confined area round the target’s position. It is also assured that collisions between the USVs will be deterred.
Table 1

RMSE of tracking with nonlinear filtering (Gaussian noise)

 

\(\mathrm{{RMSE}}_x\)

\(\mathrm{{RMSE}}_y\)

\(\mathrm{{RMSE}}_{\theta }\)

UIF

0.0088

0.0104

0.0013

EIF

0.0123

0.0167

0.0019

DEIF

0.0087

0.0093

0.0013

Table 2

Run time of nonlinear estimation algorithms

 

UIF

EIF

DEIF

Total runtime (sec)

203.97

181.04

162.65

Cycle time (sec)

0.0410

0.0366

0.0325

Another major problem in the design of the proposed distributed controller for the USVs has been the localization of the target vessel and the estimation of its motion characteristics. To this end a new nonlinear distributed filtering approach under the name Derivative-free distributed nonlinear Kalman Filter has been introduced. Actually, this filter consists of fusion states estimates about the target’s motion provided by local nonlinear Kalman filters running on the individual USVs. Each one of these local Kalman filters is based on differential flatness-theory and on the transformation of the target’s kinematic model into a canonical state-space description. It has been demonstrated that comparing to the EIF and to UIF, the proposed nonlinear Kalman Filter has improved performance both in terms of accuracy of estimation and in terms of speed of computation. These parameters provide a significant advantage for the design of efficient multi-USVs systems for target tracking applications and intelligent maritime transportation systems.

References

  1. 1.
    Xiang, X., Zheng, J., Yu, C., Xu, G.: Nonlinear path following control of autonomous underwater vehicles: under-actuated and fully-actuated cases, In: Proceeding of the 33rd Chinese Control Conference, Nanjing, China (2014)Google Scholar
  2. 2.
    Borhaug, E., Pavlov, A., Panteley, E., Pettersen, K.: Straight-line path following for formations of underactuated marine surface vessels. IEEE Trans. Control Syst. Technol. 13(3), 493–506 (2011)CrossRefGoogle Scholar
  3. 3.
    Thorvaldsen, C., Skjetne, R.: Formation control of fully actuated marine vessels using group agreement protocols. In: 2011 50th IEEE Conference on Decision and Control and European Control Conference (CDC-ECC), Orlando Florida, USA (2011)Google Scholar
  4. 4.
    Marco, B., Cacciu, M., Lapierre, L., Gabriele, B.: Control of unmanned surface vehicles: experiments in vehicle following, IEEE Robotics and Automation Magazine, pp. 92–102 (2012)Google Scholar
  5. 5.
    Almeida, J., Silvestre, C., Pascoal, A.M.: Cooperative control of multiple surface vessels with discrete-time periodic communications. Int. J. Robust Nonlinear Control 22, 398–419 (2011)MathSciNetCrossRefGoogle Scholar
  6. 6.
    Yung, E., Gu, D.: Nonlinear formationn-keeping and mooring control of multiple autonomous underwater vehicles. IEEE/ASME Trans. Mechatron. 12(2), 164–178 (2011)CrossRefGoogle Scholar
  7. 7.
    Rigatos, G.: Modelling and Control for Intelligent Industrial Systems: Adaptive Algorithms in Robotics and Industrial Engineering. Springer, Berlin (2011)CrossRefGoogle Scholar
  8. 8.
    Rigatos, G.: Advanced Models of Neural Networks. Springer, Netherlands (2013)Google Scholar
  9. 9.
    Rigatos, G.: Nonlinear Control and Filtering Using Differential Flatness Approaches: Applications to Electromechanical Systems. Springer, Berlin (2015)CrossRefGoogle Scholar
  10. 10.
    Lévine, J.: On necessary and sufficient conditions for differential flatness. Appl. Algebr. Eng. Commun. Comput. 22(1), 47–90 (2011)CrossRefMATHGoogle Scholar
  11. 11.
    Bououden, S., Boutat, D., Zheng, G., Barbot, J.P., Kratz, F.: A triangular canonical form for a class of 0-flat nonlinear systems. Int. J. Control 84(2), 261–269 (2011)MathSciNetCrossRefMATHGoogle Scholar
  12. 12.
    Rigatos, G.G.: Sensor fusion-based dynamic positioning of ships using extended Kalman and particle filtering. Robotica 31(3), 389–403 (2013)MathSciNetCrossRefGoogle Scholar
  13. 13.
    Rigatoa, G., Raffo, V.: Input-output linearizing control of the underactuated hovercraft using the derivative-free nonlinear Kalman filter, Journal of Unmanned Systems, World Scientific (2015)Google Scholar
  14. 14.
    Vissière, D., Bristeau, P.J., Martin, A.P., Petit, N.: Experimental autonomous flight of a small-scaled helicopter using accurate dynamics model and low-cost sensors, In: Proceedings of the 17th World Congress The International Federation of Automatic Control Seoul, Korea, July 6–11 (2008)Google Scholar
  15. 15.
    Léchevin, N., Rabbath, C.A.: Sampled-data control of a class of nonlinear flat systems with application to unicycle trajectory tracking. ASME J. Dyn. Syst. Meas. Control Trans. ASME 128(3), 722–728 (2006)CrossRefGoogle Scholar
  16. 16.
    Rigatos, G.G.: Extended Kalman and particle filtering for sensor fusion in motion control of mobile robots. Math. Comput. Simul. 81(3), 590–607 (2010)MathSciNetCrossRefMATHGoogle Scholar
  17. 17.
    Fliess, M., Mounier, H.: Tracking control and \(\pi \)-freeness of infinite dimensional linear systems. In: Picci, G., Gilliam, D.S. (eds.) Dynamical Systems, Control, Coding and Computer Vision, pp. 41–68. Birkhaüser, Boston (1999)Google Scholar
  18. 18.
    Villagra, J., D’Andrea-Novel, B., Mounier, H., Pengov, M.: Flatness-based vehicle steering control strategy with SDRE feedback gains tuned via a sensitivity approach. IEEE Trans. Control Syst. Technol. 15, 554–565 (2007)CrossRefGoogle Scholar
  19. 19.
    Oriolo, G., De Luca, A., Vendittelli, M.: WMR control via dynamic feedback linearization: Design. Implementation and experimental validation. IEEE Trans. Control Syst. Technol. 10(6), 835–852 (2002)CrossRefGoogle Scholar
  20. 20.
    Groß, R., Bonani, M., Mondada, F., Dorigo, M.: Autonomous self-assembly in swarm-bots. IEEE Trans. Robot. 22(6), 1115–1130 (2006)CrossRefGoogle Scholar
  21. 21.
    Bishop, B.E.: On the use of redundant manipulator techniques for control of platoons of cooperative robotic vehicles. IEEE Trans. Syst. Man Cybern. Part A 33(5), 608–615 (2003)CrossRefGoogle Scholar
  22. 22.
    Hong, Y., Gao, L., Cheng, D., Hu, J.: Luapunov-based approach to multi-agent systems with switching jointly connected interconnection. IEEE Trans. Autom. Control 52(5), 943–948 (2007)MathSciNetCrossRefGoogle Scholar
  23. 23.
    Sinha, A., Ghose, D.: Generalization of linear cyclic pursuit with application to Rendezvous of multiple autonomous agents. IEEE Trans. Autom. Control 51(11), 1819–1824 (2006)MathSciNetCrossRefGoogle Scholar
  24. 24.
    Pagello, E., Angelo, A.D’., Menegatti, E.: Cooperation issues and distributed sensing for multi-robot systems. Proc. IEEE 94(7), 1370–1383 (2006)CrossRefGoogle Scholar
  25. 25.
    Sepulchre, R., Paley, D.A., Leonard, N.E.: Stabilization of planar collective motion: all to all communication. IEEE Trans. Autom. Control 52(5), 811–824 (2007)Google Scholar
  26. 26.
    Masoud, M.A., Masoud, A.A.: Motion planning in the presence of directional and regional avoidance constraints using nonlinear, anisotropic, harmonic potential fields: a physical metaphor. IEEE Trans. Syst. Man Cybern. Part A 32(6), 705–723 (2002)CrossRefGoogle Scholar
  27. 27.
    Rigatos, G.G.: Coordinated motion of autonomous vehicles with the use of a distributed gradient algorithm. J. Appl. Math. Comput. 199(2), 494–503 (2008)MathSciNetCrossRefMATHGoogle Scholar
  28. 28.
    Rigatos, G.G.: Distributed gradient and particle swarm optimization for multi-robot motion planning. Robotica 26(3), 357–370 (2008)CrossRefGoogle Scholar
  29. 29.
    Gazi, V., Passino, K.: Stability analysis of social foraging swarms. IEEE Trans. Syst. Man Cybern. Part B 34(1), 539–557 (2004)CrossRefGoogle Scholar
  30. 30.
    Duflo, M.: Algorithmes stochastiques, Mathématiques et Applications, vol. 23. Springer, New York (1996)Google Scholar
  31. 31.
    Comets, F., Meyre, T.: Calcul stochastique et modèles de diffusions. Dunod, New York (2006)Google Scholar
  32. 32.
    Benveniste, A., Métivier, M., Priouret, P.: Adaptive Algorithms and Stochastic Approximations. Springer, Berlin (1990)CrossRefMATHGoogle Scholar
  33. 33.
    Khalil, H.: Nonlinear Systems. Prentice Hall, Englewood Cliffs (1996)Google Scholar
  34. 34.
    Olfati-Saber, R.: Distributed Kalman Filter with embedded consensus filters. In: Proceedings of 44th IEEE Conference on Decision and Control, Seville, pp. 8179–8184 (2005)Google Scholar
  35. 35.
    Olfati-Saber, R.: Distributed tracking for mobile sensor networks with information-driven mobility. In: American Control Conference, ACC 2007, New York, pp. 4606–4612 (2007)Google Scholar
  36. 36.
    Rigatos, G.G., Tzafestas, S.G.: Extended Kalman Filtering for Fuzzy Modeling and Multi-Sensor Fusion, Mathematical and Computer Modeling of Dynamical Systems, vol. 3. Taylor and Francis, London (2007)Google Scholar
  37. 37.
    Rigatos, G.G.: Sigma-point Kalman Filters and particle filters for integrated navigation of unmanned aerial vehicles. In: International Workshop on Robotics for Risky Interventions and Environmental Surveillance, RISE 2009, Brussels (2009)Google Scholar
  38. 38.
    Lee, D.J.: Unscented Information Filtering for Distributed Estimation and multiple sensor fusion, AIAA Guidance, Navigation and Control Conference and Exhibit, Hawai (2008)Google Scholar
  39. 39.
    Lee, D.J.: Nonlinear estimation and multiple sensor fusion using unscented information filtering. IEEE Signal Process. Lett. 15, 861–864 (2008)CrossRefGoogle Scholar
  40. 40.
    Vercauteren, T., Wang, X.: Decentralized sigma-point information filters for target tracking in collaborative sensor networks. IEEE Trans. Signal Process. 53(8), 2997–3009 (2005)MathSciNetCrossRefGoogle Scholar
  41. 41.
    Manyika, J., Durrant-Whyte, H.: Data Fusion and Sensor Management: A Decentralized Information Theoretic Approach. Prentice Hall, Englewood Cliffs (1994)Google Scholar
  42. 42.
    Rigatos, G., Zhang, Q.: Fuzzy model validation using the local statistical approach. Fuzzy Sets Syst. 60(7), 882–904 (2009)MathSciNetCrossRefGoogle Scholar
  43. 43.
    Julier, S., Uhlmann, J., Durrant-Whyte, H.F.: A new method for the nonlinear transformations of means and covariances in filters and estimators. IEEE Trans. Autom. Control 45(3), 477–482 (2000)MathSciNetCrossRefMATHGoogle Scholar
  44. 44.
    Julier, S.J., Uhlmann, J.K.: Unscented filtering and nonlinear estimation. Proc. IEEE 92, 401–422 (2004)CrossRefGoogle Scholar
  45. 45.
    Särrkä, S.: On unscented Kalman Filtering for state estimation of continuous-time nonlinear systems. IEEE Trans. Autom. Control 52(9), 1631–1641 (2007)CrossRefGoogle Scholar
  46. 46.
    Rigatos, G.G.: Particle filtering for state estimation in nonlinear industrial systems. IEEE Trans. Instrum. Meas. 58(11), 3885–3900 (2009)CrossRefGoogle Scholar

Copyright information

© Springer Science+Business Media Singapore 2015

Authors and Affiliations

  1. 1.Unit of Industrial AutomationIndustrial Systems InstituteRion PatrasGreece
  2. 2.Department of Industrial Eng.University of SalernoFiscianoItaly
  3. 3.Department of Electronic Eng.Federal University of Minas GeraisBelo HorizonteBrazil

Personalised recommendations