close all
clear all
%cmd = 'NavigationControlLoopTest > foo';
%system(cmd)
res = load('foo');
Lat = res(:,1);
Lon = res(:,2);
GPSLat = res(:,3);
GPSLon = res(:,4);
TargLat = res(:,5);
TargLon = res(:,6);

ThrustRequest = res(:,7)
COG_Targ = res(:,8);
DistToTarg = res(:,9);
figure
plot(Lon,Lat,'-x',GPSLon,GPSLat,'-o',TargLon,TargLat,'ko');
xlabel('Longitude')
ylabel('Latitude')
legend('Actual Path','GPS Reports','Target Positions');

figure('Position',[  1000         559         560         420]);
subplot(2,1,1)
plot(1:length(ThrustRequest),[ThrustRequest COG_Targ]);
xlabel('time (s)');
ylabel('Thrust Direction Request (degrees)');
legend('Thrust Request','Bearing To Target');
subplot(2,1,2)
plot(1:length(ThrustRequest),DistToTarg);
xlabel('time (s)');
ylabel('Distance To Target (m)');