Extended Information Filter Code
3 Ansichten (letzte 30 Tage)
Ältere Kommentare anzeigen
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
0 Kommentare
Antworten (0)
Siehe auch
Kategorien
Mehr zu Robotics finden Sie in Help Center und File Exchange
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!