% tomsplot.m
% Plot DVL and CrossBow heading
% Francois Cazenave 06/13/06

clear
close all
loadData
review
close all

CrssBwTime=dataNav(:,1);
DVLTime=dataDvl(:,1);
CrssBwHeading=mod360(dataNav(:,mPsi)*180/pi);
DVLHeading=mod360(dvlPsi);

subplot(2,1,1);
plot(CrssBwTime,CrssBwHeading);
ylabel('Heading, Deg')
title(['CrossBow, Degrees',str])
grid on
zoom on

subplot(2,1,2);
plot(DVLTime,DVLHeading);
ylabel('Heading, Deg')
xlabel('Time, s')
title(['Dvl AHRS, Degrees',str])
grid on
zoom on

figure

plot(CrssBwTime,CrssBwHeading,'b');
hold on
plot(DVLTime,DVLHeading,'r');

ylabel('Heading, Deg')
xlabel('Time, s')
title(['Dvl AHRS in red and CrossBow in blue, Degrees',str])
grid on
zoom on

j=2;
for i=1:length(DVLTime);
    CrssBwTimeShort(i)=CrssBwTime(j);
    CrssBwHeadingShort(i)=CrssBwHeading(j);
    j=j+3;
end
CrssBwTimeShort=CrssBwTimeShort';
CrssBwHeadingShort=CrssBwHeadingShort';

Difference=DVLHeading-CrssBwHeadingShort;
Difference(find(Difference < -100))=Difference(find(Difference < -100))+360
% figure
% 
% subplot(2,1,1)
% plot(dataDvl(:,1), mod360(dvlPsi));
% ylabel('Heading, Deg')
%   title(['Dvl AHRS, Degrees',str])
%   grid on
%   zoom on
%   xlim = get(gca,'XLim');
%   xlim(1) = 0;
%   set(gca,'XLim',xlim);
% heading2