Last active
November 27, 2018 14:22
-
-
Save komasaru/65c6e73e278c5274da2361006af94975 to your computer and use it in GitHub Desktop.
Python script to calclulate the ISS position/velocity with SGP4 algorithm.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| #! /usr/local/bin/python3.6 | |
| """ | |
| Getting of ISS position. | |
| Date Author Version | |
| 2018.06.10 mk-mode.com 1.00 新規作成 | |
| 2018.11.27 mk-mode.com 1.01 EOP からの Polar Motion(y 値) 取得に | |
| 誤りがあったため修正 | |
| Copyright(C) 2018 mk-mode.com All Rights Reserved. | |
| --- | |
| 引数: 日時(JST) | |
| 書式：YYYYMMDD or YYYYMMDDHHMMSS | |
| 無指定なら現在(システム日時)を JST とみなす。 | |
| --- | |
| MEMO: | |
| TEME: True Equator, Mean Equinox; 真赤道面平均春分点 | |
| PEF: Pseudo Earth Fixed; 擬地球固定座標系 | |
| ECEF: Earth Centered, Earth Fixed; 地球中心・地球固定直交座標系 | |
| """ | |
| import math | |
| import re | |
| import sys | |
| import traceback | |
| import numpy as np | |
| from datetime import datetime | |
| from datetime import timedelta | |
| from sgp4.earth_gravity import wgs84 | |
| from sgp4.io import twoline2rv | |
| class IssSgp4: | |
| JST_UTC = 9 | |
| PI2 = math.pi * 2 # => 6.283185307179586 | |
| D2R = math.pi / 180 # => 0.017453292519943295 | |
| CSV_EOP = "data/eop.csv" | |
| FILE_DAT = "file/Leap_Second.dat" | |
| FILE_TLE = "data/tle_iss_nasa.txt" | |
| PTN_0 = re.compile(r'\n') | |
| PTN_1 = re.compile(r',') | |
| PTN_2 = re.compile(r'\s+') | |
| PI_180 = math.pi / 180.0 | |
| # 以下、 WGS84 座標パラメータ | |
| A = 6378137.0 # a(地球楕円体長半径(赤道面平均半径)) | |
| ONE_F = 298.257223563 # 1 / f(地球楕円体扁平率=(a - b) / a) | |
| B = A * (1 - 1 / ONE_F) # b(地球楕円体短半径) | |
| E2 = (1 / ONE_F) * (2 - (1 / ONE_F)) | |
| # e^2 = 2 * f - f * f | |
| # = (a^2 - b^2) / a^2 | |
| ED2 = E2 * A * A / (B * B) # e'^2= (a^2 - b^2) / b^2 | |
| def __init__(self): | |
| # コマンドライン引数取得 | |
| self.__get_arg() | |
| # 各種時刻換算 | |
| self.utc = self.jst - timedelta(hours=self.JST_UTC) | |
| eop = self.__get_eop() | |
| flag_pm, self.pm_x, self.pm_y = eop[2], float(eop[3]), float(eop[5]) | |
| flag_dut, dut1 = eop[7], float(eop[8]) | |
| self.lod = 0.0 if eop[10] == "" else float(eop[10]) | |
| self.ut1 = self.utc + timedelta(seconds=dut1) | |
| dat = self.__get_dat() | |
| tai = self.utc + timedelta(seconds=dat) | |
| tt = tai + timedelta(seconds=32.184) | |
| self.jd_ut1 = self.__gc2jd(self.ut1) | |
| jd_tt = self.__gc2jd(tt) | |
| self.t_tt= (jd_tt - 2451545) / 36525 | |
| # TLE 取得 | |
| self.tle_1, self.tle_2 = self.__get_tle(self.utc) | |
| print(( | |
| "[初期データ]\n" | |
| "{} JST\n" | |
| "{} UTC\n" | |
| "DUT1 = {} sec.\n" | |
| "{} UT1 ({})\n" | |
| "TAI - UTC = {} sec.\n" | |
| "{} TAI\n" | |
| "{} TT\n" | |
| "JD(UT1) = {} day.\n" | |
| " JD(TT) = {} day.\n" | |
| " T(TT) = {}\n" | |
| " LOD = {} msec.\n" | |
| "Polar Motion = [{} mas., {} mas.] ({})\n" | |
| "TLE = {}\n {}" | |
| ).format( | |
| self.jst.strftime("%Y-%m-%d %H:%M:%S.%f"), | |
| self.utc.strftime("%Y-%m-%d %H:%M:%S.%f"), | |
| dut1, | |
| self.ut1.strftime("%Y-%m-%d %H:%M:%S.%f"), | |
| flag_dut, | |
| dat, | |
| tai.strftime("%Y-%m-%d %H:%M:%S.%f"), | |
| tt.strftime("%Y-%m-%d %H:%M:%S.%f"), | |
| self.jd_ut1, | |
| jd_tt, | |
| self.t_tt, | |
| self.lod, | |
| self.pm_x, self.pm_y, flag_pm, | |
| self.tle_1, self.tle_2 | |
| )) | |
| def exec(self): | |
| """ Execution """ | |
| print("\n[途中経過]") | |
| try: | |
| # ISS 位置／速度取得 | |
| pos, vel = self.__get_pv(self.tle_1, self.tle_2, self.ut1) | |
| print(( | |
| "TEME POS = {}\n" | |
| " VEL = {}" | |
| ).format(pos, vel)) | |
| # GMST（グリニッジ平均恒星時）計算 | |
| gmst = self.__calc_gmst(self.jd_ut1) | |
| print("GMST = {} rad.".format(gmst)) | |
| # Ω（月の平均昇交点黄経）計算（IAU1980章動理論） | |
| om = self.__calc_om(self.t_tt) | |
| print("om =", om) | |
| # GMST に運動項を適用（1997年より新しい場合） | |
| gmst_g = self.__apply_kinematic(gmst, self.jd_ut1, om) | |
| print("GMST_G = {} rad.".format(gmst_g)) | |
| # GMST 回転行列（z軸を中心とした回転） | |
| mtx_z = self.__r_z(gmst_g) | |
| print("ROTATE MATRIX(for GMST) =\n{}".format(mtx_z)) | |
| # 極運動(Polar Motion)回転行列 | |
| pm_x_r = self.pm_x * math.pi / (180 * 60 * 60 * 1000) | |
| pm_y_r = self.pm_y * math.pi / (180 * 60 * 60 * 1000) | |
| print("Polar Motion [x, y] =\n[{} rad., {} rad.]" \ | |
| .format(pm_x_r, pm_y_r)) | |
| mtx_pm = self.__r_pm(pm_x_r, pm_y_r, self.t_tt) | |
| print("ROTATE MATRIX(for Polar Motion) =\n{}".format(mtx_pm)) | |
| # PEF 座標の計算（GMST 回転行列の適用） | |
| pos = np.matrix([[pos[0]],[pos[1]],[pos[2]]]) | |
| r_pef = mtx_z * pos | |
| print("POSITION(PEF) =\n{}".format(r_pef.T)) | |
| # ECEF 座標（位置）の計算（極運動(Polar Motion)の適用） | |
| r_ecef = mtx_pm * r_pef | |
| print("POSITION(ECEF) =\n{}".format(r_ecef.T)) | |
| # Ω_earth値の計算 | |
| om_e = self.__calc_om_e(self.lod) | |
| print("om_earth =\n{}".format(om_e)) | |
| # PEF 座標（速度）の計算（GMST 回転行列の適用） | |
| vel = np.matrix([[vel[0]], [vel[1]], [vel[2]]]) | |
| v_pef = mtx_z * vel - np.cross(om_e, r_pef.T).T | |
| print("VELOCITY(PEF) =\n{}".format(v_pef.T)) | |
| # ECEF 座標（速度）の計算（極運動(Polar Motion)の適用） | |
| v_ecef = mtx_pm * v_pef | |
| print("VELOCITY(ECEF) =\n{}".format(v_ecef.T)) | |
| # ECEF 座標 => BLH(Beta, Lambda, Height) 変換 | |
| x, y, z = r_ecef[0, 0], r_ecef[1, 0], r_ecef[2, 0] | |
| lat, lon, ht = self.__ecef2blh(x, y, z) | |
| vel_ecef = math.sqrt( | |
| sum(map(lambda x: x * x, v_ecef.T.tolist()[0])) | |
| ) | |
| # 結果出力 | |
| print(( | |
| "\n[計算結果]\nWGS84(BLH):\n" | |
| " POSITION LAT = {:9.4f} °\n" | |
| " LON = {:9.4f} °\n" | |
| " HEIGHT = {:9.4f} km\n" | |
| " VELOCITY = {:9.4f} km/s" | |
| ).format(lat, lon, ht, vel_ecef)) | |
| except Exception as e: | |
| raise | |
| def __get_arg(self): | |
| """ コマンドライン引数の取得 | |
| * 引数で指定した日時を self.jst に設定 | |
| * 引数が存在しなければ、現在時刻を self.jst に設定 | |
| """ | |
| try: | |
| if len(sys.argv) < 2: | |
| self.jst = datetime.now() | |
| return | |
| if re.search(r"^(\d{8}|\d{14})$", sys.argv[1]): | |
| dt = sys.argv[1].ljust(14, "0") | |
| else: | |
| sys.exit(0) | |
| try: | |
| self.jst = datetime.strptime(dt, "%Y%m%d%H%M%S") | |
| except ValueError as e: | |
| print("Invalid date!") | |
| sys.exit(0) | |
| except Exception as e: | |
| raise | |
| def __get_eop(self): | |
| """ EOP （地球回転パラメータ）取得 | |
| * 予め作成しておいた CSV データから取得する | |
| :return list eop: [date, mjd, flag_pm, pm_x, pm_x_e, pm_y, pm_y_e, | |
| flag_dut, dut1, dut1_e, lod, lod_e, | |
| flag_nut, nut_x, nut_x_e, nut_y, nut_y_e] | |
| """ | |
| eop = [] | |
| try: | |
| with open(self.CSV_EOP, "r") as f: | |
| csv = f.read() | |
| lines = re.split(self.PTN_0, csv.strip()) | |
| for l in reversed(lines): | |
| items = re.split(self.PTN_1, l) | |
| if items[0] == self.utc.strftime("%Y-%m-%d"): | |
| eop = items | |
| break | |
| return eop | |
| except Exception as e: | |
| raise | |
| def __get_dat(self): | |
| """ DAT (= TAI - UTC)（うるう秒の総和）取得 | |
| * 予め取得しておいたテキストファイルから取得する | |
| :return int dat: DAT = TAI - UTC | |
| """ | |
| dat = 0 | |
| try: | |
| with open(self.FILE_DAT, "r") as f: | |
| for l in reversed(f.readlines()): | |
| items = re.split(self.PTN_2, l.strip()) | |
| if items[0] == "#": | |
| break | |
| date = "{:04d}-{:02d}-{:02d}".format( | |
| int(items[3]), int(items[2]), int(items[1]) | |
| ) | |
| dat = int(items[-1]) | |
| if date <= self.utc.strftime("%Y-%m-%d"): | |
| break | |
| return dat | |
| except Exception as e: | |
| raise | |
| def __gc2jd(self, gc): | |
| """ GC（グレゴリオ暦） -> JD（ユリウス日） 変換 | |
| * フリーゲルの公式を使用 | |
| :param datetime gc: グレゴリオ暦 | |
| :return float jd: ユリウス日 | |
| """ | |
| year, month, day = gc.year, gc.month, gc.day | |
| hour, minute, second = gc.hour, gc.minute, gc.second | |
| second += gc.microsecond * 1e-6 | |
| try: | |
| if month < 3: | |
| year -= 1 | |
| month += 12 | |
| d = int(365.25 * year) + year // 400 - year // 100 \ | |
| + int(30.59 * (month - 2)) + day + 1721088.5 | |
| t = (second / 3600 + minute / 60 + hour) / 24 | |
| return d + t | |
| except Exception as e: | |
| raise | |
| def __get_tle(self, utc): | |
| """ TLE 取得 | |
| :param datetime utc: UTC | |
| :return list: [tle_1, tle_2] | |
| """ | |
| tle = "" | |
| utc_tle = None | |
| try: | |
| with open(self.FILE_TLE, "r") as f: | |
| txt = f.read() | |
| for new in reversed(re.split(r'\n\n', txt)): | |
| tle = new | |
| item_utc = re.split(" +", tle)[3] | |
| y = 2000 + int(item_utc[0:2]) | |
| d = float(item_utc[2:]) | |
| utc_tle = datetime(y, 1, 1) + timedelta(days=d) | |
| if utc_tle <= self.utc: | |
| break | |
| return re.split(r'\n', tle) | |
| except Exception as e: | |
| raise | |
| def __calc_gmst(self, jd_ut1): | |
| """ GMST（グリニッジ平均恒星時）計算 | |
| : IAU1982理論(by David Vallado)によるもの | |
| GMST = 18h 41m 50.54841s | |
| + 8640184.812866s T + 0.093104s T^2 - 0.0000062s T^3 | |
| (但し、 T = 2000年1月1日12時(UT1)からのユリウス世紀単位) | |
| :param float jd_ut1: UT1 に対するユリウス日 | |
| :return datetime gmst: グリニッジ平均恒星時(単位:radian) | |
| """ | |
| try: | |
| t_ut1 = (jd_ut1 - 2451545.0) / 36525.0 | |
| gmst = 67310.54841 \ | |
| + (876600.0 * 3600.0 + 8640184.812866 \ | |
| + (0.093104 \ | |
| - 6.2e-6 * t_ut1) * t_ut1) * t_ut1 | |
| gmst = (gmst * self.D2R / 240) % self.PI2 | |
| if gmst < 0.0: | |
| gmst += self.PI2 | |
| return gmst | |
| except Exception as e: | |
| raise | |
| def __calc_om(self, t_tt): | |
| """ Ω（月の平均昇交点黄経）計算（IAU1980章動理論） | |
| : Ω = 125°02′40″.280 | |
| - ((5 * 360)° + 134°08′10″.539) * T | |
| + 7″.455 * T2 | |
| + 0″.008 * T3 | |
| :param float t_tt: ユリウス世紀数(TT) | |
| :return float om: Ω（月の平均昇交点黄経） | |
| """ | |
| try: | |
| om = 125.04452222 \ | |
| + ((-6962890.5390 \ | |
| + (7.455 \ | |
| + 0.008 * t_tt) * t_tt) * t_tt) / 3600 | |
| om %= 360 | |
| om *= self.D2R | |
| return om | |
| except Exception as e: | |
| raise | |
| def __calc_om_e(self, lod): | |
| """ Ω_earth値の計算 | |
| :param float lod: Length Of Day | |
| :return np.matrix | |
| """ | |
| try: | |
| theta_sa = 7.29211514670698e-05 * (1 - lod / 86400) | |
| return np.matrix([[0, 0, theta_sa]]) | |
| except Exception as e: | |
| raise | |
| def __apply_kinematic(self, gmst, jd_ut1, om): | |
| """ GMST に運動項を適用（1997年より新しい場合） | |
| : gmst_g = gmst \ | |
| + 0.00264 * PI / (3600 * 180) * sin(om) \ | |
| + 0.000063 * PI / (3600 * 180) * sin(2.0 * om) | |
| :param float gmst: GMST（グリニッジ平均恒星時）（適用前） | |
| :param float jd_ut1: ユリウス日(UT1) | |
| :param float om: Ω（月の平均昇交点黄経） | |
| return float gmst_g: GMST（グリニッジ平均恒星時）（適用後） | |
| """ | |
| try: | |
| if jd_ut1 > 2450449.5: | |
| gmst_g = gmst \ | |
| + 0.00264 * math.pi / (3600 * 180) * math.sin(om) \ | |
| + 0.000063 * math.pi / (3600 * 180) * math.sin(2 * om) | |
| else: | |
| gmst_g = gmst | |
| gmst_g %= self.PI2 | |
| return gmst_g | |
| except Exception as e: | |
| raise | |
| def __get_pv(self, tle_1, tle_2, ut1): | |
| """ ISS 位置／速度取得 | |
| :param string tle_1: TLE 1行目 | |
| :param string tle_2: TLE 2行目 | |
| :param datetime ut1: UT1 | |
| :return list: [position, velocity] | |
| """ | |
| try: | |
| sat = twoline2rv(tle_1, tle_2, wgs84) | |
| pos, vel = sat.propagate( | |
| ut1.year, ut1.month, ut1.day, | |
| ut1.hour, ut1.minute, ut1.second | |
| ) | |
| if sat.error != 0: | |
| print(sat.error_message) | |
| sys.exit(0) | |
| return [pos, vel] | |
| except Exception as e: | |
| raise | |
| def __r_z(self, theta): | |
| """ 回転行列生成(z軸中心) | |
| ( cos(θ) -sin(θ) 0 ) | |
| ( sin(θ) +cos(θ) 0 ) | |
| ( 0 0 1 ) | |
| :param float theta: 角度(Unit: rad) | |
| :return np.matrix r_mtx: 回転行列 | |
| """ | |
| try: | |
| s, c = np.sin(theta), np.cos(theta) | |
| r_mtx = np.matrix([ | |
| [ c, s, 0.0], | |
| [ -s, c, 0.0], | |
| [0.0, 0.0, 1.0] | |
| ], dtype="float64") | |
| return r_mtx | |
| except Exception as e: | |
| raise | |
| def __r_pm(self, pm_x, pm_y, t_tt): | |
| """ 極運動(Polar Motion)回転行列 | |
| :param float pm_x: Polar Motion X | |
| :param float pm_y: Polar Motion Y | |
| :param float t_tt: ユリウス世紀数(TT) | |
| :return np.matrix r_mtx: 回転行列 | |
| """ | |
| try: | |
| conv = math.pi / (3600 * 180) | |
| c_xp = np.cos(pm_x) | |
| s_xp = np.sin(pm_x) | |
| c_yp = np.cos(pm_y) | |
| s_yp = np.sin(pm_y) | |
| # approximate sp value in rad | |
| sp = -47.0e-6 * t_tt * conv | |
| s_sp, c_sp = np.sin(sp), np.cos(sp) | |
| r_mtx = np.matrix([ | |
| [ c_xp * c_sp, | |
| c_xp * s_sp, | |
| s_xp], | |
| [-c_yp * s_sp + s_yp * s_xp * c_sp, | |
| c_yp * c_sp + s_yp * s_xp * s_sp, | |
| -s_yp * c_xp,], | |
| [-s_yp * s_sp - c_yp * s_xp * c_sp, | |
| s_yp * c_sp - c_yp * s_xp * s_sp, | |
| c_yp * c_xp] | |
| ], dtype="float64") | |
| return r_mtx | |
| except Exception as e: | |
| raise | |
| def __ecef2blh(self, x, y, z): | |
| """ ECEF 座標 => BLH(Beta, Lambda, Height) 変換 | |
| β = atan {(z + e'^2 * b * sin^3(θ)) / (p − e^2 * a * cos^3(θ))} | |
| λ = atan (y / x) | |
| h = (p / cos(β)) − N | |
| 但し、 | |
| p = sqrt(x^2 + y^2) | |
| θ = atan(za / pb) | |
| e^2 = (a^2 - b^2) / a^2 | |
| e'^2 = (a^2 - b^2) / b^2 | |
| :param float x: X(unit: km) | |
| :param float y: Y(unit: km) | |
| :param float z: Z(unit: km) | |
| :return list blh: BLH [latitude(°), longitude(°), height(km)] | |
| """ | |
| try: | |
| x *= 1000 | |
| y *= 1000 | |
| z *= 1000 | |
| n = lambda x: self.A / \ | |
| math.sqrt(1 - self.E2 * math.sin(x * self.PI_180)**2) | |
| p = math.sqrt(x * x + y * y) | |
| theta = math.atan2(z * self.A, p * self.B) / self.PI_180 | |
| lat = math.atan2( | |
| z + self.ED2 * self.B * math.sin(theta * self.PI_180)**3, | |
| p - self.E2 * self.A * math.cos(theta * self.PI_180)**3 | |
| ) / self.PI_180 | |
| lon = math.atan2(y, x) / self.PI_180 | |
| ht = (p / math.cos(lat * self.PI_180)) - n(lat) | |
| ht /= 1000 | |
| return [lat, lon, ht] | |
| except Exception as e: | |
| raise | |
| if __name__ == '__main__': | |
| try: | |
| obj = IssSgp4() | |
| obj.exec() | |
| except Exception as e: | |
| traceback.print_exc() | |
| sys.exit(1) |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment