Moving target positioning algorithm based on multidimensional scaling analysis from TDOA and FDOA with sensor uncertainties

Abstract

The problem of moving target localization from range and velocity difference measurements has attracted considerable attention in recent years. In this article, a novel weighted multidimensional scaling (MDS) algorithm is proposed to estimate the position and velocity of a moving target by utilizing the time difference of arrival (TDOA) and frequency difference of arrival (FDOA) measurements with sensor position and velocity errors. The proposed estimator is based on the optimization of a cost function related to the scalar product matrix in classical MDS. The estimator is accurate and closed form. The algorithm has a small mean square error compared with the 2-step weighted least squares (LS) algorithm in a moderate and high noise power level.

Keywords:

• Positioning
• multidimensional scaling
• time/frequency difference of arrivals
• sensor uncertainties
• CRLB

Reviewing Editor:

• Ai Qingsong, Wuhan University of Technology, CHINA

Subjects:

• Technology
• Engineering & Technology
• Electrical & Electronic Engineering
• Communications & Information Processing
• Communication Networks & Systems
• Electromagnetics & Communication
• Telecommunication

1. Introduction

Finding the source position has attracted a lot of debates in recent years in various research fields such as communication, navigation, radar, surveillance and wireless sensor networks. Recently, many scientific contributions have been focused on this issue since the US Federal Commission has adopted a decision to develop the Emergency 911 (E-911) services (Commission F.C., 1996). The position of source can be estimated by exploiting the time of arrival (TOA), the angle of arrival (AOA), time difference of arrival (TDOA), frequency difference of arrival (FDOA) and received signal strength (RSS) or a combination of them (Saeed et al., 2019; Yang et al., 2021; Ahmed et al., 2013; Reza & Buehrer, 2019).

Nowadays, multidimensional scaling (MDS) analysis (Saeed et al., 2019; Yang et al., 2021; Borg & Groenen, 2005; Cox & Cox, 2000; Ahmed et al., 2016; Li et al., 2021) becomes a powerful tool in exploratory data analysis. The MDS is a method that represents similarity/dissimilarity among pairs of objects as distances between points of a low-dimensional multidimensional space. The similarity/dissimilarity may be measured in a variety of ways, e.g. the difference between color images or the Euclidean distances.

The classical MDS-based location estimation problem is started by constructing a similarity or dissimilarity distance matrix among all pairs of sensors, then the scalar product matrix is constructed. Finally, a set of linear equations related to the signal subspace or noise subspace of the scalar product matrix is solved to estimate the unknown position in the LS sense. The MDS technique is used to find the position of emitting sources because its robustness to the large measurement noise due to the Eigen-structure and dimension knowledge of the scalar product matrix.

For localization with sensor uncertainties via TDOA and FDOA measurement, Yang and Ho introduced a closed-form solution for target estimation using range difference measurements (Yang & Ho, 2009) and extended by considering FDOA measurements (Sun, Yang, et al., 2012), while Ho et al. introduced solution to the range difference and the gain ratio of arrivals (Hao et al., 2012a, 2012b). They addressed two-step weighted least squares (2WLS) approaches in which the statistical distribution of the sensor locations is taken into account to enhance the localization precision, but it attains the CRLB only in sufficiently low noise power. Wei et al. proposed the MDS for range difference (Wei & Lu, 2020; Wei et al., 2008) as well as the rate of range difference localization (He-Wen et al., 2010; Hesham et al., 2017). They proposed weighted MDS localization algorithms that could achieve the CRLB at a moderate noise level. Its threshold effect occurs later than the 2WLS when the sensor positions are known exactly. Liming et.al introduced an iterative localization algorithm from TDOA measurement based on MDS analysis (Li et al., 2021), it is computationally efficient but it does not consider the uncertainties of sensor positions.

The proposed algorithm here is inspired by He-Wen et al. (2010) where it takes the uncertainties of sensors into consideration. In this article, we present an MDS localization algorithm using TDOA and FDOA measurements with the inaccurate position and velocity of sensors; it is closed-form and an accurate estimator. The performance reaches the CRLB at the low and moderate level of sensor noise power before the threshold effect occurs, and it is better than the 2WLS method is the moderate and high level of sensor noise power. Moreover, the estimator is suitable to estimate the position and velocity of non-cooperative and passive targets.

This article is organized as the following; the second section includes the system model and CRLB analysis, then, the proposed solution of localization of sensor position and velocity uncertainties introduced in the third section, the simulation configuration and result are presented in the fourth section while the article is concluded in the last section.

For mathematics notations, the vectors will be presented in bold lowercase letters, while the matrices will be presented in bold uppercase letters. The scalars will be introduced in lowercase italic. The notation ${[\cdot ]}^T$ stands for transpose operator and ${[\cdot ]}^{-1}$ represents the inverse operator.

2. The proposed model and CRLB

As showing in Figure 1, supposed that there are a number of sensors located in 3-dimensional coordinates system. Assume that the location of the target at an unknown position $\mathbf{u}={\left[x,y,z\right]}^T,$ and velocity $\dot{\mathbf{u}}={\left[\dot{x},\dot{y},\dot{z}\right]}^T.$ The sensors are located at ${\mathbf{s}}_m={\left[x_m,y_m,z_m\right]}^T$ with velocity ${\dot{\mathbf{s}}}_m={\left[{\dot{x}}_m,{\dot{y}}_m,{\dot{z}}_m\right]}^T,$ $m=1,2,\dots .,M$ where $M$ is the total number of sensors in the system. Assume that there is a relative motion between the target and sensors. The sensors have received the signal from a target at the different time slot, in this paper, we focus on the over-determined scenario where numbers of Sensors are more than 4. To get a unique location and velocity solution for the target, we consider that the receiving sensors are neither deploying on straight line nor a plane.

Figure 1. The TDOA and FDOA localization setup.

Unlike the conventional localization algorithms where the sensor positions and velocities are known precisely, the noisy version of sensor locations and velocities are existing, and they can be expressed as (1) $$\begin{array}{c}\mathbf{S}={\mathbf{S}}^o+\Delta \mathbf{S},\\ \dot{\mathbf{S}}={\dot{\mathbf{S}}}^o+\Delta \dot{\mathbf{S}},\end{array}$$(1)

${\mathbf{S}}^o={\left[{\mathbf{s}}_1^o,{\mathbf{s}}_2^o,\dots \dots ,{\mathbf{s}}_M^o\right]}^T,$ and $\Delta \mathbf{S}={\left[\Delta {\mathbf{s}}_1^T,\Delta {\mathbf{s}}_2^T,\dots .,\Delta {\mathbf{s}}_M^T\right]}^T,$ with the associated velocities ${\dot{\mathbf{S}}}^0={\left[{\dot{\mathbf{s}}}_1^o,{\dot{\mathbf{s}}}_2^o,\dots \dots ,{\dot{\mathbf{s}}}_M^o\right]}^T,$ and $\Delta \dot{\mathbf{S}}={\left[\Delta {\dot{\mathbf{s}}}_1^T,\Delta {\dot{\mathbf{s}}}_2^T,\dots .,\Delta {\dot{\mathbf{s}}}_M^T\right]}^T,$ respectively. $\Delta {\mathbf{s}}_m={\left[\Delta x_m,\Delta y_m,\Delta z_m\right]}^T$ is the location error vector while $\Delta {\dot{\mathbf{s}}}_m={\left[\Delta {\dot{x}}_m,\Delta {\dot{y}}_m,\Delta {\dot{z}}_m\right]}^T$ is the velocity error vector, the location and velocity errors are zero mean Gaussian distributed vectors with the co-variance matrices ${\mathbf{Q}}_s=E\left\{\Delta \mathbf{S}\Delta {\mathbf{S}}^T\right\}.$ and ${\mathbf{Q}}_{\dot{s}}=E\left\{\Delta \dot{\mathbf{S}}\Delta {\dot{\mathbf{S}}}^T\right\}.$

The first sensor is selected as the reference one. Therefore, the TDOA and FDOA measurements are obtained by subtracting the distances and its rate between all Sensors and the reference one, this operation produces $(M^2-M)/2$ TDOAs and FDOAs from all possible Sensors. However, only the non-redundant distances will be considered which is equal to $M-1.$ The range difference and its rate are proportional TDOA and FDOA measurements in a speed of propagation medium (speed of light ($3\times {10}^8$ m/sec)/acoustic (340 m/sec) according to signal propagation nature). The conventional range difference $d_{i1}$ and its rate ${\dot{d}}_{i1}$ are given as (2) $$\begin{array}{c}{d}_{i1}=c.{\tau }_{i1}=d_i-d_1=\sqrt{{\left({\mathbf{s}}_i^o-\mathbf{u}\right)}^T\left({\mathbf{s}}_i^o-\mathbf{u}\right)},\\ {\dot{d}}_{i1}=c.f_{i1}/f_o={\dot{d}}_i-{\dot{d}}_1=\frac{{\left({\dot{\mathbf{s}}}_i^o-\dot{\mathbf{u}}\right)}^T\left({\mathbf{s}}_i^o-\mathbf{u}\right)}{d_{i1}},\end{array}$$(2) where $f_0$ represents the signal carrier frequency. From the above equation, $d_{01}=-d_1,$ ${\dot{d}}_{01}=-{\dot{d}}_1,$ $d_{11}=0$ and ${\dot{d}}_{11}=0.$ In practical situations, the effect of noise should be considered, thus, by define $r_{i1}$ and its rate ${\dot{r}}_{i1}$ as the noisy measurements are (3) $$\begin{array}{c}{r}_{i1}=d_{i1}+n_{i1},\\ {\dot{r}}_{i1}={\dot{d}}_{i1}+q_{i1}.\end{array}$$(3)

Then the matrix form of the previous equation is (4) $$\begin{array}{c}\mathbf{r}=\mathbf{d}+\mathbf{n},\\ \dot{\mathbf{r}}=\dot{\mathbf{d}}+\mathbf{q},\end{array}$$(4) where $\mathbf{r}={\left[r_{21},r_{31},\dots ,r_{M1}\right]}^T,$ $\dot{\mathbf{r}}={\left[{\dot{r}}_{21},{\dot{r}}_{31},\dots ,{\dot{r}}_{M1}\right]}^T,$ $\mathbf{d}={\left[d_{21},d_{31},\dots ,d_{M1}\right]}^T,$ $\mathbf{d}={\left[{\dot{d}}_{21},{\dot{d}}_{31},\dots ,{\dot{d}}_{M1}\right]}^T,$ $\mathbf{n}={\left[n_{21},n_{31},\dots ,n_{M1}\right]}^T,$ and $\mathbf{q}=\left[q_{21},q_{31},\dots ,q_{M1}\right]$ are modeled as the Gaussian noise with zero mean and co-variance ${\mathbf{Q}}_n=E\left\{\mathbf{n}{\mathbf{n}}^T\right\}$ and ${\mathbf{Q}}_q=E\left\{\mathbf{q}{\mathbf{q}}^T\right\}.$ The TDOA noise $\mathbf{n}$ and the FDOA noise $\mathbf{q}$ are assumed to be uncorrelated.

Let $\mathbf{\alpha }={\left[\begin{array}{cc}{\mathbf{r}}^T& {\dot{\mathbf{r}}}^T\end{array}\right]}^T,$ $\mathbf{\beta }={\left[\begin{array}{cc}{\mathbf{s}}^T& {\dot{\mathbf{s}}}^T\end{array}\right]}^T$ and $\mathbf{\theta }={\left[\begin{array}{cc}{\mathbf{u}}^T& {\dot{\mathbf{u}}}^T\end{array}\right]}^T,$ where $\mathbf{\theta }$ represents the unknown parameter vector. The CRLB is the minimum variance that could be attained by any unbiased estimator; it defined as the inverse of Fisher information matrix (Le & Ho, 2009), the logarithmic PDF for data vector is given as (5) $$\begin{array}{c}\hspace{0.17em}\mathrm{\text{ln}}\hspace{0.17em}f\left(\mathbf{m},\vartheta \right)=\hspace{0.17em}\mathrm{\text{ln}}\hspace{0.17em}f\left(\mathbf{\alpha },\vartheta \right)+\hspace{0.17em}\mathrm{\text{ln}}\hspace{0.17em}f\left(\mathbf{\beta },\vartheta \right)\\ =K-\frac{1}{2}\left({\left(\mathbf{\alpha }-{\mathbf{\alpha }}^o\right)}^T{\mathbf{Q}}_{\alpha }^{-1}\left(\mathbf{\alpha }-{\mathbf{\alpha }}^o\right)+{\left(\mathbf{\beta }-{\mathbf{\beta }}^o\right)}^T{\mathbf{Q}}_{\beta }^{-1}\left(\mathbf{\beta }-{\mathbf{\beta }}^o\right)\right)\end{array}$$(5) where K is a constant and does not depend on the unknowns, ${\mathbf{Q}}_{\beta }=\left\{{\mathbf{Q}}_s,{\mathbf{Q}}_{\dot{s}}\right\}$ and ${\mathbf{Q}}_{\alpha }=\left\{{\mathbf{Q}}_{\mathbf{n}},{\mathbf{Q}}_q\right\}.$ The CRLB for $\vartheta$ is expressed as (6) $$\mathit{\text{CRLB}}\left(\vartheta \right)=-E{\left[\frac{\hspace{0.17em}\mathrm{\text{ln}}\hspace{0.17em}f\left(\mathbf{m};\vartheta \right)}{\partial \vartheta \partial {\vartheta }^T}\right]}^{-1}={\left[\begin{array}{cc}\mathbf{X}& \mathbf{Y}\\ {\mathbf{Y}}^T& \mathbf{Z}\end{array}\right]}^{-1}$$(6) where $\mathbf{Z}={(\partial \mathbf{\alpha }/\partial \mathbf{\beta })}^T{\mathbf{Q}}_{\alpha }^{-1}(\partial \mathbf{\alpha }/\partial \mathbf{\beta })+{\mathbf{Q}}_{\beta }^{-1}$ is $6M\times 6M$ matrix, $\mathbf{Y}={(\partial \mathbf{\alpha }/\partial \mathbf{\theta })}^T{\mathbf{Q}}_{\alpha }^{-1}(\partial \mathbf{\alpha }/\partial \mathbf{\beta })$ is $6\times 6M$ matrix and $\mathbf{X}={(\partial \mathbf{\alpha }/\partial \mathbf{\theta })}^T{\mathbf{Q}}_{\alpha }^{-1}(\partial \mathbf{\alpha }/\partial \mathbf{\theta })$ is $6\times 6$ matrix, $\partial \mathbf{\alpha }/\partial \mathbf{\beta }$ contains the partial derivatives $(\partial \mathbf{r}/\partial \mathbf{s}),$ $(\partial \dot{\mathbf{r}}/\partial \mathbf{s}),$ $(\partial \mathbf{r}/\partial \dot{\mathbf{s}})$ and $(\partial \dot{\mathbf{r}}/\partial \dot{\mathbf{s}}).$ And $\partial \mathbf{\alpha }/\partial \mathbf{\theta }$ contains the partial derivatives $(\partial \mathbf{r}/\partial \mathbf{u}),$ $(\partial \mathbf{r}/\partial \dot{\mathbf{u}}),$ $(\partial \dot{\mathbf{r}}/\partial \mathbf{u})$ and $(\partial \dot{\mathbf{r}}/\partial \dot{\mathbf{u}}),$ and they are given in Appendix B. Applying the partitioned matrix inverse formula (Scharf, 1991), the CRLB and can be expressed as follows (7) $$\mathit{\text{CRLB}}(\mathbf{\theta })={\mathbf{X}}^{-1}-{\mathbf{X}}^{-1}\mathbf{Y}{\left(\mathbf{Z}-{\mathbf{Y}}^T{\mathbf{X}}^{-1}\mathbf{Y}\right)}^{-1}{\mathbf{Y}}^T{\mathbf{X}}^{-1}$$(7)

It is worth to remarkable that the first part of the right-hand side of Equation (7)(7) $$\mathit{\text{CRLB}}(\mathbf{\theta })={\mathbf{X}}^{-1}-{\mathbf{X}}^{-1}\mathbf{Y}{\left(\mathbf{Z}-{\mathbf{Y}}^T{\mathbf{X}}^{-1}\mathbf{Y}\right)}^{-1}{\mathbf{Y}}^T{\mathbf{X}}^{-1}$$(7) is the CRLB of $\mathbf{\alpha }$ when there is no sensor position and velocity error. The trace of Equation (7)(7) $$\mathit{\text{CRLB}}(\mathbf{\theta })={\mathbf{X}}^{-1}-{\mathbf{X}}^{-1}\mathbf{Y}{\left(\mathbf{Z}-{\mathbf{Y}}^T{\mathbf{X}}^{-1}\mathbf{Y}\right)}^{-1}{\mathbf{Y}}^T{\mathbf{X}}^{-1}$$(7) is the possible minimum square error that could be achieved by this estimator. We consider this CRLB as a benchmark to judge our proposed algorithm.

3. The proposed algorithm

The relationship between the coordinates of sensors and the target in MDS algorithm for TDOA, FDOA is given by (He-Wen et al., 2010) and (Scharf, 1991) (8) $$\mathbf{\text{BA}}\left[\begin{array}{c}0\\ \dot{\mathbf{z}}\end{array}\right]+\left(\dot{\mathbf{B}}\mathbf{A}+\mathbf{B}\dot{\mathbf{A}}\right)\left[\begin{array}{c}1\\ \mathbf{z}\end{array}\right]={\mathbf{0}}_M$$(8)

For TDOA only, the linear MDS based function given as (9) $$\mathbf{\text{BA}}\left[\begin{array}{c}1\\ \mathbf{z}\end{array}\right]={\mathbf{0}}_M$$(9) where $\mathbf{z}={\left[x,y,z,-d_{01}\right]}^T$ and $\dot{\mathbf{z}}={\left[\dot{x},\dot{y},\dot{z},-{\dot{d}}_{01}\right]}^T$ are unknown parameters vectors. The above two equations is a linear equations w.r.t the unknown parameters, the scalar product matrix $\mathbf{B}$ and $\dot{\mathbf{B}}$ is $M\times M$ with elements given by (10) $${\left[\mathbf{B}\right]}_{\mathit{\text{mn}}}=0.5\left[{\left(d_{m1}-d_{n1}\right)}^2-{\left(s_m^o-s_n^o\right)}^T\left(s_m^o-s_n^o\right)\right]$$(10) (11) $${\left[\dot{\mathbf{B}}\right]}_{\mathit{\text{mn}}}=0.5\left[\left(d_{m1}-d_{n1}\right)\left({\dot{d}}_{m1}-{\dot{d}}_{n1}\right)-{\left(s_m^o-s_n^o\right)}^T\left({\dot{s}}_m^o-{\dot{s}}_n^o\right)\right]$$(11) $\mathbf{A}$ and $\dot{\mathbf{A}}$ is $5\times M$ matrix they are written as (12) $$\mathbf{A}={\mathbf{P}}^T{\left(\mathbf{P}{\mathbf{P}}^T\right)}^{-1}$$(12) (13) $$\dot{\mathbf{A}}=-\mathbf{A}\dot{\mathbf{P}}\mathbf{A}$$(13) where (14) $$\mathbf{P}=\left[\begin{array}{cccc}1& 1& \dots & 1\\ x_1& x_2& \dots & x_M\\ y_1& y_2& \dots & y_M\\ z_1& z_2& \dots & z_M\\ d_{11}& d_{21}& \dots & d_{M1}\end{array}\right]$$(14) and (15) $$\dot{\mathbf{P}}=\left[\begin{array}{cccc}0& 0& \dots & 0\\ {\dot{x}}_1& {\dot{x}}_2& \dots & {\dot{x}}_M\\ {\dot{y}}_1& {\dot{y}}_2& \dots & {\dot{y}}_M\\ {\dot{z}}_1& {\dot{z}}_2& \dots & {\dot{z}}_M\\ {\dot{d}}_{11}& {\dot{d}}_{21}& \dots & {\dot{d}}_{M1}\end{array}\right]$$(15)

Define the unknown parameter vector y that collect vector $\mathbf{z}$ and $\dot{\mathbf{z}}.$ In the MDS algorithm for TDOA and FDOA measurements, the linear relationship w.r.t $\mathbf{y}$ is given as (16) $$\mathbf{\text{Hy}}=\mathbf{h}$$(16) where (17) $$\mathbf{H}=\left[\begin{array}{cc}\mathbf{B}{\mathbf{A}}_2& {\mathbf{0}}_{M\times 4}\\ \dot{\mathbf{B}}{\mathbf{A}}_2+\mathbf{B}{\dot{\mathbf{A}}}_2& \mathbf{B}{\mathbf{A}}_2\end{array}\right]$$(17) (18) $$\mathbf{h}=-{\left[\begin{array}{cc}\mathbf{B}{\mathbf{A}}_1& \dot{\mathbf{B}}{\mathbf{A}}_1+\mathbf{B}{\dot{\mathbf{A}}}_1\end{array}\right]}^T$$(18) ${\mathbf{A}}_1$ represents the first column $\mathbf{A},$ while the rest of columns construct the matrix ${\mathbf{A}}_2.$ By considering the noisy range difference measurements and its rate instead of the actual one, Equations (8)(8) $$\mathbf{\text{BA}}\left[\begin{array}{c}0\\ \dot{\mathbf{z}}\end{array}\right]+\left(\dot{\mathbf{B}}\mathbf{A}+\mathbf{B}\dot{\mathbf{A}}\right)\left[\begin{array}{c}1\\ \mathbf{z}\end{array}\right]={\mathbf{0}}_M$$(8) and (9)(9) $$\mathbf{\text{BA}}\left[\begin{array}{c}1\\ \mathbf{z}\end{array}\right]={\mathbf{0}}_M$$(9) is rewritten as (19) $${\mathbf{\epsilon }}_t=\stackrel{\mathbf{⁁}}{\mathbf{B}}\stackrel{\mathbf{⁁}}{\mathbf{A}}\left[\begin{array}{c}1\\ \mathbf{z}\end{array}\right]$$(19) (20) $${\mathbf{\epsilon }}_f=\stackrel{\mathbf{⁁}}{\mathbf{B}}\stackrel{\mathbf{⁁}}{\mathbf{A}}\left[\begin{array}{c}0\\ \dot{\mathbf{z}}\end{array}\right]+\left(\stackrel{\mathbf{⁁}}{\dot{\mathbf{B}}}\stackrel{\mathbf{⁁}}{\mathbf{A}}+\stackrel{\mathbf{⁁}}{\mathbf{B}}\stackrel{\mathbf{⁁}}{\dot{\mathbf{A}}}\right)\left[\begin{array}{c}1\\ \mathbf{z}\end{array}\right]$$(20) where $\stackrel{\mathbf{⁁}}{\mathbf{B}},$ $\stackrel{\mathbf{⁁}}{\dot{\mathbf{B}}},$ $\stackrel{\mathbf{⁁}}{\mathbf{A}}$ and $\stackrel{\mathbf{⁁}}{\dot{\mathbf{A}}}$ is the erroneous of $\mathbf{B},$ $\dot{\mathbf{B}},$ $\mathbf{A}$ and $\dot{\mathbf{A}},$ respectively. Unlike the location estimation with accurate Sensors positions, the error terms include the TDOA and FDOA measurements errors the Sensors positions uncertainties. The error terms in $\mathbf{B},$ $\dot{\mathbf{B}}$ have the form as (21) $${\left[\Delta \mathbf{B}\right]}_{\mathit{\text{mn}}}=\left(d_{m1}-d_{n1}\right)\left(n_{m1}-n_{n1}\right)-{\left(s_m^o-s_n^o\right)}^T\left(\Delta s_m-\Delta s_n\right)$$(21) (22) $$\begin{array}{c}{\left[\Delta \dot{\mathbf{B}}\right]}_{\mathit{\text{mn}}}=\left({\dot{d}}_{m1}-{\dot{d}}_{n1}\right)\left(n_{m1}-n_{n1}\right)+\left(d_{m1}-d_{n1}\right)\left(e_{m1}-e_{n1}\right)\\ -{\left(s_m-s_n\right)}^T\left(\Delta {\dot{s}}_m-\Delta {\dot{s}}_n\right)-{\left({\dot{s}}_m-{\dot{s}}_n\right)}^T\left(\Delta s_m-\Delta s_n\right)\end{array}$$(22)

The error terms of $\stackrel{\mathbf{⁁}}{\mathbf{A}},$ $\stackrel{\mathbf{⁁}}{\dot{\mathbf{A}}}$ are expressed as (23) $$\Delta \mathbf{A}=-{\mathbf{A}}^{-1}\Delta \mathbf{P}{\mathbf{A}}^{-1}+(\mathbf{I}-\mathbf{\text{AP}})\Delta {\mathbf{P}}^T{\left(\mathbf{P}{\mathbf{P}}^T\right)}^{-1},$$(23) and (24) $$\Delta \dot{\mathbf{A}}=-\mathbf{A}\dot{\mathbf{P}}\Delta \mathbf{A}-\mathbf{A}\Delta \dot{\mathbf{P}}\mathbf{A}-\Delta \mathbf{A}\dot{\mathbf{P}}\mathbf{A},$$(24)

The error terms in $\mathbf{P}$ and $\dot{\mathbf{P}}$ are expressed as (25) $$\Delta \mathbf{P}=\left[\begin{array}{cccc}0& 0& \dots & 0\\ \Delta x_1& \Delta x_2& \dots & \Delta x_M\\ \Delta y_1& \Delta y_2& \dots & \Delta y_M\\ \Delta z_1& \Delta z_2& \dots & \Delta z_M\\ n_{11}& n_{21}& \dots & n_{M1}\end{array}\right]$$(25) (26) $$\Delta \dot{\mathbf{P}}=\left[\begin{array}{cccc}0& 0& \dots & 0\\ \Delta {\dot{x}}_1& \Delta {\dot{x}}_2& \dots & \Delta {\dot{x}}_M\\ \Delta {\dot{y}}_1& \Delta {\dot{y}}_2& \dots & \Delta {\dot{y}}_M\\ \Delta {\dot{z}}_1& \Delta {\dot{z}}_2& \dots & \Delta {\dot{z}}_M\\ {\dot{n}}_{11}& {\dot{n}}_{21}& \dots & {\dot{n}}_{M1}\end{array}\right]$$(26)

By substituting $\stackrel{\mathbf{⁁}}{\dot{\mathbf{B}}}=\dot{\mathbf{B}}+\Delta \dot{\mathbf{B}},$ $\stackrel{\mathbf{⁁}}{\mathbf{B}}=\mathbf{B}+\Delta \mathbf{B}$ $\stackrel{\mathbf{⁁}}{\dot{\mathbf{A}}}=\dot{\mathbf{A}}+\Delta \dot{\mathbf{A}},$ $\stackrel{\mathbf{⁁}}{\mathbf{A}}=\mathbf{A}+\Delta \mathbf{A}$ in Equations (19)(19) $${\mathbf{\epsilon }}_t=\stackrel{\mathbf{⁁}}{\mathbf{B}}\stackrel{\mathbf{⁁}}{\mathbf{A}}\left[\begin{array}{c}1\\ \mathbf{z}\end{array}\right]$$(19) and (20)(20) $${\mathbf{\epsilon }}_f=\stackrel{\mathbf{⁁}}{\mathbf{B}}\stackrel{\mathbf{⁁}}{\mathbf{A}}\left[\begin{array}{c}0\\ \dot{\mathbf{z}}\end{array}\right]+\left(\stackrel{\mathbf{⁁}}{\dot{\mathbf{B}}}\stackrel{\mathbf{⁁}}{\mathbf{A}}+\stackrel{\mathbf{⁁}}{\mathbf{B}}\stackrel{\mathbf{⁁}}{\dot{\mathbf{A}}}\right)\left[\begin{array}{c}1\\ \mathbf{z}\end{array}\right]$$(20) , then applying Equations (23)(23) $$\Delta \mathbf{A}=-{\mathbf{A}}^{-1}\Delta \mathbf{P}{\mathbf{A}}^{-1}+(\mathbf{I}-\mathbf{\text{AP}})\Delta {\mathbf{P}}^T{\left(\mathbf{P}{\mathbf{P}}^T\right)}^{-1},$$(23) and (24)(24) $$\Delta \dot{\mathbf{A}}=-\mathbf{A}\dot{\mathbf{P}}\Delta \mathbf{A}-\mathbf{A}\Delta \dot{\mathbf{P}}\mathbf{A}-\Delta \mathbf{A}\dot{\mathbf{P}}\mathbf{A},$$(24) it, yields (27) $${\mathbf{\epsilon }}_t=\left(\Delta \mathbf{\text{BA}}+\mathbf{B}\Delta \mathbf{A}\right)\left[\begin{array}{c}1\\ \mathbf{z}\end{array}\right]$$(27) (28) $${\mathbf{\epsilon }}_f=\left(\Delta \dot{\mathbf{B}}\mathbf{A}+\dot{\mathbf{B}}\Delta \mathbf{A}+\Delta \mathbf{B}\dot{\mathbf{A}}+\mathbf{B}\Delta \dot{\mathbf{A}}\right)\left[\begin{array}{c}1\\ \mathbf{z}\end{array}\right]+\left(\Delta \mathbf{\text{BA}}+\mathbf{B}\Delta \mathbf{A}\right)\left[\begin{array}{c}0\\ \dot{\mathbf{z}}\end{array}\right]$$(28)

Define vector $\mathbf{\epsilon }$ which contains all the residual vectors. Therefore, we reorganize Equations (19)(19) $${\mathbf{\epsilon }}_t=\stackrel{\mathbf{⁁}}{\mathbf{B}}\stackrel{\mathbf{⁁}}{\mathbf{A}}\left[\begin{array}{c}1\\ \mathbf{z}\end{array}\right]$$(19) and (20)(20) $${\mathbf{\epsilon }}_f=\stackrel{\mathbf{⁁}}{\mathbf{B}}\stackrel{\mathbf{⁁}}{\mathbf{A}}\left[\begin{array}{c}0\\ \dot{\mathbf{z}}\end{array}\right]+\left(\stackrel{\mathbf{⁁}}{\dot{\mathbf{B}}}\stackrel{\mathbf{⁁}}{\mathbf{A}}+\stackrel{\mathbf{⁁}}{\mathbf{B}}\stackrel{\mathbf{⁁}}{\dot{\mathbf{A}}}\right)\left[\begin{array}{c}1\\ \mathbf{z}\end{array}\right]$$(20) , we have the vector equation w.r.t the unknown vector $\mathbf{z}$ as (29) $$\mathbf{\epsilon }=\left[\begin{array}{c}{\mathbf{\epsilon }}_t\\ {\mathbf{\epsilon }}_f\end{array}\right]=\stackrel{\mathbf{⁁}}{\mathbf{H}}\mathbf{y}-\stackrel{\mathbf{⁁}}{\mathbf{h}}$$(29)

To achieve CRLB performance, a full consideration of on Sensors position and velocity error should be taken in this proposed estimator. Equation (29)(29) $$\mathbf{\epsilon }=\left[\begin{array}{c}{\mathbf{\epsilon }}_t\\ {\mathbf{\epsilon }}_f\end{array}\right]=\stackrel{\mathbf{⁁}}{\mathbf{H}}\mathbf{y}-\stackrel{\mathbf{⁁}}{\mathbf{h}}$$(29) can be further expressed as (30) $$\mathbf{\epsilon }=\mathbf{G}\Delta \mathbf{\alpha }+{\mathbf{G}}_s\Delta \mathbf{\beta },$$(30)

The preceding expression in the residuals error vector $\Delta \mathbf{\alpha }={\left[\begin{array}{cc}{\mathbf{n}}^T& {\dot{\mathbf{n}}}^T\end{array}\right]}^T$ associated with TDOA and FDOA measurements and the sensor position and velocity error $\Delta \mathbf{\beta }={\left[\begin{array}{cc}\Delta {\mathbf{S}}^T& \Delta {\dot{\mathbf{S}}}^T\end{array}\right]}^T.$ $\mathbf{G}$ and ${\mathbf{G}}_{\mathbf{s}}$ are defined as (31) $$\mathbf{G}=\left[\begin{array}{cc}{\mathbf{T}}_1-\mathbf{\text{BA}}{\mathbf{T}}_2+\mathbf{F}& {\mathbf{0}}_{M\times \left(M-1\right)}\\ {\dot{\mathbf{T}}}_1-\dot{\mathbf{B}}\mathbf{A}{\mathbf{T}}_2-\mathbf{B}\dot{\mathbf{A}}{\mathbf{T}}_2-\mathbf{\text{BA}}{\dot{\mathbf{T}}}_2+\mathbf{L}& {\mathbf{T}}_1-\mathbf{\text{BA}}{\dot{\mathbf{T}}}_2\end{array}\right]$$(31) and (32) $${\mathbf{G}}_{\mathbf{S}}=\left[\begin{array}{cc}{\mathbf{H}}_1-\mathbf{\text{BA}}{\mathbf{H}}_2+{\mathbf{F}}_{\mathbf{s}}& {\mathbf{0}}_{M\times \left(M-1\right)}\\ {\dot{\mathbf{H}}}_1-\dot{\mathbf{B}}\mathbf{A}{\mathbf{H}}_2-\mathbf{B}\dot{\mathbf{A}}{\mathbf{H}}_2-\mathbf{\text{BA}}{\dot{\mathbf{H}}}_2+\mathbf{C}& {\mathbf{H}}_1-\mathbf{\text{BA}}{\dot{\mathbf{H}}}_2\end{array}\right]$$(32) where ${\mathbf{T}}_1,{\dot{\mathbf{T}}}_1,{\mathbf{T}}_2,{\dot{\mathbf{T}}}_2,{\mathbf{H}}_1,{\dot{\mathbf{H}}}_1,{\mathbf{H}}_2,{\dot{\mathbf{H}}}_2,\mathbf{F},{\mathbf{F}}_s,\mathbf{L}$ and $\mathbf{C}$ are defined in Appendix A.

Equation (30)(30) $$\mathbf{\epsilon }=\mathbf{G}\Delta \mathbf{\alpha }+{\mathbf{G}}_s\Delta \mathbf{\beta },$$(30) represents the residuals vector which has a linear relationship with TDOA and FDOA measurements error as well as the sensor errors. If we neglect the sensor errors, Equation (30)(30) $$\mathbf{\epsilon }=\mathbf{G}\Delta \mathbf{\alpha }+{\mathbf{G}}_s\Delta \mathbf{\beta },$$(30) become as $\mathbf{\epsilon }=\mathbf{G}\Delta \mathbf{\alpha }$ which is the same as the expression in Wei et al. (2008), the solution is easy and can be obtained by least square (LS) and WLS. The LS provides an optimum performance only when the noise components in the linear equation are independent and identically distributed (i.i.d). The WLS is a straightforward and optimal solution, in addition, it overcomes the LS solution when the noise components are not i.i.d. In this work, and we consider the WLS solution for our proposed algorithm when the static information of TDOA and FDOA measurements and sensor position errors are known.

Noting that the elements in the residual vector are correlated, the WLS solution is proposed to enhance the correlation between the elements in $\mathbf{\epsilon },$ the solution given by (33) $$\stackrel{\mathbf{⁁}}{\mathbf{y}}=-{\left({\stackrel{\mathbf{⁁}}{\mathbf{H}}}^T\mathbf{W}\stackrel{\mathbf{⁁}}{\mathbf{H}}\right)}^{-1}{\stackrel{\mathbf{⁁}}{\mathbf{H}}}^T\mathbf{W}\stackrel{\mathbf{⁁}}{\mathbf{h}}$$(33) where $\mathbf{W}$ is the symmetric weighting matrix, which is equal to the inverse of the covariance of the residual vector $\mathbf{\epsilon }$ and given by (34) $$\mathbf{W}={\left[E\left\{\mathbf{\epsilon }{\mathbf{\epsilon }}^T\right\}\right]}^{-1}={\left(\mathbf{G}{\mathbf{Q}}_{\alpha }{\mathbf{G}}^T+{\mathbf{G}}_{\mathbf{s}}{\mathbf{Q}}_{\beta }{\mathbf{G}}_{\mathbf{s}}^T\right)}^{-1}$$(34)

Applying the matrix inversion lemma Equation (34)(34) $$\mathbf{W}={\left[E\left\{\mathbf{\epsilon }{\mathbf{\epsilon }}^T\right\}\right]}^{-1}={\left(\mathbf{G}{\mathbf{Q}}_{\alpha }{\mathbf{G}}^T+{\mathbf{G}}_{\mathbf{s}}{\mathbf{Q}}_{\beta }{\mathbf{G}}_{\mathbf{s}}^T\right)}^{-1}$$(34) given by (35) $$\mathbf{W}={\left(\mathbf{G}{\mathbf{Q}}_{\alpha }{\mathbf{G}}^T\right)}^{-1}-{\left(\mathbf{G}{\mathbf{Q}}_{\alpha }{\mathbf{G}}^T\right)}^{-1}{\mathbf{G}}_{\mathbf{s}}^T\left({\mathbf{Q}}_{\beta }^T+{\mathbf{G}}_{\mathbf{s}}^T\left({\mathbf{G}}_{\mathbf{s}}{\mathbf{Q}}_{\beta }{\mathbf{G}}_{\mathbf{s}}^T\right){\mathbf{G}}_{\mathbf{s}}\right){\mathbf{G}}_{\mathbf{s}}^T{\left(\mathbf{G}{\mathbf{Q}}_{\alpha }{\mathbf{G}}^T\right)}^{-1}$$(35)

At the end the first three entries of $\stackrel{\mathbf{⁁}}{\mathbf{y}}$ in Equation (33)(33) $$\stackrel{\mathbf{⁁}}{\mathbf{y}}=-{\left({\stackrel{\mathbf{⁁}}{\mathbf{H}}}^T\mathbf{W}\stackrel{\mathbf{⁁}}{\mathbf{H}}\right)}^{-1}{\stackrel{\mathbf{⁁}}{\mathbf{H}}}^T\mathbf{W}\stackrel{\mathbf{⁁}}{\mathbf{h}}$$(33) represent the estimation of the target position. And the fifth to the seventh terms is the estimated target velocity.

The summery of proposed algorithm using the weighting matrix given as following:

1. compute $\mathbf{H}$ and $\mathbf{h}$ using the noisy range difference ${\mathbf{r}}_{m1},$$m=1,2,\dots M,$

2. find the initial estimate of $\stackrel{\mathbf{⁁}}{\mathbf{y}}$using (33), using $\mathbf{W}={\mathbf{I}}_{M+1},$

3. update the following steps one or two times

   1. use $\stackrel{\mathbf{⁁}}{\mathbf{y}}$to obtain $\mathbf{G}$

   2. Update $\mathbf{H}$ and$\mathbf{h}.$

   3. Obtain $\mathbf{W}$ using (34)

4. Find $\stackrel{\mathbf{⁁}}{\mathbf{y}}$

4. Simulation and results

In this part, the simulation results evaluate the performance of the proposed algorithm. The performance compared with two-stage WLS (Le & Ho, 2009; Sun, Le, et al., 2012), and the CRLB. The three-dimensional case will be considered in these experiments. The sensors positions are configured as in Table 1. The estimation accuracy regarding the target position and velocity estimation in term of mean squares error are defined as (36) $$\begin{array}{c}\mathit{\text{MSE}}(\mathbf{u})=\frac{1}{K}\sum _{k=1}^K\Vert {\stackrel{\mathbf{⁁}}{\mathbf{u}}}_k-{\mathbf{u}}_k\Vert ,\\ \mathit{\text{MSE}}(\dot{\mathbf{u}})\frac{1}{K}\sum _{k=1}^K\Vert {\stackrel{\mathbf{⁁}}{\dot{\mathbf{u}}}}_k-{\dot{\mathbf{u}}}_k\Vert \end{array}$$(36) where ${\stackrel{\mathbf{⁁}}{\mathbf{u}}}_k$ and ${\stackrel{\mathbf{⁁}}{\dot{\mathbf{u}}}}_k$ are the estimated position and velocity vectors of the $k$ trial, and $K$ is the total number of simulation trials which is equal 5000 in this experiments. The TDOA and FDOA measurements are generated by adding white Gaussian noise with zero mean and covariance equal to (37) $$\mathbf{Q}=\left[\begin{array}{cc}{\sigma }_{\mathbf{n}}^2\mathbf{\Theta }& \mathbf{0}\\ \mathbf{0}& {\sigma }_{\dot{\mathbf{n}}}^2\mathbf{\Theta }\end{array}\right],$$(37) where ${\sigma }_{\mathbf{n}}^2$ and ${\sigma }_{\dot{\mathbf{n}}}^2$ are the TDOA and FDOA measurements noise power respectively, and they are equal to ${10}^{-5},$ $\mathbf{\Theta }$ is diagonal matrix has a size $M-1\times M-1$ which it is diagonal elements equal to one and the rest of elements equal to 0.5. Let ${\sigma }_S^2$ and ${\sigma }_{\dot{S}}^2$ the sensor position and velocity noise power, respectively. Therefore, the noisy sensors position and velocity errors are created in much the same way using covariance matrix (38) $${\mathbf{Q}}_{\beta }=\left[\begin{array}{cc}{\sigma }_S^2\mathbf{\Phi }& \mathbf{0}\\ \mathbf{0}& {\sigma }_{\dot{S}}^2\mathbf{\Phi }\end{array}\right],$$(38) where $\Phi =\mathit{\text{diag}}[1,1,1,2,2,2,10,10,10,40,40,40,20,20,20,3,3,3]$ which is indicated the different amount of noise power on the sensors. In addition, the unknown passive target is assumed to be located at $(280,320,275)$ and $(3000,2500,2000)$ for near-field and far-field scenarios, respectively, with velocity $(-20,15,40)$ and for both cases and ${\sigma }_{\dot{S}}^2=0.1{\sigma }_S^2.$

Table 1. The sensors position and velocity.

Sensor No. i	1	2	3	4	5	6
$x_i$	300	400	300	350	−100	200
$y_i$	100	150	500	200	−100	−300
$z_i$	150	100	200	100	−100	−200
${\dot{x}}_i$	30	−30	10	10	−20	20
${\dot{y}}_i$	−20	10	−20	20	10	−10
${\dot{z}}_i$	20	20	10	30	10	10

Figures 2 and 3 show the CRLB as a function of sensor position and velocity error. The simulation has shown the comparison between CRLB without and with sensors position uncertainty for same and different amount of noise power, for same amount of noise power matrix is assumed to be diagonal matrix with all ones diagonal elements. From the figures, it can be seen that as the noise power increases, the CRLB with the presence of sensor position and velocity uncertainties diverges further from the variance accuracy without it. In these two figures, we show the CRLB for the case when the noise power is equal on each sensor. The figures show significant result for the CRLB with the sensor position uncertainty when the noise power is varying among all sensors.

Figure 2. The comparison of the CRLB for position estimation, the red and black colors are the near-field and far-field target scenarios, respectively.

Figure 3. The comparison CRLB for velocity estimation, the red and black colors are the near-field and far-field target scenarios, respectively.

Figures 4 and 5 give the estimation accuracy for the proposed estimator, they are concern about the position and velocity MSE as a function of sensor noise power. The MSE of the proposed estimator for near field target is compared with the two-step WLS, and CRLB. It can be observed that the performance of the proposed algorithm and the 2WLS overlap with the CRLB level when the noise power less than 5 dB. In addition, when the noise power greater than 5 dB, the proposed solution diverges from the CLRB but is less than the two-step WLS algorithm. We can say that the proposed estimator is performed well in small measurement of noise power.

Figure 4. The comparison of the position MSE of the proposed algorithm with the 2WLS and the CRLB against the sensor noise power for near field target.

Figure 5. The comparison of the velocity MSE of the proposed algorithm with the 2WLS and the CRLB against the sensor noise power for near field target.

Figures 6 and 7 concern about the position and velocity MSE as a function of sensor noise power. The MSE of the proposed estimator for far field target is compared with the two-step WLS, and CRLB. It can be observed that the performance of the proposed algorithm and the 2WLS overlap with the CRLB level when the noise power in less than 20 dB. In addition, when sensors noise power greater than 20 dB, the proposed solution diverges from the CLRB but is less than the two-step WLS algorithm. We can realize that the proposed estimator is performed well in small measurement of noise power.

Figure 6. The comparison of the position MSE of the proposed algorithm with the 2WLS and the CRLB against the sensor noise power for far field target.

Figure 7. The comparison of the velocity MSE of the proposed algorithm with the 2WLS and the CRLB against the sensor noise power for far field target.

5. Conclusions

In this article, the novel weighted MDS has been introduced with the presence of sensor position and velocity uncertainties. The estimator is accurate and unbiased. The proposed method achieves the CRLB at low noise power and small TDOA and FDOA measurement errors. In addition, the results show that the performance of the proposed method overcomes the two-step WLS in moderate and high levels of sensor noise power.

Disclosure statement

The authors have no relevant financial or non-financial interests to disclose.

Additional information

Funding

The authors declare that no funds, grants or other support were received during the preparation of this manuscript.

Notes on contributors

Hesham Ibrahim Ahmed

Hesham Ibrahim Ahmed, Department of Electrical and Electronic Engineering, Faculty of Engineering, University of Khartoum D.Eng. 2017, M. Eng. 2013 in Information and Communication Engineering, School of Electronic Engineering, University of Electronic Science and Technology of China, Chengdu City, Sichuan, China. BSc in Electrical and Computer Engineering 2009 faculty of Engineering, Karary University, Omdurman, Sudan. Current interests include Signal and Information processing, Radar, and communication systems.

Appendix A

From equations (31) and (32), the $T_1,{\dot{\mathbf{T}}}_1,T_2,{\dot{\mathbf{T}}}_2,H_1,{\dot{\mathbf{H}}}_1,H_2,{\dot{\mathbf{H}}}_2,F,F_s,L$ and $C$ are defined as (A.1) $$T_1=\left[\begin{array}{cccc}-{\mathit{\gamma }}_{12}& -{\mathit{\gamma }}_{13}& \mathrm{\dots }& -{\mathit{\gamma }}_{1M}\\ {\displaystyle {\sum }_{m=1}^M{\mathit{\gamma }}_{2m}}& -{\mathit{\gamma }}_{23}& \mathrm{\dots }& -{\mathit{\gamma }}_{2M}\\ -{\mathit{\gamma }}_{32}& {\displaystyle {\sum }_{m=1}^M{\mathit{\gamma }}_{3m}}& \mathrm{\dots }& -{\mathit{\gamma }}_{3M}\\ ⋮& ⋮& \ddots & ⋮\\ -{\mathit{\gamma }}_{M2}& -{\mathit{\gamma }}_{M3}& \mathrm{\dots }& {\displaystyle {\sum }_{m=1}^M{\mathit{\gamma }}_{\mathit{\text{Mm}}}}\end{array}\right]$$(A.1) (A.2) $${\dot{\mathbf{T}}}_1=\left[\begin{array}{cccc}-{\mathit{\lambda }}_{12}& -{\mathit{\lambda }}_{13}& \mathrm{\dots }& -{\mathit{\lambda }}_{1M}\\ {\displaystyle {\sum }_{m=1}^M{\mathit{\lambda }}_{2m}}& -{\mathit{\lambda }}_{23}& \mathrm{\dots }& -{\mathit{\lambda }}_{2M}\\ -{\mathit{\lambda }}_{32}& {\displaystyle {\sum }_{m=1}^M{\mathit{\lambda }}_{3m}}& \mathrm{\dots }& -{\mathit{\lambda }}_{3M}\\ ⋮& ⋮& \ddots & ⋮\\ -{\mathit{\lambda }}_{M2}& -{\mathit{\lambda }}_{M3}& \mathrm{\dots }& {\displaystyle {\sum }_{m=1}^M{\mathit{\lambda }}_{\mathit{\text{Mm}}}}\end{array}\right]\text{}-\left[\begin{array}{cccc}-{\mathit{\omega }}_{12}& -{\mathit{\omega }}_{13}& \mathrm{\dots }& -{\mathit{\omega }}_{1M}\\ {\displaystyle {\sum }_{m=1}^M{\mathit{\omega }}_{2m}}& -{\mathit{\omega }}_{23}& \mathrm{\dots }& -{\mathit{\omega }}_{2M}\\ -{\mathit{\omega }}_{32}& {\displaystyle {\sum }_{m=1}^M{\mathit{\omega }}_{3m}}& \mathrm{\dots }& -{\mathit{\omega }}_{3M}\\ ⋮& ⋮& \ddots & ⋮\\ -{\mathit{\omega }}_{M2}& -{\mathit{\omega }}_{M3}& \mathrm{\dots }& {\displaystyle {\sum }_{m=1}^M{\mathit{\omega }}_{\mathit{\text{Mm}}}}\end{array}\right]$$(A.2) where ${\mathit{\gamma }}_{\mathit{\text{mn}}}=a_n(d_{m1}-d_{n1})$, ${\mathit{\lambda }}_{\mathit{\text{mn}}}={\dot{a}}_n\left(d_{m1}-d_{n1}\right)$ and ${\mathit{\omega }}_{\mathit{\text{mn}}}=a_n\left({\dot{d}}_{m1}-{\dot{d}}_{n1}\right)$ $m=1,2,\mathrm{\dots },M$, and $\hspace{0.17em}n=2,3,\mathrm{\dots },M$ from equation, the terms $a_2,a_3,\mathrm{\dots .},a_M$ and ${\dot{a}}_2,{\dot{a}}_3,\mathrm{\dots .},{\dot{a}}_M$ are defined as (A.3) $$a=A\left[\begin{array}{c}1\\ z\end{array}\right]={\left[a_1,a_2,\mathrm{\dots },a_M\right]}^T$$(A.3) (A.4) $$\dot{\mathbf{a}}=\dot{\mathbf{A}}\left[\begin{array}{c}1\\ z\end{array}\right]+A\left[\begin{array}{c}0\\ \dot{\mathbf{z}}\end{array}\right]={\left[{\dot{a}}_1,{\dot{a}}_2,\mathrm{\dots },{\dot{a}}_M\right]}^T$$(A.4) (A.5) $$T_2=\left[\begin{array}{cccc}{0}_4& 0_4& \mathrm{\dots }& 0_4\\ a_2& a_3& \mathrm{\dots }& a_M\end{array}\right]$$(A.5) (A.6) $${\dot{\mathbf{T}}}_2=\left[\begin{array}{cccc}{0}_4& 0_4& \mathrm{\dots }& 0_4\\ {\dot{a}}_2& {\dot{a}}_3& \mathrm{\dots }& {\dot{a}}_M\end{array}\right]$$(A.6) (A.7) $$F={\left[B\left(I-\mathit{\text{AP}}\right)\right]}_{2:\mathit{\text{end}}}\cdot e_5$$(A.7)

$e_5$ is the last row in the following equation (A.8) $$e={\left(P{P}^T\right)}^{-1}{\left[\begin{array}{cc}1& \widehat{\mathbf{z}}\end{array}\right]}^T=\left[\begin{array}{c}{e}_1\\ e_2\\ e_3\\ e_4\\ e_5\end{array}\right]$$(A.8) (A.9) $$H_1=\left[\begin{array}{cccc}{\displaystyle {\sum }_{m=1}^M{\kappa }_{1m}}& {\kappa }_{12}& \mathrm{\dots }& {\kappa }_{1M}\\ {\kappa }_{21}& {\displaystyle {\sum }_{m=1}^M{\kappa }_{2m}}& \mathrm{\dots }& {\kappa }_{2M}\\ ⋮& ⋮& \ddots & ⋮\\ {\kappa }_{M1}& {\kappa }_{M2}& \mathrm{\dots }& {\displaystyle {\sum }_{m=1}^M{\kappa }_{\mathit{\text{Mm}}}}\end{array}\right]$$(A.9) (A.10) $${\dot{\mathbf{H}}}_1=\left[\begin{array}{cccc}{\displaystyle {\sum }_{m=1}^M{\mathit{\xi }}_{1m}}& {\mathit{\xi }}_{12}& \mathrm{\dots }& {\mathit{\xi }}_{1M}\\ {\mathit{\xi }}_{21}& {\displaystyle {\sum }_{m=1}^M{\mathit{\xi }}_{2m}}& \mathrm{\dots }& {\mathit{\xi }}_{2M}\\ ⋮& ⋮& \ddots & ⋮\\ {\mathit{\xi }}_{M1}& {\mathit{\xi }}_{M2}& \mathrm{\dots }& {\displaystyle {\sum }_{m=1}^M{\mathit{\xi }}_{\mathit{\text{Mm}}}}\end{array}\right]-\left[\begin{array}{cccc}{\displaystyle {\sum }_{m=1}^M{\mathit{\zeta }}_{1m}}& {\mathit{\zeta }}_{12}& \mathrm{\dots }& {\mathit{\zeta }}_{1M}\\ {\mathit{\zeta }}_{21}& {\displaystyle {\sum }_{m=1}^M{\mathit{\zeta }}_{2m}}& \mathrm{\dots }& {\mathit{\zeta }}_{2M}\\ ⋮& ⋮& \ddots & ⋮\\ {\mathit{\zeta }}_{M1}& {\mathit{\zeta }}_{M2}& \mathrm{\dots }& {\displaystyle {\sum }_{m=1}^M{\mathit{\zeta }}_{\mathit{\text{Mm}}}}\end{array}\right]$$(A.10) where ${\kappa }_{\mathit{\text{mn}}}=a_n{(s_m-s_n)}^T$, $\mathit{\xi }={\dot{a}}_n{(s_m-s_n)}^T$ and ${\mathit{\zeta }}_{\mathit{\text{mn}}}=a_n\left({\dot{\mathbf{s}}}_m-{\dot{\mathbf{s}}}_n\right)$ where $m=1,2,\mathrm{\dots },M$. and $n=1,2,\mathrm{\dots },M$ (A.11) $${\mathbf{H}}_2=\left[\begin{array}{c}{\mathbf{0}}_{3M\times 1}^T\\ a_1{\mathbf{I}}_3{a}_1{\mathbf{I}}_3\dots a_1{\mathbf{I}}_3\\ {\mathbf{0}}_{3M\times 1}^T\end{array}\right]$$(A.11) (A.12) $${\dot{\mathbf{H}}}_2=\left[\begin{array}{c}{0}_{3M\times 1}^T\\ {\dot{a}}_1{I}_3{\dot{a}}_1{I}_3\hspace{0.17em}\mathrm{\dots }\hspace{0.17em}{\dot{a}}_1{I}_3\\ 0_{3M\times 1}^T\end{array}\right]$$(A.12) (A.13) $$F_s=B\left(I-\mathit{\text{AP}}\right){\left[\begin{array}{cccc}{e}_{2:4}^T& 0_{3\times 1}& \mathrm{\dots }& 0_{3\times 1}\\ 0_{3\times 1}& e_{2:4}^T& \mathrm{\dots }& 0_{3\times 1}\\ ⋮& ⋮& \ddots & ⋮\\ 0_{3\times 1}& 0_{3\times 1}& \mathrm{\dots }& e_{2:4}^T\end{array}\right]}_{M\times 3M}$$(A.13)

To express the matrix $\mathbf{L}$ and $\mathbf{C}$ first we define the following terms (A.14) $$\mathbf{w}={\left(\mathbf{P}{\mathbf{P}}^T\right)}^{-1}\left[\begin{array}{c}0\\ \stackrel{⁁}{\mathbf{z}}\end{array}\right]={\left[w_1,w_2,\dots ,w_M\right]}^T$$(A.14) (A.15) $$g={\left(P{P}^T\right)}^{-\mathrm{1 }}P\left(\dot{\mathbf{A}}\left[\begin{array}{c}1\\ \stackrel{⁁}{\mathbf{z}}\end{array}\right]+A\left[\begin{array}{c}0\\ \stackrel{\mathbf{⁁}}{\dot{\mathbf{z}}}\end{array}\right]\right)=\left[\begin{array}{c}{g}_1\\ g_2\\ ⋮\\ g_M\end{array}\right]$$(A.15) (A.16) $$\begin{array}{c}\mathbf{L}=\left(\dot{\mathbf{B}}-\mathbf{\text{BA}}\dot{\mathbf{P}}\right)\left(\mathbf{I}-\mathbf{\text{AP}}\right)\left({\mathbf{e}}_5{\left[\begin{array}{cc}{\mathbf{0}}_{M-1\times 1}& {\mathbf{I}}_{M-1}\end{array}\right]}^T\right)\\ +\mathbf{B}\left(\mathbf{I}-\mathbf{\text{AP}}\right)\left(\left(w_5+g_5\right){\left[\begin{array}{cc}{\mathbf{0}}_{M-1\times 1}& {\mathbf{I}}_{M-1}\end{array}\right]}^T\right)\end{array}$$(A.16) (A.17) $$\begin{array}{c}\mathbf{C}=\mathbf{L}=\left(\dot{\mathbf{B}}-\mathbf{\text{BA}}\dot{\mathbf{P}}\right)\left(\mathbf{I}-\mathbf{\text{AP}}\right){\left[\begin{array}{cccc}{\mathbf{e}}_{2:4}^T& {\mathbf{0}}_{3\times 1}& \dots & {\mathbf{0}}_{3\times 1}\\ {\mathbf{0}}_{3\times 1}& {\mathbf{e}}_{2:4}^T& \dots & {\mathbf{0}}_{3\times 1}\\ ⋮& ⋮& \ddots & ⋮\\ {\mathbf{0}}_{3\times 1}& {\mathbf{0}}_{3\times 1}& \dots & {\mathbf{e}}_{2:4}^T\end{array}\right]}_{M\times 3M}\\ +\mathbf{B}\left(\mathbf{I}-\mathbf{\text{AP}}\right){\left[\begin{array}{cccc}{\mathbf{w}}_{2:4}^T+{\mathbf{g}}_{2:4}^T& {\mathbf{0}}_{3\times 1}& \dots & {\mathbf{0}}_{3\times 1}\\ {\mathbf{0}}_{3\times 1}& {\mathbf{w}}_{2:4}^T+{\mathbf{g}}_{2:4}^T& \dots & {\mathbf{0}}_{3\times 1}\\ ⋮& ⋮& \ddots & ⋮\\ {\mathbf{0}}_{3\times 1}& {\mathbf{0}}_{3\times 1}& \dots & {\mathbf{w}}_{2:4}^T+{\mathbf{g}}_{2:4}^T\end{array}\right]}_{M\times 3M}\end{array}$$(A.17)

Appendix B

The $\partial \mathbf{\alpha }/\partial \mathbf{\beta }$ is contained the partial derivatives $(\partial \mathbf{r}/\partial \mathbf{s})$, $(\partial \dot{\mathbf{r}}/\partial \mathbf{s})$, $(\partial \mathbf{r}/\partial \dot{\mathbf{s}})$ and $(\partial \dot{\mathbf{r}}/\partial \dot{\mathbf{s}})$ and they are evaluated as in (B.1), (B.2), (B.3) and (B.4)

(B.1) $$\left(\frac{\partial \mathbf{r}}{\partial \mathbf{s}}\right)=\left[\begin{array}{ccccc}\frac{{({\mathbf{s}}_1-\mathbf{u})}^T}{r_1}& -\frac{{({\mathbf{s}}_2-\mathbf{u})}^T}{r_2}& \mathbf{0}& \cdots & \mathbf{0}\\ \frac{{({\mathbf{s}}_1-\mathbf{u})}^T}{r_1}& \mathbf{0}& -\frac{{({\mathbf{s}}_3-\mathbf{u})}^T}{r_3}& \cdots & \mathbf{0}\\ ⋮& ⋮& ⋮& \ddots & \mathbf{0}\\ \frac{{({\mathbf{s}}_1-\mathbf{u})}^T}{r_1}& \mathbf{0}& \mathbf{0}& \dots & -\frac{{({\mathbf{s}}_M-\mathbf{u})}^T}{r_M}\end{array}\right]$$(B.1) (B.2) $$\left(\partial \dot{\mathbf{r}}/\partial \dot{\mathbf{s}}\right)=\left(\partial \mathbf{r}/\partial \mathbf{s}\right)$$(B.2) (B.3) $$\left(\partial \mathbf{r}/\partial \dot{\mathbf{s}}\right)={\mathbf{0}}_{(M-1)\times 3}$$(B.3) (B.4) $$\begin{array}{c}\left(\frac{\partial \dot{\mathbf{r}}}{\partial \mathbf{s}}\right)=\left[\begin{array}{ccccc}\frac{{\left({\dot{\mathbf{s}}}_1-\dot{\mathbf{u}}\right)}^T}{r_1}& \frac{{\left({\mathbf{s}}_2-\mathbf{u}\right)}^T\mathrm{ }{\dot{r}}_2}{r_2^2}& \mathbf{0}& \cdots & \mathbf{0}\\ \frac{{\left({\dot{\mathbf{s}}}_1-\dot{\mathbf{u}}\right)}^T}{r_1}& \mathbf{0}& \frac{{\left({\mathbf{s}}_3-\mathbf{u}\right)}^T{\dot{r}}_3}{\dot{r}}& \cdots & \mathbf{0}\\ ⋮& ⋮& ⋮& \ddots & \mathbf{0}\\ \frac{{\left({\dot{\mathbf{s}}}_1-\dot{\mathbf{u}}\right)}^T}{r_1}& \mathbf{0}& \mathbf{0}& \dots & \frac{{\left({\mathbf{s}}_M-\mathbf{u}\right)}^T\mathrm{ }{\dot{r}}_M}{r_M^2}\end{array}\right]\\ -\left[\begin{array}{ccccc}\frac{{\left({\mathbf{s}}_1-\mathbf{u}\right)}^T{\dot{r}}_1}{r_1^2}& \frac{{\left({\dot{\mathbf{s}}}_2-\dot{\mathbf{u}}\right)}^T}{r_2}& \mathbf{0}& \cdots & \mathbf{0}\\ \frac{{\left({\mathbf{s}}_1-\mathbf{u}\right)}^T{\dot{r}}_1}{r_1^2}& \mathbf{0}& \frac{{\left({\mathbf{s}}_3-\mathbf{u}\right)}^T}{r_3}& \cdots & \mathbf{0}\\ ⋮& ⋮& ⋮& \ddots & \mathbf{0}\\ \frac{{\left({\mathbf{s}}_1-\mathbf{u}\right)}^T{\dot{r}}_1}{r_1^2}& \mathbf{0}& \mathbf{0}& \dots & \frac{{\left({\mathbf{s}}_M-\mathbf{u}\right)}^T}{r_M}\end{array}\right]\end{array}$$(B.4)

In addition, the $\partial \mathbf{\alpha }/\partial \mathbf{\theta }$ is contained the partial derivatives $(\partial \mathbf{r}/\partial \mathbf{u})$, $(\partial \mathbf{r}/\partial \dot{\mathbf{u}})$, $(\partial \dot{\mathbf{r}}/\partial \mathbf{u})$ and $(\partial \dot{\mathbf{r}}/\partial \dot{\mathbf{u}})$, and they are given as in (B.5), (B.6), (B.7), and (B.8), respectively. (B.5) $$\left(\frac{\partial \mathbf{r}}{\partial \mathbf{u}}\right)=\left[\begin{array}{c}\frac{{\left({\mathbf{s}}_2-\mathbf{u}\right)}^T}{r_2}-\frac{{\left({\mathbf{s}}_1-\mathbf{u}\right)}^T}{r_1}\\ \frac{{\left({\mathbf{s}}_3-\mathbf{u}\right)}^T}{r_3}-\frac{{\left({\mathbf{s}}_1-\mathbf{u}\right)}^T}{r_1}\\ ⋮\\ \frac{{\left({\mathbf{s}}_M-\mathbf{u}\right)}^T}{r_M}-\frac{{\left({\mathbf{s}}_1-\mathbf{u}\right)}^T}{r_1}\end{array}\right]$$(B.5) (B.6) $$\left(\partial \dot{\mathbf{r}}/\partial \dot{\mathbf{u}}\right)=\left(\partial \mathbf{r}/\partial \mathbf{u}\right)$$(B.6) (B.7) $$\left(\frac{\partial \dot{\mathbf{r}}}{\partial \mathbf{u}}\right)=\left[\begin{array}{c}\frac{{\left({\mathbf{s}}_2-\mathbf{u}\right)}^T{\dot{r}}_2}{r_2^2}-\frac{{\left({\mathbf{s}}_1-\mathbf{u}\right)}^T{\dot{r}}_1}{r_1^2}\\ \frac{{\left({\mathbf{s}}_3-\mathbf{u}\right)}^T{\dot{r}}_3}{r_3^2}-\frac{{\left({\mathbf{s}}_1-\mathbf{u}\right)}^T{\dot{r}}_1}{{\dot{r}}_1^2}\\ ⋮\\ \frac{{\left({\mathbf{s}}_M-\mathbf{u}\right)}^T{\dot{r}}_M}{{\dot{r}}_M^2}-\frac{{\left({\mathbf{s}}_1-\mathbf{u}\right)}^T{\dot{r}}_1}{r_1^2}\end{array}\right]+\left[\begin{array}{c}\frac{{\left({\dot{\mathbf{s}}}_2-\dot{\mathbf{u}}\right)}^T}{r_2}-\frac{{\left({\dot{\mathbf{s}}}_1-\dot{\mathbf{u}}\right)}^T}{r_1}\\ \frac{{\left({\dot{\mathbf{s}}}_3-\dot{\mathbf{u}}\right)}^T}{r_3}-\frac{{\left({\dot{\mathbf{s}}}_1-\dot{\mathbf{u}}\right)}^T}{r_1}\\ ⋮\\ \frac{{\left({\dot{\mathbf{s}}}_M-\dot{\mathbf{u}}\right)}^T}{r_M}-\frac{{\left({\dot{\mathbf{s}}}_1-\dot{\mathbf{u}}\right)}^T}{r_1}\end{array}\right]$$(B.7) (B.8) $$\left(\partial \mathbf{r}/\partial \dot{\mathbf{u}}\right)=0$$(B.8)