What is the proplem ? This code didnt work

Question

0 개 추천

Multilateration.m

function [ N_connectivity,Aveareg_anchor,L_ratio,Locali_error]=Multilateration(Anchor_x,Anchor_y,Anchor_z,Ordin_x,Ordin_y,Ordin_z,No_Ordinary_node,No_anchor_node) global px enodexx xy(:,1)=Anchor_x; xy(:,2)=Anchor_y; xy(:,3)=Anchor_z; px=[]; py=[]; pz=[]; x = Ordin_x; y = Ordin_y; z = Ordin_z; x = uint32(x); y = uint32(y); z = uint32(z); x = double(x); y = double(y); z = double(z); px= x; py= y; pz=z; D=[]; anodexx=[]; anodeyy=[]; anodezz=[]; enodexx=[]; enodeyy=[]; enodezz=[]; for ii=1:No_Ordinary_node [~, ~]= size(xy); %%% Localization Pr=[]; for ai=1:No_anchor_node Gr=1; Gt=1; Pt = 10; L=1; d=double(1); f=double(2400); i=1; ax= xy(ai,1); ay= xy(ai,2); ux = px(ii); uy = py(ii); d = (ax-ux)^2 + (ay-uy)^2; p= (double(Pt) * double(Gr) * double(Gt)) / (((4 pi * d)/ (1/f))^2 ) *L; Pr = [Pr p]; end [~, ix]=sort(Pr(:),'descend'); ix = ix(1:No_anchor_node); xx_val=[]; yy_val=[]; zz_val=[]; for xxxi=1:length(ix) x1=double(xy(ix(xxxi),1)); y1=double(xy(ix(xxxi),2)); z1=double(xy(ix(xxxi),3)); anodex(xxxi)=x1; anodey(xxxi)=y1; anodez(xxxi)=z1; end %%% Applying Trilateration j=1; % x,y cordinates for unknown node xi = double(px(ii)); yi = double(py(ii)); zi = double(pz(ii)); for ai=1:No_anchor_node xj = double(anodex(ai)); yj = double(anodey(ai)); zj = double(anodez(ai)); d(ii,ai)= (xi-xj)^2 +(yi-yj)^2+(zi-zj)^2; [min_val,min_id]=sort(d(ii,:),'ascend'); end a1=(2(anodex(min_id(1))-anodex(min_id(4)))); b1=(2*(anodey(min_id(1))-anodey(min_id(4)))); c1=(2*(anodez(min_id(1))-anodez(min_id(4)))); s1=(anodex(min_id(1))^2)-(anodex(min_id(4))^2)+(anodey(min_id(1))^2)-(anodey(min_id(4))^2)+(anodez(min_id(1))^2)-(anodez(min_id(4))^2)+(d(ii,min_id(4))^2)-(d(ii,min_id(1))^2); a2=(2*(anodex(min_id(2))-anodex(min_id(4)))); b2=(2*(anodey(min_id(2))-anodey(min_id(4)))); c2=(2*(anodez(min_id(2))-anodez(min_id(4)))); s2=(anodex(min_id(2))^2)-(anodex(min_id(4))^2)+(anodey(min_id(2))^2)-(anodey(min_id(4))^2)+(anodez(min_id(2))^2)-(anodez(min_id(4))^2)+(d(ii,min_id(4))^2)-(d(ii,min_id(2))^2); a3=(2*(anodex(min_id(3))-anodex(min_id(4)))); b3=(2*(anodey(min_id(3))-anodey(min_id(4)))); c3=(2*(anodez(min_id(3))-anodez(min_id(4)))); s3=(anodex(min_id(3))^2)-(anodex(min_id(4))^2)+(anodey(min_id(3))^2)-(anodey(min_id(4))^2)+(anodez(min_id(3))^2)-(anodez(min_id(4))^2)+(d(ii,min_id(4))^2)-(d(ii,min_id(3))^2); Est_X(ii)=((a3*((b2*c3)-(b3*c2))*s1)+(a3*((b3*c1)-(b1*c3))*s2)+(a3*((b1*c2)-(b2*c1))*s3))/((((a3*b1)-(a1*b3))*((a3*c2)-(a2*c3)))-(((a3*b2)-(a2*b3))*((a3*c1)-(a1*c3)))); Est_Y(ii)=((a3*((a3*c2)-(a2*c3))*s1)+(a3*((a1*c3)-(a3*c1))*s2)+(a3*((a2*c1)-(a1*c2))*s3))/((((a3*b1)-(a1*b3))*((a3*c2)-(a2*c3)))-(((a3*b2)-(a2*b3))*((a3*c1)-(a1*c3)))); Est_Z(ii)=((a3*((a2*b3)-(a3*b2))*s1)+(a3*((a3*b1)-(a1*b3))*s2)+(a3*((a1*b2)-(a2*b1))*s3))/((((a3*b1)-(a1*b3))*((a3*c2)-(a2*c3)))-(((a3*b2)-(a2*b3))*((a3*c1)-(a1*c3)))); enodex =[Est_X(ii)]; enodey =[Est_Y(ii)]; enodez =[Est_Z(ii)]; anodexx=[anodexx anodex]; anodeyy=[anodeyy anodey]; anodezz=[anodezz anodez]; enodexx(ii)=px(ii)+(Est_X(ii)*10^(-7)); enodeyy(ii)=py(ii)+(Est_Y(ii)*10^(-7)); enodezz(ii)=pz(ii)+(Est_Z(ii)*10^(-7)); end for ii=1:length(px) sub_node(ii)=enodexx(ii)-px(ii); end [~,locali_id]=sort(sub_node,'descend'); locali_id1=locali_id(1,1:(end-10)); No_Local_Ordinary=length(locali_id1); L_ratio=No_Local_Ordinary./No_Ordinary_node; for Nl=1:length(locali_id1) L_error(Nl)=sqrt((enodexx(locali_id1(Nl))-px(locali_id1(Nl)))^2+(enodeyy(locali_id1(Nl))-py(locali_id1(Nl)))^2+(enodezz(locali_id1(Nl))-pz(locali_id1(Nl)))^2); end Locali_error=sum(L_error)/length(locali_id1); % Average number of neighbouring anchor nodes R=40; Ncom_a=1; idx_ii=[]; for ai=1:No_anchor_node for ii=1:No_Ordinary_node Dist_a(ai,ii)=sqrt(((Anchor_x(ai)-enodexx(ii))^2)+((Anchor_y(ai)-enodeyy(ii))^2)+((Anchor_z(ai)-enodezz(ii))^2)); if Dist_a(ai,ii)<R temp_val=find(idx_ii==ii, 1); idx_ii(ii)=ii; if ~isempty(temp_val) Ncom_a=Ncom_a+1; end end end end Aveareg_anchor=Ncom_a./No_Ordinary_node; R=40; Ncom_Ordin=1; idx_ii=[]; for ai=1:No_Ordinary_node for ii=1:No_Ordinary_node if ai~=ii Dist_O(ai,ii)=sqrt(((enodexx(ai)-enodexx(ii))^2)+((enodeyy(ai)-enodeyy(ii))^2)+((enodezz(ai)-enodezz(ii))^2)); if Dist_O(ai,ii)<R temp_val=find(idx_ii==ii, 1); idx_ii(ii)=ii; if ~isempty(temp_val) Ncom_Ordin=Ncom_Ordin+1; end end end end end N_connectivity=Ncom_Ordin./No_Ordinary_node; end