###### sinat_34871558

2016-05-08 07:39 阅读 4.5k

# 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 07:20

想知道这个程序哪里找来的？

点赞 评论 复制链接分享