扩展卡尔曼滤波算法.doc

《扩展卡尔曼滤波算法.doc》由会员上传分享，免费在线阅读，更多相关内容在行业资料-天天文库。

1、扩展卡尔曼滤波算法——作者，niewei120，nuaaEKF算法是在标准Kalman滤波算法的基础上发展起来的，它的基本思想是：在滤波值附近，应用泰勒展开算法将非线性系统展开，对于二阶以上的高阶项全部都省去，从而原系统就变成了一个线性系统，再利用标准Kalman滤波算法的思想对系统线性化模型进行滤波。滤波过程如下：其matlab程序如下：Fort=1：N%预测更新mu_ekfPred(t)=feval('ffun',mu_ekf(t-1),t);%状态量的一步预测，ffun为状态方程PPred(t)=Q+Jx*P_ekf(t-

2、1)*Jx';一步预测方差阵，Jx为状态量的雅可比矩阵%修正阶段yPred(t)=feval('hfun',mu_ekfPred(t),t);%量测量的一步预测，hfun为量测方程M=R+Jy*PPred(t)*Jy';%CkPkCk+Rk的误差反差计算，Jy为量测量的雅可比矩阵K=PPred(t)*Jy'*inv(M);%卡尔曼滤波增益mu_ekf(t)=mu_ekfPred(t)+K*(y(t)-yPred(t));%状态估计量，y(t)为实际测量值。P_ekf(t)=PPred(t)-K*Jy*PPred(t);%估计误差

3、方差矩阵的更新End