室内定位为什么 调不出来 求修改??? function

Question

function[OUT,RMSE]=tdoa2d3p(LS,sensor,guass)

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(guass,5);
R31=R3-R1-normrnd(guass,5);
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

RMSE=sqrt((OUT(1)-sensor(1))^2+(OUT(2)-sensor(2))^2);