# Tracking of Moving Targets Through Asynchronous Measures
## 2 Asynchronous Tracking Algorithm  
In this paragraph, we derive the novel 3D tracking algorithm. Firstly we define the state vector \(\mathbf{q}\) as  
\[\mathbf{q}= \begin{pmatrix}\mathbf{r}\\ \mathbf{i}\\ \bar{\mathbf{r}}\end{pmatrix} , \tag{1}\]  
where \(\mathbf{r}\) denotes the 3D position of the target. Then, we denote \(\mathbf{\hat{q}}\) and \(\mathbf{C}\) as the expectation and covariance matrix of \(\mathbf{q}\), respectively, i.e.,  
\[\mathbf{\hat{q}}=\mathbb{E}\{\mathbf{q}\} \tag{2}\]  
\[\mathbf{C}=\mathbb{E}\Big{\{}\big{(}\mathbf{q}-\mathbf{\hat{q}}\big{)}\cdot \big{(}\mathbf{q}-\mathbf{\hat{q}}\big{)}^{T}\Big{\}}. \tag{3}\]  
The objective of the estimator is to update \(\mathbf{\hat{q}}\) and \(\mathbf{C}\) through a single range measurement between the target and an anchor whose position \(\mathbf{r}_{s}\) is perfectly known. Multiple range measurements can simply be included by sequentially applying the algorithm for each new measurement. Defining \(\mathbf{\hat{q}}_{0}\) and \(\mathbf{C}_{0}\) as the values of \(\mathbf{\hat{q}}\) and \(\mathbf{C}\) immediately before the update through a range measurement, the range \(\rho\) between the anchor and the target is given by  
\[\rho=|\mathbf{r}-\mathbf{r}_{s}|, \tag{4}\]  
which can be approximated through [PERSON]'s expansion as  
\[ \begin{split}\rho\sim&|\mathbf{\hat{r}}-\mathbf{r}_ {s}|+\frac{(\mathbf{\hat{r}}-\mathbf{r}_{s})\cdot(\mathbf{r}-\mathbf{\hat{r} })}{|\mathbf{\hat{r}}-\mathbf{r}_{s}|}\\ &=|\mathbf{\hat{r}}-\mathbf{r}_{s}|+(\mathbf{q}-\mathbf{\hat{q}} _{0})^{T}\cdot\mathbf{L},\end{split}  \tag{5}\]  
where the \(9\times 1\) matrix \(\mathbf{L}\) is defined as the Jacobian of \(\rho\) with respect to \(\mathbf{q}\) i.e.,  
\[\mathbf{L}= \begin{pmatrix}\frac{\mathbf{\hat{r}}-\mathbf{r}_{s}}{|\mathbf{ \hat{r}}-\mathbf{r}_{s}|}\\ \mathbf{0}\\ \mathbf{0}\end{pmatrix} . \tag{6}\]  
Now, let us define \(\hat{\rho}\) as the outcome of the range measurement between the anchor and the target, and \(\sigma_{\rho}\) as the corresponding Root Mean Square Error (RMSE). Since the range measurement is not correlated with the a priori estimate of \(\mathbf{q}\), the best linear estimator \(\mathbf{\hat{q}}_{1}\), after the update through the range measurement, can be found by minimizing the quadratic form \(\mathrm{Y}(\mathbf{q})\)[14], defined as  
\[ \begin{split}\mathrm{Y}(\mathbf{q})&=\big{(}\mathbf{q}- \mathbf{\hat{q}}_{0}\big{)}^{T}\cdot\mathbf{C}_{0}^{-1}\cdot\big{(}\mathbf{q} -\mathbf{\hat{q}}_{0}\big{)}+\left(\frac{\rho-\hat{\rho}}{\sigma_{\rho}} \right)^{2}\\ &\sim\big{(}\mathbf{q}-\mathbf{\hat{q}}_{0}\big{)}^{T}\cdot \mathbf{C}_{0}^{-1}\cdot\big{(}\mathbf{q}-\mathbf{\hat{q}}_{0}\big{)}\\ &+\left(\frac{|\mathbf{\hat{r}}-\mathbf{r}_{s}|+(\mathbf{q}- \mathbf{\hat{q}}_{0})^{T}\cdot\mathbf{L}-\hat{\rho}}{\sigma_{\rho}}\right)^{ 2},\end{split}  \tag{7}\]  
where we have replaced (5). Now, rearranging the terms, we obtain\[ \begin{split}\text{Y}(\mathbf{q})&=\left(\mathbf{q}-\mathbf{ \hat{q}}_{0}\right)^{\text{T}}\cdot\left(\mathbf{C}_{0}^{-1}+\frac{\mathbf{L }\cdot\mathbf{L}^{T}}{\sigma_{\rho}^{2}}\right)\cdot\left(\mathbf{q}-\mathbf{ \hat{q}}_{0}\right)\\ &-2\cdot(\mathbf{q}-\mathbf{\hat{q}}_{0})^{\text{T}}\cdot\left( \frac{\hat{\rho}-|\mathbf{\hat{r}}-\mathbf{r}_{\text{s}}|}{\sigma_{\rho}^{2}} \right)\cdot\mathbf{L}\\ &+\left(\frac{\hat{\rho}-|\mathbf{\hat{r}}-\mathbf{r}_{\text{s}}| }{\sigma_{\rho}}\right)^{2}\end{split}