positioning8.rar

function[X_1,Y_1,X_2,Y_2,X_3,Y_3,X_4,Y_4]=positioning(x1,y1,x_2,y_2,x2,x3,y2,y3) clc %% ==============Anchor nodes========================== x1=0;y1=0;%z1=0; %% anchor node A %x2= input('Please enter an X axis for Anck 2 :') %y2= input('Please enter an Y axis for Anck 2 :') x2=-120;y2=--110; %% anchor node B %x3= input('Please enter an X axis for Anck 3 :') x3=-70;y3=-100; %% anchor node C %y3= input('Please enter an Y axis for Anck :') %% ================================================== %% ==== the actuale trajectories for Mobile nodes ==============%% %% =================================================== x=[0:+4.5:100,100:-3:40,40:+2.5:90];%x axis of actual trajectory of mobile station 1 y=[130:-2:90,90:-3:30,30:-3:-36];%y axis of actual trajectory of mobile station 1 x_2=[30:+4.5:120,120:-1:100,100:-3:33];%x axis of actual trajectory of mobile station 2 y_2=[-100:+1:-80,-80:+4:0,0:+2:44];%y axis of actual trajectory of mobile station 2 x_3=[140:-1:120,120:-2:80,80:-1:58];%x axis of actual trajectory of mobile station 3 y_3=[130:-3:70,70:-4.5:-20, -20:-4:-108];%y axis of actual trajectory of mobile station 3 x_4=[10:+5:60,60:+6:120,120:+1.5:135,135:-2:115,115:-4:34];%y axis of actual trajectory of mobile station 4 y_4=[-100:+2:-80,-80:+2.5:-55,-55:+4.5:-10,-10:+3:20,20:+2:60];%y axis of actual trajectory of mobile station 4 x_5=[155:-2:135,135:-4:54,54:-4:-16,-16:-2:-46];%y axis of actual trajectory of mobile station 4 y_5=[50:+3:-10,-10:+2:10,10:+3:70,70:+3,115];%y axis of actual trajectory of mobile station 4 %%=================================================================== %% find the estimation position of mobile station 1 which takes information from 3fixed nodes. %% ================================================================== %%segma=3;%3.7 for i=1:20% loop 1 start %% Actual distances between the 3 anchor nodes and mobile 1 %% =================================================================== d_1_1=sqrt((x(i)-x1).^2+(y(i)-y1).^2); %% distance between the referance nod and Mobile station 1 d_1_2=sqrt((x(i)-x2).^2+(y(i)-y2).^2); %% distance between B nod and Mobile station 1 d_1_3=sqrt((x(i)-x3).^2+(y(i)-y3).^2); %% distance between C nod and Mobile station 1 %% ================================================================== %% adding noise to actual distances(device error is 15cm) %%%%%%%%%%%%%%%%%%%%%%%% d_1_1=d_1_1+d_1_1*0.02*randn; d_1_2=d_1_2+d_1_1*0.02*randn; d_1_3=d_1_3+d_1_1*0.02*randn; %% ========== Least square method to calculat the estimated position for M1 ====%%%%%%%%% H=[2*x2 2*y2;2*x3 2*y3]; b=[x2.^2+y2.^2+d_1_1.^2-d_1_2.^2;x3.^2+y3.^2+d_1_1.^2-d_1_3.^2]; var_d1=exp(4*log(d_1_1))*0.5256; var_d2=exp(4*log(d_1_2))*0.5256; var_d3=exp(4*log(d_1_3))*0.5256; S=[(var_d1+var_d2) var_d1 ; var_d1 (var_d1+var_d3)]; %x_est=(inv(transpose(H)*(inv(S))*H))*(transpose(H)*(inv(S))*b); x_est=(inv(transpose(H)*H))*(transpose(H)*b); X_1=x_est(1,:);% x axis Y_1=x_est(2,:);% x axis X_1_m_a(i)=X_1; Y_1_m_a(i)=Y_1; %%========================================================= %% find the estimation position of mobile station 2 which takes information from 2fixed nodes and one mobile station %% =================================================================== %% Actual distances between the 2 anchor nodes and mobile 2 , %%========================================================= d_2_1=sqrt((x_2(i)-x1).^2+(y_2(i)-y1).^2); %% distance between the referance nod and Mobile station2 d_2_2=sqrt((x_2(i)-x2).^2+(y_2(i)-y2).^2); %% distance between B nod and Mobile station 2 d_2_3=sqrt((x_2(i)-X_1).^2+(y_2(i)-Y_1).^2); %% distance between MOBILE STATION 1 and Mobile station 2 %%========================================================= %% adding noise to actual distances(device error is 15cm) %%%%%%%%%%%%%%%%% d_2_1=d_2_1+d_1_1*0.02*randn; d_2_2=d_2_2+d_1_1*0.02*randn; %%========================================================= %% ========== Least square method to calculat the estimated position for M2==== H_2=[2*x2 2*y2;2*X_1 2*Y_1]; b_2=[x2.^2+y2.^2+d_2_1.^2-d_2_2.^2;X_1.^2+Y_1.^2+d_2_1.^2-d_2_3.^2]; var_d1_2=exp(4*log(d_2_1))*0.5256; var_d2_2=exp(4*log(d_2_2))*0.5256; var_d3_2=exp(4*log(d_2_3))*0.5256; S_2=[(var_d1_2+var_d2_2) var_d1_2 ; var_d1 (var_d1_2+var_d3_2)]; %x_est_2=(inv(transpose(H_2)*(inv(S_2))*H_2))*(transpose(H_2)*(inv(S_2))*b_2); x_est_2=(inv(transpose(H_2)*H_2))*(transpose(H_2)*b_2); X_2=x_est_2(1,:);%x axis Y_2=x_est_2(2,:);%y axis X_2_m_a(i)=X_2; Y_2_m_a(i)=Y_2; %=========================================================================== %% find the estimation position of mobile station 2 which takes information from 2fixed nodes and one %% ==================mobile station============================================ %% Actual distances between the thired mobile station(3) and referance node and two mobile %% =====================stations(1 and 2)======================================= %%========================================================= d_3_1=sqrt((x_3(i)-x1).^2+(y_3(i)-y1).^2); %% distance between the referance nod and Mobile station2 d_3_2=sqrt((x_3(i)-X_2).^2+(y_3(i)-Y_2).^2); %% distance between B nod and Mobile station 12 d_3_3=sqrt((x_3(i)-X_1).^2+(y_3(i)-Y_1).^2); %% distance between MOBILE STATION 1 and Mobile station 2 %%========================================================= %% adding noise to actual distances(device error is 15cm) %%%%%%%%%%%%%%%%% d_3_1=d_3_1+d_1_1*0.02*randn; %%========================================================= %% ===Least square method to calculat the estimated position for mobile station 3 ==%% H_3=[2*X_2 2*Y_2 ;2*X_1 2*Y_1]; b_3=[X_2.^2+Y_2.^2+d_3_1.^2-d_3_2.^2; X_1.^2+Y_1.^2+d_3_1.^2-d_3_3.^2]; var_d1_3=exp(4*log(d_3_1))*0.5256; var_d2_3=exp(4*log(d_3_2))*0.5256; var_d3_3=exp(4*log(d_3_3))*0.5256; S_3=[(var_d1_3+var_d2_3) var_d1_3 ; var_d1_3 (var_d1_3+var_d3_3)]; %x_est_3=(inv(transpose(H_3)*(inv(S_3))*H_3))*(transpose(H_3)*(inv(S_3))*b_3); x_est_3=(inv(transpose(H_3)*H_3))*(transpose(H_3)*b_3); X_3=x_est_3(1,:);% x axis Y_3=x_est_3(2,:);% y axis X_3_m_a(i)=X_3; Y_3_m_a(i)=Y_3; %% ============================================================= %% find the estimation position of mobile station 4 which takes information from onle the referance node and two mobiles(2 anad 3) %%========================================================= %% Actual distances between the forth mobile station (4) and referance node %% and two mobile stations(2 and 3 ) %% ========================================================= d_4_1=sqrt((x_4(i)-x1).^2+(y_4(i)-y1).^2); %% distance between the referance nod and Mobile station2 d_4_2=sqrt((x_4(i)-X_3).^2+(y_4(i)-Y_3).^2); %% distance between B nod and Mobile station 12 d_4_3=sqrt((x_4(i)-X_2).^2+(y_4(i)-Y_2).^2); %% distance between MOBILE STATION 1 and Mobile station 2 d_4_4=sqrt((x_4(i)-X_1).^2+(y_4(i)-Y_1).^2); %% distance between MOBILE STATION 1 and Mobile station 2 %%========================================================= %% adding noise to actual distances(device error is 15cm) %% ========================================= d_4_1=d_4_1+0.15*randn; %%========================================= %% ========== Least square method to calculat the estimated position for M4 ==== %% ============================================================== H_4=[2*X_3 2*Y_3 ;2*X_2 2*Y_2;2*X_1 2*Y_1]; b_4=[X_3.^2+Y_3.^2+d_4_1.^2-d_4_2.^2;X_2.^2+Y_2.^2+d_4_1.^2-d_4_3.^2;X_1.^2+Y_1.^2+d_4_1.^2-d_4_4.^2]; var_d1_4=exp(4*log(d_4_1))*0.5256; var_d2_4=exp(4*log(d_4_2))*0.5256; var_d3_4=exp(4*log(d_4_3))*0.5256; var_d4_4=exp(4*log(d_4_4))*0.5256; S_4=[(var_d1_4+var_d2_4) var_d1_4 var_d1_4 ; var_d1_4 (var_d1_4+var_d4_4) var_d1_4 var_d1_4 var_d1_4 (var_d1_4+var_d3_4)]; %x