本发明公开了具有数据缺失UWB行人定位的EFIR滤波算法及系统，包括：通过UWB系统和惯性导航器件INS系统分别测量参考节点到目标节点之间的距离；在此基础上，将两种系统测量得到的距离信息作差，差值作为数据融合算法所使用的滤波模型的观测量；在此基础上，对传统的EFIR滤波算法进行改进，引入变量表示第i个信道的距离信息是否可用。一旦距离信息不可用，则对不可用的距离信息进行预估，以弥补不可用的距离信息，保证滤波器对位置误差的预估；在此基础上，将惯性导航器件INS测量得到的行人位置与EFIR滤波器得到的位置误差预估作差，最终得到当前时刻最优的行人位置预估。
1.具有数据缺失UWB行人定位的EFIR滤波算法，其特征在于，包括：以惯性导航器件INS在t时刻在导航系下的东方向位置、北方向位置、东方向速度、北方向速度作为状态量，以INS与UWB分别测量的参考节点与未知节点之间距离的差值作为系统观测量，构建滤波模型；
利用EFIR滤波算法对位置误差进行预估，具体为：定义两个变量m和s：
判断当前时刻t是否大于EFIR滤波器的滤波窗口长度N，如果t＜N，则进行EKF滤波算法，其步骤如下：首先，进行一步预估
判断距离信息是否可用，引入变量 若第i个距离信息不可用，则 对不可用的距离信息进行预估：其中，h(Xt|t-1)(i,1)表示用矩阵h(Xt|t-1)的第i行第1列替代不可用的距离信息；
其中， 表示EKF在t时刻预估的状态向量， 表示EKF由t-1时刻到t时刻预估的状态向量，Pt|t-1表示EKF由t-1时刻到t时刻的最小预测均方误差矩阵；Pt表示EKF t时刻的最小预测均方误差矩阵；Kt表示EKF在t时刻的误差增益矩阵；表示单位阵；其Rt为νt的协方差矩阵；
如果t≥N，则进行EFIR滤波算法，其步骤如下：为s时刻EFIR滤波器的状态向量， 为s时刻自适应EKF预估滤波器的状态向量；
利用中间变量l，另l在m+M时刻到t时刻进行下列迭代：令
其中，Fl表示l时刻的系统矩阵， 表示EFIR在t时刻预估的状态向量，Kl表示EFIR在l时刻的误差增益矩阵；
为由l-1时刻到l时刻的状态向量预估、 为由l-1时刻的状态向量预估、Gl为l时刻的广义噪声功率增益、最终得到当前时刻目标行人最优的导航信息。
2.如权利要求1所述的具有数据缺失UWB行人定位的EFIR滤波算法，其特征在于，所述EFIR滤波器的状态方程为：其中， 和 分别为t和t-1时刻惯性导航器件
INS的东方向位置、北方向位置、东方向速度、北方向速度；T为采样周期；ωt-1为t-1时刻的系统噪声。
3.如权利要求1所述的具有数据缺失UWB行人定位的EFIR滤波算法，其特征在于，所述EFIR滤波器的观测方程为：其中，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测量得到的参考节点与未知节点之间的距离信息是否有缺失，如果有，对缺失的距离信息进行预估，具体为：引入变量 表示UWB测量得到的参考节点与未知节点之间的第i个距离信息；如果第i个距离信息缺失，则重新对 进行预估；采用矩阵h(Xt|t-1)的第i行第1列替代缺失的距离信息。
5.如权利要求4所述的具有数据缺失UWB行人定位的EFIR滤波算法，其特征在于，对缺失数据进行预估后，EFIR滤波器的观测方程变为：
6.具有数据缺失UWB行人定位的EFIR滤波系统，其特征在于，包括服务器，所述服务器包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现以下步骤：以惯性导航器件INS在t时刻在导航系下的东方向位置、北方向位置、东方向速度、北方向速度作为状态量，以INS与UWB分别测量的参考节点与未知节点之间距离的差值作为系统观测量，构建滤波模型；
利用EFIR滤波算法对位置误差进行预估，具体为：定义两个变量m和s：
判断当前时刻t是否大于EFIR滤波器的滤波窗口长度N，如果t＜N，则进行EKF滤波算法，其步骤如下：首先，进行一步预估
判断距离信息是否可用，引入变量 若第i个距离信息不可用，则 对不可用的距离信息进行预估：其中，h(Xt|t-1)(i,1)表示用矩阵h(Xt|t-1)的第i行第1列替代不可用的距离信息；
其中， 表示EKF在t时刻预估的状态向量， 表示EKF由t-1时刻到t时刻预估的状态向量，Pt|t-1表示EKF由t-1时刻到t时刻的最小预测均方误差矩阵；Pt表示EKF t时刻的最小预测均方误差矩阵；Kt表示EKF在t时刻的误差增益矩阵；表示单位阵；其Rt为νt的协方差矩阵；
如果t≥N，则进行EFIR滤波算法，其步骤如下：为s时刻EFIR滤波器的状态向量， 为s时刻自适应EKF预估滤波器的状态向量；
利用中间变量l，另l在m+M时刻到t时刻进行下列迭代：令
其中，Fl表示l时刻的系统矩阵， 表示EFIR在t时刻预估的状态向量，Kl表示EFIR在l时刻的误差增益矩阵；
为由l-1时刻到l时刻的状态向量预估、 为由l-1时刻的状态向量预估、Gl为l时刻的广义噪声功率增益、最终得到当前时刻目标行人最优的导航信息。
7.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时执行以下步骤：以惯性导航器件INS在t时刻在导航系下的东方向位置、北方向位置、东方向速度、北方向速度作为状态量，以INS与UWB分别测量的参考节点与未知节点之间距离的差值作为系统观测量，构建滤波模型；
利用EFIR滤波算法对位置误差进行预估，具体为：定义两个变量m和s：
判断当前时刻t是否大于EFIR滤波器的滤波窗口长度N，如果t＜N，则进行EKF滤波算法，其步骤如下：首先，进行一步预估
判断距离信息是否可用，引入变量 若第i个距离信息不可用，则 对不可用的距离信息进行预估：其中，h(Xt|t-1)(i,1)表示用矩阵h(Xt|t-1)的第i行第1列替代不可用的距离信息；
其中， 表示EKF在t时刻预估的状态向量， 表示EKF由t-1时刻到t时刻预估的状态向量，Pt|t-1表示EKF由t-1时刻到t时刻的最小预测均方误差矩阵；Pt表示EKF t时刻的最小预测均方误差矩阵；Kt表示EKF在t时刻的误差增益矩阵；表示单位阵；其Rt为νt的协方差矩阵；
如果t≥N，则进行EFIR滤波算法，其步骤如下：为s时刻EFIR滤波器的状态向量， 为s时刻自适应EKF预估滤波器的状态向量；
利用中间变量l，另l在m+M时刻到t时刻进行下列迭代：令
其中，Fl表示l时刻的系统矩阵， 表示EFIR在t时刻预估的状态向量，Kl表示EFIR在l时刻的误差增益矩阵；
为由l-1时刻到l时刻的状态向量预估、 为由l-1时刻的状态向量预估、Gl为l时刻的广义噪声功率增益、最终得到当前时刻目标行人最优的导航信息。
具有数据缺失UWB行人定位的EFIR滤波算法及系统
技术领域
[0001] 本发明涉及复杂环境下组合定位技术领域，尤其涉及具有数据缺失UWB行人定位的EFIR滤波算法及系统。
背景技术
[0002] 近年来，行人导航(Pedestrian Navigation,PN)作为导航技术应用的新兴领域，正越来越受到各国学者的重视，并逐渐成为该领域的研究热点。然而在隧道、大型仓库、地下停车场等室内环境下，外界无线电信号微弱、电磁干扰强烈等因素都会对目标行人导航信息获取的准确性、实时性及鲁棒性有很大影响。如何将室内环境下获取的有限信息进行有效的融合以消除室内复杂环境影响，保证行人导航精度的持续稳定，具有重要的科学理论意义和实际应用价值。
[0003] 在现有的定位方式中，全球卫星导航系统(Global Navigation Satellite System,GNSS)是最为常用的一种方式。虽然GNSS能够通过精度持续稳定的位置信息，但是其易受电磁干扰、遮挡等外界环境影响的缺点限制了其应用范围，特别是在室内、地下巷道等一些密闭的、环境复杂的场景，GNSS信号被严重遮挡，无法进行有效的工作。近年来，UWB(Ultra Wideband)以其在复杂环境下定位精度高的特点在短距离局部定位领域表现出很大的潜力。学者们提出将基于UWB的目标跟踪应用于GNSS失效环境下的行人导航。这种方式虽然能够实现室内定位，但是由于室内环境复杂多变，UWB信号十分容易受到干扰而导致定位精度下降甚至失锁；与此同时，由于UWB采用的通信技术通常为短距离无线通信技术，因此若想完成大范围的室内目标跟踪定位，需要大量的网络节点共同完成，这必将引入网络组织结构优化设计、多节点多簇网络协同通信等一系列问题。因此现阶段基于UWB的目标跟踪在室内导航领域仍旧面临很多挑战。
发明内容
[0004] 本发明的目的就是为了解决由于在实时系统中UWB由于受到室内环境的影响不能得到正常的距离信息的问题，提出了一种具有数据缺失UWB行人定位的EFIR滤波算法及系统，该方法对传统的EFIR滤波算法进行改进，首先判断UWB测量得到的距离信息是否有缺失，如果第i个距离信息缺失，则对第i个距离信息进行预估，以保证滤波器的正常运行，最终得到当前时刻最优的行人位置预估。
[0005] 为实现上述目的，本发明的具体方案如下：
[0006] 本发明的第一目的是公开一种具有数据缺失UWB行人定位的EFIR滤波算法，包括：
[0007] 以惯性导航器件INS在t时刻在导航系下的东方向位置、北方向位置、东方向速度、北方向速度作为状态量，以INS与UWB分别测量的参考节点与未知节点之间距离的差值作为系统观测量，构建滤波模型；
[0008] 利用EFIR滤波算法对位置误差进行预估，预估过程中实时判断UWB测量得到的参考节点与未知节点之间的距离信息是否有缺失，如果有，对缺失的距离信息进行预估；
[0009] 最终得到当前时刻目标行人最优的导航信息。
[0010] 进一步地，所述EFIR滤波器的状态方程为：
[0011]
[0012] 其中， 和 分别为t和t-1时刻惯性导航器件INS的东方向位置、北方向位置、东方向速度、北方向速度；T为采样周期；ωt-1为t-1时刻的系统噪声；
[0013] 进一步地，所述EFIR滤波器的观测方程为：
[0014]
[0015] 其中，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时刻的观测噪声。
[0016] 进一步地，所述的预估过程中实时判断UWB测量得到的参考节点与未知节点之间的距离信息是否有缺失，如果有，对缺失的距离信息进行预估，具体为：
[0017] 引入变量 表示UWB测量得到的参考节点与未知节点之间的第i个距离信息；如果第i个距离信息缺失，则重新对 进行预估；采用矩阵h(Xt|t-1)的第i行第1列替代缺失的距离信息。
[0018] 进一步地，对缺失数据进行预估后，EFIR滤波器的观测方程变为：
[0019]
[0020] 进一步地，所述利用EFIR滤波算法对位置误差进行预估，具体为：
[0021] 定义两个变量m和s：
[0022] m＝t-N+1，s＝m+M-1；
[0023] 判断当前时刻t是否大于EFIR滤波器的滤波窗口长度N，如果t＜N，则进行EKF滤波算法，其步骤如下：
[0024] 首先，进行一步预估
[0025]
[0026]
[0027] Ft-1为t-1时刻的系统矩阵；
[0028] 判断距离信息是否可用，引入变量 若第i个距离信息不可用，则 对不可用的距离信息进行预估
[0029]
[0030] 其中，h(Xt|t-1)(i,1)表示用矩阵h(Xt|t-1)的第i行第1列替代不可用的距离信息；
[0031]
[0032]
[0033] Pt＝(I-KtHt)Pt|t-1；
[0034] 其中， 表示EKF在t时刻预估的状态向量， 表示EKF由t-1时刻到t时刻预估的状态向量，Pt|t-1表示EKF由t-1时刻到t时刻的最小预测均方误差矩阵；Pt表示EKF t时刻的最小预测均方误差矩阵；Kt表示EKF在t时刻的误差增益矩阵；I表示单位阵；其Rt为νt的协方差矩阵；
[0035] 如果t≥N，则进行EFIR滤波算法，其步骤如下：
[0036]
[0037] 为s时刻EFIR滤波器的状态向量， 为s时刻自适应EKF预估滤波器的状态向量；
[0038] 利用中间变量l，另l在m+M时刻到t时刻进行下列迭代：
[0039]
[0040]
[0041]
[0042]
[0043] 令
[0044] 其中，Fl表示表示l时刻的系统矩阵， 表示EFIR在t时刻预估的状态向量，Kl表示EFIR在l时刻的误差增益矩阵；
[0045] 为由l-1时刻到l时刻的状态向量预估、 为由l-1时刻的状态向量预估、Gl为l时刻的广义噪声功率增益、
[0046] 本发明的第二目的是公开一种具有数据缺失UWB行人定位的EFIR滤波系统，包括服务器，所述服务器包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现以下步骤：
[0047] 以惯性导航器件INS在t时刻在导航系下的东方向位置、北方向位置、东方向速度、北方向速度作为状态量，以INS与UWB分别测量的参考节点与未知节点之间距离的差值作为系统观测量，构建滤波模型；
[0048] 利用EFIR滤波算法对位置误差进行预估，预估过程中实时判断UWB测量得到的参考节点与未知节点之间的距离信息是否有缺失，如果有，对缺失的距离信息进行预估；
[0049] 最终得到当前时刻目标行人最优的导航信息。
[0050] 本发明的第三目的是公开一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时执行以下步骤：
[0051] 以惯性导航器件INS在t时刻在导航系下的东方向位置、北方向位置、东方向速度、北方向速度作为状态量，以INS与UWB分别测量的参考节点与未知节点之间距离的差值作为系统观测量，构建滤波模型；
[0052] 利用EFIR滤波算法对位置误差进行预估，预估过程中实时判断UWB测量得到的参考节点与未知节点之间的距离信息是否有缺失，如果有，对缺失的距离信息进行预估；
[0053] 最终得到当前时刻目标行人最优的导航信息。
[0054] 本发明的有益效果：
[0055] 1、通过引入变量 表示第i个信道的UWB距离信息是否可用，若第i个距离信息不可用，则 对不可用的距离信息进行预估，弥补UWB距离信息不可用导致的数据融合算法不可用的问题。
[0056] 2、可用于室内环境下的中高精度定位。
附图说明
[0057] 图1为一种具有数据缺失INS/UWB紧组合行人导航的自适应预估EKF滤波算法的系统示意图；
[0058] 图2为本发明构建滤波模型进行数据融合示意图；
[0059] 图3为自适应预估EKF滤波算法流程图。具体实施方式：
[0060] 下面结合附图对本发明进行详细说明：
[0061] 本发明一种具有数据缺失INS/UWB紧组合行人导航的EFIR滤波算法的系统如图1所示，包括：组合导航算法采用UWB和INS两种导航系统，其中，UWB包括UWB参考节点和UWB定位标签，UWB参考节点预先固定在已知坐标上，UWB定位标签固定在目标行人上。INS主要由固定在目标行人足部的IMU组成。
[0062] 基于上述系统，本发明公开了具有数据缺失INS/UWB紧组合行人导航的EFIR滤波算法，包括：
[0063] (1)如图2所示，以惯性导航器件INS在t时刻在导航系下的东方向位置、北方向位置、东方向速度、北方向速度作为状态量，以INS与UWB分别测量的参考节点与未知节点之间距离的差值作为系统观测量，构建滤波模型进行数据融合；
[0064] (2)利用EFIR滤波算法对位置误差进行预估，EFIR预估滤波器的状态方程为：
[0065]
[0066] 其中， 和 分别为t和t-1时刻惯性导航器件INS的东方向位置、北方向位置、东方向速度、北方向速度；T为采样周期；ωt-1为t-1时刻的系统噪声。
[0067] 进一步地，所述EFIR滤波器的观测方程为：
[0068]
[0069] 其中，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时刻的观测噪声。
[0070]
[0071] 其中，x为UWB解算出的未知节点东方向位置，y为UWB解算出的未知节点北方向位置，xi,i∈(1,2,...,g)和yi,i∈(1,2,...,g)分别为参考节点的东向位置和北向位置。在此基础上，判断一下距离信息是否可用，引入变量 若第i个距离信息不可用，则 对不可用的距离信息进行预估：
[0072]
[0073] 进一步地，在t时刻EFIR滤波算法的步骤如下：
[0074] 首先定义两个变量m和s如下：
[0075] m＝t-N+1,s＝m+M-1
[0076] 判断当前时刻t是否大于EFIR滤波器的滤波窗口长度N，如果t＜N，则进行EKF滤波算法，其步骤如下：
[0077] 首先，进行一步预估
[0078]
[0079]
[0080] 判断距离信息是否可用，引入变量 若第i个距离信息不可用，则 对不可用的距离信息进行预估
[0081]
[0082] 其中，h(Xt|t-1)(i,1)表示用矩阵h(Xt|t-1)的第i行第1列替代不可用的距离信息。
[0083]
[0084]
[0085] Pt＝(I-KtHt)Pt|t-1
[0086] 其中， 表示自适应EKF预估滤波器在t时刻预估的状态向量， 表示EKF由t-1时刻到t时刻预估的状态向量，Pt|t-1表示EKF由t-1时刻到t时刻的最小预测均方误差矩阵；Pt表示自适应EKF预估滤波器t时刻的最小预测均方误差矩阵；Kt表示自适应EKF预估滤波器在t时刻的误差增益矩阵；I表示单位阵。
[0087] 如果t≥N，则进行EFIR滤波算法，其步骤如下：
[0088]
[0089] 利用中间变量l，另l在m+M时刻到t时刻进行下列迭代：
[0090]
[0091]
[0092]
[0093]
[0094] 令
[0095] 其中，Fl表示表示l时刻的系统矩阵， 表示EFIR在t时刻预估的状态向量，Kl表示EFIR在l时刻的误差增益矩阵；
[0096] 为由l-1时刻到l时刻的状态向量预估、 为由l-1时刻的状态向量预估、Gl为l时刻的广义噪声功率增益、
[0097] 上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。
