CN106707235B - 一种基于改进的无迹卡尔曼滤波的室内测距定位方法 - Google Patents

Abstract

本发明公开了一种基于改进的无迹卡尔曼滤波的室内测距定位方法，包括选用卡尔曼滤波对接收到的RSSI值进行预处理得出状态向量的最优估计；采用最小二乘法对经过卡尔曼滤波处理过的RSSI值进行数据拟合，得出当前实验环境下的两个环境参数A和m；用改进的无迹卡尔曼滤波方法对RSSI值进行二次处理，得出锚节点和目标节点间的距离值；将求得的锚节点和目标节点间的距离值通过三边测量法估计出未知节点位置坐标。本发明方法借助于鲁棒目标函数实时修正无迹卡尔曼滤波中的噪声估计值，降低噪声不准的估计值的权重，提高状态变量估计精度。

Description

一种基于改进的无迹卡尔曼滤波的室内测距定位方法

技术领域

本发明涉及一种基于改进的无迹卡尔曼滤波的室内测距定位方法，属于室内测距定位技术领域。

背景技术

当今时代，随着移动通信和无线传感器网络技术的发展，基于位置的服(Location-Based Services,LBS)受到了人们的广泛关注，如文献ERYA B O,RALPH WP.Theory and practice of simultaneous data reconciliation a gross errordetection for chemical processes[J].Computers and Chemical Engineering,2004,28(3):381-402。对于室外定位***，全球导航卫星***(Global Navigation SatelliteSystem,GNSS)能够提供精确定位，应用广泛，如文献ZHEN L,FANG H J.Modified stateprediction algorothm based on UKF[J].Journal of Systems Engineering andElectronics.2013,24(1):135-140。在建筑密集区或者复杂的室内环境中，由于受到建筑物的影响，卫星信号强度大大衰减，使用受限甚至不能完成定位。因此室内定位研究受到广泛关注。

室内定位算法分为基于测距定位算法和非测距定位算法，ZHEN L,FANG HJ.Modified state prediction algorothm based on UKF[J].Journal of SystemsEngineering and Electronics.2013,24(1):135-140。基于非测距的定位方法是利用网络连通性等信息实现节点定位，如近似三角形内点测试算法(Approximate Point-In-Triangulation Test,APIT)，如冯秀芳,崔秀锋,祈会波.无线传感器网络中基于移动锚节点的APIT的改进定位算法[J]，传感技术学报,2011,24(2):269-274]、DV-Hop(DistanceVector-hop)等；基于测距的定位方法是通过测量节点间的角度或距离等信息，通过三边测量或最大似然估计来计算节点的位置，定位精度相对较高，如GARG V,JHAMB M.A Reviewof Wireless Sensor Network on Localization Techniques[J].InternationalJournal of Engineering Trendsand Technology,2013,4(4):1049－1053。早期Hightower等人[HIGHTOWER J，WANT R，BORRIELLO G.SpotON:An Indoor 3D LocationSensing Technology Based on RF Signal Strength[D].UW CSE 00－02－02，University of Washington，Department of Computer ScienceandEngineering，Seattle，WA，2000，1]通过接收信号强度(RSSI，Received Signal Strength Indicator)测距方法来估计两点间的距离，通过节点间的相互位置来进行定位，但室内环境对该方法的精度造成严重影响。文献[MINGHUI Z,HUIQING Z.Research on Model of IndoorDistance Measurement Based on Receiving SignalStrength[C]//Computer Designand Applications(ICCDA),2010International Conference on.IEEE,2010,5:54－58]采用线性回归法分析测距模型中的两个参数(距离发射节点1m的RSSI值和路径损耗指数)，但仍不能抑制非视距(NLOS)误差。文献[张亚明,史浩山,刘燕,等.基于抗差估计方法的WSN节点定位算法研究[J].传感技术学报,2014,27(8):1088－1093]借助抗差估计理论，对不同的误差进行降权和剔除等处理，提高定位精度。

发明内容

本发明所要解决的技术问题是克服现有技术的缺陷，提供一种基于改进的无迹卡尔曼滤波的室内测距定位方法，改善测距误差，提高定位精度。

为解决上述技术问题，本发明提供一种基于改进的无迹卡尔曼滤波的室内测距定位方法，包括以下步骤：

1)在室内布置n个锚节点，n≥3，用于发射信号，锚节点位置固定，且坐标已知，目标节点作为接收节点，位置可移动，用于接收锚节点发送的RSSI值；

2)选用卡尔曼滤波对接收到的RSSI值进行预处理得出状态向量的最优估计；

3)采用最小二乘法对经过卡尔曼滤波处理过的RSSI值进行数据拟合，得出当前实验环境下的两个环境参数A和m，A为信号传输距离1m远时接收信号的功率，m为与环境有关的路径散逸指数；

4)用改进的无迹卡尔曼滤波方法对RSSI值进行二次处理，得出锚节点和目标节点间的距离值；

5)将步骤4)中求得的锚节点和目标节点间的距离值通过三边测量法估计出未知节点位置坐标。

前述的步骤2)中，卡尔曼滤波过程具体步骤如下：

2-1)预测状态估计值：

2-2)预测状态误差协方差矩阵：

P(k|k-1)＝φ(k|k-1)+ψ(k|k-1)Q(k-1)ψ^T(k|k-1)

2-3)修正状态误差协方差矩阵：

P(k)＝[I-K_kH(k)]P(k|k-1)

其中，K_k＝P(k|k-1)H^T(k)[H(k)P(k|k-1)×H^T(k)+R(k)]^-1

2-4)修正状态估计值，得出状态向量的最优估计：

其中:为已知测量值y(k)时，k时刻的修正状态估计值，为k时刻的前一刻k-1时刻的状态估计值，为用k时刻的前一刻k-1时刻的状态估计值对k时刻的预测状态估计值，φ(k|k-1)为k-1时刻到k时刻的一步转移矩阵，P(k|k-1)为预测状态误差协方差矩阵，P(k)为修正状态误差协方差矩阵，ψ(k|k-1)为***噪声驱动矩阵，I为单位矩阵，K_k为卡尔曼增益矩阵，H(k)为k时刻的***测量矩阵，R(k)为k时刻的***观测噪声方差阵，Q(k-1)为k-1时刻的***过程方差阵。

前述的步骤4)中，改进的无迹卡尔曼滤波方法是指基于对数鲁棒函数的无迹卡尔曼滤波改进方法。

前述的基于对数鲁棒函数的无迹卡尔曼滤波改进方法，具体如下：

4-1)无线局域网定位***是离散非线性***，待定节点位置不变，状态方程为线性方程：

s(k)＝s(k-1)+ω(k)

其中，s(k)表示k时刻锚节点和目标节点间的距离值，ω(k)表示状态误差，假定满足ω(k)～N{0,Q(k)}，Q(k)为k时刻***过程方差阵；

观测方程是非线性方程：

z(k)＝g(s(k))+υ(k)

其中，z(k)表示k时刻的经过卡尔曼滤波处理过后的RSSI值，υ(k)表示观测误差，满足υ(k)～N{0,R(k)}，R(k)为k时刻的***观测噪声方差阵，

g(s(k))＝A-10m lg(s(k))

4-2)采用对数鲁棒函数对***过程方差阵进行动态校正，首先构造目标函数ρ(r(k))：

其中，c是可调参数，r(k)为k时刻的相对残差值，为先验测量值，z(k)为实际测量值即k时刻的经过卡尔曼滤波处理过后的RSSI值，σ(k)为测量值标准差；

该鲁棒估计的影响函数ψ(r(k))为：

则对数鲁棒函数的联合权函数I(r(k))为：

4-3)在滤波过程中，取先验测量值与实际测量值z(k)的相对残差作为r(k)校正***过程方差阵的估计值，间接地使目标函数最小，因此，将对数鲁棒函数的联合权函数作为***过程方差阵的修正系数，确定过程噪声方差的校正公式为：

其中，为k时刻校正后的***过程方差阵估计值，为k-1时刻的***过程方差阵估计值。

前述的步骤4-3)中，设定一个阈值η，对***过程方差阵进行修正，即：

If|r(k)|≤η

前述的改进的无迹卡尔曼滤波具体步骤如下：

4-a)设定锚节点和目标节点间的初始距离值估计值和协方差P₀：

其中，E(s₀)表示期望，s₀表示k为0时刻时的锚节点和目标节点间的距离值；

4-b)对于给定的k-1时刻锚节点和目标节点间的距离值和协方差P(k-1)，用U-变换构造2l+1个sigma点，l表示维度，并预测下一状态的锚节点和目标节点间的距离值和协方差矩阵

当k>1时，构造2l+1个sigma点如下：

式中，χ_i(k-1)表示k-1时刻第i列的sigma点，i＝0,1,2…2l，λ＝α²(l+k)-l是一个比例因子，α是一个尺度参数，0≤α≤1，k是可调参数，表示矩阵平方根的第i列，i＝1,2…2l，P_s,k-1表示k-1时刻状态值s的协方差；

下一状态的锚节点和目标节点间的距离值为：

下一状态的协方差矩阵为：下一状态的第i列sigma点为：

其中，W_i ^(m)，W_i ^(c)，i＝0,1,2…2l表示权值；

确定权值：

W_i ^(m)＝W_i ^(c)＝1/[2(l+1)],i＝1,2,...2l

计算预测sigma点的均值和方差：

If|r(k)|≤η

4-c)测量更新

当获得新的测量值后，对状态均值和方差进行更新：

其中，

其中：P_z,k表示k时刻观测值z对应的协方差，表示测量值更新后k时刻状态均值，P_s(k)表示测量值更新后k时刻状态值s对应的协方差，P_sz,k表示状态值和观测值的预测协方差，K表示增益矩阵，

本发明所达到的有益效果：

本发明方法借助于鲁棒目标函数实时修正无迹卡尔曼滤波中的噪声估计值，降低噪声不准的估计值的权重，提高状态变量估计精度，使用改进后的滤波方法对数据进行二次处理，求得未知节点到锚节点的距离，减少测距误差。本发明方法与的传统无迹卡尔曼滤波测距方法及室内定位方法相比，在不需要额外增加硬件设备条件下，将测距误差降低了14.4％，有效的减少测距误差，提高室内定位***的定位精度。

附图说明

图1为距离与RSSI值的关系；

图2为实施例中不同距离下对应的定位误差；

图3为实施例中不同距离下对应的误差均方值；

图4为实施例中定位结果。

具体实施方式

下面对本发明作进一步描述。以下实施例仅用于更加清楚地说明本发明的技术方案，而不能以此来限制本发明的保护范围。

在室内布置n(n≥3)个锚节点，用于发射信号，锚节点位置固定，且坐标已知，目标节点作为接收节点，位置可移动，用于接收锚节点发送的RSSI值，定位过程如下：

(1)首先选用卡尔曼滤波对接收到的RSSI值进行预处理得出状态向量的最优估计。

卡尔曼滤波的状态方程和测量方程如下：

x(k)＝φx(k-1)+ω(k-1)

y(k)＝Hx(k)+υ(k)

式中，x(k)为k时刻的状态值，y(k)为k时刻的测量值，x(k-1)为k-1时刻的状态值，φ为状态转移矩阵，H为***测量矩阵，ω(k-1)为k-1时刻输入白噪声，υ(k)为k时刻的观测噪声。

卡尔曼滤波过程具体步骤如下：

1)预测状态估计值

2)预测状态误差协方差矩阵

P(k|k-1)＝φ(k|k-1)+ψ(k|k-1)Q(k-1)ψ^T(k|k-1)

3)修正状态误差协方差矩阵

P(k)＝[I-K_kH(k)]P(k|k-1)

K_k＝P(k|k-1)H^T(k)[H(k)P(k|k-1)×H^T(k)+R(k)]^-1

4)修正状态估计值，得出状态向量的最优估计：

其中:为已知测量值y(k)时，k时刻的修正状态估计值；为k时刻的前一刻k-1时刻的状态估计值；为用k时刻的前一刻k-1时刻的状态估计值对k时刻的预测状态估计值；φ(k|k-1)为k-1时刻到k时刻的一步转移矩阵；ψ(k|k-1)为***噪声驱动矩阵；I为单位矩阵；K_k为卡尔曼增益矩阵；H(k)为k时刻的***测量矩阵；R(k)为k时刻的***观测噪声方差阵；Q(k)为k时刻***过程方差阵；Q(k-1)为k-1时刻的***过程方差阵。

(2)采用最小二乘法对经过卡尔曼滤波处理过的RSSI值进行数据拟合，得出当前实验环境下的两个环境参数A和m，A为信号传输距离1m远时接收信号的功率(dBm)，m为与环境有关的路径散逸指数，根据实验所测数据求得A＝-45.524 2dBm，m＝2.039 07，拟合曲线如图1所示。

(3)用改进的无迹卡尔曼滤波方法对RSSI值进行二次处理，得出锚节点和目标节点间的距离值。

基于对数鲁棒函数的无迹卡尔曼滤波改进方法如下：

无线局域网定位***是离散非线性***。待定节点位置不变，状态方程为线性方程：

s(k)＝s(k-1)+ω(k)

式中：s(k)表示k时刻锚节点和目标节点间的距离值，ω(k)表示状态误差，假定满足ω(k)～N{0,Q(k)}，Q(k)为k时刻***过程方差阵。

观测方程是非线性方程：

z(k)＝g(s(k))+υ(k)

式中，z(k)表示k时刻的经过卡尔曼滤波处理过后的RSSI值，υ(k)表示观测误差，满足υ(k)～N{0,R(k)}，R(k)为k时刻的***观测噪声方差阵，其中：

g(s(k))＝A-10m lg(s(k))

本发明采用对数鲁棒函数对***过程方差阵进行动态校正，首先构造目标函数ρ(r(k))：

式中，c是可调参数，根据效率选取；r为相对残差值，r(k)为k时刻的相对残差值。

σ(k)为测量值标准差。

该鲁棒估计的影响函数ψ(r(k))为：

则对数鲁棒函数的联合权函数I(r(k))为：

在滤波过程中，取先验测量值与实际测量值z(k)的相对残差作为r(k)校正***过程方差阵的估计值，间接地使目标函数最小，即测量数据估计与实际测量值偏差极小。实际测量值z(k)即k时刻的经过卡尔曼滤波处理过后的RSSI值。

当r(k)越小时，说明***过程方差阵对先验测量值的影响越小，***过程方差阵需要校正量越小，r(k)趋近于0(极小点)时，应使***过程方差阵不被校正，因此将对数鲁棒函数的权函数作为***过程方差阵的修正系数。这样，***过程方差阵的校正公式为：

其中，为k时刻校正后的***过程方差阵估计值，由无迹卡尔曼滤波计算公式可以得到k时刻先验测量值是基于k-1时刻的值而获得的。因此，需要以迭代的方式对***过程方差阵进行修正。当迭代修正的时候，由于先验测量值与实际测量值z(k)的差值不可能精确为零，所以数千次的迭代将使***过程方差阵值越来越小，显然不符合实际情况。因此需要设定一个阈值η，使先验测量值与实际测量值z(k)的偏差小于这个阈值时，不对***过程方差阵进行修正，使***过程方差阵保持不变。超过这个阈值时，对***过程方差阵进行修正。即：

If|r(k)|≤η

具体滤波步骤如下：

1)设定锚节点和目标节点间的初始距离值估计值和协方差P₀：

其中，E(s₀)表示期望，s₀表示k为0时刻时的锚节点和目标节点间的距离值。

2)对于给定的k-1时刻锚节点和目标节点间的距离值和方差P(k-1)，用U-变换构造2l+1(l表示维度)个sigma点，并预测下一状态的锚节点和目标节点间的距离值和协方差矩阵

当k>1时，构造2l+1个sigma点如下：

式中，χ_i(k-1)表示k-1时刻第i列的sigma点，i＝0,1,2...2l；λ＝α²(l+k)-l是一个比例因子；α是一个尺度参数，0≤α≤1；k是可调参数；表示矩阵平方根的第i列；P_s,k-1表示k-1时刻距离值s(k-1)的协方差。

下一状态的锚节点和目标节点间的距离值为：

下一状态的协方差矩阵为：

下一状态的第i列sigma点为：

确定权值：

W_i ^(m)＝W_i ^(c)＝1/[2(l+1)],i＝1,2,...2l

计算预测sigma点的均值和方差P_s,k：

If|r(k)|≤η

UKF算法采用UT变换，在自变量中选择若干个特定样本，又称sigma点，这些点包含自变量的一、二阶矩统计特性信息，由这些sigma点加权计算，获得因变量的一、二阶矩统计特性。sigma点就是算法中自变量中的样本，所以P_s,k是sigma点的均值和方差。

3)测量更新

UKF算法采用UT变换，在自变量中选择若干个特定样本，又称sigma点，这些点包含自变量的一、二阶矩统计特性信息，由这些sigma点加权计算，获得因变量的一、二阶矩统计特性。sigma点就是算法中自变量中的样本，所以下一状态的锚节点和目标节点间的距离值和协方差矩阵即对应sigma点下一状态的均值和方差。

当获得新的测量值后，对状态均值和方差进行更新：

其中

式中：P_z,k表示k时刻测量值z(k)对应的协方差；表示测量值更新后k时刻状态均值；P_s(k)表示测量值更新后k时刻距离值s(k)对应的协方差；P_sz,k表示状态值和观测值的预测协方差；K表示增益矩阵；

(4)将步骤(3)中求得的锚节点和目标节点间的距离值通过三边测量法估计出未知节点位置坐标。三边测量法可参考[吴君钦，卢陶.基于RSSI测距的无线传感器网络定位算法[J].微电子与计算机，2014：49-52]，不再详述。

实施例

在实验室内设置两个节点，发射节点和接收节点，放置在同一高度进行数据采集，实验测距范围为0～15m，在距发射节点0.96m开始测量，每间隔0.96m，测量100次RSSI值，对实验数据进行相应处理，以距离值为状态值，以预处理过的RSSI值为观测值，利用改进的无迹卡尔曼滤波方法求得距离值，实验结果如图2、图3所示。从图2和图3的结果可以看出，三种处理方法，基于对数鲁棒函数的无迹卡尔曼滤波改进方法(aukf)的数据变化最大，先后用卡尔曼滤波和无迹卡尔曼滤波(kf+ukf)处理RSSI值，以及先后用卡尔曼滤波和基于对数鲁棒函数的无迹卡尔曼滤波改进方法(kf+aukf)处理RSSI值的数据变化程度差不多，但先后用卡尔曼滤波和基于对数鲁棒函数的无迹卡尔曼滤波改进方法处理RSSI值的定位误差最小，说明采用卡尔曼滤波和基于对数鲁棒函数的无迹卡尔曼滤波改进方法先后对RSSI值进行处理后，测距误差最小，较前两种处理方法测距精度分别提高了4.46％和14.4％，测距精度明显提升。

进一步的，将本发明方法与参考文献[方庆，李平.基于Unscented卡尔曼滤波的室内定位.计算机工程与应用.2014,50(15):74-77]采用的定位方法进行对比。在一个15m×15m的室内进行实验，首先设定三个锚节点，其位置坐标分别为(1,4)，(2,0)，(4,4)，目标节点放置坐标为(2,2)，(3,1)(1,1)，给定初始值为(2,3)，初始误差协方差矩阵P₀＝1，UT变换的参数设置α＝0.01，κ＝0，过程噪声协方差矩阵Q＝0.01,观测噪声协方差矩阵R＝0.01。定位结果如图4所示，由图中结果可知，本发明方法定位方法优于参考文献采用的定位算法。表1和表2分别表示在x轴和y轴方向上，两种处理方法的最大误差、最小误差和平均误差的比较。由实验结果可知，本发明方法在x轴和y轴方向上的最大误差、最小误差和平均误差都有所降低，优于参考文献采用的定位算法。由实验结果可知，本发明方法能够进一步降低测距误差，提高定位精度。

表1 x轴方向2种处理方法误差比较

表2 y轴方向2种处理方法误差比较

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。

Claims (4)

1.一种基于改进的无迹卡尔曼滤波的室内测距定位方法，其特征在于，包括以下步骤：

1)在室内布置n个锚节点，n≥3，用于发射信号，锚节点位置固定，且坐标已知，目标节点作为接收节点，位置可移动，用于接收锚节点发送的RSSI值；

2)选用卡尔曼滤波对接收到的RSSI值进行预处理得出状态向量的最优估计；

3)采用最小二乘法对经过卡尔曼滤波处理过的RSSI值进行数据拟合，得出当前实验环境下的两个环境参数A和m，A为信号传输距离1m远时接收信号的功率，m为与环境有关的路径散逸指数；

4)用改进的无迹卡尔曼滤波方法对RSSI值进行二次处理，得出锚节点和目标节点间的距离值；

所述改进的无迹卡尔曼滤波方法是指基于对数鲁棒函数的无迹卡尔曼滤波改进方法，具体如下：

4-1)无线局域网定位***是离散非线性***，待定节点位置不变，状态方程为线性方程：

s(k)＝s(k-1)+ω(k)

其中，s(k)表示k时刻锚节点和目标节点间的距离值，ω(k)表示状态误差，假定满足ω(k)～N{0,Q(k)}，Q(k)为k时刻***过程方差阵；

观测方程是非线性方程：

z(k)＝g(s(k))+υ(k)

其中，z(k)表示k时刻的经过卡尔曼滤波处理过后的RSSI值，υ(k)表示观测误差，满足υ(k)～N{0,R(k)}，R(k)为k时刻的***观测噪声方差阵，

g(s(k))＝A-10mlg(s(k))

4-2)采用对数鲁棒函数对***过程方差阵进行动态校正，首先构造目标函数ρ(r(k))：

其中，c是可调参数，r(k)为k时刻的相对残差值， 为先验测量值，z(k)为实际测量值即k时刻的经过卡尔曼滤波处理过后的RSSI值，σ(k)为测量值标准差；

该鲁棒估计的影响函数ψ(r(k))为：

则对数鲁棒函数的联合权函数I(r(k))为：

4-3)在滤波过程中，取先验测量值与实际测量值z(k)的相对残差作为r(k)校正***过程方差阵的估计值，间接地使目标函数最小，因此，将对数鲁棒函数的联合权函数作为***过程方差阵的修正系数，确定过程噪声方差的校正公式为：

其中，为k时刻校正后的***过程方差阵估计值，为k-1时刻的***过程方差阵估计值；

5)将步骤4)中求得的锚节点和目标节点间的距离值通过三边测量法估计出未知节点位置坐标。

2.根据权利要求1所述的一种基于改进的无迹卡尔曼滤波的室内测距定位方法，其特征在于，所述步骤2)中，卡尔曼滤波过程具体步骤如下：

2-1)预测状态估计值：

2-2)预测状态误差协方差矩阵：

P(k|k-1)＝φ(k||k-1)+ψ(k|k-1)Q(k-1)ψ^T(k|k-1)

2-3)修正状态误差协方差矩阵：

P(k)＝[I-K_kH(k)]P(k|k-1)

其中，K_k＝P(k|k-1)H^T(k)[H(k)P(k|k-1)×H^T(k)+R(k)]^-1

2-4)修正状态估计值，得出状态向量的最优估计：

其中:为已知测量值y(k)时，k时刻的修正状态估计值，为k时刻的前一刻k-1时刻的状态估计值，为用k时刻的前一刻k-1时刻的状态估计值对k时刻的预测状态估计值，φ(k|k-1)为k-1时刻到k时刻的一步转移矩阵，P(k|k-1)为预测状态误差协方差矩阵，P(k)为修正状态误差协方差矩阵，ψ(k|k-1)为***噪声驱动矩阵，I为单位矩阵，K_k为卡尔曼增益矩阵，H(k)为k时刻的***测量矩阵，R(k)为k时刻的***观测噪声方差阵，Q(k-1)为k-1时刻的***过程方差阵。

3.根据权利要求1所述的一种基于改进的无迹卡尔曼滤波的室内测距定位方法，其特征在于，所述步骤4-3)中，设定一个阈值η，对***过程方差阵进行修正，即：

If|r(k)|≤η

4.根据权利要求3所述的一种基于改进的无迹卡尔曼滤波的室内测距定位方法，其特征在于，改进的无迹卡尔曼滤波具体步骤如下：

4-a)设定锚节点和目标节点间的初始距离值估计值和协方差P₀：

其中，E(s₀)表示期望，s₀表示k为0时刻时的锚节点和目标节点间的距离值；

4-b)对于给定的k-1时刻锚节点和目标节点间的距离值和协方差P(k-1)，用U-变换构造2l+1个sigma点，l表示维度，并预测下一状态的锚节点和目标节点间的距离值和协方差矩阵

当k>1时，构造2l+1个sigma点如下：

式中，χ_i(k-1)表示k-1时刻第i列的sigma点，i＝0,1,2...2l，λ＝α²(l+k)-l是一个比例因子，α是一个尺度参数，0≤α≤1，k是可调参数，表示矩阵平方根的第i列，i＝1,2…2l，P_s,k-1表示k-1时刻状态值s的协方差；

下一状态的锚节点和目标节点间的距离值为：

下一状态的协方差矩阵为：

下一状态的第i列sigma点为：

其中，W_i ^(m)，W_i ^(c)，i＝0,1,2…2l表示权值；

确定权值：

W_i ^(m)＝W_i ^(c)＝1/[2(l+1)],i＝1,2,...2l

计算预测sigma点的均值和方差：

If|r(k)|≤η

4-c)测量更新

当获得新的测量值后，对状态均值和方差进行更新：

其中，

其中：P_z,k表示k时刻观测值z对应的协方差，表示测量值更新后k时刻状态均值，P_s(k)表示测量值更新后k时刻状态值s对应的协方差，P_sz,k表示状态值和观测值的预测协方差，K表示增益矩阵，