{"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":"!pip install pymap3d\n\nimport numpy as np\nimport pandas as pd\nimport pymap3d as pm\nimport pymap3d.vincenty as pmv\nimport matplotlib.pyplot as plt\nimport glob as gl\nimport scipy.optimize\nfrom tqdm.auto import tqdm\nfrom scipy.interpolate import InterpolatedUnivariateSpline\nfrom scipy.spatial import distance\nimport plotly.express as px\n\n\n# Constants\nCLIGHT = 299_792_458   # speed of light (m/s)\nRE_WGS84 = 6_378_137   # earth semimajor axis (WGS84) (m)\nOMGE = 7.2921151467E-5  # earth angular velocity (IS-GPS) (rad/s)","metadata":{"execution":{"iopub.status.busy":"2022-09-08T13:18:53.498815Z","iopub.execute_input":"2022-09-08T13:18:53.499260Z","iopub.status.idle":"2022-09-08T13:19:02.476841Z","shell.execute_reply.started":"2022-09-08T13:18:53.499226Z","shell.execute_reply":"2022-09-08T13:19:02.475428Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"# Satellite Selection","metadata":{}},{"cell_type":"code","source":"# Satellite selection using carrier frequency error, elevation angle, and C/N0\ndef satellite_selection(df, column):\n    \"\"\"\n    Args:\n        df : DataFrame from device_gnss.csv\n        column : Column name\n    Returns:\n        df: DataFrame with eliminated satellite signals\n    \"\"\"\n    idx = df[column].notnull()\n    idx &= df['CarrierErrorHz'] < 2.0e5  # carrier frequency error (Hz)\n    idx &= df['SvElevationDegrees'] > 5.0  # elevation angle (deg)\n    idx &= df['Cn0DbHz'] > 15.0  # C/N0 (dB-Hz)\n    idx &= df['MultipathIndicator'] == 0 # Multipath flag\n    idx &= df['RawPseudorangeUncertaintyMeters'] < 50\n    \n    return df[idx]","metadata":{"execution":{"iopub.status.busy":"2022-09-08T13:19:02.479585Z","iopub.execute_input":"2022-09-08T13:19:02.480086Z","iopub.status.idle":"2022-09-08T13:19:02.487337Z","shell.execute_reply.started":"2022-09-08T13:19:02.480046Z","shell.execute_reply":"2022-09-08T13:19:02.486265Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"# Pseudorange/Doppler Residuals and Jacobian","metadata":{}},{"cell_type":"code","source":"# Compute line-of-sight vector from user to satellite\ndef los_vector(xusr, xsat):\n    \"\"\"\n    Args:\n        xusr : user position in ECEF (m)\n        xsat : satellite position in ECEF (m)\n    Returns:\n        u: unit line-of-sight vector in ECEF (m)\n        rng: distance between user and satellite (m)\n    \"\"\"\n    u = xsat - xusr\n    rng = np.linalg.norm(u, axis=1).reshape(-1, 1)\n    u /= rng\n    \n    return u, rng.reshape(-1)\n\n\n# Compute Jacobian matrix\ndef jac_pr_residuals(x, xsat, pr, W):\n    \"\"\"\n    Args:\n        x : current position in ECEF (m)\n        xsat : satellite position in ECEF (m)\n        pr : pseudorange (m)\n        W : weight matrix\n    Returns:\n        W*J : Jacobian matrix\n    \"\"\"\n    u, _ = los_vector(x[:3], xsat)\n    J = np.hstack([-u, np.ones([len(pr), 1])])  # J = [-ux -uy -uz 1]\n\n    return W @ J\n\n\n# Compute pseudorange residuals\ndef pr_residuals(x, xsat, pr, W):\n    \"\"\"\n    Args:\n        x : current position in ECEF (m)\n        xsat : satellite position in ECEF (m)\n        pr : pseudorange (m)\n        W : weight matrix\n    Returns:\n        residuals*W : pseudorange residuals\n    \"\"\"\n    u, rng = los_vector(x[:3], xsat)\n\n    # Approximate correction of the earth rotation (Sagnac effect) often used in GNSS positioning\n    rng += OMGE * (xsat[:, 0] * x[1] - xsat[:, 1] * x[0]) / CLIGHT\n\n    # Add GPS L1 clock offset\n    residuals = rng - (pr - x[3])\n\n    return residuals @ W\n\n\n# Compute Jacobian matrix\ndef jac_prr_residuals(v, vsat, prr, x, xsat, W):\n    \"\"\"\n    Args:\n        v : current velocity in ECEF (m/s)\n        vsat : satellite velocity in ECEF (m/s)\n        prr : pseudorange rate (m/s)\n        x : current position in ECEF (m)\n        xsat : satellite position in ECEF (m)\n        W : weight matrix\n    Returns:\n        W*J : Jacobian matrix\n    \"\"\"\n    u, _ = los_vector(x[:3], xsat)\n    J = np.hstack([-u, np.ones([len(prr), 1])])\n\n    return W @ J\n\n\n# Compute pseudorange rate residuals\ndef prr_residuals(v, vsat, prr, x, xsat, W):\n    \"\"\"\n    Args:\n        v : current velocity in ECEF (m/s)\n        vsat : satellite velocity in ECEF (m/s)\n        prr : pseudorange rate (m/s)\n        x : current position in ECEF (m)\n        xsat : satellite position in ECEF (m)\n        W : weight matrix\n    Returns:\n        residuals*W : pseudorange rate residuals\n    \"\"\"\n    u, rng = los_vector(x[:3], xsat)\n    rate = np.sum((vsat-v[:3])*u, axis=1) \\\n          + OMGE / CLIGHT * (vsat[:, 1] * x[0] + xsat[:, 1] * v[0]\n                           - vsat[:, 0] * x[1] - xsat[:, 0] * v[1])\n\n    residuals = rate - (prr - v[3])\n\n    return residuals @ W","metadata":{"execution":{"iopub.status.busy":"2022-09-08T13:19:02.489383Z","iopub.execute_input":"2022-09-08T13:19:02.489779Z","iopub.status.idle":"2022-09-08T13:19:02.509055Z","shell.execute_reply.started":"2022-09-08T13:19:02.489745Z","shell.execute_reply":"2022-09-08T13:19:02.507112Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"# Carrier Smoothing\nSmoothing noisy pseudoranges with accurate (but absolute distance bias exists) carrier phase. Note that this is a post-processing method. The average absolute distance is added to continuously observed carrier phase.","metadata":{}},{"cell_type":"code","source":"# Carrier smoothing of pseudarange\ndef carrier_smoothing(gnss_df):\n    \"\"\"\n    Args:\n        df : DataFrame from device_gnss.csv\n    Returns:\n        df: DataFrame with carrier-smoothing pseudorange 'pr_smooth'\n    \"\"\"\n    carr_th = 1.5 # carrier phase jump threshold [m] ** 2.0 -> 1.5 **\n    pr_th =  20.0 # pseudorange jump threshold [m]\n\n    prsmooth = np.full_like(gnss_df['RawPseudorangeMeters'], np.nan)\n    # Loop for each signal\n    for (i, (svid_sigtype, df)) in enumerate((gnss_df.groupby(['Svid', 'SignalType']))):\n        df = df.replace(\n            {'AccumulatedDeltaRangeMeters': {0: np.nan}})  # 0 to NaN\n\n        # Compare time difference between pseudorange/carrier with Doppler\n        drng1 = df['AccumulatedDeltaRangeMeters'].diff() - df['PseudorangeRateMetersPerSecond']\n        drng2 = df['RawPseudorangeMeters'].diff() - df['PseudorangeRateMetersPerSecond']\n\n        # Check cycle-slip\n        slip1 = (df['AccumulatedDeltaRangeState'].to_numpy() & 2**1) != 0  # reset flag\n        slip2 = (df['AccumulatedDeltaRangeState'].to_numpy() & 2**2) != 0  # cycle-slip flag\n        slip3 = np.fabs(drng1.to_numpy()) > carr_th # Carrier phase jump\n        slip4 = np.fabs(drng2.to_numpy()) > pr_th # Pseudorange jump\n\n        idx_slip = slip1 | slip2 | slip3 | slip4\n        idx_slip[0] = True\n\n        # groups with continuous carrier phase tracking\n        df['group_slip'] = np.cumsum(idx_slip)\n\n        # Psudorange - carrier phase\n        df['dpc'] = df['RawPseudorangeMeters'] - df['AccumulatedDeltaRangeMeters']\n\n        # Absolute distance bias of carrier phase\n        meandpc = df.groupby('group_slip')['dpc'].mean()\n        df = df.merge(meandpc, on='group_slip', suffixes=('', '_Mean'))\n\n        # Index of original gnss_df\n        idx = (gnss_df['Svid'] == svid_sigtype[0]) & (\n            gnss_df['SignalType'] == svid_sigtype[1])\n\n        # Carrier phase + bias\n        prsmooth[idx] = df['AccumulatedDeltaRangeMeters'] + df['dpc_Mean']\n\n    # If carrier smoothing is not possible, use original pseudorange\n    idx_nan = np.isnan(prsmooth)\n    prsmooth[idx_nan] = gnss_df['RawPseudorangeMeters'][idx_nan]\n    gnss_df['pr_smooth'] = prsmooth\n\n    return gnss_df","metadata":{"execution":{"iopub.status.busy":"2022-09-08T13:19:02.512909Z","iopub.execute_input":"2022-09-08T13:19:02.515153Z","iopub.status.idle":"2022-09-08T13:19:02.542925Z","shell.execute_reply.started":"2022-09-08T13:19:02.515070Z","shell.execute_reply":"2022-09-08T13:19:02.539975Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"# Score Computation","metadata":{}},{"cell_type":"code","source":"# Compute distance by Vincenty's formulae\ndef vincenty_distance(llh1, llh2):\n    \"\"\"\n    Args:\n        llh1 : [latitude,longitude] (deg)\n        llh2 : [latitude,longitude] (deg)\n    Returns:\n        d : distance between llh1 and llh2 (m)\n    \"\"\"\n    d, az = np.array(pmv.vdist(llh1[:, 0], llh1[:, 1], llh2[:, 0], llh2[:, 1]))\n\n    return d\n\n\n# Compute score\ndef calc_score(llh, llh_gt):\n    \"\"\"\n    Args:\n        llh : [latitude,longitude] (deg)\n        llh_gt : [latitude,longitude] (deg)\n    Returns:\n        score : (m)\n    \"\"\"\n    d = vincenty_distance(llh, llh_gt)\n    score = np.mean([np.quantile(d, 0.50), np.quantile(d, 0.95)])\n\n    return score","metadata":{"execution":{"iopub.status.busy":"2022-09-08T13:19:02.545538Z","iopub.execute_input":"2022-09-08T13:19:02.546256Z","iopub.status.idle":"2022-09-08T13:19:02.570307Z","shell.execute_reply.started":"2022-09-08T13:19:02.546200Z","shell.execute_reply":"2022-09-08T13:19:02.569279Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"# Robust WLS\nI used `soft_l1` loss function. It is robust, but its computation speed is considerably slower than that of ordinary least squares...","metadata":{}},{"cell_type":"code","source":"# GNSS single point positioning using pseudorange\ndef point_positioning(gnss_df):\n    # Add nominal frequency to each signal\n    # Note: GLONASS is an FDMA signal, so each satellite has a different frequency\n    CarrierFrequencyHzRef = gnss_df.groupby(['Svid', 'SignalType'])[\n        'CarrierFrequencyHz'].median()\n    gnss_df = gnss_df.merge(CarrierFrequencyHzRef, how='left', on=[\n                            'Svid', 'SignalType'], suffixes=('', 'Ref'))\n    gnss_df['CarrierErrorHz'] = np.abs(\n        (gnss_df['CarrierFrequencyHz'] - gnss_df['CarrierFrequencyHzRef']))\n\n    # Carrier smoothing\n    gnss_df = carrier_smoothing(gnss_df)\n\n    # GNSS single point positioning\n    utcTimeMillis = gnss_df['utcTimeMillis'].unique()\n    nepoch = len(utcTimeMillis)\n    x0 = np.zeros(4)  # [x,y,z,tGPSL1]\n    v0 = np.zeros(4)  # [vx,vy,vz,dtGPSL1]\n    x_wls = np.full([nepoch, 3], np.nan)  # For saving position\n    v_wls = np.full([nepoch, 3], np.nan)  # For saving velocity\n    cov_x = np.full([nepoch, 3, 3], np.nan) # For saving position covariance\n    cov_v = np.full([nepoch, 3, 3], np.nan) # For saving velocity covariance\n\n    # Loop for epochs\n    for i, (t_utc, df) in enumerate(tqdm(gnss_df.groupby('utcTimeMillis'), total=nepoch)):\n        # Valid satellite selection\n        df_pr = satellite_selection(df, 'pr_smooth')\n        df_prr = satellite_selection(df, 'PseudorangeRateMetersPerSecond')\n\n        # Corrected pseudorange/pseudorange rate\n        pr = (df_pr['pr_smooth'] + df_pr['SvClockBiasMeters'] - df_pr['IsrbMeters'] -\n              df_pr['IonosphericDelayMeters'] - df_pr['TroposphericDelayMeters']).to_numpy()\n        prr = (df_prr['PseudorangeRateMetersPerSecond'] +\n               df_prr['SvClockDriftMetersPerSecond']).to_numpy()\n\n        # Satellite position/velocity\n        xsat_pr = df_pr[['SvPositionXEcefMeters', 'SvPositionYEcefMeters',\n                         'SvPositionZEcefMeters']].to_numpy()\n        xsat_prr = df_prr[['SvPositionXEcefMeters', 'SvPositionYEcefMeters',\n                           'SvPositionZEcefMeters']].to_numpy()\n        vsat = df_prr[['SvVelocityXEcefMetersPerSecond', 'SvVelocityYEcefMetersPerSecond',\n                       'SvVelocityZEcefMetersPerSecond']].to_numpy()\n\n        if len(df_pr) >= 4:\n\n            # Weight matrix for peseudorange/pseudorange rate\n            efact = {\"GPS_L1\": 1.0, \"GPS_L5\": 1.0, \"GAL_E1\": 1.5, \"GAL_E5A\": 1.5, \"BDS_B1I\": 1.0, \"QZS_J1\": 1.0, \"QZS_J5\": 1.0, \"GLO_G1\": 2.0, np.nan:1.0}\n            scaled_pr_uncertainty = df_pr['RawPseudorangeUncertaintyMeters'].to_numpy().copy()\n            for j, gnss_sys in enumerate(df_pr[\"SignalType\"].values):\n                scaled_pr_uncertainty[j] = scaled_pr_uncertainty[j]*efact[gnss_sys]\n                \n            scaled_prr_uncertainty = df_prr['PseudorangeRateUncertaintyMetersPerSecond'].to_numpy().copy()\n            for j, gnss_sys in enumerate(df_pr[\"SignalType\"].values):\n                scaled_prr_uncertainty[j] = scaled_prr_uncertainty[j]*efact[gnss_sys]\n            \n            Wx = np.diag(1 / scaled_pr_uncertainty)\n            Wv = np.diag(1 / scaled_prr_uncertainty)\n\n        # Robust WLS requires accurate initial values for convergence,\n        # so perform normal WLS for the first time\n        if len(df_pr) >= 4:\n            # Normal WLS\n            if np.all(x0 == 0):\n                opt = scipy.optimize.least_squares(\n                    pr_residuals, x0, jac_pr_residuals, args=(xsat_pr, pr, Wx), xtol=1e-15, ftol=1e-15, gtol=1e-15)\n                x0 = opt.x \n            # Robust WLS for position estimation\n            opt = scipy.optimize.least_squares(\n                 pr_residuals, x0, jac_pr_residuals, args=(xsat_pr, pr, Wx), loss='cauchy', xtol=1e-15, ftol=1e-15, gtol=1e-15)\n            if opt.status < 1 or opt.status == 2:\n                print(f'i = {i} position lsq status = {opt.status}')\n            else:\n                # Covariance estimation\n                cov = np.linalg.inv(opt.jac.T @ Wx @ opt.jac)\n                cov_x[i, :, :] = cov[:3, :3]\n                x_wls[i, :] = opt.x[:3]\n                x0 = opt.x\n                 \n        # Velocity estimation\n        if len(df_prr) >= 4:\n            if np.all(v0 == 0): # Normal WLS\n                opt = scipy.optimize.least_squares(\n                    prr_residuals, v0, jac_prr_residuals, args=(vsat, prr, x0, xsat_prr, Wv), xtol=1e-15, ftol=1e-15, gtol=1e-15)\n                v0 = opt.x\n            # Robust WLS for velocity estimation\n            opt = scipy.optimize.least_squares(\n                prr_residuals, v0, jac_prr_residuals, args=(vsat, prr, x0, xsat_prr, Wv), loss='cauchy', xtol=1e-15, ftol=1e-15, gtol=1e-15)\n            if opt.status < 1:\n                print(f'i = {i} velocity lsq status = {opt.status}')\n            else:\n                # Covariance estimation\n                cov = np.linalg.inv(opt.jac.T @ Wv @ opt.jac)\n                cov_v[i, :, :] = cov[:3, :3]\n                v_wls[i, :] = opt.x[:3]\n                v0 = opt.x\n\n    return utcTimeMillis, x_wls, v_wls, cov_x, cov_v","metadata":{"execution":{"iopub.status.busy":"2022-09-08T13:19:02.572634Z","iopub.execute_input":"2022-09-08T13:19:02.573799Z","iopub.status.idle":"2022-09-08T13:19:02.598645Z","shell.execute_reply.started":"2022-09-08T13:19:02.573721Z","shell.execute_reply":"2022-09-08T13:19:02.596893Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"# Outlier Detection and Interpolation","metadata":{}},{"cell_type":"code","source":"# Simple outlier detection and interpolation\ndef exclude_interpolate_outlier(x_wls, v_wls, cov_x, cov_v):\n    # Up velocity / height threshold\n    v_up_th = 2.6  # m/s  2.0 -> 2.6\n    height_th = 200.0 # m\n    v_out_sigma = 3.0 # m/s\n    x_out_sigma = 30.0 # m\n    \n    # Coordinate conversion\n    x_llh = np.array(pm.ecef2geodetic(x_wls[:, 0], x_wls[:, 1], x_wls[:, 2])).T\n    x_llh_mean = np.nanmean(x_llh, axis=0)\n    v_enu = np.array(pm.ecef2enuv(\n        v_wls[:, 0], v_wls[:, 1], v_wls[:, 2], x_llh_mean[0], x_llh_mean[1])).T\n\n    # Up velocity jump detection\n    # Cars don't jump suddenly!\n    idx_v_out = np.abs(v_enu[:, 2]) > v_up_th\n    idx_v_out |= np.isnan(v_enu[:, 2])\n    v_wls[idx_v_out, :] = np.nan\n    cov_v[idx_v_out] = v_out_sigma**2 * np.eye(3)\n    print(f'Number of velocity outliers {np.count_nonzero(idx_v_out)}')\n\n    # Height check\n    hmedian = np.nanmedian(x_llh[:, 2])\n    idx_x_out = np.abs(x_llh[:, 2] - hmedian) > height_th\n    idx_x_out |= np.isnan(x_llh[:, 2])\n    x_wls[idx_x_out, :] = np.nan\n    cov_x[idx_x_out] = x_out_sigma**2 * np.eye(3)\n    print(f'Number of position outliers {np.count_nonzero(idx_x_out)}')\n\n    # Interpolate NaNs at beginning and end of array\n    x_df = pd.DataFrame({'x': x_wls[:, 0], 'y': x_wls[:, 1], 'z': x_wls[:, 2]})\n    x_df = x_df.interpolate(limit_area='outside', limit_direction='both')\n\n    # Interpolate all NaN data\n    v_df = pd.DataFrame({'x': v_wls[:, 0], 'y': v_wls[:, 1], 'z': v_wls[:, 2]})\n    v_df = v_df.interpolate(limit_area='outside', limit_direction='both')\n    v_df = v_df.interpolate('spline', order=3)\n\n    return x_df.to_numpy(), v_df.to_numpy(), cov_x, cov_v","metadata":{"execution":{"iopub.status.busy":"2022-09-08T13:19:02.600413Z","iopub.execute_input":"2022-09-08T13:19:02.600798Z","iopub.status.idle":"2022-09-08T13:19:02.626368Z","shell.execute_reply.started":"2022-09-08T13:19:02.600770Z","shell.execute_reply":"2022-09-08T13:19:02.624783Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"# Kalman Smoother\nApplication of Kalman smoother; integration of velocity and position obtained by WLS.\nThe covariance matrix is a fixed value.","metadata":{}},{"cell_type":"code","source":"# Kalman filter\n# def Kalman_filter(zs, us, cov_zs, cov_us, phone):\n#     # Parameters\n#     sigma_mahalanobis = 30.0  # Mahalanobis distance for rejecting innovation\n\n#     n, dim_x = zs.shape\n#     G = np.eye(3)  # Transition matrix\n#     F = np.eye(3)  # Measurement function\n\n#     # Initial state and covariance\n#     m = zs[0, :3].T  # State\n#     P = 5.0**2 * np.eye(3)  # State covariance\n#     I = np.eye(dim_x)\n\n#     x_kf = np.zeros([n, dim_x])\n#     P_kf = np.zeros([n, dim_x, dim_x])\n\n#     # Kalman filtering\n#     for i, (u, z) in enumerate(zip(us, zs)):\n#         # First step\n#         if i == 0:\n#             x_kf[i] = m.T\n#             P_kf[i] = P\n#             continue\n\n#         # Prediction step\n#         Q = cov_us[i] # Estimated WLS velocity covariance\n#         a = G @ m + u.T\n#         P = (G @ P) @ G.T + Q\n\n#         # Check outliers for observation\n#         d = distance.mahalanobis(z, F @ a, np.linalg.inv(P))\n\n#         # Update step\n#         if d < sigma_mahalanobis:\n#             R = cov_zs[i] # Estimated WLS position covariance\n#             y = z.T - F @ a\n#             S = (F @ P) @ F.T + R\n#             K = (P @ F.T) @ np.linalg.inv(S)\n#             m = a + K @ y\n#             P = (I - (K @ F)) @ P\n#         else:\n#             # If observation update is not available, increase covariance\n#             P += 10**2*Q\n#             m = a\n\n\n#         x_kf[i] = m.T\n#         P_kf[i] = P\n\n#     return x_kf, P_kf\n\n# def Kalman_filter(zs, us, cov_zs, cov_us, phone):\n    # Parameters\n    sigma_mahalanobis = 30.0  # Mahalanobis distance for rejecting innovation\n\n    n, dim_x = zs.shape\n    G = np.eye(3)  # Transition matrix\n    F = np.eye(3)  # Measurement function\n\n    # Initial state and covariance\n    m = zs[0, :3].T  # State\n    C = 5.0**2 * np.eye(3)  # State covariance\n    I = np.eye(dim_x)\n\n    x_kf = np.zeros([n, dim_x])\n    P_kf = np.zeros([n, dim_x, dim_x])\n\n    # Kalman filtering\n    for i, (u, z) in enumerate(zip(us, zs)):\n        # First step\n        if i == 0:\n            x_kf[i] = m.T\n            P_kf[i] = C\n            continue\n\n        # 一期先予測分布\n        W = cov_us[i] # Estimated WLS velocity covariance\n        a = G @ m + u.T # 平均の更新\n        R = (G @ C) @ G.T + W # 分散の更新\n\n        # Check outliers for observation\n        d = distance.mahalanobis(z, F @ m, np.linalg.inv(C))\n\n \n        if d < sigma_mahalanobis:\n            # 一期先予測尤度\n            V = cov_zs[i] # Estimated WLS position covariance\n            f = F @ a # 平均の更新\n            Q = (F @ R) @ F.T + V # 分散の更新\n            \n            # カルマン利得\n            K = (R @ F.T) @ np.linalg.inv(Q)\n            \n            # フィルタリング分布の更新（状態の更新）\n            m = a + K @ (z.T - f) # 平均の更新\n            C = (I - (K @ F)) @ R # 分散の更新\n        else:\n            # If observation update is not available, increase covariance\n            C += 10**2*W\n            m = a\n\n        x_kf[i] = m.T\n        P_kf[i] = C\n\n    return x_kf, P_kf\n\n\n# Forward + backward Kalman filter and smoothing\ndef Kalman_smoothing(x_wls, v_wls, cov_x, cov_v, phone):\n    n, dim_x = x_wls.shape\n\n    # For some unknown reason, the speed estimation is wrong only for XiaomiMi8\n    # so the variance is increased\n    if phone == 'XiaomiMi8':\n        v_wls = np.vstack([(v_wls[:-1, :] + v_wls[1:, :])/2, np.zeros([1, 3])])\n        cov_v = 1000.0**2 * cov_v\n         \n    # Forward\n    v = np.vstack([np.zeros([1, 3]), (v_wls[:-1, :] + v_wls[1:, :])/2])\n    x_f, P_f = Kalman_filter(x_wls, v, cov_x, cov_v, phone)\n\n    # Backward\n    v = -np.flipud(v_wls)\n    v = np.vstack([np.zeros([1, 3]), (v[:-1, :] + v[1:, :])/2])\n    cov_xf = np.flip(cov_x, axis=0)\n    cov_vf = np.flip(cov_v, axis=0)\n    x_b, P_b = Kalman_filter(np.flipud(x_wls), v, cov_xf, cov_vf, phone)\n\n    # Smoothing\n    x_fb = np.zeros_like(x_f)\n    P_fb = np.zeros_like(P_f)\n    for (f, b) in zip(range(n), range(n-1, -1, -1)):\n        P_fi = np.linalg.inv(P_f[f])\n        P_bi = np.linalg.inv(P_b[b])\n\n        P_fb[f] = np.linalg.inv(P_fi + P_bi)\n        x_fb[f] = P_fb[f] @ (P_fi @ x_f[f] + P_bi @ x_b[b])\n\n    return x_fb, x_f, np.flipud(x_b)","metadata":{"execution":{"iopub.status.busy":"2022-09-08T13:29:19.333418Z","iopub.execute_input":"2022-09-08T13:29:19.333806Z","iopub.status.idle":"2022-09-08T13:29:19.354447Z","shell.execute_reply.started":"2022-09-08T13:29:19.333780Z","shell.execute_reply":"2022-09-08T13:29:19.352335Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"# velocity filter","metadata":{}},{"cell_type":"code","source":"from scipy.signal import savgol_filter\ndef apply_savgol_filter(df, wl, poly):\n    df[\"x_filterd\"] = savgol_filter(df.x, wl, poly,  mode='nearest')\n    df[\"y_filterd\"] = savgol_filter(df.y, wl, poly,  mode='nearest')\n    df[\"z_filterd\"] = savgol_filter(df.z, wl, poly,  mode='nearest')\n    return df","metadata":{"execution":{"iopub.status.busy":"2022-09-08T13:19:02.659166Z","iopub.execute_input":"2022-09-08T13:19:02.660870Z","iopub.status.idle":"2022-09-08T13:19:02.681960Z","shell.execute_reply.started":"2022-09-08T13:19:02.660805Z","shell.execute_reply":"2022-09-08T13:19:02.680172Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"# Step1.WLSにより衛星通信データから各時点の位置、速度、位置の共分散、速度の共分散を得る","metadata":{}},{"cell_type":"code","source":"# Target course/phone\npath = '/kaggle/input/smartphone-decimeter-2022/train/2021-03-16-US-MTV-1/GooglePixel4XL'\n\ndrive, phone = path.split('/')[-2:]\n\n# Read data\ngnss_df = pd.read_csv(f'{path}/device_gnss.csv')  # GNSS data\ngt_df = pd.read_csv(f'{path}/ground_truth.csv')  # ground truth\n\n# Point positioning\nutc, x_wls, v_wls, cov_x, cov_v = point_positioning(gnss_df)\n\n# Exclude velocity outliers\nx_wls, v_wls, cov_x, cov_v = exclude_interpolate_outlier(x_wls, v_wls, cov_x, cov_v)\n\n# filter velocity\nv_df = pd.DataFrame({'x': v_wls[:, 0], 'y': v_wls[:, 1], 'z': v_wls[:, 2]})\nwl = 7\npoly = 5\npred_df = apply_savgol_filter(v_df, wl, poly)\nv_wls = pred_df[[\"x_filterd\", \"y_filterd\", \"z_filterd\"]].values\n\n# Exclude velocity outliers\nx_wls, v_wls, cov_x, cov_v = exclude_interpolate_outlier(x_wls, v_wls, cov_x, cov_v)\n\n# Convert to latitude and longitude\nllh_wls = np.array(pm.ecef2geodetic(x_wls[:, 0], x_wls[:, 1], x_wls[:, 2])).T\n\n# Baseline\nx_bl = gnss_df.groupby('TimeNanos')[\n    ['WlsPositionXEcefMeters', 'WlsPositionYEcefMeters', 'WlsPositionZEcefMeters']].mean().to_numpy()\nllh_bl = np.array(pm.ecef2geodetic(x_bl[:, 0], x_bl[:, 1], x_bl[:, 2])).T\n","metadata":{"execution":{"iopub.status.busy":"2022-09-08T13:19:02.684001Z","iopub.execute_input":"2022-09-08T13:19:02.684823Z","iopub.status.idle":"2022-09-08T13:19:59.178528Z","shell.execute_reply.started":"2022-09-08T13:19:02.684793Z","shell.execute_reply":"2022-09-08T13:19:59.177940Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"def visualize_trafic(df, zoom=9):\n    fig = px.scatter_mapbox(df,\n                            \n                            # Here, plotly gets, (x,y) coordinates\n                            lat=\"LatitudeDegrees\",\n                            lon=\"LongitudeDegrees\",\n                            \n#                             #Here, plotly detects color of series\n                            color=\"tripId\",\n                            labels=\"tripId\",\n                            \n                            zoom=zoom,\n                            center={\"lat\":df[\"LatitudeDegrees\"].mean(), \"lon\":df[\"LongitudeDegrees\"].mean()},\n                            height=500,\n                            width=1000,\n#                             hover_data=['index']\n                           )\n    fig.update_layout(mapbox_style='stamen-terrain')\n    fig.update_layout(margin={\"r\": 0, \"t\": 0, \"l\": 0, \"b\": 0})\n    fig.update_layout(title_text=\"GPS trafic\")\n    fig.show()","metadata":{"execution":{"iopub.status.busy":"2022-09-08T13:19:59.179645Z","iopub.execute_input":"2022-09-08T13:19:59.180252Z","iopub.status.idle":"2022-09-08T13:19:59.186938Z","shell.execute_reply.started":"2022-09-08T13:19:59.180222Z","shell.execute_reply":"2022-09-08T13:19:59.186287Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"gt_df","metadata":{"execution":{"iopub.status.busy":"2022-09-08T13:19:59.188230Z","iopub.execute_input":"2022-09-08T13:19:59.188717Z","iopub.status.idle":"2022-09-08T13:19:59.222138Z","shell.execute_reply.started":"2022-09-08T13:19:59.188677Z","shell.execute_reply":"2022-09-08T13:19:59.221533Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"llh_wls_pdf = pd.DataFrame(llh_wls)\nllh_wls_pdf.columns = [\"LatitudeDegrees\", \"LongitudeDegrees\", \"height\"]\nllh_wls_pdf[\"tripId\"] = \"wls\"\n\n# llh_bl_pdf = pd.DataFrame(llh_bl)\n# llh_bl_pdf.columns = [\"LatitudeDegrees\", \"LongitudeDegrees\", \"height\"]\n# llh_bl_pdf[\"tripId\"] = \"bl\"\n\n# Ground truth\nllh_gt_pdf = gt_df[['LatitudeDegrees', 'LongitudeDegrees']]\nllh_gt_pdf[\"tripId\"] = \"gt\"\n\nllh_pdf = pd.concat((llh_wls_pdf, llh_gt_pdf),axis=0)\n\nvisualize_trafic(llh_pdf)","metadata":{"execution":{"iopub.status.busy":"2022-09-08T13:19:59.226277Z","iopub.execute_input":"2022-09-08T13:19:59.228885Z","iopub.status.idle":"2022-09-08T13:19:59.305582Z","shell.execute_reply.started":"2022-09-08T13:19:59.228850Z","shell.execute_reply":"2022-09-08T13:19:59.304881Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"# Step2.カルマンフィルタを適用する","metadata":{"execution":{"iopub.status.busy":"2022-09-04T08:04:06.127096Z","iopub.execute_input":"2022-09-04T08:04:06.127555Z","iopub.status.idle":"2022-09-04T08:04:06.132815Z","shell.execute_reply.started":"2022-09-04T08:04:06.127495Z","shell.execute_reply":"2022-09-04T08:04:06.131632Z"}}},{"cell_type":"code","source":"\n# Kalman smoothing\nx_kf, _, _ = Kalman_smoothing(x_wls, v_wls, cov_x, cov_v, phone)\n\n# Convert to latitude and longitude\nllh_kf = np.array(pm.ecef2geodetic(x_kf[:, 0], x_kf[:, 1], x_kf[:, 2])).T\n\nllh_kf_pdf = pd.DataFrame(llh_kf)\nllh_kf_pdf.columns = [\"LatitudeDegrees\", \"LongitudeDegrees\", \"height\"]\nllh_kf_pdf[\"tripId\"] = \"kf\"\n\nllh_pdf = pd.concat((llh_wls_pdf, llh_gt_pdf, llh_kf_pdf),axis=0)\n\nvisualize_trafic(llh_pdf)","metadata":{"execution":{"iopub.status.busy":"2022-09-08T13:29:24.381632Z","iopub.execute_input":"2022-09-08T13:29:24.381958Z","iopub.status.idle":"2022-09-08T13:29:24.862770Z","shell.execute_reply.started":"2022-09-08T13:29:24.381934Z","shell.execute_reply":"2022-09-08T13:29:24.862171Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"","metadata":{},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"","metadata":{},"execution_count":null,"outputs":[]}]}