*2.1. Multiple Target Measurements*

The trace of the *k*-th target can be expressed as

$$\mathbf{x}\_{p+1}^{k} = \mathbf{F}\_{p}\mathbf{x}\_{p}^{k} + \mathbf{v}\_{p}^{k} \tag{1}$$

.

where **x***<sup>k</sup> <sup>p</sup>* is the *k*-th target kinematic state at time *p*, **F***<sup>p</sup>* denotes the transition matrix of target state and **v***k <sup>p</sup>* describes the vector of additive white Gaussian noise (AWGN) with zero mean and covariance **Q***k <sup>p</sup>*. In the Cartesian coordinate system [1], the target kinematic state can be denoted by a vector of six components including position and velocity for each axis **x***<sup>k</sup> <sup>p</sup>* <sup>=</sup> *xp* . *xp yp* . *yp zp* . *zp* During the *<sup>p</sup>*-th scan, a set of *mp* sonar observations **<sup>Z</sup>***<sup>p</sup>* <sup>=</sup> **z***p*,1, **z***p*,2, ... , **z***p*,*mp* are chosen from the system detections. Each observation's **z***p*,*<sup>i</sup>* , *i* = 1, 2, ... , *mp* is a vector of four observed values which comprise both range and velocity components from the *i*-th target **z***p*,*<sup>i</sup>* = **y***c p*,*i* , *y<sup>d</sup> p*,*i* . The accumulated measurement sequence up to the *p*-th scan can be denoted as

$$\mathbf{Z}^{p} = \left\{ \mathbf{Z}\_{1}, \mathbf{Z}\_{2}, \dots, \mathbf{Z}\_{p} \right\} = \left\{ \mathbf{Y}\_{c}^{p}, \mathbf{Y}\_{d}^{p} \right\}. \tag{2}$$

The target position observation has a linear correlation with the target kinematic state. The *k*-th target position observation during the *p*-th scan is denoted as:

$$\mathbf{y}\_p^c = \mathbf{H}\_p^c \mathbf{x}\_p^k + \boldsymbol{\omega}\_p^k. \tag{3}$$

*Sensors* **2019**, *19*, 2003

In Equation (3), **H***<sup>c</sup> <sup>p</sup>* = ⎡ ⎢⎢⎢⎢⎢⎢⎢⎢⎣ 100000 001000 000010 ⎤ ⎥⎥⎥⎥⎥⎥⎥⎥⎦ denotes the system transition matrix. ω*<sup>k</sup> <sup>p</sup>* describes the

vector of AWGN with zero mean and covariance **R***c*,*<sup>k</sup> <sup>p</sup>* . The target velocity measurement from the *k*-th target is denoted as

$$y\_p^d = h(\mathbf{x}\_p^k) + n\_p^k. \tag{4}$$

The measurement error *n<sup>k</sup> <sup>p</sup>* denotes AWGN with zero mean and covariance <sup>R</sup>*d*,*<sup>k</sup> <sup>p</sup>* . According to the Reference [34], the term *h* , **x***k p* - in Equation (4) can be expressed by *<sup>h</sup>* , **x***k p* - = (*xp*−*x<sup>s</sup> p*)(. *xp*−. *x s p*)+(*yp*−*y<sup>s</sup> p*) ,. *yp*<sup>−</sup> . *y s p* - +(*zp*−*zs p*)(. *zp*−. *z s p*) (*xp*−*xs p*) 2 +(*yp*−*y<sup>s</sup> p*) 2 +(*zp*−*zs p*) <sup>2</sup> , here **<sup>x</sup>***<sup>s</sup> <sup>p</sup>* <sup>=</sup> *xs p*, . *x s <sup>p</sup>*, *ys p*, . *y s <sup>p</sup>*, *zs p*, . *z s p* describes the given state vector including sonar position and sonar velocity during the *p*-th scan. We assume that the system process noise *v<sup>k</sup> <sup>j</sup>* and measurement noises *<sup>w</sup><sup>k</sup> <sup>k</sup>* and *nk <sup>i</sup>* are independent of each other for all *j*, *p*, *i* and *k*. From position and Doppler measurement models Equations (3) and (4), the conditional probability density functions (PDFs) can be written as

$$\begin{array}{l} p\left(\mathbf{y}\_p^c \middle| \mathbf{x}\_p^k\right) \sim \mathcal{N}\left(\mathbf{y}\_p^c; \mathbf{H}\_p^c \mathbf{x}\_p^k, \mathbf{R}\_p^{c,k}\right) \\ p\left(\mathbf{y}\_p^d \middle| \mathbf{x}\_p^k\right) \sim \mathcal{N}\left(\mathbf{y}\_p^d; h\left(\mathbf{x}\_p^k\right), \mathbf{R}\_p^{d,k}\right) \\ p\left(\mathbf{y}\_p^c, \mathbf{y}\_p^d \middle| \mathbf{x}\_p^k\right) = p\left(\mathbf{y}\_p^c \middle| \mathbf{x}\_p^k\right) p\left(\mathbf{y}\_p^d \middle| \mathbf{x}\_p^k\right) \\ \sim \mathcal{N}\left(\begin{array}{cc} \mathbf{y}\_p^c \\ \mathbf{y}\_p^d \end{array}\right) / \left[\begin{array}{cc} \mathbf{H}\_p^c \mathbf{x}\_p^k \\ h\left(\mathbf{x}\_p^k\right) \end{array}\right] / \left[\begin{array}{cc} \mathbf{R}\_p^{c,k} & 0 \\ 0 & R\_p^{d,k} \end{array}\right] \end{array} \tag{5}$$
