{"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":"In this notebook I tried to get the location of the phone at every timestamp of the accelerometer measures for the train set. This is useful to get the position when wifi or ibeacon were measured. A similar model could be derived from the ones in this notebook to get the position during inference.\n\nI used a state space model and the Kalman smoother to compute these locations. I tried with two models since I was a bit disappointed with the first one. Even with the second one I am still a bit frustrated. The model seems not to trust the accelerometer measures when I look at the optimized observation variances of the state space model. This makes me think I perhaps got visually correct results but with a poor modelling.\n\nI would be glad to get any feedback to correct or improve the model.","metadata":{}},{"cell_type":"markdown","source":"___\n# Packages","metadata":{}},{"cell_type":"code","source":"import json\nimport os\nfrom collections import OrderedDict\nfrom dataclasses import dataclass\n\n!pip install ahrs\nfrom ahrs.filters import EKF\nimport matplotlib.pyplot as plot\nimport numpy as np\nimport pandas as pd\nimport plotly.graph_objs as go\nimport statsmodels.api as sm\nfrom PIL import Image","metadata":{"_uuid":"8f2839f25d086af736a60e9eeb907d3b93b6e0e5","_cell_guid":"b1076dfc-b9ad-4769-8c92-a6c4dae69d19","trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"___\n# Parameters","metadata":{}},{"cell_type":"code","source":"data_path = \"/kaggle/input/indoor-location-navigation/\"","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"___\n# Functions","metadata":{}},{"cell_type":"markdown","source":"## Imported","metadata":{}},{"cell_type":"code","source":"# copy from https://github.com/location-competition/indoor-location-competition-20/blob/master/io_f.py\n\n\n@dataclass\nclass ReadData:\n    acce: np.ndarray\n    acce_uncali: np.ndarray\n    gyro: np.ndarray\n    gyro_uncali: np.ndarray\n    magn: np.ndarray\n    magn_uncali: np.ndarray\n    ahrs: np.ndarray\n    wifi: np.ndarray\n    ibeacon: np.ndarray\n    waypoint: np.ndarray\n\n\ndef read_data_file(data_filename):\n    acce = []\n    acce_uncali = []\n    gyro = []\n    gyro_uncali = []\n    magn = []\n    magn_uncali = []\n    ahrs = []\n    wifi = []\n    ibeacon = []\n    waypoint = []\n\n    with open(data_filename, \"r\", encoding=\"utf-8\") as file:\n        lines = file.readlines()\n\n    for line_data in lines:\n        line_data = line_data.strip()\n        if not line_data or line_data[0] == \"#\":\n            continue\n\n        line_data = line_data.split(\"\\t\")\n\n        if line_data[1] == \"TYPE_ACCELEROMETER\":\n            acce.append(\n                [\n                    int(line_data[0]),\n                    float(line_data[2]),\n                    float(line_data[3]),\n                    float(line_data[4]),\n                ]\n            )\n            continue\n\n        if line_data[1] == \"TYPE_ACCELEROMETER_UNCALIBRATED\":\n            acce_uncali.append(\n                [\n                    int(line_data[0]),\n                    float(line_data[2]),\n                    float(line_data[3]),\n                    float(line_data[4]),\n                ]\n            )\n            continue\n\n        if line_data[1] == \"TYPE_GYROSCOPE\":\n            gyro.append(\n                [\n                    int(line_data[0]),\n                    float(line_data[2]),\n                    float(line_data[3]),\n                    float(line_data[4]),\n                ]\n            )\n            continue\n\n        if line_data[1] == \"TYPE_GYROSCOPE_UNCALIBRATED\":\n            gyro_uncali.append(\n                [\n                    int(line_data[0]),\n                    float(line_data[2]),\n                    float(line_data[3]),\n                    float(line_data[4]),\n                ]\n            )\n            continue\n\n        if line_data[1] == \"TYPE_MAGNETIC_FIELD\":\n            magn.append(\n                [\n                    int(line_data[0]),\n                    float(line_data[2]),\n                    float(line_data[3]),\n                    float(line_data[4]),\n                ]\n            )\n            continue\n\n        if line_data[1] == \"TYPE_MAGNETIC_FIELD_UNCALIBRATED\":\n            magn_uncali.append(\n                [\n                    int(line_data[0]),\n                    float(line_data[2]),\n                    float(line_data[3]),\n                    float(line_data[4]),\n                ]\n            )\n            continue\n\n        if line_data[1] == \"TYPE_ROTATION_VECTOR\":\n            ahrs.append(\n                [\n                    int(line_data[0]),\n                    float(line_data[2]),\n                    float(line_data[3]),\n                    float(line_data[4]),\n                ]\n            )\n            continue\n\n        if line_data[1] == \"TYPE_WIFI\":\n            sys_ts = line_data[0]\n            ssid = line_data[\n                2\n            ]  # unique to a network, a WIFI network could be compose of many devices\n            bssid = line_data[3]  # unique to a device\n            rssi = line_data[4]  # strengh of signal\n            lastseen_ts = line_data[6]\n            wifi_data = [sys_ts, ssid, bssid, rssi, lastseen_ts]\n            wifi.append(wifi_data)\n            continue\n\n        if line_data[1] == \"TYPE_BEACON\":\n            ts = line_data[0]\n            uuid = line_data[2]\n            major = line_data[3]\n            minor = line_data[4]\n            rssi = line_data[6]\n            ibeacon_data = [ts, \"_\".join([uuid, major, minor]), rssi]\n            ibeacon.append(ibeacon_data)\n            continue\n\n        if line_data[1] == \"TYPE_WAYPOINT\":\n            waypoint.append(\n                [int(line_data[0]), float(line_data[2]), float(line_data[3])]\n            )\n\n    acce = np.array(acce)\n    acce_uncali = np.array(acce_uncali)\n    gyro = np.array(gyro)\n    gyro_uncali = np.array(gyro_uncali)\n    magn = np.array(magn)\n    magn_uncali = np.array(magn_uncali)\n    ahrs = np.array(ahrs)\n    wifi = np.array(wifi)\n    ibeacon = np.array(ibeacon)\n    waypoint = np.array(waypoint)\n\n    #     print(acce.shape)\n    #     print(acce_uncali.shape)\n    #     print(gyro.shape)\n    #     print(gyro_uncali.shape)\n    #     print(magn.shape)\n    #     print(magn_uncali.shape)\n    #     print(ahrs.shape)\n    #     print(wifi.shape)\n    #     print(ibeacon.shape)\n    #     print(waypoint.shape)\n\n    return ReadData(\n        acce,\n        acce_uncali,\n        gyro,\n        gyro_uncali,\n        magn,\n        magn_uncali,\n        ahrs,\n        wifi,\n        ibeacon,\n        waypoint,\n    )","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"## Imported and slightly modified","metadata":{}},{"cell_type":"code","source":"def visualize_trajectory(\n    trajectory,\n    floor_plan_filename,\n    width_meter,\n    height_meter,\n    title=None,\n    mode=\"lines + markers + text\",\n    show=False,\n    locs=None,\n    directions=None\n):\n    \"\"\"\n    Copied from from https://github.com/location-competition/indoor-location-competition-20/blob/master/visualize_f.py\n\n    \"\"\"\n    fig = go.Figure()\n\n    # add trajectory\n    size_list = [6] * trajectory.shape[0]\n    size_list[0] = 10\n    size_list[-1] = 10\n\n    color_list = [\"rgba(4, 174, 4, 0.5)\"] * trajectory.shape[0]\n    color_list[0] = \"rgba(12, 5, 235, 1)\"\n    color_list[-1] = \"rgba(235, 5, 5, 1)\"\n\n    position_count = {}\n    text_list = []\n    for i in range(trajectory.shape[0]):\n        if str(trajectory[i]) in position_count:\n            position_count[str(trajectory[i])] += 1\n        else:\n            position_count[str(trajectory[i])] = 0\n        text_list.append(\"        \" * position_count[str(trajectory[i])] + f\"{i}\")\n    text_list[0] = \"Start 0\"\n    text_list[-1] = f\"End {trajectory.shape[0] - 1}\"\n\n    fig.add_trace(\n        go.Scattergl(\n            x=trajectory[:, 0],\n            y=trajectory[:, 1],\n            mode=mode,\n            marker=dict(size=size_list, color=color_list),\n            line=dict(shape=\"linear\", color=\"lightgrey\", width=3, dash=\"dash\"),\n            text=text_list,\n            textposition=\"top center\",\n            name=\"trajectory\",\n        )\n    )\n\n    if isinstance(locs, pd.DataFrame):\n        fig.add_trace(go.Scatter(x=locs[\"x\"], y=locs[\"y\"], mode=\"lines\", name=\"position\"))\n        \n    if isinstance(directions, list):\n        for direction in directions:\n            fig.add_trace(\n                go.Scatter(\n                    x=[direction[\"x\"], direction[\"x\"] + direction[\"d_x\"]],\n                    y=[direction[\"y\"], direction[\"y\"] + direction[\"d_y\"]],\n                    mode=\"lines\",\n                    **direction[\"kwargs\"],\n                )\n            )\n\n    # add floor plan\n    floor_plan = Image.open(floor_plan_filename)\n    fig.update_layout(\n        images=[\n            go.layout.Image(\n                source=floor_plan,\n                xref=\"x\",\n                yref=\"y\",\n                x=0,\n                y=height_meter,\n                sizex=width_meter,\n                sizey=height_meter,\n                sizing=\"contain\",\n                opacity=1,\n                layer=\"below\",\n            )\n        ]\n    )\n\n    # configure\n    fig.update_xaxes(autorange=False, range=[0, width_meter])\n    fig.update_yaxes(\n        autorange=False, range=[0, height_meter], scaleanchor=\"x\", scaleratio=1\n    )\n    fig.update_layout(\n        title=go.layout.Title(\n            text=title or \"No title.\",\n            xref=\"paper\",\n            x=0,\n        ),\n        autosize=True,\n        width=800,\n        height=800 * height_meter / width_meter,\n        template=\"plotly_white\",\n    )\n\n    if show:\n        fig.show()\n\n    return fig\n\n\ndef visualize_train_trajectory(path, **kwargs):\n    \"\"\"\n    Edited from\n    https://www.kaggle.com/ihelon/indoor-location-exploratory-data-analysis\n    \"\"\"\n    _id, floor = path.split(\"/\")[:2]\n\n    train_floor_data = read_data_file(os.path.join(data_path, \"train\", path))\n    with open(os.path.join(data_path, \"metadata\", _id, floor, \"floor_info.json\")) as f:\n        train_floor_info = json.load(f)\n\n    return visualize_trajectory(\n        train_floor_data.waypoint[:, 1:3],\n        os.path.join(data_path, \"metadata\", _id, floor, \"floor_image.png\"),\n        train_floor_info[\"map_info\"][\"width\"],\n        train_floor_info[\"map_info\"][\"height\"],\n        f\"Visualization of {path}\",\n        **kwargs\n    )","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"## Own ones","metadata":{}},{"cell_type":"markdown","source":"### To get data","metadata":{}},{"cell_type":"code","source":"def build_meas_df(data):\n    acc = pd.DataFrame(data.acce, columns=[\"timestamp\", \"acc_x\", \"acc_y\", \"acc_z\"])\n    gyro = pd.DataFrame(data.gyro, columns=[\"timestamp\", \"gyro_x\", \"gyro_y\", \"gyro_z\"])\n    magn = pd.DataFrame(data.magn, columns=[\"timestamp\", \"magn_x\", \"magn_y\", \"magn_z\"])\n    ahrs = pd.DataFrame(data.ahrs, columns=[\"timestamp\", \"ahrs_x\", \"ahrs_y\", \"ahrs_z\"])\n    df = (\n        acc.merge(gyro, on=\"timestamp\")\n        .merge(magn, on=\"timestamp\")\n        .merge(ahrs, on=\"timestamp\")\n    )\n    df[\"timestamp\"] = df[\"timestamp\"].astype(int)\n    return df\n\n\ndef build_waypoint_df(data):\n    out = pd.DataFrame(\n        data.waypoint,\n        columns=[\n            \"timestamp\",\n            \"x\",\n            \"y\",\n        ],\n    )\n    out[\"timestamp\"] = out[\"timestamp\"].astype(int)\n    out[\"x\"] = out[\"x\"].astype(float)\n    out[\"y\"] = out[\"y\"].astype(float)\n    return out","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"### Location models","metadata":{}},{"cell_type":"markdown","source":"#### Model 1\n\nState space model. The x and y position can then be computed by means of Kalman smoother.\n\nBetween two time steps I consider that the acceleration is constant, hence the dynamic equations below. I also assumed that the measurement error variances of the accelerometers may differ from an axis to another. I finally assumed that the variances of the acceleration transition could also be different from an axis to another.\n\nThe states are:\n- positions x and y in the coordinate system of the buildings\n- speeds along x and y in the coordinate system of the buildings\n- accelerations along x, y and z in the coordinate system of the phone\n- bias along x, y and z of the phone accelerometers\n\nI arbitrarily the following order:\n[x, v_x, y, v_y, acc_x, acc_y, acc_z, bias_x, bias_y, bias_z]\n\nThe measures are:\n- positions x and y sometimes\n- accelerations + bias from the phone sensors at every time steps on the 3 axes","metadata":{}},{"cell_type":"code","source":"class Model1(sm.tsa.statespace.MLEModel):\n    def __init__(\n        self,\n        acc_x_t: np.array,\n        acc_y_t: np.array,\n        acc_z_t: np.array,\n        x_t: np.array,\n        y_t: np.array,\n        r_x_x: np.array,\n        r_x_y: np.array,\n        r_x_z: np.array,\n        r_y_x: np.array,\n        r_y_y: np.array,\n        r_y_z: np.array,\n        delta_t: float,\n    ):\n        super(Model1, self).__init__(\n            endog=np.c_[x_t, y_t, acc_x_t, acc_y_t, acc_z_t],\n            k_states=10,\n            k_posdef=3,\n            initialization=\"approximate_diffuse\",\n        )\n\n        self.ssm[\"design\"] = np.zeros((self.k_endog, self.k_states, self.nobs))\n        self.ssm[\"design\", 0, 0, :] = 1  # x\n        self.ssm[\"design\", 1, 2, :] = 1  # y\n        self.ssm[\"design\", 2, 4, :] = 1  # acc x phone\n        self.ssm[\"design\", 2, 7, :] = 1  # bias x phone\n        self.ssm[\"design\", 3, 5, :] = 1  # acc y phone\n        self.ssm[\"design\", 3, 8, :] = 1  # bias y phone\n        self.ssm[\"design\", 4, 6, :] = 1  # acc z phone\n        self.ssm[\"design\", 4, 9, :] = 1  # bias z phone\n\n        self.ssm[\"transition\"] = np.zeros((self.k_states, self.k_states, self.nobs))\n        self.ssm[\"selection\"] = np.zeros((self.k_states, 3, self.nobs))\n        # x(n+1)\n        self.ssm[\"transition\", 0, 0, :] = 1\n        self.ssm[\"transition\", 0, 1, :] = delta_t\n        self.ssm[\"selection\", 0, 0, :] = r_x_x * delta_t ** 2 / 2\n        self.ssm[\"selection\", 0, 1, :] = r_x_y * delta_t ** 2 / 2\n        self.ssm[\"selection\", 0, 2, :] = r_x_z * delta_t ** 2 / 2\n        # v_x(n+1)\n        self.ssm[\"transition\", 1, 1, :] = 1\n        self.ssm[\"selection\", 1, 0, :] = r_x_x * delta_t\n        self.ssm[\"selection\", 1, 1, :] = r_x_y * delta_t\n        self.ssm[\"selection\", 1, 2, :] = r_x_z * delta_t\n        # y(n+1)\n        self.ssm[\"transition\", 2, 2, :] = 1\n        self.ssm[\"transition\", 2, 3, :] = delta_t\n        self.ssm[\"selection\", 2, 0, :] = r_y_x * delta_t ** 2 / 2\n        self.ssm[\"selection\", 2, 1, :] = r_y_y * delta_t ** 2 / 2\n        self.ssm[\"selection\", 2, 2, :] = r_y_z * delta_t ** 2 / 2\n        # v_y(n+1)\n        self.ssm[\"transition\", 3, 3, :] = 1\n        self.ssm[\"selection\", 3, 0, :] = r_y_x * delta_t\n        self.ssm[\"selection\", 3, 1, :] = r_y_y * delta_t\n        self.ssm[\"selection\", 3, 2, :] = r_y_z * delta_t\n        # acc_x(n+1)\n        self.ssm[\"selection\", 4, 0, :] = 1\n        # acc_y(n+1)\n        self.ssm[\"selection\", 5, 1, :] = 1\n        # acc_z(n+1)\n        self.ssm[\"selection\", 6, 2, :] = 1\n        # bias_x(n+1)\n        self.ssm[\"transition\", 7, 7, :] = 1\n        # bias_x(n+1)\n        self.ssm[\"transition\", 8, 8, :] = 1\n        # bias_x(n+1)\n        self.ssm[\"transition\", 9, 9, :] = 1\n\n        self.position_dict = OrderedDict(\n            var_acc_x_dyn=1,\n            var_acc_y_dyn=2,\n            var_acc_z_dyn=3,\n            var_acc_x_meas=4,\n            var_acc_y_meas=5,\n            var_acc_z_meas=6,\n        )\n        self.initial_values = starting_values\n        self.positive_parameters = slice(0, 6)\n\n    @property\n    def param_names(self):\n        return list(self.position_dict.keys())\n\n    @property\n    def start_params(self):\n        params = np.r_[\n            self.initial_values[\"var.acc_x_dyn\"],\n            self.initial_values[\"var.acc_y_dyn\"],\n            self.initial_values[\"var.acc_z_dyn\"],\n            self.initial_values[\"var.acc_x_meas\"],\n            self.initial_values[\"var.acc_y_meas\"],\n            self.initial_values[\"var.acc_z_meas\"],\n        ]\n        return params\n\n    def transform_params(self, unconstrained):\n        \"\"\"\n        We constraint all parameters to be positive,\n        because they are variances\n        \"\"\"\n        constrained = unconstrained.copy()\n        constrained[self.positive_parameters] = (\n            constrained[self.positive_parameters] ** 2\n        )\n        return constrained\n\n    def untransform_params(self, constrained):\n        \"\"\"\n        Need to unstransform all the parameters you transformed\n        in the `transform_params` function\n        \"\"\"\n        unconstrained = constrained.copy()\n        unconstrained[self.positive_parameters] = (\n            unconstrained[self.positive_parameters] ** 0.5\n        )\n        return unconstrained\n\n    def update(self, params, **kwargs):\n        params = super(Model1, self).update(params, **kwargs)\n        self[\"obs_cov\", 2, 2] = params[3]\n        self[\"obs_cov\", 3, 3] = params[4]\n        self[\"obs_cov\", 4, 4] = params[5]\n        self[\"state_cov\", 0, 0] = params[0]\n        self[\"state_cov\", 1, 1] = params[1]\n        self[\"state_cov\", 2, 2] = params[2]","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"#### Model 2\n\nThe model 2 is very similar to the model 1. I was not satisfied with the results obtained with the first model. I observed that the vertical accelerometer seems to indicate that the phone carrier was steady when the positions were stored. The vertical acceleration is almost constant which indicates that there is no step. For these two reasons I tried to add the speeds to the observations when locations x and y were measured. I arbitrarily chose to set the speed at these points to 0, which I would say is a strong assumption. \n\nThe states are the same:\n- positions x and y in the coordinate system of the buildings\n- speeds along x and y in the coordinate system of the buildings\n- accelerations along x, y and z in the coordinate system of the phone\n- bias along x, y and z of the phone accelerometers\n\nThe order is the same:\n[x, v_x, y, v_y, acc_x, acc_y, acc_z, bias_x, bias_y, bias_z]\n\nThe measures are now:\n- positions x and y sometimes\n- accelerations + bias from the phone sensors at every time steps on the 3 axes\n- speeds along x and y sometimes","metadata":{}},{"cell_type":"code","source":"class Model2(sm.tsa.statespace.MLEModel):\n    def __init__(\n        self,\n        acc_x_t: np.array,\n        acc_y_t: np.array,\n        acc_z_t: np.array,\n        x_t: np.array,\n        y_t: np.array,\n        r_x_x: np.array,\n        r_x_y: np.array,\n        r_x_z: np.array,\n        r_y_x: np.array,\n        r_y_y: np.array,\n        r_y_z: np.array,\n        delta_t: float,\n        v_x: np.array,\n        v_y:np.array\n    ):\n        super(Model2, self).__init__(\n            endog=np.c_[x_t, y_t, acc_x_t, acc_y_t, acc_z_t, v_x, v_y],\n            k_states=10,\n            k_posdef=3,\n            initialization=\"approximate_diffuse\",\n        )\n\n        self.ssm[\"design\"] = np.zeros((self.k_endog, self.k_states, self.nobs))\n        self.ssm[\"design\", 0, 0, :] = 1  # x\n        self.ssm[\"design\", 1, 2, :] = 1  # y\n        self.ssm[\"design\", 2, 4, :] = 1  # acc x phone\n        self.ssm[\"design\", 2, 7, :] = 1  # bias x phone\n        self.ssm[\"design\", 3, 5, :] = 1  # acc y phone\n        self.ssm[\"design\", 3, 8, :] = 1  # bias y phone\n        self.ssm[\"design\", 4, 6, :] = 1  # acc z phone\n        self.ssm[\"design\", 4, 9, :] = 1  # bias z phone\n        self.ssm[\"design\", 5, 1, :] = 1  # v_x\n        self.ssm[\"design\", 6, 3, :] = 1  # v_y\n\n        self.ssm[\"transition\"] = np.zeros((self.k_states, self.k_states, self.nobs))\n        self.ssm[\"selection\"] = np.zeros((self.k_states, 3, self.nobs))\n        # x(n+1)\n        self.ssm[\"transition\", 0, 0, :] = 1\n        self.ssm[\"transition\", 0, 1, :] = delta_t\n        self.ssm[\"selection\", 0, 0, :] = r_x_x * delta_t ** 2 / 2\n        self.ssm[\"selection\", 0, 1, :] = r_x_y * delta_t ** 2 / 2\n        self.ssm[\"selection\", 0, 2, :] = r_x_z * delta_t ** 2 / 2\n        # v_x(n+1)\n        self.ssm[\"transition\", 1, 1, :] = 1\n        self.ssm[\"selection\", 1, 0, :] = r_x_x * delta_t\n        self.ssm[\"selection\", 1, 1, :] = r_x_y * delta_t\n        self.ssm[\"selection\", 1, 2, :] = r_x_z * delta_t\n        # y(n+1)\n        self.ssm[\"transition\", 2, 2, :] = 1\n        self.ssm[\"transition\", 2, 3, :] = delta_t\n        self.ssm[\"selection\", 2, 0, :] = r_y_x * delta_t ** 2 / 2\n        self.ssm[\"selection\", 2, 1, :] = r_y_y * delta_t ** 2 / 2\n        self.ssm[\"selection\", 2, 2, :] = r_y_z * delta_t ** 2 / 2\n        # v_y(n+1)\n        self.ssm[\"transition\", 3, 3, :] = 1\n        self.ssm[\"selection\", 3, 0, :] = r_y_x * delta_t\n        self.ssm[\"selection\", 3, 1, :] = r_y_y * delta_t\n        self.ssm[\"selection\", 3, 2, :] = r_y_z * delta_t\n        # acc_x(n+1)\n        self.ssm[\"selection\", 4, 0, :] = 1\n        # acc_y(n+1)\n        self.ssm[\"selection\", 5, 1, :] = 1\n        # acc_z(n+1)\n        self.ssm[\"selection\", 6, 2, :] = 1\n        # bias_x(n+1)\n        self.ssm[\"transition\", 7, 7, :] = 1\n        # bias_x(n+1)\n        self.ssm[\"transition\", 8, 8, :] = 1\n        # bias_x(n+1)\n        self.ssm[\"transition\", 9, 9, :] = 1\n\n        self.position_dict = OrderedDict(\n            var_acc_x_dyn=1,\n            var_acc_y_dyn=2,\n            var_acc_z_dyn=3,\n            var_acc_x_meas=4,\n            var_acc_y_meas=5,\n            var_acc_z_meas=6,\n        )\n        self.initial_values = starting_values\n        self.positive_parameters = slice(0, 6)\n\n    @property\n    def param_names(self):\n        return list(self.position_dict.keys())\n\n    @property\n    def start_params(self):\n        params = np.r_[\n            self.initial_values[\"var.acc_x_dyn\"],\n            self.initial_values[\"var.acc_y_dyn\"],\n            self.initial_values[\"var.acc_z_dyn\"],\n            self.initial_values[\"var.acc_x_meas\"],\n            self.initial_values[\"var.acc_y_meas\"],\n            self.initial_values[\"var.acc_z_meas\"],\n        ]\n        return params\n\n    def transform_params(self, unconstrained):\n        \"\"\"\n        We constraint all parameters to be positive,\n        because they are variances\n        \"\"\"\n        constrained = unconstrained.copy()\n        constrained[self.positive_parameters] = (\n            constrained[self.positive_parameters] ** 2\n        )\n        return constrained\n\n    def untransform_params(self, constrained):\n        \"\"\"\n        Need to unstransform all the parameters you transformed\n        in the `transform_params` function\n        \"\"\"\n        unconstrained = constrained.copy()\n        unconstrained[self.positive_parameters] = (\n            unconstrained[self.positive_parameters] ** 0.5\n        )\n        return unconstrained\n\n    def update(self, params, **kwargs):\n        params = super(Model2, self).update(params, **kwargs)\n        self[\"obs_cov\", 2, 2] = params[3]\n        self[\"obs_cov\", 3, 3] = params[4]\n        self[\"obs_cov\", 4, 4] = params[5]\n        self[\"state_cov\", 0, 0] = params[0]\n        self[\"state_cov\", 1, 1] = params[1]\n        self[\"state_cov\", 2, 2] = params[2]","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"> ___\n# Compute location at every time steps","metadata":{}},{"cell_type":"markdown","source":"## Chosen file","metadata":{}},{"cell_type":"code","source":"building = \"5a0546857ecc773753327266\"\nfloor = \"F4\"\npath = \"5d11dc28ffe23f0008604f67\"\nbuilding_floor_path = building + \"/\" + floor + \"/\" + path + \".txt\"","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"## Get data","metadata":{}},{"cell_type":"code","source":"data = read_data_file(os.path.join(data_path, \"train\", building_floor_path))\ndynamic_df = build_meas_df(data)\ndynamic_df[\"path\"] = path\nwaypoint_df = build_waypoint_df(data)\nwaypoint_df[\"path\"] = path","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"## Add waypoint to dynamic","metadata":{}},{"cell_type":"code","source":"dynamic_with_waypoint = dynamic_df.merge(\n    waypoint_df, on=\"path\", suffixes=(\"_dynamic\", \"_waypoint\")\n)\ndynamic_with_waypoint[\"timestamp_diff\"] = (\n    dynamic_with_waypoint[\"timestamp_dynamic\"] - dynamic_with_waypoint[\"timestamp_waypoint\"]\n).abs()\ndynamic_with_waypoint.sort_values([\"timestamp_waypoint\", \"timestamp_diff\"], inplace=True)\ndynamic_with_waypoint.drop_duplicates([\"timestamp_waypoint\"], inplace=True)\ndynamic_with_waypoint = dynamic_with_waypoint[[\"timestamp_dynamic\", \"x\", \"y\"]]\ndynamic_with_waypoint.rename(columns={\"timestamp_dynamic\": \"timestamp\"}, inplace=True)\ndynamic_df_with_waypoint = dynamic_df.merge(dynamic_with_waypoint, how=\"left\")","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"## Get rotation matrix (only 6 components are enough in the model)\n\nThe rotation vector of the phone is already given in the provided data through an unit quaternion at every timestamp. For a sanity check, a comparison has been carried out between the existing rotation vector data and the estimation of the phone orientation with an extended Kalman filter provided by the library [ahrs](http://ahrs.readthedocs.io/en/latest/filters/ekf.html).","metadata":{}},{"cell_type":"markdown","source":"### Provided data","metadata":{}},{"cell_type":"code","source":"# Add quaternion first component\nfirst_comp_quarternion = (\n    1\n    - dynamic_df_with_waypoint[\"ahrs_x\"] ** 2\n    - dynamic_df_with_waypoint[\"ahrs_y\"] ** 2\n    - dynamic_df_with_waypoint[\"ahrs_z\"] ** 2\n)\nfirst_comp_quarternion.loc[first_comp_quarternion < 0] = 0\ndynamic_df_with_waypoint[\"quaternion_cos\"] = first_comp_quarternion.apply(np.sqrt)\n\n# Add rotation matrix components\ndynamic_df_with_waypoint[\"r_x_x\"] = (\n    dynamic_df_with_waypoint[\"quaternion_cos\"] ** 2\n    + dynamic_df_with_waypoint[\"ahrs_x\"] ** 2\n    - dynamic_df_with_waypoint[\"ahrs_y\"] ** 2\n    - dynamic_df_with_waypoint[\"ahrs_z\"] ** 2\n)\ndynamic_df_with_waypoint[\"r_x_y\"] = (\n    2 * dynamic_df_with_waypoint[\"ahrs_x\"] * dynamic_df_with_waypoint[\"ahrs_y\"]\n    - 2\n    * dynamic_df_with_waypoint[\"quaternion_cos\"]\n    * dynamic_df_with_waypoint[\"ahrs_z\"]\n)\ndynamic_df_with_waypoint[\"r_x_z\"] = (\n    2 * dynamic_df_with_waypoint[\"quaternion_cos\"] * dynamic_df_with_waypoint[\"ahrs_y\"]\n    + 2 * dynamic_df_with_waypoint[\"ahrs_x\"] * dynamic_df_with_waypoint[\"ahrs_z\"]\n)\n\ndynamic_df_with_waypoint[\"r_y_x\"] = (\n    2 * dynamic_df_with_waypoint[\"quaternion_cos\"] * dynamic_df_with_waypoint[\"ahrs_z\"]\n    + 2 * dynamic_df_with_waypoint[\"ahrs_x\"] * dynamic_df_with_waypoint[\"ahrs_y\"]\n)\ndynamic_df_with_waypoint[\"r_y_y\"] = (\n    dynamic_df_with_waypoint[\"quaternion_cos\"] ** 2\n    - dynamic_df_with_waypoint[\"ahrs_x\"] ** 2\n    + dynamic_df_with_waypoint[\"ahrs_y\"] ** 2\n    - dynamic_df_with_waypoint[\"ahrs_z\"] ** 2\n)\ndynamic_df_with_waypoint[\"r_y_z\"] = (\n    2 * dynamic_df_with_waypoint[\"ahrs_y\"] * dynamic_df_with_waypoint[\"ahrs_z\"]\n    - 2\n    * dynamic_df_with_waypoint[\"quaternion_cos\"]\n    * dynamic_df_with_waypoint[\"ahrs_x\"]\n)\n\ndynamic_df_with_waypoint[\"r_z_x\"] = (\n    2 * dynamic_df_with_waypoint[\"ahrs_x\"] * dynamic_df_with_waypoint[\"ahrs_z\"]\n    - 2\n    * dynamic_df_with_waypoint[\"quaternion_cos\"]\n    * dynamic_df_with_waypoint[\"ahrs_y\"]\n)\ndynamic_df_with_waypoint[\"r_z_y\"] = (\n    2 * dynamic_df_with_waypoint[\"quaternion_cos\"] * dynamic_df_with_waypoint[\"ahrs_x\"]\n    + 2 * dynamic_df_with_waypoint[\"ahrs_y\"] * dynamic_df_with_waypoint[\"ahrs_z\"]\n)\ndynamic_df_with_waypoint[\"r_z_z\"] = (\n    dynamic_df_with_waypoint[\"quaternion_cos\"] ** 2\n    - dynamic_df_with_waypoint[\"ahrs_x\"] ** 2\n    - dynamic_df_with_waypoint[\"ahrs_y\"] ** 2\n    + dynamic_df_with_waypoint[\"ahrs_z\"] ** 2\n)","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"### By means of an extend Kalman filter and gyroscope, acceleration and magnetic field measures","metadata":{}},{"cell_type":"code","source":"ekf = EKF(\n    gyr=dynamic_df_with_waypoint[[\"gyro_x\", \"gyro_y\", \"gyro_z\"]].values,\n    acc=dynamic_df_with_waypoint[[\"acc_x\", \"acc_y\", \"acc_z\"]].values,\n    mag=dynamic_df_with_waypoint[[\"magn_x\", \"magn_y\", \"magn_z\"]].values,\n    frequency=1/dynamic_df_with_waypoint[\"timestamp\"].diff().mean()*1e3,\n    frame=\"ENU\",\n)\n\nQ = ekf.Q\nq0 = Q[:, 0]\nq1 = Q[:, 1]\nq2 = Q[:, 2]\nq3 = Q[:, 3]\n\ndynamic_df_with_waypoint[\"r_x_x_2\"] = q0 ** 2 + q1 ** 2 - q2 ** 2 - q3 ** 2\ndynamic_df_with_waypoint[\"r_x_y_2\"] = 2 * (q1 * q2 - q0 * q3)\ndynamic_df_with_waypoint[\"r_x_z_2\"] = 2 * (q1 * q3 + q0 * q2)\n\ndynamic_df_with_waypoint[\"r_y_x_2\"] = 2 * (q1 * q2 + q0 * q3)\ndynamic_df_with_waypoint[\"r_y_y_2\"] = q0 ** 2 - q1 ** 2 + q2 ** 2 - q3 ** 2\ndynamic_df_with_waypoint[\"r_y_z_2\"] = 2 * (q2 * q3 - q0 * q1)","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"### Get directions at all waypoint measures\n\nAs explained in the data description, the phone is held horizontally with its y axis pointing towards where the person walks. I then expect the phone y axis to be tangent to the path. ","metadata":{}},{"cell_type":"code","source":"arrows = dynamic_df_with_waypoint.loc[~dynamic_df_with_waypoint[\"x\"].isnull()]\n\ndirections = []\nshowlegend = True\nfor _, row in arrows.iterrows():\n    # direction from provided data\n    direction = {\n        \"x\": row[\"x\"],\n        \"y\": row[\"y\"],\n        \"d_x\": row[\"r_x_y\"],\n        \"d_y\": row[\"r_y_y\"],\n        \"kwargs\": {\n            \"line\": {\"color\": \"red\"},\n            \"legendgroup\": 1,\n            \"showlegend\": showlegend,\n            \"name\": \"provided data\",\n        },\n    }\n    directions.append(direction)\n\n    # direction from the EKF results\n    direction = {\n        \"x\": row[\"x\"],\n        \"y\": row[\"y\"],\n        \"d_x\": row[\"r_x_y_2\"],\n        \"d_y\": row[\"r_y_y_2\"],\n        \"kwargs\": {\n            \"line\": {\"color\": \"blue\"},\n            \"legendgroup\": 2,\n            \"showlegend\": showlegend,\n            \"name\": \"computed with EKF\",\n        },\n    }\n    directions.append(direction)\n    showlegend = False","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"visualize_train_trajectory(building_floor_path, directions=directions)","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"The directions are not perfectly superimposed. This may be due to different parameters of the extended Kalman filter used by the Android phone to get the rotation vector or maybe because something else is used. However, even though they are not superimposed, they are very similar which is comforting.","metadata":{}},{"cell_type":"markdown","source":"## Get data for models","metadata":{}},{"cell_type":"code","source":"r_x_x = dynamic_df_with_waypoint[\"r_x_x\"].to_numpy()\nr_x_y = dynamic_df_with_waypoint[\"r_x_y\"].to_numpy()\nr_x_z = dynamic_df_with_waypoint[\"r_x_z\"].to_numpy()\n\nr_y_x = dynamic_df_with_waypoint[\"r_y_x\"].to_numpy()\nr_y_y = dynamic_df_with_waypoint[\"r_y_y\"].to_numpy()\nr_y_z = dynamic_df_with_waypoint[\"r_y_z\"].to_numpy()\n\nr_z_x = dynamic_df_with_waypoint[\"r_z_x\"].to_numpy()\nr_z_y = dynamic_df_with_waypoint[\"r_z_y\"].to_numpy()\nr_z_z = dynamic_df_with_waypoint[\"r_z_z\"].to_numpy()\n\n# Get acceleration without gravity. A steady accelerometer along the vertical axis and towards up measures +1g.\ng = 9.81\nacc_x_t = (dynamic_df_with_waypoint[\"acc_x\"] - r_z_x * g).to_numpy()\nacc_y_t = (dynamic_df_with_waypoint[\"acc_y\"] - r_z_y * g).to_numpy()\nacc_z_t = (dynamic_df_with_waypoint[\"acc_z\"] - r_z_z * g).to_numpy()\n\nx_t = dynamic_df_with_waypoint[\"x\"].to_numpy()\ny_t = dynamic_df_with_waypoint[\"y\"].to_numpy()\n\nv_x = (dynamic_df_with_waypoint[\"x\"] * 0).to_numpy()\nv_y = (dynamic_df_with_waypoint[\"x\"] * 0).to_numpy()\n\ndelta_t = dynamic_df_with_waypoint[\"timestamp\"].diff().mean() / 1e3","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"## Model 1","metadata":{}},{"cell_type":"code","source":"starting_values = {\n    \"var.acc_x_dyn\": 1,\n    \"var.acc_y_dyn\": 1,\n    \"var.acc_z_dyn\": 1,\n    \"var.acc_x_meas\": 0.1,\n    \"var.acc_y_meas\": 0.1,\n    \"var.acc_z_meas\": 0.1,\n}\n\nmodel_1 = Model1(\n    acc_x_t,\n    acc_y_t,\n    acc_z_t,\n    x_t,\n    y_t,\n    r_x_x,\n    r_x_y,\n    r_x_z,\n    r_y_x,\n    r_y_y,\n    r_y_z,\n    delta_t,\n)\nres_model_1 = model_1.fit(maxiter=100)","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"res_model_1.summary()","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"dynamic_df_with_waypoint[\"x_smoothed_model_1\"] = res_model_1.smoothed_state[0, :]\ndynamic_df_with_waypoint[\"y_smoothed_model_1\"] = res_model_1.smoothed_state[2, :]","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"fig = plot.figure(figsize=(16, 16))\n\nss = pd.DataFrame(\n    res_model_1.smoothed_state.T,\n    columns=[\n        \"x\",\n        \"v_x\",\n        \"y\",\n        \"v_y\",\n        \"acc_x\",\n        \"acc_y\",\n        \"acc_z\",\n        \"bias_acc_x\",\n        \"bias_acc_y\",\n        \"bias_acc_z\",\n    ],\n    index=dynamic_df_with_waypoint[\"timestamp\"],\n)\n\nax = fig.add_subplot(2, 1, 1)\nax.plot(ss[\"x\"], label=\"Smoothed estimate x\")\nax.plot(\n    waypoint_df[\"timestamp\"],\n    waypoint_df[\"x\"],\n    linestyle=\"\",\n    marker=\"+\",\n    label=\"Actual x\"\n)\nax.set_xlabel(\"timestamp [s]\")\nax.set_ylabel(\"position [m]\")\nax.legend()\n\nax = fig.add_subplot(2, 1, 2)\nax.plot(ss[\"y\"], label=\"Smoothed estimate y\")\nax.plot(\n    waypoint_df[\"timestamp\"],\n    waypoint_df[\"y\"],\n    linestyle=\"\",\n    marker=\"+\",\n    label=\"Actual y\"\n)\nax.set_xlabel(\"timestamp [s]\")\nax.set_ylabel(\"position [m]\")\nax.legend()\n\nplot.show()","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"locs = dynamic_df_with_waypoint[[\"x_smoothed_model_1\", \"y_smoothed_model_1\"]]\nlocs.rename(columns={\"x_smoothed_model_1\": \"x\", \"y_smoothed_model_1\": \"y\"}, inplace=True)\nvisualize_train_trajectory(building_floor_path, locs=locs)","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"Some positions are out of the path which makes me try to improve the model.","metadata":{}},{"cell_type":"markdown","source":"## Model 2","metadata":{}},{"cell_type":"code","source":"starting_values = {\n    \"var.acc_x_dyn\": 1,\n    \"var.acc_y_dyn\": 1,\n    \"var.acc_z_dyn\": 1,\n    \"var.acc_x_meas\": 0.1,\n    \"var.acc_y_meas\": 0.1,\n    \"var.acc_z_meas\": 0.1,\n}\n\nmodel_2 = Model2(\n    acc_x_t,\n    acc_y_t,\n    acc_z_t,\n    x_t,\n    y_t,\n    r_x_x,\n    r_x_y,\n    r_x_z,\n    r_y_x,\n    r_y_y,\n    r_y_z,\n    delta_t,\n    v_x,\n    v_y\n)\nres_model_2 = model_2.fit(maxiter=100)","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"res_model_2.summary()","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"dynamic_df_with_waypoint[\"x_smoothed_model_2\"] = res_model_2.smoothed_state[0, :]\ndynamic_df_with_waypoint[\"y_smoothed_model_2\"] = res_model_2.smoothed_state[2, :]","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"fig = plot.figure(figsize=(16, 16))\n\nss = pd.DataFrame(\n    res_model_2.smoothed_state.T,\n    columns=[\n        \"x\",\n        \"v_x\",\n        \"y\",\n        \"v_y\",\n        \"acc_x\",\n        \"acc_y\",\n        \"acc_z\",\n        \"bias_acc_x\",\n        \"bias_acc_y\",\n        \"bias_acc_z\",\n    ],\n    index=dynamic_df_with_waypoint[\"timestamp\"],\n)\n\nax = fig.add_subplot(2, 1, 1)\nax.plot(ss[\"x\"], label=\"Smoothed estimate x\")\nax.plot(\n    waypoint_df[\"timestamp\"],\n    waypoint_df[\"x\"],\n    linestyle=\"\",\n    marker=\"+\",\n    label=\"Actual x\"\n)\nax.set_xlabel(\"timestamp [s]\")\nax.set_ylabel(\"position [m]\")\nax.legend()\n\nax = fig.add_subplot(2, 1, 2)\nax.plot(ss[\"y\"], label=\"Smoothed estimate y\")\nax.plot(\n    waypoint_df[\"timestamp\"],\n    waypoint_df[\"y\"],\n    linestyle=\"\",\n    marker=\"+\",\n    label=\"Actual y\"\n)\nax.set_xlabel(\"timestamp [s]\")\nax.set_ylabel(\"position [m]\")\nax.legend()\n\nplot.show()","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"locs = dynamic_df_with_waypoint[[\"x_smoothed_model_2\", \"y_smoothed_model_2\"]]\nlocs.rename(columns={\"x_smoothed_model_2\": \"x\", \"y_smoothed_model_2\": \"y\"}, inplace=True)\nvisualize_train_trajectory(building_floor_path, locs=locs)","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"___\n# Outlook\nI am thinking of a state space model during inference to compute the position of the phone at every timestamp. During inference, there is no waypoint measure. Location could be estimated when wifi or ibeacon are measured. Contrary to for the train set, these measures would not be 100% accurate. Then the observation covariance matrix would be changed with additional non zero values for the variances of these measured. I don't know yet how to evaluate these values. Regarding the parameters optimized with the train set, I would fix them during inference using for instance means of the values obtained with all the paths.","metadata":{}}]}