A Robust Constrained Total Least Squares Algorithm for Three-Dimensional Target Localization with Hybrid TDOA–AOA Measurements

Abstract

Three-dimensional (3D) target localization by using hybrid time difference of arrival (TDOA) and angle of arrival (AOA) measurements from multiple sensors has been an active research area for several decades due to its extensive applications in various fields. For this nonlinear estimation problem, the pseudolinear system of equations constructed by using the measurements generally acts as the basis of numerous localization algorithms. In this paper, we aim to improve the performance of 3D TDOA–AOA localization by introducing the constrained total least squares (CTLS) framework wherein the inherent characteristics of the pseudolinear equations can be properly taken into consideration. On the basis of the total least squares model, the CTLS model for 3D TDOA–AOA localization is established by imposing the inherent characteristics of the pseudolinear equations as additional constraints. Then, the multi-constraint optimization problem in CTLS model is solved by using an iterative algorithm based on successive projections. Extensive numerical simulations are accomplished for evaluating the performance of the proposed CTLS algorithm. The results show that the proposed algorithm gives moderate accuracy enhancement with acceptable computational cost, and more importantly, it is more robust to large measurement noise than the compared algorithms.

Appendices

Appendix A: The derivation of the BR-WLS algorithm

Based on Eq. (20), the cost function of the WLS model can be equivalently written as

$$ \begin{aligned} J& = ({\hat{\mathbf{A}}}{\varvec{u}} - \hat{\boldsymbol{h}})^{T} {\mathbf{W}}({\hat{\mathbf{A}}}{\varvec{u}} - \hat{\boldsymbol{h}}) \\ &= ({\hat{\mathbf{A}}}_{o} {\varvec{u}} - \hat{\boldsymbol{h}}_{o} )^{T} {\mathbf{W}}_{o} ({\hat{\mathbf{A}}}_{o} {\varvec{u}} - \hat{\boldsymbol{h}}_{o} ), \\ \end{aligned} $$

(37)

where

$$ \begin{aligned} & {\hat{\mathbf{A}}}_{o} = [{\hat{\mathbf{A}}}_{(1:2K)}^{T} ,{\mathbf{0}}_{4} ,{\hat{\mathbf{A}}}_{(2K + 1:3K - 1)}^{T} ]^{T} , \hfill \\ & \hat{\boldsymbol{h}}_{o} = [\hat{\boldsymbol{h}}_{(1:2K)}^{T} ,0,\hat{\boldsymbol{h}}_{(2K + 1:3K - 1)}^{T} ]^{T} , \hfill \\ & {\mathbf{W}}_{o} = \left[ {\begin{array}{*{20}c} {{\mathbf{W}}_{o1,1} } &\vline & {{\mathbf{W}}_{o1,2} } &\vline & {{\mathbf{W}}_{o1,3} } \\ \hline {{\mathbf{W}}_{o2,1} } &\vline & {{\mathbf{W}}_{o2,2} } &\vline & {{\mathbf{W}}_{o2,3} } \\ \hline {{\mathbf{W}}_{o3,1} } &\vline & {{\mathbf{W}}_{o3,2} } &\vline & {{\mathbf{W}}_{o3,3} } \\ \end{array} } \right] \hfill \\ &\quad = \left[ {\begin{array}{*{20}c} {{\mathbf{W}}_{(1:K,1:K)} } &\vline & {{\mathbf{W}}_{(1:K,K + 1:2K)} } &\vline & {{\mathbf{W}}_{(1:K,2K + 1:3K - 1)} } \\ \hline {{\mathbf{W}}_{(K + 1:2K,1:K)} } &\vline & {{\mathbf{W}}_{(K + 1:2K,K + 1:2K)} } &\vline & {{\mathbf{W}}_{(K + 1:2K,2K + 1:3K - 1)} } \\ \hline {{\mathbf{0}}_{K}^{T} } &\vline & {{\mathbf{0}}_{K}^{T} } &\vline & {{\mathbf{0}}_{K + 1}^{T} } \\ {{\mathbf{W}}_{(2K + 1:3K - 1,1:K)} } &\vline & {{\mathbf{W}}_{(2K + 1:3K - 1,K + 1:2K)} } &\vline & {\begin{array}{*{20}c} {{\mathbf{0}}_{K - 1} } & {{\mathbf{W}}_{(2K + 1:3K - 1,2K + 1:3K - 1)} } \\ \end{array} } \\ \end{array} } \right]. \hfill \\ \end{aligned} $$

(38)

In Eq. (38), the matrices ${\hat{\mathbf{A}}}, \, \hat{\boldsymbol{h}}$ and ${\mathbf{W}}$ are expanded by zero. Let ${\hat{\mathbf{G}}}_{o} = [{\hat{\mathbf{A}}}_{o} , - \hat{\boldsymbol{h}}_{o} ]$ and ${\varvec{\eta}} = [{\varvec{u}}^{T} ,1]^{T}$ be the intermediate matrix and vector, then Eq. (37) can be expressed as

$$ J = {\varvec{\eta}}^{T} {\hat{\mathbf{G}}}_{o}^{T} {\mathbf{W}}_{o} {\hat{\mathbf{G}}}_{o} {\varvec{\eta}}. $$

(39)

In practice, the matrix ${\hat{\mathbf{G}}}_{o}$ is noise corrupted and can be decomposed as

$$ {\hat{\mathbf{G}}}_{o} = {\mathbf{G}}_{o} + \Delta {\mathbf{G}}_{o} , $$

(40)

where G_o denotes the true counterpart of ${\hat{\mathbf{G}}}_{o}$. After some direct algebraic manipulations, the noise contamination term ΔG_o can be approximated as

$$ \Delta {\mathbf{G}}_{o} \approx blkdiag({{\varvec{\Gamma}}},{{\varvec{\Gamma}}},{{\varvec{\Gamma}}})\left[ {\begin{array}{*{20}c} {{{\varvec{\Lambda}}}_{1} } \\ {{{\varvec{\Lambda}}}_{2} } \\ {{{\varvec{\Lambda}}}_{3} } \\ \end{array} } \right], $$

(41)

where we omit some θ₁(or φ₁)-related terms for a simpler expression (Note that although we make some truncation here, it is shown by simulations that this algorithm still can improve the localization performance over the WLS algorithm). The matrix Γ contains the measurement noise and Λ_i, i = 1, 2, 3 are the corresponding coefficients related to each noise term. They are, respectively, defined as

$$ \begin{gathered} {{\varvec{\Gamma}}} = \left[ {diag({\varvec{n}}),diag({\varvec{\omega}}),diag([0;{\varvec{v}}])} \right], \hfill \\ {{\varvec{\Lambda}}}_{1} = \left[ {{\mathbf{F}}_{1,\theta }^{T} ,{\mathbf{O}}_{4 \times K} ,{\mathbf{O}}_{4 \times K} } \right]^{T} , \hfill \\ {{\varvec{\Lambda}}}_{2} = \left[ {{\mathbf{F}}_{2,\theta }^{T} ,{\mathbf{F}}_{2,\varphi }^{T} ,{\mathbf{O}}_{4 \times K} } \right]^{T} , \hfill \\ {{\varvec{\Lambda}}}_{3} = \left[ {{\mathbf{F}}_{3,\theta }^{T} ,{\mathbf{F}}_{3,\varphi }^{T} ,{\mathbf{F}}_{3,\tau }^{T} } \right]^{T} . \hfill \\ \end{gathered} $$

(42)

The matrices F_i,θ, F_j,φ, and F_3,τ are defined with their nth row, n = 1, 2, …, K, given by

$$ \begin{gathered} \left( {{\mathbf{F}}_{1,\theta } } \right)_{(n)} = \left[ {{\varvec{f}}_{1,\theta ,n}^{T} , - {\varvec{f}}_{1,\theta ,n}^{T} {\varvec{s}}_{n} } \right],{\varvec{f}}_{1,\theta ,n} = [\cos \theta_{n} ,\sin \theta_{n} ,0]^{T} , \hfill \\ \left( {{\mathbf{F}}_{2,\theta } } \right)_{(n)} = \left[ {{\varvec{f}}_{2,\theta ,n}^{T} , - {\varvec{f}}_{2,\theta ,n}^{T} {\varvec{s}}_{n} } \right],{\varvec{f}}_{2,\theta ,n} = [ - \sin \theta_{n} \sin \varphi_{n} ,\cos \theta_{n} \sin \varphi_{n} ,0]^{T} , \hfill \\ \left( {{\mathbf{F}}_{3,\theta } } \right)_{(n)} = \left[ {2{\varvec{f}}_{a,n}^{T} ,{\varvec{f}}_{a,n}^{T} {\varvec{e}}_{n} } \right],{\varvec{e}}_{n} = ( - {\varvec{s}}_{1} - {\varvec{s}}_{n} + c\tau_{n} {\varvec{b}}_{1} ), \hfill \\ \left( {{\mathbf{F}}_{2,\varphi } } \right)_{(n)} = \left[ {{\varvec{f}}_{2,\varphi ,n}^{T} , - {\varvec{f}}_{2,\varphi ,n}^{T} {\varvec{s}}_{n} } \right]\boldsymbol{,f}_{2,\varphi ,n} = [\cos \theta_{n} \cos \varphi_{n} ,\sin \theta_{n} \cos \varphi_{n} ,\sin \varphi_{n} ]^{T} , \hfill \\ \left( {{\mathbf{F}}_{3,\varphi } } \right)_{(n)} = \left[ { - 2{\varvec{g}}_{2,n}^{T} , - {\varvec{g}}_{2,n}^{T} {\varvec{e}}_{n} } \right], \hfill \\ \left( {{\mathbf{F}}_{3,\tau } } \right)_{(n)} = \left[ {0,0,0,{\varvec{b}}_{1}^{T} ({\varvec{b}}_{n} - {\varvec{b}}_{1} )} \right]. \hfill \\ \end{gathered} $$

(43)

Substituting Eq. (40) into Eq. (39) results in

$$ J = {\varvec{\eta}}^{T} {\mathbf{G}}_{o}^{T} {\mathbf{W}}_{o} {\mathbf{G}}_{o} {\varvec{\eta}} + {\varvec{\eta}}^{T} \Delta {\mathbf{G}}_{o}^{T} {\mathbf{W}}_{o} \Delta {\mathbf{G}}_{o} {\varvec{\eta}} + 2{\varvec{\eta}}^{T} \Delta {\mathbf{G}}_{o}^{T} {\mathbf{W}}_{o} {\mathbf{G}}_{o}^{T} {\varvec{\eta}}, $$

(44)

whose mathematical expectation can be given as

$$ E[J] = {\varvec{\eta}}^{T} {\mathbf{G}}_{o}^{T} {\mathbf{W}}_{o} {\mathbf{G}}_{o} {\varvec{\eta}} + {\varvec{\eta}}^{T} E[\Delta {\mathbf{G}}_{o}^{T} {\mathbf{W}}_{o} \Delta {\mathbf{G}}_{o} ]{\varvec{\eta}}. $$

(45)

Clearly, the second term in Eq. (45) makes the minimum of E[J] deviate from the ideal solution (i.e., zero) and causes the estimation bias since it changes with η. The idea of the bias reduction technique is to minimize J subject to ${\varvec{\eta}}^{T} E[\Delta {\mathbf{G}}_{o}^{T} {\mathbf{W}}_{o} \Delta {\mathbf{G}}_{o} ]{\varvec{\eta}}$ equaling to a constant [28]. Therefore, the bias reduction model can be constructed as

$$ \begin{gathered} \mathop {\min }\limits_{{\varvec{\eta}}} \, {\varvec{\eta}}^{T} {\hat{\mathbf{G}}}_{o}^{T} {\mathbf{W}}_{o} {\hat{\mathbf{G}}}_{o} {\varvec{\eta}} \hfill \\ {\text{s.t.}}\, {\varvec{\eta}}^{T} {{\varvec{\Omega}}}{\varvec{\eta}} = k, \hfill \\ \end{gathered} $$

(46)

where ${{\varvec{\Omega}}} = E[\Delta {\mathbf{G}}_{o}^{T} {\mathbf{W}}_{o} \Delta {\mathbf{G}}_{o} ]$. The constant k does not affect the final solution and it can be any positive value. The Lagrange multiplier technique can be applied to solve the problem and the corresponding auxiliary function can be constructed as

$$ {\varvec{\eta}}^{T} {\hat{\mathbf{G}}}_{o}^{T} {\mathbf{W}}_{o} {\hat{\mathbf{G}}}_{o} {\varvec{\eta}} + \lambda (k - {\varvec{\eta}}^{T} {{\varvec{\Omega}}}{\varvec{\eta}}), $$

(47)

where λ is Lagrange multiplier. Taking the derivate of Eq. (47) with respect to η and setting it to zero lead to

$$ {\hat{\mathbf{G}}}_{o}^{T} {\mathbf{W}}_{o} {\hat{\mathbf{G}}}_{o} {\varvec{\eta}} = \lambda {{\varvec{\Omega}}}{\varvec{\eta}}. $$

(48)

Premultiplying η^T at both sides of Eq. (48) and considering the constraint in Eq. (46), we arrive at ${\varvec{\eta}}^{T} {\hat{\mathbf{G}}}_{o}^{T} {\mathbf{W}}_{o} {\hat{\mathbf{G}}}_{o} {\varvec{\eta}} = \lambda k$, indicating that the solution of η is the generalized eigenvector corresponding to the minimal generalized eigenvalue of the matrix pair $({\hat{\mathbf{G}}}_{o}^{T} {\mathbf{W}}_{o} {\hat{\mathbf{G}}}_{o} , \, {{\varvec{\Omega}}})$. The bias-reduced solution can then be given as

$$ \tilde{\boldsymbol{u}}_{{\text{BR - WLS}}} = {\varvec{\eta}}_{(1:3)} /{\varvec{\eta}}_{(4)} . $$

(49)

The matrix Ω can be calculated as

$$ {{\varvec{\Omega}}} = E[\Delta {\mathbf{G}}_{o}^{T} {\mathbf{W}}_{o} \Delta {\mathbf{G}}_{o} ] = \sum\limits_{i = 1}^{3} {\sum\limits_{j = 1}^{3} {{{\varvec{\Lambda}}}_{i}^{T} E[{{\varvec{\Gamma}}}^{T} {\mathbf{W}}_{oi,j} {{\varvec{\Gamma}}}]{{\varvec{\Lambda}}}_{j} } } , $$

(50)

where the matrices W_oi,j i, j = 1, 2, 3 are defined in Eq. (38). Supported by Eq. (50), it is straightforward to obtain

$$ E[{{\varvec{\Gamma}}}^{T} {\mathbf{W}}_{oi,j} {{\varvec{\Gamma}}}] = blkdiag\left[ {{\mathbf{Q}}_{a} \odot {\mathbf{W}}_{oi,j} ,{\mathbf{Q}}_{a} \odot {\mathbf{W}}_{oi,j} ,blkdiag(0,{\mathbf{Q}}_{t} ) \odot {\mathbf{W}}_{oi,j} } \right], $$

(51)

where ⊙ denotes the Hadamard production. In practice, the values of AOA and TDOA measurements in Λ_i, i = 1, 2, 3 can be replaced by their noisy counterparts and this algorithm is named as BR-WLS in this paper.

This bias-reduced technique was first proposed for TDOA localization problem in Ref. [12] and then utilized for AOA localization in Ref. [28]. Here, we derive the BR-WLS algorithm for the 3D TDOA–AOA localization problem. Although a similar work seems to have been reported in Ref. [33], we shall declare that the algorithm in Ref. [33] is designed for two sensors while the one derived here is more universal and suitable for multiple sensors.

Appendix B: The CRLB of 3D TDOA–AOA localization problem

Assume that the measurement noise vector $\varvec{e}\in\mathbb{R}^{3K-1}$ is zero-mean Gaussian with the covariance matrix $\varvec{Q}\in\mathbb{R}^{(3K-1)\times(3K-1)}$. The fisher information matrix (FIM) conforms the following expression:

$$ {\text{FIM}} = \left[ {\frac{{\partial {\varvec{m}}}}{{\partial {\varvec{u}}}}} \right]^{T} {\mathbf{Q}}^{ - 1} \left[ {\frac{{\partial {\varvec{m}}}}{{\partial {\varvec{u}}}}} \right], $$

(52)

Based on Eqs. (2)–(4), the partial derivatives in Eq. (52) can be expressed as

$$ \frac{{\partial {\varvec{m}}}}{{\partial {\varvec{u}}}} = \left[ {{\varvec{J}}_{\theta ,1} , \cdots ,{\varvec{J}}_{\theta ,M} ,{\varvec{J}}_{\varphi ,1} , \cdots {\varvec{J}}_{\varphi ,M} ,{\varvec{J}}_{\tau ,2} , \cdots ,{\varvec{J}}_{\tau ,M} } \right]^{T} , $$

(53)

where

$$ {\varvec{J}}_{\theta ,k} = \frac{1}{{d_{k}^{2} }}\left[ { - (u_{y} - s_{k,y} ),(u_{x} - s_{k,x} ),0} \right]^{T} , $$

(54)

$$ {\varvec{J}}_{\varphi ,k} = \frac{1}{{r_{k}^{2} d_{k} }}\left[ { - (u_{x} - s_{k,x} )(u_{z} - s_{k,z} ), - (u_{y} - s_{k,y} )(u_{z} - s_{k,z} ),d_{k}^{2} } \right]^{T} , $$

(55)

$$ {\varvec{J}}_{\tau ,k} = \frac{{{\varvec{u}} - {\varvec{s}}_{k} }}{{r_{k} }} - \frac{{{\varvec{u}} - {\varvec{s}}_{1} }}{{r_{1} }}, $$

(56)

where $d_{k} = \sqrt {(u_{x} - s_{k,x} )^{2} + (u_{y} - s_{k,y} )^{2} }$ denotes the horizontal distance between the target and the kth sensor. The CRLB can be computed from the inverse of FIM and it holds the expression CRLB = FIM⁻¹. It describes the lower bound of RMSE that an unbiased estimator can achieve. Although most of the existing algorithms are typically biased, the CRLB is still an important benchmark in terms of RMSE performance, particularly when the estimation bias is significantly smaller than variance.

Appendix C: Computational Complexity Analysis of the Investigated Algorithms

See Tables 3, 4, 5, 6, 7 and 8.

Table 3 Computational complexity for the common variables

Table 4 Computational complexity for the WLS algorithm

Table 5 Computational complexity for the BR-WLS algorithm

Table 6 Computational complexity for the TLS algorithm

Table 7 Computational complexity for the STLS algorithm [13, 14] in each iteration

Table 8 Computational complexity for the CTLS algorithm in each iteration