%DR    Dead Reckon.
%    Script to plot Odyssey II dead reckoning trajectory

% Copyright (C) 1994-1997, Massachussetts Institute of Technology
% Sea Grant Underwater Vehicles Laboratory
% J. J. Leonard, B. A. Moran
% $Revision: 1.1 $

if exist( 'do_3d_plot' ) ~= 1
    do_3d_plot = 0;
end

% Because the vehicle has a little problem with position initialization,
% we're going to try and correct the first cycle if a large jump is
% detectable.

rows = 1:length(data);
step = norm( diff( data(1:2,[m_dr_north m_dr_east])) );
if step > 10
    disp( ['ignoring 1st cycle because step = ' num2str(step) ' m...'] );
    rows(1) = [];
end

cla
if do_3d_plot == 1
    plot3( data(rows,m_dr_east)/1000, data(rows,m_dr_north)/1000, ...
        data(:,m_depth), 'Color', [0 .7 0] ), set( gca, 'zdir', 'reverse' );
    line( data(rows,m_dr_east)/1000, data(rows,m_dr_north)/1000, ...
        'Color', 'r' )

    zlabel( 'depth (m)' );
else
    plot( data(rows,m_dr_east)/1000, data(rows,m_dr_north)/1000 )
    text( data(rows(1),m_dr_east)/1000, data(rows(1),m_dr_north)/1000, ...
        'S', 'FontSize', 16 );
    text( data(rows(end),m_dr_east)/1000, data(rows(end),m_dr_north)/1000, ...
        'E', 'FontSize', 16 );
    set( gca, 'DataAspectRatio', [1 1 1] );
    zoom on
end

                                % common to 2d and 3d plots
title( ['Vehicle Trajectory by Dead Reckoning: ' run_name] );
xlabel( 'East (km)' );
ylabel( 'North (km)' );

clear rows
