{"metadata":{"kernelspec":{"language":"python","display_name":"Python 3","name":"python3"},"language_info":{"pygments_lexer":"ipython3","nbconvert_exporter":"python","version":"3.6.4","file_extension":".py","codemirror_mode":{"name":"ipython","version":3},"name":"python","mimetype":"text/x-python"}},"nbformat_minor":4,"nbformat":4,"cells":[{"cell_type":"markdown","source":"## Libraries","metadata":{}},{"cell_type":"code","source":"%%writefile GLONASS_FCN_OSN_MAP.json\n{\n  \"test_2020-08-03-US-MTV-2_Mi8\": {\n    \"96\": -1,\n    \"98\": 13,\n    \"103\": 19,\n    \"104\": -1,\n    \"105\": 3\n  },\n  \"test_2020-08-13-US-MTV-1_Mi8\": {\n    \"105\": 7,\n    \"106\": 4,\n    \"103\": 19\n  },\n  \"test_2021-03-25-US-PAO-1_Mi8\": {\n    \"98\": 9,\n    \"102\": 24\n  },\n  \"train_2020-05-21-US-MTV-2_Pixel4\": {\n    \"96\": -1\n  },\n  \"train_2020-05-21-US-MTV-2_Pixel4XL\": {\n    \"96\": -1\n  },\n  \"train_2020-05-29-US-MTV-1_Pixel4\": {\n    \"103\": 23\n  },\n  \"train_2020-07-17-US-MTV-1_Mi8\": {\n    \"106\": -1,\n    \"99\": 16,\n    \"100\": 15,\n    \"103\": 23\n  },\n  \"train_2020-07-17-US-MTV-2_Mi8\": {\n    \"106\": -1,\n    \"99\": -1,\n    \"101\": 1,\n    \"103\": -1\n  },\n  \"train_2020-08-03-US-MTV-1_Mi8\": {\n    \"98\": 9,\n    \"99\": -1,\n    \"102\": -1,\n    \"103\": 19\n  },\n  \"train_2020-08-06-US-MTV-2_Mi8\": {\n    \"105\": 7,\n    \"102\": 20,\n    \"103\": -1\n  },\n  \"train_2020-09-04-US-SF-1_Mi8\": {\n    \"105\": 3,\n    \"106\": 8,\n    \"93\": 14,\n    \"103\": 23\n  },\n  \"train_2020-09-04-US-SF-2_Mi8\": {\n    \"106\": -1,\n    \"100\": 15,\n    \"101\": 1\n  },\n  \"train_2021-01-05-US-SVL-1_Mi8\": {\n    \"104\": 21,\n    \"97\": 22\n  },\n  \"train_2021-04-26-US-SVL-1_Mi8\": {\n    \"106\": 4\n  },\n  \"train_2021-04-28-US-SJC-1_Pixel4\": {\n    \"93\": -1\n  }\n}","metadata":{"_kg_hide-input":true,"execution":{"iopub.status.busy":"2021-08-02T14:22:09.139541Z","iopub.execute_input":"2021-08-02T14:22:09.139978Z","iopub.status.idle":"2021-08-02T14:22:09.154851Z","shell.execute_reply.started":"2021-08-02T14:22:09.139889Z","shell.execute_reply":"2021-08-02T14:22:09.153637Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"%%writefile constants.py\nimport json\nimport datetime\nfrom collections import defaultdict\nimport numpy as np\n\nwith open(f'GLONASS_FCN_OSN_MAP.json') as f:\n    GLONASS_FCN_OSN_MAP = json.load(f)\n\nGPS_ORIGIN_DAY       = datetime.date(1980, 1, 6)\nGPS_ORIGIN_DATETIME  = datetime.datetime(1980, 1, 6)\nGLONASS_LEAP_SECONDS = 18\nBEIDOU_LEAP_SECONDS  = 14\nTZ_MSK = datetime.timezone(datetime.timedelta(hours=+3), 'MSK')\n\nWGS84_SEMI_MAJOR_AXIS = 6378137.0\nWGS84_SEMI_MINOR_AXIS = 6356752.314245\nWGS84_SQUARED_FIRST_ECCENTRICITY  = 6.69437999013e-3\nWGS84_SQUARED_SECOND_ECCENTRICITY = 6.73949674226e-3\nWGS84_FIRST_ECCENTRICITY  = np.sqrt(WGS84_SQUARED_FIRST_ECCENTRICITY)\nWGS84_SECOND_ECCENTRICITY = np.sqrt(WGS84_SQUARED_SECOND_ECCENTRICITY)\n\nLIGHT_SPEED = 299792458.0\n\nOMEGA_EARTH = 7.2921151467e-5\nMU_EARTH    = 3.986005e+14\n\nFREQ_GPS_L1  = 1.575420e+09\nFREQ_GPS_L5  = 1.176450e+09\nFREQ_GAL_E1  = FREQ_GPS_L1\nFREQ_GAL_E5A = FREQ_GPS_L5\nFREQ_QZS_J1  = FREQ_GPS_L1\nFREQ_QZS_J5  = FREQ_GPS_L5\nFREQ_BDS_B1I = 1.561098e+09\nFREQ_GLO_G1_NOMINAL = 1602.00 * 1e+6\nFREQ_GLO_G1_DELTA   = 562.5 * 1e+3\n\nCONSTELLATION_TYPE_MAP = {\n    'GPS'     : 1,\n    'GLONASS' : 3,\n    'QZSS'    : 4,\n    'BEIDOU'  : 5,\n    'GALILEO' : 6,\n}\n\nRAW_STATE_BIT_MAP = {\n     0: \"Code Lock\",\n     1: \"Bit Sync\",\n     2: \"Subframe Sync\",\n     3: \"Time Of Week Decoded State\",\n     4: \"Millisecond Ambiguity\",\n     5: \"Symbol Sync\",\n     6: \"GLONASS String Sync\",\n     7: \"GLONASS Time Of Day Decoded\",\n     8: \"BEIDOU D2 Bit Sync\",\n     9: \"BEIDOU D2 Subframe Sync\",\n    10: \"Galileo E1BC Code Lock\",\n    11: \"Galileo E1C 2^nd^ Code Lock\",\n    12: \"Galileo E1B Page Sync\",\n    13: \"SBAS Sync\",\n    14: \"Time Of Week Known\",\n    15: \"GLONASS Time Of Day Known\",\n}\nRAW_STATE_BIT_INV_MAP = { value : key for key, value in RAW_STATE_BIT_MAP.items() }\n\nSYSTEM_NAME_MAP = {\n    'GPS'     : 'G',\n    'GLONASS' : 'R',\n    'GALILEO' : 'E',\n    'BEIDOU'  : 'C',\n    'QZSS'    : 'J',\n}\n\nGLONASS_FREQ_CHANNEL_MAP = {\n    1 : 1,\n    2 : -4,\n    3 : 5,\n    4 : 6,\n    5 : 1,\n    6 : -4,\n    7 : 5,\n    8 : 6,\n    9 : -2,\n    10 : -7,\n    11 : 0,\n    12 : -1,\n    13 : -2,\n    14 : -7,\n    15 : 0,\n    16 : -1,\n    17 : 4,\n    18 : -3,\n    19 : 3,\n    20 : 2,\n    21 : 4,\n    22 : -3,\n    23 : 3,\n    24 : 2,\n}\n\nQZSS_PRN_SVID_MAP = {\n    193 : 1,\n    194 : 2,\n    199 : 3,\n    195 : 4,\n}\n\nINIT_B = np.deg2rad(  37.5)\nINIT_L = np.deg2rad(-122.2)\nINIT_H = 0.0\n\nFREQ_TOL = 100.0\nCn0DbHz_THRESHOLD = 20.0\nReceivedSvTimeUncertaintyNanos_THRESHOLD = 500\nRAW_PSEUDO_RANGE_THRESHOLD = 50_000 * 1e+3\n\nCLOCK_TIME_MARGIN = datetime.timedelta(seconds=90)\nORBIT_TIME_MARGIN = datetime.timedelta(hours=3)\nIONO_TIME_MARGIN  = datetime.timedelta(hours=2)\n\nEPSILON_M = 0.01\nDEFAULT_TROPO_DELAY_M = 2.48\n\nHAVERSINE_RADIUS = 6_371_000","metadata":{"_kg_hide-input":true,"execution":{"iopub.status.busy":"2021-08-02T14:22:09.156873Z","iopub.execute_input":"2021-08-02T14:22:09.157340Z","iopub.status.idle":"2021-08-02T14:22:09.182439Z","shell.execute_reply.started":"2021-08-02T14:22:09.157298Z","shell.execute_reply":"2021-08-02T14:22:09.181199Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"%%writefile io_f.py\nimport io\nimport datetime\nfrom dataclasses import dataclass, asdict\nimport numpy as np\nimport pandas as pd\nfrom scipy.interpolate import InterpolatedUnivariateSpline, RectBivariateSpline\n\n@dataclass\nclass IONEX:\n    iono_height : float\n    base_radius : float\n    lat_1       : float\n    lat_2       : float\n    lat_delta   : float\n    lng_1       : float\n    lng_2       : float\n    lng_delta   : float\n    time_1      : np.datetime64\n    time_2      : np.datetime64\n    time_delta  : np.timedelta64\n    iono_map    : np.array\n    lat_range   : np.array\n    lng_range   : np.array\n\ndef concat_clk(clk_df_list):\n    clk_df = pd.concat(clk_df_list, axis=0)\n    sat_set_list = [frozenset(clk_df['SatName']) for clk_df in clk_df_list]\n    sat_prod = sat_set_list[0]\n    for sat_set in sat_set_list[1:]:\n        sat_prod = sat_prod & sat_set\n    clk_df = clk_df[clk_df['SatName'].isin(sat_prod)]\n    clk_df = clk_df.reset_index(drop=True)\n    return clk_df\n\ndef concat_sp3(sp3_df_list):\n    sp3_df  = pd.concat(sp3_df_list, axis=0)\n    sat_set_list = [frozenset(sp3_df['SatName']) for sp3_df in sp3_df_list]\n    sat_prod = sat_set_list[0]\n    for sat_set in sat_set_list[1:]:\n        sat_prod = sat_prod & sat_set\n    sp3_df = sp3_df[sp3_df['SatName'].isin(sat_prod)]\n    sp3_df = sp3_df.reset_index(drop=True)\n    return sp3_df\n\ndef concat_ionex(ionex_list):\n    assert(len(np.unique([ionex.iono_height for ionex in ionex_list])) == 1)\n    assert(len(np.unique([ionex.base_radius for ionex in ionex_list])) == 1)\n    assert(len(np.unique([ionex.lat_1       for ionex in ionex_list])) == 1)\n    assert(len(np.unique([ionex.lat_2       for ionex in ionex_list])) == 1)\n    assert(len(np.unique([ionex.lat_delta   for ionex in ionex_list])) == 1)\n    assert(len(np.unique([ionex.lng_1       for ionex in ionex_list])) == 1)\n    assert(len(np.unique([ionex.lng_2       for ionex in ionex_list])) == 1)\n    assert(len(np.unique([ionex.lng_delta   for ionex in ionex_list])) == 1)\n    assert(len(np.unique([ionex.time_delta  for ionex in ionex_list])) == 1)\n    N = len(ionex_list)\n    iono_map = []\n    for i in range(N-1):\n        assert(ionex_list[i].time_2 == ionex_list[i+1].time_1)\n        iono_map.append(ionex_list[i].iono_map[0:-1, :, :])\n    iono_map.append(ionex_list[-1].iono_map)\n    kw = asdict(ionex_list[0])\n    kw['time_2']   = ionex_list[-1].time_2\n    kw['iono_map'] = np.concatenate(iono_map, axis=0)\n    return IONEX(**kw)\n\ndef read_GnssLog_Raw(filename):\n    lines = []\n    with open(filename, 'r') as f:\n        for line in f:\n            if 'Raw' in line:\n                line = line.rstrip().lstrip('#')\n                lines.append(line)\n    sio = io.StringIO('\\n'.join(lines))\n    return pd.read_csv(sio)\n\ndef read_clock_file(filename):\n    with open(filename, 'r') as f:\n        lines = f.readlines()\n    for index, line in enumerate(lines):\n        if 'TIME SYSTEM ID' in line:\n            assert(line.strip().split()[0] == 'GPS')\n            continue\n        if 'END OF HEADER' in line:\n            start_index = index + 1\n            break\n    lines = lines[start_index:]\n    SAT, EPOCH, DELTA_TSV = [], [], []\n    for line in lines:\n        if not line.startswith('AS '):\n            continue\n        tokens = line.rstrip().split()\n        sat = tokens[1]\n        epoch = datetime.datetime(year   = int(tokens[2]),\n                                  month  = int(tokens[3]),\n                                  day    = int(tokens[4]),\n                                  hour   = int(tokens[5]),\n                                  minute = int(tokens[6]),\n                                  second = int(float(tokens[7])),\n                                  )\n        if 'D' in tokens[9]:\n            tokens[9] = tokens[9].replace('D', 'E')\n        delta_tsv = float(tokens[9])\n        SAT.append(sat)\n        EPOCH.append(epoch)\n        DELTA_TSV.append(delta_tsv)\n    df = pd.DataFrame({\n        'Epoch'    : EPOCH,\n        'SatName'  : SAT,\n        'DeltaTSV' : DELTA_TSV,\n    })\n    df = df[df['Epoch'] < (df['Epoch'].values[0] + pd.Timedelta(1, unit='day'))]\n    df = df.reset_index(drop=True)\n    return df\n\ndef read_sp3_file(filename):\n    with open(filename, 'r') as f:\n        lines = f.readlines()        \n    for index, line in enumerate(lines):\n        if line.startswith('%c '):\n            time_system = line.split()[3]\n            assert((time_system == 'GPS') or (time_system == 'ccc'))\n            continue\n        if line.startswith('* '):\n            start_index = index\n            break\n    lines = lines[start_index:]\n\n    data = []\n    for line in lines:\n        if line.startswith('* '):\n            tokens = line.rstrip().split()\n            epoch = datetime.datetime(\n                year   = int(tokens[1]),\n                month  = int(tokens[2]),\n                day    = int(tokens[3]),\n                hour   = int(tokens[4]),\n                minute = int(tokens[5]),\n                second = int(float(tokens[6])),\n            )\n        elif line.startswith('P'):\n            tokens = line.rstrip().split()\n            sat = tokens[0][1:]\n            x, y, z, delta_t = [float(s) for s in tokens[1:5]]\n            x = x * 1e+3\n            y = y * 1e+3\n            z = z * 1e+3\n            delta_t = delta_t * 1e-6\n            data.append([epoch, sat, x, y, z, delta_t])\n    columns = ['Epoch', 'SatName', 'X', 'Y', 'Z', 'DeltaTSV_SP3']\n    df = pd.DataFrame(data, columns=columns)\n    df = df[df['Epoch'] < (df['Epoch'].values[0] + pd.Timedelta(1, unit='day'))]\n    df = df[~((df['X'] == 0) & (df['Y'] == 0) & (df['Z'] == 0))]\n    df = df.reset_index(drop=True)\n    return df\n\ndef read_SINEX_TRO_file(filename):\n    with open(filename, 'r') as f:\n        lines = f.readlines()\n    for index, line in enumerate(lines):\n        if '+TROP/SOLUTION' in line:\n            start_index = index + 2\n            break\n    lines = lines[start_index:]\n    data = []\n    for line in lines:\n        if '-TROP/SOLUTION' in line:\n            break\n        tokens  = line.strip().split()\n        y, d, s = [int(x) for x in tokens[1].split(':')]\n        epoch = datetime.datetime(y+2000, 1, 1) + datetime.timedelta(days=d-1) + datetime.timedelta(seconds=s)\n        data.append([epoch] + [1e-3 * float(x) for x in tokens[2:]])\n    columns = ['Epoch',\n               'TROTOT', 'TROTOT_STD',\n               'TGNTOT', 'TGNTOT_STD',\n               'TGETOT', 'TGETOT_STD']\n    df = pd.DataFrame(data, columns=columns)\n    df = df[df['Epoch'] < (df['Epoch'].values[0] + pd.Timedelta(1, unit='day'))]\n    df = df.reset_index(drop=True)\n    return df\n\ndef read_IONEX_file(filename):\n    with open(filename, 'r') as f:\n        lines = f.readlines()\n    kw = dict()\n    #==============================\n    # read header\n    #==============================\n    for index, line in enumerate(lines):\n        tokens = line.strip().split()\n        if 'EPOCH OF FIRST MAP' in line:\n            kw['time_1'] = np.datetime64(datetime.datetime(\n                year   = int(tokens[0]),\n                month  = int(tokens[1]),\n                day    = int(tokens[2]),\n                hour   = int(tokens[3]),\n                minute = int(tokens[4]),\n                second = int(tokens[5]),\n            ))\n            continue\n        if 'EPOCH OF LAST MAP' in line:\n            kw['time_2'] = np.datetime64(datetime.datetime(\n                year   = int(tokens[0]),\n                month  = int(tokens[1]),\n                day    = int(tokens[2]),\n                hour   = int(tokens[3]),\n                minute = int(tokens[4]),\n                second = int(tokens[5]),                \n            ))\n            continue\n        if 'INTERVAL' in line:\n            kw['time_delta'] = np.timedelta64(datetime.timedelta(\n                seconds=int(tokens[0]),\n            ))\n            continue\n        if 'HGT1 / HGT2 / DHGT' in line:\n            h1, h2, dh = [float(x) for x in tokens[0:3]]\n            assert(h1 == h2)\n            assert(dh == 0.0)\n            kw['iono_height'] = h1 * 1000\n            continue\n        if 'LAT1 / LAT2 / DLAT' in line:\n            lat_1, lat_2, lat_delta = [float(x) for x in tokens[0:3]]\n            assert((lat_2 - lat_1) * lat_delta > 0)\n            if (lat_1 > lat_2):\n                flip_lat = True\n                lat_1, lat_2 = lat_2, lat_1\n                lat_delta = - lat_delta\n            else:\n                flip_lat = False\n            kw['lat_1']     = np.deg2rad(lat_1)\n            kw['lat_2']     = np.deg2rad(lat_2)\n            kw['lat_delta'] = np.deg2rad(lat_delta)\n            continue\n        if 'LON1 / LON2 / DLON' in line:\n            lng_1, lng_2, lng_delta = [float(x) for x in tokens[0:3]]\n            assert((lng_2 - lng_1) * lng_delta > 0)\n            if (lng_1 > lng_2):\n                flip_lng = True\n                lng_1, lng_2 = lng_2, lng_1\n                lng_delta = - lng_delta\n            else:\n                flip_lng = False\n            kw['lng_1']     = np.deg2rad(lng_1)\n            kw['lng_2']     = np.deg2rad(lng_2)\n            kw['lng_delta'] = np.deg2rad(lng_delta)\n            continue\n        if 'MAPPING FUNCTION' in line:\n            assert(tokens[0] == 'COSZ')\n            continue\n        if 'BASE RADIUS' in line:\n            kw['base_radius'] = 1000 * float(tokens[0])\n            continue\n        if 'EXPONENT' in line:\n            TEC_coeff = 10**float(tokens[0])\n            continue\n        if 'MAP DIMENSION' in line:\n            assert(int(tokens[0]) == 2)\n            continue\n        if 'END OF HEADER' in line:\n            line_count = index + 1\n            break\n    #==============================\n    # read data\n    #==============================\n    roundint = lambda x : int(round(x))\n    N_lat  = 1 + roundint((kw['lat_2'] - kw['lat_1']) / kw['lat_delta'])\n    N_lng  = 1 + roundint((kw['lng_2'] - kw['lng_1']) / kw['lng_delta'])\n    N_time = 1 + roundint((kw['time_2'] - kw['time_1']) / kw['time_delta'])\n    iono_map = np.zeros((N_time, N_lat, N_lng), dtype=np.float64)\n\n    data_per_line = 16\n    lines_per_data = (N_lng + data_per_line - 1) // data_per_line\n    \n    for time_count in range(N_time):\n        assert('START OF TEC MAP' in lines[line_count])\n        assert(int(lines[line_count].strip().split()[0]) == time_count + 1)\n        line_count += 1\n        assert('EPOCH OF CURRENT MAP' in lines[line_count])\n        line_count += 1\n        for lat_count in range(N_lat):\n            assert('LAT/LON1/LON2/DLON/H' in lines[line_count])\n            line_count += 1\n            values = []\n            for i in range(lines_per_data):\n                values.extend([int(x) for x in lines[line_count+i].strip().split()])\n            if 9999 in values:\n                print('Warning: There is non-available TEC values.')\n            iono_map[time_count, lat_count, :] = np.array(values).astype(float)\n            line_count += lines_per_data\n        assert('END OF TEC MAP' in lines[line_count])\n        assert(int(lines[line_count].strip().split()[0]) == time_count + 1)\n        line_count += 1\n\n    if flip_lat:\n        iono_map = np.flip(iono_map, axis=1)\n    if flip_lng:\n        iono_map = np.flip(iono_map, axis=2)\n    iono_map = iono_map * TEC_coeff\n    kw['iono_map']  = iono_map\n    kw['lat_range'] = np.linspace(kw['lat_1'], kw['lat_2'], N_lat)\n    kw['lng_range'] = np.linspace(kw['lng_1'], kw['lng_2'], N_lng)\n    return IONEX(**kw)","metadata":{"_kg_hide-input":true,"execution":{"iopub.status.busy":"2021-08-02T14:22:09.259170Z","iopub.execute_input":"2021-08-02T14:22:09.259538Z","iopub.status.idle":"2021-08-02T14:22:09.270524Z","shell.execute_reply.started":"2021-08-02T14:22:09.259505Z","shell.execute_reply":"2021-08-02T14:22:09.269180Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"%%writefile transform.py\nimport numpy as np\nfrom dataclasses import dataclass\n\nimport constants as C\n\n@dataclass\nclass ECEF:\n    x: np.array\n    y: np.array\n    z: np.array\n\n    def to_numpy(self):\n        return np.stack([self.x, self.y, self.z], axis=0)\n\n    @staticmethod\n    def from_numpy(pos):\n        x, y, z = [np.squeeze(w) for w in np.split(pos, 3, axis=-1)]\n        return ECEF(x=x, y=y, z=z)\n\n@dataclass\nclass BLH:\n    lat : np.array\n    lng : np.array\n    hgt : np.array\n\n@dataclass\nclass ENU:\n    east  : np.array\n    north : np.array\n    up    : np.array\n\n@dataclass\nclass AZEL:\n    elevation : np.array\n    azimuth   : np.array\n    zenith    : np.array\n\ndef BLH_to_ECEF(blh):\n    a  = C.WGS84_SEMI_MAJOR_AXIS\n    e2 = C.WGS84_SQUARED_FIRST_ECCENTRICITY\n    sin_B = np.sin(blh.lat)\n    cos_B = np.cos(blh.lat)\n    sin_L = np.sin(blh.lng)\n    cos_L = np.cos(blh.lng)\n    n = a / np.sqrt(1 - e2*sin_B**2)\n    x = (n + blh.hgt) * cos_B * cos_L\n    y = (n + blh.hgt) * cos_B * sin_L\n    z = ((1 - e2) * n + blh.hgt) * sin_B\n    return ECEF(x=x, y=y, z=z)\n\ndef ECEF_to_BLH_approximate(ecef):\n    a = C.WGS84_SEMI_MAJOR_AXIS\n    b = C.WGS84_SEMI_MINOR_AXIS\n    e2  = C.WGS84_SQUARED_FIRST_ECCENTRICITY\n    e2_ = C.WGS84_SQUARED_SECOND_ECCENTRICITY\n    x = ecef.x\n    y = ecef.y\n    z = ecef.z\n    r = np.sqrt(x**2 + y**2)\n    t = np.arctan2(z * (a/b), r)\n    B = np.arctan2(z + (e2_*b)*np.sin(t)**3, r - (e2*a)*np.cos(t)**3)\n    L = np.arctan2(y, x)\n    n = a / np.sqrt(1 - e2*np.sin(B)**2)\n    H = (r / np.cos(B)) - n\n    return BLH(lat=B, lng=L, hgt=H)\n\nECEF_to_BLH = ECEF_to_BLH_approximate\n\ndef ECEF_to_ENU(pos, base):\n    dx = pos.x - base.x\n    dy = pos.y - base.y\n    dz = pos.z - base.z\n    base_blh = ECEF_to_BLH(base)\n    sin_B = np.sin(base_blh.lat)\n    cos_B = np.cos(base_blh.lat)\n    sin_L = np.sin(base_blh.lng)\n    cos_L = np.cos(base_blh.lng)\n    e = -sin_L*dx + cos_L*dy\n    n = -sin_B*cos_L*dx - sin_B*sin_L*dy + cos_B*dz\n    u =  cos_B*cos_L*dx + cos_B*sin_L*dy + sin_B*dz\n    return ENU(east=e, north=n, up=u)\n\ndef ENU_to_AZEL(enu):\n    e = enu.east\n    n = enu.north\n    u = enu.up\n    elevation = np.arctan2(u, np.sqrt(e**2 + n**2))\n    azimuth   = np.arctan2(e, n)\n    zenith    = (0.5 * np.pi) - elevation\n    return AZEL(elevation=elevation,\n                azimuth=azimuth,\n                zenith=zenith)\n\ndef ECEF_to_AZEL(pos, base):\n    return ENU_to_AZEL(ECEF_to_ENU(pos, base))\n\ndef haversine_distance(blh_1, blh_2):\n    dlat = blh_2.lat - blh_1.lat\n    dlng = blh_2.lng - blh_1.lng\n    a = np.sin(dlat/2)**2 + np.cos(blh_1.lat) * np.cos(blh_2.lat) * np.sin(dlng/2)**2\n    dist = 2 * C.HAVERSINE_RADIUS * np.arcsin(np.sqrt(a))\n    return dist\n\ndef hubenys_distance(blh_1, blh_2):\n    Rx = C.WGS84_SEMI_MAJOR_AXIS\n    Ry = C.WGS84_SEMI_MINOR_AXIS\n    E2 = C.WGS84_SQUARED_FIRST_ECCENTRICITY\n    num_M = Rx * (1 - E2)\n    Dy = blh_1.lat - blh_2.lat\n    Dx = blh_1.lng - blh_2.lng\n    P  = 0.5 * (blh_1.lat + blh_2.lat)\n    W  = np.sqrt(1 - E2 * np.sin(P)**2)\n    M  = num_M / W**3\n    N  = Rx / W\n    d2 = (Dy * M)**2 + (Dx * N * np.cos(P))**2\n    d  = np.sqrt(d2)\n    return d\n\ndef jacobian_BLH_to_ECEF(blh):\n    a  = C.WGS84_SEMI_MAJOR_AXIS\n    e2 = C.WGS84_SQUARED_FIRST_ECCENTRICITY\n    B = blh.lat\n    L = blh.lng\n    H = blh.hgt\n    cos_B = np.cos(B)\n    sin_B = np.sin(B)\n    cos_L = np.cos(L)\n    sin_L = np.sin(L)\n    N = a / np.sqrt(1 - e2*sin_B**2)\n    dNdB = a * e2 * sin_B * cos_B * (1 - e2*sin_B**2)**(-3/2)\n    N_plus_H = N + H\n    cos_B_cos_L = cos_B * cos_L\n    cos_B_sin_L = cos_B * sin_L\n    sin_B_cos_L = sin_B * cos_L\n    sin_B_sin_L = sin_B * sin_L\n\n    dXdB = dNdB*cos_B_cos_L - N_plus_H*sin_B_cos_L\n    dYdB = dNdB*cos_B_sin_L - N_plus_H*sin_B_sin_L\n    dZdB = (1-e2)*dNdB*sin_B + (1-e2)*N_plus_H*cos_B\n\n    dXdL = - N_plus_H * cos_B_sin_L\n    dYdL =   N_plus_H * cos_B_cos_L\n    dZdL = np.zeros_like(dXdL)\n\n    dXdH = cos_B_cos_L\n    dYdH = cos_B_sin_L\n    dZdH = sin_B\n\n    J = np.stack([[dXdB, dXdL, dXdH],\n                  [dYdB, dYdL, dYdH],\n                  [dZdB, dZdL, dZdH]], axis=0)\n    axes = list(range(2, J.ndim)) + [0, 1]\n    J = np.transpose(J, axes)\n    return J\n\ndef jacobian_ECEF_to_ENU(blh):\n    B = blh.lat\n    L = blh.lng\n    cos_B = np.cos(B)\n    sin_B = np.sin(B)\n    cos_L = np.cos(L)\n    sin_L = np.sin(L)\n    \n    dEdX = -sin_L\n    dEdY =  cos_L\n    dEdZ = np.zeros_like(dEdX)\n    \n    dNdX = -sin_B*cos_L\n    dNdY = -sin_B*sin_L\n    dNdZ =  cos_B\n\n    dUdX = cos_B*cos_L\n    dUdY = cos_B*sin_L\n    dUdZ = sin_B\n\n    J = np.stack([[dEdX, dEdY, dEdZ],\n                  [dNdX, dNdY, dNdZ],\n                  [dUdX, dUdY, dUdZ]], axis=0)\n    axes = list(range(2, J.ndim)) + [0, 1]\n    J = np.transpose(J, axes)\n    return J\n\ndef jacobian_BL_to_EN(BLH):\n    J_ECEF_BLH = jacobian_BLH_to_ECEF(BLH)\n    J_ENU_ECEF = jacobian_ECEF_to_ENU(BLH)\n    J_EN_BL    = np.einsum('nij,njk->nik', J_ENU_ECEF[:, 0:2, :], J_ECEF_BLH[:, :, 0:2])\n    return J_EN_BL\n\ndef pd_haversine_distance(df1, df2):\n    blh1 = BLH(\n        lat=np.deg2rad(df1['latDeg'].values),\n        lng=np.deg2rad(df1['lngDeg'].values),\n        hgt=0,\n    )\n    blh2 = BLH(\n        lat=np.deg2rad(df2['latDeg'].values),\n        lng=np.deg2rad(df2['lngDeg'].values),\n        hgt=0,\n    )\n    return haversine_distance(blh1, blh2)","metadata":{"_kg_hide-input":true,"execution":{"iopub.status.busy":"2021-08-02T14:22:09.273087Z","iopub.execute_input":"2021-08-02T14:22:09.273948Z","iopub.status.idle":"2021-08-02T14:22:09.290218Z","shell.execute_reply.started":"2021-08-02T14:22:09.273857Z","shell.execute_reply":"2021-08-02T14:22:09.288863Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"%%writefile preprocess.py\nimport sys\nimport os\nimport glob\nimport itertools\nimport traceback\nimport multiprocessing\nimport numpy as np\nimport pandas as pd\nfrom tqdm.notebook import tqdm\nfrom scipy.interpolate import InterpolatedUnivariateSpline\n\nimport io_f\nimport constants as C\n\nINPUT_PATH  = '../input/google-smartphone-decimeter-challenge'\nMERGED_PATH = '../input/gsdc-merged-clk-and-sp3'\nDEST_PATH   = '_features'\n\nCONSTE_ID_GPS = C.CONSTELLATION_TYPE_MAP['GPS']\nCONSTE_ID_GLO = C.CONSTELLATION_TYPE_MAP['GLONASS']\nCONSTE_ID_GAL = C.CONSTELLATION_TYPE_MAP['GALILEO']\nCONSTE_ID_QZS = C.CONSTELLATION_TYPE_MAP['QZSS']\nCONSTE_ID_BDS = C.CONSTELLATION_TYPE_MAP['BEIDOU']\n\ndef add_extra_features_1st(key, gnss_df, log):\n    nanosSinceGpsEpoch = gnss_df['TimeNanos'] - gnss_df['FullBiasNanos']\n    gnss_df['millisSinceGpsEpoch'] = nanosSinceGpsEpoch // 10**6\n    gnss_df['Epoch'] = pd.to_timedelta(nanosSinceGpsEpoch, unit='ns') + C.GPS_ORIGIN_DATETIME\n\n    N = gnss_df.shape[0]\n    SatName    = np.empty(shape=(N, ), dtype=np.object)\n    SignalType = np.empty(shape=(N, ), dtype=np.object)\n    FixedSvid  = np.empty(shape=(N, ), dtype=np.int32)\n    columns = ['ConstellationType', 'Svid', 'CarrierFrequencyHz']\n    for index, (conste, svid, freq) in enumerate(gnss_df[columns].itertuples(index=False, name=None)):\n        if (conste == CONSTE_ID_GPS) and abs(freq - C.FREQ_GPS_L1) < C.FREQ_TOL:\n            sat_name_prefix = C.SYSTEM_NAME_MAP['GPS']\n            signal_type = 'GPS_L1'\n            assert((1 <= svid) and (svid <= 32))\n            fixed_svid = svid\n        elif (conste == CONSTE_ID_GPS) and abs(freq - C.FREQ_GPS_L5) < C.FREQ_TOL:\n            sat_name_prefix = C.SYSTEM_NAME_MAP['GPS']\n            signal_type = 'GPS_L5'\n            assert((1 <= svid) and (svid <= 32))\n            fixed_svid = svid\n        elif (conste == CONSTE_ID_GAL) and abs(freq - C.FREQ_GAL_E1) < C.FREQ_TOL:\n            sat_name_prefix = C.SYSTEM_NAME_MAP['GALILEO']\n            signal_type = 'GAL_E1'\n            assert((1 <= svid) and (svid <= 36))\n            fixed_svid = svid\n        elif (conste == CONSTE_ID_GAL) and abs(freq - C.FREQ_GAL_E5A) < C.FREQ_TOL:\n            sat_name_prefix = C.SYSTEM_NAME_MAP['GALILEO']\n            signal_type = 'GAL_E5A'\n            assert((1 <= svid) and (svid <= 36))\n            fixed_svid = svid\n        elif (conste == CONSTE_ID_BDS) and abs(freq - C.FREQ_BDS_B1I) < C.FREQ_TOL:\n            sat_name_prefix = C.SYSTEM_NAME_MAP['BEIDOU']\n            signal_type = 'BDS_B1I'\n            assert((1 <= svid) and (svid <= 61))\n            fixed_svid = svid\n        elif (conste == CONSTE_ID_QZS) and abs(freq - C.FREQ_QZS_J1) < C.FREQ_TOL:\n            sat_name_prefix = C.SYSTEM_NAME_MAP['QZSS']\n            signal_type = 'QZS_J1'\n            fixed_svid = C.QZSS_PRN_SVID_MAP[svid]\n        elif (conste == CONSTE_ID_QZS) and abs(freq - C.FREQ_QZS_J5) < C.FREQ_TOL:\n            sat_name_prefix = C.SYSTEM_NAME_MAP['QZSS']\n            signal_type = 'QZS_J5'\n            fixed_svid = C.QZSS_PRN_SVID_MAP[svid]\n        elif (conste == CONSTE_ID_GLO):\n            sat_name_prefix = C.SYSTEM_NAME_MAP['GLONASS']\n            signal_type = 'GLO_G1'\n            if svid > 24:\n                freq_channel = svid - 100\n                fixed_svid   = C.GLONASS_FCN_OSN_MAP[key][str(svid)]\n            else:\n                freq_channel = C.GLONASS_FREQ_CHANNEL_MAP[svid]\n                fixed_svid   = svid\n            assert( (-7 <= freq_channel) and (freq_channel <= 6) )\n            freq_nominal = C.FREQ_GLO_G1_NOMINAL + freq_channel * C.FREQ_GLO_G1_DELTA\n            assert( abs(freq - freq_nominal) < C.FREQ_TOL )\n        else:\n            print((conste, svid, freq))\n            raise RuntimeError('unknown signal type')\n        SatName[index]    = f'{sat_name_prefix}{fixed_svid:02d}'\n        SignalType[index] = signal_type\n        FixedSvid[index]  = fixed_svid\n        del sat_name_prefix, signal_type, fixed_svid\n        pass\n    gnss_df['SatName']    = SatName\n    gnss_df['SignalType'] = SignalType\n    gnss_df['FixedSvid']  = FixedSvid\n    return gnss_df\n\ndef bit_check(X, name):\n    mask = 2**C.RAW_STATE_BIT_INV_MAP[name]\n    return (X & mask) != 0\n\ndef remove_invalid_measurements(df, log):\n    state    = df['State'].values\n    sig_type = df['SignalType'].values\n    conste   = df['ConstellationType'].values\n\n    is_gal_E1     = (sig_type == 'GAL_E1')\n    is_non_gal_E1 = np.logical_not(is_gal_E1)\n    code_lock_0   = is_non_gal_E1 & bit_check(state, 'Code Lock')\n    code_lock_1   = is_gal_E1     & bit_check(state, 'Galileo E1BC Code Lock')\n    code_lock_ok  = code_lock_0 | code_lock_1\n\n    is_glo     = (conste == C.CONSTELLATION_TYPE_MAP['GLONASS'])\n    is_nonglo  = np.logical_not(is_glo)\n    time_of_0  = is_nonglo & bit_check(state, 'Time Of Week Decoded State') & bit_check(state, 'Time Of Week Known')\n    time_of_1  = is_glo & bit_check(state, 'GLONASS Time Of Day Decoded') & bit_check(state, 'GLONASS Time Of Day Known')\n    time_of_ok = time_of_0 | time_of_1\n\n    msec_ambi_ok     = np.logical_not(bit_check(state, 'Millisecond Ambiguity'))\n    sigma_rectime_ok = (df['ReceivedSvTimeUncertaintyNanos'] <= C.ReceivedSvTimeUncertaintyNanos_THRESHOLD).values\n    cn0_ok           = (df['Cn0DbHz'] >= C.Cn0DbHz_THRESHOLD).values\n    svid_ok          = (df['FixedSvid'] >= 1).values\n\n    valid = (code_lock_ok\n             & time_of_ok\n             & msec_ambi_ok\n             & sigma_rectime_ok\n             & cn0_ok\n             & svid_ok)\n    df = df[valid]\n    df = df.reset_index(drop=True)\n    return df\n\ndef add_clock_drift(raw_df, derived_df, log):\n    output_df_list = []\n    for (sat_name, sig_type), raw_sat_df in raw_df.groupby(['SatName', 'SignalType']):\n        svid = int(sat_name[1:])\n        derived_sat_df = derived_df[ (derived_df['signalType'] == sig_type) & (derived_df['svid'] == svid) ]\n        if derived_sat_df.shape[0] <= 1:\n            log.append(f'{sat_name} not in derived.csv.')\n            continue\n        t_ref = derived_sat_df['millisSinceGpsEpoch'].min()\n        X_in  = 1e-3 * (derived_sat_df['millisSinceGpsEpoch'] - t_ref).values\n        Y_in  = derived_sat_df['satClkDriftMps'].values\n        X_out = 1e-3 * (raw_sat_df['millisSinceGpsEpoch'] - t_ref).values\n        Y_out = InterpolatedUnivariateSpline(X_in, Y_in, k=1, ext=3)(X_out)\n        raw_sat_df['satClkDriftMps'] = Y_out\n        output_df_list.append(raw_sat_df)\n    output_df = pd.concat(output_df_list, axis=0)\n    output_df.sort_values(['millisSinceGpsEpoch', 'SatName', 'SignalType'], inplace=True, ignore_index=True)\n    return output_df\n\ndef add_satellite_orbit(gnss_df, log):\n    sp3_days = np.unique([(gnss_df['Epoch'].min() - C.ORBIT_TIME_MARGIN).date(),\n                          (gnss_df['Epoch'].max() + C.ORBIT_TIME_MARGIN).date()])\n    sp3_filelist = [os.path.join(MERGED_PATH, d.strftime('SP3_%Y%m%d.csv')) for d in sp3_days]\n    sp3_df_list = [pd.read_csv(f) for f in sp3_filelist]\n    for df in sp3_df_list:\n        df['Epoch'] = df['Epoch'].astype(np.datetime64)\n    sp3_df = io_f.concat_sp3(sp3_df_list)\n\n    t0 = gnss_df['Epoch'].min()\n\n    gnss_sat_dfs = []\n    for sat, gnss_sat_df in gnss_df.groupby('SatName'):\n        sp3_sat_df = sp3_df[sp3_df['SatName'] == sat]\n        if sp3_sat_df.shape[0] == 0:\n            log.append(f'{sat} not available.')\n            continue\n        TIME_ref = (sp3_sat_df['Epoch'] - t0).astype(np.int64).values * 1e-9\n        xPos_fn  = InterpolatedUnivariateSpline(TIME_ref, sp3_sat_df['X'].values, k=5)\n        yPos_fn  = InterpolatedUnivariateSpline(TIME_ref, sp3_sat_df['Y'].values, k=5)\n        zPos_fn  = InterpolatedUnivariateSpline(TIME_ref, sp3_sat_df['Z'].values, k=5)\n        xVel_fn  = xPos_fn.derivative()\n        yVel_fn  = yPos_fn.derivative()\n        zVel_fn  = zPos_fn.derivative()\n\n        TIME = (gnss_sat_df['Epoch'] - t0).astype(np.int64).values * 1e-9\n        xPos = xPos_fn(TIME)\n        yPos = yPos_fn(TIME)\n        zPos = zPos_fn(TIME)\n        xVel = xVel_fn(TIME)\n        yVel = yVel_fn(TIME)\n        zVel = zVel_fn(TIME)\n        gnss_sat_df['xSatPosM']   = xPos\n        gnss_sat_df['ySatPosM']   = yPos\n        gnss_sat_df['zSatPosM']   = zPos\n        gnss_sat_df['xSatVelMps'] = xVel\n        gnss_sat_df['ySatVelMps'] = yVel\n        gnss_sat_df['zSatVelMps'] = zVel\n        gnss_sat_dfs.append(gnss_sat_df)\n        pass\n    gnss_df = pd.concat(gnss_sat_dfs, axis=0)\n    gnss_df = gnss_df.sort_values(['millisSinceGpsEpoch', 'SatName', 'SignalType'])\n    gnss_df = gnss_df.reset_index(drop=True)\n    return gnss_df\n\ndef remove_QZSS(df, log):\n    if df[df['SignalType'] == 'QZS_J1'].shape[0] < 50:\n        df = df[df['SignalType'] != 'QZS_J1']\n    if df[df['SignalType'] == 'QZS_J5'].shape[0] < 50:\n        df = df[df['SignalType'] != 'QZS_J5']\n    df = df.sort_values(['millisSinceGpsEpoch', 'SatName', 'SignalType'])\n    df = df.reset_index(drop=True)\n    return df\n\ndef fix_derived_timestamp(df_raw, df_derived):\n    raw_timestamps     = df_raw['millisSinceGpsEpoch'].unique()\n    derived_timestamps = df_derived['millisSinceGpsEpoch'].unique()\n    indexes = np.searchsorted(raw_timestamps, derived_timestamps)\n    from_t_to_fix_derived = dict(zip(derived_timestamps, raw_timestamps[indexes-1]))\n    df_derived['millisSinceGpsEpoch'] = np.array(list(map(lambda v: from_t_to_fix_derived[v], df_derived['millisSinceGpsEpoch'])))\n\n    df_derived = df_derived.drop_duplicates(['millisSinceGpsEpoch', 'constellationType', 'svid', 'signalType'])    \n    df_derived = df_derived.sort_values('millisSinceGpsEpoch', ignore_index=True)\n    \n    return df_derived\n\ndef process_gnss_df(prefix, drive, phone, key, df):\n    log = []\n    df  = add_extra_features_1st(key, df, log)\n    df  = remove_invalid_measurements(df, log)\n    df  = remove_QZSS(df, log)\n    \n    derived_df = pd.read_csv(f'{INPUT_PATH}/{prefix}/{drive}/{phone}/{phone}_derived.csv')\n    derived_df = fix_derived_timestamp(df, derived_df)\n    df  = add_clock_drift(df, derived_df, log)\n    \n    df  = add_satellite_orbit(df, log)\n    log = [f'[{key:<50}] {msg}' for msg in log]\n    return df, log\n\ndef do_preprocess(path):\n    prefix, drive, phone = path.split('/')[-3:]\n    key = f'{prefix}_{drive}_{phone}'\n    df1 = io_f.read_GnssLog_Raw(f'{INPUT_PATH}/{prefix}/{drive}/{phone}/{phone}_GnssLog.txt')\n    try:\n        df2, log = process_gnss_df(prefix, drive, phone, key, df1)\n        df2.to_csv(f'{DEST_PATH}/{key}.csv', index=False)\n        N1  = df1.shape[0]\n        N2  = df2.shape[0]\n        percent = 100.0 * N2 / N1\n        log.append(f'[{key:<50}] {N2}/{N1} ({percent:.1f})')\n        return log\n    except ValueError:\n        traceback.print_exc(file=sys.stderr)\n        return f'{key:<50}: Fail'\n    return\n\ndef main():\n    os.makedirs(DEST_PATH, exist_ok=True)\n\n    pathlist = sorted(glob.glob(f'{INPUT_PATH}/train/*/*') + glob.glob(f'{INPUT_PATH}/test/*/*'))\n\n    processes = multiprocessing.cpu_count()\n    with multiprocessing.Pool(processes=processes) as pool:\n        results = pool.imap_unordered(do_preprocess, pathlist)\n        results = tqdm(results, total=len(pathlist))\n        results = itertools.chain.from_iterable(results)\n        results = list(results)\n    with open('_preprocess.log', 'w') as f:\n        for msg in sorted(results):\n            print(msg, file=f)\n    return","metadata":{"_kg_hide-input":true,"execution":{"iopub.status.busy":"2021-08-02T14:22:09.293133Z","iopub.execute_input":"2021-08-02T14:22:09.293672Z","iopub.status.idle":"2021-08-02T14:22:09.310257Z","shell.execute_reply.started":"2021-08-02T14:22:09.293636Z","shell.execute_reply":"2021-08-02T14:22:09.309406Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"%%writefile estimate_velocity.py\nimport glob\nimport datetime\nimport sys\nimport os\nimport multiprocessing\nimport numpy as np\nimport pandas as pd\nfrom tqdm.notebook import tqdm\nfrom scipy.interpolate import InterpolatedUnivariateSpline\n\nimport io_f\nimport constants as C\nimport transform\n\nINPUT_PATH   = '../input/google-smartphone-decimeter-challenge'\nFEATURE_PATH = '_features'\nDEST_PATH    = '_doppler_velocity'\n\nBASELINE_DF = pd.concat([pd.read_csv('../input/gsdc-baseline-smoothing-result/smoothing_1st_train.csv'),\n                         pd.read_csv('../input/gsdc-baseline-smoothing-result/smoothing_1st_test.csv'),\n                        ], axis=0)\ndef get_baseline(drive, phone):\n    key = f'{drive}_{phone}'\n    df = BASELINE_DF[BASELINE_DF['phone'] == key].copy()\n    df.reset_index(drop=True, inplace=True)\n    return df\n\ndef L2_norm(x):\n    return np.sqrt(np.sum(x**2, axis=1))\n\ndef calc_azel(sat_pos, rec_pos):\n    sat = transform.ECEF.from_numpy(sat_pos)\n    rec = transform.ECEF.from_numpy(rec_pos)\n    return transform.ECEF_to_AZEL(pos=sat, base=rec)\n\ndef calc_blh(pos):\n    blh = transform.ECEF.from_numpy(pos)\n    return transform.ECEF_to_BLH(blh)\n\ndef mask_to_weight(mask, epsilon=1e-2):\n    return (1 - epsilon) * mask.astype(float) + epsilon\n\ndef solve_least_square(A, b, epsilon=1e-12):\n    \"\"\"\n    J = ||A x - b||^2\n    \"\"\"\n    sys_A = A.T @ A\n    sys_b = A.T @ b\n    N = A.shape[1]\n    u, s, vh = np.linalg.svd(sys_A, hermitian=True)\n    cond = s[0] / s[-1]\n    if cond * epsilon > 1:\n        return\n    sys_x = vh.T @ ((u.T @ sys_b) / s)\n    return sys_x\n\ndef estimate_velocity_1epoch(name, epoch_df, rec_blh):\n    ms_epoch = epoch_df['millisSinceGpsEpoch'].values[0]\n    M = epoch_df.shape[0]\n    if M < 4:\n        print(f'[{name} {ms_epoch}] Too few satellites.')\n        return\n    \n    sat_pos_rec       = epoch_df[['xSatPosM', 'ySatPosM', 'zSatPosM']].values\n    sat_vel_rec       = epoch_df[['xSatVelMps', 'ySatVelMps', 'zSatVelMps']].values\n    sat_clk_drift_mps = epoch_df['satClkDriftMps'].values\n    v_doppler         = epoch_df['PseudorangeRateMetersPerSecond'].values\n    std_mps           = 0.05 + epoch_df['PseudorangeRateUncertaintyMetersPerSecond'].values\n\n    rec_pos  = transform.BLH_to_ECEF(rec_blh).to_numpy()\n    tof      = L2_norm(rec_pos - sat_pos_rec) / C.LIGHT_SPEED\n    sat_pos  = sat_pos_rec - (sat_vel_rec.T * tof).T\n    sat_azel = calc_azel(sat_pos, rec_pos)\n    el_mask  = (sat_azel.elevation > np.deg2rad(5.0))\n    J = transform.jacobian_ECEF_to_ENU(rec_blh)\n\n    P  = sat_pos - rec_pos\n    E  = (P.T / L2_norm(P)).T\n    A  = np.concatenate([-E, np.ones((M, 1))], axis=1)\n    b  = v_doppler - np.sum(E * sat_vel_rec, axis=1) + sat_clk_drift_mps\n    W  = mask_to_weight(el_mask) * (1 / std_mps)\n    WA = (W * A.T).T\n    Wb = W * b\n    x  = solve_least_square(WA, Wb)\n    if x is None:\n        print(f'[{name} {ms_epoch}] Rank deficient.')\n        return\n    v_rec_ecef = x[0:3]\n    v_rec_enu  = J @ v_rec_ecef\n    return v_rec_ecef, v_rec_enu\n\ndef estimate_velocity(args):\n    prefix, drive, phone = args\n    name = f'{prefix}_{drive}_{phone}'\n\n    gnss_df  = pd.read_csv(f'{FEATURE_PATH}/{prefix}_{drive}_{phone}.csv')\n    gnss_df['Epoch'] = gnss_df['Epoch'].astype(np.datetime64)\n\n    base_df = get_baseline(drive, phone)\n    t_ref   = base_df['millisSinceGpsEpoch'].min()\n    TIME    = 1e-3 * (base_df['millisSinceGpsEpoch'] - t_ref).values\n    B       = np.deg2rad(base_df['latDeg'].values)\n    L       = np.deg2rad(base_df['lngDeg'].values)\n    B_fn    = InterpolatedUnivariateSpline(TIME, B, k=3, ext=3)\n    L_fn    = InterpolatedUnivariateSpline(TIME, L, k=3, ext=3)\n\n    data = []\n    for index, (epoch, epoch_df) in enumerate(gnss_df.groupby('millisSinceGpsEpoch')):\n        t = 1e-3 * (epoch - t_ref)\n        rec_blh = transform.BLH(lat=B_fn(t),\n                                lng=L_fn(t),\n                                hgt=0.0)\n        result = estimate_velocity_1epoch(name, epoch_df, rec_blh)\n        if result is not None:\n            v_rec_ecef, v_rec_enu = result\n            data.append((epoch, \n                         v_rec_ecef[0], v_rec_ecef[1], v_rec_ecef[2],\n                         v_rec_enu[0], v_rec_enu[1], v_rec_enu[2],\n                         ))\n    columns = ['millisSinceGpsEpoch', 'v_x', 'v_y', 'v_z', 'v_east', 'v_north', 'v_up']\n    pred_df = pd.DataFrame(data, columns=columns).sort_values('millisSinceGpsEpoch')\n    pred_df['collectionName'] = drive\n    pred_df['phoneName'] = phone\n    pred_df['phone'] = f'{drive}_{phone}'\n    pred_df.to_csv(f'{DEST_PATH}/{prefix}_{drive}_{phone}.csv', index=False)\n    return\n    \ndef main():\n    os.makedirs(DEST_PATH, exist_ok=True)\n\n    path_list = sorted(glob.glob(f'{INPUT_PATH}/train/*/*') + glob.glob(f'{INPUT_PATH}/test/*/*'))\n    args_list = [path.split('/')[-3:] for path in path_list]\n\n    processes = multiprocessing.cpu_count()\n    with multiprocessing.Pool(processes=processes) as pool:\n        results = pool.imap_unordered(estimate_velocity, args_list)\n        results = tqdm(results, total=len(args_list))\n        results = list(results)\n\n    train_df_list = [pd.read_csv(f) for f in sorted(glob.glob(f'{DEST_PATH}/train_*.csv'))]\n    test_df_list  = [pd.read_csv(f) for f in sorted(glob.glob(f'{DEST_PATH}/test_*.csv' ))]\n    train_df = pd.concat(train_df_list, axis=0)\n    test_df  = pd.concat(test_df_list, axis=0)\n    train_df.to_csv(f'{DEST_PATH}/doppler_velocity_train.csv', index=False)\n    test_df.to_csv(f'{DEST_PATH}/doppler_velocity_test.csv', index=False)\n    return","metadata":{"_kg_hide-input":true,"execution":{"iopub.status.busy":"2021-08-02T14:22:09.378239Z","iopub.execute_input":"2021-08-02T14:22:09.378881Z","iopub.status.idle":"2021-08-02T14:22:09.386168Z","shell.execute_reply.started":"2021-08-02T14:22:09.378838Z","shell.execute_reply":"2021-08-02T14:22:09.385097Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"## main","metadata":{}},{"cell_type":"code","source":"import preprocess\npreprocess.main()","metadata":{"execution":{"iopub.status.busy":"2021-08-02T14:22:09.387646Z","iopub.execute_input":"2021-08-02T14:22:09.388173Z","iopub.status.idle":"2021-08-02T14:26:39.882332Z","shell.execute_reply.started":"2021-08-02T14:22:09.388130Z","shell.execute_reply":"2021-08-02T14:26:39.880906Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"import estimate_velocity\nestimate_velocity.main()","metadata":{"execution":{"iopub.status.busy":"2021-08-02T14:26:39.885138Z","iopub.execute_input":"2021-08-02T14:26:39.885500Z","iopub.status.idle":"2021-08-02T14:30:34.210856Z","shell.execute_reply.started":"2021-08-02T14:26:39.885453Z","shell.execute_reply":"2021-08-02T14:30:34.209215Z"},"trusted":true},"execution_count":null,"outputs":[]}]}