非线性非高斯条件下基于ekf和pf的协同定位方法
【专利摘要】本发明公开非线性非高斯条件下基于EKF和PF的协同定位方法。该方法获取目标节点与各基站的TOA原始数据，计算得到目标节点与各基站之间的距离；利用Wylie鉴别法判断是否有NLOS误差；TOA值做差得到TDOA值，重构TDOA值对应的距离差rm1；分别使用EKF算法和PF算法估算tk时刻目标节点的位置坐标；进行残差加权得到tk时刻的最终估计值；对所有时刻的位置坐标进行加权平滑即可得到最终定位结果。该方法较EKF更适用于非线性非高斯的定位环境，较PF有效避免了使用不正确数据，减少了计算量。该方法有效降低NLOS误差的影响，结合EKF和PF两者的优势，同时克服两者的不足，实现比较精确的定位。
【专利说明】
非线性非高斯条件下基于EKF和PF的协同定位方法
技术领域
[0001] 本发明涉及室内定位跟踪技术领域，具体为一种非线性非高斯条件下基于EKF和 PF的协同定位方法。
【背景技术】
[0002] 随着微电子技术、通信技术和计算机技术的快速发展，无线定位作为传感网和物 联网的重要应用，越来越受到人们的关注。当前，基于测距的定位方法主要有RSSI(到达信 号强度）、Α0Α(到达信号角度）、Τ0Α(信号到达时间）、TD0A(信号到达时差)等。其中，RSSI是 一种理论上比较理想的算法，将传播损耗转化为距离，但是在实际定位环境中，电磁波传播 具有动态特性，对测距结果造成严重干扰，会产生较大的测距误差;基于Α0Α测距的定位方 式所需要的硬件系统设备复杂，成本较高;Τ0Α和TD0A定位精度较高，定位时间短，系统实现 简单，具有一定的抗多径能力，所以具有较好的应用前景。其中，TD0A的估计方法一般有两 种:一种是求两个基站的信号到达时间Τ0Α之差来获得TD0A值；另一种是采用互相关技术， 将一个基站接收到的信号与另一个基站接收到的信号进行互相关运算计算出TD0A值。
[0003] 室内环境一般较为复杂，目标节点和基站之间的信号传播很可能存在NL0S误差 (非视距传播）。采用Τ0Α和TD0A技术对目标节点进行定位估计时，Τ0Α值中会产生一个正的 附加超量时延，TD0A值也会对应产生一个误差分量。将这种具有较大误差的Τ0Α或TD0A值应 用于目标节点的定位估计，必然造成定位算法性能的显著下降，使估计位置出现较大误差。 因此，如何鉴别和抑制NL0S误差成为了提高定位精度的一个重要因素。对此，Wylie鉴别法 是一种比较有效的方法，它首先从测量参数误差的方差来判断是否为视距传播，若不是视 距传播，则重构视距传播值，并对重构的视距值进行滤波，然后用视距定位算法估计移动台 的位置。
[0004] 卡尔曼滤波是在线性高斯情况下利用最小均方误差准则进行目标动态估计的最 优滤波方法。不过对于非线性非高斯的情况，还需要对其进行改进和扩展，而扩展卡尔曼滤 波(EKF,Extended Kalman Filter)是一种比较有效的改进方法。粒子滤波(PF，Particle Filter)是一种基于蒙特卡罗思想的非线性、非高斯系统滤波方法，它完全突破了卡尔曼滤 波理论框架，对系统的过程噪声和量测噪声没有任何限制，在处理非线性、非高斯时变系统 的参数估计和状态滤波问题方面有独特的优势和广阔的前景。
[0005] 在实际应用过程中，EKF是一种比较常用的非线性滤波方法，但是它仅适用于滤波 误差和预测误差很小的情况，否则可能会导致滤波不稳定甚至发散;PF摆脱了解决非线性 滤波问题时随机量必须满足高斯分布的制约条件，但是该算法也存在着一些问题，它依赖 大量的样本数据才能更好地近似系统的后验概率密度，计算量较大。
[0006] 综上可知，EKF和PF有各自的优势，但也各有不足。因此，本发明提出一种非线性非 高斯条件下基于EKF和PF的协同定位方法。
【发明内容】
[0007]本发明的目的是针对现有技术的不足，提供一种非线性非高斯条件下基于EKF和 PF的协同定位方法。
[0008] 步骤(1)、利用nanoLOC Development kit 3.0开发套件在定位空间中部署目标节 点和基站，读取目标节点分别与各基站的Τ0Α原始数据，计算得到目标节点与各基站之间的 距离，其中Μ为基站总个数；
[0009] 步骤(2)、利用Wylie鉴别法判断rm是否有NL0S误差；
[0010] 步骤(3)、将Τ0Α值做差得到TD0A值，并对所有TD0A值对应的距离差rml进行重构；
[0011] 步骤(4)、使用EKF TD0A算法估算tk时刻目标节点的位置坐标并进行判断；
[0012]步骤(5)、使用PF TD0A算法估算tk时刻目标节点的位置坐标并进行判断；
[0013] 步骤(6)、对步骤(4)，（5)的两个位置坐标进行判断，残差加权得到tk时刻的最终 估计值；
[0014] 步骤(7)、对步骤(6)得到的所有定位坐标数据进行加权平滑，得到目标节点的最 终估计位置坐标^刃，与目标节点的真实坐标(x，y)进行比较，本发明所使用方法的定位 精度优于nanoLOC开发套件自带的定位方法的定位精度，也优于单独使用EKF算法或PF算法 的定位精度。
[0015] 该方法具体步骤如下：
[0016] CSS(Chirp Spread Spectrum，线性调频扩频)是以IEEE802.15.4a协议为基础开 发的无线通信技术。本发明的定位实验平台采用Nanotron公司nanoLOC Development kit 3.0开发套件，该开发套件基于nanoLOC TRX射频芯片，可用于开发基于CSS技术的通信、测 距、定位等无线应用。本发明将使用这套开发套件进行定位系统的搭建。
[0017] 本发明的定位系统利用nanoLOC Development kit 3.0开发套件在定位空间中部 署目标节点和基站，采用Μ个基站定位一个目标节点；其中，Μ个基站按照逆时针顺序分别命 名为，…，Am，目标节点命名为Tag，经过测量得到实际坐标值为(x，y)。结合图1 来说明本发明的具体实施步骤：
[0018]步骤一:首先读取目标节点与基站的Τ0Α原始数据，计算得到目标节点与Μ个基站 之间的距离rm(m=l，2,…，Μ)。
[0019] 步骤二:利用Wylie鉴别法判断目标节点与基站之间的距离是否有NL0S误差。
[0020] Wylie鉴别法具体流程如下：
[0021 ]将定位时刻记为tk = 0，t，…，tK，其中tK为总定位时间，T为两个相邻时刻的间隔 时间，假设目标节点与每个基站的距离测量结果通过多项式拟合来进行平滑处理，即
[0022]
(1)
[0023]其中，J-1表示多项式的阶数;rm(tk)表示目标节点在tk时刻与第m个基站的距离。
[0024] 利用最小二乘法求解出未知系数可得到平滑后的测量值为
[0025]
(2)
[0026] &Sm(tk)作为真实距离参考值，计算r m(tk)的距离测量标准偏差，可表示为
[0027]
(3)
[0028] 假设LOS环境下的距离测量标准偏差为〇m，在实际应用中，〇m可预先根据实验现场 测量获得。
[0029] NL0S环境下，由于NL0S误差与L0S环境下的测量误差同时存在，可以得到以下结 果：
[0030] ①在L0S环境下，可知七
[0031] ②在NL0S环境下，可知。
[0032] 所得标准偏差越大，表明受NL0S影响越大。
[0033]步骤三:通过步骤二可以鉴别出目标节点与所有基站间的信号传播是否受NL0S影 响。将测得的距离测量标准偏差 从小到大排序，选出所有中受NL0S影响最小的值，将其 设为n，对应基站作为参考基站，坐标为(X1， yi);目标节点与其余基站之间的距离按照基站 的逆时针分布顺序分别设为Γ2,…,ΓΜ，对应基站坐标分别设为(X2，y2)，…，（XM，yM)。
[0034] 将Τ0Α值转换生成TD0A值，并得到对应的距离差为：
[0035] rmi = rm-ri,2 <m<M (4)
[0036] 由实际测量统计可以得出：若目标节点与基站为LOS传播，则平均时延对应的距离 为riLos。若目标节点与基站为NL0S传播，则平均时延对应的距离为riNLCis。对所有TD0A值对应 的距离差r ml的值进行重构，方法如下：
[0037] ①若rjPn均为L0S传播或NL0S传播，则rml = rm-n;
[0038] ②若rm为NL0S传播，ri为LOS传播，则rmi= (rm-riNLQS)-(:ri-riL〇s);
[0039] 注意：因为所有^值已经利用Wylie鉴别法判断并排序，所以Γ1的距离测量标准差 一定比rm的小，故不存在rm为L0S传播而ri为NL0S传播的情况。
[0040] 步骤四：将TD0A值对应的距离差rml进行视距重构后，通过Kalman TD0A算法计算得 到目标节点在tk时刻的理论坐标值(1,?);然后，设置第一阈值31，根据不等式组：
[0041 ]
(5)
[0042] 判断是否满足公式(5)，若满足，则保留此次定位结果(心,九.），继续下一步；否则 放弃下一步计算，结束此次定位过程，返回步骤一重新读取原始Τ0Α数据。
[0043] 步骤五:经过上述四个步骤，可以认为此次定位所用数据是较准确的数据;利用该 数据，通过PF TD0A算法计算得到目标节点在tk时刻的理论坐标值，具体如下：
[0044] 5.1建立目标节点运动的状态方程和观测方程如下所示：
[0045] Xtk = F · Xt(k-i)+S · fft(k-i) (6)
[0046] Ztk = f(Xtk)+Vtk (7)
[0047] 式中，F为状态转移矩阵;S为干扰转移矩阵;Wt(k-:〇和Vtk分别为过程噪声和观测噪 声；Z tk为目标节点在tk时刻与所有基站的TD0A观测值转换得到的距离差； =[义名,:4，九]7为目标节点在时刻tk的状态信息，Xt(k- υ为目标节点在时刻tk-必状态信 息，其中氏，九]为"时刻目标节点的位置坐标，[乜九]为站寸刻目标节点的速度(该速度可 由定位设备中的加速度传感器、方向传感器估算得到，或通过预先设置目标节点的初始速 度和加速度计算得到），转移矩阵为：
[0048]
(8)
[0049] 5.2根据目标节点运动的状态方程和观测方程，建立似然概率密度函数，见公式 (9)：
[0050]
(9)
[0051] 其中，念；为目标节点在tk时刻与第m个基站的TD0A预测值转换得到的距离差， 为目标节点在tk时刻与第m个基站的TD0A观测值转换得到的距离差， 〇v为观测噪声Vtk的方 差。&表示tk时刻第i个粒子的状态信息，i = 1，2，…，N，N为粒子总数；
[0052] 5.3通过5.1和5.2得到了状态方程和观测方程以及似然概率密度函数之后，结合 PF TD0A算法滤波过程，得到定位结果更新步骤如下：
[0053] (1)初始化:tk = 〇
[0054] 由先验分布p (xo)产生粒子群!不匕，所有粒子权值为1/N。其中，p (xo)由目标节点 已知的初始状态信息结合状态方程得到。
[0055] (2)for tk = ti,-",tK
[0056] ①在tk时刻，更新粒子权值
[0057] (1〇)
[0058] 并且归一化
[0059]
(11)
[0060] ②重采样，解决粒子退化的问题，即随着迭代的一步步进行，很多粒子的权值会变 得很小，只有少数的粒子权值较大，状态空间中的有效粒子数减少的问题。
[0061 ]利用有效抽样尺度Nrff来衡量粒子权值的退化程度
Π2)
[0062] ' i=l
[0063] 设定一个有效样本数Nthreshold作为阈值，若Neff〈Nthreshold，则进行重采样，得到新 的粒子群G，所有粒子权值置为1 /N。
[0064]③状态估计
[0065] 目标节点在tk时刻的状态信息为
[0066]
[0067] 得到目标节点在tk时刻的理论坐标值) = (I J':*)。 (13)
[0068] ④更新状态信息
[0069] 将步骤六中得到的tk时刻目标节点的最终定位坐标值(xtk，ytk)赋值给。
[0070] end
[0071 ]步骤六:设置第二阈值δ2，利用不等式：
[0072]
Π 4)
[0073] 判断步骤四、五得到的两次估计结果(H)和(??)是否相近，若满足不等式， 则利用残差加权公式（15)得到tk时刻目标节点的最终定位坐标值(xtk，ytk)，利用该坐标值 分别更新EKF TD0A算法和PF TD0A算法中目标节点前一时刻的状态信息;若不满足不等式， 则结束此次定位过程，返回步骤一读取原始数据。
(15)
[0076] 其中，(?)对应的残差平方和为
[0077] _
(16) m=2
[0078] 对应的残差平方和为
[0079]
(1-7).
[0080] 阈值δ!;〗的选取主要是根据定位设备的理论精度进行初始选定，再经过实际的实 验测试进行微调，以此实现过滤受NL0S误差影响的数据，提高定位精度。
[0081] 步骤七、通过步骤六，将会得到α(α<Κ)个定位坐标值(Xtk，ytk)，给予每个数据加 权系数，进行加权平滑得到最终估计结果(元歹)。其中，每个数据的加权系数与获取到该数 据的时间早晚有关，数据获取得越早，加权系数越小，获取得越晚，加权系数越大。
[0082] 对于定位方法性能的评价，定位精度是重要指标之一。在本发明中，定位精度由理 论估计位置坐标(.?，?·)和真实位置坐标(x，y)之间的接近程度来衡量：
[0083]
(1S)
[0084] 根据设备使用手册的说明，在L0S环境下下，nanoLOC Development kit 3.0开发 套件能达到室内2米，室外1米的定位精度。将本发明提出的协同定位方法应用于该开发套 件中，可以获得比其更好的定位精度，也将优于单独使用已有的EKF或PF算法。
[0085]本发明的有益效果是:利用Wylie鉴别法对NL0S误差进行识别和抑制，初步排除不 正确数据；同时考虑EKF和PF两种TD0A估计算法的定位性能，提出一种非线性非高斯条件下 基于EKF和PF的协同定位方法，该方法较EKF而言更适用于非线性非高斯的定位环境，较PF 而言较好地避免了对不正确数据的使用，减少了计算量。该协同定位方法有效降低NL0S误 差的影响，结合EKF和PF两者的优势，同时克服两者的不足，实现比较精确的定位。
【附图说明】
[0086]图1为本发明的定位流程图；
[0087]图2为本发明的传感节点TD0A定位模型图。
【具体实施方式】
[0088]本发明涉及室内定位跟踪技术领域，具体为一种非线性非高斯条件下基于EKF和 PF的协同定位方法，减少了 NL0S误差，结合EKF和PF两者的优势，克服了单独使用其中一种 算法的不足。
[0089 ]以下结合附图对本发明做进一步说明。
[0090] CSS(Chirp Spread Spectrum，线性调频扩频)是以IEEE802.15.4a协议为基础开 发的无线通信技术。本发明的定位实验平台采用Nanotron公司nanoLOC Development kit 3.0开发套件，该开发套件基于nanoLOC TRX射频芯片，可用于开发基于CSS技术的通信、测 距、定位等无线应用。本发明将使用这套开发套件进行定位系统的搭建。
[0091] 本发明的定位系统利用nanoLOC Development kit 3.0开发套件在定位空间中部 署目标节点和基站，采用四个基站定位一个目标节点，如图2所示;其中，四个基站按照逆时 针顺序分别命名为此知，知山，目标节点命名为1&8，经过测量得到实际坐标值为(1，7)。结 合图1来说明本发明的具体实施步骤：
[0092]步骤一:首先读取目标节点与基站的Τ0Α原始数据，计算得到目标节点与四个基站 之间的距离rm(m=l，2,3,4)。
[0093 ]步骤二:利用Wy 1 ie鉴别法判断T〇4.值是否有NL0S误差。
[0094] Wylie鉴别法具体流程如下：
[0095] 将定位时刻记为" = 0，"，…，tK，其中tK为总定位时间，T为两个相邻时刻的间隔 时间，假设每个基站与目标节点间的距离测量结果通过多项式拟合来进行平滑处理，即
[0096](1) j--y
[0097] 其中，J-1表示多项式的阶数;rm(tk)表示目标节点在tk时刻与第m个基站的距离。
[0098] 利用最小二乘法求解出未知系数从，可得到平滑后的测量值为
[0099]
(2)： 7=〇
[0100] Wsm(tk)作为真实距离参考值，计算rm(tk)的距离测量标准偏差，可表示为
[0101]
(3)
[0102] 假设L0S环境下的距离测量标准偏差为，在实际应用中，可预先根据实验现场 测量获得。
[0103] NL0S环境下，由于NL0S误差与L0S环境下的测量误差同时存在，可以得到以下结 果：
[0104] ①在L0S环境下，可知心;
[0105] ②在NL0S环境下，可知
[0106] 所得标准偏差越大，表明受NL0S影响越大。
[0107] 步骤三:通过步骤二可以鉴别出目标节点与所有基站间的传播是否受NL0S影响。 将测得的距离测量标准偏差之从小到大排序，选出所有中受NL0S影响最小的值，将其设 为n，对应基站作为参考基站，坐标为( X1，yi);目标节点与其余基站之间的距离按照基站的 逆时针分布顺序分别设为r2，n，r4,对应基站坐标分别设为(X2，y2)，（X3，y3)，（X4，y4)。
[0108] 将Τ0Α值转换生成TD0A值，并得到对应的距离差为：
[0109] rmi = rm-ri ,2 < m < 4 (4)
[0110] 由实际测量统计可以得出：若目标节点与基站为LOS传播，则平均时延对应的距离 为nLQS。若目标节点与基站为NL0S传播，则平均时延对应的距离为I1NLQS。对所有TD0A值对应 的距离差r ml的值进行重构，方法如下：
[0111] ①若rm和ri均为L0S传播或NL0S传播，则rmi = rm-ri;
[0112] ②若rm为NL0S传播，ri为LOS传播，则rmi= (rm-riNLC)s)-(:ri-riL〇s);
[0113] 注意：因为所有^值已经利用Wylie鉴别法判断并排序，所以Γ1的距离测量标准差 一定比rm的小，故不存在rm为L0S传播而ri为NL0S传播的情况。
[0114] 步骤四：将TD0A值对应的距离差rml进行视距重构后，通过Kalman TD0A算法计算得 到目标节点在tk时刻的理论坐标值然后，设置第一阈值31，根据不等式组：
[0115:
(?)
[0116] 判断是否满足公式(5)，若满足，则保留此次定位结果(U,t)，继续下一步；否则 放弃下一步计算，结束此次定位过程，返回步骤一重新读取原始Τ0Α数据。
[0117] 步骤五:经过上述四个步骤，可以认为此次定位所用数据是较准确的数据;利用该 数据，通过PF TD0A算法计算得到目标节点在tk时刻的理论坐标值(^,?)，具体如下：
[0118] 5.1建立目标节点运动的状态方程和观测方程如下所示：
[0119] Xtk = F · Xt(k-i)+S · fft(k-i) (6)
[0120] Ztk = f(Xtk)+Vtk (7)
[0121] 式中，F为状态转移矩阵;S为干扰转移矩阵;WtH)和Vtk分别为过程噪声和观测噪 声；Ztk为目标节点在tk时刻与所有基站的TD0A观测值转换得到的距离差； A = 为目标节点在时刻tk的状态信息，Xt(k-1)为目标节点在时刻tk-必状态信 息，其中[4忑]为"时刻目标节点的位置坐标，[&，九]为tk时刻目标节点的速度(该速度可 由定位设备中的加速度传感器、方向传感器估算得到，或通过预先设置目标节点的初始速 度和加谏庶计笪得剞），鮮務钽陈为，
[0122]
後）
[0123] 5.2根据目标节点运动的状态方程和观测方程，建立似然概率密度函数，见公式 (9)：
[0124]
(9)
[0125] 其中，为目标节点在tk时刻与第m个基站的TD0A预测值转换得到的距离差， 为目标节点在tk时刻与第m个基站的TD0A观测值转换得到的距离差， 〇v为观测噪声Vtk的方 差。时刻第i个粒子的状态信息，i = 1，2，…，N，N为粒子总数；
[0126] 5.3通过5.1和5.2得到了状态方程和观测方程以及似然概率密度函数之后，结合 PF TD0A算法滤波过程，得到定位结果更新步骤如下：
[0127] (3)初始化:tk = 0
[0128] 由先验分布p (xo)产生粒子群认:匕，所有粒子权值为1/N。其中，p (xo)由目标节点 已知的初始状态信息结合状态方程得到。
[0129] (4)for tk = ti，."，tK
[0130] ①在tk时刻，更新粒子权值
(10) (11) z=l
[0134] ②重采样，解决粒子退化的问题，即随着迭代的一步步进行，很多粒子的权值会变 得很小，只有少数的粒子权值较大，状态空间中的有效粒子数减少的问题。
[0135] 利用有效抽样尺度Nrff来衡量粒子权值的退化程度
Π2)
[0136] 分别更新EKF TDOA算法和PF TDOA算法中目标节点前一时刻的状态信息;若不满足不等式， 则结束此次定位过程，返回步骤一读取原始数据。
(15)
[0150] 其中，(?)对应的残差平方和为
[0151]
(16)
[0152] 对应的残差平方和为
[0153]
(17)
[0154] 阈值的选取主要是根据定位设备的理论精度进行初始选定，再经过实际的实 验测试进行微调，以此实现过滤受NL0S误差影响的数据，提高定位精度。
[0155] 步骤七通过步骤六，将会得到α(ΚΚ)个定位坐标值(Xtk，ytk)，给予每个数据加权 系数，进行加权平滑得到最终估计结果(义.?·),其中，每个数据的加权系数与获取到该数据 的时间早晚有关，数据获取得越早，加权系数越小，获取得越晚，加权系数越大。
[0156] 对于定位方法性能的评价，定位精度是重要指标之一。在本发明中，定位精度由理 论估计位置坐标(元力和真实位置坐标(x，y)之间的接近程度来衡量：
[0157]
(!8)
[0158] 根据设备使用手册的说明，在L0S环境下，nanoLOC Development kit 3.0开发套 件能达到室内2米，室外1米的定位精度。将本发明提出的协同定位方法应用于该开发套件 中，可以获得比其更好的定位精度，也将优于单独使用已有的EKF或PF算法。
[0159] 总之，本发明旨在减少NL0S误差对TD0A值的影响，将EKF和PF相互结合，充分利用 各自的优势，同时克服单独使用一种算法的不足，使得定位方法更适用于非线性非高斯的 情况，提高定位精度的同时减少算法计算量。
[0160] 以上通过参考附图对本发明做了特别的展示和说明，对本领域的技术人员来说， 应该明白，在不背离本发明的思想和范围下做出在形式上和细节上的各种修改和改变，都 将是对本发明专利的侵犯。因此本发明要保护的真正思想和范围由所附的权利要求书来限 定。
【主权项】
1. 非线性非高斯条件下基于EKF和PF的协同定位方法，其特征在于该方法包括以下步 骤： 步骤（1)、获取目标节点分别与各基站的TOA原始数据，计算得到目标节点与各基站之 间的距离rm(m=l，2,…，M)，其中M为基站总个数； 步骤(2)、利用Wylie鉴别法判断是否有NLOS误差； 步骤(3 )、将TOA值做差得到TDOA值，并对所有TDOA值对应的距离差rml进行重构； 步骤(4)、使用EKF TDOA算法估算tk时刻目标节点的位置坐标并进行判断； 步骤(5)、使用PF TDOA算法估算tk时刻目标节点的位置坐标并进行判断； 步骤(6)、对步骤(4)、（5)的两个位置坐标进行判断，残差加权得到tk时刻的最终估计 值； 步骤(7)、对步骤(6)得到的所有定位坐标数据进行加权平滑，得到目标节点的最终估 计位置坐标(又乃。2. 如权利要求1所述的非线性非高斯条件下基于EKF和PF的协同定位方法，其特征在于 步骤(2)具体如下： 将定位时刻记为U = OA1，…，tK，其中tK为总定位时间，T为两个相邻时刻的间隔时间， 假设目标节点与每个基站的距离测量结果通过多项式拟合来进行平滑处理，即(1) 其中，J-I表示多项式的阶数;rm(tk)表示目标节点在tk时刻与第m个基站的距离；利用最小二乘法求解出未知系数{^(/))·^，可得到平滑后的测量值为 (2) :)的距离测量标准偏差，可表示为 (3) 假设LOS环境下的距离测量标准偏差为，可预先根据实验现场测量获得； 若九，则判断目标节点与基站之间是LOS传播;若么,>σ";，则判断目标节点与基站 之间是NLOS传播。3. 如权利要求2所述的非线性非高斯条件下基于EKF和PF的协同定位方法，其特征在于 步骤(3)具体如下： 若存在NLOS传播，将距离测量标准偏差中最小值所对应的目标节点与对应基站的距 离设为η，该基站作为参考基站，坐标为（X1，yi);目标节点与其余基站之间的距离设为 Γ2，···，ΓΜ，对应基站坐标分别设为(X2J2)，…，（XMJM); 将TOA值转换生成TDOA值，并对所有TDOA值对应的距离差rml的值进行重构： 若rm和ri均为LOS传播或NLOS传播，则rmi = rm-ri; 若rm为NLOS传播，ri为LOS传播，则rmi= (rm-riNLQS^lHri-riLos); 其中nLQS为目标节点与基站间为LOS传播时的平均时延对应距离;mos为目标节点与基 站间为NLOS传播时平均时延对应距离。4. 如权利要求3所述的非线性非高斯条件下基于EKF和PF的协同定位方法，其特征在于 步骤(4)具体如下： 通过Kalman TDOA算法计算得到目标节点在tk时刻的理论坐标值(?,3^.);然后根据公 式(5)进行判断;若满足，则保留此次定位结果(?，尺），继续下一步；否则放弃下一步计算， 结亩此汝宙仿忖趕，坂冋步骤Π )重新读取原始TOA数据：其中S1为阈值，可根据定位设备的理论精度和实际实验进行人为选定。5. 如权利要求4所述的非线性非高斯条件下基于EKF和PF的协同定位方法，其特征在于 步骤(5)通过PF TDOA算法计算得到目标节点在tk时刻的理论坐标值(心,1)，具体如下： 5.1建立目标节点运动的状态方程和观测方程如下所示： Xtk=F · Xt(k-i)+S · fft(k-i) (6) Ztk=f (Xtk)+Vtk (7) 式中，F为状态转移矩阵，见公式(8) ;S为干扰转移矩阵，见公式(8) ;Wt(k〇和Vtk分别为 过程噪声和观测噪声；Ztk为目标节点在tk时刻与所有基站的TDOA观测值转换得到的距离 差；A ，瓦,.匕f为目标节点在时刻t k的状态信息，Xt0^1)为目标节点在时刻的状 态信息，其中[L JU为tk时刻目标节点的位置坐标，[勾，)\]为tk时刻目标节点的速度；(8) 5.2根据目标节点运动的状态方程和观测方程，建立似然概率密度函数，见公式(9): 似然概率密度可衷示为：(^) 其中，#为目标节点在tk时刻与第m个基站的TDOA预测值转换得到的距离差，g为目 标节点在tk时刻与第m个基站的TDOA观测值转换得到的距离差，σν为观测噪声Vtk的方差; 表不tk时刻第i个粒子的状态信息，i = 1，2，…，N，N为粒子总数； 5.3通过5.1和5.2得到了状态方程和观测方程以及似然概率密度函数之后，结合PF TDOA算法滤波过程，得到定位结果更新步骤如下： a) 初始化:tk = 0 由先验分布P(XO)产生粒子群所有粒子权值为1/N;其中，P(XO)由目标节点已知 的初始状态信息结合状态方程得到； b) tk = ti, ··· ,?κ ①在tk时刻，更新粒子权值 (10) 并且归一 {(11) ② 重采样；利用有效抽样尺度Neff来衡量粒子权值的退化程度(12) 设定一个有效样本数Nthmhold作为阈值，若NrfKNthreshold，则进行重采样，得到新的粒 子群!AC，所有粒子权值置为1/N; ③ 状态估计 目标节点在tk时刻的状态信息为(13) 得到目标节点在tk时刻的理论坐标值(.\, J7M) = (:? ④ 更新状态信息 将步骤六中得到的tk时刻目标节点的最终定位坐标值(Xtk, ytk)赋值给- )6.如权利要求5所述的非线性非高斯条件下基于EKF和PF的协同定位方法，其特征在于 步骤(6)具体如下： 设置第二阈值S2，利用不等式：(W) 判断步骤(4)、（5)得到的两次估计结果和(4,1)是否相近，若满足不等式，则 利用残差加权公式（15)得到tk时刻目标节点的最终定位坐标值(Xtk，ytk)，利用该坐标值分 别更新EKF TDOA算法和PF TDOA算法中目标节点前一时刻的状态信息;若不满足不等式，则 结束此次定位过程，返回步骤(1)读取TOA原始数据；其中，(?，无)对应的残差平方和为
【文档编号】G01S5/02GK105898865SQ201610439442
【公开日】2016年8月24日
【申请日】2016年6月17日
【发明人】王瑞荣, 许春璐, 王敏, 叶杨
【申请人】杭州电子科技大学