|
| 1 | +import numpy as np |
| 2 | +from datetime import datetime, timedelta |
| 3 | +from sgp4.api import Satrec, jday |
| 4 | + |
| 5 | + |
| 6 | +class OrbitalMechanicsEngine: |
| 7 | + def __init__(self): |
| 8 | + self.equatorialRadius = 6378.137 |
| 9 | + self.flatteningRatio = 1.0 / 298.257223563 |
| 10 | + self.polarRadius = self.equatorialRadius * (1 - self.flatteningRatio) |
| 11 | + self.e2Ellipsoid = 1 - (self.polarRadius**2) / (self.equatorialRadius**2) |
| 12 | + self.earthGravParameter = 398600.4418 |
| 13 | + |
| 14 | + @staticmethod |
| 15 | + def datetimeToJd(dt: datetime): |
| 16 | + return jday(dt.year, dt.month, dt.day, dt.hour, dt.minute, dt.second + dt.microsecond / 1e6) |
| 17 | + |
| 18 | + @staticmethod |
| 19 | + def datetimeToJulianCenturies(dt: datetime): |
| 20 | + return (dt - datetime(2000, 1, 1, 12, 0)) / timedelta(days=1) / 36525.0 |
| 21 | + |
| 22 | + @staticmethod |
| 23 | + def arcsecToDegrees(arcsec): |
| 24 | + return arcsec / 3600.0 |
| 25 | + |
| 26 | + def propagateSgp4(self, sat: Satrec, dt: datetime): |
| 27 | + jd, fr = self.datetimeToJd(dt) |
| 28 | + error, r, v = sat.sgp4(jd, fr) |
| 29 | + if error != 0: |
| 30 | + raise RuntimeError(f'SGP4 error code {error}') |
| 31 | + return np.array(r), np.array(v) |
| 32 | + |
| 33 | + def greenwichMeridianSiderealTime(self, dt: datetime): |
| 34 | + julianDate, fraction = self.datetimeToJd(dt) |
| 35 | + julianDate += fraction |
| 36 | + T = (julianDate - 2451545) / 36525 |
| 37 | + degGmst = (280.46061837 + 360.98564736629 * (julianDate - 2451545) + 0.000387933 * T ** 2 - T ** 3 / 38710000) |
| 38 | + return np.deg2rad(degGmst % 360) |
| 39 | + |
| 40 | + def eciToEcef(self, rEci, dt: datetime): |
| 41 | + gmstAngle = self.greenwichMeridianSiderealTime(dt) |
| 42 | + rot = np.array([[np.cos(gmstAngle), np.sin(gmstAngle), 0], [-np.sin(gmstAngle), np.cos(gmstAngle), 0], [0, 0, 1]]) |
| 43 | + return rot @ rEci |
| 44 | + |
| 45 | + def ecefToEci(self, rEcef, dt: datetime): |
| 46 | + gmstAngle = self.greenwichMeridianSiderealTime(dt) |
| 47 | + rot = np.array([[np.cos(gmstAngle), -np.sin(gmstAngle), 0], [np.sin(gmstAngle), np.cos(gmstAngle), 0], [0, 0, 1]]) |
| 48 | + return rot @ rEcef |
| 49 | + |
| 50 | + def ecefToLongitudeLatitude(self, rEcef, radians=True): |
| 51 | + x, y, z = rEcef |
| 52 | + longitude = np.arctan2(y, x) |
| 53 | + ep2 = (self.equatorialRadius ** 2 - self.polarRadius ** 2) / self.polarRadius ** 2 |
| 54 | + p = np.sqrt(x * x + y * y) |
| 55 | + theta = np.arctan2(z * self.equatorialRadius, p * self.polarRadius) |
| 56 | + cosTheta, sinTheta = np.cos(theta), np.sin(theta) |
| 57 | + latitude = np.arctan2(z + ep2 * self.polarRadius * sinTheta * sinTheta * sinTheta, p - self.e2Ellipsoid * self.equatorialRadius * cosTheta * cosTheta * cosTheta) |
| 58 | + cosLatitude, sinLatitude = np.cos(latitude), np.sin(latitude) |
| 59 | + primeVerticalRadius = self.equatorialRadius / np.sqrt(1 - self.e2Ellipsoid * sinLatitude ** 2) |
| 60 | + altitude = p / np.cos(latitude) - primeVerticalRadius |
| 61 | + if not radians: |
| 62 | + return np.rad2deg(longitude), np.rad2deg(latitude), altitude |
| 63 | + return longitude, latitude, altitude |
| 64 | + |
| 65 | + def longitudeLatitudeToEcef(self, longitude, latitude, altitude, radians=True): |
| 66 | + if not radians: |
| 67 | + longitude, latitude = np.deg2rad(longitude), np.deg2rad(latitude) |
| 68 | + cosLatitude, sinLatitude = np.cos(latitude), np.sin(latitude) |
| 69 | + cosLongitude, sinLongitude= np.cos(longitude), np.sin(longitude) |
| 70 | + N = self.equatorialRadius / np.sqrt(1 - self.e2Ellipsoid * sinLatitude ** 2) |
| 71 | + x = (N + altitude) * cosLatitude * cosLongitude |
| 72 | + y = (N + altitude) * cosLatitude * sinLongitude |
| 73 | + z = (N * (1 - self.e2Ellipsoid) + altitude) * sinLatitude |
| 74 | + return np.array([x, y, z]) |
| 75 | + |
| 76 | + def ecefToEnu(self, rEcef, obsLongitude, obsLatitude, obsAltitude, radians=True): |
| 77 | + if not radians: |
| 78 | + obsLongitude, obsLatitude = np.deg2rad(obsLongitude), np.deg2rad(obsLatitude) |
| 79 | + obsPosition = self.longitudeLatitudeToEcef(obsLongitude, obsLatitude, obsAltitude) |
| 80 | + xDelta, yDelta, zDelta = rEcef - obsPosition |
| 81 | + longitude, latitude = np.deg2rad(obsLongitude), np.deg2rad(obsLatitude) |
| 82 | + cosLatitude, sinLatitude = np.cos(latitude), np.sin(latitude) |
| 83 | + cosLongitude, sinLongitude= np.cos(longitude), np.sin(longitude) |
| 84 | + rot = np.array([[-sinLongitude, cosLongitude, 0], [-sinLatitude * cosLongitude, -sinLatitude * sinLongitude, cosLatitude], [cosLatitude * cosLongitude, cosLatitude * sinLongitude, sinLatitude]]) |
| 85 | + enu = rot @ np.array([xDelta, yDelta, zDelta]) |
| 86 | + return enu |
| 87 | + |
| 88 | + @staticmethod |
| 89 | + def enuToAzimuthElevation(enu): |
| 90 | + E, N, U = enu |
| 91 | + slantRange = np.sqrt(E ** 2 + N ** 2 + U ** 2) |
| 92 | + elevation = np.arcsin(U / slantRange) |
| 93 | + azimuth = np.arctan2(E, N) |
| 94 | + if azimuth < 0: |
| 95 | + azimuth += 2 * np.pi |
| 96 | + return azimuth, elevation, slantRange |
| 97 | + |
| 98 | + def satelliteState(self, sat: Satrec, dt: datetime, obsLongitude=None, obsLatitude=None, obsAltitude=None, radians=True): |
| 99 | + if not radians: |
| 100 | + obsLongitude, obsLatitude = np.deg2rad(obsLongitude), np.deg2rad(obsLatitude) |
| 101 | + rEci, vEci = self.propagateSgp4(sat, dt) |
| 102 | + rEcef = self.eciToEcef(rEci, dt) |
| 103 | + longitude, latitude, altitude = self.ecefToLongitudeLatitude(rEcef) |
| 104 | + state = {'rECI': rEci, 'vECI': vEci, 'rECEF': rEcef, 'altitude': altitude, 'latitude': latitude, 'longitude': longitude} |
| 105 | + if obsLongitude is not None: |
| 106 | + enu = self.ecefToEnu(rEcef, obsLongitude, obsLatitude, obsAltitude) |
| 107 | + state['azimuth'], state['elevation'], state['range'] = self.enuToAzimuthElevation(enu) |
| 108 | + return state |
| 109 | + |
| 110 | + def satelliteVisibilityFootPrint(self, state, nbPoints=360): |
| 111 | + longitude, latitude, altitude = state['longitude'], state['latitude'], state['altitude'] |
| 112 | + cosLatitude, sinLatitude = np.cos(latitude), np.sin(latitude) |
| 113 | + localRadius = np.sqrt((self.equatorialRadius ** 2 * cosLatitude ** 2 + self.polarRadius ** 2 * sinLatitude ** 2) / (cosLatitude ** 2 + sinLatitude ** 2)) |
| 114 | + angleHorizon = np.arccos(localRadius / (localRadius + altitude)) |
| 115 | + circlePoints = np.linspace(0, 2 * np.pi, nbPoints) |
| 116 | + cosHorizon, sinHorizon = np.cos(angleHorizon), np.sin(angleHorizon) |
| 117 | + circleLatitude = np.arcsin(sinLatitude * cosHorizon + cosLatitude * sinHorizon * np.cos(circlePoints)) |
| 118 | + circleLongitude = longitude + np.arctan2(np.sin(circlePoints) * sinHorizon * cosLatitude, cosHorizon - sinLatitude * np.sin(circleLatitude)) |
| 119 | + circleLongitude = (circleLongitude + np.pi) % (2 * np.pi) - np.pi |
| 120 | + return circleLongitude, circleLatitude |
| 121 | + |
| 122 | + def satelliteOrbitPath(self, sat: Satrec, dt: datetime, nbPoints=361, nbPast=0.5, nbFuture=0.5): |
| 123 | + orbitalPeriod = self.orbitalPeriod(sat) |
| 124 | + times = np.linspace( - nbPast * orbitalPeriod, nbFuture * orbitalPeriod, nbPoints) |
| 125 | + positionsEci = np.empty((nbPoints, 3)) |
| 126 | + for i, offset in enumerate(times): |
| 127 | + t = dt + timedelta(seconds=float(offset)) |
| 128 | + rEci, _ = self.propagateSgp4(sat, t) |
| 129 | + positionsEci[i, :] = rEci |
| 130 | + return positionsEci |
| 131 | + |
| 132 | + def satelliteGroundTrack(self, sat: Satrec, dt: datetime, nbPoints=361, nbPast=0.5, nbFuture=0.5): |
| 133 | + orbitalPeriod = self.orbitalPeriod(sat) |
| 134 | + times = np.linspace( - nbPast * orbitalPeriod, nbFuture * orbitalPeriod, nbPoints) |
| 135 | + longitudes, latitudes, altitudes = np.empty(nbPoints), np.empty(nbPoints), np.empty(nbPoints) |
| 136 | + for i, offset in enumerate(times): |
| 137 | + t = dt + timedelta(seconds=float(offset)) |
| 138 | + rEci, _ = self.propagateSgp4(sat, t) |
| 139 | + rEcef = self.eciToEcef(rEci, t) |
| 140 | + longitudes[i], latitudes[i], altitudes[i] = self.ecefToLongitudeLatitude(rEcef) |
| 141 | + longitudes = (longitudes + np.pi) % (2 * np.pi) - np.pi |
| 142 | + return longitudes, latitudes, altitudes |
| 143 | + |
| 144 | + @staticmethod |
| 145 | + def orbitalPeriod(sat: Satrec): |
| 146 | + meanMotion = sat.no / 60 |
| 147 | + return 2 * np.pi / meanMotion |
| 148 | + |
| 149 | + def semiMajorAxis(self, sat: Satrec): |
| 150 | + meanMotion = sat.no / 60 |
| 151 | + return np.cbrt(self.earthGravParameter / meanMotion ** 2) |
| 152 | + |
| 153 | + @staticmethod |
| 154 | + def tleOrbitalElements(sat: Satrec): |
| 155 | + return {'inclination': sat.inclo, 'RAAN': sat.nodeo, 'arg_perigee': sat.argpo, 'eccentricity': sat.ecco} |
| 156 | + |
| 157 | + @staticmethod |
| 158 | + def flightPathAngle(rVec, vVec): |
| 159 | + rNorm, vNorm = np.linalg.norm(rVec), np.linalg.norm(vVec) |
| 160 | + if rNorm == 0 or vNorm == 0: |
| 161 | + return 0.0 |
| 162 | + return np.arcsin(np.dot(rVec, vVec) / (rNorm * vNorm)) |
| 163 | + |
| 164 | + def subSolarPoint(self, dt: datetime, radians=True): |
| 165 | + sunEciPosition = self.solarDirectionEci(dt) |
| 166 | + sunDeclination, sunRightAscension = np.arcsin(sunEciPosition[2]), np.arctan2(sunEciPosition[1], sunEciPosition[0]) |
| 167 | + gmstAngle = self.greenwichMeridianSiderealTime(dt) |
| 168 | + subSolarLongitude = sunRightAscension - gmstAngle |
| 169 | + subSolarLongitude = (subSolarLongitude + np.pi) % (2 * np.pi) - np.pi |
| 170 | + if not radians: |
| 171 | + return np.rad2deg(subSolarLongitude), np.rad2deg(sunDeclination), 1 |
| 172 | + return subSolarLongitude, sunDeclination, 1 |
| 173 | + |
| 174 | + def solarDirectionEci(self, dt: datetime): |
| 175 | + T = self.datetimeToJulianCenturies(dt) |
| 176 | + meanLongitude = 280.46646 + T * (36000.76983 + T * 0.0003032) |
| 177 | + meanAnomaly = np.deg2rad(357.52911 + T * (35999.05029 - T * 0.0001537)) |
| 178 | + earthOrbitEccentricity = 0.016708634 - T * (0.000042037 + T * 0.0000001267) |
| 179 | + centerEquation = np.deg2rad((1.914602 - T * (0.004817 + T * 0.000014)) * np.sin(meanAnomaly) + (0.019993 - T * 0.000101) * np.sin(2 * meanAnomaly) + 0.000289 * np.sin(3 * meanAnomaly)) |
| 180 | + trueLongitude = meanLongitude + np.rad2deg(centerEquation) |
| 181 | + trueAnomaly = meanAnomaly + centerEquation |
| 182 | + sunDistance = 1.000001018 * (1 - earthOrbitEccentricity ** 2) / (1 + earthOrbitEccentricity * np.cos(trueAnomaly)) |
| 183 | + moonOrbitAscendingNode = np.deg2rad(125.04 - 1934.136 * T) |
| 184 | + eclipticLongitude = np.deg2rad(trueLongitude - 0.00569 - 0.00478 * np.sin(moonOrbitAscendingNode)) |
| 185 | + eclipticObliquity = np.deg2rad(23 + (26 + (21.448 - T * (46.8150 + T * (0.00059 - T * 0.001813))) / 60) / 60 + 0.00256 * np.cos(moonOrbitAscendingNode)) |
| 186 | + # ECI POSITION VECTOR |
| 187 | + x = sunDistance * np.cos(eclipticLongitude) |
| 188 | + y = sunDistance * np.sin(eclipticLongitude) * np.cos(eclipticObliquity) |
| 189 | + z = sunDistance * np.sin(eclipticLongitude) * np.sin(eclipticObliquity) |
| 190 | + r = np.array([x, y, z]) |
| 191 | + return r / np.linalg.norm(r) |
| 192 | + |
| 193 | + def terminatorCurve(self, dt: datetime, nbPoints=361, radians=True): |
| 194 | + sunLongitude, sunLatitude, _ = self.subSolarPoint(dt, radians=True) |
| 195 | + longitudes = np.linspace(-np.pi, np.pi, nbPoints) |
| 196 | + latitudes = np.arctan(-np.cos(longitudes - sunLongitude) / np.tan(sunLatitude)) |
| 197 | + if not radians: |
| 198 | + return np.rad2deg(longitudes), np.rad2deg(latitudes) |
| 199 | + return longitudes, latitudes |
| 200 | + |
| 201 | + def getVernalSubPoint(self, dt: datetime, radians=True): |
| 202 | + vernalUnitVectorEci = np.array([1, 0, 0]) |
| 203 | + vernalLongitude = (np.arctan2(vernalUnitVectorEci[1], vernalUnitVectorEci[0]) - self.greenwichMeridianSiderealTime(dt)) % (2 * np.pi) |
| 204 | + if vernalLongitude > np.pi: |
| 205 | + vernalLongitude -= 2 * np.pi |
| 206 | + if not radians: |
| 207 | + return np.rad2deg(vernalLongitude), np.rad2deg(0) |
| 208 | + return vernalLongitude, 0 |
| 209 | + |
| 210 | + |
| 211 | + |
0 commit comments