无线传感器网络定位算法matlab代码

%信标节点位于等边三角形顶点的仿真 clc clear all %S为三个选定的信标节点坐标矩阵,节点坐标已知(为便于仿真及验证,代码中采用的等边三角形) %依次测试信标节点间距离放大t倍时的定位效果 for t = 1:10 S=[0,0;5*t,5*t*sqrt(3);10*t,0]; nums = [S(1,1),S(1,2),S(2,1),S(2,2),S(3,1),S(3,2)]; p = min(nums); q = max(nums); L = sqrt((S(1,1)-S(3,1))^2+(S(1,2)-S(3,2))^2); m = 100; %生成在[p,q]上满足均匀分布的随机数矩阵 %即生成一组m行2列的有可能落在等边三角形区域内的坐标 numbox = p+(q-p)*rand(m,2); %计数初值,最终根据计算将随机生成的点中落在等边三角形区域内的坐标存放于新的矩阵 n = 1; for i = 1:m dA(i) = sqrt((numbox(i,1)-S(1,1))^2+(numbox(i,2)-S(1,2))^2); dB(i) = sqrt((numbox(i,1)-S(2,1))^2+(numbox(i,2)-S(2,2))^2); dC(i) = sqrt((numbox(i,1)-S(3,1))^2+(numbox(i,2)-S(3,2))^2); %将确实在等边三角形区域内的坐标存入P_position矩阵 if (dA(i)<=L) && (dB(i)<=L) && (dC(i)<=L) P_position(n,1) = numbox(i,1); P_position(n,2) = numbox(i,2); n = n+1; end end %N为随机生成的点中落在等边三角形区域内的点(测试点)的个数 N = n-1 if N == 0 disp('所取的随机坐标无一落在等边三角形内,请增大m值重新运行程序.') return end %计算测试点离三个顶点的实际距离 %dis为N行3列的矩阵,用于存放N个测试点分别到等边三角形三个顶点A,B,C的实际距离 for i = 1:N dis(i,1) = sqrt((P_position(i,1)-S(1,1))^2+(P_position(i,2)-S(1,2))^2); dis(i,2) = sqrt((P_position(i,1)-S(2,1))^2+(P_position(i,2)-S(2,2))^2); dis(i,3) = sqrt((P_position(i,1)-S(3,1))^2+(P_position(i,2)-S(3,2))^2); end %根据函数Distance计算测试点离三个顶点的测试距离(考虑了衰减及环境误差等) %dis_test为N行3列的矩阵,用于存放N个测试点分别到等边三角形三个顶点A,B,C的测试距离 a = 7; %由RSSI计算T-R距离时使用的参数 for i = 1:N dis_test(i,1) = Distance(dis(i,1),a); dis_test(i,2) = Distance(dis(i,2),a); dis_test(i,3) = Distance(dis(i,3),a); end %根据函数Triangle及求得的测试距离进行定位 %P_calculate为N行2列的矩阵,用于存放定位后的N个坐标 for i = 1:N [P_temp_SAPSO,fv]= SAPSO(@fitness,500,2,2,0.6,20,2,S,dis_test(i,1),dis_test(i,2),dis_test(i,3)); P_calculate_SAPSO(i,1) = P_temp_SAPSO(1); P_calculate_SAPSO(i,2) = P_temp_SAPSO(2); P_temp = Triangle(S,dis_test(i,1),dis_test(i,2),dis_test(i,3)); P_calculate_Triangle(i,1) = P_temp(1); P_calculate_Triangle(i,2) = P_temp(2); end %由于测试距离相比真实距离有误差,三角计算中的两圆有可能无交点,导致方程无实根. %于是P_calculate中会出现虚数.在测试中虚数无实际意义,因此取其实部存放于另一矩阵 for i = 1:N P_calculate_Triangle_real(i,1) = real(P_calculate_Triangle(i,1)); P_calculate_Triangle_real(i,2) = real(P_calculate_Triangle(i,2)); end %对比测试点的定位坐标与实际坐标之间的误差 P_position; P_calculate_SAPSO; P_calculate_Triangle_real; %计算定位结果与真实坐标之间的距离误差平均值e_average(测试点等概率) e_sum_SAPSO = 0; e_sum_Triangle=0; e_max_SAPSO=0; e_max_Triangle=0; for i = 1:N e_SAPSO = sqrt((P_calculate_SAPSO(i,1)-P_position(i,1))^2+(P_calculate_SAPSO(i,2)-P_position(i,2))^2); if e_SAPSO>e_max_SAPSO e_max_SAPSO=e_SAPSO; end e_sum_SAPSO = e_sum_SAPSO+e_SAPSO; e_Triangle=sqrt((P_calculate_Triangle_real(i,1)-P_position(i,1))^2+(P_calculate_Triangle_real(i,2)-P_position(i,2))^2); if e_Triangle>e_max_Triangle e_max_Triangle=e_Triangle; end e_sum_Triangle=e_sum_Triangle+e_Triangle; end e_average_SAPSO = e_sum_SAPSO/N; e_average_SAPSO_box(t)=e_average_SAPSO e_average_Triangle=e_sum_Triangle/N; e_average_Triangle_box(t)=e_average_Triangle e_max_SAPSO_box(t)=e_max_SAPSO e_max_Triangle_box(t)=e_max_Triangle e_average_percent_SAPSO = e_average_SAPSO/L; e_average_percent_SAPSO_box(t)= e_average_percent_SAPSO e_average_percent_Triangle = e_average_Triangle/L; e_average_percent_Triangle_box(t)=e_average_percent_Triangle end t = [1:10]; figure; plot(t,e_average_SAPSO_box(t),'k-',t,e_average_Triangle_box(t),'k-.') xlabel('t') ylabel('平均定位误差') legend('SAPSO','Triangle') grid on; figure; plot(t,e_max_SAPSO_box(t),'k-',t,e_max_Triangle_box(t),'k-.') xlabel('t') ylabel('最大定位误差') legend('SAPSO','Triangle') grid on; figure; plot(t,e_average_percent_SAPSO_box(t),'k-',t, e_average_percent_Triangle_box(t),'k-.') xlabel('t') ylabel('平均定位误差率') legend('SAPSO','Triangle') grid on;