{"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":"## ⚠️This notebook is FORKED from [this](https://www.kaggle.com/code/hyperc/gsdc-reproducing-baseline-wls-on-one-measurement).","metadata":{}},{"cell_type":"code","source":"import glob\nfrom dataclasses import dataclass\nimport numpy as np\nimport pandas as pd\nfrom tqdm.notebook import tqdm\nfrom scipy.interpolate import InterpolatedUnivariateSpline\nimport scipy.optimize as opt\npd.set_option('display.max_columns',100)\nINPUT_PATH = '../input/smartphone-decimeter-2022'\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\n\nHAVERSINE_RADIUS = 6_371_000\n","metadata":{"execution":{"iopub.execute_input":"2022-05-26T12:48:53.794813Z","iopub.status.busy":"2022-05-26T12:48:53.794256Z","iopub.status.idle":"2022-05-26T12:48:54.594642Z","shell.execute_reply":"2022-05-26T12:48:54.593643Z"},"papermill":{"duration":0.817427,"end_time":"2022-05-26T12:48:54.597183","exception":false,"start_time":"2022-05-26T12:48:53.779756","status":"completed"},"tags":[]},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"@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\ndef ECEF_to_BLH(ecef):\n    a = WGS84_SEMI_MAJOR_AXIS\n    b = WGS84_SEMI_MINOR_AXIS\n    e2  = WGS84_SQUARED_FIRST_ECCENTRICITY\n    e2_ = 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\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 * HAVERSINE_RADIUS * np.arcsin(np.sqrt(a))\n    return dist\n\ndef pandas_haversine_distance(df1, df2):\n    blh1 = BLH(\n        lat=np.deg2rad(df1['LatitudeDegrees'].to_numpy()),\n        lng=np.deg2rad(df1['LongitudeDegrees'].to_numpy()),\n        hgt=0,\n    )\n    blh2 = BLH(\n        lat=np.deg2rad(df2['LatitudeDegrees'].to_numpy()),\n        lng=np.deg2rad(df2['LongitudeDegrees'].to_numpy()),\n        hgt=0,\n    )\n    return haversine_distance(blh1, blh2)","metadata":{"execution":{"iopub.execute_input":"2022-05-26T12:48:54.625501Z","iopub.status.busy":"2022-05-26T12:48:54.625226Z","iopub.status.idle":"2022-05-26T12:48:54.639303Z","shell.execute_reply":"2022-05-26T12:48:54.638478Z"},"papermill":{"duration":0.030759,"end_time":"2022-05-26T12:48:54.641590","exception":false,"start_time":"2022-05-26T12:48:54.610831","status":"completed"},"tags":[]},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"# Apply WLS on one collection and one measurement","metadata":{"papermill":{"duration":0.012556,"end_time":"2022-05-26T12:48:54.667080","exception":false,"start_time":"2022-05-26T12:48:54.654524","status":"completed"},"tags":[]}},{"cell_type":"code","source":"trip_id = '2020-05-15-US-MTV-1/GooglePixel4XL'\ndf = pd.read_csv(f'../input/smartphone-decimeter-2022/train/{trip_id}/device_gnss.csv')","metadata":{"execution":{"iopub.execute_input":"2022-05-26T12:48:54.693981Z","iopub.status.busy":"2022-05-26T12:48:54.693700Z","iopub.status.idle":"2022-05-26T12:48:56.271263Z","shell.execute_reply":"2022-05-26T12:48:56.270412Z"},"papermill":{"duration":1.593898,"end_time":"2022-05-26T12:48:56.273718","exception":false,"start_time":"2022-05-26T12:48:54.679820","status":"completed"},"tags":[]},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"measurement_epoch_time = 1589573679445\ndf = df[df.utcTimeMillis == measurement_epoch_time]\ndf","metadata":{"execution":{"iopub.execute_input":"2022-05-26T12:48:56.301640Z","iopub.status.busy":"2022-05-26T12:48:56.301085Z","iopub.status.idle":"2022-05-26T12:48:56.379289Z","shell.execute_reply":"2022-05-26T12:48:56.378160Z"},"papermill":{"duration":0.096396,"end_time":"2022-05-26T12:48:56.383250","exception":false,"start_time":"2022-05-26T12:48:56.286854","status":"completed"},"tags":[]},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"# Corrected pseudorange according to data instructions\ndf['correctedPrM'] = df.apply(\n    lambda r: r.RawPseudorangeMeters + r.SvClockBiasMeters - r.IsrbMeters - r.IonosphericDelayMeters - r.TroposphericDelayMeters,\n    axis=1\n)\n\n# Time it took for signal to travel\nlight_speed = 299_792_458\ndf['transmissionTimeSeconds'] = df['correctedPrM'] / light_speed","metadata":{"execution":{"iopub.execute_input":"2022-05-26T12:48:56.416619Z","iopub.status.busy":"2022-05-26T12:48:56.416299Z","iopub.status.idle":"2022-05-26T12:48:56.427911Z","shell.execute_reply":"2022-05-26T12:48:56.427220Z"},"papermill":{"duration":0.030158,"end_time":"2022-05-26T12:48:56.429672","exception":false,"start_time":"2022-05-26T12:48:56.399514","status":"completed"},"tags":[]},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"# Compute true sat positions at arrival time\nomega_e = 7.2921151467e-5\ndf['xSatPosMRotated'] = \\\n    np.cos(omega_e * df['transmissionTimeSeconds']) * df['SvPositionXEcefMeters'] \\\n    + np.sin(omega_e * df['transmissionTimeSeconds']) * df['SvPositionYEcefMeters']\n    \ndf['ySatPosMRotated'] = \\\n    - np.sin(omega_e * df['transmissionTimeSeconds']) * df['SvPositionXEcefMeters'] \\\n    + np.cos(omega_e * df['transmissionTimeSeconds']) * df['SvPositionYEcefMeters']\n    \ndf['zSatPosMRotated'] = df['SvPositionZEcefMeters']","metadata":{"execution":{"iopub.execute_input":"2022-05-26T12:48:56.462242Z","iopub.status.busy":"2022-05-26T12:48:56.461978Z","iopub.status.idle":"2022-05-26T12:48:56.476173Z","shell.execute_reply":"2022-05-26T12:48:56.475283Z"},"papermill":{"duration":0.032952,"end_time":"2022-05-26T12:48:56.478228","exception":false,"start_time":"2022-05-26T12:48:56.445276","status":"completed"},"tags":[]},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"# Uncertainty weight for the WLS method\ndf['uncertaintyWeight'] = 1 / df['RawPseudorangeUncertaintyMeters']","metadata":{"execution":{"iopub.execute_input":"2022-05-26T12:48:56.510969Z","iopub.status.busy":"2022-05-26T12:48:56.510687Z","iopub.status.idle":"2022-05-26T12:48:56.516232Z","shell.execute_reply":"2022-05-26T12:48:56.515358Z"},"papermill":{"duration":0.024396,"end_time":"2022-05-26T12:48:56.518185","exception":false,"start_time":"2022-05-26T12:48:56.493789","status":"completed"},"tags":[]},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"# Set up least squares methods\ndef distance(sat_pos, x):\n    sat_pos_diff = sat_pos.copy(deep=True)\n    \n    sat_pos_diff['xSatPosMRotated'] = sat_pos_diff['xSatPosMRotated'] - x[0]\n    sat_pos_diff['ySatPosMRotated'] = sat_pos_diff['ySatPosMRotated'] - x[1]\n    sat_pos_diff['zSatPosMRotated'] = sat_pos_diff['zSatPosMRotated'] - x[2]\n\n    sat_pos_diff['d'] = sat_pos_diff.apply(\n        lambda r: r.uncertaintyWeight * \n            (np.sqrt((r.xSatPosMRotated**2 + r.ySatPosMRotated**2 + r.zSatPosMRotated**2)) + x[3] - r.correctedPrM),\n        axis=1\n    )\n\n    return sat_pos_diff['d']\n\ndef distance_fixed_satpos(x):\n    return distance(df[['xSatPosMRotated', 'ySatPosMRotated', 'zSatPosMRotated', 'correctedPrM', 'uncertaintyWeight']], x)","metadata":{"execution":{"iopub.execute_input":"2022-05-26T12:48:56.551253Z","iopub.status.busy":"2022-05-26T12:48:56.550992Z","iopub.status.idle":"2022-05-26T12:48:56.558047Z","shell.execute_reply":"2022-05-26T12:48:56.557198Z"},"papermill":{"duration":0.025678,"end_time":"2022-05-26T12:48:56.559960","exception":false,"start_time":"2022-05-26T12:48:56.534282","status":"completed"},"tags":[]},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"# Start point for the optimiser\nx0= [0,0,0,0]\n\nopt_res = opt.least_squares(distance_fixed_satpos, x0)\n\n# Optimiser yields a position in the ECEF coordinates\nopt_res_pos = opt_res.x","metadata":{"execution":{"iopub.execute_input":"2022-05-26T12:48:56.593971Z","iopub.status.busy":"2022-05-26T12:48:56.593510Z","iopub.status.idle":"2022-05-26T12:48:57.055644Z","shell.execute_reply":"2022-05-26T12:48:57.054761Z"},"papermill":{"duration":0.480878,"end_time":"2022-05-26T12:48:57.057964","exception":false,"start_time":"2022-05-26T12:48:56.577086","status":"completed"},"tags":[]},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"wls_estimated_pos = ECEF_to_BLH(ECEF.from_numpy(opt_res_pos[:3]))","metadata":{"execution":{"iopub.execute_input":"2022-05-26T12:48:57.090886Z","iopub.status.busy":"2022-05-26T12:48:57.090312Z","iopub.status.idle":"2022-05-26T12:48:57.095829Z","shell.execute_reply":"2022-05-26T12:48:57.095239Z"},"papermill":{"duration":0.024413,"end_time":"2022-05-26T12:48:57.097829","exception":false,"start_time":"2022-05-26T12:48:57.073416","status":"completed"},"tags":[]},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"baseline = df[['WlsPositionXEcefMeters','WlsPositionYEcefMeters','WlsPositionZEcefMeters']].values[0]\nbaseline = ECEF_to_BLH(ECEF.from_numpy(baseline))","metadata":{"execution":{"iopub.execute_input":"2022-05-26T12:48:57.130719Z","iopub.status.busy":"2022-05-26T12:48:57.130170Z","iopub.status.idle":"2022-05-26T12:48:57.135483Z","shell.execute_reply":"2022-05-26T12:48:57.134912Z"},"papermill":{"duration":0.024124,"end_time":"2022-05-26T12:48:57.137365","exception":false,"start_time":"2022-05-26T12:48:57.113241","status":"completed"},"tags":[]},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"gt_df = pd.read_csv(f'../input/smartphone-decimeter-2022/train/{trip_id}/ground_truth.csv')\ngt_df = gt_df[gt_df.UnixTimeMillis==measurement_epoch_time]\ngt = gt_df[['LatitudeDegrees','LongitudeDegrees']].values[0]\ngt = BLH(*np.deg2rad(gt),None)","metadata":{"execution":{"iopub.execute_input":"2022-05-26T12:48:57.169784Z","iopub.status.busy":"2022-05-26T12:48:57.169251Z","iopub.status.idle":"2022-05-26T12:48:57.186531Z","shell.execute_reply":"2022-05-26T12:48:57.185878Z"},"papermill":{"duration":0.036196,"end_time":"2022-05-26T12:48:57.188686","exception":false,"start_time":"2022-05-26T12:48:57.152490","status":"completed"},"tags":[]},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"print(\"Baseline distance with groundtruth position (m)\")\nhaversine_distance(baseline,gt)","metadata":{"execution":{"iopub.execute_input":"2022-05-26T12:48:57.222934Z","iopub.status.busy":"2022-05-26T12:48:57.222356Z","iopub.status.idle":"2022-05-26T12:48:57.229677Z","shell.execute_reply":"2022-05-26T12:48:57.228587Z"},"papermill":{"duration":0.027932,"end_time":"2022-05-26T12:48:57.232016","exception":false,"start_time":"2022-05-26T12:48:57.204084","status":"completed"},"tags":[]},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"print(\"Our estimated position (with WLS) distance with groundtruth position (m)\")\nhaversine_distance(wls_estimated_pos,gt)","metadata":{"execution":{"iopub.execute_input":"2022-05-26T12:48:57.266708Z","iopub.status.busy":"2022-05-26T12:48:57.265773Z","iopub.status.idle":"2022-05-26T12:48:57.274213Z","shell.execute_reply":"2022-05-26T12:48:57.273448Z"},"papermill":{"duration":0.027989,"end_time":"2022-05-26T12:48:57.276637","exception":false,"start_time":"2022-05-26T12:48:57.248648","status":"completed"},"tags":[]},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"","metadata":{"papermill":{"duration":0.016484,"end_time":"2022-05-26T12:48:57.310546","exception":false,"start_time":"2022-05-26T12:48:57.294062","status":"completed"},"tags":[]},"execution_count":null,"outputs":[]}]}