MATLAB实现三边定位

Posted 2020-10-10 Aaron12

 

MATLAB实现

trilateration_position.m

 1 % ----------------采用三边定位法对未知节点定位-------------------------------
 2 
 3 %{
 4     clc命令是用来清除命令窗口的内容。不管开启多少个应用程序，命令窗口只有一个，
 5     所以clc无论是在脚本m文件或者函数m文件调用时，clc命令都会清除命令窗口的内容。
 6 
 7     clear命令可以用来清除工作空间的内容。MATLAB有个基本的工作空间，用base标识，
 8     此外，当打开一个函数m文件时，可能会产生很多工作空间。每一个函数对应一个工作空间。
 9 %}
10 clear;
11 
12 maxx = 1000;%参考节点分布的最大横坐标
13 maxy = 1000;%参考节点分布的最大纵坐标
14 
15 %----------------------随机初始化三个已知的参考点[cx,cy]-----------------
16 %{
17     rand()产生0和1之间均匀分布的随机数
18     rand(m)产生一个m*m的矩阵，当然矩阵的值是0和1之间均匀分布的随机数
19     rand(m,n)或者rand([m,n])产生一个m*n的矩阵
20     randn()产生均值为0, 方差为1的正态分布的随机数。用法和rand类似。
21 %}
22 cx = maxx*rand(1,3);
23 cy = maxy*rand(1,3);
24 plot(cx,cy,\'k^\');%参考节点图
25 
26 %--------随机初始化一个未知节点（mx,my）-----------
27 mx = maxx*rand();
28 my = maxy*rand();
29 hold on;
30 % 盲节点图
31 plot(mx,my,\'go\'); 
32 
33 da = sqrt((mx-cx(1))^2+(my-cy(1))^2);
34 db = sqrt((mx-cx(2))^2+(my-cy(2))^2);
35 dc = sqrt((mx-cx(3))^2+(my-cy(3))^2);
36 
37 % 计算定位坐标
38 [locx,locy] = triposition(cx(1),cy(1),da,cx(2),cy(2),db,cx(3),cy(3),dc);      
39 plot(locx,locy,\'r*\');
40 legend(\'定位基站\',\'盲节点\',\'定位标签\',\'Location\',\'SouthEast\');  
41 title(\'三边测量法的定位\');
42 
43 derror = sqrt((locx-mx)^2 + (locy-my)^2);
44 disp(derror);

 triposition.m

 1 function [locx,locy] = triposition(xa,ya,da,xb,yb,db,xc,yc,dc)
 2 %              三点定位法                          %
 3 %输入：
 4 %   1.参考节点A（xa,ya）,B(xb,yb),C(xc,yc)
 5 %   2.定位节点D(locx,locy)到这三点的距离分别为da,db,dc
 6 %返回：
 7 %   （locx，locy)为计算的定位节点D点的位置坐标
 8 %
 9 syms x y   %f符号变量
10 %--------------求解方程组------------------------------------
11 f1 = \'2*x*(xa-xc)+xc^2-xa^2+2*y*(ya-yc)+yc^2-ya^2=dc^2-da^2\';
12 f2 = \'2*x*(xb-xc)+xc^2-xb^2+2*y*(yb-yc)+yc^2-yb^2=dc^2-db^2\';
13 % 解关于x,y的符号方程组，得到解的符号表示，并存入xx,yy
14 [xx,yy] = solve(f1,f2,x,y); 
15 px = eval(xx);  %解的数值px(1),px(2)
16 py = eval(yy);  %解的数值py(1),py(2)
17 locx = px;
18 locy = py;

 运行效果