{"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\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-08-05T06:21:55.709806Z","iopub.execute_input":"2022-08-05T06:21:55.710702Z","iopub.status.idle":"2022-08-05T06:22:10.21105Z","shell.execute_reply.started":"2022-08-05T06:21:55.710666Z","shell.execute_reply":"2022-08-05T06:22:10.210153Z"},"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.0e6 #2.0e6  # carrier frequency error (Hz)\n    idx &= df['SvElevationDegrees'] > 10.0  # elevation angle (deg)\n    #idx &= df['Cn0DbHz'] > 29.6  # C/N0 (dB-Hz)\n#     idx &= df['MultipathIndicator'] == 0 # Multipath flag\n    \n#     ### new columns added\n#     idx &= df['PseudorangeRateUncertaintyMetersPerSecond'] < 1 # prange unc 1 original\n#     idx &= df['RawPseudorangeUncertaintyMeters'] < 10.0  # raw prange unc\n \n\n    return df[idx]","metadata":{"execution":{"iopub.status.busy":"2022-08-05T06:22:14.713289Z","iopub.execute_input":"2022-08-05T06:22:14.713939Z","iopub.status.idle":"2022-08-05T06:22:14.719489Z","shell.execute_reply.started":"2022-08-05T06:22:14.713902Z","shell.execute_reply":"2022-08-05T06:22:14.718637Z"},"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-08-05T06:22:17.813574Z","iopub.execute_input":"2022-08-05T06:22:17.814088Z","iopub.status.idle":"2022-08-05T06:22:17.827825Z","shell.execute_reply.started":"2022-08-05T06:22:17.814054Z","shell.execute_reply":"2022-08-05T06:22:17.826401Z"},"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    carr_th = 1.5 # carrier phase jump threshold [m] ** 2.0 -> 1.5 **  1.5, 20 are original\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        #print ('cp diff', drng1.to_numpy())\n        \n        \n        drng2 = df['RawPseudorangeMeters'].diff() - df['PseudorangeRateMetersPerSecond']\n        #print ('pseudo diff', drng2.to_numpy())\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-08-05T06:22:21.896363Z","iopub.execute_input":"2022-08-05T06:22:21.897093Z","iopub.status.idle":"2022-08-05T06:22:21.907289Z","shell.execute_reply.started":"2022-08-05T06:22:21.89705Z","shell.execute_reply":"2022-08-05T06:22:21.906347Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"from scipy import integrate\nimport math\n\n# GNSS single point positioning using pseudorange\ndef point_positioning(gnss_df):\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        # Weight matrix for peseudorange/pseudorange rate\n        Wx = np.diag(1 / df_pr['RawPseudorangeUncertaintyMeters'].to_numpy())\n        Wv = np.diag(1 / df_prr['PseudorangeRateUncertaintyMetersPerSecond'].to_numpy())\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))\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='soft_l1')\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#                 print ('jacobian',opt.jac.T )\n#                 print ('wx',Wx )\n### added these two lines for addressing singularity if needed\n                k = opt.jac.T @ Wx @ opt.jac\n                k += np.eye(k.shape[0])\n\n                #cov = np.linalg.pinv(opt.jac.T @ Wx @ opt.jac)\n                cov = np.linalg.inv(k)\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))\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='soft_l1')\n            if opt.status < 1:\n                print(f'i = {i} velocity lsq status = {opt.status}')\n            else:\n                # Covariance estimation\n                k = opt.jac.T @ Wv @ opt.jac\n                k += np.eye(k.shape[0])\n                #cov = np.linalg.pinv(opt.jac.T @ Wv @ opt.jac)\n                cov = np.linalg.inv(k)\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\n\n\n\n# Simple outlier detection and interpolation\ndef exclude_interpolate_outlier(x_wls, v_wls, cov_x, cov_v, df_temp, utc):\n      \n    v_up_th = 2  # m/s  2.0 -> 2.6\n    height_th = 180  #200.0 # m\n    v_out_sigma = 0.1  #1.0 # m/s\n    x_out_sigma = 5.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    \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\n","metadata":{"execution":{"iopub.status.busy":"2022-08-05T06:22:26.065923Z","iopub.execute_input":"2022-08-05T06:22:26.066299Z","iopub.status.idle":"2022-08-05T06:22:26.23488Z","shell.execute_reply.started":"2022-08-05T06:22:26.066268Z","shell.execute_reply":"2022-08-05T06:22:26.234031Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"# Kalman filter\ndef Kalman_filter(zs, us, cov_zs, cov_us, phone, pval, pupd,sigma_mahalanobis): # 10 originally\n    n, dim_x = zs.shape\n    F = np.eye(3)  # Transition matrix\n    H = np.eye(3)  # Measurement function\n\n    # Initial state and covariance\n    x = zs[0, :3].T  # State\n    P = 50.0**2 * np.eye(3)  # State covariance\n    P = pval **2 * np.eye(3)\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] = x.T\n            P_kf[i] = P\n            continue\n\n        # Prediction step\n        Q = cov_us[i] # Estimated WLS velocity covariance\n        x = F @ x + u.T\n        P = (F @ P) @ F.T + Q\n\n        # Check outliers for observation\n        d = distance.mahalanobis(z, H @ x, 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 - H @ x\n            S = (H @ P) @ H.T + R\n            K = (P @ H.T) @ np.linalg.inv(S)\n            x = x + K @ y\n            P = (I - (K @ H)) @ P\n        else:\n            # If observation update is not available, increase covariance\n            P += 10**2*Q\n\n        x_kf[i] = x.T\n        P_kf[i] = P\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, pval, pupd,sigma_mahalanobis):\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,pval, pupd,sigma_mahalanobis)\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,pval, pupd,sigma_mahalanobis)\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-08-05T06:22:30.046912Z","iopub.execute_input":"2022-08-05T06:22:30.047589Z","iopub.status.idle":"2022-08-05T06:22:30.064514Z","shell.execute_reply.started":"2022-08-05T06:22:30.047554Z","shell.execute_reply":"2022-08-05T06:22:30.063669Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"import numpy as np\nimport numpy.matlib\n\n\ndef preprocessing(gnss_df, xkf):\n    num_sats = 30\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    features_all = np.zeros((nepoch,num_sats,6)) # first index is time, second index is max. no of sats and third index is no. of features\n    wls_solution =  np.zeros((nepoch,3))\n    #print (features_all.shape)\n    # Loop for epochs\n    for i, (t_utc, df) in enumerate(tqdm(gnss_df.groupby('utcTimeMillis'), total=nepoch)):\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        ## change this based on if using WLS solution or KF solution\n        #xusr =  df_pr[['WlsPositionXEcefMeters','WlsPositionYEcefMeters','WlsPositionZEcefMeters']].to_numpy() \n        \n        punc = df_pr[['RawPseudorangeUncertaintyMeters']].to_numpy()\n        cn0 = df_pr[['Cn0DbHz']].to_numpy()\n        \n        if pr.shape[0] == 0:\n            features_all[i,0,0] = -500000\n        else:\n            xusr = np.matlib.repmat(xkf[i], xsat_pr.shape[0],1)\n            wls_solution[i,:] = xusr[0,:]\n\n            los, expected_ranges = los_vector(xusr, xsat_prr)\n            residuals = (pr - expected_ranges)\n            features = np.concatenate((np.reshape(residuals, [-1, 1]), los, punc, cn0), axis=1)\n#             print (features.shape)\n#             print(features[0:20])\n            m = features.shape[0]  ## get the number of entries in this column ..\n            if m < num_sats:\n                features_all[i,0:m,:] = features\n            else:\n                features_all[i,0:num_sats,:] = features[:num_sats,:]\n                \n\n\n\n\n    return features_all, wls_solution","metadata":{"execution":{"iopub.status.busy":"2022-08-05T06:22:33.546158Z","iopub.execute_input":"2022-08-05T06:22:33.546563Z","iopub.status.idle":"2022-08-05T06:22:33.562936Z","shell.execute_reply.started":"2022-08-05T06:22:33.54653Z","shell.execute_reply":"2022-08-05T06:22:33.562076Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"def return_labels(gt_ecef, kf_solution):\n    error = gt_ecef - kf_solution\n    #print ('error', error.shape)\n    return error","metadata":{"execution":{"iopub.status.busy":"2022-08-05T06:22:36.875633Z","iopub.execute_input":"2022-08-05T06:22:36.87628Z","iopub.status.idle":"2022-08-05T06:22:36.882011Z","shell.execute_reply.started":"2022-08-05T06:22:36.876243Z","shell.execute_reply":"2022-08-05T06:22:36.880035Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"**TRAINING SCRIPT**","metadata":{}},{"cell_type":"code","source":"path = '/kaggle/input/smartphone-decimeter-2022'\ntest_dfs = []\n\nlabels_all = []\nfeatures_all = []\n\n# Loop for each trip\nfor i, dirname in enumerate(tqdm(sorted(gl.glob(f'{path}/train/*/*/')))):\n    drive, phone = dirname.split('/')[-3:-1]\n    fname = dirname + 'device_gnss.csv'\n    #print (fname, phone)\n   \n    gtfname = dirname + 'ground_truth.csv'\n    gt_df = pd.read_csv(gtfname) \n    inds = gt_df['AltitudeMeters'].isnull().values.any()\n    if inds == True:\n        continue\n    \n    else:\n        print (fname)\n        gt_lla = gt_df[['LatitudeDegrees', 'LongitudeDegrees','AltitudeMeters']].to_numpy()\n        gt_ecef = np.array(pm.geodetic2ecef(gt_lla[:, 0], gt_lla[:, 1], gt_lla[:, 2])).T\n        \n        gnss_df = pd.read_csv(fname)\n        utc, x_wls, v_wls, cov_x, cov_v = point_positioning(gnss_df)  # modified\n        x_wls, v_wls, cov_x, cov_v = exclude_interpolate_outlier(x_wls, v_wls, cov_x, cov_v, gnss_df, utc)\n        x_kf, _, _ = Kalman_smoothing(x_wls, v_wls, cov_x, cov_v, phone,1,137,10)\n        features_dataset, soln = preprocessing(gnss_df, x_kf)  ## here wls solution is the KF solution\n        \n        if np.any(features_dataset[:,0,0] == -500000):\n            print ('skipping')\n            continue\n        else:\n            labels = return_labels(gt_ecef, x_kf)  \n            labels_all.append(labels)\n            features_all.append(features_dataset)\n        \n","metadata":{"jupyter":{"source_hidden":true},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"import numpy as np\nlabels_all = np.array(labels_all)\nfeatures_all = np.array(features_all)\n#print (labels_all[:100], features_all[:100])\n\n\nnp.save('labels.npy',labels_all , allow_pickle=True)\nnp.save('features.npy', features_all, allow_pickle=True)","metadata":{"jupyter":{"source_hidden":true},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"import numpy as np\nlabels_all = np.load('../input/dataset/labels (2).npy', allow_pickle = True)\nfeatures_all = np.load('../input/dataset/features (2).npy', allow_pickle = True)","metadata":{"jupyter":{"source_hidden":true},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"import torch\nprint(torch.__version__)\nprint(torch.version.cuda)\n!pip install torch-scatter -f https://data.pyg.org/whl/torch-1.11.0+cu110.html\n!pip install torch-sparse -f https://data.pyg.org/whl/torch-1.11.0+cu110.html\n!pip install torch-geometric","metadata":{"execution":{"iopub.status.busy":"2022-08-05T05:46:18.566636Z","iopub.execute_input":"2022-08-05T05:46:18.567074Z","iopub.status.idle":"2022-08-05T06:21:27.031475Z","shell.execute_reply.started":"2022-08-05T05:46:18.566991Z","shell.execute_reply":"2022-08-05T06:21:27.029899Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"import torchvision\nimport torch.nn.functional as F\nimport torch.nn as nn\nimport torch_geometric.nn as pyg_nn\n\nclass GCN(nn.Module):\n    def __init__(self, input_dim, hidden_dim, output_dim): \n        super(GCN, self).__init__()\n        self.convs = nn.ModuleList()\n        self.convs.append(self.build_conv_model(input_dim, hidden_dim))\n        self.lns = nn.ModuleList()\n        self.lns.append(nn.LayerNorm(hidden_dim))\n        self.lns.append(nn.LayerNorm(hidden_dim))\n        for l in range(2):\n            self.convs.append(self.build_conv_model(hidden_dim, hidden_dim))\n        self.post_mp = nn.Sequential(\n            nn.Linear(hidden_dim, hidden_dim), nn.Dropout(0.25), \n            nn.Linear(hidden_dim, output_dim))\n        self.dropout = 0.25\n        self.num_layers = 2\n\n    def build_conv_model(self, input_dim, hidden_dim):\n        return pyg_nn.GINConv(nn.Sequential(nn.Linear(input_dim, hidden_dim),\n                                  nn.ReLU(), nn.Linear(hidden_dim, hidden_dim)))\n\n    def forward(self, data):\n        x, edge_index, label, batch = data\n        x = torch.tensor((x[1]), dtype=torch.float).cuda()\n        label = torch.tensor((label[1]), dtype=torch.float).cuda()\n        edge_index = edge_index[1].cuda()\n        \n        if len(x) == 0:\n          x = torch.ones(data.num_nodes, 1)\n\n        for i in range(self.num_layers):\n            x = self.convs[i](x, edge_index)\n            emb = x\n            x = F.relu(x)\n            x = F.dropout(x, p=self.dropout, training=self.training)\n            if not i == self.num_layers - 1:\n                x = self.lns[i](x)\n            \n        s = len(data.x)\n        batch_idx = (torch.tensor(np.zeros(s)).long()).cuda()\n        #print (batch_idx)\n        x = pyg_nn.global_mean_pool(x, batch_idx)\n\n        x = self.post_mp(x)\n        \n        #print ('embeddings', emb)\n\n        return (x, emb)\n    \n\n","metadata":{"execution":{"iopub.status.busy":"2022-08-05T07:31:40.955093Z","iopub.execute_input":"2022-08-05T07:31:40.95549Z","iopub.status.idle":"2022-08-05T07:31:40.971353Z","shell.execute_reply.started":"2022-08-05T07:31:40.955429Z","shell.execute_reply":"2022-08-05T07:31:40.970145Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"def get_edges(signal, residuals):\n    \"\"\"\n    Given a datapoint in the AndroidGNSSDataset, returns a list of tuples\n    that indicates the edges between each satellite/signal type in the datapoint.\n    Two nodes in the datapoint are connected if they are within the same constellation\n    and have the same signal type.\n    \"\"\"\n    edges = []\n    res_thr = 5  # threshold for comparing pseudorange residuals instead of constellation type\n    for i in range(len(signal)):\n        for j in range(1,len(signal)):\n            if i == j:\n                continue\n            elif signal[i] == signal[j] or abs(residuals[i] - residuals[j]) <= res_thr:\n                edges.append(torch.Tensor([i, j]))\n                edges.append(torch.Tensor([j, i]))\n    edges_tensor = torch.stack(edges, dim=1) # Creates a tensor of shape (2, 2 * num_edges)\n    # assert(len(edges) == 0 or edges_tensor.shape == (2, num_edges))\n\n    return edges_tensor.long()","metadata":{"execution":{"iopub.status.busy":"2022-08-05T06:23:15.865572Z","iopub.execute_input":"2022-08-05T06:23:15.866408Z","iopub.status.idle":"2022-08-05T06:23:15.873261Z","shell.execute_reply.started":"2022-08-05T06:23:15.866371Z","shell.execute_reply":"2022-08-05T06:23:15.872207Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"**FULLY defined NN******","metadata":{}},{"cell_type":"code","source":"import torch\nimport torchvision\n\nclass CorrectionModel(torch.nn.Module):\n    ## 1000, 1000 x 500, 500 x 300, 300 x 150, 150 x 3\n\n    def __init__(self):\n        super(CorrectionModel, self).__init__()\n        self.linear1 = torch.nn.Linear(3 * 30, 1000)\n        self.activation1 = torch.nn.ReLU()\n#         self.linear2 = torch.nn.Linear(2000, 1500)\n#         self.activation2 = torch.nn.ReLU()\n#         self.linear3 = torch.nn.Linear(1500, 1000)\n#         self.activation3 = torch.nn.ReLU()\n        self.linear4 = torch.nn.Linear(1000, 800)\n        self.activation4 = torch.nn.ReLU()\n        self.linear5 = torch.nn.Linear(800, 500)\n        self.activation5 = torch.nn.ReLU()\n        self.linear6 = torch.nn.Linear(500, 250)\n        self.activation6 = torch.nn.ReLU()\n        self.dropout = torch.nn.Dropout(p = 0.5)\n        self.linear7 = torch.nn.Linear(250, 3)\n\n    def forward(self, x):\n        x = self.linear1(x)\n        x = self.activation1(x)\n#         x = self.linear2(x)\n#         x = self.activation2(x)\n#         x = self.linear3(x)\n#         x = self.activation3(x)\n        x = self.linear4(x)\n        x = self.activation4(x)\n        x = self.linear5(x)\n        x = self.activation5(x)\n        x = self.linear6(x)\n        x = self.activation6(x)\n        x = self.dropout(x)\n        x = self.linear7(x)\n        return x\n    \nmodel = CorrectionModel()\nprint (model)\nlearning_rate = 0.0001\nl = torch.nn.MSELoss()\noptimizer = torch.optim.Adam(model.parameters(), lr = learning_rate, weight_decay = 0.01)\nnum_epochs = 10000\n#losses = []\ndevice = torch.device('cuda') if torch.cuda.is_available() else torch.device('cpu')\nmodel.to(device)\nprint (device)","metadata":{"jupyter":{"source_hidden":true},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"l = labels_all[0]\nf = features_all[0]\n\nprint (l.shape, f.shape)\nX_train = torch.from_numpy(f.astype(np.float32).reshape(f.shape[0],f.shape[-1] * f.shape[-2] ))\nX_train = X_train.cuda()\ny_train = torch.from_numpy(l.astype(np.float32))\ny_train = y_train.cuda()\n\nfrac = 0.75\ndataset =  torch.utils.data.TensorDataset(X_train, y_train)\ntrain_set, val_set = torch.utils.data.random_split(dataset, [int(frac*len(dataset)), len(dataset) - int(frac*len(dataset))])","metadata":{"jupyter":{"source_hidden":true},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"import numpy as np\nimport numpy.matlib\n\ndef gnn_preprocessing(gnss_df, xkf):\n    \n    # initialize arrays/dictionaries\n    data = {}\n    num_sats = 30\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    utcTimeMillis = gnss_df['utcTimeMillis'].unique()\n    nepoch = len(utcTimeMillis)\n    #print (nepoch)\n\n    for i, (t_utc, df) in enumerate(tqdm(gnss_df.groupby('utcTimeMillis'), total=nepoch)):\n        \n        value = {}\n            \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        ## change this based on if using WLS solution or KF solution\n        #xusr =  df_pr[['WlsPositionXEcefMeters','WlsPositionYEcefMeters','WlsPositionZEcefMeters']].to_numpy() \n        \n        punc = df_pr[['RawPseudorangeUncertaintyMeters']].to_numpy()\n        cn0 = df_pr[['Cn0DbHz']].to_numpy()\n        signal = df_pr[['SignalType']].to_numpy()\n        \n        if pr.shape[0] == 0:\n            features[0] = -50000\n        else:\n            xusr = np.matlib.repmat(xkf[i], xsat_pr.shape[0],1)\n            los, expected_ranges = los_vector(xusr, xsat_prr)\n            residuals = (pr - expected_ranges)\n            #print ('residuals', residuals)\n            features = np.concatenate((np.reshape(residuals, [-1, 1]), los), axis=1)\n            edges = get_edges(signal, residuals)\n            \n        value['features'] = features\n        value['edges'] = edges\n            \n            \n        data[str(i)] = value  # shape num sats by num of features which is 3 or 4 usually\n\n\n    return data # should be dictionary with timesteps as keys .. values as key/value pair","metadata":{"execution":{"iopub.status.busy":"2022-08-05T07:47:14.032669Z","iopub.execute_input":"2022-08-05T07:47:14.033064Z","iopub.status.idle":"2022-08-05T07:47:14.049635Z","shell.execute_reply.started":"2022-08-05T07:47:14.03303Z","shell.execute_reply":"2022-08-05T07:47:14.048697Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"gtfname = '../input/smartphone-decimeter-2022/train/2021-12-15-US-MTV-1/GooglePixel5/ground_truth.csv'\nfname = '../input/smartphone-decimeter-2022/train/2021-12-15-US-MTV-1/GooglePixel5/device_gnss.csv'\n\ngnss_df = pd.read_csv(fname)\ngt_df = pd.read_csv(gtfname) \ngt_lla = gt_df[['LatitudeDegrees', 'LongitudeDegrees','AltitudeMeters']].to_numpy()\ngt_ecef = np.array(pm.geodetic2ecef(gt_lla[:, 0], gt_lla[:, 1], gt_lla[:, 2])).T\nutc, x_wls, v_wls, cov_x, cov_v = point_positioning(gnss_df)  # modified\nx_wls, v_wls, cov_x, cov_v = exclude_interpolate_outlier(x_wls, v_wls, cov_x, cov_v, gnss_df, utc)\nphone = 'GooglePixel4XL'\nx_kf, _, _ = Kalman_smoothing(x_wls, v_wls, cov_x, cov_v, phone,1,137,10)","metadata":{"execution":{"iopub.status.busy":"2022-08-05T06:23:24.283397Z","iopub.execute_input":"2022-08-05T06:23:24.284015Z","iopub.status.idle":"2022-08-05T06:24:17.009714Z","shell.execute_reply.started":"2022-08-05T06:23:24.283979Z","shell.execute_reply":"2022-08-05T06:24:17.008485Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"from torch_geometric.data import Dataset, Data\n\nclass GraphDataset(Dataset):\n    \"\"\"\n    Converts a given torch.utils.Dataset to a GraphDataset for use with torch_geometric.loader.data_laoder.\n    Accepts custom functions for the creation of the node feature matrix and the node edges\n    \"\"\"\n    def __init__(\n        self,\n        dataset,\n    ):\n        super().__init__(\"\") # Expects a root variable which we do not use at this level of abstraction\n        self.dataset = dataset\n        self.labels = labels\n        self.batch = 16\n\n    def len(self):\n        return len(self.dataset)\n\n    def get(self, idx):\n        return Data(\n            x= self.dataset[str(idx)]['features'],\n            edge_index= self.dataset[str(idx)]['edges'],\n            y= self.labels[idx],\n            batch = [166])","metadata":{"execution":{"iopub.status.busy":"2022-08-05T06:24:31.945475Z","iopub.execute_input":"2022-08-05T06:24:31.945881Z","iopub.status.idle":"2022-08-05T06:24:37.830764Z","shell.execute_reply.started":"2022-08-05T06:24:31.945849Z","shell.execute_reply":"2022-08-05T06:24:37.829815Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"dataset =  gnn_preprocessing(gnss_df, x_kf) # no.timespochs dictionary with utc time as keys\nlabels = return_labels(gt_ecef, x_kf)  \n\n## Append the right labels to the dataset and modify into a graph structure\nfor i in range(len(labels)):\n    dataset[str(i)]['labels'] = labels[i,:] \n\n","metadata":{"execution":{"iopub.status.busy":"2022-08-05T07:47:22.879717Z","iopub.execute_input":"2022-08-05T07:47:22.880105Z","iopub.status.idle":"2022-08-05T07:47:44.253904Z","shell.execute_reply.started":"2022-08-05T07:47:22.880075Z","shell.execute_reply":"2022-08-05T07:47:44.252185Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"data = GraphDataset(dataset)\nprint (data[0])\n#print (data['1']['edges'])","metadata":{"execution":{"iopub.status.busy":"2022-08-05T07:47:48.124089Z","iopub.execute_input":"2022-08-05T07:47:48.124688Z","iopub.status.idle":"2022-08-05T07:47:48.133782Z","shell.execute_reply.started":"2022-08-05T07:47:48.124638Z","shell.execute_reply":"2022-08-05T07:47:48.132671Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"### BASELINE GRAPH ARCHITECTURE ...\nimport torch\nimport torch.nn.functional as F\nfrom torch_geometric.nn import GCNConv\n\nclass GNN(torch.nn.Module):\n    def __init__(self, input_dim, hidden_dim, output_dim):\n        super(GNN, self).__init__()\n        self.conv1 = GCNConv(input_dim, hidden_dim)\n        self.conv2 = GCNConv(hidden_dim, 100)\n        self.linear1 = torch.nn.Linear(100,output_dim)\n        \n    def forward(self, data):\n        x, edge_index, label, batch = data\n        \n        x = torch.tensor((x[1]), dtype=torch.float).cuda()\n        label = torch.tensor((label[1]), dtype=torch.float).cuda()\n        edge_index = edge_index[1].cuda()\n        \n        x = self.conv1(x, edge_index)\n        x = x.relu()\n        x = F.dropout(x, p=0.5, training=self.training)\n        s = len(data.x)\n        batch_idx = (torch.tensor(np.zeros(s)).long()).cuda()\n        #print (batch_idx)\n        x = self.conv2(x, edge_index)\n        x = pyg_nn.global_mean_pool(x, batch_idx)\n        \n        x = self.linear1(x)\n        \n                \n\n        return x\n","metadata":{"execution":{"iopub.status.busy":"2022-08-05T07:48:13.433571Z","iopub.execute_input":"2022-08-05T07:48:13.434603Z","iopub.status.idle":"2022-08-05T07:48:13.444327Z","shell.execute_reply.started":"2022-08-05T07:48:13.434554Z","shell.execute_reply":"2022-08-05T07:48:13.44335Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"## DEFINE THE MODEL AND START TRAINING \nimport pandas as pd\nfrom matplotlib import pyplot as plt\n\nm = len(data)\nprint (m)\ntrain_data = data[: round(0.9 * m)]\nval_data = data[round(0.9 * m): m]\n\n\ndevice = torch.device('cuda' if torch.cuda.is_available() else 'cpu')\nmodel = GCN(4,32,3).to(device)\noptimizer = torch.optim.Adam(model.parameters(), lr=0.01, weight_decay=5e-4)\ncriterion = torch.nn.MSELoss()\nnum_epochs = 100\ntrain_loss = []\n\nmodel.train()\nfor epoch in tqdm(range(num_epochs)):\n    epoch_loss = 0\n    for i in range(len(train_data)):\n        ds = train_data[i].to(device)\n        y = torch.from_numpy(ds.y)\n        y = y.to(device).to(torch.float32)\n        optimizer.zero_grad()\n        (pred_y,emb)= model(ds)\n        #print (pred_y,y)\n        loss = criterion(pred_y, y)\n        epoch_loss += loss.item()\n        #print (loss.cpu().detach().numpy())\n        loss.backward()\n        optimizer.step()\n    epoch_loss /= len(train_data)\n    train_loss.append(epoch_loss)\n\nplt.figure()\nplt.plot(train_loss)\nplt.xlabel('No. of Epochs')\nplt.ylabel('Training loss in Meters')\nplt.show()\n\n\n    \n    \n# #model.eval()\n","metadata":{"execution":{"iopub.status.busy":"2022-08-05T08:04:28.230465Z","iopub.execute_input":"2022-08-05T08:04:28.230981Z","iopub.status.idle":"2022-08-05T08:13:29.00009Z","shell.execute_reply.started":"2022-08-05T08:04:28.230942Z","shell.execute_reply":"2022-08-05T08:13:28.999271Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"## VALIDATION LOOP\nval_losses = []\nfor epoch in tqdm(range(num_epochs)):       \n        model.eval()\n        val_loss = 0\n        with torch.no_grad():\n            for k in range(len(val_data)):\n                ds = val_data[k].to(device)\n                y = torch.from_numpy(ds.y)\n                y = y.to(device).to(torch.float32)\n                (pred_y,emb)= model(ds)\n                loss = criterion(pred_y, y)\n                val_loss += loss.item()\n            val_loss /= len(val_data)\n            #print (val_loss)\n            val_losses.append(val_loss)\nplt.figure()\nplt.plot(val_losses)\nplt.xlabel('No. of Epochs')\nplt.ylabel('Validatioin loss in Meters')\nplt.show()\n","metadata":{"execution":{"iopub.status.busy":"2022-08-05T08:26:03.632854Z","iopub.execute_input":"2022-08-05T08:26:03.633237Z","iopub.status.idle":"2022-08-05T08:26:26.91714Z","shell.execute_reply.started":"2022-08-05T08:26:03.633205Z","shell.execute_reply":"2022-08-05T08:26:26.916301Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"#torch.save(model.state_dict(), 'trained_model_dense.pth')\nmodel.load_state_dict(torch.load('../input/newmodels/trained_model_residuals.pth'))\nmodel.eval()\nprint (model.state_dict())","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"train_size = round (0.75 * labels_all.shape[0])\nprint (train_size)\nfor i in range(train_size):\n    lab = labels_all[i]\n    print (lab.shape)\n\n    feat = features_all[i]\n    feat = feat[:,:,:3]\n    print ('shape',feat.shape)\n    X_train = torch.from_numpy(feat.astype(np.float32).reshape(feat.shape[0],feat.shape[-1] * feat.shape[-2] ))\n    X_train = X_train.cuda()\n    y_train = torch.from_numpy(lab.astype(np.float32))\n    y_train = y_train.cuda()\n    #print (X_train.shape, y_train.shape)\n    loss_all = []\n\n    print ('Entering training loop')\n    for i in tqdm(range(num_epochs)):\n        y_pred = model(X_train.requires_grad_())\n        #print (y_pred)\n        loss = l(y_pred, y_train)\n        loss_all.append(loss_all)\n        loss.backward()\n        optimizer.step()\n        optimizer.zero_grad()\n        if divmod(i, 1000)[1] == 0:\n          print('epoch {}, loss {}'.format(i, loss.item()))","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"## Validation loop\nlabels_all = np.load('../input/dataset/labels (2).npy', allow_pickle = True)\nfeatures_all = np.load('../input/dataset/features (2).npy', allow_pickle = True)\n#print (features_all.shape, labels_all.shape)\n\nfeat_val = features_all[60:]\nlab_val = labels_all[60:]\n\n\n#print (feat_val.shape, lab_val.shape)\nerror_all = []\n\nfor i in range(len(lab_val)):\n    lval = lab_val[i]\n    feat = feat_val[i]\n    fval = feat[:,:,:3]\n    #print (lval.shape, fval.shape)\n    X_val = torch.from_numpy(fval.astype(np.float32).reshape(fval.shape[0],fval.shape[-1] * fval.shape[-2] ))\n    X_val = X_val.cuda()\n    y_t = model(X_val.requires_grad_())\n    y_val = y_t.cpu().detach().numpy()\n    error = np.sqrt((np.square(y_val - lval))).mean(axis= 1)\n    print ('error',np.mean(error))\n#     error_all.append(np.mean(error))\n#     print (error)\n    \n","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"import numpy as np\nimport numpy.matlib\n\n\ndef preprocessing_test(gnss_df, xkf):\n    num_sats = 30\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    features_all = np.zeros((nepoch,num_sats,6)) # first index is time, second index is max. no of sats and third index is no. of features\n    wls_solution =  np.zeros((nepoch,3))\n    #print (features_all.shape)\n    # Loop for epochs\n    for i, (t_utc, df) in enumerate(tqdm(gnss_df.groupby('utcTimeMillis'), total=nepoch)):\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        ## change this based on if using WLS solution or KF solution\n        #xusr =  df_pr[['WlsPositionXEcefMeters','WlsPositionYEcefMeters','WlsPositionZEcefMeters']].to_numpy() \n        \n        punc = df_pr[['RawPseudorangeUncertaintyMeters']].to_numpy()\n        cn0 = df_pr[['Cn0DbHz']].to_numpy()\n    \n        xusr = np.matlib.repmat(xkf[i], xsat_pr.shape[0],1)\n        wls_solution[i,:] = xusr[0,:]\n\n        los, expected_ranges = los_vector(xusr, xsat_prr)\n        residuals = (pr - expected_ranges)\n        features = np.concatenate((np.reshape(residuals, [-1, 1]), los, punc, cn0), axis=1)\n        m = features.shape[0]  ## get the number of entries in this column ..\n        if m < num_sats:\n            features_all[i,0:m,:] = features\n        else:\n            features_all[i,0:num_sats,:] = features[:num_sats,:]\n\n\n    return features_all, wls_solution","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"### TEST DATA\n\npath = '/kaggle/input/smartphone-decimeter-2022'\nsample_df = pd.read_csv('../input/submissions/sample_new_submission.csv')\ntest_dfs = []\n\n# Loop for each trip\nfor i, dirname in enumerate(tqdm(sorted(gl.glob(f'{path}/test/*/*/')))):\n    #print ('i',i)\n    drive, phone = dirname.split('/')[-3:-1]\n    tripID = f'{drive}/{phone}'\n    print(tripID)\n\n    gnss_df = pd.read_csv(f'{dirname}/device_gnss.csv')\n    utc, x_wls, v_wls, cov_x, cov_v = point_positioning(gnss_df)\n    x_wls, v_wls, cov_x, cov_v = exclude_interpolate_outlier(x_wls, v_wls, cov_x, cov_v, gnss_df, utc)\n    x_kf, _, _ = Kalman_smoothing(x_wls, v_wls, cov_x, cov_v,phone,1,137,10)   \n    features_dataset, wls_solution = preprocessing(gnss_df, x_kf)\n    \n    if np.any(features_dataset[:,0,0] == -500000):\n        llh_kf = np.array(pm.ecef2geodetic(x_kf[:, 0], x_kf[:, 1], x_kf[:, 2])).T\n        print ('skipping')\n        continue\n    else:\n        f = features_dataset[:,:,0:3]\n        X_test = torch.from_numpy(f.astype(np.float32).reshape(f.shape[0],f.shape[-1] * f.shape[-2]))\n        X_test = X_test.cuda()\n\n        y_t = model(X_test.requires_grad_())\n        y_test = y_t.cpu().detach().numpy()\n\n        x_final = x_kf + y_test\n        llh_kf = np.array(pm.ecef2geodetic(x_final[:, 0], x_final[:, 1], x_final[:, 2])).T\n\n    UnixTimeMillis = sample_df[sample_df['tripId'] == tripID]['UnixTimeMillis'].to_numpy()\n    lat = InterpolatedUnivariateSpline(utc, llh_kf[:,0], ext=3)(UnixTimeMillis)\n    lng = InterpolatedUnivariateSpline(utc, llh_kf[:,1], ext=3)(UnixTimeMillis)\n    print (len(UnixTimeMillis), len(lat), len(lng))\n    trip_df = pd.DataFrame({\n        'tripId' : tripID,\n        'UnixTimeMillis': UnixTimeMillis,\n        'LatitudeDegrees': lat,\n        'LongitudeDegrees': lng\n        })\n\n    test_dfs.append(trip_df)\n\n# Write submission.csv\ntest_df = pd.concat(test_dfs)\ntest_df.to_csv('submission_new_fc_network_residuals_only.csv', index=False)","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"test_df = pd.concat(test_dfs)\ntest_df.to_csv('submission_new.csv', index=False)","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"print(model)\nfeatures_dataset, wls_solution = preprocessing_test(gnss_df, x_kf)\n    \nX_test = torch.from_numpy(features_dataset.astype(np.float32).reshape(features_dataset.shape[0],features_dataset.shape[-1] * features_dataset.shape[-2] ))\nX_test = X_test.cuda()\n\ny_test = model(X_test.requires_grad_())\n\n\n    ","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"# Test data and Submission\nThis part is based on @saitodevel01 's [code](https://www.kaggle.com/code/saitodevel01/gsdc2-baseline-submission). Thank you!","metadata":{}},{"cell_type":"code","source":"class CorrectionModel(torch.nn.Module):\n\n    def __init__(self):\n        super(CorrectionModel, self).__init__()\n        self.linear1 = torch.nn.Linear(input_size, 2000)\n        self.activation1 = torch.nn.ReLU()\n        self.linear2 = torch.nn.Linear(2000, 1000)\n        self.activation2 = torch.nn.ReLU()\n        self.linear3 = torch.nn.Linear(1000, 500)\n        self.activation3 = torch.nn.ReLU()\n        self.linear4 = torch.nn.Linear(500, 250)\n        self.activation4 = torch.nn.ReLU()\n        self.linear5 = torch.nn.Linear(250, output_size)\n        self.softmax = torch.nn.Softmax()\n\n    def forward(self, x):\n        x = self.linear1(x)\n        x = self.activation1(x)\n        x = self.linear2(x)\n        x = self.activation2(x)\n        x = self.linear3(x)\n        x = self.activation3(x)\n        x = self.linear4(x)\n        x = self.activation4(x)\n        x = self.linear5(x)\n        x = self.softmax(x)\n        return x\n    \n","metadata":{"_kg_hide-input":true,"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"model = CorrectionModel()\nlearning_rate = 0.001\nl = torch.nn.MSELoss()\noptimizer = torch.optim.SGD(model.parameters(), lr = learning_rate)\nnum_epochs = 10000\nlosses = []\n\nfor i in tqdm(range(num_epochs)):\n    #forward feed\n    y_pred = model(X_train.requires_grad_())\n\n    #calculate the loss\n    loss = l(y_pred, y_train)\n    \n    #backward propagation: calculate gradients\n    loss.backward()\n\n    #update the weights\n    optimizer.step()\n\n    #clear out the gradients from the last step loss.backward()\n    optimizer.zero_grad()\n    \n    losses.append(loss.item())\n    \n    if divmod(i, 100)[1] == 0:\n        print('epoch {}, loss {}'.format(i, loss.item()))","metadata":{"trusted":true},"execution_count":null,"outputs":[]}]}