chan算法详细代码

chan算法详细代码

function X = ChanAlgorithm（BSN MSP Radius Noise）
%CHANALGORITHM 本函数用于实现无线定位中的CHAN算法
%               - BSN  为基站个数，3 < BSN <= 7；
%               - MSP  为移动台的初始位置 MSx MSy均为[01]之间的数；
%                      特别要注意服务小区与MS之间的关系，MS的位置不能越界。
%               - Noise 测距误差方差。
%               - R    为小区半径，单位（meter）；
%               - X    为移动台经算法处理后的位置.
%See also: ChanAlgorithm.m



%   参数检查：
if  nargout>1
    error（‘Too many output arguments.‘）;
end
if nargin<2 | nargin>4
    error（‘Wrong number of input arguments.‘）;
end


% 算法开始：
BS = Radius*NetworkTop（BSN）;
MS = Radius*MSP;

% 噪声功率：
Q = eye（BSN-1）;

% 第一次LS：
% Ri
K1 = 0;
for i = 1: BSN
    R0（i） = sqrt（（BS（1i） - MS（1））^2 + （BS（2i） - MS（2））^2）;
end

for i = 1: BSN-1
    R（i） = R0（i+1） - R0（1） + Noise*randn（1）;
    K（i） = BS（1i+1）^2 + BS（2i+1）^2;
end

% Ga
for i = 1: BSN-1
    Ga（i1） = -BS（1 i+1）;
    Ga（i2） = -BS（2 i+1）;
    Ga（i3） = -R（i）;
end

% h
for 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（ii） = sqrt（（BS（1i+1） - Za0（1））^2 + （BS（2i+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（ii） = Za1（i）;
end

% sFI：
sFI = 4*sB*CovZa*sB;

% sGa：
sGa = [1 0; 0 1; 1 1];

% sh
sh  = [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）;

% out = Za;

if nargout == 1
    X = out;
elseif nargout == 0
    disp（out）;
end

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       1223  2007-12-09 07:19  Chan算法\41695026ChanAlgorithm1.zip

     文件       1888  2007-09-04 09:02  Chan算法\ChanAlgorithm1.m

     文件       1464  2007-12-09 07:17  Chan算法\95302917ChanAlgorithm.rar

     文件       2238  2007-12-09 10:21  Chan算法\ChanAlgorithm.m

     文件       2297  2007-12-09 10:20  Chan算法\ChanAlgorithm.asv

     文件       1880  2007-12-09 11:00  Chan算法\ChanAlgorithm1.asv

     目录          0  2007-12-09 07:28  Chan算法

----------- ---------  ---------- -----  ----

                11208                    8