基于RSSI和TOA测距的WIFI室内定位方法

1.一种基于RSSI和TOA测距的WIFI室内定位方法，包括采集信号并建立数据库、基于RSSI的匹配定位算法粗略定位、采用消除有色噪声的到达时间测距、建立平面框架和旋转以及搜索和定位，其特征在于，其过程包括以下步骤：
步骤1，采集RSSI信号并建立数据库
在定位区域内均匀设置访问接入点AP后，分别进行离线定位状态和在线定位状态下RSSI信号采集及整理；
离线定位状态：先根据各访问接入点AP位置建立样本空间，然后在定位区域内选取采样点RP，接收并测量访问接入点AP的无线信号数据的RSSI值；通过公式
求平均值，其中RSSIu表示采样点RP第u次接收并测量
访问接入点AP的无线信号数据的RSSI， 为第i个采样点RP在100次接收并测量访问接入点AP的无线信号数据的RSSI后的平均值；再将 连同采样点RP的位置坐标组成定位指纹向量 其中(xxi,yyi)表示第i个采
样点RP的位置坐标，最后将定位指纹向量 存入位置指纹数据库；
在线定位状态：未知节点和位置未知的四个对等辅助节点A、B、C、D先接收并测量访问接入点AP的无线信号数据的RSSI值，然后根据得到的RSSI值组成在线时的向量其中SSi表示未知节点和四个对等辅助节点A、B、C、D接收并测
量第i个访问接入点AP的无线信号数据的RSSI值；
所述的离线定位状态指采样点RP和各访问接入点AP位置为已知的状态，所述的在线定位状态指各访问接入点AP位置已知，而未知节点的位置未知的状态；
步骤2，对步骤1得到的数据采用定位匹配算法进行处理，得到四个对等辅助节点A、B、C、D和未知节点的初始位置坐标(xxh',yyh')(h＝1,2,3,4,5)，包括以下步骤：
步骤2.1，计算步骤1所得到的SSi和 之间的信号距离，所述信号距离定义如下，其中，
L为未知节点和四个对等辅助节点A、B、C、D的向量 与指纹库
中采样点RP的向量 之间的信号距离；
q是距离参数，取值范围为正整数；
W为访问接入点AP的数量；
是离线阶段第i个采样点RP在100次接收并测量访问接入点AP的无线信号数据的RSSI后的平均值；
SSi是在线定位阶段未知节点和四个对等辅助节点A、B、C、D接收并测量第i个访问接入点AP的RSSI值；
步骤2.2，设步骤2.1所述信号距离L<2m的采样点RP有b个，按照下式分别计算未知节点和四个对等辅助节点A、B、C、D的初始位置坐标，
其中，
b为信号距离L<2m的采样点RP的个数；
(xxh',yyh')(h＝1,2,3,4,5)表示未知节点和四个对等辅助节点A、B、C、D的初始位置坐标；
(xxi,yyi)表示采样点RP的位置坐标，其中i＝1,2,...b；
步骤3、根据步骤2得到四个对等辅助节点A、B、C、D和未知节点的初始位置，采用TOA测距方法求出四个节点A、B、C、D和未知节点各自之间的TOA测距距离，然后为消除测距过程中有色噪声的影响，再对求得的TOA测距数据进行卡尔曼滤波处理，求得四个节点A、B、C、D和未知节点各自之间的精确距离；
步骤4、建立平面旋转框架并旋转；
步骤4.1，首先将步骤2得到的四个对等辅助节点A、B、C、D和未知节点的初始位置坐标(xxh',yyh')，映射到步骤1所建立的样本空间里，然后将四个对等辅助节点A、B、C、D按照顺时针的方向根据步骤3得到的四个对等辅助节点之间的精确距离进行连接，形成四边形；最后将四个对等辅助节点与未知节点之间根据步骤3得到的四个对等辅助节点和未知节点间的精确距离连接，建立平面旋转框架；
步骤4.2，在保持上述平面旋转框架整体性不变的前提下，旋转框架使其最长边与x轴平行，由下式计算最长边的方向向量：
上式中，
(xxi,yyi)，(xxj,yyj)分别为使最长边与x轴平行时最长边两个端点处的坐标；
e＝1表示框架图中最长的边；
e＝2表示框架图按边长长度由大到小排列的第二条边；
e＝3表示框架图按边长长度由大到小排列的第三条边；
步骤4.3，在保持上述平面旋转框架整体性不变的前提下，旋转框架α角度使未知节点始终处于平面旋转框架的范围中，其中α∈[φ-20°,φ+20°]，φ为最长边和x轴的夹角根据下式求出：
其中，
为框架没有旋转时最长边的方向向量，(xxhi',
yyhi')，(xxhj',yyhj')分别为没有旋转框架时最长边的两个端点处的坐标；步骤5、搜索定位和判断
步骤5.1，先以步骤2得到的未知节点的初始位置坐标为圆心，选取半径为2米的圆为搜索范围，然后将已建立的圆化成均等的100份，得到100个点的坐标；
步骤5.2，在保证步骤4所述平面旋转框架整体性不变的基础上，未知节点在搜索范围内移动到步骤5.1所述100个点的每个点，并通过下式计算每个点的f(RSSI)值；
其中，
ssa表示未知节点移动到a点时，四个对等辅助节点A、B、C、D和未知节点在步骤1所述在线定位状态下测得的RSSI值，a＝1,2,3,4,5分别代表节点A、B、C、D和未知节点；
表示未知节点移到a点时，步骤1所述离线定位状态下建立的位置指纹库中记载的分别与四个对等辅助节点A、B、C、D和未知节点最近的坐标点的RSSI值；a＝1,2,3,4,5，分别代表节点A、B、C、D和未知节点；
步骤5.3，对步骤5.2求得的100个f(RSSI)值进行比较，找到f(RSSI)值最小的点，该点的位置坐标即为未知节点的实际位置。
2.根据权利要求1所述的一种基于RSSI和TOA测距的WIFI室内定位方法，其特征在于，步骤3所述的卡尔曼滤波处理的步骤如下：
步骤2.1，建立目标状态方程和有色噪声量测方程
xk+1＝Axk+Bαk，
vk＝Cxk+βk，
βk＝Nk-1βk-1+γk，
其中，
A为状态矩阵，A的取值为 t为滤波采样间隔时间，单位为秒，
B为误差系数矩阵，B的取值为
C为测量矩阵，C的取值为C＝[1 0]，
xk为目标在k(k＝1，2，3，……n)时刻的二维状态向量， 其中的rk为待估计的飞行时间值、为其一阶导数，当xk的下标k＝n时取值200，
αk为系统噪声，是一个均值为0的高斯白噪声序列，协方差用Q表示，
vk为一维测量向量，表示k时刻的测距数据，
βk为k时刻的测量误差，协方差矩阵为RK，
Nk-1为自回归系数，由步骤2.4计算NK-1得到，
γk为高斯白噪声序列；
步骤2.2，根据原始卡尔曼滤波算法，由k-1时刻的滤波结果得到目标的k时刻的预测值以及k时刻的新息值zk，计算公式为
其中， 为k-1时刻的滤波结果；
0时刻的滤波结果根据初始测距数据由下式得出，
其中，x0取为第一个测距数据、v0为第一个测距数据、 取为前几次测距数据的方差；
步骤2.3，求出测量噪声协方差k时刻的估计值
先计算参数Sk，其由下式来估计得到
其中，
是前i次新息值的均值，计算为
其中，zi，zj分别表示第i，j次的新息值，
再按照下两式得出测量噪声协方差在k时刻的估计值
Pk,k-1＝APk-1,k-1AT+BQBT，
其中，Pk,k-1是k时刻预测误差方差阵，Pk-1,k-1是k-1时刻估计误差方差阵；
步骤2.4，计算Nk-1
根据βk-1和γk独立，且有协方差的关系得出Nk-1的计算式为
其中，var(γk)是γk的方差；
步骤2.5，计算k时刻的门限值mk
计算前k次测距数据的方差 取门限值为方差值，即
步骤2.6，更新卡尔曼滤波增益
将k时刻的新息值与门限值进行比较，若新息值大于门限值，则将卡尔曼增益置为零，否则按以下两步计算滤波增益Gk，
Hk-1＝[CA-Nk-1C]，
步骤2.7，更新估计误差方差阵
由 得出估计误差方差阵的初值，
其中，
x0取为第一个测距数据， 为误差协方差初值， 取作前几次试验测距数据的方差，估计误差方差阵更新表达式为
Pk,k＝(A-GkHk-1)·Pk-1,k-1AT+(I-GKC)BQBT；
步骤2.8，根据k时刻滤波输出方程输出k时刻的滤波结果，滤波方程为
如此循环往复，输出1到n时刻的滤波结果，得到对TOA测距数据的处理结果。

基于RSSI和TOA测距的WIFI室内定位方法\n技术领域\n[0001] 本发明涉及一种基于RSSI的室内定位方法，尤其是一种建立在TOA测距基础上的定位方法。\n背景技术\n[0002] 在WIFI的室内定位中，基于RSSI的定位算法，是目前非常通用的一种定位算法。\nRSSI(Received Signal Strength Indicator)是接收信号强度指示，它的实现是在反向通道基带接收滤波器之后进行的。基于RSSI的定位算法是通过接收到的信号强弱测定信号点与接收点之间的距离，进而根据相应数据进行定位计算的一种定位技术。该种方法的基本原理是通过射频信号的强度来进行距离估测，分别在发送节点和接受节点测量发射功率和接受功率，从而计算传播损耗，使用数据或经验信号传播模型将传播损耗转化为对距离的一种估测算法。与传统定位技术相比，其无需额外添加设备来进行角度测量与时间同步，且充分利用了已有WIFI无线网络，降低了使用成本；其次，WIFI位置指纹定位技术与传统室内定位技术相比，其扩展性更强、应用范围更广。但是由于环境的制约，给信号传播模型的建模带来了一定的困难，并且在通信原理中，反射、多径等效应对定位的结果产生一定的影响以及干扰，所以该方法具有较大的定位误差，精度受到很大的限制。\n[0003] 与此同时，TOA测距，即到达测距技术，是通过测量节点间信号传输的时间来得到两节点间的距离，它的核心思路在于已知信号传播速度，通过两个节点之间的信号传播时间，转化成距离信息，其数值即为到达时间测距数据。由于非视距(NLOS)的存在，到达时间测距数据会存在NLOS误差，其误差一般具有正偏性，到达时间所测量的距离会偏大于实际距离。如果直接将测距数据用于节点的定位，其结果会大大地偏离节点的实际位置。所以如何对到达时间的测距数据进行处理，消除其中的NLOS误差显得尤为重要。\n[0004] 为此，人们做了一些尝试，王新峰等撰写的“采用RSSI和TOF的融合算法定位方法”(《化工自动化仪表》，2013年08期)提出了将TOF和RSSI双层数据融合的方法，即第一层数据通过初始位置估计之间的自检，剔除异常数据；第二层数据融合则以所测RSSI值和TOF定位值相结合，获取更优的估计值。但是所涉及的数据太多，处理数据时间长，计算量大。中国专利文献“一种基于RSSI的室内定位方法”(公开号：CN103813448A)提出了采用三个采样点进行定位的方法，即根据三个采样点的位置将采样点摆放成等腰三角形，然后根据路径损耗模型计算采样点和未知节点的距离，最后根据它们之间几何关系求出未知节点的位置坐标。这种定位方法降低了时间复杂度，计算速度快，节省硬件资源，但是采样点个数太少，收集的数据太少，降低了定位的精度和准确性。\n发明内容\n[0005] 本发明要解决的技术问题为克服上述各种技术方案的局限性，提供一种能够进行准确定位的室内定位方法。\n[0006] 为实现本发明的目的，提供了一种基于RSSI和TOA测距的WIFI室内定位方法，所采用的技术方案为：采集信号并建立数据库、基于RSSI的匹配定位算法粗略定位、采用消除有色噪声的到达时间测距、建立平面框架和旋转以及搜索和定位，其过程如下：\n[0007] 步骤1，采集RSSI信号并建立数据库\n[0008] 在定位区域内均匀设置访问接入点AP后，分别进行离线定位状态和在线定位状态下RSSI信号采集及整理；\n[0009] 离线定位状态：先根据各访问接入点AP位置建立样本空间，然后在定位区域内选取采样点RP，接收并测量访问接入点AP的无线信号数据的RSSI值；通过公式\n求平均值，其中RSSIu表示采样点RP第u次接收并测量\n访问接入点AP的无线信号数据的RSSI值， 为第i个采样点RP在100次接收并测量访问接入点AP的无线信号数据的RSSI后的平均值；再将 连同采样点RP的位置坐标组成定位指纹向量 其中(xxi，yyi)表示第i个采\n样点RP的位置坐标，最后将定位指纹向量 存入位置指纹数据库；\n[0010] 在线定位状态：未知节点和位置未知的四个对等辅助节点A、B、C、D先接收并测量访问接入点AP的无线信号数据的RSSI值，然后根据得到的RSSI值组成在线时的向量其中SSi表示未知节点和对等辅助节点A、B、C、D接收并测量第i\n个访问接入点AP的无线信号数据的RSSI值；\n[0011] 所述的离线定位状态指采样点RP和各访问接入点AP位置为已知的状态，所述的在线定位状态指各访问接入点AP位置已知，而未知节点的位置未知的状态；\n[0012] 步骤2，对步骤1得到的数据采用定位匹配算法进行处理，得到四个对等辅助节点A、B、C、D和未知节点的初始位置坐标(xxh′，yyh′)(h＝1，2，3，4，5)，包括以下步骤：\n[0013] 步骤2.1，计算步骤1所得到的SSi和 之间的信号距离，所述信号距离定义如下，\n[0014]\n[0015] 其中，\n[0016] L为未知节点和四个对等辅助节点A、B、C、D的向量 与\n指纹库中采样点RP的向量 之间的信号距\n离；\n[0017] q是距离参数，取值范围为正整数；\n[0018] W为访问接入点AP的数量；\n[0019] 是离线阶段第i个采样点RP在100次接收并测量访问接入点AP的无线信号数据的RSSI后的平均值；\n[0020] SSi是在线定位阶段未知节点和四个对等辅助节点A、B、C、D接收并测量第i个访问接入点AP的RSSI值；\n[0021] 步骤2.2，设步骤2.1所述信号距离L＜2m的采样点RP有b个，按照下式分别计算未知节点和四个对等辅助节点A、B、C、D的初始位置坐标，\n[0022]\n[0023] 其中，\n[0024] b为信号距离L＜2m的采样点RP的个数；\n[0025] (xxh′，yyh′)(h＝1，2，3，4，5)表示未知节点和四个对等辅助节点A、B、C、D的初始位置坐标；\n[0026] (xxi，yyi)表示采样点RP的位置坐标，其中i＝1，2，…b；\n[0027] 步骤3、根据步骤2得到四个对等辅助节点A、B、C、D和未知节点的初始位置，采用TOA测距方法求出四个节点A、B、C、D和未知节点各自之间的TOA测距距离，然后为消除测距过程中有色噪声的影响，再对求得的TOA测距数据进行卡尔曼滤波处理，求得四个节点A、B、C、D和未知节点各自之间的精确距离；\n[0028] 步骤4、建立平面旋转框架并旋转；\n[0029] 步骤4.1，首先将步骤2得到的四个对等辅助节点A、B、C、D和未知节点的初始位置坐标(xxh′，yyh′)，映射到步骤1所建立的样本空间里，然后将四个对等辅助节点A、B、C、D按照顺时针的方向根据步骤3得到的四个对等辅助节点之间的精确距离进行连接，形成四边形；最后将四个对等辅助节点与未知节点之间根据步骤3得到的四个对等辅助节点和未知节点间的精确距离连接，建立平面旋转框架；\n[0030] 步骤4.2，在保持上述平面旋转框架整体性不变的前提下，旋转框架使其最长边与x轴平行，由下式计算最长边的方向向量：\n[0031]\n[0032] 上式中，\n[0033] (xxi，yyi)，(xxj，yyj)分别为使最长边与x轴平行时最长边两个端点处的坐标；\n[0034] e＝1表示框架图中最长的边；\n[0035] e＝2表示框架图按边长长度由大到小排列的第二条边；\n[0036] e＝3表示框架图按边长长度由大到小排列的第三条边；\n[0037] 步骤4.3，在保持上述平面旋转框架整体性不变的前提下，旋转框架α角度使未知节点始终处于平面旋转框架的范围中，其中α∈[φ-20°，φ+20°]，φ为最长边和x轴的夹角根据下式求出：\n[0038]\n[0039] 其中，\n[0040] 为框架没有旋转时最长边的方向向量，xxhi′，\nyyhi′，xxhj′，yyhj′分别为没有旋转框架时最长边的两个端点处的坐标；\n[0041] 步骤5、搜索定位和判断\n[0042] 步骤5.1，先以步骤2得到的未知节点的初始位置坐标为圆心，选取半径为2米的圆为搜索范围，然后将已建立的圆化成均等的100份，得到100个点的坐标；\n[0043] 步骤5.2，在保证步骤4所述平面旋转框架整体性不变的基础上，未知节点在搜索范围内移动到步骤5.1所述100个点的每个点，并通过下式计算每个点的f(RSSI)值；\n[0044]\n[0045] 其中，\n[0046] SSa表示未知节点移动到a点时，四个对等辅助节点A、B、C、D和未知节点在步骤1所述在线定位状态下测得的RSSI值，a＝1，2，3，4，5分别代表节点A、B、C、D和未知节点；\n[0047] 表示未知节点移到a点时，步骤1所述离线定位状态下建立的位置指纹库中记载的分别与四个对等辅助节点A、B、C、D和未知节点最近的坐标点的RSSI值；a＝1，2，3，4，\n5，分别代表节点A、B、C、D和未知节点；\n[0048] 步骤5.3，对步骤5.2求得的100个f(RSSI)值进行比较，找到f(RSSI)值最小的点，该点的位置坐标即为未知节点的实际位置。\n[0049] 步骤3所述的卡尔曼滤波处理的步骤如下：\n[0050] (1)建立目标状态方程和有色噪声量测方程\n[0051] xk+1＝Axk+Bαk，\n[0052] vk＝Cxk+βk，\n[0053] βk＝Nk-1βk-1+γk，\n[0054] 其中，\n[0055] A为状态矩阵，A的取值为 t为滤波采样间隔时间，单位为秒\n[0056] B为误差系数矩阵，B的取值为\n[0057] C为测量矩阵，C的取值为C＝[10]，\n[0058] xk为目标在k(k＝1，2，3，……n)时刻的二维状态向量， 其中的rk为待估计的飞行时间值、为其一阶导数，当xk的下标k＝n时取值200，\n[0059] αk为系统噪声，是一个均值为0的高斯白噪声序列，协方差用Q表示，\n[0060] vk为一维测量向量，表示k时刻的测距数据，\n[0061] βk为k时刻的测量误差，协方差矩阵为Rk，\n[0062] Nk-1为自回归系数，由(4)计算Nk-1得到，\n[0063] γk为高斯白噪声序列；\n[0064] (2)根据原始卡尔曼滤波算法，由k-1时刻的滤波结果得到目标的k时刻的预测值以及k时刻的新息值zk，计算公式为\n[0065]\n[0066]\n[0067] 其中， 为k-1时刻的滤波结果；\n[0068] 0时刻的滤波结果根据初始测距数据由下式得出，\n[0069]\n[0070] 其中，x0取为第一个测距数据、v0为第一个测距数据、 取为前几次测距数据的方差。\n[0071] (3)求出测量噪声协方差k时刻的估计值\n[0072] 先计算参数Sk，其由下式来估计得到\n[0073]\n[0074] 其中，\n[0075] 是前i次新息值的均值，计算为\n[0076]\n[0077] 其中，zi，zj分别表示第i，j次的新息值，\n[0078] 再按照下两式得出测量噪声协方差在k时刻的估计值\n[0079] Pk，k-1＝APk-1，k-1AT+BQBT，\n[0080]\n[0081] 其中，Pk，k-1是k时刻预测误差方差阵，Pk-1，k-1是k-1时刻估计误差方差阵；\n[0082] (4)计算Nk-1\n[0083] 根据βk-1和γk独立，且有协方差的关系得出Nk-1的计算式为\n[0084]\n[0085] 其中，var(γk)是γk的方差；\n[0086] (5)计算k时刻的门限值mk\n[0087] 计算前k次测距数据的方差 取门限值为方差值，即\n[0088] (6)更新卡尔曼滤波增益\n[0089] 将k时刻的新息值与门限值进行比较，若新息值大于门限值，则将卡尔曼增益置为零，否则按以下两步计算滤波增益Gk，\n[0090] Hk-1＝[CA-Nk-1C]，\n[0091]\n[0092] (7)更新估计误差方差阵\n[0093] 由 得出估计误差方差阵的初值，\n[0094] 其中，\n[0095] x0取为第一个测距数据， 为误差协方差初值， 取作前几次试验测距数据的方差，\n[0096] 估计误差方差阵更新表达式为\n[0097] Pk，k＝(A-GkHk-1)·Pk-1，k-1AT+(I-GkC)BQBT；\n[0098] (8)根据k时刻滤波输出方程输出k时刻的滤波结果，滤波方程为\n[0099]\n[0100] 如此循环往复，输出1到n时刻的滤波结果，得到对TOA测距数据的处理结果。\n[0101] 相对于现有技术的有益效果是：\n[0102] 1、通过结合信号强度指示RSSI和距离信息进行综合定位，避免大量RSSI数据的计算，使得基于WIFI的室内定位精度提高。\n[0103] 2、采用了消除有色噪声的自适应卡尔曼滤波方法测距，消除了非视距NLOS对其影响，提高了室内复杂环境的测距精度。\n[0104] 3、本发明建立的平面旋转框架图使定位过程更加精确，定位结果准确度更高，更加接近实际结果，使其可广泛的地用于基于WIFI的室内定位中。\n附图说明\n[0105] 图1是本发明基本方法流程图。\n[0106] 图2是本发明实施例中的定位区域示例图。\n[0107] 图3是本发明消除TOA测距中有色噪声的流程图。\n[0108] 图4是本发明建立的平面框架图。\n[0109] 图5是本发明实施例TOA测距的测量值和滤波后的精确值。\n具体实施方式\n[0110] 本文本中所出现的相同的数学符号所代表的含义完全相同，如果记载有差异，以相同的数学符号在文本中第一次出现时的解释为准。\n[0111] 下面结合附图对本发明的优选方式作进一步详细的描述。\n[0112] 本发明采用TP-LINK公司的型号为TL-2543N的路由器作为访问接入点AP，同时使用带有智能WIFI模块的手机采集RSSI信号和TOA数据。\n[0113] 图1是本发明基本方法流程图。由该图可见，本发明主要包括以下步骤：\n[0114] 步骤1，采集RSSI信号并建立数据库\n[0115] 在如图2所示的10×4m2的房间内设置3个TL-2543N路由器座作为3个访问接入点AP，四个带有智能WIFI模块的手机作为对等辅助节点，同时设定60个采样点RP后，分别进行离线定位状态和在线定位状态下RSSI信号采集及整理；\n[0116] 离线定位状态：先根据各访问接入点AP位置建立样本空间，定位区域内的采样点RP接收并测量访问接入点AP的无线信号数据的RSSI值；随后通过公式\n求平均值，其中RSSIu表示采样点RP第u次接收并测量\n访问接入点AP的无线信号数据的RSSI值， 为第i个采样点RP在100次接收并测量访问接入点AP的无线信号数据的RSSI后的平均值；再将 连同采样点RP位置坐标(x，y)组成定位指纹向量 其中 表\n示第1个采样点RP在100次接收并测量第1个访问接入点AP的无线信号数据的RSSI值，最后将定位指纹向量 存入位置指纹数据库，其余依次类推，例如坐标为(5，2)处的为[-78dBm，-19dBm，-95dBm]；\n[0117] 在线定位状态：未知节点和位置未知的四个对等辅助节点A、B、C、D先接收并测量访问接入点AP的无线信号数据的RSSI值，然后根据得到的RSSI值组成在线时的向量其中SS1表示未知节点和四个对等辅助节点A、B、C、D接收并测量第1个\n访问接入点AP的无线信号数据的RSSI值，其余依次类推；\n[0118] 所述的离线定位状态指采样点RP和各访问接入点AP位置为已知的状态，所述的在线定位状态指各访问接入点AP位置已知，而未知节点的位置未知的状态。\n[0119] 步骤2，对步骤1得到的数据采用定位匹配算法进行处理，得到四个节点A、B、C、D和未知节点的初始位置坐标(xxh′，yyh′)(h＝1，2，3，4，5)，包括以下步骤：\n[0120] 步骤2.1，计算步骤1所得到的SSi和 之间的信号距离，所述信号距离定义如下：\n[0121]\n[0122] 上式中：\n[0123] L为未知节点和四个对等辅助节点A、B、C、D的向量 与\n指纹库中采样点RP的向量 之间的信号\n距离；\n[0124] q是距离参数，取值范围为正整数，本实施例中q＝2；\n[0125] W为访问接入点AP的数量，本实施例中W＝3；\n[0126] 是离线定位状态第i个采样点RP在100次接收并测量访问接入点AP的无线信号数据的RSSI后的RSSI值；\n[0127] SSi是在线定位状态未知节点和四个对等辅助节点A、B、C、D接收并测量第i个访问接入点AP的RSSI值。\n[0128] 步骤2.2，设步骤2.1中所述信号距离L＜2m的采样点RP有b个，按照下式分别计算未知节点和对等辅助节点A、B、C、D的初始位置坐标：\n[0129]\n[0130] 上式中：\n[0131] b为信号距离L＜2m的采样点RP的个数；\n[0132] (xxh′，yyh′)(h＝1，2，3，4，5)表示为未知节点和四个对等辅助节点A、B、C、D的初始位置坐标；\n[0133] (xxi，yyi)表示采样点RP的位置坐标，其中i＝1，2，…b，\n[0134] 本实施例中根据上述步骤得到四个节点A、B、C、D的初始位置坐标分别为(3.2，\n3.5)，(4.7，2.9)，(4.2，1.4)，(2.7，2.2)，和未知节点的初始位置坐标(3.7，2.7)。\n[0135] 步骤3、根据步骤2得到的四个对等辅助节点A、B、C、D和未知节点的初始位置，采用TOA测距方法求出四个节点A、B、C、D和未知节点各自之间的TOA测距距离，然后为消除测距过程中有色噪声的影响，再对求得的TOA测距数据进行卡尔曼滤波处理，求得四个节点A、B、C、D和未知节点各自之间的精确距离。\n[0136] 在本实施例中，先根据步骤2得到的四个对等辅助节点A、B、C、D和未知节点的初始位置，相互之间发送信号，得到四个对等辅助节点A、B、C、D和未知节点各自之间的信号到达时间，然后将上述时间乘以光速，得到四个对等辅助节点A、B、C、D和未知节点各自之间的TOA测距距离。\n[0137] 为了消除NLOS误差，对TOA测距距离进行卡尔曼滤波处理，卡尔曼滤波处理的步骤如图3所示，包括以下步骤：\n[0138] (1)建立目标状态方程和有色噪声量测方程\n[0139] xk+1＝Axk+Bαk，\n[0140] vk＝Cxk+βk，\n[0141] βk＝Nk-1βk-1+γk，\n[0142] 上式中，\n[0143] A为状态矩阵，A的取值为 t为滤波采样间隔时间，单位为秒，\n[0144] B为误差系数矩阵，B的取值为\n[0145] C为测量矩阵，C的取值为C＝[10]，\n[0146] xk为目标在k(k＝1，2，3，……n)时刻的二维状态向量， 其中的rk为\n待估计的飞行时间值、为其一阶导数，当xk的下标k＝n时取值200，\n[0147] αk为系统噪声，是一个均值为0的高斯白噪声序列，协方差用Q表示，\n[0148] vk为一维测量向量，表示k时刻的测距数据，\n[0149] βk为k时刻的测量误差，协方差矩阵为Rk，\n[0150] Nk-1为自回归系数，由(4)计算Nk-1得到，\n[0151] γk为高斯白噪声序列；\n[0152] (2)根据原始卡尔曼滤波算法，由k-1时刻的滤波结果得到目标的k时刻的预测值以及k时刻的新息值zk，计算公式为：\n[0153]\n[0154]\n[0155] 上式中， 为k-1时刻的滤波结果；\n[0156] 0时刻的滤波结果根据初始测距数据由下式得出：\n[0157]\n[0158] 上式中，x0取为第一个测距数据、v0为第一个测距数据、本实施例中 取为前30次测距数据的方差。\n[0159] (3)求出测量噪声协方差k时刻的估计值\n[0160] 先计算参数Sk，其由下式来估计得到\n[0161]\n[0162] 上式中，\n[0163] 是前i次新息值的均值，计算公式为：\n[0164]\n[0165] 其中，zi，zj分别表示第i，j次的新息值，\n[0166] 再按照下两式得出测量噪声协方差在k时刻的估计值\n[0167] Pk，k-1＝APk-1，k-1AT+BQBT，\n[0168]\n[0169] 其中，Pk，k-1是k时刻预测误差方差阵，Pk-1，k-1是k-1时刻估计误差方差阵；\n[0170] (4)计算Nk-1\n[0171] 根据βk-1和γk独立，且有协方差的关系得出Nk-1的计算式为：\n[0172]\n[0173] 其中，var(γk)是γk的方差；\n[0174] (5)计算k时刻的门限值mk\n[0175] 计算前k次测距数据的方差 取门限值为方差值，即\n[0176] (6)更新卡尔曼滤波增益\n[0177] 将k时刻的新息值与门限值进行比较，若新息值大于门限值，则将卡尔曼增益置为零，否则按以下两步计算滤波增益Gk，\n[0178] Hk-1＝[CA-Nk-1C]，\n[0179]\n[0180] (7)更新估计误差方差阵\n[0181] 由 得出估计误差方差阵的初值，\n[0182] 上式中，\n[0183] x0取为第一个测距数据， 为误差协方差初值， 取作前30次试验测距数据的方差，\n[0184] 估计误差方差阵更新表达式为\n[0185] Pk，k＝(A-GkHk-1)·Pk-1，k-1AT+(I-GkC)BQBT；\n[0186] (8)根据k时刻滤波输出方程输出k时刻的滤波结果，滤波方程为：\n[0187]\n[0188] 如此循环往复，输出1到n时刻的滤波结果，得到对TOA测距数据的处理结果。\n[0189] 本实施例中取n＝200，其滤波前和滤波后的结果可参见附图5。如图5所示，滤波前测量值波动很大，和实际值相差大，经过滤波后的精确值接近实际值。\n[0190] 步骤4、建立平面旋转框架并旋转；\n[0191] 步骤4.1，首先将步骤2得到的四个对等辅助节点A、B、C、D和未知节点的初始位置坐标(xxh′，yyh′)，映射到步骤1所建立的样本空间里，然后将四个对等辅助节点A、B、C、D按照顺时针的方向根据步骤3得到的四个对等辅助节点之间的精确距离进行连接，形成四边形；最后将四个对等辅助节点与未知节点之间根据步骤3得到的四个对等辅助节点和未知节点间的精确距离连接，建立平面旋转框架；\n[0192] 步骤4.2，在保持上述平面旋转框架整体性不变的前提下，旋转框架使其最长边与x轴平行，由下式计算最长边的方向向量：\n[0193]\n[0194] 上式中，\n[0195] (xxi，yyi)，(xxj，yyj)分别为使最长边与x轴平行时最长边两个端点处的坐标；\n[0196] e＝1表示框架图中最长的边；\n[0197] e＝2表示框架图按边长长度由大到小排列的第二条边；\n[0198] e＝3表示框架图按边长长度由大到小排列的第三条边；\n[0199] 步骤4.3，在保持上述平面旋转框架整体性不变的前提下，旋转框架α角度使未知节点始终处于平面旋转框架的范围中，其中α∈[φ-20°，φ+20°]，φ为最长边和x轴的夹角根据下式求出：\n[0200]\n[0201] 其中，\n[0202] 为框架没有旋转时最长边的方向向量，(xxhi′，\nyyhi′)，(xxhj′，yyhj′)分别为没有旋转框架时最长边的两个端点处的坐标；\n[0203] 步骤5、搜索定位和判断\n[0204] 步骤5.1，先以步骤2得到的未知节点的初始位置坐标为圆心，选取半径为2米的圆为搜索范围；然后将已建立的圆化成均等的100份，得到100个点的坐标；\n[0205] 骤5.2，在保证步骤4所述平面旋转框架整体性不变的基础上，未知节点在搜索范围内移动到步骤5.1所述100个点的每个点，并通过下式计算每个点的f(RSSI)值；\n[0206]\n[0207] 上式中，\n[0208] SSa表示未知节点移动到a点时，四个对等辅助节点A、B、C、D和未知节点在步骤1所述在线定位状态下测得的RSSI值，a＝1，2，3，4，5分别代表节点A、B、C、D和未知节点；\n[0209] 表示未知节点移到a点时，步骤1所述离线定位状态下建立的位置指纹库中记载的分别与四个对等辅助节点A、B、C、D和未知节点最近的坐标点的RSSI值；a＝1，2，3，4，\n5，分别代表节点A、B、C、D和未知节点；\n[0210] 步骤5.3，对步骤5.2求得的100个f(RSSI)值进行比较，找到f(RSSI)值最小的点，该点的位置坐标即为未知节点的实际位置。\n[0211] 具体实施过程中，当未知节点移到第80个点时f(RSSI)最小，此点对应的坐标为(4.1，2.4)，此坐标就是通过本发明得到的未知节点的位置。\n[0212] 步骤4和步骤5中所述的平面旋转框架及搜索可参见图4。