www.gusucode.com > MATLAB编程毕业设计 EKF SLAM仿真全部源代码 > predict.m

    function [x,P]= predict (x,P,v,g,Q,WB,dt)
%卡尔曼滤波的预测步骤，通过运动模型，给出预测的状态X和状态协方差P

s= sin(g+x(3)); c= cos(g+x(3));
vts= v*dt*s; vtc= v*dt*c;

% 雅可比矩阵 
Gv= [1 0 -vts;
     0 1  vtc;
     0 0 1];
Gu= [dt*c -vts;
     dt*s  vtc;
     dt*sin(g)/WB v*dt*cos(g)/WB];
  
% 预测状态协方差
P(1:3,1:3)= Gv*P(1:3,1:3)*Gv' + Gu*Q*Gu';
if size(P,1)>3
    P(1:3,4:end)= Gv*P(1:3,4:end);
    P(4:end,1:3)= P(1:3,4:end)';
end    

% 预测状态
x(1:3)= [x(1) + vtc; 
         x(2) + vts;
         pi_to_pi(x(3)+ v*dt*sin(g)/WB)];