recpos23.m

GPS导航电文相关的计算程序

function[phi,lambda] = recpos23(filename)
% RECPOS_923:   Least-squares searching for receiver position. Given 
%               4 or more pseudoranges and data from ephemerides. 
%               Idea to this script originates from Clyde C. Goad
%
% Made by Kai Borre 04-19-96
% Copyright (c) by Kai Borre
% Revision 1.0 Date: 1997/09/24
% Modified by group 923 AAU, Date: 1998/11/25
% Improved by: 1) A better and faster intial guess taking 
%                 care of the problem near the Greenwich meridian
%              2) A simpler data input format
%              3) Using rotation outside loops
%              4) Using a faster/converging grid + an optimized
%                 interation bound
%              5) Removing all sin/cos ops. from code, only those
%                 which can be tabelized remain
%              6) Moving asin ops. out of loops leaving only two
%                 asins left 
% Using data: filename (ex. recpos_input_ohio.mat)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Load file for Space vehicle positions, pseudoranges and 
% the correction value tcorr 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
format compact
str = 'load*';                  % these four lines loads a file 
str = strcat(str,filename);                     
str = strrep(str,'*',' ');      
eval(str);                      
disp(ID);                       % display name of the datafile

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Define constants and initialize variables % 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Grid parameters
iterations = 38;     % iterations in main loop, signifies precision
scalediv = 1.5;      % scale division factor  
ndiv = 1;            % no. of slices in inclination of spherical cap 
azimuth_angle = 60;  % angle to slice spherical cap in azimuth 
last_angle = 300;    % maximum angle in azimuth
scale = 70;          % initialize scale (deg from zenith to cap edge) 

% Universal and global constants 
dtr = pi/180;                   % conversion factor between degrees and radians
vlight = 299792458;	           % vacuum speed of light in m/s
Omegae_dot = 7.292115147e-5;    % Earth rotation rate in rad/s
semi_major_axis = 6378137;      % Earth semi major axis       
flattening = 1/298.257223563;   % Earth flattening
esq=(2-flattening)*flattening;  % Earth eccentricity squared (6.69437999014e-3)

% Inverse constants, used to avoid division
inv_scalediv = 1/scalediv;     % inverse of scalediv (used only with psi LUT's) 
inv_ndiv = 1/ndiv;             % inverse of ndiv     (used only with psi LUT's) 
inv_vlight = 1/vlight;         % inverse of vlight     
inv_dtr = 180/pi;              % inverse of dtr


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Generate lookup tables for the sine and cosine of alpha and psi, respectively %%%%%%%%% 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
alpha_counter=0;
for alpha = 0:azimuth_angle:last_angle,    % generate cos/sin table for alpha (2*6 values)
alpha_counter = alpha_counter+1;           % inc alpha_counter
cos_alpha(alpha_counter) = cos(alpha*dtr);      
sin_alpha(alpha_counter) = sin(alpha*dtr);      
end

psi_counter=0;
for iter=1:iterations,
  for b = 1:ndiv,                               % generate cos/sin table for psi (2*38 values)
    psi_counter=psi_counter+1;                  % inc psi_counter
    psi = b*scale*inv_ndiv;                     % angle to zenith in spherical cap
    cos_psi(psi_counter) = cos(psi*dtr);        % compute sine and cosine of psi
    sin_psi(psi_counter) = sin(psi*dtr);        %
    scale=scale*inv_scalediv;                   % reduce scale
  end;
end;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Compute a start coordinate to get the algorithm going % 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Space vehicle positions are known in the ECEF system in form of 
% Carthesian coordinates (X,Y,Z). The pseudoranges and tcorr are 
% known as well.

% Initialize phi_old & lambda_old thus establishing the initial 
% receiver position estimate using the known Carthesian coordinates
% (phi_old,lambda_old) signifies the initial point (zenith) of 
% the present spherical cap throughout the algorithm

% compute mass mean of the SV positions and find the appropriate ECEF octant
signs=sign(mean(XS'));                          % define a vector of signums
signs(find(signs)==0)=1;                        % repair sign if input==0
% cos(phi) is constantly sqrt(1/2) and sin(phi) has same sign as Z
% cos(lambda) has same sign as X and sin(lambda) has same sign as Y 
cos_phi_old = sqrt(1/2);
if signs(3)==1, sin_phi_old = sqrt(1/2);    else sin_phi_old = -sqrt(1/2);    end;
if signs(1)==1, cos_lambda_old = sqrt(1/2); else cos_lambda_old = -sqrt(1/2); end;
if signs(2)==1, sin_lambda_old = sqrt(1/2); else sin_lambda_old = -sqrt(1/2); end;

% Convert sine and cosine of initial position estimate to 
% Carthesian ECEFs. Beginning of simplified FRGEOD.M unfolded
% (convert point on sphere to point on ellipsoid)
% compute radius of curvature in prime vertical
  N_phi = semi_major_axis/sqrt(1-esq*sin_phi_old*sin_phi_old);
  P = N_phi*cos_phi_old;                        % P is distance from Z axis
  init_z = N_phi*(1-esq)*sin_phi_old;           % initial estimate for z 
  init_x = P*cos_lambda_old;                    % initial estimate for x
  init_y = P*sin_lambda_old;                    % initial estimate for y 
% End of simplified FRGEOD.M unfolded

% Initialize Old_Sum as the squared residual for the initial receiver 
% position estimate
m = length(pseudoranges);          % find number of SVs
for t = 1:m
  sat_clock(t) = tcorr(t)*vlight;  % compute tcorrs influence on SV clocks
  cal_one_way(1,t) = norm(XS(:,t)-[init_x,init_y,init_z]'); % find distance to initial position
  one_way_res(1,t) = pseudoranges(t,2)-cal_one_way(1,t)+sat_clock(t);
end;
resid_t = one_way_res(1,:)-one_way_res(1,1);    % compute residual for initial position
Old_Sum = resid_t*resid_t';                     % initialize Old_Sum for initial position

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Main part of code, with the Least-squares error search %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for iter = 1:iterations,                        % begining of the main loop 
  sin_phi0 = sin_phi_old;                       % update sin_phi0 and cos_phi0
  cos_phi0 = cos_phi_old;                       %  
  for b = 1:ndiv,                               % begining of slicing loop (inclination)
    psi_index=iter*ndiv+b-1;                    % psi_index=iter when ndiv=1
    for alpha_index = 1:alpha_counter,          % begining of slicing loop (azimuth)
                                                % update grid point using spherical triangle
      sin_phi2 = sin_phi0*cos_psi(psi_index)+cos_phi0*sin_psi(psi_index)*cos_alpha(alpha_index);
      cos_phi2 = sqrt(1-sin_phi2*sin_phi2);     % compute sin and cos for new possible phi
      if cos_phi2 == 0,                         % check to avoid divide by zero
        sin_dlambda = 0;                        % if cos_phi2 = 0 => sin_dlambda = 0
      else                                      % else sin_dlambda is computed like this
        sin_dlambda = sin_alpha(alpha_index)*sin_psi(psi_index)/cos_phi2;  % a real division
      end;                                      % end of divide by zero check

%     To avoid computation of dlambda using asin and thereafter updating lambda2 by adding 
%     lambda_old and dlambda, we instead compute cos_dlambda by sqrt and update sin_lambda2
%     using the fact that sin(A+B)=sin(A)*cos(B)+cos(A)*sin(B). The same is done for cos_lambda2
      cos_dlambda=sqrt(1-sin_dlambda*sin_dlambda);
      sin_lambda2=sin_lambda_old*cos_dlambda+cos_lambda_old*sin_dlambda;
      cos_lambda2=cos_lambda_old*cos_dlambda-sin_lambda_old*sin_dlambda;

%     The new posible (phi,lambda) coordinates (which are really only known as the sine
% 					and cosine of the angles) are converted to Carthesian koordinates (X,Y,Z) using 
%     an simplified unfolded version of FRGEOD.M
        N_phi = semi_major_axis/sqrt(1-esq*sin_phi2*sin_phi2);
        P = N_phi*cos_phi2;
        XR(3,1) = N_phi*(1-esq)* sin_phi2;      % XR is a column vector with receiver coordinates
        XR(1,1) = P*cos_lambda2;                % XR(1,1) ~ x, XR(2,1) ~ y, XR(3,1) ~ z
        XR(2,1) = P*sin_lambda2;                % 
%     End of simplified FRGEOD.M unfolded

      for t = 1:m,                              % satellite loop
        cal_one_way(1,t) = norm(XS(:,t)-XR);    % distance to one of the SVs
        % compute the difference between the pseudorange and the new posible position
        one_way_res(1,t) = pseudoranges(t,2)-cal_one_way(1,t)+sat_clock(t);
      end;                                      % end of satellite loop
      resid_t = one_way_res(1,:)-one_way_res(1,1);  % difference between the first distance
                                                % and the others
      New_Sum = resid_t*resid_t';               % compute sum of squares of the residuals
      if New_Sum < Old_Sum,                     % test if this sum is the least so far
        Old_Sum = New_Sum;                      % if so, save it
        sin_phi_old = sin_phi2;                 % and save (phi,lambda) for next iteration
        cos_phi_old = cos_phi2;                 %
        sin_lambda_old = sin_lambda2;           %
        cos_lambda_old = cos_lambda2;           %
      end;                                      % end of test
    end                                         % end of slicing loop (azimuth) 
  end                                           % end of slicing loop (inclination) 
end                                             % end of main loop (iter)

% The main loop is finished and now the result must be corrected for the 
% rotation of the Earth, while the signals travelled through space:                

corr = (Omegae_dot*mean(pseudoranges(:,2))*inv_vlight)*inv_dtr;  % correction angle in radians
if sign(cos_lambda_old) == 1 & sign(sin_lambda_old) == 1,
  lambda_old = asin(sin_lambda_old)*inv_dtr;
end;
if sign(cos_lambda_old) == 1 & sign(sin_lambda_old) ==-1,
  lambda_old = 360+asin(sin_lambda_old)*inv_dtr;
end;
if sign(cos_lambda_old) ==-1, 
  lambda_old = 180-asin(sin_lambda_old)*inv_dtr;
end;

lambda = mod(lambda_old-corr,360);  % correct lambda (only 
                                    % lambda is corrected)
phi = asin(sin_phi_old)*inv_dtr;    % compute final phi
%%%%%%%%%%%%%%%%%% end recpos23.m %%%%%%%%%%%%%%%%%%%%%