3.2 定位估计算法

无线电辐射源定位算法涉及对定位跟踪模型选择何种最优准则构建相应的估计量，以及如何完成优化计算两方面问题。

3.2.1 定位静止目标或无状态方程表征的运动辐射源算法

3.2.1.1 估计与优化算法

通常采用将观测值全部集中起来进行批处理估计静止辐射源位置或集中全部 L 个观测站的瞬时观测值估计无状态方程表征的运动辐射源位置与速度，为此将式（3-3）改写为

式中， z = ， h （ x ， x _O ）=[ h ^T （ x ， x _O，1 ）， h ^T （ x ， x _O，2 ），…， h ^T （ x ， x _{O， K} ）] ^T ， h （ x ， x _O ）可简写为 h （ x ）， x _O = ，ξ= ，且假定 = 0 ，cov{ξ}=Σ _ξ 为正定矩阵。

对式（3-5）表示的辐射源位置 x ，既可以用直接给出其值 的点估计方法，也可以用以指定概率的某个区域包含 x 的区间估计方法，对于区间估计参见2.3.4节或文献 ^[2] 。本书后面只讨论点估计。

若无 x 的先验分布信息，也没有ξ的概率分布信息，则可以用广义最小二乘 ^[3] 估计式（3-5）中的 x ：

式中， Ω _x 为包含辐射源全部可能位置的集合，为书写简便可将 简写为 。

若式（3-5）中 h （ x ， x _O ）近似为 x 线性方程 h （ x ， x _O ）≈ H _x x ，则式（3-6）简化为

若Σ _ξ = ，则式（3-7）变为

若有 x 的先验概率分布密度函数信息，则可以用最大后验概率（MAP）估计 x ，该方法又称广义极大似然估计 ^[4] ，可以表示为

式中， p _X|Z （ x | z ）为后验概率密度函数， p _Z|X （ z | x ）为似然概率密度函数， p _X （ x ）为先验概率密度函数。

如果 x 的先验概率密度函数 p _X （ x ）是均匀分布，则式（3-9）转化为极大似然估计

进一步，若ξ～ ，则式（3-10）简化为

一般情况下，如果 x 的先验分布是均匀的，那么 是对 x 的一种较好估计。大样本理论表明，当 z ^（1） ， z ^（2） ， … ， z ^{（ K ）} 相互独立，且与 z 同分布，在满足某些正则条件 ^[3，4] 时，有 （ z ^（1） ， z ^（2） ， … ， z ^{（ K ）} ）是 x 的相合估计，即 =1，且 是 x 的最优渐近正态（BAN）估计，即 表示依分布收敛），其中 I （ x ）是基于观测量 z 的关于 x 信息矩阵，这也是某些情况下假定极大似然估计的误差服从高斯分布的原因之一。当式（3-5）中 ξ ～ 时， I ^-1 （ x ）= ，即当 K 很大时，近似有 。

当 ξ ～ 时，式（3-11）中极大似然估计 与式（3-6）中广义最小二乘估计 的表达式一致，因此 也是 x 的 BAN 估计， 具有与 相同的统计特点，这是常用广义最小二乘估计的原因之一。但需要注意的是，在 ξ 服从非高斯分布时未必有 ，特别地，广义最小二乘估计式（3-6）并无必要知道概率密度函数，同理，极大似然估计未必需要有先验分布。

式（3-7）和式（3-8）是近似估计的解析表达式，可以直接计算，某些情况下，通过对式（3-5）作数学变换，也可以用解析法求解，从而避免式（3-6）中非线性函数优化问题，参见第5章和第7章。

避免式（3-6）中非线性函数优化问题的一般方法是对式（3-5）中的非线性函数 h （ x ）作线性近似，仿式（3-7）得到近似解的解析表达式。下面简单介绍一下Gauss-Newton迭代法（简称G-N法）。

假设 x ₀ 是 x 附近的点，对式（3-5）作线性近似，有

式中， J = 。

因此有

式中， y = z-h （ x ₀ ）+ Jx ₀ 。

对式（3-13）中 x 的广义最小二乘估计为

将 作为 x ₀ ，重复式（3-12）、式（3-13）和式（3-14）进行迭代计算，当满足迭代停止条件时给出优化解。文献 ^[3] 基于最大固有曲率、最大参数效应曲率与标准相对曲率之间的大小关系，给出了适合使用G-N法求解的非线性方程情况。针对文献 ^[3] 中G-N法的不足，改进的G-N迭代法可以保证计算结果收敛，避免迭代过程波动，并能提高计算速度 ^[5，6] 。

除式（3-14）线性化近似迭代算法外，还有一些算法可用于式（3-9）、式（3-10）和式（3-11）优化计算 ^[7，8] ，这里简要介绍网格搜索方法。

采用网格搜索法计算时，可用适当密度的网格覆盖取值范围 Ω _x ，将处于 Ω _x 内的所有网格点逐一作为 x 代入式（3-9）、式（3-10）和式（3-11）取极值括号内的表达式中，为了便于图形显示，通常利用目标函数 C （ x ）进行绘制：

根据算法准则取 C （ x ）最大值对应的网格点作为 x 的初始估计，通过缩小搜索范围，增加网格密度再次重复上述过程，得到 x 的最终估计。网格搜索法的计算量虽然较大，但是适用于任何函数，并且对于地面上的辐射源定位，通过选取 C （ x ）值与对应网格点关联绘制曲面图，有助于直观了解定位情况（如例5.1所示）。另外，网格搜索法得到的 x 初始估计可以作为G-N法的初始值用于迭代求解。对于空中辐射源或时效性要求高的辐射源定位，建议采用其他定位估计算法如G-N法、解析法等。

当 h （ x ）隐含未知参数 γ 时，直接用网格搜索法计算量可能很大，这时需要根据 h （ x ）的具体情况进行等价数学变换消除 γ 的影响（参见第7、第9章）。必要时，也可以考虑对观测方程作变换，构建不受 γ 影响的新方程，如参数型定位方程，再用新方程求 x ，即两步定位。

3.2.1.2 定位误差几何表示

由式（3-14）可以得到定位误差协方差矩阵：

特别地，取 x ₀ = x _T ， x _T 是辐射源的真实位置，那么式（3-16）右边也是当 ξ ～ 时基于式（3-5）对 x _T 所有无偏估计的CRLB，即 I ^-1 （ x _T ）。

从几何角度 ^[2] 分析 ，有助于对 、 、ECM、CRLB及MSE的进一步理解。以下为了叙述简便，考虑对平面上位于 x _T 的静止辐射源定位，测量次数 K =1，一次测量两个参数，省略下标 k ，记

由式（2-41），有 J ^T =[∇ h ₁ ，∇ h ₂ ]，因此有

下面结合图3-1来说明式（3-18）中各个量的几何意义。

在图3-1中，无观测站误差和噪声时，两条定位线相交于 x _T ，其方程为

当存在观测噪声 ξ 时，记 = 为 x _T 的广义最小二乘估计，那么测量方程可以表示为

当| ξ _l |很小时， 在 x _T 的附近，将式（3-20）后一部分在 x _T 处线性展开，有

式中，Δ x _T = -x _T ， α _l = o （‖Δ x _T ‖），即 =0，记 α =[ α ₁ ， α ₂ ] ^T ，将式（3-21）与式（3-20）第一个等式比较，有

记 b =-（∇ ^T h ） ^-1 α ， ，则 =Δ x _T -b ，即

将式（3-23）代入式（3-22），有

式（3-24）表明Δ x 在∇ h _l 方向上的投影为

n _l 的长度 n _l 为

由式（3-25）和式（3-26）知，如果 ξ _l ≥0，那么Δ x 在∇ h _l 上的投影 n _l 的长度为 ，方向与∇ h _l 相同，如图3-1中 n ₁ 所示方向；否则，Δ x 在∇ h _l 上的投影 n _l 的长度为 ，方向与∇ h _l 相反，如图3-1中 n ₂ 所示方向，基于 A 、 D 、 C 、 G 四点共圆，以及四边形 ADCG 的边 DA 和边 DC 的长度分别为 n ₁ 和 n ₂ ，∠ ADC = φ ， φ 是∇ h ₁ 与-∇ h ₂ 的夹角，已知对角线 DG 的长度平方 为

将式（3-26）代入式（3-27），并将 φ 用∇ h ₁ 和∇ h ₂ 表示，除 ξ _l 为正数与负数时对 n _l 和 φ 的几何解释不同外，式（3-27）可以统一表示为

故 与式（3-18）一致，可表示为

特别地，若 =0，则式（3-29）可简化为

由于 φ 与∇ h _l 有关，因此不同体制定位误差 最小值所对应的梯度矢量夹角 φ 不同，具体可以参见4.2.1节和5.2.1节。需要特别指出的是， =0不是必然的，如多站TDOA定位中同时估计其他站与中心站之间的到达时差时，其时差测量误差之间是相关的。

由式（3-24）可知， = 0 ，故 是 x 的无偏估计， 。由图3-1可以看到，将 h ₁ 和 h ₂ 在 x _T 的附近看成直线，那么 变为 ，有 = ≈ = 。附录 B 表明，在 ξ～ 的情况下，如果近似到一阶，极大似然估计 的 =tr[CRLB（ x _T ）]，因此，式（3-29）也可以被视为高斯分布下tr[CRLB（ x _T ）]的几何表示。此外，由式（3-23）可知， =-（∇ ^T h ） ^-1 ，通常 h 为 x 的非线性函数， ≠ 0 ，故 为 x _T 的有偏估计，参见文献 ^[3] 和文献 ^[9] 。考虑高阶项时，极大似然估计 的 一般大于tr[CRLB（ x _T ）] ^[3] 。

后面涉及利用极大似然估计方法进行位置估计时，均假定 ξ～ ，并近似到一阶，用 =tr[CRLB（ x _T ）]表示，具体情况可见第4～第8章中误差分析。

对于用适定观测方程定位空中辐射源的MSE几何表示见附录C。

需要指出的是，式（3-16）考虑的是没有观测站状态误差的定位误差协方差矩阵，对于运动观测站等场景，通常还需考虑观测站位置或速度误差的影响。此时，构建观测方程

式中， q 、 p （ x _O ）和 ε 分别为对观测站状态的测量值、测量函数和测量误差。假设 x ₀ 是 x 附近的点， x _O，0 是 x _O 附近的点，对式（3-31）作线性近似，有

式中， J = ； J 2= ； J 3= ，最简单情况下 p （ x _O ）= x _O ，那么 J ₃ = ，其中dim（ x _O ）是 x _O 的维数。

因此，有

式中， e = z-h （ x ₀ ， x _O，0 ）+ Jx ₀ + J ₂ x _O，0 ， e _O = q-p （ x _O，0 ）+ J ₃ x _O，0 。

记 u = ， A = ， θ = ， w = ，则式（3-33）可表示为

式（3-34）中 θ 的广义最小二乘估计和估计协方差矩阵为

式中， Σ _w =cov{ w }。由式（3-35）得

式中， = - 。

根据附录A中的式（A-4）有

因此，有

式中， Σ = Σ _ξ + 。

式（3-32）的第一个方程即

式中，Δ x _O = x _O -x _O，0 ， = 0 ，cov{Δ x _O }= ， = 0 。

因此，式（3-37）中最后一项恰好是用式（3-38）对 x 最小二乘估计 的误差协方差矩阵。

由上可知，观测站状态存在随机误差将增加定位误差，当状态随机误差的一阶矩和二阶矩已知时，增加观测站状态测量方程不能减小定位误差。

当误差服从高斯分布时，根据式（2-41）和式（3-36）同样可以计算Fisher信息矩阵

式中， g （ x ， x _O ）= ， θ = ， J 、 J ₂ 、 J ₃ 都在 x 、 x _O 真值处取值。

由式（3-37）可知，关于辐射源位置 x 无偏估计的CRLB为 ，当观测站状态测量方程为最简单情况，即 p （ x _O ）= x _O 时，不限定适定方程，且包含站址误差的CRLB为

3.2.2 定位跟踪已知状态方程的动目标算法

通常采用序贯滤波算法估计已知状态方程的运动辐射源的时变位置，主要是在得到观测量 z _{0： k} ={ z ₀ ， z ₁ ， … ， z _k }后通过估计后验概率密度函数 p （ x _k | z _{0： k} ），得到 x _k 的 MAP 估计 = ，或最小均方误差（MMSE）估计 。对式（3-4）表示的运动辐射源定位跟踪问题，这里简要介绍扩展卡尔曼滤波（EKF）、无迹卡尔曼滤波（UKF）、粒子滤波（PF）等算法，其他如中心差分卡尔曼滤波（CDKF） ^[10] 、迭代EKF （IEKF） ^[11] 、修正增益的EKF（MGEKF） ^[12] 、修正协方差的EKF（MVEKF） ^[13] 等算法这里不展开，感兴趣的读者可以查阅相关文献。

3.2.2.1 扩展卡尔曼滤波算法

扩展卡尔曼滤波（EKF）算法主要解决由式（3-42）状态与观测方程表示的定位跟踪问题

其中，状态转移方程和观测方程均为非线性函数，过程噪声 ε _k 和观测噪声 ξ _k 均为 0 均值高斯白噪声，其协方差矩阵分别为 Q _k = ， R _k = 。其算法要点 ^[14] 如下。

首先，EKF将非线性函数在滤波值 作一阶线性近似，有

式中， F k = ， g _k = ， H _k = ， y k = 。

对状态转移方程和观测方程线性化后，在初始条件 P _0，0 = ， = x ₀ 下，EKF递推滤波过程如下。

（1）求状态的预测值：

（2）求解状态协方差的预测值：

（3）求卡尔曼增益：

（4）状态更新：

（5）协方差更新：

3.2.2.2 无迹卡尔曼滤波算法

当观测方程的非线性化程度比较严重时，EKF 算法由于只利用了一阶线性化，因此其估计精度和收敛性能都会受到较大的影响。为了降低线性化所带来的误差，文献 ^[15] 提出了基于二阶近似的无迹卡尔曼滤波（UKF）算法。无迹变换是UKF算法的基础，其基本原理可参见文献 ^[16] 。下面同样基于式（3-42）对UKF滤波算法进行简单介绍。

（1）根据已经估计出的 和 P _{k -1， k -1} ，利用无迹变换获得一组矢量：

式中， = + ， - =（ n + m ） P _{k -1， k -1} ， 表示矩阵 的第 n 列， n 为状态矢量 的维数， m 为需要优化选取的参数。

（2）求上述采样点的一步预测值：

（3）计算状态矢量的一步预测值及协方差矩阵：

式中， 为矢量 的第 i 个元素。

（4）根据一步预测值，再次利用无迹变换获得一组新的矢量：

式中， 。

（5）将新获得的矢量代入观测方程，得到预测的观测量：

（6）将得到的预测观测量通过加权求和得到预测的均值及协方差矩阵：

（7）计算预测值 与 之间的协方差矩阵：

（8）计算卡尔曼增益：

（9）计算状态更新和协方差更新：

m 的优化选取可以用来减少式（3-54）中均值和协方差矩阵近似高阶误差（大于）三阶，对于高斯随机矢量而言，取 m =3 -n 会使其均值和协方差矩阵中的某些四阶项最小。

3.2.2.3 粒子滤波算法

粒子滤波 ^[17] （PF）算法与前面介绍的EKF和UKF算法一样，也是在获得 z _{0： k} 后估计 x _k 后验状态的方法，但与前面介绍的EKF或UKF算法不同的是，PF算法对函数 f 和 h 没有限制，对噪声 ε _k 和 ξ _k 没有限定加性噪声，也不要求高斯概率分布。PF 算法本质上是一种求解后验概率密度函数的蒙特卡罗（Monte Carlo）算法，该算法的主要思想是将目标状态的后验分布密度通过随机采样的粒子点与权重来近似，进而滤出目标最可能状态。基于定位跟踪模型式（3-4），我们在重要性密度函数特殊选择，以及有条件重采样的情况下，简述序贯重要性重采样（SIR）粒子滤波算法的步骤。

（1）初始（ k -1=0）采样，从先验分布密度函数 p （ x ₀ ）= p （ x ₀ z ₀ ）中采样 M 个粒子 ， p （ x ₀ ）可以取均匀分布或高斯分布等，权重为

（2）重要性采样（ k 时刻），从重要性密度函数 = 中采样生成粒子 ，即

式中， = ， p （ x _k | x _{k -1} ）为由式（3-4）状态方程确定的转移分布密度函数。

（3）更新 的权值：

式中， 为由式（3-4）观测方程确定的似然分布密度函数。

（4）归一化权值：

（5）状态估计，后验状态分布概率密度函数的估计为

式中， δ 为狄拉克德尔塔（Dirac delta）函数。

状态 x _k 的MMSE估计为

或状态 x _k 的MAP的估计为

（6）计算有效粒子数：

（7）设置重采样门限值 N _T ，若 < N _T ，则按如下过程重采样，否则转到（2）：

① 计算分布函数 c （ i ）= ， i =1，2， … ， M ， c （0）=0；

② 随机选取 u （1）∈[0，1 /M ]，设置 u _j = u （1）+（ j -1）/ M ， j =1，2， … ， M ；

③ 将 u （ j ）（ j =1，2， … ， M ）与 c （ i ）（ i =1，2， … ， M ）顺序比较，通过复制权值大的粒子和丢弃权值小的粒子，维持 M 个粒子，并将权值重新取为 =1/ M ，回到（2）。

作为 PF 的特殊情况，如果滤波后的概率密度函数 p （ x _k | z _{0： k} ），预测的后验概率密度函数 p （ x _k | z _{0： k -1} ），初始预测后验概率密度函数 p （ x ₀ z ₀ ）均服从高斯分布，则可用高斯粒子滤波 ^[18] （GPF）估计 p （ x _k z _{0： k} ）和 。关于PF的具体算法和应用参见第10章。

3.2.3 算法特点比较

定位算法可从两个方面比较，第一个方面涉及辐射源状态先验信息，第二个方面涉及观测数据是一次性处理给出位置还是逐次处理更新位置。

从是否涉及辐射源状态先验信息方面看，用于定位计算的MAP、MMSE，以及EKF、UKF、PF等均属于需要先验信息的贝叶斯算法，而ML、GLS等属于不需要先验信息的费歇算法 ^[1] 。部分用于对无线电辐射源定位的贝叶斯算法、费歇算法特点比较如表3-1所示。

常用于无线电辐射源定位的GLS、ML、MAP、MMSE属于批处理算法，而EKF、UKF和PF等属于序贯处理算法。定位处理算法分类框图如图3-2所示。

批处理算法可用于多个观测平台对静止辐射源位置或运动辐射源位置及速度一次性估计，序贯处理算法主要适用于对运动辐射源跟踪。序贯处理算法需要辐射源初始位置的先验信息，初始位置先验信息可以用过去一段时间的观测数据批处理给出。需要注意的是，批处理方法也可能在序贯处理的局部应用（参见PF）。批处理与序贯处理算法的特点比较如表3-2所示。

[1] 当 h （ x ， x _O ）隐含未知参数γ时，记θ= ，将式（3-6）改为求 ， 为 的相应分矢量。 CJkVpRRVseBiUCecLnYoKGcH+JPlR6eHQ0SPd2MO/t8gCmK8jfwuxNsUXLdVqu51