
%% AUV latitude/longitude calculations based on ship GPS position, ship heading
%% and AUV bearing and range
%% Uses function bearing to calculate AUV start and end lat and lon
%% Endpoint information is:
% ship lat [deg min], ship lon [deg min], ship heading, AUV bearing, AUV range
startpt=[36 56.157 122 3.277 190 123 168];
endpt=[36 54.875 122 7.706 170 43 221];

%% Starting point
startlat=startpt(1)+startpt(2)/60; startlon=-startpt(3)-startpt(4)/60;
ship=[startlat startlon]; rng=startpt(5:7);
startlatlon=bearing(ship,rng);
disp('START');
disp(['Ship GPS : ' num2str(startlat) ', ' num2str(startlon)]);
disp(['Ship Heading: ' num2str(startpt(5))]);
disp(['AUV Bearing: ' num2str(startpt(6))]);
disp(['AUV Range: ' num2str(startpt(7))]);
disp(['AUV location : ' num2str(startlatlon(1)) ', ' num2str(startlatlon(2))]);

%% End point
disp('END')
endlat=endpt(1)+endpt(2)/60; endlon=-endpt(3)-endpt(4)/60;
ship=[endlat endlon]; rng=endpt(5:7);
endlatlon=bearing(ship,rng);
disp(['Ship GPS : ' num2str(endlat) ', ' num2str(endlon)]);
disp(['Ship Heading: ' num2str(endpt(5))]);
disp(['AUV Bearing: ' num2str(endpt(6))]);
disp(['AUV Range: ' num2str(endpt(7))]);
disp(['AUV location : ' num2str(endlatlon(1)) ', ' num2str(endlatlon(2))]);
