%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FILENAME: gpsPlot.m
% PURPOSE:  Plot gps data
% DATE:     00/1/19
% AUTHOR:   Initial; rsm
% NOTES:
% MODS:
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Units and Fundamental Constants:
  units
%
% Compute the distance between each measurement:
%
  delta  = 2*pi - gps(:,longitude)/100*pi/180;       %Convert to East Long.
  lambda = geod2cen(gps(:,latitude)/100)*pi/180;
%
% Let rEV_ECF be the vector from the Earth's center to the vehicle CM,
% coordinatized in the Earth Centered Fixed reference frame.
%
  rEV_ECF = [ cos(-lambda).*cos(delta) cos(-lambda).*sin(delta) -sin(-lambda) ] * rE;
  TP2 = [0  0  1;
         0  1  0;
        -1  0  0];

  i = 1;
%
%    Compute the Dir Cosine matrix from ECF to LVH, where LVH is the Local
%    Vertical Local-Horizontal reference frame, with x North, y East, and 
%    z down.
%
%    First, increasing long is to the East; psi > 0
%
     TY = [ cos(delta(i))    sin(delta(i))       0;
	   -sin(delta(i))    cos(delta(i))       0;
		0             0                  1];
%
%    Assume Northern Hemisphere, which is a minus pitch; theta<0
%
     TP = [ cos(-lambda(i))       0      -sin(-lambda(i));
  		   0              1              0;
	    sin(-lambda(i))       0       cos(-lambda(i))];
%
%    Finally, do a -pi/2 pitch to flip the LVH z axis down, and x north;
%  
     T_LVH_ECF = TP2 * TP * TY;
%
%    Convert rEV from ECF to LVH
%
  rEV_LVH = rEV_ECF * T_LVH_ECF';

  magR = sqrt( rEV_LVH(:,1).^2 + rEV_LVH(:,2).^2 + rEV_LVH(:,3).^2 );

  str = [', Case:',run_name, ' ',myDate];

  if exist( 'gpsFig1' ) ~= 1
      gpsFig1 = figure;
  else 
      figure( gpsFig1 ); 
  end
    
  set( gpsFig1, 'name', 'GPS and Dead Reckoned Positions', ...
       'DeleteFcn', 'clear gpsFig1' );
  set(gcf,'position',position)
  clf
%  plot(gps(:,longitude)/100, gps(:,latitude)/100,'.')
%  ylabel(' Decimal Degrees North Latitude')
%  xlabel(' Decimal Degrees West Longitude')
  plot(rEV_LVH(:,1), rEV_LVH(:,2),'.r');
  line( dataNav(:,mPos_y), dataNav(:,mPos_x), 'linewidth', 2 );

  ylabel(' North, Meters')
  xlabel(' East, Meters')
  set( gca, 'DataAspectRatio', [1 1 .25] );
  title([' Vehicle Position from GPS', str])
  grid on
  zoom on