% ShoreHeightCalibration.m
%
% Adam S. Frankel 2007
% Hawai'i Marine Mammal Consortium
%
% This program is written to calculate the shorestation height, given a known GPS location at the waterline.
% 

clf

EyeHtInches = 60; %61	% Eye Height in inches
StationHtV = 40:90;%66:0.01:68; %40:90	% a range of candidate elevation values that the program will test

% sample data provided for demonstration
% declination angle, sighting number, GPS latitude (decimal minutes), GPS longitude (decimal minutes)
data = [
95.52777777777780	1	4.647	52.049
95.53194444444440	2	4.646	52.049
94.50277777777780	3	4.687	52.2
94.49027777777780	4	4.688	52.202
93.44444444444440	5	4.737	52.386
93.45694444444440	6	4.736	52.385
92.20416666666670	7	4.879	52.766
92.20416666666670	8	4.88	52.767
91.80138888888890	9	5.162	52.958
91.80277777777780	10	5.163	52.958
91.37222222222220	11	5.39	53.286
91.36527777777780	12	5.397	53.289
91.12777777777780	13	5.635	53.556
91.12777777777780	14	5.64	53.558
90.85694444444440	15	6.155	53.984
90.84722222222220	16	5.999	54.103
90.84722222222220	17	5.994	54.107
90.81388888888890	18	5.644	54.363
90.81250000000000	19	5.636	54.366
90.77222222222220	20	4.715	54.6
90.77222222222220	21	4.709	54.603
90.83611111111110	22	4.529	54.361
90.84027777777780	23	4.524	54.361
90.91666666666670	24	4.571	54.112
90.92361111111110	25	4.57	54.111
91.13333333333330	26	4.704	53.681
91.12916666666670	27	4.703	53.681
91.36527777777780	28	4.822	53.366
91.36111111111110	29	4.82	53.366
91.47222222222220	30	4.874	53.252
91.46944444444440	31	4.877	53.251
91.77500000000000	32	4.983	53.003
91.78194444444440	33	4.986	52.997
92.18333333333330	34	5.024	52.773
92.17222222222220	35	5.028	52.77
92.52500000000000	36	5.106	52.623
92.51944444444440	37	5.108	52.628
93.28888888888890	38	4.62	52.366
93.25277777777780	39	4.62	52.368
92.03750000000000	40	4.377	52.68
92.03333333333330	41	4.379	52.681
91.41250000000000	42	4.122	53.059
91.41527777777780	43	4.127	53.059
];    

% Known location for the theodolite (decimal mintues)

ShoreLat = 4.925283850520;
ShoreLon =  51.794984516976;

Eyeheight = EyeHtInches/39.37; %m

% constants
EarthRadius = 6371000.0; % m

BoatLat =  data(:,3); % decimal minutes
BoatLon = data(:,4); % decimal minutes
Declination =  data(:,1);% degrees

DeltaLat = abs(ShoreLat-BoatLat);
DeltaLon = abs(ShoreLon-BoatLon);

DeltaY = DeltaLat * 1852 ; % m
DeltaX = DeltaLon * 1852 * cos(deg2rad(ShoreLat)); % m

range = sqrt(DeltaX.^2 + DeltaY.^2);

% loop through and create matrix of distances for each alitutde

for i=1:length(StationHtV)

StHt = EarthRadius + StationHtV(i) + Eyeheight; 
DecRadians = deg2rad(Declination);
SinAngleB = ((StHt .* (sin(DecRadians)))/EarthRadius); 


  if SinAngleB >= 1.000 % {If this angle is >= 1.0, then vessel is above the horizon
      SinAngleB = 0.999999999; 
      ErrorCondition = 1;    %{Flag used to later indicate a bad, bad fix.} 
  end; 	% if
 
  AngleB = asin(SinAngleB); 
  EarthCenterAngle =(pi-(AngleB + DecRadians)); 
  ArcLength = EarthRadius * EarthCenterAngle; 
  DistanceM(:,i) = abs (ArcLength); 

  errorM(:,i) = range-DistanceM(:,i);

end	% for
  


figure(1)
plot(DistanceM)
hold on
plot(range,'r')
hold off
legend('Theo', 'GPS')
xlabel ('fix Number')
ylabel ('Range')

figure(2)
plot(range,DistanceM,'.')
xlabel('GPS range')
ylabel('Theo range')

%figure(3)
%plot(range,resids,'.')

figure(3)
clf
    hold on
    plot(range,'r','Linewidth',2)

for i=1:length(data)

    plot(DistanceM(:,i))
end
  plot(range,'r','Linewidth',2)
xlabel('Measurmement Number')
ylabel('Distance from shore to boat in meters')
legend('GPS','Theo')

me=mean(errorM);

rmsErr=rms(errorM);

figure(5)
plot(StationHtV,rmsErr);
xlabel('Candidate Station Height')
ylabel('RMS error')

[y,i]=min(rmsErr)
height1=StationHtV(i)


% repeat this procedure, with a range of +/- 1 meter from the best guess in  the previous procedure.

StationHtV2= height1-1:0.01:height1+1;

for i=1:length(StationHtV2)

StHt = EarthRadius + StationHtV2(i) + Eyeheight; 
DecRadians = deg2rad(Declination);
SinAngleB = ((StHt .* (sin(DecRadians)))/EarthRadius); 


  if SinAngleB >= 1.000 % {If this angle is >= 1.0, then vessel is above the horizon
      SinAngleB = 0.999999999; 
      ErrorCondition = 1;    %{Flag used to later indicate a bad, bad fix.} 
  end; 
 
  AngleB = asin(SinAngleB); 
  EarthCenterAngle =(pi-(AngleB + DecRadians)); 
  ArcLength = EarthRadius * EarthCenterAngle; 
  DistanceM(:,i) = abs (ArcLength); 

  errorM2(:,i) = range-DistanceM(:,i);

end

me2=mean(errorM2);

rmsErr2=rms(errorM2);

figure(6)
plot(StationHtV2,rmsErr2);
xlabel('Candidate Station Height')
ylabel('RMS error')

[y,i]=min(rmsErr2)
height1=StationHtV2(i)

figure(7)
clf
subplot(1,2,1)
plot(StationHtV,rmsErr);
xlabel('Candidate Station Height (m)')
ylabel('RMS error')
text(45,1300,'A')

subplot(1,2,2)
plot(StationHtV2,rmsErr2);
xlabel('Candidate Station Height (m)')
ylabel('RMS error')
text (66.2,65,'B')


% normalized error

for i=1:length(range)
    NerrorM(i,:)=errorM(i,:)./range(i);
    NerrorM2(i,:)=errorM2(i,:)./range(i);
end


NeM = rms(NerrorM);
NeM2 = rms(NerrorM2);

figure(8)
clf
subplot(1,2,1)
plot(StationHtV,NeM);
xlabel('Candidate Station Height (m)')
ylabel('Normalized RMS error')
text(45,0.35,'A')

subplot(1,2,2)
plot(StationHtV2,NeM2);
xlabel('Candidate Station Height (m)')
ylabel('Normalized RMS error')
text (66.2,0.018,'B')