扩展卡尔曼滤波算法.docxVIP

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