【技术领域】
本发明提供一种多模型自校准扩展卡尔曼滤波方法,属于鲁棒卡尔曼滤波技术领域。
背景技术:
卡尔曼滤波是一种利用系统状态方程和量测方程对系统状态进行估计的方法,自1960年被提出后在工程领域得到了广泛的应用,然而无论是针对线性系统还是非线性系统的卡尔曼滤波方法,它们都要求系统方程是精确的。但在工程实际中,由于环境因素的影响、模型和参数选取不当等原因,系统状态方程往往受到未知输入的干扰,从而使滤波精度下降,甚至导致滤波发散。针对此问题,文献“自校准kalman滤波方法[j].航空动力学报.2014,29(06):1363-1368”提出了一种自校准卡尔曼滤波方法(self-calibrationkalmanfilter,skf),该方法在依照原始状态方程进行迭代运算的同时,对未知输入项进行估计,从而使未知输入的影响自动得到补偿。
自校准卡尔曼滤波方法的提出很好地解决了系统状态方程受未知输入影响的问题,但是由于系统不确定因素的存在,未知输入也有为零的可能。在这种情况下,自校准卡尔曼滤波方法由于在先验估计中引入了对未知输入项的估计,尽管该估计值很小,其滤波精度仍不及没有考虑未知输入影响的标准卡尔曼滤波方法。为了进一步提升自校准卡尔曼滤波在未知输入为零段的滤波精度,研究人员基于多模型估计理论,提出了多模型自校准卡尔曼滤波方法(multiple-modelself-calibrationkalmanfilter,mskf)。该方法同时采用kf和skf进行滤波,根据贝叶斯定理实时更新两者先验估计的权重,通过加权融合进而得到最终的状态估计值,充分发挥了两种方法在不同阶段各自的优势。然而,多模型自校准卡尔曼滤波方法虽然在保证了未知输入非零段滤波精度的同时提升了未知输入为零段的精度,但是只适用于线性系统,无法处理工程实际中普遍存在的非线性系统受未知输入影响的问题。
技术实现要素:
本发明的目的是提供一种多模型自校准扩展卡尔曼滤波方法(multiple-modelself-calibrationextendedkalmanfilter,msekf),它通过将多模型估计理论引入到sekf中,将mskf的应用范围拓展到了非线性领域。其同时采用ekf与sekf进行计算,实时更新二者权重,进而得到状态估计。大量数值仿真表明,无论非线性系统状态方程受到未知输入的影响是否为零,本文方法都取得了良好的滤波结果,且计算简单,适用范围广,便于工程实际应用。
本发明一种多模型自校准扩展卡尔曼滤波方法,它包含以下七个步骤:
步骤一:建立系统基本方程
多模型自校准扩展卡尔曼滤波采用自校准扩展卡尔曼滤波与扩展卡尔曼滤波两种方法进行运算,故系统包含两个状态方程,第一个为含有未知输入项的状态方程,第二个为标准的非线性状态方程,其具体表达式为
zk=hk(xk)+vk(3)
式中,xk表示系统的状态向量,
式中,cov[·]为协方差,e[·]为数学期望,δkj为δ函数,当k=j时,δkj=1,当k≠j时,δkj=0;
步骤二:对由式(1)、式(2)和式(3)所组成系统进行线性化处理
经过线性化处理后的系统方程变为
zk=hkxk+vk(9)
式中,φk和hk分别为状态转移矩阵和量测矩阵;
步骤三:对系统进行滤波初始化
设定状态估计与估计误差方差矩阵的初始值为
同时,为了完成两模型估计结果的融合,还需要设定两种模型的概率初始值
pr(1|z3)=pr(2|z3)=0.5(12)
以及用于迭代计算的概率初始值prmax和prmin。初始化prmax和prmin的原因如下:
在多模型估计运算过程中,一些模型会由于对应的概率逐渐趋近为零而被淘汰,故参与运算的模型数量n在不断减小,这会降低系统对复杂环境的适应能力;目前国际上针对这一问题的解决途径是为系统设置概率下限,当某一模型的概率值低于此下限时,概率值便保持此下限值不再改变;这样处理的优点是在不影响估计结果的同时保持了模型的多样性,但缺点同样明显,即处于概率下限的模型如果要再次发挥作用,其概率值需要较长时间的迭代运算才能恢复,这对卡尔曼滤波的实时性有很大的影响。为了克服这一缺点,针对本文方法只选取两个动力学模型且采用最高概率法选取先验估计值,故只需要定性分析两种模型的概率大小而不需要精确计算概率值的特点,多模型自校准卡尔曼滤波方法不再使用计算得到的条件概率值进行迭代,而是设定两个确定的概率初始值prmin和prmax=1-prmin;在每一步滤波之前,通过比较上一步概率结果的大小将其分别赋给两个模型,并以它们为初始值更新当前时刻的模型概率;由于prmin并不是概率下限那样的极小值,因此可以保证概率恢复的速度,从而使卡尔曼滤波的实时性得到保证;
步骤四:对系统进行时间更新
当k=1,2时,状态一步预测值
一步预测误差方差矩阵
当k>2时,状态一步预测值
一步预测误差方差矩阵
式中
j=argmaxjpr(j|zk)(19)
其中,函数argmax[f(x)]返回当f(x)最大时x的值,并且
s1=p1(27)
在上述计算过程中,式(15)提供自校准状态方程的一步预测结果,式(16)提供标准状态方程计算的一步预测结果,然后通过式(12)中概率大小的比对,由式(14)完成最终的一步预测值筛选。概率大小的计算依托于贝叶斯原理,比较的是在当前时刻量测值已知的情况下,两种模型的条件概率,如式(19)和式(20)所示,其他计算过程则来自于多模型估计理论;
步骤五:迭代变量更新
在步骤(三)中有很多中间变量需要实时更新,因此有必要得到它们的递推公式,进而保证整个滤波过程的顺利进行。这些迭代变量包括:
各模型量测更新
各模型条件概率重置
pr(j|zk)=prmax(33)
pr[(3-j)|zk]=prmin(34)
步骤六:量测更新
滤波增益矩阵
状态估计值
此步骤的计算公式由于不涉及自校准技术的基本假设与多模型估计理论的相关内容,所以计算过程与标准卡尔曼滤波方法的后验估计公式保持一致;同时需要说明的是,由于多模型自校准卡尔曼滤波方法是通过两个模型结果的融合计算得到的,因此在计算过程中并不需要估计误差方差矩阵在相邻时刻间的传递,所以在计算步骤中没有给出pk的计算公式;
步骤七:迭代计算
根据k时刻的两个模型分别得到的状态估计值
通过以上步骤,工程中经常遇到的非线性系统状态方程受到未知输入影响的问题得到了解决,同时,由于考虑了未知输入为零的情况,因此相比自校准扩展卡尔曼滤波方法,对复杂环境的适应性得到了提升。而且由于本发明基于扩展卡尔曼滤波方法,不需要通过采样的形式完成信息的传递,所以其处理非线性系统的运算速度较其他非线性滤波方法更加快捷,便于工程实际应用。
本发明的优点与积极效果在于:
(1)将多模型估计理论引入非线性系统状态方程受未知输入干扰的问题中,基于自校准扩展尔曼滤波和扩展卡尔曼滤波,在理论上推导得到了多模型自校准扩展卡尔曼滤波方法的完整过程。
(2)该发明融合了ekf和sekf两种方法各自的优点,既解决了扩展卡尔曼滤波在未知输入非零段滤波发散的问题,也显著提升了自校准扩展卡尔曼滤波在未知输入为零段的滤波精度。
(3)该发明同时提升了未知输入为零段和非零段的滤波精度,且针对非线性系统提出。适用范围得到了极大的扩展,同时系统鲁棒性也在自校准扩展卡尔曼滤波方法的基础上得到了进一步的提升。
【附图说明】
图1为本发明方法流程示意图。
图2为步骤(四)时间更新流程示意图。
发明中序号、符号、代号说明如下:
xk:系统的状态向量
f(·)、h(·):非线性向量函数
zk:系统量测向量
φk:状态转移矩阵
hk:量测矩阵
bk:未知输入
wk:系统噪声向量
vk:量测噪声向量
qk:系统噪声向量方差矩阵
rk:量测噪声向量方差矩阵
cov[·]:协方差
e[·]:数学期望
δkj:δ函数
pr(1|zk)、pr(2|zk):迭代计算的概率初始值
prmax、prmin:用于迭代计算的概率初始值
pk/k-1:一步预测误差方差矩阵
argmax[f(x)]:函数最大值返回函数
rk:残差
tk:量测估计误差方差矩阵
kk:滤波增益矩阵
【具体实施方式】
下面结合附图对本发明作详细说明。
本发明提出了一种多模型自校准扩展卡尔曼滤波方法,其流程图如图1所示,时间更新流程图如图2所示,它包括以下七个步骤:
步骤一:建立系统基本方程
zk=hk(xk)+vk(39)
式中,xk表示系统的状态向量,
式中,cov[·]为协方差,e[·]为数学期望,δkj为δ函数,当k=j时,δkj=1,当k≠j时,δkj=0;
步骤二:系统线性化
zk=hkxk+vk(43)
式中,φk和hk分别为状态转移矩阵和量测矩阵;
步骤三:滤波初始化
设定状态估计与估计误差方差矩阵的初始值为
同时,设定两种模型的概率初始值
pr(1|z3)=pr(2|z3)=0.5(46)
以及用于迭代计算的概率初始值prmax和prmin;
步骤四:时间更新
当k=1,2时,状态一步预测值
一步预测误差方差矩阵
当k>2时,状态一步预测值
一步预测误差方差矩阵
式中
j=argmaxjpr(j|zk)(53)
其中,函数argmax[f(x)]返回当f(x)最大时x的值,并且
s1=p1(61)
其时间更新流程示意图,见图2所示;
步骤五:迭代变量更新
各模型量测更新
各模型条件概率重置
pr(j|zk)=prmax(67)
pr[(3-j)|zk]=prmin(68);
步骤六:量测更新
滤波增益矩阵
状态估计值
步骤七:迭代计算
k=k+1(73)
重复步骤四、步骤五和步骤六,直至滤波过程结束。