{"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":"## Config","metadata":{}},{"cell_type":"code","source":"import pandas as pd\nBASELINE_DF = pd.concat([pd.read_csv('../input/gsdc-improved-raw-gnss-baseline-result/raw_gnss_train.csv'),\n                         pd.read_csv('../input/gsdc-improved-raw-gnss-baseline-result/raw_gnss_test.csv'),\n                        ], axis=0)","metadata":{"execution":{"iopub.status.busy":"2021-08-02T14:58:30.067209Z","iopub.execute_input":"2021-08-02T14:58:30.067738Z","iopub.status.idle":"2021-08-02T14:58:30.772043Z","shell.execute_reply.started":"2021-08-02T14:58:30.067609Z","shell.execute_reply":"2021-08-02T14:58:30.771021Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"## Libraries","metadata":{}},{"cell_type":"code","source":"%%writefile constants.py\nimport json\nimport datetime\nfrom collections import defaultdict\nimport numpy as np\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 = 100\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\nELEVATION_CUTOFF = np.deg2rad(7.0)\nDEFAULT_TROPO_DELAY_M = 2.48\n\nHAVERSINE_RADIUS = 6_371_000\n\nMAGNETIC_DECLINATION = np.deg2rad(10.0)","metadata":{"_kg_hide-input":true,"execution":{"iopub.status.busy":"2021-08-02T14:58:30.77367Z","iopub.execute_input":"2021-08-02T14:58:30.774047Z","iopub.status.idle":"2021-08-02T14:58:30.781741Z","shell.execute_reply.started":"2021-08-02T14:58:30.774003Z","shell.execute_reply":"2021-08-02T14:58:30.780825Z"},"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\nUTC_TO_GPS_OFFSET_MS = ((datetime.date(1980, 1, 6) - datetime.date(1970, 1, 1)).days * 24 * 3600 - 18) * 1000\n\ndef read_GnssLog_sensors(filename):\n    acce_lines = []\n    gyro_lines = []\n    magn_lines = []\n    orie_lines = []\n    with open(filename, 'r') as f:\n        for line in f:\n            if 'UncalAccel' in line:\n                line = line.rstrip().lstrip('#')\n                acce_lines.append(line)\n                continue\n            if 'UncalGyro' in line:\n                line = line.rstrip().lstrip('#')\n                gyro_lines.append(line)\n                continue\n            if 'UncalMag' in line:\n                line = line.rstrip().lstrip('#')\n                magn_lines.append(line)\n                continue\n            if 'OrientationDeg' in line:\n                line = line.rstrip().lstrip('#')\n                orie_lines.append(line)\n                continue\n    acce_df = pd.read_csv(io.StringIO('\\n'.join(acce_lines))) if len(acce_lines) != 0 else None\n    gyro_df = pd.read_csv(io.StringIO('\\n'.join(gyro_lines))) if len(gyro_lines) != 0 else None\n    magn_df = pd.read_csv(io.StringIO('\\n'.join(magn_lines))) if len(magn_lines) != 0 else None\n    orie_df = pd.read_csv(io.StringIO('\\n'.join(orie_lines))) if len(orie_lines) != 0 else None\n    def modify_df(df):\n        if df is None:\n            return None\n        df.dropna(axis=0, inplace=True)\n        df.drop_duplicates(subset='utcTimeMillis', inplace=True)\n        if df.shape[0] < 2:\n            return None\n        dt_valid = np.concatenate([[True], np.diff(df['utcTimeMillis'].values) > 0])\n        df = df[dt_valid].copy()\n        df.reset_index(drop=True, inplace=True)\n        df['millisSinceGpsEpoch'] = df['utcTimeMillis'] - UTC_TO_GPS_OFFSET_MS\n        return df\n    dfs = dict(\n        acce = modify_df(acce_df),\n        gyro = modify_df(gyro_df),\n        magn = modify_df(magn_df),\n        orie = modify_df(orie_df),\n    )\n    return dfs\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_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_sum  = sat_set_list[0]\n    sat_prod = sat_set_list[0]\n    for sat_set in sat_set_list[1:]:\n        sat_sum  = sat_sum  | sat_set\n        sat_prod = sat_prod & sat_set\n    sat_partial = sat_sum - sat_prod\n    for sat in sat_partial:\n        # print(f'concat_sp3: drop {sat}')\n        sp3_df = sp3_df[sp3_df['SatName'] != sat]\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        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    err_df = df[(df['X'] == 0) & (df['Y'] == 0) & (df['Z'] == 0)]\n    for sat in np.unique(err_df['SatName'].values):\n        # print(f'read_sp3_file: drop {sat}')\n        df = df[df['SatName'] != sat]\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    N_lat  = 1 + int((kw['lat_2'] - kw['lat_1']) / kw['lat_delta'])\n    N_lng  = 1 + int((kw['lng_2'] - kw['lng_1']) / kw['lng_delta'])\n    N_time = 1 + (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:58:30.783372Z","iopub.execute_input":"2021-08-02T14:58:30.783625Z","iopub.status.idle":"2021-08-02T14:58:30.800126Z","shell.execute_reply.started":"2021-08-02T14:58:30.783599Z","shell.execute_reply":"2021-08-02T14:58:30.799168Z"},"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:58:30.801647Z","iopub.execute_input":"2021-08-02T14:58:30.801902Z","iopub.status.idle":"2021-08-02T14:58:30.818739Z","shell.execute_reply.started":"2021-08-02T14:58:30.801876Z","shell.execute_reply":"2021-08-02T14:58:30.81775Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"%%writefile design_filter.py\nimport numpy as np\nimport scipy.signal\nfrom scipy.special import sinc\n\ndef make_sinc_filter(F_cutoff, dt, n_sinc=3, n_gauss=2):\n    N_FLT = round(n_sinc / (2 * F_cutoff * dt))\n    x = (n_sinc / (N_FLT + 1)) * (np.arange(2*N_FLT+1) - N_FLT)\n    S = sinc(x)\n    W = scipy.signal.windows.gaussian(2*N_FLT+1, std=N_FLT/n_gauss)\n    flt = S * W\n    flt = flt * (1 / np.sum(flt))\n    return flt\n\ndef main():\n    import scipy.signal\n    import matplotlib.pyplot as plt\n\n    dt = 2.5 * 1e-3\n    F_cutoff = 2.0\n    FLT = make_sinc_filter(F_cutoff=F_cutoff, dt=dt)\n\n    f_nyquist = (1 / (2 * dt))\n    print(f'dt = {1000*dt} [ms]')\n    print(f'f_nyquist = {f_nyquist:.2f} [Hz]')\n    \n    num = FLT\n    den = np.zeros_like(num)\n    den[0] = 1\n    sys = scipy.signal.TransferFunction(num, den, dt=dt)\n\n    frange = np.logspace(-1, 2, 1000)\n    wrange = (2 * np.pi * dt) * frange\n    _, mag, phase = sys.bode(wrange)\n\n    assert(len(num) % 2 == 1)\n    N_FLT = (len(num) - 1) // 2\n    it = np.linspace(-N_FLT, N_FLT, 2*N_FLT+1)\n    print(f'N_FLT = {N_FLT}')\n\n    plt.figure()\n    plt.plot(it, num)\n\n    plt.figure()\n    plt.semilogx(frange, mag)\n    plt.grid(True)\n    plt.xlim([0.1, 100])\n    plt.ylim([-80, 20])\n\n    plt.show()\n    return","metadata":{"_kg_hide-input":true,"execution":{"iopub.status.busy":"2021-08-02T14:58:30.820057Z","iopub.execute_input":"2021-08-02T14:58:30.820392Z","iopub.status.idle":"2021-08-02T14:58:30.835167Z","shell.execute_reply.started":"2021-08-02T14:58:30.820364Z","shell.execute_reply":"2021-08-02T14:58:30.834295Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"%%writefile signal_f.py\nfrom dataclasses import dataclass\nimport numpy as np\nimport pandas as pd\nfrom scipy.interpolate import InterpolatedUnivariateSpline\n\nimport constants as C\nimport design_filter\n\n@dataclass\nclass Trig:\n    cos : np.array\n    sin : np.array\n\n    def __add__(self, other):\n        cos = (self.cos * other.cos) - (self.sin * other.sin)\n        sin = (self.sin * other.cos) + (self.cos * other.sin)\n        return Trig(cos=cos, sin=sin)\n\n    def __sub__(self, other):\n        cos = (self.cos * other.cos) + (self.sin * other.sin)\n        sin = (self.sin * other.cos) - (self.cos * other.sin)\n        return Trig(cos=cos, sin=sin)\n\n    def __neg__(self):\n        return Trig(cos=self.cos, sin=-self.sin)\n\n    def to_rad(self):\n        return np.arctan2(self.sin, self.cos)\n\n    @staticmethod\n    def from_data(x, y):\n        r = np.sqrt(x**2 + y**2)\n        cos = x / r\n        sin = y / r\n        return Trig(cos=cos, sin=sin)\n\n    @staticmethod\n    def from_rad(th):\n        cos = np.cos(th)\n        sin = np.sin(th)\n        return Trig(cos=cos, sin=sin)\n\ndef apply_filter_padding_edge(X, flt):\n    assert(len(flt) % 2 == 1)\n    n_pad = (len(flt) - 1) // 2\n    X_pad = np.concatenate([np.full(n_pad, X[0]), X, np.full(n_pad, X[-1])], axis=0)\n    return np.convolve(X_pad, flt, mode='valid')\n\ndef preprocess_sensor_data(sensor_dfs, t_ref, dt_up, dt_down, flt):\n    assert(len(flt) % 2 == 1)\n    acce_df = sensor_dfs['acce']\n    gyro_df = sensor_dfs['gyro']\n    magn_df = sensor_dfs['magn']\n    acce_time = 1e-3 * (acce_df['millisSinceGpsEpoch'] - t_ref).values\n    gyro_time = 1e-3 * (gyro_df['millisSinceGpsEpoch'] - t_ref).values\n    magn_time = 1e-3 * (magn_df['millisSinceGpsEpoch'] - t_ref).values\n    t_up_min  = np.ceil( np.max([acce_time[ 0], gyro_time[ 0], magn_time[ 0]]) / dt_up) * dt_up\n    t_up_max  = np.floor(np.min([acce_time[-1], gyro_time[-1], magn_time[-1]]) / dt_up) * dt_up\n    time_up   = np.arange(t_up_min, t_up_max + dt_up / 10, dt_up)\n    \n    acce_x_up = InterpolatedUnivariateSpline(acce_time, acce_df['UncalAccelXMps2'].values, k=1)(time_up)\n    acce_y_up = InterpolatedUnivariateSpline(acce_time, acce_df['UncalAccelYMps2'].values, k=1)(time_up)\n    acce_z_up = InterpolatedUnivariateSpline(acce_time, acce_df['UncalAccelZMps2'].values, k=1)(time_up)\n    gyro_x_up = InterpolatedUnivariateSpline(gyro_time, gyro_df['UncalGyroXRadPerSec'].values, k=1)(time_up)\n    gyro_y_up = InterpolatedUnivariateSpline(gyro_time, gyro_df['UncalGyroYRadPerSec'].values, k=1)(time_up)\n    gyro_z_up = InterpolatedUnivariateSpline(gyro_time, gyro_df['UncalGyroZRadPerSec'].values, k=1)(time_up)\n    magn_x_up = InterpolatedUnivariateSpline(magn_time, magn_df['UncalMagXMicroT'].values, k=1)(time_up)\n    magn_y_up = InterpolatedUnivariateSpline(magn_time, magn_df['UncalMagYMicroT'].values, k=1)(time_up)\n    magn_z_up = InterpolatedUnivariateSpline(magn_time, magn_df['UncalMagZMicroT'].values, k=1)(time_up)\n    sensor_flt = np.stack([\n        np.convolve(acce_x_up, flt, mode='valid'),\n        np.convolve(acce_y_up, flt, mode='valid'),\n        np.convolve(acce_z_up, flt, mode='valid'),\n        np.convolve(gyro_x_up, flt, mode='valid'),\n        np.convolve(gyro_y_up, flt, mode='valid'),\n        np.convolve(gyro_z_up, flt, mode='valid'),\n        np.convolve(magn_x_up, flt, mode='valid'),\n        np.convolve(magn_y_up, flt, mode='valid'),\n        np.convolve(magn_z_up, flt, mode='valid'),\n    ], axis=1)\n    n_flt = (len(flt) - 1) // 2\n    time_flt   = time_up[n_flt:-n_flt]\n\n    roundint = lambda x : int(np.round(x))\n    \n    dt_ratio    = roundint(dt_down / dt_up)\n    t_down_min  = np.ceil( time_flt[ 0] / dt_down) * dt_down\n    t_down_max  = np.floor(time_flt[-1] / dt_down) * dt_down\n    N_down      = roundint((t_down_max - t_down_min) / dt_down) + 1\n    idx_offset  = roundint((t_down_min - time_flt[ 0]) / dt_up)\n    idx_down    = dt_ratio * np.arange(N_down) + idx_offset\n    time_down   = time_flt[idx_down]\n    sensor_down = sensor_flt[idx_down, :]\n\n    columns = ['UncalAccelXMps2', 'UncalAccelYMps2', 'UncalAccelZMps2',\n               'UncalGyroXRadPerSec', 'UncalGyroYRadPerSec', 'UncalGyroZRadPerSec',\n               'UncalMagXMicroT', 'UncalMagYMicroT', 'UncalMagZMicroT']\n    df = pd.DataFrame(sensor_down, columns=columns)\n    df['Time'] = time_down\n    return df\n\ndef calibrate_magn_offset(sensor_df):\n    N = sensor_df.shape[0]\n    x = sensor_df['UncalMagXMicroT']\n    y = sensor_df['UncalMagYMicroT']\n    z = sensor_df['UncalMagZMicroT']\n    x2 = x**2\n    y2 = y**2\n    z2 = z**2\n    r2 = x2 + y2 + z2\n\n    a11 = 2 * np.sum(x2)\n    a12 = 2 * np.sum(x * y)\n    a13 = 2 * np.sum(x * z)\n    a14 = 2 * np.sum(x)\n    a21 = a12\n    a22 = 2 * np.sum(y2)\n    a23 = 2 * np.sum(y * z)\n    a24 = 2 * np.sum(y)\n    a31 = a13\n    a32 = a23\n    a33 = 2 * np.sum(z2)\n    a34 = 2 * np.sum(z)\n    a41 = a14\n    a42 = a24\n    a43 = a34\n    a44 = 2 * N\n    \n    b1 = np.sum(x * r2)\n    b2 = np.sum(y * r2)\n    b3 = np.sum(z * r2)\n    b4 = np.sum(r2)\n\n    A = np.array([[a11, a12, a13, a14],\n                  [a21, a22, a23, a24],\n                  [a31, a32, a33, a34],\n                  [a41, a42, a43, a44]])\n    b = np.array([b1, b2, b3, b4])\n    mx0, my0, mz0, d = np.linalg.solve(A, b)\n    mr = np.sqrt(mx0**2 + my0**2 + mz0**2 + 2*d)\n    return mx0, my0, mz0, mr\n\ndef remove_gyro_drift(omega, threshold):\n    mask  = (np.abs(omega) < threshold).astype(float)\n    drift = np.sum(omega * mask) / np.sum(mask)\n    return omega - drift\n\ndef trapezoidal_integration(V, dt):\n    dX = (0.5 * dt) * (V[0:-1] + V[1:])\n    return np.concatenate([[0], np.cumsum(dX)], axis=0)\n\ndef central_difference(X, dt):\n    V = (1 / (2*dt)) * (X[2:] - X[0:-2])\n    return np.concatenate([[0], V, [0]], axis=0)\n\ndef add_calibrated_signals(sensor_df, dt_down):\n    mx0, my0, mz0, mr = calibrate_magn_offset(sensor_df)\n    MX = sensor_df['UncalMagXMicroT'].values - mx0\n    MZ = sensor_df['UncalMagZMicroT'].values - mz0\n    trig_th_hat = Trig.from_data(-MX, -MZ)\n\n    omega = sensor_df['UncalGyroYRadPerSec'].values\n    omega = remove_gyro_drift(omega, 0.02)\n    omega = remove_gyro_drift(omega, 0.01)\n    integ_omega = trapezoidal_integration(omega, dt_down)\n    trig_integ_omega = Trig.from_rad(integ_omega)\n\n    trig_th_drift      = trig_th_hat - trig_integ_omega\n    trig_th_drift_mean = Trig.from_data(np.mean(trig_th_drift.cos), np.mean(trig_th_drift.sin))\n    trig_th_residual   = trig_th_drift - trig_th_drift_mean\n    th_drift_mean      = trig_th_drift_mean.to_rad()\n    FLT = design_filter.make_sinc_filter(F_cutoff=1/15.0, dt=dt_down)\n    th_residual = apply_filter_padding_edge(trig_th_residual.to_rad(), FLT)\n    th = integ_omega + th_residual + (th_drift_mean - C.MAGNETIC_DECLINATION)\n    sensor_df['omega']  = omega\n    sensor_df['theta']  = th\n    sensor_df['cos_th'] = np.cos(th)\n    sensor_df['sin_th'] = np.sin(th)\n    sensor_df['dotV']   = - (sensor_df[f'UncalAccelZMps2'] - sensor_df[f'UncalAccelZMps2'].mean())\n    return sensor_df\n\ndef check_sensor_availability(sensor_dfs):\n    if sensor_dfs['acce'] is None:\n        return False\n    if sensor_dfs['gyro'] is None:\n        return False\n    if sensor_dfs['magn'] is None:\n        return False\n    acce_df = sensor_dfs['acce']\n    gyro_df = sensor_dfs['gyro']\n    magn_df = sensor_dfs['magn']\n    if acce_df.shape[0] == 0:\n        return False\n    if gyro_df.shape[0] == 0:\n        return False\n    if magn_df.shape[0] == 0:\n        return False\n    XYZ = ['X', 'Y', 'Z']\n    acce = acce_df[[f'UncalAccel{axis}Mps2'     for axis in XYZ]].values\n    gyro = gyro_df[[f'UncalGyro{axis}RadPerSec' for axis in XYZ]].values\n    magn = magn_df[[f'UncalMag{axis}MicroT'     for axis in XYZ]].values\n    if np.sum(np.abs(np.diff(acce, axis=0))) == 0:\n        return False\n    if np.sum(np.abs(np.diff(acce, axis=0))) == 0:\n        return False\n    if np.sum(np.abs(np.diff(acce, axis=0))) == 0:\n        return False\n    acce_dt = 1e-3 * np.diff(acce_df['millisSinceGpsEpoch'].values)\n    gyro_dt = 1e-3 * np.diff(acce_df['millisSinceGpsEpoch'].values)\n    magn_dt = 1e-3 * np.diff(acce_df['millisSinceGpsEpoch'].values)\n    if np.std(acce_dt) > 1e-3:\n        return False\n    if np.std(gyro_dt) > 1e-3:\n        return False\n    if np.std(magn_dt) > 1e-3:\n        return False\n    acce_mean     = np.mean(acce, axis=0)\n    expected_acce = np.array([0, 9.8, 0])\n    cos_angle_num = np.dot(acce_mean, expected_acce)\n    cos_angle_den = np.linalg.norm(acce_mean) * np.linalg.norm(expected_acce)\n    angle = np.arccos(cos_angle_num / cos_angle_den)\n    if angle > np.deg2rad(20):\n        return False\n    return True\n\ndef remove_different_posture(sensor_df):\n    ay = sensor_df['UncalAccelYMps2'] # gravitational acceleration\n    valid_orig = np.abs(ay - np.mean(ay)) < 5\n    valid = valid_orig\n    # Delete the previous and next 5 seconds also.\n    for i in range(1, 6):\n        valid_shift_plus  = np.concatenate([np.full(i, True), valid_orig[0:-i]], axis=0)\n        valid_shift_minus = np.concatenate([valid_orig[i:], np.full(i, True)], axis=0)\n        valid = valid & valid_shift_plus & valid_shift_minus\n    sensor_df = sensor_df[valid].copy()\n    return sensor_df","metadata":{"_kg_hide-input":true,"execution":{"iopub.status.busy":"2021-08-02T14:58:30.836513Z","iopub.execute_input":"2021-08-02T14:58:30.836907Z","iopub.status.idle":"2021-08-02T14:58:30.85846Z","shell.execute_reply.started":"2021-08-02T14:58:30.836865Z","shell.execute_reply":"2021-08-02T14:58:30.857372Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"%%writefile qpsolver.py\nimport numpy as np\nimport scipy.sparse\nimport scipy.sparse.linalg\n\ndef solve_qp(R, Q, q, A, B):\n    \"\"\"\n    minimize (1/2) u^T R u + (1/2)x^T Q x - q^T x\n    subject to Ax + Bu = 0\n    \"\"\"\n    BRB = B @ scipy.sparse.linalg.spsolve(R, B.T)\n    A_sys  = scipy.sparse.bmat([[Q, A.T], [A, -BRB]], format='csc')\n    b_sys  = np.concatenate([q, np.zeros(A.shape[0])], axis=0)\n    x_sys  = scipy.sparse.linalg.spsolve(A_sys, b_sys)\n    X_star = x_sys[0:A.shape[1]]\n    return X_star\n\ndef make_newton_solver(Q, A, BRB, G, w):\n    N_x = A.shape[1]\n    N_z = w.shape[0]\n    w2inv = w**(-2)\n    W2inv = scipy.sparse.spdiags(w2inv, [0], N_z, N_z)\n    S     = Q + (G.T @ W2inv @ G)\n    sys_A = scipy.sparse.bmat([[S, A.T], [A, -BRB]], format='csc')\n    sys_B = scipy.sparse.linalg.splu(sys_A)\n    def solver(rx, ry, rz, rs):\n        rz2 = rz - (w * rs)\n        rx2 = rx + G.T @ (w2inv * rz2)\n        sys_x = sys_B.solve(np.concatenate([rx2, ry]))\n        dx = sys_x[0:N_x]\n        dy = sys_x[N_x:]\n        dz = w2inv * (G @ dx - rz2)\n        ds = w * (rs - w * dz)\n        return dx, dy, dz, ds\n    return solver\n\ndef calc_alpha(z, s, dz, ds):\n    x  = np.concatenate([z, s], axis=0)\n    dx = np.concatenate([dz, ds], axis=0)\n    cond = (dx < 0)\n    if cond.shape[0] == 0:\n        return 1.0\n    else:\n        return min(1.0, 0.95 * np.min(- x[cond] / dx[cond]))\n\ndef solve_qp_with_inequality(R, Q, q, A, B, G, h):\n    \"\"\"\n    minimize (1/2) u^T R u + (1/2)x^T Q x - q^T x\n    subject to Ax + Bu = 0\n               Gx + s  = h, s >= 0\n    \"\"\"\n    N_x = A.shape[1]\n    N_y = A.shape[0]\n    N_z = G.shape[0]\n    N_s = N_z\n\n    x = np.zeros((N_x, ))\n    y = np.zeros((N_y, ))\n    z = np.ones((N_z, ))\n    s = np.ones((N_z, ))\n    \n    X_ZERO = np.zeros_like(x)\n    Y_ZERO = np.zeros_like(y)\n    Z_ZERO = np.zeros_like(z)\n    BRB = B @ scipy.sparse.linalg.spsolve(R, B.T)\n\n    success  = False\n    LOOP_MAX = 20\n    for loop in range(LOOP_MAX):\n        rx  = - ( (Q @ x) - q + (A.T @ y) + (G.T @ z) )\n        ry  = - ( (A @ x) - (BRB @ y) )\n        rz  = - ( (G @ x) + s - h )\n        gap = np.dot(z, s)\n        terminal_cond = ( (np.sqrt(np.mean(rx**2)) < 1e-10) and\n                          (np.sqrt(np.mean(ry**2)) < 1e-10) and\n                          (np.sqrt(np.mean(rz**2)) < 1e-10) and\n                          (gap / N_z < 1e-10) )\n        if terminal_cond:\n            success = True\n            break\n\n        sqrt_s = np.sqrt(s)\n        sqrt_z = np.sqrt(z)\n        w = sqrt_s / sqrt_z\n        lambda_ = sqrt_s * sqrt_z\n        newton_solver = make_newton_solver(Q, A, BRB, G, w)\n\n        dx1, dy1, dz1, ds1 = newton_solver(rx, ry, rz, rs=-lambda_)\n        alpha = calc_alpha(z, s, dz1, ds1)\n        mu_prev   = gap / N_z\n        mu_hat    = np.dot(z + alpha*dz1, s + alpha*ds1) / N_z\n        mu_target = mu_prev * (mu_hat / mu_prev)**3\n\n        rs = (mu_target - (dz1 * ds1)) / lambda_\n        dx2, dy2, dz2, ds2 = newton_solver(X_ZERO, Y_ZERO, Z_ZERO, rs)\n\n        dx = dx1 + dx2\n        dy = dy1 + dy2\n        dz = dz1 + dz2\n        ds = ds1 + ds2\n        alpha = calc_alpha(z, s, dz, ds)\n        x += alpha * dx\n        y += alpha * dy\n        z += alpha * dz\n        s += alpha * ds\n\n    if not success:\n        raise RuntimeError('Not solved')\n    X_star = x\n    # print(np.max(G @ X_star))\n    return X_star","metadata":{"_kg_hide-input":true,"execution":{"iopub.status.busy":"2021-08-02T14:58:30.85987Z","iopub.execute_input":"2021-08-02T14:58:30.860196Z","iopub.status.idle":"2021-08-02T14:58:30.874413Z","shell.execute_reply.started":"2021-08-02T14:58:30.860166Z","shell.execute_reply":"2021-08-02T14:58:30.873715Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"%%writefile map_matching.py\nimport glob\nimport numpy as np\nimport pandas as pd\n\nimport transform\n\nINPUT_PATH = '../input/google-smartphone-decimeter-challenge'\n\nCOLLECTION_LIST_ALL = np.array(sorted(path.split('/')[-1] for path in glob.glob(f'{INPUT_PATH}/train/*')))\nCOLLECTION_LIST_DOWNTOWN = COLLECTION_LIST_ALL[np.array([24,27,29]) - 1]\n\ndef get_database_vectors():\n    gt_df_list = []\n    for collection in COLLECTION_LIST_DOWNTOWN:\n        phone_list = [path.split('/')[-1] for path in glob.glob(f'{INPUT_PATH}/train/{collection}/*')]\n        for phone in phone_list:\n            gt_df = pd.read_csv(f'{INPUT_PATH}/train/{collection}/{phone}/ground_truth.csv')\n            gt_df_list.append(gt_df)\n    gt_df  = pd.concat(gt_df_list, axis=0)\n    gt_df  = gt_df[['latDeg', 'lngDeg']].drop_duplicates(ignore_index=True)\n    gt_blh = transform.BLH(\n        lat=np.deg2rad(gt_df['latDeg']),\n        lng=np.deg2rad(gt_df['lngDeg']),\n        hgt=np.zeros(gt_df.shape[0]),\n    )\n    P  = transform.BLH_to_ECEF(gt_blh).to_numpy() # shape = (3, N_P)\n    PP = np.sum(P**2, axis=0) # shape = (N_P, )\n    return gt_df, P, PP\n\ngt_df, P, PP = get_database_vectors()\n\ndef snap_to_nearest_neighbor(pred_df):\n    pred_blh = transform.BLH(\n        lat=np.deg2rad(pred_df['latDeg']),\n        lng=np.deg2rad(pred_df['lngDeg']),\n        hgt=np.zeros(pred_df.shape[0]),\n    )\n    Q = transform.BLH_to_ECEF(pred_blh).to_numpy() # shape=(3, N_Q)\n\n    QQ  = np.sum(Q**2, axis=0) # shape=(N_Q, )\n    PQ  = P.T @ Q              # shape=(N_P, N_Q)\n    d2  = (QQ - 2 * PQ).T + PP # shape=(N_Q, N_P)\n    idx = np.argmin(d2, axis=1)\n\n    nn_df = gt_df.iloc[idx, :].reset_index(drop=True)\n    pp_df = pred_df.copy()\n    pp_df['latDeg'] = nn_df['latDeg']\n    pp_df['lngDeg'] = nn_df['lngDeg']\n    return pp_df\n\ndef distance_to_nearest_neighbor(pred_df):\n    pred_blh = transform.BLH(\n        lat=np.deg2rad(pred_df['latDeg']),\n        lng=np.deg2rad(pred_df['lngDeg']),\n        hgt=np.zeros(pred_df.shape[0]),\n    )\n    Q = transform.BLH_to_ECEF(pred_blh).to_numpy() # shape=(3, N_Q)\n    N_Q = Q.shape[1]\n\n    QQ  = np.sum(Q**2, axis=0) # shape=(N_Q, )\n    PQ  = P.T @ Q              # shape=(N_P, N_Q)\n    d2  = (QQ - 2 * PQ).T + PP # shape=(N_Q, N_P)\n    idx = np.argmin(d2, axis=1)\n    d   = np.zeros(N_Q)\n    for i, j in zip(range(N_Q), idx):\n        d[i] = np.sqrt(np.maximum(0, d2[i, j]))\n    return d","metadata":{"_kg_hide-input":true,"execution":{"iopub.status.busy":"2021-08-02T14:58:30.875921Z","iopub.execute_input":"2021-08-02T14:58:30.876322Z","iopub.status.idle":"2021-08-02T14:58:30.892365Z","shell.execute_reply.started":"2021-08-02T14:58:30.876292Z","shell.execute_reply":"2021-08-02T14:58:30.891355Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"%%writefile area_prediction.py\nimport numpy as np\nimport pandas as pd\nfrom pathlib import Path\nfrom glob import glob\nfrom sklearn.neighbors import KNeighborsClassifier\n\nBASE_DIR = Path('../input/google-smartphone-decimeter-challenge')\n\ntrain_base = pd.read_csv(BASE_DIR / 'baseline_locations_train.csv')\ntrain_base = train_base.sort_values([\n    \"collectionName\", \"phoneName\", \"millisSinceGpsEpoch\"\n]).reset_index(drop=True)\n\ntrain_base['area'] = train_base['collectionName'].map(lambda x: x.split('-')[4])\n\ntrain_name = np.array(sorted(path.split('/')[-1] for path in glob(f'{BASE_DIR}/train/*')))\ntrain_highway  = train_name[np.array([1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21]) - 1]\ntrain_tree     = train_name[np.array([22,23,25,26,28]) - 1]\ntrain_downtown = train_name[np.array([24,27,29]) - 1]\n\ntrain_base['area_target'] = -1\ntrain_base.loc[train_base['collectionName'].isin(train_highway),  'area_target'] = 0\ntrain_base.loc[train_base['collectionName'].isin(train_tree),     'area_target'] = 1\ntrain_base.loc[train_base['collectionName'].isin(train_downtown), 'area_target'] = 2\n\ndef processing_downtown(input_df: pd.DataFrame, is_train=False):\n    output_df = input_df.groupby('collectionName')[['latDeg', 'lngDeg']].std()\n    if is_train:\n        output_df = output_df.merge(\n            input_df.groupby('collectionName')[['area_target']].first(),\n            on='collectionName')\n    output_df = output_df.merge(\n        input_df.groupby('collectionName')['area'].first(),\n        on='collectionName')\n    output_df = output_df.merge(\n        input_df.groupby('collectionName')['phoneName'].unique().apply(list),\n        on='collectionName')\n    return output_df\n\ntrain = processing_downtown(train_base, is_train=True)\ntrain['downtown_target'] = (train['area_target']==2).astype(int)\n\ndowntown_model_knn = KNeighborsClassifier(n_neighbors=1)\ndowntown_model_knn.fit(\n    train[['latDeg', 'lngDeg']],\n    train['downtown_target'],\n)\n\ndef processing_highway_tree(input_df: pd.DataFrame, is_train=False):\n    output_df = input_df.groupby('collectionName')[['latDeg', 'lngDeg']].min()\n    if is_train:\n        output_df = output_df.merge(\n            input_df.groupby('collectionName')[['area_target']].first(),\n            on='collectionName')\n    output_df = output_df.merge(\n        input_df.groupby('collectionName')['area'].first(),\n        on='collectionName')\n    output_df = output_df.merge(\n        input_df.groupby('collectionName')['phoneName'].unique().apply(list),\n        on='collectionName')\n    return output_df\n\ntrain = processing_highway_tree(train_base, is_train=True)\n\nhighway_tree_model_knn = KNeighborsClassifier(n_neighbors=1)\nhighway_tree_model_knn.fit(\n    train.loc[train['area_target']!=2, ['latDeg', 'lngDeg']],\n    train.loc[train['area_target']!=2, 'area_target'],\n)\n\ndef predict_area(test_base):\n    test_base = test_base.copy()\n    test_base = test_base.sort_values([\n        \"collectionName\", \"phoneName\", \"millisSinceGpsEpoch\"\n    ]).reset_index(drop=True)\n    test_base['area'] = test_base['collectionName'].map(lambda x: x.split('-')[4])\n\n    test = processing_downtown(test_base)\n    downtown_pred = downtown_model_knn.predict(test[['latDeg', 'lngDeg']])\n\n    test = processing_highway_tree(test_base)\n    test.loc[downtown_pred==1, 'area_pred'] = 2\n    pred = highway_tree_model_knn.predict(test.loc[test['area_pred'].isnull(), ['latDeg', 'lngDeg']])\n    test.loc[test['area_pred'].isnull(), 'area_pred'] = pred\n    test['area_pred'] = test['area_pred'].astype(int)\n    test['collectionName'] = test.index\n\n    test_highway  = []\n    test_tree     = []\n    test_downtown = []\n    for collection, area_pred in test[['collectionName', 'area_pred']].itertuples(index=False):\n        if area_pred == 0:\n            test_highway.append(collection)\n        elif area_pred == 1:\n            test_tree.append(collection)\n        else:\n            test_downtown.append(collection)\n    return (test_highway, test_tree, test_downtown)","metadata":{"_kg_hide-input":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"## main","metadata":{}},{"cell_type":"code","source":"import numpy as np\nimport pandas as pd\nimport scipy.sparse\nimport scipy.sparse.linalg\nimport multiprocessing\nimport glob\nfrom tqdm.notebook import tqdm\n\nimport io_f\nimport signal_f\nimport design_filter\nimport transform\nimport qpsolver\nimport map_matching\nimport area_prediction\n\nINPUT_PATH    = '../input/google-smartphone-decimeter-challenge'\nVELOCITY_PATH = '../input/gsdc-vehicle-speed-estimation-result/_doppler_velocity'\nDT_X = 1.0\n\nVELOCITY_DF = pd.concat([pd.read_csv(f'{VELOCITY_PATH}/doppler_velocity_train.csv'),\n                         pd.read_csv(f'{VELOCITY_PATH}/doppler_velocity_test.csv'),\n                         ], axis=0)\n\ndef get_optimization_constants(base_df, velocity_df, sensor_df, params, use_sensor):\n    const = dict()\n    dt    = DT_X\n    TIME_y = base_df['Time'].values\n    TIME_d = velocity_df['Time'].values\n    N_y = TIME_y.shape[0]\n    N_d = TIME_d.shape[0]\n    N_x = int(np.ceil(np.max(TIME_y) / dt) + 1)\n    const['N_y'] = N_y\n    const['N_d'] = N_d\n    const['N_x'] = N_x\n\n    a = np.array([[1, dt, (1/2)*dt**2],\n                  [0,  1,  dt],\n                  [0,  0,  1]])\n    e3 = scipy.sparse.eye(3)\n    A = np.empty(shape=(2*(N_x-1), 2*N_x), dtype=np.object)\n    for i_x in range(N_x-1):\n        A[2*i_x  , 2*i_x  ] = a\n        A[2*i_x+1, 2*i_x+1] = a\n        A[2*i_x  , 2*i_x+2] = -e3\n        A[2*i_x+1, 2*i_x+3] = -e3\n    const['A'] = scipy.sparse.bmat(A, format='csr')\n    \n    b = np.array([[(1/6)*dt**3,\n                   (1/2)*dt**2,\n                   dt]]).T\n    const['B'] = scipy.sparse.block_diag([b for _ in range(2*(N_x-1))], format='csr')\n\n    diag_R  = np.full(2*N_x - 2, params['sigma_u']**(-2) * dt)\n    const['R'] = scipy.sparse.spdiags(diag_R, [0], 2*N_x - 2, 2*N_x - 2, format='csc')\n    \n    x_index  = np.floor(TIME_y / dt).astype(int)\n    alpha    = (TIME_y / dt) - x_index\n    coeff_y0 = 1 - 3*alpha**2 + 2*alpha**3\n    coeff_y1 =     3*alpha**2 - 2*alpha**3\n    coeff_v0 = dt * alpha * (alpha - 1)**2\n    coeff_v1 = dt * alpha**2 * (alpha - 1)\n    C = np.empty(shape=(2*N_y, 2*N_x), dtype=np.object)\n    for i_x in range(N_x):\n        C[0, 2*i_x  ] = scipy.sparse.coo_matrix((1, 3))\n        C[0, 2*i_x+1] = scipy.sparse.coo_matrix((1, 3))\n    for i_y in range(N_y):\n        i_x = x_index[i_y]\n        c_i = np.array([[coeff_y0[i_y], coeff_v0[i_y], 0]])\n        C[2*i_y,   2*i_x]   = c_i\n        C[2*i_y+1, 2*i_x+1] = c_i\n        if i_x < N_x - 1:\n            c_iplus = np.array([[coeff_y1[i_y], coeff_v1[i_y], 0]])\n            C[2*i_y,   2*i_x+2] = c_iplus\n            C[2*i_y+1, 2*i_x+3] = c_iplus\n    const['Cp_orig']  = scipy.sparse.bmat(C, format='csr')\n\n    diag_Lp = np.full(2*N_y, params['sigma_p']**(-2))\n    const['Lp_orig'] = scipy.sparse.spdiags(diag_Lp, [0], 2*N_y, 2*N_y, format='csr')\n    const['Yp_orig'] = base_df[['latDeg', 'lngDeg']].values.flatten()\n\n    BLH = transform.BLH(\n        lat=np.deg2rad(base_df['latDeg'].values),\n        lng=np.deg2rad(base_df['lngDeg'].values),\n        hgt=np.zeros(N_y),\n    )\n    DEG2RAD = np.pi / 180.0\n    J = transform.jacobian_BL_to_EN(BLH) * DEG2RAD\n    J = np.mean(J, axis=0)\n    J[0, 0] = 0\n    J[1, 1] = 0\n    JJ = scipy.sparse.block_diag([J, J], format='csr')\n    const['J'] = J\n\n    # ドップラ速度に関するパラメータ\n    x_index  = np.floor(TIME_d / dt).astype(int)\n    alpha    = (TIME_d / dt) - x_index\n    coeff_y0 = 1 - 3*alpha**2 + 2*alpha**3\n    coeff_y1 =     3*alpha**2 - 2*alpha**3\n    coeff_v0 = dt * alpha * (alpha - 1)**2\n    coeff_v1 = dt * alpha**2 * (alpha - 1)\n    C = np.empty(shape=(N_d, N_x), dtype=np.object)\n    for i_x in range(N_x):\n        C[0, i_x] = scipy.sparse.coo_matrix((2, 6))\n    for i_d in range(N_d):\n        i_x = x_index[i_d]\n        c = np.array([[0, coeff_y0[i_d], coeff_v0[i_d], 0, 0, 0],\n                      [0, 0, 0, 0, coeff_y0[i_d], coeff_v0[i_d]]])\n        C[i_d, i_x] = J @ c\n        if i_x < N_x - 1:\n            c = np.array([[0, coeff_y1[i_d], coeff_v1[i_d], 0, 0, 0],\n                          [0, 0, 0, 0, coeff_y1[i_d], coeff_v1[i_d]]])\n            C[i_d, i_x+1] = J @ c\n    const['Cd_orig']  = scipy.sparse.bmat(C, format='csr')\n\n    diag_Ld = np.full(2*N_d, params['sigma_d']**(-2))\n    const['Ld_orig'] = scipy.sparse.spdiags(diag_Ld, [0], 2*N_d, 2*N_d, format='csr')\n    const['Yd_orig'] = velocity_df[['v_east', 'v_north']].values.flatten()\n\n    if sensor_df is None:\n        const['use_sensor'] = False\n        const['use_inquality'] = False\n        return const\n\n    TIME_s = sensor_df['Time'].values\n    N_s = TIME_s.shape[0]\n    const['N_s'] = N_s\n    const['use_sensor'] = use_sensor\n    const['use_inquality'] = (use_sensor and params['use_not_go_back_constraint'])\n    x_index = np.round(TIME_s / dt).astype(int)\n    const['x_index_sensor'] = x_index\n    if not use_sensor:\n        return const\n\n    # 速度制約・速度コストに関するパラメータ\n    COS_TH = sensor_df['cos_th'].values\n    SIN_TH = sensor_df['sin_th'].values\n    CV = np.empty(shape=(N_s, N_x), dtype=np.object)\n    GV = np.empty(shape=(N_s, N_x), dtype=np.object)\n    cv = np.array([[0, 1, 0, 0, 0, 0],\n                   [0, 0, 0, 0, 1, 0]], dtype=np.float64)\n    for i_x in range(N_x):\n        CV[0, i_x] = scipy.sparse.coo_matrix((1, 6))\n        GV[0, i_x] = scipy.sparse.coo_matrix((1, 6))\n    for i_s in range(N_s):\n        i_x = x_index[i_s]\n        k = np.array([[SIN_TH[i_s], -COS_TH[i_s]]])\n        CV[i_s, i_x] = k @ J @ cv\n        k = np.array([[-COS_TH[i_s], -SIN_TH[i_s]]])\n        GV[i_s, i_x] = k @ J @ cv\n    const['Cv'] = scipy.sparse.bmat(CV, format='csr')\n    const['Gv'] = scipy.sparse.bmat(GV, format='csr')\n    const['hv'] = np.full((N_s, ), -params['vmin'])\n\n    diag_Lv = np.full(N_s, params['sigma_v']**(-2))\n    const['Lv'] = scipy.sparse.spdiags(diag_Lv, [0], N_s, N_s, format='csr')\n\n    # 加速度コストに関するパラメータ\n    DOT_V_COS_TH = sensor_df['dotV'] * sensor_df['cos_th'].values\n    DOT_V_SIN_TH = sensor_df['dotV'] * sensor_df['sin_th'].values\n    OMEGA = sensor_df['omega'].values\n    CA = np.empty(shape=(N_s, N_x), dtype=np.object)\n    ca = np.array([[0, 1, 0, 0, 0, 0],\n                   [0, 0, 0, 0, 1, 0],\n                   [0, 0, 1, 0, 0, 0],\n                   [0, 0, 0, 0, 0, 1]], dtype=np.float64)\n    for i_x in range(N_x):\n        CA[0, i_x] = scipy.sparse.coo_matrix((2, 6))\n    for i_s in range(N_s):\n        i_x = x_index[i_s]\n        k = np.array([[0,  OMEGA[i_s], 1, 0],\n                      [-OMEGA[i_s], 0, 0, 1]])\n        CA[i_s, i_x] = k @ JJ @ ca\n    const['Ca'] = scipy.sparse.bmat(CA, format='csr')\n    const['Ya'] = np.stack([DOT_V_COS_TH, DOT_V_SIN_TH], axis=1).flatten()\n\n    diag_La = np.full(2*N_s, params['sigma_a']**(-2))\n    const['La'] = scipy.sparse.spdiags(diag_La, [0], 2*N_s, 2*N_s, format='csr')\n\n    return const\n\ndef solve_QP(const, p_valid, d_valid):\n    A = const['A']\n    B = const['B']\n    R = const['R']\n    Cp_orig = const['Cp_orig']\n    Lp_orig = const['Lp_orig']\n    Yp_orig = const['Yp_orig']\n    Cd_orig = const['Cd_orig']\n    Ld_orig = const['Ld_orig']\n    Yd_orig = const['Yd_orig']\n    \n    p_valid2 = np.stack([p_valid, p_valid], axis=1).flatten()\n    Cp = Cp_orig[p_valid2, :]\n    Lp = Lp_orig[np.ix_(p_valid2, p_valid2)]\n    Yp = Yp_orig[p_valid2]\n\n    d_valid2 = np.stack([d_valid, d_valid], axis=1).flatten()\n    Cd = Cd_orig[d_valid2, :]\n    Ld = Ld_orig[np.ix_(d_valid2, d_valid2)]\n    Yd = Yd_orig[d_valid2]\n\n    CLC_p = Cp.T @ (Lp @ Cp)\n    CLC_d = Cd.T @ (Ld @ Cd)\n    \n    CLY_p = Cp.T @ (Lp @ Yp)\n    CLY_d = Cd.T @ (Ld @ Yd)\n    \n    if const['use_sensor']:\n        Cv = const['Cv']\n        Lv = const['Lv']\n        Ca = const['Ca']\n        La = const['La']\n        Ya = const['Ya']\n        CLC_v = Cv.T @ (Lv @ Cv)\n        CLC_a = Ca.T @ (La @ Ca)\n        Q     = CLC_p + CLC_d + CLC_v + CLC_a\n\n        CLY_a = Ca.T @ (La @ Ya)\n        q     = CLY_p + CLY_d + CLY_a\n    else:\n        Q = CLC_p + CLC_d\n        q = CLY_p + CLY_d\n\n    if const['use_inquality']:\n        G = const['Gv']\n        h = const['hv']\n        X_star = qpsolver.solve_qp_with_inequality(R=R, Q=Q, q=q, A=A, B=B, G=G, h=h)\n    else:\n        X_star = qpsolver.solve_qp(R=R, Q=Q, q=q, A=A, B=B)\n    return X_star\n\ndef get_baseline(collection_name):\n    df = BASELINE_DF[BASELINE_DF['collectionName'] == collection_name].copy()\n    df.reset_index(drop=True, inplace=True)\n    return df\n\ndef get_velocity(collection_name):\n    df = VELOCITY_DF[VELOCITY_DF['collectionName'] == collection_name].copy()\n    df.reset_index(drop=True, inplace=True)\n    return df\n\ndef apply_costmin(base_df, velocity_df, sensor_df, params, N_LOOP):\n    const = get_optimization_constants(base_df, velocity_df, sensor_df, params, use_sensor=True)\n\n    if params['use_map']:\n        distance = map_matching.distance_to_nearest_neighbor(base_df)\n        default_p_valid = (distance < params['threshold_distance_to_nearest_neighbor'])\n        p_valid = default_p_valid\n    else:\n        default_p_valid = np.full(const['N_y'], True)\n        p_valid = default_p_valid\n\n    V = np.sqrt(np.sum(velocity_df[['v_east', 'v_north']].values**2, axis=1))\n    default_d_valid = (V < params['vmax'])\n    d_valid = default_d_valid\n\n    for loop in range(N_LOOP):\n        X_star = solve_QP(const, p_valid, d_valid)\n        Y_star = const['Cp_orig'] @ X_star\n        Y_star = np.reshape(Y_star, (-1, 2))\n        pp_df  = base_df.copy()\n        pp_df['latDeg'] = Y_star[:, 0]\n        pp_df['lngDeg'] = Y_star[:, 1]\n        distance = transform.pd_haversine_distance(pp_df, base_df)\n        p_valid = default_p_valid & (distance < params['reject_p'])\n\n        dXYdt = const['Cd_orig'] @ X_star\n        dXYdt = np.reshape(dXYdt, (-1, 2))\n        v_err = dXYdt - velocity_df[['v_east', 'v_north']].values\n        v_err = np.sqrt(np.sum(v_err**2, axis=1))\n        d_valid = default_d_valid & (v_err < params['reject_d'])\n\n    return pp_df\n\ndef recalibrate_sensor_by_vehicle_motion(base_df, velocity_df, sensor_df, params):\n    const = get_optimization_constants(base_df, velocity_df, sensor_df, params, use_sensor=False)\n\n    if params['use_map']:\n        distance = map_matching.distance_to_nearest_neighbor(base_df)\n        p_valid  = (distance < params['threshold_distance_to_nearest_neighbor'])\n    else:\n        p_valid = np.full(const['N_y'], True)\n\n    V = np.sqrt(np.sum(velocity_df[['v_east', 'v_north']].values**2, axis=1))\n    d_valid = (V < params['vmax'])\n\n    X_star = solve_QP(const, p_valid, d_valid)\n    X_mat  = np.reshape(X_star, (-1, 6))\n    dotB   = X_mat[const['x_index_sensor'], 1]\n    dotL   = X_mat[const['x_index_sensor'], 4]\n    dotXY  = const['J'] @ np.stack([dotB, dotL], axis=0) # shape = (2, N)\n    dotX   = dotXY[0, :]\n    dotY   = dotXY[1, :]\n    V = np.sqrt(np.sum(dotXY**2, axis=0))\n    cond = (V > (20 / 3.6))\n    trig_moving_direction = signal_f.Trig.from_data(dotX[cond], dotY[cond])\n    trig_theta   = signal_f.Trig.from_rad(sensor_df['theta'].values[cond])\n    trig_offset  = trig_theta - trig_moving_direction\n    angle_offset = np.arctan2(np.mean(trig_offset.sin), np.mean(trig_offset.cos))\n    sensor_df['theta']  = sensor_df['theta'] - angle_offset\n    sensor_df['cos_th'] = np.cos(sensor_df['theta'].values)\n    sensor_df['sin_th'] = np.sin(sensor_df['theta'].values)\n\n    return sensor_df\n\ndef do_postprocess(args):\n    train_or_test, collection, params = args\n\n    base_df = get_baseline(collection)\n    t_ref   = base_df['millisSinceGpsEpoch'].min()\n    base_df['Time'] = 1e-3 * (base_df['millisSinceGpsEpoch'] - t_ref).values\n\n    velocity_df = get_velocity(collection)\n    velocity_df['Time'] = (1e-3 * (velocity_df['millisSinceGpsEpoch'] - t_ref).values\n                           -  params['Mi8_velocity_timeshift'] * (velocity_df['phoneName'] == 'Mi8').astype(float)\n                           )\n    velocity_df = velocity_df[(  velocity_df['Time'] >= base_df['Time'].min())\n                              & (velocity_df['Time'] <= base_df['Time'].max())]\n    velocity_df.reset_index(drop=True, inplace=True)\n    \n    phone_list = [path.split('/')[-1] for path in sorted(glob.glob(f'{INPUT_PATH}/{train_or_test}/{collection}/*'))]\n    sensor_df_list   = []\n    dt_up   = 2.5 * 1e-3\n    dt_down = DT_X\n    FLT = design_filter.make_sinc_filter(F_cutoff=2.0, dt=dt_up)\n    for phone in phone_list:\n        gnss_log_filename = f'{INPUT_PATH}/{train_or_test}/{collection}/{phone}/{phone}_GnssLog.txt'\n        sensor_df_orig = io_f.read_GnssLog_sensors(gnss_log_filename)\n        if signal_f.check_sensor_availability(sensor_df_orig):\n            sensor_df = signal_f.preprocess_sensor_data(sensor_df_orig, t_ref, dt_up, dt_down, FLT)\n            sensor_df = signal_f.remove_different_posture(sensor_df)\n            sensor_df = sensor_df[(  sensor_df['Time'] >= base_df['Time'].min())\n                                  & (sensor_df['Time'] <= base_df['Time'].max())].copy()\n            sensor_df.reset_index(drop=True, inplace=True)\n            sensor_df_list.append(sensor_df)\n    if len(sensor_df_list) > 0:\n        time_list = [df['Time'].max() - df['Time'].min() for df in sensor_df_list]\n        idx = np.argmax(time_list)\n        sensor_df = sensor_df_list[idx]\n        sensor_df = signal_f.add_calibrated_signals(sensor_df, dt_down)\n        sensor_df = recalibrate_sensor_by_vehicle_motion(base_df, velocity_df, sensor_df, params)\n    else:\n        sensor_df = None\n\n    pp_df = base_df\n    pp_df = apply_costmin(pp_df, velocity_df, sensor_df, params, N_LOOP=3)\n    if params['use_map']:\n        params_stage2 = dict(params)\n        params_stage2['sigma_p'] = params['sigma_p_stage2']\n        for _ in range(params['num_stage2_iterations']):\n            pp_df = map_matching.snap_to_nearest_neighbor(pp_df)\n            pp_df = apply_costmin(pp_df, velocity_df, sensor_df, params_stage2, N_LOOP=1)\n    return pp_df\n\ndef make_postprocessing_df(train_or_test, config):\n    args_list = []\n    for collection_list, params in config:\n        for collection_name in collection_list:\n            args_list.append((train_or_test, collection_name, params))\n    processes = multiprocessing.cpu_count()\n    with multiprocessing.Pool(processes=processes) as pool:\n        df_list = pool.imap_unordered(do_postprocess, args_list)\n        df_list = tqdm(df_list, total=len(args_list))\n        df_list = list(df_list)\n    output_df = pd.concat(df_list, axis=0).sort_values(['phone', 'millisSinceGpsEpoch'])\n    return output_df\n\ndef print_score(output_df):\n    score_list = []\n    for gid, phone_df in output_df.groupby('phone'):\n        drive, phone = gid.split('_')\n        gt_df = pd.read_csv(f'{INPUT_PATH}/train/{drive}/{phone}/ground_truth.csv')\n        d = transform.pd_haversine_distance(phone_df, gt_df)\n        score = np.mean([np.quantile(d, 0.50), np.quantile(d, 0.95)])\n        score_list.append(score)\n    score = np.mean(score_list)\n    print(f'train score: {score:.3f}')\n    return\n\ndef main():\n    params_highway = { 'sigma_u'  : 1.0,\n                       'sigma_p'  : 3.0,\n                       'sigma_a'  : 2.0 * 1e+5,\n                       'sigma_v'  : 4.0 * 1e+5,\n                       'sigma_d'  : 0.16 * 1e+5,\n                       'reject_p' : 7.0,   # [m]\n                       'reject_d' : 1.0,   # [m/s]\n                       'vmin'     : -0.05, # [m/s]\n                       'vmax'     : 50.0,  # [m/s]\n                       'Mi8_velocity_timeshift' : 0.46,\n                       'use_not_go_back_constraint' : False,\n                       'use_map'  : False,\n                      }\n    params_treeway = { 'sigma_u'  : 1.0,\n                       'sigma_p'  : 6.0,\n                       'sigma_a'  : 0.8 * 1e+5,\n                       'sigma_v'  : 3.0 * 1e+5,\n                       'sigma_d'  : 0.12 * 1e+5,\n                       'reject_p' : 12.0,  # [m]\n                       'reject_d' : 1.0,   # [m/s]\n                       'vmin'     : -0.05, # [m/s]\n                       'vmax'     : 50.0,  # [m/s]\n                       'Mi8_velocity_timeshift' : 0.30,\n                       'use_not_go_back_constraint' : False,\n                       'use_map'  : False,\n                      }\n    params_downtown = { 'sigma_u'  : 1.0,\n                        'sigma_p'  : 20.0,\n                        'sigma_a'  : 0.4 * 1e+5,\n                        'sigma_v'  : 1.0 * 1e+5,\n                        'sigma_d'  : 1.3 * 1e+5,\n                        'reject_p' : 20.0,  # [m]\n                        'reject_d' : 3.0,   # [m/s]\n                        'vmin'     : -0.05, # [m/s]\n                        'vmax'     : 50.0,  # [m/s]\n                        'Mi8_velocity_timeshift' : 0.0,\n                        'use_not_go_back_constraint' : False,\n                        'use_map'  : True,\n                        'threshold_distance_to_nearest_neighbor' : 8.0,\n                        'sigma_p_stage2' : 3.0,\n                        'num_stage2_iterations' : 1,\n                       }\n    collection_list_all = np.array(sorted(path.split('/')[-1] for path in glob.glob(f'{INPUT_PATH}/train/*')))\n    collection_list_highway  = collection_list_all[np.array([1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21]) - 1]\n    collection_list_treeway  = collection_list_all[np.array([22,23,25,26,28]) - 1]\n    collection_list_downtown = collection_list_all[np.array([24,27,29]) - 1]\n    config = [\n        (collection_list_highway,  params_highway),\n        (collection_list_treeway,  params_treeway),\n        (collection_list_downtown, params_downtown),\n    ]\n    train_pp_df = make_postprocessing_df('train', config)\n    print_score(train_pp_df)\n\n    test_base = pd.read_csv(f'{INPUT_PATH}/baseline_locations_test.csv')\n    collection_list_highway, collection_list_treeway, collection_list_downtown = area_prediction.predict_area(test_base)\n    config = [\n        (collection_list_highway,  params_highway),\n        (collection_list_treeway,  params_treeway),\n        (collection_list_downtown, params_downtown),\n    ]\n    test_pp_df = make_postprocessing_df('test', config)\n\n    train_pp_df.to_csv('smoothing_2nd_train.csv', index=False)\n    test_pp_df.to_csv('smoothing_2nd_test.csv', index=False)\n\n    columns = ['phone', 'millisSinceGpsEpoch', 'latDeg', 'lngDeg']\n    sub_df = test_pp_df[columns]\n    sub_df.to_csv('submission.csv', index=False)\n    return","metadata":{"execution":{"iopub.status.busy":"2021-08-02T14:58:30.894089Z","iopub.execute_input":"2021-08-02T14:58:30.894361Z","iopub.status.idle":"2021-08-02T14:58:32.993005Z","shell.execute_reply.started":"2021-08-02T14:58:30.894333Z","shell.execute_reply":"2021-08-02T14:58:32.992013Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"main()","metadata":{"execution":{"iopub.status.busy":"2021-08-02T14:58:32.994225Z","iopub.execute_input":"2021-08-02T14:58:32.9945Z"},"trusted":true},"execution_count":null,"outputs":[]}]}