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

1"""Implements functions for useful coordinate transforms.""" 

2import numpy as np 

3 

4import gncpy.wgs84 as wgs84 

5 

6 

7def ecef_to_LLA(pos): 

8 """Convert an ECEF position to LLA coordinates. 

9 

10 Parameters 

11 ---------- 

12 pos : 3 x 1 numpy array 

13 Position in meters in the ECEF frame 

14 

15 Returns 

16 ------- 

17 3 x 1 numpy array 

18 Latitude (rad), Longitude (rad), Altitude (m) 

19 """ 

20 xyz = pos.copy().squeeze() 

21 lon = np.arctan2(xyz[1], xyz[0]) 

22 

23 p = np.sqrt(xyz[0] ** 2 + xyz[1] ** 2) 

24 E = np.sqrt(wgs84.EQ_RAD ** 2 - wgs84.POL_RAD ** 2) 

25 F = 54 * (wgs84.POL_RAD * xyz[2]) ** 2 

26 G = ( 

27 p ** 2 

28 + (1 - wgs84.ECCENTRICITY ** 2) * (xyz[2] ** 2) 

29 - (wgs84.ECCENTRICITY * E) ** 2 

30 ) 

31 c = wgs84.ECCENTRICITY ** 4 * F * p ** 2 / G ** 3 

32 s = (1 + c + np.sqrt(c ** 2 + 2 * c)) ** (1 / 3) 

33 P = (F / (3 * G ** 2)) / (s + 1 / s + 1) ** 2 

34 Q = np.sqrt(1 + 2 * wgs84.ECCENTRICITY ** 4 * P) 

35 k1 = -P * wgs84.ECCENTRICITY ** 2 * p / (1 + Q) 

36 k2 = 0.5 * wgs84.EQ_RAD ** 2 * (1 + 1 / Q) 

37 k3 = -P * (1 - wgs84.ECCENTRICITY ** 2) * xyz[2] ** 2 / (Q * (1 + Q)) 

38 k4 = -0.5 * P * p ** 2 

39 k5 = p - wgs84.ECCENTRICITY ** 2 * (k1 + np.sqrt(k2 + k3 + k4)) 

40 U = np.sqrt(k5 ** 2 + xyz[2] ** 2) 

41 V = np.sqrt(k5 ** 2 + (1 - wgs84.ECCENTRICITY ** 2) * xyz[2] ** 2) 

42 alt = U * (1 - wgs84.POL_RAD ** 2 / (wgs84.EQ_RAD * V)) 

43 

44 z0 = wgs84.POL_RAD ** 2 * xyz[2] / (wgs84.EQ_RAD * V) 

45 ep = wgs84.EQ_RAD / wgs84.POL_RAD * wgs84.ECCENTRICITY 

46 lat = np.arctan((xyz[2] + z0 * ep ** 2) / p) 

47 

48 return np.array([lat, lon, alt]).reshape((3, 1)) 

49 

50 

51def lla_to_ECEF(lat, lon, alt): 

52 """Convert LLA coordinates to ECEF coordinates. 

53 

54 Parameters 

55 ---------- 

56 lat : float 

57 Latitude in radians 

58 lon : float 

59 Longitude in radians 

60 alt : float 

61 Altitude in meters 

62 

63 Returns 

64 ------- 

65 3 x 1 numpy array 

66 ECEF position in meters 

67 """ 

68 re = wgs84.calc_ew_rad(lat) 

69 c_lat = np.cos(lat) 

70 s_lat = np.sin(lat) 

71 c_lon = np.cos(lon) 

72 s_lon = np.sin(lon) 

73 

74 x = (re + alt) * c_lat * c_lon 

75 y = (re + alt) * c_lat * s_lon 

76 z = ((1 - wgs84.ECCENTRICITY ** 2) * re + alt) * s_lat 

77 return np.array([x, y, z]).reshape((3, 1)) 

78 

79 

80def lla_to_NED(ref_lat, ref_lon, ref_alt, pos_lat, pos_lon, pos_alt): 

81 """Convert LLA to NED coordinates. 

82 

83 Parameters 

84 ---------- 

85 ref_lat : float 

86 Reference latitude (rad) 

87 ref_lon : float 

88 Reference longitude (rad) 

89 ref_alt : float 

90 Reference altitude (m) 

91 pos_lat : float 

92 Latitude (rad) 

93 pos_lon : float 

94 Longitude (rad) 

95 pos_alt : float 

96 Altitude (m) 

97 

98 Returns 

99 ------- 

100 3 x 1 numpy array 

101 Position in NED coordinates 

102 """ 

103 c_lat = np.cos(ref_lat) 

104 s_lat = np.sin(ref_lat) 

105 c_lon = np.cos(ref_lon) 

106 s_lon = np.sin(ref_lon) 

107 R = np.array( 

108 [ 

109 [-s_lat * c_lon, -s_lon, -c_lat * c_lon], 

110 [-s_lat * s_lon, c_lon, -c_lat * s_lon], 

111 [c_lat, 0, -s_lat], 

112 ] 

113 ) 

114 ref_E = lla_to_ECEF(ref_lat, ref_lon, ref_alt) 

115 pos_E = lla_to_ECEF(pos_lat, pos_lon, pos_alt) 

116 return R.T @ (pos_E - ref_E) 

117 

118 

119def ecef_to_NED(ref_xyz, pos_xyz): 

120 """Convert an ECEF position to the NED frame. 

121 

122 Parameters 

123 ---------- 

124 ref_xyz : 3 x 1 numpy array 

125 Reference position (m) in the ECEF frame 

126 pos_xyz : 3 x 1 numpy array 

127 Position (m) in the ECEF frame 

128 

129 Returns 

130 ------- 

131 3 x 1 numpy array 

132 Position (m) in the NED frame 

133 """ 

134 ref_LLA = ecef_to_LLA(ref_xyz).squeeze() 

135 c_lat = np.cos(ref_LLA[0]) 

136 s_lat = np.sin(ref_LLA[0]) 

137 c_lon = np.cos(ref_LLA[1]) 

138 s_lon = np.sin(ref_LLA[1]) 

139 R = np.array( 

140 [ 

141 [-s_lat * c_lon, -s_lon, -c_lat * c_lon], 

142 [-s_lat * s_lon, c_lon, -c_lat * s_lon], 

143 [c_lat, 0, -s_lat], 

144 ] 

145 ) 

146 return R.T @ (pos_xyz - ref_xyz) 

147 

148 

149def ned_to_LLA(ned, ref_lat, ref_lon, ref_alt): 

150 """Convert NED to LLA. 

151 

152 Parameters 

153 ---------- 

154 ned : numpy array 

155 NED positon. 

156 ref_lat : flaot 

157 Reference latitude (radians). 

158 ref_lon : flaot 

159 Reference longitude (radians). 

160 ref_alt : float 

161 Reference altitude (meters). 

162 

163 Returns 

164 ------- 

165 numpy array 

166 Lat/lon/alt in rad/rad/m. 

167 """ 

168 alt = ref_alt + -ned[2] 

169 if isinstance(alt, np.ndarray): 

170 alt = alt.item() 

171 f_fact = 2 * wgs84.FLATTENING - wgs84.FLATTENING ** 2 

172 s_lat2 = np.sin(ref_lat) ** 2 

173 Rn = wgs84.EQ_RAD / np.sqrt(1 - f_fact * s_lat2) 

174 Rm = Rn * ((1 - f_fact) / (1 - f_fact * s_lat2)) 

175 dlat = ned[0] * np.arctan2(1, Rm) 

176 dlon = ned[1] * np.arctan2(1, Rn * np.cos(ref_lat)) 

177 lat = ref_lat + dlat 

178 lon = ref_lon + dlon 

179 if isinstance(lat, np.ndarray): 

180 lat = lat.item() 

181 if isinstance(lon, np.ndarray): 

182 lon = lon.item() 

183 return np.array([lat, lon, alt]).reshape((3, 1))