CN107966143A - 一种基于多窗口的自适应efir数据融合方法 - Google Patents

一种基于多窗口的自适应efir数据融合方法 Download PDF

Info

Publication number
CN107966143A
CN107966143A CN201711123461.8A CN201711123461A CN107966143A CN 107966143 A CN107966143 A CN 107966143A CN 201711123461 A CN201711123461 A CN 201711123461A CN 107966143 A CN107966143 A CN 107966143A
Authority
CN
China
Prior art keywords
mrow
msub
msubsup
efir
moment
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.)
Pending
Application number
CN201711123461.8A
Other languages
English (en)
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 CN201711123461.8A priority Critical patent/CN107966143A/zh
Publication of CN107966143A publication Critical patent/CN107966143A/zh
Pending legal-status Critical Current

Links

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

本发明公开了一种基于多窗口的自适应EFIR数据融合方法，包括：通过UWB系统和惯性导航器件INS系统分别测量参考节点到目标节点之间的距离；在此基础上，将两种系统测量得到的距离信息的平方作差，差值作为数据融合算法所使用的滤波模型的观测量；在此基础上，对传统的扩展有限脉冲响应(EFIR)滤波器进行改进，以在离线阶段选择的不同局部滤波窗口构建不同的子EFIR滤波器，并通过IMM方式对构建的子EFIR滤波器的输出进行融合，得到当前时刻的惯性导航器件INS测量系统测量的行人最优的位置误差预估；在此基础上，将惯性导航器件INS测量得到的行人位置与EFIR滤波器得到的位置误差预估作差，最终得到当前时刻最优的行人位置预估。

Description

一种基于多窗口的自适应EFIR数据融合方法
技术领域
本发明涉及复杂环境下组合定位技术领域，尤其涉及一种基于多窗口的自适应EFIR数据融合方法。
背景技术
近年来，行人导航(Pedestrian Navigation,PN)作为导航技术应用的新兴领域，正越来越受到各国学者的重视，并逐渐成为该领域的研究热点。然而在隧道、大型仓库、地下停车场等室内环境下，外界无线电信号微弱、电磁干扰强烈等因素都会对目标行人导航信息获取的准确性、实时性及鲁棒性有很大影响。如何将室内环境下获取的有限信息进行有效的融合以消除室内复杂环境影响，保证行人导航精度的持续稳定，具有重要的科学理论意义和实际应用价值。
在现有的定位方式中，全球卫星导航系统(Global Navigation SatelliteSystem,GNSS)是最为常用的一种方式。虽然GNSS能够通过精度持续稳定的位置信息，但是其易受电磁干扰、遮挡等外界环境影响的缺点限制了其应用范围，特别是在室内、地下巷道等一些密闭的、环境复杂的场景，GNSS信号被严重遮挡，无法进行有效的工作。近年来，UWB(Ultra Wideband)以其在复杂环境下定位精度高的特点在短距离局部定位领域表现出很大的潜力。学者们提出将基于UWB的目标跟踪应用于GNSS失效环境下的行人导航。这种方式虽然能够实现室内定位，但是由于室内环境复杂多变，UWB信号十分容易受到干扰而导致定位精度下降甚至失锁；与此同时，由于UWB采用的通信技术通常为短距离无线通信技术，因此若想完成大范围的室内目标跟踪定位，需要大量的网络节点共同完成，这必将引入网络组织结构优化设计、多节点多簇网络协同通信等一系列问题。因此现阶段基于UWB的目标跟踪在室内导航领域仍旧面临很多挑战。
发明内容
本发明的目的就是为了解决由于在实时系统中很难获取对扩展有限状态响应(Extended finite impulse response，EFIR)滤波器的滤波性能有较大影响的最优滤波窗口N的问题，提出了一种采用IMM(交互多模型)窗口的自适应EFIR数据融合方法，该方法对传统的扩展有限脉冲响应(EFIR)滤波器进行改进，以在离线阶段选择的不同局部滤波窗口构建不同的子EFIR滤波器，并通过IMM方式对构建的子EFIR滤波器的输出进行融合，最终得到当前时刻最优的行人位置预估。
为实现上述目的，本发明的具体方案如下：
本发明公开了一种基于多窗口的自适应EFIR数据融合方法，包括：
(1)以惯性导航器件INS在k时刻在东向和北向的位置误差和速度误差作为状态量，以惯性导航器件INS与UWB分别测量的目标节点与参考节点之间的距离的平方的差值作为系统观测量，构建滤波模型；
(2)在扩展有限脉冲响应滤波器的基础上，根据在离线阶段选择的不同局部滤波窗口，构建P个不同的子EFIR滤波器，P为设定值；
(3)通过IMM方式对构建的子EFIR滤波器的输出进行融合，得到当前时刻的惯性导航器件INS测量系统测量的行人最优的位置误差预估；
(4)将惯性导航器件INS测量得到的行人位置与EFIR滤波器得到的位置误差预估作差，最终得到当前时刻最优的行人位置预估。
进一步地，所述步骤(2)中，子EFIR滤波器的状态方程为：
其中，(δPEast,k+1,δPNorth,k+1)、(δPEast,k,δPNorth,k)分别为k+1和k时刻惯性导航器件INS解算的东向和北向的位置误差；(δVEast,k+1,δVNorth,k+1)、(δVEast,k,δVNorth,k)为k+1和k时刻惯性导航器件INS解算的东向和北向的速度误差；T为采样周期；wk为k时刻的系统噪声。
进一步地，所述步骤(2)中，子EFIR滤波器的观测方程为：
其中，为k时刻惯性导航器件INS和UWB分别测量得到的参考节点与未知节点之间的距离的平方的差；n为参考节点的数目；为k时刻惯性导航器件INS测量得到的参考节点与未知节点之间的距离；为k时刻UWB测量得到的参考节点与未知节点之间的距离；
其中，(xi,yi),i∈[1,n]为参考节点的位置；ηk为系统k时刻的观测噪声，其协方差矩阵为Rk；分别为k时刻惯性导航器件INS测量得到的东向和北向位置。
进一步地，所述步骤(2)中，在离线阶段选择的不同局部滤波窗口构建不同的子EFIR滤波器的数量根据对算法性能的需求自行选择。
进一步地，所述步骤(2)中，各子EFIR滤波器通过一组独立的数据，将能获取到的均方误差最小的P个局部滤波窗口N，分别作为子EFIR滤波器的局部滤波窗口。
进一步地，子EFIR滤波器的迭代算法为：
设定中间变量m和s：
m＝k-N+1,s＝m+M-1
其中，N为子EFIR滤波器的局部滤波窗口的大小；M为状态向量的维数；
其中，为s时刻的状态向量、为s时刻的状态向量预估、ys为s时刻的观测向量；
设定中间变量j的范围为从s+1时刻到k时刻，子EFIR滤波器进行的迭代如下：
Kj＝Gj(Hj)T
在此基础上，
其中，Fj和Fj-1分别为j时刻和j-1时刻带观测时滞模型的系统矩阵；和分别为j时刻和j-1时刻带观测时滞模型的状态向量迭代中间量；为由j-1时刻得到的j时刻带观测时滞模型的状态向量迭代中间量；Gj和Gj-1分别为j和j-1时刻的广义噪声功率增益；Kj为j时刻带观测时滞模型的偏差校正增益；yj为带观测时滞模型在j时刻的观测向量；为j时刻带观测时滞模型下的状态向量预估值；为关于的函数；
为从j-1到j时刻的误差矩阵、为从j-1时刻的误差矩阵、Q为系统噪声wk的协方差、Rj为观测噪声的协方差；
为第i个子EFIR滤波器的状态预估，为状态预估中间变量，为第i个子EFIR滤波器的误差矩阵，为协方差矩阵中间变量。
进一步地，所述步骤(3)中，通过IMM方式对构建的子EFIR滤波器的输出进行融合具体为：
1)构建马尔可夫的转移概率矩阵，在此基础上，引入权重矩阵；
2)计算各子EFIR滤波器的权重；
3)根据各子EFIR滤波器的权重以及子EFIR滤波器的状态向量预估，得到数据融合滤波器的总输出。
进一步地，所述步骤2)中，各子EFIR滤波器的权重θEFIR#χ的计算方法如下：
其中，为上一时刻的权重值；χ＝1,2,3,4，…g；g为子EFIR滤波器个数；
yk为k时刻的观测向量，Rk为系统k时刻的观测噪声的协方差矩阵，为第χ个EFIR滤波器的一步状态预估，第χ个EFIR滤波器通过一步预估获取到的观测向量预估 为第χ个EFIR滤波器的误差矩阵。
进一步地，所述步骤3)中，数据融合滤波器的总输出具体为：
其中，为k时刻第χ个子EFIR滤波器的权重，χ＝1,2,3,4，…P；P为子EFIR滤波器个数；为子EFIR滤波器的状态向量预估。
本发明的有益效果：
1、通过在离线阶段通过独立的数据获取的子EFIR滤波器的滤波窗口长度，构建各子EFIR滤波器，在此基础上，通过交互多模型(IMM)算法，将各子滤波器的输出进行融合，最终得到最优的惯性导航器件INS位置解算误差预估，并进而得到惯性导航器件INS的最优行人位置预估。
2、可用于室内环境下的中高精度定位。
附图说明
图1为一种基于多窗口的自适应EFIR数据融合系统的示意图；
图2为本发明构建滤波模型进行数据融合示意图；
图3为本发明通过IMM方式对构建的子EFIR滤波器的输出进行融合示意图。
具体实施方式：
下面结合附图对本发明进行详细说明：
本发明一种基于多窗口的自适应EFIR数据融合系统如图1所示，包括：组合导航算法采用UWB和INS两种导航系统，其中，UWB包括UWB参考节点和UWB定位标签，UWB参考节点预先固定在已知坐标上，UWB定位标签固定在目标行人上。INS主要由固定在目标行人足部的IMU组成。
基于上述系统，本发明公开了一种基于多窗口的自适应EFIR数据融合方法，包括：
(1)如图2所示，以惯性导航器件INS在k时刻在东向和北向的位置误差和速度误差作为状态量，以惯性导航器件INS与UWB分别测量的目标节点与参考节点之间的距离的平方的差值作为系统观测量，构建滤波模型进行数据融合；
(2)在扩展有限脉冲响应滤波器的基础上，根据在离线阶段选择的不同局部滤波窗口，构建不同的子EFIR滤波器；
子EFIR滤波器的状态方程为：
其中，(δPEast,k+1,δPNorth,k+1)、(δPEast,k,δPNorth,k)为k+1和k时刻惯性导航器件INS解算的东向和北向的位置误差；(δVEast,k+1,δVNorth,k+1)、(δVEast,k,δVNorth,k)为k+1和k时刻惯性导航器件INS解算的东向和北向的速度误差；T为采样周期；wk为k时刻的系统噪声；
子EFIR滤波器的观测方程为：
其中，为k时刻惯性导航器件INS和UWB分别测量得到的参考节点与未知节点之间的距离的平方的差；n为参考节点的数目；为k时刻惯性导航器件INS测量得到的参考节点与未知节点之间的距离；为k时刻UWB测量得到的参考节点与未知节点之间的距离；
其中，(xi,yi),i∈[1,n]为参考节点的位置；ηk为系统k时刻的观测噪声；
(3)如图3所示，通过IMM方式对构建的子EFIR滤波器的输出进行融合，得到当前时刻的惯性导航器件INS测量系统测量的行人最优的位置误差预估；
在离线阶段选择的不同局部滤波窗口构建不同的子EFIR滤波器的数量可以根据对算法性能的需求自行选择，本发明以离线阶段选择4个不同的局部滤波窗口构建4个子EFIR滤波器为例进行介绍。
各子EFIR滤波器通过一组独立的数据，将能获取到的最小均方误差的4个局部滤波窗口N，分别作为子EFIR滤波器的局部滤波窗口；其中，对子EFIR滤波器，其均方根误差的表达式如下：
EFIR算法的迭代方程为：
m＝k-N+1,s＝m+M-1
其中，N为EFIR滤波器的局部滤波窗口的大小；M为状态向量的维数；
从s+1时刻到k时刻，EFIR滤波器进行的迭代如下：
Gj＝[(Hl)THj+(FjGj-1(Fj)T)-1]-1
Kj＝Gj(Hj)T
在此基础上，
IMM算法具体为：
首先构建马尔可夫的转移概率矩阵：
其中，aij,ij∈[1,4]表示4个子EFIR滤波器之间的转移概率；在此基础上，引入权重矩阵θk，如下：
对各子EFIR滤波器(编号为#i,i∈[1,4])的权重计算如下：
其中，为上一时刻的权重值；
故，数据融合滤波器的总输出为：
(4)将惯性导航器件INS测量得到的行人位置与EFIR滤波器得到的位置误差预估作差，最终得到当前时刻最优的行人位置预估。
上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。

Claims (9)

1.一种基于多窗口的自适应EFIR数据融合方法，其特征在于，包括：
(1)以惯性导航器件INS在k时刻在东向和北向的位置误差和速度误差作为状态量，以惯性导航器件INS与UWB分别测量的目标节点与参考节点之间的距离的平方的差值作为系统观测量，构建滤波模型；
(2)在扩展有限脉冲响应滤波器的基础上，根据在离线阶段选择的不同局部滤波窗口，构建P个不同的子EFIR滤波器，P为设定值；
(3)通过IMM方式对构建的子EFIR滤波器的输出进行融合，得到当前时刻的惯性导航器件INS测量系统测量的行人最优的位置误差预估；
(4)将惯性导航器件INS测量得到的行人位置与EFIR滤波器得到的位置误差预估作差，最终得到当前时刻最优的行人位置预估。
2.如权利要求1所述的一种基于多窗口的自适应EFIR数据融合方法，其特征在于，所述步骤(2)中，子EFIR滤波器的状态方程为：
其中，(δPEast,k+1,δPNorth,k+1)、(δPEast,k,δPNorth,k)分别为k+1和k时刻惯性导航器件INS解算的东向和北向的位置误差；(δVEast,k+1,δVNorth,k+1)、(δVEast,k,δVNorth,k)为k+1和k时刻惯性导航器件INS解算的东向和北向的速度误差；T为采样周期；wk为k时刻的系统噪声。
3.如权利要求1所述的一种基于多窗口的自适应EFIR数据融合方法，其特征在于，所述步骤(2)中，子EFIR滤波器的观测方程为：
其中，为k时刻惯性导航器件INS和UWB分别测量得到的参考节点与未知节点之间的距离的平方的差；n为参考节点的数目；为k时刻惯性导航器件INS测量得到的参考节点与未知节点之间的距离；为k时刻UWB测量得到的参考节点与未知节点之间的距离；
<mrow> <mtable> <mtr> <mtd> <mrow> <msub> <mi>h</mi> <msub> <mi>d</mi> <mi>i</mi> </msub> </msub> <mrow> <mo>(</mo> <msubsup> <mi>&amp;delta;P</mi> <mrow> <mi>E</mi> <mi>a</mi> <mi>s</mi> <mi>t</mi> <mo>,</mo> <mi>k</mi> </mrow> <mrow> <mo>(</mo> <mi>I</mi> <mi>N</mi> <mi>S</mi> <mo>)</mo> </mrow> </msubsup> <mo>,</mo> <msubsup> <mi>&amp;delta;P</mi> <mrow> <mi>N</mi> <mi>o</mi> <mi>r</mi> <mi>t</mi> <mi>h</mi> <mo>,</mo> <mi>k</mi> </mrow> <mrow> <mo>(</mo> <mi>I</mi> <mi>N</mi> <mi>S</mi> <mo>)</mo> </mrow> </msubsup> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>=</mo> <mn>2</mn> <mrow> <mo>(</mo> <msubsup> <mi>P</mi> <mrow> <mi>E</mi> <mi>a</mi> <mi>s</mi> <mi>t</mi> <mo>,</mo> <mi>k</mi> </mrow> <mrow> <mo>(</mo> <mi>I</mi> <mi>N</mi> <mi>S</mi> <mo>)</mo> </mrow> </msubsup> <mo>-</mo> <msub> <mi>x</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> <msubsup> <mi>&amp;delta;P</mi> <mrow> <mi>E</mi> <mi>a</mi> <mi>s</mi> <mi>t</mi> <mo>,</mo> <mi>k</mi> </mrow> <mrow> <mo>(</mo> <mi>I</mi> <mi>N</mi> <mi>S</mi> <mo>)</mo> </mrow> </msubsup> <mo>+</mo> <mn>2</mn> <mrow> <mo>(</mo> <msubsup> <mi>P</mi> <mrow> <mi>N</mi> <mi>o</mi> <mi>r</mi> <mi>t</mi> <mi>h</mi> </mrow> <mrow> <mi>I</mi> <mi>N</mi> <mi>S</mi> </mrow> </msubsup> <mo>-</mo> <msub> <mi>y</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> <msubsup> <mi>&amp;delta;P</mi> <mrow> <mi>N</mi> <mi>o</mi> <mi>r</mi> <mi>t</mi> <mi>h</mi> <mo>,</mo> <mi>k</mi> </mrow> <mrow> <mo>(</mo> <mi>I</mi> <mi>N</mi> <mi>S</mi> <mo>)</mo> </mrow> </msubsup> <mo>-</mo> <mrow> <mo>(</mo> <msup> <mrow> <mo>(</mo> <mrow> <msubsup> <mi>&amp;delta;P</mi> <mrow> <mi>E</mi> <mi>a</mi> <mi>s</mi> <mi>t</mi> <mo>,</mo> <mi>k</mi> </mrow> <mrow> <mo>(</mo> <mi>I</mi> <mi>N</mi> <mi>S</mi> <mo>)</mo> </mrow> </msubsup> </mrow> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>+</mo> <msup> <mrow> <mo>(</mo> <mrow> <msubsup> <mi>&amp;delta;P</mi> <mrow> <mi>N</mi> <mi>o</mi> <mi>r</mi> <mi>t</mi> <mi>h</mi> <mo>,</mo> <mi>k</mi> </mrow> <mrow> <mo>(</mo> <mi>I</mi> <mi>N</mi> <mi>S</mi> <mo>)</mo> </mrow> </msubsup> </mrow> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> <mo>;</mo> </mrow>
其中，(xi,yi),i∈[1,n]为参考节点的位置；ηk为系统k时刻的观测噪声，其协方差矩阵为Rk；分别为k时刻惯性导航器件INS测量得到的东向和北向位置。
4.如权利要求1所述的一种基于多窗口的自适应EFIR数据融合方法，其特征在于，所述步骤(2)中，在离线阶段选择的不同局部滤波窗口构建不同的子EFIR滤波器的数量根据对算法性能的需求自行选择。
5.如权利要求1所述的一种基于多窗口的自适应EFIR数据融合方法，其特征在于，所述步骤(2)中，各子EFIR滤波器通过一组独立的数据，将能获取到的均方误差最小的P个局部滤波窗口N，分别作为子EFIR滤波器的局部滤波窗口。
6.如权利要求1所述的一种基于多窗口的自适应EFIR数据融合方法，其特征在于，子EFIR滤波器的迭代算法为：
设定中间变量m和s：
m＝k-N+1,s＝m+M-1
其中，N为子EFIR滤波器的局部滤波窗口的大小；M为状态向量的维数；
<mrow> <msub> <mover> <mi>x</mi> <mo>~</mo> </mover> <mi>s</mi> </msub> <mo>=</mo> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msub> <mi>y</mi> <mi>s</mi> </msub> <mo>,</mo> </mrow> </mtd> <mtd> <mrow> <mi>i</mi> <mi>f</mi> </mrow> </mtd> <mtd> <mrow> <mi>s</mi> <mo>&lt;</mo> <mi>N</mi> <mo>-</mo> <mn>1</mn> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>x</mi> <mo>^</mo> </mover> <mi>s</mi> </msub> <mo>,</mo> </mrow> </mtd> <mtd> <mrow> <mi>i</mi> <mi>f</mi> </mrow> </mtd> <mtd> <mrow> <mi>s</mi> <mo>&amp;GreaterEqual;</mo> <mi>N</mi> <mo>-</mo> <mn>1</mn> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>;</mo> </mrow>
其中，为s时刻的状态向量、为s时刻的状态向量预估、ys为s时刻的观测向量；
设定中间变量j的范围为从s+1时刻到k时刻，子EFIR滤波器进行的迭代如下：
<mrow> <mtable> <mtr> <mtd> <mrow> <msub> <mover> <mi>x</mi> <mo>~</mo> </mover> <mrow> <mi>j</mi> <mo>|</mo> <mi>j</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msub> <mi>F</mi> <mi>j</mi> </msub> <msub> <mover> <mi>x</mi> <mo>~</mo> </mover> <mrow> <mi>j</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>P</mi> <mo>~</mo> </mover> <mrow> <mi>j</mi> <mo>|</mo> <mi>j</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msub> <mi>F</mi> <mi>j</mi> </msub> <msub> <mover> <mi>P</mi> <mo>~</mo> </mover> <mrow> <mi>j</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>F</mi> <mi>j</mi> <mi>T</mi> </msubsup> <mo>+</mo> <mi>Q</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>G</mi> <mi>j</mi> </msub> <mo>=</mo> <msup> <mrow> <mo>&amp;lsqb;</mo> <mrow> <msup> <mrow> <mo>(</mo> <msub> <mi>H</mi> <mi>l</mi> </msub> <mo>)</mo> </mrow> <mi>T</mi> </msup> <msub> <mi>H</mi> <mi>j</mi> </msub> <mo>+</mo> <msup> <mrow> <mo>(</mo> <msub> <mi>F</mi> <mi>j</mi> </msub> <msub> <mi>G</mi> <mrow> <mi>j</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msup> <mrow> <mo>(</mo> <msub> <mi>F</mi> <mi>j</mi> </msub> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>)</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> </mrow> <mo>&amp;rsqb;</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>K</mi> <mi>j</mi> </msub> <mo>=</mo> <msub> <mi>G</mi> <mi>j</mi> </msub> <msup> <mrow> <mo>(</mo> <msub> <mi>H</mi> <mi>j</mi> </msub> <mo>)</mo> </mrow> <mi>T</mi> </msup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>x</mi> <mo>~</mo> </mover> <mi>j</mi> </msub> <mo>=</mo> <msub> <mover> <mi>x</mi> <mo>~</mo> </mover> <mrow> <mi>j</mi> <mo>|</mo> <mi>j</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mi>K</mi> <mi>j</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>y</mi> <mi>j</mi> </msub> <mo>-</mo> <mi>h</mi> <mo>(</mo> <msub> <mover> <mi>x</mi> <mo>~</mo> </mover> <mrow> <mi>j</mi> <mo>|</mo> <mi>j</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>P</mi> <mo>~</mo> </mover> <mi>j</mi> </msub> <mo>=</mo> <mrow> <mo>(</mo> <mi>I</mi> <mo>-</mo> <msub> <mi>K</mi> <mi>j</mi> </msub> <msub> <mi>H</mi> <mi>j</mi> </msub> <mo>)</mo> </mrow> <msub> <mi>P</mi> <mi>j</mi> </msub> <msup> <mrow> <mo>(</mo> <mi>I</mi> <mo>-</mo> <msub> <mi>K</mi> <mi>j</mi> </msub> <msub> <mi>H</mi> <mi>j</mi> </msub> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>+</mo> <msub> <mi>K</mi> <mi>j</mi> </msub> <msub> <mi>R</mi> <mi>j</mi> </msub> <msup> <msub> <mi>K</mi> <mi>j</mi> </msub> <mi>T</mi> </msup> </mrow> </mtd> </mtr> </mtable> <mo>;</mo> </mrow>
在此基础上，
其中，Fj和Fj-1分别为j时刻和j-1时刻带观测时滞模型的系统矩阵；和分别为j时刻和j-1时刻带观测时滞模型的状态向量迭代中间量；为由j-1时刻得到的j时刻带观测时滞模型的状态向量迭代中间量；Gj和Gj-1分别为j和j-1时刻的广义噪声功率增益；Kj为j时刻带观测时滞模型的偏差校正增益；yj为带观测时滞模型在j时刻的观测向量；为j时刻带观测时滞模型下的状态向量预估值；为关于的函数；
为从j-1到j时刻的误差矩阵、为从j-1时刻的误差矩阵、Q为系统噪声wk的协方差、Rj为观测噪声的协方差；
为第i个子EFIR滤波器的状态预估，为状态预估中间变量，为第i个子EFIR滤波器的误差矩阵，为协方差矩阵中间变量。
7.如权利要求1所述的一种基于多窗口的自适应EFIR数据融合方法，其特征在于，所述步骤(3)中，通过IMM方式对构建的子EFIR滤波器的输出进行融合具体为：
1)构建马尔可夫的转移概率矩阵，在此基础上，引入权重矩阵；
2)计算各子EFIR滤波器的权重；
3)根据各子EFIR滤波器的权重以及子EFIR滤波器的状态向量预估，得到数据融合滤波器的总输出。
8.如权利要求7所述的一种基于多窗口的自适应EFIR数据融合方法，其特征在于，所述步骤2)中，各子EFIR滤波器的权重θEFIR#χ的计算方法如下：
<mrow> <msubsup> <mi>&amp;theta;</mi> <mi>k</mi> <mrow> <mi>E</mi> <mi>F</mi> <mi>I</mi> <mi>R</mi> <mo>#</mo> <mi>&amp;chi;</mi> </mrow> </msubsup> <mo>=</mo> <mfrac> <mn>1</mn> <msub> <mi>b</mi> <mi>k</mi> </msub> </mfrac> <msubsup> <mi>&amp;Lambda;</mi> <mi>k</mi> <mrow> <mi>E</mi> <mi>F</mi> <mi>I</mi> <mi>R</mi> <mo>#</mo> <mi>&amp;chi;</mi> </mrow> </msubsup> <msubsup> <mi>&amp;theta;</mi> <mi>k</mi> <mrow> <mi>E</mi> <mi>F</mi> <mi>I</mi> <mi>R</mi> <mo>#</mo> <mi>&amp;chi;</mi> <mo>-</mo> </mrow> </msubsup> <mo>;</mo> </mrow>
其中，为上一时刻的权重值；χ＝1,2,3,4，…g；g为子EFIR滤波器个数；
<mrow> <msub> <mi>b</mi> <mi>k</mi> </msub> <mo>=</mo> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>f</mi> </munderover> <msubsup> <mi>&amp;Lambda;</mi> <mi>k</mi> <mrow> <mi>E</mi> <mi>F</mi> <mi>I</mi> <mi>R</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </mrow> </msubsup> <msup> <mrow> <mo>(</mo> <msubsup> <mi>&amp;theta;</mi> <mi>k</mi> <mrow> <mi>E</mi> <mi>F</mi> <mi>I</mi> <mi>R</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </mrow> </msubsup> <mo>)</mo> </mrow> <mo>-</mo> </msup> </mrow>
<mrow> <msubsup> <mi>&amp;Lambda;</mi> <mi>k</mi> <mrow> <mi>E</mi> <mi>F</mi> <mi>I</mi> <mi>R</mi> <mo>#</mo> <mi>&amp;chi;</mi> </mrow> </msubsup> <mo>=</mo> <mo>|</mo> <mn>2</mn> <msubsup> <mi>&amp;pi;S</mi> <mi>k</mi> <mrow> <mi>E</mi> <mi>F</mi> <mi>I</mi> <mi>R</mi> <mo>#</mo> <mi>&amp;chi;</mi> </mrow> </msubsup> <msup> <mo>|</mo> <mrow> <mo>-</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> </mrow> </msup> <mi>exp</mi> <mo>&amp;lsqb;</mo> <mo>-</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <msubsup> <mi>e</mi> <mi>k</mi> <mrow> <mi>E</mi> <mi>F</mi> <mi>I</mi> <mi>R</mi> <mo>#</mo> <mi>&amp;chi;</mi> </mrow> </msubsup> <msup> <mrow> <mo>(</mo> <msubsup> <mi>S</mi> <mi>k</mi> <mrow> <mi>E</mi> <mi>F</mi> <mi>I</mi> <mi>R</mi> <mo>#</mo> <mi>&amp;chi;</mi> </mrow> </msubsup> <mo>)</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <msup> <mrow> <mo>(</mo> <msubsup> <mi>e</mi> <mi>k</mi> <mrow> <mi>E</mi> <mi>F</mi> <mi>I</mi> <mi>R</mi> <mo>#</mo> <mi>&amp;chi;</mi> </mrow> </msubsup> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>&amp;rsqb;</mo> <mo>;</mo> </mrow>
<mrow> <msubsup> <mi>S</mi> <mi>k</mi> <mrow> <mi>E</mi> <mi>F</mi> <mi>I</mi> <mi>R</mi> <mo>#</mo> <mi>&amp;chi;</mi> </mrow> </msubsup> <mo>=</mo> <msub> <mi>H</mi> <mi>k</mi> </msub> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mi>E</mi> <mi>F</mi> <mi>I</mi> <mi>R</mi> <mo>#</mo> <mi>&amp;chi;</mi> </mrow> </msubsup> <msubsup> <mi>H</mi> <mi>k</mi> <mi>T</mi> </msubsup> <mo>+</mo> <msub> <mi>R</mi> <mi>k</mi> </msub> <mo>;</mo> </mrow>
<mrow> <msubsup> <mi>e</mi> <mi>k</mi> <mrow> <mi>E</mi> <mi>F</mi> <mi>I</mi> <mi>R</mi> <mo>#</mo> <mi>&amp;chi;</mi> </mrow> </msubsup> <mo>=</mo> <msub> <mi>y</mi> <mi>k</mi> </msub> <mo>-</mo> <mi>h</mi> <mrow> <mo>(</mo> <msubsup> <mi>x</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mi>E</mi> <mi>F</mi> <mi>I</mi> <mi>R</mi> <mo>#</mo> <mi>&amp;chi;</mi> </mrow> </msubsup> <mo>)</mo> </mrow> <mo>;</mo> </mrow>
yk为k时刻的观测向量，Rk为系统k时刻的观测噪声的协方差矩阵，为第χ个EFIR滤波器的一步状态预估，第χ个EFIR滤波器通过一步预估获取到的观测向量预估 为第χ个EFIR滤波器的误差矩阵。
9.如权利要求7所述的一种基于多窗口的自适应EFIR数据融合方法，其特征在于，所述步骤3)中，数据融合滤波器的总输出具体为：
<mrow> <msub> <mover> <mi>x</mi> <mo>^</mo> </mover> <mi>k</mi> </msub> <mo>=</mo> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>&amp;chi;</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>P</mi> </munderover> <msubsup> <mi>&amp;theta;</mi> <mi>k</mi> <mrow> <mi>E</mi> <mi>F</mi> <mi>I</mi> <mi>R</mi> <mo>#</mo> <mi>&amp;chi;</mi> </mrow> </msubsup> <msubsup> <mover> <mi>x</mi> <mo>^</mo> </mover> <mi>k</mi> <mrow> <mi>E</mi> <mi>F</mi> <mi>I</mi> <mi>R</mi> <mo>#</mo> <mi>&amp;chi;</mi> </mrow> </msubsup> <mo>;</mo> </mrow>
其中，为k时刻第χ个子EFIR滤波器的权重，χ＝1,2,3,4，…P；P为子EFIR滤波器个数；为子EFIR滤波器的状态向量预估。
CN201711123461.8A 2017-11-14 2017-11-14 一种基于多窗口的自适应efir数据融合方法 Pending CN107966143A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201711123461.8A CN107966143A (zh) 2017-11-14 2017-11-14 一种基于多窗口的自适应efir数据融合方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201711123461.8A CN107966143A (zh) 2017-11-14 2017-11-14 一种基于多窗口的自适应efir数据融合方法

Publications (1)

Publication Number Publication Date
CN107966143A true CN107966143A (zh) 2018-04-27

Family

ID=62000195

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201711123461.8A Pending CN107966143A (zh) 2017-11-14 2017-11-14 一种基于多窗口的自适应efir数据融合方法

Country Status (1)

Country Link
CN (1) CN107966143A (zh)

Cited By (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108759825A (zh) * 2018-08-06 2018-11-06 济南大学 面向有数据缺失INS/UWB行人导航的自适应预估Kalman滤波算法及系统
CN109141413A (zh) * 2018-08-06 2019-01-04 济南大学 具有数据缺失uwb行人定位的efir滤波算法及系统
CN109141412A (zh) * 2018-08-06 2019-01-04 济南大学 面向有数据缺失ins/uwb组合行人导航的ufir滤波算法及系统
CN109269498A (zh) * 2018-08-06 2019-01-25 济南大学 面向具有数据缺失uwb行人导航的自适应预估ekf滤波算法及系统
CN109708632A (zh) * 2019-01-31 2019-05-03 济南大学 一种面向移动机器人的激光雷达/ins/地标松组合导航系统及方法
CN110375731A (zh) * 2019-07-01 2019-10-25 东南大学 一种混合交互式多模型滤波方法
CN111578939A (zh) * 2020-03-23 2020-08-25 济南大学 考虑采样周期随机变化的机器人紧组合导航方法及系统
CN113218388A (zh) * 2021-03-02 2021-08-06 济南大学 一种考虑可变有色测量噪声的移动机器人定位方法及系统
CN113295753A (zh) * 2021-04-29 2021-08-24 山东非金属材料研究所 一种pH计电压误差的双FIR测量方法
CN114440880A (zh) * 2022-01-28 2022-05-06 山东省路桥集团有限公司 基于自适应迭代ekf的施工现场控制点定位方法及系统

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104020466A (zh) * 2014-06-17 2014-09-03 西安电子科技大学 基于变结构多模型的机动目标跟踪方法
CN106680765A (zh) * 2017-03-03 2017-05-17 济南大学 基于分布式组合滤波ins/uwb行人导航系统及方法
CN107255795A (zh) * 2017-06-13 2017-10-17 山东大学 基于ekf/efir混合滤波的室内移动机器人定位方法和装置

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104020466A (zh) * 2014-06-17 2014-09-03 西安电子科技大学 基于变结构多模型的机动目标跟踪方法
CN106680765A (zh) * 2017-03-03 2017-05-17 济南大学 基于分布式组合滤波ins/uwb行人导航系统及方法
CN107255795A (zh) * 2017-06-13 2017-10-17 山东大学 基于ekf/efir混合滤波的室内移动机器人定位方法和装置

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
SHUNYI ZHAO: ""Fusion Kalman/UFIR Filter for State Estimation"", 《IEEE TRANSACTIONS ON INDUSTRIAL ELECTRONICS》 *
YUAN XU: ""UWB-Based Indoor Human Localization With"", 《SPECIAL SECTION ON ANALYSIS AND SYNTHESIS OF TIME-DELAY SYSTEMS》 *

Cited By (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108759825A (zh) * 2018-08-06 2018-11-06 济南大学 面向有数据缺失INS/UWB行人导航的自适应预估Kalman滤波算法及系统
CN109141413A (zh) * 2018-08-06 2019-01-04 济南大学 具有数据缺失uwb行人定位的efir滤波算法及系统
CN109141412A (zh) * 2018-08-06 2019-01-04 济南大学 面向有数据缺失ins/uwb组合行人导航的ufir滤波算法及系统
CN109269498A (zh) * 2018-08-06 2019-01-25 济南大学 面向具有数据缺失uwb行人导航的自适应预估ekf滤波算法及系统
CN109708632A (zh) * 2019-01-31 2019-05-03 济南大学 一种面向移动机器人的激光雷达/ins/地标松组合导航系统及方法
CN109708632B (zh) * 2019-01-31 2024-05-28 济南大学 一种面向移动机器人的激光雷达/ins/地标松组合导航系统及方法
CN110375731A (zh) * 2019-07-01 2019-10-25 东南大学 一种混合交互式多模型滤波方法
CN111578939A (zh) * 2020-03-23 2020-08-25 济南大学 考虑采样周期随机变化的机器人紧组合导航方法及系统
CN113218388A (zh) * 2021-03-02 2021-08-06 济南大学 一种考虑可变有色测量噪声的移动机器人定位方法及系统
CN113218388B (zh) * 2021-03-02 2022-07-05 济南大学 一种考虑可变有色测量噪声的移动机器人定位方法及系统
CN113295753A (zh) * 2021-04-29 2021-08-24 山东非金属材料研究所 一种pH计电压误差的双FIR测量方法
CN114440880A (zh) * 2022-01-28 2022-05-06 山东省路桥集团有限公司 基于自适应迭代ekf的施工现场控制点定位方法及系统

Similar Documents

Publication Publication Date Title
CN107966143A (zh) 一种基于多窗口的自适应efir数据融合方法
CN107966142A (zh) 一种基于多窗口的室内行人自适应ufir数据融合方法
CN105928518B (zh) 采用伪距和位置信息的室内行人uwb/ins紧组合导航系统及方法
CN105509739B (zh) 采用固定区间crts平滑的ins/uwb紧组合导航系统及方法
CN103471595B (zh) 一种面向ins/wsn室内移动机器人紧组合导航的迭代扩展rts均值滤波方法
Zhou et al. Activity sequence-based indoor pedestrian localization using smartphones
CN107402375A (zh) 一种带观测时滞的室内行人定位efir数据融合系统及方法
CN104864865B (zh) 一种面向室内行人导航的ahrs/uwb无缝组合导航方法
CN106680765A (zh) 基于分布式组合滤波ins/uwb行人导航系统及方法
CN110057354B (zh) 一种基于磁偏角修正的地磁匹配导航方法
Han et al. A combined matching algorithm for underwater gravity-aided navigation
Han et al. An improved TERCOM-based algorithm for gravity-aided navigation
CN102692223B (zh) 用于wsn/ins组合导航的多级非线性滤波器的控制方法
CN105589064A (zh) Wlan位置指纹数据库快速建立和动态更新系统及方法
CN105588566A (zh) 一种基于蓝牙与mems融合的室内定位系统及方法
CN106597363A (zh) 一种室内wlan环境下的行人定位方法
CN101793522B (zh) 基于抗差估计的稳健滤波方法
CN111536967A (zh) 一种基于ekf的多传感器融合温室巡检机器人跟踪方法
CN102494685B (zh) 用于wsn/ins高精度实时组合导航的h无穷信息融合方法
CN104066179B (zh) 一种改进的自适应迭代ukf的wsn节点定位方法
CN109141413A (zh) 具有数据缺失uwb行人定位的efir滤波算法及系统
CN106772524A (zh) 一种基于秩滤波的农业机器人组合导航信息融合方法
CN106840211A (zh) 一种基于kf和stupf组合滤波的sins大方位失准角初始对准方法
CN106052684A (zh) 采用多模式描述的移动机器人imu/uwb/码盘松组合导航系统及方法
CN104316058B (zh) 一种采用交互多模型的移动机器人wsn/ins组合导航方法

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
RJ01 Rejection of invention patent application after publication
RJ01 Rejection of invention patent application after publication

Application publication date: 20180427