CN104316905B - 处理飞行时间测距数据的自适应卡尔曼滤波的方法 - Google Patents

Abstract

本发明公开了一种处理飞行时间测距数据的自适应卡尔曼滤波的方法。它包括对移动节点和锚节点之间飞行时间测距数据的卡尔曼滤波处理，特别是，一是通过建立目标状态方程和有色噪声量测方程，使测量误差自适应于测距数据的变化，二是通过设立自适应于测距数据变化的门限值来滤除偏差较大的测距数据。本发明所建的误差模型符合实际的误差模型，使得滤波过程更准确，而且在滤去偏差过大的测距数据时能够自适应测距数据的变化，使得对大偏差的数据滤去效果更好，最终所得的处理结果非常接近真实数据；使其可广泛地用于无线传感器网络中飞行时间的准确测距。

Description

处理飞行时间测距数据的自适应卡尔曼滤波的方法

技术领域

本发明涉及一种卡尔曼滤波的方法，尤其是一种处理飞行时间测距数据的自适应卡尔曼滤波的方法。

背景技术

在无线传感器网络(WSN)的应用中，传感器节点定位技术是无线传感器网络定位的关键技术，在无线传感器网络中占有重要的地位。基于飞行时间(TOF)的节点定位技术具有定位精度较高、能耗小、对硬件要求低等特点，是目前应用最广泛的定位方法之一。飞行时间测距技术由来已久，它是通过测量节点间信号传输的时间来得到两节点间的距离，其数值即为飞行时间测距数据。由于非视距(NLOS)的存在，飞行时间测距数据会存在NLOS误差，其误差一般具有正偏性，飞行时间所测量的距离会偏大于实际距离。如果直接将测距数据用于节点的定位，其结果会大大地偏离节点的实际位置。所以如何对飞行时间的测距数据进行处理，消除其中的NLOS误差显得尤为重要。

卡尔曼(Kalman)滤波作为一种最优状态估计的方法，给出了一种递推算法，其由实时获得的受噪声污染的离散观测数据，对系统状态进行线性、无偏，及最小误差方差的最优估计。卡尔曼滤波应用于飞行时间测距数据的估计由来已久，但现有的卡尔曼滤波存在着不足，其测量误差只作为一种固定的未知模型，模型的不确定性对最后的滤波输出产生较大影响，实际的测量误差是有色噪声且不断变化的，而现有的卡尔曼滤波仅将其协方差作为固定值，显然与实际不符。为解决这一问题，人们做出了各种努力，如题为“基于Kalman滤波器的非视距误差抑制算法”，张美杨，等，《计算机中文》2010年第36卷第11期的文章。该文将到达时间(TOA)测量值及非视距误差作为Kalman滤波器的状态变量；其虽能够同时估计出测量值和非视距误差，但由于测量噪声的模型不确定，且测量噪声协方差R固定不变，从而导致了滤波结果的精确度不高。

NLOS误差的随机性引起了NLOS误差的剧烈变化，使得某些飞行时间测距数据的偏差特别大。这些受NLOS误差污染严重的测距数据将严重影响对飞行时间的正确估计。所以，如果能消除这些包含较大误差测距数据的影响，就可以从很大程度上消除NLOS误差。为此，人们做了一些尝试，如在“用卡尔曼滤波器消除TOA中NLOS误差的三种方法”，李静，等，《通信学报》第26卷第1期于2005年公开的测距数据丢弃法，该法将偏离实际值较大的测距点丢弃，用状态预测值代替状态测量值；此方法虽能够减小含有较大NLOS误差的测距数据，然其门限值的确定是靠实验得到的，并不能根据测距数据的变化而实时地变化。

发明内容

本发明要解决的技术问题为克服上述各种技术方案的局限性，提供一种能够获得较准确滤波结果的处理飞行时间测距数据的自适应卡尔曼滤波的方法。

为解决本发明的技术问题，所采用的技术方案为：处理飞行时间测距数据的自适应卡尔曼滤波的方法包括对移动节点和锚节点之间飞行时间测距数据的卡尔曼滤波处理，特别是主要步骤如下：

步骤1，建立目标状态方程和有色噪声量测方程

x_k+1＝Ax_k+Bα_k，

v_k＝Cx_k+β_k，

β_k＝N_k-1β_k-1+γ_k，

其中，

A为状态矩阵，B为误差系数矩阵，C为测量矩阵，

x_k为目标在k(k＝1，2，3，......n)时刻的二维状态向量， $x_k={\left[\begin{array}{cc}{r}_k& {\stackrel{\·}{r}}_k\end{array}\right]}^T,$ 其中的r_k为待估计的飞行时间值、为其一阶导数，

α_k为系统噪声，是一个高斯白噪声序列，协方差用Q表示，

v_k为一维测量向量，表示k时刻的测距数据，

β_k为k时刻的测量误差，协方差矩阵为R_k，

N_k-1为自回归系数，

γ_k为高斯白噪声序列；

步骤2，根据原始卡尔曼滤波算法，由k-1时刻的滤波结果得到目标的k时刻的预测值以及k时刻的新息值z_k，计算公式为

$\begin{array}{c}{\hat{x}}_{k/k-1}=A{\hat{x}}_{k-1/k-1},\\ z_k=v_k-C{\hat{x}}_{k/k-1},\end{array}$

其中，为k-1时刻的滤波结果；

步骤3，求出测量噪声协方差k时刻的估计值

先计算参数S_k，其由下式来估计得到

${\hat{S}}_k=\frac{1}{k-1}\underset{i=1}{\overset{k}{\mathrm{\Σ}}}(z_i-{\stackrel{\‾}{z}}_i){(z_i-{\stackrel{\‾}{z}}_i)}^T,$

其中，

是前i次新息值的均值，计算为

${\stackrel{\‾}{z}}_i=\frac{1}{i}\underset{j=1}{\overset{i}{\mathrm{\Σ}}}{z}_j,$

再按照下两式得出测量噪声协方差在k时刻的估计值

P_k，k-1＝AP_k-1，k-1A^T+BQB^T，

${\hat{R}}_k={\hat{S}}_k-{\mathrm{CP}}_{k,k-1}{C}^T,$

其中，P_k，k-1是k时刻预测误差方差阵，P_k-1，k-1是k-1时刻估计误差方差阵；

步骤4，计算N_k-1

根据β_k-1和γ_k独立，且有协方差的关系得出N_k-1的计算式为

$N_{k-1}=\sqrt{({\hat{R}}_k-\mathrm{var}\left({\mathrm{\γ}}_k\right))/{\hat{R}}_{k-1}},$

其中，var(γ_k)是γ_k的方差；

步骤5，计算k时刻的门限值m_k

计算前k次测距数据的方差取门限值为方差值，即

步骤6，更新卡尔曼滤波增益

将k时刻的新息值与门限值进行比较，若新息值大于门限值，则将卡尔曼增益置为零，否则按以下两步计算滤波增益G_k，

H_k-1＝[CA-N_k-1C]，

$G_k=({\mathrm{AP}}_{k-1,k-1}{H}_{k-1}^T+{\mathrm{BQB}}^T{C}^T)\·{(H_{k-1}{P}_{k-1,k-1}{H}_{k-1}^T+{\mathrm{CBQB}}^T{C}^T+{\hat{R}}_{k-1})}^{-1};$

步骤7，更新估计误差方差阵

由得出估计误差方差阵的初值，

其中，

x₀取为第一个测距数据，为误差协方差初值，取作前几次试验测距数据的方差，

估计误差方差阵更新表达式为

P_k，k＝(A-G_kH_k-1)·P_k-1，k-1A^T+(I-G_kC)BQB^T；

步骤8，根据k时刻滤波输出方程输出k时刻的滤波结果，滤波方程为

${\hat{x}}_{k/k}=A{\hat{x}}_{k-1/k-1}+G_k\·(v_k-N_{k-1}{v}_{k-1}-H_{k-1}{\hat{x}}_{k-1/k-1}),$

如此循环往复，输出1到n时刻的滤波结果，得到对飞行时间测距数据的处理结果。

作为处理飞行时间测距数据的自适应卡尔曼滤波的方法的进一步改进：

优选地，步骤1中的状态矩阵A的取值为 $A=\left[\begin{array}{cc}1& T\\ 0& 1\end{array}\right],$ 其中，T为滤波采样间隔。

优选地，步骤1中的误差系数矩阵B的取值为 $B=\left[\begin{array}{c}1\\ 0\end{array}\right].$

优选地，步骤1中的测量矩阵C的取值为C＝[1 0]。

优选地，步骤1中的x_k的下标k＝n时的取值为1000。

优选地，步骤2中的0时刻的滤波结果根据初始测距数据由下式得出，

${\hat{x}}_0=E\left(x_0\right)-\left[\mathrm{var}\left(x_0\right)\right]C^T\·{[C\·\mathrm{var}\left(x_0\right)\·C^T+{\hat{R}}_0]}^{-1}\·[\mathrm{CE}\left(x_0\right)-v_0],$

其中，x₀取为第一个测距数据、v₀为第一个测距数据、取为前30次测距数据的方差。

相对于现有技术的有益效果是：

1.由于建立了服从有色噪声的测量误差模型，使测量误差自适应于测距数据的变化，从而完全符合实际，进而大大地消除了模型误差导致的滤波结果的偏差。

2.门限值选择为测距值的方差，且自适应于测距值的变化，更好地滤除了偏差过大的测距值，使滤波的结果更加准确。

附图说明

图1是本发明的一种基本方法流程示意图。

具体实施方式

下面结合附图对本发明的优选方式作进一步详细的描述。

本发明基于的飞行测距系统为无线传感器网络，该网络中的移动节点和锚节点由jennic公司的型号为JN5148的处理器及其内含TOF测距模块的外围电路组成。

参见图1，处理飞行时间测距数据的自适应卡尔曼滤波的方法的主要步骤如下：

先设定测距个数n为1000，再以此采集移动节点和锚节点之间飞行时间测距数据，以获得1000个TOF测距数据。接着，

步骤1，建立目标状态方程和有色噪声量测方程

x_k+1＝Ax_k+Bα_k，

v_k＝Cx_k+β_k，

β_k＝N_k-1β_k-1+γ_k；

其中，

A为状态矩阵，取值为 $A=\left[\begin{array}{cc}1& T\\ 0& 1\end{array}\right],$ 其中的T为滤波采样间隔，B为误差系数矩阵，取值为 $B=\left[\begin{array}{c}1\\ 0\end{array}\right],$ C为测量矩阵，取值为C＝[1 0]，

x_k为目标在k(k＝1，2，3，......，1000)时刻的二维状态向量， $x_k={\left[\begin{array}{cc}{r}_k& {\stackrel{\·}{r}}_k\end{array}\right]}^T,$ 其中的r_k为待估计的飞行时间值、为其一阶导数，

α_k为系统噪声，是一个高斯白噪声序列，协方差用Q表示，

v_k为一维测量向量，表示k时刻的测距数据，

β_k为k时刻的测量误差，协方差矩阵为R_k，

N_k-1为自回归系数，

γ_k为高斯白噪声序列。

步骤2，滤波过程由k＝1开始，根据原始卡尔曼滤波算法，由k-1时刻的滤波结果得到目标的k时刻的预测值以及k时刻的新息值z_k，计算公式为

$\begin{array}{c}{\hat{x}}_{k/k-1}=A{\hat{x}}_{k-1/k-1},\\ z_k=v_k-C{\hat{x}}_{k/k-1},;\end{array}$

其中，为k-1时刻的滤波结果。

0时刻的滤波结果根据初始测距数据由下式得出，

${\hat{x}}_0=E\left(x_0\right)-\left[\mathrm{var}\left(x_0\right)\right]C^T\·{[C\·\mathrm{var}\left(x_0\right)\·C^T+{\hat{R}}_0]}^{-1}\·[\mathrm{CE}\left(x_0\right)-v_0],$

其中，x₀取为第一个测距数据、v₀为第一个测距数据、取为前30次测距数据的方差。

步骤3，求出测量噪声协方差k时刻的估计值

先计算参数S_k，其由下式来估计得到

${\hat{S}}_k=\frac{1}{k-1}\underset{i=1}{\overset{k}{\mathrm{\Σ}}}(z_i-{\stackrel{\‾}{z}}_i){(z_i-{\stackrel{\‾}{z}}_i)}^T;$

其中，

是前i次新息值的均值，计算为

${\stackrel{\‾}{z}}_i=\frac{1}{i}\underset{j=1}{\overset{i}{\mathrm{\Σ}}}{z}_j.$

再按照下两式得出测量噪声协方差在k时刻的估计值

P_k，k-1＝AP_k-1，k-1A^T+BQB^T，

${\hat{R}}_k={\hat{S}}_k-{\mathrm{CP}}_{k,k-1}{C}^T;$

其中，P_k，k-1是k时刻预测误差方差阵，P_k-1，k-1是k-1时刻估计误差方差阵。

步骤4，计算N_k-1

根据β_k-1和γ_k独立，且有协方差的关系得出N_k-1的计算式为

$N_{k-1}=\sqrt{({\hat{R}}_k-\mathrm{var}\left({\mathrm{\γ}}_k\right))/{\hat{R}}_{k-1}};$

其中，var(γ_k)是γ_k的方差。

步骤5，计算k时刻的门限值m_k

计算前k次测距数据的方差取门限值为方差值，即

步骤6，更新卡尔曼滤波增益

将k时刻的新息值与门限值进行比较，若新息值大于门限值，则将卡尔曼增益置为零；否则按以下两步计算滤波增益G_k，

H_k-1＝[CA-N_k-1C]，

$G_k=({\mathrm{AP}}_{k-1,k-1}{H}_{k-1}^T+{\mathrm{BQB}}^T{C}^T)\·{(H_{k-1}{P}_{k-1,k-1}{H}_{k-1}^T+{\mathrm{CBQB}}^T{C}^T+{\hat{R}}_{k-1})}^{-1}.$

步骤7，更新估计误差方差阵

由得出估计误差方差阵的初值；

其中，

x₀取为第一个测距数据，为误差协方差初值，取作前几次试验测距数据的方差。

估计误差方差阵更新表达式为

P_k，k＝(A-G_kH_k-1)·P_k-1，k-1A^T+(I-G_kC)BQB^T。

步骤8，根据k时刻滤波输出方程输出k时刻的滤波结果，滤波方程为

${\hat{x}}_{k/k}=A{\hat{x}}_{k-1/k-1}+G_k\·(v_k-N_{k-1}{v}_{k-1}-H_{k-1}{\hat{x}}_{k-1/k-1}).$

如此循环往复，输出1到1000时刻的滤波结果。该1000个滤波结果即为移动节点和锚节点之间TOF测距值的处理值，也即对飞行时间测距数据的处理结果。

显然，本领域的技术人员可以对本发明的处理飞行时间测距数据的自适应卡尔曼滤波的方法进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若对本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。

Claims (6)

1.一种处理飞行时间测距数据的自适应卡尔曼滤波的方法，包括对移动节点和锚节点之间飞行时间测距数据的卡尔曼滤波处理，其特征在于主要步骤如下：

步骤1，建立目标状态方程和有色噪声量测方程

x_k+1＝Ax_k+Bα_k，

v_k＝Cx_k+β_k，

β_k＝N_k-1β_k-1+γ_k，

其中，

A为状态矩阵，B为误差系数矩阵，C为测量矩阵，

x_k为目标在k(k＝1，2，3，……n)时刻的二维状态向量，其中的r_k为待估计的飞行时间值、为其一阶导数，

α_k为系统噪声，是一个高斯白噪声序列，协方差用Q表示，

v_k为一维测量向量，表示k时刻的测距数据，

β_k为k时刻的测量噪声，测量噪声协方差矩阵为R_k,

N_k-1为自回归系数，

γ_k为高斯白噪声序列；

步骤2，根据原始卡尔曼滤波算法，由k-1时刻的滤波结果得到目标的k时刻的预测值以及k时刻的新息值z_k,计算公式为

${\hat{x}}_{k/k-1}=A{\hat{x}}_{k-1/k-1},$

$z_k=v_k-C{\hat{x}}_{k/k-1},$

其中，为k-1时刻的滤波结果；

步骤3，求出测量噪声协方差k时刻的估计值

先计算参数s_k，其由下式来估计得到

${\widehat{\mathrm{\δ}}}_k=\frac{1}{k-1}\underset{i=1}{\overset{k}{\mathrm{\Σ}}}(z_i-{\stackrel{\‾}{z}}_i){(z_i-{\stackrel{\‾}{z}}_i)}^T,$

其中，

是前i次新息值的均值，计算为

${\stackrel{\‾}{z}}_i=\frac{1}{i}\underset{j=1}{\overset{1}{\mathrm{\Σ}}}{z}_j,$

再按照下两式得出测量噪声协方差在k时刻的估计值

P_k，k-1＝AP_k-1，k-1A^T+BQB^T，

${\hat{R}}_k={\hat{S}}_k-{\mathrm{CP}}_{k,k-1}{C}^T,$

其中，P_k，k-1是k时刻预测误差方差阵，P_k-1，k-1是k-1时刻估计误差方差阵；

步骤4，计算N_k-1

根据β_h-1和γ_k独立，且由协方差的关系得出N_k-1的计算式为

$N_{k-1}=\sqrt{({\hat{R}}_k-\mathrm{var}({\mathrm{\γ}}_k\left)\right)/{\hat{R}}_{k-1}},$

其中，var(γ_k)是γ_k的方差；

步骤5，计算k时刻的门限值m_k

计算前k次测距数据的方差取门限值为方差值，即

步骤6，更新卡尔曼滤波增益

将k时刻的新息值与门限值进行比较，若新息值大于门限值，则将卡尔曼增益置为零，否则按以下两步计算滤波增益G_k,

H_k-1＝[CA-N_k-1C]，

$G_K=({\mathrm{AP}}_{k-1,k-1}{H}_{k-1}^T+{\mathrm{BQB}}^T{C}^T)\·{(H_{k-1}{P}_{k-1,k-1}{H}_{k-1}^T+{\mathrm{CBQB}}^T{C}^T+{\hat{R}}_{k-1})}^{-1};$

步骤7，更新估计误差方差阵

由得出估计误差方差阵的初值，

其中，

x₀取为第一个测距数据，为测量噪声协方差初始估计值，取为前几次测距数据的方差，

估计误差方差阵更新表达式为

P_k，k-(A-G_kD_k-1)·P_k-1，k-1A^T+(I-G_kC)BQB^T；

步骤8，根据k时刻滤波输出方程输出k时刻的滤波结果，滤波输出方程为

${\hat{x}}_{k/k}=A{\hat{x}}_{k-1/k-1}+G_k\·(v_k-N_{k-1}{v}_{k-1}-H_{k-1}{\hat{x}}_{k-1/k-1}),$

如此循环往复，输出1到n时刻的滤波结果，得到对飞行时间测距数据的处理结果。

2.根据权利要求1所述的处理飞行时间测距数据的自适应卡尔曼滤波的方法，其特征是步骤1中的状态矩阵A的取值为其中，T为滤波采样间隔。

3.根据权利要求1所述的处理飞行时间测距数据的自适应卡尔曼滤波的方法，其特征是步骤1中的误差系数矩阵B的取值为

4.根据权利要求1所述的处理飞行时间测距数据的自适应卡尔曼滤波的方法，其特征是步骤1中的测量矩阵C的取值为C＝[1 0]。

5.根据权利要求1所述的处理飞行时间测距数据的自适应卡尔曼滤波的方法，其特征是步骤1中的x_k的下标k＝n时的取值为1000。

6.根据权利要求1所述的处理飞行时间测距数据的自适应卡尔曼滤波的方法，其特征是步骤2中的0时刻的滤波结果根据初始测距数据由下式得出，

${\hat{x}}_0=E\left(x_0\right)-\[\mathrm{var}\left(x_0\right)\]C^T\·{\[C\·\mathrm{var}\left(x_0\right)\·C^T+{\hat{R}}_0\]}^{-1}\·\[CE\left(x_0\right)-v_0\],$

其中，x₀取为第一个测距数据、v₀为第一个测距数据、取为前30次测距数据的方差。