无线定位中的FANG算法

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

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（12） - MS（1））*（BS（12） - MS（1）） + （BS（22） - MS（2））*（BS（22） - MS（2）））;
    R3 = sqrt（（BS（13） - MS（1））*（BS（13） - MS（1）） + （BS（23） - MS（2））*（BS（23） - 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（12））/R21 - BS（13））/BS（23）;
    h = （BS（13）*BS（13） + BS（23）*BS（23） - R31*R31 + R31*R21*（1 - （BS（12）/R21）*（BS（12）/R21）））/（2*BS（23））;
    d = -（（1 - （BS（12）/R21）*（BS（12）/R21）） + g*g）;
    e = BS（12）*（1 - （BS（12）/R21）*（BS（12）/R21）） - 2*g*h;
    f = （R21*R21/4）*（1-（BS（12）/R21）*（BS（12）/R21））*（1-（BS（12）/R21）*（BS（12）/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

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

     文件       1789  2005-05-26 10:49  FangAlgorithm.m

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

                 2007                    2