chan算法2.txt

无线室内定位中精确定位算法

function SUM=zhao2(BSN1,BS1,MSP1,Radius1,Noise1)
%              - BSN1  为基站个数，3 < BSN <= 7；
%               - MSP1  为参考坐标。
%               - Noise1 测距误差方差。
%               - Radius1    为小区半径，单位(mm)；
%               - Za2  为移动台经算法处理后的位置.
   SUM=0;
   for number=1:10
MS1 = Radius1*MSP1;
     Q = eye(BSN1-1);
     K1 = 0;
for i = 1: BSN1,
    R0(i) = sqrt((BS1(1,i) - MS1(1,1))^2 + (BS1(2,i) - MS1(2,1))^2);
end
for i = 1: BSN1-1,
    R(i) = R0(i+1) - R0(1) + Noise1*randn(1);
    K(i) = BS1(1,i+1)^2 + BS1(2,i+1)^2;
end
for i = 1: BSN1-1,
    Ga(i,1) = -BS1(1, i+1);
    Ga(i,2) = -BS1(2, i+1);
    Ga(i,3) = -R(i);
end
for i = 1: BSN1-1,
    h(i) = 0.5*(R(i)^2 - K(i) + K1);
end
Za = inv(Ga'*inv(Q)*Ga)*Ga'*inv(Q)*h';
B = eye(BSN1-1);
for i = 1: BSN1-1,
    B(i,i) = sqrt((BS1(1,i+1) - Za(1))^2 + (BS1(2,i+1) - Za(2))^2);
end
FI = B*Q*B;
Za1 = inv(Ga'*inv(FI)*Ga)*Ga'*inv(FI)*h';
CovZa = inv(Ga'*inv(FI)*Ga);
sB = eye(3);
for i = 1: 3,
    sB(i,i) = Za1(i);
end
sFI = 4*sB*CovZa*sB;
sGa = [1, 0; 0, 1; 1, 1];
sh  = [Za1(1)^2; Za1(2)^2; Za1(3)^2];
Za2 = inv(sGa'*inv(sFI)*sGa)*sGa'*inv(sFI)*sh;
Za = sqrt(Za2);
SUM=SUM+Za;
end
SUM=SUM/10