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

权利要求书

1.一种多模型自校准无迹卡尔曼滤波方法，其特征在于：它包含以下六个步骤：

步骤一：建立系统基本方程

多模型自校准无迹卡尔曼滤波采用自校准无迹卡尔曼滤波与无迹卡尔曼滤波两种方法进行运算，故系统包含两个状态方程，第一个为含有未知输入项的状态方程，第二个为标准的非线性状态方程，其具体表达式为

Xk1=fk-1(Xk-11)+bk-1+Wk-1...(1)]]>

Xk2=fk-1(Xk-12)+Wk-1...(2)]]>

Z_k＝h_k(X_k)+V_k·············(3)

式中，X_k表示系统的状态向量，和分别对应含未知输入的动力学模型和标准的动力学模型，Z_k表示系统量测向量，f_k(·)和h_k(·)分别为非线性状态递推方程和量测方程,b_k表示未知输入，W_k与V_k分别为系统噪声向量和量测噪声向量，其方差矩阵分别为Q_k和R_k，并且满足

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|Z₃)＝Pr(2Z₃)＝0.5············(7)

以及用于迭代计算的概率初始值Pr_max和Pr_min；针对本发明只选取两个动力学模型且采用最高概率法选取先验估计值，只需要定性分析两种模型的概率大小而不需要精确计算概率值的特点，多模型自校准卡尔曼滤波方法不再使用计算得到的条件概率值进行迭代，而是设定两个确定的概率初始值Pr_min和Pr_max＝1-Pr_min；在每一步滤波之前，通过比较上一步概率结果的大小将其分别赋给两个模型，并以它们为初始值更新当前时刻的模型概率；由于Pr_min并不是概率下限那样的极小值，因此能保证概率恢复的速度，从而使卡尔曼滤波的实时性得到保证；

步骤三：对系统进行时间更新

设k-1时刻的状态估计值和误差方差矩阵分别为和P_k-1，基于它们对系统进行时间更新，即计算k时刻的状态一步预测和一步预测误差方差矩阵P_k/k-1；基于无迹卡尔曼滤波的一般性递推公式，在时间更新过程中首先需要计算Sigma点集{χ_i}

当k＝1,2时

状态一步预测值

χk-1,i=X^k-1-((n+λ)Pk-1)ii=1,2,...,nX^k-1i=n+1X^k-1+((n+λ)Pk-1)ii=n+2,...,2n+1...(8)]]>

X_k/k-1,i＝f(χ_k-1,i)············(9)

X^k/k-1=Σi=12n+1(wmiXk/k-1,i)...(10)]]>

式中，λ＝α²(n+κ)-n为Sigma点的比例参数，n为状态向量X_k的维度，κ为调节参数，一般取0或3-n，α为另一调节参数；

一步预测误差方差矩阵

Pk/k-1=Σi=12n+1{wci(Xk/k-1,i-X^k/k-1)(Xk/k-1,i-X^k/k-1)T}+Qk-1...(11)]]>

式中，为协方差权重系数，其计算公式为

wci=12(n+λ)i=1,2,...,2n+1i≠n+1λn+λ+(1-α2+β)i=n+1---(12)]]>

当k＞2时

状态一步预测值

X^k/k-1=X^k/k-1J...(13)]]>

一步预测误差方差矩阵

Pk/k-1=Pk/k-1J...(14)]]>

式中

J＝argmax_jPr(j|Z_k)···········(15)

χk-2,i1=X^k-21-((n+λ)Pk-21)ii=1,2,...,nX^k-21i=n+1X^k-21+((n+λ)Pk-21)ii=n+2,...,2n+1...(16)]]>

χk-1,i1=X^k-11-((n+λ)Pk-11)ii=1,2,...,nX^k-11i=n+1X^k-11+((n+λ)Pk-11)ii=n+2,...,2n+1...(17)]]>

Xk/k-1,i1=f(χk-1,i1)+χk-1,i1-f(χk-1,i1)...(18)]]>

χk-1,i2=X^k-12-((n+λ)Pk-12)ii=1,2,...,nX^k-12i=n+1X^k-12+((n+λ)Pk-11)ii=n+2,...,2n+1....(19)]]>

Xk/k-1,i2=f(χk-1,i2)...(20)]]>

X^k/k-1j=Σi=12n+1(wmiXk/k-1,ij)...(21)]]>

Pk/k-1j=Σi=12n+1{wci(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=Σi=12n+1(wmiZk/k-1,ij)...(29)]]>

PZZj=Σi=12n+1{wci(Zk/k-1,ij-Z^k/k-1,ij)(Zk/k-1,ij-Z^k/k-1j)T}+Rk...(30)]]>

PXZj=Σi=12n+1{wci(χk/k-1,ij-X^k/k-1j)(Zk/k-1,ij-Z^k/k-1j)T}...(31)]]>

各模型条件概率重置

Pr(J|Z_k)＝Pr_max············(32)

Pr[(3-J)|Z_k]＝Pr_min···········(33)；

步骤五：量测更新

首先，基于状态一步预测和一步预测误差方差矩阵P_k/k-1，根据无迹卡尔曼滤波的一般性递推公式对系统状态进行重采样，得到新Sigma点集{χ_k/k-1,i}

χk/k-1,i=X^k/k-1-((n+λ)Pk/k-1)ii=1,2,...,nX^k/k-1i=n+1X^k/k-1+((n+λ)Pk/k-1)ii=n+2,...,2n+1...(34)]]>

然后，根据{χ_k/k-1,i}计算量测估计值

Z_k/k-1,i＝h(χ_k/k-1,i)···········(35)

Z^k/k-1=Σi=12n+1(wmiZk/k-1,i)...(36)]]>

由式(35)和式(36)计算量测误差方差矩阵P_ZZ

PZZ=Σi=12n+1{wci(Zk/k-1,i-Z^k/k-1)(Zk/k-1,i-Z^k/k-1)T}+Rk...(37)]]>

由式(34)、式(35)、和式(36)计算误差协方差矩阵P_XZ

PXZ=Σi=12n+1{wci(χ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时刻的状态估计值和误差方差矩阵P_k，重复步骤三、步骤四和步骤五，进而得到k+1时刻的状态估计值和误差方差矩阵，往复迭代，直至滤波过程结束。