多边测量法应用

题目

35个未知点到6个信标节点的距离

解决

1. 先提取有用的数据

clc,clear;
nodeList=xlsread("G:\BaiduNetdiskDownload\附件.xls",2,"B2:C16");%读取信标节点x，y坐标
dismat=xlsread("G:\BaiduNetdiskDownload\附件.xls",1,"Q2:AE36");%读取未知节点到信标节点的距离矩阵
[sz1,sz2]=size(dismat);%获取矩阵大小

%%------处理数据,将距离为0的值去除
for i=1:sz1
n=1;
for m=1:sz2
if dismat(i,m)~=0%如果距离矩阵相应的值不为0
x(i,n)=nodeList(m,1);%则取相应信标节点的x，y
y(i,n)=nodeList(m,2);
distance(i,n)=dismat(i,m);
n=n+1;
end
end
end

将0值去除后的距离矩阵

2. 利用三边测量法遍历6个已知距离的信标节点

%%-----根据三边测量法，遍历已知的6个信标节点距离求解未知节点
for i=1:sz1
n=1;
for o=1:4
for p=o+1:5
for q=p+1:6
A=[2*(x(i,o)-x(i,q)),2*(y(i,o)-y(i,q));2*(x(i,p)-x(i,q)),2*(y(i,p)-y(i,q))];
B=[x(i,o)^2-x(i,q)^2+y(i,o)^2-y(i,q)^2+distance(i,q)^2-distance(i,o)^2;x(i,p)^2-x(i,q)^2+y(i,p)^2-y(i,q)^2+distance(i,q)^2-distance(i,p)^2];
X=(A'*A)^(-1)*A'*B;
xx(i,n)=X(1);
yy(i,n)=X(2);
n=n+1;
end
end
end
end

遍历得到的未知节点  可以看到遍历之后，通过6个已知距离的信号标点可以求得20个未知标点。因此，我们应该取其中最好的一个点，本文用的是 K-means 聚类算法，关于此方法不在此赘述。

从20个点中选出一个最集中的点

%%画图
figure
plot(xx(1,:),yy(1,:),'r.','MarkerSize',10)
hold on
plot(P1(1,1),P1(1,2),'bo','MarkerSize',6)
legend('20 个未知节点','聚类中心点')
hold off
figure
plot(P1(:,1),P1(:,2),'ro','MarkerSize',6)
hold on
plot(nodeList(:,1),nodeList(:,2),'b*','MarkerSize',10)
legend('未知节点','信标节点')
hold off

 之后遍历35个未知点，可得如下图所示：

3. 用多边测量法求得未知节点坐标

 未知节点1是用K-means方法求得的节点，未知节点2是多边测量法求得的节点。

各个信号节点到信标节点的距离误差

两种方法的平均误差

 可以看到两种方法求得的未知节点坐标到6个信标节点的距离的最大误差不超过6.5%，平均误差小于4.5%。
完整代码已上传到资源中。