Extended Information Filter Code

3 次查看（过去 30 天）

Murat 2015-12-22

0
链接

此问题的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/261331-extended-information-filter-code

编辑： Walter Roberson 2015-12-22

Hi, i want to write a script code about extended information filter and make a robot localization with features int he map. Actually i already wrote a code but after calculating the information matrix and information vector, my robot location changes to an extremely different position. Can yu please help me about this? Here is the code:

function [Si,mu]= eifLocalization_alt(dt,Si, mu,t,betas)
global data; global landMarks;
drot1 = atan2(data(t,11)-data(t-1,11), data(t,10)-data(t-1,10)) -  data(t-1,12);
dtrans = sqrt((data(t,10)-data(t-1,10))^2 + (data(t,11)-data(t-1,11))^2);
drot2 = normalize_angle(data(t,12) - data(t-1,12) - drot1);
LandMarkDetect = 1; 
R1 = [0.1  0 0; 0 0.1 0; 0 0 0.1]; 
Q1 =[betas(1)^2 0 ; 0 betas(2)^2 ]; 
omega0 = inv(Si);
epsilon0 = omega0*mu;
g = [mu(1) + (dtrans)*cos(mu(3)+drot1);%+pth+deltatheta); 
    mu(2) + (dtrans)*sin(mu(3)+drot1);%+pth+deltatheta);
    mu(3)+drot1+drot2];%+deltatheta+pth];
G = [ 1 0 -dtrans*sin(mu(3) + drot1);
      0 1 dtrans*cos(mu(3) + drot1); 
      0 0 1]; 
omega_p = inv(G*(inv(omega0))*G' + R1);
epsilon_p = omega_p * g;
mu1_p = g;
if(data(t-1,6)==0)%if there is no landmark;
    LandMarkDetect=0;
end
if (LandMarkDetect == 1)
    l_loc_x = landMarks(1,data(t-1,6)); % x position of the landmark
    l_loc_y = landMarks(2,data(t-1,6)); % y position of the landmark
      % New z 
      q1 = (l_loc_x - mu1_p(1))^2;
      q2 = (l_loc_y - mu1_p(2))^2;
      q = q1+q2;
      z_ro = sqrt(q);
      z_theta = normalize_angle(atan2(l_loc_y - mu1_p(2),(l_loc_x- mu1_p(1))) - mu1_p(3));
      hz = [mu1_p(1)-l_loc_x;mu1_p(2)-l_loc_y;mu1_p(3)-z_theta];%[sqrt(q1);sqrt(q2);z_theta];
      z1 = data(t,1:2)';
      zz = [z_ro;z_theta];
      H = [-(l_loc_x-mu1_p(1))/z_ro -(l_loc_y-mu1_p(2))/z_ro 0;
           (l_loc_y-mu1_p(2))/q     -(l_loc_x-mu1_p(1))/q   -1];
                 % 0                                0          1];
      omega1 = omega_p + (H'*(inv(Q1))*H);
      epsilon1 = epsilon_p + H'*(inv(Q1))*zz;%(z1-hz-H*mu1_p)
      Si = inv(omega1);
      mu = Si*epsilon1;
  else
      mu = mu1_p;
      Si = inv(omega_p);
  end
end