三角定位matlab,基于RSSI校正的三角形质心定位算法

%三边测量的定位算法

%dA,dB,dC为A,B,C到未知节点(假定坐标[x,y]未知)的模拟测量距离

function [P] = Triangle(A,B,C,dA,dB,dC)

%A,B,C为三个选定的信标节点,节点坐标已知(为便于防真及验证,代码中采用的等边三角形)

A = [0,0];

B = [25,25*sqrt(3)];

C = [50,0];

dA=;

dB=;%只需赋上相应的值即可(通过之前的那个程序将RSSI转化成距离即可)

dC=;

%定义未知坐标x,y为符号变量

syms x y;

%距离方程,以信标节点为圆心,信标节点到未知节点的测量距离为半径作三个圆

f1 = (A(1)-x)^2+(A(2)-y)^2-dA^2;

f2 = (B(1)-x)^2+(B(2)-y)^2-dB^2;

f3 = (C(1)-x)^2+(C(2)-y)^2-dC^2;

%任两个方程联立,求任两圆交点

s1 = solve(f1,f2); %求A,B两圆的交点

s2 = solve(f2,f3); %求B,C两圆的交点

s3 = solve(f1,f3); %求A,C两圆的交点

%将结果(符号变量)转换为双精度数值

x1 = double(s1.x);

y1 = double(s1.y);

x2 = double(s2.x);

y2 = double(s2.y);

x3 = double(s3.x);

y3 = double(s3.y);

%选