[发明专利]一种迭代扩展增量卡尔曼滤波方法

权利要求书

1.一种迭代扩展增量卡尔曼滤波方法，其特征在于：它包括以下步骤： 

步骤一：建立非线性离散增量系统 

针对工程实践中所遇到非线性离散增量系统中的状态方程和量测方程，特别是具有加性噪声的系统噪声和量测噪声的非线性系统，将初始量测数据做差处理，得到相应的非线性离散增量系统，该系统为： 

X_k=f_k-1(X_k-1)+W_k-1         （1） 

ΔZ_k=h_k(X_k)-h_k-1(X_k-1)+V_k        （2） 

式中,X_k为状态向量；f_k(·)为非线性离散函数；W_k为系统噪声向量；ΔZ_k=Z_k-Z_k-1为量测向量增量；Z_k为量测向量；h_k(·)为非线性离散函数；V_k为量测噪声向量；k是指第k步，代表t_k时刻；系统噪声向量W_k和量测噪声向量V_k的方差阵分别为Q_k和R_k，并且W_k和V_k满足 

在工程实际中，通常量测向量Z_k存在系统误差，而量测向量增量ΔZ_k=Z_k-Z_k-1的系统误差为相对小量，忽略不计； 

步骤二：对非线性离散增量系统进行线性化：首先，将式（1）中的非线性离散函数f_k-1(X_k-1)围绕估计值展开成泰勒级数，并略去二阶以上项，得线性化之后的状态方程 

X_k=Φ_k/k-1X_k-1+U_k-1+W_k-1        （4） 

式中， 

然后，再将式（2）中的非线性离散函数h_k(X_k)-h_k-1(X_k-1)围绕估计值展开成泰勒级数，并略去二阶以上项，得线性化之后的增量量测方程： 

ΔZ_k=H_kX_k-H_k-1X_k-1+Y_k+V_k        （7） 

式中， 

通过上述过程，就得到了线性化后的状态方程和增量量测方程，式中U_k和Y_k看作确定性输入矩阵； 

步骤三：对由式（4）和式（7）组成的线性化之后的非线性离散增量系统的状态和误差方差阵进行初始化和时间更新：首先，对状态和误差方差阵分别赋予初值 

其次，设第k-1步即t_k-1时刻的状态估计值和误差方差阵分别为和P_k-1，在此基础之上对第k步即t_k时刻的状态向量X_k和误差方差阵P_k进行时间更新分别得到状态的一步预测 

和相应的一步预测的误差方差阵 

步骤四：使用迭代方法进行量测更新：当i=1,2,3,…时，循环进行如下步骤： 

(1)计算滤波的第i步增益矩阵

(2)计算第i步量测信息残差

(3)得到第i步的状态估计

式中， 

在上述使用迭代方法进行量测更新过程中，当所得状态向量的估计值满足向量的2范数满足的条件,其中阈值设为ε_limit: 

时即停止计算； 

步骤五： 利用步骤四中的状态估计值及相应的参数，给出估计的误差方差阵 

通过上述步骤一、二、三、四、五，达到消除量测方程中未知的系统误差和建立精确的增量量测方程的目的，而且利用迭代方法优化对非线性量测方程进行线性化产生的误差，从而减少了发散现象，提高了滤波精度，增强了滤波稳定性。