fang算法的仿真

function X = FangAlgorithm（MSP R）
%  本程序实现无线定位中的FANG算法
%  FANGALGORITHM
%    参数说明：
%       MSP：移动台相对位置；
%       R：  小区半径；
%       X：  输出移动台的估计位置。
%  Also see: FangAlgorithm.
 %  输入参数检测:
    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 = 2000; %单位：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（12） - MS（1））*（B