CN106646356B - 一种基于卡尔曼滤波定位的非线性系统状态估计方法 - Google Patents

Abstract

本发明公开了一种基于卡尔曼滤波定位的非线性系统状态估计方法，提出了一种非线性滤波算法与卡尔曼滤波相结合的强适应卡尔曼滤波机制，使用基于平方根容积卡尔曼滤波的RSSI状态估计算法，对节点位置和信道参数同时进行估计，得到状态向量的估计值；依据状态方程的线性变化，使用卡尔曼滤波进一步处理得到最优估计，建立强适应平方根容积卡尔曼滤波算法；同理给出强适应扩展卡尔曼滤波算法的设计步骤；计算状态空间模型下基于RSSI状态估计的理论均方根误差下界。本发明使得估计的结果得到改善，提高精度。本发明不过分依赖于不合适的初始条件；能够很好地适用于高度非线性化系统，不会轻易让算法发散失效。

Description

一种基于卡尔曼滤波定位的非线性系统状态估计方法

技术领域

本发明属于通信技术领域，尤其涉及一种基于卡尔曼滤波定位的非线性系统状态估计方法。

背景技术

无线传感器网络(Wireless Sensor Networks，WSNs)作为计算、通信和传感器技术相结合的产物，改变了人类与自然界的交互方式，将逻辑上的信息世界与客观上的物理世界融合在一起，提升了人类认识世界、改造世界的能力。在WSNs的应用领域中，不含有节点位置的感知信息不会产生任何价值。为了顺利完成监测任务，随机散布的传感器节点需要具备及时准确提供自身位置信息的能力，这对整个系统功能的实现至关重要。把节点定位方法按照是否需要测距进行划分是最常用的一种分类方法，即基于测距(Range-based)和测距无关(Range-free)的定位方法。Range-based的定位方法假定节点能运用几何关系测量相互间的距离或方向，并利用这个距离或方向信息作为约束确定节点的位置，其中基于信号接收强度指示(RSSI)的测距方法被广泛应用于节点定位中。而Range-free的方法则无需测量节点间的距离和方向，仅仅利用网络拓扑的信息(例如节点的邻居关系或节点间的跳数等)来确定节点的位置，由于传感器节点在部署中难以做到均匀分布，再加上无线信号沿各方向传播的不规则性，使得这类定位方法的误差较大。以目前看来，尽管行之有效的定位算法层出不穷，仍有很多问题亟待解决：1)广泛使用的RSSI定位方法仅需要少量的信号采样值进行测距，然后同时对信道参数与节点位置进行估计。然而这类方法通常具有一个固定前提，即假设发射信号功率恒定不变，但是实际上发射信号的功率不能总保持恒定，因为要考虑到工作电源的变化、器件参数不同及环境扰动等因素影响，这就使得RSSI定位方法受到局限。2)很多基于测距的方法都是利用无线电传播损耗模型测距，然而由于实际环境不同，模型中的参数也不同。虽然基于非线性滤波的RSSI定位方法能够利用RSSI值对模型中的参数进行动态估计，但同时估计的节点位置精度并不十分让人满意，需要对此类方法作进一步的改进。近年来，非线性状态估计越来越发挥出了重要的作用，其在目标跟踪、信息处理、参数估计及定位等方面都具有重要的应用。先后发展了扩展卡尔曼滤波(ExtendedKalman Filter,EKF)、容积卡尔曼滤波(Cubature Kalman Filter,CKF)和无迹卡尔曼滤波(Unscented Kalman Filter,UKF)等多种非线性滤波方法，然而这些衍生的卡尔曼滤波方法对于非高斯模型来说均不是最优的滤波器。粒子滤波是另一类非线性滤波方法，与衍生的卡尔曼滤波方法相比，系统的非线性和非高斯性越强，粒子滤波效果越好。但是针对高斯非线性模型，有人采用球面径向容积准则的积分法则进行系统状态的非线性传递的CKF算法，不仅有效克服了EKF在强非线性系统中的应用局限性，且滤波精度甚至高于粒子滤波。为了提高CKF的性能，在其滤波过程中加入误差协方差阵的平方根，用来保证CKF误差协方差阵的对称性和正(半)定性，从而构成了平方根容积卡尔曼滤波(Square-rootCubature KalmanFilter,SCKF)算法。SCKF算法是一种免微分的滤波算法，其以CKF为框架，使用容积原则进行相应的数值积分来处理状态均值和协方差的非线性传递，滤波精度高，所需容积点个数较少，计算量小且运算时间较短。有人提出的基于SCKF的RSSI状态估计算法，将RSSI定位问题转变为非线性系统的状态估计问题，利用SCKF对WSNs中待定位节点位置和RSSI信道衰减系数及距离信源1m处的接收信号功率同时进行估计，并利用参数动态变化实时修正估计的定位节点坐标，进一步提高定位精度。SCKF算法不具有对测量条件变化和系统模型不确定性的自适应性，不良的测量或系统模型的变化会影响滤波性能甚至导致滤波故障，其对不良测量的鲁棒性可以通过应用新息协方差匹配技术建立自适应滤波、鲁棒滤波得到提高，其对模型不确定的影响可以利用强跟踪滤波器来缓解，但STF具有计算精度低及需要计算雅克比矩阵等理论局限性。考虑受到噪声的影响，特别在低信噪比情况下，SCKF的滤波结果会出现发散情况，其性能会受到限制。

综上所述，现有的非线性系统状态估计方法存在均方根误差较大、状态估计不准确的问题。

发明内容

本发明的目的在于提供一种基于卡尔曼滤波定位的非线性系统状态估计方法，旨在解决现有的非线性系统状态估计方法存在均方根误差较大、状态估计不准确的问题。

本发明是这样实现的，一种基于卡尔曼滤波定位的非线性系统状态估计方法，所述基于卡尔曼滤波定位的非线性系统状态估计方法提出了一种普通非线性滤波算法与卡尔曼滤波相结合的强适应卡尔曼滤波机制，即在SCKF或EKF的状态向量估计基础上利用卡尔曼滤波再进行处理，从而使得估计的结果得到改善。该机制首先对非线性模型利用SCKF进行容积近似，得到状态向量的估计值，又依据状态方程的线性变化，使用卡尔曼滤波进一步处理得到最优估计，建立强适应SCKF算法。为对比分析，同理给出了强适应EKF算法的设计步骤以及系统模型下基于RSSI状态向量估计的理论均方根误差下界。

进一步，所述平方根容积卡尔曼滤波算法的基础上利用卡尔曼滤波进行处理的方法包括：

步骤一，初始化滤波，定义系统在k＝0时刻状态向量的初值为其协方差矩阵为P_0|0，通过QR分解得到方差阵的平方根S_0|0，模拟生成待定位节点接收到的n个通信范围内的信标节点的RSSI值；

步骤二，循环执行平方根容积卡尔曼滤波算法，For k＝1,2,…N，N为取样次数，将和S_k|k作为输入，重复执行直到N次取样完成；由平方根容积卡尔曼滤波算法得到的估计值与之间的误差应该符合一个加性噪声的关系，即其中观测噪声e_k假设服从均值为零，协方差矩阵为Φ_k＝cov(e_k)的高斯分布；

综上得到如下动态系统，其状态空间模型为：

状态方程:

测量方程:

进一步，所述步骤二包括：

(1)按照平方根容积卡尔曼滤波算法，进行时间更新和测量更新得到当前时刻的状态估计值及相应的误差协方差阵的平方根S_k|k；

(2)执行卡尔曼滤波递推更新

在平方根容积卡尔曼滤波算法估计的基础上进行进一步卡尔曼滤波，得到最优估计

P_k|k-1＝AP_k-1|k-1A^T+Q_k-1；

G_k＝P_k|k-1(Φ_k+P_k|k-1)^-1；

P_k|k＝(I-G_k)P_k|k-1；

其中P_k-1|k-1为k-1时刻最优估计值的协方差矩阵，Q_k-1为过程噪声w_k-1的协方差矩阵P_k|k-1为k时刻预测值的协方差矩阵，P_k|k为k时刻最优估计值的协方差矩阵，G_k是卡尔曼增益。关于初值的设置，令为平方根容积卡尔曼滤波算法估计前状态向量的初值，P_0|0为平方根容积卡尔曼滤波算法估计前协方差矩阵的初值；

得到的协方差矩阵P_k|k-1，卡尔曼增益G_k，得到状态向量的最优估计得到的协方差矩阵P_k|k；

(3)计算状态向量理论上的均方根误差下界，对于v＝[v₁,v₂,…v_n]^T，其中v_i是待定位节点接收第i个信标节点的RSSI时对应的随机测量噪声，且即均方根误差下界通过克拉默-拉奥界获得如下：

其中F通过下式来计算：

进一步，所述扩展卡尔曼滤波的基础上利用卡尔曼滤波进行处理的方法包括：

(1)初始化滤波，定义系统在k＝0时刻状态向量的初值为其协方差矩阵初值为C_0|0，卡尔曼滤波状态向量的协方差初值为P_0|0，模拟生成待定位节点接收到的n个通信范围内的信标节点的RSSI值；

(2)循环执行扩展卡尔曼滤波

For k＝1,2,…N，N为取样次数，将得到的和C_k|k作为输入，重复执行直到N次取样完成；

在状态空间模型中，对于待估计的状态向量由于系统方程γ_k＝Aγ_k-1+w_k-1中的A是单位状态转移矩阵，无需求导；测量方程中的非线性观测函数h(γ_k)＝[h₁(γ_k),h₂(γ_k),…h_n(γ_k)]^T，其中1≤i≤n；对此函数在γ_k处求偏导数得到如下Jacobian矩阵H_k：

进一步，所述(2)包括：

按照下述公式，得到当前时刻的状态估计值及相应的误差协方差阵C_k|k：

C_k|k-1＝AC_k-1|k-1A^T+Q_k-1

C_k|k＝(I-W_kH_k)C_k|k-1

执行卡尔曼滤波更新：

得到的协方差矩阵P_k|k-1，得到卡尔曼增益G_k，得到状态向量的最优估计得到的协方差矩阵P_k|k。

本发明的另一目的在于提供一种利用所述基于卡尔曼滤波定位的非线性系统状态估计方法的无线传感器网络。

本发明提供的基于卡尔曼滤波定位的非线性系统状态估计方法，比起现有的未经强适应递推的非线性滤波算法以及单一的仅适用于线性系统模型的卡尔曼滤波算法，强适应滤波设计使得系统的状态估计取得了更低的均方根误差，相对于理论均方根误差下界甚至会获得更低的误差下界。本发明不过分依赖于不合适的初始条件，具有较好的算法稳定性，能够很好地适用于高度非线性化系统，不会轻易让算法发散失效，计算量小且运算时间较短，而且提出的强适应机制是建立在系统状态模型、观测模型及噪声都符合高斯分布假设的基础上，所以将来的研究工作可以拓展到非高斯背景下的非线性状态估计研究。

在SCKF或EKF的状态向量估计基础上利用卡尔曼滤波再进行处理，从而使得估计的结果得到改善，提高精度。在50次独立仿真试验中，状态向量的前两个分量即位置的平均累加均方根误差分别由0.3279和0.1572降低到0.1886和0.0901，下降幅度为0.1393和0.0671，提高了定位精度。另外状态向量每个分量的累加均方根误差分别为(0.2716,0.1838,0.3014,2.0909)、(0.1509,0.1131,0.2261,1.6196)、(0.1109,0.1114,0.1162,0.6843)和(0.0620,0.0654,0.0704,0.4047)，而全部分量的累加均方根误差则分别降低了0.4917和0.2910，明显有所下降。由此可见，强适应滤波设计使得非线性动态系统的状态估计达到了更低的均方根误差，取得了更好的估计效果。

附图说明

图1是本发明实施例提供的基于卡尔曼滤波定位的非线性系统状态估计方法流程图。

图2是本发明实施例提供的实施例1的总流程图。

图3是本发明实施例提供的随着采样次数的变化，本发明和其他算法的节点位置估计过程的比较图。

图4是本发明实施例提供的随着采样次数的变化，本发明和其他算法的节点位置均方根误差的比较图。

图5是本发明实施例提供的随着取样次数的变化，本发明和其他算法的节点状态估计的比较图。

图6是本发明实施例提供的随着采样次数的变化，本发明和其他算法的节点状态估计均方根误差的比较图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

下面结合附图对本发明的应用原理作详细的描述。

如图1所示，本发明实施例提供的基于卡尔曼滤波定位的非线性系统状态估计方法包括以下步骤：

S101：根据无线信道衰减模型建立状态空间模型，将定位问题转化为非线性系统的状态估计问题；

S102：使用基于平方根容积卡尔曼滤波(SCKF)的RSSI状态估计算法，对节点位置和信道参数同时进行估计，得到状态向量的估计值；

S103：依据状态方程的线性变化，使用卡尔曼滤波进一步处理得到最优估计；

S104：给出强适应扩展卡尔曼滤波(EKF)算法的设计步骤；

S105：对系统状态空间模型下基于RSSI状态估计的理论均方根误差下界进行分析。

下面结合具体实施例对本发明的应用原理作进一步的描述。

实施例1：

参照图2，针对SCKF算法的强适应卡尔曼滤波设计发明的实现步骤如下：

步骤1.初始化滤波

定义系统在k＝0时刻状态向量的初值为其协方差矩阵为P_0|0，通过QR分解得到方差阵的平方根S_0|0，模拟生成待定位节点接收到的n个通信范围内的信标节点的RSSI值；

步骤2.循环执行SCKF

For k＝1,2,…N，N为取样次数，将步骤2.2中得到的和步骤2.1中得到的S_k|k作为输入，重复执行步骤2.1和2.2直到N次取样完成。

由式(5)，考虑到k时刻待定位节点的状态向量满足这一顺序估计，此时由SCKF得到的估计值与之间的误差应该符合一个加性噪声的关系，即其中观测噪声e_k假设服从均值为零，协方差矩阵为Φ_k＝cov(e_k)的高斯分布。

综上得到如下动态系统，其状态空间模型为：

状态方程:

测量方程:

步骤2.1.按照SCKF算法设计，进行时间更新和测量更新得到当前时刻的状态估计值及相应的误差协方差阵的平方根S_k|k；

步骤2.2.执行卡尔曼滤波递推更新

对于线性高斯模型，卡尔曼滤波器被公认为是最优滤波器，因此在SCKF估计的基础上进行进一步卡尔曼滤波，得到最优估计

P_k|k-1＝AP_k-1|k-1A^T+Q_k-1 (9)

G_k＝P_k|k-1(Φ_k+P_k|k-1)^-1 (10)

P_k|k＝(I-G_k)P_k|k-1 (12)

其中P_k-1|k-1为k-1时刻最优估计值的协方差矩阵，Q_k-1为过程噪声w_k-1的协方差矩阵P_k|k-1为k时刻预测值(因为A为单位状态转移矩阵，故预测值与前一时刻的最优估计值相同)的协方差矩阵，P_k|k为k时刻最优估计值的协方差矩阵，G_k是卡尔曼增益。关于初值的设置，令为SCKF估计前状态向量的初值，P_0|0为SCKF估计前协方差矩阵的初值。

由式(9)得到的协方差矩阵P_k|k-1，由式(10)得到卡尔曼增益G_k，由式(11)得到状态向量的最优估计由式(12)得到的协方差矩阵P_k|k。

步骤2.3.计算状态向量理论上的均方根误差下界

本发明所提的强适应滤波算法对状态向量γ_k的估计收敛于最优估计，为分析算法性能优劣，将的估计方差与均方根误差下界进行对比。对于v＝[v₁,v₂,…v_n]^T，其中v_i是待定位节点接收第i个信标节点的RSSI时对应的随机测量噪声，且即对于由系统模型转化成的固定参数无偏估计问题，理论上的均方根误差下界可以通过克拉默-拉奥界(CRB)获得如下：

其中F可通过下式来计算：

参照图2，针对EKF算法的强适应卡尔曼滤波设计发明的实现步骤如下：

步骤1.初始化滤波

定义系统在k＝0时刻状态向量的初值为其协方差矩阵初值为C_0|0，卡尔曼滤波状态向量的协方差初值为P_0|0，模拟生成待定位节点接收到的n个通信范围内的信标节点的RSSI值；

步骤2.循环执行EKF

For k＝1,2,…N，N为取样次数，将步骤2.2中得到的和步骤2.1中得到的C_k|k作为输入，重复执行步骤2.1和2.2直到N次取样完成。

在状态空间模型中，对于待估计的状态向量由于系统方程γ_k＝Aγ_k-1+w_k-1中的A是单位状态转移矩阵，无需求导。测量方程中的非线性观测函数h(γ_k)＝[h₁(γ_k),h₂(γ_k),…h_n(γ_k)]^T，其中1≤i≤n。对此函数在γ_k处求偏导数得到如下Jacobian矩阵H_k：

步骤2.1.执行EKF

按照下述公式设计，得到当前时刻的状态估计值及相应的误差协方差阵C_k|k：

C_k|k-1＝AC_k-1|k-1A^T+Q_k-1

C_k|k＝(I-W_kH_k)C_k|k-1

步骤2.2.执行卡尔曼滤波更新

由式(9)得到的协方差矩阵P_k|k-1，由式(10)得到卡尔曼增益G_k，由式(13)得到状态向量的最优估计由式(12)得到的协方差矩阵P_k|k。

本发明的效果可以通过以下仿真实验结果进行进一步的说明。

1.仿真条件：

为了验证算法的有效性，在Matlab条件下对本发明算法进行了仿真实验，该算法的仿真参数为：

该发明算法的评价指标定义如下：

1)待定位节点在k时刻的位置均方根误差：

其中为k时刻待定位节点横坐标x_k的估计值，为纵坐标y_k的估计值。

2)待定位节点的位置累加均方根误差：

3)状态向量中每个分量的累加均方根误差：

4)状态向量中全部分量的累加均方根误差：

2.仿真内容及仿真结果：

表2：本发明和其他算法关于状态向量估计过程的累加均方根误差比较表；

图3：随着采样次数的变化，本发明和其他算法的节点位置估计过程的比较图；

图4：随着采样次数的变化，本发明和其他算法的节点位置均方根误差的比较图；

图5：随着取样次数的变化，本发明和其他算法的节点状态估计的比较图；

图6：随着采样次数的变化，本发明和其他算法的节点状态估计均方根误差的比较图；

仿真1，给出了各算法的累加均方根误差对比结果，如表2所示，由表2看出，强适应滤波设计在原本的非线性滤波算法，即SCKF和EKF的基础上，使得非线性系统的状态向量估计问题取得了更低的均方根误差，达到了更高的估计精度，由此可见，此类设计可行且高效。

仿真2，针对待定位节点的位置估计过程，分别给出了SCKF和强适应SCKF、EKF和强适应EKF这四种滤波算法的对比结果，仿真如图3所示。从图3中可以看到，明显强适应算法估计坐标的收敛性优于未经强适应递推的普通算法。

仿真3，在卡尔曼滤波的N次数据采样过程中(也即1s时长内)，分别给出了四种不同滤波算法的位置均方根误差曲线，仿真如图4所示。从图4中可以看到，对于待定位节点位置坐标的估计，强适应滤波算法在原算法的基础上取得了更低的误差且具有较好的稳定性。

仿真4，随着滤波0.1s的间隔采样，分别对状态空间模型中节点状态向量的每一个分量的状态估计值进行了对比，仿真结果如图5所示。从图5可以看出，这四种算法均能够渐进收敛到节点状态向量分量的真实值，SCKF与EKF算法的估计结果不够稳定，会在真实值上下浮动，而强适应SCKF与EKF算法由于是对原来算法的进一步滤波，结果看起来更精确可靠。而且强适应滤波算法对待定位节点的横纵坐标估计更加准确，而对信道衰减系数及距离信源1m处的RSSI值估计不够理想。但是不可否认的是，SCKF算法虽然利用信道参数反馈修正节点位置坐标，在室内定位情形下仍然没能达到和EKF一样好的状态估计精度。

仿真5，给出了对应于仿真3中节点状态估计的均方根误差对比曲线图，并与理论上的均方根误差下界CRB进行对比，仿真结果如图6所示。从图6可以看出，随着取样次数的增加，四种算法基本都接近误差下界，但强适应滤波算法会获得更低的误差下界，这是由于CRB下界的推导根据的是式(3-5)和式(3-6)所示的非线性模型，其只能作为SCKF和EKF的误差下界，而强适应滤波算法是在SCKF和EKF的基础上进行的线性递推估计，能够进一步消除噪声的影响，其向量估计误差可与CRB对比分析。

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

Claims (5)

1.一种基于卡尔曼滤波定位的非线性系统状态估计方法，其特征在于，所述基于卡尔曼滤波定位的非线性系统状态估计方法采用非线性滤波算法与卡尔曼滤波相结合的强适应卡尔曼滤波机制，使用基于平方根容积卡尔曼滤波的RSSI状态估计算法，对节点位置和信道参数同时进行估计，得到状态向量的估计值；依据状态方程的线性变化，使用卡尔曼滤波进一步处理得到最优估计；

设计同样的基于扩展卡尔曼滤波基础的强适应算法；计算状态空间模型下基于RSSI状态估计的理论均方根误差下界；即在平方根容积卡尔曼滤波算法或扩展卡尔曼滤波的状态向量估计基础上利用卡尔曼滤波再进行处理；

所述平方根容积卡尔曼滤波算法的基础上利用卡尔曼滤波进行处理的方法包括：

步骤一，初始化滤波，定义系统在k＝0时刻状态向量的初值为其协方差矩阵为P_0|0，通过QR分解得到方差阵的平方根S_0|0，模拟生成待定位节点接收到的n个通信范围内的信标节点的RSSI值；

步骤二，循环执行平方根容积卡尔曼滤波算法，For k＝1,2,…N，N为取样次数，将和S_k|k作为输入，为k时刻最优估计值，S_k|k为当前时刻状态估计值的误差协方差的平方根，重复执行直到N次取样完成；由平方根容积卡尔曼滤波算法得到的估计值与之间的误差符合一个加性噪声的关系，即其中观测噪声e_k假设服从均值为零，协方差矩阵为Φ_k＝cov(e_k)的高斯分布；

综上得到如下动态系统，其状态空间模型为：

状态方程:A为单位状态转移矩阵；w_k-1为过程噪声；

测量方程:

2.如权利要求1所述的基于卡尔曼滤波定位的非线性系统状态估计方法，其特征在于，所述步骤二包括：

(1)按照平方根容积卡尔曼滤波算法，进行时间更新和测量更新得到当前时刻的状态估计值及相应的误差协方差阵的平方根S_k|k；

(2)执行卡尔曼滤波递推更新

在平方根容积卡尔曼滤波算法估计的基础上进行进一步卡尔曼滤波，得到最优估计

P_kk-1＝AP_k-1k-1A^T+Q_k-1；

G_k＝P_k|k-1(Φ_k+P_k|k-1)^-1；

P_k|k＝(I-G_k)P_k|k-1；

其中P_k-1|k-1为k-1时刻最优估计值的协方差矩阵，Q_k-1为过程噪声w_k-1的协方差矩阵P_k|k-1为k时刻预测值的协方差矩阵，P_k|k为k时刻最优估计值的协方差矩阵，G_k是卡尔曼增益；关于初值的设置，令为平方根容积卡尔曼滤波算法估计前状态向量的初值，P_0|0为平方根容积卡尔曼滤波算法估计前协方差矩阵的初值；

得到的协方差矩阵P_k|k-1，卡尔曼增益G_k，得到状态向量的最优估计得到的协方差矩阵P_k|k；

(3)计算状态向量理论上的均方根误差下界，对于v＝[v₁,v₂,…v_n]^T，其中v_i是待定位节点接收第i个信标节点的RSSI时对应的随机测量噪声，且即均方根误差下界通过克拉默-拉奥界获得如下：

其中F通过下式来计算：

3.如权利要求1所述的基于卡尔曼滤波定位的非线性系统状态估计方法，其特征在于，所述扩展卡尔曼滤波的基础上利用卡尔曼滤波进行处理的方法包括：

(1)初始化滤波，定义系统在k＝0时刻状态向量的初值为其协方差矩阵初值为C_0|0，卡尔曼滤波状态向量的协方差初值为P_0|0，模拟生成待定位节点接收到的n个通信范围内的信标节点的RSSI值；

(2)循环执行扩展卡尔曼滤波

For k＝1,2,…N，N为取样次数，将得到的和C_k|k作为输入，重复执行直到N次取样完成；

在状态空间模型中，对于待估计的状态向量x_k为待定位节点横坐标，y_k待定位节点纵坐标，由于系统方程γ_k＝Aγ_k-1+w_k-1中的A是单位状态转移矩阵，无需求导；测量方程中的非线性观测函数h(γ_k)＝[h₁(γ_k),h₂(γ_k),…h_n(γ_k)]^T，其中对此函数在γ_k处求偏导数得到如下Jacobian矩阵H_k：

4.如权利要求3所述的基于卡尔曼滤波定位的非线性系统状态估计方法，其特征在于，所述(2)包括：

按照下述公式，得到当前时刻的状态估计值及相应的误差协方差阵C_k|k：

C_k|k-1＝AC_k-1|k-1A^T+Q_k-1

C_k|k＝(I-W_kH_k)C_k|k-1

Q_k-1表示过程噪声的协方差矩阵；执行卡尔曼滤波更新：

得到的协方差矩阵P_k|k-1，得到卡尔曼增益G_k，得到状态向量的最优估计得到的协方差矩阵P_k|k。

5.一种利用权利要求1～4任意一项所述基于卡尔曼滤波定位的非线性系统状态估计方法的无线传感器网络。