本发明涉及滤波器领域，尤其是一种卡尔曼滤波器的设计方法。
背景技术：
传统的非线性滤波器一般采用采样的方法逼近状态的后验概率密度函数或者将非线性函数线性化，然后根据近似结果计算状态和误差协方差的期望值，以简化后验概率密度函数(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,zzkkt(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时刻是状态转移概率，定义
置信优化的下界定义为：
其中，eq[·]是
最大化
步骤2.2：引入kl散度作为约束项
其中，i表示迭代次数，βi表示惩罚因子，
在高斯假设条件下，变分分布为
第三步：在变分分布条件下线性化迭代优化函数
步骤3.1：在
在高斯假设条件下对优化函数矩阵进行线性化，定义参数
根据bonnet定理和price定理，一阶梯度和二阶梯度可分别表示为：
其中，
1)当采用线性化近似的方法，则有：
由公式(18)得到公式(19)：
将公式(19)带入公式(13)中使用；
2)若采用采样近似的方法，则有：
由公式(20)得到公式(21)：
将公式(21)带入公式(13)中；
步骤3.2：在变分分布服从高斯分布的条件下，计算状态预测的概率密度函数和第i+1次变分分布之间的kl散度：
假设维数为d的变量ξ1服从均值为μ1方差为c1的高斯分布
因此，第i次迭代的变分分布和第i+1次迭代变分分布之间的kl散度为：
dx表示服从高斯分布的变量x的维数；
同理，公式(14)可化为：
步骤3.3：结合公式(13)-(15)和公式(23)-(24)，优化函数矩阵可化为：
第四步：将线性化后的迭代优化函数
步骤4.1：将线性化后的迭代优化函数在
步骤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,zzkkt(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时刻是状态转移概率，定义
置信优化的下界定义为：
其中，eq[·]是
最大化
步骤2.2：引入kl散度作为约束项
其中，i表示迭代次数，βi表示惩罚因子，在本发明中βi∈[1,5]，
在高斯假设条件下，变分分布为
第三步：在变分分布条件下线性化迭代优化函数
步骤3.1：在
在高斯假设条件下对优化函数矩阵进行线性化，定义参数
根据bonnet定理和price定理，一阶梯度和二阶梯度可分别表示为：
其中，
1)当采用线性化近似的方法，则有：
由公式(18)得到公式(19)：
将公式(19)带入公式(13)中使用；
2)若采用采样近似的方法，则有
由公式(20)得到公式(21)：
将公式(21)带入公式(13)中；
本发明采用线性化方法近似eq[logp(zk|xk)]；
步骤3.2：在变分分布服从高斯分布的条件下，计算状态预测的概率密度函数和第i+1次变分分布之间的kl散度：
假设维数为d的变量ξ1服从均值为μ1方差为c1的高斯分布
因此，第i次迭代的变分分布和第i+1次迭代变分分布之间的kl散度为：
dx表示服从高斯分布的变量x的维数；
同理，公式(14)可化为：
步骤3.3：结合公式(13)-(15)和公式(23)-(24)，优化函数矩阵可化为：
第四步：将线性化后的迭代优化函数
由变分贝叶斯方法机理知，置信下界到达最大时隐变量所对应的值和方差即是所求估计值和估计误差协方差，而置信下界的最大值等价于优化函数的最大值。因此，可通过对优化函数矩阵求极值，计算状态估计和估计误差协方差；
步骤4.1：将线性化后的迭代优化函数在
步骤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)作为滤波器估计精度的指标，montecarlo仿真次数为100。内核滤波执行器和迭代的新息期望近似方法如表1示。
表1仿真实验中pnkf-vb的滤波执行器和新息期望的近似方法
a)仿真场景1：
考虑笛卡尔二维坐标系下匀速直线运动(cv)模型，系统状态
其中，系统噪声方差和量测噪声方差分别为
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＝[50.627.40]t和p0＝diag{0.020.010.050.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和执行时间的定量对比
从以上仿真结果可知，本发明所推导的一种变分贝叶斯的非线性卡尔曼滤波器在保证一定计算量的同时提高状态估计精度，具有一定的理论价值和工程指导意义。