www.gusucode.com > MATLAB编程毕业设计 EKF SLAM仿真全部源代码 > batch_update.m
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %更新状态x和协方差P %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function [x,P]= batch_update(x,P,z,R,idf) lenz= size(z,2); lenx= length(x); H= zeros(2*lenz, lenx); v= zeros(2*lenz, 1); RR= zeros(2*lenz); for i=1:lenz ii= 2*i + (-1:0); [zp,H(ii,:)]= observe_model(x, idf(i)); v(ii)= [z(1,i)-zp(1); pi_to_pi(z(2,i)-zp(2))]; RR(ii,ii)= R; end [x,P]= KF_cholesky_update(x,P,v,RR,H); % %