CN109141412B - 面向有数据缺失ins/uwb组合行人导航的ufir滤波算法及系统 - Google Patents

面向有数据缺失ins/uwb组合行人导航的ufir滤波算法及系统 Download PDF

Info

Publication number
CN109141412B
CN109141412B CN201810886557.8A CN201810886557A CN109141412B CN 109141412 B CN109141412 B CN 109141412B CN 201810886557 A CN201810886557 A CN 201810886557A CN 109141412 B CN109141412 B CN 109141412B
Authority
CN
China
Prior art keywords
time
ufir
distance information
missing
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
CN201810886557.8A
Other languages
English (en)
Other versions
CN109141412A (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 CN201810886557.8A priority Critical patent/CN109141412B/zh
Publication of CN109141412A publication Critical patent/CN109141412A/zh
Application granted granted Critical
Publication of CN109141412B publication Critical patent/CN109141412B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12Navigation; 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/16Navigation; 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/165Navigation; 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
    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20Instruments for performing navigational calculations
    • G01C21/206Instruments 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

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

Description

面向有数据缺失INS/UWB组合行人导航的UFIR滤波算法及 系统
技术领域
本发明涉及复杂环境下组合定位技术领域,尤其涉及面向有数据缺失INS/UWB组合行人导航的UFIR滤波算法及系统。
背景技术
近年来,行人导航(Pedestrian Navigation,PN)作为导航技术应用的新兴领域,正越来越受到各国学者的重视,并逐渐成为该领域的研究热点。然而在隧道、大型仓库、地下停车场等室内环境下,外界无线电信号微弱、电磁干扰强烈等因素都会对目标行人导航信息获取的准确性、实时性及鲁棒性有很大影响。如何将室内环境下获取的有限信息进行有效的融合以消除室内复杂环境影响,保证行人导航精度的持续稳定,具有重要的科学理论意义和实际应用价值。
在现有的定位方式中,全球卫星导航系统(Global Navigation SatelliteSystem,GNSS)是最为常用的一种方式。虽然GNSS能够通过精度持续稳定的位置信息,但是其易受电磁干扰、遮挡等外界环境影响的缺点限制了其应用范围,特别是在室内、地下巷道等一些密闭的、环境复杂的场景,GNSS信号被严重遮挡,无法进行有效的工作。近年来,UWB(Ultra Wideband)以其在复杂环境下定位精度高的特点在短距离局部定位领域表现出很大的潜力。学者们提出将基于UWB的目标跟踪应用于GNSS失效环境下的行人导航。这种方式虽然能够实现室内定位,但是由于室内环境复杂多变,UWB信号十分容易受到干扰而导致定位精度下降甚至失锁;与此同时,由于UWB采用的通信技术通常为短距离无线通信技术,因此若想完成大范围的室内目标跟踪定位,需要大量的网络节点共同完成,这必将引入网络组织结构优化设计、多节点多簇网络协同通信等一系列问题。因此现阶段基于UWB的目标跟踪在室内导航领域仍旧面临很多挑战。
发明内容
本发明的目的就是为了解决由于在实时系统中UWB由于受到室内环境的影响不能得到正常的距离信息的问题,提出了一种面向有数据缺失INS/UWB组合行人导航的UFIR滤波算法及系统,该方法对传统的UFIR滤波算法进行改进,首先判断UWB测量得到的距离信息是否有缺失,如果第i个距离信息缺失,则对第i个距离信息进行预估,以保证滤波器的正常运行,最终得到当前时刻最优的行人位置预估。
为实现上述目的,本发明的具体方案如下:
本发明的第一目的是公开一种面向有数据缺失INS/UWB组合行人导航的UFIR滤波算法,包括:
以惯性导航器件INS在t时刻在导航系下的位置误差、速度误差、姿态误差、加速度误差和角速度误差作为状态量,以INS与UWB分别测量的目标节点与参考节点之间距离的差值作为系统观测量,构建滤波模型;
利用UFIR滤波算法对位置误差进行预估,预估过程中实时判断UWB测量得到的目标节点与参考节点之间的距离信息是否有缺失,如果有,对缺失的距离信息进行预估;
最终得到当前时刻目标行人最优的导航信息。
进一步地,所述UFIR滤波器的状态方程为:
Figure GDA0002591835620000021
其中,
Figure GDA0002591835620000022
Figure GDA0002591835620000023
分别为t和t-1时刻惯性导航系统INS在导航系下的姿态、速度、位置误差向量以及在载体系下的加速度误差和角速度误差;T为采样周期;
Figure GDA0002591835620000024
为从载体系到导航系的姿态转移矩阵;wt-1为t-1时刻的系统噪声;
Figure GDA0002591835620000025
为惯性导航系统INS测量得到的导航坐标系下t时刻的东向、北向和天向加速度。
进一步地,所述从载体系到导航系的姿态转移矩阵
Figure GDA0002591835620000026
具体为:
Figure GDA0002591835620000027
其中,(γ ψ θ)为t时刻载体的横摇、纵摇和航向角。
进一步地,所述UFIR滤波器的观测方程为:
Figure GDA0002591835620000031
其中,δdi,t,i∈(1,2,...,g)为t时刻惯性导航器件INS和UWB分别测量得到的目标节点与参考节点之间的距离的差;g为参考节点的数目;
Figure GDA0002591835620000032
为t时刻惯性导航器件INS测量得到的第i个目标节点与参考节点之间的距离;
Figure GDA0002591835620000033
为t时刻UWB测量得到的第i个目标节点与参考节点之间的距离;νt为系统t时刻的观测噪声;
Figure GDA0002591835620000034
PE,t为t时刻的东向位置,PN,t为t时刻的北向位置。
进一步地,所述的预估过程中实时判断UWB测量得到的目标节点与参考节点之间的距离信息是否有缺失,如果有,对缺失的距离信息进行预估,具体为:
引入变量
Figure GDA0002591835620000035
表示UWB测量得到的目标节点与参考节点之间的第i个距离信息;如果第i个距离信息缺失,则重新对
Figure GDA0002591835620000036
进行预估;采用矩阵HtXt|t-1的第i行第1列替代缺失的距离信息。
进一步地,对缺失数据进行预估后,UFIR滤波器的观测方程变为:
Figure GDA0002591835620000037
其中,Ft-1为t-1时刻的系统矩阵。
进一步地,所述利用UFIR滤波算法对位置误差进行预估,具体为:
定义两个变量m和s:
m=t-N+1,s=m+M-1;
判断当前时刻t是否大于UFIR滤波器的滤波窗口长度N,如果t<N,则进行Kalman滤波算法,其步骤如下:
首先,进行一步预估
Figure GDA0002591835620000041
Figure GDA0002591835620000042
判断距离信息是否可用,引入变量
Figure GDA0002591835620000043
若第i个距离信息不可用,则
Figure GDA0002591835620000044
对不可用的距离信息进行预估
Figure GDA0002591835620000045
其中,HtXt|t-1(i,1)表示用矩阵HtXt|t-1的第i行第1列替代不可用的距离信息;
Figure GDA0002591835620000046
Figure GDA0002591835620000047
Pt=(I-KtHt)Pt|t-1
其中,
Figure GDA0002591835620000048
表示KF在t时刻预估的状态向量,
Figure GDA0002591835620000049
表示KF由t-1时刻到t时刻预估的状态向量,Ft表示t时刻的系统矩阵,Pt|t-1表示KF由t-1时刻到t时刻的最小预测均方误差矩阵;Pt表示KF在t时刻的最小预测均方误差矩阵;Kt表示KF在t时刻的误差增益矩阵;I表示单位阵;Rt为νt的协方差矩阵;
如果t≥N,则进行UFIR滤波算法,其步骤如下:
Figure GDA00025918356200000410
Figure GDA00025918356200000411
为s时刻UFIR滤波器的状态向量,
Figure GDA00025918356200000412
为s时刻自适应Kalman预估滤波器的状态向量;
利用中间变量l,另l在m+M时刻到t时刻进行下列迭代:
Figure GDA0002591835620000051
Figure GDA0002591835620000052
Figure GDA0002591835620000053
Figure GDA0002591835620000054
Figure GDA0002591835620000055
其中,Fl表示l时刻的系统矩阵,
Figure GDA0002591835620000056
表示UFIR在t时刻预估的状态向量,Kl表示UFIR在l时刻的误差增益矩阵;
Figure GDA0002591835620000057
为由l-1时刻到l时刻的状态向量预估、
Figure GDA0002591835620000058
为由l-1时刻的状态向量预估、Gl为l时刻的广义噪声功率增益、
Figure GDA0002591835620000059
本发明的第二目的是公开一种面向有数据缺失INS/UWB组合行人导航的UFIR滤波系统,包括服务器,所述服务器包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序,所述处理器执行所述程序时实现以下步骤:
以惯性导航器件INS在t时刻在导航系下的位置误差、速度误差、姿态误差、加速度误差和角速度误差作为状态量,以INS与UWB分别测量的目标节点与参考节点之间距离的差值作为系统观测量,构建滤波模型;
利用UFIR滤波算法对位置误差进行预估,预估过程中实时判断UWB测量得到的目标节点与参考节点之间的距离信息是否有缺失,如果有,对缺失的距离信息进行预估;
最终得到当前时刻目标行人最优的导航信息。
本发明的第三目的是公开一种计算机可读存储介质,其上存储有计算机程序,该程序被处理器执行时执行以下步骤:
以惯性导航器件INS在t时刻在导航系下的位置误差、速度误差、姿态误差、加速度误差和角速度误差作为状态量,以INS与UWB分别测量的目标节点与参考节点之间距离的差值作为系统观测量,构建滤波模型;
利用UFIR滤波算法对位置误差进行预估,预估过程中实时判断UWB测量得到的目标节点与参考节点之间的距离信息是否有缺失,如果有,对缺失的距离信息进行预估;
最终得到当前时刻目标行人最优的导航信息。
本发明的有益效果:
1、通过引入变量
Figure GDA0002591835620000061
表示第i个信道的UWB距离信息是否可用,若第i个距离信息不可用,则
Figure GDA0002591835620000062
,对不可用的距离信息进行预估,弥补UWB距离信息不可用导致的数据融合算法不可用的问题。
2、可用于室内环境下的中高精度定位。
附图说明
图1为一种面向有数据缺失INS/UWB组合行人导航的UFIR滤波算法的实现系统示意图;
图2为本发明构建滤波模型进行数据融合示意图;
图3为面向有数据缺失INS/UWB组合行人导航的UFIR滤波算法流程图。
具体实施方式:
下面结合附图对本发明进行详细说明:
本发明一种具有数据缺失INS/UWB紧组合行人导航的UFIR滤波算法的实现系统如图1所示,包括:组合导航算法采用UWB和INS两种导航系统,其中,UWB包括UWB参考节点和UWB定位标签,UWB参考节点预先固定在已知坐标上,UWB定位标签固定在目标行人上。INS主要由固定在目标行人足部的IMU组成。
基于上述系统,本发明公开了具有数据缺失INS/UWB紧组合行人导航的UFIR滤波算法,包括:
(1)如图2所示,以惯性导航器件INS在t时刻在导航系下的位置误差、速度误差、姿态误差、加速度误差和角速度误差作为状态量,以INS与UWB分别测量的目标节点与参考节点之间距离的差值作为系统观测量,构建滤波模型进行数据融合;
(2)利用UFIR滤波算法对位置误差进行预估,UFIR预估滤波器的状态方程为:
Figure GDA0002591835620000071
其中,
Figure GDA0002591835620000072
Figure GDA0002591835620000073
分别为t和t-1时刻惯性导航系统INS在导航系下的姿态、速度、位置误差向量以及在载体系下的加速度误差和角速度误差;T为采样周期;
Figure GDA0002591835620000074
为从载体系到导航系的姿态转移矩阵;wk为t时刻的系统噪声;
Figure GDA0002591835620000075
其中,(γ ψ θ)为t时刻的横摇、纵摇和航向角;
Figure GDA0002591835620000076
其中,
Figure GDA0002591835620000077
为t时刻的东向、北向和天向加速度;
UFIR预估滤波器的的观测方程为:
Figure GDA0002591835620000078
其中,δdi,t,i∈(1,2,...,g)为t时刻惯性导航器件INS和UWB分别测量得到的目标节点与参考节点之间的距离的差;g为参考节点的数目;
Figure GDA0002591835620000079
i∈[1,g]为t时刻惯性导航器件INS测量得到的目标节点与参考节点之间的距离;
Figure GDA00025918356200000710
i∈[1,g]为t时刻UWB测量得到的目标节点与参考节点之间的距离;
Figure GDA00025918356200000711
其中,(δPE,t,δPN,t)为东向和北向的位置误差;νt为系统t时刻的观测噪声,其协方差矩阵为Rt。在此基础上,判断一下距离信息是否可用,引入变量
Figure GDA0002591835620000081
若第i个距离信息不可用,则
Figure GDA0002591835620000082
对不可用的距离信息进行预估
Figure GDA0002591835620000083
进一步地,在t时刻UFIR滤波算法的步骤如下:
首先定义两个变量m和s如下:
m=t-N+1,s=m+M-1
判断当前时刻t是否大于UFIR滤波器的滤波窗口长度N,如果t<N,则进行Kalman滤波算法,其步骤如下:
首先,进行一步预估
Figure GDA0002591835620000084
Figure GDA0002591835620000085
判断距离信息是否可用,引入变量
Figure GDA0002591835620000086
若第i个距离信息不可用,则
Figure GDA0002591835620000087
对不可用的距离信息进行预估
Figure GDA0002591835620000088
其中,HtXt|t-1(i,1)表示用矩阵HtXt|t-1的第i行第1列替代不可用的距离信息。
Figure GDA0002591835620000089
Figure GDA00025918356200000810
Pt=(I-KtHt)Pt|t-1
其中,
Figure GDA00025918356200000811
表示自适应Kalman预估滤波器在t时刻预估的状态向量,
Figure GDA00025918356200000812
表示KF由t-1时刻到t时刻预估的状态向量,Ft-1表示t-1时刻的系统矩阵,Pt|t-1表示KF由t-1时刻到t时刻的最小预测均方误差矩阵;Pt表示自适应Kalman预估滤波器在t时刻的最小预测均方误差矩阵;Kt表示自适应Kalman预估滤波器在t时刻的误差增益矩阵;I表示单位阵。
如果t≥N,则进行UFIR滤波算法,其步骤如下:
Figure GDA0002591835620000091
利用中间变量l,另l在m+M时刻到t时刻进行下列迭代:
Figure GDA0002591835620000092
Figure GDA0002591835620000093
Figure GDA0002591835620000094
Figure GDA0002591835620000095
Figure GDA0002591835620000096
其中,Fl表示l时刻的系统矩阵,
Figure GDA0002591835620000097
表示UFIR在t时刻预估的状态向量,Kl表示UFIR在l时刻的误差增益矩阵;
Figure GDA0002591835620000098
为由l-1时刻到l时刻的状态向量预估、
Figure GDA0002591835620000099
为由l-1时刻的状态向量预估、Gl为l时刻的广义噪声功率增益、
Figure GDA00025918356200000910
上述虽然结合附图对本发明的具体实施方式进行了描述,但并非对本发明保护范围的限制,所属领域技术人员应该明白,在本发明的技术方案的基础上,本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。

Claims (8)

1.面向有数据缺失INS/UWB组合行人导航的UFIR滤波算法,其特征在于,包括:
以惯性导航器件INS在t时刻在导航系下的位置误差、速度误差、姿态误差、加速度误差和角速度误差作为状态量,以INS与UWB分别测量的目标节点与参考节点之间距离的差值作为系统观测量,构建滤波模型;
利用UFIR滤波算法对位置误差进行预估,预估过程中实时判断UWB测量得到的目标节点与参考节点之间的距离信息是否有缺失,如果有,对缺失的距离信息进行预估,具体为:
引入变量
Figure FDA0002591835610000011
Figure FDA0002591835610000012
表示UWB测量得到的目标节点与参考节点之间的第i个距离信息;如果第i个距离信息缺失,则重新对
Figure FDA0002591835610000013
进行预估;采用矩阵HtXt|t-1的第i行第1列替代缺失的距离信息;
最终得到当前时刻目标行人最优的导航信息。
2.如权利要求1所述的面向有数据缺失INS/UWB组合行人导航的UFIR滤波算法,其特征在于,所述UFIR滤波器的状态方程为:
Figure FDA0002591835610000014
其中,
Figure FDA0002591835610000015
Figure FDA0002591835610000016
分别为t和t-1时刻惯性导航系统INS在导航系下的姿态、速度、位置误差向量以及在载体系下的加速度误差和角速度误差;T为采样周期;
Figure FDA0002591835610000017
为从载体系到导航系的姿态转移矩阵;ωt-1为t-1时刻的系统噪声;
Figure FDA0002591835610000018
Figure FDA0002591835610000019
为惯性导航系统INS测量得到的导航坐标系下t时刻的东向、北向和天向加速度。
3.如权利要求2所述的面向有数据缺失INS/UWB组合行人导航的UFIR滤波算法,其特征在于,所述从载体系到导航系的姿态转移矩阵
Figure FDA00025918356100000110
具体为:
Figure FDA00025918356100000111
其中,(γψθ)为t时刻载体的横摇、纵摇和航向角。
4.如权利要求1所述的面向有数据缺失INS/UWB组合行人导航的UFIR滤波算法,其特征在于,所述UFIR滤波器的观测方程为:
Figure FDA0002591835610000021
其中,δdi,t,i∈(1,2,...,g)为t时刻惯性导航器件INS和UWB分别测量得到的目标节点与参考节点之间的距离的差;g为参考节点的数目;
Figure FDA0002591835610000022
为t时刻惯性导航器件INS测量得到的第i个目标节点与参考节点之间的距离;
Figure FDA0002591835610000023
为t时刻UWB测量得到的第i个目标节点与参考节点之间的距离;νt为系统t时刻的观测噪声;
Figure FDA0002591835610000024
PE,t为t时刻的东向位置,PN,t为t时刻的北向位置。
5.如权利要求1所述的面向有数据缺失INS/UWB组合行人导航的UFIR滤波算法,其特征在于,对缺失数据进行预估后,UFIR滤波器的观测方程变为:
Figure FDA0002591835610000025
其中,Ft-1为t-1时刻的系统矩阵。
6.如权利要求1所述的面向有数据缺失INS/UWB组合行人导航的UFIR滤波算法,其特征在于,所述利用UFIR滤波算法对位置误差进行预估,具体为:
定义两个变量m和s:
m=t-N+1,s=m+M-1;
判断当前时刻t是否大于UFIR滤波器的滤波窗口长度N,如果t<N,则进行Kalman滤波算法,其步骤如下:
首先,进行一步预估
Figure FDA0002591835610000031
Figure FDA0002591835610000032
判断距离信息是否可用,引入变量
Figure FDA0002591835610000033
若第i个距离信息不可用,则
Figure FDA0002591835610000034
对不可用的距离信息进行预估
Figure FDA0002591835610000035
其中,HtXt|t-1(i,1)表示用矩阵HtXt|t-1的第i行第1列替代不可用的距离信息;
Figure FDA0002591835610000036
Figure FDA0002591835610000037
Pt=(I-KtHt)Pt|t-1
其中,
Figure FDA0002591835610000038
表示KF在t时刻预估的状态向量,
Figure FDA0002591835610000039
表示KF由t-1时刻到t时刻预估的状态向量,Ft表示t时刻的系统矩阵,Pt|t-1表示KF由t-1时刻到t时刻的最小预测均方误差矩阵;Pt表示KF在t时刻的最小预测均方误差矩阵;Kt表示KF在t时刻的误差增益矩阵;I表示单位阵;Rt为νt的协方差矩阵;
如果t≥N,则进行UFIR滤波算法,其步骤如下:
Figure FDA00025918356100000310
Figure FDA00025918356100000311
为s时刻UFIR滤波器的状态向量,
Figure FDA00025918356100000312
为s时刻自适应Kalman预估滤波器的状态向量;
利用中间变量l,另l在m+M时刻到t时刻进行下列迭代:
Figure FDA0002591835610000041
Figure FDA0002591835610000042
Figure FDA0002591835610000043
Figure FDA0002591835610000044
Figure FDA0002591835610000045
其中,Fl表示l时刻的系统矩阵,
Figure FDA0002591835610000046
表示UFIR在t时刻预估的状态向量,Kl表示UFIR在l时刻的误差增益矩阵;
Figure FDA0002591835610000047
为由l-1时刻到l时刻的状态向量预估、
Figure FDA0002591835610000048
为由l-1时刻的状态向量预估、Gl为l时刻的广义噪声功率增益、
Figure FDA0002591835610000049
7.面向有数据缺失INS/UWB组合行人导航的UFIR滤波系统,其特征在于,包括服务器,所述服务器包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序,所述处理器执行所述程序时实现以下步骤:
以惯性导航器件INS在t时刻在导航系下的位置误差、速度误差、姿态误差、加速度误差和角速度误差作为状态量,以INS与UWB分别测量的目标节点与参考节点之间距离的差值作为系统观测量,构建滤波模型;
利用UFIR滤波算法对位置误差进行预估,预估过程中实时判断UWB测量得到的目标节点与参考节点之间的距离信息是否有缺失,如果有,对缺失的距离信息进行预估,具体为:
引入变量
Figure FDA00025918356100000410
Figure FDA00025918356100000411
表示UWB测量得到的目标节点与参考节点之间的第i个距离信息;如果第i个距离信息缺失,则重新对
Figure FDA00025918356100000412
进行预估;采用矩阵HtXt|t-1的第i行第1列替代缺失的距离信息;
最终得到当前时刻目标行人最优的导航信息。
8.一种计算机可读存储介质,其上存储有计算机程序,其特征在于,该程序被处理器执行时执行以下步骤:
以惯性导航器件INS在t时刻在导航系下的位置误差、速度误差、姿态误差、加速度误差和角速度误差作为状态量,以INS与UWB分别测量的目标节点与参考节点之间距离的差值作为系统观测量,构建滤波模型;
利用UFIR滤波算法对位置误差进行预估,预估过程中实时判断UWB测量得到的目标节点与参考节点之间的距离信息是否有缺失,如果有,对缺失的距离信息进行预估,具体为:
引入变量
Figure FDA0002591835610000051
Figure FDA0002591835610000052
表示UWB测量得到的目标节点与参考节点之间的第i个距离信息;如果第i个距离信息缺失,则重新对
Figure FDA0002591835610000053
进行预估;采用矩阵HtXt|t-1的第i行第1列替代缺失的距离信息;
最终得到当前时刻目标行人最优的导航信息。
CN201810886557.8A 2018-08-06 2018-08-06 面向有数据缺失ins/uwb组合行人导航的ufir滤波算法及系统 Active CN109141412B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810886557.8A CN109141412B (zh) 2018-08-06 2018-08-06 面向有数据缺失ins/uwb组合行人导航的ufir滤波算法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810886557.8A CN109141412B (zh) 2018-08-06 2018-08-06 面向有数据缺失ins/uwb组合行人导航的ufir滤波算法及系统

Publications (2)

Publication Number Publication Date
CN109141412A CN109141412A (zh) 2019-01-04
CN109141412B true CN109141412B (zh) 2020-09-22

Family

ID=64791995

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810886557.8A Active CN109141412B (zh) 2018-08-06 2018-08-06 面向有数据缺失ins/uwb组合行人导航的ufir滤波算法及系统

Country Status (1)

Country Link
CN (1) CN109141412B (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110414173B (zh) * 2019-08-06 2023-04-18 上海智驾汽车科技有限公司 一种基于ufir滤波器的交叉路口车辆状态估计方法
CN111238486B (zh) * 2020-03-12 2021-11-19 北京三快在线科技有限公司 用于无人设备的导航方法、装置、存储介质及无人设备
CN112833876B (zh) * 2020-12-30 2022-02-11 西南科技大学 一种融合里程计与uwb的多机器人协作定位方法
CN116718153B (zh) * 2023-08-07 2023-10-27 成都云智北斗科技有限公司 一种基于gnss和ins的形变监测方法及系统

Family Cites Families (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103471595B (zh) * 2013-09-26 2016-04-06 东南大学 一种面向ins/wsn室内移动机器人紧组合导航的迭代扩展rts均值滤波方法
CN104864865B (zh) * 2015-06-01 2017-09-22 济南大学 一种面向室内行人导航的ahrs/uwb无缝组合导航方法
CN105928518B (zh) * 2016-04-14 2018-10-19 济南大学 采用伪距和位置信息的室内行人uwb/ins紧组合导航系统及方法
CN106680765B (zh) * 2017-03-03 2024-02-20 济南大学 基于分布式组合滤波ins/uwb行人导航系统及方法
CN107255795B (zh) * 2017-06-13 2020-01-07 山东大学 基于ekf/efir混合滤波的室内移动机器人定位方法和装置
CN107402375B (zh) * 2017-08-08 2020-05-05 济南大学 一种带观测时滞的室内行人定位efir数据融合系统及方法
CN107966143A (zh) * 2017-11-14 2018-04-27 济南大学 一种基于多窗口的自适应efir数据融合方法

Also Published As

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

Similar Documents

Publication Publication Date Title
CN109141413B (zh) 具有数据缺失uwb行人定位的efir滤波算法及系统
CN109141412B (zh) 面向有数据缺失ins/uwb组合行人导航的ufir滤波算法及系统
CN111780755B (zh) 一种基于因子图和可观测度分析的多源融合导航方法
CN110243358B (zh) 多源融合的无人车室内外定位方法及系统
CN108759825B (zh) 面向有数据缺失INS/UWB行人导航的自适应预估Kalman滤波算法及系统
CN108731670B (zh) 基于量测模型优化的惯性/视觉里程计组合导航定位方法
CN107255795B (zh) 基于ekf/efir混合滤波的室内移动机器人定位方法和装置
CN105509739A (zh) 采用固定区间crts平滑的ins/uwb紧组合导航系统及方法
CN103471595B (zh) 一种面向ins/wsn室内移动机器人紧组合导航的迭代扩展rts均值滤波方法
CN111880207B (zh) 一种基于小波神经网络的视觉惯性卫星紧耦合定位方法
CN108387236B (zh) 一种基于扩展卡尔曼滤波的偏振光slam方法
CN112697138B (zh) 一种基于因子图优化的仿生偏振同步定位与构图的方法
CN109269498B (zh) 面向具有数据缺失uwb行人导航的自适应预估ekf滤波算法及系统
CN113252033A (zh) 基于多传感器融合的定位方法、定位系统及机器人
CN112797985A (zh) 基于加权扩展卡尔曼滤波的室内定位方法及室内定位系统
CN109655060B (zh) 基于kf/fir和ls-svm融合的ins/uwb组合导航算法及系统
CN114440880B (zh) 基于自适应迭代ekf的施工现场控制点定位方法及系统
CN111964667A (zh) 一种基于粒子滤波算法的地磁_ins组合导航方法
CN114915913A (zh) 一种基于滑窗因子图的uwb-imu组合室内定位方法
CN104296741A (zh) 采用距离平方和距离平方变化率的wsn/ahrs紧组合方法
CN111578939B (zh) 考虑采样周期随机变化的机器人紧组合导航方法及系统
CN110849349A (zh) 一种基于磁传感器与轮式里程计融合定位方法
CN117320148A (zh) 多源数据融合定位方法、系统、电子设备及存储介质
CN114459474B (zh) 一种基于因子图的惯性/偏振/雷达/光流紧组合导航的方法
CN109916401B (zh) 采用ls-svm辅助ekf滤波方法的分布式无缝紧组合导航方法及系统

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