[go: up one dir, main page]

CN108599737B - 一种变分贝叶斯的非线性卡尔曼滤波器的设计方法 - Google Patents

一种变分贝叶斯的非线性卡尔曼滤波器的设计方法 Download PDF

Info

Publication number
CN108599737B
CN108599737B CN201810315809.1A CN201810315809A CN108599737B CN 108599737 B CN108599737 B CN 108599737B CN 201810315809 A CN201810315809 A CN 201810315809A CN 108599737 B CN108599737 B CN 108599737B
Authority
CN
China
Prior art keywords
state
formula
variational
function
distribution
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.)
Expired - Fee Related
Application number
CN201810315809.1A
Other languages
English (en)
Other versions
CN108599737A (zh
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.)
Northwestern Polytechnical University
Original Assignee
Northwestern Polytechnical University
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 Northwestern Polytechnical University filed Critical Northwestern Polytechnical University
Priority to CN201810315809.1A priority Critical patent/CN108599737B/zh
Publication of CN108599737A publication Critical patent/CN108599737A/zh
Application granted granted Critical
Publication of CN108599737B publication Critical patent/CN108599737B/zh
Expired - Fee Related legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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散度建立迭代优化函数
Figure BDA0001623731420000031
详细步骤如下：
步骤2.1：根据量测和状态之间的贝叶斯公式和Jensen不等式定理建立变分置信下界，并将置信下界作为目标函数；
变分贝叶斯的置信下界为目标函数矩阵在以xk为隐变量和zk为观测变量的状态空间模型中，k时刻的边缘概率密度函数表示为：
Figure BDA0001623731420000032
其中，p(zk,xk)＝p(zk|xk)p(xk|xk-1)为k时刻的联合分布，p(xk|xk-1)为从k-1到k时刻是状态转移概率，定义
Figure BDA0001623731420000033
其中，xk|k和Pk|k分别表示待估计变量的状态估计及其误差协方差，公式(9)满足Jensen不等式定理，因此公式(9)可化为公式(10)：
Figure BDA0001623731420000034
置信优化的下界定义为：
Figure BDA0001623731420000035
其中，Eq[·]是
Figure BDA0001623731420000036
的简化形式，p(xk)和q(xk|ψk)分别表示状态的真实分布和其变分分布，p(zk|xk)表示条件似然函数；
最大化
Figure BDA0001623731420000037
即实现将积分问题到优化问题的转化：
Figure BDA0001623731420000038
步骤2.2：引入KL散度作为约束项
Figure BDA0001623731420000039
其中，i表示迭代次数，βi表示惩罚因子，
Figure BDA0001623731420000041
表示变分分布q(xk|ψk)和变分分布
Figure BDA0001623731420000042
之间的KL散度；
在高斯假设条件下，变分分布为
Figure BDA0001623731420000043
假设k-1时刻的状态估计后验概率密度函数已知
Figure BDA0001623731420000044
因此公式(13)中第二项可进一步表示为：
Figure BDA0001623731420000045
第三步：在变分分布条件下线性化迭代优化函数
Figure BDA0001623731420000046
详细步骤如下：
步骤3.1：在
Figure BDA0001623731420000047
处线性化量测矩阵hk(xk)，以计算似然函数对数期望Eq[logp(zk|xk)]的一阶梯度和二阶梯度，进而根据泰勒公式线性化对数似然函数的期望Eq[logp(zk|xk)]，详细步骤如下：
在高斯假设条件下对优化函数矩阵进行线性化，定义参数
Figure BDA0001623731420000048
和
Figure BDA0001623731420000049
分别为Eq[logp(zk|xk|k)]的一阶梯度和二阶梯度，则：
Figure BDA00016237314200000410
根据Bonnet定理和Price定理，一阶梯度和二阶梯度可分别表示为：
Figure BDA00016237314200000411
Figure BDA0001623731420000051
其中，
Figure BDA0001623731420000052
公式(16)中
Figure BDA0001623731420000053
表示迭代过程中新息的期望，本发明采用线性化近似和采样近似两种近似方法：
1)当采用线性化近似的方法，则有：
Figure BDA0001623731420000054
由公式(18)得到公式(19)：
Figure BDA0001623731420000055
将公式(19)带入公式(13)中使用；
2)若采用采样近似的方法，则有：
Figure BDA0001623731420000056
由公式(20)得到公式(21)：
Figure BDA0001623731420000057
将公式(21)带入公式(13)中；
步骤3.2：在变分分布服从高斯分布的条件下，计算状态预测的概率密度函数和第i+1次变分分布之间的KL散度：
假设维数为D的变量ξ1服从均值为μ1方差为C1的高斯分布
Figure BDA0001623731420000058
相同维数变量ξ2服从均值为μ2方差为C2的高斯分布
Figure BDA0001623731420000061
则两者的KL散度可化为如下形式：
Figure BDA0001623731420000062
因此，第i次迭代的变分分布和第i+1次迭代变分分布之间的KL散度为：
Figure BDA0001623731420000063
Dx表示服从高斯分布的变量x的维数；
同理，公式(14)可化为：
Figure BDA0001623731420000064
步骤3.3：结合公式(13)-(15)和公式(23)-(24)，优化函数矩阵可化为：
Figure BDA0001623731420000065
第四步：将线性化后的迭代优化函数
Figure BDA0001623731420000066
求偏导和计算状态估计和估计误差协方差，并由此设计卡尔曼滤波器，详细计算步骤如下：
步骤4.1：将线性化后的迭代优化函数在
Figure BDA0001623731420000067
处求偏导，分别计算Φ(xk|k,Pk|k)对xk|k和Pk|k的偏导矩阵：
Figure BDA0001623731420000071
步骤4.2：令偏导等于0，以获取第i+1次迭代的状态估计
Figure BDA0001623731420000072
Figure BDA0001623731420000073
Figure BDA0001623731420000074
其中bi＝1/(1+βi)；
步骤4.3：将线性化后的迭代优化函数在
Figure BDA0001623731420000075
处求偏导；
步骤4.4：令偏导等于0，以获取第i+1次迭代的状态估计误差协方差
Figure BDA0001623731420000076
第五步：第四步所设计的卡尔曼滤波器的自适应迭代终止条件如下：
Figure BDA0001623731420000077
其中，ε为迭代终止门限。
本发明的有益效果是由于采用在变分贝叶斯框架下推导给出一种变分贝叶斯的非线性卡尔曼滤波器的方法，能够获得非线性状态后验概率密度函数更“紧”的逼近形式，从而提高状态估计精度。本发明可以实现系统状态估计过程中后验概率密度函数积分的难以求解问题转化为优化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散度建立迭代优化函数
Figure BDA0001623731420000091
详细步骤如下：
步骤2.1：根据量测和状态之间的贝叶斯公式和Jensen不等式定理建立变分置信下界，并将置信下界作为目标函数；
在变分贝叶斯框架中，通过恰当的变分分布近似状态后验概率密度函数，当置信函数最大，变分分布和后验概率密度函数之间的KL散度为0时，如图2所示，后验概率密度函数等于变分分布，因此，考虑分别将置信函数最大和KL散度趋于0作为目标函数和约束项，从而构建优化函数并推导迭代优化状态估计。其过程如下：
变分贝叶斯的置信下界为目标函数矩阵在以xk为隐变量和zk为观测变量的状态空间模型中，k时刻的边缘概率密度函数表示为：
Figure BDA0001623731420000101
其中，p(zk,xk)＝p(zk|xk)p(xk|xk-1)为k时刻的联合分布，p(xk|xk-1)为从k-1到k时刻是状态转移概率，定义
Figure BDA0001623731420000102
其中，xk|k和Pk|k分别表示待估计变量的状态估计及其误差协方差，公式(9)满足Jensen不等式定理，因此公式(9)可化为公式(10)：
Figure BDA0001623731420000103
置信优化的下界定义为：
Figure BDA0001623731420000104
其中，Eq[·]是
Figure BDA0001623731420000105
的简化形式，p(xk)和q(xk|ψk)分别表示状态的真实分布和其变分分布，p(zk|xk)表示条件似然函数；
最大化
Figure BDA0001623731420000106
即实现将积分问题到优化问题的转化：
Figure BDA0001623731420000107
步骤2.2：引入KL散度作为约束项
Figure BDA0001623731420000108
其中，i表示迭代次数，βi表示惩罚因子，在本发明中βi∈[1,5]，
Figure BDA0001623731420000109
表示变分分布q(xk|ψk)和变分分布
Figure BDA00016237314200001010
之间的KL散度；
在高斯假设条件下，变分分布为
Figure BDA00016237314200001011
假设k-1时刻的状态估计后验概率密度函数已知
Figure BDA00016237314200001012
因此公式(13)中第二项可进一步表示为：
Figure BDA0001623731420000111
第三步：在变分分布条件下线性化迭代优化函数
Figure BDA0001623731420000112
详细步骤如下：
步骤3.1：在
Figure BDA0001623731420000113
处线性化量测矩阵hk(xk)，以计算似然函数对数期望Eq[logp(zk|xk)]的一阶梯度和二阶梯度，进而根据泰勒公式线性化对数似然函数的期望Eq[logp(zk|xk)]，详细步骤如下：
在高斯假设条件下对优化函数矩阵进行线性化，定义参数
Figure BDA0001623731420000114
和
Figure BDA0001623731420000115
分别为Eq[logp(zk|xk|k)]的一阶梯度和二阶梯度，则：
Figure BDA0001623731420000116
根据Bonnet定理和Price定理，一阶梯度和二阶梯度可分别表示为：
Figure BDA0001623731420000117
Figure BDA0001623731420000121
其中，
Figure BDA0001623731420000122
公式(16)中
Figure BDA0001623731420000123
表示迭代过程中新息的期望，本发明给出线性化近似和采样近似两种近似方法：
1)当采用线性化近似的方法，则有：
Figure BDA0001623731420000124
由公式(18)得到公式(19)：
Figure BDA0001623731420000125
将公式(19)带入公式(13)中使用；
2)若采用采样近似的方法，则有
Figure BDA0001623731420000126
由公式(20)得到公式(21)：
Figure BDA0001623731420000127
将公式(21)带入公式(13)中；
本发明采用线性化方法近似Eq[logp(zk|xk)]；
步骤3.2：在变分分布服从高斯分布的条件下，计算状态预测的概率密度函数和第i+1次变分分布之间的KL散度：
假设维数为D的变量ξ1服从均值为μ1方差为C1的高斯分布
Figure BDA0001623731420000131
相同维数变量ξ2服从均值为μ2方差为C2的高斯分布
Figure BDA0001623731420000132
则两者的KL散度可化为如下形式：
Figure BDA0001623731420000133
因此，第i次迭代的变分分布和第i+1次迭代变分分布之间的KL散度为：
Figure BDA0001623731420000134
Dx表示服从高斯分布的变量x的维数；
同理，公式(14)可化为：
Figure BDA0001623731420000135
步骤3.3：结合公式(13)-(15)和公式(23)-(24)，优化函数矩阵可化为：
Figure BDA0001623731420000136
第四步：将线性化后的迭代优化函数
Figure BDA0001623731420000137
求偏导和计算状态估计和估计误差协方差，并由此设计卡尔曼滤波器，详细计算步骤如下：
由变分贝叶斯方法机理知，置信下界到达最大时隐变量所对应的值和方差即是所求估计值和估计误差协方差，而置信下界的最大值等价于优化函数的最大值。因此，可通过对优化函数矩阵求极值，计算状态估计和估计误差协方差；
步骤4.1：将线性化后的迭代优化函数在
Figure BDA0001623731420000141
处求偏导，分别计算Φ(xk|k,Pk|k)对xk|k和Pk|k的偏导矩阵：
Figure BDA0001623731420000142
步骤4.2：令偏导等于0，以获取第i+1次迭代的状态估计
Figure BDA0001623731420000143
Figure BDA0001623731420000144
Figure BDA0001623731420000145
其中bi＝1/(1+βi)；
步骤4.3：将线性化后的迭代优化函数在
Figure BDA0001623731420000146
处求偏导；
步骤4.4：令偏导等于0，以获取第i+1次迭代的状态估计误差协方差
Figure BDA0001623731420000147
第五步：第四步所设计的卡尔曼滤波器的自适应迭代终止条件如下：
Figure BDA0001623731420000148
其中，ε为迭代终止门限，本发明取值为ε＝10-6。
本发明借助变分贝叶斯框架通过构建变分分布将难以获得解析解的后验概率密度函数的积分问题转化为置信下界的优化问题，在估计优化的推导过程中，首先利用贝叶斯估计方法获取系统状态估计，并将其作为迭代优化的初始条件；然后分别以置信下界最大和KL散度为目标函数和惩罚函数设计优化函数；继而对优化函数进行线性化、求偏导等一系列推导；同时，在一定迭代门限和状态估计条件下，设计自适应迭代步骤，从而获得状态的迭代估计。针对高斯非线性状态估计的问题，本发明借鉴置信下界优化思想，灵活运用变分贝叶斯框架，提高了非线性状态估计精度，并且由于结合贝叶斯滤波方法，可通过多种非线性滤波器实现迭代的初始化，具有较好的可移植性。
本发明考虑通过将难以获得解析解的后验概率密度函数的积分问题转化为置信下界的优化问题，借助于变分贝叶斯框架和罚函数思想推导给出迭代优化的状态估计，从而提升非线性系统状态估计精度。
一般而言，在非线性系统中由于状态的后验概率密度函数难以获得解析解，要解决非线性状态的估计问题，必须采用恰当的逼近方法对后验概率密度函数进行近似，并且其逼近程度决定着非线性状态估计精度。本发明基于变分贝叶斯框架给出了非线性系统中后验概率密度函数的变分分布近似策略，提出一种变分贝叶斯的非线性卡尔曼滤波器，实现了系统状态的较“紧”估计。
为验证算法的有效性，采用目标跟踪场景下的状态估计模型对比本发明方法与EKF，IEKF，CKF，UKF和PF的估计精度和实时性。均方根误差(RMSE)作为滤波器估计精度的指标，Monte Carlo仿真次数为100。内核滤波执行器和迭代的新息期望近似方法如表1示。
表1仿真实验中PNKF-VB的滤波执行器和新息期望的近似方法
Figure BDA0001623731420000151
A)仿真场景1：
考虑笛卡尔二维坐标系下匀速直线运动(CV)模型，系统状态
Figure BDA0001623731420000152
其中(xk,yk)和
Figure BDA0001623731420000153
分别表示k时刻目标在x轴和y轴位置分量和速度分量。状态转移阵和系统噪声驱动矩阵分别表示为：
Figure BDA0001623731420000154
其中，系统噪声方差和量测噪声方差分别为
Figure BDA0001623731420000155
初始状态和相应的误差协方差分别为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均值和执行时间的定量对比
Figure BDA0001623731420000161
B)仿真场景2：
在仿真场景2中，采用匀速圆周运动(CT)，相应的状态转移矩阵为：
Figure BDA0001623731420000162
其中，ω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和执行时间的定量对比
Figure BDA0001623731420000171
从以上仿真结果可知，本发明所推导的一种变分贝叶斯的非线性卡尔曼滤波器在保证一定计算量的同时提高状态估计精度，具有一定的理论价值和工程指导意义。

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)Τ] (3)
Pk,xz＝E[(xk-xk|k-1)(zk-zk|k-1)Τ] (4)
Pk,zz＝E[(zk-zk|k-1)(zk-zk|k-1)Τ] (5)
其中，xk-1表示k-1时刻的状态值，xk|k-1和zk|k-1分别表示k-1时刻到k时刻的状态预测值和量测预测值，fk(g)和hk(g)分别表示状态转移函数和量测函数，Pk|k-1，Pk,xz和Pk,zz分别表示k-1时刻到k时刻的状态预测误差协方差，状态预测与量测预测之间的交互误差协方差，量测预测误差协方差，E[g]表示求期望；
步骤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 Τ (8)
第二步：根据变分贝叶斯框架和KL散度建立迭代优化函数
Figure FDA0003212239260000011
详细步骤如下：
步骤2.1：根据量测和状态之间的贝叶斯公式和Jensen不等式定理建立变分置信下界，并将置信下界作为目标函数；
变分贝叶斯的置信下界为目标函数矩阵在以xk为隐变量和zk为观测变量的状态空间模型中，k时刻的边缘概率密度函数表示为：
Figure FDA0003212239260000021
其中，p(zk,xk)＝p(zk|xk)p(xk|xk-1)为k时刻的联合分布，p(xk|xk-1)为从k-1到k时刻是状态转移概率，定义ψk@(xk|k,Pk|k)，其中，xk|k和Pk|k分别表示待估计变量的状态估计及其误差协方差，公式(9)满足Jensen不等式定理，因此公式(9)可化为公式(10)：
Figure FDA0003212239260000022
置信优化的下界定义为：
Figure FDA0003212239260000023
其中，
Figure FDA0003212239260000024
是
Figure FDA0003212239260000025
的简化形式，p(xk)和q(xk|ψk)分别表示状态的真实分布和其变分分布，p(zk|xk)表示条件似然函数；
最大化L(ψk)即实现将积分问题到优化问题的转化：
Figure FDA0003212239260000026
步骤2.2：引入KL散度作为约束项
Figure FDA0003212239260000027
其中，i表示迭代次数，βi表示惩罚因子，
Figure FDA0003212239260000028
表示变分分布q(xk|ψk)和变分分布
Figure FDA0003212239260000029
之间的KL散度；
在高斯假设条件下，表示xk服从均值为xk|k方差为Pk|k的高斯分布N(xk|xk|k,Pk|k)，假设k-1时刻的状态估计后验概率密度函数已知p(xk)～N(xk|xk|k-1,Pk|k-1)，因此公式(13)中第二项可进一步表示为：
Figure FDA0003212239260000031
第三步：在变分分布条件下线性化迭代优化函数
Figure FDA0003212239260000032
详细步骤如下：
步骤3.1：在
Figure FDA0003212239260000033
处线性化量测矩阵hk(xk)，以计算似然函数对数期望Eq[logp(zk|xk)]的一阶梯度和二阶梯度，进而根据泰勒公式线性化对数似然函数的期望Eq[logp(zk|xk)]，详细步骤如下：
在高斯假设条件下对优化函数矩阵进行线性化，定义参数
Figure FDA0003212239260000034
和
Figure FDA0003212239260000035
分别为Eq[logp(zk|xk|k)]的一阶梯度和二阶梯度，则：
Figure FDA0003212239260000036
其中，
Figure FDA0003212239260000037
和
Figure FDA0003212239260000038
分别表示计算关于xk|k和Pk|k的梯度；
根据Bonnet定理和Price定理，一阶梯度和二阶梯度可分别表示为：
Figure FDA0003212239260000039
Figure FDA0003212239260000041
其中，
Figure FDA0003212239260000042
表示量测噪声方差Rk的逆，
Figure FDA0003212239260000043
公式(16)中
Figure FDA0003212239260000044
表示迭代过程中信息的期望，采用线性化近似和采样近似两种近似方法：
1)当采用线性化近似的方法，则有：
Figure FDA0003212239260000045
由公式(18)得到公式(19)：
Figure FDA0003212239260000046
将公式(19)带入公式(13)中使用；
2)若采用采样近似的方法，则有：
Figure FDA0003212239260000047
其中，N表示状态估计样本
Figure FDA0003212239260000048
个数；
由公式(20)得到公式(21)：
Figure FDA0003212239260000049
将公式(21)带入公式(13)中；
步骤3.2：在变分分布服从高斯分布的条件下，计算状态预测的概率密度函数和第i+1次变分分布之间的KL散度：
假设维数为D的变量ξ1服从均值为μ1方差为C1的高斯分布N(ξ1|μ1,C1)，相同维数变量ξ2服从均值为μ2方差为C2的高斯分布N(ξ2|μ2,C2)，则两者的KL散度可化为如下形式：
Figure FDA0003212239260000051
因此，第i次迭代的变分分布和第i+1次迭代变分分布之间的KL散度为：
Figure FDA0003212239260000052
Dx表示服从高斯分布的变量x的维数；
同理，公式(14)可化为：
Figure FDA0003212239260000053
步骤3.3：结合公式(13)-(15)和公式(23)-(24)，优化函数矩阵可化为：
Figure FDA0003212239260000054
第四步：将线性化后的迭代优化函数
Figure FDA0003212239260000055
求偏导和计算状态估计和估计误差协方差，并由此设计卡尔曼滤波器，详细计算步骤如下：
步骤4.1：将线性化后的迭代优化函数在
Figure FDA0003212239260000061
处求偏导，分别计算Φ(xk|k,Pk|k)对xk|k和Pk|k的偏导矩阵：
Figure FDA0003212239260000062
步骤4.2：令偏导等于0，以获取第i+1次迭代的状态估计
Figure FDA0003212239260000063
Figure FDA0003212239260000064
Figure FDA0003212239260000065
其中bi＝1/(1+βi)；
步骤4.3：将线性化后的迭代优化函数在
Figure FDA0003212239260000066
处求偏导；
步骤4.4：令偏导等于0，以获取第i+1次迭代的状态估计误差协方差
Figure FDA0003212239260000067
第五步：第四步所设计的卡尔曼滤波器的自适应迭代终止条件如下：
Figure FDA0003212239260000068
其中，ε为迭代终止门限。
CN201810315809.1A 2018-04-10 2018-04-10 一种变分贝叶斯的非线性卡尔曼滤波器的设计方法 Expired - Fee Related CN108599737B (zh)

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 CN108599737A (zh) 2018-09-28
CN108599737B true CN108599737B (zh) 2021-11-23

Family

ID=63621486

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810315809.1A Expired - Fee Related CN108599737B (zh) 2018-04-10 2018-04-10 一种变分贝叶斯的非线性卡尔曼滤波器的设计方法

Country Status (1)

Country Link
CN (1) CN108599737B (zh)

Families Citing this family (16)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
GB2579414B (en) * 2018-11-30 2021-11-17 Thales Holdings Uk Plc Method and apparatus for determining a position of a vehicle
CN109508445B (zh) * 2019-01-14 2023-05-05 哈尔滨工程大学 一种带有色量测噪声和变分贝叶斯自适应卡尔曼滤波的目标跟踪方法
CN110225454B (zh) * 2019-06-26 2020-12-18 河南大学 一种置信度传递的分布式容积卡尔曼滤波协作定位方法
CN110649911B (zh) * 2019-07-17 2023-10-27 电子科技大学 一种基于α散度的分布式非线性卡尔曼滤波方法
CN110516198B (zh) * 2019-07-17 2023-04-07 电子科技大学 一种分布式非线性卡尔曼滤波方法
CN111667513B (zh) * 2020-06-01 2022-02-18 西北工业大学 一种基于ddpg迁移学习的无人机机动目标跟踪方法
CN111737883B (zh) * 2020-07-30 2021-08-13 哈尔滨工业大学 一种具有输出时滞的非线性双率电路系统鲁棒辨识方法
CN112417219B (zh) * 2020-11-16 2022-07-01 吉林大学 基于超图卷积的超边链接预测方法
CN112764345B (zh) * 2020-12-21 2022-08-02 杭州电子科技大学 基于目标状态跟踪的强非线性系统卡尔曼滤波器设计方法
US11435234B1 (en) * 2021-02-10 2022-09-06 Quantum Valley Ideas Laboratories Increasing the measurement precision of optical instrumentation using Kalman-type filters
CN114124033B (zh) * 2021-10-11 2024-10-25 北京川速微波科技有限公司 卡尔曼滤波器的实现方法、装置、存储介质和设备
CN114329338A (zh) * 2021-12-02 2022-04-12 复旦大学 一种非线性或非高斯分布系统的贝叶斯动态估计算法
CN114282320B (zh) * 2021-12-24 2024-06-07 厦门大学 一种利用贝叶斯优化算法优化工程参数的方法
CN114567288B (zh) * 2022-01-25 2024-04-26 河南大学 基于变分贝叶斯的分布协同非线性系统状态估计方法
CN114626307B (zh) * 2022-03-29 2023-04-07 电子科技大学 一种基于变分贝叶斯的分布式一致性目标状态估计方法
CN118171050B (zh) * 2024-05-14 2024-07-12 河海大学 用于组合导航系统的低计算成本非线性卡尔曼滤波方法

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103888100A (zh) * 2014-03-29 2014-06-25 北京航空航天大学 一种基于负熵的非高斯线性随机系统滤波方法
CN104112079A (zh) * 2014-07-29 2014-10-22 洛阳理工学院 一种模糊自适应变分贝叶斯无迹卡尔曼滤波方法
CN104298650A (zh) * 2014-09-30 2015-01-21 杭州电子科技大学 基于多方法融合的量化卡尔曼滤波方法
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 南京理工大学 基于变分贝叶斯期望最大算法的传感矩阵测量方法

Family Cites Families (4)

* Cited by examiner, † Cited by third party
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
FR2943233B1 (fr) * 2009-03-18 2013-10-11 Imra Europe Sas Procede de surveillance d'un parametre biologique d'une personne au moyen d'un filtrage non-lineaire bayesien
US20130246017A1 (en) * 2012-03-14 2013-09-19 Microsoft Corporation Computing parameters of a predictive model
US10310068B2 (en) * 2014-12-08 2019-06-04 Northrop Grumman Systems Corporation Variational track management

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103888100A (zh) * 2014-03-29 2014-06-25 北京航空航天大学 一种基于负熵的非高斯线性随机系统滤波方法
CN104112079A (zh) * 2014-07-29 2014-10-22 洛阳理工学院 一种模糊自适应变分贝叶斯无迹卡尔曼滤波方法
CN104298650A (zh) * 2014-09-30 2015-01-21 杭州电子科技大学 基于多方法融合的量化卡尔曼滤波方法
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)

* Cited by examiner, † Cited by third party
Title
A kernel-based bayesian classifier for fault detection and classification;ChunMei Yu 等;《2008 7th World Congress on Intelligent Control and Automation》;20080808;124-128 *
Generalized Iterated Kalman Filter and its Performance Evaluation;Xiaoqing Hu 等;《IEEE Transactions on Signal Processing》;20150415;第63卷(第12期);3204-3217 *
The variational Bayesian-variable structure filter for uncertain system with model imprecision and unknown measurement noise;Xiaogong Lin 等;《2017 36th Chinese Control Conference (CCC)》;20170911;5469-5474 *
一种自适应变分贝叶斯容积卡尔曼滤波方法;沈锋 等;《电机与控制学报》;20150415;第19卷(第4期);94-99 *
基于Metropolis-Hastings采样的多传感器集合卡尔曼滤波算法;胡振涛 等;《电子学报》;20170415;第45卷(第4期);868-873 *
基于自适应卡尔曼滤波的侧滑移动机器人运动模型估计;吴耀 等;《电子与信息学报》;20151215;第37卷(第12期);3016-3024 *

Also Published As

Publication number Publication date
CN108599737A (zh) 2018-09-28

Similar Documents

Publication Publication Date Title
CN108599737B (zh) 一种变分贝叶斯的非线性卡尔曼滤波器的设计方法
CN111985093A (zh) 一种带噪声估计器的自适应无迹卡尔曼滤波状态估计方法
CN109990786B (zh) 机动目标跟踪方法及装置
Grisetti et al. Improved techniques for grid mapping with rao-blackwellized particle filters
CN110501696B (zh) 一种基于多普勒量测自适应处理的雷达目标跟踪方法
Särkkä et al. On continuous-discrete cubature Kalman filtering
CN106487358A (zh) 一种基于统计线性回归的最大相关熵容积卡尔曼滤波方法
CN106683122A (zh) 一种基于高斯混合模型和变分贝叶斯的粒子滤波方法
Steinbring et al. LRKF Revisited-The Smart Sampling Kalman Filter (S²KF)
CN109444841B (zh) 基于修正切换函数的平滑变结构滤波方法及系统
CN103217175A (zh) 一种自适应容积卡尔曼滤波方法
Yu et al. Nonlinear filtering in unknown measurement noise and target tracking system by variational Bayesian inference
CN111047627A (zh) 一种平滑约束无迹卡尔曼滤波方法及目标跟踪方法
CN104121907A (zh) 一种基于平方根容积卡尔曼滤波器的飞行器姿态估计方法
CN109520503A (zh) 一种平方根容积模糊自适应卡尔曼滤波slam方法
CN108871365B (zh) 一种航向约束下的状态估计方法及系统
Meskin et al. Parameter estimation of biological phenomena: an unscented Kalman filter approach
CN117392215A (zh) 一种基于改进amcl和pl-icp点云匹配的移动机器人位姿校正方法
Feng et al. Overview of nonlinear Bayesian filtering algorithm
Qian et al. Algorithm of Gaussian sum filter based on SGQF for nonlinear non-Gaussian models
CN115495707A (zh) 一种基于模糊自适应ukf的交互多模型机动目标跟踪方法
CN106934124B (zh) 一种基于量测变化检测的自适应变划窗方法
Javad et al. Comparing EKF and SPKF algorithms for simultaneous localization and mapping (SLAM)
CN114236524A (zh) 多普勒雷达序贯平滑变结构滤波方法及装置
Sun et al. State estimation for systems with unknown inputs based on variational Bayes method

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
CF01 Termination of patent right due to non-payment of annual fee

Granted publication date: 20211123