CN108599737A - 一种变分贝叶斯的非线性卡尔曼滤波器的设计方法 - Google Patents
一种变分贝叶斯的非线性卡尔曼滤波器的设计方法 Download PDFInfo
- Publication number
- CN108599737A CN108599737A CN201810315809.1A CN201810315809A CN108599737A CN 108599737 A CN108599737 A CN 108599737A CN 201810315809 A CN201810315809 A CN 201810315809A CN 108599737 A CN108599737 A CN 108599737A
- Authority
- CN
- China
- Prior art keywords
- formula
- variation
- function
- state
- iteration
- 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.)
- Granted
Links
Classifications
-
- H—ELECTRICITY
- H03—ELECTRONIC CIRCUITRY
- H03H—IMPEDANCE NETWORKS, e.g. RESONANT CIRCUITS; RESONATORS
- H03H21/00—Adaptive networks
- H03H21/0012—Digital adaptive filters
- H03H21/0025—Particular filtering methods
- H03H21/0029—Particular filtering methods based on statistics
- H03H21/003—KALMAN filters
-
- 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/0255—Filters based on statistics
- H03H17/0257—KALMAN filters
-
- H—ELECTRICITY
- H03—ELECTRONIC CIRCUITRY
- H03H—IMPEDANCE NETWORKS, e.g. RESONANT CIRCUITS; RESONATORS
- H03H21/00—Adaptive networks
- H03H21/0012—Digital adaptive filters
- H03H21/0043—Adaptive algorithms
Landscapes
- Physics & Mathematics (AREA)
- Probability & Statistics with Applications (AREA)
- Engineering & Computer Science (AREA)
- Computer Hardware Design (AREA)
- Mathematical Physics (AREA)
- Complex Calculations (AREA)
Abstract
本发明提供了一种变分贝叶斯的非线性卡尔曼滤波器的设计方法,涉及滤波器领域，本发明首先在高斯假设条件下构造逼近后验概率密度函数的变分分布，并以KL散度作为惩罚函数以实现状态估计的迭代逼近，继而根据变分贝叶斯框架以置信下界最大为目标函数推导出一种变分贝叶斯的非线性卡尔曼滤波器。本发明能够获得非线性状态后验概率密度函数更“紧”的逼近形式，从而提高状态估计精度，可以实现系统状态估计过程中后验概率密度函数积分的难以求解问题转化为优化ELBO下界的问题，并且将自适应加权的KL散度作为惩罚函数以提高优化的灵活性，从而改善状态估计精度，对于非线性状态估计理论和实际工程应用具有非常重要的意义。
Description
技术领域
本发明涉及滤波器领域，尤其是一种卡尔曼滤波器的设计方法。
背景技术
传统的非线性滤波器一般采用采样的方法逼近状态的后验概率密度函数或者将非线性函数线性化，然后根据近似结果计算状态和误差协方差的期望值，以简化后验概率密度函数(PDF)的积分问题。
扩展卡尔曼滤波器(EKF)和迭代扩展卡尔曼滤波器(IEKF)作为线性化方法的代表，通过泰勒展式将非线性状态函数和测量函数进行线性化，从而能够直接采用卡尔曼滤波(KF)框架，但是由于其只保留一阶项而忽略高阶项，在系统非线性特征较强时容易造成滤波发散现象。粒子滤波器(PF)是一种蒙特卡洛方法，通过随机采样服从建议分布大量粒子近似真实状态的PDF，可应用于非线性非高斯状态空间模型。但是，随机性采样非线性滤波器计算量较大，需要大量粒子才能保证滤波的精度和收敛性。同时，不敏卡尔曼滤波器(UKF)，容积卡尔曼滤波器(CKF)和中心差分卡尔曼滤波器(CDKF)是确定性采样算法。UKF和CDKF分别通过UT变换和Sterling插值近似非线性状态转移矩阵和量测矩阵，然后结合采样点和PDF计算状态估计和估计误差协方差。而CKF采用容积法则获得的容积点作为PDF采样，继而根据样本点的PDF计算状态估计。上述算法的机制依赖于近似非线性函数矩阵或非线性状态的PDF，然后根据近似结果计算状态和误差协方差的期望值。因此，与直接近似状态和误差协方差的期望值相比，这类近似方法是“不紧的”。
几年来，变分贝叶斯(VB)方法能够将难于求解的积分问题转化为下界优化问题的优势日益受到关注。Simon等人在传统贝叶斯滤波框架下采用VB方法估计非线性随机系统中的时变噪声协方差和系统状态。为了提高VB推理的准确性，Khan等采用Kullback-Leibler(KL)散度作为近似项，考虑从PDF的几何学的角度进行逼近。
发明内容
为了克服现有技术的不足，本发明针对高斯非线性系统的状态估计问题，利用置信下界方法能够将难以获得解析解的积分问题转化为优化问题的优势，首先在高斯假设条件下构造逼近后验概率密度函数的变分分布，并以Kullback-Leibler散度作为惩罚函数以实现状态估计的迭代逼近。继而，根据变分贝叶斯框架以置信下界最大为目标函数推导出一种变分贝叶斯的非线性卡尔曼滤波器(PNKF-VB)。算法以贝叶斯滤波器为内核，以获取迭代初值，并根据当前采样时间和当前迭代次数自适应给出遗忘率和迭代步长。从而实现更紧的非线性状态逼近形式，为高斯非线性系统的状态估计应该提供参考。
本发明解决其技术问题所采用的技术方案包括以下步骤：
第一步：根据贝叶斯滤波理论获取非线性系统状态的估计值，并作为迭代初始值；详细步骤如下：
步骤1.1：根据先验信息，状态演化信息和前一时刻的量测，计算状态预测，量测预测，状态预测误差协方差，量测预测误差协方差及状态预测与量测预测的交互协方差；
在迭代优化的过程中，非线性贝叶斯滤波器作为内核滤波执行器为迭代优化提供初始值，非线性贝叶斯滤波器的方程如下：
xk|k-1＝fk(xk-1) (1)
zk|k-1＝h(xk|k-1) (2)
Pk|k-1＝E[(xk-xk|k-1)(xk-xk|k-1)T] (3)
Pk,xz＝E[(xk-xk|k-1)(zk-zk|k-1)T] (4)
Pk,zz＝E[(zk-zk|k-1)(zk-zk|k-1)T] (5)
其中，xk-1表示k-1时刻的状态值，xk|k-1和zk|k-1分别表示k-1时刻到k时刻的状态预测值和量测预测值，fk(·)和hk(·)分别表示状态转移函数和量测函数，Pk|k-1，Pk,xz和Pk,zz分别表示k-1时刻到k时刻的状态预测误差协方差，状态预测与量测预测之间的交互误差协方差，量测预测误差协方差，E[·]表示求期望；
步骤1.2：根据传感器当前时刻传感器量测信息zk和Kalman滤波的更新公式，计算滤波增益Kk，状态估计xk|k和状态估计误差协方差Pk|k：
Kk＝Pk,xz(Pk,zz)-1 (6)
xk|k＝xk|k-1+Kk(zk-zk|k-1) (7)
Pk|k＝Pk|k-1-KkPk,zzKk T (8)
第二步：根据变分贝叶斯框架和KL散度建立迭代优化函数详细步骤如下：
步骤2.1：根据量测和状态之间的贝叶斯公式和Jensen不等式定理建立变分置信下界，并将置信下界作为目标函数；
变分贝叶斯的置信下界为目标函数矩阵在以xk为隐变量和zk为观测变量的状态空间模型中，k时刻的边缘概率密度函数表示为：
其中，p(zk,xk)＝p(zk|xk)p(xk|xk-1)为k时刻的联合分布，p(xk|xk-1)为从k-1到k时刻是状态转移概率，定义其中，xk|k和Pk|k分别表示待估计变量的状态估计及其误差协方差，公式(9)满足Jensen不等式定理，因此公式(9)可化为公式(10)：
置信优化的下界定义为：
其中，Eq[·]是的简化形式，p(xk)和q(xk|ψk)分别表示状态的真实分布和其变分分布，p(zk|xk)表示条件似然函数；
最大化即实现将积分问题到优化问题的转化：
步骤2.2：引入KL散度作为约束项
其中，i表示迭代次数，βi表示惩罚因子，表示变分分布q(xk|ψk)和变分分布之间的KL散度；
在高斯假设条件下，变分分布为假设k-1时刻的状态估计后验概率密度函数已知因此公式(13)中第二项可进一步表示为：
第三步：在变分分布条件下线性化迭代优化函数详细步骤如下：
步骤3.1：在处线性化量测矩阵hk(xk)，以计算似然函数对数期望Eq[logp(zk|xk)]的一阶梯度和二阶梯度，进而根据泰勒公式线性化对数似然函数的期望Eq[logp(zk|xk)]，详细步骤如下：
在高斯假设条件下对优化函数矩阵进行线性化，定义参数和分别为Eq[logp(zk|xk|k)]的一阶梯度和二阶梯度，则：
根据Bonnet定理和Price定理，一阶梯度和二阶梯度可分别表示为：
其中，公式(16)中表示迭代过程中新息的期望，本发明采用线性化近似和采样近似两种近似方法：
1)当采用线性化近似的方法，则有：
由公式(18)得到公式(19)：
将公式(19)带入公式(13)中使用；
2)若采用采样近似的方法，则有：
由公式(20)得到公式(21)：
将公式(21)带入公式(13)中；
步骤3.2：在变分分布服从高斯分布的条件下，计算状态预测的概率密度函数和第i+1次变分分布之间的KL散度：
假设维数为D的变量ξ1服从均值为μ1方差为C1的高斯分布相同维数变量ξ2服从均值为μ2方差为C2的高斯分布则两者的KL散度可化为如下形式：
因此，第i次迭代的变分分布和第i+1次迭代变分分布之间的KL散度为：
Dx表示服从高斯分布的变量x的维数；
同理，公式(14)可化为：
步骤3.3：结合公式(13)-(15)和公式(23)-(24)，优化函数矩阵可化为：
第四步：将线性化后的迭代优化函数求偏导和计算状态估计和估计误差协方差，并由此设计卡尔曼滤波器，详细计算步骤如下：
步骤4.1：将线性化后的迭代优化函数在处求偏导，分别计算Φ(xk|k,Pk|k)对xk|k和Pk|k的偏导矩阵：
步骤4.2：令偏导等于0，以获取第i+1次迭代的状态估计
其中bi＝1/(1+βi)；
步骤4.3：将线性化后的迭代优化函数在处求偏导；
步骤4.4：令偏导等于0，以获取第i+1次迭代的状态估计误差协方差
第五步：第四步所设计的卡尔曼滤波器的自适应迭代终止条件如下：
其中，ε为迭代终止门限。
本发明的有益效果是由于采用在变分贝叶斯框架下推导给出一种变分贝叶斯的非线性卡尔曼滤波器的方法，能够获得非线性状态后验概率密度函数更“紧”的逼近形式，从而提高状态估计精度。本发明可以实现系统状态估计过程中后验概率密度函数积分的难以求解问题转化为优化ELBO下界的问题，并且将自适应加权的KL散度作为惩罚函数以提高优化的灵活性，从而改善状态估计精度，对于非线性状态估计理论和实际工程应用具有非常重要的意义。
附图说明
图1为本发明的近端迭代非线性滤波器流程图；
图2为迭代更新置信下界变化的示意图；
图3为仿真场景1中位置估计RMSE对比图，其中，图3(a)为水平方向的对比图，图3(b)为竖直方向的对比图。
图4为仿真场景2中位置估计RMSE对比图，其中，图4(a)为水平方向的对比图，图4(b)为竖直方向的对比图。
图5为某一采样时刻的ELBO和边缘概率密度函数与迭代次数的关系图；
图6为所有采样时刻的ELBO和边缘概率密度函数与迭代次数之间的关系图；
图7为PUKF-VB算法的RMSE和计算量与迭代次数之间的关系图，其中，图7(a)为水平方向的关系图，图7(b)为竖直方向的关系图。
具体实施方式
下面结合附图和实施例对本发明进一步说明。
状态估计往往伴随着非线性问题，例如机动目标的跟踪、智能车辆定位与导航等，其最优的解决方案需要得到条件后验概率的完整描述，但是这种精确的描述需要无尽的参数，而无法实际应用，仅有部分特定问题可以得到最优解。因此，需要能够更“紧”的分布近似后验概率密度函数的非线性滤波器以提高状态估计精度，本发明涉及一种变分贝叶斯的非线性卡尔曼滤波器。与传统的基于线性化方法的非线性滤波器和基于采样的方法的滤波器不同，所提算法能够实现非线性状态的近端迭代近似，具有更“紧”的状态逼近形式。
本发明在VB框架下，充分利用加权KL散度的灵活性，从证据下界优化(ELBO)入手推导出一种变分贝叶斯的非线性卡尔曼滤波器框架。与现有算法近似PDF的方式不同，所提算法根据置信下界条件将状态在后验概率密度条件下的积分问题转换为对状态期望的优化问题。同时，采用加权的KL散度作为系统状态的几何近似项。因此，非线性随机系统状态的估计是“紧”的，能有效的改善状态估计精度。
本发明以非线性系统状态估计为应用背景，提出一种变分贝叶斯的非线性卡尔曼滤波器(PNKF-VB)。
图1是本发明算法框架流图，下面根据流程图对本发明的具体实施方案作进一步的描述：
第一步：根据贝叶斯滤波理论获取非线性系统状态的估计值，并作为迭代初始值；详细步骤如下：
步骤1.1：根据先验信息，状态演化信息和前一时刻的量测，计算状态预测，量测预测，状态预测误差协方差，量测预测误差协方差及状态预测与量测预测的交互协方差；
在迭代优化的过程中，非线性贝叶斯滤波器作为内核滤波执行器为迭代优化提供初始值，其选择具有多样性，下面给出非线性贝叶斯滤波器的一般方程：
xk|k-1＝fk(xk-1) (9)
zk|k-1＝h(xk|k-1) (10)
Pk|k-1＝E[(xk-xk|k-1)(xk-xk|k-1)T] (11)
Pk,xz＝E[(xk-xk|k-1)(zk-zk|k-1)T] (12)
Pk,zz＝E[(zk-zk|k-1)(zk-zk|k-1)T] (13)
其中，xk-1表示k-1时刻的状态值，xk|k-1和zk|k-1分别表示k-1时刻到k时刻的状态预测值和量测预测值，fk(·)和hk(·)分别表示状态转移函数和量测函数，Pk|k-1，Pk,xz和Pk,zz分别表示k-1时刻到k时刻的状态预测误差协方差，状态预测与量测预测之间的交互误差协方差，量测预测误差协方差，E[·]表示求期望；
步骤1.2：根据传感器当前时刻传感器量测信息zk和Kalman滤波的更新公式，计算滤波增益Kk，状态估计xk|k和状态估计误差协方差Pk|k：
Kk＝Pk,xz(Pk,zz)-1 (14)
xk|k＝xk|k-1+Kk(zk-zk|k-1) (15)
Pk|k＝Pk|k-1-KkPk,zzKk T (16)
第二步：根据变分贝叶斯框架和KL散度建立迭代优化函数详细步骤如下：
步骤2.1：根据量测和状态之间的贝叶斯公式和Jensen不等式定理建立变分置信下界，并将置信下界作为目标函数；
在变分贝叶斯框架中，通过恰当的变分分布近似状态后验概率密度函数，当置信函数最大，变分分布和后验概率密度函数之间的KL散度为0时，如图2所示，后验概率密度函数等于变分分布，因此，考虑分别将置信函数最大和KL散度趋于0作为目标函数和约束项，从而构建优化函数并推导迭代优化状态估计。其过程如下：
变分贝叶斯的置信下界为目标函数矩阵在以xk为隐变量和zk为观测变量的状态空间模型中，k时刻的边缘概率密度函数表示为：
其中，p(zk,xk)＝p(zk|xk)p(xk|xk-1)为k时刻的联合分布，p(xk|xk-1)为从k-1到k时刻是状态转移概率，定义其中，xk|k和Pk|k分别表示待估计变量的状态估计及其误差协方差，公式(9)满足Jensen不等式定理，因此公式(9)可化为公式(10)：
置信优化的下界定义为：
其中，Eq[·]是的简化形式，p(xk)和q(xk|ψk)分别表示状态的真实分布和其变分分布，p(zk|xk)表示条件似然函数；
最大化即实现将积分问题到优化问题的转化：
步骤2.2：引入KL散度作为约束项
其中，i表示迭代次数，βi表示惩罚因子，在本发明中βi∈[1,5]，表示变分分布q(xk|ψk)和变分分布之间的KL散度；
在高斯假设条件下，变分分布为假设k-1时刻的状态估计后验概率密度函数已知因此公式(13)中第二项可进一步表示为：
第三步：在变分分布条件下线性化迭代优化函数详细步骤如下：
步骤3.1：在处线性化量测矩阵hk(xk)，以计算似然函数对数期望Eq[logp(zk|xk)]的一阶梯度和二阶梯度，进而根据泰勒公式线性化对数似然函数的期望Eq[logp(zk|xk)]，详细步骤如下：
在高斯假设条件下对优化函数矩阵进行线性化，定义参数和分别为Eq[logp(zk|xk|k)]的一阶梯度和二阶梯度，则：
根据Bonnet定理和Price定理，一阶梯度和二阶梯度可分别表示为：
其中，公式(16)中表示迭代过程中新息的期望，本发明给出线性化近似和采样近似两种近似方法：
1)当采用线性化近似的方法，则有：
由公式(18)得到公式(19)：
将公式(19)带入公式(13)中使用；
2)若采用采样近似的方法，则有
由公式(20)得到公式(21)：
将公式(21)带入公式(13)中；
本发明采用线性化方法近似Eq[logp(zk|xk)]；
步骤3.2：在变分分布服从高斯分布的条件下，计算状态预测的概率密度函数和第i+1次变分分布之间的KL散度：
假设维数为D的变量ξ1服从均值为μ1方差为C1的高斯分布相同维数变量ξ2服从均值为μ2方差为C2的高斯分布则两者的KL散度可化为如下形式：
因此，第i次迭代的变分分布和第i+1次迭代变分分布之间的KL散度为：
Dx表示服从高斯分布的变量x的维数；
同理，公式(14)可化为：
步骤3.3：结合公式(13)-(15)和公式(23)-(24)，优化函数矩阵可化为：
第四步：将线性化后的迭代优化函数求偏导和计算状态估计和估计误差协方差，并由此设计卡尔曼滤波器，详细计算步骤如下：
由变分贝叶斯方法机理知，置信下界到达最大时隐变量所对应的值和方差即是所求估计值和估计误差协方差，而置信下界的最大值等价于优化函数的最大值。因此，可通过对优化函数矩阵求极值，计算状态估计和估计误差协方差；
步骤4.1：将线性化后的迭代优化函数在处求偏导，分别计算Φ(xk|k,Pk|k)对xk|k和Pk|k的偏导矩阵：
步骤4.2：令偏导等于0，以获取第i+1次迭代的状态估计
其中bi＝1/(1+βi)；
步骤4.3：将线性化后的迭代优化函数在处求偏导；
步骤4.4：令偏导等于0，以获取第i+1次迭代的状态估计误差协方差
第五步：第四步所设计的卡尔曼滤波器的自适应迭代终止条件如下：
其中，ε为迭代终止门限，本发明取值为ε＝10-6。
本发明借助变分贝叶斯框架通过构建变分分布将难以获得解析解的后验概率密度函数的积分问题转化为置信下界的优化问题，在估计优化的推导过程中，首先利用贝叶斯估计方法获取系统状态估计，并将其作为迭代优化的初始条件；然后分别以置信下界最大和KL散度为目标函数和惩罚函数设计优化函数；继而对优化函数进行线性化、求偏导等一系列推导；同时，在一定迭代门限和状态估计条件下，设计自适应迭代步骤，从而获得状态的迭代估计。针对高斯非线性状态估计的问题，本发明借鉴置信下界优化思想，灵活运用变分贝叶斯框架，提高了非线性状态估计精度，并且由于结合贝叶斯滤波方法，可通过多种非线性滤波器实现迭代的初始化，具有较好的可移植性。
本发明考虑通过将难以获得解析解的后验概率密度函数的积分问题转化为置信下界的优化问题，借助于变分贝叶斯框架和罚函数思想推导给出迭代优化的状态估计，从而提升非线性系统状态估计精度。
一般而言，在非线性系统中由于状态的后验概率密度函数难以获得解析解，要解决非线性状态的估计问题，必须采用恰当的逼近方法对后验概率密度函数进行近似，并且其逼近程度决定着非线性状态估计精度。本发明基于变分贝叶斯框架给出了非线性系统中后验概率密度函数的变分分布近似策略，提出一种变分贝叶斯的非线性卡尔曼滤波器，实现了系统状态的较“紧”估计。
为验证算法的有效性，采用目标跟踪场景下的状态估计模型对比本发明方法与EKF，IEKF，CKF，UKF和PF的估计精度和实时性。均方根误差(RMSE)作为滤波器估计精度的指标，Monte Carlo仿真次数为100。内核滤波执行器和迭代的新息期望近似方法如表1示。
表1仿真实验中PNKF-VB的滤波执行器和新息期望的近似方法
A)仿真场景1：
考虑笛卡尔二维坐标系下匀速直线运动(CV)模型，系统状态其中(xk,yk)和分别表示k时刻目标在x轴和y轴位置分量和速度分量。状态转移阵和系统噪声驱动矩阵分别表示为：
其中，系统噪声方差和量测噪声方差分别为初始状态和相应的误差协方差分别为x0＝[15km 0.15km/s 15.4km 0.13km/s]T，P0＝diag{1km2 0.1km2/s2 1km2 0.1km2}，采样时间间隔T＝1s。仿真结果如下：
1)图3(a)和图3(b)是本发明推导的非线性卡尔曼滤波器与EKF，UKF，PF和IEKF在水平位置和竖直位置估计精度方面的比较。从图中可以看出，以UKF作为内核滤波执行器采样方法近似迭代的新息期望的PUKF-VB滤波器具有最低的RMSE曲线，其精度最高。
2)为了定量地比较不同算法之间的精度和实时性。表2给出仿真场景1中不同算法的RMSE均值和执行时间。从表中看出：在精度方面，PUKF-VB无论在X轴还是Y轴方向都具有最小的RMSE；在实时性方面，PUKF-VB的执行时间略高于EKF，UKF和IEKF，但远低于PF。
表2仿真场景1中不同算法的RMSE均值和执行时间的定量对比
B)仿真场景2：
在仿真场景2中，采用匀速圆周运动(CT)，相应的状态转移矩阵为：
其中，ωCT＝-0.25，零均值高斯白系统噪声和量测噪声的方差分别Q＝0.01I4×4和R＝diag{0.42,(0.5π/180)2}，量测矩阵与仿真场景1相同。初始状态及其协方差矩阵分别为x0＝[5 0.6 27.4 0]T和P0＝diag{0.02 0.01 0.05 0.01}。仿真结果如下：
1)图4(a)和图4(b)是以EKF为内核滤波执行器线性化方法近似迭的代新息期望的近端迭代非线性卡尔曼滤波器(PEKF-VB)与EKF，CKF，UKF和PF在水平位置和竖直位置估计精度方面的比较。与仿真场景1相似，PEKF-VB具有最低的RMSE曲线。
2)图5-图6给出ELBO和边缘概率密度函数与迭代次数之间的关系。图中ELBO曲线和边缘概率密度函数曲线均随着迭代次数的增加而增大，并且随着迭代次数的增加其增大速度减缓。这印证了变分贝叶斯方法应用于本发明的正确性。
3)图7给出PEKF-VB的精度和实时性与迭代次数之间的关系。图中算法的计算时间随着迭代次数的增加基本呈线性增长，而RMSE曲线呈下降趋势，但当迭代次数大于5时下降不明显。
4)表3对仿真场景2中不同算法的RMSE和执行时间进行定量对比。与仿真场景1相似，PEKF-VB的RMSE最小，且执行时间远小于PF。
表3仿真场景2中不同算法的RMSE和执行时间的定量对比
从以上仿真结果可知，本发明所推导的一种变分贝叶斯的非线性卡尔曼滤波器在保证一定计算量的同时提高状态估计精度，具有一定的理论价值和工程指导意义。
Claims (1)
1.一种变分贝叶斯的非线性卡尔曼滤波器的设计方法，其特征在于包括下述步骤：
第一步：根据贝叶斯滤波理论获取非线性系统状态的估计值，并作为迭代初始值；详细步骤如下：
步骤1.1：根据先验信息，状态演化信息和前一时刻的量测，计算状态预测，量测预测，状态预测误差协方差，量测预测误差协方差及状态预测与量测预测的交互协方差；
在迭代优化的过程中，非线性贝叶斯滤波器作为内核滤波执行器为迭代优化提供初始值，非线性贝叶斯滤波器的方程如下：
xk|k-1＝fk(xk-1) (1)
zk|k-1＝h(xk|k-1) (2)
Pk|k-1＝E[(xk-xk|k-1)(xk-xk|k-1)T] (3)
Pk,xz＝E[(xk-xk|k-1)(zk-zk|k-1)T] (4)
Pk,zz＝E[(zk-zk|k-1)(zk-zk|k-1)T] (5)
其中，xk-1表示k-1时刻的状态值，xk|k-1和zk|k-1分别表示k-1时刻到k时刻的状态预测值和量测预测值，fk(·)和hk(·)分别表示状态转移函数和量测函数，Pk|k-1，Pk,xz和Pk,zz分别表示k-1时刻到k时刻的状态预测误差协方差，状态预测与量测预测之间的交互误差协方差，量测预测误差协方差，E[·]表示求期望；
步骤1.2：根据传感器当前时刻传感器量测信息zk和Kalman滤波的更新公式，计算滤波增益Kk，状态估计xk|k和状态估计误差协方差Pk|k：
Kk＝Pk,xz(Pk,zz)-1 (6)
xk|k＝xk|k-1+Kk(zk-zk|k-1) (7)
Pk|k＝Pk|k-1-KkPk,zzKk T (8)
第二步：根据变分贝叶斯框架和KL散度建立迭代优化函数详细步骤如下：
步骤2.1：根据量测和状态之间的贝叶斯公式和Jensen不等式定理建立变分置信下界，并将置信下界作为目标函数；
变分贝叶斯的置信下界为目标函数矩阵在以xk为隐变量和zk为观测变量的状态空间模型中，k时刻的边缘概率密度函数表示为：
其中，p(zk,xk)＝p(zk|xk)p(xk|xk-1)为k时刻的联合分布，p(xk|xk-1)为从k-1到k时刻是状态转移概率，定义其中，xk|k和Pk|k分别表示待估计变量的状态估计及其误差协方差，公式(9)满足Jensen不等式定理，因此公式(9)可化为公式(10)：
置信优化的下界定义为：
其中，Eq[·]是的简化形式，p(xk)和q(xk|ψk)分别表示状态的真实分布和其变分分布，p(zk|xk)表示条件似然函数；
最大化即实现将积分问题到优化问题的转化：
步骤2.2：引入KL散度作为约束项
其中，i表示迭代次数，βi表示惩罚因子，表示变分分布q(xk|ψk)和变分分布之间的KL散度；
在高斯假设条件下，变分分布为假设k-1时刻的状态估计后验概率密度函数已知因此公式(13)中第二项可进一步表示为：
第三步：在变分分布条件下线性化迭代优化函数详细步骤如下：
步骤3.1：在处线性化量测矩阵hk(xk)，以计算似然函数对数期望Eq[logp(zk|xk)]的一阶梯度和二阶梯度，进而根据泰勒公式线性化对数似然函数的期望Eq[logp(zk|xk)]，详细步骤如下：
在高斯假设条件下对优化函数矩阵进行线性化，定义参数和分别为Eq[log p(zk|xk|k)]的一阶梯度和二阶梯度，则：
根据Bonnet定理和Price定理，一阶梯度和二阶梯度可分别表示为：
其中，公式(16)中表示迭代过程中新息的期望，本发明采用线性化近似和采样近似两种近似方法：
1)当采用线性化近似的方法，则有：
由公式(18)得到公式(19)：
将公式(19)带入公式(13)中使用；
2)若采用采样近似的方法，则有：
由公式(20)得到公式(21)：
将公式(21)带入公式(13)中；
步骤3.2：在变分分布服从高斯分布的条件下，计算状态预测的概率密度函数和第i+1次变分分布之间的KL散度：
假设维数为D的变量ξ1服从均值为μ1方差为C1的高斯分布相同维数变量ξ2服从均值为μ2方差为C2的高斯分布则两者的KL散度可化为如下形式：
因此，第i次迭代的变分分布和第i+1次迭代变分分布之间的KL散度为：
Dx表示服从高斯分布的变量x的维数；
同理，公式(14)可化为：
步骤3.3：结合公式(13)-(15)和公式(23)-(24)，优化函数矩阵可化为：
第四步：将线性化后的迭代优化函数求偏导和计算状态估计和估计误差协方差，并由此设计卡尔曼滤波器，详细计算步骤如下：
步骤4.1：将线性化后的迭代优化函数在处求偏导，分别计算Φ(xk|k,Pk|k)对xk|k和Pk|k的偏导矩阵：
步骤4.2：令偏导等于0，以获取第i+1次迭代的状态估计
其中bi＝1/(1+βi)；
步骤4.3：将线性化后的迭代优化函数在处求偏导；
步骤4.4：令偏导等于0，以获取第i+1次迭代的状态估计误差协方差
第五步：第四步所设计的卡尔曼滤波器的自适应迭代终止条件如下：
其中，ε为迭代终止门限。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810315809.1A CN108599737B (zh) | 2018-04-10 | 2018-04-10 | 一种变分贝叶斯的非线性卡尔曼滤波器的设计方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810315809.1A CN108599737B (zh) | 2018-04-10 | 2018-04-10 | 一种变分贝叶斯的非线性卡尔曼滤波器的设计方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN108599737A true CN108599737A (zh) | 2018-09-28 |
| CN108599737B CN108599737B (zh) | 2021-11-23 |
Family
ID=63621486
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201810315809.1A Active CN108599737B (zh) | 2018-04-10 | 2018-04-10 | 一种变分贝叶斯的非线性卡尔曼滤波器的设计方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN108599737B (zh) |
Cited By (15)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109508445A (zh) * | 2019-01-14 | 2019-03-22 | 哈尔滨工程大学 | 一种带有色量测噪声和变分贝叶斯自适应卡尔曼滤波的目标跟踪方法 |
| CN110225454A (zh) * | 2019-06-26 | 2019-09-10 | 河南大学 | 一种置信度传递的分布式容积卡尔曼滤波协作定位方法 |
| CN110516198A (zh) * | 2019-07-17 | 2019-11-29 | 电子科技大学 | 一种分布式非线性卡尔曼滤波方法 |
| CN110649911A (zh) * | 2019-07-17 | 2020-01-03 | 电子科技大学 | 一种基于α散度的分布式非线性卡尔曼滤波方法 |
| CN111667513A (zh) * | 2020-06-01 | 2020-09-15 | 西北工业大学 | 一种基于ddpg迁移学习的无人机机动目标跟踪方法 |
| CN111737883A (zh) * | 2020-07-30 | 2020-10-02 | 哈尔滨工业大学 | 一种具有输出时滞的非线性双率电路系统鲁棒辨识方法 |
| CN112417219A (zh) * | 2020-11-16 | 2021-02-26 | 吉林大学 | 基于超图卷积的超边链接预测方法 |
| CN112764345A (zh) * | 2020-12-21 | 2021-05-07 | 杭州电子科技大学 | 基于目标状态跟踪的强非线性系统卡尔曼滤波器设计方法 |
| CN113165678A (zh) * | 2018-11-30 | 2021-07-23 | 泰雷兹控股英国有限公司 | 用于确定载运工具的位置的方法和装置 |
| CN114124033A (zh) * | 2021-10-11 | 2022-03-01 | 北京川速微波科技有限公司 | 卡尔曼滤波器的实现方法、装置、存储介质和设备 |
| CN114282320A (zh) * | 2021-12-24 | 2022-04-05 | 厦门大学 | 一种利用贝叶斯优化算法优化工程参数的方法 |
| CN114567288A (zh) * | 2022-01-25 | 2022-05-31 | 河南大学 | 基于变分贝叶斯的分布协同非线性系统状态估计方法 |
| CN114626307A (zh) * | 2022-03-29 | 2022-06-14 | 电子科技大学 | 一种基于变分贝叶斯的分布式一致性目标状态估计方法 |
| US20220260428A1 (en) * | 2021-02-10 | 2022-08-18 | Quantum Valley Ideas Laboratories | Increasing the Measurement Precision of Optical Instrumentation using Kalman-Type Filters |
| CN114282320B (zh) * | 2021-12-24 | 2024-06-07 | 厦门大学 | 一种利用贝叶斯优化算法优化工程参数的方法 |
Citations (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20040236604A1 (en) * | 2002-12-20 | 2004-11-25 | Mcnair Douglas S. | System and method for detecting spatiotemporal clusters |
| US20120197138A1 (en) * | 2009-03-18 | 2012-08-02 | Aisin Seiki Kabushiki Kaisha | Biological parameter monitoring method, computer-readable storage medium and biological parameter monitoring device |
| US20130246017A1 (en) * | 2012-03-14 | 2013-09-19 | Microsoft Corporation | Computing parameters of a predictive model |
| CN103888100A (zh) * | 2014-03-29 | 2014-06-25 | 北京航空航天大学 | 一种基于负熵的非高斯线性随机系统滤波方法 |
| CN104112079A (zh) * | 2014-07-29 | 2014-10-22 | 洛阳理工学院 | 一种模糊自适应变分贝叶斯无迹卡尔曼滤波方法 |
| CN104298650A (zh) * | 2014-09-30 | 2015-01-21 | 杭州电子科技大学 | 基于多方法融合的量化卡尔曼滤波方法 |
| US20160161606A1 (en) * | 2014-12-08 | 2016-06-09 | Northrop Grumman Systems Corporation | Variational track management |
| CN105719314A (zh) * | 2016-01-30 | 2016-06-29 | 西北工业大学 | 基于单应性估计和扩展卡尔曼滤波的无人机位置估计方法 |
| CN106683122A (zh) * | 2016-12-16 | 2017-05-17 | 华南理工大学 | 一种基于高斯混合模型和变分贝叶斯的粒子滤波方法 |
| CN107526794A (zh) * | 2017-08-16 | 2017-12-29 | 九次方大数据信息集团有限公司 | 数据处理方法和装置 |
| CN107783944A (zh) * | 2017-09-20 | 2018-03-09 | 北京航空航天大学 | 一种多模型自校准无迹卡尔曼滤波方法 |
| CN107796788A (zh) * | 2016-08-29 | 2018-03-13 | 南京理工大学 | 基于变分贝叶斯期望最大算法的传感矩阵测量方法 |
-
2018
- 2018-04-10 CN CN201810315809.1A patent/CN108599737B/zh active Active
Patent Citations (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20040236604A1 (en) * | 2002-12-20 | 2004-11-25 | Mcnair Douglas S. | System and method for detecting spatiotemporal clusters |
| US20120197138A1 (en) * | 2009-03-18 | 2012-08-02 | Aisin Seiki Kabushiki Kaisha | Biological parameter monitoring method, computer-readable storage medium and biological parameter monitoring device |
| US20130246017A1 (en) * | 2012-03-14 | 2013-09-19 | Microsoft Corporation | Computing parameters of a predictive model |
| CN103888100A (zh) * | 2014-03-29 | 2014-06-25 | 北京航空航天大学 | 一种基于负熵的非高斯线性随机系统滤波方法 |
| CN104112079A (zh) * | 2014-07-29 | 2014-10-22 | 洛阳理工学院 | 一种模糊自适应变分贝叶斯无迹卡尔曼滤波方法 |
| CN104298650A (zh) * | 2014-09-30 | 2015-01-21 | 杭州电子科技大学 | 基于多方法融合的量化卡尔曼滤波方法 |
| US20160161606A1 (en) * | 2014-12-08 | 2016-06-09 | Northrop Grumman Systems Corporation | Variational track management |
| CN105719314A (zh) * | 2016-01-30 | 2016-06-29 | 西北工业大学 | 基于单应性估计和扩展卡尔曼滤波的无人机位置估计方法 |
| CN107796788A (zh) * | 2016-08-29 | 2018-03-13 | 南京理工大学 | 基于变分贝叶斯期望最大算法的传感矩阵测量方法 |
| CN106683122A (zh) * | 2016-12-16 | 2017-05-17 | 华南理工大学 | 一种基于高斯混合模型和变分贝叶斯的粒子滤波方法 |
| CN107526794A (zh) * | 2017-08-16 | 2017-12-29 | 九次方大数据信息集团有限公司 | 数据处理方法和装置 |
| CN107783944A (zh) * | 2017-09-20 | 2018-03-09 | 北京航空航天大学 | 一种多模型自校准无迹卡尔曼滤波方法 |
Non-Patent Citations (6)
| Title |
|---|
| CHUNMEI YU 等: "A kernel-based bayesian classifier for fault detection and classification", 《2008 7TH WORLD CONGRESS ON INTELLIGENT CONTROL AND AUTOMATION》 * |
| XIAOGONG LIN 等: "The variational Bayesian-variable structure filter for uncertain system with model imprecision and unknown measurement noise", 《2017 36TH CHINESE CONTROL CONFERENCE (CCC)》 * |
| XIAOQING HU 等: "Generalized Iterated Kalman Filter and its Performance Evaluation", 《IEEE TRANSACTIONS ON SIGNAL PROCESSING》 * |
| 吴耀 等: "基于自适应卡尔曼滤波的侧滑移动机器人运动模型估计", 《电子与信息学报》 * |
| 沈锋 等: "一种自适应变分贝叶斯容积卡尔曼滤波方法", 《电机与控制学报》 * |
| 胡振涛 等: "基于Metropolis-Hastings采样的多传感器集合卡尔曼滤波算法", 《电子学报》 * |
Cited By (23)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113165678B (zh) * | 2018-11-30 | 2023-05-02 | 泰雷兹控股英国有限公司 | 用于确定载运工具的位置的方法和装置 |
| CN113165678A (zh) * | 2018-11-30 | 2021-07-23 | 泰雷兹控股英国有限公司 | 用于确定载运工具的位置的方法和装置 |
| CN109508445A (zh) * | 2019-01-14 | 2019-03-22 | 哈尔滨工程大学 | 一种带有色量测噪声和变分贝叶斯自适应卡尔曼滤波的目标跟踪方法 |
| CN109508445B (zh) * | 2019-01-14 | 2023-05-05 | 哈尔滨工程大学 | 一种带有色量测噪声和变分贝叶斯自适应卡尔曼滤波的目标跟踪方法 |
| CN110225454A (zh) * | 2019-06-26 | 2019-09-10 | 河南大学 | 一种置信度传递的分布式容积卡尔曼滤波协作定位方法 |
| CN110516198A (zh) * | 2019-07-17 | 2019-11-29 | 电子科技大学 | 一种分布式非线性卡尔曼滤波方法 |
| CN110649911A (zh) * | 2019-07-17 | 2020-01-03 | 电子科技大学 | 一种基于α散度的分布式非线性卡尔曼滤波方法 |
| CN110516198B (zh) * | 2019-07-17 | 2023-04-07 | 电子科技大学 | 一种分布式非线性卡尔曼滤波方法 |
| CN110649911B (zh) * | 2019-07-17 | 2023-10-27 | 电子科技大学 | 一种基于α散度的分布式非线性卡尔曼滤波方法 |
| CN111667513B (zh) * | 2020-06-01 | 2022-02-18 | 西北工业大学 | 一种基于ddpg迁移学习的无人机机动目标跟踪方法 |
| CN111667513A (zh) * | 2020-06-01 | 2020-09-15 | 西北工业大学 | 一种基于ddpg迁移学习的无人机机动目标跟踪方法 |
| CN111737883A (zh) * | 2020-07-30 | 2020-10-02 | 哈尔滨工业大学 | 一种具有输出时滞的非线性双率电路系统鲁棒辨识方法 |
| CN112417219A (zh) * | 2020-11-16 | 2021-02-26 | 吉林大学 | 基于超图卷积的超边链接预测方法 |
| CN112764345A (zh) * | 2020-12-21 | 2021-05-07 | 杭州电子科技大学 | 基于目标状态跟踪的强非线性系统卡尔曼滤波器设计方法 |
| US11435234B1 (en) * | 2021-02-10 | 2022-09-06 | Quantum Valley Ideas Laboratories | Increasing the measurement precision of optical instrumentation using Kalman-type filters |
| US20220260428A1 (en) * | 2021-02-10 | 2022-08-18 | Quantum Valley Ideas Laboratories | Increasing the Measurement Precision of Optical Instrumentation using Kalman-Type Filters |
| CN114124033A (zh) * | 2021-10-11 | 2022-03-01 | 北京川速微波科技有限公司 | 卡尔曼滤波器的实现方法、装置、存储介质和设备 |
| CN114282320A (zh) * | 2021-12-24 | 2022-04-05 | 厦门大学 | 一种利用贝叶斯优化算法优化工程参数的方法 |
| CN114282320B (zh) * | 2021-12-24 | 2024-06-07 | 厦门大学 | 一种利用贝叶斯优化算法优化工程参数的方法 |
| CN114567288A (zh) * | 2022-01-25 | 2022-05-31 | 河南大学 | 基于变分贝叶斯的分布协同非线性系统状态估计方法 |
| CN114567288B (zh) * | 2022-01-25 | 2024-04-26 | 河南大学 | 基于变分贝叶斯的分布协同非线性系统状态估计方法 |
| CN114626307A (zh) * | 2022-03-29 | 2022-06-14 | 电子科技大学 | 一种基于变分贝叶斯的分布式一致性目标状态估计方法 |
| CN114626307B (zh) * | 2022-03-29 | 2023-04-07 | 电子科技大学 | 一种基于变分贝叶斯的分布式一致性目标状态估计方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN108599737B (zh) | 2021-11-23 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108599737A (zh) | 一种变分贝叶斯的非线性卡尔曼滤波器的设计方法 | |
| Wang et al. | Nonlinear Gaussian smoothers with colored measurement noise | |
| Steinbring et al. | LRKF revisited: The smart sampling Kalman filter (S2KF) | |
| Kim et al. | Unscented FastSLAM: A robust algorithm for the simultaneous localization and mapping problem | |
| KR100816269B1 (ko) | 언센티드 필터를 적용한 강인한 동시 위치 추정 및 지도작성 방법 | |
| CN107783944A (zh) | 一种多模型自校准无迹卡尔曼滤波方法 | |
| CN104112079A (zh) | 一种模糊自适应变分贝叶斯无迹卡尔曼滤波方法 | |
| CN104950678A (zh) | 一种柔性机械臂系统的神经网络反演控制方法 | |
| CN110503071A (zh) | 基于变分贝叶斯标签多伯努利叠加模型的多目标跟踪方法 | |
| CN109284677A (zh) | 一种贝叶斯滤波目标跟踪算法 | |
| Yu et al. | Nonlinear filtering in unknown measurement noise and target tracking system by variational Bayesian inference | |
| Li et al. | State estimation for jump Markov linear systems by variational Bayesian approximation | |
| CN109582916A (zh) | 一种基于观测噪声方差未知的自适应卡尔曼滤波方法 | |
| CN103955600A (zh) | 一种目标跟踪方法及截断积分卡尔曼滤波方法、装置 | |
| Kim et al. | Exactly Rao-Blackwellized unscented particle filters for SLAM | |
| Liu et al. | Adaptive Gaussian sum squared-root cubature Kalman filter with split-merge scheme for state estimation | |
| Demim et al. | An adaptive SVSF-SLAM algorithm to improve the success and solving the UGVs cooperation problem | |
| CN109447122B (zh) | 一种分布式融合结构中的强跟踪渐消因子计算方法 | |
| CN106054167A (zh) | 基于强度滤波器的多扩展目标跟踪方法 | |
| CN107807906A (zh) | 一种多模型自校准秩滤波方法 | |
| CN109509207B (zh) | 一种对点目标和扩展目标进行无缝跟踪的方法 | |
| CN105701292B (zh) | 一种机动目标转弯角速度的解析辨识方法 | |
| CN109115228A (zh) | 一种基于加权最小二乘容积卡尔曼滤波的目标定位方法 | |
| Xiao et al. | A multiple model particle filter for maneuvering target tracking based on composite sampling | |
| CN106570536B (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 |