chansuanfa定位.doc

实现无线定位中的CHAN算法function X = ChanAlgorithm(BSN, MSP, Radius, Noise)%CHANALGORITHM 本函数用于实现无线定位中的CHAN算法% - BSN为基站个数，3 BSN 1, error(Too many output arguments.);endif nargin4, error(Wrong number of input arguments.);end% 算法开始：BS = Radius*NetworkTop(BSN);MS = Radius*MSP;% 噪声功率：Q = eye(BSN-1);% 第一次LS：% RiK1 = 0;for i = 1: BSN, R0(i) = sqrt(BS(1,i) - MS(1)2 + (BS(2,i) - MS(2)2);endfor i = 1: BSN-1, R(i) = R0(i+1) - R0(1) + Noise*randn(1); K(i) = BS(1,i+1)2 + BS(2,i+1)2;end% Gafor i = 1: BSN-1, Ga(i,1) = -BS(1, i+1); Ga(i,2) = -BS(2, i+1); Ga(i,3) = -R(i);end% hfor i = 1: BSN-1, h(i) = 0.5*(R(i)2 - K(i) + K1);end% 由（14b）给出B的估计值：Za0 = inv(Ga*inv(Q)*Ga)*Ga*inv(Q)*h;% 利用这个粗略估计值计算B：B = eye(BSN-1);for i = 1: BSN-1, B(i,i) = sqrt(BS(1,i+1) - Za0(1)2 + (BS(2,i+1) - Za0(2)2);end% FI:FI = B*Q*B;% 第一次LS结果：Za1 = inv(Ga*inv(FI)*Ga)*Ga*inv(FI)*h;if Za1(3) 0, Za1(3) = abs(Za1(3);% Za1(3) = 0;end%*% 第二次LS：% 第一次LS结果的协方差：CovZa = inv(Ga*inv(FI)*Ga);% sB：sB = eye(3);for i = 1: 3, sB(i,i) = Za1(i);end% sFI：sFI = 4*sB*CovZa*sB;% sGa：sGa = 1, 0; 0, 1; 1, 1;% shsh= Za1(1)2; Za1(2)2; Za1(3)2;% 第二次LS结果：Za2 = inv(sGa*inv(sFI)*sGa)*sGa*inv(sFI)*sh;% Za = sqrt(abs(Za2);Za = sqrt(Za2);% 输出:% if Za1(1) 0,% out1 = -Za(1);% else% out1 = Za(1);% end% if Za2(1) 0,% out2 = -Za(2);% else% out2 = Za(2);% end% % out = out1;out2;out = abs(Za)