|
| 1 | +""" |
| 2 | +This module provides functions to convert between Geodetic coordinates and |
| 3 | +Earth-Centered, Earth-Fixed (ECEF) Cartesian coordinates, as well as calculating |
| 4 | +target coordinates based on radar measurements. |
| 5 | +
|
| 6 | +Reference: |
| 7 | +- https://en.wikipedia.org/wiki/Geographic_coordinate_conversion |
| 8 | +- https://en.wikipedia.org/wiki/Local_tangent_plane_coordinates |
| 9 | +""" |
| 10 | + |
| 11 | +import math |
| 12 | + |
| 13 | +# WGS84 Ellipsoid Constants |
| 14 | +WGS84_A = 6378137.0 # Semi-major axis in meters |
| 15 | +WGS84_B = 6356752.314245 # Semi-minor axis in meters |
| 16 | +WGS84_E_SQ = 1.0 - (WGS84_B**2 / WGS84_A**2) # First eccentricity squared |
| 17 | +WGS84_EP_SQ = (WGS84_A**2 - WGS84_B**2) / WGS84_B**2 # Second eccentricity squared |
| 18 | + |
| 19 | + |
| 20 | +def geodetic_to_ecef( |
| 21 | + lat_deg: float, lon_deg: float, alt_m: float |
| 22 | +) -> tuple[float, float, float]: |
| 23 | + """ |
| 24 | + Converts Geodetic coordinates (Latitude, Longitude, Altitude) to |
| 25 | + Earth-Centered, Earth-Fixed (ECEF) Cartesian coordinates. |
| 26 | +
|
| 27 | + >>> x, y, z = geodetic_to_ecef(0.0, 0.0, 0.0) |
| 28 | + >>> round(x, 2), round(y, 2), round(z, 2) |
| 29 | + (6378137.0, 0.0, 0.0) |
| 30 | + >>> x, y, z = geodetic_to_ecef(90.0, 0.0, 0.0) |
| 31 | + >>> round(x, 2), round(y, 2), round(z, 2) |
| 32 | + (0.0, 0.0, 6356752.31) |
| 33 | + """ |
| 34 | + lat_rad = math.radians(lat_deg) |
| 35 | + lon_rad = math.radians(lon_deg) |
| 36 | + |
| 37 | + sin_lat = math.sin(lat_rad) |
| 38 | + cos_lat = math.cos(lat_rad) |
| 39 | + |
| 40 | + # N is the prime vertical radius of curvature |
| 41 | + n_radius = WGS84_A / math.sqrt(1.0 - WGS84_E_SQ * sin_lat**2) |
| 42 | + |
| 43 | + # Calculate ECEF X, Y, Z |
| 44 | + x = (n_radius + alt_m) * cos_lat * math.cos(lon_rad) |
| 45 | + y = (n_radius + alt_m) * cos_lat * math.sin(lon_rad) |
| 46 | + z = (n_radius * (1.0 - WGS84_E_SQ) + alt_m) * sin_lat |
| 47 | + |
| 48 | + return x, y, z |
| 49 | + |
| 50 | + |
| 51 | +def ecef_to_geodetic( |
| 52 | + x_ecef: float, y_ecef: float, z_ecef: float |
| 53 | +) -> tuple[float, float, float]: |
| 54 | + """ |
| 55 | + Converts Earth-Centered, Earth-Fixed (ECEF) coordinates to |
| 56 | + Geodetic coordinates (Latitude, Longitude, Altitude) using Bowring's method. |
| 57 | +
|
| 58 | + >>> lat, lon, alt = ecef_to_geodetic(6378137.0, 0.0, 0.0) |
| 59 | + >>> round(lat, 2), round(lon, 2), round(alt, 2) |
| 60 | + (0.0, 0.0, 0.0) |
| 61 | + >>> lat, lon, alt = ecef_to_geodetic(0.0, 0.0, 6356752.314245) |
| 62 | + >>> round(lat, 2), round(lon, 2), round(alt, 2) |
| 63 | + (90.0, 0.0, 0.0) |
| 64 | + """ |
| 65 | + p = math.sqrt(x_ecef**2 + y_ecef**2) |
| 66 | + |
| 67 | + # Handle the special case where the point is exactly at the poles |
| 68 | + if p == 0: |
| 69 | + lon_deg = 0.0 |
| 70 | + lat_deg = 90.0 if z_ecef > 0 else -90.0 |
| 71 | + alt_m = abs(z_ecef) - WGS84_B |
| 72 | + return lat_deg, lon_deg, alt_m |
| 73 | + |
| 74 | + theta = math.atan2(z_ecef * WGS84_A, p * WGS84_B) |
| 75 | + |
| 76 | + sin_theta = math.sin(theta) |
| 77 | + cos_theta = math.cos(theta) |
| 78 | + |
| 79 | + # Calculate exact latitude and longitude |
| 80 | + lon_rad = math.atan2(y_ecef, x_ecef) |
| 81 | + lat_rad = math.atan2( |
| 82 | + z_ecef + WGS84_EP_SQ * WGS84_B * sin_theta**3, |
| 83 | + p - WGS84_E_SQ * WGS84_A * cos_theta**3, |
| 84 | + ) |
| 85 | + |
| 86 | + sin_lat = math.sin(lat_rad) |
| 87 | + |
| 88 | + # Recalculate prime vertical radius to find altitude |
| 89 | + n_radius = WGS84_A / math.sqrt(1.0 - WGS84_E_SQ * sin_lat**2) |
| 90 | + |
| 91 | + alt_m = (p / math.cos(lat_rad)) - n_radius |
| 92 | + |
| 93 | + return math.degrees(lat_rad), math.degrees(lon_rad), alt_m |
| 94 | + |
| 95 | + |
| 96 | +def enu_to_ecef( |
| 97 | + east: float, north: float, up: float, ref_lat_deg: float, ref_lon_deg: float |
| 98 | +) -> tuple[float, float, float]: |
| 99 | + """ |
| 100 | + Rotates East-North-Up (ENU) offset coordinates to ECEF offset coordinates, |
| 101 | + based on the reference (Radar) latitude and longitude. |
| 102 | +
|
| 103 | + >>> dx, dy, dz = enu_to_ecef(100.0, 200.0, 50.0, 0.0, 0.0) |
| 104 | + >>> round(dx, 2), round(dy, 2), round(dz, 2) |
| 105 | + (50.0, 100.0, 200.0) |
| 106 | + """ |
| 107 | + lat_rad = math.radians(ref_lat_deg) |
| 108 | + lon_rad = math.radians(ref_lon_deg) |
| 109 | + |
| 110 | + sin_lat = math.sin(lat_rad) |
| 111 | + cos_lat = math.cos(lat_rad) |
| 112 | + sin_lon = math.sin(lon_rad) |
| 113 | + cos_lon = math.cos(lon_rad) |
| 114 | + |
| 115 | + # Rotation matrix components for ENU to ECEF |
| 116 | + dx = -sin_lon * east - sin_lat * cos_lon * north + cos_lat * cos_lon * up |
| 117 | + dy = cos_lon * east - sin_lat * sin_lon * north + cos_lat * sin_lon * up |
| 118 | + dz = cos_lat * north + sin_lat * up |
| 119 | + |
| 120 | + return dx, dy, dz |
| 121 | + |
| 122 | + |
| 123 | +def calculate_target_coordinates( |
| 124 | + radar_lat: float, |
| 125 | + radar_lon: float, |
| 126 | + radar_alt: float, |
| 127 | + azimuth_deg: float, |
| 128 | + range_m: float, |
| 129 | + elevation_deg: float = 0.0, |
| 130 | +) -> tuple[float, float, float]: |
| 131 | + """ |
| 132 | + Main function to calculate target (ship) coordinates from radar measurements. |
| 133 | +
|
| 134 | + Parameters: |
| 135 | + radar_lat (float): Radar latitude in degrees |
| 136 | + radar_lon (float): Radar longitude in degrees |
| 137 | + radar_alt (float): Radar altitude above sea level in meters |
| 138 | + azimuth_deg (float): True bearing to the target (0 is North, 90 is East) |
| 139 | + range_m (float): Direct line-of-sight distance to the target in meters |
| 140 | + elevation_deg (float): Antenna elevation angle in degrees |
| 141 | + (default 0 for surface ships) |
| 142 | +
|
| 143 | + Returns: |
| 144 | + tuple: (Target Latitude, Target Longitude, Target Altitude) |
| 145 | +
|
| 146 | + >>> lat, lon, alt = calculate_target_coordinates(0.0, 0.0, 0.0, 90.0, 111319.5) |
| 147 | + >>> round(lat, 1), round(lon, 1), round(alt, 1) |
| 148 | + (0.0, 1.0, 971.4) |
| 149 | + """ |
| 150 | + # Step 1: Convert Radar polar measurements to Local ENU Cartesian coordinates |
| 151 | + az_rad = math.radians(azimuth_deg) |
| 152 | + el_rad = math.radians(elevation_deg) |
| 153 | + |
| 154 | + # Standard spherical to cartesian for ENU |
| 155 | + # North is aligned with 0 degrees Azimuth, East is 90 degrees |
| 156 | + east = range_m * math.cos(el_rad) * math.sin(az_rad) |
| 157 | + north = range_m * math.cos(el_rad) * math.cos(az_rad) |
| 158 | + up = range_m * math.sin(el_rad) |
| 159 | + |
| 160 | + # Step 2: Get absolute ECEF position of the Radar |
| 161 | + radar_x, radar_y, radar_z = geodetic_to_ecef(radar_lat, radar_lon, radar_alt) |
| 162 | + |
| 163 | + # Step 3: Convert the Local ENU offsets to ECEF offsets |
| 164 | + dx, dy, dz = enu_to_ecef(east, north, up, radar_lat, radar_lon) |
| 165 | + |
| 166 | + # Step 4: Add offsets to the Radar's ECEF coordinates to find Target ECEF |
| 167 | + target_x = radar_x + dx |
| 168 | + target_y = radar_y + dy |
| 169 | + target_z = radar_z + dz |
| 170 | + |
| 171 | + # Step 5: Convert Target ECEF back to Geodetic coordinates |
| 172 | + target_lat, target_lon, target_alt = ecef_to_geodetic(target_x, target_y, target_z) |
| 173 | + |
| 174 | + return target_lat, target_lon, target_alt |
| 175 | + |
| 176 | + |
| 177 | +if __name__ == "__main__": |
| 178 | + import doctest |
| 179 | + |
| 180 | + doctest.testmod() |
0 commit comments