getbodycoordinates.m

利用gps多天线载波相位技术

%%========================================
%%     Toolbox for attitude determination
%%     Zhen Dai
%%     [e-mail]
%%     ZESS, University of Siegen, Germany
%%     Last Modified  : 1.Sep.2008
%%========================================
%% Functions:
%%      Calculate the antenna body frame of each antenna
%%      where  antenna 1 is the master antenna, antenna 2 is the second
%%      slave antenna,... etc. 
%% Input parameters:
%%      l12 -> baseline between antenna 1 and 2
%%      l13 -> baseline between antenna 1 and 3
%%      l23 -> baseline between antenna 2 and 3
%%      mBaseline -> baseline between the additional antenna and the
%%                              antenna 1 2 3.
%%                              mBaseline(1,1) is between ant  4 and 1
%%                              mBaseline(1,2) is between ant  4 and 2
%%                              mBaseline(1,3) is between ant  4 and 3
%%                              mBaseline(2,1) is between ant  5 and 1
%%                              mBaseline(3,2) is between ant  6 and 2
%%                              ................
%% Output:
%%      mAntXYZ_body-> Antenna body frame
%% Remarks:
%%      Apply the least squares adjustment, like single point positioning.

function mAntXYZ_body = GetBodyCoordinates(l12,l13,l23,mBaseline) 

totalantenna=size(mBaseline,1);
alfa=acos((l23^2-l12^2-l13^2)/(-2*l12*l13));
%% The following antennas define the body frame
mAntXYZ_body(1,:)=[0 0 0]; %% Master antenna
mAntXYZ_body(2,:)=[0 l12 0]; %% Slave antenna 1
mAntXYZ_body(3,:)=[l13*sin(alfa) l13*cos(alfa) 0]; %% Slave antenna 2

%% Project the additional antenna into the antenna body frame
for antid=4:1:3+totalantenna, %% For each additional slave antenna
    vRi=mBaseline(antid-3,:);
    iLSQIteration=0;
    %% initial position can not be 0 otherwise  it causes sigularity due to
    %% the 0 values in the master antenna coordinate
    vXYZ0=[0.1 0.1 0.1];
    %% Start the adjustment 
    while(iLSQIteration<11),        
        for i=1:1:3,
            vRou_i0(i)=norm(vXYZ0-mAntXYZ_body(i,:));
            vL(i)=vRi(i)-vRou_i0(i);
        end
        for i=1:1:3, %% For antenna 
            for j=1:1:3, %% From X to Z
                mA(i,j)=[-(mAntXYZ_body(i,j)-vXYZ0(j))/vRou_i0(i)];
            end
        end
        %% Update the unknown values
        vDeltaX=inv(mA'*mA)*mA'*vL';
        if (norm(vDeltaX)<1e-5);
            mAntXYZ_body(antid,1:3)=vXYZ0;
            break;
        end
        vXYZ0=vXYZ0+vDeltaX';
        iLSQIteration=iLSQIteration+1;
    end
    %% If iteration is greater than 10
    %% It might be a singularity problem
    if iLSQIteration>10,
        msgbox('The adjustment in the calculation of antenna body frame can not converge!!!')
        error('Program terminated')
    end
end %% For the next slave antenna