CN109388778A - 一种迭代容积点无迹卡尔曼滤波方法 - Google Patents
一种迭代容积点无迹卡尔曼滤波方法 Download PDFInfo
- Publication number
- CN109388778A CN109388778A CN201811058809.4A CN201811058809A CN109388778A CN 109388778 A CN109388778 A CN 109388778A CN 201811058809 A CN201811058809 A CN 201811058809A CN 109388778 A CN109388778 A CN 109388778A
- Authority
- CN
- China
- Prior art keywords
- point
- kalman filtering
- sigma
- unscented kalman
- volume
- 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
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/15—Correlation function computation including computation of convolution operations
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/18—Complex mathematical operations for evaluating statistical data, e.g. average values, frequency distributions, probability functions, regression analysis
Abstract
本发明公开了一种迭代容积点无迹卡尔曼滤波方法,包括以下步骤:迭代容积点无迹卡尔曼滤波算法sigma点的选取;重新确定sigma点的加权系数;给出容积点无迹卡尔曼滤波算法的流程;迭代计算容积点无迹卡尔曼滤波算法。本发明能够有效应用在含有随机噪声的高自由度强非线性的系统中,协同处理解决计算量问题、非线性滤波发散问题、负权重问题,能够有效提高状态量的估计精度和实时性,不会使滤波结果发散。本发明相对于容积卡尔曼滤波可以更好地拟合非线性系统函数的统计特性,相对于无迹卡尔曼滤波可以避免sigma点权重的非正定性。
Description
技术领域
本发明属于通信导航技术领域,涉及一种迭代容积点无迹卡尔曼滤波方法。
背景技术
卡尔曼滤波技术是通过系统输入输出观测数据,对系统状态进行最优估计的算法,具有重要的意义,在通信,导航,制导与控制等多领域得到了较好的应用。在含有已知噪声的线性定常系统中,一般的线性卡尔曼滤波即可有效使用,但是在非线性系统中,由于状态转移矩阵无法被线性的表示,因此有多种改进的卡尔曼滤波技术产生。
扩展卡尔曼滤波是将非线性系统函数按照泰勒展开,取其线性部分进行状态矩阵的求解,此方法虽然相比于其他非线性滤波在计算量上较小,但无法解决高自由度非线性强的系统估计问题。容积卡尔曼滤波和无迹卡尔曼滤波是一种以点来拟合线的滤波技术,通过在非线性函数中取一系列的点,然后通过点集的映射来拟合整个非线性函数,无迹卡尔曼滤波流程如图1所示,容积点的特点是权重相同,但是容积点无法很好拟合强非线性函数的统计特性,而无迹卡尔曼滤波的sigma点在滤波过程中权重会变为负数,这会使滤波的结果发散。
综上所述,面对实际环境中多种传感器间的非线性耦合、协同导航模式,传统的滤波方式无法将计算量问题、非线性滤波发散问题、负权重问题协同处理,需要得到改进。
发明内容
为解决上述问题,本发明公开了一种迭代容积点无迹卡尔曼滤波方法,在原有卡尔曼滤波方法上进行改进,能够有效应用在高自由度强非线性的系统中。本发明首先给出了新的sigma点的选取方法;其次根据滤波过程中权重正定性的要求,给出了各sigma点的权重系数,更接近状态量的统计特性,从而解决了传统无迹卡尔曼滤波由于误差协方差矩阵的非正定性引起的滤波发散问题;再次,给出了一般性的容积点卡尔曼滤波算法的设计流程;最后利用参数迭代的方式,在线诊断滤波中sigma点权重的正定性。
为了达到上述目的,本发明提供如下技术方案:
一种迭代容积点无迹卡尔曼滤波算法,包括如下步骤:
1)迭代容积点无迹卡尔曼滤波算法sigma点的选取:将容积卡尔曼滤波算法中选取的容积点添加到无迹卡尔曼滤波算法的sigma点中,形成新的sigma点集在线计算状态量的均值和协方差;
2)重新确定sigma点的加权系数:重新确定滤波过程中sigma点的权重系数,在线计算上一时刻状态预测值的平均值,并与通过sigma点非线性映射加权平均计算后的状态量相比,确定权重系数的正定性,通过对sigma点加权平均计算后的状态量平均值必须等于前一步状态预测值的平均值,并且保证在整个滤波过程中权重均是正定的;
3)给出容积点无迹卡尔曼滤波算法的流程:利用改进后的sigma点拟合非线性函数,并通过非线性映射后的统计特性更新最优估计的状态量和协方差矩阵;
4)迭代计算容积点无迹卡尔曼滤波算法:在容积点无迹卡尔曼滤波中引入卡尔曼增益迭代系数,并实时检测滤波过程中权重的正定性,避免滤波的发散。
进一步的,所述步骤1)中迭代容积点无迹卡尔曼滤波算法sigma点的选取的具体步骤包括:
(1.1)在容积积分中,利用2n个等权球面点来积分计算∫f(y)dy,其中f(y)是任意非线性系统函数,其中,n是函数变量的个数;
(1.2)在原有的无迹卡尔曼滤波的2n+1个sigma点中添加容积点,形成新的4n+1个sigma点集,通过对sigma点加权平均计算后的状态值平均值必须等于前一步状态预测值的平均值,新的sigma点定义为:
其中,m是初始状态量的均值;P是初始状态量的误差协方差矩阵;k为经验取值;α决定sigma点的散布程度;T是矩阵的转置;Ei定义为:
进一步的,所述步骤2)中重新确定sigma点的加权系数的具体步骤包括:
(2.1)在原有的容积点等权重系数中引入微调参数λ、κ、α、β,抑制无迹卡尔曼滤波sigma点权重的非正定性,将权重定义为:
进一步的,所述步骤3)具体包括如下步骤:
(3.1)利用所述步骤1)和2)中改进的sigma点进行非线性函数的映射,对系统进行状态量的一步预测,并估计预测更新中的协方差矩阵:
其中,X是状态量,Q是噪声的协方差矩阵,u是输入,F(χk,uk)是状态量;
(3.2)使用新的微调参数λ和来计算sigma点,实现重排传递函数点,即χ′k+1/k和w′i,计算卡尔曼滤波系数,并完成本次状态量的最优估计:
其中,Υ为sigma点通过观测矩阵的映射;H为观测矩阵;为观测量的权重;Rk为观测噪声;y为观测值;K为卡尔曼增益;
(3.3)将式(3.2)中Pk+1和用于下次循环计算的新sigma点集:
进一步的,所述步骤4)具体包括如下步骤:
(4.1)定义参数j如下,用于索引在迭代过程中的次数:
(4.2)将如权利要求1所述步骤1)中式(1.1)的参数表示为P0=Pk+1,j+1,计算sigma点,重复更新如权利要求1步骤3)中的卡尔曼滤波流程;
(4.3)引入参数计算迭代过程中新的状态量,将式(4.1)中的状态量形式改写为:
其中,G为比例参数;
(4.4)定义参数为:
其中,h为观测向量函数;
(4.5)定义不等式为:
用于在线诊断滤波过程中权重的正定性,并且在每一次迭代中计算j=j+1,G=ηG,直到迭代过程结束,其中,R为观测噪声协方差矩阵。
与现有技术相比,本发明具有如下优点和有益效果:
本发明能够有效应用在含有随机噪声的高自由度强非线性的系统中,协同处理解决计算量问题、非线性滤波发散问题、负权重问题,能够有效提高状态量的估计精度和实时性,不会使滤波结果发散。本发明相对于容积卡尔曼滤波可以更好地拟合非线性系统函数的统计特性,相对于无迹卡尔曼滤波可以避免sigma点权重的非正定性。
附图说明
图1是无迹卡尔曼滤波算法的流程图。
图2是本发明提供的迭代计算容积点卡尔曼滤波方法的流程图。
具体实施方式
以下将结合具体实施例对本发明提供的技术方案进行详细说明,应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。
本发明所述的一种迭代容积点卡尔曼滤波方法,其流程如图2所示,包含以下步骤:
步骤1:初始化非线性系统的状态初值与协方差矩阵,并将容积卡尔曼滤波算法中选取的容积点添加到无迹卡尔曼滤波算法的sigma点中,形成新的sigma点集在线计算状态量的均值和协方差,具体包括如下子步骤:
在容积积分中,利用2n个等权球面点(n是函数变量的个数)来积分计算∫f(y)dy,其中f(y)是任意非线性系统函数。
在原有的无迹卡尔曼滤波的2n+1个sigma点中添加容积点,形成新的4n+1个sigma点集,本发明方法中通过对sigma点加权平均计算后的状态值平均值必须等于前一步状态预测值的平均值,新的sigma点定义为:
其中,m是初始状态量的均值;P是初始状态量的误差协方差矩阵;k为经验取值,通常取0;α决定sigma点的散布程度,通常取一个小的正值;T是矩阵的转置;Ei定义为:
其中,I是单位矩阵。
步骤2:重新确定滤波过程中sigma点的权重系数,在线计算上一时刻状态预测值的平均值,并与通过sigma点非线性映射加权平均计算后的状态量相比,并令sigma点加权平均计算后的状态量平均值必须等于前一步状态预测值的平均值,确定权重系数的正定性,重新确定权重系数的过程如下:
在原有的容积点等权重系数中引入微调参数λ、κ、α、β,抑制无迹卡尔曼滤波sigma点权重的非正定性,将权重定义为:
步骤3:利用步骤1和步骤2中改进的sigma点集求解状态转移矩阵,建立非线性系统的预测更新模型,最优估计本时刻的状态量,具体包括如下过程:
改进的sigma点进行非线性函数的映射,对系统进行状态量的一步预测,并估计预测更新中的协方差矩阵。
其中,X是状态量,Q是噪声的协方差矩阵,u是输入,F(χk,uk)是状态量。
步骤4:计算预测更新中的协方差矩阵以及卡尔曼增益,在下一时刻中继续更新状态量,具体包括如下过程:
使用新的微调参数λ和来计算sigma点,实现重排传递函数点,即χ′k+1/k和w′i,计算卡尔曼滤波系数,并完成本次状态量的最优估计。
其中,Υ为sigma点通过观测矩阵的映射;H为观测矩阵;为观测量的权重;Rk为观测噪声;y为观测值;K为卡尔曼增益。
将式(4.1)中Pk+1和用于下次循环计算的新sigma点集:
P′=Pk+1
步骤5:在卡尔曼增益中引入迭代系数,在线诊断权重系数的正定性,重复计算迭代过程中新的状态量,具体包括如下过程:
引入参数计算迭代过程中新的状态量,将式(4.1)中的状态量形式改写为:
其中,G为比例参数,j用于所以迭代过程的次数,且如下定义:
其中,x为迭代过程中索引的状态量。
将如步骤1中式(1.1)的参数表示为P0=Pk+1,j+1,计算sigma点,重复更新步骤3中的卡尔曼滤波流程。
定义参数为:
其中,h为观测向量函数。
定义不等式为:
用于在线诊断滤波过程中权重的正定性,其中,R为观测噪声协方差矩阵,并且在每一次迭代中j=j+1,G=ηG,直到迭代过程结束。
本发明方案所公开的技术手段不仅限于上述实施方式所公开的技术手段,还包括由以上技术特征任意组合所组成的技术方案。应当指出,对于本技术领域的普通技术人员来说,在不脱离本发明原理的前提下,还可以做出若干改进和润饰,这些改进和润饰也视为本发明的保护范围。
Claims (6)
1.一种迭代容积点无迹卡尔曼滤波算法,其特征在于,包括如下步骤:
1)迭代容积点无迹卡尔曼滤波算法sigma点的选取:将容积卡尔曼滤波算法中选取的容积点添加到无迹卡尔曼滤波算法的sigma点中,形成新的sigma点集在线计算状态量的均值和协方差;
2)重新确定sigma点的加权系数:重新确定滤波过程中sigma点的权重系数,在线计算上一时刻状态预测值的平均值,并与通过sigma点非线性映射加权平均计算后的状态量相比,确定权重系数的正定性;
3)给出容积点无迹卡尔曼滤波算法的流程:利用改进后的sigma点拟合非线性函数,并通过非线性映射后的统计特性更新最优估计的状态量和协方差矩阵;
4)迭代计算容积点无迹卡尔曼滤波算法:在容积点无迹卡尔曼滤波中引入卡尔曼增益迭代系数,并实时检测滤波过程中权重的正定性。
2.根据权利要求1所述的迭代容积点无迹卡尔曼滤波算法,其特征在于,所述步骤1)中迭代容积点无迹卡尔曼滤波算法sigma点的选取的具体步骤包括:
(1.1)在容积积分中,利用2n个等权球面点来积分计算∫f(y)dy,其中f(y)是任意非线性系统函数,其中,n是函数变量的个数;
(1.2)在原有的无迹卡尔曼滤波的2n+1个sigma点中添加容积点,形成新的4n+1个sigma点集,通过对sigma点加权平均计算后的状态值平均值必须等于前一步状态预测值的平均值,新的sigma点定义为:
其中,其中,m是初始状态量的均值;P是初始状态量的误差协方差矩阵;k为经验取值;α决定sigma点的散布程度;T是矩阵的转置;Ei的定义为:
3.根据权利要求1所述的迭代容积点无迹卡尔曼滤波算法,其特征在于,所述步骤2)中重新确定sigma点的加权系数的具体步骤包括:
(2.1)在原有的容积点等权重系数中引入微调参数λ、κ、α、β,抑制无迹卡尔曼滤波sigma点权重的非正定性,将权重定义为:
4.根据权利要求1所述的迭代容积点无迹卡尔曼滤波算法,其特征在于,所述步骤3)具体包括如下步骤:
(3.1)利用所述步骤1)和2)中改进的sigma点进行非线性函数的映射,对系统进行状态量的一步预测,并估计预测更新中的协方差矩阵:
其中,X是状态量,Q是噪声的协方差矩阵,u是输入,F(χk,uk)是状态量;
(3.2)使用新的微调参数λ和来计算sigma点,实现重排传递函数点,即χ′k+1/k和w′i,计算卡尔曼滤波系数,并完成本次状态量的最优估计:
其中,Υ为sigma点通过观测矩阵的映射;H为观测矩阵;Wi m为观测量的权重;Rk为观测噪声;y为观测值;K为卡尔曼增益;
(3.3)将式(3.2)中Pk+1和用于下次循环计算的新sigma点集:
5.根据权利要求1所述的迭代容积点无迹卡尔曼滤波算法,其特征在于,所述步骤4)具体包括如下步骤:
(4.1)定义参数j如下,用于索引在迭代过程中的次数:
(4.2)将如权利要求1所述步骤1)中式(1.1)的参数表示为P0=Pk+1,j+1,计算sigma点,重复更新如权利要求1步骤3)中的卡尔曼滤波流程;
(4.3)引入参数计算迭代过程中新的状态量,将式(4.1)中的状态量形式改写为:
其中,G为比例参数;
(4.4)定义参数为:
其中,h为观测向量函数;
(4.5)定义不等式为:
6.用于在线诊断滤波过程中权重的正定性,并且在每一次迭代中计算j=j+1,G=ηG,直到迭代过程结束,其中,R为观测噪声协方差矩阵。
Priority Applications (2)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201811058809.4A CN109388778A (zh) | 2018-09-11 | 2018-09-11 | 一种迭代容积点无迹卡尔曼滤波方法 |
PCT/CN2019/079135 WO2020052213A1 (zh) | 2018-09-11 | 2019-03-21 | 一种迭代容积点无迹卡尔曼滤波方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201811058809.4A CN109388778A (zh) | 2018-09-11 | 2018-09-11 | 一种迭代容积点无迹卡尔曼滤波方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN109388778A true CN109388778A (zh) | 2019-02-26 |
Family
ID=65418908
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201811058809.4A Pending CN109388778A (zh) | 2018-09-11 | 2018-09-11 | 一种迭代容积点无迹卡尔曼滤波方法 |
Country Status (2)
Country | Link |
---|---|
CN (1) | CN109388778A (zh) |
WO (1) | WO2020052213A1 (zh) |
Cited By (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2020052213A1 (zh) * | 2018-09-11 | 2020-03-19 | 东南大学 | 一种迭代容积点无迹卡尔曼滤波方法 |
CN111238484A (zh) * | 2020-02-28 | 2020-06-05 | 上海航天控制技术研究所 | 一种基于球形无迹变换的环火轨道自主导航方法 |
CN111756353A (zh) * | 2020-06-12 | 2020-10-09 | 杭州电子科技大学 | 一种基于非线性融合滤波的液位仪噪声优化方法 |
CN113805075A (zh) * | 2021-09-15 | 2021-12-17 | 上海电机学院 | 一种基于bcrls-ukf的锂电池荷电状态估计方法 |
CN117216482A (zh) * | 2023-11-07 | 2023-12-12 | 南开大学 | 借助迭代策略增强卡尔曼滤波器抗野值干扰能力的方法 |
Family Cites Families (4)
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 |
CN105975747A (zh) * | 2016-04-27 | 2016-09-28 | 渤海大学 | 一种基于无迹卡尔曼滤波算法的cstr模型参数辨识方法 |
CN108225337B (zh) * | 2017-12-28 | 2020-12-15 | 西安空间无线电技术研究所 | 基于sr-ukf滤波的星敏感器和陀螺组合定姿方法 |
CN109388778A (zh) * | 2018-09-11 | 2019-02-26 | 东南大学 | 一种迭代容积点无迹卡尔曼滤波方法 |
-
2018
- 2018-09-11 CN CN201811058809.4A patent/CN109388778A/zh active Pending
-
2019
- 2019-03-21 WO PCT/CN2019/079135 patent/WO2020052213A1/zh active Application Filing
Cited By (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2020052213A1 (zh) * | 2018-09-11 | 2020-03-19 | 东南大学 | 一种迭代容积点无迹卡尔曼滤波方法 |
CN111238484A (zh) * | 2020-02-28 | 2020-06-05 | 上海航天控制技术研究所 | 一种基于球形无迹变换的环火轨道自主导航方法 |
CN111756353A (zh) * | 2020-06-12 | 2020-10-09 | 杭州电子科技大学 | 一种基于非线性融合滤波的液位仪噪声优化方法 |
CN111756353B (zh) * | 2020-06-12 | 2024-04-16 | 杭州电子科技大学 | 一种基于非线性融合滤波的液位仪噪声优化方法 |
CN113805075A (zh) * | 2021-09-15 | 2021-12-17 | 上海电机学院 | 一种基于bcrls-ukf的锂电池荷电状态估计方法 |
CN117216482A (zh) * | 2023-11-07 | 2023-12-12 | 南开大学 | 借助迭代策略增强卡尔曼滤波器抗野值干扰能力的方法 |
CN117216482B (zh) * | 2023-11-07 | 2024-03-01 | 南开大学 | 借助迭代策略增强卡尔曼滤波器抗野值干扰能力的方法 |
Also Published As
Publication number | Publication date |
---|---|
WO2020052213A1 (zh) | 2020-03-19 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109388778A (zh) | 一种迭代容积点无迹卡尔曼滤波方法 | |
Grathwohl et al. | Ffjord: Free-form continuous dynamics for scalable reversible generative models | |
Colburn et al. | State estimation in wall-bounded flow systems. Part 3. The ensemble Kalman filter | |
WO2016086329A1 (zh) | 基于序列递归滤波三维变分的实测海洋环境数据同化方法 | |
Hlinka et al. | Distributed Gaussian particle filtering using likelihood consensus | |
Bocharov et al. | Numerical solution by LMMs of stiff delay differential systems modelling an immune response | |
CN106599368A (zh) | 基于改进粒子提议分布和自适应粒子重采样的FastSLAM方法 | |
CN111027732B (zh) | 一种多风电场出力场景的生成方法及系统 | |
CN104462015B (zh) | 处理非高斯Lévy噪声的分数阶线性离散系统状态更新方法 | |
CN104376581A (zh) | 一种采用自适应重采样的高斯混合无迹粒子滤波算法 | |
CN112115419A (zh) | 系统状态估计方法、系统状态估计装置 | |
CN104836760B (zh) | 一种信道估计方法及装置 | |
Hariz et al. | Optimal rate of convergence for nonparametric change-point estimators for nonstationary sequences | |
Hlinka et al. | Likelihood consensus-based distributed particle filtering with distributed proposal density adaptation | |
CN114626307B (zh) | 一种基于变分贝叶斯的分布式一致性目标状态估计方法 | |
CN108228959A (zh) | 利用删失数据估计系统实际状态的方法及应用其的滤波器 | |
Safarinejadian et al. | Kalman randomized joint UKF algorithm for dual estimation of states and parameters in a nonlinear system | |
CN109253727B (zh) | 一种基于改进迭代容积粒子滤波算法的定位方法 | |
JP2015219714A (ja) | 状態推定装置、方法、及びプログラム | |
Malek et al. | Imputation of time series data via Kohonen self organizing maps in the presence of missing data | |
CN104794101A (zh) | 一种分数阶非线性系统状态估计方法 | |
JP2016018323A (ja) | パラメータ推定方法、装置、及びプログラム | |
Short | Fast online identification of low-order time-delayed industrial processes | |
Liu et al. | Echo state networks with double-reservoir for time-series prediction | |
Algamal et al. | Reliability estimation of three parameters Weibull distribution based on particle swarm optimization |
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 |
Application publication date: 20190226 |
|
RJ01 | Rejection of invention patent application after publication |