{"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":"## Introduction\n\nThis notebook is for \"Google Smartphone Decimeter Challenge 2022\" where we need to estimate the precise location (longitude, latitude) of a mobile phone using Global Navigation Satellite System (GNSS) signal.\n\nHowever, because the organizer gave us a baseline for this challenge, we might firsly build a baseline by simply smoothering the baseline.\n\nIn this notebook you'll find:\n\n- What is Kalman Filter\n- How to tune its parameters with Optuna\n\nCredit to this discussion https://www.kaggle.com/competitions/smartphone-decimeter-2022/discussion/323548 from @Ravi Shad that I found many interesting resources\n- baseline https://www.kaggle.com/competitions/smartphone-decimeter-2022/discussion/323548 from @saitodevel01\n- Last year kalman filter approach https://www.kaggle.com/code/tqa236/kalman-filter-hyperparameter-search-with-bo by @Trinh Quoc Anh","metadata":{}},{"cell_type":"markdown","source":"### What is Kalman filter\n\n- Actually, I don't really like the term \"filter\" in Kalman Filter, it makes me think about it simply smoothing the output. However, Kalman filter is more powerful than that, it has the learning process behind. It will readapt the parameters on the go by predicting and comparing with the real measurements with a goal is minimizing the uncertainty as much as possible (or covariance)\n\n- There are 3 steps in Kalman filter:\n    1. Predicting the output with the current parameters \n    2. Comparing with the real mesurement from sensors\n    3. Update the parameters to minimize the uncertainty\n    \n![Kalman Filter steps](https://upload.wikimedia.org/wikipedia/commons/thumb/a/a5/Basic_concept_of_Kalman_filtering.svg/1200px-Basic_concept_of_Kalman_filtering.svg.png ) \n   \n[Fun fact]: In the 1960s, the Kalman filter was applied to navigation for the Apollo Project, which required estimates of the trajectories of manned spacecraft going to the Moon and back . Source: https://ieeexplore.ieee.org/document/5466132\n\nDisadvantage: However, Kalman filter have some unavoidable weakness:\n- We have to correctly modelling the model (which is most of the time not obvious)\n- The uncertainty is modeled by Gaussian noise, which is not always the case, and even if we have that, getting the value is not obvious neither ( It's why I use Optuna for this purpose )\n","metadata":{}},{"cell_type":"markdown","source":"## Code","metadata":{}},{"cell_type":"code","source":"# install kalmal filter library\n!pip install simdkalman ","metadata":{"execution":{"iopub.status.busy":"2022-05-12T08:57:39.796906Z","iopub.execute_input":"2022-05-12T08:57:39.797547Z","iopub.status.idle":"2022-05-12T08:57:52.588290Z","shell.execute_reply.started":"2022-05-12T08:57:39.797489Z","shell.execute_reply":"2022-05-12T08:57:52.587496Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"from tqdm.notebook import tqdm\nfrom dataclasses import dataclass\nfrom scipy.interpolate import InterpolatedUnivariateSpline\nimport glob\nfrom joblib import Parallel, delayed\nimport random\nimport simdkalman\nimport optuna\nfrom functools import partial\nimport numpy as np\nimport pandas as pd\npd.set_option('display.max_columns', 50)","metadata":{"_uuid":"8f2839f25d086af736a60e9eeb907d3b93b6e0e5","_cell_guid":"b1076dfc-b9ad-4769-8c92-a6c4dae69d19","execution":{"iopub.status.busy":"2022-05-12T08:59:55.506679Z","iopub.execute_input":"2022-05-12T08:59:55.507026Z","iopub.status.idle":"2022-05-12T08:59:55.515026Z","shell.execute_reply.started":"2022-05-12T08:59:55.506988Z","shell.execute_reply":"2022-05-12T08:59:55.514078Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"Many snippets below I copy from 2 notebooks you can find in the introduction","metadata":{}},{"cell_type":"code","source":"WGS84_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","metadata":{"execution":{"iopub.status.busy":"2022-05-12T08:59:56.035552Z","iopub.execute_input":"2022-05-12T08:59:56.035903Z","iopub.status.idle":"2022-05-12T08:59:56.040331Z","shell.execute_reply.started":"2022-05-12T08:59:56.035865Z","shell.execute_reply":"2022-05-12T08:59:56.039506Z"},"trusted":true},"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","metadata":{"execution":{"iopub.status.busy":"2022-05-12T08:59:56.355117Z","iopub.execute_input":"2022-05-12T08:59:56.356028Z","iopub.status.idle":"2022-05-12T08:59:56.364644Z","shell.execute_reply.started":"2022-05-12T08:59:56.355968Z","shell.execute_reply":"2022-05-12T08:59:56.363847Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"def 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)","metadata":{"execution":{"iopub.status.busy":"2022-05-12T09:00:03.936246Z","iopub.execute_input":"2022-05-12T09:00:03.936552Z","iopub.status.idle":"2022-05-12T09:00:03.945199Z","shell.execute_reply.started":"2022-05-12T09:00:03.936524Z","shell.execute_reply":"2022-05-12T09:00:03.944007Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"def 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.status.busy":"2022-05-12T09:00:32.106746Z","iopub.execute_input":"2022-05-12T09:00:32.107118Z","iopub.status.idle":"2022-05-12T09:00:32.116748Z","shell.execute_reply.started":"2022-05-12T09:00:32.107079Z","shell.execute_reply":"2022-05-12T09:00:32.115866Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"def ecef_to_lat_lng(gnss_df, UnixTimeMillis):\n    ecef_columns = ['WlsPositionXEcefMeters', 'WlsPositionYEcefMeters', 'WlsPositionZEcefMeters']\n    columns = ['utcTimeMillis'] + ecef_columns\n    ecef_df = (gnss_df.drop_duplicates(subset='utcTimeMillis')[columns]\n               .dropna().reset_index(drop=True))\n    ecef = ECEF.from_numpy(ecef_df[ecef_columns].to_numpy())\n    blh  = ECEF_to_BLH(ecef)\n\n    TIME = ecef_df['utcTimeMillis'].to_numpy()\n    lat = InterpolatedUnivariateSpline(TIME, blh.lat, ext=3)(UnixTimeMillis)\n    lng = InterpolatedUnivariateSpline(TIME, blh.lng, ext=3)(UnixTimeMillis)\n    return pd.DataFrame({\n        'UnixTimeMillis'   : UnixTimeMillis,\n        'LatitudeDegrees'  : np.degrees(lat),\n        'LongitudeDegrees' : np.degrees(lng),\n    })","metadata":{"execution":{"iopub.status.busy":"2022-05-12T09:00:35.858290Z","iopub.execute_input":"2022-05-12T09:00:35.858708Z","iopub.status.idle":"2022-05-12T09:00:35.867359Z","shell.execute_reply.started":"2022-05-12T09:00:35.858663Z","shell.execute_reply":"2022-05-12T09:00:35.866216Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"The state transition in our problem is quite obvious with a highschool physics, we know that. D_new = D_old + V * T + 1/2 * A * T^2 \n\nwhere as: D(Distance), V(Speed), A(Acceleration)\n\nThe order of states vector (6x1) are: [Latitude, Longitue, V_lat, V_lon, A_lat, A_lon] \n\nWe only tune the observation noise and process noise and the sampling time (T)","metadata":{}},{"cell_type":"code","source":"def make_kalman_filter(T, process_cov_mat, obs_cov_mat):\n    \n    state_transition =  np.array([[1, 0, T, 0, 0.5 * T ** 2, 0], \n                             [0, 1, 0, T, 0, 0.5 * T ** 2], \n                             [0, 0, 1, 0, T, 0],\n                             [0, 0, 0, 1, 0, T], \n                             [0, 0, 0, 0, 1, 0], \n                             [0, 0, 0, 0, 0, 1]])\n    \n    observation_model = np.array([[1, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0]])\n    \n    observation_noise = obs_cov_mat\n    \n    process_noise = process_cov_mat\n\n    kf = simdkalman.KalmanFilter(\n            state_transition = state_transition,\n            process_noise = process_noise,\n            observation_model = observation_model,\n            observation_noise = observation_noise)\n    return kf","metadata":{"execution":{"iopub.status.busy":"2022-05-12T09:09:06.397060Z","iopub.execute_input":"2022-05-12T09:09:06.397413Z","iopub.status.idle":"2022-05-12T09:09:06.406069Z","shell.execute_reply.started":"2022-05-12T09:09:06.397376Z","shell.execute_reply":"2022-05-12T09:09:06.405197Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"def calc_score(pred_df, gt_df):\n    d = pandas_haversine_distance(pred_df, gt_df)\n    score = np.mean([np.quantile(d, 0.50), np.quantile(d, 0.95)])    \n    return score","metadata":{"execution":{"iopub.status.busy":"2022-05-12T09:09:22.435770Z","iopub.execute_input":"2022-05-12T09:09:22.436143Z","iopub.status.idle":"2022-05-12T09:09:22.440944Z","shell.execute_reply.started":"2022-05-12T09:09:22.436085Z","shell.execute_reply":"2022-05-12T09:09:22.440130Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"INPUT_PATH = '../input/smartphone-decimeter-2022/'","metadata":{"execution":{"iopub.status.busy":"2022-05-12T09:10:32.195236Z","iopub.execute_input":"2022-05-12T09:10:32.195852Z","iopub.status.idle":"2022-05-12T09:10:32.199692Z","shell.execute_reply.started":"2022-05-12T09:10:32.195796Z","shell.execute_reply":"2022-05-12T09:10:32.198886Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"def apply_kf_smoothing(df, _kf):\n    df_filter = df.copy()\n    data = df_filter[['LatitudeDegrees','LongitudeDegrees']].to_numpy()\n    data = data.reshape(1, len(data), 2)\n    smoothed = _kf.smooth(data)\n    df_filter['LatitudeDegrees'] = smoothed.states.mean[0, :, 0]\n    df_filter['LongitudeDegrees'] = smoothed.states.mean[0, :, 1]\n    return df_filter","metadata":{"execution":{"iopub.status.busy":"2022-05-12T09:11:10.265613Z","iopub.execute_input":"2022-05-12T09:11:10.265943Z","iopub.status.idle":"2022-05-12T09:11:10.272625Z","shell.execute_reply.started":"2022-05-12T09:11:10.265909Z","shell.execute_reply":"2022-05-12T09:11:10.271681Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"dirnames = sorted(glob.glob(f'{INPUT_PATH}/train/*/*'))","metadata":{"execution":{"iopub.status.busy":"2022-05-12T09:11:27.414937Z","iopub.execute_input":"2022-05-12T09:11:27.415735Z","iopub.status.idle":"2022-05-12T09:11:27.459699Z","shell.execute_reply.started":"2022-05-12T09:11:27.415689Z","shell.execute_reply":"2022-05-12T09:11:27.458728Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"I will try to tune all the diagonal values and one value for off-diagonal. I think lattitude and logitude is diagonal so normally they are not correlated. Question: Do I need to tune Speed and Acceleration, or it can just derived from the position ?","metadata":{}},{"cell_type":"markdown","source":"## Testing the process for 1 tripId","metadata":{}},{"cell_type":"code","source":"process_cov_00 = 10**-6\nprocess_cov_11 = 10**-6\nprocess_cov_22 = 10**-6\nprocess_cov_33 = 10**-6\nprocess_cov_44 = 10**-6\nprocess_cov_55 = 10**-6\nprocess_cov_66 = 10**-6\nprocess_cov_off_diag = 10**-9","metadata":{"execution":{"iopub.status.busy":"2022-05-12T09:14:43.873206Z","iopub.execute_input":"2022-05-12T09:14:43.873553Z","iopub.status.idle":"2022-05-12T09:14:43.879488Z","shell.execute_reply.started":"2022-05-12T09:14:43.873514Z","shell.execute_reply":"2022-05-12T09:14:43.878562Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"process_diag = [process_cov_00, process_cov_11, process_cov_22, process_cov_33, process_cov_44, process_cov_55]","metadata":{"execution":{"iopub.status.busy":"2022-05-12T09:14:46.714887Z","iopub.execute_input":"2022-05-12T09:14:46.715338Z","iopub.status.idle":"2022-05-12T09:14:46.719598Z","shell.execute_reply.started":"2022-05-12T09:14:46.715306Z","shell.execute_reply":"2022-05-12T09:14:46.718717Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"def make_cov_mat(process_diag, process_cov_off_diag):\n    rank = len(process_diag)\n    process_cov_mat = np.zeros((rank,rank))\n    np.fill_diagonal(process_cov_mat, process_diag)\n    process_cov_mat = process_cov_mat + process_cov_off_diag\n    return process_cov_mat","metadata":{"execution":{"iopub.status.busy":"2022-05-12T09:14:51.395248Z","iopub.execute_input":"2022-05-12T09:14:51.395548Z","iopub.status.idle":"2022-05-12T09:14:51.400351Z","shell.execute_reply.started":"2022-05-12T09:14:51.395518Z","shell.execute_reply":"2022-05-12T09:14:51.399682Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"process_cov_mat = make_cov_mat(process_diag, process_cov_off_diag)","metadata":{"execution":{"iopub.status.busy":"2022-05-12T09:14:59.625373Z","iopub.execute_input":"2022-05-12T09:14:59.625687Z","iopub.status.idle":"2022-05-12T09:14:59.629949Z","shell.execute_reply.started":"2022-05-12T09:14:59.625656Z","shell.execute_reply":"2022-05-12T09:14:59.629197Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"obs_cov_00 = 10**-6\nobs_cov_11 = 10**-6\nobs_cov_off_diag = 10**-9","metadata":{"execution":{"iopub.status.busy":"2022-05-12T09:15:05.597477Z","iopub.execute_input":"2022-05-12T09:15:05.598125Z","iopub.status.idle":"2022-05-12T09:15:05.602693Z","shell.execute_reply.started":"2022-05-12T09:15:05.598075Z","shell.execute_reply":"2022-05-12T09:15:05.601879Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"obs_cov_mat = make_cov_mat([obs_cov_00, obs_cov_11], obs_cov_off_diag)","metadata":{"execution":{"iopub.status.busy":"2022-05-12T09:15:09.735316Z","iopub.execute_input":"2022-05-12T09:15:09.735645Z","iopub.status.idle":"2022-05-12T09:15:09.740521Z","shell.execute_reply.started":"2022-05-12T09:15:09.735607Z","shell.execute_reply":"2022-05-12T09:15:09.739576Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"process_cov_mat","metadata":{"execution":{"iopub.status.busy":"2022-05-12T09:15:12.675231Z","iopub.execute_input":"2022-05-12T09:15:12.675554Z","iopub.status.idle":"2022-05-12T09:15:12.682628Z","shell.execute_reply.started":"2022-05-12T09:15:12.675510Z","shell.execute_reply":"2022-05-12T09:15:12.681732Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"obs_cov_mat","metadata":{"execution":{"iopub.status.busy":"2022-05-12T09:15:16.454785Z","iopub.execute_input":"2022-05-12T09:15:16.455767Z","iopub.status.idle":"2022-05-12T09:15:16.460779Z","shell.execute_reply.started":"2022-05-12T09:15:16.455720Z","shell.execute_reply":"2022-05-12T09:15:16.460217Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"T = 1","metadata":{"execution":{"iopub.status.busy":"2022-05-12T09:15:21.017499Z","iopub.execute_input":"2022-05-12T09:15:21.018293Z","iopub.status.idle":"2022-05-12T09:15:21.022785Z","shell.execute_reply.started":"2022-05-12T09:15:21.018238Z","shell.execute_reply":"2022-05-12T09:15:21.021902Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"kf0 = make_kalman_filter(T, process_cov_mat, obs_cov_mat)","metadata":{"execution":{"iopub.status.busy":"2022-05-12T09:15:24.645547Z","iopub.execute_input":"2022-05-12T09:15:24.646099Z","iopub.status.idle":"2022-05-12T09:15:24.651286Z","shell.execute_reply.started":"2022-05-12T09:15:24.646038Z","shell.execute_reply":"2022-05-12T09:15:24.650287Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"# Test for 1 tripId\ndirname = dirnames[0]\npred_dfs = []\ndrive, phone = dirname.split('/')[-2:]\ntripID  = f'{drive}/{phone}'\ngnss_df = pd.read_csv(f'{dirname}/device_gnss.csv')\ngt_df   = pd.read_csv(f'{dirname}/ground_truth.csv')\npred_df = ecef_to_lat_lng(gnss_df, gt_df['UnixTimeMillis'].to_numpy())\ngnss_df = pd.read_csv(f'{dirnames[0]}/device_gnss.csv')\npred_dfs.append(pred_df)","metadata":{"execution":{"iopub.status.busy":"2022-05-12T09:15:50.866414Z","iopub.execute_input":"2022-05-12T09:15:50.866733Z","iopub.status.idle":"2022-05-12T09:15:53.216941Z","shell.execute_reply.started":"2022-05-12T09:15:50.866701Z","shell.execute_reply":"2022-05-12T09:15:53.215923Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"score = calc_score(pred_df, gt_df)","metadata":{"execution":{"iopub.status.busy":"2022-05-12T09:15:59.825926Z","iopub.execute_input":"2022-05-12T09:15:59.826243Z","iopub.status.idle":"2022-05-12T09:15:59.835936Z","shell.execute_reply.started":"2022-05-12T09:15:59.826214Z","shell.execute_reply":"2022-05-12T09:15:59.835029Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"score","metadata":{"execution":{"iopub.status.busy":"2022-05-12T09:16:03.255516Z","iopub.execute_input":"2022-05-12T09:16:03.255804Z","iopub.status.idle":"2022-05-12T09:16:03.261522Z","shell.execute_reply.started":"2022-05-12T09:16:03.255774Z","shell.execute_reply":"2022-05-12T09:16:03.260540Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"pred_df_filter = apply_kf_smoothing(pred_df, kf0)","metadata":{"execution":{"iopub.status.busy":"2022-05-12T09:16:25.021153Z","iopub.execute_input":"2022-05-12T09:16:25.021479Z","iopub.status.idle":"2022-05-12T09:16:26.146952Z","shell.execute_reply.started":"2022-05-12T09:16:25.021442Z","shell.execute_reply":"2022-05-12T09:16:26.145876Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"score = calc_score(pred_df_filter, gt_df)","metadata":{"execution":{"iopub.status.busy":"2022-05-12T09:16:28.492983Z","iopub.execute_input":"2022-05-12T09:16:28.493321Z","iopub.status.idle":"2022-05-12T09:16:28.500244Z","shell.execute_reply.started":"2022-05-12T09:16:28.493289Z","shell.execute_reply":"2022-05-12T09:16:28.499346Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"score","metadata":{"execution":{"iopub.status.busy":"2022-05-12T09:16:31.765276Z","iopub.execute_input":"2022-05-12T09:16:31.765743Z","iopub.status.idle":"2022-05-12T09:16:31.772337Z","shell.execute_reply.started":"2022-05-12T09:16:31.765710Z","shell.execute_reply":"2022-05-12T09:16:31.771307Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"### Training (Tuning with Optuna)","metadata":{}},{"cell_type":"code","source":"nb_file = len(dirnames)","metadata":{"execution":{"iopub.status.busy":"2022-05-12T09:18:08.296102Z","iopub.execute_input":"2022-05-12T09:18:08.296446Z","iopub.status.idle":"2022-05-12T09:18:08.299807Z","shell.execute_reply.started":"2022-05-12T09:18:08.296407Z","shell.execute_reply":"2022-05-12T09:18:08.299213Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"train_dfs = [pd.read_csv(f'{dirname}/device_gnss.csv') for dirname in dirnames[:nb_file]]","metadata":{"execution":{"iopub.status.busy":"2022-05-12T09:18:13.235701Z","iopub.execute_input":"2022-05-12T09:18:13.236053Z","iopub.status.idle":"2022-05-12T09:20:39.278421Z","shell.execute_reply.started":"2022-05-12T09:18:13.236015Z","shell.execute_reply":"2022-05-12T09:20:39.276272Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"train_gts = [pd.read_csv(f'{dirname}/ground_truth.csv') for dirname in dirnames[:nb_file]]","metadata":{"execution":{"iopub.status.busy":"2022-05-12T09:20:39.281853Z","iopub.execute_input":"2022-05-12T09:20:39.282203Z","iopub.status.idle":"2022-05-12T09:20:41.156238Z","shell.execute_reply.started":"2022-05-12T09:20:39.282159Z","shell.execute_reply":"2022-05-12T09:20:41.155326Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"filter_fn = partial(apply_kf_smoothing, _kf=kf0)","metadata":{"execution":{"iopub.status.busy":"2022-05-12T09:20:41.160309Z","iopub.execute_input":"2022-05-12T09:20:41.160708Z","iopub.status.idle":"2022-05-12T09:20:41.165364Z","shell.execute_reply.started":"2022-05-12T09:20:41.160657Z","shell.execute_reply":"2022-05-12T09:20:41.164506Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"def all_score(train_dfs, train_gts, filter_fn=None):\n    \"\"\" Calculate the score for list of df\"\"\"\n    scores = []\n    for gnss_df, gt_df in zip(train_dfs, train_gts):\n        pred_df = ecef_to_lat_lng(gnss_df, gt_df['UnixTimeMillis'].to_numpy())\n        if filter_fn:\n            pred_df = filter_fn(pred_df)\n        score = calc_score(pred_df, gt_df)\n        scores.append(score)\n    return scores","metadata":{"execution":{"iopub.status.busy":"2022-05-12T09:20:59.356434Z","iopub.execute_input":"2022-05-12T09:20:59.357518Z","iopub.status.idle":"2022-05-12T09:20:59.363881Z","shell.execute_reply.started":"2022-05-12T09:20:59.357459Z","shell.execute_reply":"2022-05-12T09:20:59.363280Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"def _make_kalman_filter(T, process_cov_mat, obs_cov_mat):\n    state_transition =  np.array([[1, 0, T, 0, 0.5 * T ** 2, 0], \n                             [0, 1, 0, T, 0, 0.5 * T ** 2], \n                             [0, 0, 1, 0, T, 0],\n                             [0, 0, 0, 1, 0, T], \n                             [0, 0, 0, 0, 1, 0], \n                             [0, 0, 0, 0, 0, 1]])\n    \n    observation_model = np.array([[1, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0]])\n    \n    observation_noise = obs_cov_mat\n    \n    process_noise = process_cov_mat\n\n    kf = simdkalman.KalmanFilter(\n            state_transition = state_transition,\n            process_noise = process_noise,\n            observation_model = observation_model,\n            observation_noise = observation_noise)\n    return kf","metadata":{"execution":{"iopub.status.busy":"2022-05-12T09:21:13.825095Z","iopub.execute_input":"2022-05-12T09:21:13.825444Z","iopub.status.idle":"2022-05-12T09:21:13.834753Z","shell.execute_reply.started":"2022-05-12T09:21:13.825405Z","shell.execute_reply":"2022-05-12T09:21:13.833690Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"def make_kalman_filter(param):\n    process_diag = [param[\"process_cov_00\"], \n                    param[\"process_cov_11\"], \n                    param[\"process_cov_22\"], \n                    param[\"process_cov_33\"], \n                    param[\"process_cov_44\"], \n                    param[\"process_cov_55\"]]\n    process_cov_off_diag = param[\"process_cov_off_diag\"]\n\n    obs_diag = [param[\"obs_cov_00\"],\n                param[\"obs_cov_11\"]]\n    obs_cov_off_diag = [param[\"obs_cov_off_diag\"]]\n\n    T = param['T']\n\n    process_cov_mat = make_cov_mat(process_diag, process_cov_off_diag)\n    obs_cov_mat = make_cov_mat([obs_cov_00, obs_cov_11], obs_cov_off_diag)\n\n    _kf = _make_kalman_filter(T, process_cov_mat, obs_cov_mat)\n    return _kf","metadata":{"execution":{"iopub.status.busy":"2022-05-12T09:21:29.716314Z","iopub.execute_input":"2022-05-12T09:21:29.716642Z","iopub.status.idle":"2022-05-12T09:21:29.725193Z","shell.execute_reply.started":"2022-05-12T09:21:29.716612Z","shell.execute_reply":"2022-05-12T09:21:29.724105Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"def objective(trial):\n    \n    param = {\n        'process_cov_00': trial.suggest_float(\"process_cov_00\", 1e-8, 1e-4, log=True),\n        'process_cov_11': trial.suggest_float(\"process_cov_11\", 1e-8, 1e-4, log=True),\n        'process_cov_22': trial.suggest_float(\"process_cov_22\", 1e-8, 1e-4, log=True),\n        'process_cov_33': trial.suggest_float(\"process_cov_33\", 1e-8, 1e-4, log=True),\n        'process_cov_44': trial.suggest_float(\"process_cov_44\", 1e-8, 1e-4, log=True),\n        'process_cov_55': trial.suggest_float(\"process_cov_55\", 1e-8, 1e-4, log=True),\n        'process_cov_off_diag': trial.suggest_float(\"process_cov_off_diag\", 1e-12, 1e-8, log=True),\n        \n        'obs_cov_00': trial.suggest_float(\"obs_cov_00\", 1e-8, 1e-4, log=True),\n        'obs_cov_11': trial.suggest_float(\"obs_cov_11\", 1e-8, 1e-4, log=True),\n        'obs_cov_off_diag': trial.suggest_float(\"obs_cov_off_diag\", 1e-12, 1e-8, log=True),\n        \n        'T': trial.suggest_float(\"T\", 0.6, 1.4),\n    }\n    \n    process_diag = [param[\"process_cov_00\"], \n                    param[\"process_cov_11\"], \n                    param[\"process_cov_22\"], \n                    param[\"process_cov_33\"], \n                    param[\"process_cov_44\"], \n                    param[\"process_cov_55\"]]\n    process_cov_off_diag = param[\"process_cov_off_diag\"]\n    \n    obs_diag = [param[\"obs_cov_00\"],\n                param[\"obs_cov_11\"]]\n    obs_cov_off_diag = [param[\"obs_cov_off_diag\"]]\n    \n    T = param['T']\n    \n    process_cov_mat = make_cov_mat(process_diag, process_cov_off_diag)\n    obs_cov_mat = make_cov_mat([obs_cov_00, obs_cov_11], obs_cov_off_diag)\n    \n    _kf = _make_kalman_filter(T, process_cov_mat, obs_cov_mat)\n    filter_fn = partial(apply_kf_smoothing,  _kf=_kf)\n    scores = all_score(train_dfs, train_gts, filter_fn=filter_fn)\n    \n    return np.mean(scores)","metadata":{"execution":{"iopub.status.busy":"2022-05-12T09:21:52.724585Z","iopub.execute_input":"2022-05-12T09:21:52.725067Z","iopub.status.idle":"2022-05-12T09:21:52.738269Z","shell.execute_reply.started":"2022-05-12T09:21:52.725021Z","shell.execute_reply":"2022-05-12T09:21:52.737336Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"study = optuna.create_study(direction='minimize')\n","metadata":{"execution":{"iopub.status.busy":"2022-05-12T09:21:56.155689Z","iopub.execute_input":"2022-05-12T09:21:56.156237Z","iopub.status.idle":"2022-05-12T09:21:56.164043Z","shell.execute_reply.started":"2022-05-12T09:21:56.156201Z","shell.execute_reply":"2022-05-12T09:21:56.163333Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"study.optimize(objective, n_trials=3)","metadata":{"execution":{"iopub.status.busy":"2022-05-12T09:22:25.076931Z","iopub.execute_input":"2022-05-12T09:22:25.077311Z","iopub.status.idle":"2022-05-12T09:27:46.463980Z","shell.execute_reply.started":"2022-05-12T09:22:25.077271Z","shell.execute_reply":"2022-05-12T09:27:46.463038Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"# These parameters below I got after trying n_trials = 100 \nparam = {'process_cov_00': 1.2020309620263925e-08,\n 'process_cov_11': 2.5818076069527384e-08,\n 'process_cov_22': 1.7418556333561943e-05,\n 'process_cov_33': 8.783630087500364e-06,\n 'process_cov_44': 3.190877235713106e-07,\n 'process_cov_55': 1.2137227780694694e-07,\n 'process_cov_off_diag': 2.1258126417177658e-09,\n 'obs_cov_00': 2.5228974904239324e-07,\n 'obs_cov_11': 1.2085757874535475e-05,\n 'obs_cov_off_diag': 7.79887179271382e-09,\n 'T': 0.7190536201746304}","metadata":{"execution":{"iopub.status.busy":"2022-05-12T09:29:02.137322Z","iopub.execute_input":"2022-05-12T09:29:02.137702Z","iopub.status.idle":"2022-05-12T09:29:02.144491Z","shell.execute_reply.started":"2022-05-12T09:29:02.137664Z","shell.execute_reply":"2022-05-12T09:29:02.143288Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"sample_df = pd.read_csv(f'{INPUT_PATH}/sample_submission.csv')\npred_dfs  = []\nfor dirname in sorted(glob.glob(f'{INPUT_PATH}/test/*/*')):\n    drive, phone = dirname.split('/')[-2:]\n    tripID  = f'{drive}/{phone}'\n    gnss_df = pd.read_csv(f'{dirname}/device_gnss.csv')\n    UnixTimeMillis = sample_df[sample_df['tripId'] == tripID]['UnixTimeMillis'].to_numpy()\n    pred_df = ecef_to_lat_lng(gnss_df, UnixTimeMillis)\n    _kf = make_kalman_filter(param)\n    pred_df = apply_kf_smoothing(pred_df, _kf)\n    pred_df.insert(0, 'tripId', tripID)\n    pred_dfs.append(pred_df)\nbaseline_test_df = pd.concat(pred_dfs)\nbaseline_test_df.to_csv('baseline_test.csv', index=False)\nbaseline_test_df.to_csv('submission.csv', index=False)\n","metadata":{"execution":{"iopub.status.busy":"2022-05-12T09:29:16.644347Z","iopub.execute_input":"2022-05-12T09:29:16.645618Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"","metadata":{},"execution_count":null,"outputs":[]}]}