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

摘要

本发明提供一种多模型自校准扩展卡尔曼滤波方法，步骤如下一建立系统基本方程；二对由式(1)、式(2)和式(3)所组成系统进行线性化处理；三对系统进行滤波初始化；四对系统进行时间更新；五迭代变量更新；六量测更新；七进行迭代计算；通过以上步骤，工程中经常遇到的非线性系统状态方程受到未知输入影响的问题得到了解决，同时，由于考虑了未知输入为零的情况，因此相比自校准扩展卡尔曼滤波方法，对复杂环境的适应性得到了提升；而且由于本发明基于扩展卡尔曼滤波方法，不需要通过采样的形式完成信息的传递，所以其处理非线性系统的运算速度较其他非线性滤波方法更加快捷，便于工程实际应用。

主权项

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