00001 function xhat = localize(y)
00002 init
00003 Phi=[diff(y); y(1)-y(end)];
00004
00005 beacon=desc.area.beacon;
00006
00007 beta=Phi(3);
00008 alpha=Phi(2);
00009 theta=beacon_angle(1, beacon);
00010 phi=beacon_angle(2, beacon);
00011 psi=beacon_angle(3, beacon);
00012
00013 q=sin(phi)/sin(theta)*sin(alpha)/sin(beta);
00014 omega=2*pi-psi-alpha-beta;
00015
00016 delta = atan2(sin(omega), 1/q+cos(omega));
00017
00018 v32=beacon(2,:)-beacon(3,:);
00019
00020 l1ang=atan2(v32(2), v32(1)) -pi+delta;
00021
00022 a=norm(beacon(2,:)-beacon(3,:));
00023
00024 l1=a/sin(alpha)*sin(pi-alpha-delta);
00025 r=beacon(2,:)+l1*[cos(l1ang) sin(l1ang)];
00026
00027 angle=mean(atan2(beacon(:,2)-r(2), beacon(:,1)-r(1))-y);
00028
00029 xhat=[r angle];
00030
00031 function angle=beacon_angle(b, beacon)
00032 beacons=size(beacon,1);
00033 prev=mod(b-1-1, beacons)+1;
00034 next=mod(b-1+1, beacons)+1;
00035 bp=beacon(prev,:);
00036 bn=beacon(next,:);
00037 bb=beacon(b,:);
00038 v1=bp-bb;
00039 v2=bn-bb;
00040 angle=acos(sum(v1.*v2)/(norm(v1)*norm(v2)));
