{"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":"### Disclaimer\n* Credits go to https://www.kaggle.com/museas for revisiting the cost minimisation solution. \n* I came across this method from https://www.kaggle.com/mehrankazeminia/1-3-indoor-navigation-cost-minimization-floor, which I thank here for their contribution.\n* The submission file used was from the best public ensemble, published in this notebook: https://www.kaggle.com/saurabhbagchi/ensembling-best-performing-notebooksCredits go to https://www.kaggle.com/museas for revisiting the cost minimisation solution. I came across this method from https://www.kaggle.com/mehrankazeminia/1-3-indoor-navigation-cost-minimization-floor, which I thank here for their contribution","metadata":{}},{"cell_type":"markdown","source":"### Solution follows","metadata":{}},{"cell_type":"markdown","source":"\n\n**The step estimation module provided by the host is great, but sometimes it points to strange positions. That of the host uses TYPE_ROTATION_VECTOR. Therefore, by using it in combination with the estimation using TYPE_MAGNETIC_FIELD, the walking direction can be brought closer to the accurate one.**\n\n\n\nScore improved 0.018 in this sub\n\n4.545 →　4.527\n","metadata":{}},{"cell_type":"markdown","source":"This notebook demonstrates a post-processing strategy for the\n[Indoor Location & Navigation](https://www.kaggle.com/c/indoor-location-navigation)\ncompetition.\n\nTo combine machine learning (wifi features) predictions with sensor data (acceleration, attitude heading),\nI defined cost function as follows,\n$$\nL(X_{1:N}) = \\sum_{i=1}^{N} \\alpha_i \\| X_i - \\hat{X}_i \\|^2 + \\sum_{i=1}^{N-1} \\beta_i \\| (X_{i+1} - X_{i}) - \\Delta \\hat{X}_i \\|^2\n$$\nwhere $\\hat{X}_i$ is absolute position predicted by machine learning and $\\Delta \\hat{X}_i$ is relative position predicted by sensor data.\n\nSince the cost function is quadratic, the optimal $X$ is solved by linear equation $Q X = c$\n, where $Q$ and $c$ are derived from above cost function.\nBecause the matrix $Q$ is tridiagonal,\neach machine learning prediction is corrected by *all* machine learning predictions and sensor data.\n\nThe optimal hyperparameters ($\\alpha$ and $\\beta$) can be estimated by expected error of machine learning and sensor data,\nor just tuned by public score.","metadata":{}},{"cell_type":"markdown","source":"## References\n+ [Simple 99% Accurate Floor Model](https://www.kaggle.com/nigelhenry/simple-99-accurate-floor-model)\n+ [Indoor Location Competition 2.0 (Sample Data and Code)](https://github.com/location-competition/indoor-location-competition-20)","metadata":{}},{"cell_type":"code","source":"!git clone --depth 1 https://github.com/location-competition/indoor-location-competition-20 indoor_location_competition_20\n!rm -rf indoor_location_competition_20/data","metadata":{"_uuid":"8f2839f25d086af736a60e9eeb907d3b93b6e0e5","_cell_guid":"b1076dfc-b9ad-4769-8c92-a6c4dae69d19","trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"import multiprocessing\nimport numpy as np\nimport pandas as pd\nimport scipy.interpolate\nimport scipy.sparse\nfrom tqdm import tqdm\n\nfrom indoor_location_competition_20.io_f import read_data_file\nimport indoor_location_competition_20.compute_f as compute_f","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"INPUT_PATH = '../input/indoor-location-navigation'","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"def compute_rel_positions(acce_datas, ahrs_datas):\n    step_timestamps, step_indexs, step_acce_max_mins = compute_f.compute_steps(acce_datas)\n    headings = compute_f.compute_headings(ahrs_datas)\n    stride_lengths = compute_f.compute_stride_length(step_acce_max_mins)\n    step_headings = compute_f.compute_step_heading(step_timestamps, headings)\n    rel_positions = compute_f.compute_rel_positions(stride_lengths, step_headings)\n    return rel_positions","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"import math\n\norder = 6\n# fs = 50.0  # sample rate, Hz\n# cutoff = 3\n\nfs = 100\ncutoff = 3.667  # desired cutoff frequency of the filter, Hz\n\nstep_distance = 0.8\nw_height = 1.7\nm_trans = -2","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"from scipy.signal import butter, lfilter\n\ndef butter_lowpass(cutoff, fs, order=5):\n    nyq = 0.5 * fs\n    normal_cutoff = cutoff / nyq\n    b, a = butter(order, normal_cutoff, btype='low', analog=False)\n    return b, a\n\ndef butter_lowpass_filter(data, cutoff, fs, order=5):\n    b, a = butter_lowpass(cutoff, fs, order=order)\n    y = lfilter(b, a, data)\n    return y","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"def peak_accel_threshold(data, timestamps, threshold):\n    d_acc = []\n    last_state = 'below'\n    crest_troughs = 0\n    crossings = []\n\n    for i, datum in enumerate(data):\n        \n        current_state = last_state\n        if datum < threshold:\n            current_state = 'below'\n        elif datum > threshold:\n            current_state = 'above'\n\n        if current_state is not last_state:\n            if current_state is 'above':\n                crossing = [timestamps[i], threshold]\n                crossings.append(crossing)\n            else:\n                crossing = [timestamps[i], threshold]\n                crossings.append(crossing)\n\n            crest_troughs += 1\n        last_state = current_state\n    return np.array(crossings)","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"**The blending method is to halve the stride length and adopt all steps, the number of steps will be doubled, but here only the movement distance is required**","metadata":{}},{"cell_type":"code","source":"def steps_compute_rel_positions(sample_file):\n    \n    mix_acce = np.sqrt(sample_file.acce[:,1:2]**2 + sample_file.acce[:,2:3]**2 + sample_file.acce[:,3:4]**2)\n    mix_acce = np.concatenate([sample_file.acce[:,0:1], mix_acce], 1)\n    mix_df = pd.DataFrame(mix_acce)\n    mix_df.columns = [\"timestamp\",\"acce\"]\n    \n    filtered = butter_lowpass_filter(mix_df[\"acce\"], cutoff, fs, order)\n\n    threshold = filtered.mean() * 1.1\n    crossings = peak_accel_threshold(filtered, mix_df[\"timestamp\"], threshold)\n\n    step_sum = len(crossings)/2\n    distance = w_height * 0.4 * step_sum\n\n    mag_df = pd.DataFrame(sample_file.magn)\n    mag_df.columns = [\"timestamp\",\"x\",\"y\",\"z\"]\n    \n    acce_df = pd.DataFrame(sample_file.acce)\n    acce_df.columns = [\"timestamp\",\"ax\",\"ay\",\"az\"]\n    \n    mag_df = pd.merge(mag_df,acce_df,on=\"timestamp\")\n    mag_df.dropna()\n    \n    time_di_list = []\n\n    for i in mag_df.iterrows():\n\n        gx,gy,gz = i[1][1],i[1][2],i[1][3]\n        ax,ay,az = i[1][4],i[1][5],i[1][6]\n\n        roll = math.atan2(ay,az)\n        pitch = math.atan2(-1*ax , (ay * math.sin(roll) + az * math.cos(roll)))\n\n        q = m_trans - math.degrees(math.atan2(\n            (gz*math.sin(roll)-gy*math.cos(roll)),(gx*math.cos(pitch) + gy*math.sin(roll)*math.sin(pitch) + gz*math.sin(pitch)*math.cos(roll))\n        )) -90\n        if q <= 0:\n            q += 360\n        time_di_list.append((i[1][0],q))\n\n    d_list = [x[1] for x in time_di_list]\n    \n    steps = []\n    step_time = []\n    di_dict = dict(time_di_list)\n\n    for n,i in enumerate(crossings[:,:1]):\n        if n % 2 == 1:\n            continue\n        direct_now = di_dict[i[0]]\n        dx = math.sin(math.radians(direct_now))\n        dy = math.cos(math.radians(direct_now))\n#         print(int(n/2+1),\"歩目/x:\",dx,\"/y:\",dy,\"/角度：\",direct_now)\n        steps.append((i[0],dx,dy))\n        step_time.append(i[0])\n    \n        step_dtime = np.diff(step_time)/1000\n        step_dtime = step_dtime.tolist()\n        step_dtime.insert(0,5)\n        \n        rel_position = []\n\n        wp_idx = 0\n#         print(\"WP:\",round(sample_file.waypoint[0,1],3),round(sample_file.waypoint[0,2],3),sample_file.waypoint[0,0])\n#         print(\"------------------\")\n        for p,i in enumerate(steps):\n            step_distance = 0\n            if step_dtime[p] >= 1:\n                step_distance = w_height*0.25\n            elif step_dtime[p] >= 0.75:\n                step_distance = w_height*0.3\n            elif step_dtime[p] >= 0.5:\n                step_distance = w_height*0.4\n            elif step_dtime[p] >= 0.35:\n                step_distance = w_height*0.45\n            elif step_dtime[p] >= 0.2:\n                step_distance = w_height*0.5\n            else:\n                step_distance = w_height*0.4\n\n#             step_x += i[1]*step_distance\n#             step_y += i[2]*step_distance\n            \n            rel_position.append([i[0], i[1]*step_distance, i[2]*step_distance])\n#     print(rel_position)\n    \n    return np.array(rel_position)","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"def correct_path(args):\n    path, path_df = args\n    \n    T_ref  = path_df['timestamp'].values\n    xy_hat = path_df[['x', 'y']].values\n    \n    example = read_data_file(f'{INPUT_PATH}/test/{path}.txt')\n\n    rel_positions1 = compute_rel_positions(example.acce, example.ahrs)\n    rel_positions2 = steps_compute_rel_positions(example)\n    rel1 = rel_positions1.copy()\n    rel2 = rel_positions2.copy()\n    rel1[:,1:] = rel_positions1[:,1:] / 2\n    rel2[:,1:] = rel_positions2[:,1:] / 2\n    rel_positions = np.vstack([rel1,rel2])\n    rel_positions = rel_positions[np.argsort(rel_positions[:, 0])]\n    \n    if T_ref[-1] > rel_positions[-1, 0]:\n        rel_positions = [np.array([[0, 0, 0]]), rel_positions, np.array([[T_ref[-1], 0, 0]])]\n    else:\n        rel_positions = [np.array([[0, 0, 0]]), rel_positions]\n    rel_positions = np.concatenate(rel_positions)\n    \n    T_rel = rel_positions[:, 0]\n    delta_xy_hat = np.diff(scipy.interpolate.interp1d(T_rel, np.cumsum(rel_positions[:, 1:3], axis=0), axis=0)(T_ref), axis=0)\n\n    N = xy_hat.shape[0]\n    delta_t = np.diff(T_ref)\n    alpha = (8.1)**(-2) * np.ones(N)\n    beta  = (0.3 + 0.3 * 1e-3 * delta_t)**(-2)\n    A = scipy.sparse.spdiags(alpha, [0], N, N)\n    B = scipy.sparse.spdiags( beta, [0], N-1, N-1)\n    D = scipy.sparse.spdiags(np.stack([-np.ones(N), np.ones(N)]), [0, 1], N-1, N)\n\n    Q = A + (D.T @ B @ D)\n    c = (A @ xy_hat) + (D.T @ (B @ delta_xy_hat))\n    xy_star = scipy.sparse.linalg.spsolve(Q, c)\n\n    return pd.DataFrame({\n        'site_path_timestamp' : path_df['site_path_timestamp'],\n        'floor' : path_df['floor'],\n        'x' : xy_star[:, 0],\n        'y' : xy_star[:, 1],\n    })","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"sub = pd.read_csv('../input/indoor-loc-and-nav-subs/submission_4475.csv')  # ('../input/simple-99-accurate-floor-model/submission.csv')\ntmp = sub['site_path_timestamp'].apply(lambda s : pd.Series(s.split('_')))\nsub['site'] = tmp[0]\nsub['path'] = tmp[1]\nsub['timestamp'] = tmp[2].astype(float)\n\nprocesses = multiprocessing.cpu_count()\nwith multiprocessing.Pool(processes=processes) as pool:\n    dfs = pool.imap_unordered(correct_path, sub.groupby('path'))\n    dfs = tqdm(dfs)\n    dfs = list(dfs)\nsub = pd.concat(dfs).sort_values('site_path_timestamp')\nsub.to_csv('submission.csv', index=False)","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"","metadata":{"trusted":true},"execution_count":null,"outputs":[]}]}