mainpro.m

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

%%========================================
%%     Toolbox for attitude determination
%%     Zhen Dai
%%     [e-mail]
%%     ZESS, University of Siegen, Germany
%%     Last Modified  : 1.Sep.2008
%%========================================
%% Functions:
%%      Main program for attitude determination using C/A code.
%% Input parameters:
%%      -------  From data file DataSatRecord ------:
%%      mCommonEpoch
%%          -> Synchronized epochs for all antennas
%%      mEpochStr
%%          -> Epochs expressed in string
%%      mMeasurement
%%          -> Code data, phase data, satellite visibility
%%      maskangle
%%          -> Satellite elevation mask angle (cut-off angle)
%%      smoothing_interval
%%          -> Smoothing inteval, i.e. the length of accumulated data
%%               for HATCH smoothing
%%      mBaseline
%%          -> Magnitude of the baselines
%%
%%      -------  From data file RinexNav ------:
%%      mEphemerides
%%          -> Ephemerides parameters needed for the calculation of
%%               satellite positions and the acquirement of  some
%%               corrections
%%      mEphTime
%%          -> Time tag of each ephemerides. It is used to search for the
%%               proper ephemerides for a specific satelilte at  a specific time
%% Output:
%%      mAttitudeRecordLSQLinear
%%          -> Attitude parameters from least squares estimation
%%      mAttitudeRecordDirect
%%          -> Attitude parameters from direct attitude computation
%%  Remarks:
%%      1. Result will be illustrated and also saved into the data file "Results.txt"


clear all
clc

%% ********************************************************************
%%                                       Constant Definition
%% ********************************************************************
sol=299792458; %% Speed of light
freq1=1575.42e6; %%frequency of L1
lambda1=sol/(freq1);%% lambda on L1
totalGPSsatellite=32; %% Assuming that there are maximal 32 GPS satellites

%% Some global variables for the ephemerides
global mEphTime mEphemerides

%% ********************************************************************
%%                                     Load Measurement
%% ********************************************************************
load DataSatRecord
load RinexNav

%% ********************************************************************
%%                                      Initialization
%% ********************************************************************
totalepoch=size(mCommonEpoch,1); %% how many synchonized epochs to be proessed
totalantenna=size(mMeasurement,1); %% how many antenna/receiver pairs
%% If baselines are identified, then extract them.
if num_baseline~=0
    % Baseline 1-2 1-3 2-3
    b12=mBaseline(1,1);
    b13=mBaseline(2,1);
    b23=mBaseline(3,1);
    % Baseline 1-4 2-4 3-4,  1-5 2-5 3-5, 1-6 2-6 3-6
    mBaselineAdd=[];
    for antid=4:1:totalantenna,
        mBaselineAdd(antid-3,1:3)= mBaseline(antid,1:3);
    end
else
    %% if no baseline given, then we consider the first 3 antennas
    totalantenna=3; 
end


%% some matrix for recording the estimated  attitude parameters
%% 1st epoch will be deleted
mAttitudeRecordDirect=zeros(totalepoch-1,3);
mAttitudeRecordLSQMatrix=zeros(totalepoch-1,3);
mAttitudeRecordLSQLinear=zeros(totalepoch-1,3);

%% Initilize the matrix recording measurement
mPhase=zeros(totalantenna,totalGPSsatellite,totalepoch);
mCode=zeros(totalantenna,totalGPSsatellite,totalepoch);
mCodeSmoothed=zeros(totalantenna,totalGPSsatellite,totalepoch);
mSat=zeros(totalantenna,totalGPSsatellite,totalepoch);
mSatPos=zeros(totalantenna,totalGPSsatellite,totalepoch,3);
vKeySat=zeros(totalepoch,1);

%% ********************************************************************
%%                                       Data extraction
%% ********************************************************************
%% Extract the measurement and satellite information to
%%              mPhase : Phase data
%%              mCode: C/A Code data
%%              mSat: Satellite visibility
%% For each matrix:
%% 1st dimension : receiver ID( 1~n), No.1 is master antenna
%% 2nd dimension: satelite ID (1~totalGPSsatellite of 32)
%% 3rd dimension : epoch (1~totalepoch)
%% ********************************************************************

for epoch=1:1:totalepoch,%% Loop for epochs
    for antid=1:1:totalantenna,    %% Loop for receiver
        %% Satellites visibility. 1=visible
        mSat(antid,1:totalGPSsatellite,epoch)=squeeze(squeeze(mMeasurement(antid,epoch,1,:)));
        for k=1:1:totalGPSsatellite, %% Loop for satelltes of current epoch
            if mSat(antid,k,epoch)==1,
                mPhase(antid,k,epoch)=mMeasurement(antid,epoch,3,k);%% Phase data
                mCode(antid,k,epoch)=mMeasurement(antid,epoch,2,k); %% Code range without smoothing
            end
        end
    end
end
%% ********************************************************************
%%                       Smoothing code by carrier phase
%% ********************************************************************
mCodeSmoothed=zeros(totalantenna,totalGPSsatellite,totalepoch);
for i=1:1:totalantenna,
    clear mCodeTemp
    mRawCode=squeeze(mCode(i,:,:));
    mRawPhase=squeeze(mPhase(i,:,:));
    mCodeSmoothed(i,:,:)=Smoothing(mRawCode,mRawPhase,smoothing_interval);
end
%% ********************************************************************
%%            Single point positioning for master antenna
%% ********************************************************************
waitbar_sp = waitbar(0,'Single point positioning......');
%% Satellite clock correction
vSatClockCor=zeros(1, totalGPSsatellite);
%% Satellite position in ECEF
%% Used for single point positioning , as well as for future differential
%% positioning
mSatPos=zeros(totalantenna,totalepoch,totalGPSsatellite,3);

for epoch = 1:1:totalepoch, %%Loop for epoch
    %% Clear temporary data for master antenna
    mSatPosCur=zeros(totalGPSsatellite,3); %% Satellite positions of current epoch
    vCodeCur=zeros(1,totalGPSsatellite); %% Code data of current epoch
    vPhaseCur=zeros(1,totalGPSsatellite); %% Phase data of current epoch
    vCodeSmoothedCur=zeros(1,totalGPSsatellite); %% Smoothed code data of current epoch

    clear vSatUsed vElevationAngle
    %% Extract the satellite info and measurement for master antenna
    vSatIDs=find(squeeze(mSat(1,1:totalGPSsatellite,epoch))~=0);%% visible satellites PRNs of current epoch
    vCodeCur=mCode(1,:,epoch);
    vPhaseCur=mPhase(1,:,epoch);
    vCodeSmoothedCur=mCodeSmoothed(1,:,epoch);
    %% Initialization for the first epoch
    if epoch==1,
        vInitPos=[1 1 1]; %% The starting point of adjustment
        receiver_clock=0; %% Receiver clock error
        vEpheEopchs=zeros(totalGPSsatellite,1); %% the closet ephemerides epoch
        vGroupDelay=zeros(1, totalGPSsatellite); %% Group delay error
        vSatClockCor=zeros(1, totalGPSsatellite);%% Satellite clock error
    else
        %% if not the first epoch, the LSQ adjustment starts from the position of last epoch
        vInitPos(1:3)=mAntXYZAll(1,epoch-1,1:3);
    end
    time=mCommonEpoch(epoch);  %% "Second of the week " of current epoch
    %% Calculate the coordinate of satellites
    for i=1:1:length(vSatIDs),
        satid=vSatIDs(i); %% Satellite PRN
        %% Find a proper ephemerides paragraph, i.e. the ephemerides
        %% epoch closet to the current GPS measurement
        vEpheEopchs(satid)=SeekEpheEpoch(time,satid);
        %% Estimate the transit time of GPS signal.
        transit_time=vCodeCur(satid)/sol+vSatClockCor(satid)-receiver_clock-vGroupDelay(satid);
        %% Calculate the satellite position
        [vSatPosNoRotation ,vSatClockCor(satid),vGroupDelay(satid)]= GetSatPosECEF(satid,vEpheEopchs(satid),time,transit_time);
        %% Filter out the satellite with low elevation angle
        vSatENU= ECEF2ENU(vSatPosNoRotation,vInitPos');
        vElevationAngle(i) = atan(vSatENU(3)/norm(vSatENU(1:2)))*180/pi;
        if (vElevationAngle(i) < maskangle) && (epoch~=1),
            low_angle_id=satid;
            %% Delete this satellite from the matrix having
            %% satellite PRNs, this satellite will then not be used
            %% any more in the following parts.
            mSat(1,satid,epoch)=0;
            continue; %% go to process the next satellite
        end
        %% Correct the code data by removing receiver clock error,
        %% satellite clock error, group delay for smoothed and raw code data
        vCorrectedCodeSmoothed(satid)=vCodeSmoothedCur(satid)-receiver_clock*sol+...
            vSatClockCor(satid)*sol-vGroupDelay(satid)*sol;
        vCorrectedCode(satid)=vCodeCur(satid)-receiver_clock*sol+...
            vSatClockCor(satid)*sol-vGroupDelay(satid)*sol;
        %% Implement the earth rotation to the satellite
        vSatXYZ=EarthRotation(transit_time,vSatPosNoRotation);
        %% Save the satellite positions of current epoch for single point positioning
        mSatPosCur(satid,1:3)= vSatXYZ;
        %% Save the satellite positions for future DGPS
        mSatPos(1,epoch,satid,1:3)= vSatXYZ;
    end  %% End of loop for satellites
    %% Satellites used for the positioning at current epoch
    vSatUsed=find(squeeze(squeeze(mSat(1,:,epoch)))~=0);
    %% For master antenna, choose the key satellite for future DGPS
    %% Key satellite  implies the satellite having the highest
    %% elevation angle herein.
    %% For the post-processing and for the ambiguity resolution, the
    %% key satellite could also be chosen as the satellite having the
    %% longest visibility within a specific time span.
    max_elevation=max(vElevationAngle);
    vKeySat(epoch)=vSatIDs(find(vElevationAngle==max_elevation));
    %% Single point positioning using smoothed code data