matlab三角定位算法

clear all
disp('三边测量法')
x=[8 30 50]
y=[50 10 50]
plot(x,y,'o')
axis([-5 65 -5 65])
hold on
[x0,y0]=ginput(1)
plot(x0,y0,'g:o')
axis([-5 65 -5 65])
distance=[0 0 0]
for i=1:3
distance(i)=sqrt((x(i)-x0)^2+(y(i)-y0)^2)
end
error=rand(1,3)-0.3
for j=1:7
distance=distance-distance.*(error*j/7); %误差控制？
aa=inv(2*([x(1)-x(3) y(1)-y(3);x(2)-x(3) y(2)-y(3)]))
bb=[x(1)^2-x(3)^2+y(1)^2-y(3)^2+distance(3)^2-distance(1)^2;x(2)^2-x(3)^2+y(2)^2-y(3)^2+distance(3)^2-distance(2)^2]
cc(j,:)=(aa*bb)' %计算并存放估算位置
plot(cc(j,1),cc(j,2),'+')
axis([-50 100 -50 100])
e(j)=sqrt((cc(j,1)-x0)^2+(cc(j,2)-y0)^2) %求出误差
end
legend('信标节点','未知节点','误差节点',4) %标出图例
s='加权质心算法（部分）' %书写图名
title(s)
hold on
figure
rate=1:1:7
plot(rate,e,'g:*')
xlabel('x/误差率') %是xlabel,x-l-a-b-e-l
ylabel('y/误差')
axis([0 7 -20 50])
hold on