Wifi Localization sample code(MATLAB)

From Lofaro Lab Wiki
Jump to: navigation, search

== The simulation below is used to test the localization algorithm We used python to construct the World model, Wi-Fi signal strength, and path of the object moving. The signal strength with added Gausian noise is Wifi obtained from the antenae and Wifi without noise is the ground truth map.Our algorithm for localization is similar to the way human localize themselves that is: taking observation to gain some idea about what the location is, then making a predction based on orientation. Taking new observation and confirm prediction with new observation.

localization step:
  - Normalize both the Wifi reading and Ground Truth Wifi data
  - Subtract/Divide/dot Product between the reading and ground truth data
to find the closest match within range
  - Predict next location would be
  - Make new observation after moving 
  - Compare prediction with new observation. ==

1. Normalize Data:

   for i=1:length(xs)
       maximum = max( [Mac1(i), Mac2(i), Mac3(i), Mac4(i), Mac5(i) ]);
       Mac1(i) = uMac1(i)/maximum;
       Mac2(i) = uMac2(i)/maximum;
       Mac3(i) = uMac3(i)/maximum;
       Mac4(i) = uMac4(i)/maximum;
       Mac5(i) = uMac5(i)/maximum;end

2. Initial Guess For subtraction method, the best way to find the closest match is compute the Euclidean distance using: sqrt((R1-G1)^2+(R2-G2)^2+...+(Rn-Gn)^2). Where R: Wifi reading and G: Ground truth data. However, we want to avoid computing the sqrt so that we can obtain a faster computing time. Performing just the subtracting works just fine.

   n=1;
   m=0;
   Q=[Mac11(1) Mac22(1) Mac33(1)  Mac44(1) Mac55(1)];
   for j = 1:length(xs)  %% for every (x,y), do this 
           Z=[Mac1(j) Mac2(j) Mac3(j)  Mac4(j) Mac5(j)];
           S=Q-Z; 
           corrMat(j)=5-(abs(S(1))+abs(S(2))+abs(S(3))+abs(S(4))+abs(S(5)));
           [p f]=max(corrMat);
       %%%%%%%%%%%%%%%%%%%%
   end
   for j=1:length(corrMat)
          if(corrMat(j) >=5-0.1)
              n=j;
          end
      f(j)=n; % index where the ratio is within range
end

Shifting: Make prediction next location would be

   for i=1:length(xs)
       for n=1:length(xs)       
           if (abs(xs(n)-xs(f(i)))==0.5 && ys(n)-ys(f(i))==0) || (abs(ys(n)-ys(f(i)))==0.5 && xs(n)-xs(f(i))==0)
           bel(n)=n;
           end        
       end
   end
   bel=unique(bel);
   scatter(xs, ys, a, 's', 'b');
   axis([-2 18 -.5 14 ]);

Compute the ratio of each Wi-Fi reading with MAP

   up=ones(1,488);
   for i=2:length(path)-1   
       Q=[Mac11(path(i)+1) Mac22(path(i)+1) Mac33(path(i)+1) Mac44(path(i)+1) Mac55(path(i)+1)];
       for j = 1:length(xs)  %% for every (x,y), do this 
           Z=[Mac1(j) Mac2(j) Mac3(j) Mac4(j) Mac5(j)];
           S=Q-Z; 
           corrMat(j)=5-(abs(S(1))+abs(S(2))+abs(S(3))+abs(S(4))+abs(S(5)));
           [p k]=max(corrMat);
       %%%%%%%%%%%%%%%%%%%%
   end

Return the index where the differences is within (max-1,max) This range can be changed to any numbers

   for j=1:length(corrMat)
       if(corrMat(j)>=p-1 )
           n=j;
       end
      k(j)=n;
   end
   k=unique(k);

Compare result with prediction

   up= intersect(bel,k);
   diff=setdiff(bel,k);
   bel=ones(1,488);
   for l=1:length(up)
       for n=1:length(xs)       
           if xs(path(i+1)+1)-xs(path(i)+1)==0.5
               if ((xs(n)-xs(up(l)))==0.5 && abs(ys(n)-ys(up(l)))==0)% || (abs(ys(n)-ys(up(l)))==0.5 && abs(xs(n)-xs(up(l)))==0)
               bel(n)=n;
               end
           elseif xs(path(i)+1)-xs(path(i+1)+1)==0.5
               if ((xs(n)-xs(up(l)))==-0.5 && abs(ys(n)-ys(up(l)))==0)% || (abs(ys(n)-ys(up(l)))==0.5 && abs(xs(n)-xs(up(l)))==0)
               bel(n)=n;
               end
           elseif ys(path(i+1)+1)-ys(path(i)+1)==0.5
               if ((ys(n)-ys(up(l)))==0.5 && abs(xs(n)-xs(up(l)))==0)% || (abs(ys(n)-ys(up(l)))==0.5 && abs(xs(n)-xs(up(l)))==0)
               bel(n)=n;
               end
           elseif ys(path(i)+1)-ys(path(i+1)+1)==0.5
               if ((ys(n)-ys(up(l)))==-0.5 && abs(xs(n)-xs(up(l)))==0)% || (abs(ys(n)-ys(up(l)))==0.5 && abs(xs(n)-xs(up(l)))==0)
               bel(n)=n;
               end
           end        
       end
   end
   bel=unique(bel);
   f=setdiff(k,up);
   corrMat(f)=corrMat(f)*.8;
Ploting Heat Map, World model and Motion
       cor=vec2mat(corrMat,2);
       figure(1);
       tri = delaunay(x,y);
       trisurf(tri,x,y,cor);
       shading interp;  
       colorbar;
       view(2)
       h=figure(2);
       scatter(xs, ys, a, 's', 'b')
       hold on
       scatter(xs(path(i)+1), ys(path(i)+1), 'r', 's', 'filled');
       %scatter(xs(bel), ys(bel), 'b', 'p');          
       scatter(xs(up), ys(up), 'k', 'd', 'filled');
       %scatter(xs(k), ys(k), 'm', 's');
       axis([-2 18 -.5 14 ]);
       legend('.5 meter wall' ,'actual location','estimate location');
       hold off
       mov(:,i)=getframe;      
end