classdef WGS84 < matlab.unittest.TestCase
%WGS84 World Geodetic System
% World Geodetic System 1984 (2004 edition)
% Copyright(c) 2014 Jonathan Kipling Knight (<a href="matlab:
% web('mailto:drkipknight@aol.com')">drkipknight@aol.com</a>)
% For more information, see the final <a href="matlab:
% web('http://earth-info.nga.mil/GandG/publications/tr8350.2/wgs84fin.pdf')">specification</a>.
  properties (Constant)
    muWithAtm = 3986004.418e+8; % mass of Earth with atmosphere (m^3/kg/s^2) Paragraph 3.2.3.1 Eqn 3-3
    muAtm = 3.5e+8; % mass of atmosphere (m^3/kg/s^2) Paragraph 3.2.3.3 Eqn 3-4
    sqrtmuWithAtm = sqrt(WGS84.muWithAtm); % sqrt of muWithAtm (m^1.5/kg^0.5/s)
    mass = WGS84.muWithAtm/6.673e-11; % mass of Earth with atmosphere (kg)
    
    normalGravityAtPole = 9.8321849378; % normal gravitational acceleration at poles (m/s^2) from Table 3-4
    normalGravityAtEquator = 9.7803253359; % normal gravitational acceleration at equator (m/s^2) from Table 3-4
    normalGravityPotential = 62636851.7146; % normal gravitational potential (m^2/s^2) from Table 3-4
    
    equatorialRadius = 6378137.0; % Equatorial radius (m) Paragraph 3.2.1 Eqn 3-1
    inverseFlattening = 298.257223563; % inverse of flattening Paragraph 3.2.2 
    sqrt1minusEcc2 = 1.0 - 1.0 / WGS84.inverseFlattening; % sqrt(1-e^2)
    oneMinusEcc2 = WGS84.sqrt1minusEcc2*WGS84.sqrt1minusEcc2; % 1-e^2
    bodyEccentricity2 = 1.0 - WGS84.oneMinusEcc2; % body eccentricity
    polarRadius = WGS84.equatorialRadius * WGS84.sqrt1minusEcc2; % polar radius (m)
    
    angularVelocity = 7292115.0000e-11; % Angular velocity (radians/second) Paragraph 3.2.4 Eqn 3-6
    angularVelocityPrime = 7292115.1467e-11; % Angular velocity prime (radians/second) 3.2.4 Eqn 3-7
    
    G873Epoch = 2450448.5; % Julian UTC day of the G873 Epoch
    % Site list for G873 set. List of Name,Number,X,Y,Z,lat,lon,alt.
    SetG873 = ...
    { ...
      {'Colorado Springs'       ,85128, ...
          -1248597.221,-4819433.246, 3976500.193, ...
          38.80305475,255.47540928,1911.757}, ...
      {'Ascension'              ,85129, ...
          6118524.214,-1572350.829, -876464.089, ...
          -7.95133048,345.58786992, 106.654}, ...
      {'Diego Garcia(<2 Mar 97)',85130, ...
          1917032.190, 6029782.349, -801376.113, ...
          -7.26654999, 72.36312094, -63.667}, ...
      {'Diego Garcia(>2 Mar 97)',85130, ...
          1916179.323, 6029998.996, -801737.517, ...
          -7.26985101, 72.37107552, -69.407}, ...
      {'Kwajalein'              ,85131, ...
          -6160884.561, 1339851.686,  960842.977, ...
          8.72249973,167.73052910,  40.039}, ...
      {'Hawaii'                 ,85132, ...
          -5511982.282,-2200248.096, 2329481.654, ...
          21.56148998,201.76067177, 428.234}, ...
      {'Australia'              ,85402, ...
          -3939181.976, 3467075.383,-3613221.035, ...
          -34.72900255,138.64734382,  38.183}, ...
      {'Argentina'              ,85403, ...
          2745499.094,-4483636.553,-3599054.668, ...
          -34.57370240,301.48070108,  48.781}, ...
      {'England'                ,85404, ...
          3981776.718,  -89239.153, 4965284.609, ...
          51.45374207,358.71610807, 163.113}, ...
      {'Bahrain'                ,85405, ...
          3633910.911, 4425277.706, 2799862.677, ...
          26.20913892, 50.60814318, -13.853}, ...
      {'Ecuador'                ,85406, ...
          1272867.278,-6252772.267,  -23801.890, ...
          -0.21515827,281.50639133,2922.667}, ...
      {'US Naval Observatory'   ,85407, ...
          1112168.441,-4842861.714, 3985487.203, ...
          38.92045014,282.93377605,  59.169}, ...
      {'China'                  ,85409, ...
          -2148743.914, 4426641.465, 4044656.101, ...
          39.60860198,115.89248252,  87.642}, ...
    };

    G1150Epoch = 2451909.5; % Julian UTC day of the G1150 Epoch
    % Site list for G1150 set. List of Name,Number,X,Y,Z,lat,lon,alt.
    SetG1150 = ...
    { ...
      {'Colorado Springs'       ,85128, ...
          -1248597.295,-4819433.239, 3976500.175, ...
          38.80305456,255.47540844,1911.755}, ...
      {'Ascension'              ,85129, ...
          6118524.122,-1572350.853, -876463.990, ...
          -7.95132970,345.58786950, 106.558}, ...
      {'Diego Garcia'           ,85130, ...
          1916197.142, 6029999.007, -801737.366, ...
          -7.26984347, 72.37092177, -64.063}, ...
      {'Kwajalein'              ,85131, ...
          -6160884.370, 1339851.965,  960843.071, ...
          8.72250074,167.73052625,  39.927}, ...
      {'Hawaii'                 ,85132, ...
          -5511980.484,-2200247.093, 2329480.952, ...
          21.56149086,201.76066922, 426.077}, ...
      {'Cape Canaveral'         ,85143, ...
          918988.120,-5534552.966, 3023721.377, ...
          28.48373800,279.42769549, -24.005}, ...
    };

  end
  methods
    function model = WGS84()
    end
    [ pos, vel ] = cartesianCoordinates( model, lla, dlla );
    [ lla, dlla ] = geodeticCoordinates( model, pos, vel );
    [ g ] = surfaceGravity( model, geodeticlatitude );
  end
  methods (Static)
    w = angularVelocityStar( JD );
    pot = centrifugalPotential( position, w );
    g = centrifugalGravity( position, w );
  end
  methods (Test)
    function testG873(testCase)
      % test to verify converting between Cartesian and Geodetic using G873 dataset
      velsG873 = {[ 1063878.319779,  136236.229454,495825.721708], ...
                  [  399368.972932, 1201887.374249,627584.188088], ...
                  [-1181673.596491,  459785.145434,628550.294289], ...
                  [-1181716.581457,  459652.396832,628545.197642], ...
                  [ -174065.959573,-1252598.969770,626365.176423], ...
                  [  656594.045753,-1015957.234030,590050.566472], ...
                  [ -965234.525315, -548594.206031,522386.580815], ...
                  [ 1085079.928227,  241503.767212,523353.674207], ...
                  [ -480597.180721,  807526.439756,397237.871043], ...
                  [-1062973.106817,  510119.017846,569521.728316], ...
                  [ 1251029.187086,  252241.396346,633831.821194], ...
                  [  879130.923898,  611900.254220,494873.614935], ...
                  [ -708222.736160, -794605.104801,490098.849329]};
      dlla = [0.1,0.2,-0.3];

      torad = pi/180.0;
      model = WGS84();
      for i=1:length(WGS84.SetG873)
        lla = [WGS84.SetG873{i}{6}*torad,WGS84.SetG873{i}{7}*torad,WGS84.SetG873{i}{8}];
        if lla(2) > pi
          lla(2) = lla(2) - 2*pi;
        end
        pos = [WGS84.SetG873{i}{3},WGS84.SetG873{i}{4},WGS84.SetG873{i}{5}];
        vel = velsG873{i};
        [ pos1, vel1 ] = model.cartesianCoordinates( lla, dlla );
        [ lla1, dlla1 ] = model.geodeticCoordinates( pos, vel );
        str = sprintf('%s lat off %f != %f (%e)\n',...
          WGS84.SetG873{i}{1},lla(1)/torad,lla1(1)/torad,(lla(1)-lla1(1))/torad);
        testCase.verifyEqual(lla(1),lla1(1),'AbsTol',1.0e-8*torad,str);
        str = sprintf('%s lon off %f != %f (%e)\n',...
          WGS84.SetG873{i}{1},lla(2)/torad,lla1(2)/torad,(lla(2)-lla1(2))/torad);
        testCase.verifyEqual(lla(2),lla1(2),'AbsTol',1.0e-8*torad,str);
        str = sprintf('%s alt off %f != %f (%e)\n',...
          WGS84.SetG873{i}{1},lla(3),lla1(3),(lla(3)-lla1(3)));
        testCase.verifyEqual(lla(3),lla1(3),'AbsTol',1.0e-3,str);
        str = sprintf('%s x off %f != %f (%e)\n',...
          WGS84.SetG873{i}{1},pos(1),pos1(1),(pos(1)-pos1(1)));
        testCase.verifyEqual(pos(1),pos1(1),'AbsTol',1.0e-3,str);
        str = sprintf('%s y off %f != %f (%e)\n',...
          WGS84.SetG873{i}{1},pos(2),pos1(2),(pos(2)-pos1(2)));
        testCase.verifyEqual(pos(2),pos1(2),'AbsTol',1.0e-3,str);
        str = sprintf('%s z off %f != %f (%e)\n',...
          WGS84.SetG873{i}{1},pos(3),pos1(3),(pos(3)-pos1(3)));
        testCase.verifyEqual(pos(3),pos1(3),'AbsTol',1.0e-3,str);
        str = sprintf('%s dlat off %f != %f (%e)\n',...
          WGS84.SetG873{i}{1},dlla(1)/torad,dlla1(1)/torad,(dlla(1)-dlla1(1))/torad);
        testCase.verifyEqual(dlla(1),dlla1(1),'AbsTol',1.0e-8*torad,str);
        str = sprintf('%s dlon off %f != %f (%e)\n',...
          WGS84.SetG873{i}{1},dlla(2)/torad,dlla1(2)/torad,(dlla(2)-dlla1(2))/torad);
        testCase.verifyEqual(dlla(2),dlla1(2),'AbsTol',1.0e-8*torad,str);
        str = sprintf('%s dalt off %f != %f (%e)\n',...
          WGS84.SetG873{i}{1},dlla(3),dlla1(3),(dlla(3)-dlla1(3)));
        testCase.verifyEqual(dlla(3),dlla1(3),'AbsTol',1.0e-3,str);
        str = sprintf('%s dx off %f != %f (%e)\n',...
          WGS84.SetG873{i}{1},vel(1),vel1(1),(vel(1)-vel1(1)));
        testCase.verifyEqual(vel(1),vel1(1),'AbsTol',1.0e-5,str);
        str = sprintf('%s dy off %f != %f (%e)\n',...
          WGS84.SetG873{i}{1},vel(2),vel1(2),(vel(2)-vel1(2)));
        testCase.verifyEqual(vel(2),vel1(2),'AbsTol',1.0e-5,str);
        str = sprintf('%s dz off %f != %f (%e)\n',...
          WGS84.SetG873{i}{1},vel(3),vel1(3),(vel(3)-vel1(3)));
        testCase.verifyEqual(vel(3),vel1(3),'AbsTol',1.0e-5,str);
      end
    end
    function testNearCenter(testCase)
      % test to verify conversion near the Earth's center
      torad = pi/180.0;
      model = WGS84();
      lla = [40*torad,-105*torad,-WGS84.equatorialRadius];
      dlla = [0.1,0.2,-0.3];
      pos = [-1752.513906,-6540.470938,-21801.864283];
      vel = [1036.625361,-1363.638233,-1250.467268];
      [ pos1, vel1 ] = model.cartesianCoordinates( lla, dlla );
      [ lla1, dlla1 ] = model.geodeticCoordinates( pos, vel );
      str = sprintf('%s lat off %f != %f (%e)\n','near center',...
          lla(1)/torad,lla1(1)/torad,(lla(1)-lla1(1))/torad);
      testCase.verifyEqual(lla(1),lla1(1),'AbsTol',1.0e-8*torad,str);
      str = sprintf('%s lon off %f != %f (%e)\n','near center',...
          lla(2)/torad,lla1(2)/torad,(lla(2)-lla1(2))/torad);
      testCase.verifyEqual(lla(2),lla1(2),'AbsTol',1.0e-8*torad,str);
      str = sprintf('%s alt off %f != %f (%e)\n','near center',...
          lla(3),lla1(3),(lla(3)-lla1(3)));
      testCase.verifyEqual(lla(3),lla1(3),'AbsTol',1.0e-3,str);
      str = sprintf('%s x off %f != %f (%e)\n','near center',...
          pos(1),pos1(1),(pos(1)-pos1(1)));
      testCase.verifyEqual(pos(1),pos1(1),'AbsTol',1.0e-3,str);
      str = sprintf('%s y off %f != %f (%e)\n','near center',...
          pos(2),pos1(2),(pos(2)-pos1(2)));
      testCase.verifyEqual(pos(2),pos1(2),'AbsTol',1.0e-3,str);
      str = sprintf('%s z off %f != %f (%e)\n','near center',...
          pos(3),pos1(3),(pos(3)-pos1(3)));
      testCase.verifyEqual(pos(3),pos1(3),'AbsTol',1.0e-3,str);
      str = sprintf('%s dlat off %f != %f (%e)\n','near center',...
          dlla(1)/torad,dlla1(1)/torad,(dlla(1)-dlla1(1))/torad);
      testCase.verifyEqual(dlla(1),dlla1(1),'AbsTol',1.0e-8*torad,str);
      str = sprintf('%s dlon off %f != %f (%e)\n','near center',...
          dlla(2)/torad,dlla1(2)/torad,(dlla(2)-dlla1(2))/torad);
      testCase.verifyEqual(dlla(2),dlla1(2),'AbsTol',1.0e-8*torad,str);
      str = sprintf('%s dalt off %f != %f (%e)\n','near center',...
          dlla(3),dlla1(3),(dlla(3)-dlla1(3)));
      testCase.verifyEqual(dlla(3),dlla1(3),'AbsTol',1.0e-4,str);
      str = sprintf('%s dx off %f != %f (%e)\n','near center',...
          vel(1),vel1(1),(vel(1)-vel1(1)));
      testCase.verifyEqual(vel(1),vel1(1),'AbsTol',1.0e-5,str);
      str = sprintf('%s dy off %f != %f (%e)\n','near center',...
          vel(2),vel1(2),(vel(2)-vel1(2)));
      testCase.verifyEqual(vel(2),vel1(2),'AbsTol',1.0e-5,str);
      str = sprintf('%s dz off %f != %f (%e)\n','near center',...
          vel(3),vel1(3),(vel(3)-vel1(3)));
      testCase.verifyEqual(vel(3),vel1(3),'AbsTol',1.0e-5,str);
    end
    function testNorthPole(testCase)
      % test to verify conversion at the North Pole
      torad = pi/180.0;
      model = WGS84();
      lla = [90*torad,0*torad,WGS84.equatorialRadius];
      dlla = [0.1,0.2,-0.3];
      pos = [0.0,0.0,WGS84.polarRadius+WGS84.equatorialRadius];
      vel = [-1277773.062576,0.0,-0.3];
      [ pos1, vel1 ] = model.cartesianCoordinates( lla, dlla );
      [ lla1, dlla1 ] = model.geodeticCoordinates( pos, vel );
      str = sprintf('%s lat off %f != %f (%e)\n','near pole',...
          lla(1)/torad,lla1(1)/torad,(lla(1)-lla1(1))/torad);
      testCase.verifyEqual(lla(1),lla1(1),'AbsTol',1.0e-8*torad,str);
      str = sprintf('%s lon off %f != %f (%e)\n','near pole',...
          lla(2)/torad,lla1(2)/torad,(lla(2)-lla1(2))/torad);
      testCase.verifyEqual(lla(2),lla1(2),'AbsTol',1.0e-8*torad,str);
      str = sprintf('%s alt off %f != %f (%e)\n','near pole',...
          lla(3),lla1(3),(lla(3)-lla1(3)));
      testCase.verifyEqual(lla(3),lla1(3),'AbsTol',1.0e-3,str);
      str = sprintf('%s x off %f != %f (%e)\n','near pole',...
          pos(1),pos1(1),(pos(1)-pos1(1)));
      testCase.verifyEqual(pos(1),pos1(1),'AbsTol',1.0e-3,str);
      str = sprintf('%s y off %f != %f (%e)\n','near pole',...
          pos(2),pos1(2),(pos(2)-pos1(2)));
      testCase.verifyEqual(pos(2),pos1(2),'AbsTol',1.0e-3,str);
      str = sprintf('%s z off %f != %f (%e)\n','near pole',...
          pos(3),pos1(3),(pos(3)-pos1(3)));
      testCase.verifyEqual(pos(3),pos1(3),'AbsTol',1.0e-3,str);
      str = sprintf('%s dlat off %f != %f (%e)\n','near pole',...
          dlla(1)/torad,dlla1(1)/torad,(dlla(1)-dlla1(1))/torad);
      testCase.verifyEqual(dlla(1),dlla1(1),'AbsTol',1.0e-1*torad,str);
      str = sprintf('%s dlon off %f != %f (%e)\n','near pole',...
          dlla(2)/torad,dlla1(2)/torad,(dlla(2)-dlla1(2))/torad);
      testCase.verifyEqual(dlla(2),dlla1(2),'AbsTol',1.0e-1*torad,str);
      str = sprintf('%s dalt off %f != %f (%e)\n','near pole',...
          dlla(3),dlla1(3),(dlla(3)-dlla1(3)));
      testCase.verifyEqual(dlla(3),dlla1(3),'AbsTol',1.0e-4,str);
      str = sprintf('%s dx off %f != %f (%e)\n','near pole',...
          vel(1),vel1(1),(vel(1)-vel1(1)));
      testCase.verifyEqual(vel(1),vel1(1),'AbsTol',1.0e-5,str);
      str = sprintf('%s dy off %f != %f (%e)\n','near pole',...
          vel(2),vel1(2),(vel(2)-vel1(2)));
      testCase.verifyEqual(vel(2),vel1(2),'AbsTol',1.0e-5,str);
      str = sprintf('%s dz off %f != %f (%e)\n','near pole',...
          vel(3),vel1(3),(vel(3)-vel1(3)));
      testCase.verifyEqual(vel(3),vel1(3),'AbsTol',1.0e-5,str);
    end
  end
end

