CN109916410B - 一种基于改进平方根无迹卡尔曼滤波的室内定位方法 - Google Patents

一种基于改进平方根无迹卡尔曼滤波的室内定位方法 Download PDF

Info

Publication number
CN109916410B
CN109916410B CN201910227539.3A CN201910227539A CN109916410B CN 109916410 B CN109916410 B CN 109916410B CN 201910227539 A CN201910227539 A CN 201910227539A CN 109916410 B CN109916410 B CN 109916410B
Authority
CN
China
Prior art keywords
square root
formula
time
value
uwb
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201910227539.3A
Other languages
English (en)
Other versions
CN109916410A (zh
Inventor
邢宗义
刘源
杨行
包建东
胡明月
孙强
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Nanjing University of Science and Technology
Original Assignee
Nanjing University of Science and Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Nanjing University of Science and Technology filed Critical Nanjing University of Science and Technology
Priority to CN201910227539.3A priority Critical patent/CN109916410B/zh
Publication of CN109916410A publication Critical patent/CN109916410A/zh
Application granted granted Critical
Publication of CN109916410B publication Critical patent/CN109916410B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
    • Y02D30/00—Reducing energy consumption in communication networks
    • Y02D30/70—Reducing energy consumption in communication networks in wireless communication networks

Landscapes

  • Navigation (AREA)

Abstract

本发明公开了一种基于改进平方根无迹卡尔曼滤波的室内定位方法。该算法为：首先建立所述融合室内定位方法中UWB的定位模型和行人航位推算PDR模型，然后根据残差鉴别法判据来判断UWB定位模型定位结果中是否存在NLOS判据，若存在NLOS，则通过改进平方根无迹卡尔曼滤波算法将惯导航位推算的坐标与UWB的定位坐标进行融合，对UWB解算坐标数据进行修正；若不存在NLOS，则采用UWB解析出当前坐标，完成实时定位。本发明利用行人航位推算PDR来补偿UWB信号丢失或非视距的情况，提高了系统的定位精度，具有精度高和鲁棒性强的优点。

Description

一种基于改进平方根无迹卡尔曼滤波的室内定位方法
技术领域
本发明涉及室内定位技术领域，特别是一种基于改进平方根无迹卡尔曼滤波的室内定位方法。
背景技术
无线室内定位技术的应用范围十分广泛，该技术的出现解决了全球定位系统(GPS)由于信号受障碍物遮蔽、室内环境复杂、反射严重等原因无法在室内提供精确定位服务的问题。
常见的室内定位技术有：Wi-Fi技术、射频识别技术、红外技术、ZigBee技术、超声波技术、UWB技术等。这些技术有各自的优缺点，应用领域也各不相同。其中，超宽带技术(UWB)利用纳秒级的窄脉冲来传输数据，不需要载波，具有GHz量级的带宽。另外，超宽带技术具有较高的时间分辨率、较强的穿透力和抗多径能力，然而由于室内环境复杂，会发生UWB信号折射或反射的情况，或者由于障碍物阻挡导致非视距(NLOS)误差。
发明内容
本发明的目的在于提供一种精度高、稳定性好的基于改进平方根无迹卡尔曼滤波的室内定位方法。
实现本发明目的的技术解决方案为：一种基于改进平方根无迹卡尔曼滤波的室内定位方法，包括以下步骤：
步骤1：建立UWB定位模型；
步骤2：建立行人航位推算PDR模型；
步骤3：建立UWB/PDR组合定位模型；
步骤4：建立改进平方根无迹卡尔曼滤波算法；
步骤5：使用UWB进行实时定位，同时惯导依据初始位置进行行人航位推算，接收到TDOA数据后，用残差鉴别法鉴别是否存在NLOS情况；
若存在NLOS，则通过改进平方根无迹卡尔曼滤波算法将惯导航位推算的坐标与UWB的定位坐标进行融合，对UWB解算坐标数据进行修正，返回步骤5；
若不存在NLOS，则采用UWB解析出当前坐标，完成实时定位。
本发明与现有技术相比，其显著优点为：(1)UWB定位模型进行TDOA解算时，统一基站高度为H，简化了计算过程，提高了计算效率；(2)基于改进Sage-Husa自适应平方根无迹卡尔曼滤波算法和基于多重渐消因子强跟踪平方根无迹卡尔曼滤波算法的改进平方根无迹卡尔曼滤波算法，利用二者优势，构成既可以抑制滤波发散又能保证滤波精度和效率的滤波算法；(3)在基于改进Sage-Husa自适应平方根无迹卡尔曼滤波算法中，对自适应因子dk进行改进，解决了收敛速读与收敛值不匹配的问题；(4)在状态变量误差协方差预测值中引入多重渐消因子，对不同的状态估计进行调整，使系统具有较好的跟踪能力；(5)利用行人航位推算PDR来补偿UWB信号丢失或非视距的情况，提高了系统的定位精度，保证了系统的稳定性和连续性。
附图说明
图1是本发明基于改进平方根无迹卡尔曼滤波的室内定位方法的行人航位推算模型示意图。
图2是本发明中的UWB/PDR组合定位原理图。
图3是本发明中的残差鉴别法流程图。
图4是本发明实施例中的UWB、PDR、SR-UKF、ISR-UKF定位轨迹对比图。
图5是本发明实施例中的定位位置误差对比图。
具体实施方式
结合图1，本发明基于改进平方根无迹卡尔曼滤波的室内定位方法，包括以下步骤：
步骤1：建立UWB定位模型，具体如下：
在三维空间中，利用4个传感器所得到的TDOA测量值来估算待定位节点的位置，设定基站传感器的坐标为(xi,yi,zi)，其中i＝1,2,3,4，待定位点A的坐标为(x,y,z)，则第i个定位基站到A点的距离Ri为：
Figure BDA0002005679310000021
其中Ki＝xi 2+yi 2+zi 2；
设定第一个定位基站为主传感器，则定位目标到从传感器和到主传感器之间的距离差为Ri,1，信号传播时间差，即TDOA测量值为ti,1，则：
Ri,1＝cti,1＝Ri-R1 (2)
其中c为传输信号的速率即光速；
根据式(1)和式(2)得：
Figure BDA0002005679310000037
在式(1)中，令i＝1，则得：
R1 2＝K1-2x1x-2y1y-2z1z+x2+y2+z2 (4)
用式(3)减去式(4)得：
Ri,1 2+2Ri,1R1＝Ki-K1-2(xi-x1)x-2(yi-y1)y-2(zi-z1)z (5)
设定传感器铺设在室内天花板上，高度为H，则zi＝H；设定主传感器的坐标为(0，0，H)，第二、三、四个传感器的坐标分别为：(x2，0，H)、(0，y3，H)、(x2，y3，H)，由式(5)得：
Figure BDA0002005679310000031
解式(6)得：
Figure BDA0002005679310000032
式(7)中：
Figure BDA0002005679310000033
Figure BDA0002005679310000034
Figure BDA0002005679310000035
Figure BDA0002005679310000036
将x，y值代入式(6)求得R1值，将x，y，R1值代入式(4)求得z值，得到待定位目标的位置。
步骤2：建立行人航位推算PDR模型，具体如下：
设定行人在室内二维平面中运动，对行人的运动进行超短时间采样，可将行人在此阶段的运动近似为直线运动。假设行人的初始位置坐标为M0(E0,N0),初始航向角为
Figure BDA0002005679310000045
行人t1时间内的移动距离为L0，得到行人的当前位置坐标M1(E1,N1)：
Figure BDA0002005679310000041
由上，同理可以推出ti时刻的位置坐标Mi(Ei,Ni)为：
Figure BDA0002005679310000042
上式(9)中，(Ei-1,Ni-1)表示ti-1时刻的位置坐标，
Figure BDA0002005679310000043
为ti-1时刻的航向角，Li-1表示采样时间内的位移。
由式(9)知，行人航位推算PDR包括初始位姿、步频检测、步长估计和航向计算：
所述步频检测，通过峰值检测法，基于行人的行进过程中产生的行进方向和竖直方向的正负加速度，来确定行人的步频；
所述步长估计，根据行人的生理特征和行走习惯建立模型，得到步长估计；
所述航向计算，通过扩展卡尔曼滤波融合陀螺仪、加速度计和磁力计的数据，得到航向角估计值。
步骤3：建立UWB/PDR组合定位定位模型，如图2所示。
步骤4：建立改进平方根无迹卡尔曼滤波算法，具体如下：
结合基于改进Sage-Husa自适应平方根无迹卡尔曼滤波算法和基于多重渐消因子强跟踪平方根无迹卡尔曼滤波算法，建立改进平方根无迹卡尔曼滤波算法，利用二者优势，构成既可以抑制滤波发散又能保证滤波精度和效率的滤波算法；根据发散判据判断滤波器是否滤波发散，当未发散时，采用基于改进Sage-Husa自适应平方根无迹卡尔曼滤波算法，反之，则采用基于多重渐消因子强跟踪平方根无迹卡尔曼滤波算法；
在Sage-Husa自适应滤波算法中，设定系统噪声和观测噪声的均值为0，由于系统噪声的协方差和观测噪声的协方差不能同时被实时动态估计，因此只能根据其中一个的协方差估计另一个的协方差；在室内定位系统中，系统噪声相对稳定，而观测噪声由传感器特性和外界环境因素引起，具有较大的不确定性，因而设定系统噪声已知，可得k时刻的观测噪声协方差Rk如下：
Figure BDA0002005679310000044
式(10)中，Rk-1表示k-1时刻的观测噪声协方差，zk表示时k刻的观测量，
Figure BDA00020056793100000510
为加权后k时刻的观测预测值，自适应因子dk＝(1-b)/(1-bk)，b为遗忘因子，取值范围为(0,1)；误差协方差矩阵
Figure BDA0002005679310000051
Figure BDA0002005679310000052
表示k时刻的误差协方差平方根预测值；上标为T代表式该矩阵的转置；Hk为是k时刻近似的观测转移矩阵，Hk的表达式如式(11)所示：
Figure BDA0002005679310000053
式(11)中，
Figure BDA0002005679310000054
表示第i个采样点误差协方差权重；
Figure BDA0002005679310000055
表示第i个采样点的观测预测值。
当在滤波过程中趋近收敛时，误差协方差矩阵
Figure BDA0002005679310000056
会逐渐减小，并且Hk是k时刻近似的观测转移矩阵，是一个有限常数，因此
Figure BDA0002005679310000057
的值会逐渐减小，此时对观测噪声协方差的影响较小。因此，可通过简化观测噪声的协方差来提高计算效率，将式(10)简化如下：
Figure BDA0002005679310000058
Sage-Husa算法通过自适应因子dk来调整观测噪声，dk越大，表明对传感器的观测噪声的统计特性的置信度越高，反之，则相反。
在基于改进Sage-Husa自适应平方根无迹卡尔曼滤波算法中，对自适应因子dk进行改进，从而解决收敛速速与收敛值的不匹配的问题。通过自适应调节参数Wk来调节自适应因子dk，从而自适应调节不同传感器噪声特性对于系统的影响，将式(12)转换为：
Figure BDA0002005679310000059
式(13)中，Ik为n×n的单位矩阵，Wk为自适应调节参数，Wk＝diag(m1,...,mn)，其中0＜mi＜1,i＝1,…,n，diag()为由向量构成的对角矩阵；
自适应因子dk如式(14)所示：
dk＝(dupper-dlower)·bk+dlower (14)
式中，dupper为自适应因子取值的上限，dlower为自适应因子取值的下限，dupper和dlower的取值范围均为(0,1]，且dupper>dlower；b为遗忘因子，取值范围为(0.9,1)；
当b越接近于1时，dk收敛速度越慢，然而此时的dk的取值范围由dupper和dlower决定，并由dupper向dlower收敛，其收敛下限值为dlower，通过调整dlower，可确保新数据对观测噪声协方差的更新作用。
改进后的方法，不仅对于新数据的权重值可以设置其收敛于特定值dlower，而不依赖于遗忘因子b的取值，而且自适应因子dk收敛速度更慢，导致dk的数值在滤波起始的时候相对更大，1-dk的数值则相对更小。随着k值的不断增大，距离当前时刻较远的历史数据对于观测噪声协方差的更新作用较小，此时观测噪声协方差的更新依赖于距离当前时刻较近的历史数据。
在本发明中，dupper取值为0.95，dlower取值为0.4，b取值为0.99。
通过调整dlower，进行对观测噪声协方差的更新；
估计观测噪声统计特性的方法为：
Figure BDA0002005679310000061
Figure BDA0002005679310000062
上式(15)(16)中，
Figure BDA0002005679310000063
是k-1时刻的观测噪声协方差的Cholesky因子更新值。
步骤5：使用UWB进行实时定位，同时惯导依据初始位置进行行人航位推算，接收到TDOA数据后，用残差鉴别法鉴别是否存在NLOS情况，结合图3，具体如下：
残差鉴别法的具体步骤为：设定有N个UWB基站传感器，首先设定残差D：
Figure BDA0002005679310000064
其中c为光速，ti,1为信号到达传感器i和主传感器的传播时间差；
如果待定位目标和各传感器节点是LOS传播，则能够求得待定位节点的坐标；相反，待定位节点和传感器之间的非视距信道会使得TDOA的三个双曲线面相交于一个区域，此时目标位置不确定，设定残差阈值进行两种情况的判断。
第一种情况：若存在NLOS，则通过改进平方根无迹卡尔曼滤波算法将惯导航位推算的坐标与UWB的定位坐标进行融合，对UWB解算坐标数据进行修正，重复步骤5，具体如下：
在强跟踪卡尔曼滤波算法中，为保证滤波器的可靠收敛，可以牺牲一定的精度换取滤波稳定性，将状态变量误差协方差或者观测变量误差协方差乘以加权系数，来包含更多未建模的误差，使算法具有很强的突变状态跟踪能力，同时降低对初值和噪声统计特性的敏感性。在室内定位系统中，单一渐消因子不能满足要求，为了使滤波器具有应对系统模型不确定的鲁棒性，本发明在状态变量误差协方差预测值中引入多重渐消因子，对不同的状态估计进行调整，来保证系统具有较好的跟踪能力。
基于多重渐消因子强跟踪平方根无迹卡尔曼滤波算法的具体步骤如下：
步骤6.1、初始条件：
Figure BDA0002005679310000071
式(18)中，
Figure BDA0002005679310000072
表示状态变量估计的初值，x0表示状态变量的初值，Px,0表示状态变量估计的误差协方差初值，Sx,0表示的Cholesky因子；且k时刻引入多重渐消因子后的状态变量误差协方差平方根预测初值
Figure BDA0002005679310000073
同Sx,0。
步骤6.2、状态变量预测：
根据状态变量
Figure BDA0002005679310000074
和误差协方差平方根Sx,k-1利用无迹变换得到2n+1个sigma点，如下：
Figure BDA0002005679310000075
式(19)中，
Figure BDA0002005679310000076
表示k-1时刻的第i个采样点，括号后的i或者i-n表示状态变量误差协方差平方根矩阵的第i或者i-n列；λ＝a2×(n+k)-n，其中a表示散布程度因子，取值范围为[10-6,1]，n表示状态变量的维数，k表示辅助尺度因子，满足k+n≠0。
然后，根据式(19)对状态变量进行时间更新，见式(20)和(21)。
Figure BDA0002005679310000081
Figure BDA0002005679310000082
式中，
Figure BDA0002005679310000083
表示第i个采样点的状态预测值，
Figure BDA0002005679310000084
表示第i个采样点的状态值，
Figure BDA0002005679310000085
表示k时刻的状态变量预测值，
Figure BDA0002005679310000086
表示第i个采样点期望的权重，具体见式(22)。
Figure BDA0002005679310000087
其中误差协方差平方根Sx,k-1使用*Sx,k-1替换。
步骤6.3、引入多重渐消因子的状态变量误差协方差平方根预测：
注：前加*代表引入多重渐消因子后的各变量值。
k时刻未引入多重渐消因子的误差协方差平方根预测值
Figure BDA0002005679310000088
如式(24)所示，Pxz,k是状态量和观测量的误差协方差矩阵，如式(25)所示：
Figure BDA0002005679310000089
Figure BDA00020056793100000810
Figure BDA00020056793100000811
式中，
Figure BDA00020056793100000812
表示k时刻的误差协方差平方根预测值，
Figure BDA00020056793100000813
表示第i个采样点误差协方差权重，Qk是K时刻的系统噪声的方差，具体见式(26)，sign()表示取符号位。
Figure BDA00020056793100000814
式中，b表示验前分布因子，一般取2。
可得到由
Figure BDA00020056793100000815
和
Figure BDA00020056793100000816
构造的sigma采样点
Figure BDA00020056793100000817
且
Figure BDA00020056793100000818
如下：
Figure BDA00020056793100000819
Figure BDA0002005679310000091
Figure BDA0002005679310000092
表示引入强跟踪后的第i个采样点的观测预测值。
从而获得k时刻引入多重渐消因子后的误差协方差平方根预测值如式(30)所示：
Figure BDA0002005679310000093
Figure BDA0002005679310000094
式(29)中ζk表示多重渐消因子。
步骤6.4、观测变量更新：
Figure BDA0002005679310000095
Figure BDA0002005679310000096
式中，
Figure BDA0002005679310000097
表示引入强跟踪后的第i个采样点的观测预测值；
Figure BDA0002005679310000098
为由
Figure BDA0002005679310000099
和
Figure BDA00020056793100000910
构造的sigma采样点；
Figure BDA00020056793100000911
表示引入多重渐消因子后的加权后k时刻的观测预测值。
步骤6.5、观测变量误差协方差平方根更新：
Figure BDA00020056793100000912
Figure BDA00020056793100000913
Figure BDA00020056793100000914
其中，*Sz,k表示引入多重渐消因子后的观测变量误差协方差平方根更新值，*Pxz,k是状态量和观测量的误差协方差矩阵。
步骤6.6、卡尔曼增益更新*Kk为：
Figure BDA00020056793100000915
步骤6.7、状态变量
Figure BDA00020056793100000916
和状态变量误差协方差平方根*Sx,k更新：
Figure BDA00020056793100000917
Figure BDA0002005679310000101
重复步骤6.2至步骤6.7，得到在线最优状态估计。
当滤波器未出现发散现象时，
Figure BDA0002005679310000102
序列是均值为0，方差阵为
Figure BDA0002005679310000103
的高斯噪声序列，因此得到服从自由度为n的χ2分布的检验统计量，如式(39)：
Figure BDA0002005679310000104
式中，zk表示k时刻的观测量，观测变量误差协方差平方根Sz,k的表达式为：
Figure BDA0002005679310000105
构造如下发散判据来判断滤波算法是否出现了发散现象：
H0:滤波器未出现发现现象；
H1:滤波器出现了发现现象；
根据式(39)，如果
Figure BDA0002005679310000106
其中1-α是χ2分布的置信度，取值为95％，此时
Figure BDA0002005679310000107
则接受H0，此时滤波器处于未发散状态；否则接受H1，此时滤波器处于发散状态。
第二种情况：若不存在NLOS，则采用UWB解析出当前坐标，完成实时定位。
下面结合附图和实施例对本发明作进一步描述。
实施例1
图4的仿真结果表明，可以看出由于行人航迹推算中累积误差问题的存在，随着时间的推移，导致轨迹出现较大的波动；在前30s范围内，UWB的定位轨迹和规划路线基本保持一致，由于模拟了UWB信号被遮蔽的情况所导致的非视距误差问题的存在，导致在30s至42.5s之间，UWB信号漂移，从而导致定位轨迹出现较大的波动；SR-UKF和改进SR-UKF都能够融合行人航迹推算和UWB定位的位置信息，并有效提高定位精度。
结合图5可以看出，改进SR-UKF算法在定位精度和轨迹平滑性上都优于SR-UKF。使用SR-UKF与单纯的UWB定位相比，位置误差减少了约68.8％，使用改进SR-UKF与单纯的UWB定位相比，位置误差减少了约75.4％，因此，改进SR-UKF比SR-UKF具有更高的定位精度，可以有效提高定位的精度。相比于SR-UKF，改进SR-UKF算法将位置误差减少了21.3％，且位置误差的方差更小，即误差数据分布更集中，波动更小。

Claims (6)

1.一种基于改进平方根无迹卡尔曼滤波的室内定位方法，其特征在于，包括以下步骤：
步骤1：建立UWB定位模型；
步骤2：建立行人航位推算PDR模型；
步骤3：建立UWB/PDR组合定位模型；
步骤4：建立改进平方根无迹卡尔曼滤波算法；
步骤5：使用UWB进行实时定位，同时惯导依据初始位置进行行人航位推算，接收到TDOA数据后，用残差鉴别法鉴别是否存在NLOS情况：
若存在NLOS，则通过改进平方根无迹卡尔曼滤波算法将惯导航位推算的坐标与UWB的定位坐标进行融合，对UWB解算坐标数据进行修正，重复步骤5；
若不存在NLOS，则采用UWB解析出当前坐标，完成实时定位；
步骤4所述的建立改进平方根无迹卡尔曼滤波算法，具体如下：
结合基于改进Sage-Husa自适应平方根无迹卡尔曼滤波算法和基于多重渐消因子强跟踪平方根无迹卡尔曼滤波算法，建立改进平方根无迹卡尔曼滤波算法，根据发散判据判断滤波器是否滤波发散，当未发散时，采用基于改进Sage-Husa自适应平方根无迹卡尔曼滤波算法，反之，则采用基于多重渐消因子强跟踪平方根无迹卡尔曼滤波算法；
当在滤波过程中趋近收敛时，通过简化观测噪声的协方差来提高计算效率，得k时刻的观测噪声协方差Rk如下：
Figure FDA0004006476430000011
式(10)中，Rk-1表示k-1时刻的观测噪声协方差，zk表示时k刻的观测量，
Figure FDA0004006476430000012
为加权后k时刻的观测预测值，自适应因子dk＝(1-b)/(1-bk)，b为遗忘因子，取值范围为(0,1)；误差协方差矩阵
Figure FDA0004006476430000013
Figure FDA0004006476430000014
表示k时刻的误差协方差平方根预测值；上标为T代表式该矩阵的转置；Hk为是k时刻近似的观测转移矩阵，Hk的表达式如式(11)所示：
Figure FDA0004006476430000015
式(11)中，
Figure FDA0004006476430000016
表示第i个采样点误差协方差权重；
Figure FDA0004006476430000017
表示第i个采样点的观测预测值；
当在滤波过程中趋近收敛时，误差协方差矩阵
Figure FDA0004006476430000021
逐渐减小，并且Hk是k时刻近似的观测转移矩阵，为有限常数，因此
Figure FDA0004006476430000022
的值逐渐减小，通过简化观测噪声的协方差来提高计算效率，将式(10)简化如下：
Figure FDA0004006476430000023
通过自适应调节参数Wk来调节自适应因子dk，从而自适应调节不同传感器噪声特性对于系统的影响，将式(12)转换为：
Figure FDA0004006476430000024
式(13)中，Ik为n×n的单位矩阵，Wk为自适应调节参数，Wk＝diag(m1,...,mn)，其中0＜mi＜1,i＝1,…,n，diag()为由向量构成的对角矩阵；
自适应因子dk如式(14)所示：
dk＝(dupper-dlower)·bk+dlower (14)
式中，dupper为自适应因子取值的上限，dlower为自适应因子取值的下限，dupper和dlower的取值范围均为(0,1]，且dupper>dlower；b为遗忘因子，取值范围为(0.9,1)；
通过调整dlower，进行对观测噪声协方差的更新；
估计观测噪声统计特性的方法为：
Figure FDA0004006476430000025
Figure FDA0004006476430000026
上式(15)(16)中，
Figure FDA0004006476430000027
是k-1时刻的观测噪声协方差的Cholesky因子更新值。
2.根据权利要求1所述的基于改进平方根无迹卡尔曼滤波的室内定位方法，其特征在于，步骤1所述的建立UWB定位模型，具体如下：
在三维空间中，利用4个传感器所得到的TDOA测量值来估算待定位节点的位置，设定基站传感器的坐标为(xi,yi,zi)，其中i＝1,2,3,4，待定位点A的坐标为(x,y,z)，则第i个定位基站到A点的距离Ri为：
Ri 2＝(xi-x)2+(yi-y)2+(zi-z)2 (1)
  ＝Ki-2xix-2yiy-2ziz+x2+y2+z2
其中Ki＝xi 2+yi 2+zi 2；
设定第一个定位基站为主传感器，则定位目标到从传感器和到主传感器之间的距离差为Ri,1，信号传播时间差，即TDOA测量值为ti,1，则：
Ri,1＝cti,1＝Ri-R1 (2)
其中c为传输信号的速率即光速；
根据式(1)和式(2)得：
Figure FDA0004006476430000031
在式(1)中，令i＝1，则得：
R1 2＝K1-2x1x-2y1y-2z1z+x2+y2+z2 (4)
用式(3)减去式(4)得：
Ri,1 2+2Ri,1R1＝Ki-K1-2(xi-x1)x-2(yi-y1)y-2(zi-z1)z (5)
设定传感器铺设在室内天花板上，高度为H，则zi＝H；设定主传感器的坐标为(0，0，H)，第二、三、四个传感器的坐标分别为：(x2，0，H)、(0，y3，H)、(x2，y3，H)，由式(5)得：
Figure FDA0004006476430000032
解式(6)得：
Figure FDA0004006476430000033
式(7)中：
Figure FDA0004006476430000034
Figure FDA0004006476430000035
Figure FDA0004006476430000036
Figure FDA0004006476430000041
将x，y值代入式(6)求得R1值，将x，y，R1值代入式(4)求得z值，得到待定位目标的位置。
3.根据权利要求1所述的基于改进平方根无迹卡尔曼滤波的室内定位方法，其特征在于，步骤2所述的建立行人航位推算PDR模型，具体如下：
假设行人在室内二维平面中运动，对行人的运动进行超短时间采样，可将行人在此阶段的运动近似为直线运动；假设行人的初始位置坐标为M0(E0,N0)，初始航向角为
Figure FDA0004006476430000042
行人t1时间内的移动距离为L0，得到行人的当前位置坐标M1(E1,N1)：
Figure FDA0004006476430000043
由上，同理推出ti时刻的位置坐标Mi(Ei,Ni)为：
Figure FDA0004006476430000044
上式(9)中，(Ei-1,Ni-1)表示ti-1时刻的位置坐标，
Figure FDA0004006476430000045
为ti-1时刻的航向角，Li-1表示采样时间内的位移；
由式(9)知，行人航位推算PDR包括初始位姿、步频检测、步长估计和航向计算：
所述步频检测，通过峰值检测法，基于行人的行进过程中产生的行进方向和竖直方向的正负加速度，来确定行人的步频；
所述步长估计，根据行人的生理特征和行走习惯建立模型，得到步长估计；
所述航向计算，通过扩展卡尔曼滤波融合陀螺仪、加速度计和磁力计的数据，得到航向角估计值。
4.根据权利要求1所述的基于改进平方根无迹卡尔曼滤波的室内定位方法，其特征在于，步骤5中所述的用残差鉴别法鉴别是否存在NLOS情况，具体如下：
设定有N个UWB基站传感器，首先设定残差D：
Figure FDA0004006476430000046
其中c为光速，ti,1为信号到达传感器i和主传感器的传播时间差；
如果待定位目标和各传感器节点是LOS传播，则能够求得待定位节点的坐标；相反，待定位节点和传感器之间的非视距信道会使得TDOA的三个双曲线面相交于一个区域，此时目标位置不确定，设定残差阈值进行两种情况的判断。
5.根据权利要求1所述的基于改进平方根无迹卡尔曼滤波的室内定位方法，其特征在于，步骤5所述的通过改进平方根无迹卡尔曼滤波算法将惯导航位推算的坐标与UWB的定位坐标进行融合，对UWB解算坐标数据进行修正，具体如下：
步骤6.1、初始条件：
Figure FDA0004006476430000051
式(18)中，
Figure FDA0004006476430000052
表示状态变量估计的初值，x0表示状态变量的初值，Px,0表示状态变量估计的误差协方差初值，Sx,0表示的Cholesky因子；且k时刻引入多重渐消因子后的状态变量误差协方差平方根预测初值
Figure FDA0004006476430000053
同Sx,0；
步骤6.2、状态变量预测：
根据状态变量
Figure FDA0004006476430000054
和误差协方差平方根Sx,k-1利用无迹变换得到2n+1个sigma点，如下：
Figure FDA0004006476430000055
式(19)中，
Figure FDA0004006476430000056
表示k-1时刻的第i个采样点，括号后的i或者i-n表示状态变量误差协方差平方根矩阵的第i或者i-n列；λ＝a2×(n+k)-n，其中a表示散布程度因子，取值范围为[10-6,1]，n表示状态变量的维数，k表示辅助尺度因子，满足k+n≠0；
然后，根据式(19)对状态变量进行时间更新，见式(20)和(21)：
Figure FDA0004006476430000057
Figure FDA0004006476430000058
式中，
Figure FDA0004006476430000061
表示第i个采样点的状态预测值，
Figure FDA0004006476430000062
表示第i个采样点的状态值，
Figure FDA0004006476430000063
表示k时刻的状态变量预测值，
Figure FDA0004006476430000064
表示第i个采样点期望的权重，具体见式(22)：
Figure FDA0004006476430000065
其中误差协方差平方根Sx,k-1使用*Sx,k-1替换；
步骤6.3、引入多重渐消因子的状态变量误差协方差平方根预测：
前加*代表引入多重渐消因子后的各变量值；
k时刻未引入多重渐消因子的误差协方差平方根预测值
Figure FDA0004006476430000066
如式(24)所示，Pxz,是状态量和观测量的误差协方差矩阵，如式(25)所示：
Figure FDA0004006476430000067
Figure FDA0004006476430000068
Figure FDA0004006476430000069
式中，
Figure FDA00040064764300000610
表示k时刻的误差协方差平方根预测值，
Figure FDA00040064764300000611
表示第i个采样点误差协方差权重，Qk是k时刻的系统噪声的方差，具体见式(26)，sign()表示取符号位：
Figure FDA00040064764300000612
式中，b表示验前分布因子，取2；
可得到由
Figure FDA00040064764300000613
和
Figure FDA00040064764300000614
构造的sigma采样点
Figure FDA00040064764300000615
且
Figure FDA00040064764300000616
如下：
Figure FDA00040064764300000617
Figure FDA00040064764300000618
Figure FDA00040064764300000619
表示引入强跟踪后的第i个采样点的观测预测值；
从而获得k时刻引入多重渐消因子后的误差协方差平方根预测值如式(30)所示：
Figure FDA0004006476430000071
Figure FDA0004006476430000072
式(29)中ζk表示多重渐消因子；
步骤6.4、观测变量更新：
Figure FDA0004006476430000073
Figure FDA0004006476430000074
式中，
Figure FDA0004006476430000075
表示引入强跟踪后的第i个采样点的观测预测值；
Figure FDA0004006476430000076
为由
Figure FDA0004006476430000077
和
Figure FDA0004006476430000078
构造的sigma采样点；
Figure FDA0004006476430000079
表示引入多重渐消因子后的加权后k时刻的观测预测值；
步骤6.5、观测变量误差协方差平方根更新：
Figure FDA00040064764300000710
Figure FDA00040064764300000711
Figure FDA00040064764300000712
其中，*Sz,k表示引入多重渐消因子后的观测变量误差协方差平方根更新值，*Pxz,k是状态量和观测量的误差协方差矩阵；
步骤6.6、卡尔曼增益更新*Kk为：
Figure FDA00040064764300000713
步骤6.7、状态变量
Figure FDA00040064764300000714
和状态变量误差协方差平方根*Sx,k更新：
Figure FDA00040064764300000715
Figure FDA00040064764300000716
重复步骤6.2至步骤6.7，得到在线最优状态估计。
6.根据权利要求1所述的基于改进平方根无迹卡尔曼滤波的室内定位方法，其特征在于，所述的发散判据，具体如下：
当滤波器未出现发散现象时，
Figure FDA0004006476430000081
序列是均值为0，方差阵为
Figure FDA0004006476430000082
的高斯噪声序列，因此得到服从自由度为n的χ2分布的检验统计量，如式(39)：
Figure FDA0004006476430000083
式中，zk表示k时刻的观测量，观测变量误差协方差平方根Sz,k的表达式为：
Figure FDA0004006476430000084
构造如下发散判据来判断滤波算法是否出现了发散现象：
H0:滤波器未出现发散现象；
H1:滤波器出现了发散现象；
根据式(39)，如果
Figure FDA0004006476430000085
其中1-α是χ2分布的置信度，取值为95％，此时
Figure FDA0004006476430000086
则接受H0，此时滤波器处于未发散状态；否则接受H1，此时滤波器处于发散状态。
CN201910227539.3A 2019-03-25 2019-03-25 一种基于改进平方根无迹卡尔曼滤波的室内定位方法 Active CN109916410B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910227539.3A CN109916410B (zh) 2019-03-25 2019-03-25 一种基于改进平方根无迹卡尔曼滤波的室内定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910227539.3A CN109916410B (zh) 2019-03-25 2019-03-25 一种基于改进平方根无迹卡尔曼滤波的室内定位方法

Publications (2)

Publication Number Publication Date
CN109916410A CN109916410A (zh) 2019-06-21
CN109916410B true CN109916410B (zh) 2023-04-28

Family

ID=66966552

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910227539.3A Active CN109916410B (zh) 2019-03-25 2019-03-25 一种基于改进平方根无迹卡尔曼滤波的室内定位方法

Country Status (1)

Country Link
CN (1) CN109916410B (zh)

Families Citing this family (27)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110426040A (zh) * 2019-07-08 2019-11-08 中国人民解放军陆军工程大学 具有非视距识别功能的室内行人定位方法
CN110672093B (zh) * 2019-08-23 2023-08-04 华清科盛(北京)信息技术有限公司 一种基于uwb与惯导融合的车载导航定位方法
CN110401915B (zh) * 2019-08-27 2021-02-05 杭州电子科技大学 一种nlos条件下sekf与距离重构相结合的移动目标定位方法
CN110631576B (zh) * 2019-08-28 2023-01-13 南京理工大学 一种基于uwb和imu的抗nlos的室内定位系统及其定位方法
CN110926460B (zh) * 2019-10-29 2021-03-02 广东工业大学 一种基于IMU的uwb定位异常值处理方法
CN112399366A (zh) * 2020-05-27 2021-02-23 南京邮电大学 基于Hankel矩阵及WKNN方差提取的室内定位法
CN111860976B (zh) * 2020-06-30 2024-04-30 广州地铁集团有限公司 一种闸机通行时间预测方法及装置
CN111722180B (zh) * 2020-07-02 2021-08-13 广东工业大学 一种基于卡尔曼滤波的室内行人定位方法、装置及系统
CN111811503B (zh) * 2020-07-15 2022-02-18 桂林电子科技大学 基于超宽带和二维码的无迹卡尔曼滤波融合定位方法
CN111954153B (zh) * 2020-08-12 2022-11-29 南京工程学院 一种基于uwb定位的智能召车导航定位方法
CN112073909B (zh) * 2020-08-20 2022-05-24 哈尔滨工程大学 基于uwb/mems组合的uwb基站位置误差补偿方法
CN111896008A (zh) * 2020-08-20 2020-11-06 哈尔滨工程大学 一种改进的鲁棒无迹卡尔曼滤波组合导航方法
CN112152954B (zh) * 2020-09-22 2022-09-27 中国人民解放军海军航空大学青岛校区 一种飞行模拟器坐标数据联网传输失真抑制方法
CN112325878A (zh) * 2020-10-30 2021-02-05 南京航空航天大学 基于ukf与空中无人机节点辅助的地面载体组合导航方法
CN112957033B (zh) * 2021-02-01 2022-10-18 山东大学 人机协作中人体实时室内定位与运动姿态捕捉方法与系统
CN113120713B (zh) * 2021-04-28 2023-09-08 上海有个机器人有限公司 电梯定位方法
CN113382354B (zh) * 2021-06-08 2022-04-22 东南大学 一种基于因子图的无线定位非视距信号判别方法
CN113566820B (zh) * 2021-06-17 2023-05-16 电子科技大学 一种基于位置指纹和pdr算法的融合人行定位方法
CN113701742B (zh) * 2021-08-24 2024-04-26 江苏海事职业技术学院 一种基于云端与边端融合计算的移动机器人slam方法
CN113899369B (zh) * 2021-09-26 2024-04-09 东南大学 一种基于自适应降噪算法的超宽带/pdr室内定位方法
CN114111802B (zh) * 2021-12-21 2023-06-13 中国有色金属长沙勘察设计研究院有限公司 一种行人航迹推算辅助uwb的定位方法
CN114088091B (zh) * 2022-01-21 2022-05-17 北京慧拓无限科技有限公司 一种基于多传感器的井工矿位姿融合方法和系统
CN114245459B (zh) * 2022-02-23 2022-06-24 深圳市海豚科技创新有限公司 基于pdr模型的融合定位方法、装置、计算机设备及介质
CN114234984B (zh) * 2022-02-28 2022-05-20 湖南工商大学 基于差分矩阵的室内定位轨迹平滑方法、系统及设备
CN115119141B (zh) * 2022-06-08 2024-05-28 南京邮电大学 一种可用于复杂室内环境基于卡尔曼滤波的uwb定位方法
CN115348539B (zh) * 2022-08-15 2023-10-10 南京濠暻通讯科技有限公司 一种基于5g+的井下通信与定位一体化系统
CN116456460B (zh) * 2023-06-14 2023-09-01 河北师范大学 用于tdoa室内定位的滤波预处理方法

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10511951B2 (en) * 2017-01-17 2019-12-17 3AM Innovations LLC Tracking and accountability device and system
CN106707235B (zh) * 2017-03-08 2019-07-02 南京信息工程大学 一种基于改进的无迹卡尔曼滤波的室内测距定位方法
CN108225337B (zh) * 2017-12-28 2020-12-15 西安空间无线电技术研究所 基于sr-ukf滤波的星敏感器和陀螺组合定姿方法
CN109302690B (zh) * 2018-09-30 2021-02-26 桂林电子科技大学 一种基于优化卡尔曼滤波的非视距室内定位方法

Also Published As

Publication number Publication date
CN109916410A (zh) 2019-06-21

Similar Documents

Publication Publication Date Title
CN109916410B (zh) 一种基于改进平方根无迹卡尔曼滤波的室内定位方法
Yang et al. A novel NLOS error compensation method based IMU for UWB indoor positioning system
US10648816B2 (en) Device and method for integrated navigation based on wireless fingerprints and MEMS sensors
González et al. Mobile robot localization based on ultra-wide-band ranging: A particle filter approach
Plets et al. Experimental performance evaluation of outdoor tdoa and rss positioning in a public lora network
Zampella et al. Robust indoor positioning fusing PDR and RF technologies: The RFID and UWB case
US20070139269A1 (en) System and method for model-free position estimation and tracking
US10567918B2 (en) Radio-location method for locating a target device contained within a region of space
Kaune et al. Wide area multilateration using ADS-B transponder signals
CN109782289B (zh) 一种基于基线几何结构约束的水下航行器定位方法
Gentner et al. Simultaneous localization and mapping in multipath environments
Nguyen et al. Joint transmitter waveform and receiver path optimization for target tracking by multistatic radar system
Si et al. An adaptive weighted Wi-Fi FTM-based positioning method in an NLOS environment
JP3750859B2 (ja) レーダ追尾装置及びレーダ追尾処理方法
Shalihan et al. NLOS ranging mitigation with neural network model for UWB localization
RU2599259C1 (ru) Способ бондаренко а.в. получения радиотехнической информации и радиотехнический комплекс для его осуществления
KR102119196B1 (ko) 머신러닝 기반의 실내 측위 방법 및 시스템
RU2562616C1 (ru) Способ получения радиотехнической информации и радиотехнический комплекс для его осуществления
Retscher et al. Cooperative localization in indoor environments using constrained differential Wi-Fi and UWB measurements
Ulmschneider et al. Association of transmitters in multipath-assisted positioning
Chóliz et al. Evaluation of algorithms for UWB indoor tracking
CN113891251B (zh) 一种基于AGV的惯导-WiFi信号融合定位算法
CN114035154B (zh) 一种运动参数辅助的单站射频信号定位方法
Ebner et al. On prior navigation knowledge in multi sensor indoor localisation
Kong et al. Hybrid indoor positioning method of BLE and monocular VINS based smartphone

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
CB03 Change of inventor or designer information
CB03 Change of inventor or designer information

Inventor after: Xing Zongyi

Inventor after: Liu Yuan

Inventor after: Yang Xing

Inventor after: Bao Jiandong

Inventor after: Hu Mingyue

Inventor after: Sun Qiang

Inventor before: Liu Yuan

Inventor before: Yang Xing

Inventor before: Bao Jiandong

Inventor before: Hu Mingyue

Inventor before: Sun Qiang

Inventor before: Xing Zongyi

GR01 Patent grant
GR01 Patent grant