

    %figure(lrNavFigPtr);
    %hold on
    if(~exist('aos'))
      if( exist('AOSplot.mat') == 2 )
        aos = load('AOSplot.mat');
      elseif( exist('filepath') == 1 )
        aos = load( [strtok(filepath,'/'),'/AOSplot.mat'])        
      else
        fprintf( 'Can''t find AOSplot.mat.\n' );
        return
      end
    end
    sensor = 'AOS';
    if( strcmpi( sensor, 'Vehicle') )
      cpsi   = 1 * data.platform_pitch_angle.value;
      spsi   = 0 * data.platform_pitch_angle.value;
      ctheta = cos( data.platform_pitch_angle.value );
      stheta = sin( data.platform_pitch_angle.value );
      cphi   = cos( data.platform_roll_angle.value );
      sphi   = sin( data.platform_roll_angle.value );
    elseif( strcmpi( sensor, 'AOS') )
      %aos.pitch = aos.pitch - 3.7;
      cpsi   = 1 * aos.pitch;
      spsi   = 0 * aos.pitch;
      ctheta = cos( aos.pitch*pi/180 );
      stheta = sin( aos.pitch*pi/180 );
      cphi   = cos( aos.roll*pi/180 );
      sphi   = sin( aos.roll*pi/180 );
    end
    %
    % Form the direction cosine matrix that recoordinatizes a vector from the 
    % body frame (B) into the local-level-local-vertical (LV) navigation 
    % frame, which is (North, East, Down).  See Fossen, p.10, Eqn 2.11.
    % Here we will assume the LV frame is intertially fixed and thus
    % also a Newtonian (N) frame.
    %
    % N is the inertially fixed, or Newtonian frame.
    %
    % The transformation T_B_N, is 3-2-1 Euler.  The matrix below, T_N_B, is
    % the transpose.
    %
    %Initialize it.
    npoints = length(data.platform_pitch_angle.value);
    if( 0 )
      T_N_B = zeros(3,3,npoints)
      %
      T_N_B(1,1,:) =  cpsi.*ctheta; 
      T_N_B(2,1,:) =  spsi.*ctheta; 
      T_N_B(3,1,:) = -stheta; 
      
      T_N_B(1,2,:) = -spsi.*cphi + cpsi.*stheta.*sphi; 
      T_N_B(2,2,:) =  cpsi.*cphi + sphi.*stheta.*spsi; 
      T_N_B(3,2,:) =  ctheta.*sphi; 

      T_N_B(1,3,:) =  spsi.*sphi + cpsi.*cphi.*stheta; 
      T_N_B(2,3,:) = -cpsi.*sphi + stheta.*spsi.*cphi;
      T_N_B(3,3,:) =  ctheta.*cphi; 

      %
      % For now, plot this as if the vehicle moves along the surface, because the
      % data.depth doesn't match up with data.lat/lon.  The depth and nav logs
      % have different lengths.  In the midwater this is ok.  If the beam trace on
      % the seabed matters then I'll have to solve this.
      %
      for( i=1:length(indxnan) )
        vehicle_N = [utmProj(indxnan(i),1);
          utmProj(indxnan(i),2);
          0];
        beam_N(:,1,i)  = T_N_B(:,:,i) * [0 0 1]' + vehicle_N;
        beam_N0(:,1,i) = T_N_B(:,:,i) * [0 0 1]';
        error(i,1) = asin(sqrt(sphi^2 + cphi^2*stheta^2));
      end
    end
    error    = asin(sqrt(sphi.^2 + cphi.^2.*stheta.^2));

    plot(ptr_vptheta, datenum(aos.t), [aos.pitch error*180/pi])
    legend(ptr_vptheta, 'Vehicle Pitch', 'AOS Pitch', 'Sonar Pointing Error');
    

    if( exist( 'aosFigPtr' ) ~= 1 )
    aosFigPtr= figure;
    set( aosFigPtr, 'Name', 'LR Pitch', 'DeleteFcn', 'clear aosFigPtr' );
    set(gcf,'position',position)
    if(exist('fontsize')) 
      aosFontsize=fontsize;
    else
      aosFontsize=20;
    end
    aosFontweight='normal';
    set(gca,'fontsize',aosFontsize,'fontweigh',aosFontweight)
    set(gcf,'defaultaxesfontsize',aosFontsize,'defaulttextfontsize', ...
        aosFontsize)
    aosAxesPtr = gca;
  else
    figure(aosFigPtr);
  end

  plot(data.platform_pitch_angle.time, ...
      data.platform_pitch_angle.value*180/pi,'b');
  hold on;
  grid on;
  xlim = get(gca,'xlim');
  plot(datenum(aos.t), [aos.pitch error*180/pi])
  ylabel('Pitch, Deg.')
  dynamicDateTicks
  set(gca,'xlim',xlim);

% Now try subtracting the aos pitch from the vehicle pitch:

% First drop out the nans.

  indx = find(isnan(datenum(aos.t)))
  aos.t(indx) = [];
  aos.pitch(indx) = [];

  aosPitch = interp1(datenum(aos.t),aos.pitch,data.platform_pitch_angle.time);


  plot(data.platform_pitch_angle.time,  data.platform_pitch_angle.value*180/pi ...
      - aosPitch, 'm');
  legend('Vehicle Pitch', 'AOS Pitch', 'Sonar Pointing Error','Vehicle-AOS Pitch')

  if(exist('titleStr')) 
    title(titleStr) 
  end
