CN117367431B - 带未知量测偏置的mems与uwb紧组合定位方法及系统 - Google Patents
带未知量测偏置的mems与uwb紧组合定位方法及系统 Download PDFInfo
- Publication number
- CN117367431B CN117367431B CN202311376185.1A CN202311376185A CN117367431B CN 117367431 B CN117367431 B CN 117367431B CN 202311376185 A CN202311376185 A CN 202311376185A CN 117367431 B CN117367431 B CN 117367431B
- Authority
- CN
- China
- Prior art keywords
- error
- measurement
- mems
- bias
- 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
Links
- 238000005259 measurement Methods 0.000 title claims abstract description 107
- 238000000034 method Methods 0.000 title claims abstract description 84
- 238000009826 distribution Methods 0.000 claims abstract description 57
- 239000013598 vector Substances 0.000 claims abstract description 47
- 239000011159 matrix material Substances 0.000 claims abstract description 36
- 230000014509 gene expression Effects 0.000 claims description 12
- 238000004364 calculation method Methods 0.000 claims description 9
- 230000007704 transition Effects 0.000 claims description 6
- 238000003860 storage Methods 0.000 claims description 5
- 238000004590 computer program Methods 0.000 claims description 4
- 230000007480 spreading Effects 0.000 claims description 3
- 238000003892 spreading Methods 0.000 claims description 3
- 238000004422 calculation algorithm Methods 0.000 description 25
- 238000001914 filtration Methods 0.000 description 14
- 230000006870 function Effects 0.000 description 9
- 238000005516 engineering process Methods 0.000 description 5
- 238000004458 analytical method Methods 0.000 description 3
- 230000008901 benefit Effects 0.000 description 3
- 230000004927 fusion Effects 0.000 description 3
- 230000006872 improvement Effects 0.000 description 3
- 230000008569 process Effects 0.000 description 3
- 230000001419 dependent effect Effects 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000009499 grossing Methods 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007423 decrease Effects 0.000 description 1
- 238000005315 distribution function Methods 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 230000010355 oscillation Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
Classifications
-
- H—ELECTRICITY
- H04—ELECTRIC COMMUNICATION TECHNIQUE
- H04W—WIRELESS COMMUNICATION NETWORKS
- H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
- H04W4/30—Services specially adapted for particular environments, situations or purposes
- H04W4/33—Services specially adapted for particular environments, situations or purposes for indoor environments, e.g. buildings
-
- 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/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/18—Stabilised platforms, e.g. by gyroscope
-
- 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/183—Compensation of inertial measurements, e.g. for temperature effects
-
- 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
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
- G06N7/00—Computing arrangements based on specific mathematical models
- G06N7/01—Probabilistic graphical models, e.g. probabilistic networks
-
- H—ELECTRICITY
- H04—ELECTRIC COMMUNICATION TECHNIQUE
- H04W—WIRELESS COMMUNICATION NETWORKS
- H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
- H04W4/02—Services making use of location information
- H04W4/023—Services making use of location information using mutual or relative location information between multiple location based services [LBS] targets or of distance thresholds
-
- Y—GENERAL 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
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02D—CLIMATE 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/00—Reducing energy consumption in communication networks
- Y02D30/70—Reducing energy consumption in communication networks in wireless communication networks
Landscapes
- Engineering & Computer Science (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Signal Processing (AREA)
- Theoretical Computer Science (AREA)
- Algebra (AREA)
- Mathematical Physics (AREA)
- Mathematical Analysis (AREA)
- Mathematical Optimization (AREA)
- Pure & Applied Mathematics (AREA)
- Computing Systems (AREA)
- General Engineering & Computer Science (AREA)
- Evolutionary Computation (AREA)
- Software Systems (AREA)
- Data Mining & Analysis (AREA)
- Computational Mathematics (AREA)
- Artificial Intelligence (AREA)
- Probability & Statistics with Applications (AREA)
- Navigation (AREA)
Abstract
带未知量测偏置的MEMS与UWB紧组合定位方法及系统，涉及室内定位技术领域，为解决现有的室内定位系统由于UWB输出的测距信息带未知量测偏置而导致室内定位精度低的问题。包括如下过程：选取MEMS的位置误差、速度误差、姿态角误差、陀螺偏置误差、加速度计偏置误差作为室内定位系统的导航误差状态向量，并基于此构建状态方程和量测方程；因UWB输出的测距信息带未知量测偏置，故本室内定位方法的量测噪声被假设服从非零均值高斯分布；提出正态类别分布并对其边缘化来对上述的非零均值高斯量测噪声进行建模；在贝叶斯框架下分别计算权重导航误差状态向量系统增益矩阵以及相应的估计误差协方差矩阵更新形状参数更新导航误差量的状态，实现定位结果输出。
Description
技术领域
本发明涉及室内定位技术领域，具体而言，涉及带未知量测偏置的MEMS/UWB室内定位方法。
背景技术
目前，室内定位技术在当前日益普及和精确化的定位应用中具有非常重要的研究意义和应用价值。随着室内定位技术的不断创新和改进，它能够在导航、安全监控、智能家居以及物流管理等多个领域带来更广阔的应用前景。其中MEMS/UWB紧组合定位系统，能够实现优势互补，提供丰富的传感器数据和精确的距离测量，为室内定位系统提供准确、实时和稳定的定位结果。由于室内环境通常存在信号衰减、多路径传播、信号阻塞等问题，因此提高室内定位技术的定位精确度和稳定性至关重要。卡尔曼滤波算法是目前基于MEMS/UWB紧组合室内定位技术中最常用的算法之一，它通过融合系统的动态模型和量测数据来提高状态的估计精度。但UWB输出的测距信息往往带未知量测偏置，这导致现有的室内定位方法精度降低。
目前研究室内定位系统都假设量测噪声是服从零均值的高斯分布，但带未知量测偏置的UWB室内定位系统中的量测噪声往往诱导量测噪声呈现非零均值高斯分布。因此，亟需一种室内定位方法来提高量测噪声的建模精度，能够能准确的估计量测偏置进而提高室内定位精度。
文献号为CN109764865A现有技术公开了一种基于MEMS和UWB的室内定位方法，其公开的方法通过获取连续的UWB数据从而得到移动终端的移动轨迹，然后利用移动终端上的MEMS测量模块测量得到的加速度值的振荡规律确定移动终端的运动状态。然后基于确定得到的运动状态对移动终端的移动轨迹进行修正得到移动终端当前的真实位置，从而可以获得移动终端室内环境中的正确移动轨迹。文献号为CN114216457A的现有技术公开了一种基于超宽带信号的多源数据融合定位方法及系统，其涉及非暴露空间环境下复杂建筑结构、多种服务设施及交通路线位置定位技术领域，特别涉及一种基于超宽带信号的多源融合定位方法及系统。其方法包括利用北斗定位系统确定室外定位的方法和利用IMU/UWB/PDR技术完成室内定位的方法，基于城市轨道交通环境中部署的超宽带基站，融合北斗卫星导航定位信号和MEMS传感器信息，结合DBS+UWB+PDR(航迹跟踪)+IMU多种算法，通过参数优化，实现非暴露空间的多源融合导航定位。
现有的室内定位方法针对如何提高量测噪声的建模精度较低，以能够准确地估计量测偏置进而提高室内定位精度，一直没有人提出来。
发明内容
本发明要解决的技术问题是：
本发明为了解决现有室内定位方法存在量测噪声的建模精度低，无法准确地估计量测偏置影响室内定位精度的问题，进而提供了一种带未知量测偏置的MEMS和UWB紧组合定位方法及系统(基于MEMS/UWB紧组合室内定位方法)。
本发明为解决上述技术问题所采用的技术方案：
本发明提供了带未知量测偏置的MEMS/UWB紧组合定位方法，包括以下步骤：
S1、MEMS的位置误差、速度误差、姿态角误差、陀螺偏置误差、加速度计偏置误差作为室内定位系统的导航误差状态向量并基于此构建状态方程和量测方程；
S2、带未知量测偏置的量测噪声被假设服从非零均值高斯分布；
S3、正态类别分布是多个高斯分布乘积形式，对其边缘化可得到多个高斯分布加权和形式，本方法利用正态类别分布边缘化对S2中所提出的非零均值高斯量测噪声进行建模；
S4、在贝叶斯框架下分别计算权重导航误差状态向量/>系统增益矩阵/>以及相应的估计误差协方差矩阵/>
S5、利用S4中得到的权重来更新正态类别分布中的形状参数/>
S6、利用S4中得到的导航误差状态向量来修正MEMS的导航状态向量，并实现定位结果输出。
进一步地，S1具体为：选取MEMS的位置误差、速度误差、姿态角误差、陀螺偏置误差、加速度计偏置误差作为室内定位系统的导航误差状态向量，即：
其中，δψk表示姿态角误差，分别表示在k时刻的真实速度和位置误差， 分别表示陀螺偏置误差和加速度计偏置误差；
构建室内定位的系统方程为：
δxk＝Ak-1δxk-1+Bk-1ωk-1 (2)
其中，δxk为导航误差状态向量，Ak状态转移矩阵，Bk控制输入矩阵，ωk为系统噪声；
状态转移矩阵Ak和控制输入矩阵Bk的表达式为：
估计的导航误差向量反馈给MEMS，故可得：
ferr(015×1,xk,uk)＝015×1 (3)
根据公式(3)，本室内定位方法估计的导航误差向量反馈到MEMS，因此每个时间更新中的一步预测状态向量被设置为零。一步预测估计误差协方差更新如下：
其中Fk和Gk分别为进行离散化的Ak和Bk。
S2具体为：当UWB输出的测距值可用的时候，室内定位方法进入量测更新阶段。量测方程如下：
其中，Hk是量测矩阵，vk是量测噪声。由于UWB输出的测距信息带未知量测偏置，故本室内定位方法假设量测噪声vk是服从非零均值高斯分布。
量测矩阵Hk进行线性化后表示如下：
根据误差协方差，卡尔曼滤波增益Kk计算如下：
其中Rk为量测噪声。
进一步地，S3具体为：将量测噪声的概率密度函数建模成正态-类别分布：
其中均值和相应的方差/> 表示随机向量且服从类别分布；/>表示混合概率向量且服从狄利克雷分布，具体表达式如下：
其中，狄利克雷分布的先验形状参数通过遗忘因子ρ∈(0,1]传播上一时刻的信息来获得的，即/> 是上一时刻获得的后验形状参数。
将对概率密度函数边缘化，量测噪声的概率密度/>可以建模成5个高斯和形式：
进一步地,将一步预测概率密度函数p(δxk|δz1:k-1)可以表示为:
p(δxk|δz1:k-1)＝N(δxk；015×1,Pk|k-1) (11)
其中，因为导航误差状态在上一时刻就反馈给了MEMS。所以一步预测概率密度函数p(δxk|δz1:k-1)的均值向量设为015×1。误差协方差矩阵Pk|k-1可以通过经典卡尔曼滤波算法中的时间更新过程可得。
进一步地，S4中计算相关参数：
采用变分贝叶斯方法计算后验概率密度可以利用NC分布的性质和贝叶斯准则进行计算，具体表达式如下：
其中表示权重，/>表示状态估计向量，/>为误差估计协方差矩阵。具体计算如下：
进一步地，S5具体为：
根据VB方法将混合概率向量的后验概率密度函数更新为狄利克雷分布，并对狄利克雷分布中的形状参数/>更新计算，表达式如下：
由于在进行状态估计时每一时刻的概率值都是相互独立的，仅仅依赖于残差和统计信息进行更新。
进一步地，S6具体为：
根据权利要求6所述的带未知量测偏置的MEMS/UWB紧组合定位方法，其特征在于，将导航误差反馈到MEMS，并对状态估计值即提供协方差矩阵进行状态更新，即：
Pk＝Pk|k (16)
带未知量测偏置的MEMS/UWB紧组合定位方法，该紧组合定位系统具有与上述技术方案中任一项的步骤对应的程序模块，运行时执行上述的带未知量测偏置的MEMS/UWB紧组合定位方法中的步骤。
一种计算机可读存储介质，其特征在于：所述计算机可读存储介质存储有计算机程序，所述计算机程序配置为由处理器调用时实现上述的带未知量测偏置的MEMS和UWB紧组合定位方法的步骤。
相较于现有技术，本发明的有益效果是：
经研究发现，UWB输出带未知量测偏置的测距信息诱导基于MEMS/UWB紧组合系统中的量测噪声呈现非零均值高斯分布，这导致了现有的基于零均值假设条件的室内定位方法精度下降。因此，本发明提出通过提高量测噪声的建模精度来准确地估计量测偏置，从而实现室内定位精度的大幅度提高。
本发明基于带未知量测偏置的MEMS/UWB紧组合定位方法，在紧组合模式下由MEMS输出的状态信息辅助UWB系统完成定位，利用MEMS短期定位精度高且抗干扰性强的优点，使得UWB系统在测距信息不足的情况下也能够实现定位。其中将带量测偏置的UWB诱导的非零均值量测噪声进一步分析，并将非零均值量测噪声按正态-类别分布建模，来提高对非零均值量测噪声建模精度，降低估计误差，进而提高系统的定位精度。
本发明根据正态-类别分布来估计量测噪声与经典的卡尔曼滤波器相比，在量测噪声具有未知量测偏置情况下，通过对量测噪声的非零均值实时估计来更新状态估计，能够有效降低带未知量测偏置的量测噪声对定位精度产生的影响。
本发明提出的基于MEMS/UWB紧组合室内定位方法，有效解决了现有的室内定位系统由于UWB输出的测距信息带未知量测偏置而导致室内定位精度低的问题。包括如下过程：选取MEMS的位置误差、速度误差、姿态角误差、陀螺偏置误差、加速度计偏置误差作为室内定位系统的导航误差状态向量，并基于此构建状态方程和量测方程；因UWB输出的测距信息带未知量测偏置，故本室内定位方法的量测噪声被假设服从非零均值高斯分布；提出正态类别分布并对其边缘化来对上述的非零均值高斯量测噪声进行建模；在贝叶斯框架下分别计算权重，导航误差状态向量，系统增益矩阵以及相应的估计误差协方差矩阵；更新形状参数；更新导航误差量的状态，实现定位结果输出。本发明所述带未知量测偏置的MEMS与UWB紧组合定位方法可用于有遮挡非视距条件下的室内定位。
附图说明
图1为本发明实施例中带未知量测偏置的MEMS/UWB紧组合定位方法流程图；
图2为本发明实施例中不同滤波方法的轨迹对比图；
图3为本发明实施例中不同滤波方法的误差分布函数图；
图4为本发明实施例中不同滤波方法的误差对比图；
表1为本发明实施例中不同滤波方法的误差对比。
具体实施方式
在本发明的描述中，应当说明的是，在本发明的实施例中所提到的术语“第一”、“第二”、“第三”仅用于描述目的，并不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”、“第三”的特征可以明示或者隐含地包括一个或者多个该特征。
为使本发明的上述目的、特征和优点能够更为明显易懂，下面结合附图对本发明的具体实施例做详细的说明。
如图1所示，本发明提供带未知量测偏置的MEMS/UWB紧组合定位方法，包括如下步骤：
S1、MEMS的位置误差、速度误差、姿态角误差、陀螺偏置误差、加速度计偏置误差作为室内定位系统的导航误差状态向量并基于此构建状态方程和量测方程；
S2、带未知量测偏置的量测噪声被假设服从非零均值高斯分布；
S3、正态类别分布是多个高斯分布乘积形式，对其边缘化可得到多个高斯分布加权和形式，本方法利用正态类别分布边缘化对S2中所提出的非零均值高斯量测噪声进行建模；
S4、在贝叶斯框架下分别计算权重导航误差状态向量/>系统增益矩阵/>以及相应的估计误差协方差矩阵/>
S5、利用S4中得到的权重来更新正态类别分布中的形状参数/>
S6、利用S4中得到的导航误差状态向量来修正MEMS的导航状态向量，并实现定位结果输出。
S1具体为：选取MEMS的位置误差、速度误差、姿态角误差、陀螺偏置误差、加速度计偏置误差作为室内定位系统的导航误差状态向量，即：
其中，δψk表示姿态角误差，分别表示在k时刻的真实速度和位置误差， 分别表示陀螺偏置误差和加速度计偏置误差；
构建室内定位的系统方程为：
δxk＝Ak-1δxk-1+Bk-1ωk-1 (2)
其中，δxk为导航误差状态向量，Ak状态转移矩阵，Bk控制输入矩阵，ωk为系统噪声；
状态转移矩阵Ak和控制输入矩阵Bk的表达式为：
估计的导航误差向量反馈给MEMS，故可得：
ferr(015×1,xk,uk)＝015×1 (3)
根据公式(3)，本室内定位方法估计的导航误差向量反馈到MEMS，因此每个时间更新中的一步预测状态向量被设置为零。一步预测估计误差协方差更新如下：
其中Fk和Gk分别为进行离散化的Ak和Bk。
S2具体为：当UWB输出的测距值可用的时候，室内定位方法进入量测更新阶段。量测方程如下：
其中，Hk是量测矩阵，vk是量测噪声。由于UWB输出的测距信息带未知量测偏置，故本室内定位方法假设量测噪声vk是服从非零均值高斯分布。
量测矩阵Hk进行线性化后表示如下：
根据误差协方差，卡尔曼滤波增益Kk计算如下：
其中Rk为量测噪声。
S3中将量测噪声的概率密度函数建模成正态-类别分布为：
其中均值和相应的方差/> 表示随机向量且服从类别分布；/>表示混合概率向量且服从狄利克雷分布，具体表达式如下：
其中，狄利克雷分布的先验形状参数通过遗忘因子ρ∈(0,1]传播上一时刻的信息来获得的，即/> 是上一时刻获得的后验形状参数。
将对概率密度函数边缘化，量测噪声的概率密度/>可以建模成5个高斯和形式：
将一步预测概率密度函数p(δxk|δz1:k-1)可以表示为:
p(δxk|δz1:k-1)＝N(δxk；015×1,Pk|k-1) (11)
其中，因为导航误差状态在上一时刻就反馈给了MEMS。所以一步预测概率密度函数p(δxk|δz1:k-1)的均值向量设为015×1。误差协方差矩阵Pk|k-1可以通过经典卡尔曼滤波算法中的时间更新过程可得。
S4中计算相关参数，具体为：
采用变分贝叶斯方法计算后验概率密度可以利用NC分布的性质和贝叶斯准则进行计算，具体表达式如下：
其中表示权重，/>表示状态估计向量，/>为误差估计协方差矩阵。具体计算如下：
S5中计算形状参数具体为：
根据VB方法将混合概率向量的后验概率密度函数更新为狄利克雷分布，并对狄利克雷分布中的形状参数/>更新计算，表达式如下：
由于在进行状态估计时每一时刻的概率值都是相互独立的，仅仅依赖于残差和统计信息进行更新。
S6中将导航误差反馈到MEMS，并对状态估计值即提供协方差矩阵进行状态更新，即：
Pk＝Pk|k (16)
实施例1
在测量过程中存在过程噪声和量测噪声时，对本方法提出带未知量测偏置的MEMS/UWB紧组合定位方法(NCKF)进行试验验证分析，并于其他几种传统的定位滤波方法最小二乘法(LS)和经典的卡尔曼滤波算法(Kalman Filter，KF)进行对比。
在实验分析中，对本发明的带未知量测偏置的MEMS/UWB紧组合定位方法，最小二乘法(LS)和经典的卡尔曼滤波算法(Kalman Filter，KF)进行比较分析，得到如图2所示的不同滤波器下的定位轨迹分布图。从图2可以看出，所提出的方法相比最小二乘法(LS)和经典的卡尔曼滤波算法(Kalman Filter，KF)能够更加贴近真实轨迹，表现出较好的定位性能。
从图3和图4可以看出卡尔曼滤波算法相比较于LS算法能够更好的起到数据的滤波和平滑作用，提高定位系统的稳定性。所提出的方法NCKF滤波算法相比较于经典的KF滤波算法在数据处理的过程中更加的平滑能够起到更好的滤波效果。且本发明所提出的方法的平均定位精度是呈现了定位优势，为0.1170m，LS算法和KF算法的平均定位精度分别为0.1737m和0.1817m。同时可知NCKF的定位误差的方差值最小，可以表明该算法相比于其他两种算法有更高的定位精度和稳定性。
由误差统计表1可知，带未知量测偏置的MEMS/UWB紧组合定位方法(NCKF)平均误差相比于LS算法和KF算法降低了33％和36％，误差最大值比LS算法和KF算法降低了47％和10％，方差比LS算法和KF算法降低了75％和40％。通过上述分析表明，NCKF平滑可以有效降低定位误差，优化定位结果。
表1
本发明所述方法经仿真实验和实际应用，均验证了本发明所声称的定位效果。
本发明提出的算法(方法)是本发明底层的技术内核，基于所述算法能衍生出各种产品。基于本发明提出的算法(方法)利用程序语言开发出带未知量测偏置的MEMS与UWB紧组合定位系统，该系统具有与上述技术方案的步骤对应的程序模块，运行时执行上述的带未知量测偏置的MEMS与UWB紧组合定位方法中的步骤。
将开发出的系统(软件)的计算机程序存储在计算机可读存储介质上，所述计算机程序配置为由处理器调用时实现上述的带未知量测偏置的MEMS与UWB紧组合定位方法及系统的步骤。即将本发明物化在载体上，成为一种计算机程序产品。
应该理解，可以使用上面所示的各种形式的流程，重新排序、增加或删除步骤。例如，本发申请中记载的各步骤可以并行地执行也可以顺序地执行也可以不同的次序执行，只要能够实现本申请公开的技术方案所期望的结果，都在本发明的保护范围之内。
Claims (5)
1.带未知量测偏置的MEMS和UWB紧组合定位方法，其特征在于，所述方法包括以下步骤：
S1、MEMS的位置误差、速度误差、姿态角误差、陀螺偏置误差、加速度计偏置误差作为室内定位系统的导航误差状态向量并基于此构建状态方程和量测方程；
S2、带未知量测偏置的量测噪声被假设服从非零均值高斯分布；
S3、正态类别分布是多个高斯分布乘积形式，对其边缘化可得到多个高斯分布加权和形式，利用正态类别分布边缘化对S2中所提出的非零均值高斯量测噪声进行建模；
S4、在贝叶斯框架下分别计算权重导航误差状态向量/>系统增益矩阵/>以及相应的估计误差协方差矩阵/>
S5、利用S4中得到的权重来更新正态类别分布中的形状参数/>
S6、利用S4中得到的导航误差状态向量来修正MEMS的导航状态向量，并实现定位结果输出；
当UWB输出的测距值可用的时候，室内定位方法进入量测更新阶段，量测方程如下：
其中，Hk是量测矩阵，vk是量测噪声；由于UWB输出的测距信息带未知量测偏置，故假设量测噪声vk是服从非零均值高斯分布的；
量测矩阵Hk进行线性化后表示如下：
根据误差协方差，卡尔曼滤波增益Kk计算如下：
其中Rk为量测噪声；
S3中将非零均值高斯量测噪声的概率密度函数建模成正态类别分布：
其中均值相应的方差/> 表示随机向量且服从类别分布；/>表示混合概率向量且服从狄利克雷分布，具体表达式如下：
其中，狄利克雷分布的先验形状参数通过遗忘因子ρ∈(0,1]传播上一时刻的信息来获得的，即/> 是上一时刻获得的后验形状参数；
对概率密度函数边缘化，量测噪声的概率密度/>建模成5个高斯和形式：
其特征在于，S4中计算相关参数：
采用变分贝叶斯方法计算后验概率密度可以利用NC分布的性质和贝叶斯准则进行计算，具体表达式如下：
其中表示权重，/>表示状态估计向量，/>为误差估计协方差矩阵；具体计算如下：
S5中计算形状参数为：
根据VB方法将混合概率向量的后验概率密度函数更新为狄利克雷分布，并对狄利克雷分布中的形状参数/>更新计算，表达式如下：
由于在进行状态估计时每一时刻的概率值都是相互独立的，仅依赖于残差和统计信息进行更新；
将导航误差反馈到MEMS，并对状态估计值即提供协方差矩阵进行状态更新，即：
Pk＝Pk|k (16)。
2.根据权利要求1所述的带未知量测偏置的MEMS和UWB紧组合定位方法，其特征在于，S1具体为：选取MEMS的位置误差、速度误差、姿态角误差、陀螺偏置误差、加速度计偏置误差作为室内定位系统的导航误差状态向量，即：
其中，δψk表示姿态角误差，分别表示在k时刻的真实速度和位置误差，/> 分别表示陀螺偏置误差和加速度计偏置误差；
构建室内定位的系统方程为：
δxk＝Ak-1δxk-1+Bk-1ωk-1 (2)
其中，δxk为导航误差状态向量，Ak状态转移矩阵，Bk控制输入矩阵，ωk为系统噪声；
状态转移矩阵Ak和控制输入矩阵Bk的表达式为：
估计的导航误差向量反馈给MEMS，故可得：
ferr(015×1,xk,uk)＝015×1 (3)
根据公式(3)，估计的导航误差向量反馈到MEMS，因此每个时间更新中的一步预测状态向量被设置为零；一步预测估计误差协方差更新如下：
其中Fk和Gk分别为进行离散化的Ak和Bk。
3.根据权利要求1所述的带未知量测偏置的MEMS和UWB紧组合定位方法，其特征在于，所述带未知量测偏置的MEMS与UWB紧组合定位方法可用于有遮挡非视距条件下的室内定位。
4.带未知量测偏置的MEMS和UWB紧组合定位系统，其特征在于，该紧组合定位系统具有与上述权利要求1～3任一项权利要求的步骤对应的程序模块，运行时执行上述的带未知量测偏置的MEMS和UWB紧组合定位方法中的步骤。
5.一种计算机可读存储介质，其特征在于：所述计算机可读存储介质存储有计算机程序，所述计算机程序配置为由处理器调用时实现权利要求1～3中任一项所述的带未知量测偏置的MEMS和UWB紧组合定位方法的步骤。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311376185.1A CN117367431B (zh) | 2023-10-23 | 2023-10-23 | 带未知量测偏置的mems与uwb紧组合定位方法及系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311376185.1A CN117367431B (zh) | 2023-10-23 | 2023-10-23 | 带未知量测偏置的mems与uwb紧组合定位方法及系统 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN117367431A CN117367431A (zh) | 2024-01-09 |
| CN117367431B true CN117367431B (zh) | 2024-05-14 |
Family
ID=89388827
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311376185.1A Active CN117367431B (zh) | 2023-10-23 | 2023-10-23 | 带未知量测偏置的mems与uwb紧组合定位方法及系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117367431B (zh) |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114370873A (zh) * | 2022-01-19 | 2022-04-19 | 东南大学 | 一种基于改进ukf的星光折射cns/sins组合导航方法 |
| CN114860706A (zh) * | 2022-05-05 | 2022-08-05 | 东北电力大学 | 一种基于时空联合的配电网量测数据清洗方法 |
| CN115451946A (zh) * | 2022-07-25 | 2022-12-09 | 宁波大学 | 一种MEMS-IMU和Wi-Fi组合的室内行人定位方法 |
| CN116182867A (zh) * | 2023-02-22 | 2023-05-30 | 东南大学 | 复杂室内环境下基于紧组合的ins/uwb无人车定位方法 |
| CN116644264A (zh) * | 2023-05-26 | 2023-08-25 | 北京航空航天大学 | 一种非线性偏置-不变卡尔曼滤波方法 |
| CN116907503A (zh) * | 2023-07-10 | 2023-10-20 | 哈尔滨工业大学（威海） | 一种基于抗野值鲁棒定位算法的遥感编队卫星定位方法及系统 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US7289906B2 (en) * | 2004-04-05 | 2007-10-30 | Oregon Health & Science University | Navigation system applications of sigma-point Kalman filters for nonlinear estimation and sensor fusion |
-
2023
- 2023-10-23 CN CN202311376185.1A patent/CN117367431B/zh active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114370873A (zh) * | 2022-01-19 | 2022-04-19 | 东南大学 | 一种基于改进ukf的星光折射cns/sins组合导航方法 |
| CN114860706A (zh) * | 2022-05-05 | 2022-08-05 | 东北电力大学 | 一种基于时空联合的配电网量测数据清洗方法 |
| CN115451946A (zh) * | 2022-07-25 | 2022-12-09 | 宁波大学 | 一种MEMS-IMU和Wi-Fi组合的室内行人定位方法 |
| CN116182867A (zh) * | 2023-02-22 | 2023-05-30 | 东南大学 | 复杂室内环境下基于紧组合的ins/uwb无人车定位方法 |
| CN116644264A (zh) * | 2023-05-26 | 2023-08-25 | 北京航空航天大学 | 一种非线性偏置-不变卡尔曼滤波方法 |
| CN116907503A (zh) * | 2023-07-10 | 2023-10-20 | 哈尔滨工业大学（威海） | 一种基于抗野值鲁棒定位算法的遥感编队卫星定位方法及系统 |
Non-Patent Citations (3)
| Title |
|---|
| A novel adaptive kalman filter with unknown probability of measurement lost;Jia Guangle;IEEE Signal Processing Letters;20191204;第26卷(第12期);全文 * |
| Nonlinear state esrimation using neural-cubature kalman filter;MIao Zhiyong;Automatika;20180321;全文 * |
| UWB/IMU integration approach with NLOS identification and mitigation;Zeng Zhouqi等;2018 52nd Annual Conference on Information Sciences and Systems;20180524;全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN117367431A (zh) | 2024-01-09 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Shen et al. | Dual-optimization for a MEMS-INS/GPS system during GPS outages based on the cubature Kalman filter and neural networks | |
| CN113074739B (zh) | 基于动态鲁棒容积卡尔曼的uwb/ins融合定位方法 | |
| CN110208740A (zh) | Tdoa-imu数据自适应融合定位装置及方法 | |
| CN113108791A (zh) | 一种导航定位方法及导航定位设备 | |
| Ullah et al. | Evaluation of localization by extended Kalman filter, unscented Kalman filter, and particle filter-based techniques | |
| Zhou et al. | GNSS windowing navigation with adaptively constructed dynamic model | |
| Aslinezhad et al. | ANN-assisted robust GPS/INS information fusion to bridge GPS outage | |
| CN109520503A (zh) | 一种平方根容积模糊自适应卡尔曼滤波slam方法 | |
| CN106643724A (zh) | 基于地图信息和位置自适应修正的粒子滤波室内定位方法 | |
| KR101390776B1 (ko) | 퍼지 확장 칼만 필터를 이용한 위치인식 장치, 방법 및 이동로봇 | |
| CN113701742B (zh) | 一种基于云端与边端融合计算的移动机器人slam方法 | |
| Zhao et al. | Fusing vehicle trajectories and gnss measurements to improve gnss positioning correction based on actor-critic learning | |
| Chen et al. | An optimal selection of sensors in multi-sensor fusion navigation with factor graph | |
| CN117367431B (zh) | 带未知量测偏置的mems与uwb紧组合定位方法及系统 | |
| CN111291319B (zh) | 一种应用于非高斯噪声环境下的移动机器人状态估计方法 | |
| Zhang et al. | Adaptive cubature Kalman filter based on the variance-covariance components estimation | |
| CN105549003A (zh) | 一种汽车雷达目标跟踪方法 | |
| Lai et al. | Adaptive fuzzy neural network-aided progressive Gaussian approximate filter for GPS/INS integration navigation | |
| Chen et al. | On SINS/GPS integrated navigation filtering method aided by radial basis function neural network | |
| CN116048091B (zh) | 一种考虑位姿估计不确定性的机器人轨迹规划方法和装置 | |
| Jwo et al. | Application of optimization technique for GPS navigation Kalman filter adaptation | |
| CN115574818B (zh) | 基于改进联邦滤波的结构化道路车辆导航定位方法及系统 | |
| Murthy et al. | Application of fuzzy logic based Kalman filter and vehicle rate sensor in optimizing differential global position system | |
| Witzgall et al. | Doppler geolocation with drifting carrier | |
| CN117271967B (zh) | 一种基于强化学习补偿滤波的救援协同定位方法及系统 |
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 |