[发明专利]一种多模型自校准卡尔曼滤波方法

摘要

本发明提供一种多模型自校准卡尔曼滤波方法，步骤如下一建立系统基本方程；二对由式(1)、式(2)和式(3)所组成的系统进行滤波初始化；三对系统进行时间更新；四迭代变量更新；五量测更新；六进行迭代计算；本发明将多模型估计理论引入系统状态方程受未知输入干扰的问题中，基于自校准卡尔曼滤波和标准卡尔曼滤波，得到了多模型自校准卡尔曼滤波方法的完整过程；既解决了标准卡尔曼滤波在未知输入非零段滤波发散的问题，也显著提升了自校准卡尔曼滤波在未知输入为零段的滤波精度；该发明同时提升了未知输入为零段和非零段的滤波精度，适用范围进一步扩展，系统鲁棒性也在自校准卡尔曼滤波方法的基础上得到了进一步的提升。

主权项

一种多模型自校准卡尔曼滤波方法，其特征在于：它包含以下六个步骤：步骤一：建立系统基本方程多模型自校准卡尔曼滤波采用自校准卡尔曼滤波与标准卡尔曼滤波两种方法进行运算，故系统包含两个状态方程，第一个为自校准状态方程，第二个为标准的状态方程，其具体表达式为Xk1=Φk-1Xk-11+bk-1+Wk-1...(1)]]>Xk2=Φk-1Xk-12+Wk-1...(2)]]>Zk＝HkXk+Vk·············(3)式中，Xk表示系统的状态向量，和分别对应含未知输入的动力学模型和标准的动力学模型，Zk表示系统量测向量，Φk和Hk分别为状态转移矩阵和量测矩阵,bk表示未知输入，Wk与Vk分别为系统噪声向量和量测噪声向量，其方差矩阵分别为Qk和Rk，并且满足E[Wk]=0Cov[Wk,Wj]=E[WkWjT]=QkδkjE[Vk]=0Cov[Vk,Vj]=E[VkVjT]=RkδkjCov[Wk,Vj]=E[WkVjT]=0...(4)]]>式中，Cov[·]为协方差，E[·]为数学期望，δkj为δ函数，当k＝j时，δkj＝1，当k≠j时，δkj＝0；步骤二：对由式(1)、式(2)和式(3)所组成的系统进行滤波初始化设定状态估计与估计误差方差矩阵的初始值为X^0=E[X0]...(5)]]>P0=E[(X0-X^0)(X0-X^0)T]...(6)]]>同时，为了完成两模型估计结果的融合，还需要设定两种模型的概率初始值Pr(1|Z3)＝Pr(2|Z3)＝0.5············(7)以及用于迭代计算的概率初始值Prmax和Prmin；初始化Prmax和Prmin的原因如下：在多模型估计运算过程中，一些模型会由于对应的概率逐渐趋近为零而被淘汰，故参与运算的模型数量N在不断减小，这会降低系统对复杂环境的适应能力；针对本发明只选取两个动力学模型且采用最高概率法选取先验估计值，故只需要定性分析两种模型的概率大小而不需要精确计算概率值的特点，多模型自校准卡尔曼滤波方法不再使用计算得到的条件概率值进行迭代，而是设定两个确定的概率初始值Prmin和Prmax＝1‑Prmin；在每一步滤波之前，通过比较上一步概率结果的大小将其分别赋给两个模型，并以它们为初始值更新当前时刻的模型概率；由于Prmin并不是概率下限那样的极小值，因此保证概率恢复的速度，从而使卡尔曼滤波的实时性得到保证；步骤三：对系统进行时间更新当k＝1,2时，状态一步预测值X^1/0=Φ0X^0...(8)]]>X^2/1=Φ1X^1...(9)]]>一步预测误差方差矩阵P1/0=Φ0P0Φ0T+Q0...(10)]]>P2/1＝Φ1P1Φ1T+Q1············(11)当k＞2时，状态一步预测值X^k/k-1=X^k/k-1J...(12)]]>一步预测误差方差矩阵Pk/k-1=Pk/k-1J...(13)]]>式中J＝argmaxjPr(j|Zk)···········(14)X^k/k-11=(I+Φk-1)X^k-11-Φk-2X^k-21...(15)]]>X^k/k-12=Φk-1X^k-12...(16)]]>Pk/k-11=(I+Φk-1)Pk-11(I+Φk-1)T+Φk-2Pk-21Φk-2T-(I+Φk-1)Sk-1×Φk-2T-Φk-2Sk-1T(I+Φk-1)T-(I+Φk-1)(I-Kk-11Hk-1)Qk-2-Qk-2(I-Kk-11Hk-1)T×(I+Φk-1)T+Qk-1+Qk-2---(17)]]>Pk/k-12=Φk-1Pk-12Φk-1T+Qk-1...(18)]]>其中，函数argmax[f(x)]返回当f(x)最大时x的值，并且Pr(j|Zk)=pdf(Zk|j)Pr(j|Zk-1)Σi=1Npdf(Zk|i)Pr(i|Zk-1)...(19)]]>pdf(Zk|j)≈exp(-rkTTk-1rk/2)(2π)q/2|Tk|1/2...(20)]]>Sk-1=(I-Kk-11Hk-1)[(I+Φk-2)Pk-2-Φk-3Sk-2T-Qk-3(I-Kk-21Hk-2)T]....(21)]]>S1＝P1··············(22)rk=Zk-HkX^k/k-1j...(23)]]>Tk=HkPk/k-1jHkT+Rk...(24)]]>在上述计算过程中，式(15)提供自校准状态方程的一步预测结果，式(16)提供标准状态方程计算的一步预测结果，然后通过式(12)中概率大小的比对，由式(14)完成最终的一步预测值筛选；两种模型的条件概率，如式(19)和式(20)所示；步骤四：迭代变量更新在步骤(三)中有很多中间变量需要实时更新，因此有必要得到它们的递推公式，进而保证整个滤波过程的顺利进行；这些迭代变量包括：各模型量测更新Kkj=Pk/k-1jHkT(HkPk/k-1jHkT+Rk)-1...(25)]]>X^kj=X^k/k-1j+Kkj(Zk-HkX^k/k-1j)...(26)]]>Pkj=(I-KkjHk)Pk/k-1j(I-KkjHk)T+KkjRk(Kkj)T......(27)]]>各模型条件概率重置Pr(J|Zk)＝Prmax············(28)Pr[(3‑J)|Zk]＝Prmin···········(29)；步骤五：量测更新滤波增益矩阵Kk=Pk/k-1HkT(HkPk/k-1HkT+Rk)-1...(30)]]>状态估计值X^k=X^k/k-1+Kk(Zk-HkX^k/k-1)...(31)]]>此步骤的计算公式由于不涉及自校准技术的基本假设与多模型估计理论的相关内容，所以计算过程与标准卡尔曼滤波方法的后验估计公式保持一致；同时需要说明的是，由于多模型自校准卡尔曼滤波方法是通过两个模型结果的融合计算得到的，因此在计算过程中并不需要估计误差方差矩阵在相邻时刻间的传递，所以在计算步骤中没有给出Pk的计算公式；步骤六：迭代计算根据k时刻的两个模型分别得到的状态估计值和误差方差矩阵Pk，重复步骤(三)、(四)和(五)，进而得到k+1时刻的状态估计值，往复迭代，直至滤波过程结束。