sinat_34871558 于 2016.05.08 15:39 提问

MATLAB室内定位程序仿真 看不懂请注释 万分感谢

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个回答

qq_33525335   2016.05.12 15:20