基于多向寻优的全跟踪式UKF滤波算法的双站定位方法

摘要

本发明涉及一种基于多向寻优的全跟踪式UKF滤波算法的双站定位方法，含有如下步骤：步骤1、测量目标TSOA参数：在双站定位模型中，两个测量站分别独立测量目标的传输时间参数TSOA，并通过定时设备对所测量的TSOA数据添加时间标签；步骤2、多向寻优UKF滤波过程：对两个测量站所给出的TSOA数据进行预处理，剔除其中的野点，然后执行多向寻优全跟踪式UKF滤波过程，得到两个UKF滤波结果；步骤3、UKF滤波结果的鉴别：分别计算两个UKF滤波结果在二维平面上的均方误差，根据得到的两个均方误差的大小判决目标的真实位置，均方误差大的收敛结果为虚假点，均方误差小的收敛结果为真实点；本发明的定位效果好。

说明书

(一)、技术领域：本发明涉及一种定位方法，特别是涉及一种基于多向寻优的全跟踪式UKF滤波算法的双站定位方法。

(二)、背景技术：扩展Kalman滤波(EKF)算法由于具有实时数据处理能力，成为在非线性强度较弱的非线性系统动态数据处理时最受欢迎的非线性数据处理算法。但EKF也有不足:(1)非线性模型的线性化过程容易引入误差，因此降低模型的准确性，对于强非线性系统，无法保证估计精度；(2)滤波前必须手动计算非线性函数的Jacobi矩阵。对于高维的复杂系统模型，这一过程非常烦琐而且容易出错。

近似任意非线性函数的概率分布比近似非线性函数更容易，在此思想的指导下，Simon Julier等人提出了基于unscented变换的Kalman滤波(UKF)，在确保随机向量均值和协方差不变的前提下，选择一组Sigma样点集，每个Sigma点通过非线性变换，由变换后样点的统计量来估计随机向量通过非线性变换后的均值及方差，避免了线性化所带来的误差，且不需要计算非线性方程的Jacobi矩阵，比EKF类算法具有更好的稳定性；另外，这种采样方法提取了更多的统计特性信息，较之EKF算法可以获得更多的观测假设，因此，UKF算法对状态统计特性的估计比EKF算法更为准确。然而针对复杂的非线性系统，特别是具有多个局部最优的非线性系统，UKF算法容易陷入局部最优解，导致错误的滤波结果，该问题严重影响了UKF滤波算法性能的稳定性，也大大限制了其应用场合。

(三)、发明内容：

本发明要解决的技术问题是：提供一种基于多向寻优的全跟踪式UKF滤波算法的双站定位方法，该方法的定位效果好。

本发明的技术方案：

一种基于多向寻优的全跟踪式UKF滤波算法的双站定位方法，具体为：

步骤1、测量目标TSOA(Time Summation of Arrival，到达时间和)参数：在双站定位模型中，两个测量站分别独立测量目标的传输时间参数TSOA，并通过定时设备对所测量的TSOA数据添加时间标签；

步骤2、多向寻优UKF滤波过程：对两个测量站所给出的TSOA数据进行预处理，剔除其中的野点，然后执行多向寻优全跟踪式UKF滤波过程，得到两个UKF滤波结果；

步骤3、UKF滤波结果的鉴别：分别计算两个UKF滤波结果在二维平面上的均方误差，根据得到的两个均方误差的大小判决目标的真实位置，均方误差大的收敛结果为虚假点，均方误差小的收敛结果为真实点。

在双站协同定位模型下，由于目标的真实点和模糊点之间必定存在着相对的左右位置关系，可以利用UKF的初值设定影响收敛结果的特性，让滤波器从不同方向对目标位置的估计结果进行收敛，则两次收敛的结果分别是真实点和模糊点的估计位置，由此可以得到基于UKF两次收敛的不同结果，通过进一步判断收敛结果的均方误差即可从两次收敛当中挑选出真实点，从而实现去模糊、精确跟踪定位。

步骤1中的定时设备为GPS时钟。

双站定位模型的建立方法如下：

以基站坐标为原点构建平面直角坐标系，设目标P的位置坐标为(x,y)，测量站A和测量站B的位置坐标分别为(x₁,y₁)、(x₂,y₂)，根据测量站A和测量站B分别获得TSOA值τ₁和TSOA值τ₂；

$>{\tau }_1={\mathrm{\Delta r}}_1=r_1\left(i\right)+r_0\left(i\right)=\sqrt{{(x_1-x)}^2+{(y_1-y)}^2}+\sqrt{x^2+y^2};---(3-1)$>

$>{\tau }_2={\mathrm{\Delta r}}_2=r_2\left(i\right)+r_0\left(i\right)=\sqrt{{(x_2-x)}^2+{(y_2-y)}^2}+\sqrt{x^2+y^2};---(3-2)$>

其中，Δr₁表示第i时刻目标P到基站与到测量站A的距离和，Δr₂表示第i时刻目标P到基站与到测量站B的距离和，r₀(t)表示第i时刻目标P到基站的距离，r₁(i)和r₂(i)分别表示第i时刻目标P到测量站A和测量站B的距离；目标P位于以基站和测量站A为焦点的椭圆和以基站和测量站B为焦点的椭圆的交点处，由于两个椭圆都将基站作为其一个焦点，因此，两个椭圆会有两交点，其中一个交点为真实点，另一个交点为模糊点；

双站定位模型的求解方法如下：

在双站定位模型中，观测方程数目与目标的位置坐标数相等，故将直接采用位置线交叉法实现目标位置估计；针对式(3-1)和(3-2)，移项并两边同求平方，可得：

$>\left(\begin{array}{c}{({\mathrm{\Delta r}}_1-\sqrt{x^2+y^2})}^2={\left(\sqrt{{(x_1-x)}^2+{(y_1-y)}^2}\right)}^2\\ {({\mathrm{\Delta r}}_2-\sqrt{x^2+y^2})}^2={\left(\sqrt{{(x_2-x)}^2+{(y_2-y)}^2}\right)}^2\end{array}\right)$>

展开可得，

$>\left(\begin{array}{c}{\left({\mathrm{\Delta r}}_1\right)}^2-2·{\mathrm{\Delta r}}_1·\sqrt{x^2+y^2}+(x^2+y^2)={(x_1-x)}^2+{(y_1-y)}^2\\ {\left({\mathrm{\Delta r}}_1\right)}^2-2·{\mathrm{\Delta r}}_2·\sqrt{x^2+y^2}+(x^2+y^2)={(x_2-x)}^2+{(y_2-y)}^2\end{array}\right)*\mathrm{MERGEFORMAT}---\left(4\right)$>

用θ表示目标与基站(原点)的连线与x轴正方向的夹角，用r表示目标到基站之间的距离，则目标的位置坐标可以表示为：

$>\left(\begin{array}{c}x=r·\cos \theta \\ y=r·\sin \theta \end{array}\right)*\mathrm{MERGEFORMAT}---\left(5\right)$>

将上式代入方程组，可得：

$>\left(\begin{array}{c}{\left({\mathrm{\Delta r}}_1\right)}^2-2·{\mathrm{\Delta r}}_1·r+r^2={(x_1-r·\cos \theta )}^2+{(y_1-r·\sin \theta )}^2\\ {\left({\mathrm{\Delta r}}_2\right)}^2-2·{\mathrm{\Delta r}}_2·r+r^2={(x_2-r·\cos \theta )}^2+{(y_2-r·\sin \theta )}^2\end{array}\right)*\mathrm{MERGEFORMAT}---\left(6\right)$>

将方程组中的两个方程化简整理，可得：

$>r=\frac{{x_1}^2+{y_1}^2-{\left({\mathrm{\Delta r}}_1\right)}^2}{2(x_1\cos \theta +y_1\sin \theta -{\mathrm{\Delta r}}_1)}=\frac{{x_2}^2+{y_2}^2-{\left({\mathrm{\Delta r}}_2\right)}^2}{2(x_2\cos \theta +y_2\sin \theta -{\mathrm{\Delta r}}_2)}*\mathrm{MERGEFORMAT}---\left(7\right)$>

上式经变形可得：

a cosθ+b sinθ＝c   *MERGEFORMAT(8)

其中，

$>\left(\begin{array}{c}a=[{x_1}^2+{y_1}^2-{\left({\mathrm{\Delta r}}_1\right)}^2]·x_2-[{x_2}^2+{y_2}^2-{\left({\mathrm{\Delta r}}_2\right)}^2]·x_1\\ b=[{x_1}^2+{y_1}^2-{\left({\mathrm{\Delta r}}_1\right)}^2]·y_2-[{x_2}^2+{y_2}^2-{\left({\mathrm{\Delta r}}_2\right)}^2]·y_1\\ c=[{x_1}^2+{y_1}^2-{\left({\mathrm{\Delta r}}_1\right)}^2]·{\mathrm{\Delta r}}_2+[{x_2}^2+{y_2}^2-{\left({\mathrm{\Delta r}}_2\right)}^2]·{\mathrm{\Delta r}}_1\end{array}\right)$>

然后利用关系，

$>\theta =\arcsin \frac{c}{\sqrt{a^2+b^2}}-\arcsin \frac{a}{\sqrt{a^2+b^2}}$>或者$>\theta =\pi -\arcsin \frac{c}{\sqrt{a^2+b^2}}+\arcsin \frac{a}{\sqrt{a^2+b^2}}$>

将这两个θ的取值分别代入式(7)即可求得r，得到最终的定位解，这个观测方程组有两个解，其中包含一个模糊点；

基于TSOA的双站定位系统对目标进行定位，实质上是利用观测值来估计椭圆曲线交点位置过程；从一般意义上来讲，通过增加测量站的个数或者增添测量方向角的设备在真实点和模糊点中判决出目标的真实位置，究其本质，这两种方式均是通过增加信息量的思路消除模糊。虽然单次定位不能一次性解决双站定位模型下的模糊点问题，但是可以利用它能够同时输出真实解和模糊解的特性，通过采用移动测量站的策略进行多次测量，以增加测量站在不同位置的观测信息，实现模糊点消除。下面就该策略的可行性进行定性分析：

当真实的目标处于静止或低速移动状态时，考虑移动测量站A或者测量站B进行多次数据测量以实现模糊去除；当测量站A位于A₁点时，目标位于以基站和A₁点为焦点的椭圆上，同时也位于以基站和测量站B为焦点的椭圆上，此时模糊点位于P₁点。当测量站A位于A₂点时，测量站B的TSOA测量值不变，而测量站A的TSOA测量值发生了变化，致使以基站和测量站A为焦点的椭圆随着测量站A的移动发生了变化，此时模糊点的位置由P₁点变到P₂点。因此，当不断移动测量站测量站A或测量站B的时，模糊点的变化范围会很大，而真实点的变化范围很小，依据这个特征即可去除模糊点；

以上去除模糊点的过程，从本质上说均是通过测量站在不同观测位置的定位信息，然后基于数据融合的思想对这些冗余信息进行特征提取，利用真实点与模糊点之间的特征差异完成对模糊点的判决，最终实现去模糊后的精确定位。

在直角坐标系下，UKF滤波的状态方程表示为：

χ_k+1＝f(χ_k)+ω_k   (12)

其中，χ_k＝[x_1k,x_2k,…,x_nk]^T为采样时间t_k的系统状态向量,n为状态向量的维数，f[·]为状态向量的状态转移函数，ω_k为过程噪声向量；

系统观测方程为：

Y_k＝h[X_k]+υ_k   (13)

其中，Y_k＝[y_1k,y_2k,…,y_mk]^T为采样时间t_k的系统观测向量，m为观测向量的维数，h[·]为状态向量的非线性观测函数，υ_k为观测噪声向量，设ω_k和υ_k为互不相关的零均值高斯白噪声；

步骤2中，多向寻优全跟踪式UKF滤波(IUKF)的算法为：

多向寻优全跟踪式UKF滤波采用多线程同时搜索的策略进行滤波，其滤波过程分为如下四个部分：

部分一﹑多向寻优全跟踪式UKF滤波的多线程管理及初始化：

多向寻优全跟踪式UKF滤波的搜索线程数S根据其应用环境确定，设定正确的搜索线程数是其能够有效工作的前提，如果搜索线程数设定过多，将会浪费较多的计算资源，如果线程数设定过少，可能会漏掉对有效局部最优解的跟踪，最后导致滤波结果的错误；在实际的应用当中，对多向寻优全跟踪式UKF滤波的非线性观测方程进行数学分析，预先评估非线性观测方程局部极值点的个数，并根据极值点的个数确定多向寻优全跟踪式UKF滤波的搜索线程数S；

确定了搜索线程数S后，设定各个子搜索线程的搜索起点，它决定了搜索子线程是否可以准确收敛至最邻近的局部最优值点；子搜索线程的搜索起点的选取以多向寻优全跟踪式UKF滤波的状态向量X_k的维数和取值范围为依据，尽可能选择方位相对的向量位置为搜索起点，设第q个子线程的搜索起点为χ^s₀＝[x^s₁₀,x^s₂₀,…,x^s_n0]^T；

根据搜索线程数S和子搜索线程的起点构造多向寻优全跟踪式UKF滤波的迭代状态向量和观测向量，设k时刻多向寻优全跟踪式UKF滤波的迭代状态向量为迭代观测向量为z_k，则有：

$>{\hat{x}}_0={[{{{\chi }^1}_0}^T,{{{\chi }^2}_0}^T,···,{{{\chi }^S}_0}^T]}^T$>

z_k＝[Y_k^T,Y_k^T,…,Y_k^T]^T

其中，为多向寻优全跟踪式UKF滤波的初始化迭代状态向量，z_k向量的维数为S×m；

部分二﹑计算Sigma点：

UT(Unscented Transform)变换是一种计算随机变量非线性变换统计特性的方法，基于UT变换的最小方差估计方法是：选择一组Sigma点，使其样本均值和协方差与状态变量的均值和协方差P_xx一致，将这些Sigma点进行非线性变换后可获得变换点的均值和协方差P_zz；

在进行确定性采样之前需要计算每个Sigma点相应的均值和方差的权值，其中采样均值权值：

W₀^(b)＝λ/(nS+λ)   W_i^(b)＝1/[2(nS+λ)]   i＝1,…,2nS

采样方差权值：

W₀^(c)＝λ/(nS+λ)+(1-ε+β)   W_i^(c)＝1/[2(nS+λ)]   i＝1,…,2nS

其中，λ＝ε²(nS+γ)-nS，nS为状态向量维数，ε为尺度参数，决定采样点与均值的远近程度；γ一般为零，β包含了χ的先验分布信息，这里取β＝2；

Sigma点的计算过程如下：

$>X_0=\hat{x}$>

$>X_0=\hat{x}+{\left(\sqrt{(n+\lambda )P}\right)}_i,i=1,···,n$>

$>X_0=\hat{x}+{\left(\sqrt{(n+\lambda )P}\right)}_{i-n},i=n+1,···,2n$>

其中，表示取矩阵均方根的第i列；

部分三﹑时间更新：

状态测量值：(X_k/k-1)_i＝f₁((X_k-1)_i)   i＝1,…,2nS

状态预测值均值：$>{\hat{x}}_{k/k-1}=\underset{i=0}{\overset{2\mathrm{nS}}{\Sigma }}{W_i}^{\left(m\right)}{\left(X_{k/k-1}\right)}_i$>

状态预测误差矩阵：$>P_{k/k-1}=\underset{i=0}{\overset{2\mathrm{nS}}{\Sigma }}{W_i}^{\left(c\right)}[{\left(X_{k/k-1}\right)}_i-{\hat{x}}_{k/k-1}]{[{\left(X_{k/k-1}\right)}_i-{\hat{x}}_{k/k-1}]}^T+Q$>

观测量预测值：(Z_k/k-1)_i＝h₁((X_k/k-1)_i)i＝1,…,2nS

观测量预测均值：$>{\hat{z}}_{k/k-1}=\underset{i=0}{\overset{2\mathrm{nS}}{\Sigma }}{W_i}^{\left(b\right)}{\left(Z_{k/k-1}\right)}_i$>

由于状态向量X_k是原始状态向量χ_k的增广向量，为了保持不同搜索子线程之间的搜索独立性，在计算上每间隔S个变量使用函数f₁[·]和h₁[·]对状态向量(X_k)_i进行计算；

部分四﹑测量更新：

$>P_{\mathrm{ZZ}}=\underset{i=0}{\overset{2\mathrm{nS}}{\Sigma }}{W_i}^{\left(c\right)}[{\left(Z_{k/k-1}\right)}_i-{\hat{z}}_{k/k-1}]{[{\left(Z_{k/k-1}\right)}_i-{\hat{z}}_{k/k-1}]}^T+R$>

$>P_{\mathrm{XZ}}=\underset{i=0}{\overset{2\mathrm{nS}}{\Sigma }}{W_i}^{\left(c\right)}[{\left(X_{k/k-1}\right)}_i-{\hat{x}}_{k/k-1}]{[{\left(Z_{k/k-1}\right)}_i-{\hat{z}}_{k/k-1}]}^T$>

Kalman增益：K＝P_XZP_ZZ^-1

状态值更新：$>{\hat{x}}_k={\hat{x}}_{k/k-1}+K(z_k-{\hat{z}}_{k/k-1})$>

滤波误差矩阵更新：P_k＝P_k/k-1-KP_ZZK^T

部分五﹑滤波结果决策：

对各个搜索子线程的滤波结果进行决策对比，具体的决策算法准则和多向寻优全跟踪式UKF滤波的应用环境有关，根据不同的应用环境灵活设定适合的决策准则；这里将决策的准则表示为函数p[·]，其中ξ_k为k时刻对S个搜索子线程滤波性能的评估结果向量，

本发明的有益效果：

1、本发明在UKF算法的基础上，提出了一种基于多向寻优的全跟踪式UKF滤波算法，该算法能够从不同的方向开始滤波搜索，对所有的局部最优解进行同步跟踪，在滤波算法稳定收敛之后，通过对比不同局部最优解的统计性能，对全局最优进行准确决策；通过在无线定位应用中的仿真结果表明，本发明在原始UKF算法无法使用的环境下依旧能够正常工作，有效拓展了UKF算法的应用范围，定位效果好。

2、本发明的多向寻优全跟踪式UKF滤波通过多线程跟踪非线性系统中尽可能多的局部最优值点，最终给出正确的全局最优值，该滤波方法能够有效弥补经典UKF滤波器容易陷入局部最优值点的应用缺陷，提高UKF滤波器的性能，滤波效果好。

(四)、附图说明：

图1为双站定位模型的结构示意图；

图2为针对静止目标去模糊过程的示意图；

图3为基于时延测量的无线定位原理示意图；

图4为两个跟踪子线程的搜索路径示意图；

图5为跟踪线程1的状态向量迭代结果示意图；

图6为跟踪线程2的状态向量迭代结果示意图；

图7为跟踪线程1的位置离散度示意图；

图8为跟踪线程2的位置离散度示意图。

(五)、具体实施方式：

基于多向寻优的全跟踪式UKF滤波算法的双站定位方法为：

步骤1、测量目标TSOA(Time Summation of Arrival，到达时间和)参数：在双站定位模型中，两个测量站分别独立测量目标的传输时间参数TSOA，并通过定时设备对所测量的TSOA数据添加时间标签；

步骤2、多向寻优UKF滤波过程：对两个测量站所给出的TSOA数据进行预处理，剔除其中的野点，然后执行多向寻优全跟踪式UKF滤波过程，得到两个UKF滤波结果；

步骤3、UKF滤波结果的鉴别：分别计算两个UKF滤波结果在二维平面上的均方误差，根据得到的两个均方误差的大小判决目标的真实位置，均方误差大的收敛结果为虚假点，均方误差小的收敛结果为真实点。

在双站协同定位模型下，由于目标的真实点和模糊点之间必定存在着相对的左右位置关系，可以利用UKF的初值设定影响收敛结果的特性，让滤波器从不同方向对目标位置的估计结果进行收敛，则两次收敛的结果分别是真实点和模糊点的估计位置，由此可以得到基于UKF两次收敛的不同结果，通过进一步判断收敛结果的均方误差即可从两次收敛当中挑选出真实点，从而实现去模糊、精确跟踪定位。

步骤1中的定时设备为GPS时钟。

双站定位模型的建立方法如下(如图1所示)：

以基站J坐标为原点构建平面直角坐标系，设目标P的位置坐标为(x,y)，测量站A和测量站B的位置坐标分别为(x₁,y₁)、(x₂,y₂)，根据测量站A和测量站B分别获得TSOA值τ₁和TSOA值τ₂；

$>{\tau }_1={\mathrm{\Delta r}}_1=r_1\left(i\right)+r_0\left(i\right)=\sqrt{{(x_1-x)}^2+{(y_1-y)}^2}+\sqrt{x^2+y^2};---(3-1)$>

$>{\tau }_2={\mathrm{\Delta r}}_2=r_2\left(i\right)+r_0\left(i\right)=\sqrt{{(x_2-x)}^2+{(y_2-y)}^2}+\sqrt{x^2+y^2};---(3-2)$>

其中，Δr₁表示第i时刻目标P到基站J与到测量站A的距离和，Δr₂表示第i时刻目标P到基站J与到测量站B的距离和，r₀(t)表示第i时刻目标P到基站J的距离，r₁(i)和r₂(i)分别表示第i时刻目标P到测量站A和测量站B的距离；目标P位于以基站J和测量站A为焦点的椭圆和以基站和测量站B为焦点的椭圆的交点处，由于两个椭圆都将基站J作为其一个焦点，因此，两个椭圆会有两交点，其中一个交点为真实点P，另一个交点为模糊点MH；

双站定位模型的求解方法如下：

在双站定位模型中，观测方程数目与目标的位置坐标数相等，故将直接采用位置线交叉法实现目标位置估计；针对式(3-1)和(3-2)，移项并两边同求平方，可得：

$>\left(\begin{array}{c}{({\mathrm{\Delta r}}_1-\sqrt{x^2+y^2})}^2={\left(\sqrt{{(x_1-x)}^2+{(y_1-y)}^2}\right)}^2\\ {({\mathrm{\Delta r}}_2-\sqrt{x^2+y^2})}^2={\left(\sqrt{{(x_2-x)}^2+{(y_2-y)}^2}\right)}^2\end{array}\right)$>

展开可得，

$>\left(\begin{array}{c}{\left({\mathrm{\Delta r}}_1\right)}^2-2·{\mathrm{\Delta r}}_1·\sqrt{x^2+y^2}+(x^2+y^2)={(x_1-x)}^2+{(y_1-y)}^2\\ {\left({\mathrm{\Delta r}}_1\right)}^2-2·{\mathrm{\Delta r}}_2·\sqrt{x^2+y^2}+(x^2+y^2)={(x_2-x)}^2+{(y_2-y)}^2\end{array}\right)*\mathrm{MERGEFORMAT}---\left(4\right)$>

用θ表示目标与基站(原点)的连线与x轴正方向的夹角，用r表示目标到基站之间的距离，则目标的位置坐标可以表示为：

$>\left(\begin{array}{c}x=r·\cos \theta \\ y=r·\sin \theta \end{array}\right)*\mathrm{MERGEFORMAT}---\left(5\right)$>

将上式代入方程组，可得：

$>\left(\begin{array}{c}{\left({\mathrm{\Delta r}}_1\right)}^2-2·{\mathrm{\Delta r}}_1·r+r^2={(x_1-r·\cos \theta )}^2+{(y_1-r·\sin \theta )}^2\\ {\left({\mathrm{\Delta r}}_2\right)}^2-2·{\mathrm{\Delta r}}_2·r+r^2={(x_2-r·\cos \theta )}^2+{(y_2-r·\sin \theta )}^2\end{array}\right)*\mathrm{MERGEFORMAT}---\left(6\right)$>

将方程组中的两个方程化简整理，可得：

$>r=\frac{{x_1}^2+{y_1}^2-{\left({\mathrm{\Delta r}}_1\right)}^2}{2(x_1\cos \theta +y_1\sin \theta -{\mathrm{\Delta r}}_1)}=\frac{{x_2}^2+{y_2}^2-{\left({\mathrm{\Delta r}}_2\right)}^2}{2(x_2\cos \theta +y_2\sin \theta -{\mathrm{\Delta r}}_2)}*\mathrm{MERGEFORMAT}---\left(7\right)$>

上式经变形可得：

a cosθ+b sinθ＝c   *MERGEFORMAT(8)

其中，

$>\left(\begin{array}{c}a=[{x_1}^2+{y_1}^2-{\left({\mathrm{\Delta r}}_1\right)}^2]·x_2-[{x_2}^2+{y_2}^2-{\left({\mathrm{\Delta r}}_2\right)}^2]·x_1\\ b=[{x_1}^2+{y_1}^2-{\left({\mathrm{\Delta r}}_1\right)}^2]·y_2-[{x_2}^2+{y_2}^2-{\left({\mathrm{\Delta r}}_2\right)}^2]·y_1\\ c=[{x_1}^2+{y_1}^2-{\left({\mathrm{\Delta r}}_1\right)}^2]·{\mathrm{\Delta r}}_2+[{x_2}^2+{y_2}^2-{\left({\mathrm{\Delta r}}_2\right)}^2]·{\mathrm{\Delta r}}_1\end{array}\right)$>

然后利用关系，

$>\theta =\arcsin \frac{c}{\sqrt{a^2+b^2}}-\arcsin \frac{a}{\sqrt{a^2+b^2}}$>或者$>\theta =\pi -\arcsin \frac{c}{\sqrt{a^2+b^2}}+\arcsin \frac{a}{\sqrt{a^2+b^2}}$>

将这两个θ的取值分别代入式(7)即可求得r，得到最终的定位解，这个观测方程组有两个解，其中包含一个模糊点；

基于TSOA的双站定位系统对目标进行定位，实质上是利用观测值来估计椭圆曲线交点位置过程；从一般意义上来讲，通过增加测量站的个数或者增添测量方向角的设备在真实点和模糊点中判决出目标的真实位置，究其本质，这两种方式均是通过增加信息量的思路消除模糊。虽然单次定位不能一次性解决双站定位模型下的模糊点问题，但是可以利用它能够同时输出真实解和模糊解的特性，通过采用移动测量站的策略进行多次测量，以增加测量站在不同位置的观测信息，实现模糊点消除。下面就该策略的可行性进行定性分析：

当真实的目标处于静止或低速移动状态时，考虑移动测量站A或者测量站B进行多次数据测量以实现模糊去除(如图2所示)；当测量站A位于A₁点时，目标位于以基站J和A₁点为焦点的椭圆上，同时也位于以基站J和测量站B为焦点的椭圆上，此时模糊点位于P₁点。当测量站A位于A₂点时，测量站B的TSOA测量值不变，而测量站A的TSOA测量值发生了变化，致使以基站J和测量站A为焦点的椭圆随着测量站A的移动发生了变化，此时模糊点的位置由P₁点变到P₂点。因此，当不断移动测量站测量站A或测量站B的时，模糊点的变化范围会很大，而真实点P的变化范围很小，依据这个特征即可去除模糊点；

以上去除模糊点的过程，从本质上说均是通过测量站在不同观测位置的定位信息，然后基于数据融合的思想对这些冗余信息进行特征提取，利用真实点与模糊点之间的特征差异完成对模糊点的判决，最终实现去模糊后的精确定位。

在直角坐标系下，UKF滤波的状态方程表示为：

χ_k+1＝f(χ_k)+ω_k   (12)

其中，χ_k＝[x_1k,x_2k,…,x_nk]^T为采样时间t_k的系统状态向量,n为状态向量的维数，f[·]为状态向量的状态转移函数，ω_k为过程噪声向量；

系统观测方程为：

Y_k＝h[X_k]+υ_k   (13)

其中，Y_k＝[y_1k,y_2k,…,y_mk]^T为采样时间t_k的系统观测向量，m为观测向量的维数，h[·]为状态向量的非线性观测函数，υ_k为观测噪声向量，设ω_k和υ_k为互不相关的零均值高斯白噪声；

步骤2中，多向寻优全跟踪式UKF滤波(IUKF)的算法为：

多向寻优全跟踪式UKF滤波采用多线程同时搜索的策略进行滤波，其滤波过程分为如下四个部分：

部分一﹑多向寻优全跟踪式UKF滤波的多线程管理及初始化：

多向寻优全跟踪式UKF滤波的搜索线程数S根据其应用环境确定，设定正确的搜索线程数是其能够有效工作的前提，如果搜索线程数设定过多，将会浪费较多的计算资源，如果线程数设定过少，可能会漏掉对有效局部最优解的跟踪，最后导致滤波结果的错误；在实际的应用当中，对多向寻优全跟踪式UKF滤波的非线性观测方程进行数学分析，预先评估非线性观测方程局部极值点的个数，并根据极值点的个数确定多向寻优全跟踪式UKF滤波的搜索线程数S；

确定了搜索线程数S后，设定各个子搜索线程的搜索起点，它决定了搜索子线程是否可以准确收敛至最邻近的局部最优值点；子搜索线程的搜索起点的选取以多向寻优全跟踪式UKF滤波的状态向量X_k的维数和取值范围为依据，尽可能选择方位相对的向量位置为搜索起点，设第q个子线程的搜索起点为χ^s₀＝[x^s₁₀,x^s₂₀,…,x^s_n0]^T；

根据搜索线程数S和子搜索线程的起点构造多向寻优全跟踪式UKF滤波的迭代状态向量和观测向量，设k时刻多向寻优全跟踪式UKF滤波的迭代状态向量为迭代观测向量为z_k，则有：

$>{\hat{x}}_0={[{{{\chi }^1}_0}^T,{{{\chi }^2}_0}^T,···,{{{\chi }^S}_0}^T]}^T$>

z_k＝[Y_k^T,Y_k^T,…,Y_k^T]^T

其中，为多向寻优全跟踪式UKF滤波的初始化迭代状态向量，z_k向量的维数为S×m；

部分二﹑计算Sigma点：

UT(Unscented Transform)变换是一种计算随机变量非线性变换统计特性的方法，基于UT变换的最小方差估计方法是：选择一组Sigma点，使其样本均值和协方差与状态变量的均值和协方差P_xx一致，将这些Sigma点进行非线性变换后可获得变换点的均值和协方差P_zz；

在进行确定性采样之前需要计算每个Sigma点相应的均值和方差的权值，其中采样均值权值：

W₀^(b)＝λ/(nS+λ)   W_i^(b)＝1/[2(nS+λ)]   i＝1,…,2nS

采样方差权值：

W₀^(c)＝λ/(nS+λ)+(1-ε+β)   W_i^(c)＝1/[2(nS+λ)]   i＝1,…,2nS

其中，λ＝ε²(nS+γ)-nS，nS为状态向量维数，ε为尺度参数，决定采样点与均值的远近程度；γ一般为零，β包含了χ的先验分布信息，这里取β＝2；

Sigma点的计算过程如下：

$>X_0=\hat{x}$>

$>X_0=\hat{x}+{\left(\sqrt{(n+\lambda )P}\right)}_i,i=1,···,n$>

$>X_0=\hat{x}+{\left(\sqrt{(n+\lambda )P}\right)}_{i-n},i=n+1,···,2n$>

其中，表示取矩阵均方根的第i列；

部分三﹑时间更新：

状态测量值：(X_k/k-1)_i＝f₁((X_k-1)_i)   i＝1,…,2nS

状态预测值均值：$>{\hat{x}}_{k/k-1}=\underset{i=0}{\overset{2\mathrm{nS}}{\Sigma }}{W_i}^{\left(m\right)}{\left(X_{k/k-1}\right)}_i$>

状态预测误差矩阵：$>P_{k/k-1}=\underset{i=0}{\overset{2\mathrm{nS}}{\Sigma }}{W_i}^{\left(c\right)}[{\left(X_{k/k-1}\right)}_i-{\hat{x}}_{k/k-1}]{[{\left(X_{k/k-1}\right)}_i-{\hat{x}}_{k/k-1}]}^T+Q$>

观测量预测值：(Z_k/k-1)_i＝h₁((X_k/k-1)_i)   i＝1,…,2nS

观测量预测均值：$>{\hat{z}}_{k/k-1}=\underset{i=0}{\overset{2\mathrm{nS}}{\Sigma }}{W_i}^{\left(b\right)}{\left(Z_{k/k-1}\right)}_i$>

由于状态向量X_k是原始状态向量χ_k的增广向量，为了保持不同搜索子线程之间的搜索独立性，在计算上每间隔S个变量使用函数f₁[·]和h₁[·]对状态向量(X_k)_i进行计算；

部分四﹑测量更新：

$>P_{\mathrm{ZZ}}=\underset{i=0}{\overset{2\mathrm{nS}}{\Sigma }}{W_i}^{\left(c\right)}[{\left(Z_{k/k-1}\right)}_i-{\hat{z}}_{k/k-1}]{[{\left(Z_{k/k-1}\right)}_i-{\hat{z}}_{k/k-1}]}^T+R$>

$>P_{\mathrm{XZ}}=\underset{i=0}{\overset{2\mathrm{nS}}{\Sigma }}{W_i}^{\left(c\right)}[{\left(X_{k/k-1}\right)}_i-{\hat{x}}_{k/k-1}]{[{\left(Z_{k/k-1}\right)}_i-{\hat{z}}_{k/k-1}]}^T$>

Kalman增益：K＝P_XZP_ZZ^-1

状态值更新：$>{\hat{x}}_k={\hat{x}}_{k/k-1}+K(z_k-{\hat{z}}_{k/k-1})$>

滤波误差矩阵更新：P_k＝P_k/k-1-KP_ZZK^T

部分五﹑滤波结果决策：

对各个搜索子线程的滤波结果进行决策对比，具体的决策算法准则和多向寻优全跟踪式UKF滤波的应用环境有关，根据不同的应用环境灵活设定适合的决策准则；这里将决策的准则表示为函数p[·]，其中ξ_k为k时刻对S个搜索子线程滤波性能的评估结果向量，

仿真结果及分析：

现以多向寻优全跟踪式UKF滤波在无线定位中的应用为例进行仿真，该滤波算法的应用环境如下(如图3所示)：定位系统中有两台定位观测站(LS1﹑LS2)对目标信号的到达时延(TOA)进行测量，其中LS2位置固定，坐标设为(1000，0)，LS1从原点出发沿y轴正方向运动，运动速度为2米/秒，待定位目标的位置坐标为(500，500)，TOA测量的采样间隔为0.01秒。在该定位模型下，目标位置除了一个真实目标位置MB之外，还有一个伪目标WMB位置。常用的位置估计算法，如两步最小二乘(Chan)算法、遗传算法等，都使用单测量点进行快拍式的目标位置解算，在这种定位应用场景下，均无法正确区分目标MB位置和伪目标WMB位置。以UKF为代表的滤波型定位算法，可以实时跟踪TOA测量值的变化，但在该定位模型下，容易陷入以伪目标位置为中心的局部最优位置，导致错误的定位结果。本申请提出的IUKF算法，能够同时跟踪所有的局部最优位置，并根据各个局部最优位置的特征，决策最终的全局最优位置，所以适合在该定位环境下的应用。

全局最优解的决策准则根据无线定位模型的特点，设置为位置坐标的离散度，其中第s个子搜索线程的位置离散度定义为：

$>{{\xi }^s}_k=\frac{1}{k}\underset{i=1}{\overset{k}{\Sigma }}\sqrt{[{({x^s}_{1k}-\frac{1}{k}\underset{j=1}{\overset{k}{\Sigma }}{x^s}_{1j})}^2+{({x^s}_{2k}-\frac{1}{k}\underset{j=1}{\overset{k}{\Sigma }}{x^s}_{2j})}^2]}$>

该离散度标示了搜索子线程s定位结果位置坐标点分布的离散程度，这里选择离散度较小的搜索子线程滤波结果为最终的定位结果。

仿真中的参数设置如下：仿真时间为30秒，TOA测量值采样服从高斯分布，均值为TOA的真实值，标准差为100米。由于模型下存在两个局部最优值点，设置跟踪子线程数S＝2，跟踪子线程1的跟踪起点为:χ¹₀＝[-2000,2000]^T，跟踪子线程2的跟踪起点为:χ²₀＝[2000,-2000]^T。系统过程噪声的协方差矩阵：Q_k＝δ₁I，测量噪声协方差矩阵为：R＝δ₂I，初始滤波误差协方差矩阵：p(0)＝δ₃×I，其中，δ₁＝10³，δ₂＝10²，δ₃＝10³，I为4×4的单位矩阵。仿真结果如图4～图8所示：图4为两个跟踪子线程的搜索路径图，图5和图6分别表示跟踪线程1和跟踪线程2的状态向量迭代结果，其中，图5和图6的上半部分表示横坐标的迭代结果，下半部分表示纵坐标的迭代结果。图7和图8表示两个搜索线程第1000到3000个采样点的位置离散度ξ^s_k，其中线程1的平均离散度为24.135，线程2的平均离散度为58.426，这里判决线程1的跟踪结果为全局最优解，为IUKF滤波器最终的输出结果。