mainpro.m

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

    [vAntPos_sm,receiver_clock]=SinglePointGPS(vCorrectedCodeSmoothed,mSatPosCur,vSatUsed,vInitPos);
    mAntXYZAll(1,epoch,1:3)=vAntPos_sm;

    str=sprintf('Single point positioning  %2.0f%%%', epoch/totalepoch*100);
    waitbar(epoch/totalepoch,waitbar_sp,str)
end %% End of loop for epochs
close (waitbar_sp)

%% ********************************************************************
%%                             Differential  positioning
%% ********************************************************************
waitbar_DGPS = waitbar(0,'DGPS Processing');
for epoch=1:1:totalepoch, %%  Loop for epoch
    %% ~~~~~~~~~~~~~~~~ ~~~~~~~~~~~~~~~~~~~~~~
    %% Get the data of the master antenna
    %%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
    %% Data of master antenna at this epoch
    vSatIDMaster=find(squeeze(squeeze(mSat(1,:,epoch)))~=0);
    vCodeMaster=mCode(1,:,epoch);
    vPhaseMaster=mPhase(1,:,epoch);
    vCodeSmoothedMaster=mCodeSmoothed(1,:,epoch);
    mSatPosMaster=squeeze(mSatPos(1,epoch,:,1:3));
    %% Estimated positiong of master antenna from single point positioning
    vMasterPosition=squeeze(mAntXYZAll(1, epoch,1:3));

    for antid=2:1:totalantenna,
        %% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
        %% Get the data of slave antenna
        %% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
        clear mSlaveSatID   vElevationAngle
        %% Satellite IDs for slave antenna given in a compact format
        vSatIDSlave=find(squeeze(squeeze(mSat(antid,:,epoch)))~=0);
        %% Code data of slave antenna (compact)
        vCodeSlave=mCode(antid,:,epoch);
        vPhaseSlave=mPhase(antid,:,epoch);
        vCodeSmoothedSlave=mCodeSmoothed(antid,:,epoch);
        %% Search for the common satellites of master and this slave antenna
        vCommonSatID= intersect(vSatIDSlave,vSatIDMaster);
        satnum_common=length(vCommonSatID);
        %% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
        %% Calculate the satellite position for slave antenna
        %% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
        for i=1:1:satnum_common,
            satid=vCommonSatID(i); %% Satellite PRN
            %% Find a proper ephemerides paragraph
            time=mCommonEpoch(epoch);  %% "Second of the week " of current epoch
            eph_epoch=SeekEpheEpoch(time,satid);
            %% "True" distance for master antenna
            dis_true_master=norm(vMasterPosition-mSatPosMaster(satid,:)');
            %%  "True" distance for slave antenna, from single-difference
            %%  positioning
            dis_true_slave=dis_true_master-vCodeMaster(satid)+vCodeSlave(satid);
            %% Estimate the transit time of GPS signal.
            transit_time=dis_true_slave/sol;
            %% Calculate the satellite position
            [vSatPosNoRotation ,sat_clock_err,sat_group_delay]= GetSatPosECEF(satid,eph_epoch,time,transit_time );
            %% Implement the earth rotation to the satellite
            vSatXYZ=EarthRotation(transit_time,vSatPosNoRotation);
            %% Satellite position of slave antenna
            mSatPosSlave(satid,:)=vSatXYZ;
        end
        %% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
        %% Find the data from common satellites to construct baselines
        %% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
        %% Get the key satellite derived in the single point positioning
        key_sat_id=vKeySat(epoch);
        %% Pick up the common visible satellites for DGPS processing
        mCommonSatPosSlave=zeros(totalGPSsatellite,3);
        mCommonSatPosMaster=zeros(totalGPSsatellite,3);
        mCommonSatPosSlave(vCommonSatID,:)=mSatPosSlave(vCommonSatID,:);
        mCommonSatPosMaster(vCommonSatID,:)=mSatPosMaster(vCommonSatID,:);
        %% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
        %%                              differential positining
        %%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
        vSlavePosition_code =DGPS_code(mCommonSatPosMaster,mCommonSatPosSlave,vCodeSmoothedMaster,....
            vCodeSmoothedSlave, key_sat_id,vCommonSatID,vMasterPosition   );
        %% Record the slave antenna coordinates and baselines
        %% for further attitude determination processing
        mAntXYZAll(antid,epoch,1:3)=vSlavePosition_code;
        if num_baseline~=0,
            %% Calculate the 3D error in the estimated baselines.
            if antid==2,
                baseline_true=b12;
            elseif antid==3,
                baseline_true=b13;
            elseif antid>3,
                baseline_true=mBaselineAdd(antid-3,1);
            end
            mBaselineEst(epoch,antid)=norm(vSlavePosition_code-vMasterPosition');
            mBaselineErr( epoch,antid )=mBaselineEst(epoch,antid)-baseline_true;
        end
        %% Save the local level coordinates of the slave antenna
        mAntENUAll(antid,epoch,1:3)=ECEF2ENU(vSlavePosition_code,vMasterPosition);
    end
    str=sprintf('Differential processing  %2.0f%%%', epoch/totalepoch*100  );
    waitbar(epoch/totalepoch,waitbar_DGPS,str)
end %% End DGPS
close (waitbar_DGPS)

%% Since the results of 1st epoch are always of low quality, we therefore
%% delete this value.
if num_baseline~=0,
    mBaselineEst(1,:)=[];
    mBaselineErr(1,:)=[];
end
mAntXYZAll(:,1,:)=[];
mAntENUAll(:,1,:)=[];
totalepoch=totalepoch-1;

%% If baselines are identified, then display the baseline error
if num_baseline~=0,
    %% Show baselines errors
    for antid=2:1:totalantenna,
        vBaselineErr=mBaselineErr(:,antid);
        title_str=sprintf('Estimated baseline 3D error between antenna 1 and %d',antid);
        DisplayBaselineErr(vBaselineErr,title_str);
    end
end

%% ###################################################
%% ###################################################
%%                              Attitude determination
%% ###################################################
%% ###################################################


%% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
%%                          Direct computation
%% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
waitbar_attitude1=waitbar(0,'Direct attitude computation');
for epoch=1:1:totalepoch,
    %% Local level frame for slave antennas
    %% only consider the antenna 1-3 and ignore the others
    for antid=1:1:3,
        mAntLocal(antid,1:3)=squeeze(mAntENUAll(antid,epoch,1:3));
    end
    %% Attitude determination by direct computation
    %% Antenna 1 is [0 0 0]
    %% Use only the local level coordinate of antenna 2 and 3
    [yaw_deg_direct,roll_deg_direct,pitch_deg_direct]=AD_Direct(mAntLocal(2,:),mAntLocal(3,:));
    %% Save the data
    mAttitudeRecordDirect(epoch,1:3)=[yaw_deg_direct roll_deg_direct pitch_deg_direct ];

    str=sprintf('Direct attitude computation %2.0f%%%', epoch/totalepoch*100);
    waitbar(epoch/totalepoch,waitbar_attitude1,str)
end
close (waitbar_attitude1)
%% Demonstrate the result
DrawAttitudeFigure(mAttitudeRecordDirect, 'Attitude parameters based on direct computation')
SaveResultInFile(mAttitudeRecordDirect,mEpochStr, 'Result from direct attitude determination using CODE','Results.txt')
%% Once no baseline identified, then exit.
if num_baseline==0, return;  end

%% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
%%            Attitude computation by LSQ
%% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

%% ~~~~~ Step 1: Calculate the antenna body frame ~~~~~
mAntBody=GetBodyCoordinates(b12,b13,b23,mBaselineAdd);
%% Knowing only the magnitude of baselines we can not specify the Z value
%% of the antenna body frame for the antenna 4,5,6. We should employ the
%% calculated attitude parameters and the local level coordinate to derive
%% the estimated antenna body frame. The estimated antenna body frame will
%% help the determination of the Z value.
if totalantenna>=4, 
    %% Choose the first 50 epoch to calculate the antenna body frame based on
    %% the attitude parameters obtained from the direct computation
    if totalepoch>50, testepoch=50; else testepoch=totalepoch; end
    %% Now get the sign of the Z value in the antenna body frame by using the
    %% relationship: Body Frame=Rotation matrix* Local level frame, where the
    %% rotation matrix is obtained from the attitude parameters based on the
    %% direct attitude computation.
    for epoch=1:1:testepoch,
        mAntLocal =squeeze(mAntENUAll(:,epoch,1:3));
        mR=GetCombinedRotationMatrix(mAttitudeRecordDirect(epoch,1),...
            mAttitudeRecordDirect(epoch,2),mAttitudeRecordDirect(epoch,3));
        mBodyTemp=(mR*mAntLocal')';
        for i=4:1:totalantenna,
            mSign(epoch,i)=mBodyTemp(i,3)/abs(mBodyTemp(i,3));
        end
    end
    vSumSign=sum(mSign);
    %% If 80% of the Z values are of the same sign, we then consider it as the
    %% correct sign. Otherwise the user has to specify the sign(s).
    for antid=4:1:totalantenna,
        if abs(vSumSign(antid)/testepoch)>0.8,
            sign_z=vSumSign(antid)/abs(vSumSign(antid));
            mAntBody(antid,3)=abs(mAntBody(antid,3))*sign_z;
        else
            str=sprintf('Can not determine the Z value of antenna body frame of antenna %d, use default value instead',antid);
            msgbox(str);
        end
    end
end

%% ~~~~~ Step 2: Start attitude calculation LSQ ~~~~~
waitbar_attitude2 = waitbar(0,'Attitude determination using LSQ estimation');
for epoch=1:1:totalepoch,
    %% Local level frame for slave antennas
    mAntLocal =squeeze(mAntENUAll(:,epoch,1:3));
    %% Attitude determination by applying LSQ
    [yaw_deg_lsq,roll_deg_lsq,pitch_deg_lsq]=AD_LSQ(mAntLocal ,mAntBody ,0,0,0);
    %% Save the data
    mAttitudeRecordLSQLinear(epoch,1:3)=[yaw_deg_lsq roll_deg_lsq pitch_deg_lsq];
    
    str=sprintf('Least squares attitude determination %2.0f%%%', epoch/totalepoch*100);
    waitbar(epoch/totalepoch,waitbar_attitude2,str)
end
%%
close (waitbar_attitude2)
DrawAttitudeFigure(mAttitudeRecordLSQLinear,'Attitude parameters based on linearized least squares adjustment')
SaveResultInFile(mAttitudeRecordLSQLinear,mEpochStr, 'Result from least squares estimation using CODE','Results.txt')