clc
clear
%xyz = zeros(2,1);
% 设定基站的值,四个固定节点
A=[0,0,0];
B=[0,1000,0];
C=[1000,0,0];
D=[0,0,1000];
data=rand(1000,3)*10; %产生一个1000行3列的(0-1)之间的数,再乘以10(1-10)之间的数
data(:,1) = rand(1000,1)*1000; %第一列随机产生1000个数乘以1000,就是0-1000之间的数 带上一位小数
data(:,2) = rand(1000,1)*1000;
data(:,3) = rand(1000,1)*1000;
length(data); %返回1000
rtlength =zeros(length(data),4); %rtlength为1000行4列的0矩阵
for t=1:length(data) %循环1000次
rtlength(t,1) =sqrt((data(t,1)-A(1))^2+(data(t,2)-A(2))^2+(data(t,3)-A(3))^2)+(rand(1)-0.5)*1; %第i个点与A的距离
rtlength(t,2) =sqrt((data(t,1)-B(1))^2+(data(t,2)-B(2))^2+(data(t,3)-B(3))^2)+(rand(1)-0.5)*1; %第i个点与B的距离
rtlength(t,3) =sqrt((data(t,1)-C(1))^2+(data(t,2)-C(2))^2+(data(t,3)-C(3))^2)+(rand(1)-0.5)*1; %第i个点与C的距离
rtlength(t,4) =sqrt((data(t,1)-D(1))^2+(data(t,2)-D(2))^2+(data(t,3)-D(3))^2)+(rand(1)-0.5)*1; %第i个点与D的距离
end
% rtlength 每一行为随机点到4个基站的距离
%plot3(data(:,1),data(:,2),data(:,3),'.');
plot3(data(2,1),data(2,2),data(2,3),'.');
hold on
plot3(data(3,1),data(3,2),data(3,3),'.');
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(3,length(rtlength)); %xyz为3行1000列的矩阵
for h=1:length(rtlength)
AP =[0,0,0
0,1000,0
1000,0,0
0,0,1000] ;
num_ap = length(AP); %length(AP)为4
if num_ap>=4
Q =zeros(1,num_ap);
for j=1:num_ap
Q(1,j) =0.5;
end
Q = diag(Q); %Q为1行4列的向量,从赋值到左边Q为4行4列的矩阵,对角线元素为Q
for i=1:num_ap
Node(i)=AP(i,1)^2+AP(i,2)^2+AP(i,3)^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(i,1)-AP(1,1)),2*(AP(i,2)-AP(1,2)),2*(AP(i,3)-AP(1,3))];
A=[A;2*AP(i,1),2*AP(i,2),2*AP(i,3)]; %系数矩阵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(2,1),xyzzz(2,2),xyzzz(2,3),'go');
plot3(xyzzz(3,1),xyzzz(3,2),xyzzz(3,3),'go');
answerx = 0;
answery = 0;
for i=1:length(xyzzz) %1-1000循环
answerx = answerx+abs(xyzzz(i,1)-data(i,1)); %abs取绝对值 累计x的总误差
answery = answery+abs(xyzzz(i,2)-data(i,2));
end
answerx = answerx/length(xyzzz); %平均误差
answery = answery/length(xyzzz);