{"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":"Since the smartphones are placed in front of the vehicle, the radio waves from the back of the vehicle should always be affected by multipath.\nTherefore, the GPS positioning results should be statistically biased toward the front of the vehicle.\n\n![](https://www.googleapis.com/download/storage/v1/b/kaggle-user-content/o/inbox%2F603584%2F88764e97cd9c2df83195454039b9e544%2Fsmartphone-dec.png?generation=1620664283744618&alt=media)","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-01T08:42:35.229193Z","iopub.execute_input":"2021-08-01T08:42:35.229769Z","iopub.status.idle":"2021-08-01T08:42:35.243094Z","shell.execute_reply.started":"2021-08-01T08:42:35.229684Z","shell.execute_reply":"2021-08-01T08:42:35.242208Z"},"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 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-01T08:42:35.244817Z","iopub.execute_input":"2021-08-01T08:42:35.245135Z","iopub.status.idle":"2021-08-01T08:42:35.26006Z","shell.execute_reply.started":"2021-08-01T08:42:35.245106Z","shell.execute_reply":"2021-08-01T08:42:35.259143Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"import glob\nimport numpy as np\nimport pandas as pd\nimport matplotlib.pyplot as plt\nfrom scipy.interpolate import InterpolatedUnivariateSpline\n\nimport transform\n\nINPUT_PATH = '../input/google-smartphone-decimeter-challenge'\n\nBASELINE_DF = pd.read_csv(f'{INPUT_PATH}/baseline_locations_train.csv')\ndef get_baseline(drive, phone):\n    df = BASELINE_DF[BASELINE_DF['phone'] == f'{drive}_{phone}'].copy()\n    df.reset_index(drop=True, inplace=True)\n    return df\n\ndef get_ground_truth(drive, phone):\n    return pd.read_csv(f'{INPUT_PATH}/train/{drive}/{phone}/ground_truth.csv')\n\ndef calc_bias(drive, phone):\n    gt_df   = get_ground_truth(drive, phone)\n    base_df = get_baseline(drive, phone)\n\n    BLH_gt = transform.BLH(\n        lat = np.deg2rad(gt_df['latDeg'].values),\n        lng = np.deg2rad(gt_df['lngDeg'].values),\n        hgt = np.zeros(gt_df.shape[0]),\n    ) \n    BLH_base = transform.BLH(\n        lat = np.deg2rad(base_df['latDeg'].values),\n        lng = np.deg2rad(base_df['lngDeg'].values),\n        hgt = np.zeros(gt_df.shape[0]),\n    )\n    err_ENU = transform.BLH_to_ENU(pos=BLH_base, base=BLH_gt)\n    errE = err_ENU.east\n    errN = err_ENU.north\n    \n    t_ref  = gt_df['millisSinceGpsEpoch'].min()\n    time   = 1e-3 * (gt_df['millisSinceGpsEpoch'] - t_ref).values\n    B_fn   = InterpolatedUnivariateSpline(time, np.deg2rad(gt_df['latDeg'].values), k=3)\n    L_fn   = InterpolatedUnivariateSpline(time, np.deg2rad(gt_df['lngDeg'].values), k=3)\n    dotB   = B_fn.derivative()(time)\n    dotL   = L_fn.derivative()(time)\n    dotBL  = np.stack([dotB, dotL], axis=1)\n    J      = transform.jacobian_BL_to_EN(BLH_gt)\n    dotEN  = np.einsum('nij,nj->ni', J, dotBL)\n    th_az  = np.arctan2(dotEN[:, 0], dotEN[:, 1])\n    cos_az = np.cos(th_az)\n    sin_az = np.sin(th_az)\n    absV   = np.sqrt(np.sum(dotEN**2, axis=1))\n    errX   = cos_az * errE - sin_az * errN\n    errY   = sin_az * errE + cos_az * errN\n    cond   = (absV > (10 / 3.6))\n\n    biasX = np.mean(errX[cond])\n    biasY = np.mean(errY[cond])\n\n    return biasX, biasY","metadata":{"execution":{"iopub.status.busy":"2021-08-01T08:42:35.261597Z","iopub.execute_input":"2021-08-01T08:42:35.262052Z","iopub.status.idle":"2021-08-01T08:42:35.938134Z","shell.execute_reply.started":"2021-08-01T08:42:35.262021Z","shell.execute_reply":"2021-08-01T08:42:35.937126Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"path_list = sorted(glob.glob(f'{INPUT_PATH}/train/*/*'))\nargs_list = [path.split('/')[-2:] for path in path_list]\nbiasX_list = []\nbiasY_list = []\nfor drive, phone in args_list:\n    biasX, biasY = calc_bias(drive, phone)\n    key = f'{drive}_{phone}'\n    print(f'{key:<40} : (biasX, biasY) = ({biasX:.3f}, {biasY:.3f})')\n    biasX_list.append(biasX)\n    biasY_list.append(biasY)\nbiasX_mean = np.mean(biasX_list)\nbiasY_mean = np.mean(biasY_list)\nprint('--')\nprint(f'(biasX, biasY) = ({biasX_mean:.3f}, {biasY_mean:.3f})')","metadata":{"execution":{"iopub.status.busy":"2021-08-01T08:44:32.03083Z","iopub.execute_input":"2021-08-01T08:44:32.031261Z","iopub.status.idle":"2021-08-01T08:44:34.400226Z","shell.execute_reply.started":"2021-08-01T08:44:32.031228Z","shell.execute_reply":"2021-08-01T08:44:34.399387Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"fig = plt.figure()\nplt.scatter(biasX_list, biasY_list, s=10)\nplt.grid(True)\nplt.axis('equal')\nplt.xlim([-10, 10])\nplt.ylim([-10, 10])\nNone","metadata":{"execution":{"iopub.status.busy":"2021-08-01T08:43:42.415966Z","iopub.execute_input":"2021-08-01T08:43:42.416378Z","iopub.status.idle":"2021-08-01T08:43:42.608102Z","shell.execute_reply.started":"2021-08-01T08:43:42.416337Z","shell.execute_reply":"2021-08-01T08:43:42.606968Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"The outliers have a large effect on the lateral bias of the vehicle, but the front-rear direction is clearly biased forward.\n\n(PS)\nThe lateral bias may be an effect of the driver.\nSince the driver is on the left side, it is difficult for radio waves from the left to reach directly to smartphones, which may be the reason why position is biased to the right.","metadata":{}}]}