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

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

Info

Publication number
CN109916410A
CN109916410A CN201910227539.3A CN201910227539A CN109916410A CN 109916410 A CN109916410 A CN 109916410A CN 201910227539 A CN201910227539 A CN 201910227539A CN 109916410 A CN109916410 A CN 109916410A
Authority
CN
China
Prior art keywords
formula
square root
time
uwb
value
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.)
Granted
Application number
CN201910227539.3A
Other languages
English (en)
Other versions
CN109916410B (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

Classifications

    • YGENERAL 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
    • Y02TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02DCLIMATE 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/00Reducing energy consumption in communication networks
    • Y02D30/70Reducing 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为:
其中Ki=xi 2+yi 2+zi 2
设定第一个定位基站为主传感器,则定位目标到从传感器和到主传感器之间的距离差为Ri,1,信号传播时间差,即TDOA测量值为ti,1,则:
Ri,1=cti,1=Ri-R1 (2)
其中c为传输信号的速率即光速;
根据式(1)和式(2)得:
在式(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)得:
解式(6)得:
式(7)中:
将x,y值代入式(6)求得R1值,将x,y,R1值代入式(4)求得z值,得到待定位目标的位置。
步骤2:建立行人航位推算PDR模型,具体如下:
设定行人在室内二维平面中运动,对行人的运动进行超短时间采样,可将行人在此阶段的运动近似为直线运动。假设行人的初始位置坐标为M0(E0,N0),初始航向角为行人t1时间内的移动距离为L0,得到行人的当前位置坐标M1(E1,N1):
由上,同理可以推出ti时刻的位置坐标Mi(Ei,Ni)为:
上式(9)中,(Ei-1,Ni-1)表示ti-1时刻的位置坐标,为ti-1时刻的航向角,Li-1表示采样时间内的位移。
由式(9)知,行人航位推算PDR包括初始位姿、步频检测、步长估计和航向计算:
所述步频检测,通过峰值检测法,基于行人的行进过程中产生的行进方向和竖直方向的正负加速度,来确定行人的步频;
所述步长估计,根据行人的生理特征和行走习惯建立模型,得到步长估计;
所述航向计算,通过扩展卡尔曼滤波融合陀螺仪、加速度计和磁力计的数据,得到航向角估计值。
步骤3:建立UWB/PDR组合定位定位模型,如图2所示。
步骤4:建立改进平方根无迹卡尔曼滤波算法,具体如下:
结合基于改进Sage-Husa自适应平方根无迹卡尔曼滤波算法和基于多重渐消因子强跟踪平方根无迹卡尔曼滤波算法,建立改进平方根无迹卡尔曼滤波算法,利用二者优势,构成既可以抑制滤波发散又能保证滤波精度和效率的滤波算法;根据发散判据判断滤波器是否滤波发散,当未发散时,采用基于改进Sage-Husa自适应平方根无迹卡尔曼滤波算法,反之,则采用基于多重渐消因子强跟踪平方根无迹卡尔曼滤波算法;
在Sage-Husa自适应滤波算法中,设定系统噪声和观测噪声的均值为0,由于系统噪声的协方差和观测噪声的协方差不能同时被实时动态估计,因此只能根据其中一个的协方差估计另一个的协方差;在室内定位系统中,系统噪声相对稳定,而观测噪声由传感器特性和外界环境因素引起,具有较大的不确定性,因而设定系统噪声已知,可得k时刻的观测噪声协方差Rk如下:
式(10)中,Rk-1表示k-1时刻的观测噪声协方差,zk表示时k刻的观测量,为加权后k时刻的观测预测值,自适应因子dk=(1-b)/(1-bk),b为遗忘因子,取值范围为(0,1);误差协方差矩阵 表示k时刻的误差协方差平方根预测值;上标为T代表式该矩阵的转置;Hk为是k时刻近似的观测转移矩阵,Hk的表达式如式(11)所示:
式(11)中,表示第i个采样点误差协方差权重;表示第i个采样点的观测预测值。
当在滤波过程中趋近收敛时,误差协方差矩阵会逐渐减小,并且Hk是k时刻近似的观测转移矩阵,是一个有限常数,因此的值会逐渐减小,此时对观测噪声协方差的影响较小。因此,可通过简化观测噪声的协方差来提高计算效率,将式(10)简化如下:
Sage-Husa算法通过自适应因子dk来调整观测噪声,dk越大,表明对传感器的观测噪声的统计特性的置信度越高,反之,则相反。
在基于改进Sage-Husa自适应平方根无迹卡尔曼滤波算法中,对自适应因子dk进行改进,从而解决收敛速速与收敛值的不匹配的问题。通过自适应调节参数Wk来调节自适应因子dk,从而自适应调节不同传感器噪声特性对于系统的影响,将式(12)转换为:
式(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,进行对观测噪声协方差的更新;
估计观测噪声统计特性的方法为:
上式(15)(16)中,是k-1时刻的观测噪声协方差的Cholesky因子更新值。
步骤5:使用UWB进行实时定位,同时惯导依据初始位置进行行人航位推算,接收到TDOA数据后,用残差鉴别法鉴别是否存在NLOS情况,结合图3,具体如下:
残差鉴别法的具体步骤为:设定有N个UWB基站传感器,首先设定残差D:
其中c为光速,ti,1为信号到达传感器i和主传感器的传播时间差;
如果待定位目标和各传感器节点是LOS传播,则能够求得待定位节点的坐标;相反,待定位节点和传感器之间的非视距信道会使得TDOA的三个双曲线面相交于一个区域,此时目标位置不确定,设定残差阈值进行两种情况的判断。
第一种情况:若存在NLOS,则通过改进平方根无迹卡尔曼滤波算法将惯导航位推算的坐标与UWB的定位坐标进行融合,对UWB解算坐标数据进行修正,重复步骤5,具体如下:
在强跟踪卡尔曼滤波算法中,为保证滤波器的可靠收敛,可以牺牲一定的精度换取滤波稳定性,将状态变量误差协方差或者观测变量误差协方差乘以加权系数,来包含更多未建模的误差,使算法具有很强的突变状态跟踪能力,同时降低对初值和噪声统计特性的敏感性。在室内定位系统中,单一渐消因子不能满足要求,为了使滤波器具有应对系统模型不确定的鲁棒性,本发明在状态变量误差协方差预测值中引入多重渐消因子,对不同的状态估计进行调整,来保证系统具有较好的跟踪能力。
基于多重渐消因子强跟踪平方根无迹卡尔曼滤波算法的具体步骤如下:
步骤6.1、初始条件:
式(18)中,表示状态变量估计的初值,x0表示状态变量的初值,Px,0表示状态变量估计的误差协方差初值,Sx,0表示的Cholesky因子;且k时刻引入多重渐消因子后的状态变量误差协方差平方根预测初值同Sx,0
步骤6.2、状态变量预测:
根据状态变量和误差协方差平方根Sx,k-1利用无迹变换得到2n+1个sigma点,如下:
式(19)中,表示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)。
式中,表示第i个采样点的状态预测值,表示第i个采样点的状态值,表示k时刻的状态变量预测值,表示第i个采样点期望的权重,具体见式(22)。
其中误差协方差平方根Sx,k-1使用*Sx,k-1替换。
步骤6.3、引入多重渐消因子的状态变量误差协方差平方根预测:
注:前加*代表引入多重渐消因子后的各变量值。
k时刻未引入多重渐消因子的误差协方差平方根预测值如式(24)所示,Pxz,k是状态量和观测量的误差协方差矩阵,如式(25)所示:
式中,表示k时刻的误差协方差平方根预测值,表示第i个采样点误差协方差权重,Qk是K时刻的系统噪声的方差,具体见式(26),sign()表示取符号位。
式中,b表示验前分布因子,一般取2。
可得到由构造的sigma采样点如下:
表示引入强跟踪后的第i个采样点的观测预测值。
从而获得k时刻引入多重渐消因子后的误差协方差平方根预测值如式(30)所示:
式(29)中ζk表示多重渐消因子。
步骤6.4、观测变量更新:
式中,表示引入强跟踪后的第i个采样点的观测预测值;为由构造的sigma采样点;表示引入多重渐消因子后的加权后k时刻的观测预测值。
步骤6.5、观测变量误差协方差平方根更新:
其中,*Sz,k表示引入多重渐消因子后的观测变量误差协方差平方根更新值,*Pxz,k是状态量和观测量的误差协方差矩阵。
步骤6.6、卡尔曼增益更新*Kk为:
步骤6.7、状态变量和状态变量误差协方差平方根*Sx,k更新:
重复步骤6.2至步骤6.7,得到在线最优状态估计。
当滤波器未出现发散现象时,序列是均值为0,方差阵为的高斯噪声序列,因此得到服从自由度为n的χ2分布的检验统计量,如式(39):
式中,zk表示k时刻的观测量,观测变量误差协方差平方根Sz,k的表达式为:
构造如下发散判据来判断滤波算法是否出现了发散现象:
H0:滤波器未出现发现现象;
H1:滤波器出现了发现现象;
根据式(39),如果其中1-α是χ2分布的置信度,取值为95%,此时则接受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 (7)

1.一种基于改进平方根无迹卡尔曼滤波的室内定位方法,其特征在于,包括以下步骤:
步骤1:建立UWB定位模型;
步骤2:建立行人航位推算PDR模型;
步骤3:建立UWB/PDR组合定位模型;
步骤4:建立改进平方根无迹卡尔曼滤波算法;
步骤5:使用UWB进行实时定位,同时惯导依据初始位置进行行人航位推算,接收到TDOA数据后,用残差鉴别法鉴别是否存在NLOS情况:
若存在NLOS,则通过改进平方根无迹卡尔曼滤波算法将惯导航位推算的坐标与UWB的定位坐标进行融合,对UWB解算坐标数据进行修正,重复步骤5;
若不存在NLOS,则采用UWB解析出当前坐标,完成实时定位。
2.根据权利要求1所述的基于改进平方根无迹卡尔曼滤波的室内定位方法,其特征在于,步骤1所述的建立UWB定位模型,具体如下:
在三维空间中,利用4个传感器所得到的TDOA测量值来估算待定位节点的位置,设定基站传感器的坐标为(xi,yi,zi),其中i=1,2,3,4,待定位点A的坐标为(x,y,z),则第i个定位基站到A点的距离Ri为:
其中Ki=xi 2+yi 2+zi 2
设定第一个定位基站为主传感器,则定位目标到从传感器和到主传感器之间的距离差为Ri,1,信号传播时间差,即TDOA测量值为ti,1,则:
Ri,1=cti,1=Ri-R1 (2)
其中c为传输信号的速率即光速;
根据式(1)和式(2)得:
在式(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)得:
解式(6)得:
式(7)中:
将x,y值代入式(6)求得R1值,将x,y,R1值代入式(4)求得z值,得到待定位目标的位置。
3.根据权利要求1所述的基于改进平方根无迹卡尔曼滤波的室内定位方法,其特征在于,步骤2所述的建立行人航位推算PDR模型,具体如下:
假设行人在室内二维平面中运动,对行人的运动进行超短时间采样,可将行人在此阶段的运动近似为直线运动;假设行人的初始位置坐标为M0(E0,N0),初始航向角为行人t1时间内的移动距离为L0,得到行人的当前位置坐标M1(E1,N1):
由上,同理推出ti时刻的位置坐标Mi(Ei,Ni)为:
上式(9)中,(Ei-1,Ni-1)表示ti-1时刻的位置坐标,为ti-1时刻的航向角,Li-1表示采样时间内的位移;
由式(9)知,行人航位推算PDR包括初始位姿、步频检测、步长估计和航向计算:
所述步频检测,通过峰值检测法,基于行人的行进过程中产生的行进方向和竖直方向的正负加速度,来确定行人的步频;
所述步长估计,根据行人的生理特征和行走习惯建立模型,得到步长估计;
所述航向计算,通过扩展卡尔曼滤波融合陀螺仪、加速度计和磁力计的数据,得到航向角估计值。
4.根据权利要求1所述的基于改进平方根无迹卡尔曼滤波的室内定位方法,其特征在于,步骤4所述的建立改进平方根无迹卡尔曼滤波算法,具体如下:
结合基于改进Sage-Husa自适应平方根无迹卡尔曼滤波算法和基于多重渐消因子强跟踪平方根无迹卡尔曼滤波算法,建立改进平方根无迹卡尔曼滤波算法,根据发散判据判断滤波器是否滤波发散,当未发散时,采用基于改进Sage-Husa自适应平方根无迹卡尔曼滤波算法,反之,则采用基于多重渐消因子强跟踪平方根无迹卡尔曼滤波算法;
当在滤波过程中趋近收敛时,通过简化观测噪声的协方差来提高计算效率,得k时刻的观测噪声协方差Rk如下:
式(10)中,Rk-1表示k-1时刻的观测噪声协方差,zk表示时k刻的观测量,为加权后k时刻的观测预测值,自适应因子dk=(1-b)/(1-bk),b为遗忘因子,取值范围为(0,1);误差协方差矩阵 表示k时刻的误差协方差平方根预测值;上标为T代表式该矩阵的转置;Hk为是k时刻近似的观测转移矩阵,Hk的表达式如式(11)所示:
式(11)中,表示第i个采样点误差协方差权重;表示第i个采样点的观测预测值;
当在滤波过程中趋近收敛时,误差协方差矩阵逐渐减小,并且Hk是k时刻近似的观测转移矩阵,为有限常数,因此的值逐渐减小,通过简化观测噪声的协方差来提高计算效率,将式(10)简化如下:
通过自适应调节参数Wk来调节自适应因子dk,从而自适应调节不同传感器噪声特性对于系统的影响,将式(12)转换为:
式(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,进行对观测噪声协方差的更新;
估计观测噪声统计特性的方法为:
上式(15)(16)中,是k-1时刻的观测噪声协方差的Cholesky因子更新值。
5.根据权利要求1所述的基于改进平方根无迹卡尔曼滤波的室内定位方法,其特征在于,步骤5中所述的用残差鉴别法鉴别是否存在NLOS情况,具体如下:
设定有N个UWB基站传感器,首先设定残差D:
其中c为光速,ti,1为信号到达传感器i和主传感器的传播时间差;
如果待定位目标和各传感器节点是LOS传播,则能够求得待定位节点的坐标;相反,待定位节点和传感器之间的非视距信道会使得TDOA的三个双曲线面相交于一个区域,此时目标位置不确定,设定残差阈值进行两种情况的判断。
6.根据权利要求1所述的基于改进平方根无迹卡尔曼滤波的室内定位方法,其特征在于,步骤5所述的通过改进平方根无迹卡尔曼滤波算法将惯导航位推算的坐标与UWB的定位坐标进行融合,对UWB解算坐标数据进行修正,具体如下:
步骤6.1、初始条件:
式(18)中,表示状态变量估计的初值,x0表示状态变量的初值,Px,0表示状态变量估计的误差协方差初值,Sx,0表示的Cholesky因子;且k时刻引入多重渐消因子后的状态变量误差协方差平方根预测初值同Sx,0
步骤6.2、状态变量预测:
根据状态变量和误差协方差平方根Sx,k-1利用无迹变换得到2n+1个sigma点,如下:
式(19)中,表示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):
式中,表示第i个采样点的状态预测值,表示第i个采样点的状态值,表示k时刻的状态变量预测值,表示第i个采样点期望的权重,具体见式(22):
其中误差协方差平方根Sx,k-1使用*Sx,k-1替换;
步骤6.3、引入多重渐消因子的状态变量误差协方差平方根预测:
前加*代表引入多重渐消因子后的各变量值;
k时刻未引入多重渐消因子的误差协方差平方根预测值如式(24)所示,Pxz,k是状态量和观测量的误差协方差矩阵,如式(25)所示:
式中,表示k时刻的误差协方差平方根预测值,表示第i个采样点误差协方差权重,Qk是k时刻的系统噪声的方差,具体见式(26),sign()表示取符号位:
式中,b表示验前分布因子,取2;
可得到由构造的sigma采样点如下:
表示引入强跟踪后的第i个采样点的观测预测值;
从而获得k时刻引入多重渐消因子后的误差协方差平方根预测值如式(30)所示:
式(29)中ζk表示多重渐消因子;
步骤6.4、观测变量更新:
式中,表示引入强跟踪后的第i个采样点的观测预测值;为由构造的sigma采样点;表示引入多重渐消因子后的加权后k时刻的观测预测值;
步骤6.5、观测变量误差协方差平方根更新:
其中,*Sz,k表示引入多重渐消因子后的观测变量误差协方差平方根更新值,*Pxz,k是状态量和观测量的误差协方差矩阵;
步骤6.6、卡尔曼增益更新*Kk为:
步骤6.7、状态变量和状态变量误差协方差平方根*Sx,k更新:
重复步骤6.2至步骤6.7,得到在线最优状态估计。
7.根据权利要求4所述的基于改进平方根无迹卡尔曼滤波的室内定位方法,其特征在于,所述的发散判据,具体如下:
当滤波器未出现发散现象时,序列是均值为0,方差阵为的高斯噪声序列,因此得到服从自由度为n的χ2分布的检验统计量,如式(39):
式中,zk表示k时刻的观测量,观测变量误差协方差平方根Sz,k的表达式为:
构造如下发散判据来判断滤波算法是否出现了发散现象:
H0:滤波器未出现发现现象;
H1:滤波器出现了发现现象;
根据式(39),如果其中1-α是χ2分布的置信度,取值为95%,此时则接受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 true CN109916410A (zh) 2019-06-21
CN109916410B 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)

Cited By (28)

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

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106707235A (zh) * 2017-03-08 2017-05-24 南京信息工程大学 一种基于改进的无迹卡尔曼滤波的室内测距定位方法
CN108225337A (zh) * 2017-12-28 2018-06-29 西安电子科技大学 基于sr-ukf滤波的星敏感器和陀螺组合定姿方法
US20180206099A1 (en) * 2017-01-17 2018-07-19 3AM Innovations LLC Tracking and accountability device and system
CN109302690A (zh) * 2018-09-30 2019-02-01 桂林电子科技大学 一种基于优化卡尔曼滤波的非视距室内定位方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20180206099A1 (en) * 2017-01-17 2018-07-19 3AM Innovations LLC Tracking and accountability device and system
CN106707235A (zh) * 2017-03-08 2017-05-24 南京信息工程大学 一种基于改进的无迹卡尔曼滤波的室内测距定位方法
CN108225337A (zh) * 2017-12-28 2018-06-29 西安电子科技大学 基于sr-ukf滤波的星敏感器和陀螺组合定姿方法
CN109302690A (zh) * 2018-09-30 2019-02-01 桂林电子科技大学 一种基于优化卡尔曼滤波的非视距室内定位方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
SIAMAK YOUSEFI: ""Mobile Localization in Non-Line-of-Sight Using Constrained Square-Root Unscented Kalman Filter"", 《IEEE TRANSACTIONS ON VEHICULAR TECHNOLOGY》 *
丁宁: ""基于UWB/IMU组合的AGV导航技术研究"", 《中国优秀博硕士学位论文全文数据库(硕士)信息科技辑》 *
许波: ""改进型平方根无迹卡尔曼滤波及其在无轴承永磁同步电机无速度传感器运行中的应用"", 《控制理论与应用》 *

Cited By (47)

* 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与惯导融合的车载导航定位方法
CN110672093A (zh) * 2019-08-23 2020-01-10 华清科盛(北京)信息技术有限公司 一种基于uwb与惯导融合的车载导航定位方法
CN110401915A (zh) * 2019-08-27 2019-11-01 杭州电子科技大学 一种nlos条件下sekf与距离重构相结合的移动目标定位方法
CN110401915B (zh) * 2019-08-27 2021-02-05 杭州电子科技大学 一种nlos条件下sekf与距离重构相结合的移动目标定位方法
CN110631576A (zh) * 2019-08-28 2019-12-31 南京理工大学 一种基于uwb和imu的抗nlos的室内定位系统及其定位方法
CN110631576B (zh) * 2019-08-28 2023-01-13 南京理工大学 一种基于uwb和imu的抗nlos的室内定位系统及其定位方法
CN110926460A (zh) * 2019-10-29 2020-03-27 广东工业大学 一种基于IMU的uwb定位异常值处理方法
JP2022500617A (ja) * 2019-10-29 2022-01-04 広東工業大学Guangdong University Of Technology IMUに基づくuwb測位異常値処理方法
JP7055484B2 (ja) 2019-10-29 2022-04-18 広東工業大学 IMUに基づくuwb測位異常値処理方法
WO2021082790A1 (zh) * 2019-10-29 2021-05-06 广东工业大学 一种基于IMU的uwb定位异常值处理方法
CN112399366A (zh) * 2020-05-27 2021-02-23 南京邮电大学 基于Hankel矩阵及WKNN方差提取的室内定位法
CN111860976A (zh) * 2020-06-30 2020-10-30 广州地铁集团有限公司 一种闸机通行时间预测方法及装置
CN111860976B (zh) * 2020-06-30 2024-04-30 广州地铁集团有限公司 一种闸机通行时间预测方法及装置
CN111722180A (zh) * 2020-07-02 2020-09-29 广东工业大学 一种基于卡尔曼滤波的室内行人定位方法、装置及系统
CN111722180B (zh) * 2020-07-02 2021-08-13 广东工业大学 一种基于卡尔曼滤波的室内行人定位方法、装置及系统
CN111811503A (zh) * 2020-07-15 2020-10-23 桂林电子科技大学 基于超宽带和二维码的无迹卡尔曼滤波融合定位方法
CN111954153A (zh) * 2020-08-12 2020-11-17 南京工程学院 一种基于uwb定位的智能召车导航定位方法
CN111954153B (zh) * 2020-08-12 2022-11-29 南京工程学院 一种基于uwb定位的智能召车导航定位方法
CN112073909A (zh) * 2020-08-20 2020-12-11 哈尔滨工程大学 基于uwb/mems组合的uwb基站位置误差补偿方法
CN111896008A (zh) * 2020-08-20 2020-11-06 哈尔滨工程大学 一种改进的鲁棒无迹卡尔曼滤波组合导航方法
CN112073909B (zh) * 2020-08-20 2022-05-24 哈尔滨工程大学 基于uwb/mems组合的uwb基站位置误差补偿方法
CN112152954A (zh) * 2020-09-22 2020-12-29 中国人民解放军海军航空大学青岛校区 一种飞行模拟器坐标数据联网传输失真抑制方法
CN112325878A (zh) * 2020-10-30 2021-02-05 南京航空航天大学 基于ukf与空中无人机节点辅助的地面载体组合导航方法
CN112957033A (zh) * 2021-02-01 2021-06-15 山东大学 人机协作中人体实时室内定位与运动姿态捕捉方法与系统
CN113120713A (zh) * 2021-04-28 2021-07-16 上海有个机器人有限公司 电梯定位方法
CN113120713B (zh) * 2021-04-28 2023-09-08 上海有个机器人有限公司 电梯定位方法
CN113382354B (zh) * 2021-06-08 2022-04-22 东南大学 一种基于因子图的无线定位非视距信号判别方法
CN113382354A (zh) * 2021-06-08 2021-09-10 东南大学 一种基于因子图的无线定位非视距信号判别方法
CN113566820A (zh) * 2021-06-17 2021-10-29 电子科技大学 一种基于位置指纹和pdr算法的融合人行定位方法
CN113566820B (zh) * 2021-06-17 2023-05-16 电子科技大学 一种基于位置指纹和pdr算法的融合人行定位方法
CN113701742B (zh) * 2021-08-24 2024-04-26 江苏海事职业技术学院 一种基于云端与边端融合计算的移动机器人slam方法
CN113701742A (zh) * 2021-08-24 2021-11-26 吕太之 一种基于云端与边端融合计算的移动机器人slam方法
CN113899369B (zh) * 2021-09-26 2024-04-09 东南大学 一种基于自适应降噪算法的超宽带/pdr室内定位方法
CN114111802A (zh) * 2021-12-21 2022-03-01 中国有色金属长沙勘察设计研究院有限公司 一种行人航迹推算辅助uwb的定位方法
CN114428227A (zh) * 2022-01-21 2022-05-03 湖北大学 一种基于改进无迹粒子滤波的室内可见光车辆定位方法
CN114088091A (zh) * 2022-01-21 2022-02-25 北京慧拓无限科技有限公司 一种基于多传感器的井工矿位姿融合方法和系统
CN114428227B (zh) * 2022-01-21 2024-09-17 湖北大学 一种基于改进无迹粒子滤波的室内可见光车辆定位方法
CN114245459A (zh) * 2022-02-23 2022-03-25 深圳市海豚科技创新有限公司 基于pdr模型的融合定位方法、装置、计算机设备及介质
CN114234984B (zh) * 2022-02-28 2022-05-20 湖南工商大学 基于差分矩阵的室内定位轨迹平滑方法、系统及设备
CN114234984A (zh) * 2022-02-28 2022-03-25 湖南工商大学 基于差分矩阵的室内定位轨迹平滑方法、系统及设备
CN115119141A (zh) * 2022-06-08 2022-09-27 南京邮电大学 一种可用于复杂室内环境基于卡尔曼滤波的uwb定位方法
CN115119141B (zh) * 2022-06-08 2024-05-28 南京邮电大学 一种可用于复杂室内环境基于卡尔曼滤波的uwb定位方法
CN115348539A (zh) * 2022-08-15 2022-11-15 南京濠暻通讯科技有限公司 一种基于5g+的井下通信与定位一体化系统
CN115348539B (zh) * 2022-08-15 2023-10-10 南京濠暻通讯科技有限公司 一种基于5g+的井下通信与定位一体化系统
CN116456460A (zh) * 2023-06-14 2023-07-18 河北师范大学 用于tdoa室内定位的滤波预处理方法
CN116456460B (zh) * 2023-06-14 2023-09-01 河北师范大学 用于tdoa室内定位的滤波预处理方法

Also Published As

Publication number Publication date
CN109916410B (zh) 2023-04-28

Similar Documents

Publication Publication Date Title
CN109916410A (zh) 一种基于改进平方根无迹卡尔曼滤波的室内定位方法
Yang et al. A novel NLOS error compensation method based IMU for UWB indoor positioning system
Jourdan et al. Monte Carlo localization in dense multipath environments using UWB ranging
US10648816B2 (en) Device and method for integrated navigation based on wireless fingerprints and MEMS sensors
US9244152B1 (en) Determining device locations using movement, signal strength
US10567918B2 (en) Radio-location method for locating a target device contained within a region of space
Wu et al. Particle-filter-based radio localization for mobile robots in the environments with low-density WLAN APs
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
CN105372628A (zh) 一种基于Wi-Fi的室内定位导航方法
US6947880B2 (en) Method for improving accuracy of a velocity model
Zeng et al. UWB/IMU integration approach with NLOS identification and mitigation
Lee et al. Indoor localization by Kalman filter based combining of UWB-positioning and PDR
CN104457755B (zh) 一种位置获取方法
Wen et al. An indoor localization and tracking system using successive weighted RSS projection
Si et al. An adaptive weighted Wi-Fi FTM-based positioning method in an NLOS environment
Zhao et al. Comparing centralized Kalman filter schemes for indoor positioning in wireless sensor network
JP3750859B2 (ja) レーダ追尾装置及びレーダ追尾処理方法
CN105898711A (zh) 一种基于地磁基准线的定位方法及装置
CN115031723A (zh) 一种uwb定位精度提升办法
Zhou et al. An Indoor Positioning Algorithm Based on Particle Filter and Neighbor-Guided Particle Optimization for Wireless Sensor Networks
Retscher et al. Cooperative localization in indoor environments using constrained differential Wi-Fi and UWB measurements
CN117570980A (zh) 一种基于uwb与gps融合定位算法的方法及系统
Lou et al. High-accuracy positioning algorithm based on UWB
Ebner et al. On prior navigation knowledge in multi sensor indoor localisation

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

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

CB03 Change of inventor or designer information
GR01 Patent grant
GR01 Patent grant