基于最小二乘法的定位算法——matlab仿真_注释齐全

仿真程序注释齐全，仿真结果在三维坐标中，可视化高。1000个节点随机分布在三维坐标中，随机选取两个节点进行定位，随机节点以点表示，估计节点以圈圈表示！

 clc
    clear
%xyz = zeros（21）;  
% 设定基站的值，四个固定节点
A=[000];
B=[010000];
C=[100000];
D=[001000];
data=rand（10003）*10;               %产生一个1000行3列的（0-1）之间的数，再乘以10（1-10）之间的数  
data（:1） = rand（10001）*1000;      %第一列随机产生1000个数乘以1000，就是0-1000之间的数 带上一位小数
data（:2） = rand（10001）*1000;
data（:3） = rand（10001）*1000;
length（data）;                       %返回1000
rtlength =zeros（length（data）4）;    %rtlength为1000行4列的0矩阵
for t=1:length（data）                %循环1000次
    rtlength（t1） =sqrt（（data（t1）-A（1））^2+（data（t2）-A（2））^2+（data（t3）-A（3））^2）+（rand（1）-0.5）*1;      %第i个点与A的距离
    rtlength（t2） =sqrt（（data（t1）-B（1））^2+（data（t2）-B（2））^2+（data（t3）-B（3））^2）+（rand（1）-0.5）*1;      %第i个点与B的距离
    rtlength（t3） =sqrt（（data（t1）-C（1））^2+（data（t2）-C（2））^2+（data（t3）-C（3））^2）+（rand（1）-0.5）*1;      %第i个点与C的距离
    rtlength（t4） =sqrt（（data（t1）-D（1））^2+（data（t2）-D（2））^2+（data（t3）-D（3））^2）+（rand（1）-0.5）*1;      %第i个点与D的距离
end
% rtlength 每一行为随机点到4个基站的距离

%plot3（data（:1）data（:2）data（:3）‘.‘）;
plot3（data（21）data（22）data（23）‘.‘）;
hold on 
plot3（data（31）data（32）data（33）‘.‘）;

hold on 
plot3（A（1）A（2）A（3）‘*r‘）;
plot3（B（1）B（2）B（3）‘*r‘）;
plot3（C（1）C（2）C（3）‘*r‘）;
plot3（D（1）D（2）D（3）‘*r‘）;

%axis （[0 6 0 8 0 3]）
axis （[0 1000 0 1000 0 1000]）
grid on;

xyz=zeros（3length（rtlength））;      %xyz为3行1000列的矩阵


for h=1:length（rtlength）
              AP =[000
                  010000
                  100000
                  001000] ;
              
              num_ap = length（AP）;  %length（AP）为4
              
              if num_ap>=4
              Q =zeros（1num_ap）;
              for j=1:num_ap
                 Q（1j） =0.5;
              end
               Q = diag（Q）;        %Q为1行4列的向量，从赋值到左边Q为4行4列的矩阵，对角线元素为Q
              for i=1:num_ap     
                  Node（i）=AP（i1）^2+AP（i2）^2+AP（i3）^2;    %固定参数便于位置估计  为1行四列的数
              end
              A=[];b=[];                %空矩阵A，b
              L =rtlength（h:）;          %TOA测距，将每一列分别提取出来给L从1到1000
              for i=1:num_ap             %三边定位公式逐一作差化成矩阵：A*x=b
                 % A=[A;2*（AP（i1）-AP（11））2*（AP（i2）-AP（12））2*（AP（i3）-AP（13））];  
                 A=[A;2*AP（i1）2*AP（i2）2*AP（i3）];                                 %系数矩阵A
                  b=[b;L（1）^2-L（i）^2+Node（i）-Node（1）];                                %增广矩阵b
              end
               x=inv（A‘*inv（Q）*A）*（A‘*inv（Q）*b）      %利用最小二乘法求解目的点坐标位置
%                x=inv（A‘*A）*（A‘*b）      %利用最小二乘法求解目的点坐标位置
               xyz（:h）=x;   %将x求解所得每列的数，存放到1-1000行中
%                sprintf（‘%2.2f%%‘ （line/bg）*100）    %%2.2f是保留2位小数了，也可以直接写%f
              end
end


xyzzz = zeros（length（data）3）;
xyzzz = xyz‘;               %将3行1000列的数，转化为1000行3列的矩阵

plot3（xyzzz（21）xyzzz（22）xyzzz（23）‘go‘）;
plot3（xyzzz（31）xyzzz（32）xyzzz（33）‘go‘）;

answerx = 0;
answery = 0;

for i=1:length（xyzzz）           %1-1000循环
    answerx = answerx+

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     文件        3475  2019-01-29 21:23  LMS.m