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;

運行效果

MATLAB實現三邊定位