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