{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "8aff0fe3",
   "metadata": {},
   "source": [
    "# GPS-IMU State Estimation\n",
    "\n",
    "This notebook demonstrates sensor fusion with two estimators to illustrate the\n",
    "difference between `sync_mode=\"any\"` and `sync_mode=\"all\"`.\n",
    "\n",
    "## System Overview\n",
    "\n",
    "$$\n",
    "\\underbrace{\\text{GPS}_{1\\,\\text{Hz}}}_{\\text{position}} \\;\\;\\&\\;\\;\n",
    "\\underbrace{\\text{IMU}_{100\\,\\text{Hz}}}_{\\text{acceleration}}\n",
    "\\;\\xrightarrow{}\\;\n",
    "\\begin{cases}\n",
    "  \\text{Kalman Filter} & \\texttt{sync\\_mode=\\textquotedbl{}any\\textquotedbl{}} \\\\\n",
    "  \\text{ANN (stub)}    & \\texttt{sync\\_mode=\\textquotedbl{}all\\textquotedbl{}}\n",
    "\\end{cases}\n",
    "\\;\\xrightarrow{}\\; \\hat{x}_k\n",
    "$$\n",
    "\n",
    "| Node | Rate | Message | Field used | Noise |\n",
    "|------|------|---------|------------|-------|\n",
    "| GPS  | 1 Hz | `NavSatFix` | `latitude` = position | σ = 5 m |\n",
    "| IMU  | 100 Hz | `sensor_msgs/Imu` | `linear_acceleration.x` (index 7) | σ = 0.5 m/s² |\n",
    "\n",
    "**KF** (`sync_mode=\"any\"`) fires on every incoming message — ~101 Hz total.\n",
    "It predicts using IMU acceleration every tick and corrects using GPS when available.\n",
    "\n",
    "**ANN** (`sync_mode=\"all\"`) fires only when *both* `/gps` and `/imu` are fresh simultaneously.\n",
    "Since IMU (100 Hz, `stale_after=0.02 s`) is almost always fresh, the bottleneck is GPS\n",
    "(1 Hz, `stale_after=2.0 s`), so the ANN fires at ~1 Hz.\n",
    "\n",
    "The ANN uses pre-set random weights (untrained stub) — the output is meaningless,\n",
    "but the sync behaviour is what this notebook illustrates."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "35b023b8",
   "metadata": {
    "lines_to_next_cell": 2
   },
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "from dynamicalnodes import DynamicalSystem"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "cbcd6e4e",
   "metadata": {},
   "source": [
    "## Component Definitions\n",
    "\n",
    "Each component is a `DynamicalSystem(f=..., h=...)` where:\n",
    "- `f(x_k, ...)` → `x_{k+1}` updates the internal state\n",
    "- `h(x_k, ...)` → `y_k` produces the observable output\n",
    "\n",
    "The GPS and IMU simulator blocks are stateless — their output is a deterministic\n",
    "function of time `tk`. Noise is added externally in the simulation loop."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "bac8a3ec",
   "metadata": {
    "lines_to_next_cell": 2
   },
   "source": [
    "### GPS Simulator\n",
    "\n",
    "Stateless: returns a `NavSatFix`-compatible array `[lat=x, lon=0, alt=0]`\n",
    "from the true sinusoidal trajectory."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1d4eaf85",
   "metadata": {
    "lines_to_next_cell": 2
   },
   "outputs": [],
   "source": [
    "def gps_h(tk, A, omega):\n",
    "    x_true = A * np.sin(omega * tk)\n",
    "    return np.array([x_true, 0.0, 0.0])  # [lat=x, lon=0, alt=0]\n",
    "\n",
    "\n",
    "gps_block = DynamicalSystem(h=gps_h)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b1e6ab62",
   "metadata": {
    "lines_to_next_cell": 2
   },
   "source": [
    "### IMU Simulator\n",
    "\n",
    "Stateless: returns an `Imu`-compatible array with `linear_acceleration.x` set\n",
    "to the true acceleration (index 7 in the ros2py_imu layout).\n",
    "\n",
    "Layout: `[ori.x, ori.y, ori.z, ori.w, gyro.x, gyro.y, gyro.z, acc.x, acc.y, acc.z]`"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2662e79c",
   "metadata": {
    "lines_to_next_cell": 2
   },
   "outputs": [],
   "source": [
    "def imu_h(tk, A, omega):\n",
    "    ax = -A * omega**2 * np.sin(omega * tk)  # true acceleration\n",
    "    return np.array(\n",
    "        [\n",
    "            0.0,\n",
    "            0.0,\n",
    "            0.0,\n",
    "            1.0,  # orientation (unit quaternion)\n",
    "            0.0,\n",
    "            0.0,\n",
    "            0.0,  # angular velocity\n",
    "            ax,\n",
    "            0.0,\n",
    "            0.0,\n",
    "        ]\n",
    "    )  # linear acceleration\n",
    "\n",
    "\n",
    "imu_block = DynamicalSystem(h=imu_h)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "0933551c",
   "metadata": {
    "lines_to_next_cell": 2
   },
   "source": [
    "### Kalman Filter\n",
    "\n",
    "State: $z_k = (\\hat{x}_k,\\, P_k)$ — position/velocity estimate and covariance.\n",
    "\n",
    "IMU acceleration drives the **predict** step on every tick.\n",
    "GPS position triggers a **measurement update** only when `gps is not None`.\n",
    "Passing `gps=None` (on non-GPS ticks) skips the update — only predicting.\n",
    "\n",
    "$$\n",
    "\\hat{x}_{k|k-1} = F_k \\hat{x}_{k-1} + B_k a_x, \\qquad\n",
    "P_{k|k-1} = F_k P_{k-1} F_k^\\top + Q_k\n",
    "$$\n",
    "\n",
    "$$\n",
    "K_k = P_{k|k-1} H^\\top S^{-1}, \\quad\n",
    "\\hat{x}_k = \\hat{x}_{k|k-1} + K_k(y_k - H \\hat{x}_{k|k-1}), \\quad\n",
    "P_k = (I - K_k H) P_{k|k-1}\n",
    "$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f6291a09",
   "metadata": {},
   "outputs": [],
   "source": [
    "def kf_f(zk, imu, Fk, Bk, Hk, Qk, Rk, gps=None):\n",
    "    \"\"\"Predict always; update only when GPS is available.\"\"\"\n",
    "    x, P = zk\n",
    "    ax = float(imu[7])  # linear_acceleration.x is at index 7\n",
    "    x_pred = Fk @ x + Bk * ax\n",
    "    P_pred = Fk @ P @ Fk.T + Qk\n",
    "    if gps is not None:\n",
    "        y = np.array([float(gps[0])])  # latitude = x position\n",
    "        S = Hk @ P_pred @ Hk.T + Rk\n",
    "        K = P_pred @ Hk.T @ np.linalg.inv(S)\n",
    "        x_upd = x_pred + K @ (y - Hk @ x_pred)\n",
    "        P_upd = (np.eye(2) - K @ Hk) @ P_pred\n",
    "    else:\n",
    "        x_upd, P_upd = x_pred, P_pred\n",
    "    return (x_upd, P_upd)\n",
    "\n",
    "\n",
    "def kf_h(zk):\n",
    "    x, P = zk\n",
    "    return {\"x_est\": float(x[0]), \"P00\": float(P[0, 0])}  # position + position variance\n",
    "\n",
    "\n",
    "kf_block = DynamicalSystem(f=kf_f, h=kf_h)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "719bc2f2",
   "metadata": {},
   "source": [
    "### ANN State Estimator (stub)\n",
    "\n",
    "Stateless two-layer feedforward network:\n",
    "$[x_\\text{gps},\\, a_x^\\text{imu}] \\xrightarrow{W_1, b_1} \\tanh \\xrightarrow{W_2, b_2} [\\hat{x},\\, \\hat{v}_x]$\n",
    "\n",
    "Weights are random (untrained). The output is not meaningful — this block exists to\n",
    "demonstrate `sync_mode=\"all\"`: the node fires only when **both** `/gps` and `/imu` are\n",
    "fresh, so it runs at GPS rate (1 Hz) regardless of IMU rate."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b915ccad",
   "metadata": {},
   "outputs": [],
   "source": [
    "rng_ann = np.random.default_rng(seed=0)\n",
    "n_hidden = 8\n",
    "W1 = rng_ann.standard_normal((n_hidden, 2)) * 0.1\n",
    "b1 = np.zeros(n_hidden)\n",
    "W2 = rng_ann.standard_normal((2, n_hidden)) * 0.1\n",
    "b2 = np.zeros(2)\n",
    "\n",
    "\n",
    "def ann_h(gps, imu, W1, b1, W2, b2):\n",
    "    inp = np.array([float(gps[0]), float(imu[7])])  # [x_gps, ax_imu]\n",
    "    return W2 @ np.tanh(W1 @ inp + b1) + b2  # [x_est, vx_est]\n",
    "\n",
    "\n",
    "ann_block = DynamicalSystem(h=ann_h)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "61f0eba4",
   "metadata": {},
   "source": [
    "## Parameters"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "fa31652b",
   "metadata": {},
   "outputs": [],
   "source": [
    "# ── Simulation ────────────────────────────────────────────────────────────────\n",
    "dt = 0.01  # 100 Hz — matches IMU rate\n",
    "T_sim = 120.0  # seconds (2 full sinusoid periods)\n",
    "sim_time = np.arange(0, T_sim, dt)\n",
    "gps_stride = int(1.0 / dt)  # 100: GPS fires every 100 IMU ticks\n",
    "\n",
    "# ── True trajectory: sinusoidal position ─────────────────────────────────────\n",
    "A = 50.0  # amplitude, m\n",
    "omega = 2 * np.pi / 60.0  # rad/s — period = 60 s\n",
    "\n",
    "# ── Noise ────────────────────────────────────────────────────────────────────\n",
    "sigma_gps = 5.0  # GPS position noise std, m\n",
    "sigma_imu = 0.5  # IMU acceleration noise std, m/s²\n",
    "\n",
    "# ── Kalman filter matrices (tuned for dt = 0.01 s) ───────────────────────────\n",
    "Fk = np.array([[1.0, dt], [0.0, 1.0]])  # constant-velocity process model\n",
    "Bk = np.array([0.5 * dt**2, dt])  # acceleration → [Δx, Δv]\n",
    "Hk = np.array([[1.0, 0.0]])  # GPS observes position only\n",
    "Qk = np.array([[1e-4, 0.0], [0.0, 1e-2]])  # process noise (tuned for dt = 0.01)\n",
    "Rk = np.array([[sigma_gps**2]])  # GPS measurement noise variance"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f3008d96",
   "metadata": {},
   "source": [
    "## Simulation Loop — Figure 1\n",
    "\n",
    "The KF runs at 100 Hz (every IMU tick), predicting with `ax` and updating\n",
    "only on the 1 Hz GPS ticks. The ANN runs only on GPS ticks to mirror the\n",
    "`sync_mode=\"all\"` behaviour it will have in the deployed node."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4e0ad805",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Initial States\n",
    "zk = (np.zeros(2), np.eye(2) * 100.0)  # KF: large initial covariance\n",
    "\n",
    "true_out, kf_out, kf_P_out, ann_out = [], [], [], []\n",
    "gps_meas_out = []  # (tk, x_gps) for scatter\n",
    "\n",
    "for i, tk in enumerate(sim_time):\n",
    "    x_true = A * np.sin(omega * tk)\n",
    "    ax_true = -A * omega**2 * np.sin(omega * tk)\n",
    "    true_out.append(x_true)\n",
    "\n",
    "    # ── IMU (100 Hz) ─────────────────────────────────────────────────────\n",
    "    imu_meas = np.array(\n",
    "        [\n",
    "            0.0,\n",
    "            0.0,\n",
    "            0.0,\n",
    "            1.0,\n",
    "            0.0,\n",
    "            0.0,\n",
    "            0.0,\n",
    "            ax_true + np.random.normal(0.0, sigma_imu),\n",
    "            0.0,\n",
    "            0.0,\n",
    "        ]\n",
    "    )\n",
    "\n",
    "    # ── GPS (1 Hz) ───────────────────────────────────────────────────────\n",
    "    gps_meas = None\n",
    "    if i % gps_stride == 0:\n",
    "        x_gps = x_true + np.random.normal(0.0, sigma_gps)\n",
    "        gps_meas = np.array([x_gps, 0.0, 0.0])\n",
    "        gps_meas_out.append((tk, x_gps))\n",
    "\n",
    "    # ── Kalman Filter ────────────────────────────────────────────────────\n",
    "    zk_next, kf_yk = kf_block.step(\n",
    "        zk=zk, gps=gps_meas, imu=imu_meas, Fk=Fk, Bk=Bk, Hk=Hk, Qk=Qk, Rk=Rk\n",
    "    )\n",
    "    kf_out.append(kf_yk[\"x_est\"])\n",
    "    kf_P_out.append(kf_yk[\"P00\"])\n",
    "    zk = zk_next\n",
    "\n",
    "    # ── ANN (GPS ticks only — mimics sync_mode=\"all\") ────────────────────\n",
    "    if gps_meas is not None:\n",
    "        est = ann_block.step(gps=gps_meas, imu=imu_meas, W1=W1, b1=b1, W2=W2, b2=b2)\n",
    "        ann_out.append((tk, float(est[0])))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "59c841ad",
   "metadata": {},
   "outputs": [],
   "source": [
    "gps_tks, gps_xs = zip(*gps_meas_out) if gps_meas_out else ([], [])\n",
    "ann_tks, ann_xs = zip(*ann_out) if ann_out else ([], [])\n",
    "\n",
    "fig1, axs1 = plt.subplots(3, 1, sharex=True, figsize=(11, 10))\n",
    "\n",
    "# ── KF position estimate ─────────────────────────────────────────────────────\n",
    "axs1[0].plot(sim_time, true_out, \"k--\", alpha=0.35, label=\"True $x(t)$\")\n",
    "axs1[0].plot(\n",
    "    sim_time, kf_out, color=\"steelblue\", label=\"KF estimate (100 Hz, sync_mode='any')\"\n",
    ")\n",
    "axs1[0].scatter(\n",
    "    gps_tks, gps_xs, s=14, color=\"orange\", zorder=3, label=\"GPS measurement (1 Hz)\"\n",
    ")\n",
    "axs1[0].set_ylabel(\"Position (m)\")\n",
    "axs1[0].set_title(\n",
    "    \"Kalman Filter — sync_mode='any'  \"\n",
    "    \"(predict every IMU tick; GPS update when available)\"\n",
    ")\n",
    "axs1[0].legend(fontsize=8)\n",
    "\n",
    "# ── KF covariance P[0,0] ─────────────────────────────────────────────────────\n",
    "axs1[1].plot(sim_time, kf_P_out, color=\"steelblue\", alpha=0.8)\n",
    "axs1[1].scatter(\n",
    "    gps_tks,\n",
    "    [0] * len(gps_tks),\n",
    "    s=20,\n",
    "    color=\"orange\",\n",
    "    zorder=3,\n",
    "    label=\"GPS update (P drops)\",\n",
    ")\n",
    "axs1[1].set_ylabel(\"P[0,0] — position variance (m²)\")\n",
    "axs1[1].set_title(\n",
    "    \"KF Position Variance — drops on GPS update, grows during predict-only\"\n",
    ")\n",
    "axs1[1].set_ylim(bottom=0)\n",
    "axs1[1].legend(fontsize=8)\n",
    "\n",
    "# ── ANN estimate ─────────────────────────────────────────────────────────────\n",
    "axs1[2].plot(sim_time, true_out, \"k--\", alpha=0.35, label=\"True $x(t)$\")\n",
    "axs1[2].scatter(\n",
    "    ann_tks,\n",
    "    ann_xs,\n",
    "    s=22,\n",
    "    color=\"orchid\",\n",
    "    zorder=3,\n",
    "    label=\"ANN estimate (1 Hz, sync_mode='all', untrained)\",\n",
    ")\n",
    "axs1[2].scatter(\n",
    "    gps_tks, gps_xs, s=14, color=\"orange\", zorder=2, label=\"GPS measurement (1 Hz)\"\n",
    ")\n",
    "axs1[2].set_ylabel(\"Position (m)\")\n",
    "axs1[2].set_xlabel(\"Time (s)\")\n",
    "axs1[2].set_title(\n",
    "    \"ANN Estimator — sync_mode='all'  \"\n",
    "    \"(fires only when BOTH GPS and IMU are fresh → 1 Hz)\"\n",
    ")\n",
    "axs1[2].legend(fontsize=8)\n",
    "\n",
    "plt.tight_layout()\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "3934f612",
   "metadata": {},
   "source": [
    "## ROSNode Simulation — Converter Verification\n",
    "\n",
    "This section re-runs the same loop through the `ROSNode` layer to confirm that\n",
    "the `NavSatFix` and `Imu` message converters and the `DynamicalSystem` wiring\n",
    "are correct before generating the deployment nodes.\n",
    "\n",
    "Noise is added to the ROS messages directly (modifying `gps_ros.latitude` and\n",
    "`imu_ros.linear_acceleration.x`) to mirror the per-field injection that will\n",
    "happen in hardware.\n",
    "\n",
    "The output should closely match Figure 1 (differences are from the independent\n",
    "RNG state)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "184b474d",
   "metadata": {},
   "outputs": [],
   "source": [
    "from dynamicalnodes import ROSNode\n",
    "from sensor_msgs.msg import NavSatFix, Imu\n",
    "from std_msgs.msg import Float64, Float64MultiArray\n",
    "from dynamicalnodes.ros2py_py2ros import (\n",
    "    ros2py_nav_sat_fix,\n",
    "    py2ros_nav_sat_fix,\n",
    "    ros2py_imu,\n",
    "    py2ros_imu,\n",
    "    ros2py_float64,\n",
    "    py2ros_float64,\n",
    "    ros2py_float64_multiarray,\n",
    "    py2ros_float64_multiarray,\n",
    ")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "deb44563",
   "metadata": {},
   "outputs": [],
   "source": [
    "gps_node = ROSNode(\n",
    "    dynamical_system=gps_block,\n",
    "    publishes_to=[\n",
    "        {\"topic\": \"/gps\", \"msg_type\": NavSatFix, \"py2ros\": py2ros_nav_sat_fix}\n",
    "    ],\n",
    "    timer_hz=1,\n",
    ")\n",
    "\n",
    "imu_node = ROSNode(\n",
    "    dynamical_system=imu_block,\n",
    "    publishes_to=[{\"topic\": \"/imu\", \"msg_type\": Imu, \"py2ros\": py2ros_imu}],\n",
    "    timer_hz=100,\n",
    ")\n",
    "\n",
    "# ── Kalman Filter ─────────────────────────────────────────────────────────────\n",
    "# sync_mode=\"any\": fires whenever any subscription receives a fresh message.\n",
    "# At runtime: ~101 Hz (IMU at 100 Hz + GPS at 1 Hz).\n",
    "# Two publishers: position estimate on /x_est_kf, covariance P[0,0] on /P_kf.\n",
    "kf_node = ROSNode(\n",
    "    dynamical_system=kf_block,\n",
    "    subscribes_to=[\n",
    "        {\n",
    "            \"topic\": \"/gps\",\n",
    "            \"msg_type\": NavSatFix,\n",
    "            \"arg\": \"gps\",\n",
    "            \"ros2py\": ros2py_nav_sat_fix,\n",
    "            \"stale_after\": 2.0,\n",
    "            \"buffer_size\": 5,\n",
    "        },\n",
    "        {\n",
    "            \"topic\": \"/imu\",\n",
    "            \"msg_type\": Imu,\n",
    "            \"arg\": \"imu\",\n",
    "            \"ros2py\": ros2py_imu,\n",
    "            \"stale_after\": 0.02,\n",
    "            \"buffer_size\": 20,\n",
    "        },\n",
    "    ],\n",
    "    publishes_to=[\n",
    "        {\n",
    "            \"topic\": \"/x_est_kf\",\n",
    "            \"msg_type\": Float64,\n",
    "            \"py2ros\": py2ros_float64,\n",
    "            \"key\": \"x_est\",\n",
    "        },\n",
    "        {\"topic\": \"/P_kf\", \"msg_type\": Float64, \"py2ros\": py2ros_float64, \"key\": \"P00\"},\n",
    "    ],\n",
    "    sync_mode=\"any\",\n",
    "    state_name=\"zk\",\n",
    ")\n",
    "\n",
    "# ── ANN Estimator ─────────────────────────────────────────────────────────────\n",
    "# sync_mode=\"all\": fires only when BOTH /gps and /imu are within stale_after.\n",
    "# IMU (100 Hz, stale_after=0.02 s) is almost always fresh.\n",
    "# GPS (1 Hz, stale_after=2.0 s) is the bottleneck → ANN fires at ~1 Hz.\n",
    "ann_node = ROSNode(\n",
    "    dynamical_system=ann_block,\n",
    "    subscribes_to=[\n",
    "        {\n",
    "            \"topic\": \"/gps\",\n",
    "            \"msg_type\": NavSatFix,\n",
    "            \"arg\": \"gps\",\n",
    "            \"ros2py\": ros2py_nav_sat_fix,\n",
    "            \"stale_after\": 2.0,\n",
    "            \"buffer_size\": 5,\n",
    "        },\n",
    "        {\n",
    "            \"topic\": \"/imu\",\n",
    "            \"msg_type\": Imu,\n",
    "            \"arg\": \"imu\",\n",
    "            \"ros2py\": ros2py_imu,\n",
    "            \"stale_after\": 0.02,\n",
    "            \"buffer_size\": 20,\n",
    "        },\n",
    "    ],\n",
    "    publishes_to=[\n",
    "        {\n",
    "            \"topic\": \"/x_est_ann\",\n",
    "            \"msg_type\": Float64MultiArray,\n",
    "            \"py2ros\": py2ros_float64_multiarray,\n",
    "        }\n",
    "    ],\n",
    "    sync_mode=\"all\",\n",
    ")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e75f31d4",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Initial States\n",
    "kf_node._state = (np.zeros(2), np.eye(2) * 100.0)  # KF: (x_est, P)\n",
    "\n",
    "kf_node_out, kf_P_node_out, ann_node_out = [], [], []\n",
    "gps_node_out = []  # (tk, x_gps) for scatter\n",
    "\n",
    "for i, tk in enumerate(sim_time):\n",
    "\n",
    "    # ── IMU (100 Hz) ─────────────────────────────────────────────────────\n",
    "    imu_ros = imu_node.step(tk=tk, A=A, omega=omega)\n",
    "    imu_ros.linear_acceleration.x += np.random.normal(0.0, sigma_imu)\n",
    "\n",
    "    # ── GPS (1 Hz) ───────────────────────────────────────────────────────\n",
    "    gps_ros = None\n",
    "    if i % gps_stride == 0:\n",
    "        gps_ros = gps_node.step(tk=tk, A=A, omega=omega)\n",
    "        gps_ros.latitude += np.random.normal(0.0, sigma_gps)\n",
    "        gps_node_out.append((tk, gps_ros.latitude))\n",
    "\n",
    "        # ANN (sync_mode=\"all\" — both GPS and IMU fresh)\n",
    "        ann_ros = ann_node.step(gps=gps_ros, imu=imu_ros, W1=W1, b1=b1, W2=W2, b2=b2)\n",
    "        ann_node_out.append((tk, float(ann_ros.data[0])))\n",
    "\n",
    "    # ── KF (sync_mode=\"any\" — IMU always, GPS when available) ────────────\n",
    "    kf_ros = kf_node.step(gps=gps_ros, imu=imu_ros, Fk=Fk, Bk=Bk, Hk=Hk, Qk=Qk, Rk=Rk)\n",
    "    kf_node_out.append(float(kf_ros[\"/x_est_kf\"].data))\n",
    "    kf_P_node_out.append(float(kf_ros[\"/P_kf\"].data))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0de4a3d4",
   "metadata": {},
   "outputs": [],
   "source": [
    "gps_n_tks, gps_n_xs = zip(*gps_node_out) if gps_node_out else ([], [])\n",
    "ann_n_tks, ann_n_xs = zip(*ann_node_out) if ann_node_out else ([], [])\n",
    "\n",
    "fig2, axs2 = plt.subplots(3, 1, sharex=True, figsize=(11, 10))\n",
    "\n",
    "# ── KF position ──────────────────────────────────────────────────────────────\n",
    "axs2[0].plot(\n",
    "    sim_time,\n",
    "    kf_out,\n",
    "    color=\"steelblue\",\n",
    "    linestyle=\"--\",\n",
    "    alpha=0.45,\n",
    "    label=\"DynamicalSystem\",\n",
    ")\n",
    "axs2[0].plot(\n",
    "    sim_time, kf_node_out, color=\"steelblue\", label=\"ROSNode (converter verification)\"\n",
    ")\n",
    "axs2[0].scatter(\n",
    "    gps_n_tks, gps_n_xs, s=14, color=\"orange\", zorder=3, label=\"GPS measurement (1 Hz)\"\n",
    ")\n",
    "axs2[0].plot(sim_time, true_out, \"k--\", alpha=0.25, label=\"True $x(t)$\")\n",
    "axs2[0].set_ylabel(\"Position (m)\")\n",
    "axs2[0].set_title(\"KF Position — ROSNode vs DynamicalSystem\")\n",
    "axs2[0].legend(fontsize=8)\n",
    "\n",
    "# ── KF covariance ────────────────────────────────────────────────────────────\n",
    "axs2[1].plot(\n",
    "    sim_time,\n",
    "    kf_P_out,\n",
    "    color=\"steelblue\",\n",
    "    linestyle=\"--\",\n",
    "    alpha=0.45,\n",
    "    label=\"DynamicalSystem\",\n",
    ")\n",
    "axs2[1].plot(\n",
    "    sim_time, kf_P_node_out, color=\"steelblue\", label=\"ROSNode (converter verification)\"\n",
    ")\n",
    "axs2[1].scatter(\n",
    "    gps_n_tks,\n",
    "    [0] * len(gps_n_tks),\n",
    "    s=20,\n",
    "    color=\"orange\",\n",
    "    zorder=3,\n",
    "    label=\"GPS update (P drops)\",\n",
    ")\n",
    "axs2[1].set_ylabel(\"P[0,0] — position variance (m²)\")\n",
    "axs2[1].set_title(\"KF Position Variance — ROSNode vs DynamicalSystem\")\n",
    "axs2[1].set_ylim(bottom=0)\n",
    "axs2[1].legend(fontsize=8)\n",
    "\n",
    "# ── ANN ──────────────────────────────────────────────────────────────────────\n",
    "axs2[2].scatter(\n",
    "    [t for t, _ in ann_out],\n",
    "    [x for _, x in ann_out],\n",
    "    s=22,\n",
    "    color=\"orchid\",\n",
    "    alpha=0.4,\n",
    "    label=\"DynamicalSystem\",\n",
    ")\n",
    "axs2[2].scatter(\n",
    "    ann_n_tks, ann_n_xs, s=22, color=\"orchid\", label=\"ROSNode (converter verification)\"\n",
    ")\n",
    "axs2[2].scatter(\n",
    "    gps_n_tks, gps_n_xs, s=14, color=\"orange\", zorder=3, label=\"GPS measurement (1 Hz)\"\n",
    ")\n",
    "axs2[2].plot(sim_time, true_out, \"k--\", alpha=0.25, label=\"True $x(t)$\")\n",
    "axs2[2].set_ylabel(\"Position (m)\")\n",
    "axs2[2].set_xlabel(\"Time (s)\")\n",
    "axs2[2].set_title(\"ANN — ROSNode vs DynamicalSystem\")\n",
    "axs2[2].legend(fontsize=8)\n",
    "\n",
    "plt.tight_layout()\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "ae884763",
   "metadata": {},
   "source": [
    "## Deploying to ROS2\n",
    "\n",
    "`write_ROSNode_to_rclpy()` generates a self-contained rclpy Python file for each node.\n",
    "\n",
    "The KF and ANN matrices are `static_params` — baked into the generated file at write time.\n",
    "The KF matrices were already computed for `dt = 0.01 s` (100 Hz IMU rate), matching the\n",
    "deployed node's effective predict rate.\n",
    "\n",
    "| Parameter category | Used for | Examples |\n",
    "|---|---|---|\n",
    "| `static_params` | Constants baked in at generation — never change | `A`, `omega`, `Fk`, `W1` |\n",
    "| `pub_noise` | Gaussian noise injected before publishing — for simulation testing | `{\"/gps\": sigma_gps}` |\n",
    "\n",
    "**Note on IMU noise**: `pub_noise` is not used for the IMU node because it adds the\n",
    "same standard deviation to *all 10 array elements*, which would corrupt the orientation\n",
    "quaternion fields. In hardware deployment the IMU driver provides real sensor noise\n",
    "inherently. For simulation, inject noise selectively in `imu_h` (make `sigma_imu` a\n",
    "static param) rather than using `pub_noise`."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7ac2a0ee",
   "metadata": {},
   "outputs": [],
   "source": [
    "import os\n",
    "\n",
    "os.makedirs(\"./ros_nodes\", exist_ok=True)\n",
    "\n",
    "gps_node.write_ROSNode_to_rclpy(\n",
    "    \"./ros_nodes/gps_node.py\",\n",
    "    node_name=\"gps\",\n",
    "    static_params={\"A\": A, \"omega\": omega},\n",
    "    pub_noise={\"/gps\": sigma_gps},  # adds N(0, sigma_gps) to [lat, lon, alt]\n",
    ")\n",
    "imu_node.write_ROSNode_to_rclpy(\n",
    "    \"./ros_nodes/imu_node.py\",\n",
    "    node_name=\"imu\",\n",
    "    static_params={\"A\": A, \"omega\": omega},\n",
    ")\n",
    "kf_node.write_ROSNode_to_rclpy(\n",
    "    \"./ros_nodes/kf_estimator_node.py\",\n",
    "    node_name=\"kf_estimator\",\n",
    "    initial_state=(np.zeros(2), np.eye(2) * 100.0),\n",
    "    static_params={\"Fk\": Fk, \"Bk\": Bk, \"Hk\": Hk, \"Qk\": Qk, \"Rk\": Rk},\n",
    "    # Publishes /x_est_kf (Float64) and /P_kf (Float64) on every step.\n",
    "    # In PlotJuggler: plot /P_kf/data to see variance drop on each GPS update.\n",
    ")\n",
    "ann_node.write_ROSNode_to_rclpy(\n",
    "    \"./ros_nodes/ann_estimator_node.py\",\n",
    "    node_name=\"ann_estimator\",\n",
    "    static_params={\"W1\": W1, \"b1\": b1, \"W2\": W2, \"b2\": b2},\n",
    ")\n",
    "print(\"Written: gps_node.py  imu_node.py  kf_estimator_node.py  ann_estimator_node.py\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "ee31d7ce",
   "metadata": {},
   "source": [
    "### Running the nodes\n",
    "\n",
    "```bash\n",
    "# Terminal 1 — GPS simulator (1 Hz)\n",
    "python3 ros_nodes/gps_node.py\n",
    "\n",
    "# Terminal 2 — IMU simulator (100 Hz)\n",
    "python3 ros_nodes/imu_node.py\n",
    "\n",
    "# Terminal 3 — KF estimator (sync_mode=any, ~101 Hz)\n",
    "python3 ros_nodes/kf_estimator_node.py\n",
    "\n",
    "# Terminal 4 — ANN estimator (sync_mode=all, ~1 Hz)\n",
    "python3 ros_nodes/ann_estimator_node.py\n",
    "```\n",
    "\n",
    "**Inspect sync_mode difference at runtime:**\n",
    "\n",
    "```bash\n",
    "# KF publishes at ~101 Hz — you will see ~101 messages/sec\n",
    "ros2 topic hz /x_est_kf\n",
    "\n",
    "# ANN publishes at ~1 Hz — you will see ~1 message/sec\n",
    "ros2 topic hz /x_est_ann\n",
    "\n",
    "# Kill the GPS node — ANN stops immediately; KF keeps predicting from IMU\n",
    "# (restart GPS node to resume both)\n",
    "```\n",
    "\n",
    "**Tune KF noise matrices without restarting** (promote `Qk`/`Rk` to `dynamic_params`):\n",
    "\n",
    "```bash\n",
    "ros2 param set /kf_estimator Qk \"[1e-4, 0.0, 0.0, 1e-2]\"\n",
    "```"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "orphan": true
 },
 "nbformat": 4,
 "nbformat_minor": 5
}