CN106226732A - 基于tof及迭代无迹滤波的室内无线定位跟踪方法 - Google Patents

Abstract

本发明公开了一种基于TOF及迭代无迹滤波的室内无线定位跟踪方法，主要解决目前定位跟踪方法缺乏有效的误差控制及数据利用率不高的问题。其实现方案是：1.根据目标节点与锚节点之间的通信时间信息，利用对称双向双边测距算法计算目标节点与锚节点之间的距离；2.根据距离信息进行三角形内点测试，得到被保留的锚节点组合；3.设定距离阈值和权重阈值，根据被保留的锚节点组合及对应的距离信息，计算目标节点当前时刻的坐标；4.根据目标节点当前时刻的坐标和实际运动状态，利用迭代无迹滤波算法预测目标节点下一时刻的坐标。本发明提高了数据的利用率和定位跟踪精度，可用于物流管理、紧急救助、设备检测、灾害预防及医疗保健。

Description

基于TOF及迭代无迹滤波的室内无线定位跟踪方法

技术领域

本发明属于无线通信技术领域，具体涉及室内无线定位及跟踪方法，可用于物流管理、紧急救助、设备检测、灾害预防及医疗保健。

背景技术

近年来，随着物联网IoT的飞速发展，室内定位，作为一种基于位置的服务LBS，在物流管理、紧急救助、设备检测、灾害预防、医疗保健、个性化信息传递等领域显示出了巨大的活力。全球卫星导航系统，例如美国的GPS和中国的北斗2号均能够在室外为人们提供良好的定位服务，但在室内却显得有些无能为力。这是由于建筑物的阻挡，使得信号传递到室内时衰减严重，加之室内环境复杂，障碍物众多，信号极易受到噪声干扰及产生多径效应，而导致的定位精度大大降低。因此，传统的卫星定位技术难以应用到室内环境。

目前室内无线定位跟踪技术种类繁多，依据定位过程中是否需要测距，室内无线定位技术可以分为无需测距的定位技术和基于测距的定位技术。相较于无需测距的定位技术，基于测距的定位技术有着节点密度低、定位精度高的优势，因此被广泛应用。目前基于测距的定位技术可分为以下五种：基于到达角度AOA测距的室内无线定位技术、基于到达时间TOA测距的室内无线定位技术、基于到达时间差TDOA测距的室内无线定位技术、基于接收信号强度RSSI测距的室内无线定位技术和基于飞行时间TOF测距的室内无线定位技术。其中：

基于AOA测距的室内无线定位技术，其需要特殊的硬件设备如天线阵列或有向天线等来支持，有天线阵列的节点的耗能、尺寸以及价格都要超过普通的传感节点，与无线传感器网络低成本和低能耗的特性相违背，所以实用性较差；

基于TOA测距的室内无线定位技术，其要求节点之间保持严格的时间同步，由于无线电的传输速度非常快，而传感节点之间的距离又较小，因此实现高精度计时同步是非常困难的，限制了该技术的实用性；

基于TDOA测距的室内无线定位技术，其虽不要求节点之间保持严格的时间同步，但是传输信号容易受环境因素影响产生多径效应及噪声干扰，因此系统难以适应复杂的室内环境；

基于RSSI测距的定位技术实现简单，其对硬件设备要求不高，成本低廉，是目前应用广泛的定位技术。但是该技术需要部署较多的锚节点，并且在障碍物较多的室内环境中，噪声干扰、多路径反射、非视线问题等因素导致节点获取的接收信号强度可能出现较大的随机波动，规律性较差，从而降低定位系统的准确性，使得定位系统难以满足高精度定位的需求；

基于TOF测距的室内无线定位技术，其需要额外的硬件设备支持，该技术使用双向通讯时间进行距离测量，有较精准的传输时间测量机制，因此相较以上四种测距技术有着较高的测距精度，但由于存在系统处理延迟及多径干扰，在近距离测量时会存在较大的测距误差，从而影响定位系统的精度。

福建师范大学在其专利申请号：201210010521.6，公开号：CN102573057A中公开了一种“一种优化的RFID室内定位方法”。该方法利用未知节点读取室内平均布置的锚节点的坐标信息，对读取到的坐标信息利用粒子滤波算法进行滤波，然后采用等边三角形的质心算法或权重质心算法来计算未知节点的坐标。这种方法由于对坐标信息进行粒子滤波，在坐标信息较少的情况下容易发生粒子贫化现象，并且由于对坐标信息没有合适的筛选机制，无法有效抑制异常数据引起的误差，导致系统定位误差较大。

上海机电学院在其专利申请号：201510916856.8，公开号：CN105491661A中提出了一种“基于改进的Kalman滤波算法的室内定位系统及方法”。该定位系统通过高斯滤波处理对各锚节点接收的RSSI值进行筛选并得到符合条件的值，由上位机计算出各RSSI值对应的距离，并将这些锚节点对应的距离值进行Kalman滤波处理，最后再经过三边定位及质心算法求出未知节点的坐标。这种方法通过滤波筛选了最优的三组数据进行定位计算，该方法由于在多锚节点下仅仅选取了三个锚节点的信息，因而没有充分利用其他锚节点的信息，使得数据利用率较低，定位结果可靠性不高。

发明内容

本发明的目的在于提供一种基于TOF及迭代无迹滤波的室内无线定位跟踪方法，以解决目前定位跟踪技术缺乏有效的误差控制及数据利用率不高的问题，提高室内定位跟踪的稳定性及定位精度。

本发明的技术方案是这样实现的：

本发明通过锚节点与目标节点之间相互通信获取时间信息，根据时间信息利用对称双向双边测距算法计算目标节点与锚节点间的距离，再根据距离变化信息进行三角形内点测试筛选所需要的锚节点组合，然后设定距离阈值和权重阈值对锚节点组合进行二次筛选，筛选后的锚节点组合利用三角质心加权法计算目标节点当前时刻的坐标，最后利用迭代无迹滤波算法预测目标节点的下一时刻的坐标，其实现步骤包括如下：

1)在k时刻和k+1时刻分别获取目标节点与锚节点A_i之间的通信时间信息，根据该时间信息利用对称双向双边测距方法分别计算k时刻和k+1时刻目标节点与锚节点A_i之间的距离D_i(k)和D_i(k+1)；

2)任意选择三个不同坐标的锚节点作为一组进行三角形内点测试；根据k时刻和k+1时刻目标节点与锚节点的距离信息D_i(k)、D_i(k+1)，舍弃D_i(k)均小于D_i(k+1)或者D_i(k)均大于D_i(k+1)的锚节点组合，直到遍历完所有组合，保留未被舍弃的锚节点组合；

3)根据2)中被保留的锚节点组合及k时刻对应的距离信息D_i(k)，计算出k时刻目标节点的坐标(x(k),y(k))：

3.1)根据锚节点数及实际部署情况设定距离阈值p和权重阈值q；

3.2)将2)中被保留的每个锚节点组合中最小的距离值设为d_i(k)，若d_i(k)≥p，保留对应的锚节点组合；否则，舍弃该组合；

3.3)计算3.2)中被保留的锚节点组合的权重值h_i，若h_i≥q，保留对应的锚节点组合；否则，舍弃该组合；

3.4)对3.3)中保留的锚节点组合利用三角质心加权法计算出k时刻目标节点的坐标(x(k),y(k))；

4)根据3)得到的坐标(x(k),y(k))及目标节点的实际运动状态，利用迭代无迹滤波方法预测出目标节点k+1时刻的坐标(x(k+1),y(k+1))。

本发明与现有技术相比，具有如下优点：

第一，由于本发明在定位计算前对距离信息进行了筛选，通过设定距离阈值和权重阈值舍弃精度较差的距离信息，克服了现有技术中未对距离信息进行合理筛选，导致距离信息在定位计算过程中容易引起较大误差，造成定位结果可靠性较低的不足，提高了定位跟踪的精度。

第二，由于本发明在处理定位坐标数据时使用了迭代无迹滤波算法，通过迭代计算加快了坐标数据的收敛，克服了现有滤波算法适应性差、易发散，无法有效抑制异常数据引起的误差的不足，提高了定位跟踪的稳定性。

附图说明

图1是本发明的总流程图；

图2是本发明中对锚节点组合进行三角形内点测试的子流程图；

图3是本发明中计算k时刻目标节点坐标的子流程图；

图4是本发明中预测目标节点k+1时刻的坐标的子流程图；

图5是用本发明和现有三种定位方法对目标节点定位的平均误差仿真图；

图6是用本发明和现有三种定位方法对目标节点定位的误差累计分布仿真图；

图7是用本发明和现有三种跟踪方法在单目标节点跟踪时的跟踪误差累计分布仿真图。

具体实施方式

下面结合附图及具体实施例对本发明中的方法进行详细完整的描述。本实施例以本发明的方法为前提进行实施，给出了详细的实施方式和具体操作过程，但本发明的保护范围不限于下述实施例，本领域普通技术人员在没有做出创造性前提下所获得的所有其他实施例，都属于本发明保护的范围。

参照图1，本实施例提供的基于TOF及迭代无迹滤波的室内无线定位跟踪方法包括以下步骤：

步骤1，获取目标节点与锚节点A_i之间的通信时间信息，根据该时间信息利用对称双向双边测距方法分别计算k时刻和k+1时刻目标节点与锚节点A_i之间的距离D_i(k)和D_i(k+1)。

本步骤的具体实现如下：

1.1)目标节点与通信范围内的锚节点A_i建立通信，其中锚节点坐标为A_i(x_i,y_i)，i＝1,2…,n,n≥3；

1.2)k时刻目标节点接收到的n组时间信息为：

$t_i\left(k\right)=\{t_i^1\left(k\right),t_i^2\left(k\right),t_i^3\left(k\right),t_i^4\left(k\right)\},i=\mathrm{1.2...}n;$

其中表示k时刻目标节点的传播延迟，表示k时刻锚节点A_i的处理延迟，表示k时刻锚节点A_i的传播延迟，表示k时刻目标节点的处理延迟；

1.3)根据1.2)接收的n组时间信息计算k时刻目标节点与每个锚节点A_i之间的距离D_i(k)：

$D_i\left(k\right)=\frac{C}{4}\×\[\left(t_i^1\right(k)-t_i^2(k\left)\right)+\left(t_i^3\right(k)-t_i^4(k\left)\right)\],i=1,2,\mathrm{...},n$

其中，C表示光速3×10⁸m/s；

1.4)k+1时刻目标节点接收的n组时间信息为

$t_i(k+1)=\{t_i^1(k+1),t_i^2(k+1),t_i^3(k+1),t_i^4(k+1)\},i=\mathrm{1.2...}n$

其中表示k+1时刻目标节点的传播延迟，表示k+1时刻锚节点A_i的处理延迟，表示k+1时刻锚节点A_i的传播延迟，表示k+1时刻目标节点的处理延迟；

1.5)根据1.4)接收的n组时间信息计算k+1时刻目标节点与每个锚节点A_i之间的距离D_i(k+1)：

$D_i(k+1)=\frac{C}{4}\×\[\left(t_i^1\right(k+1)-t_i^2(k+1\left)\right)+\left(t_i^3\right(k+1)-t_i^4(k+1\left)\right)\],i=1,2,\mathrm{...},n,$

其中，C表示光速3×10⁸m/s；

步骤2，任意选择三个不同坐标的锚节点作为一组进行三角形内点测试，获得被保留的锚节点组合。

参照图2，本步骤的具体实现如下：

2.1)任意选择三个不同坐标的锚节点A_i(x_i,y_i)，A_j(x_j,y_j)，A_k(x_k,y_k)作为一个组合，本实施例中部署了9个锚节点，因此有84种组合；

2.2)记录k时刻2.1)选择的三个锚节点A_i(x_i,y_i)，A_j(x_j,y_j)，A_k(x_k,y_k)对应的距离信息D_i(k)，D_j(k)，D_k(k)以及k+1时刻对应的距离信息D_i(k+1)，D_j(k+1)，D_k(k+1)；

2.3)将2.2)中k时刻的距离信息与k+1时刻的距离信息进行比较：

如果D_i(k)＞D_i(k+1)，D_j(k)＞D_j(k+1)，D_k(k)＞D_k(k+1)同时成立或者D_i(k)＜D_i(k+1)，D_j(k)＜D_j(k+1)，D_k(k)＜D_k(k+1)同时成立，则舍去当前所选择的锚节点组合，并返回2.1)；

否则，保留当前的锚节点组合；

2.4)重复2.1)到2.3)，直到遍历完所有组合，得到被保留的锚节点组合。

步骤3，根据被保留的锚节点组合及对应k时刻的距离信息D_i(k)，计算k时刻的目标节点坐标(x(k),y(k))。

参照图3，本步骤的具体实现如下：

3.1)根据锚节点数及实际部署情况设置距离阈值p和权重阈值q，本实施例中，p＝1.5，q＝0.3；

3.2)在步骤2保留的锚节点组合中，每个组合取出k时刻3个距离值中最小的距离值，假设k时刻每个组合中最小的距离值分别为d_i(k)，i＝0,1,2,…，比较d_i(k)与p的大小：

若d_i(k)≥p，则继续保留该锚节点组合；

否则，舍弃该锚节点组合；

3.3)计算3.2)中被保留的锚节点组合的权重值h_i；

3.3a)设锚节点组合中三个锚节点的坐标分别为A₁(x₁,y₁)，A₂(x₂,y₂)，A₃(x₃,y₃)，以这三个锚节点为顶点组成三角形ΔA₁A₂A₃，计算∠A₁,∠A₂,∠A₃分别对应的斜率K₁，K₂，K₃：

K₁＝(y₂-y₃)/(x₂-x₃)

K₂＝(y₃-y₁)/(x₃-x₁)

K₃＝(y₂-y₁)/(x₂-x₁)；

3.3b)根据上述的三个斜率K₁，K₂，K₃，计算三角形ΔA₁A₂A₃三个角的正切值：

tan A₁＝(K₂-K₃)/(1+K₂K₃)

tan A₂＝(K₁-K₃)/(1+K₁K₃)

tan A₃＝(K₂-K₁)/(1+K₂K₁)；

3.3c)在三个正切值tan A₁，tan A₂，tan A₃中，将最小且非负的正切值作为锚节点组合A₁,A₂,A₃的权重值h；

3.3d)根据3.3a)到3.3c)的计算方法，分别计算出3.2)中被保留的所有的锚节点组合的权重值h_i，i＝0,1,2,…。

3.4)将3.3)中得到的权重值h_i与权重阈值q进行比较：

若h_i≥q，则继续保留该权重值h_i对应的锚节点组合；

否则，舍弃该权重值h_i对应的锚节点组合；

3.5)对3.4)中被保留的锚节点组合利用三角质心加权法计算出k时刻目标节点的坐标(x(k),y(k))：

3.5a)设锚节点组合中三个锚节点的坐标分别为A₁(x₁,y₁)，A₂(x₂,y₂)，A₃(x₃,y₃)，对应的距离分别为D₁(k)，D₂(k)，D₃(k)，建立如下方程：

${(x-x_1)}^2+{(y-y_1)}^2=D_1^2\left(k\right)---<1>$

${(x-x_2)}^2+{(y-y_2)}^2=D_2^2\left(k\right)---<2>$

${(x-x_3)}^2+{(y-y_3)}^2=D_3^2\left(k\right)---<3>$

3.5b)求解方程组<1>、<2>、<3>，得到3个点(x₁₂,y₁₂)，(x₂₃,y₂₃)，(x₁₃,y₁₃)；

3.5b1)求解方程组<1>和<2>，得到两个解，记为(x₁₂,y₁₂),(x′₁₂,y′₁₂)，选择这两个解中距离(x₃,y₃)最近的点，假设选择的点为(x₁₂,y₁₂)；

3.5b2)求解方程组<2>和<3>，得到两个解，记为(x₂₃,y₂₃),(x′₂₃,y′₂₃)，选择这两个解中距离(x₁,y₁)最近的点，假设选择的点为(x₂₃,y₂₃)；

3.5b3)求解方程组<1>和<3>，得到两个解，记为(x₁₃,y₁₃),(x′₁₃,y′₁₃)，选择这两个解中距离(x₂,y₂)最近的点，假设选择的点为(x₁₃,y₁₃)；

3.5c)利用3.5b)得到的3个点，计算出目标节点的参考坐标

$\left(\tilde{x}\right(k),\tilde{y}(k\left)\right)=(\frac{x_{12}+x_{23}+x_{13}}{3},\frac{y_{12}+y_{23}+y_{13}}{3});$

3.5d)根据3.5a)到3.5c)的计算方法，计算3.4)中被保留的所有的锚节点组合所对应的目标节点的参考坐标

3.5e)利用3.5d)得到的参考坐标计算出k时刻目标节点的坐标(x(k),y(k))：

$\left(x\right(k),y(k\left)\right)=\frac{\underset{i=1}{\overset{\mathrm{...}}{\&Sigma;}}(h_i\&CenterDot;({\tilde{x}}_i\left(k\right),{\tilde{y}}_i\left(k\right)\left)\right)}{\underset{i=1}{\overset{\mathrm{...}}{\&Sigma;}}{h}_i}.$

步骤4，预测k+1时刻的目标节点坐标(x(k+1),y(k+1))。

参照图4，本步骤的具体实现如下：

4.1)根据步骤3得到的k时刻目标节点的坐标(x(k),y(k))以及目标节点的实际运动状态，计算k+1时刻的状态估计量及对应的协方差矩阵P(k+1)：

4.1a)计算估计状态变量及对应的方差矩阵P(k)：

$\hat{X}\left(k\right)=X\left(k\right)+W\left(k\right),$

$P\left(k\right)=E\&lsqb;\left(X\right(k)-\hat{X}(k\left)\right){\left(X\right(k)-\hat{X}(k\left)\right)}^T\&rsqb;,$

其中，状态变量x(k),y(k)分别为k时刻目标节点的横坐标和纵坐标，由步骤3计算得到；分别为k时刻目标节点的横向速度和纵向速度，分别为k时刻目标节点的横向加速度和纵向加速度；W(k)是一个均值为0，方差为Q的高斯白噪声；本实施例中，

4.1b)对状态变量X(k)进行无迹变换，获取采样点集X⁽ⁱ⁾(k)及对应权值w⁽ⁱ⁾：

$X^{\left(i\right)}\left(k\right)=\left\{\begin{array}{c}\hat{X}\left(k\right),i=0\\ \hat{X}\left(k\right)+\left(\sqrt{(y+\mathrm{\&lambda;})P\left(k\right)}\right),i=1~y\\ \hat{X}\left(k\right)-\left(\sqrt{(y+\mathrm{\&lambda;})P\left(k\right)}\right),i=y+1~2y\end{array}\right.,$

$w^{\left(i\right)}=\left\{\begin{array}{c}{w}_m^{\left(0\right)}=\frac{\mathrm{\&lambda;}}{y+\mathrm{\&lambda;}}\\ w_c^{\left(0\right)}=\frac{\mathrm{\&lambda;}}{y+\mathrm{\&lambda;}}+(1-{\mathrm{\&alpha;}}^2+\mathrm{\&beta;})\\ w_c^{\left(i\right)}=w_m^{\left(i\right)}=\frac{\mathrm{\&lambda;}}{2(y+\mathrm{\&lambda;})},i=1~2y\end{array}\right.,$

其中，y表示展开维数，y的取值越大则计算精度越高，α为分布状态因子，α的取值决定了采样点集X⁽ⁱ⁾(k)的分布状态；η为待选参数，用以确保矩阵(y+λ)P(k)为半正定矩阵，λ＝α²(y+η)-y；β为非负待选参数，用于表征状态变量高阶项的影响；在本实施例中，y＝6，α＝0.01，η＝0，β＝2；

4.1c)计算每个采样点X⁽ⁱ⁾(k)的一步预测X⁽ⁱ⁾(k+1|k)：

X⁽ⁱ⁾(k+1|k)＝F·X⁽ⁱ⁾(k)+G·W(k),i＝0～2y

其中，F表示状态转移矩阵，G表示过程噪声矩阵，F、G的取值分别如下：

$\begin{array}{cc}F=\left[\begin{array}{cccccc}1& 0& t_s& 0& \frac{t_s^2}{2}& 0\\ 0& 1& 0& t_s& 0& \frac{t_s^2}{2}\\ 0& 0& 1& 0& t_s& 0\\ 0& 0& 0& 1& 0& t_s\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right],& G=\left[\begin{array}{ccc}\frac{t_s^2}{2}& 0& 0\\ 0& t_s& 0\\ 0& 0& 0\\ \frac{t_s^2}{2}& 0& 0\\ 0& t_s& 0\\ 0& 0& 0\end{array}\right],\end{array}$

本实施例中，采样时间t_s＝1s；

4.1d)计算状态变量X(k)的一步预测及对应的协方差矩阵P(k+1|k)：

$\hat{X}(k+1|k)=\underset{i=0}{\overset{2y}{\&Sigma;}}{w}_m^{\left(i\right)}{X}^{\left(i\right)}(k+1|k),$

$P(k+1|k)=\underset{i=0}{\overset{2y}{\&Sigma;}}{w}_c^{\left(i\right)}\&lsqb;\hat{X}<k+1\left|k\right)-X^{\left(i\right)}(k+1|k)\&rsqb;{\&lsqb;\hat{X}(k+1|k)-X^{\left(i\right)}(k+1|k)\&rsqb;}^T+Q;$

4.1e)对一步预测进行无迹变换，获取新的采样点集X⁽ⁱ⁾(k+1|k)：

$X^{\left(i\right)}(k+1|k)=\left\{\begin{array}{c}\hat{X}(k+1|k),i=0\\ \hat{X}(k+1|k)+\sqrt{(y+\mathrm{\&lambda;})P(k+1|k)},i=1~y\\ \hat{X}(k+1|k)-\sqrt{(y+\mathrm{\&lambda;})P(k+1|k)},i=y+1~2y\end{array}\right.;$

4.1f)计算每个采样点X⁽ⁱ⁾(k+1|k)预测的观测量Z⁽ⁱ⁾(k+1|k)：

$Z^{\left(i\right)}(k+1|k)=\sqrt{x^{\left(i\right)}{(k+1|k)}^2+y^{\left(i\right)}{(k+1|k)}^2}+V\left(k\right),i=0~2y,$

其中x⁽ⁱ⁾(k+1|k)，y⁽ⁱ⁾(k+1|k)表示4.1e)中X⁽ⁱ⁾(k+1|k)的横坐标和纵坐标，V(k)是一个均值为0，方差为R的高斯白噪声，R的取值根据实际环境设定，本实施例中，R＝2；

4.1g)计算系统预测的观测量及对应的协方差矩阵和互协方差矩阵

$\stackrel{\&OverBar;}{Z}(k+1|k)=\underset{i=0}{\overset{2y}{\&Sigma;}}{w}_m^{\left(i\right)}{Z}^{\left(i\right)}(k+1|k),$

$P_{z_k{z}_k}=\underset{i=0}{\overset{2y}{\&Sigma;}}{w}^{\left(i\right)}\&lsqb;Z^{\left(i\right)}(k+1|k)-\stackrel{\&OverBar;}{Z}(k+1|k)\&rsqb;{\&lsqb;Z^{\left(i\right)}(k+1|k)-\stackrel{\&OverBar;}{Z}(k+1|k)\&rsqb;}^T+R,$

$P_{x_k{z}_k}=\underset{i=0}{\overset{2y}{\&Sigma;}}{w}^{\left(i\right)}\&lsqb;X^{\left(i\right)}(k+1|k)-\stackrel{\&OverBar;}{Z}(k+1|k)\&rsqb;{\&lsqb;Z^{\left(i\right)}(k+1|k)-\stackrel{\&OverBar;}{Z}(k+1|k)\&rsqb;}^T+R;$

4.1h)根据观测量的协方差矩阵和互协方差矩阵计算卡尔曼滤波增益K(k+1)：

$K(k+1)=P_{x_k{z}_k}{P}_{z_k{z}_k}^{-1};$

4.1i)计算k+1时刻状态估计量及对应的协方差矩阵P(k+1)：

$\hat{X}(k+1)=\hat{X}(k+1|k)+K(k+1)\&lsqb;\hat{Z}(k+1)-\stackrel{\&OverBar;}{Z}(k+1|k)\&rsqb;,$

$P(k+1)=P(k+1|k)-K(k+1)P_{z_k{z}_k}{K}^T(k+1),$

其中K^T(k+1)为卡尔曼滤波增益K(k+1)的转置矩阵；

4.2)根据4.1)得到的状态估计量及协方差矩阵P(k+1)，计算k+1时刻的迭代状态估计量及相应的协方差矩阵P(k+1)_b；

4.2a)令P(k+1)₀＝P(k+1|k)，P(k+1)₁＝P(k+1)；令b＝2，b表示迭代次数；

4.2b)利用无迹变换获取采样点集X⁽ⁱ⁾(k+1)_b：

$X^{\left(i\right)}{(k+1)}_b=\left\{\begin{array}{c}\hat{X}{(k+1)}_{b-1}^{*},i=0\\ \hat{X}{(k+1)}_{b-1}^{*}+\sqrt{(y+\mathrm{\&lambda;})P{(k+1)}_{b-1}^{*}},i=1~y\\ \hat{X}{(k+1)}_{b-1}^{*}-\sqrt{(y+\mathrm{\&lambda;})P{(k+1)}_{b-1}^{*}},i=y+1~2y\end{array}\right.,$

其中

4.2c)计算迭代一步预测估计

$\hat{X}{(k+1|k)}_b=\underset{i=0}{\overset{2y}{\&Sigma;}}{w}^{\left(i\right)}{X}^{\left(i\right)}{(k+1|k)}_b;$

4.2d)计算每个迭代采样点预测的迭代观测量Z⁽ⁱ⁾(k+1|k)_b：

$Z^{\left(i\right)}{(k+1|k)}_b=\sqrt{x^{\left(i\right)}{{(k+1|k)}_b}^2+y^{\left(i\right)}{{(k+1|k)}_b}^2}+V\left(k\right),i=0~2y,$

其中x⁽ⁱ⁾(k+1|k)_b，y⁽ⁱ⁾(k+1|k)_b表示4.2b)X⁽ⁱ⁾(k+1|k)_b中对应的横坐标和纵坐标；

4.2e)计算系统预测的迭代观测量及对应的协方差矩阵和互协方差矩阵

$\stackrel{\&OverBar;}{Z}{(k+1|k)}_b=\underset{i=0}{\overset{2y}{\&Sigma;}}{w}_m^{\left(i\right)}{Z}^{\left(i\right)}{(k+1|k)}_b,$

$P_{z_k{z}_k,b}=\underset{i=0}{\overset{2y}{\&Sigma;}}{w}_c^{\left(i\right)}\&lsqb;Z^{\left(i\right)}{(k+1|k)}_b-\stackrel{\&OverBar;}{Z}{(k+1|k)}_b\&rsqb;{\&lsqb;Z^{\left(i\right)}{(k+1|k)}_b-\stackrel{\&OverBar;}{Z}{(k+1|k)}_b\&rsqb;}^T,$

$P_{x_k{z}_k,b}=\underset{i=0}{\overset{2y}{\&Sigma;}}{w}_c^{\left(i\right)}\&lsqb;X^{\left(i\right)}{(k+1|k)}_b-\stackrel{\&OverBar;}{Z}{(k+1|k)}_b\&rsqb;{\&lsqb;Z^{\left(i\right)}{(k+1|k)}_b-\stackrel{\&OverBar;}{Z}{(k+1|k)}_b\&rsqb;}^T;$

4.2f)根据迭代观测量的协方差矩阵和互协方差矩阵计算迭代卡尔曼滤波增益K(k+1)_b：

$K{(k+1)}_b=P_{x_k{z}_k,b}{P}_{z_k{z}_k,b}^{-1};$

4.2g)计算k+1时刻迭代状态估计量及对应的协方差矩阵P(k+1)_b：

${\hat{X}}_b(k+1)=\hat{X}{(k+1|k)}_b+s\&CenterDot;K{(k+1)}_b\&lsqb;\stackrel{\&OverBar;}{Z}{(k+1)}_b-\stackrel{\&OverBar;}{Z}{(k+1|k)}_b\&rsqb;,$

$P{(k+1)}_b=P{(k+1|k)}_b-K(k+1)P_{z_k{z}_k,b}{K}^T{(k+1)}_b,$

其中调整系数s＝1；

4.2h)令并判断下式是否成立：

$\stackrel{\&CenterDot;}{X}{(k+1)}_b^{T}P{(k+1)}_{b-1}^{-1}\stackrel{\&CenterDot;}{X}{(k+1)}_b+\stackrel{\&CenterDot;}{Z}{(k+1)}_b^T-R^{-1}\stackrel{\&CenterDot;}{Z}{(k+1)}_b<\stackrel{\&CenterDot;}{Z}{(k+1)}_{b-1}^T-R^{-1}\stackrel{\&CenterDot;}{Z}{(k+1)}_{b-1}$

若成立且b≤N，则令b＝b+1，s＝s·γ，返回步骤4.2b)，其中N为最大迭代次数，需根据具体环境设定，γ为衰减因子，取值范围为0～1，本实施例中，N＝5，γ＝0.8；

否则，将和P(k+1)_b作为结果输出；

4.3)令P(k+1)＝P(k+1)_b，并从状态变量估计中得到k+1时刻目标节点的坐标(x(k+1),y(k+1))。

结合以下的仿真对本发明的应用效果作进一步的说明：

一、仿真条件：在10m*10m视距可达的空间内，随机分布100个目标，并在空间边缘均匀部署n个锚节点。

二、仿真内容与结果：

仿真1，用本发明与基于循环三边算法的室内无线定位方法、基于最大似然估计算法的室内无线定位方法以及基于三角质心算法的室内无线定位方法对目标节点定位的平均误差进行仿真，结果如图5所示。

由图5可见，在相同锚节点数的情况下，本发明与基于循环三边算法的室内无线定位方法、基于最大似然估计算法的室内无线定位方法以及基于三角质心算法的室内无线定位方法相比，平均定位误差最小，并且随着锚节点数的增加，本发明的定位精度也逐渐提高。

仿真2，当锚节点数n＝5时，用本发明与基于循环三边算法的室内无线定位方法、基于最大似然估计算法的室内无线定位方法以及基于三角质心算法的室内无线定位方法对目标节点定位的误差累计分布进行仿真，结果如图6所示。

由图6可见，当定位精度为0.5米时，本发明与基于循环三边算法的室内无线定位方法、基于三角质心算法的室内无线定位方法以及基于最大似然估计算法的室内无线定位方法的概率分别为80.7％、78.9％、76.7％和68％；当定位精度为0.8米时，本发明与基于循环三边算法的室内无线定位方法、基于三角质心算法的室内无线定位方法以及基于最大似然估计算法的室内无线定位方法的概率分别为98.2％、96.3％、95.7％和94.4％；因而相较于这三种定位方法，本发明的定位精度更高，稳定性能更好。

仿真3，用本发明与基于卡尔曼滤波的室内无线定位方法KF、基于扩展卡尔曼滤波的室内无线定位方法EKF以及基于无迹卡尔曼滤波的室内无线定位方法UKF对单目标节点跟踪的误差累计分布进行仿真，结果如图7所示。

由图7可见，当跟踪精度为0.5m时，本发明与所述的基于卡尔曼滤波的室内无线定位跟踪方法KF、基于扩展卡尔曼滤波的室内无线定位跟踪方法EKF、以及基于无迹卡尔曼滤波的室内无线定位跟踪方法UKF的概率分别为82.7％、32.2％、60.7％和64.2％；因而相较于这三种跟踪滤波方法，本发明的跟踪精度更高，跟踪性能更好。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (6)

1.基于TOF及迭代无迹滤波的室内无线定位跟踪方法，包括：

1)在k时刻和k+1时刻分别获取目标节点与锚节点A_i之间的通信时间信息，根据该时间信息利用对称双向双边测距方法分别计算k时刻和k+1时刻目标节点与锚节点A_i之间的距离D_i(k)和D_i(k+1)；

2)任意选择三个不同坐标的锚节点作为一组进行三角形内点测试；根据k时刻和k+1时刻目标节点与锚节点的距离信息D_i(k)、D_i(k+1)，舍弃D_i(k)均小于D_i(k+1)或者D_i(k)均大于D_i(k+1)的锚节点组合，直到遍历完所有组合，保留未被舍弃的锚节点组合；

3)根据2)中被保留的锚节点组合及k时刻对应的距离信息D_i(k)，计算出k时刻目标节点的坐标(x(k),y(k))：

3.1)根据锚节点数及实际部署情况设定距离阈值p和权重阈值q；

3.2)将2)中被保留的每个锚节点组合中最小的距离值设为d_i(k)，若d_i(k)≥p，保留对应的锚节点组合；否则，舍弃该组合；

3.3)计算3.2)中被保留的锚节点组合的权重值h_i，若h_i≥q，保留对应的锚节点组合；否则，舍弃该组合；

3.4)对3.3)中保留的锚节点组合利用三角质心加权法计算出k时刻目标节点的坐标(x(k),y(k))；

4)根据3)得到的坐标(x(k),y(k))及目标节点的实际运动状态，利用迭代无迹滤波方法预测出目标节点k+1时刻的坐标(x(k+1),y(k+1))。

2.根据权利要求1所述的方法,步骤3.3)中计算锚节点组合的权重值h_i，按如下步骤进行：

3.3a)设锚节点组合中三个锚节点的坐标分别为A₁(x₁,y₁)，A₂(x₂,y₂)，A₃(x₃,y₃)，以这三个锚节点为顶点组成三角形ΔA₁A₂A₃，计算∠A₁,∠A₂,∠A₃分别对应的斜率K₁，K₂，K₃：

K₁＝(y₂-y₃)/(x₂-x₃)

K₂＝(y₃-y₁)/(x₃-x₁)

K₃＝(y₂-y₁)/(x₂-x₁)

3.3b)根据斜率K₁，K₂，K₃，计算三角形ΔA₁A₂A₃三个角的正切值：

tanA₁＝(K₂-K₃)/(1+K₂K₃)

tanA₂＝(K₁-K₃)/(1+K₁K₃)

tanA₃＝(K₂-K₁)/(1+K₂K₁)

3.3c)在三个正切值tanA₁，tanA₂，tanA₃中，将最小且非负的正切值作为锚节点组合A₁,A₂,A₃的权重值h；

3.3d)根据3.3a)到3.3c)的计算方法，分别计算出3.2)中被保留的所有的锚节点组合的权重值h_i，i＝0,1,2,…。

3.根据权利要求1所述的方法，步骤3.4)中利用三角质心加权法计算出k时刻目标节点的坐标(x(k),y(k))，按如下步骤进行：

3.4a)设锚节点组合中三个锚节点的坐标分别为A₁(x₁,y₁)，A₂(x₂,y₂)，A₃(x₃,y₃)，对应的距离分别为D₁(k)，D₂(k)，D₃(k)，建立如下方程：

${(x-x_1)}^2+{(y-y_1)}^2=D_1^2\left(k\right)---<1>$

${(x-x_2)}^2+{(y-y_2)}^2=D_2^2\left(k\right)---<2>$

${(x-x_3)}^2+{(y-y_3)}^2=D_3^2\left(k\right)---<3>$

3.4b)求解方程组<1>、<2>、<3>，得到3个点(x₁₂,y₁₂)，(x₂₃,y₂₃)，(x₁₃,y₁₃)；

3.4b1)求解方程组<1>和<2>，得到两个解，记为(x₁₂,y₁₂),(x′₁₂,y′₁₂)，选择这两个解中距离(x₃,y₃)最近的点，假设选择的点为(x₁₂,y₁₂)；

3.4b2)求解方程组<2>和<3>，得到两个解，记为(x₂₃,y₂₃),(x′₂₃,y′₂₃)，选择这两个解中距离(x₁,y₁)最近的点，假设选择的点为(x₂₃,y₂₃)；

3.4b3)求解方程组<1>和<3>，得到两个解，记为(x₁₃,y₁₃),(x′₁₃,y′₁₃)，选择这两个解中距离(x₂,y₂)最近的点，假设选择的点为(x₁₃,y₁₃)；

3.4c)利用3.4b)得到的3个点，计算出目标节点的参考坐标

$\left(\tilde{x}\right(k),\tilde{y}(k\left)\right)=(\frac{x_{12}+x_{23}+x_{13}}{3},\frac{y_{12}+y_{23}+y_{13}}{3});$

3.4d)根据3.4a)到3.4c)的计算方法，计算3.3)中被保留的所有的锚节点组合所对应的目标节点的参考坐标

3.4e)利用3.4d)得到的参考坐标计算出k时刻目标节点的坐标(x(k),y(k))：

$\left(x\right(k),y(k\left)\right)=\frac{\underset{i=1}{\overset{\mathrm{...}}{\&Sigma;}}(h_i\&CenterDot;({\tilde{x}}_i\left(k\right),{\tilde{y}}_i\left(k\right)\left)\right)}{\underset{i=1}{\overset{\mathrm{...}}{\&Sigma;}}{h}_i}.$

4.根据权利要求1所述的方法，其中步骤4)中计算目标节点k+1时刻的坐标(x(k+1),y(k+1))，按如下步骤进行：

4.1)根据步骤3得到的坐标(x(k),y(k))以及目标节点的实际运动状态，计算k+1时刻的状态估计量及对应的协方差矩阵P(k+1)；

4.2)根据4.1)得到的状态估计量及协方差矩阵P(k+1)，计算k+1时刻的迭代状态估计量及对应的协方差矩阵P(k+1)_b；

4.3)令P(k+1)＝P(k+1)_b，并从状态变量估计中得到k+1时刻目标节点的坐标(x(k+1),y(k+1))。

5.根据权利要求4所述的方法，其中步骤4.1)中计算k+1时刻的状态估计量及对应的协方差矩阵P(k+1)，按如下步骤进行：

4.1a)计算状态估计变量及对应的方差矩阵P(k)：

$\hat{X}\left(k\right)=X\left(k\right)+W\left(k\right),$

$P\left(k\right)=E\&lsqb;\left(X\right(k)-\hat{X}(k\left)\right){\left(X\right(k)-\hat{X}(k\left)\right)}^T\&rsqb;,$

其中，状态变量x(k),y(k)分别为k时刻目标节点的横坐标和纵坐标，分别为k时刻目标节点的横向速度和纵向速度，分别为k时刻目标节点的横向加速度和纵向加速度，W(k)是一个均值为0，方差为Q的高斯白噪声；

4.1b)对状态变量X(k)进行无迹变换，获取采样点集X⁽ⁱ⁾(k)及对应权值w⁽ⁱ⁾：

$X^{\left(i\right)}\left(k\right)=\left\{\begin{array}{c}\hat{X}\left(k\right),i=0\\ \hat{X}\left(k\right)+\left(\sqrt{(y+\mathrm{\&lambda;})P\left(k\right)}\right),i=1~y\\ \hat{X}\left(k\right)-\left(\sqrt{(y+\mathrm{\&lambda;})P\left(k\right)}\right),i=y+1~2y\end{array}\right.,$

$w^{\left(i\right)}=\left\{\begin{array}{c}{w}_m^{\left(0\right)}=\frac{\mathrm{\&lambda;}}{y+\mathrm{\&lambda;}}\\ w_c^{\left(0\right)}=\frac{\mathrm{\&lambda;}}{y+\mathrm{\&lambda;}}+(1-{\mathrm{\&alpha;}}^2+\mathrm{\&beta;})\\ w_c^{\left(i\right)}=w_m^{\left(i\right)}=\frac{\mathrm{\&lambda;}}{2(y+\mathrm{\&lambda;})},i=1~2y\end{array}\right.,$

其中，y表示展开维数，α为分布状态因子；η为待选参数，用以确保矩阵(y+λ)P(k)为半正定矩阵，λ＝α²(y+η)-y；β为非负待选参数，用于表征状态变量高阶项的影响；y，α，η，β的取值需要根据实际需求设定；

4.1c)计算每个采样点X⁽ⁱ⁾(k)的一步预测X⁽ⁱ⁾(k+1|k)：

X⁽ⁱ⁾(k+1|k)＝F·X⁽ⁱ⁾(k)+G·W(k),i＝0～2y，

其中，F表示状态转移矩阵，G表示过程噪声矩阵，F、G的取值分别如下：

$F=\left[\begin{array}{cccccc}1& 0& t_s& 0& \frac{t_s^2}{2}& 0\\ 0& 1& 0& t_s& 0& \frac{t_s^2}{2}\\ 0& 0& 1& 0& t_s& 0\\ 0& 0& 0& 1& 0& t_s\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right],G=\left[\begin{array}{ccc}\frac{t_s^2}{2}& 0& 0\\ 0& t_s& 0\\ 0& 0& 0\\ \frac{t_s^2}{2}& 0& 0\\ 0& t_s& 0\\ 0& 0& 0\end{array}\right],$

其中采样时间t_s根据实际需求设定；

4.1d)计算状态变量X(k)的一步预测及对应的协方差矩阵P(k+1|k)：

$\hat{X}(k+1|k)=\underset{i=0}{\overset{2y}{\&Sigma;}}{w}_m^{\left(i\right)}{X}^{\left(i\right)}(k+1|k),$

$P(k+1|k)=\underset{i=0}{\overset{2y}{\&Sigma;}}{w}_c^{\left(i\right)}\&lsqb;\hat{X}(k+1|k)-X^{\left(i\right)}(k+1|k)\&rsqb;{\&lsqb;\hat{X}(k+1|k)-X^{\left(i\right)}(k+1|k)\&rsqb;}^T+Q;$

4.1e)对一步预测进行无迹变换，获取新的采样点集X⁽ⁱ⁾(k+1|k)：

$X^{\left(i\right)}(k+1|k)=\left\{\begin{array}{c}\hat{X}(k+1|k),i=0\\ \hat{X}(k+1|k)+\sqrt{(y+\mathrm{\&lambda;})P(k+1|k)},i=1~y\\ \hat{X}(k+1|k)-\sqrt{(y+\mathrm{\&lambda;})P(k+1|k)},i=y+1~2y\end{array}\right.,$

4.1f)计算每个采样点X⁽ⁱ⁾(k+1|k)预测的观测量Z⁽ⁱ⁾(k+1|k)：

$Z^{\left(i\right)}(k+1|k)=\sqrt{x^{\left(i\right)}{(k+1|k)}^2+y^{\left(i\right)}{(k+1|k)}^2}+V\left(k\right),i=0~2y,$

其中x⁽ⁱ⁾(k+1|k)，y⁽ⁱ⁾(k+1|k)表示4.1e)中X⁽ⁱ⁾(k+1|k)的横坐标和纵坐标，V(k)是一个均值为0，方差为R的高斯白噪声，R的取值根据实际环境设定；

4.1g)计算观测量及对应的协方差矩阵和互协方差矩阵

$\stackrel{\&OverBar;}{Z}(k+1|k)=\underset{i=0}{\overset{2y}{\&Sigma;}}{w}_m^{\left(i\right)}{Z}^{\left(i\right)}(k+1|k),$

$P_{z_k{z}_k}=\underset{i=0}{\overset{2y}{\&Sigma;}}{w}^{\left(i\right)}\&lsqb;Z^{\left(i\right)}(k+1|k)-\stackrel{\&OverBar;}{Z}(k+1|k)\&rsqb;{\&lsqb;Z^{\left(i\right)}(k+1|k)-\stackrel{\&OverBar;}{Z}(k+1|k)\&rsqb;}^T+R,$

$P_{x_k{z}_k}=\underset{i=0}{\overset{2y}{\&Sigma;}}{w}^{\left(i\right)}\&lsqb;X^{\left(i\right)}(k+1|k)-\stackrel{\&OverBar;}{Z}(k+1|k)\&rsqb;{\&lsqb;Z^{\left(i\right)}(k+1|k)-\stackrel{\&OverBar;}{Z}(k+1|k)\&rsqb;}^T+R;$

4.1h)根据观测量的协方差矩阵和互协方差矩阵计算卡尔曼滤波增益K(k+1)：

$K(k+1)=P_{x_k{z}_k}{P}_{z_k{z}_k}^{-1};$

4.1i)计算k+1时刻状态估计量及对应的协方差矩阵P(k+1)：

$\hat{X}(k+1)=\hat{X}(k+1|k)+K(k+1)\&lsqb;\hat{Z}(k+1)-\stackrel{\&OverBar;}{Z}(k+1|k)\&rsqb;,$

$P(k+1)=P(k+1|k)-K(k+1)P_{z_k{z}_k}{K}^T(k+1).$

6.根据权利要求4所述的方法，其中步骤4.2)中计算k+1时刻的迭代状态估计量及对应的协方差矩阵P(k+1)_b，按如下步骤进行：

4.2a)令P(k+1)₀＝P(k+1|k)，P(k+1)₁＝P(k+1)；令b＝2，b表示迭代次数；

4.2b)利用无迹变换获取采样点集X⁽ⁱ⁾(k+1)_b：

$X^{\left(i\right)}{(k+1)}_b=\left\{\begin{array}{c}\hat{X}{(k+1)}_{b-1}^{*},i=0\\ \hat{X}{(k+1)}_{b-1}^{*}+\sqrt{(y+\mathrm{\&lambda;})P{(k+1)}_{b-1}^{*}},i=1~y\\ \hat{X}{(k+1)}_{b-1}^{*}-\sqrt{(y+\mathrm{\&lambda;})P{(k+1)}_{b-1}^{*}},i=y+1~2y\end{array}\right.,$

其中

4.2c)计算迭代一步预测估计

$\hat{X}{(k+1|k)}_b=\underset{i=0}{\overset{2y}{\&Sigma;}}{w}^{\left(i\right)}{X}^{\left(i\right)}{(k+1|k)}_b;$

4.2d)计算每个迭代采样点预测的迭代观测量Z⁽ⁱ⁾(k+1|k)_b：

$Z^{\left(i\right)}{(k+1|k)}_b=\sqrt{x^{\left(i\right)}{{(k+1|k)}_b}^2+y^{\left(i\right)}{{(k+1|k)}_b}^2}+V\left(k\right),i=0~2y,$

其中x⁽ⁱ⁾(k+1|k)_b，y⁽ⁱ⁾(k+1|k)_b表示4.2b)X⁽ⁱ⁾(k+1|k)_b中对应的横坐标和纵坐标；

4.2e)计算系统预测的迭代观测量及对应的协方差矩阵和互协方差矩阵

$\stackrel{\&OverBar;}{Z}{(k+1|k)}_b=\underset{i=0}{\overset{2y}{\&Sigma;}}{w}_m^{\left(i\right)}{Z}^{\left(i\right)}{(k+1|k)}_b,$

$P_{z_k{z}_k,b}=\underset{i=0}{\overset{2y}{\&Sigma;}}{\mathrm{\&omega;}}_c^{\left(i\right)}\&lsqb;Z^{\left(i\right)}{(k+1|k)}_b-\stackrel{\&OverBar;}{Z}{(k+1|k)}_b\&rsqb;{\&lsqb;Z^{\left(i\right)}{(k+1|k)}_b-\stackrel{\&OverBar;}{Z}{(k+1|k)}_b\&rsqb;}^T,$

$P_{x_k{z}_k,b}=\underset{i=0}{\overset{2y}{\&Sigma;}}{\mathrm{\&omega;}}_c^{\left(i\right)}\&lsqb;X^{\left(i\right)}{(k+1|k)}_b-\stackrel{\&OverBar;}{Z}{(k+1|k)}_b\&rsqb;{\&lsqb;Z^{\left(i\right)}{(k+1|k)}_b-\stackrel{\&OverBar;}{Z}{(k+1|k)}_b\&rsqb;}^T;$

4.2f)根据迭代观测量的协方差矩阵和互协方差矩阵计算迭代卡尔曼滤波增益K(k+1)_b：

$K{(k+1)}_b=P_{x_k{z}_k,b}{P}_{z_k{z}_k,b}^{-1};$

4.2g)计算k+1时刻迭代状态估计量及对应的协方差矩阵P(k+1)_b：

${\hat{X}}_b(k+1)=\hat{X}{(k+1|k)}_b+s\&CenterDot;K{(k+1)}_b\&lsqb;\stackrel{\&OverBar;}{Z}{(k+1)}_b-\stackrel{\&OverBar;}{Z}{(k+1|k)}_b\&rsqb;,$

$P{(k+1)}_b=P{(k+1|k)}_b-K{(k+1)}_b{P}_{z_k{z}_k,b}{K}^T{(k+1)}_b,$

其中调整系数s＝1；

4.2h)令并判断下式是否成立：

$\stackrel{\&CenterDot;}{X}{(k+1)}_b^{T}P{(k+1)}_{b-1}^{-1}\stackrel{\&CenterDot;}{X}{(k+1)}_b+\stackrel{\&CenterDot;}{Z}{(k+1)}_b^T{R}^{-1}\stackrel{\&CenterDot;}{Z}{(k+1)}_b<\stackrel{\&CenterDot;}{Z}{(k+1)}_{b-1}^T{R}^{-1}\stackrel{\&CenterDot;}{Z}{(k+1)}_{b-1}$

若成立且b≤N，则令b＝b+1，s＝s·γ，并返回4.2b，其中N为最大迭代次数，需根据具体环境设定，γ为衰减因子，取值范围为0～1；

否则，将和P(k+1)_b作为结果输出。