Wifi Localization sample code(MATLAB)
== 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