%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% PURPOSE: Plot AHRS data.
% AUTHOR:  Initial; rsm
% DATE:    00/1/10
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Units and Fundamental Constants:
  units
  hardflag=0;

  time = 1;

  if( exist('titleStr') ~=1 )
    titleStr = ['; Data Set: ', DirName];
    PaperPosn = [.5 .5 3.75 2.5];
    FigurePosn = [5 30 815 760];
    position = [4 32 815 658];
  end

  %str = [' ',titleStr, ' ',myDate];
  str = [' ',titleStr];
  i = 0;

  if exist( 'dataNav' ) ~= 1
    %
    % OK.  I'm going to assume that this routine is being run stand-alone on
    % raw navigation data consisting of a subset of Kearfott, Ixsea, and
    % Crossbow. Since dataNav isn't present, we'll have to perform the time
    % synchronization here.
    %
    % Assume that Kearfott data is present, and that it has not been
    % time-synchronized yet. 
    %
    % Find latest start time.
    %
    startTime = 0;
    if( (exist('dataK') == 1) & (dataK(1,1) > startTime) )
      startTime = dataK(1,1);
    end
    if( (exist('dataO') == 1) & (dataO(1,1) > startTime) )
      startTime = dataO(1,1);
    end
    if( (exist('dataCB') == 1) & (dataCB(1,1) > startTime) )
      startTime = dataCB(1,1);
    end
    %
    % Synchronize the time axes of the various logs:
    %
    if( (exist('dataK') == 1) )
      dataK(:,1) = dataK(:,1) - startTime;
      dataK = NegTrunc(dataK);
    end
    if( (exist('dataO') == 1) )
      dataO(:,1) = dataO(:,1) - startTime;
      dataO = NegTrunc(dataO);
    end
    if( (exist('dataCB') == 1) )
      dataCB(:,1) = dataCB(:,1) - startTime;
      dataCB = NegTrunc(dataCB);
    end
    %
    % In this case where dataNav isn't present, I'll assume the user wants to
    % see the Octans, Dvl, and Crossbow data, if present, so I'll set the
    % switches here. 
    %
    if exist('dataO') == 1, plotSwOct = 1; end
    if exist('dataK') == 1, plotSwKft = 1; end
    if exist('dataDvl') == 1, plotSwDvl = 1; end
  end
%
% For backward compatibility; 2004/12/1.
%
  if( exist('mVbodyxK') == 1 )
    mVnavxK = mVbodyxK;
    mVnavyK = mVbodyyK;
    mVnavzK = mVbodyzK;
  end
  
  if exist( 'crsBwFig1' ) ~= 1
      crsBwFig1 = figure;
  else
      figure( crsBwFig1 );
  end
%
% You can plot all available AHRS instruments.  The following flags can
% be used to shut off unwanted plots.
%
  nstrips = 3;
  if exist('plotSwOct') ~= 1, plotSwOct = 1; end
  if exist('plotSwDvl') ~= 1, plotSwDvl = 0; end
  if exist('plotSwKft') ~= 1, plotSwKft = 1; end

  if( (plotSwOct == 1) & (exist('dataO') == 1) ), nstrips=4; end

  set( crsBwFig1, 'name', 'Attitude & Heading Reference', ...
       'DeleteFcn', 'clear crsBwFig1' );
  set(gcf,'position',position)
  clf

  lstr = {};           %Cell array to hold legend names
  
  subplot(nstrips,1,1)
    if exist('dataNav') == 1,
      [xx,yy]   = stairs( dataNav(:,time), unwrap(dataNav(:,mPhi))/d2r);
      plot(xx, yy);
      lstr(length(lstr)+1,1) = {'Nav'};
    end
    if exist('dataCB') == 1,
      [xx1,yy1] = stairs( dataCB(:,time),  dataCB(:,mRollCB)/d2r);
      line(xx1, yy1,'Color','g');
      lstr(length(lstr)+1,1) = {'Crossbow'};
    end
    if exist('dvlRoll') == 1,
    if plotSwDvl
      [xx1,yy1] = stairs( dataDvl(:,time),  unwrap(dataDvl(:,dvlRoll))/d2r);
      line(xx1, yy1,'Color','c');
      lstr(length(lstr)+1,1) = {'Dvl'};
    end
    end
    if exist('dataO') == 1,
    if plotSwOct
      [xx2,yy2] = stairs( dataO(:,time),  unwrap(dataO(:,mRollO))/d2r);
      line(xx2, yy2,'Color','r');
      lstr(length(lstr)+1,1) = {'Octans'};
    end
    end
    if exist('dataK') == 1,
    if plotSwKft
      [xx2,yy2] = stairs( dataK(:,time),  unwrap(dataK(:,mRollK))/d2r);
%      if( plotSwIce == 0 )
%	[xx2,yy2] = stairs( dataK(:,time),  unwrap(dataK(:,mRollK))/d2r);
%      else
%	[xx2,yy2] = stairs( dataK(:,time),  -unwrap(dataK(:,mRollK))/d2r);
%      end
      line(xx2, yy2,'Color','m');
      lstr(length(lstr)+1,1) = {'Kearfott'};
    end
    end
    ylabel('Degrees')
    title(['Roll, Degrees',str]);
    legend(lstr);
    grid on
    zoom on

    subplot(nstrips,1,2)
    if exist('dataNav') == 1,
      [xx,yy]   = stairs( dataNav(:,time), dataNav(:,mTheta)/d2r);
      plot(xx, yy);
    end
    if exist('dataCB') == 1,
      [xx1,yy1] = stairs( dataCB(:,time),  dataCB(:,mPitchCB)/d2r);
      line(xx1, yy1,'Color','g');
    end
    if exist('dvlPitch') == 1,
    if plotSwDvl
      [xx1,yy1] = stairs( dataDvl(:,time),  dataDvl(:,dvlPitch)/d2r);
      line(xx1, yy1,'Color','c');
    end
    end
    if exist('dataO') == 1,
    if plotSwOct
      [xx2,yy2] = stairs( dataO(:,time),  dataO(:,mPitchO)/d2r);
      line(xx2, yy2,'Color','r');
    end
    end
    if exist('dataK') == 1,
    if plotSwKft
      [xx2,yy2] = stairs( dataK(:,time),  dataK(:,mPitchK)/d2r);
      line(xx2, yy2,'Color','m');
    end
    end
    ylabel('Degrees')
    title(['Pitch, Degrees',str]);
    grid on
    zoom on

    subplot(nstrips,1,3)
    if exist('dataNav') == 1,
      [xx,yy]   = stairs( dataNav(:,time), mod360(dataNav(:,mPsi)/d2r));
      plot(xx, yy);
    end
    if exist('dataCB') == 1,
      [xx1,yy1] = stairs( dataCB(:,time),  mod360(dataCB(:,mYawCB)/d2r));
      line(xx1, yy1,'Color','g');
    end
    if exist('dvlHeading') == 1,
    if plotSwDvl
      [xx1,yy1] = stairs( dataDvl(:,time),  mod360(dataDvl(:,dvlHeading)/d2r));
      line(xx1, yy1,'Color','c');
    end
    end
    if exist('dataO') == 1,
    if plotSwOct
      [xx2,yy2] = stairs( dataO(:,time),  mod360(dataO(:,mYawO)/d2r));
      line(xx2, yy2,'Color','r');
    end
    end
    if exist('dataK') == 1,
    if plotSwKft
      [xx2,yy2] = stairs( dataK(:,time),  mod360(dataK(:,mHeadK)/d2r));
      line(xx2, yy2,'Color','m');
    end
    end
    ylabel('Degrees')
    xlabel('Elapsed Time (s)');
    title(['Yaw, Degrees',str]);
    grid on
    zoom on

    if nstrips == 4 & (exist('mStatusO') == 1),
    subplot(nstrips,1,4)
    if plotSwOct
      [xx2,yy2] = stairs( dataO(:,time),  dataO(:,mStatusO));
      plot(xx2, yy2,'r','linestyle','.');
    end
    if exist('dataK') == 1,
    if plotSwKft
      [xx2,yy2] = stairs( dataK(:,time),  dataK(:,mMonK));
      line(xx2, yy2,'color','m','linestyle','.');
    end
    end
    ylabel('Status')
    xlabel('Elapsed Time (s)');
    title(['Status',str]);
    grid on
    zoom on
    end
  linkaxes(get(gcf,'children'),'x')

%
%   Body Rates;  omega_B_N_B
%
%
    pause(.5)
  if exist( 'CrsBwFig2' ) ~= 1
      CrsBwFig2 = figure;
  else
      CrsBwFig2 = figure( CrsBwFig2 );
  end

  set( CrsBwFig2, 'name', 'Body Rates', ...
       'DeleteFcn', 'clear CrsBwFig2' );
  set(gcf,'position',position)
  clf

  lstr = {'Nav'};
  YEMax = 15;  %Prevent autoscaling to outliers
  YEMin =-15;
  
  subplot(3,1,1)
    if exist('dataNav') == 1,
      [xx,yy]   = stairs( dataNav(:,time), dataNav(:,mOmega_x)/d2r);
      plot(xx, yy);
    end
    if exist('dataCB') == 1,
      [xx1,yy1] = stairs( dataCB(:,time),  dataCB(:,mOmega_xCB)/d2r);
      line(xx1, yy1,'Color','g');
      lstr(length(lstr)+1,1) = {'Crossbow'};
    end
    if exist('dataO') == 1,
    if plotSwOct
      [xx2,yy2] = stairs( dataO(:,time),  dataO(:,mOmega_xO)/d2r);
      line(xx2, yy2,'Color','r');
      lstr(length(lstr)+1,1) = {'Octans'};
    end
    end
    if exist('dataK') == 1,
    if plotSwKft
      [xx2,yy2] = stairs( dataK(:,time),  dataK(:,mPrateK)/d2r);
      line(xx2, yy2,'Color','m');
      lstr(length(lstr)+1,1) = {'Kearfott'};
    end
    end
    ylabel('Degrees/Second')
    title(['X Axis Body Rate (Omega B x), Degrees/Sec',str]);
    legend(lstr);
    grid on
    zoom on
    Ylim = get(gca,'YLim');
    Ylim(1) = max([Ylim(1) YEMin]);
    Ylim(2) = min([Ylim(2) YEMax]);
    set(gca,'YLim',Ylim)

    subplot(3,1,2)
    if exist('dataNav') == 1,
      [xx,yy] = stairs( dataNav(:,time), dataNav(:,mOmega_y)/d2r);
      plot(xx, yy);
    end
    if exist('dataCB') == 1,
      [xx1,yy1] = stairs( dataCB(:,time),  dataCB(:,mOmega_yCB)/d2r);
      line(xx1, yy1,'Color','g');
    end
    if exist('dataO') == 1,
    if plotSwOct
      [xx2,yy2] = stairs( dataO(:,time),  dataO(:,mOmega_yO)/d2r);
      line(xx2, yy2,'Color','r');
    end
    end
    if exist('dataK') == 1,
    if plotSwKft
      [xx2,yy2] = stairs( dataK(:,time),  dataK(:,mQrateK)/d2r);
      line(xx2, yy2,'Color','m');
    end
    end
    ylabel('Degrees/Second')
    title(['Y Axis Body Rate (Omega B y), Degrees/Sec',str]);
    grid on
    zoom on
    Ylim = get(gca,'YLim');
    Ylim(1) = max([Ylim(1) YEMin]);
    Ylim(2) = min([Ylim(2) YEMax]);
    set(gca,'YLim',Ylim)

    subplot(3,1,3)
    if exist('dataNav') == 1,
      [xx,yy] = stairs( dataNav(:,time), dataNav(:,mOmega_z)/d2r);
      plot(xx, yy);
    end
    if exist('dataCB') == 1,
      [xx1,yy1] = stairs( dataCB(:,time),  dataCB(:,mOmega_zCB)/d2r);
      line(xx1, yy1,'Color','g');
    end
    if exist('dataO') == 1,
    if plotSwOct
      [xx2,yy2] = stairs( dataO(:,time),  dataO(:,mOmega_zO)/d2r);
      line(xx2, yy2,'Color','r');
    end
    end
    if exist('dataK') == 1,
    if plotSwKft
      [xx2,yy2] = stairs( dataK(:,time),  dataK(:,mRrateK)/d2r);
      line(xx2, yy2,'Color','m');
    end
    end
    ylabel('Degrees/Second')
    xlabel('Elapsed Time (s)');
    title(['Z Axis Body Rate (Omega B z), Degrees/Sec',str]);
    grid on
    zoom on
    Ylim = get(gca,'YLim');
    Ylim(1) = max([Ylim(1) YEMin]);
    Ylim(2) = min([Ylim(2) YEMax]);
    set(gca,'YLim',Ylim)
  linkaxes(get(gcf,'children'),'x')

  
  if( ~exist('dataCB') && ~exist('dataK') && ~exist('dataO') )
      return;
  end
  
  
%
% Begin some computations:
%
if exist('dataK') == 1
    GravMagK  = sqrt( dataK(:,mAccelxK).^2 + ...
                      dataK(:,mAccelyK).^2 + ...
                      dataK(:,mAccelzK).^2 )/g;
    MagVelK   = sqrt( dataK(:,mVnavxK).^2 + ...
                      dataK(:,mVnavyK).^2 + ...
                      dataK(:,mVnavzK).^2 );
end

if exist('dataCB') == 1
    %
    % Compute norm of Gravity Field:
    %
    GravMagCB = sqrt( dataCB(:,mAccel_xCB).^2 + ...
                      dataCB(:,mAccel_yCB).^2 + ...
                      dataCB(:,mAccel_zCB).^2 );
    %
    % Compute norm of Magnetic Field:
    %
    MagneticMag = sqrt( dataCB(:,mMag_xCB).^2 + ...
                        dataCB(:,mMag_yCB).^2 + ...
                        dataCB(:,mMag_zCB).^2 );
    %
    % Compute magnetic inclination:
    %
    magUnitVec  = dataCB(:,mMag_xCB:mMag_zCB)./(MagneticMag*[1 1 1]);
    gravUnitVec = dataCB(:,mAccel_xCB:mAccel_zCB)./(GravMagCB*[1 1 1]);
   %magIncl     = asin( norm(cross(magUnitVec,gravUnitVec)) )*180/pi;    
   %magIncl     = acos( diag(magUnitVec*gravUnitVec') )*180/pi;  %SLOW
    magIncl     = acos( (magUnitVec.*gravUnitVec)*[1 1 1]' )*180/pi;
    %
    % Compute the heading from the 3 components measured by the
    % magnetometer.  Check to see if they are compensating for attitude.
    %
    Yvec = cross(magUnitVec,gravUnitVec);
    Xvec = -cross(gravUnitVec,Yvec);
    if 0
    Xvec(:,2) = -Xvec(:,2)
    Xvec(:,3) = -Xvec(:,3)
    end
    XvecMag = sqrt( Xvec(:,1).^2 + Xvec(:,2).^2 + Xvec(:,3).^2 );
    % Here's hoping they don't become  co-linear...
    XunitVec = Xvec./(XvecMag*[1 1 1]);
    compHead = atan2(XunitVec(:,2),XunitVec(:,1));
  end
%
% Now resume plotting...
%
%
% Plot various heading errors:
%
  if( exist('tscale') ~= 1 ) tscale = 1; end

  if( (plotSwKft ==1) & (exist('dataK') == 1) & ...
       ( (exist('dataCB') == 1) | (exist('dataO') == 1) ) )
  pause(.5)
  if exist( 'HdgErrFig' ) ~= 1
    HdgErrFig = figure;
  end
  HdgErrFig = figure( HdgErrFig );

  set( HdgErrFig, 'name', 'Heading Error', ...
       'DeleteFcn', 'clear HdgErrFig' );
  set(gcf,'position',position)
  if( exist('fontsize') ~= 1 ) fontsize = 10; end
  if( exist('fontweight') ~= 1 ) fontweight = 'normal'; end
  lstr={};

  if( exist('subsamp') ~=1 ) subsamp = 1; end
%
% Beware that the Crossbow driver is already in units of degrees.
%
  subplot(2,1,1)
  set(gca,'fontsize',fontsize,'fontweigh',fontweight)
  if( exist('dataCB') == 1 )
    cbIntrpHdg = interp1( dataCB(:,1), ...
	                  mod360(dataCB(:,mYawCB))*d2r, dataK(:,1) );
    compIntrpHdg = interp1( dataCB(:,1), ...
	                    mod360(compHead/d2r)*d2r, dataK(:,1) );
    cbErr      = cbIntrpHdg - dataK(:,mHeadK);
    compErr    = compIntrpHdg - dataK(:,mHeadK);
    plot( dataK(1:subsamp:end,1)/tscale, cbErr(1:subsamp:end)/d2r,'g' )
    lstr(length(lstr)+1,1) = {'Magnetic Compass'};
    clmap = colormap;
%    line( dataK(:,1), compErr/d2r, 'color', clmap(30,:) );
%    lstr(length(lstr)+1,1) = {'Crsbw Mag'};
  end
  %set(gca,'YLim',[-10 10]);
  if( (exist('dataO') == 1) & (plotSwOct == 1) )
%
%   Interp1 gags if there are any doubled time points in dataO(:,1).  Check
%   for doubles, (actually make dataO(:,1) monotonic), and throw out
%   duplicated points. 
%
    indx = [1; find(diff(dataO(:,1))>0)+1];  %Tricky!  See Logical Subscripting.
    oIntrpHdg = interp1( dataO(indx,1), dataO(indx,mYawO), dataK(:,1),...
			 'linear',0 );
    oErr      = oIntrpHdg - dataK(:,mHeadK);
    line( dataK(1:subsamp:end,1)/tscale, oErr(1:subsamp:end)/d2r, 'color','r' )
    lstr(length(lstr)+1,1) = {'Gyrocompass'};
  end
  grid on
  zoom on
  %title(['Heading Error wrto Kearfott',str]);
  title(['Heading Error. ',str]);
  ylabel('Degrees')
  YEMax = 0;  %Prevent autoscaling to outliers
  YEMin =-15;
  Ylim = get(gca,'YLim');
  Ylim(1) = max([Ylim(1) YEMin]);
  Ylim(2) = min([Ylim(2) YEMax]);
  set(gca,'YLim',Ylim)
  legend(lstr,2)
  
  lstr={};
  subplot(2,1,2)
  set(gca,'fontsize',fontsize,'fontweigh',fontweight)
  if 0
  [xx2,yy2] = stairs( dataK(:,time)/tscale,  dataK(:,mMonK));
  plot(xx2(1:subsamp:end), yy2(1:subsamp:end),'color','m','linestyle','.');
  lstr(length(lstr)+1,1) = {'Kearfott'};
  if(exist('mStatusO') == 1),
    if plotSwOct
      [xx2,yy2] = stairs( dataO(:,time)/tscale,  dataO(:,mStatusO));
      line(xx2(1:subsamp:end), yy2(1:subsamp:end),'color','r','linestyle','.');
      lstr(length(lstr)+1,1) = {'Octans'};
    end
  end
  ylabel('Monitor Status')
  grid on
  legend(lstr)

  else  

  if exist('m_present_time') ~= 1 m_present_time = 1;end
  colors = get( gca, 'ColorOrder' );
  %[xx,yy] = stairs( dataDC(:,m_present_time)/tscale, dataDC(:,cTheta)*180/pi );
  %plot( dataNav(:,m_present_time)/tscale, dataNav(:,mTheta)*180/pi, xx, yy );
  plot( dataNav(:,m_present_time)/tscale, dataNav(:,mTheta)*180/pi, ...
        dataDC(:,m_present_time)/tscale, dataDC(:,cTheta)*180/pi );
  if( exist( 'dIntegral' ) == 1 & 0)
    line( dataDC(1:subsamp:end,1)/tscale, ...
	  dataDC(1:subsamp:end,dIntegral)*180/pi, 'Color', colors(4,:) ); 
    title( 'Vehicle Pitch: Commanded-Green; Measured-Blue; Depth Integral-Cyan')
  else
    %title( 'Vehicle Pitch: Commanded-Green; Measured-Blue')
    title( 'Vehicle Pitch')
  end
  legend('Measured','Commanded',2)
  ylabel('Degrees')
  grid on
  zoom on
  end

  timeStr = 'Seconds';
  if( tscale == 60 )  timeStr = 'Minutes'; end
  xlabel(timeStr)
  
  end
%
% Accelerations; accel_B_N_B
%
% The Kearfott reference frame is rotated pi around x from SNAME.  I'll
% just transform back in the plots below with minus signs.
%
% The Crossbow accels are all flipped by a minus sign?? I'll flip them back.
%
  pause(.5)
  if exist( 'CrsBwFig3' ) ~= 1
      CrsBwFig3 = figure;
  else
      CrsBwFig3 = figure( CrsBwFig3 );
  end

  set( CrsBwFig3, 'name', 'Vehicle Acceleration', ...
       'DeleteFcn', 'clear CrsBwFig3' );
  set(gcf,'position',position)
  clf
  lstr = {};
    subplot(4,1,1)
    hold on
    if( exist('dataCB') ==1 ) 
      [xx,yy] = stairs( dataCB(:,time),  -dataCB(:,mAccel_xCB));
      plot(xx, yy, 'g');
      lstr(length(lstr)+1,1) = {'Crossbow'};
    end
    if( (exist('dataO') == 1) & plotSwOct )
      [xx2,yy2] = stairs( dataO(:,time),  dataO(:,mAccel_xO));
      plot(xx2, yy2,'Color','r');
      lstr(length(lstr)+1,1) = {'Octans'};
    end
    if( (exist('dataK') == 1) & plotSwKft )
      [xx2,yy2] = stairs( dataK(:,time),  dataK(:,mAccelxK)/g);
      plot(xx2, yy2,'Color','m');
      lstr(length(lstr)+1,1) = {'Kearfott'};
    end
    ylabel('g''s')
    title(['Acceleration Resolved onto Body X, g''s',str]);
    grid on
    zoom on
    hold off
    legend(lstr)
    pause(.5)

    subplot(4,1,2)
    hold on
    if( exist('dataCB') ==1 ) 
      [xx,yy] = stairs( dataCB(:,time),  -dataCB(:,mAccel_yCB));
      plot(xx, yy, 'g');
    end
    if( (exist('dataO') == 1) & plotSwOct )
      [xx2,yy2] = stairs( dataO(:,time),  dataO(:,mAccel_yO));
      plot(xx2, yy2,'Color','r');
    end
    if( (exist('dataK') == 1) & plotSwKft )
      if( plotSwIce == 0 )
	[xx2,yy2] = stairs( dataK(:,time),  -dataK(:,mAccelyK)/g);
      else
	[xx2,yy2] = stairs( dataK(:,time),   dataK(:,mAccelyK)/g);
      end
      plot(xx2, yy2,'Color','m');
    end
    ylabel('g''s')
    title(['Acceleration Resolved onto Body Y, g''s',str]);
    grid on
    zoom on
    hold off

    subplot(4,1,3)
    hold on
    if( exist('dataCB') ==1 ) 
      [xx,yy] = stairs( dataCB(:,time),  -dataCB(:,mAccel_zCB));
      plot(xx, yy, 'g');
    end
    if( (exist('dataO') == 1) & plotSwOct )
      [xx2,yy2] = stairs( dataO(:,time),  dataO(:,mAccel_zO));
      plot(xx2, yy2,'Color','r');
    end
    if( (exist('dataK') == 1) & plotSwKft )
      if( plotSwIce == 0 )
	[xx2,yy2] = stairs( dataK(:,time),  -dataK(:,mAccelzK)/g);
      else
	[xx2,yy2] = stairs( dataK(:,time),   dataK(:,mAccelzK)/g);
      end
      plot(xx2, yy2,'Color','m');
    end
    ylabel('g''s')
    title(['Acceleration Resolved onto Body Z, g''s',str]);
    grid on
    zoom on
    hold off


    subplot(4,1,4)
    hold on
    if( exist('dataCB') ==1 ) 
      [xx,yy] = stairs( dataCB(:,time),  GravMagCB);
      plot(xx, yy, 'g');
    end
    if( (exist('dataK') == 1) & plotSwKft )
      [xx2,yy2] = stairs( dataK(:,time),   GravMagK);
      plot(xx2, yy2,'Color','m');
    end
    ylabel('Mag. g''s')
    xlabel('Elapsed Time (s)');
    title(['Acceleration Magnitude, g''s',str]);
    grid on
    zoom on
    hold off

    linkaxes(get(gcf,'children'),'x')
    
    pause(.5)
    

%
% Navigation frame velocity from the Kearfott
% 
  
  if exist( 'KftFig' ) ~= 1
      KftFig = figure;
  else
      KftFig = figure( KftFig );
  end
  
  set( KftFig, 'name', 'Vehicle Velocity in Nav Frame', ...
       'DeleteFcn', 'clear KftFig' );
  set(gcf,'position',position)
  clf
  lstr = {};
    subplot(4,1,1)
    hold on
    if( (exist('dataK') == 1) & plotSwKft )
      [xx2,yy2] = stairs( dataK(:,time),  dataK(:,mVnavxK));
      plot(xx2, yy2);
      lstr(length(lstr)+1,1) = {'Kearfott'};
    end
    ylabel('m/s')
    title(['Velocity Resolved onto Navigation X, m/s',str]);
    grid on
    zoom on
    hold off
    legend(lstr)


    subplot(4,1,2)
    hold on
    if( (exist('dataK') == 1) & plotSwKft )
      [xx2,yy2] = stairs( dataK(:,time),  dataK(:,mVnavyK));
      plot(xx2, yy2);
    end
    ylabel('m/s')
    title(['Velocity Resolved onto Navigation Y, m/s',str]);
    grid on
    zoom on
    hold off

    subplot(4,1,3)
    hold on
    if( (exist('dataK') == 1) & plotSwKft )
      [xx2,yy2] = stairs( dataK(:,time),  dataK(:,mVnavzK));
      plot(xx2, yy2);
    end
    ylabel('m/s')
    title(['Velocity Resolved onto Navigation Z, m/s',str]);
    grid on
    zoom on
    hold off

    subplot(4,1,4)
    hold on
    if( (exist('dataK') == 1) & plotSwKft )
      [xx2,yy2] = stairs( dataK(:,time),   MagVelK);
      plot(xx2, yy2,'Color','m');
    end
    ylabel('Mag. m/s')
    xlabel('Elapsed Time (s)');
    title(['Vehicle Speed, m/s',str]);
    grid on
    zoom on
    hold off
    pause(.5)

    
  if( exist('dataCB') ~=1 ) return; end

  if exist( 'CrsBwFig4' ) ~= 1
      CrsBwFig4 = figure;
  else
      figure( CrsBwFig4 );
  end

  set( CrsBwFig4, 'name', 'Crossbow Magnetic Field', ...
       'DeleteFcn', 'clear CrsBwFig4' );
  set(gcf,'position',position)
  clf
    subplot(5,1,1)
    [xx,yy] = stairs( dataCB(:,time),  dataCB(:,mMag_xCB));
    plot(xx, yy);
    ylabel('Gauss')
    title(['Magnetic Field Resolved onto Body X',str]);
    grid on
    zoom on


    subplot(5,1,2)
    [xx,yy] = stairs( dataCB(:,time),  dataCB(:,mMag_yCB));
    plot(xx, yy);
    ylabel('Gauss')
    title(['Magnetic Field Resolved onto Body Y',str]);
    grid on
    zoom on


    subplot(5,1,3)
    [xx,yy] = stairs( dataCB(:,time),  dataCB(:,mMag_zCB));
    plot(xx, yy);
    ylabel('Gauss')
    title(['Magnetic Field Resolved onto Body Z',str]);
    grid on
    zoom on


    subplot(5,1,4)
    [xx,yy] = stairs( dataCB(:,time),  MagneticMag );
    plot(xx, yy);
    ylabel('Gauss')
    xlabel('Elapsed Time (s)');
    title(['Magnitude of the Magnetic Field ',str]);
    grid on
    zoom on


    subplot(5,1,5)
    [xx,yy] = stairs( dataCB(:,time),  magIncl );
    plot(xx, yy);
    ylabel('Degrees')
    xlabel('Elapsed Time (s)');
    title(['pi/2 - Inclination of the Magnetic Field ',str]);
    grid on
    zoom on

  return






  i = i+1;
  figure(i)
  if hardflag ==3, set(gcf,'PaperPosition',PaperPosn), end
  set(gcf,'position',position)
  clf
  subplot(3,1,1)
  plot(dataNav(:,tIx), dataNav(:,[pitchIx])/d2r);
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Degrees')
  title(['Pitch, Degrees',str]);
  grid on
  subplot(3,1,2)
  plot(dataNav(:,tIx), [dataNav(:,qIx)]/d2r);
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Deg/Sec')
  title(['Body Rotational Rate q (Omega_N_B_y)',str]);
  xlabel('Seconds');
  grid on
  subplot(3,1,3)
  plot(dataNav(:,tIx), [dataNav(:,deltaEIx)]*180/pi);
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Degrees')
  title(['Elevator Angle',str]);
  xlabel('Seconds');
  grid on

  i = i+1;
  figure(i)
  if hardflag ==3, set(gcf,'PaperPosition',PaperPosn), end
  set(gcf,'position',position)
  clf
  subplot(3,1,1)
  plot(dataNav(:,tIx), dataNav(:,[yawIx])/d2r);
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Degrees')
  title(['Heading, Degrees',str]);
  grid on
  subplot(3,1,2)
  plot(dataNav(:,tIx), dataNav(:,[rIx])/d2r);
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Deg/Sec')
  title(['Body Rotational Rate r (Omega_N_B_z)',str]);
  grid on
  subplot(3,1,3)
  plot(dataNav(:,tIx), [dataNav(:,deltaRIx)]*180/pi);
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Degrees')
  title(['Rudder Angle',str]);
  xlabel('Seconds');
  grid on

  i = i+1;
  figure(i)
  if hardflag ==3, set(gcf,'PaperPosition',PaperPosn), end
  set(gcf,'position',position)
  clf
  subplot(2,1,1)
  plot(dataNav(:,tIx), dataNav(:,[rollIx])/d2r);
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Degrees')
  title(['Roll, Degrees',str]);
  grid on
  subplot(2,1,2)
  plot(dataNav(:,tIx), dataNav(:,[pIx])/d2r);
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Deg/Sec')
  title(['Body Rotational Rate p (Omega_N_B_x)',str]);
  grid on

  i = i+1;
  figure(i)
  if hardflag ==3, set(gcf,'PaperPosition',PaperPosn), end
  set(gcf,'position',position)
  clf
  plot(dataNav(:,yPosIx), dataNav(:,[xPosIx]));
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('North, Meters')
  xlabel('East, Meters')
  title(['Vehicle Location in Earth Fixed Coordinates',str]);
  grid on

