# Trajectory Optimisation for Cooperative Target Tracking with Passive Mobile Sensors
## 2 Problem Definition  
Let us consider the problem of two-dimensional target tracking using measurements from two cooperative bearing-only sensors as shown in Figure 1.  
Figure 1: Illustration of the bearings-only tracking scenario, where the measurement covariance depends on the locations of the two sensors. The target state, which consists of both target location \((x_{k},y_{k})\) and velocity \((\dot{x}_{k},\dot{y}_{k})\), at time \(t_{k},k=1,2,\cdots\) is  
\[\mathbf{x}_{k}=[\;x_{k},\;y_{k},\;\dot{x}_{k},\;\dot{y}_{k}\;]^{\prime}.\]  
The system and measurement models are  
\[\mathbf{x}_{k}=F_{k}\mathbf{x}_{k-1}+\mathbf{w}_{k}, \tag{1}\] \[\mathbf{\beta}_{k}=\left[ \begin{array}{c}\beta_{1k}\\ \beta_{2k}\end{array} \right]=\mathbf{h}(\mathbf{x}_{k})+\mathbf{v}_{k}, \tag{2}\]  
where the system transition matrix is  
\[F_{k}=\left[ \begin{array}{cc}1&t_{k}-t_{k-1}\\ 0&1\end{array} \right]\bigotimes\mathbf{I}_{2}, \tag{3}\]  
and \(\mathbf{w}_{k}\) and \(\mathbf{v}_{k}\) are system and measurement noises, respectively. These are assumed to be independent Gaussian distributions, that is,  
\[\mathbf{\omega}_{k}\sim\mathcal{N}(0,\;\mathbf{Q}_{k}),\qquad\text{and}\;\;\mathbf{v}_{k} \sim\mathcal{N}(0,\;\mathbf{\Sigma}_{k}),\]  
where \(\mathbf{Q}_{k}\) is assumed to be known and  
\[\mathbf{\Sigma}_{k}=\left[ \begin{array}{cc}\sigma_{1}^{2}&0\\ 0&\sigma_{2}^{2}\end{array} \right].\]  
\(\sigma_{1}\) and \(\sigma_{2}\) are the standard deviations of measurement noises for sensors 1 and sensor 2, respectively. The measurement function in (2) is expressed as  
\[\mathbf{h}(\mathbf{x}_{k},\mathbf{x}_{k}^{S_{1}},\mathbf{x}_{k}^{S_{2}})=\left[ \begin{array}{c}\tan^{-1}\left(\frac{x_{k}-x_{k}^{S_{1}}}{y_{k}-y_{k}^{S_{1} }}\right)\\ \tan^{-1}\left(\frac{x_{k}-x_{k}^{S_{2}}}{y_{k}-y_{k}^{S_{2}}}\right)\end{array}  \right], \tag{4}\]  
where \((x_{k}^{S_{1}},y_{k}^{S_{1}})\) and \((x_{k}^{S_{2}},y_{k}^{S_{2}})\) are the locations of sensor 1 and sensor 2 at \(t_{k}\), respectively. The objective of target tracking is to estimate the target posterior probability density function (PDF) \(p(\mathbf{x}_{k}|\mathbf{\beta}_{1:k})\) based on the bearings-only measurement sequence \(\mathbf{\beta}_{1:k}\) up to current time \(t_{k}\) and the prior PDF \(p(\mathbf{x}_{k-1}|\mathbf{\beta}_{1:k-1})\). Clearly, the measurement model (4) is sensor state dependent, and so tracking accuracy depends on where the two sensors take measurements (i.e., on their motion). Our goal is to find the optimal future motion for the two sensors within the limitations imposed by the sensor platform dynamics, so as to minimize the future expected track error. This is a difficult problem because of the many sources of uncertainty involved in decision-making: the uncertainty in the current target state estimate as well as the uncertainty in the future target motion and measurements. A practical way to implement the sensor trajectory optimisation procedure is to assume that sensor platforms move at constant speeds, \(v_{1}\) and \(v_{2}\), respectively, and that they have a small set \(\mathcal{U}\) of admissible course corrections at time \(t_{k}\). Each element \(\mathbf{u}_{k}\) of \(\mathcal{U}\) is a pair \(\mathbf{u}_{k}=[u_{1,k},u_{2,k}]^{\prime}\), where \(u_{i,k}\) represents the course correction of the \(i\)th sensor at \(k\). This results in the velocities of the two sensors in the local coordinates in the time interval \((t_{k},t_{k+1}]\) as  
\[\dot{x}_{k}^{s_{i}}=v_{i}\sin u_{i,k},\;\;\dot{y}_{k}^{s_{i}}=v_{i}\cos u_{i,k },\;\;i=1,2. \tag{5}\]  
The assumption of constant speeds between sampling intervals is a standard practice for system analysis of discrete data sampling without loss of optimality. The analysis tends to be optimal at a high sampling rate.