CN105898865B - 非线性非高斯条件下基于ekf和pf的协同定位方法 - Google Patents

Abstract

本发明公开非线性非高斯条件下基于EKF和PF的协同定位方法。该方法获取目标节点与各基站的TOA原始数据，计算得到目标节点与各基站之间的距离；利用Wylie鉴别法判断是否有NLOS误差；TOA值做差得到TDOA值，重构TDOA值对应的距离差r_m1；分别使用EKF算法和PF算法估算t_k时刻目标节点的位置坐标；进行残差加权得到t_k时刻的最终估计值；对所有时刻的位置坐标进行加权平滑即可得到最终定位结果。该方法较EKF更适用于非线性非高斯的定位环境，较PF有效避免了使用不正确数据，减少了计算量。该方法有效降低NLOS误差的影响，结合EKF和PF两者的优势，同时克服两者的不足，实现比较精确的定位。

Description

非线性非高斯条件下基于EKF和PF的协同定位方法

技术领域

本发明涉及室内定位跟踪技术领域，具体为一种非线性非高斯条件下基于EKF和PF的协同定位方法。

背景技术

随着微电子技术、通信技术和计算机技术的快速发展，无线定位作为传感网和物联网的重要应用，越来越受到人们的关注。当前，基于测距的定位方法主要有RSSI(到达信号强度)、AOA(到达信号角度)、TOA(信号到达时间)、TDOA(信号到达时差)等。其中，RSSI是一种理论上比较理想的算法，将传播损耗转化为距离，但是在实际定位环境中，电磁波传播具有动态特性，对测距结果造成严重干扰，会产生较大的测距误差；基于AOA测距的定位方式所需要的硬件***设备复杂，成本较高；TOA和TDOA定位精度较高，定位时间短，***实现简单，具有一定的抗多径能力，所以具有较好的应用前景。其中，TDOA的估计方法一般有两种：一种是求两个基站的信号到达时间TOA之差来获得TDOA值；另一种是采用互相关技术，将一个基站接收到的信号与另一个基站接收到的信号进行互相关运算计算出TDOA值。

室内环境一般较为复杂，目标节点和基站之间的信号传播很可能存在NLOS误差(非视距传播)。采用TOA和TDOA技术对目标节点进行定位估计时，TOA值中会产生一个正的附加超量时延，TDOA值也会对应产生一个误差分量。将这种具有较大误差的TOA或TDOA值应用于目标节点的定位估计，必然造成定位算法性能的显著下降，使估计位置出现较大误差。因此，如何鉴别和抑制NLOS误差成为了提高定位精度的一个重要因素。对此，Wylie鉴别法是一种比较有效的方法，它首先从测量参数误差的方差来判断是否为视距传播，若不是视距传播，则重构视距传播值，并对重构的视距值进行滤波，然后用视距定位算法估计移动台的位置。

卡尔曼滤波是在线性高斯情况下利用最小均方误差准则进行目标动态估计的最优滤波方法。不过对于非线性非高斯的情况，还需要对其进行改进和扩展，而扩展卡尔曼滤波(EKF,Extended Kalman Filter)是一种比较有效的改进方法。粒子滤波(PF，ParticleFilter)是一种基于蒙特卡罗思想的非线性、非高斯***滤波方法，它完全突破了卡尔曼滤波理论框架，对***的过程噪声和量测噪声没有任何限制，在处理非线性、非高斯时变***的参数估计和状态滤波问题方面有独特的优势和广阔的前景。

在实际应用过程中，EKF是一种比较常用的非线性滤波方法，但是它仅适用于滤波误差和预测误差很小的情况，否则可能会导致滤波不稳定甚至发散；PF摆脱了解决非线性滤波问题时随机量必须满足高斯分布的制约条件，但是该算法也存在着一些问题，它依赖大量的样本数据才能更好地近似***的后验概率密度，计算量较大。

综上可知，EKF和PF有各自的优势，但也各有不足。因此，本发明提出一种非线性非高斯条件下基于EKF和PF的协同定位方法。

发明内容

本发明的目的是针对现有技术的不足，提供一种非线性非高斯条件下基于EKF和PF的协同定位方法。

步骤(1)、利用nanoLOC Development kit 3.0开发套件在定位空间中部署目标节点和基站，读取目标节点分别与各基站的TOA原始数据，计算得到目标节点与各基站之间的距离，其中M为基站总个数；

步骤(2)、利用Wylie鉴别法判断r_m是否有NLOS误差；

步骤(3)、将TOA值做差得到TDOA值，并对所有TDOA值对应的距离差r_m1进行重构；

步骤(4)、使用EKF TDOA算法估算t_k时刻目标节点的位置坐标并进行判断；

步骤(5)、使用PF TDOA算法估算t_k时刻目标节点的位置坐标并进行判断；

步骤(6)、对步骤(4)，(5)的两个位置坐标进行判断，残差加权得到t_k时刻的最终估计值；

步骤(7)、对步骤(6)得到的所有定位坐标数据进行加权平滑，得到目标节点的最终估计位置坐标与目标节点的真实坐标(x,y)进行比较，本发明所使用方法的定位精度优于nanoLOC开发套件自带的定位方法的定位精度，也优于单独使用EKF算法或PF算法的定位精度。

该方法具体步骤如下：

CSS(Chirp Spread Spectrum，线性调频扩频)是以IEEE802.15.4a协议为基础开发的无线通信技术。本发明的定位实验平台采用Nanotron公司nanoLOC Development kit3.0开发套件，该开发套件基于nanoLOC TRX射频芯片，可用于开发基于CSS技术的通信、测距、定位等无线应用。本发明将使用这套开发套件进行定位***的搭建。

本发明的定位***利用nanoLOC Development kit 3.0开发套件在定位空间中部署目标节点和基站，采用M个基站定位一个目标节点；其中，M个基站按照逆时针顺序分别命名为A₁，A₂，…，A_m，…，A_M，目标节点命名为Tag，经过测量得到实际坐标值为(x,y)。结合图1来说明本发明的具体实施步骤：

步骤一：首先读取目标节点与基站的TOA原始数据，计算得到目标节点与M个基站之间的距离r_m(m＝1，2，…，M)。

步骤二：利用Wylie鉴别法判断目标节点与基站之间的距离r_m是否有NLOS误差。

Wylie鉴别法具体流程如下：

将定位时刻记为t_k＝0,t₁,…,t_K，其中t_K为总定位时间，T为两个相邻时刻的间隔时间，假设目标节点与每个基站的距离测量结果通过多项式拟合来进行平滑处理，即

其中，J-1表示多项式的阶数；r_m(t_k)表示目标节点在t_k时刻与第m个基站的距离。

利用最小二乘法求解出未知系数可得到平滑后的测量值为

以S_m(t_k)作为真实距离参考值，计算r_m(t_k)的距离测量标准偏差，可表示为

假设LOS环境下的距离测量标准偏差为σ_m，在实际应用中，σ_m可预先根据实验现场测量获得。

NLOS环境下，由于NLOS误差与LOS环境下的测量误差同时存在，可以得到以下结果：

①在LOS环境下，可知

②在NLOS环境下，可知

所得标准偏差越大，表明受NLOS影响越大。

步骤三：通过步骤二可以鉴别出目标节点与所有基站间的信号传播是否受NLOS影响。将测得的距离测量标准偏差从小到大排序，选出所有r_m中受NLOS影响最小的值，将其设为r₁，对应基站作为参考基站，坐标为(x₁,y₁)；目标节点与其余基站之间的距离按照基站的逆时针分布顺序分别设为r₂,…,r_M，对应基站坐标分别设为(x₂,y₂),…,(x_M,y_M)。

将TOA值转换生成TDOA值，并得到对应的距离差为：

r_m1＝r_m-r₁,2≤m≤M (4)

由实际测量统计可以得出：若目标节点与基站为LOS传播，则平均时延对应的距离为η_LOS。若目标节点与基站为NLOS传播，则平均时延对应的距离为η_NLOS。对所有TDOA值对应的距离差r_m1的值进行重构，方法如下：

①若r_m和r₁均为LOS传播或NLOS传播，则r_m1＝r_m-r₁；

②若r_m为NLOS传播，r₁为LOS传播，则r_m1＝(r_m-η_NLOS)-(r₁-η_LOS)；

注意：因为所有r_m值已经利用Wylie鉴别法判断并排序，所以r₁的距离测量标准差一定比r_m的小，故不存在r_m为LOS传播而r₁为NLOS传播的情况。

步骤四：将TDOA值对应的距离差r_m1进行视距重构后，通过Kalman TDOA算法计算得到目标节点在t_k时刻的理论坐标值然后，设置第一阈值δ₁，根据不等式组：

判断是否满足公式(5)，若满足，则保留此次定位结果继续下一步；否则放弃下一步计算，结束此次定位过程，返回步骤一重新读取原始TOA数据。

步骤五：经过上述四个步骤，可以认为此次定位所用数据是较准确的数据；利用该数据，通过PF TDOA算法计算得到目标节点在t_k时刻的理论坐标值具体如下：

5.1建立目标节点运动的状态方程和观测方程如下所示：

X_tk＝F·X_t(k-1)+S·W_t(k-1) (6)

Z_tk＝f(X_tk)+V_tk (7)

式中，F为状态转移矩阵；S为干扰转移矩阵；W_t(k-1)和V_tk分别为过程噪声和观测噪声；Z_tk为目标节点在t_k时刻与所有基站的TDOA观测值转换得到的距离差；为目标节点在时刻t_k的状态信息，X_t(k-1)为目标节点在时刻t_k-1的状态信息，其中为t_k时刻目标节点的位置坐标，为t_k时刻目标节点的速度(该速度可由定位设备中的加速度传感器、方向传感器估算得到，或通过预先设置目标节点的初始速度和加速度计算得到)，转移矩阵为：

5.2根据目标节点运动的状态方程和观测方程，建立似然概率密度函数，见公式(9)：

其中，为目标节点在t_k时刻与第m个基站的TDOA预测值转换得到的距离差，为目标节点在t_k时刻与第m个基站的TDOA观测值转换得到的距离差，σ_v为观测噪声V_tk的方差。表示t_k时刻第i个粒子的状态信息，i＝1,2,…，N，N为粒子总数；

5.3通过5.1和5.2得到了状态方程和观测方程以及似然概率密度函数之后，结合PF TDOA算法滤波过程，得到定位结果更新步骤如下：

(1)初始化:t_k＝0

由先验分布p(x₀)产生粒子群所有粒子权值为1/N。其中，p(x₀)由目标节点已知的初始状态信息结合状态方程得到。

(2)for t_k＝t₁,…,t_K

①在t_k时刻，更新粒子权值

并且归一化

②重采样，解决粒子退化的问题，即随着迭代的一步步进行，很多粒子的权值会变得很小，只有少数的粒子权值较大，状态空间中的有效粒子数减少的问题。

利用有效抽样尺度N_eff来衡量粒子权值的退化程度

设定一个有效样本数N_threshold作为阈值，若N_eff＜N_threshold，则进行重采样，得到新的粒子群所有粒子权值置为1/N。

③状态估计

目标节点在t_k时刻的状态信息为

得到目标节点在t_k时刻的理论坐标值

④更新状态信息

将步骤六中得到的t_k时刻目标节点的最终定位坐标值(x_tk,y_tk)赋值给

end

步骤六：设置第二阈值δ₂，利用不等式：

判断步骤四、五得到的两次估计结果和是否相近，若满足不等式，则利用残差加权公式(15)得到t_k时刻目标节点的最终定位坐标值(x_tk,y_tk)，利用该坐标值分别更新EKF TDOA算法和PF TDOA算法中目标节点前一时刻的状态信息；若不满足不等式，则结束此次定位过程，返回步骤一读取原始数据。

其中，对应的残差平方和为

对应的残差平方和为

阈值δ₁、δ₂的选取主要是根据定位设备的理论精度进行初始选定，再经过实际的实验测试进行微调，以此实现过滤受NLOS误差影响的数据，提高定位精度。

步骤七、通过步骤六，将会得到α(α≤K)个定位坐标值(x_tk,y_tk)，给予每个数据加权系数，进行加权平滑得到最终估计结果其中，每个数据的加权系数与获取到该数据的时间早晚有关，数据获取得越早，加权系数越小，获取得越晚，加权系数越大。

对于定位方法性能的评价，定位精度是重要指标之一。在本发明中，定位精度由理论估计位置坐标和真实位置坐标(x,y)之间的接近程度来衡量：

根据设备使用手册的说明，在LOS环境下下，nanoLOC Development kit 3.0开发套件能达到室内2米，室外1米的定位精度。将本发明提出的协同定位方法应用于该开发套件中，可以获得比其更好的定位精度，也将优于单独使用已有的EKF或PF算法。

本发明的有益效果是：利用Wylie鉴别法对NLOS误差进行识别和抑制，初步排除不正确数据；同时考虑EKF和PF两种TDOA估计算法的定位性能，提出一种非线性非高斯条件下基于EKF和PF的协同定位方法，该方法较EKF而言更适用于非线性非高斯的定位环境，较PF而言较好地避免了对不正确数据的使用，减少了计算量。该协同定位方法有效降低NLOS误差的影响，结合EKF和PF两者的优势，同时克服两者的不足，实现比较精确的定位。

附图说明

图1为本发明的定位流程图；

图2为本发明的传感节点TDOA定位模型图。

具体实施方式

本发明涉及室内定位跟踪技术领域，具体为一种非线性非高斯条件下基于EKF和PF的协同定位方法，减少了NLOS误差，结合EKF和PF两者的优势，克服了单独使用其中一种算法的不足。

以下结合附图对本发明做进一步说明。

CSS(Chirp Spread Spectrum，线性调频扩频)是以IEEE802.15.4a协议为基础开发的无线通信技术。本发明的定位实验平台采用Nanotron公司nanoLOC Development kit3.0开发套件，该开发套件基于nanoLOC TRX射频芯片，可用于开发基于CSS技术的通信、测距、定位等无线应用。本发明将使用这套开发套件进行定位***的搭建。

本发明的定位***利用nanoLOC Development kit 3.0开发套件在定位空间中部署目标节点和基站，采用四个基站定位一个目标节点，如图2所示；其中，四个基站按照逆时针顺序分别命名为A₁，A₂，A₃，A₄，目标节点命名为Tag，经过测量得到实际坐标值为(x,y)。结合图1来说明本发明的具体实施步骤：

步骤一：首先读取目标节点与基站的TOA原始数据，计算得到目标节点与四个基站之间的距离r_m(m＝1，2，3，4)。

步骤二：利用Wylie鉴别法判断值是否有NLOS误差。

Wylie鉴别法具体流程如下：

将定位时刻记为t_k＝0,t₁,…,t_K，其中t_K为总定位时间,T为两个相邻时刻的间隔时间，假设每个基站与目标节点间的距离测量结果通过多项式拟合来进行平滑处理，即

其中，J-1表示多项式的阶数；r_m(t_k)表示目标节点在t_k时刻与第m个基站的距离。

利用最小二乘法求解出未知系数可得到平滑后的测量值为

以s_m(t_k)作为真实距离参考值，计算r_m(t_k)的距离测量标准偏差，可表示为

假设LOS环境下的距离测量标准偏差为σ_m，在实际应用中，σ_m可预先根据实验现场测量获得。

NLOS环境下，由于NLOS误差与LOS环境下的测量误差同时存在，可以得到以下结果：

①在LOS环境下，可知

②在NLOS环境下，可知

所得标准偏差越大，表明受NLOS影响越大。

步骤三：通过步骤二可以鉴别出目标节点与所有基站间的传播是否受NLOS影响。将测得的距离测量标准偏差从小到大排序，选出所有r_m中受NLOS影响最小的值，将其设为r₁，对应基站作为参考基站，坐标为(x₁,y₁)；目标节点与其余基站之间的距离按照基站的逆时针分布顺序分别设为r₂,r₃,r₄，对应基站坐标分别设为(x₂,y₂),(x₃,y₃),(x₄,y₄)。

将TOA值转换生成TDOA值，并得到对应的距离差为：

r_m1＝r_m-r₁,2≤m≤4 (4)

由实际测量统计可以得出：若目标节点与基站为LOS传播，则平均时延对应的距离为η_LOS。若目标节点与基站为NLOS传播，则平均时延对应的距离为η_NLOS。对所有TDOA值对应的距离差r_m1的值进行重构，方法如下：

①若r_m和r₁均为LOS传播或NLOS传播，则r_m1＝r_m-r₁；

②若r_m为NLOS传播，r₁为LOS传播，则r_m1＝(r_m-η_NLOS)-(r₁-η_LOS)；

注意：因为所有r_m值已经利用Wylie鉴别法判断并排序，所以r₁的距离测量标准差一定比r_m的小，故不存在r_m为LOS传播而r₁为NLOS传播的情况。

步骤四：将TDOA值对应的距离差r_m1进行视距重构后，通过Kalman TDOA算法计算得到目标节点在t_k时刻的理论坐标值然后，设置第一阈值δ₁，根据不等式组：

判断是否满足公式(5)，若满足，则保留此次定位结果继续下一步；否则放弃下一步计算，结束此次定位过程，返回步骤一重新读取原始TOA数据。

步骤五：经过上述四个步骤，可以认为此次定位所用数据是较准确的数据；利用该数据，通过PF TDOA算法计算得到目标节点在t_k时刻的理论坐标值具体如下：

5.1建立目标节点运动的状态方程和观测方程如下所示：

X_tk＝F·X_t(k-1)+S·W_t(k-1) (6)

Z_tk＝f(X_tk)+V_tk (7)

式中，F为状态转移矩阵；S为干扰转移矩阵；W_t(k-1)和V_tk分别为过程噪声和观测噪声；Z_tk为目标节点在t_k时刻与所有基站的TDOA观测值转换得到的距离差；为目标节点在时刻t_k的状态信息，X_t(k-1)为目标节点在时刻t_k-1的状态信息，其中为t_k时刻目标节点的位置坐标，为t_k时刻目标节点的速度(该速度可由定位设备中的加速度传感器、方向传感器估算得到，或通过预先设置目标节点的初始速度和加速度计算得到)，转移矩阵为：

5.2根据目标节点运动的状态方程和观测方程，建立似然概率密度函数，见公式(9)：

其中，为目标节点在t_k时刻与第m个基站的TDOA预测值转换得到的距离差，为目标节点在t_k时刻与第m个基站的TDOA观测值转换得到的距离差，σ_v为观测噪声V_tk的方差。表示t_k时刻第i个粒子的状态信息，i＝1,2,…，N，N为粒子总数；

5.3通过5.1和5.2得到了状态方程和观测方程以及似然概率密度函数之后，结合PF TDOA算法滤波过程，得到定位结果更新步骤如下：

(3)初始化:t_k＝0

由先验分布p(x₀)产生粒子群所有粒子权值为1/N。其中，p(x₀)由目标节点已知的初始状态信息结合状态方程得到。

(4)for t_k＝t₁,…,t_K

①在t_k时刻，更新粒子权值

并且归一化

②重采样，解决粒子退化的问题，即随着迭代的一步步进行，很多粒子的权值会变得很小，只有少数的粒子权值较大，状态空间中的有效粒子数减少的问题。

利用有效抽样尺度N_eff来衡量粒子权值的退化程度

设定一个有效样本数N_threshold作为阈值，若N_eff＜N_threshold，则进行重采样，得到新的粒子群所有粒子权值置为1/N。

③状态估计

目标节点在t_k时刻的状态信息为

得到目标节点在t_k时刻的理论坐标值

④更新状态信息

将步骤六中得到的t_k时刻目标节点的最终定位坐标值(x_tk,y_tk)赋值给

end

步骤六：设置第二阈值δ₂，利用不等式：

判断步骤四、五得到的两次估计结果和是否相近，若满足不等式，则利用残差加权公式(15)得到t_k时刻目标节点的最终定位坐标值(x_tk,y_tk)，利用该坐标值分别更新EKF TDOA算法和PF TDOA算法中目标节点前一时刻的状态信息；若不满足不等式，则结束此次定位过程，返回步骤一读取原始数据。

其中，对应的残差平方和为

对应的残差平方和为

阈值δ₁、δ₂的选取主要是根据定位设备的理论精度进行初始选定，再经过实际的实验测试进行微调，以此实现过滤受NLOS误差影响的数据，提高定位精度。

步骤七通过步骤六，将会得到α(α≤K)个定位坐标值(x_tk,y_tk)，给予每个数据加权系数，进行加权平滑得到最终估计结果其中，每个数据的加权系数与获取到该数据的时间早晚有关，数据获取得越早，加权系数越小，获取得越晚，加权系数越大。

对于定位方法性能的评价，定位精度是重要指标之一。在本发明中，定位精度由理论估计位置坐标和真实位置坐标(x,y)之间的接近程度来衡量：

根据设备使用手册的说明，在LOS环境下，nanoLOC Development kit 3.0开发套件能达到室内2米，室外1米的定位精度。将本发明提出的协同定位方法应用于该开发套件中，可以获得比其更好的定位精度，也将优于单独使用已有的EKF或PF算法。

总之，本发明旨在减少NLOS误差对TDOA值的影响，将EKF和PF相互结合，充分利用各自的优势，同时克服单独使用一种算法的不足，使得定位方法更适用于非线性非高斯的情况，提高定位精度的同时减少算法计算量。

以上通过参考附图对本发明做了特别的展示和说明，对本领域的技术人员来说，应该明白，在不背离本发明的思想和范围下做出在形式上和细节上的各种修改和改变，都将是对本发明专利的侵犯。因此本发明要保护的真正思想和范围由所附的权利要求书来限定。

Claims (1)

1.非线性非高斯条件下基于扩展卡尔曼滤波和粒子滤波的协同定位方法，其特征在于该方法包括以下步骤：

步骤(1)、获取目标节点分别与各基站的TOA原始数据，计算得到目标节点与各基站之间的距离r_m(m＝1，2，…，M)，其中M为基站总个数；

步骤(2)、利用Wylie鉴别法判断是否有NLOS误差；

步骤(3)、将TOA值做差得到TDOA值，并对所有TDOA值对应的距离差r_m1进行重构；

步骤(4)、使用扩展卡尔曼滤波TDOA算法估算t_k时刻目标节点的位置坐标并进行判断；

步骤(5)、使用粒子滤波TDOA算法估算t_k时刻目标节点的位置坐标并进行判断；

步骤(6)、对步骤(4)、(5)的两个位置坐标进行判断，残差加权得到t_k时刻的最终估计值；

步骤(7)、对步骤(6)得到的所有定位坐标数据进行加权平滑，得到目标节点的最终估计位置坐标

步骤(2)具体如下：

将定位时刻记为t_k＝0,t₁,…,t_K，其中t_K为总定位时间，T为两个相邻时刻的间隔时间，假设目标节点与每个基站的距离测量结果通过多项式拟合来进行平滑处理，即

其中，J-1表示多项式的阶数；r_m(t_k)表示目标节点在t_k时刻与第m个基站的距离；

利用最小二乘法求解出未知系数可得到平滑后的测量值为

以S_m(t_k)作为真实距离参考值，计算r_m(t_k)的距离测量标准偏差，可表示为

假设LOS环境下的距离测量标准偏差为σ_m，可预先根据实验现场测量获得；

若则判断目标节点与基站之间是LOS传播；若则判断目标节点与基站之间是NLOS传播；

步骤(3)具体如下：

若存在NLOS传播，将距离测量标准偏差中最小值所对应的目标节点与对应基站的距离设为r₁，该基站作为参考基站，坐标为(x₁,y₁)；目标节点与其余基站之间的距离设为r₂,…,r_M，对应基站坐标分别设为(x₂,y₂),…,(x_M,y_M)；

将TOA值转换生成TDOA值，并对所有TDOA值对应的距离差r_m1的值进行重构：

若r_m和r₁均为LOS传播或NLOS传播，则r_m1＝r_m-r₁；

若r_m为NLOS传播，r₁为LOS传播，则r_m1＝(r_m-η_NLOS)-(r₁-η_LOS)；

其中η_LOS为目标节点与基站间为LOS传播时的平均时延对应距离；η_NLOS为目标节点与基站间为NLOS传播时平均时延对应距离；

步骤(4)具体如下：

通过Kalman TDOA算法计算得到目标节点在t_k时刻的理论坐标值然后根据公式(5)进行判断；若满足，则保留此次定位结果继续下一步；否则放弃下一步计算，结束此次定位过程，返回步骤(1)重新读取原始TOA数据；

其中δ₁为阈值，可根据定位设备的理论精度和实际实验进行人为选定；

步骤(5)通过粒子滤波TDOA算法计算得到目标节点在t_k时刻的理论坐标值具体如下：

5.1建立目标节点运动的状态方程和观测方程如下所示：

X_tk＝F·X_t(k-1)+S·W_t(k-1) (6)

Z_tk＝f(X_tk)+V_tk (7)

式中，F为状态转移矩阵，见公式(8)；S为干扰转移矩阵，见公式(8)；W_t(k-1)和V_tk分别为过程噪声和观测噪声；Z_tk为目标节点在t_k时刻与所有基站的TDOA观测值转换得到的距离差；为目标节点在时刻t_k的状态信息，X_t(k-1)为目标节点在时刻t_k-1的状态信息，其中为t_k时刻目标节点的位置坐标，为t_k时刻目标节点的速度；

5.2根据目标节点运动的状态方程和观测方程，建立似然概率密度函数，见公式(9)：

其中，为目标节点在t_k时刻与第m个基站的TDOA预测值转换得到的距离差，为目标节点在t_k时刻与第m个基站的TDOA观测值转换得到的距离差，σ_v为观测噪声V_tk的方差；表示t_k时刻第i个粒子的状态信息，i＝1,2,…，N，N为粒子总数；

5.3通过5.1和5.2得到了状态方程和观测方程以及似然概率密度函数之后，结合粒子滤波TDOA算法滤波过程，得到定位结果更新步骤如下：

a)初始化:t_k＝0

由先验分布p(x₀)产生粒子群所有粒子权值为1/N；其中，p(x₀)由目标节点已知的初始状态信息结合状态方程得到；

b)t_k＝t₁,…,t_K

①在t_k时刻，更新粒子权值

并且归一化

②重采样；

利用有效抽样尺度N_eff来衡量粒子权值的退化程度

设定一个有效样本数N_threshold作为阈值，若N_eff＜N_threshold，则进行重采样，得到新的粒子群所有粒子权值置为1/N；

③状态估计

目标节点在t_k时刻的状态信息为

得到目标节点在t_k时刻的理论坐标值

④更新状态信息

将步骤六中得到的t_k时刻目标节点的最终定位坐标值(x_tk,y_tk)赋值给

步骤(6)具体如下：

设置第二阈值δ₂，利用不等式：

判断步骤(4)、(5)得到的两次估计结果和是否相近，若满足不等式，则利用残差加权公式(15)得到t_k时刻目标节点的最终定位坐标值(x_tk,y_tk)，利用该坐标值分别更新扩展卡尔曼滤波TDOA算法和粒子滤波TDOA算法中目标节点前一时刻的状态信息；若不满足不等式，则结束此次定位过程，返回步骤(1)读取TOA原始数据；

其中，对应的残差平方和为

对应的残差平方和为