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

权利要求书

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

步骤一：建立包含未知输入的非线性离散系统

针对工程实际中所遇到的状态方程受未知输入影响的非线性离散系统，其状态方程和量测方程表示为

X_k＝f(X_k-1)+b_k-1+W_k-1···········(1)

Z_k＝h(X_k)+V_k·············(2)

式中，X_k表示系统的状态向量，Z_k表示系统量测向量，f(·)和h(·)为非线性向量函数,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...(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时刻的状态估计值和误差方差矩阵分别为和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...(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)]]>

X_k/k-1,i＝f(χ_k-1,i)+χ_k-1,i-f(χ_k-2,i)········(10)

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

在式(10)中，用到了自校准技术的基本假设：由于工程中相邻两步滤波之间的时间间隔很短，所以它们的未知输入也往往相等及近似相等，即有b_k≈b_k-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)]]>

步骤四：结合量测信息，对状态一步预测和一步预测误差方差矩阵进行量测更新其“量测更新”的作法如下：

首先，基于状态一步预测和一步预测误差方差矩阵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....(15)]]>

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

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

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

由式(16)和式(17)计算量测误差方差矩阵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....(18)]]>

由式(11)、式(15)、式(16)和式(17)计算误差协方差矩阵P_XZ

PXZ=Σ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)]]>

P_k-1＝P_k··············(23)

k＝k+1··············(24)

返回式(6)·············(25)

根据k时刻的状态估计值和误差方差矩阵P_k，重复步骤(三)和(四)，进而得到k+1时刻的状态估计值和误差方差矩阵，往复迭代，直至滤波过程结束。

2.根据权利要求1所述的一种自校准无迹卡尔曼滤波方法，其特征在于：

在步骤一中所述的非线性离散系统是指对工程中常见的非线性常微分方程进行离散化处理所得到的结果。