xpfwls.m

GPS TOOLBOX包含以下内容： 1、GPS相关常量和转换因子； 2、角度变换； 3、坐标系转换： &#61656 点变换； &#61656 矩阵变换； &#61656 向量变换

%                                 xpfwls.m
%  Scope:  This Matlab program determines position fix using weighted least 
%          squares algorithm; the horizontal position error can be plotted 
%          and/or recorded into a specified output file. It is assumed that the 
%          user is stationary and the pseudorange error at each time step are 
%          randomly generated.  WGS-84 constants are used.
%  Usage:  xpfwls
%  Inputs:   - name of the input almanac data file; the default is data file 
%              wk749.dat. Each record contains the nine data related to a 
%              satellite in the following order: 
%              1) satellite id, 
%              2) satellite time of applicability (toa), in seconds,
%              3) satellite semi-major axis (a), in meters, 
%              4) satellite eccentricity (e),
%              5) satellite orbital inclination (I_0), in radians,
%              6) satellite right ascension at toa (OMEGA_0), in radians, 
%              7) satellite argument of perigee (omega), in radians,
%              8) satellite mean anomaly (M_0), in radians,
%              9) satellite rate of right ascension (OMEGA_DOT), radians/second
%            - user's true position (latitude, longitude and altitude) entered
%              from keyboard or a specified data file
%            - elevation angle limit in degrees; the default is 5. degrees
%            - selection of the measurement error standard deviation; the default is 
%              33.3 meters for SPS with SA on and 6.5 meters for SPS with SA off;
%              at the same time specific uniformly distributed weighted factors are
%              selected for up to 33 satellite measurements
%            - initial simulation time (GPS time of week); the default value is 0.
%            - number of time steps
%            - time step, in seconds
%            - name of the output file (optional)
%            - selection of the graph
%            - other parameters are initialized by default (for example, weighting
%              factors)
%  Outputs:  - file containing data for each time step (time step number,
%              time, number of visible satellites, horizontal position error,
%              3-component position error in ENU frame) (see file pf2)
%            - graph of horizontal (East-North) position error
%            - graph of East/North/Up position error versus time step
%            - graph of magnitudes of horizontal/vertival/3-dimensional position
%              error versus time step
%  Remarks:  The program can be modified to have a selection of weighting 
%            factors instead of using the default values.
%  External Matlab macros used: convcon, eleva, genrn, svpalm, uverv, 
%                               vecefenu, wgs84con
%  Last update: 08/17/00
%  Copyright (C) 1998-00 by  LL Consulting. All Rights Reserved.

clear 
close all
yes = 'y';

%  Initialization

convcon;
% global constants used:  deg_rad
rand('seed',0);
pflag = 0;

%  Read the input file containing satellites almanac data

disp('  ');
disp('Enter almanac data file - the default is data file wk749.dat');
answer1 = input('Do you want to use the default data file? (y/n)[y] ','s');
if isempty(answer1)
   answer1 = yes; 
end   
if (strcmp(answer1,yes) == 1)
   ff = 'wk749.dat';
else
   disp('  ');
   ff = input('Specify the almanac input data filename (with extension) --> ');
end
almanac = load(ff);
[nr_sat,ncol] = size(almanac);
if  ncol ~= 9
   disp('XPFWLS - Error: check the input data file (number of columns)');
   disp('  ');
   return
end

%  Enter the user's true location (in order to determine position error)

disp('  ');
answer2 = input('Enter user"s true position from keyboard? (y/n)[y] ','s');
if isempty(answer2)
   answer2 = yes;
end   
if (strcmp(answer2,yes) == 1)
   disp('  ');
   lat = input('Enter latitude, in degrees  -->  ');
   lon = input('Enter longitude, in degrees  -->  ');
   alt = input('Enter altitude, in meters  -->  ');
else
   fff = input('Specify the input filename (with extension) --> ','s');
   hh = load(fff);
   lat = hh(1);
   lon = hh(2);
   alt = hh(3);
end

lat   = lat * deg_rad;             %  in radians
lon   = lon * deg_rad;             %  in radians

%  Compute user's true location in ECEF

user_ecef = tgdecef(lat,lon,alt);

%  Specify the elevation mask angle limit

disp('  ');
disp('Enter elevation angle limit; the default value is 5. degrees');
answer3 = input('Do you want to use the default value? (y/n)[y] ','s');
if isempty(answer3)
   answer3 = yes;
end   
if (strcmp(answer3,yes) == 1)
   ele_limd = 5.;
else
   ele_limd = input('Specify the elevation angle limit (in degrees) -->  ');
end
ele_lim = ele_limd * deg_rad;   %  elevation angle limit in radians

%  Select the corresponding sigma (in meters) for each measurement

disp('  ');
disp('Enter GPS receiver type :');
disp('      1  -->  SPS with Selective Availability On');
disp('      2  -->  SPS with Selective Availability Off ');
gps_type = input('Make the selection  -->  ');
if  (gps_type ~= 1) & (gps_type ~= 2)
   disp('Error - XPFWLS.M; check the input for gps_type ');
   disp('  ');
   return
end
if  gps_type == 1
   sigma = 33.3;
   gpstype = 'SPS with SA On';
   r = 32.2:0.05:33.8;     % weighting factors
   for k = 1:33
     ri(k) = 1./r(k);
   end
else
   sigma = 6.5;
   gpstype = 'SPS with SA Off';
   r = 5.2:0.05:6.8;       % weighting factors
   for k = 1:33
     ri(k) = 1./r(k);
   end
end

%  Select initial simulation time (GPS time of week)

disp('  ');
disp('Enter initial simulation time (GPS time of week); the default value is 0. ');
answer4 = input('Do you want to use the default value? (y/n)[y] ','s');
if isempty(answer4)
   answer4 = yes;
end   
if (strcmp(answer4,yes) == 1)
   tweek = 0.;
else
   tweek = input('Specify initial simulation time (GPS time of week) -->  ');
end
tsim = tweek;

%  Select number of time samples and time step 

disp('  ');
t_sample = input('Enter number of time steps, e.g. 50 --> ');
disp('  ');
t_step  = input('Enter time step, in seconds, e.g. 60 --> ');

%  Select the output file

disp('  ');
answer5 = input('Do you want to save the generated data? (y/n)[n] ','s');
if  isempty(answer5)
   answer5 = 'no';
end
if strcmp(answer5,yes) == 1
   disp('  ');
   pf2 = input('Specify the output filename (with extension) -->  ','s');
   pflag = 1;
%else
%   pf2 = 1;     %   output to the screen
end

%  Start main computation loop    *********************************************

for kkk = 1:t_sample    %  cycle for all time samples

   clear z
   clear psat
   for k = 1:nr_sat,       %  cycle for all satellites  

%      Determine satellites position based on almanac

       temp = almanac(k,2:9);
       psat_temp = (svpalm(tsim,temp))';
       psat(k,1:3) = psat_temp';

%      Compute elevation angle and unit line-of-sight

       [temp3,temp1] = eleva(user_ecef,psat_temp);
       eangle(k) = temp3;
       ulos(:,k) = temp1; 
   end

%  Determine the number of visible satellites and the measurement matrix H

   clear  temp
   clear  htransp
   nr_vis = 0;
   for k = 1:nr_sat
      if  eangle(k) >= ele_lim
         nr_vis = nr_vis + 1;
         sv_vis(nr_vis) = k;     %  indices of visible satellites
         if nr_vis == 1
            htransp = [ulos(:,k)' 1.]';
         else
            temp = [ulos(:,k)' 1.]';
            htransp = [htransp temp];
         end
      end
   end

%  Check the number of visible satellites 

   if  nr_vis < 4
      disp('XPFWLS - Warning:  Less than 4 visible satellites');
      disp('  ');
      return
   end

%  Generate measured pseudorange and measurement

   for j = 1:nr_vis

      rx = psat(sv_vis(j),1);
      ry = psat(sv_vis(j),2);
      rz = psat(sv_vis(j),3);
      range_x = rx - user_ecef(1);
      range_y = ry - user_ecef(2);
      range_z = rz - user_ecef(3);
      noise(j) = genrn(1,0.,sigma);
   
      prange(j) = sqrt(range_x*range_x + range_y*range_y + range_z*range_z) + 				noise(j);
      d(j) = rx*htransp(1,j) + ry*htransp(2,j) + rz*htransp(3,j);
      z(j) = d(j) - prange(j);

   end

%  Determine new position fix

   for j = 1:nr_vis
       htransp(:,j) = htransp(:,j) * ri(sv_vis(j));
       z(j) = z(j) * ri(sv_vis(j));
   end
   pos_ecef = htransp'\z';

%  Determine horizontal/vertical and 3-dimensional position error

   ecef_delta(1:3) = pos_ecef(1:3) - user_ecef(1:3);
   enu_delta = vecefenu(ecef_delta,lat,lon);
   hpe(kkk) = sqrt(enu_delta(1)*enu_delta(1) + enu_delta(2)*enu_delta(2));
   vpe(kkk) = abs(enu_delta(3));
   tpe(kkk) = sqrt(enu_delta(1)*enu_delta(1) + enu_delta(2)*enu_delta(2)  + ...
                   enu_delta(3)*enu_delta(3));
   error_e(kkk) = enu_delta(1);
   error_n(kkk) = enu_delta(2);
   error_u(kkk) = enu_delta(3);

%  Save output data at each time step

   if (pflag == 1)
      fprintf(pf2,'%d %f %d %f %f %f %f\n',...
        kkk, tsim, nr_vis, hpe(kkk), enu_delta(1), enu_delta(2), enu_delta(3));
   end
  
%  Determine the simulation time for the next step

   tsim = tsim + t_step;

end      %  end of the time loop (index kkk)    *****************************

%  Select and execute the graphs

disp('  ');
answer6 = input('Do you want to execute the graphs? (y/n)[y] ','s');
if  isempty(answer6)
   answer6 = yes;
end
if strcmp(answer6,yes) == 1
   a1 = 'Weighted Least Squares Position Fix  -  Horizontal Position Error';
   a1 = [a1 ' - for ' gpstype];
   bb = 'North Position Error, in meters';
   cc = 'East Position Error, in meters';
   hpe_mean = mean(hpe);
   hpe_std = std(hpe);
   hpe_rms = rms(hpe');
   temp1 = ['mean = ',num2str(hpe_mean)];
   temp2 = ['st.dev. = ',num2str(hpe_std)];
   temp3 = ['rms = ',num2str(hpe_rms)];
   g = [temp1,' ;   ',temp2,' ;   ',temp3];
   if  gps_type == 1
      value = 60.;
   else
      value = 20.;
   end
   xmin = -value;
   xmax =  value;
   ymin = -value;
   ymax =  value;
   disp('  ');
   disp('Execute the horizontal position error graph ... ');
   disp('Select the mouse position to insert statistics text on the graph...');
   temp = input('Press a key to start ...');
   figure(1)
   plot(error_e,error_n,'*'), grid, ...
   axis([xmin xmax ymin ymax]),...
   title(a1), ylabel(bb), xlabel(cc),...
   gtext(g);       %  mouse placement of the text on a graph
   disp('  ');

   time = 1:t_sample;
   a2 = 'Weighted Least Squares Position Fix  -  East/North/Up Position Error';
   a2 = [a2 ' - for ' gpstype];  
   figure(2)
   subplot(3,1,1)
   plot(time,error_e,'*'), grid,...
   title(a2)
   ylabel('East, in meters');
   e_mean = mean(error_e);
   e_std = std(error_e);
   e_rms = rms(error_e');
   temp1 = ['mean = ',num2str(e_mean)];
   temp2 = ['st.dev. = ',num2str(e_std)];
   temp3 = ['rms = ',num2str(e_rms)];
   g = [temp1,' ;   ',temp2,' ;   ',temp3];
   v = axis;
   xtext = v(1) + 1;
   ytext = (v(3) + v(4))*0.75;
   text(xtext,ytext,g)
   
   subplot(3,1,2)
   plot(time,error_n,'*'), grid,
   ylabel('North, in meters');
   n_mean = mean(error_n);
   n_std = std(error_n);
   n_rms = rms(error_n');
   temp1 = ['mean = ',num2str(n_mean)];
   temp2 = ['st.dev. = ',num2str(n_std)];
   temp3 = ['rms = ',num2str(n_rms)];
   g = [temp1,' ;   ',temp2,' ;   ',temp3];
   v = axis;
   xtext = v(1) + 1;
   ytext = (v(3) + v(4))*0.75;
   text(xtext,ytext,g)

   subplot(3,1,3)
   plot(time,error_u,'*'), grid,
   ylabel('Up, in meters');
   c2 = ['Time step number (time step = ' num2str(t_step) ' seconds, '];
   xlabel([c2 'almanac = ' ff   ')']);
   u_mean = mean(error_u);
   u_std = std(error_u);
   u_rms = rms(error_u');
   temp1 = ['mean = ',num2str(u_mean)];
   temp2 = ['st.dev. = ',num2str(u_std)];
   temp3 = ['rms = ',num2str(u_rms)];
   g = [temp1,' ;   ',temp2,' ;   ',temp3];
   v = axis;
   xtext = v(1) + 1;
   ytext = (v(3) + v(4))/0.75;
   text(xtext,ytext,g)

   a3 = 'Weighted Least Squares Position Fix  -  Magnitude of Position Errors';
   a3 = [a3 ' - for ' gpstype];
   figure(3)
   subplot(3,1,1)
   plot(time,hpe,'*'), grid,
   title(a3)
   ylabel('Horizontal, in meters');
   h_mean = mean(hpe);
   h_std = std(hpe);
   h_rms = rms(hpe');
   temp1 = ['mean = ',num2str(h_mean)];
   temp2 = ['st.dev. = ',num2str(h_std)];
   temp3 = ['rms = ',num2str(h_rms)];
   g = [temp1,' ;   ',temp2,' ;   ',temp3];
   v = axis;
   xtext = v(1) + 1;
   ytext = (v(3) + v(4))*0.75;
   text(xtext,ytext,g)
   
   subplot(3,1,2)
   plot(time,vpe,'*'), grid,
   ylabel('Vertical, in meters');
   v_mean = mean(vpe);
   v_std = std(vpe);
   v_rms = rms(vpe');
   temp1 = ['mean = ',num2str(v_mean)];
   temp2 = ['st.dev. = ',num2str(v_std)];
   temp3 = ['rms = ',num2str(v_rms)];
   g = [temp1,' ;   ',temp2,' ;   ',temp3];
   v = axis;
   xtext = v(1) + 1;
   ytext = (v(3) + v(4))*0.75;
   text(xtext,ytext,g)

   subplot(3,1,3)
   plot(time,tpe,'*'), grid,
   ylabel('3-Dimensional, in meters');
   c3 = ['Time step number (time step = ' num2str(t_step) ' seconds, '];
   xlabel([c3 'almanac = ' ff   ')']);
   t_mean = mean(tpe);
   t_std = std(tpe);
   t_rms = rms(tpe');
   temp1 = ['mean = ',num2str(t_mean)];
   temp2 = ['st.dev. = ',num2str(t_std)];
   temp3 = ['rms = ',num2str(t_rms)];
   g = [temp1,' ;   ',temp2,' ;   ',temp3];
   v = axis;
   xtext = v(1) + 1;
   ytext = (v(3) + v(4))*0.75;
   text(xtext,ytext,g)

end

disp('End of the program  XPFWLS');
disp('  ');