%兩站和三站定位
clear;clc;
M=10;   %被測點(diǎn)數(shù)
N=100;  %對(duì)每點(diǎn)的測量次數(shù)
x1=0,y1=0;    %第一站坐標(biāo)
x2=1000,y2=0; %第二站坐標(biāo)
x3=2000,y3=0;   %第三站坐標(biāo)

delta=0.01;
xt=2000*rand(1,M);
yt=2000*rand(1,M);
theta1=atan((xt-x1)./(yt-y1));
theta2=atan((xt-x2)./(yt-y2));
theta3=atan((xt-x3)./(yt-y3));
plot(xt,yt,'or');
hold on;

for (i=1:M)   
%角度測得值  
mtheta1(i,:)=theta1(i)*ones(1,N)+delta*randn(1,N);  
mtheta2(i,:)=theta2(i)*ones(1,N)+delta*randn(1,N);
mtheta3(i,:)=theta3(i)*ones(1,N)+delta*randn(1,N);
for(j=1:N)
%利用兩站進(jìn)行計(jì)算
xe2(i,j)=(y2-y1-cot(mtheta1(i,j))*x1+cot(mtheta2(i,j))*x2)/(cot(mtheta2(i,j))-cot(mtheta1(i,j)));
ye2(i,j)=(cot(mtheta2(i,j))*y1-cot(mtheta1(i,j))*y2-cot(mtheta1(i,j))*cot(mtheta2(i,j))*(x1-x2))/(cot(mtheta2(i,j))-cot(mtheta1(i,j)));
%利用三站進(jìn)行計(jì)算
x12(i,j)=((y1-y2)*tan(mtheta1(i,j)).*tan(mtheta2(i,j))+x2*tan(mtheta1(i,j))-x1*tan(mtheta2(i,j)))/(tan(mtheta1(i,j))-tan(mtheta2(i,j)));
y12(i,j)=(y1*tan(mtheta1(i,j))-y2*tan(mtheta2(i,j))+(x2-x1))/(tan(mtheta1(i,j))-tan(mtheta2(i,j)));
x13(i,j)=((y1-y3)*tan(mtheta1(i,j)).*tan(mtheta3(i,j))+x3*tan(mtheta1(i,j))-x1*tan(mtheta3(i,j)))/(tan(mtheta1(i,j))-tan(mtheta3(i,j)));
y13(i,j)=(y1*tan(mtheta1(i,j))-y3*tan(mtheta3(i,j))+(x3-x1))/(tan(mtheta1(i,j))-tan(mtheta3(i,j)));
x23(i,j)=((y2-y3)*tan(mtheta2(i,j)).*tan(mtheta3(i,j))+x3*tan(mtheta2(i,j))-x2*tan(mtheta3(i,j)))/(tan(mtheta2(i,j))-tan(mtheta3(i,j)));
y23(i,j)=(y2*tan(mtheta2(i,j))-y3*tan(mtheta3(i,j))+(x3-x2))/(tan(mtheta2(i,j))-tan(mtheta3(i,j)));
xe3(i,j)=(x12(i,j)+x13(i,j)+x23(i,j))/3;
ye3(i,j)=(y12(i,j)+y13(i,j)+y23(i,j))/3;
end
%計(jì)算均值和方差，并以均值作為最終測得坐標(biāo)值
meanposx2(i)=mean(xe2(i,:));
meanposy2(i)=mean(ye2(i,:));
stdposx2(i)=std(xe2(i,:));
stdposy2(i)=std(ye2(i,:));
stdpos2(i)=(stdposx2(i)^2+stdposy2(i)^2)^1/2;
plot(meanposx2(i),meanposy2(i),'.b');
xlabel('X');
ylabel('Y');
axis([0 2000 0 2000]);

meanposx3(i)=mean(xe3(i,:));
meanposy3(i)=mean(ye3(i,:));
stdposx3(i)=std(xe3(i,:));
stdposy3(i)=std(ye3(i,:));
stdpos3(i)=(stdposx3(i)^2+stdposy3(i)^2)^1/2;
plot(meanposx3(i),meanposy3(i),'*g');
end
figure;
plot(1:M,stdpos2);
hold on;
plot(1:M,stdpos3,'--r');
ylabel('E');