{"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":"import numpy as np\nimport pandas as pd\nfrom math import * \nfrom pathlib import Path\nfrom matplotlib import pyplot as plt\nimport warnings\nwarnings.simplefilter('ignore')","metadata":{"_uuid":"8f2839f25d086af736a60e9eeb907d3b93b6e0e5","_cell_guid":"b1076dfc-b9ad-4769-8c92-a6c4dae69d19","execution":{"iopub.status.busy":"2021-06-18T16:08:56.583798Z","iopub.execute_input":"2021-06-18T16:08:56.584178Z","iopub.status.idle":"2021-06-18T16:08:56.589155Z","shell.execute_reply.started":"2021-06-18T16:08:56.584125Z","shell.execute_reply":"2021-06-18T16:08:56.588314Z"},"jupyter":{"source_hidden":true},"_kg_hide-input":true,"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"def gnss_log_to_dataframes(path):\n    print('Loading ' + path, flush=True)\n    gnss_section_names = {'Raw','UncalAccel', 'UncalGyro', 'UncalMag', 'Fix', 'Status', 'OrientationDeg'}\n    with open(path) as f_open:\n        datalines = f_open.readlines()\n\n    datas = {k: [] for k in gnss_section_names}\n    gnss_map = {k: [] for k in gnss_section_names}\n    for dataline in datalines:\n        is_header = dataline.startswith('#')\n        dataline = dataline.strip('#').strip().split(',')\n        # skip over notes, version numbers, etc\n        if is_header and dataline[0] in gnss_section_names:\n            try:\n                gnss_map[dataline[0]] = dataline[1:]\n            except:\n                pass\n        elif not is_header:\n            try:\n                datas[dataline[0]].append(dataline[1:])\n            except:\n                pass\n    results = dict()\n    for k, v in datas.items():\n        results[k] = pd.DataFrame(v, columns=gnss_map[k])\n    # pandas doesn't properly infer types from these lists by default\n    for k, df in results.items():\n        for col in df.columns:\n            if col == 'CodeType':\n                continue\n            try:\n                results[k][col] = pd.to_numeric(results[k][col])\n            except:\n                pass\n    return results","metadata":{"_kg_hide-input":true,"execution":{"iopub.status.busy":"2021-06-18T16:08:56.615704Z","iopub.execute_input":"2021-06-18T16:08:56.616394Z","iopub.status.idle":"2021-06-18T16:08:56.627551Z","shell.execute_reply.started":"2021-06-18T16:08:56.616354Z","shell.execute_reply":"2021-06-18T16:08:56.626337Z"},"_kg_hide-output":true,"jupyter":{"source_hidden":true},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"# lowpass filter\n\nfrom 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\n\norder = 3\nfs = 50.0\ncutoff = 2.5","metadata":{"execution":{"iopub.status.busy":"2021-06-18T16:08:56.629353Z","iopub.execute_input":"2021-06-18T16:08:56.629685Z","iopub.status.idle":"2021-06-18T16:08:56.657287Z","shell.execute_reply.started":"2021-06-18T16:08:56.629655Z","shell.execute_reply":"2021-06-18T16:08:56.656128Z"},"_kg_hide-output":true,"jupyter":{"source_hidden":true},"_kg_hide-input":true,"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"# Overview\n\n\nThis notebook calculate the direction of driving with geomagnetic and accelerometers. and compare the value obtained by this with the direction calculated from the transition of latitude and longitude of the ground truth.","metadata":{}},{"cell_type":"code","source":"# Offset correction\n# refarence https://github.com/J-ROCKET-BOY/SS-Fitting\n\ndef SS_fit(data) : \n\n    x = data[:,[0]]\n    y = data[:,[1]]\n    z = data[:,[2]]\n\n    data_len = len(x)\n    \n    x2 = np.power(x,2)\n    y2 = np.power(y,2)\n    z2 = np.power(z,2)\n\n    r1 = -x*(x2+y2+z2)\n    r2= -y*(x2+y2+z2)\n    r3 = -z*(x2+y2+z2)\n    r4 = -(x2+y2+z2)\n\n    left = np.array([[np.sum(x2),np.sum(x*y),np.sum(x*z),np.sum(x)],\n                     [np.sum(x*y),np.sum(y2),np.sum(y*z),np.sum(y)],\n                     [np.sum(x*z),np.sum(y*z),np.sum(z2),np.sum(z)],\n                     [np.sum(x), np.sum(y), np.sum(z), data_len]])\n    \n    right = np.array([np.sum(r1),\n                      np.sum(r2),\n                      np.sum(r3),\n                      np.sum(r4)])\n    \n    si = np.dot(np.linalg.inv(left),right)\n\n    x0 = (-1/2)* si[0]\n    y0 = (-1/2)* si[1]\n    z0 = (-1/2)* si[2]\n    \n    return np.array([x0,y0,z0])","metadata":{"execution":{"iopub.status.busy":"2021-06-18T16:08:56.659111Z","iopub.execute_input":"2021-06-18T16:08:56.659484Z","iopub.status.idle":"2021-06-18T16:08:56.672872Z","shell.execute_reply.started":"2021-06-18T16:08:56.659450Z","shell.execute_reply":"2021-06-18T16:08:56.671818Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"# Vincenty's formulae\n# refarence https://qiita.com/r-fuji/items/99ca549b963cedc106ab\n\ndef vincenty_inverse(lat1, lon1, lat2, lon2):\n\n    # Not advanced\n    if isclose(lat1, lat2) and isclose(lon1, lon2):\n        return False\n    \n    # WGS84\n    a = 6378137.0\n    ƒ = 1 / 298.257223563\n    b = (1 - ƒ) * a\n\n    lat_1 = atan((1 - ƒ) * tan(radians(lat1)))\n    lat_2 = atan((1 - ƒ) * tan(radians(lat2)))\n    \n    lon_diff = radians(lon2) - radians(lon1)\n    λ = lon_diff\n\n    for i in range(1000):\n        sinλ = sin(λ)\n        cosλ = cos(λ)\n        sinσ = sqrt((cos(lat_2) * sinλ) ** 2 + (cos(lat_1) * sin(lat_2) - sin(lat_1) * cos(lat_2) * cosλ) ** 2)\n        cosσ = sin(lat_1) * sin(lat_2) + cos(lat_1) * cos(lat_2) * cosλ\n        σ = atan2(sinσ, cosσ)\n        sinα = cos(lat_1) * cos(lat_2) * sinλ / sinσ\n        cos2α = 1 - sinα ** 2\n        cos2σm = cosσ - 2 * sin(lat_1) * sin(lat_2) / cos2α\n        C = ƒ / 16 * cos2α * (4 + ƒ * (4 - 3 * cos2α))\n        λʹ = λ\n        λ = lon_diff + (1 - C) * ƒ * sinα * (σ + C * sinσ * (cos2σm + C * cosσ * (-1 + 2 * cos2σm ** 2)))\n        \n        if abs(λ - λʹ) <= 1e-12:\n            break\n    else:\n        return None\n\n    α = atan2(cos(lat_2) * sinλ, cos(lat_1) * sin(lat_2) - sin(lat_1) * cos(lat_2) * cosλ)\n\n    if α < 0:\n        α = α + pi * 2\n\n    return degrees(α)","metadata":{"execution":{"iopub.status.busy":"2021-06-18T16:08:56.674842Z","iopub.execute_input":"2021-06-18T16:08:56.675196Z","iopub.status.idle":"2021-06-18T16:08:56.691690Z","shell.execute_reply.started":"2021-06-18T16:08:56.675149Z","shell.execute_reply":"2021-06-18T16:08:56.690635Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"def calc3(row):\n    deg = - degrees(atan2(-1*row['calc2'],row['calc1']))\n    if deg < 0:\n        deg += 360\n    return deg ","metadata":{"execution":{"iopub.status.busy":"2021-06-18T16:08:56.693035Z","iopub.execute_input":"2021-06-18T16:08:56.693697Z","iopub.status.idle":"2021-06-18T16:08:56.708869Z","shell.execute_reply.started":"2021-06-18T16:08:56.693616Z","shell.execute_reply":"2021-06-18T16:08:56.707883Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"data_dir = Path('../input/google-smartphone-decimeter-challenge')\nlogg_files = (data_dir / 'train').rglob('*Pixel4_GnssLog.txt')\nlogg_paths = [str(p) for p in logg_files]","metadata":{"execution":{"iopub.status.busy":"2021-06-18T16:08:56.720473Z","iopub.execute_input":"2021-06-18T16:08:56.720983Z","iopub.status.idle":"2021-06-18T16:08:56.903867Z","shell.execute_reply.started":"2021-06-18T16:08:56.720947Z","shell.execute_reply":"2021-06-18T16:08:56.903031Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"# use acce and magn\n\nfig,ax = plt.subplots(len(logg_paths)*2, 1, figsize=(25,6*2*len(logg_paths)))\n\nfor n, logg_path in enumerate(logg_paths):\n                      \n#     if n > 3:\n#         break\n    path = logg_path\n    gt_path = path.split(\"Pixel4_\")[0]+\"ground_truth.csv\"\n\n    gt_df = pd.read_csv(gt_path)\n    sample = gnss_log_to_dataframes(str(path))\n\n    acce_df = sample[\"UncalAccel\"]\n    mag_df = sample[\"UncalMag\"]\n\n    if not len(acce_df):\n        continue\n    \n    acce_df[\"millisSinceGpsEpoch\"] = acce_df[\"utcTimeMillis\"] - 315964800000\n    mag_df[\"millisSinceGpsEpoch\"] = mag_df[\"utcTimeMillis\"] - 315964800000\n    \n#     acce filtering and smooting\n    \n    acce_df[\"global_x\"] = acce_df[\"UncalAccelZMps2\"]\n    acce_df[\"global_y\"] = acce_df[\"UncalAccelXMps2\"]\n    acce_df[\"global_z\"] = acce_df[\"UncalAccelYMps2\"]\n    \n    acce_df[\"x_f\"] = butter_lowpass_filter(acce_df[\"global_x\"], cutoff, fs, order)\n    acce_df[\"y_f\"] = butter_lowpass_filter(acce_df[\"global_y\"], cutoff, fs, order)\n    acce_df[\"z_f\"] = butter_lowpass_filter(acce_df[\"global_z\"], cutoff, fs, order)\n    \n    smooth_range = 1000\n\n    acce_df[\"x_f\"] = acce_df[\"x_f\"].rolling(smooth_range, center=True, min_periods=1).mean()\n    acce_df[\"y_f\"] = acce_df[\"y_f\"].rolling(smooth_range, center=True, min_periods=1).mean()\n    acce_df[\"z_f\"] = acce_df[\"z_f\"].rolling(smooth_range, center=True, min_periods=1).mean()\n    \n#     magn filtering and smooting , offset correction\n    \n    mag_df[\"global_mx\"] = mag_df[\"UncalMagZMicroT\"]\n    mag_df[\"global_my\"] = mag_df[\"UncalMagYMicroT\"]\n    mag_df[\"global_mz\"] = mag_df[\"UncalMagXMicroT\"]\n    \n    smooth_range = 1000\n    \n    mag_df[\"global_mx\"] = mag_df[\"global_mx\"].rolling(smooth_range,  min_periods=1).mean()\n    mag_df[\"global_my\"] = mag_df[\"global_mz\"].rolling(smooth_range,  min_periods=1).mean()\n    mag_df[\"global_mz\"] = mag_df[\"global_my\"].rolling(smooth_range,  min_periods=1).mean()\n    \n    offset = SS_fit(np.array(mag_df[[\"global_mx\",\"global_my\",\"global_mz\"]]))\n    mag_df[\"global_mx\"] = (mag_df[\"global_mx\"] - offset[0])*-1\n    mag_df[\"global_my\"] = mag_df[\"global_my\"] - offset[1]\n    mag_df[\"global_mz\"] = mag_df[\"global_mz\"] - offset[2]\n\n#     merge the value of the one with the closest time \n    \n    acce_df[\"millisSinceGpsEpoch\"] = acce_df[\"millisSinceGpsEpoch\"]//1000 +10\n    mag_df[\"millisSinceGpsEpoch\"] = mag_df[\"millisSinceGpsEpoch\"]//1000 +10\n    gt_df[\"millisSinceGpsEpoch\"] = gt_df[\"millisSinceGpsEpoch\"]//1000\n    \n    acce_df = pd.merge_asof(acce_df.sort_values('millisSinceGpsEpoch'), \n                           mag_df[[\"global_mx\", \"global_my\",\"global_mz\",\"millisSinceGpsEpoch\"]].sort_values('millisSinceGpsEpoch') ,on='millisSinceGpsEpoch', direction='nearest')\n    \n    acce_df = pd.merge_asof(gt_df[[\"millisSinceGpsEpoch\",\"latDeg\",\"lngDeg\"]].sort_values('millisSinceGpsEpoch'), \n                           acce_df[[\"millisSinceGpsEpoch\", \"x_f\",\"y_f\",\"z_f\",\"global_mx\",\"global_my\",\"global_mz\"]].sort_values('millisSinceGpsEpoch') ,on='millisSinceGpsEpoch', direction='nearest')\n\n#     as a sensor value when stopped\n    \n    start_mean_range = 10\n    \n    x_start_mean = acce_df[:start_mean_range][\"x_f\"].mean()\n    y_start_mean = acce_df[:start_mean_range][\"y_f\"].mean()\n    z_start_mean = acce_df[:start_mean_range][\"z_f\"].mean() \n    \n#     roll and picth, device tilt\n    \n    r = atan(y_start_mean/z_start_mean)\n    p = atan(x_start_mean/(y_start_mean**2 + z_start_mean**2)**0.5)\n    \n#     calculation　degrees\n    \n    acce_df[\"calc1\"] = acce_df[\"global_mx\"]*cos(p) + acce_df[\"global_my\"]*sin(r)*sin(p) + acce_df[\"global_mz\"]*sin(p)*cos(r)\n    acce_df[\"calc2\"] = acce_df[\"global_mz\"]*sin(r) - acce_df[\"global_my\"]*cos(r)\n    acce_df[\"calc_deg\"] = acce_df.apply(calc3, axis=1)\n    \n#     degrees with lat and lng by ground truth\n\n    len_df = len(acce_df)\n    acce_df[\"deg\"] = 0\n    \n    gt_lat_prev = 0\n    gt_lng_prev = 0\n    \n    for i in range(1,len_df):\n        if i > 1:\n            res = vincenty_inverse(gt_lat_prev,gt_lng_prev,acce_df[\"latDeg\"].loc[i],acce_df[\"lngDeg\"].loc[i])\n            if res:\n                acce_df[\"deg\"].loc[i] = res\n            else:\n                if i > 0:\n                    acce_df[\"deg\"].loc[i] = acce_df[\"deg\"].loc[i-1]\n                else:\n                    acce_df[\"deg\"].loc[i] = 0\n        \n        gt_lat_prev = acce_df[\"latDeg\"].loc[i]\n        gt_lng_prev = acce_df[\"lngDeg\"].loc[i]\n    \n    ax[2*n].plot(acce_df[\"millisSinceGpsEpoch\"],acce_df[\"global_mx\"] ,label=\"mx\")\n    ax[2*n].plot(acce_df[\"millisSinceGpsEpoch\"],acce_df[\"global_my\"], label=\"my\")\n    ax[2*n].plot(acce_df[\"millisSinceGpsEpoch\"],acce_df[\"global_mz\"], label=\"mz\")\n    ax[2*n].legend()\n    ax[2*n].set_title(logg_path)\n \n    ax[2*n+1].plot(acce_df[\"millisSinceGpsEpoch\"],acce_df[\"deg\"] ,label=\"deg_by_gt\")\n    ax[2*n+1].plot(acce_df[\"millisSinceGpsEpoch\"],acce_df[\"calc_deg\"] ,label=\"deg_by_mag\")    \n    ax[2*n+1].legend()\n    ax[2*n+1].set_title(\"deg\")\n    ","metadata":{"execution":{"iopub.status.busy":"2021-06-18T16:15:16.890397Z","iopub.execute_input":"2021-06-18T16:15:16.890762Z","iopub.status.idle":"2021-06-18T16:23:30.846139Z","shell.execute_reply.started":"2021-06-18T16:15:16.890731Z","shell.execute_reply":"2021-06-18T16:23:30.844522Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"","metadata":{},"execution_count":null,"outputs":[]}]}