{"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 computes the weighted-least-squares (WLS) velocity from  `PseudorangeRateMetersPerSecond` data at each epoch. This is an additional piece of information using Doppler shift, not just a difference in positions computed from time (pseudo ranges). The \"epoch\" is one timestep and each epoch is treated independently in this notebook; connecting multiple timesteps, e.g Kalman Filter, is a next step, beyond the scope of this notebook.\n\nWarning:\nI am new to GPS and there is still some disagreement in the WLS positions compared to those provided as a baseline. Let me know in the comment for anything missing or misunderstood.","metadata":{}},{"cell_type":"code","source":"%matplotlib inline\nimport numpy as np\nimport pandas as pd\nimport matplotlib.pyplot as plt\nimport math\nimport glob\nimport scipy.optimize\nfrom tqdm.auto import tqdm\n\nc = 299_792_458  # speed of light in vaccum [m/s]\nomega = 7.292115e-5  # angular velocity [rad/s] in ECEF coordinate WGS 84","metadata":{"execution":{"iopub.status.busy":"2022-05-16T22:36:49.221001Z","iopub.execute_input":"2022-05-16T22:36:49.221578Z","iopub.status.idle":"2022-05-16T22:36:49.965981Z","shell.execute_reply.started":"2022-05-16T22:36:49.221545Z","shell.execute_reply":"2022-05-16T22:36:49.964805Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"path = '/kaggle/input/smartphone-decimeter-2022/train/2020-05-15-US-MTV-1/GooglePixel4XL'\ngnss = pd.read_csv('%s/device_gnss.csv' % path, dtype={'SignalType': str})\ntruth = pd.read_csv('%s/ground_truth.csv' % path)","metadata":{"execution":{"iopub.status.busy":"2022-05-16T22:36:49.968214Z","iopub.execute_input":"2022-05-16T22:36:49.968599Z","iopub.status.idle":"2022-05-16T22:36:52.417324Z","shell.execute_reply.started":"2022-05-16T22:36:49.968547Z","shell.execute_reply":"2022-05-16T22:36:52.416231Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"## 1. Position estimation\n\nPosition estimation is necessary before velocity estimation.\n\nSee also:\n- https://www.kaggle.com/code/junkoda/deriving-baseline-wls-positions-in-progress\n- https://www.kaggle.com/code/hyperc/gsdc-reproducing-baseline-wls-on-one-measurement\n- https://www.kaggle.com/code/foreveryoung/least-squares-solution-from-gnss-derived-data\n\nA short summary is:\n\n- $\\vec{x}_i$: satellite positions (known);\n- $\\vec{y}$: receiver position;\n- $r_i = \\lVert \\vec{x}_i - \\vec{y} \\rVert$: distance to satellites;\n- $\\rho_i = r_i + b$: pseudo range = distance including unknown receiver clock bias $b$;\n\nWeighted Least Square find $(\\vec{y}, b)$ that minimizes:\n\n\n$$ \\sum_i \\left( \\frac{\\lVert \\vec{x}_i - \\vec{y} \\rVert - \\rho_i + b}{\\Delta \\rho} \\right)^2 $$\n\nwhere the weight is the inverse of the error in the pseudo ranges $\\Delta \\rho$.\n\n","metadata":{}},{"cell_type":"markdown","source":"## 2. Velocity estimation\n\nVelocites projected to the direction of satellites (line-of-sight velocities) are provided as,\n\nv_los = `PseudorangeRateMetersPerSecond`,\n\nwhich is proportional to the measured Dopper shift in frequency converted to m/s.\n\nWe find receiver velocity $\\vec{v}$ that matches these line-of-sight velocities:\n\n$$ v_{\\mathrm{los}, i} = (\\vec{v}_{\\mathrm{sat},i} - \\vec{v})\\cdot \\hat{\\mathbf{r}}_i $$\n\nwhere $\\hat{\\mathbf{r}}_i$ is the unit vector poiting sattelite $i$ from the receiver:\n\n- $\\vec{r}_i = \\vec{x}_i - \\vec{y} $,\n- $\\hat{\\mathbf{r}}_i = \\vec{r}_i \\,/\\, \\lVert \\vec{r}_i \\rVert $\n\nNow, weighted least squares for velocity minimizes:\n\n$$ \\sum_i \\left[ \\frac{(\\vec{v}_{\\mathrm{sat},i} - \\vec{v})\\cdot \\hat{\\mathbf{r}}_i - v_{\\mathrm{los},i} + v_b}{\\Delta v_{\\mathrm{los},i}} \\right]^2$$","metadata":{"execution":{"iopub.status.busy":"2022-05-16T22:41:54.476856Z","iopub.execute_input":"2022-05-16T22:41:54.477201Z","iopub.status.idle":"2022-05-16T22:41:54.489578Z","shell.execute_reply.started":"2022-05-16T22:41:54.47717Z","shell.execute_reply":"2022-05-16T22:41:54.48827Z"}}},{"cell_type":"markdown","source":"## On the revolutions of the heavenly spheres\n\nThe satellite velocities are provided in Earth-centered Earth-fixed (ECEF) frame, which is\nrotating with Earth. This is adding unphysical velocity to satellites from the rotating viewpoint and inappropriate for matching the Doppler shift.\n\nFixed point in ECEF frame rorates in intertial frame (x, y):\n\n- $x = R \\cos \\omega t$,\n- $y = R \\sin \\omega t$,\n\nwhere $R = \\sqrt{x^2 + y^2}$ is fixed.\n\nTheir time derivatives:\n\n- $\\dot{x} = - R \\omega \\sin \\omega t = - \\omega y$,\n- $\\dot{y} = R \\omega \\cos \\omega t = \\omega x $.\n\n\nSubtract this rotation velocity to obtain inertial-frame velocity:\n\n- $v_x = v_x^\\mathrm{ECEF} + \\omega y$,\n- $v_y = v_y^\\mathrm{ECEF} - \\omega x$,\n- $v_z = v_z^\\mathrm{ECEF}$.\n\n\nhttps://en.wikipedia.org/wiki/Rotating_reference_frame","metadata":{}},{"cell_type":"code","source":"m = len(truth)  # Number of timesteps\ny_wls = np.zeros((m, 3))  # Receiver positions estimated here\nv_wls = np.zeros((m, 3))  # Receiver velocities estimated in ECEF frame\n\nfor i, (t_nano, df1) in enumerate(tqdm(gnss.groupby('TimeNanos'), total=m)):\n    #\n    # 1. Position estimation\n    #\n    \n    # Corrected pseudo range ρ [m]\n    rho = (df1['RawPseudorangeMeters'] + df1['SvClockBiasMeters'] - df1['IsrbMeters']\n           - df1['IonosphericDelayMeters'] - df1['TroposphericDelayMeters']).values\n\n    # Satellite positions at emmision time t_i in ECEF(t_i)\n    x_sat = df1[['SvPositionXEcefMeters', 'SvPositionYEcefMeters', 'SvPositionZEcefMeters']].values\n\n    # Inverse uncertainty weight\n    w = 1 / df1['RawPseudorangeUncertaintyMeters'].values\n\n    def f(y):\n        \"\"\"\n        Compute error for trial receiver position y\n\n        y (y1, y2, y3, b):\n          y: recerver position at receiving time\n          b: receiver clock bias in meters\n        \"\"\"\n        b = y[3]\n        r = rho - b  # distance to each satellite [m]\n        tau = r / c  # signal flight time\n\n        # Rotate satellite positions at emission to present ECEF coordinate\n        x = np.empty_like(x_sat)\n        cosO = np.cos(omega * tau)\n        sinO = np.sin(omega * tau)\n        x[:, 0] =  cosO * x_sat[:, 0] + sinO * x_sat[:, 1]\n        x[:, 1] = -sinO * x_sat[:, 0] + cosO * x_sat[:, 1]\n        x[:, 2] = x_sat[:, 2]\n\n        return w * (np.sqrt(np.sum((x - y[:3])**2, axis=1)) - r)\n\n    \n    # Fit receiver position y and clock bias b\n    x0 = np.zeros(4)  # initial guess\n    opt = scipy.optimize.least_squares(f, x0)\n    y = opt.x[:3]\n    b = opt.x[3]\n    \n    #\n    # 2. Velocity estimation\n    #\n    \n    # Use estimated position\n    r = rho - b  # distance to each satellite [m]\n    tau = r / c\n    \n    # Satellite positions at emission in present (signal arrival time) ECEF coordinate\n    x = np.empty_like(x_sat)\n    cosO = np.cos(omega * tau)\n    sinO = np.sin(omega * tau)\n    x[:, 0] =  cosO * x_sat[:, 0] + sinO * x_sat[:, 1]\n    x[:, 1] = -sinO * x_sat[:, 0] + cosO * x_sat[:, 1]\n    x[:, 2] = x_sat[:, 2]\n\n    v_sat_ecef = df1[['SvVelocityXEcefMetersPerSecond',\n                      'SvVelocityYEcefMetersPerSecond',\n                      'SvVelocityZEcefMetersPerSecond']].values\n        \n    # Velocity in inertial frame (matching ECEF at signal emission time)\n    v_sate = np.empty_like(v_sat_ecef)\n    v_sate[:, 0] = v_sat_ecef[:, 0] - omega * x_sat[:, 1]\n    v_sate[:, 1] = v_sat_ecef[:, 1] + omega * x_sat[:, 0]\n    v_sate[:, 2] = v_sat_ecef[:, 2]\n\n    # Rotate the velocity to another inertial frame matching ECEF at signal arrival time\n    v_sat = np.empty_like(v_sat_ecef)\n    v_sat[:, 0] =  cosO * v_sate[:, 0] + sinO * v_sate[:, 1]\n    v_sat[:, 1] = -sinO * v_sate[:, 0] + cosO * v_sate[:, 1]\n    v_sat[:, 2] = v_sate[:, 2]\n\n    # Direction from receiver to sattelites\n    r_vec = x - y.reshape(1, 3)\n    r_hat = r_vec / np.linalg.norm(r_vec, axis=1).reshape(-1, 1)  # unit vector\n    \n    # Line-of-sight velocity from doppler shift data\n    v_los = df1['PseudorangeRateMetersPerSecond'].values  \n\n    # Inverse uncertainty in v_los for weights\n    w_vel = 1 / df1['PseudorangeRateUncertaintyMetersPerSecond'].values\n    \n    def f_vel(v):\n        \"\"\"\n        Return weighted error for velocity estimate v\n\n        v (v1, v2, v3, v_b): Receiver velocity and velocity bias\n        \"\"\"    \n        # Line-of-sight relative velocity for fitting parameter v\n        v_rel = np.sum((v_sat - v[:3].reshape(1, 3)) * r_hat, axis=1)  # dot product to r_hat\n\n        err = w_vel * (v_rel - v_los + v[3])\n\n        return err\n    \n    v0 = np.zeros(4)  # initial guess\n    opt = scipy.optimize.least_squares(f_vel, v0)\n    v = opt.x[:3]\n    vb = opt.x[3]\n    \n    # Receiver velocity in ECEF frame\n    v_ecef = np.zeros(3)\n    v_ecef[0] = v[0] + omega * y[1]\n    v_ecef[1] = v[1] - omega * y[0]\n    v_ecef[2] = v[2]\n\n    # Save result\n    y_wls[i, :] = y\n    v_wls[i, :] = v_ecef","metadata":{"execution":{"iopub.status.busy":"2022-05-16T23:02:37.169505Z","iopub.execute_input":"2022-05-16T23:02:37.169807Z","iopub.status.idle":"2022-05-16T23:04:21.158457Z","shell.execute_reply.started":"2022-05-16T23:02:37.169773Z","shell.execute_reply":"2022-05-16T23:04:21.157197Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"# Observing the result","metadata":{}},{"cell_type":"code","source":"speed_wls = np.linalg.norm(v_wls, axis=1)\nspeed_truth = truth['SpeedMps'].values\n\nbl_truth = truth[['LatitudeDegrees', 'LongitudeDegrees']].values\n\ny_baseline = gnss.groupby('TimeNanos')[['WlsPositionXEcefMeters', 'WlsPositionYEcefMeters', 'WlsPositionZEcefMeters']].mean().values\ny_baseline.shape, speed_truth.shape","metadata":{"execution":{"iopub.status.busy":"2022-05-16T23:14:06.36563Z","iopub.execute_input":"2022-05-16T23:14:06.365929Z","iopub.status.idle":"2022-05-16T23:14:06.38306Z","shell.execute_reply.started":"2022-05-16T23:14:06.365897Z","shell.execute_reply":"2022-05-16T23:14:06.381984Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"plt.figure(figsize=(8, 6))\nplt.subplot(2, 1, 1)\nplt.title('Velocity estimation')\nplt.ylabel('speed [m/s]')\nplt.plot(speed_truth, label='truth')\nplt.plot(speed_wls, alpha=0.5, label='this work')\nplt.legend(frameon=False)\n\nplt.subplot(2, 1, 2)\nplt.xlabel('Timestep $\\\\approx$ sec')\nplt.ylabel('Error speed [m/s]')\nplt.axhline(0, color='gray', alpha=0.5)\nplt.plot(speed_wls - speed_truth)\nplt.show()","metadata":{"execution":{"iopub.status.busy":"2022-05-16T23:06:08.601534Z","iopub.execute_input":"2022-05-16T23:06:08.602053Z","iopub.status.idle":"2022-05-16T23:06:09.013932Z","shell.execute_reply.started":"2022-05-16T23:06:08.601999Z","shell.execute_reply":"2022-05-16T23:06:09.012911Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"bins = np.linspace(-1, 1, 101)\nplt.title('Speed error')\nplt.xlabel('$v - v_\\\\mathrm{true}$ [m/s]')\nplt.hist(speed_wls - speed_truth, bins, alpha=0.8)\nplt.axvline(0, color='gray', alpha=0.5)\nplt.show()","metadata":{"execution":{"iopub.status.busy":"2022-05-16T23:10:26.486465Z","iopub.execute_input":"2022-05-16T23:10:26.486794Z","iopub.status.idle":"2022-05-16T23:10:26.82835Z","shell.execute_reply.started":"2022-05-16T23:10:26.486757Z","shell.execute_reply":"2022-05-16T23:10:26.827639Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"plt.figure(figsize=(14, 6))\nplt.suptitle('Position estimation (xyz)')\nfor k in range(3):\n    plt.subplot(2, 3, k + 1)\n    plt.title('xyz'[k])\n    plt.plot(y_baseline[:, k], label='Baseline WLS')\n    plt.plot(y_wls[:, k], label='This notebook')\n    \n    if k == 0:\n        plt.ylabel('Position [m]')\n        plt.legend(frameon=False)\n    \n    plt.subplot(2, 3, k + 4)\n    plt.axhline(0, color='gray', alpha=0.5)\n    plt.plot(y_wls[:, k] - y_baseline[:, k], label='This notebook')\n    \n    if k == 0:\n        plt.ylabel('WLS disagreement [m]')","metadata":{"execution":{"iopub.status.busy":"2022-05-16T23:09:15.403327Z","iopub.execute_input":"2022-05-16T23:09:15.404596Z","iopub.status.idle":"2022-05-16T23:09:16.217914Z","shell.execute_reply.started":"2022-05-16T23:09:15.404518Z","shell.execute_reply":"2022-05-16T23:09:16.216268Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"\"\"\"\nxyz to latitude longitude\nThanks to: Akio Saito\nhttps://www.kaggle.com/code/saitodevel01/gsdc2-baseline-submission\n\"\"\"\n    \nWGS84_SEMI_MAJOR_AXIS = 6378137.0\nWGS84_SEMI_MINOR_AXIS = 6356752.314245\n\ndef to_blh(xyz, *, unit='degree'):\n    \"\"\"\n    Args:\n      x, y, z (float): ecef coordinate in meters\n      unit (str): Unit for latitude longitude, degree or radian\n    Returns:\n      B: geodetic latitude\n      L: geodesic longitude\n      H: height above the ellipsoid\n    \"\"\"\n    assert unit == 'degree' or unit == 'radian'\n    \n    x = xyz[:, 0]\n    y = xyz[:, 1]\n    z = xyz[:, 2]\n    blh = np.empty_like(xyz)\n\n    # Ellipsoidal parameters\n    a = WGS84_SEMI_MAJOR_AXIS\n    b = WGS84_SEMI_MINOR_AXIS\n    f = (a - b) / a\n    e2 = 2*f - f**2\n    ep2 = (a**2 - b**2) / b**2\n\n    # Transformation\n    r = np.sqrt(x**2 + y**2)\n    theta = np.arctan2(z * (a / b), r)\n    B = np.arctan2(z + (ep2 * b) * np.sin(theta)**3, r - (e2 * a) * np.cos(theta)**3)\n    blh[:, 0] = B\n    blh[:, 1] = np.arctan2(y, x)\n    n = a / np.sqrt(1 - e2 * np.sin(B)**2)\n    blh[:, 2] = (r / np.cos(B)) - n\n\n    if unit == 'degree':\n        blh[:, 0] = np.degrees(blh[:, 0])\n        blh[:, 1] = np.degrees(blh[:, 1])\n\n    return blh\n\nblh = to_blh(y_wls)\nblh_baseline = to_blh(y_baseline)","metadata":{"execution":{"iopub.status.busy":"2022-05-16T23:15:12.006242Z","iopub.execute_input":"2022-05-16T23:15:12.006663Z","iopub.status.idle":"2022-05-16T23:15:12.023734Z","shell.execute_reply.started":"2022-05-16T23:15:12.006624Z","shell.execute_reply":"2022-05-16T23:15:12.022538Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"plt.figure(figsize=(12, 6))\n\nfor k in range(2):\n    plt.subplot(2, 2, k + 1)\n    plt.title(['Latitude', 'Longitude'][k])\n    plt.plot(bl_truth[:, k], color='gray', label='truth')\n    plt.plot(blh_baseline[:, k], label='baseline')\n    plt.plot(blh[:, k], label='this notebook')\n    \n    if k == 0:\n        plt.ylabel('lat/lon [degree]')\n    \n    plt.subplot(2, 2, k + 3)\n    plt.axhline(0, color='gray')\n    plt.plot(blh_baseline[:, k] - bl_truth[:, k], label='baseline')\n    plt.plot(blh[:, k] - bl_truth[:, k], alpha=0.5, label='this notebook')\n    \n    if k == 0:\n        plt.ylabel('Error [degree]')\n        plt.legend(frameon=False)","metadata":{"execution":{"iopub.status.busy":"2022-05-16T23:38:05.288126Z","iopub.execute_input":"2022-05-16T23:38:05.289662Z","iopub.status.idle":"2022-05-16T23:38:05.979864Z","shell.execute_reply.started":"2022-05-16T23:38:05.289605Z","shell.execute_reply":"2022-05-16T23:38:05.97859Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"## Rough consistency check\n\nxyz is not a good coordinate system on Earth, east north up (ENU) coordinate is more intuitive locally. However, which velocity representation is best depends on how you use the velocities and positions\nwith time evolution. Since this notebook is limited to treating each timestep separately, I'll stop here with xyz velocity components and end with a rough consistency check between positions and velocities.\n\n$$ \\vec{v}_\\mathrm{diff}(t_i) \\approx \\frac{\\vec{x}(t_{i+1}) - \\vec{x}(t_{i-1})}{2\\Delta t}$$\n\n$\\Delta t$ is usually 1 s but sometimes varies.","metadata":{}},{"cell_type":"code","source":"v_diff = (y_wls[2:, :] - y_wls[:-2, :]) / 2\n    \nplt.figure(figsize=(14, 3))\nplt.suptitle('Position Velocity consistency')\n\nfor k in range(3):\n    plt.subplot(1, 3, k + 1)\n    plt.title('xyz'[k])\n    plt.xlabel('timestep ~ sec')\n    plt.plot(v_diff[:, k], label='Difference')\n    plt.plot(v_wls[:, k], label='Doppler')\n    \n    if k == 0:\n        plt.ylabel('$v_k$ [m/s]')\n        plt.legend(frameon=False)","metadata":{"execution":{"iopub.status.busy":"2022-05-16T23:31:11.866611Z","iopub.execute_input":"2022-05-16T23:31:11.866941Z","iopub.status.idle":"2022-05-16T23:31:12.396598Z","shell.execute_reply.started":"2022-05-16T23:31:11.866903Z","shell.execute_reply":"2022-05-16T23:31:12.395411Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"Nice. At least signs are correct.","metadata":{}}]}