clear all;close all;
LS=[0,0;200,0;100,100*sqrt(3)];
sensor(1)=rand*200;
sensor(2)=rand*200;
X21 = LS(2,1) - LS(1,1);
X31 = LS(3,1) - LS(1,1);
Y21 = LS(2,2) - LS(1,2);
Y31 = LS(3,2) - LS(1,2);
A = inv([X21,Y21;X31,Y31]);
R1 = sqrt((LS(1,1) - sensor(1))^2 + (LS(1,2) - sensor(2))^2);
R2 = sqrt((LS(2,1) - sensor(1))^2 + (LS(2,2) - sensor(2))^2);
R3 = sqrt((LS(3,1) - sensor(1))^2 + (LS(3,2) - sensor(2))^2);
R21=R2-R1+normrnd(0,3);
R31=R3-R1+normrnd(0,3);
B=[R21;R31];
K1 = LS(1,1)^2 + LS(1,2)^2;
K2 = LS(2,1)^2 + LS(2,2)^2;
K3 = LS(3,1)^2 + LS(3,2)^2;
C = 0.5*[R21^2 - K2 + K1; R31^2 - K3 + K1];
a = B'*A'*A*B - 1;
b = B'*A'*A*C + C'*A'*A*B;
c = C'*A'*A*C;
root1 = (-b + sqrt(b^2 - 4*a*c))/(2*a);
root2 = (-b - sqrt(b^2 - 4*a*c))/(2*a);
EMS1= -A*(B*root1 + C);
EMS2= -A*(B*root2 + C);
R21ems1=sqrt((LS(2,1) - EMS1(1))^2 + (LS(2,2) - EMS1(2))^2)-sqrt((LS(1,1) - EMS1(1))^2 + (LS(1,2) - EMS1(2))^2);
R31ems1=sqrt((LS(3,1) - EMS1(1))^2 + (LS(3,2) - EMS1(2))^2)-sqrt((LS(1,1) - EMS1(1))^2 + (LS(1,2) - EMS1(2))^2);
if sign(R21)==sign(R21ems1)&&sign(R31)==sign(R31ems1),
OUT=EMS1;
else
OUT=EMS2;
end
scatter(LS(:,1),LS(:,2),'rs','filled');
xlabel('X [m]');
ylabel('Y [m]');
box on; hold all;
scatter(sensor(1),sensor(2),'bs','filled'); hold all;
scatter(OUT(1),OUT(2),'ys','filled'); hold all;
- 1
- 2
前往页