lsnav.m

此功能包用于各种GPS坐标和时间的转换

  % generate the index for each of the four columns in ned
  obs_ind1 = (obs_index3 - 1) * 3 + 1;
  obs_ind2 = (obs_index3 - 1) * 3 + 2;
  obs_ind3 = (obs_index3 - 1) * 3 + 3;

  % put together the full index matrix with all four columns
  g_index = [obs_ind1 obs_ind2 obs_ind3];

  % allocate a sparse matrix for G which has G' for each of times
  % one the diagonal
  num_obs = length(obs_index3);
  num_times = length(I_3_sats);
  num_filled_entries = num_obs * 3;
  total_size_m = num_obs;
  total_size_n = num_times * 3;
  total_size_mn = total_size_m * total_size_n;

  G = spalloc(total_size_m, total_size_n, num_filled_entries);

  % fill in the G matrix
  column_index = reshape(g_index',1,total_size_m*3)';

  row_index1 = [1:total_size_m]' * ones(1,3);
  row_index = reshape(row_index1',total_size_m*3,1);

  % create a linear index so we can do a straight assignment without looping
  g_index = (column_index - 1) * total_size_m + row_index;

  % set the iteration counter and the converge flag
  iter = 0;
  x_converge_flag = 0;                               
  v_converge_flag = 0;                               
    
  % loop until the maximum number of iteration has been exceeded or all
  % of the navigation solutions have converged
  while iter < max_iter & converge_flag == 0

    % compute the LOS vector to the GPS satellites for the trajectory
    los_ecef = x_nav(obs_index_nav_3,1:3) - gps_orb3(:,2:4);    
    
    % normalize the LOS vector
    [los_ecef, r_mag] = normvect(los_ecef);

    % convert ECEF LOS vectors to NED for fixed altitude computations
    [los_ned] = ecef2ned(los_ecef, ecef2lla(x_nav(obs_index_nav_3,1:3)));

    % fill in the time portion (and zero out the height component)
    los_ned(:,3) = ones(size(los_ned,1),1);  % time portion of the 'Geometry' matrix
      
    % fill in the diagonal portion of G with the NED LOS vectors
    % (including the time column)
    G(g_index) = los_ned';
    
    % compute the computed PR measurement with the clock term
    comp_pr = r_mag + x_nav(obs_index_nav_3,4);  % clock term

    % compute residuals
    z = pr3 - comp_pr;

    % compute the temporary value inv(G'G)G'.  this will be used again 
    % in the Doppler processing
    GtG_temp = inv(G' * G) * G';
    
    % do least squares inv(G' * G) * G' * z
    delta_x_4(:,[1,2,4]) = reshape((GtG_temp * z)',3,num_times)';   
      
    % add the height component back into delta_x_nav
    delta_x_4(:,3) = zeros(size(delta_x_4,1),1);

    % rotate the delta_x_nav vector back to ECEF and add the clock term
    delta_x_nav = ned2ecef(delta_x_4(:,1:3), ecef2lla(x_nav(I_3_sats,1:3)));
    delta_x_nav(:,4) = delta_x_4(:,4);   % clock term
      
    % convergence calculation (including the clock)
    [dx_vect, dx_mag] = normvect(delta_x_nav);    % delta_x_nav is (1x4)
    
    % check for convergence
    I = find(dx_mag > dx_converge);
    if ~any(I)       
      x_converge_flag = 1;
    end % if ~any(I)
  
    clear I
    % update the navigation solution (position & clock)
    x_nav(I_3_sats,:) = x_nav(I_3_sats,:) + delta_x_nav;

    % if there is doppler data, also do a velocity solution
    if nargin == 7
      
      % compute the relative velocity vector
      vel_ecef = v_nav(obs_index_nav_3,1:3) - gps_vel3;
      
      % compute the computed Doppler measurement with the clock term
      comp_dop = dot(los_ecef',vel_ecef')' + v_nav(obs_index_nav_3,4);  
     
      % compute the measurement residual 
      z = doppler3 - comp_dop;

      delta_v_4(:,[1,2,4]) = reshape((GtG_temp * z)',3,num_times)';   
      
      % add the height component back into delta_x_nav
      delta_v_4(:,3) = zeros(size(delta_v_4,1),1);
      
      % rotate the delta_x_nav vector back to ECEF and add the clock term
      delta_v_nav = ned2ecef(delta_v_4(:,1:3), ecef2lla(x_nav(I_3_sats,1:3)));
      delta_v_nav(:,4) = delta_v_4(:,4);   % clock term
      
      % convergence calculation (including the clock)
      [dv_vect, dv_mag] = normvect(delta_v_nav);    % delta_v_nav is (1x4)
     
      % check for convergence
      I = find(dv_mag > dv_converge);
      if ~any(I)       
        v_converge_flag = 1;
      end % if ~any(I)
  
      clear I
      % update the navigation solution (position & clock)
      v_nav(I_3_sats,:) = v_nav(I_3_sats,:) + delta_v_nav;

    end % if nargin == 7
    
    % set the overall loop converge flag
    if nargin == 7
      if x_converge_flag == 1 & v_converge_flag == 1       
        converge_flag = 1;
      end % if x_converge_flag == 1 & v_converge_flag == 1
    
    else
      if x_converge_flag == 1
         converge_flag = 1;
      end % if x_converge_flag == 1
    end % if nargin == 7
         
    iter = iter + 1;    % increase iteration count
    
    if iter == max_iter
      fprintf('Warning from LSNAV.  Maximum number of iterations exceeded\n');
      fprintf('3 satellite cases.  Results may be in error.\n');
        
      % check to see if convergence failed or FAILED and fix x_nav accordingly
      if dx_mag > 100 * dx_converge
        x_nav(I_3_sats,:) = ones(length(I_3_sats),4) * inf;
        fprintf('Warning from LSNAV.  3 satellite solutions not converged to\n');
        fprintf('within 100 times the convergence the convergence criterion.\n');
        fprintf('All 3 satellite cases will be filled with inf.\n\n');
      end % if dx_mag > 100 * dx_converge3
        
    end % if
    
    end % while iter < max_iter & converge_flag == 0 
    
    % clear out this A and G, will use the same variables later (sparse)
    clear G delta_x_nav delta_x_nav4 delta_x_nav_ned delta_x_nav_ned4 I_i

end % if any(I_3_sats) & orbital_flag ~= 1 

% if there are not any 4 (or more) satellite cases return
if ~any(I_good)
  return
end % if ~any(I_good)

% allocate a sparse matrix for storing the G matrix
% generate the index for each of the four columns in G
obs_ind1 = (obs_index4 - 1) * 4 + 1;
obs_ind2 = (obs_index4 - 1) * 4 + 2;
obs_ind3 = (obs_index4 - 1) * 4 + 3;
obs_ind4 = (obs_index4 - 1) * 4 + 4;

% put together the full index matrix with all four columns
g_index = [obs_ind1 obs_ind2 obs_ind3 obs_ind4];

% allocate a sparse matrix for G
% one the diagonal 
num_obs = length(obs_index4);
num_times = length(I_good);
num_filled_entries = num_obs * 4;
total_size_m = num_obs;
total_size_n = num_times * 4;
total_size_mn = total_size_m * total_size_n;

% allocate the sparse matrix
G = spalloc(total_size_m, total_size_n, num_filled_entries);

% fill in the G matrix
column_index = reshape(g_index',1,total_size_m*4)';

row_index1 = [1:total_size_m]' * ones(1,4);
row_index = reshape(row_index1',total_size_m*4,1);

% create a linear index so we can do a straight assignment without looping
g_index = (column_index - 1) * total_size_m + row_index;
           
% loop while any of the solutions have not converged and have not exceeded
% the maximum number of iterations                     
iter = 0;
converge_flag = 0;       % flag for overall convergence
x_converge_flag = 0;     % flag for position convergence
v_converge_flag = 0;     % flag for velocity convergence

while iter < max_iter & converge_flag == 0
  % compute line of sight vectors
  los_ecef = x_nav(obs_index_nav_4,1:3) - gps_orb4(:,2:4);    

  % normalize the LOS vector
  [los_norm, rmag] = normvect(los_ecef);                                    
  
  % fill in the diagonal portion of G with the NED LOS vectors
  % (including the time column)
  % fill in the time portion of the LOS (soon to be G) matrix
  los_norm(:,4) = ones(size(los_ecef,1),1);  

  G(g_index) = los_norm';

  % compute the computed PR measurement with the clock term
  comp_pr = rmag + x_nav(obs_index_nav_4,4);  % clock term
    
  % compute residuals
  z = pr4 - comp_pr;
                   
  % compute the temporary value inv(G'G)G'.  this will be used again 
  % in the Doppler processing
  GtG_temp = inv(G' * G) * G';

  % least squares navigation solution
  delta_x_nav = reshape((GtG_temp * z),4,num_times)';   

  % convergence calculation (including the clock)
  [dx_vect, dx_mag] = normvect(delta_x_nav);    % delta_x_nav is (1x4)
    
  % check for convergence
  I = find(dx_mag > dx_converge);
  if ~any(I)       
    converge_flag = 1;
  end % if ~any(I)

  clear I

  % update the navigation solution (position & clock)
  x_nav(I_good,:) = x_nav(I_good,:) + delta_x_nav;

  % if there is doppler data, also do a velocity solution
  if nargin == 7
      
    % compute the relative velocity vector
    vel_ecef = v_nav(obs_index_nav_4,1:3) - gps_vel4;
      
    % compute the computed PR measurement with the clock term
    comp_dop = dot(los_norm(:,1:3)',vel_ecef')' + v_nav(obs_index_nav_4,4);  
     
    % compute the measurement residual
    z = doppler4 - comp_dop; 

    delta_v_nav = reshape((GtG_temp * z)',4,num_times)';   
      
    % convergence calculation (including the clock)
    [dv_vect, dv_mag] = normvect(delta_v_nav);    % delta_v_nav is (1x4)
     
    % check for convergence
    I = find(dv_mag > dv_converge);
    if ~any(I)       
      v_converge_flag = 1;
    end % if ~any(I)
  
    clear I
    % update the navigation solution (position & clock)
    v_nav(I_good,:) = v_nav(I_good,:) + delta_v_nav;
      
  end % if nargin == 7
    
  % set the overall loop converge flag
  if nargin == 7
    if x_converge_flag == 1 & v_converge_flag == 1       
      converge_flag = 1;
    end % if x_converge_flag == 1 & v_converge_flag == 1
    
  else
    if x_converge_flag == 1
       converge_flag = 1;
    end % if x_converge_flag == 1
  end % if nargin == 7

  iter = iter + 1;    % increase iteration count
    
  if iter == max_iter
    fprintf('Warning from LSNAV.  Maximum number of iterations exceeded\n');
    fprintf('in 4 satellite case.  Results may be in error.\n');
  end % if
    
end % while iter < max_iter & converge_flag == 0 

%%%%% END ALGORITHM CODE %%%%%

% end of LSNAV