www.pudn.com > ekfcode.zip > UPDATE.M

function [xest, Pest, innov, S]=update(xpred,Ppred,obs,beacons) 
% 
% function [xest, Pest, innov, S]=update(xpred,Ppred,obs,beacons) 
% 
% HDW 3/1/95 Modified 31/05/00 
% function to update vehicle location  
 
globals; 
 
[temp,N_OBS]=size(obs); 
if temp ~= 4 
  error('observation dimension of 4 expected') 
end 
 
[XSIZE,temp]=size(xpred); 
if temp ~= 1 
  error('xpred expected to be a column vector') 
end 
 
[s1,s2]=size(Ppred); 
if((s1 ~= XSIZE)|(s2 ~=XSIZE)) 
  error('Ppred not of size XSIZE') 
end 
 
sigma_range=SIGMA_RANGE*SIGMA_RANGE; 
sigma_bearing=SIGMA_BEARING*SIGMA_BEARING; 
 
% first put observation in cartesian vehicle-centred coords 
zv=[R_OFFSET+(obs(1)*cos(obs(2)));  
    obs(1)*sin(obs(2))]; 
T=[cos(obs(2)) -sin(obs(2));  
   sin(obs(2)) cos(obs(2))]; 
sigma_o=[sigma_range 0;  
         0 sigma_bearing*(obs(1)^2)]; 
sigma_z=T*sigma_o*T'; 
 
% then in base coordinates 
zb=[xpred(1)+(zv(1)*cos(xpred(3)))-(zv(2)*sin(xpred(3))); 
    xpred(1)+(zv(1)*sin(xpred(3)))+(zv(2)*cos(xpred(3)))]; 
Tx=[1 0 -((zv(1)*sin(xpred(3)))+(zv(2)*cos(xpred(3)))) 0; 
    1 0 -((zv(1)*cos(xpred(3)))+(zv(2)*sin(xpred(3)))) 0]; 
Tz=[cos(xpred(3)) -sin(xpred(3)); 
    sin(xpred(3))  cos(xpred(3))]; 
sigma_b=Tx*Ppred*Tx' + Tz*sigma_z*Tz'; 
 
% now try and match observation to beacon map  
 
% index=match(zb,sigma_b,beacons); 
index=obs(3); 
 
if (index) 
  dx=beacons(index,1)-xpred(1);  
  dy=beacons(index,2)-xpred(2); 
  T=[ cos(xpred(3)) sin(xpred(3)); 
    -sin(xpred(3)) cos(xpred(3))]; 
  H=[-cos(xpred(3)) -sin(xpred(3)) (-(dx*sin(xpred(3)))+(dy*cos(xpred(3)))) 0; 
      sin(xpred(3)) -cos(xpred(3)) (-(dx*cos(xpred(3)))-(dy*sin(xpred(3)))) 0]; 
  zpred=T*[dx;dy]; 
  S=H*Ppred*H' + sigma_z; 
  W=Ppred*H'* inv(S); 
  Pest=Ppred-W*S*W'; 
  innov=[zv(1)-zpred(1); zv(2)-zpred(2)]; 
  xest=xpred+W*innov; 
else 
  xest=xpred; 
  Pest=Ppred; 
  S=zeros(2,2); 
  innov=zeros(2,1); 
end