%    Copyright (C) 1995-1998
%    Massachussetts Institute of Technology
%    Sea Grant AUV Laboratory
%    $Id: rdiwh.m,v 1.1 2000/01/10 19:08:28 rob Exp $

ind = find(finite(data(:,rdi_range1)));

r   = cos(data(ind,m_roll));
p   = cos(data(ind,m_pitch));
rpo = cos(data(ind,m_roll)+20*pi/180);
rmo = cos(data(ind,m_roll)-20*pi/180);
ppo = cos(data(ind,m_pitch)+20*pi/180);
pmo = cos(data(ind,m_pitch)-20*pi/180);

subplot( 2, 1, 1 )
plot(data(ind,m_present_time),data(ind,rdi_range1), ...
    data(ind,m_present_time), data(ind,rdi_range2), ...
    data(ind,m_present_time), data(ind,rdi_range3), ...
    data(ind,m_present_time), data(ind,rdi_range4) );
legend( 'r1', 'r2', 'r3', 'r4' )
zoom on
ylabel( 'ranges 1-4 (m)' )

subplot( 2, 1, 2 )
plot(data(ind,m_present_time), data(ind,rdi_vel1), ...
    data(ind,m_present_time), data(ind,rdi_vel2), ...
    data(ind,m_present_time), data(ind,rdi_vel3), ...
    data(ind,m_present_time), data(ind,rdi_vel4) );

legend( 'v1', 'v2', 'v3', 'v4' )
zoom on
ylabel( 'vel 1-4 (m/s)' )
