{"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":"code","source":"BIAS_X = 0.3 # [m]\nBIAS_Y = 0.5 # [m]","metadata":{"execution":{"iopub.status.busy":"2021-08-02T15:46:36.362419Z","iopub.execute_input":"2021-08-02T15:46:36.362836Z","iopub.status.idle":"2021-08-02T15:46:36.373624Z","shell.execute_reply.started":"2021-08-02T15:46:36.362755Z","shell.execute_reply":"2021-08-02T15:46:36.372822Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"%%writefile constants.py\n\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":{"_uuid":"8f2839f25d086af736a60e9eeb907d3b93b6e0e5","_cell_guid":"b1076dfc-b9ad-4769-8c92-a6c4dae69d19","_kg_hide-input":true,"execution":{"iopub.status.busy":"2021-08-02T15:46:36.374962Z","iopub.execute_input":"2021-08-02T15:46:36.375378Z","iopub.status.idle":"2021-08-02T15:46:36.389187Z","shell.execute_reply.started":"2021-08-02T15:46:36.375349Z","shell.execute_reply":"2021-08-02T15:46:36.388430Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"%%writefile transform.py\n\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 BLH_to_ENU(pos, base):\n    pos  = BLH_to_ECEF(pos)\n    base = BLH_to_ECEF(base)\n    return ECEF_to_ENU(pos, base)\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-02T15:46:36.390968Z","iopub.execute_input":"2021-08-02T15:46:36.391537Z","iopub.status.idle":"2021-08-02T15:46:36.406078Z","shell.execute_reply.started":"2021-08-02T15:46:36.391489Z","shell.execute_reply":"2021-08-02T15:46:36.405129Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"import numpy as np\nimport pandas as pd\nfrom scipy.interpolate import InterpolatedUnivariateSpline\n\nimport transform\n\ndef bias_correction_phone(args):\n    phone, phone_df = args\n\n    B = np.deg2rad(phone_df['latDeg'].values)\n    L = np.deg2rad(phone_df['lngDeg'].values)\n    H = np.zeros_like(B)\n    BLH = transform.BLH(lat=B, lng=L, hgt=H)\n    J = transform.jacobian_BL_to_EN(BLH)\n\n    t_ref  = phone_df['millisSinceGpsEpoch'].min()\n    TIME   = 1e-3 * (phone_df['millisSinceGpsEpoch'] - t_ref).values\n    dotB   = InterpolatedUnivariateSpline(TIME, B, k=3).derivative()(TIME)\n    dotL   = InterpolatedUnivariateSpline(TIME, L, k=3).derivative()(TIME)\n    dotBL  = np.stack([dotB, dotL], axis=1)\n    dotEN  = np.einsum('nij,nj->ni', J, dotBL)\n    absV   = np.sqrt(np.sum(dotEN**2, axis=1))\n    th_az  = np.arctan2(dotEN[:, 0], dotEN[:, 1])\n    \n    cos_az = np.cos(th_az)\n    sin_az = np.sin(th_az)\n    valid  = (absV > (5 / 3.6))\n    cos_az = InterpolatedUnivariateSpline(TIME[valid], cos_az[valid], k=1, ext=3)(TIME)\n    sin_az = InterpolatedUnivariateSpline(TIME[valid], sin_az[valid], k=1, ext=3)(TIME)\n    th_az  = np.arctan2(sin_az, cos_az)\n    cos_az = np.cos(th_az)\n    sin_az = np.sin(th_az)\n\n    delta_X  = - BIAS_X\n    delta_Y  = - BIAS_Y\n    delta_E  = (  cos_az * delta_X) + (sin_az * delta_Y)\n    delta_N  = (- sin_az * delta_X) + (cos_az * delta_Y)\n    delta_EN = np.stack([delta_E, delta_N], axis=0) # shape = (2, N)\n    Jinv = np.linalg.inv(np.mean(J, axis=0))\n    delta_BL_rad = Jinv @ delta_EN\n    delta_BL_deg = np.rad2deg(delta_BL_rad)\n\n    output_df = pd.DataFrame({\n        'phone'               : phone_df['phone'],\n        'millisSinceGpsEpoch' : phone_df['millisSinceGpsEpoch'],\n        'latDeg'              : phone_df['latDeg'] + delta_BL_deg[0, :],\n        'lngDeg'              : phone_df['lngDeg'] + delta_BL_deg[1, :],\n    })\n    delta_df = pd.DataFrame({\n        'collectionName'      : phone_df['collectionName'],\n        'phoneName'           : phone_df['phoneName'],\n        'phone'               : phone_df['phone'],\n        'millisSinceGpsEpoch' : phone_df['millisSinceGpsEpoch'],\n        'delta_latDeg'        : delta_BL_deg[0, :],\n        'delta_lngDeg'        : delta_BL_deg[1, :],\n    })\n    return output_df, delta_df\n\ndef bias_correction(base_df):\n    result = map(bias_correction_phone, base_df.groupby('phone'))\n    output_df_list, delta_df_list = zip(*result)\n\n    output_df = pd.concat(output_df_list, axis=0)\n    output_df = output_df.sort_values(['phone', 'millisSinceGpsEpoch'])\n\n    delta_df = pd.concat(delta_df_list, axis=0)\n    delta_df = delta_df.sort_values(['phone', 'millisSinceGpsEpoch'])\n\n    return output_df, delta_df","metadata":{"execution":{"iopub.status.busy":"2021-08-02T15:46:36.481396Z","iopub.execute_input":"2021-08-02T15:46:36.481909Z","iopub.status.idle":"2021-08-02T15:46:36.882485Z","shell.execute_reply.started":"2021-08-02T15:46:36.481877Z","shell.execute_reply":"2021-08-02T15:46:36.881626Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"def main():\n    train_df = pd.read_csv('../input/dsdc-unified-post-processing/smoothing_2nd_train.csv')\n    test_df  = pd.read_csv('../input/dsdc-unified-post-processing/smoothing_2nd_test.csv')\n    _,      train_delta_df = bias_correction(train_df)\n    sub_df, test_delta_df  = bias_correction(test_df)\n    sub_df.to_csv('submission.csv', index=False)\n    train_delta_df.to_csv('bias_correction_train.csv', index=False)\n    test_delta_df.to_csv('bias_correction_test.csv', index=False)\n    return","metadata":{"execution":{"iopub.status.busy":"2021-08-02T15:46:36.884502Z","iopub.execute_input":"2021-08-02T15:46:36.884849Z","iopub.status.idle":"2021-08-02T15:46:36.891011Z","shell.execute_reply.started":"2021-08-02T15:46:36.884803Z","shell.execute_reply":"2021-08-02T15:46:36.889551Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"main()","metadata":{"execution":{"iopub.status.busy":"2021-08-02T15:46:36.893254Z","iopub.execute_input":"2021-08-02T15:46:36.893755Z","iopub.status.idle":"2021-08-02T15:46:41.399988Z","shell.execute_reply.started":"2021-08-02T15:46:36.893639Z","shell.execute_reply":"2021-08-02T15:46:41.399172Z"},"trusted":true},"execution_count":null,"outputs":[]}]}