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

摘要

本发明提供一种自校准无迹卡尔曼滤波方法，步骤如下一建立包含未知输入的非线性离散系统；二对由式(1)和式(2)所组成系统进行初始化；三对系统进行时间更新；四结合量测信息，对状态一步预测和一步预测误差方差矩阵进行量测更新；五进行迭代计算；通过上述步骤，本发明通过将自校准技术引入无迹卡尔曼滤波方法，新得到的自校准无迹卡尔曼滤波方法首先解决了对受未知输入影响的非线性系统的建模问题，在此基础上，通过对未知输入项的自动估计，消除了未知输入对滤波结果的不利影响，极大地减少了滤波发散的现象，进而提升了滤波精度，同时作为一种能抵抗系统不确定性的滤波方法，提升了系统的鲁棒性。

主权项

一种自校准无迹卡尔曼滤波方法，其特征在于：它包含以下五个步骤：步骤一：建立包含未知输入的非线性离散系统针对工程实际中所遇到的状态方程受未知输入影响的非线性离散系统，其状态方程和量测方程表示为Xk＝f(Xk‑1)+bk‑1+Wk‑1···········(1)Zk＝h(Xk)+Vk·············(2)式中，Xk表示系统的状态向量，Zk表示系统量测向量，f(·)和h(·)为非线性向量函数,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...(3)]]>式中，Cov[·]为协方差，E[·]为数学期望，δkj为δ函数，当k＝j时，δkj＝1，当k≠j时，δkj＝0；步骤二：对由式(1)和式(2)所组成系统进行初始化对系统状态和误差方差矩阵分别进行初始化X^0=E[X0]...(4)]]>P0=E[(X0-X^0)(X0-X^0)T]...(5);]]>步骤三：对系统进行时间更新设k‑1时刻的状态估计值和误差方差矩阵分别为和Pk‑1，基于它们对系统进行时间更新，即计算k时刻的状态一步预测和一步预测误差方差矩阵Pk/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...(6)]]>Xk/k-1i=f(χk-1,i)...(7)]]>当k≥3时χk-2,i=X^k-2-((n+λ)Pk-2)ii=1,2,...,nX^k-2i=n+1X^k-2+((n+λ)Pk-2)ii=n+2,...,2n+1.....(8)]]>χ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.....(9)]]>Xk/k‑1,i＝f(χk‑1,i)+χk‑1,i‑f(χk‑2,i)········(10)式中，λ＝α2(n+κ)‑n为Sigma点的比例参数，n为状态向量Xk的维度，κ为调节参数，一般取0或3‑n，α为另一调节参数；在式(10)中，用到了自校准技术的基本假设：由于工程中相邻两步滤波之间的时间间隔很短，所以它们的未知输入也往往相等及近似相等，即有bk≈bk‑1；由式(7)和式(10)计算系统状态一步预测值X^k/k-1=Σi=12n+1(wmiXk/k-1,i)...(11)]]>式中，为均值权重系数，其计算公式为wmi=12(n+λ)i=1,2,...,2n+1i≠n+1λn+λi=n+1...(12)]]>由式(7)、式(10)和式(11)计算一步预测误差方差矩阵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...(13)]]>式中，为协方差权重系数，其计算公式为wci=12(n+λ)i=1,2,...,2n+1i≠n+1λn+λ+(1-α2+β)i=n+1...(14)]]>步骤四：结合量测信息，对状态一步预测和一步预测误差方差矩阵进行量测更新其“量测更新”的作法如下：首先，基于状态一步预测和一步预测误差方差矩阵Pk/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....(15)]]>然后，根据{χk/k‑1,i}计算量测估计值Zk/k‑1,i＝h(χk/k‑1,i)···········(16)Z^k/k-1=Σi=12n+1(wmiZk/k-1,i)...(17)]]>由式(16)和式(17)计算量测误差方差矩阵PZZPZZ=Σi=12n+1{wci(Zk/k-1,i-Z^k/k-1)(Zk/k-1,i-Z^k/k-1)T}+Rk....(18)]]>由式(11)、式(15)、式(16)和式(17)计算误差协方差矩阵PXZPXZ=Σi=12n+1{wci(χk/k-1,i-X^k/k-1)(Zk/k-1,i-Z^k/k-1)T}.....(19)]]>则，由量测更新得到最终的状态估计值和状态估计误差方差矩阵X^k=X^k/k-1+Kk(Zk-Z^k/k-1)...(20)]]>Pk=Pk/k-1-KkPZZKkT...(21)]]>式中，根据无迹卡尔曼滤波的一般性递推公式，步骤五：迭代计算X^k-1=X^k...(22)]]>Pk‑1＝Pk··············(23)k＝k+1··············(24)返回式(6)·············(25)根据k时刻的状态估计值和误差方差矩阵Pk，重复步骤(三)和(四)，进而得到k+1时刻的状态估计值和误差方差矩阵，往复迭代，直至滤波过程结束。