logo

SCIENTIA SINICA Informationis, Volume 49 , Issue 5 : 570-584(2019) https://doi.org/10.1360/N112018-00341

Multi-source passive localization via multiple unmanned aerial vehicles

More info
  • ReceivedDec 30, 2018
  • AcceptedMar 10, 2019
  • PublishedMay 8, 2019

Abstract


Funded by

国家自然科学基金(61873217)

西南民族大学中央高校基本科研业务费(2019NQN28)


Supplement

Appendix

定理3.1的证明

将优化问题(22)中的$m$个线性等式约束写成矩阵形式: \begin{equation} {\boldsymbol A}\bar{{\boldsymbol u}}= \bf 0. \tag{32}\end{equation} 根据矩阵广义逆理论 1), 线性方程(32)的通解可以表示为 \begin{equation*} \bar{{\boldsymbol u}} ={{\boldsymbol A}}^\dagger \bf{0}+({\boldsymbol I}-{{\boldsymbol A}}^\dagger{\boldsymbol A}){\boldsymbol \xi} =({\boldsymbol I}-{{\boldsymbol A}}^\dagger{\boldsymbol A}){\boldsymbol \xi},\end{equation*} 其中${\boldsymbol~\xi}~\in~\mathbb{R}^{4m}~$可以为任意向量. 显然${{\boldsymbol~A}}$是一个行满秩的矩阵, 有 \begin{equation*} {{\boldsymbol A}}^\dagger={{\boldsymbol A}}'({\boldsymbol A}{{\boldsymbol A}}')^{-1},\end{equation*} 因此, 线性方程(32)的通解也可表示为 \begin{align} \bar{{\boldsymbol u}} = {\boldsymbol P}{\boldsymbol \xi}. \tag{33} \end{align} 将通解(33)代入优化问题(22)的目标函数, 得到 \begin{equation*}\bar{{\boldsymbol u}}'\bar{{\boldsymbol W}}\bar{{\boldsymbol u}}-2\bar{{\boldsymbol h}}'\bar{{\boldsymbol u}} = {\boldsymbol \xi}'{{\boldsymbol P}}'\bar{{\boldsymbol W}}{\boldsymbol P}{\boldsymbol \xi}-2\bar{{\boldsymbol h}}'{\boldsymbol P}{\boldsymbol \xi},\end{equation*} 其中${\boldsymbol~P}$是一个对称矩阵. 根据矩阵伪逆的基本性质, 有 \begin{equation*}{{\boldsymbol P}} \bar{{\boldsymbol W}}{\boldsymbol P}\big({{\boldsymbol P}} \bar{{\boldsymbol W}}{\boldsymbol P}\big)^\dagger{\boldsymbol P}\bar{{\boldsymbol h}}={\boldsymbol P}\bar{{\boldsymbol h}},\end{equation*} 因此, \begin{eqnarray}\bar{{\boldsymbol u}}'\bar{{\boldsymbol W}}\bar{{\boldsymbol u}}-2\bar{{\boldsymbol h}}'\bar{{\boldsymbol u}} = \big({\boldsymbol \xi}-({\boldsymbol P}\bar{{\boldsymbol W}}{\boldsymbol P})^\dagger{\boldsymbol P}\bar{{\boldsymbol h}}\big)'{{\boldsymbol P}} \bar{{\boldsymbol W}}{\boldsymbol P}\big({\boldsymbol \xi}-({\boldsymbol P}\bar{{\boldsymbol W}}{\boldsymbol P})^\dagger{\boldsymbol P}\bar{{\boldsymbol h}}\big) -\bar{{\boldsymbol h}}'({\boldsymbol P}\bar{{\boldsymbol W}}{\boldsymbol P})^\dagger\bar{{\boldsymbol h}}. \tag{34} \end{eqnarray} 显然, 最小化二次目标函数等价于取${\boldsymbol~\xi}=({\boldsymbol~P}\bar{{\boldsymbol~W}}{\boldsymbol~P})^\dagger{\boldsymbol~P}\bar{{\boldsymbol~h}}.$ 由于${\boldsymbol~P}({\boldsymbol~P}\bar{{\boldsymbol~W}}{\boldsymbol~P})^\dagger=({\boldsymbol~P}\bar{{\boldsymbol~W}}{\boldsymbol~P})^\dagger$且$({\boldsymbol~P}\bar{{\boldsymbol~W}}{\boldsymbol~P})^\dagger$是一个对称矩阵, 有 \begin{equation} {\boldsymbol \xi}=({\boldsymbol P}\bar{{\boldsymbol W}}{\boldsymbol P})^\dagger\bar{{\boldsymbol h}}. \tag{35}\end{equation} 将式(35)代入通解(33)中, 可得式(23)是优化问题(22)的最优解.

Ben-Israel A, Greville T N E. Generalized Inverses: Theory and Applications. 2nd ed. Hoboken: Wiley, 2002.

定理4.1的证明

从方程(28) 和 (29)的条件可知, $\hat{\bar{{\boldsymbol~u}}}$ 一定是优化问题: \begin{equation}\min_{\bar{{\boldsymbol u}}} \bar{{\boldsymbol u}}'\bar{{\boldsymbol W}}\bar{{\boldsymbol u}}-2{\bar{{\boldsymbol h}}}'\bar{{\boldsymbol u}}+\sum_{j=1}^m\lambda_j{\bar{{\boldsymbol u}}}'{\boldsymbol C}_j\bar{{\boldsymbol u}} \tag{36}\end{equation} 的最优解, 那么, 对原问题(19)的任意可行解${\bar{{\boldsymbol~u}}}$都有 \begin{align*}\bar{{\boldsymbol u}}'\bar{{\boldsymbol W}}\bar{{\boldsymbol u}}-2{\bar{{\boldsymbol h}}}'\bar{{\boldsymbol u}} &=\bar{{\boldsymbol u}}'\bar{{\boldsymbol W}}\bar{{\boldsymbol u}}-2{\bar{{\boldsymbol h}}}'\bar{{\boldsymbol u}}+\sum\limits_{j=1}^m\lambda_j{\bar{{\boldsymbol u}}}'{\boldsymbol C}_j\bar{{\boldsymbol u}} \\ & \geq \hat{\bar{{\boldsymbol u}}}'\bar{{\boldsymbol W}}\hat{\bar{{\boldsymbol u}}}-2{\bar{{\boldsymbol h}}}'\hat{\bar{{\boldsymbol u}}}+\sum\limits_{j=1}^m\lambda_j{\hat{\bar{{\boldsymbol u}}}}'{\boldsymbol C}_j\hat{\bar{{\boldsymbol u}}} \\ \nonumber&= \hat{\bar{{\boldsymbol u}}}'\bar{{\boldsymbol W}}\hat{\bar{{\boldsymbol u}}}-2{\bar{{\boldsymbol h}}}'\hat{\bar{{\boldsymbol u}}}, \end{align*} 其中第1个等式成立是由于${\bar{{\boldsymbol~u}}}$是原问题(19)的可行解, 最后一个等式成立是由于方程(30)的条件. 因此, $\hat{\bar{{\boldsymbol~u}}}$一定是优化问题(19)的全局最优解.

定理4.2的证明

首先证明如果序列$\{\hat{{\boldsymbol~u}}^{k},~k=0,1,\ldots\}$收敛到$\hat{{\boldsymbol~u}}$, 那么算法alg1涉及的变量序列$\{\bar{{\boldsymbol~u}}^{k}\}$, $\{{\bar{{\boldsymbol~W}}}^{k}\}$, $\{{\bar{{\boldsymbol~h}}}^{k}\}$ 和 $\{{{\boldsymbol~P}}^{k}\}$ 也是收敛的. 由算法alg1的第$5$步和第$10$步更新策略可得 $$\bar{{\boldsymbol u}}^{k}=2\hat{{\boldsymbol u}}^{k}-\hat{{\boldsymbol u}}^{k-1},$$ 因此, $\lim_{k\rightarrow~\infty}\bar{{\boldsymbol~u}}^k~=2\lim_{k\rightarrow~\infty}\hat{{\boldsymbol~u}}^k-\lim_{k\rightarrow~\infty}\hat{{\boldsymbol~u}}^{k-1} =2\hat{{\boldsymbol~u}}-\hat{{\boldsymbol~u}}=\hat{{\boldsymbol~u}}.$ 根据式(10), (D), (15)和(18)可知, 权矩阵${\boldsymbol~W}$是关于目标位置${\boldsymbol~u}$的连续函数, 因此如果序列$\{\hat{{\boldsymbol~u}}^{k},~k=0,1,\ldots\}$收敛到$\hat{{\boldsymbol~u}}$, 那么其更新的权矩阵序列$\{{\boldsymbol~W}(\hat{{\boldsymbol~u}}^{k}),~k=0,1,\ldots\}$也将收敛, 记其极限为${\boldsymbol~W}(\hat{{\boldsymbol~u}})$. 由于式(20)和(21)可知$\bar{{\boldsymbol~W}}$和$\bar{{\boldsymbol~h}}$均是关于${\boldsymbol~W}$的连续函数, 所以算法alg1更新的序列$\{{\bar{{\boldsymbol~W}}}^{k}\}$和$\{{\bar{{\boldsymbol~h}}}^{k}\}$均收敛, 记其极限分别为 $\bar{{\boldsymbol~W}}(\hat{{\boldsymbol~u}})$和 $\bar{{\boldsymbol~h}}(\hat{{\boldsymbol~u}})$,即 \begin{equation}\bar{{\boldsymbol W}}(\hat{{\boldsymbol u}}) = \lim_{k\rightarrow \infty} {\bar{{\boldsymbol W}}}^{k}, \bar{{\boldsymbol h}}(\hat{{\boldsymbol u}}) = \lim_{k\rightarrow \infty} {\bar{{\boldsymbol h}}}^{k}. \tag{37}\end{equation} 同理, 由式(24)更新的序列$\{{{{\boldsymbol~P}}}^{k}\}$也是收敛的, 记其极限为${{\boldsymbol~P}}(\hat{{\boldsymbol~u}})$.

根据定理3.1,算法alg1中的$\bar{{\boldsymbol~u}}^{k}$是近似CWLS多目标定位问题: \begin{align} \min_{\bar{{\boldsymbol u}}} \bar{{\boldsymbol u}}'\bar{{\boldsymbol W}}^k\bar{{\boldsymbol u}}-2(\bar{{\boldsymbol h}}^k)'\bar{{\boldsymbol u}} \mbox{s.t.} (\hat{{\boldsymbol u}}^k)'{\boldsymbol C}_j\bar{{\boldsymbol u}}=0 (j=1,\ldots,m) \tag{38} \end{align} 的最优解.由于问题(38)是一个凸优化问题, 因此其最优解$\bar{{\boldsymbol~u}}^{k}$必定满足该问题的Karush-Kuhn-Tucher (KKT)条件 2), 即存在$\bar{\lambda}_j^{k}~\in~\mathbb{R}~(j=1,\ldots,m)$, 使得下面的方程组: \begin{eqnarray}&\displaystyle \bar{{\boldsymbol W}}^{k}\bar{{\boldsymbol u}}^{k}+\sum\limits_{j=1}^m \bar{\lambda}_j^{k}{\boldsymbol C}_j\hat{{\boldsymbol u}}^{k} = \bar{{\boldsymbol h}}^{k}, \tag{39} \\ &\displaystyle(\hat{{\boldsymbol u}}^{k})'{\boldsymbol C}_j\bar{{\boldsymbol u}}^{k} =0 (j=1,\ldots,m) \tag{40} \end{eqnarray} 成立. 根据权矩阵的构造式(10), (D), (15)和(20)可知, 算法alg1中的矩阵$\bar{{\boldsymbol~W}}^{k}$在每次迭代中都是正定矩阵, 因此由(39)式可得 \begin{equation}\bar{{\boldsymbol u}}^{k}= (\bar{{\boldsymbol W}}^{k})^{-1} \left(\bar{{\boldsymbol h}}^{k}- \sum_{j=1}^m \bar{\lambda}_j^{k}{\boldsymbol C}_j\hat{{\boldsymbol u}}^{k}\right). \tag{41}\end{equation} 将(41)式代入(40)中, 有 \begin{equation}{\boldsymbol a}_j^{k}\bar{{\boldsymbol h}}^{k}= \sum_{j=1}^m {\boldsymbol a}_j^{k}{\boldsymbol C}_j' \hat{{\boldsymbol u}}^{k} \bar{\lambda}_j^{k} (j=1,\ldots,m), \tag{42}\end{equation} 其中${\boldsymbol~a}_j^{k}=(\hat{{\boldsymbol~u}}^{k})'{\boldsymbol~C}_j(\bar{{\boldsymbol~W}}^{k})^{-1}~.$ 根据(31)和(37),可得 \begin{equation}{\boldsymbol a}_j \triangleq \lim_{k\rightarrow \infty} {\boldsymbol a}_j^{k}= \hat{{\boldsymbol u}}'{\boldsymbol C}_j(\bar{{\boldsymbol W}}(\hat{{\boldsymbol u}}))^{-1} (j=1,\ldots,m), \tag{43}\end{equation} 因此, 方程组(42)的解$(\bar{\lambda}_1^{k},\ldots,\bar{\lambda}_m^{k})$也是收敛的, 记其极限为$(\hat{\lambda}_1,\ldots,\hat{\lambda}_m).$

现在对方程(39)两边取极限, 可得 \begin{equation}\bar{{\boldsymbol W}}(\hat{{\boldsymbol u}}) \hat{{\boldsymbol u}}+\sum_{j=1}^m \hat{\lambda}_j{\boldsymbol C}_j\hat{{\boldsymbol u}} = \bar{{\boldsymbol h}}(\hat{{\boldsymbol u}}). \tag{44}\end{equation} 此外, 对(40)式取极限, 有对$\forall~j=1,\ldots,m$, \begin{equation}\lim_{k\rightarrow \infty} (\hat{{\boldsymbol u}}^{k})'{\boldsymbol C}_j\bar{{\boldsymbol u}}^{k}=\hat{{\boldsymbol u}}'{\boldsymbol C}_j\hat{{\boldsymbol u}}=0. \tag{45}\end{equation}

对比原问题(19)全局最优解的充分性条件(28)$\sim$(30), 还需验证$\bar{{\boldsymbol~W}}(\hat{{\boldsymbol~u}})+\sum_{j=1}^m~\hat{\lambda}_j{\boldsymbol~C}_j~\succeq~\bf~0.$ 根据定理3.1, 算法alg1中的$\bar{{\boldsymbol~u}}^{k}$需要满足 \begin{equation}({\boldsymbol P}^k\bar{{\boldsymbol W}}^k{\boldsymbol P}^k) \bar{{\boldsymbol u}}^{k}= \bar{{\boldsymbol h}}^k, \tag{46}\end{equation} 其中${\boldsymbol~P}^k={\boldsymbol~I}-{{\boldsymbol~A}^k}^\dagger{\boldsymbol~A}^k$, ${\boldsymbol~A}^k=[(\hat{{\boldsymbol~u}}^k)'{\boldsymbol~C}_1;~\ldots;~(\hat{{\boldsymbol~u}}^k)'{\boldsymbol~C}_m].$ 显然矩阵${\boldsymbol~A}^k$是行满秩的, 因此 \begin{equation}\lim_{k\rightarrow \infty} {{\boldsymbol A}^k}^\dagger = {{\boldsymbol A}}(\hat{{\boldsymbol u}})^\dagger, \tag{47}\end{equation} 其中${\boldsymbol~A}(\hat{{\boldsymbol~u}})=[\hat{{\boldsymbol~u}}'{\boldsymbol~C}_1;~\ldots;~\hat{{\boldsymbol~u}}'{\boldsymbol~C}_m].$ 对方程(46)两边取极限, 可得 \begin{equation}\big({\boldsymbol P} (\hat{{\boldsymbol u}}) \bar{{\boldsymbol W}}(\hat{{\boldsymbol u}}){\boldsymbol P}(\hat{{\boldsymbol u}})\big) \hat{{\boldsymbol u}}= \bar{{\boldsymbol h}}(\hat{{\boldsymbol u}}), \tag{48}\end{equation} 其中${\boldsymbol~P}~(\hat{{\boldsymbol~u}})={\boldsymbol~I}-{{\boldsymbol~A}(\hat{{\boldsymbol~u}})}^\dagger{\boldsymbol~A}(\hat{{\boldsymbol~u}}).$ 对比(44)和(48)式, 最终可得 \begin{equation}\bar{{\boldsymbol W}}(\hat{{\boldsymbol u}})+\sum_{j=1}^m \hat{\lambda}_j{\boldsymbol C}_j={\boldsymbol P} (\hat{{\boldsymbol u}}) \bar{{\boldsymbol W}}(\hat{{\boldsymbol u}}){\boldsymbol P}(\hat{{\boldsymbol u}}) \succeq \bf 0. \tag{49}\end{equation} 最终, 从式(44), (45)和(49)可以看出, $(\hat{{\boldsymbol~u}},\hat{\lambda}_1,\ldots,\hat{\lambda}_m~)$ 满足优化问题(19)全局最优解的充分性条件(28)$\sim$(30). 因此, 由算法alg1得到的近似解序列的极限一定是原问题(19)的全局最优解.

Boyd S, Vandenberghe L. Convex Optimization. Cambridge: Cambridge University Press, 2004.


References

[1] Zeng Y, Zhang R, Lim T J. Wireless communications with unmanned aerial vehicles: opportunities and challenges. IEEE Commun Mag, 2016, 54: 36-42 CrossRef Google Scholar

[2] Zhu S Q, Wang D W, Low C B. Cooperative control of multiple UAVs for moving source seeking. In: Proceedings of IEEE International Conference on Unmanned Aircraft Systems, New York, 2013. 193--202. Google Scholar

[3] Kwon H, Pack D J. A Robust Mobile Target Localization Method for Cooperative Unmanned Aerial Vehicles Using Sensor Fusion Quality. J Intell Robot Syst, 2012, 65: 479-493 CrossRef Google Scholar

[4] Khelifi F, Bradai A, Singh K. Localization and Energy-Efficient Data Routing for Unmanned Aerial Vehicles: Fuzzy-Logic-Based Approach. IEEE Commun Mag, 2018, 56: 129-133 CrossRef Google Scholar

[5] Zhang J, Yuan H. Analysis of unmanned aerial vehicle navigation and height control system based on GPS. J Syst Eng Electron, 2010, 21: 643-649 CrossRef Google Scholar

[6] Seifeldin M, Saeed A, Kosba A E. Nuzzer: A Large-Scale Device-Free Passive Localization System for Wireless Environments. IEEE Trans Mobile Comput, 2013, 12: 1321-1334 CrossRef Google Scholar

[7] Kehu Yang , Gang Wang , Zhi-Quan Luo . Efficient Convex Relaxation Methods for Robust Target Localization by a Sensor Network Using Time Differences of Arrivals. IEEE Trans Signal Process, 2009, 57: 2775-2784 CrossRef ADS Google Scholar

[8] Lu L, Wu H C, Chang S Y. New direction-of-arrival-based source localization algorithm for wideband signals. IEEE Trans Wirel Commun, 2012, 11: 3850--3859. Google Scholar

[9] Yeredor A, Angel E. Joint TDOA and FDOA Estimation: A Conditional Bound and Its Use for Optimally Weighted Localization. IEEE Trans Signal Process, 2011, 59: 1612-1623 CrossRef ADS Google Scholar

[10] Smith J, Abel J. Closed-form least-squares source location estimation from range-difference measurements. IEEE Trans Acoust Speech Signal Process, 1987, 35: 1661-1669 CrossRef Google Scholar

[11] Chan Y T, Ho K C. A simple and efficient estimator for hyperbolic location. IEEE Trans Signal Process, 1994, 42: 1905-1915 CrossRef ADS Google Scholar

[12] Stoica P, Li J. Lecture Notes - Source Localization from Range-Difference Measurements. IEEE Signal Process Mag, 2006, 23: 63-66 CrossRef ADS Google Scholar

[13] Ho K C, Lu X, Kovavisaruch L. Source Localization Using TDOA and FDOA Measurements in the Presence of Receiver Location Errors: Analysis and Solution. IEEE Trans Signal Process, 2007, 55: 684-696 CrossRef ADS Google Scholar

[14] Beck A, Stoica P, Jian Li P. Exact and Approximate Solutions of Source Localization Problems. IEEE Trans Signal Process, 2008, 56: 1770-1778 CrossRef ADS Google Scholar

[15] Lui K, Chan F, So H C. Semidefinite Programming Approach for Range-Difference Based Source Localization. IEEE Trans Signal Process, 2009, 57: 1630-1633 CrossRef ADS Google Scholar

[16] Picard J S, Weiss A J. Time difference localization in the presence of outliers. Signal Processing, 2012, 92: 2432-2443 CrossRef Google Scholar

[17] Yu H, Huang G, Gao J. An Efficient Constrained Weighted Least Squares Algorithm for Moving Source Location Using TDOA and FDOA Measurements. IEEE Trans Wireless Commun, 2012, 11: 44-47 CrossRef Google Scholar

[18] Lin L, So H C, Chan F K W. A new constrained weighted least squares algorithm for TDOA-based localization. Signal Processing, 2013, 93: 2872-2878 CrossRef Google Scholar

[19] Qu X M, Xie L H. Source localization by TDOA with random sensor position errors -- part I: static sensors. In: Proceedings of the 15th IEEE International Conference on Information Fusion, Singapore, 2012, 48--53. Google Scholar

[20] Qu X, Xie L. An efficient convex constrained weighted least squares source localization algorithm based on TDOA measurements. Signal Processing, 2016, 119: 142-152 CrossRef Google Scholar

[21] Liu M, Quan T F, Yao T B, et al. Multi-sensor multi-target passive locating and tracking. Acta Electron Sin, 2006, 34: 991--997. Google Scholar

[22] Le Yang , Ho K C. An Approximately Efficient TDOA Localization Algorithm in Closed-Form for Locating Multiple Disjoint Sources With Erroneous Sensor Positions. IEEE Trans Signal Process, 2009, 57: 4598-4615 CrossRef ADS Google Scholar

[23] Ming Sun , Ho K C. An Asymptotically Efficient Estimator for TDOA and FDOA Positioning of Multiple Disjoint Sources in the Presence of Sensor Location Uncertainties. IEEE Trans Signal Process, 2011, 59: 3434-3440 CrossRef ADS Google Scholar

[24] Xiu J J, Wang W S, Sun P. Multiple target passive location of TDOA based on bidirectional elect and nearest neighbor method. J Astronautics, 2015, 36: 483--488. Google Scholar

[25] Sundar H, Sreenivas T V, Seelamantula C S. TDOA-Based Multiple Acoustic Source Localization Without Association Ambiguity. IEEE/ACM Trans Audio Speech Lang Process, 2018, 26: 1976-1990 CrossRef Google Scholar

[26] Luo Z, Ma W, So A. Semidefinite Relaxation of Quadratic Optimization Problems. IEEE Signal Process Mag, 2010, 27: 20-34 CrossRef ADS Google Scholar

  • Figure 1

    (Color online) Comparison of the localization accuracy in scenario 1 when fixing $\sigma_s$ and varying $\sigma_j$

  • Figure 2

    (Color online) Comparison of the localization accuracy in scenario 1 when fixing $\sigma_j$ and varying $\sigma_s$

  • Figure 3

    (Color online) Comparison of the localization accuracy of source 1 in scenario 2

  • Figure 4

    (Color online) Comparison of the localization accuracy of source 2 in scenario 2

  • Table 1   True positions of UAVs in scenario 1 (m)
    UAV number $i$ $x_i^0$ $y_i^0$ $z_i^0$
    1 300 100 150
    2 400 150 100
    3 300 500 200
    4 350 200 100
    5 $-$100 $-$100 $-$100
    6 200 $-$300 $-$200
  •   

    Algorithm 1 Recursive CWLS multi-source localization algorithm

    Input${\boldsymbol~s}$, $r_{j,i1}~(j=1,\ldots,m,~i=1,\ldots,n)$, ${\boldsymbol~Q}_s$, ${\boldsymbol~Q}_t$;

    Algorithm process:

    Initialize: $k=0$, $\hat{{ u}}^k=({ G}'{G})^{-1}{G}'({ h}-{G}\bar{{ s}}_1)$;

    Update: update $\bar{{\boldsymbol~W}}^k=\bar{{\boldsymbol~W}}(\hat{{\boldsymbol~u}}^k)$ and $\bar{{\boldsymbol~h}}^k=\bar{{\boldsymbol~h}}(\hat{{\boldsymbol~u}}^k)$ following (20) and (21), and then update ${\boldsymbol~P}^k={\boldsymbol~P}(\hat{{\boldsymbol~u}}^k)$ following (24) and (25);

    Calculate: $\bar{{\boldsymbol~u}}^k=({\boldsymbol~P}^k\bar{{\boldsymbol~W}}^k{\boldsymbol~P}^k)^\dagger\bar{{\boldsymbol~h}}^k$;

    $k=k+1$;

    Update: $\hat{{\boldsymbol~u}}^k=(\hat{{\boldsymbol~u}}^{k-1}+\bar{{\boldsymbol~u}}^{k-1})/2$;

    while $\frac{\|\hat{{\boldsymbol~u}}^{k}-\hat{{\boldsymbol~u}}^{k-1}\|}{\|\hat{{\boldsymbol~u}}^{k}\|}~\leq~\delta$ do

    Update: $\bar{{\boldsymbol~W}}^k=\bar{{\boldsymbol~W}}(\hat{{\boldsymbol~u}}^k)$, $\bar{{\boldsymbol~h}}^k=\bar{{\boldsymbol~h}}(\hat{{\boldsymbol~u}}^k)$, ${\boldsymbol~P}^k={\boldsymbol~P}(\hat{{\boldsymbol~u}}^k)$;

    Calculate: $\bar{{\boldsymbol~u}}^k=({\boldsymbol~P}^k\bar{{\boldsymbol~W}}^k{\boldsymbol~P}^k)^\dagger\bar{{\boldsymbol~h}}^k$;

    $k=k+1$;

    Update: $\hat{{\boldsymbol~u}}^k=(\hat{{\boldsymbol~u}}^{k-1}+\bar{{\boldsymbol~u}}^{k-1})/2$;

    end while

    Output:$\hat{{ u}}^k$, $k.$
  • Table 2   The true positions of UAVs in scenario 2 (m)
    UAV number $i$ $x_i^0$ $y_i^0$ $z_i^0$
    1 510 $-$480 30
    2 510 520 30
    3 $-$490 $-$480 30
    4 $-$490 520 30
    5 10 20 $30+500\sqrt{2}$