fangalgorithm.m

这是跳频的源程序,我的毕业设计做的,希望对大家有帮助和启发

function X = FangAlgorithm(MSP, R)
%  本程序实现无线定位中的FANG算法
%  FANGALGORITHM
%    参数说明：
%       MSP：移动台相对位置；
%       R：  小区半径；
%       X：  输出移动台的估计位置。
%  Also see: FangAlgorithm.

%  Designed by 李金伦, SWJTU, 2004.12.15 

%  输入参数检测:
    if  nargout>1,
        error('Too many output arguments.');
    end
    if nargin~=2,
        error('Wrong number of input arguments.');
    end
        flag = size(MSP);
    if flag(1)~=1 | flag(2)~=2,
        error('Wrong position vector!');
    end

%  输入：
%     R = 5000; %单位：km
%     BS = [0, sqrt(3)*R, 0.5*sqrt(3)*R;
%           0, 0,         1.5*R];
%     MSP = [0.5, 0.8];
    
%  初始参数：
    MS  = R*MSP;
    BS  = [0, sqrt(3)*R, 0.5*sqrt(3)*R;
          0, 0,         1.5*R];
    c   = 3*10^8; %单位: m/s
    DEV = 3/(c*c);
    %
    R1 = sqrt(MS(1)*MS(1) + MS(2)*MS(2));
    R2 = sqrt((BS(1,2) - MS(1))*(BS(1,2) - MS(1)) + (BS(2,2) - MS(2))*(BS(2,2) - MS(2)));
    R3 = sqrt((BS(1,3) - MS(1))*(BS(1,3) - MS(1)) + (BS(2,3) - MS(2))*(BS(2,3) - MS(2)));
    % noise
    n1 = c*sqrt(DEV)*randn(1);
    n2 = c*sqrt(DEV)*randn(1);
    %
    R21 = R2 - R1 + n1;
    R31 = R3 - R1 + n2;
    %
    g = ((R31*BS(1,2))/R21 - BS(1,3))/BS(2,3);
    h = (BS(1,3)*BS(1,3) + BS(2,3)*BS(2,3) - R31*R31 + R31*R21*(1 - (BS(1,2)/R21)*(BS(1,2)/R21)))/(2*BS(2,3));
    d = -((1 - (BS(1,2)/R21)*(BS(1,2)/R21)) + g*g);
    e = BS(1,2)*(1 - (BS(1,2)/R21)*(BS(1,2)/R21)) - 2*g*h;
    f = (R21*R21/4)*(1-(BS(1,2)/R21)*(BS(1,2)/R21))*(1-(BS(1,2)/R21)*(BS(1,2)/R21)) - h*h;
    
%  输出：
    EMSX = (-e - sqrt(e*e - 4*d*f))/(2*d);
    EMSY = g*EMSX + h;
    EMS = [EMSX, EMSY];
    if nargout == 1,
        X = EMS;
    else
        disp(EMS);
    end