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

    function [x,P]= augment(x,P,z,R)
%通过zn扩展状态矩阵X和状态协方差矩阵P

for i=1:size(z,2)
    [x,P]= add_one_z(x,P,z(:,i),R);
end

function [x,P]= add_one_z(x,P,z,R)

len= length(x);
r= z(1); b= z(2);
s= sin(x(3)+b); 
c= cos(x(3)+b);

%扩展X矩阵
x= [x;
    x(1) + r*c;
    x(2) + r*s];

%雅可比矩阵
Gv= [1 0 -r*s;
     0 1  r*c];
Gz= [c -r*s;
     s  r*c];
     
%扩展P矩阵
rng= len+1:len+2;
P(rng,rng)= Gv*P(1:3,1:3)*Gv' + Gz*R*Gz'; 
P(rng,1:3)= Gv*P(1:3,1:3); 
P(1:3,rng)= P(rng,1:3)';
if len>3
    rnm= 4:len;
    P(rng,rnm)= Gv*P(1:3,rnm); 
    P(rnm,rng)= P(rng,rnm)';
end