Coverage for src/gncpy/wgs84.py: 0%

1"""Constants and utility functions relating to the WGS-84 model.""" 

2import numpy as np 

3from warnings import warn 

4 

5 

6MU = 3.986005 * 10**14 # m^3/s^2 

7SPEED_OF_LIGHT = 2.99792458 * 10**8 # m/s 

8EARTH_ROT_RATE = 7.2921151467 * 10**-5 # rad/s 

9PI = 3.1415926535898 

10FLATTENING = 1 / 298.257223563 

11ECCENTRICITY = np.sqrt(FLATTENING * (2 - FLATTENING)) 

12EQ_RAD = 6378137 # m 

13POL_RAD = EQ_RAD * (1 - FLATTENING) 

14GRAVITY = 9.7803253359 

15 

16 

17_egm_lut = np.array([]) 

18 

19 

20def calc_earth_rate(lat): 

21 """ Calculate the earth rate 

22 

23 Args: 

24 lat (float): Latitude in radians 

25 Returns: 

26 (3 x 1 numpy array): Earth rate in radians 

27 """ 

28 return EARTH_ROT_RATE * np.array([[np.cos(lat)], [0], [-np.sin(lat)]]) 

29 

30 

31def calc_transport_rate(v_N, alt, lat): 

32 """ Calculates the transport rate 

33 

34 Args: 

35 v_N (3 numpy array): Velocity in the NED frame in m/s 

36 alt (float): Altitude in meters 

37 lat (float): Latitude in radians 

38 Returns: 

39 (3 x 1 numpy array): transport rate in rad/s 

40 """ 

41 rn = calc_ns_rad(lat) 

42 re = calc_ew_rad(lat) 

43 return np.array([v_N[1] / (re + alt), 

44 -v_N[0] / (rn + alt), 

45 -v_N[1] * np.tan(lat) / (re + alt)]) 

46 

47 

48def calc_ns_rad(lat): 

49 """ Calculates the North/South radius 

50 

51 Args: 

52 lat (float) latitude in radians 

53 Returns: 

54 (float): North/South radius in meters 

55 """ 

56 return EQ_RAD * (1 - ECCENTRICITY**2) / (1 - ECCENTRICITY**2 

57 * np.sin(lat)**2)**1.5 

58 

59 

60def calc_ew_rad(lat): 

61 """ Calculates the East/West radius 

62 

63 Args: 

64 lat (float) latitude in radians 

65 Returns: 

66 (float): East/West radius in meters 

67 """ 

68 return EQ_RAD / np.sqrt(1 - ECCENTRICITY**2 * np.sin(lat)**2) 

69 

70 

71def calc_gravity(lat, alt): 

72 """ Calculates gravity vector in NED coordinates 

73 

74 Args: 

75 lat (float): Latitude in radians 

76 alt (float): Altitude in meters 

77 Returns: 

78 (3 x 1 numpy array): Gravity vector in NED frame 

79 """ 

80 frac = alt / EQ_RAD 

81 g0 = GRAVITY / np.sqrt(1 - FLATTENING * (2 - FLATTENING) 

82 * np.sin(lat)**2) * (1 + 0.0019311853 

83 * np.sin(lat)**2) 

84 ch = 1 - 2 * (1 + FLATTENING + (EQ_RAD**3 * (1 - FLATTENING) 

85 * EARTH_ROT_RATE**2) / MU) * frac + 3 * frac**2 

86 g = ch * g0 

87 if isinstance(g, np.ndarray): 

88 g = g.item() 

89 return np.array([[0], [0], [g]]) 

90 

91 

92def init_egm_lookup_table(bin_file): 

93 global _egm_lut 

94 warn('Lookup table has not been implemented yet') 

95 _egm_lut = np.array([]) 

96 

97 

98def convert_wgs_to_msl(lat, lon, alt): 

99 global _egm_lut 

100 if _egm_lut.size == 0: 

101 warn('EGM table was not loaded. Can not convert to height above geoid') 

102 return alt 

103 else: 

104 raise NotImplemented 

105 # row, col = (None, None) 

106 # return alt - _egm_lut[row, col] 

107 

108 

109def convert_msl_to_wgs(lat, lon, alt): 

110 global _egm_lut 

111 if _egm_lut.size == 0: 

112 warn('EGM table was not loaded. Can not convert to wgs84 altitude') 

113 return alt 

114 else: 

115 raise NotImplemented 

116 # row, col = (None, None) 

117 # return alt + _egm_lut[row, col]