CN110401430A - 一种基于kld采样的改进高斯粒子滤波数据融合算法 - Google Patents
一种基于kld采样的改进高斯粒子滤波数据融合算法 Download PDFInfo
- Publication number
- CN110401430A CN110401430A CN201811165547.1A CN201811165547A CN110401430A CN 110401430 A CN110401430 A CN 110401430A CN 201811165547 A CN201811165547 A CN 201811165547A CN 110401430 A CN110401430 A CN 110401430A
- Authority
- CN
- China
- Prior art keywords
- particle
- kld
- state
- moment
- particles
- 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
Links
- 239000002245 particle Substances 0.000 title claims abstract description 142
- 238000001914 filtration Methods 0.000 title claims abstract description 43
- 238000005070 sampling Methods 0.000 title claims abstract description 12
- 230000006872 improvement Effects 0.000 title claims description 4
- 238000000605 extraction Methods 0.000 claims description 6
- 238000000034 method Methods 0.000 abstract description 11
- 230000008569 process Effects 0.000 abstract description 5
- 230000000694 effects Effects 0.000 abstract description 4
- 238000005312 nonlinear dynamic Methods 0.000 abstract description 2
- 230000003044 adaptive effect Effects 0.000 abstract 1
- 230000006870 function Effects 0.000 description 16
- 238000010586 diagram Methods 0.000 description 3
- 238000002474 experimental method Methods 0.000 description 3
- 238000012952 Resampling Methods 0.000 description 2
- 230000008859 change Effects 0.000 description 2
- 230000003247 decreasing effect Effects 0.000 description 2
- 238000000342 Monte Carlo simulation Methods 0.000 description 1
- 230000008901 benefit Effects 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 230000035772 mutation Effects 0.000 description 1
Classifications
-
- H—ELECTRICITY
- H03—ELECTRONIC CIRCUITRY
- H03H—IMPEDANCE NETWORKS, e.g. RESONANT CIRCUITS; RESONATORS
- H03H17/00—Networks using digital techniques
- H03H17/02—Frequency selective networks
- H03H17/0248—Filters characterised by a particular frequency response or filtering method
- H03H17/0282—Sinc or gaussian filters
Landscapes
- Physics & Mathematics (AREA)
- Engineering & Computer Science (AREA)
- Computer Hardware Design (AREA)
- Mathematical Physics (AREA)
- Management, Administration, Business Operations System, And Electronic Commerce (AREA)
Abstract
本发明提出的一种基于KLD改进的高斯粒子滤波算法(KLD‑GPF)，属于信号处理技术领域，涉及非线性滤波，本发明提供的方法适用于非线性动态系统的状态估计。本算法能自适应的调节粒子数目，对于噪声服从高斯分布且噪声的统计特性突变的情况下具有显著的效果。该滤波算法在采样过程中在线计算粒子的离散概率密度函数(PDF)与真实后验概率密度函数的Kullback‑Leibler(KL)距离，并根据KL距离在线调整粒子集大小，因此该算法具有较好的鲁棒性。KLD‑GPF在噪声统计特性突变的情况下，能够保持较好的估计效果。与KLD改进的粒子滤波算法(KLD‑PF)相比，虽然损失了一些滤波精度，但滤波速度大幅提升。
Description
技术领域
本发明提出的一种基于KLD改进的高斯粒子滤波算法，属于信号处理技术领域，涉及非线性滤波，本发明提供的方法适用于非线性动态系统的状态估计。
背景技术
非线性滤波问题出现在许多领域，其中包括目标跟踪、捷联惯导系统以及姿态估计等。扩展卡尔曼滤波EKF(Extend Kalman Filter)是对非线性函数进行线性化处理，对高阶项直接截断，从而导致较大的误差，滤波精度低。无迹卡尔曼滤波UKF是使用无迹变换的卡尔曼滤波，相对于EKF其滤波精度有所提高，但其非线性传递误差始终存在。由于扩展卡尔曼滤波和无迹卡尔曼滤波都是基于卡尔曼滤波的改进，而卡尔曼滤波算法是基于线性高斯假设下的一种算法，因此在对非线性系统的滤波处理，存在较大的误差。粒子滤波算法PF(Praticle Filter)是一种基于蒙特卡洛方法的滤波算法，但粒子滤波算法中重采样策略将直接影响滤波的性能，使算法的实时性受到影响。高斯粒子滤波GPF(Gaussian ParticleFilter)在粒子滤波的框架下，使用高斯分布对状态的先验及后验分布进行近似，是一种免重采样滤波算法，相对粒子滤波有着较好的实时性。但是非线性系统受到强干扰的情况下，因为固定粒子数目，不仅估计误差大且容易出现发散现象。
发明内容
本发明专利针对上述存在的问题和不足，提出一种基于KLD(Kullback-LeiblerDivergence)改进的高斯粒子滤波数据融合算法，该算法采样过程中在线计算粒子的离散概率密度函数PDF与真实后验概率密度函数的KL距离，并根据KL距离在线调整粒子集大小，在噪声统计特性突变的情况下，能够保持较好的估计效果。其中，KL距离是Kullback-Leibler距离的简称，也叫做相对熵，它衡量的是相同事件空间里的两个概率分布的差异情况。
本发明具体过程如下：
步骤1：由先验信息初始化粒子集其中：x为状态量，为初始粒子集，i为粒子索引，M为粒子总数；
步骤2：计算相应的权值并归一化其中：为第i个粒子n时刻的权值，为第i个粒子n时刻归一化后的权值，yn为n时刻的观测量，为第i个粒子n时刻的状态值，为n时刻观测量的似然函数；
步骤3：估计当前时刻粒子集的均值及方差；
其中：μn为n时刻粒子均值，Σn为n时刻粒子方差；
步骤4：从状态量的后验分布中抽取粒子其中：表示正态分布，为粒子集，M为总粒子数；
步骤5：根据对每个粒子进行状态更新，获得状态更新后n+1时刻的粒子集其中：为状态的先验概率密度函数，为状态更新后n+1时刻的粒子集；
步骤6：计算状态更新后粒子集的均值及方差；
其中：μn+1为更新后n+1时刻粒子集的均值，Σn+1为更新后n+1时刻粒子集的方差；
步骤7：根据状态维数p设定区间范围为并将该区间分为m个小区间；其中：为上分位数为1-θ自由度为1的卡方分布，p为状态维数；
其中在区间设定范围中，引入马氏距离，其样本Xi与总体X之间的马氏距离定义如下：
其中：为μ总体均值，∑为总体方差。
现设定样本的最大范围为Xi(max)＝μ±aBb，a为未知量且a＞0，则有：
P((X-μ)T∑-1(X-μ)≤(aBb)T∑-1(aBb))＝1-δ
进一步的可以写为：
其中：∑＝BBT，b为与B维数相同元素全为1的向量，1-δ为上分位数。由上式可得：
等式右边会随着p的增大而逐渐减小，当p＝1时，a取得最大值故将区间设置为
将该区间分为m个小区间，其每块区间的大小为建立区间映射表，表建立如表1：
表1 区间映射表
其中index为当前粒子所在的区间索引，Value为粒子区间。
步骤8：具体包含以下步骤
8.1从重要概率密度函数中抽取一个粒子同时将粒子数目加1，记为M＝M+1，并计算该粒子与粒子集总体之间的马氏距离；
8.2判断粒子所在的区间有无其他粒子，若无其他粒子则将非空区间数加1，记为k＝k+1；
其中：k为非空区间数；
其中判断方法具体为：根据步骤7所建立的区间映射表，计算采样粒子与总体之间的马氏距离，再构建区间映射函数，区间映射函数构建如下：
其中，[·]floor为向下取整函数，Xn为当前时刻总体均值，为当前时刻第i个状态值，index为当前粒子所在的区间索引，MD(·)为马氏距离计算函数；
8.3判断粒子数目是否小于KLD采样设置的粒子数目，如果则转至步骤8的8.1，否则转至步骤8.2；其中：为KLD采样设置的粒子数目，为自由度为k-1、上分位数为1-δ的卡方分布对应的值，e为设定的KLD阈值；
本发明具有如下优点：将KLD采样与高斯粒子滤波结合，解决了在非线性系统受到强干扰时高斯粒子滤波及粒子滤波发散问题，相较于基于KLD采样的粒子滤波，滤波速度大幅提升，有较好的实时性。其次，该算法引入马氏距离，使得区间的划分不受量纲的影响，从而该算法能更容易的与其他应用环境相结合。
附图说明
图1本滤波算法结构框图；
图2正常情况下本发明与高斯粒子滤波、粒子滤波、KLD粒子滤波的粒子数目变化图；
图3受到500倍正常噪声的强干扰情况下本发明与高斯粒子滤波、粒子滤波、KLD粒子滤波的粒子数目变化图；
图4受到500倍正常噪声的强干扰情况下本发明与高斯粒子滤波、粒子滤波、KLD粒子滤波的RMSE误差图；
图5干扰噪声逐渐增大的情况下本发明与高斯粒子滤波、粒子滤波、KLD粒子滤波的RMSE误差图；
具体实施方式
下面详细描述本发明的实施例，本实施例是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。参照说明书附图对本发明的一种基于KLD改进的高斯粒子滤波算法作以下详细地说明：
为更好体现本发明的具体步骤实施及效果，搭建如下仿真实验：采用一维强非线性模型，其模型函数可写为如下形式：
仿真实验环境及相关参数如下：仿真软件为MATLAB，硬件环境为Intel i5-6500，最高主频为3.20GHz，运行内存为7.87GB。初始粒子群为Qn为过程噪声Rn为观测噪声在正常情况下其大小分别设为σQ＝1，σR＝1，在强干扰情况下其大小分别设为σQ＝500，σR＝500，在干扰噪声逐渐增大的情况下其大小分别设为σQ＝15～1500，σR＝15～1500。
针对以上仿真实验环境，根据图1滤波算法结构框图，具体步骤实施如下：
相关KLD改进的高斯粒子滤波参数设置为e＝0.15，δ＝0.99，θ＝1×10-9子区间数目设置为m＝[20∑n]ceil。
步骤1：由先验信息初始化粒子集其中：x为状态量，为初始粒子集，i为粒子索引，M为粒子总数；
步骤2：计算相应的权值并归一化其中：为第i个粒子n时刻的权值，为第i个粒子n时刻归一化后的权值，yn为n时刻的观测量，为第i个粒子n时刻的状态值，为n时刻观测量的似然函数；
步骤3：估计当前时刻粒子集的均值及方差；
其中：μn为n时刻粒子均值，Σn为n时刻粒子方差；
步骤4：从状态量的后验分布中抽取粒子其中：表示正态分布，为粒子集，M为总粒子数；
步骤5：根据对每个粒子进行状态更新，获得状态更新后n+1时刻的粒子集其中：为状态的先验概率密度函数，为状态更新后n+1时刻的粒子集；
步骤6：计算状态更新后粒子集的均值及方差；
其中：μn+1为更新后n+1时刻粒子集的均值，Σn+1为更新后n+1时刻粒子集的方差；
步骤7：根据状态维数p设定区间范围为并将区间分为[20Σn]ceil个小区间。其中在区间的范围设定中，根据KLD改进的高斯粒子滤波参数θ＝1×10-9可将区间范围为
步骤8：具体包含以下步骤
8.1从重要概率密度函数中抽取一个粒子同时将粒子数目加1，记为M＝M+1，并计算该粒子与粒子集总体之间的马氏距离；
8.2判断粒子所在的区间有无其他粒子，若无其他粒子则将非空区间数加1，记为k＝k+1；
其中：k为非空区间数；
其中判断方法具体为：根据步骤7所建立的区间映射表，计算采样粒子与总体之间的马氏距离，再构建区间映射函数，区间映射函数构建如下：
其中，[·]floor为向下取整函数，Xn为当前时刻总体均值，为当前时刻第i个状态值，index为当前粒子所在的区间索引，MD(·)为马氏距离计算函数；
8.3判断粒子数目是否小于KLD采样设置的粒子数目，如果则转至步骤8的8.1，否则转至步骤8.2；其中：为KLD采样设置的粒子数目，为自由度为k-1、上分位数为1-δ的卡方分布对应的值，e为设定的KLD阈值；
如图2所示，在正常情况下本发明(KLD-GPF)与KLD粒子滤波(KLD-PF)的粒子数目变化相似，能够实时的调整粒子数目，且总体的平均粒子数目与高斯粒子滤波和粒子滤波相似。
如图3所示，在受到500倍正常噪声的强干扰情况下本发明的粒子数目明显增加，并根据图4中能明显看出本发明相比高斯粒子滤波、粒子滤波的RMSE值有明显下降。
如图5所示，在干扰噪声逐渐增大的情况下本发明相比高斯粒子滤波与粒子滤波的RMSE值同样有明显下降，且随着噪声的增大越加明显。上述各RMSE具体值见表2。
表2 RMSE值
在不同情况下本发明与各算法的时间消耗如表3所示，可以明显看出在正常情况下本发明算法的时间消耗要明显少于粒子滤波与KLD粒子滤波，在干扰情况下本发明算法的时间消耗要明显少于KLD粒子滤波，由于在强干扰情况下，高斯粒子滤波与粒子滤波容易发散，其算法时间消耗不列入表中比较。
表3 算法时间消耗
Claims (3)
1.一种基于KLD采样的改进高斯粒子滤波算法，其特征在于，具体包括以下步骤：
步骤1：由先验信息初始化粒子集其中：x为状态量，为初始粒子集，i为粒子索引，M为粒子总数；
步骤2：计算相应的权值并归一化其中：为第i个粒子n时刻的权值，为第i个粒子n时刻归一化后的权值，yn为n时刻的观测量，为第i个粒子n时刻的状态值，为n时刻观测量的似然函数；
步骤3：估计当前时刻粒子集的均值及方差；
其中：μn为n时刻粒子均值，∑n为n时刻粒子方差；
步骤4：从状态量的后验分布中抽取粒子其中：表示正态分布，为粒子集；
步骤5：根据对每个粒子进行状态更新，获得状态更新后n+1时刻的粒子集其中：为状态的先验概率密度函数，为状态更新后n+1时刻的粒子集；
步骤6：计算状态更新后粒子集的均值及方差，
其中：μn+1为更新后n+1时刻粒子集的均值，∑n+1为更新后n+1时刻粒子集的方差；
步骤7：根据状态维数p设定区间范围为并将该区间分为m个小区间；其中：p为状态维数，为上分位数为1-θ，自由度为1的卡方分布；
步骤8：具体包含以下步骤
8.1从重要概率密度函数中抽取一个粒子同时将粒子数目加1，记为M＝M+1，并计算该粒子与粒子集总体之间的马氏距离；
8.2判断粒子所在的区间有无其他粒子，若无其他粒子则将非空区间数加1，记为k＝k+1；
其中：k为非空区间数；
8.3判断粒子数目是否小于KLD采样设置的粒子数目，如果则转至步骤8的8.1，否则转至步骤8.2；其中：为KLD采样设置的粒子数目，为自由度为k-1、上分位数为1-δ的卡方分布对应的值，e为设定的KLD阈值。
2.权利要求1步骤7中，根据状态维数p设定区间范围，并将该区间分为m个小区间，其特征在于，引入马氏距离将区间设置为并将该区间均匀分成个m小区间，其每块区间的大小为根据Δ值建立区间映射表；其中：∑＝BBT，b为与B维数相同元素全为1的向量，∑为粒子群方差，为自由度为1、上分位数为1-δ的卡方分布对应的值。
3.权利要求1步骤8.2中，判断粒子所在的区间有无其他粒子，其特征在于，计算采样粒子与总体之间的马氏距离，再构建区间映射函数，区间映射函数构建如下：
其中：[·]floor为向下取整函数，Xn为当前时刻总体均值，为当前时刻第i个状态值，index为当前粒子所在的区间索引，MD(·)为马氏距离计算函数。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811165547.1A CN110401430A (zh) | 2018-09-28 | 2018-09-28 | 一种基于kld采样的改进高斯粒子滤波数据融合算法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811165547.1A CN110401430A (zh) | 2018-09-28 | 2018-09-28 | 一种基于kld采样的改进高斯粒子滤波数据融合算法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN110401430A true CN110401430A (zh) | 2019-11-01 |
Family
ID=68322141
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201811165547.1A Pending CN110401430A (zh) | 2018-09-28 | 2018-09-28 | 一种基于kld采样的改进高斯粒子滤波数据融合算法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110401430A (zh) |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111291319A (zh) * | 2020-03-24 | 2020-06-16 | 广东海洋大学深圳研究院 | 一种应用于非高斯噪声环境下的移动机器人状态估计方法 |
| CN112039496A (zh) * | 2020-09-10 | 2020-12-04 | 南京航空航天大学 | 一种基于人工鱼群优化的快速高斯粒子滤波数据融合方法 |
| CN112066993A (zh) * | 2020-09-10 | 2020-12-11 | 南京航空航天大学 | 一种基于误差四元数三维矢量分布的高斯粒子滤波数据处理方法 |
| CN113124863A (zh) * | 2021-04-15 | 2021-07-16 | 南京航空航天大学 | 一种基于kld采样的混合粒子联邦滤波数据处理方法 |
-
2018
- 2018-09-28 CN CN201811165547.1A patent/CN110401430A/zh active Pending
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111291319A (zh) * | 2020-03-24 | 2020-06-16 | 广东海洋大学深圳研究院 | 一种应用于非高斯噪声环境下的移动机器人状态估计方法 |
| CN111291319B (zh) * | 2020-03-24 | 2023-07-07 | 广东海洋大学深圳研究院 | 一种应用于非高斯噪声环境下的移动机器人状态估计方法 |
| CN112039496A (zh) * | 2020-09-10 | 2020-12-04 | 南京航空航天大学 | 一种基于人工鱼群优化的快速高斯粒子滤波数据融合方法 |
| CN112066993A (zh) * | 2020-09-10 | 2020-12-11 | 南京航空航天大学 | 一种基于误差四元数三维矢量分布的高斯粒子滤波数据处理方法 |
| CN112039496B (zh) * | 2020-09-10 | 2024-04-19 | 南京航空航天大学 | 一种基于人工鱼群优化的快速高斯粒子滤波数据融合方法 |
| CN113124863A (zh) * | 2021-04-15 | 2021-07-16 | 南京航空航天大学 | 一种基于kld采样的混合粒子联邦滤波数据处理方法 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110401430A (zh) | 一种基于kld采样的改进高斯粒子滤波数据融合算法 | |
| CN105205313B (zh) | 模糊高斯和粒子滤波方法、装置及目标跟踪方法、装置 | |
| Yan et al. | An improved NLOS identification and mitigation approach for target tracking in wireless sensor networks | |
| CN104881521B (zh) | 一种标号随机集滤波器分布式融合方法 | |
| Zhou et al. | Indoor fingerprint localization based on fuzzy c-means clustering | |
| CN105787959B (zh) | 基于改进型自适应粒子滤波的多智能体网络目标跟踪方法 | |
| CN109687845B (zh) | 一种鲁棒的簇稀疏正则化多任务自适应滤波器网络 | |
| CN108650626A (zh) | 一种基于泰森多边形的指纹定位算法 | |
| CN104680002B (zh) | 一种基于随机集理论的分布式融合方法 | |
| CN101256715A (zh) | 无线传感器网络中基于粒子滤波的多车辆声信号分离方法 | |
| CN110233608B (zh) | 一种基于权值自适应的粒子滤波方法和雷达系统 | |
| CN104821854B (zh) | 一种基于随机集的多主用户多维频谱感知方法 | |
| CN107329131B (zh) | 一种利用粒子滤波的雷达微弱目标检测跟踪方法 | |
| Nannuru et al. | Hybrid multi-Bernoulli and CPHD filters for superpositional sensors | |
| CN110190832B (zh) | 变正则化参数多任务自适应滤波器网络 | |
| CN113687143B (zh) | 一种电磁波信号幅值衰减与传播距离关系曲线的拟合方法 | |
| Salim et al. | Novel Bayesian track-before-detection for drones based VB-multi-Bernoulli filter and a GIGM implementation | |
| Zhang et al. | Design of an adaptive particle filter based on variance reduction technique | |
| Meng et al. | An improved particle filtering algorithm using different correlation coefficients for nonlinear system state estimation | |
| Bhaumik et al. | Adaptive grid risk-sensitive filter for non-linear problems | |
| CN110334322B (zh) | 一种粒子滤波器的粒子数自适应方法 | |
| CN108121846A (zh) | 一种基于熵惩罚的em未知杂波估计的phd多目标跟踪方法 | |
| Hoteit et al. | Particle Kalman filtering for data assimilation in meteorology and oceanography | |
| Yi et al. | An efficient particle filter for multi-target tracking using an independence assumption | |
| CN111308423A (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 | ||
| WD01 | Invention patent application deemed withdrawn after publication | ||
| WD01 | Invention patent application deemed withdrawn after publication |
Application publication date: 20191101 |