扩展卡尔曼滤波算法.docx

扩展卡尔曼滤波算法
——作者，niewei120，nuaa
EKF算法是在标准Kalman滤波算法的基础上发展起来的，它的基本思想是：在滤波值附近，应用泰勒展开算法将非线性系统展开，对于二阶以上的高阶项全部都省去，从而原系统就变成了一个线性系统，再利用标准Kalman滤波算法的思想对系统线性化模型进行滤波。

滤波过程如下：

其matlab程序如下：
Fort=1：N
%预测更新
mu_ekfPred(t)=feval('ffun',mu_ekf(t-1),t);%状态量的一步预测，ffun为状态方程
PPred(t)=Q+Jx*P_ekf(t-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);%估计误差方差矩阵的更新
End