基于TDOA/GROA的多站连续定位模型

摘要

本发明属于多传感器无源定位技术，针对固定辐射源目标，提供一种基于TDOA/GROA的多站连续定位模型。考虑利用中间变量的传统定位模型对定位场景的局限，从TDOA和GROA的量测方程出发，得到了只与目标状态向量有关的定位模型，从而避免了引入中间变量。然后根据量测模型推导了定位模型的误差项，最后利用约束加权最小二乘算法计算目标状态估计。该定位模型适用于多运动站场景下的固定辐射源目标连续定位。

说明书

技术领域

本发明属于多传感器无源定位技术，涉及利用到达增益比等量测信息对辐射源目标的定位问题，提供了一种基于到达增益比的多站连续定位模型。

背景技术

无源定位技术一直是信号处理领域的重要研究课题，具有隐蔽性好、探测距离较远、适用性强等优点，在雷达，无线传感器网络等领域受到国内外学者的广泛关注。目前主要使用的量测信息有方位角、到达时差(TDOA)、到达频差以及到达增益比(GROA)等，或以上量测组合得到的多种联合定位系统。

传统的基于方位角的辐射源定位方法，需要在不同的角度对同一信号进行检测，并且容易因为坐标系不同和观测站姿态引入新的误差项。而基于到达时差和到达增益比量测的无源定位技术无需测量方位角、俯仰角等信息，因此不会受到坐标系转换和传感器载体姿态误差的影响，具有稳定性强、精度高等优点。目前的时差定位算法中，多通过引入额外的中间变量(辐射源到参考观测站的径向距离)建立线性时差方程。在此定位模型基础上，利用两步加权最小二乘(TS-WLS)算法、约束总体最小二乘(CTLS)算法、束加权最小二乘(CWLS)算法等进一步提高了定位精度。另一方面，由于辐射源信号会随着传播而产生能量衰减，将达到增益比量测信息加入到TDOA体制中，构造TDOA/GROA联合定位体制，并利用GROA量测对中间变量的修正，改善对辐射源的定位精度。

以上都是使用的基于中间变量的定位模型的定位算法，对于多运动站的情况下，由于观测站不断运动，中间变量即辐射源到参考观测站的径向距离也在不断变化，使得上述定位模型及其算法无法直接推广到多运动站连续定位，需要建立新的定位模型。

发明内容

本发明的目的在于针对固定辐射源目标，提供一种基于TDOA/GROA的多站连续定位模型，并利用CWLS算法求解辐射源位置信息。考虑到中间变量造成的局限性，从TDOA和GROA的量测方程出发，得到了只与目标状态向量有关的定位模型，从而避免了引入中间变量。然后根据量测模型推导了定位模型的误差项，得到了一种基于CWLS的TDOA/GROA联合定位算法。该算法适用于多运动站场景下的固定辐射源目标连续定位，其方法流程如图1所示，包括以下步骤：

1问题描述

假设定位场景中有M个运动观测站接收固定辐射源的信号，通过测量两个不同站之间的达到时间差和到达增益比来估计辐射源位置。N维空间时，由文献(15]可知得到闭式解的条件是M≥N+2。本文以2维空间为研究背景，所采用的方法可推广到3维空间中。记k时刻未知辐射源的位置向量为s₀＝[x₀(k)y₀(k)]^T，第i个观测站位置向量为s_i＝[x_i(k)y_i(k)]^T(i＝1,2,…,M)。以第1个观测站为参考站，则真实时差量测为

$>{\bar{t}}_{i1}=\frac{{\bar{r}}_{i1}}{c},i=1,2,...,M---\left(1\right)$>

式中：为真实距离差量测，r_i＝|s_i-s₀|(i＝1,2,…,M)表示第i个观测站到辐射源目标的径向距离，c为电磁波传播速度。典型定位场景示意图如图2所示。

根据信号传播理论，信号的传播损耗与信号源和传感器之间距离的n次方成正比，在自由空间中，不考虑多径效应时，可将损耗因子n设定为常数2。同样以第1个观测站为参考站，则真实增益比量测为

$>{\bar{g}}_{i1}={\left(\frac{r_i}{r_1}\right)}^2=\frac{{\left|s_i-s_0\right|}^2}{{\left|s_1-s_0\right|}^2},i=2,3,...,M---\left(2\right)$>

2 TDOA/GROA定位模型

(1)TODA定位模型

基于TDOA量测方程，为表示方便，省略时间标签，该模型的向量形式为

$>\begin{array}{c}\frac{1}{t_{21}}({\left|s_2\right|}^2-{\left|s_1\right|}^2)-\frac{1}{t_{(i+2)1}}({\left|s_{(i+2)}\right|}^2-{\left|s_1\right|}^2)+c^2{t}_{(i+2)2}=\\ [\frac{2}{t_{21}}(s_2^T-s_1^T)-\frac{2}{t_{(i+2)1}}(s_{(i+2)}^T-s_1^T)]s_0\\ (i=1,2,...,M-2)\end{array}---\left(3\right)$>

对式(3)做变换，可得无测量误差时的TDOA线性方程为

$>{\bar{A}}_t\left(k\right)x={\bar{b}}_t\left(k\right)---\left(4\right)$>

式中：

$>\left(\begin{array}{c}{\bar{A}}_t\left(k\right)={[{\alpha }_{t,1}^T\left(k\right),{\alpha }_{t,2}^T\left(k\right),...,{\alpha }_{t,M-2}^T\left(k\right)]}^T\\ {\alpha }_{t,i}\left(k\right)=\left[\begin{array}{cc}2{\bar{r}}_{(i+2)1}\left(k\right)x_{21}\left(k\right)-2r_{21}\left(k\right)x_{(i+2)1}\left(k\right)& 2{\bar{r}}_{(i+2)1}\left(k\right)y_{21}\left(k\right)-2{\bar{r}}_{21}\left(k\right)y_{(i+2)1}\left(k\right)\end{array}\right]\\ {\bar{b}}_t\left(k\right)={[{\beta }_{t,1}\left(k\right),{\beta }_{t,2}\left(k\right),...,{\beta }_{t,(M-2)}\left(k\right)]}^T\\ {\beta }_{t,i}\left(k\right)={\bar{r}}_{(i+2)1}\left(k\right)\left(d_2^2\right(k)-d_1^2(k\left)\right)-{\bar{r}}_{21}\left(k\right)\left(d_{i+2}^2\right(k)-d_1^2(k\left)\right)+{\left({\bar{r}}_{(i+2)1}\right(k\left)\right)}^2{\bar{r}}_{21}\left(k\right)-{\left({\bar{r}}_{21}\right(k\left)\right)}^2{\bar{r}}_{(i+2)1}\left(k\right)\\ x_{ij}\left(k\right)=x_i\left(k\right)-x_j\left(k\right),y_{ij}\left(k\right)=y_i\left(k\right)-y_j\left(k\right),(i,j=1,2,...,M;i\ne j)\end{array}\right)---\left(5\right)$>

实际到达距离差(RDOA)测量值中含有测量噪声，则实际RDOA量测模型为

$>r_{i1}\left(k\right)={\bar{r}}_{i1}\left(k\right)+{\mathrm{\delta r}}_{i1}\left(k\right),i=2,3,...,M---\left(6\right)$>

式中：δg_i1为噪声引起的RDOA测量误差，服从零均值的高斯分布。记到RDOA误差向量为δr(k)＝[δr₂₁(k),δr₃₁(k),…,δr_M1(k)]^T，其量测噪声的协方差矩阵为

$>E\left[\delta r\left(k\right)\delta r{\left(k\right)}^T\right]=R_{rr}=c^2{▿}_{rr}^{-1}---\left(7\right)$>

$>{▿}_{rr}=\frac{T}{3\pi }{\omega }^3\frac{{\rho }^2}{1+M\rho }({\mathrm{MI}}_{M-1}-1_{(M-1)\times (M-1)})---\left(8\right)$>

式中：T为观测时间，ω为信号带宽，ρ＝P_signal/P_noise为信号的信噪比，P_signal和P_noise分别为信号和噪声的功率谱密度函数，I_M-1为M-1阶单位矩阵，1_(M-1)×(M-1)为M-1阶全1矩阵。

(2)GRDA定位模型

注意到对式(2)进行等式变换，省略时间标签可得

$>\frac{g_{i1}{\left|s_1\right|}^2-{\left|s_i\right|}^2-2(g_{i1}{s}_1^T-s_i^T)s_0}{g_{i1}-1}=-{\left|s_0\right|}^2,i=2,3,...,M---\left(9\right)$>

令i＝2，上式可写作

$>\frac{g_{21}{\left|s_1\right|}^2-{\left|s_2\right|}^2-2(g_{21}{s}_1^T-s_2^T)s_0}{g_{21}-1}=-{\left|s_0\right|}^2---\left(10\right)$>

式(9)和式(10)中均含有未知项-|s₀|²，两两相减消去，整理后得到基于GROA定位模型的向量形式为

$>\begin{array}{c}(g_{i1}-1)(g_{21}{\left|s_1\right|}^2-{\left|s_2\right|}^2)-(g_{21}-1)(g_{i1}{\left|s_1\right|}^2-{\left|s_i\right|}^2)=\\ [2(g_{i1}-1)(g_{21}{s}_1^T-s_2^T)-2(g_{21}-1)(g_{i1}{s}_1^T-s_i^T)]s_0\\ (i=3,4,...,M)\end{array}---\left(11\right)$>

这样我们就得到了无测量误差时关于辐射源目标状态的GROA线性方程

$>{\bar{A}}_g\left(k\right)x={\bar{b}}_g\left(k\right)---\left(12\right)$>

式中：

$>\left(\begin{array}{c}{\bar{A}}_g\left(k\right)={[{\bar{\alpha }}_{g,1}^T\left(k\right),{\bar{\alpha }}_{g,2}^T\left(k\right),...,{\bar{\alpha }}_{g,(M-2)}^T\left(k\right)]}^T\\ {\bar{\alpha }}_{g,i}\left(k\right)={\left(\begin{array}{c}2\left({\bar{g}}_{(i+2)1}\right(k)-1)\left({\bar{g}}_{21}\right(k\left)x_1\right(k)-x_2(k\left)\right)\\ 2\left({\bar{g}}_{(i+2)1}\right(k)-1)\left({\bar{g}}_{21}\right(k\left)y_1\right(k)-y_2(k\left)\right)\end{array}\right)}^T\\ {\bar{b}}_g\left(k\right)={[{\bar{\beta }}_{g,1}^T\left(k\right),{\bar{\beta }}_{g,2}^T\left(k\right),...,{\bar{\beta }}_{g,(M-2)}^T\left(k\right)]}^T\\ {\bar{\beta }}_{g,i}\left(k\right)=\left({\bar{g}}_{(i+2)1}\right(k)-1)\left({\bar{g}}_{21}\right(k\left)r_1^2\right(k)-r_2^2(k\left)\right)-\left({\bar{g}}_{21}\right(k)-1)\left({\bar{g}}_{(i+2)1}\right(k\left)r_1^2\right(k)-r_{i+2}^2(k\left)\right)\end{array}\right)---\left(13\right)$>

实际到达增益比测量值中含有测量噪声δg_i1(k)，则实际GROA量测模型为

$>g_{i1}\left(k\right)={\bar{g}}_{i1}\left(k\right)+{\mathrm{\delta g}}_{i1}\left(k\right),i=2,3,...,M---\left(14\right)$>

式中：δg_i1为噪声引起的到达增益比测量误差，服从零均值的高斯分布。记到达增益比误差向量为δg(k)＝[δg₂₁(k),δg₃₁(k),…,δg_M1(k)]^T，其协方差矩阵为

$>E\left[\delta g\left(k\right)\delta g{\left(k\right)}^T\right]=R_{gg}={▿}_{gg}^{-1}---\left(15\right)$>

$>{▿}_{gg}=\frac{T}{\pi }\omega \frac{{\rho }^2}{1+M\rho }({\mathrm{MI}}_{M-1}+\frac{1-M\rho }{1+M\rho }{1}_{(M-1)\times (M-1)})---\left(16\right)$>

原有需要中间变量的GROA定位模型通过联合TDOA和GROA量测得到r_i1＝(g_i1-1)r₁，可知原定位模型通过加入GROA量测信息提高对中间变量r₁的估计精度，进而提高算法性能，并没有通过GROA量测直接改善目标定位精度。由式(3)和式(11)可知所提无需中间变量的GROA定位模型能够直接估计目标状态，并避免引入中间变量，适用于多运动站的无源定位。

综上所述，通过引入GROA定位模型，构建TDOA/GROA联合定位模型，可以充分利用GROA中包含的目标位置信息，提高算法性能，并且保持了无需中间变量的优点，适用于多运动站对固定目标的连续定位。其联合定位模型为

$>\bar{A}\left(k\right)x=\bar{b}\left(k\right)---\left(17\right)$>

式中：

3基于CWLS的定位算法

将式(6)和式(14)带入到和中，忽略高阶误差项后，整理可得

$>\left(\begin{array}{c}A\left(k\right)=\bar{A}\left(k\right)+\Delta A\left(k\right)\\ b\left(k\right)=\bar{b}\left(k\right)+\Delta b\left(k\right)\end{array}\right)---\left(18\right)$>

式中：A(k)和b(k)分别是带入实际测量值的观测矩阵和观测向量，且△A(k)＝[-B₁(k)v(k)-B₂(k)v(k)]，△b(k)＝B₃(k)v(k)，忽略时间标签，误差项推导结果如式(19)所示。

$>\left(\begin{array}{c}{B}_1=blkdiag\left(\right[\begin{array}{cc}{b}_1& -2x_{21}{I}_{M-2}\end{array}],[\begin{array}{cc}{b}_1& -2x_{21}{I}_{M-2}\end{array}\left]\right),b_1={[2x_{31},2x_{41},...,2x_{M1}]}^T\\ B_2=blkdiag\left(\right[\begin{array}{cc}{b}_2& -2y_{21}{I}_{M-2}\end{array}],[\begin{array}{cc}-b_1& 2y_{21}{I}_{M-2}\end{array}\left]\right),b_2={[2y_{31},2y_{41},...,2y_{M1}]}^T\\ B_3=blkdiag\left(\right[b_{t1},b_{t2}],[b_g,\left(r_1^2-r_2^2\right)I_{M-2}\left]\right)\\ b_{t1}={\left(\begin{array}{cccc}{d}_1^2-d_3^2+{\bar{r}}_{31}^2-2{\bar{r}}_{21}{\bar{r}}_{31}& d_1^2-d_4^2+{\bar{r}}_{41}^2-2{\bar{r}}_{21}{\bar{r}}_{41}& ...& d_1^2-d_M^2+{\bar{r}}_{M1}^2-2{\bar{r}}_{21}{\bar{r}}_{M1}\end{array}\right)}^T\\ b_{t2}=blkdiag\left(\right[\begin{array}{cccc}{d}_2^2-d_1^2-{\bar{r}}_{21}^2+2r_{31}{r}_{21}& d_2^2-d_1^2-{\bar{r}}_{21}^2+2r_{41}{r}_{21}& ...& d_2^2-d_1^2-{\bar{r}}_{21}^2+2r_{M1}{r}_{21}\end{array}\left]\right)\\ b_g={\left(\begin{array}{cccc}{d}_3^2-d_1^2& d_4^2-d_1^2& ...& d_M^2-d_1^2\end{array}\right)}^T\\ v={\left(\begin{array}{cccccccc}{\mathrm{\delta r}}_{21}& {\mathrm{\delta r}}_{31}& ...& {\mathrm{\delta r}}_{M1}& {\mathrm{\delta g}}_{21}& {\mathrm{\delta g}}_{31}& ...& {\mathrm{\delta g}}_{M1}\end{array}\right)}^T\end{array}\right)---\left(19\right)$>

式中：blkdaig(·)为分块对角矩阵。

将式(15)带入式(13)，整理后可得

b(k)-A(k)x＝△b(k)-△A(k)x＝C(k)v(k)(20)

式中：C(k)＝B₁(k)x₀+B₂(k)y₀+B₃(k)

定义加权矩阵

W(k)＝E(C(k)v(k)v^T(k)C^T(k))＝C(k)RC^T(k)(21)

式中协方差矩阵R＝E(v(k)v^T(k))＝blkdiag(R_rr,R_gg)。

定义增广矩阵

A_u＝[-A>

和增广解向量

X＝[x 1]^T(23)

式中：A＝[A^T(1)A^T(2)…A^T(k)]^T，b的定义与A类似。

易得

$>A_u={\bar{A}}_u+{\mathrm{\Delta A}}_u---\left(24\right)$>

$>\left(\begin{array}{c}{\mathrm{\Delta A}}_u=\left[\begin{array}{ccc}{B}_{1}v& B_{2}v& B_{3}v\end{array}\right]\\ v={\left(\begin{array}{cccc}{v}_1^T& v_2^T& ...& v_k^T\end{array}\right)}^T\\ B_i=blkdiag(B_{i1},B_{i2},...,B_{ik}),i=1,2,3\end{array}\right)---\left(25\right)$>

从而得到CWLS算法的最小化代价函数为

$>ϵ=X^T{A}_u^T{W}^{-1}{A}_{u}X---\left(26\right)$>

式中：ε的定义与A类似；W＝blkdiag(W(1),W(2),…W(k))。

带入式(24)并对ε求期望，可得平均代价函数为

$>E\left(ϵ\right)=X^T{\bar{A}}_u^T{W}^{-1}{\bar{A}}_{u}X+X^{T}E\left({\mathrm{\Delta A}}_u^T{W}^{-1}{A}_u\right)X---\left(27\right)$>

等式右边两项均为非负且与X相关。第一项为理想代价函数，当时，可取得最小值0。正是第二项的存在，当最小化E(ε)时，使解偏离真值从而产生偏差。因此，当约束第二项为常数时，E(ε)将在X取得真值时达到最小值。于是可构造CWLS问题为

$>\left(\begin{array}{cc}\underset{X}{\min }& X^T{A}_u^T{W}^{-1}{A}_{u}X\\ s.t.& X^T\Omega X=const\end{array}\right)---\left(28\right)$>

式中：const为任意非负常数。利用拉格朗日乘子法可以解上述约束最小化问题。记λ_min为$>(A_u^T{W}^{-1}{A}_u,\Omega )$>最小的广义特征值，λ_min对应的广义特征值向量为，则辐射源的状态估计为

$>\hat{x}=\hat{X}(1:2)/\hat{X}\left(3\right)---\left(29\right)$>

下面求解约束矩阵Ω，带入△A_u＝[B₁v>2v>3v]整理后可得

$>\Omega =\left(\begin{array}{ccc}{\Omega }_{11}& {\Omega }_{12}& {\Omega }_{13}\\ {\Omega }_{21}& {\Omega }_{22}& {\Omega }_{23}\\ {\Omega }_{31}& {\Omega }_{32}& {\Omega }_{33}\end{array}\right)---\left(30\right)$>

式中：

注意到随着时间的推进，矩阵维数快速增大，导致计算量剧增，可利用式(31)和式(32)避免高维矩阵直接相乘，从而减小计算量，也可以对该算法加窗处理，提高算法的实时性。

$>A_u^T{W}^{-1}{A}_u=\underset{i=1}{\overset{k}{\Sigma }}{A}_u^T\left(i\right)W^{-1}\left(i\right)A_u\left(i\right)---\left(31\right)$>

$>{\Omega }_{mn}=\underset{i=1}{\overset{k}{\Sigma }}tr\left(B_m^T\right(i\left)W^{-1}\right(i\left)B_n\right(i\left)R\right)---\left(32\right)$>

注意到加权矩阵W与未知的辐射源的状态有关，计算时首先将W设为单位矩阵，得到辐射源的状态估计后，重新计算W，进而得到最终的目标状态估计。计算时，用实际测量值代替W和约束矩阵中的真实距离差和真实达到增益比。

附图说明

图1：技术方案流程图；

图2：典型定位场景。

具体实施方式

以下结合说明书附图对本发明作进一步详细描述。参照说明书附图1，本发明中基于TDOA/GROA的多站连续定位方法分为以下几个步骤：

步骤1：令k＝1，初始化协方差矩阵R

R＝blkdiag(R_rr,R_gg)

式中R_rr和R_gg分别为RDOA量测和GROA量测的协方差矩阵。

步骤2：根据TDOA和GORA量测信息以及站址信息计算定位模型A_u(k)

A_u(k)＝[-A(k)b(k)]

$>\left(\begin{array}{c}A\left(k\right)={\left(\begin{array}{cc}{A}_t^T\left(k\right)& A_g^T\left(k\right)\end{array}\right)}^T\\ b\left(k\right)={\left(\begin{array}{cc}{b}_t^T\left(k\right)& b_g^T\left(k\right)\end{array}\right)}^T\end{array}\right)$>

$>\left(\begin{array}{c}{A}_t\left(k\right)={[{\alpha }_{t,1}^T\left(k\right),{\alpha }_{t,2}^T\left(k\right),...,{\alpha }_{t,M-2}^T\left(k\right)]}^T\\ {\alpha }_{t,i}\left(k\right)=\left[\begin{array}{cc}2r_{(i+2)1}\left(k\right)x_{21}\left(k\right)-2r_{21}\left(k\right)x_{(i+2)1}\left(k\right)& 2r_{(i+2)1}\left(k\right)y_{21}\left(k\right)-2r_{21}\left(k\right)y_{(i+2)1}\left(k\right)\end{array}\right]\\ b_t\left(k\right)={[{\beta }_{t,1}\left(k\right),{\beta }_{t,2}\left(k\right),...,{\beta }_{t,(M-2)}\left(k\right)]}^T\\ {\beta }_{t,i}\left(k\right)=r_{(i+2)1}\left(k\right)\left(d_2^2\right(k)-d_1^2(k\left)\right)-r_{21}\left(k\right)\left(d_{i+2}^2\right(k)-d_1^2(k\left)\right)+{\left(r_{(i+2)1}\right(k\left)\right)}^2{r}_{21}\left(k\right)-{\left(r_{21}\right(k\left)\right)}^2{r}_{(i+2)1}\left(k\right)\\ x_{ij}\left(k\right)=x_i\left(k\right)-x_j\left(k\right),y_{ij}\left(k\right)=y_i\left(k\right)-y_j\left(k\right),(i,j=1,2,...,M;i\ne j)\end{array}\right)$>

$>\left(\begin{array}{c}{A}_g\left(k\right)={[{\alpha }_{g,1}^T\left(k\right),{\alpha }_{g,2}^T\left(k\right),...,{\alpha }_{g,(M-2)}^T\left(k\right)]}^T\\ {\alpha }_{g,i}\left(k\right)={\left(\begin{array}{c}2\left(g_{(i+2)1}\right(k)-1)\left(g_{21}\right(k\left)x_1\right(k)-x_2(k\left)\right)\\ 2\left(g_{(i+2)1}\right(k)-1)\left(g_{21}\right(k\left)y_1\right(k)-y_2(k\left)\right)\end{array}\right)}^T\\ b_g\left(k\right)={[{\beta }_{g,1}^T\left(k\right),{\beta }_{g,2}^T\left(k\right),...,{\beta }_{g,(M-2)}^T\left(k\right)]}^T\\ {\beta }_{g,i}\left(k\right)=\left(g_{(i+2)1}\right(k)-1)\left(g_{21}\right(k\left)r_1^2\right(k)-r_2^2(k\left)\right)-\left(g_{21}\right(k)-1)\left(g_{(i+2)1}\right(k\left)r_1^2\right(k)-r_{i+2}^2(k\left)\right)\end{array}\right)$>

其中r_i1(k)和g_i1(k)分别为k时刻的RODA和GROA的实际测量值且i＝2,3,…,M，x_i和y_i为k时刻观测站位置且i＝1,2,…,M；

省略时间标签k，误差项B₁、B₂和B₃为

$>\left(\begin{array}{c}{B}_1=blkdiag\left(\right[\begin{array}{cc}{b}_1& -2x_{21}{I}_{M-2}\end{array}],[\begin{array}{cc}{b}_1& -2x_{21}{I}_{M-2}\end{array}\left]\right),b_1={[2x_{31},2x_{41},...,2x_{M1}]}^T\\ B_2=blkdiag\left(\right[\begin{array}{cc}{b}_2& -2y_{21}{I}_{M-2}\end{array}],[\begin{array}{cc}-b_2& 2y_{21}{I}_{M-2}\end{array}\left]\right),b_2={[2y_{31},2y_{41},...,2y_{M1}]}^T\\ B_3=blkdiag\left(\right[b_{t1},b_{t2}],[b_g,\left(d_1^2-d_2^2\right)I_{M-2}\left]\right)\\ b_{t1}={\left(\begin{array}{cccc}{d}_1^2-d_3^2+{\bar{r}}_{31}^2-2{\bar{r}}_{21}{\bar{r}}_{31}& d_1^2-d_4^2+{\bar{r}}_{41}^2-2{\bar{r}}_{21}{\bar{r}}_{41}& ...& d_1^2-d_M^2+{\bar{r}}_{M1}^2-2{\bar{r}}_{21}{\bar{r}}_{M1}\end{array}\right)}^T\\ b_{t2}=blkdiag\left(\right[\begin{array}{cccc}{d}_2^2-d_1^2-{\bar{r}}_{21}^2+2r_{31}{r}_{21}& d_2^2-d_1^2-{\bar{r}}_{21}^2+2r_{41}{r}_{21}& ...& d_2^2-d_1^2-{\bar{r}}_{21}^2+2r_{M1}{r}_{21}\end{array}\left]\right)\\ b_g={\left(\begin{array}{cccc}{d}_3^2-d_1^2& d_4^2-d_1^2& ...& d_M^2-d_1^2\end{array}\right)}^T\end{array}\right)$>

其中：blkdaig(·)为分块对角矩阵，I_M-2为M-2阶单位矩阵，且i＝1,2,…,M。

步骤3：若已经得到目标状态估计则加权矩阵W(k)为

$>\left(\begin{array}{c}W\left(k\right)=E\left(C\right(k\left)v\right(k\left)v^T\right(k\left)C^T\right(k\left)\right)=C\left(k\right){\mathrm{RC}}^T\left(k\right)\\ C\left(k\right)=B_1\left(k\right){\hat{x}}_0+B_2\left(k\right){\hat{y}}_0+B_3\left(k\right)\end{array}\right)$>

若目标状态估计未知，令加权矩阵W(k)为单位阵；

步骤4：计算和Ω

$>A_u^T{W}^{-1}{A}_u=\underset{i=1}{\overset{k}{\Sigma }}{A}_u^T\left(i\right)W^{-1}\left(i\right)A_u\left(i\right)$>

$>\Omega =\left(\begin{array}{ccc}{\Omega }_{11}& {\Omega }_{12}& {\Omega }_{13}\\ {\Omega }_{21}& {\Omega }_{22}& {\Omega }_{23}\\ {\Omega }_{31}& {\Omega }_{32}& {\Omega }_{33}\end{array}\right)$>

$>{\Omega }_{mn}=\underset{i=1}{\overset{k}{\Sigma }}tr\left(B_m^T\right(i\left)W^{-1}\right(i\left)B_n\right(i\left)R\right)$>

并通过对其广义特征值分解，得到最小广义特征值对应的广义特征值向量；

步骤5：更新目标状态估计

$>\hat{x}=\hat{X}(1:2)/\hat{X}\left(3\right)$>

令k＝k+1，并从步骤2继续运行。