处理飞行时间测距数据的自适应卡尔曼滤波的方法
【专利摘要】本发明公开了一种处理飞行时间测距数据的自适应卡尔曼滤波的方法。它包括对移动节点和锚节点之间飞行时间测距数据的卡尔曼滤波处理，特别是，一是通过建立目标状态方程和有色噪声量测方程，使测量误差自适应于测距数据的变化，二是通过设立自适应于测距数据变化的门限值来滤除偏差较大的测距数据。本发明所建的误差模型符合实际的误差模型，使得滤波过程更准确，而且在滤去偏差过大的测距数据时能够自适应测距数据的变化，使得对大偏差的数据滤去效果更好，最终所得的处理结果非常接近真实数据；使其可广泛地用于无线传感器网络中飞行时间的准确测距。
【专利说明】处理飞行时间测距数据的自适应卡尔曼滤波的方法
【技术领域】
[0001] 本发明涉及一种卡尔曼滤波的方法，尤其是一种处理飞行时间测距数据的自适应 卡尔曼滤波的方法。
【背景技术】
[0002] 在无线传感器网络（WSN)的应用中，传感器节点定位技术是无线传感器网络定位 的关键技术，在无线传感器网络中占有重要的地位。基于飞行时间（TOF)的节点定位技术 具有定位精度较高、能耗小、对硬件要求低等特点，是目前应用最广泛的定位方法之一。飞 行时间测距技术由来已久，它是通过测量节点间信号传输的时间来得到两节点间的距离， 其数值即为飞行时间测距数据。由于非视距（NLOS)的存在，飞行时间测距数据会存在NLOS 误差，其误差一般具有正偏性，飞行时间所测量的距离会偏大于实际距离。如果直接将测距 数据用于节点的定位，其结果会大大地偏离节点的实际位置。所以如何对飞行时间的测距 数据进行处理，消除其中的NLOS误差显得尤为重要。
[0003] 卡尔曼（Kalman)滤波作为一种最优状态估计的方法，给出了一种递推算法，其由 实时获得的受噪声污染的离散观测数据，对系统状态进行线性、无偏，及最小误差方差的最 优估计。卡尔曼滤波应用于飞行时间测距数据的估计由来已久，但现有的卡尔曼滤波存在 着不足，其测量误差只作为一种固定的未知模型，模型的不确定性对最后的滤波输出产生 较大影响，实际的测量误差是有色噪声且不断变化的，而现有的卡尔曼滤波仅将其协方差 作为固定值，显然与实际不符。为解决这一问题，人们做出了各种努力，如题为"基于Kalman 滤波器的非视距误差抑制算法"，张美杨，等，《计算机中文》2010年第36卷第11期的文章。 该文将到达时间（TOA)测量值及非视距误差作为Kalman滤波器的状态变量；其虽能够同时 估计出测量值和非视距误差，但由于测量噪声的模型不确定，且测量噪声协方差R固定不 变，从而导致了滤波结果的精确度不高。
[0004]NLOS误差的随机性引起了NLOS误差的剧烈变化，使得某些飞行时间测距数据的 偏差特别大。这些受NLOS误差污染严重的测距数据将严重影响对飞行时间的正确估计。所 以，如果能消除这些包含较大误差测距数据的影响，就可以从很大程度上消除NLOS误差。 为此，人们做了一些尝试，如在"用卡尔曼滤波器消除TOA中NLOS误差的三种方法"，李静， 等，《通信学报》第26卷第1期于2005年公开的测距数据丢弃法，该法将偏离实际值较大的 测距点丢弃，用状态预测值代替状态测量值；此方法虽能够减小含有较大NLOS误差的测距 数据，然其门限值的确定是靠实验得到的，并不能根据测距数据的变化而实时地变化。
【发明内容】
[0005] 本发明要解决的技术问题为克服上述各种技术方案的局限性，提供一种能够获得 较准确滤波结果的处理飞行时间测距数据的自适应卡尔曼滤波的方法。
[0006] 为解决本发明的技术问题，所采用的技术方案为：处理飞行时间测距数据的自适 应卡尔曼滤波的方法包括对移动节点和锚节点之间飞行时间测距数据的卡尔曼滤波处理， 特别是主要步骤如下：
[0007] 步骤1，建立目标状态方程和有色噪声量测方程
[0008] xk+1 =Axk+Bak,
[0009] vk =Cxk+βk,
[0010] β k - Nlrf β η+ Yk，
[0011] 其中，
[0012] A为状态矩阵，B为误差系数矩阵，C为测量矩阵，
[0013] xk为目标在k(k= 1，2,3,......η)时刻的二维状态向量,? =Ok匕]'其中的 rk为待估计的飞行时间值A.为其一阶导数，
[0014] ak为系统噪声，是一个高斯白噪声序列，协方差用Q表不，
[0015] Vk为一维测量向量，表示k时刻的测距数据，
[0016] βk为k时刻的测量误差,协方差矩阵为Rk,
[0017] Nh为自回归系数，
[0018] Yk为1?斯白噪声序列；
[0019] 步骤2,根据原始卡尔曼滤波算法，由k-Ι时刻的滤波结果得到目标的k时刻的预 测值以及k时刻的新息值zk，计算公式为
【权利要求】
1. 一种处理飞行时间测距数据的自适应卡尔曼滤波的方法，包括对移动节点和锚节点 之间飞行时间测距数据的卡尔曼滤波处理，其特征在于主要步骤如下： 步骤1，建立目标状态方程和有色噪声量测方程xk+1 = Axk+B a k, vk - Cxk+ β k, β k - Nk-! β k-!+Ykj 其中， A为状态矩阵，B为误差系数矩阵，C为测量矩阵， xk为目标在k(k= 1，2,3,……η)时刻的二维状态向量，Xfc = [rkfk]T,其中的rk为 待估计的飞行时间值、4.为其一阶导数， ak为系统噪声，是一个高斯白噪声序列，协方差用Q表示， Vk为一维测量向量，表示k时刻的测距数据，βk为k时刻的测量误差，协方差矩阵为Rk， Nlrf为自回归系数， Yk为_斯白噪声序列； 步骤2,根据原始卡尔曼滤波算法，由k-Ι时刻的滤波结果得到目标的k时刻的预测值 %/k-i以及k时刻的新息值zk，计算公式为
其中，％-i/k-i为k-Ι时刻的滤波结果； 步骤3,求出测量噪声协方差k时刻的估计值 先计算参数Sk，其由下式来估计得到§k，
其中， ^是前i次新息值的均值，计算为 ? 1=1 5 再按照下两式得出测量噪声协方差在k时刻的估计值Sk, Pk, k-i一APh，hA +BQB， Rjc = §|^ - J 其中，Γ\，η是k时刻预测误差方差阵，Pm1是k_l时刻估计误差方差阵； 步骤4,计算Nlrf 根据和Yk独立，且有协方差的关系得出Nlrt的计算式为
其中，var(Yk)是Yk的方差； 步骤5，计算k时刻的门限值mk 计算前k次测距数据的方差讳，取门限值为方差值,即mk = 4; 步骤6,更新卡尔曼滤波增益 将k时刻的新息值与门限值进行比较，若新息值大于门限值，则将卡尔曼增益置为零， 否则按以下两步计算滤波增益Gk， R, =「CA-N,.Cl·
步骤7,更新估计误差方差阵 由P0 =Cbar(X0)FHCTRi1Cr^出估计误差方差阵的初值， 其中， Xtl取为第一个测距数据，R3.为误差协方差初值，靈β取作前几次试验测距数据的方差， 估计误差方差阵更新表达式为 Pk,k= (A-GA-i) -Pk-!,^1At+(I-GkC)BQBT； 步骤8,根据k时刻滤波输出方程输出k时刻的滤波结果，滤波方程为 愛k/k = + Gk * Ok - Nfe-ivk-i - Hk-:#k-：Mk-i)， 如此循环往复，输出I到n时刻的滤波结果，得到对飞行时间测距数据的处理结果。
2. 根据权利要求1所述的处理飞行时间测距数据的自适应卡尔曼滤波的方法，其特征 是步骤1中的状态矩阵A的取值为A=BI]其中，T为滤波采样间隔。 5
3. 根据权利要求1所述的处理飞行时间测距数据的自适应卡尔曼滤波的方法，其特征 是步骤1中的误差系数矩阵B的取值为B
4. 根据权利要求1所述的处理飞行时间测距数据的自适应卡尔曼滤波的方法，其特征 是步骤1中的测量矩阵C的取值为C= [1 0]。
5. 根据权利要求1所述的处理飞行时间测距数据的自适应卡尔曼滤波的方法，其特征 是步骤1中的Xk的下标k=η时的取值为1000。
6. 根据权利要求1所述的处理飞行时间测距数据的自适应卡尔曼滤波的方法，其特征 是步骤2中的0时刻的滤波结果根据初始测距数据由下式得出， S0 = E(X0) - [var(X(j)]CT [C-Var(X0) ·Ct+S0]-1[CE(X0) - vc]， 其中，Xtl取为第一个测距数据、V。为第一个测距数据、Ke.取为前30次测距数据的方差。
【文档编号】G01S5/02GK104316905SQ201410557259
【公开日】2015年1月28日 申请日期:2014年10月20日 优先权日:2014年10月20日
【发明者】李奇越, 吴忠, 王建平, 黎洁, 孙伟 申请人:合肥工业大学