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

摘要

本发明提供一种多模型自校准秩滤波方法，步骤如下一建立系统基本方程；二对系统进行滤波初始化；三对系统进行时间更新；四进行迭代变量更新；五进行量测更新；六进行迭代计算；通过步骤一到步骤六，本发明充分利用了秩滤波和自校准秩滤波两种方法的计算结果，依托基于贝叶斯原理的多模型估计理论，自动区分未知输入为零段与非零段，从而可以精确地选择其中最合适的结果作为自身的先验估计；最重要的一点在于，本发明是针对非高斯、非线性系统开发的，相较于其他方法适用范围更广。

主权项

一种多模型自校准秩滤波方法，其特征在于：它包含以下六个步骤：步骤一：建立系统基本方程多模型自校准秩滤波采用自校准秩滤波与秩滤波两种方法进行运算，故系统包含两个状态方程，第一个为含有未知输入项的状态方程，第二个为标准的非线性状态方程，其具体表达式为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；步骤二：对系统进行滤波初始化设定状态估计与估计误差方差矩阵的初始值为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；针对本发明只选取两个动力学模型且采用最高概率法选取先验估计值，只需要定性分析两种模型的概率大小而不需要精确计算概率值的特点，多模型自校准卡尔曼滤波方法不再使用计算得到的条件概率值进行迭代，而是设定两个确定的概率初始值Prmin和Prmax＝1‑Prmin；在每一步滤波之前，通过比较上一步概率结果的大小将其分别赋给两个模型，并以它们为初始值更新当前时刻的模型概率；由于Prmin并不是概率下限那样的极小值，因此能保证概率恢复的速度，从而使卡尔曼滤波的实时性得到保证；步骤三：对系统进行时间更新设k‑1时刻的状态估计值和误差方差矩阵分别为和Pk‑1，基于它们对系统进行时间更新，即计算k时刻的状态一步预测和一步预测误差方差矩阵Pk/k‑1；基于秩滤波的一般性递推公式，在时间更新过程中首先需要计算秩采样点集{χi}当k＝1,2时状态一步预测值χk-1,i=X^k-1+up1(Pk-1)ii=1,...,nX^k-1-up1(Pk-1)i-ni=n+1,...,2nX^k-1+up2(Pk-1)i-2ni=2n+1,...,3nX^k-1-up2(Pk-1)i-3ni=3n+1,...,4n......(8)]]>Xk/k‑1,i＝f(χk‑1,i)············(9)X^k/k-1=14nΣi=14nXk/k-1,i...(10)]]>式中，n为状态向量Xk的维度，标准正态偏量表示Pk‑1平方根的第i列向量；一步预测误差方差矩阵Pk/k-1=1ωΣi=14n{(Xk/k-1,i-X^k/k-1)(Xk/k-1,i-X^k/k-1)T}+Qk-1....(11)]]>式中，ω为协方差权重系数，其计算公式为ω=2(up12+up22)=3.0105...(12)]]>当k＞2时状态一步预测值X^k/k-1=X^k/k-1J...(13)]]>一步预测误差方差矩阵Pk/k-1=Pk/k-1J...(14)]]>式中J＝argmaxjPr(j|Zk)···········(15)χk-2,i1=X^k-21+up1(Pk-21)ii=1,...,nX^k-21-up1(Pk-21)i-ni=n+1,...,2nX^k-21+up2(Pk-21)i-2ni=2n+1,...,3nX^k-21-up2(Pk-21)i-3ni=3n+1,...,4n.....(16)]]>χk-1,i1=X^k-11+up1(Pk-11)ii=1,...,nX^k-11-up1(Pk-11)i-ni=n+1,...,2nX^k-11+up2(Pk-11)i-2ni=2n+1,...,3nX^k-11-up2(Pk-11)i-3ni=3n+1,...,4n......(17)]]>Xk/k-1,i1=f(χk-1,i1)+χk-1,i1-f(χk-1,i1)...(18)]]>χk-1,i2=X^k-12+up1(Pk-12)ii=1,...,nX^k-12-up1(Pk-12)i-ni=n+1,...,2nX^k-12+up2(Pk-12)i-2ni=2n+1,...,3nX^k-12-up2(Pk-12)i-3ni=3n+1,...,4n......(19)]]>Xk/k-1,i2=f(χk-1,i2)...(20)]]>X^k/k-1j=14nΣi=14nXk/k-1,ij...(21)]]>Pk/k-1j=1ωΣi=14n{(Xk/k-1,ij-X^k/k-1j)(Xk/k-1,ij-X^k/k-1j)T}+Qk-1...(22)]]>其中，函数argmax[f(x)]返回当f(x)最大时x的值，并且Pr(j|Zk)=pdf(Zk|j)Pr(j|Zk-1)Σi=1Npdf(Zk|i)Pr(i|Zk-1)...(23)]]>pdf(Zk|j)≈exp(-rkTTk-1rk/2)(2π)q/2|Tk|1/2...(24)]]>在上述计算过程中，式(18)和式(21)提供自校准状态方程的一步预测结果，式(20)和式(21)提供标准状态方程计算的一步预测结果，然后通过式(15)中概率大小的比对，由式(13)完成最终的一步预测值筛选；概率大小的计算依托于贝叶斯原理，比较的是在当前时刻量测值已知的情况下，两种模型的条件概率，如式(23)和式(24)所示；步骤四：迭代变量更新在步骤三中有很多中间变量需要实时更新，因此有必要得到它们的递推公式，进而保证整个滤波过程的顺利进行；这些迭代变量包括：各模型量测更新Kkj=PXZj(PZZj)-1...(25)]]>X^kj=X^k/k-1j+Kkj(Zk-Z^k/k-1j)...(26)]]>Pkj=Pk/k-1j-KkjPZZj(Kkj)T...(27)]]>式中Zk/k-1,ij=h(χk/k-1,ij)...(28)]]>Z^k/k-1j=14nΣi=14nZk/k-1,ij...(29)]]>PZZj=1ωΣi=14n{(Zk/k-1,ij-Z^k/k-1j)(Zk/k-1,ij-Z^k/k-1j)T}+Rk....(30)]]>PXZj=1ωΣi=14n{(χk/k-1,ij-X^k/k-1j)(Zk/k-1,ij-Z^k/k-1T)T}.....(31)]]>各模型条件概率重置Pr(J|Zk)＝Prmax············(32)Pr[(3‑J)|Zk]＝Prmin···········(33)；步骤五：量测更新首先，基于状态一步预测和一步预测误差方差矩阵Pk/k‑1，根据秩滤波的一般性递推公式对系统状态进行重采样，得到新的秩采样点集{χk/k‑1,i}χk/k-1,i=X^k/k-1+up1(Pk/k-1)ii=1,...,nX^k/k-1-up1(Pk/k-1)i-ni=n+1,...,2nX^k/k-1+up2(Pk/k-1)i-2ni=2n+1,...,3nX^k/k-1-up2(Pk/k-1)i-3ni=3n+1,...,4n.....(34)]]>然后，根据{χk/k‑1,i}计算量测估计值Zk/k‑1,i＝h(χk/k‑1,i)···········(35)Z^k/k-1=14nΣi=14nZk/k-1,i...(36)]]>由式(35)和式(36)计算量测误差方差矩阵PZZPZZ=1ωΣi=14n{(Zk/k-1,i-Z^k/k-1)(Zk/k-1,i-Z^k/k-1)T}+Rk.....(37)]]>由式(34)、式(35)和式(36)计算误差协方差矩阵PXZPXZ=1ωΣi=14n{(χk/k-1,i-X^k/k-1)(Zk/k-1,i-Z^k/k-1)T}.....(38)]]>由式(37)和式(38)计算增益矩阵Kk=PXZPZZ-1...(39)]]>则，由量测更新得到最终的状态估计值和状态估计误差方差矩阵X^k=X^k/k-1+Kk(Zk-Z^k/k-1)...(40);]]>步骤六：迭代计算根据k时刻的状态估计值和误差方差矩阵Pk，重复步骤三、步骤四和步骤五，进而得到k+1时刻的状态估计值和误差方差矩阵，往复迭代，直至滤波过程结束。