{"metadata":{"kernelspec":{"language":"python","display_name":"Python 3","name":"python3"},"language_info":{"name":"python","version":"3.11.11","mimetype":"text/x-python","codemirror_mode":{"name":"ipython","version":3},"pygments_lexer":"ipython3","nbconvert_exporter":"python","file_extension":".py"},"kaggle":{"accelerator":"none","dataSources":[{"sourceId":102335,"databundleVersionId":12518947,"sourceType":"competition"}],"dockerImageVersionId":31040,"isInternetEnabled":true,"language":"python","sourceType":"notebook","isGpuEnabled":false}},"nbformat_minor":4,"nbformat":4,"cells":[{"cell_type":"code","source":"!pip uninstall scipy -y -q\n!pip install scipy==1.10.0 -q","metadata":{"trusted":true,"execution":{"iopub.status.busy":"2025-06-06T07:07:45.733884Z","iopub.execute_input":"2025-06-06T07:07:45.734165Z","iopub.status.idle":"2025-06-06T07:08:00.164585Z","shell.execute_reply.started":"2025-06-06T07:07:45.734145Z","shell.execute_reply":"2025-06-06T07:08:00.163483Z"}},"outputs":[],"execution_count":null},{"cell_type":"code","source":"import pandas as pd\nimport numpy as np\n\nimport matplotlib.pyplot as plt\n\nfrom scipy.spatial.transform import Rotation as R\nfrom scipy.integrate import cumtrapz","metadata":{"trusted":true,"execution":{"iopub.status.busy":"2025-06-06T07:08:00.165667Z","iopub.execute_input":"2025-06-06T07:08:00.165949Z","iopub.status.idle":"2025-06-06T07:08:00.801590Z","shell.execute_reply.started":"2025-06-06T07:08:00.165918Z","shell.execute_reply":"2025-06-06T07:08:00.800945Z"}},"outputs":[],"execution_count":null},{"cell_type":"code","source":"class KalmanFilter1D:\n    def __init__(self, dt, process_var, meas_var):\n        self.dt = dt\n        self.A = np.array([[1, dt, 0.5 * dt**2],\n                           [0, 1, dt],\n                           [0, 0, 1]])\n        self.H = np.array([[1, 0, 0]])\n        self.Q = process_var * np.eye(3)\n        self.R = np.array([[meas_var]])\n        self.x = np.zeros((3, 1)) # (pos, vel, acc)\n        self.P = np.eye(3)\n\n    def predict(self):\n        self.x = self.A @ self.x\n        self.P = self.A @ self.P @ self.A.T + self.Q\n\n    def update(self, z):\n        z = np.array([[z]])\n        y = z - self.H @ self.x\n        S = self.H @ self.P @ self.H.T + self.R\n        K = self.P @ self.H.T @ np.linalg.inv(S)\n        self.x = self.x + K @ y\n        self.P = (np.eye(3) - K @ self.H) @ self.P\n\n    def get_position(self):\n        return self.x[0, 0]","metadata":{"trusted":true,"execution":{"iopub.status.busy":"2025-06-06T07:08:00.803198Z","iopub.execute_input":"2025-06-06T07:08:00.803561Z","iopub.status.idle":"2025-06-06T07:08:00.810808Z","shell.execute_reply.started":"2025-06-06T07:08:00.803540Z","shell.execute_reply":"2025-06-06T07:08:00.809957Z"}},"outputs":[],"execution_count":null},{"cell_type":"code","source":"df = pd.read_csv('/kaggle/input/cmi-detect-behavior-with-sensor-data/train.csv')\nunique_seqs = df['sequence_id'].unique()","metadata":{"trusted":true,"execution":{"iopub.status.busy":"2025-06-06T07:08:00.811802Z","iopub.execute_input":"2025-06-06T07:08:00.812131Z","iopub.status.idle":"2025-06-06T07:08:35.901684Z","shell.execute_reply.started":"2025-06-06T07:08:00.812103Z","shell.execute_reply":"2025-06-06T07:08:35.901018Z"}},"outputs":[],"execution_count":null},{"cell_type":"code","source":"np.random.seed(42)\nsample_ids = np.random.choice(unique_seqs, size=9, replace=False)","metadata":{"trusted":true,"execution":{"iopub.status.busy":"2025-06-06T07:08:35.902416Z","iopub.execute_input":"2025-06-06T07:08:35.902616Z","iopub.status.idle":"2025-06-06T07:08:35.909008Z","shell.execute_reply.started":"2025-06-06T07:08:35.902599Z","shell.execute_reply":"2025-06-06T07:08:35.908122Z"}},"outputs":[],"execution_count":null},{"cell_type":"code","source":"fig = plt.figure(figsize=(18, 18))\nfor i, seq_id in enumerate(sample_ids):\n    df_seq = df[df['sequence_id'] == seq_id].reset_index(drop=True)\n    gesture = df_seq['gesture'].iloc[0]\n\n    acc = df_seq[['acc_x', 'acc_y', 'acc_z']].values\n    quat = df_seq[['rot_w', 'rot_x', 'rot_y', 'rot_z']].values\n\n    # bias = np.mean(acc[:50], axis=0)\n    # acc -= bias\n\n    rot = R.from_quat(quat[:, [1, 2, 3, 0]]) # (x, y, z, w)\n    acc_world = rot.apply(acc)\n\n    g = np.array([0, 0, 9.81])\n    acc_world -= g\n\n    dt = 1 / 50 # 50 hz\n\n    vel = cumtrapz(acc_world, dx=dt, initial=0, axis=0)\n    pos_raw = cumtrapz(vel, dx=dt, initial=0, axis=0)\n\n    kf_x = KalmanFilter1D(dt, process_var=1e-4, meas_var=1e-2)\n    kf_y = KalmanFilter1D(dt, process_var=1e-4, meas_var=1e-2)\n    kf_z = KalmanFilter1D(dt, process_var=1e-4, meas_var=1e-2)\n\n    pos_kf = []\n    for t in range(pos_raw.shape[0]):\n        kf_x.predict()\n        kf_y.predict()\n        kf_z.predict()\n\n        kf_x.update(pos_raw[t, 0])\n        kf_y.update(pos_raw[t, 1])\n        kf_z.update(pos_raw[t, 2])\n\n        pos_kf.append([kf_x.get_position(), kf_y.get_position(), kf_z.get_position()])\n\n    pos_kf = np.array(pos_kf)\n\n    ax = fig.add_subplot(3, 3, i + 1, projection='3d')\n    ax.plot(pos_raw[:, 0], pos_raw[:, 1], pos_raw[:, 2], color='blue', alpha=0.5, label='raw integration')\n    ax.plot(pos_kf[:, 0], pos_kf[:, 1], pos_kf[:, 2], color='red', label='kalman filtered')\n    ax.scatter(pos_raw[0, 0], pos_raw[0, 1], pos_raw[0, 2], color='green', label='start')\n    ax.scatter(pos_raw[-1, 0], pos_raw[-1, 1], pos_raw[-1, 2], color='blue')\n    ax.set_title(f'{seq_id=}, {gesture=}')\n    ax.set_xlabel('x')\n    ax.set_ylabel('y')\n    ax.set_zlabel('z')\n    ax.legend()\n\nplt.tight_layout()\nplt.show()","metadata":{"trusted":true,"execution":{"iopub.status.busy":"2025-06-06T07:11:47.906685Z","iopub.execute_input":"2025-06-06T07:11:47.907004Z","iopub.status.idle":"2025-06-06T07:11:50.465433Z","shell.execute_reply.started":"2025-06-06T07:11:47.906984Z","shell.execute_reply":"2025-06-06T07:11:50.464560Z"}},"outputs":[],"execution_count":null}]}