CN109141413B - 具有数据缺失uwb行人定位的efir滤波算法及系统 - Google Patents

具有数据缺失uwb行人定位的efir滤波算法及系统 Download PDF

Info

Publication number
CN109141413B
CN109141413B CN201810886573.7A CN201810886573A CN109141413B CN 109141413 B CN109141413 B CN 109141413B CN 201810886573 A CN201810886573 A CN 201810886573A CN 109141413 B CN109141413 B CN 109141413B
Authority
CN
China
Prior art keywords
time
efir
ekf
distance information
state vector
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
CN201810886573.7A
Other languages
English (en)
Other versions
CN109141413A (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.)
University of Jinan
Original Assignee
University of Jinan
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 University of Jinan filed Critical University of Jinan
Priority to CN201810886573.7A priority Critical patent/CN109141413B/zh
Publication of CN109141413A publication Critical patent/CN109141413A/zh
Application granted granted Critical
Publication of CN109141413B publication Critical patent/CN109141413B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)

Abstract

本发明公开了具有数据缺失UWB行人定位的EFIR滤波算法及系统，包括：通过UWB系统和惯性导航器件INS系统分别测量参考节点到目标节点之间的距离；在此基础上，将两种系统测量得到的距离信息作差，差值作为数据融合算法所使用的滤波模型的观测量；在此基础上，对传统的EFIR滤波算法进行改进，引入变量
Figure DDA0001755802600000011
表示第i个信道的距离信息是否可用。一旦距离信息不可用，则对不可用的距离信息进行预估，以弥补不可用的距离信息，保证滤波器对位置误差的预估；在此基础上，将惯性导航器件INS测量得到的行人位置与EFIR滤波器得到的位置误差预估作差，最终得到当前时刻最优的行人位置预估。

Description

具有数据缺失UWB行人定位的EFIR滤波算法及系统
技术领域
本发明涉及复杂环境下组合定位技术领域，尤其涉及具有数据缺失UWB行人定位的EFIR滤波算法及系统。
背景技术
近年来，行人导航(Pedestrian Navigation,PN)作为导航技术应用的新兴领域，正越来越受到各国学者的重视，并逐渐成为该领域的研究热点。然而在隧道、大型仓库、地下停车场等室内环境下，外界无线电信号微弱、电磁干扰强烈等因素都会对目标行人导航信息获取的准确性、实时性及鲁棒性有很大影响。如何将室内环境下获取的有限信息进行有效的融合以消除室内复杂环境影响，保证行人导航精度的持续稳定，具有重要的科学理论意义和实际应用价值。
在现有的定位方式中，全球卫星导航系统(Global Navigation SatelliteSystem,GNSS)是最为常用的一种方式。虽然GNSS能够通过精度持续稳定的位置信息，但是其易受电磁干扰、遮挡等外界环境影响的缺点限制了其应用范围，特别是在室内、地下巷道等一些密闭的、环境复杂的场景，GNSS信号被严重遮挡，无法进行有效的工作。近年来，UWB(Ultra Wideband)以其在复杂环境下定位精度高的特点在短距离局部定位领域表现出很大的潜力。学者们提出将基于UWB的目标跟踪应用于GNSS失效环境下的行人导航。这种方式虽然能够实现室内定位，但是由于室内环境复杂多变，UWB信号十分容易受到干扰而导致定位精度下降甚至失锁；与此同时，由于UWB采用的通信技术通常为短距离无线通信技术，因此若想完成大范围的室内目标跟踪定位，需要大量的网络节点共同完成，这必将引入网络组织结构优化设计、多节点多簇网络协同通信等一系列问题。因此现阶段基于UWB的目标跟踪在室内导航领域仍旧面临很多挑战。
发明内容
本发明的目的就是为了解决由于在实时系统中UWB由于受到室内环境的影响不能得到正常的距离信息的问题，提出了一种具有数据缺失UWB行人定位的EFIR滤波算法及系统，该方法对传统的EFIR滤波算法进行改进，首先判断UWB测量得到的距离信息是否有缺失，如果第i个距离信息缺失，则对第i个距离信息进行预估，以保证滤波器的正常运行，最终得到当前时刻最优的行人位置预估。
为实现上述目的，本发明的具体方案如下：
本发明的第一目的是公开一种具有数据缺失UWB行人定位的EFIR滤波算法，包括：
以惯性导航器件INS在t时刻在导航系下的东方向位置、北方向位置、东方向速度、北方向速度作为状态量，以INS与UWB分别测量的参考节点与未知节点之间距离的差值作为系统观测量，构建滤波模型；
利用EFIR滤波算法对位置误差进行预估，预估过程中实时判断UWB测量得到的参考节点与未知节点之间的距离信息是否有缺失，如果有，对缺失的距离信息进行预估；
最终得到当前时刻目标行人最优的导航信息。
进一步地，所述EFIR滤波器的状态方程为：
Figure GDA0002459392170000021
其中，
Figure GDA0002459392170000025
和
Figure GDA0002459392170000026
分别为t和t-1时刻惯性导航器件INS的东方向位置、北方向位置、东方向速度、北方向速度；T为采样周期；ωt-1为t-1时刻的系统噪声；
进一步地，所述EFIR滤波器的观测方程为：
Figure GDA0002459392170000022
其中，di,t,i∈(1,2,...,g)为t时刻UWB分别测量得的参考节点与未知节点之间的距离；g为参考节点的数目；x为UWB解算出的未知节点东方向位置，y为UWB解算出的未知节点北方向位置，xi,i∈(1,2,...,g)和yi,i∈(1,2,...,g)分别为参考节点1到i的东向位置和北向位置；νt为系统t时刻的观测噪声。
进一步地，所述的预估过程中实时判断UWB测量得到的参考节点与未知节点之间的距离信息是否有缺失，如果有，对缺失的距离信息进行预估，具体为：
引入变量
Figure GDA0002459392170000023
Figure GDA0002459392170000024
表示UWB测量得到的参考节点与未知节点之间的第i个距离信息；如果第i个距离信息缺失，则重新对
Figure GDA0002459392170000031
进行预估；采用矩阵h(Xt|t-1)的第i行第1列替代缺失的距离信息。
进一步地，对缺失数据进行预估后，EFIR滤波器的观测方程变为：
Figure GDA0002459392170000032
进一步地，所述利用EFIR滤波算法对位置误差进行预估，具体为：
定义两个变量m和s：
m＝t-N+1，s＝m+M-1；
判断当前时刻t是否大于EFIR滤波器的滤波窗口长度N，如果t＜N，则进行EKF滤波算法，其步骤如下：
首先，进行一步预估
Figure GDA0002459392170000033
Figure GDA0002459392170000034
Ft-1为t-1时刻的系统矩阵；
判断距离信息是否可用，引入变量
Figure GDA0002459392170000035
若第i个距离信息不可用，则
Figure GDA0002459392170000036
对不可用的距离信息进行预估
Figure GDA0002459392170000037
其中，h(Xt|t-1)(i,1)表示用矩阵h(Xt|t-1)的第i行第1列替代不可用的距离信息；
Figure GDA0002459392170000038
Figure GDA0002459392170000039
Pt＝(I-KtHt)Pt|t-1；
其中，
Figure GDA0002459392170000041
表示EKF在t时刻预估的状态向量，
Figure GDA0002459392170000042
表示EKF由t-1时刻到t时刻预估的状态向量，Pt|t-1表示EKF由t-1时刻到t时刻的最小预测均方误差矩阵；Pt表示EKF t时刻的最小预测均方误差矩阵；Kt表示EKF在t时刻的误差增益矩阵；I表示单位阵；其Rt为νt的协方差矩阵；
如果t≥N，则进行EFIR滤波算法，其步骤如下：
Figure GDA0002459392170000043
Figure GDA0002459392170000044
为s时刻EFIR滤波器的状态向量，
Figure GDA0002459392170000045
为s时刻自适应EKF预估滤波器的状态向量；
利用中间变量l，另l在m+M时刻到t时刻进行下列迭代：
Figure GDA0002459392170000046
Figure GDA0002459392170000047
Figure GDA0002459392170000048
Figure GDA0002459392170000049
令
Figure GDA00024593921700000410
其中，Fl表示表示l时刻的系统矩阵，
Figure GDA00024593921700000411
表示EFIR在t时刻预估的状态向量，Kl表示EFIR在l时刻的误差增益矩阵；
Figure GDA00024593921700000412
为由l-1时刻到l时刻的状态向量预估、
Figure GDA00024593921700000413
为由l-1时刻的状态向量预估、Gl为l时刻的广义噪声功率增益、
Figure GDA00024593921700000414
本发明的第二目的是公开一种具有数据缺失UWB行人定位的EFIR滤波系统，包括服务器，所述服务器包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现以下步骤：
以惯性导航器件INS在t时刻在导航系下的东方向位置、北方向位置、东方向速度、北方向速度作为状态量，以INS与UWB分别测量的参考节点与未知节点之间距离的差值作为系统观测量，构建滤波模型；
利用EFIR滤波算法对位置误差进行预估，预估过程中实时判断UWB测量得到的参考节点与未知节点之间的距离信息是否有缺失，如果有，对缺失的距离信息进行预估；
最终得到当前时刻目标行人最优的导航信息。
本发明的第三目的是公开一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时执行以下步骤：
以惯性导航器件INS在t时刻在导航系下的东方向位置、北方向位置、东方向速度、北方向速度作为状态量，以INS与UWB分别测量的参考节点与未知节点之间距离的差值作为系统观测量，构建滤波模型；
利用EFIR滤波算法对位置误差进行预估，预估过程中实时判断UWB测量得到的参考节点与未知节点之间的距离信息是否有缺失，如果有，对缺失的距离信息进行预估；
最终得到当前时刻目标行人最优的导航信息。
本发明的有益效果：
1、通过引入变量
Figure GDA0002459392170000051
表示第i个信道的UWB距离信息是否可用，若第i个距离信息不可用，则
Figure GDA0002459392170000052
对不可用的距离信息进行预估，弥补UWB距离信息不可用导致的数据融合算法不可用的问题。
2、可用于室内环境下的中高精度定位。
附图说明
图1为一种具有数据缺失INS/UWB紧组合行人导航的自适应预估EKF滤波算法的系统示意图；
图2为本发明构建滤波模型进行数据融合示意图；
图3为自适应预估EKF滤波算法流程图。
具体实施方式：
下面结合附图对本发明进行详细说明：
本发明一种具有数据缺失INS/UWB紧组合行人导航的EFIR滤波算法的系统如图1所示，包括：组合导航算法采用UWB和INS两种导航系统，其中，UWB包括UWB参考节点和UWB定位标签，UWB参考节点预先固定在已知坐标上，UWB定位标签固定在目标行人上。INS主要由固定在目标行人足部的IMU组成。
基于上述系统，本发明公开了具有数据缺失INS/UWB紧组合行人导航的EFIR滤波算法，包括：
(1)如图2所示，以惯性导航器件INS在t时刻在导航系下的东方向位置、北方向位置、东方向速度、北方向速度作为状态量，以INS与UWB分别测量的参考节点与未知节点之间距离的差值作为系统观测量，构建滤波模型进行数据融合；
(2)利用EFIR滤波算法对位置误差进行预估，EFIR预估滤波器的状态方程为：
Figure GDA0002459392170000061
其中，
Figure GDA0002459392170000062
和
Figure GDA0002459392170000063
分别为t和t-1时刻惯性导航器件INS的东方向位置、北方向位置、东方向速度、北方向速度；T为采样周期；ωt-1为t-1时刻的系统噪声。
进一步地，所述EFIR滤波器的观测方程为：
Figure GDA0002459392170000064
其中，di,t,i∈(1,2,...,g)为t时刻UWB分别测量得的参考节点与未知节点之间的距离；g为参考节点的数目；x为UWB解算出的未知节点东方向位置，y为UWB解算出的未知节点北方向位置，xi,i∈(1,2,...,g)和yi,i∈(1,2,...,g)分别为参考节点1到i的东向位置和北向位置；νt为系统t时刻的观测噪声。
Figure GDA0002459392170000065
其中，x为UWB解算出的未知节点东方向位置，y为UWB解算出的未知节点北方向位置，xi,i∈(1,2,...,g)和yi,i∈(1,2,...,g)分别为参考节点的东向位置和北向位置。在此基础上，判断一下距离信息是否可用，引入变量
Figure GDA0002459392170000066
若第i个距离信息不可用，则
Figure GDA0002459392170000067
对不可用的距离信息进行预估：
Figure GDA0002459392170000071
进一步地，在t时刻EFIR滤波算法的步骤如下：
首先定义两个变量m和s如下：
m＝t-N+1,s＝m+M-1
判断当前时刻t是否大于EFIR滤波器的滤波窗口长度N，如果t＜N，则进行EKF滤波算法，其步骤如下：
首先，进行一步预估
Figure GDA0002459392170000072
Figure GDA0002459392170000073
判断距离信息是否可用，引入变量
Figure GDA0002459392170000074
若第i个距离信息不可用，则
Figure GDA0002459392170000075
对不可用的距离信息进行预估
Figure GDA0002459392170000076
其中，h(Xt|t-1)(i,1)表示用矩阵h(Xt|t-1)的第i行第1列替代不可用的距离信息。
Figure GDA0002459392170000077
Figure GDA0002459392170000078
Pt＝(I-KtHt)Pt|t-1
其中，
Figure GDA0002459392170000079
表示自适应EKF预估滤波器在t时刻预估的状态向量，
Figure GDA00024593921700000710
表示EKF由t-1时刻到t时刻预估的状态向量，Pt|t-1表示EKF由t-1时刻到t时刻的最小预测均方误差矩阵；Pt表示自适应EKF预估滤波器t时刻的最小预测均方误差矩阵；Kt表示自适应EKF预估滤波器在t时刻的误差增益矩阵；I表示单位阵。
如果t≥N，则进行EFIR滤波算法，其步骤如下：
Figure GDA0002459392170000081
利用中间变量l，另l在m+M时刻到t时刻进行下列迭代：
Figure GDA0002459392170000082
Figure GDA0002459392170000083
Figure GDA0002459392170000084
Figure GDA0002459392170000085
令
Figure GDA0002459392170000086
其中，Fl表示表示l时刻的系统矩阵，
Figure GDA0002459392170000087
表示EFIR在t时刻预估的状态向量，Kl表示EFIR在l时刻的误差增益矩阵；
Figure GDA0002459392170000088
为由l-1时刻到l时刻的状态向量预估、
Figure GDA0002459392170000089
为由l-1时刻的状态向量预估、Gl为l时刻的广义噪声功率增益、
Figure GDA00024593921700000810
上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。

Claims (7)

1.具有数据缺失UWB行人定位的EFIR滤波算法，其特征在于，包括：
以惯性导航器件INS在t时刻在导航系下的东方向位置、北方向位置、东方向速度、北方向速度作为状态量，以INS与UWB分别测量的参考节点与未知节点之间距离的差值作为系统观测量，构建滤波模型；
利用EFIR滤波算法对位置误差进行预估，具体为：
定义两个变量m和s：
m＝t-N+1，s＝m+M-1；
判断当前时刻t是否大于EFIR滤波器的滤波窗口长度N，如果t＜N，则进行EKF滤波算法，其步骤如下：
首先，进行一步预估
Figure FDA0002489918450000011
Figure FDA0002489918450000012
判断距离信息是否可用，引入变量
Figure FDA0002489918450000013
若第i个距离信息不可用，则
Figure FDA0002489918450000014
对不可用的距离信息进行预估：
Figure FDA0002489918450000015
其中，h(Xt|t-1)(i,1)表示用矩阵h(Xt|t-1)的第i行第1列替代不可用的距离信息；
Figure FDA0002489918450000016
Figure FDA0002489918450000017
Pt＝(I-KtHt)Pt|t-1；
其中，
Figure FDA0002489918450000018
表示EKF在t时刻预估的状态向量，
Figure FDA0002489918450000019
表示EKF由t-1时刻到t时刻预估的状态向量，Pt|t-1表示EKF由t-1时刻到t时刻的最小预测均方误差矩阵；Pt表示EKF t时刻的最小预测均方误差矩阵；Kt表示EKF在t时刻的误差增益矩阵；
Figure FDA00024899184500000110
表示单位阵；其Rt为νt的协方差矩阵；
如果t≥N，则进行EFIR滤波算法，其步骤如下：
Figure FDA0002489918450000021
Figure FDA0002489918450000022
为s时刻EFIR滤波器的状态向量，
Figure FDA0002489918450000023
为s时刻自适应EKF预估滤波器的状态向量；
利用中间变量l，另l在m+M时刻到t时刻进行下列迭代：
Figure FDA0002489918450000024
Figure FDA0002489918450000025
Figure FDA0002489918450000026
Figure FDA0002489918450000027
令
Figure FDA0002489918450000028
其中，Fl表示l时刻的系统矩阵，
Figure FDA0002489918450000029
表示EFIR在t时刻预估的状态向量，Kl表示EFIR在l时刻的误差增益矩阵；
Figure FDA00024899184500000210
为由l-1时刻到l时刻的状态向量预估、
Figure FDA00024899184500000211
为由l-1时刻的状态向量预估、Gl为l时刻的广义噪声功率增益、
Figure FDA00024899184500000212
最终得到当前时刻目标行人最优的导航信息。
2.如权利要求1所述的具有数据缺失UWB行人定位的EFIR滤波算法，其特征在于，所述EFIR滤波器的状态方程为：
Figure FDA00024899184500000213
其中，
Figure FDA00024899184500000214
和
Figure FDA00024899184500000215
分别为t和t-1时刻惯性导航器件INS的东方向位置、北方向位置、东方向速度、北方向速度；T为采样周期；ωt-1为t-1时刻的系统噪声。
3.如权利要求1所述的具有数据缺失UWB行人定位的EFIR滤波算法，其特征在于，所述EFIR滤波器的观测方程为：
Figure FDA0002489918450000031
其中，di,t,i∈(1,2,…,g)为t时刻UWB分别测量得的参考节点与未知节点之间的距离；g为参考节点的数目；x为UWB解算出的未知节点东方向位置，y为UWB解算出的未知节点北方向位置，xi,i∈(1,2,…,g)和yi,i∈(1,2,…,g)分别为参考节点1到i的东向位置和北向位置；νt为系统t时刻的观测噪声。
4.如权利要求1所述的具有数据缺失UWB行人定位的EFIR滤波算法，其特征在于，所述的预估过程中实时判断UWB测量得到的参考节点与未知节点之间的距离信息是否有缺失，如果有，对缺失的距离信息进行预估，具体为：
引入变量
Figure FDA0002489918450000032
Figure FDA0002489918450000033
表示UWB测量得到的参考节点与未知节点之间的第i个距离信息；如果第i个距离信息缺失，则重新对
Figure FDA0002489918450000034
进行预估；采用矩阵h(Xt|t-1)的第i行第1列替代缺失的距离信息。
5.如权利要求4所述的具有数据缺失UWB行人定位的EFIR滤波算法，其特征在于，对缺失数据进行预估后，EFIR滤波器的观测方程变为：
Figure FDA0002489918450000035
6.具有数据缺失UWB行人定位的EFIR滤波系统，其特征在于，包括服务器，所述服务器包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现以下步骤：
以惯性导航器件INS在t时刻在导航系下的东方向位置、北方向位置、东方向速度、北方向速度作为状态量，以INS与UWB分别测量的参考节点与未知节点之间距离的差值作为系统观测量，构建滤波模型；
利用EFIR滤波算法对位置误差进行预估，具体为：
定义两个变量m和s：
m＝t-N+1，s＝m+M-1；
判断当前时刻t是否大于EFIR滤波器的滤波窗口长度N，如果t＜N，则进行EKF滤波算法，其步骤如下：
首先，进行一步预估
Figure FDA0002489918450000041
Figure FDA0002489918450000042
判断距离信息是否可用，引入变量
Figure FDA0002489918450000043
若第i个距离信息不可用，则
Figure FDA0002489918450000044
对不可用的距离信息进行预估：
Figure FDA0002489918450000045
其中，h(Xt|t-1)(i,1)表示用矩阵h(Xt|t-1)的第i行第1列替代不可用的距离信息；
Figure FDA0002489918450000046
Figure FDA0002489918450000047
Pt＝(I-KtHt)Pt|t-1；
其中，
Figure FDA0002489918450000048
表示EKF在t时刻预估的状态向量，
Figure FDA0002489918450000049
表示EKF由t-1时刻到t时刻预估的状态向量，Pt|t-1表示EKF由t-1时刻到t时刻的最小预测均方误差矩阵；Pt表示EKF t时刻的最小预测均方误差矩阵；Kt表示EKF在t时刻的误差增益矩阵；
Figure FDA00024899184500000410
表示单位阵；其Rt为νt的协方差矩阵；
如果t≥N，则进行EFIR滤波算法，其步骤如下：
Figure FDA0002489918450000051
Figure FDA0002489918450000052
为s时刻EFIR滤波器的状态向量，
Figure FDA0002489918450000053
为s时刻自适应EKF预估滤波器的状态向量；
利用中间变量l，另l在m+M时刻到t时刻进行下列迭代：
Figure FDA0002489918450000054
Figure FDA0002489918450000055
Figure FDA0002489918450000056
Figure FDA0002489918450000057
令
Figure FDA0002489918450000058
其中，Fl表示l时刻的系统矩阵，
Figure FDA0002489918450000059
表示EFIR在t时刻预估的状态向量，Kl表示EFIR在l时刻的误差增益矩阵；
Figure FDA00024899184500000510
为由l-1时刻到l时刻的状态向量预估、
Figure FDA00024899184500000511
为由l-1时刻的状态向量预估、Gl为l时刻的广义噪声功率增益、
Figure FDA00024899184500000512
最终得到当前时刻目标行人最优的导航信息。
7.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时执行以下步骤：
以惯性导航器件INS在t时刻在导航系下的东方向位置、北方向位置、东方向速度、北方向速度作为状态量，以INS与UWB分别测量的参考节点与未知节点之间距离的差值作为系统观测量，构建滤波模型；
利用EFIR滤波算法对位置误差进行预估，具体为：
定义两个变量m和s：
m＝t-N+1，s＝m+M-1；
判断当前时刻t是否大于EFIR滤波器的滤波窗口长度N，如果t＜N，则进行EKF滤波算法，其步骤如下：
首先，进行一步预估
Figure FDA0002489918450000061
Figure FDA0002489918450000062
判断距离信息是否可用，引入变量
Figure FDA0002489918450000063
若第i个距离信息不可用，则
Figure FDA0002489918450000064
对不可用的距离信息进行预估：
Figure FDA0002489918450000065
其中，h(Xt|t-1)(i,1)表示用矩阵h(Xt|t-1)的第i行第1列替代不可用的距离信息；
Figure FDA0002489918450000066
Figure FDA0002489918450000067
Pt＝(I-KtHt)Pt|t-1；
其中，
Figure FDA0002489918450000068
表示EKF在t时刻预估的状态向量，
Figure FDA0002489918450000069
表示EKF由t-1时刻到t时刻预估的状态向量，Pt|t-1表示EKF由t-1时刻到t时刻的最小预测均方误差矩阵；Pt表示EKF t时刻的最小预测均方误差矩阵；Kt表示EKF在t时刻的误差增益矩阵；
Figure FDA00024899184500000614
表示单位阵；其Rt为νt的协方差矩阵；
如果t≥N，则进行EFIR滤波算法，其步骤如下：
Figure FDA00024899184500000610
Figure FDA00024899184500000611
为s时刻EFIR滤波器的状态向量，
Figure FDA00024899184500000612
为s时刻自适应EKF预估滤波器的状态向量；
利用中间变量l，另l在m+M时刻到t时刻进行下列迭代：
Figure FDA00024899184500000613
Figure FDA0002489918450000071
Figure FDA0002489918450000072
Figure FDA0002489918450000073
令
Figure FDA0002489918450000074
其中，Fl表示l时刻的系统矩阵，
Figure FDA0002489918450000075
表示EFIR在t时刻预估的状态向量，Kl表示EFIR在l时刻的误差增益矩阵；
Figure FDA0002489918450000076
为由l-1时刻到l时刻的状态向量预估、
Figure FDA0002489918450000077
为由l-1时刻的状态向量预估、Gl为l时刻的广义噪声功率增益、
Figure FDA0002489918450000078
最终得到当前时刻目标行人最优的导航信息。
CN201810886573.7A 2018-08-06 2018-08-06 具有数据缺失uwb行人定位的efir滤波算法及系统 Active CN109141413B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810886573.7A CN109141413B (zh) 2018-08-06 2018-08-06 具有数据缺失uwb行人定位的efir滤波算法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810886573.7A CN109141413B (zh) 2018-08-06 2018-08-06 具有数据缺失uwb行人定位的efir滤波算法及系统

Publications (2)

Publication Number Publication Date
CN109141413A CN109141413A (zh) 2019-01-04
CN109141413B true CN109141413B (zh) 2020-07-07

Family

ID=64791881

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810886573.7A Active CN109141413B (zh) 2018-08-06 2018-08-06 具有数据缺失uwb行人定位的efir滤波算法及系统

Country Status (1)

Country Link
CN (1) CN109141413B (zh)

Families Citing this family (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112788743B (zh) * 2019-11-11 2024-06-18 北京京邦达贸易有限公司 一种基于超宽带技术的定位方法和装置
CN112747747B (zh) * 2021-01-20 2022-10-11 重庆邮电大学 一种改进的uwb/imu融合室内行人定位方法
CN113218388B (zh) * 2021-03-02 2022-07-05 济南大学 一种考虑可变有色测量噪声的移动机器人定位方法及系统
CN113970331A (zh) * 2021-09-06 2022-01-25 济南大学 一种基于重构观测量的四旋翼定位方法及系统
CN114554392B (zh) * 2022-02-25 2023-05-16 新基线(江苏)科技有限公司 一种基于uwb与imu融合的多机器人协同定位方法
CN116321418B (zh) * 2023-03-02 2024-01-02 中国人民解放军国防科技大学 一种基于节点构型优选的集群无人机融合估计定位方法
CN116105731B (zh) * 2023-04-07 2023-06-20 中国人民解放军国防科技大学 稀疏测距条件下的导航方法、装置、计算机设备及介质

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107255795B (zh) * 2017-06-13 2020-01-07 山东大学 基于ekf/efir混合滤波的室内移动机器人定位方法和装置
CN107966143A (zh) * 2017-11-14 2018-04-27 济南大学 一种基于多窗口的自适应efir数据融合方法
CN107966142A (zh) * 2017-11-14 2018-04-27 济南大学 一种基于多窗口的室内行人自适应ufir数据融合方法

Also Published As

Publication number Publication date
CN109141413A (zh) 2019-01-04

Similar Documents

Publication Publication Date Title
CN109141413B (zh) 具有数据缺失uwb行人定位的efir滤波算法及系统
CN107255795B (zh) 基于ekf/efir混合滤波的室内移动机器人定位方法和装置
CN106680765B (zh) 基于分布式组合滤波ins/uwb行人导航系统及方法
CN109141412B (zh) 面向有数据缺失ins/uwb组合行人导航的ufir滤波算法及系统
CN111780755A (zh) 一种基于因子图和可观测度分析的多源融合导航方法
CN108759825B (zh) 面向有数据缺失INS/UWB行人导航的自适应预估Kalman滤波算法及系统
CN105509739A (zh) 采用固定区间crts平滑的ins/uwb紧组合导航系统及方法
CN107402375B (zh) 一种带观测时滞的室内行人定位efir数据融合系统及方法
CN109269498B (zh) 面向具有数据缺失uwb行人导航的自适应预估ekf滤波算法及系统
CN111964667B (zh) 一种基于粒子滤波算法的地磁_ins组合导航方法
CN109655060B (zh) 基于kf/fir和ls-svm融合的ins/uwb组合导航算法及系统
Atia et al. Particle‐Filter‐Based WiFi‐Aided Reduced Inertial Sensors Navigation System for Indoor and GPS‐Denied Environments
Yuanfeng et al. Flexible indoor localization and tracking system based on mobile phone
CN205384029U (zh) 采用固定区间crts平滑的ins/uwb紧组合导航系统
CN114440880B (zh) 基于自适应迭代ekf的施工现场控制点定位方法及系统
Nagui et al. Improved GPS/IMU loosely coupled integration scheme using two kalman filter-based cascaded stages
CN111578939B (zh) 考虑采样周期随机变化的机器人紧组合导航方法及系统
CN109916401B (zh) 采用ls-svm辅助ekf滤波方法的分布式无缝紧组合导航方法及系统
CN112965076A (zh) 一种用于机器人的多雷达定位系统及方法
CN110849349A (zh) 一种基于磁传感器与轮式里程计融合定位方法
CN114339595B (zh) 一种基于多模型预测的超宽带动态反演定位方法
CN113218388B (zh) 一种考虑可变有色测量噪声的移动机器人定位方法及系统
CN110879069A (zh) 一种面向UWB SLAM的Kalman/R-T-S混合定位方法及系统
Li et al. Multi-sensor fusion localization algorithm for outdoor mobile robot
Gu et al. Trajectory initialization foot-mounted IMU and calibration using a and UWB anchors

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
GR01 Patent grant
GR01 Patent grant