{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Kalman Filters: From Fundamentals to Advanced Filtering\n",
    "\n",
    "This notebook provides a comprehensive, hands-on introduction to Kalman filtering using PyTCL (Python Tracker Component Library). \n",
    "\n",
    "## What You'll Learn\n",
    "\n",
    "1. **Linear Kalman Filter (KF)** - Optimal recursive estimation for linear systems\n",
    "2. **Extended Kalman Filter (EKF)** - First-order nonlinear approximation using Jacobians\n",
    "3. **Unscented Kalman Filter (UKF)** - Deterministic sigma-point methods for accurate nonlinear handling\n",
    "4. **Square-Root Filters** - Numerically stable variants that maintain positive definiteness\n",
    "5. **Interactive Parameter Tuning** - Explore how Q and R affect filter performance\n",
    "\n",
    "## Key Concepts You'll Master\n",
    "\n",
    "- **Predict-Update cycle**: How filters recursively estimate state\n",
    "- **Covariance matrices**: What P, Q, R represent (uncertainty)\n",
    "- **Belief propagation**: Combining prior knowledge with new measurements\n",
    "- **Filter divergence**: When and why filters fail\n",
    "- **Real-world tradeoffs**: Accuracy vs computational cost\n",
    "\n",
    "## Prerequisites\n",
    "\n",
    "```bash\n",
    "pip install nrl-tracker matplotlib numpy scipy\n",
    "```\n",
    "\n",
    "## Estimated Time: 30-40 minutes\n",
    "\n",
    "Try running each section sequentially, modify parameters to experiment, and attempt the exercises at the end!"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "✓ PyTCL Kalman filter modules and Plotly imported successfully\n"
     ]
    }
   ],
   "source": [
    "import numpy as np\n",
    "import plotly.graph_objects as go\n",
    "from plotly.subplots import make_subplots\n",
    "import warnings\n",
    "warnings.filterwarnings('ignore')\n",
    "\n",
    "# Import PyTCL Kalman filter components\n",
    "from pytcl.dynamic_estimation.kalman import (\n",
    "    kf_predict, kf_update,\n",
    "    ekf_predict, ekf_update,\n",
    "    ukf_predict, ukf_update\n",
    ")\n",
    "\n",
    "# Import square-root filter functions\n",
    "from pytcl.dynamic_estimation.kalman.square_root import (\n",
    "    srkf_predict, srkf_update\n",
    ")\n",
    "\n",
    "np.random.seed(42)\n",
    "\n",
    "# Plotly dark theme template\n",
    "dark_template = go.layout.Template()\n",
    "dark_template.layout = go.Layout(\n",
    "    paper_bgcolor='#0d1117',\n",
    "    plot_bgcolor='#0d1117',\n",
    "    font=dict(color='#e6edf3'),\n",
    "    xaxis=dict(gridcolor='#30363d', zerolinecolor='#30363d'),\n",
    "    yaxis=dict(gridcolor='#30363d', zerolinecolor='#30363d'),\n",
    ")\n",
    "\n",
    "print(\"✓ PyTCL Kalman filter modules and Plotly imported successfully\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## The Kalman Filter: Core Concepts\n",
    "\n",
    "### Why Do We Need the Kalman Filter?\n",
    "\n",
    "In the real world, we have:\n",
    "- **Imperfect measurements** - GPS has noise, sensors drift\n",
    "- **Incomplete observations** - We can't measure everything\n",
    "- **Dynamic systems** - Things change over time\n",
    "\n",
    "The Kalman filter **optimally combines** prior predictions with new measurements to estimate the true state.\n",
    "\n",
    "### The Two-Step Recursive Algorithm\n",
    "\n",
    "The Kalman filter alternates between:\n",
    "\n",
    "**1. PREDICT** - Where do we expect the system to be based on physics?\n",
    "- Uses a process model: $\\hat{x}^-_k = F \\hat{x}_{k-1}$\n",
    "- Tracks uncertainty growing over time: $P^-_k = F P_{k-1} F^T + Q$\n",
    "\n",
    "**2. UPDATE** - Now incorporate the new measurement, what do we believe?\n",
    "- Compute Kalman gain: $K_k = P^-_k H^T (H P^-_k H^T + R)^{-1}$\n",
    "- Blend prediction with measurement: $\\hat{x}_k = \\hat{x}^-_k + K_k (z_k - H\\hat{x}^-_k)$\n",
    "- Reduce uncertainty: $P_k = (I - K_k H) P^-_k$\n",
    "\n",
    "### Key Parameters to Tune\n",
    "\n",
    "| Parameter | Meaning | Effect |\n",
    "|-----------|---------|--------|\n",
    "| **Q** | Process noise covariance | How much does the model lie? |\n",
    "| **R** | Measurement noise covariance | How much do measurements lie? |\n",
    "| **F** | State transition matrix | Physics of the system |\n",
    "| **H** | Observation matrix | What can we measure? |\n",
    "\n",
    "**Rule of thumb:** The Kalman filter balances trust in the model (Q) vs trust in measurements (R)."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 1. Linear Kalman Filter\n",
    "\n",
    "The Kalman filter is the optimal estimator for linear Gaussian systems. It consists of two steps:\n",
    "\n",
    "**Prediction:**\n",
    "$$\\hat{x}_{k|k-1} = F \\hat{x}_{k-1|k-1}$$\n",
    "$$P_{k|k-1} = F P_{k-1|k-1} F^T + Q$$\n",
    "\n",
    "**Update:**\n",
    "$$K = P_{k|k-1} H^T (H P_{k|k-1} H^T + R)^{-1}$$\n",
    "$$\\hat{x}_{k|k} = \\hat{x}_{k|k-1} + K(z - H\\hat{x}_{k|k-1})$$\n",
    "$$P_{k|k} = (I - KH) P_{k|k-1}$$\n",
    "\n",
    "### Example: Tracking a Moving Object"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "State dimension: 4 (position + velocity in 2D)\n",
      "Measurement dimension: 2 (position only)\n",
      "Process noise (Q) magnitude: 0.020\n",
      "Measurement noise (R) magnitude: 2.000\n"
     ]
    }
   ],
   "source": [
    "# System parameters\n",
    "dt = 0.1  # Time step [seconds]\n",
    "n_steps = 100  # Number of time steps\n",
    "\n",
    "# STATE DEFINITION: [x, vx, y, vy]\n",
    "# x, y: position in meters\n",
    "# vx, vy: velocity components in m/s\n",
    "# This is a 4-dimensional state space (constant velocity model)\n",
    "\n",
    "# STATE TRANSITION MATRIX: x_{k+1} = F @ x_k\n",
    "# Models constant velocity: new_pos = old_pos + velocity * dt\n",
    "F = np.array([\n",
    "    [1, dt, 0, 0],      # x_{k+1} = x_k + vx_k * dt\n",
    "    [0, 1, 0, 0],       # vx_{k+1} = vx_k (no acceleration)\n",
    "    [0, 0, 1, dt],      # y_{k+1} = y_k + vy_k * dt\n",
    "    [0, 0, 0, 1]        # vy_{k+1} = vy_k\n",
    "])\n",
    "\n",
    "# PROCESS NOISE COVARIANCE: Q\n",
    "# Accounts for model uncertainty (e.g., unmodeled accelerations)\n",
    "# Sets diagonal to larger values for more process uncertainty\n",
    "q = 0.1  # Process noise intensity (tune this!)\n",
    "Q = q * np.array([\n",
    "    [dt**3/3, dt**2/2, 0, 0],      # Higher power for position uncertainty\n",
    "    [dt**2/2, dt, 0, 0],\n",
    "    [0, 0, dt**3/3, dt**2/2],\n",
    "    [0, 0, dt**2/2, dt]\n",
    "])\n",
    "\n",
    "# MEASUREMENT MATRIX: z_k = H @ x_k\n",
    "# We observe position (x, y) but NOT velocity\n",
    "H = np.array([\n",
    "    [1, 0, 0, 0],   # Measure x position\n",
    "    [0, 0, 1, 0]    # Measure y position\n",
    "])\n",
    "\n",
    "# MEASUREMENT NOISE COVARIANCE: R\n",
    "# Sensor uncertainty (GPS noise, radar error, etc.)\n",
    "# Diagonal elements are variance of measurement error\n",
    "R = np.eye(2) * 1.0  # 1 meter measurement standard deviation\n",
    "\n",
    "# Print diagnostics\n",
    "print(f\"State dimension: {F.shape[0]} (position + velocity in 2D)\")\n",
    "print(f\"Measurement dimension: {H.shape[0]} (position only)\")\n",
    "print(f\"Process noise (Q) magnitude: {np.trace(Q):.3f}\")\n",
    "print(f\"Measurement noise (R) magnitude: {np.trace(R):.3f}\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Generated 100 measurements\n"
     ]
    }
   ],
   "source": [
    "# Generate ground truth trajectory\n",
    "true_state = np.array([0.0, 1.0, 0.0, 0.5])  # Start at origin, moving diagonally\n",
    "true_states = [true_state.copy()]\n",
    "measurements = []\n",
    "\n",
    "for _ in range(n_steps):\n",
    "    # Propagate true state\n",
    "    true_state = F @ true_state + np.random.multivariate_normal(np.zeros(4), Q)\n",
    "    true_states.append(true_state.copy())\n",
    "    \n",
    "    # Generate measurement\n",
    "    z = H @ true_state + np.random.multivariate_normal(np.zeros(2), R)\n",
    "    measurements.append(z)\n",
    "\n",
    "true_states = np.array(true_states)\n",
    "measurements = np.array(measurements)\n",
    "\n",
    "print(f\"Generated {len(measurements)} measurements\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Final position estimate: (3.22, 14.53)\n",
      "Final position uncertainty (1σ): (0.363, 0.363)\n"
     ]
    }
   ],
   "source": [
    "# Run Kalman filter\n",
    "x_est = np.array([0.0, 0.0, 0.0, 0.0])  # Initial estimate\n",
    "P_est = np.eye(4) * 10.0  # Initial covariance (uncertain)\n",
    "\n",
    "estimates = [x_est.copy()]\n",
    "covariances = [P_est.copy()]\n",
    "\n",
    "for z in measurements:\n",
    "    # Predict\n",
    "    pred = kf_predict(x_est, P_est, F, Q)\n",
    "    x_pred, P_pred = pred.x, pred.P\n",
    "    \n",
    "    # Update\n",
    "    upd = kf_update(x_pred, P_pred, z, H, R)\n",
    "    x_est, P_est = upd.x, upd.P\n",
    "    \n",
    "    estimates.append(x_est.copy())\n",
    "    covariances.append(P_est.copy())\n",
    "\n",
    "estimates = np.array(estimates)\n",
    "covariances = np.array(covariances)\n",
    "\n",
    "print(f\"Final position estimate: ({estimates[-1, 0]:.2f}, {estimates[-1, 2]:.2f})\")\n",
    "print(f\"Final position uncertainty (1σ): ({np.sqrt(covariances[-1, 0, 0]):.3f}, {np.sqrt(covariances[-1, 2, 2]):.3f})\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Visualization with Plotly\n",
    "fig = make_subplots(\n",
    "    rows=1, cols=2,\n",
    "    subplot_titles=('Kalman Filter: 2D Trajectory Tracking', 'Estimation Error vs Model Confidence'),\n",
    "    horizontal_spacing=0.12\n",
    ")\n",
    "\n",
    "# Calculate error and uncertainty\n",
    "pos_error = np.sqrt((estimates[1:, 0] - true_states[1:, 0])**2 + \n",
    "                     (estimates[1:, 2] - true_states[1:, 2])**2)\n",
    "pos_uncertainty = np.sqrt(covariances[1:, 0, 0] + covariances[1:, 2, 2])\n",
    "time = np.arange(len(pos_error)) * dt\n",
    "\n",
    "# Left plot: 2D trajectory\n",
    "fig.add_trace(\n",
    "    go.Scatter(x=true_states[:, 0], y=true_states[:, 2], mode='lines',\n",
    "               name='True trajectory', line=dict(color='#00ff88', width=2.5)),\n",
    "    row=1, col=1\n",
    ")\n",
    "fig.add_trace(\n",
    "    go.Scatter(x=measurements[:, 0], y=measurements[:, 1], mode='markers',\n",
    "               name='Measurements', marker=dict(color='#ff4757', size=5, opacity=0.4)),\n",
    "    row=1, col=1\n",
    ")\n",
    "fig.add_trace(\n",
    "    go.Scatter(x=estimates[:, 0], y=estimates[:, 2], mode='lines',\n",
    "               name='KF estimate', line=dict(color='#00d4ff', width=2, dash='dash')),\n",
    "    row=1, col=1\n",
    ")\n",
    "\n",
    "# Right plot: Error vs Uncertainty\n",
    "fig.add_trace(\n",
    "    go.Scatter(x=time, y=2*pos_uncertainty, mode='lines', fill='tozeroy',\n",
    "               name='2σ uncertainty', line=dict(color='#00d4ff', width=0),\n",
    "               fillcolor='rgba(0, 212, 255, 0.3)'),\n",
    "    row=1, col=2\n",
    ")\n",
    "fig.add_trace(\n",
    "    go.Scatter(x=time, y=pos_error, mode='lines',\n",
    "               name='Position error', line=dict(color='#00d4ff', width=2.5)),\n",
    "    row=1, col=2\n",
    ")\n",
    "\n",
    "fig.update_layout(\n",
    "    template=dark_template,\n",
    "    height=450,\n",
    "    showlegend=True,\n",
    "    legend=dict(x=0.5, y=-0.2, xanchor='center', orientation='h')\n",
    ")\n",
    "fig.update_xaxes(title_text='X position (m)', row=1, col=1)\n",
    "fig.update_yaxes(title_text='Y position (m)', row=1, col=1)\n",
    "fig.update_xaxes(title_text='Time (s)', row=1, col=2)\n",
    "fig.update_yaxes(title_text='Error magnitude (m)', row=1, col=2)\n",
    "\n",
    "fig.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 2. Extended Kalman Filter (EKF)\n",
    "\n",
    "When the system dynamics or measurements are **nonlinear**, we must adapt the linear Kalman filter. The **Extended Kalman Filter** linearizes the nonlinear functions around the current estimate.\n",
    "\n",
    "### Nonlinear System Model\n",
    "\n",
    "$$x_{k+1} = f(x_k, w_k)$$\n",
    "$$z_k = h(x_k, v_k)$$\n",
    "\n",
    "where $f(\\cdot)$ and $h(\\cdot)$ are arbitrary nonlinear functions.\n",
    "\n",
    "### EKF Algorithm\n",
    "\n",
    "**Predict Step** (linearize around predicted state):\n",
    "$$F_k = \\left.\\frac{\\partial f}{\\partial x}\\right|_{\\hat{x}_{k-1}} \\quad \\text{(Jacobian of } f \\text{)}$$\n",
    "$$\\hat{x}^-_k = f(\\hat{x}_{k-1})$$\n",
    "$$P^-_k = F_k P_{k-1} F_k^T + Q$$\n",
    "\n",
    "**Update Step** (linearize around predicted state):\n",
    "$$H_k = \\left.\\frac{\\partial h}{\\partial x}\\right|_{\\hat{x}^-_k} \\quad \\text{(Jacobian of } h \\text{)}$$\n",
    "$$K_k = P^-_k H_k^T (H_k P^-_k H_k^T + R)^{-1}$$\n",
    "$$\\hat{x}_k = \\hat{x}^-_k + K_k (z_k - h(\\hat{x}^-_k))$$\n",
    "$$P_k = (I - K_k H_k) P^-_k$$\n",
    "\n",
    "### Limitations\n",
    "\n",
    "- Only uses **first-order** (linear) approximation\n",
    "- Performance degrades with high nonlinearity\n",
    "- Jacobian computation can be error-prone\n",
    "- Can diverge if linearization is poor\n",
    "\n",
    "### Example: Tracking with Range-Bearing Measurements\n",
    "\n",
    "Radar provides nonlinear measurements: $(r, \\theta) = (\\text{range}, \\text{bearing})$ instead of Cartesian position (x, y)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Nonlinear measurement model: range and bearing from origin\n",
    "def h_radar(x):\n",
    "    \"\"\"Radar measurement: range and bearing.\"\"\"\n",
    "    px, vx, py, vy = x\n",
    "    r = np.sqrt(px**2 + py**2)\n",
    "    theta = np.arctan2(py, px)\n",
    "    return np.array([r, theta])\n",
    "\n",
    "def H_radar(x):\n",
    "    \"\"\"Jacobian of radar measurement.\"\"\"\n",
    "    px, vx, py, vy = x\n",
    "    r = np.sqrt(px**2 + py**2)\n",
    "    if r < 1e-10:\n",
    "        r = 1e-10\n",
    "    return np.array([\n",
    "        [px/r, 0, py/r, 0],\n",
    "        [-py/r**2, 0, px/r**2, 0]\n",
    "    ])\n",
    "\n",
    "# Measurement noise for range-bearing\n",
    "R_radar = np.diag([0.5**2, np.radians(2)**2])  # 0.5m range, 2 deg bearing\n",
    "\n",
    "# Generate radar measurements\n",
    "radar_measurements = []\n",
    "for state in true_states[1:]:\n",
    "    z_true = h_radar(state)\n",
    "    z = z_true + np.random.multivariate_normal(np.zeros(2), R_radar)\n",
    "    radar_measurements.append(z)\n",
    "\n",
    "radar_measurements = np.array(radar_measurements)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "EKF final position: (3.56, 15.06)\n",
      "True final position: (3.47, 15.08)\n"
     ]
    }
   ],
   "source": [
    "# Run EKF with radar measurements\n",
    "x_ekf = np.array([1.0, 0.0, 1.0, 0.0])  # Start with offset estimate\n",
    "P_ekf = np.eye(4) * 10.0\n",
    "\n",
    "# Linear dynamics (constant velocity)\n",
    "def f_cv(x):\n",
    "    return F @ x\n",
    "\n",
    "ekf_estimates = [x_ekf.copy()]\n",
    "\n",
    "for z in radar_measurements:\n",
    "    # Predict (linear dynamics)\n",
    "    pred = ekf_predict(x_ekf, P_ekf, f_cv, F, Q)\n",
    "    x_pred, P_pred = pred.x, pred.P\n",
    "    \n",
    "    # Update (nonlinear measurement)\n",
    "    upd = ekf_update(x_pred, P_pred, z, h_radar, H_radar(x_pred), R_radar)\n",
    "    x_ekf, P_ekf = upd.x, upd.P\n",
    "    \n",
    "    ekf_estimates.append(x_ekf.copy())\n",
    "\n",
    "ekf_estimates = np.array(ekf_estimates)\n",
    "\n",
    "print(f\"EKF final position: ({ekf_estimates[-1, 0]:.2f}, {ekf_estimates[-1, 2]:.2f})\")\n",
    "print(f\"True final position: ({true_states[-1, 0]:.2f}, {true_states[-1, 2]:.2f})\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Compare KF (with position measurements) vs EKF (with radar measurements)\n",
    "fig = go.Figure()\n",
    "\n",
    "# Plot trajectories\n",
    "fig.add_trace(\n",
    "    go.Scatter(x=true_states[:, 0], y=true_states[:, 2], mode='lines',\n",
    "               name='True trajectory', line=dict(color='#00ff88', width=2.5))\n",
    ")\n",
    "fig.add_trace(\n",
    "    go.Scatter(x=estimates[:, 0], y=estimates[:, 2], mode='lines',\n",
    "               name='KF (position meas.)', line=dict(color='#00d4ff', width=2, dash='dash'))\n",
    ")\n",
    "fig.add_trace(\n",
    "    go.Scatter(x=ekf_estimates[:, 0], y=ekf_estimates[:, 2], mode='lines',\n",
    "               name='EKF (radar meas.)', line=dict(color='#ff4757', width=2, dash='dot'))\n",
    ")\n",
    "\n",
    "# Plot radar position at origin\n",
    "fig.add_trace(\n",
    "    go.Scatter(x=[0], y=[0], mode='markers', name='Radar',\n",
    "               marker=dict(color='#ffb800', size=15, symbol='triangle-up',\n",
    "                          line=dict(color='white', width=2)))\n",
    ")\n",
    "\n",
    "# Plot radar measurement rays (every 10th measurement)\n",
    "for i in range(0, len(radar_measurements), 10):\n",
    "    r, theta = radar_measurements[i]\n",
    "    ray_end_x = r * np.cos(theta)\n",
    "    ray_end_y = r * np.sin(theta)\n",
    "    fig.add_trace(\n",
    "        go.Scatter(x=[0, ray_end_x], y=[0, ray_end_y], mode='lines',\n",
    "                   line=dict(color='rgba(255, 183, 0, 0.15)', width=0.8),\n",
    "                   showlegend=False, hoverinfo='skip')\n",
    "    )\n",
    "\n",
    "fig.update_layout(\n",
    "    template=dark_template,\n",
    "    title='Linear KF vs Extended KF: Handling Nonlinear Measurements',\n",
    "    xaxis_title='X position (m)',\n",
    "    yaxis_title='Y position (m)',\n",
    "    height=550,\n",
    "    showlegend=True,\n",
    "    yaxis=dict(scaleanchor='x', scaleratio=1)\n",
    ")\n",
    "\n",
    "fig.show()\n",
    "\n",
    "# Print comparison\n",
    "ekf_pos_error = np.sqrt((ekf_estimates[1:, 0] - true_states[1:, 0])**2 + \n",
    "                         (ekf_estimates[1:, 2] - true_states[1:, 2])**2)\n",
    "kf_pos_error = np.sqrt((estimates[1:, 0] - true_states[1:, 0])**2 + \n",
    "                        (estimates[1:, 2] - true_states[1:, 2])**2)\n",
    "print(f\"KF RMSE (position meas.): {np.sqrt(np.mean(kf_pos_error**2)):.4f} m\")\n",
    "print(f\"EKF RMSE (radar meas.): {np.sqrt(np.mean(ekf_pos_error**2)):.4f} m\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 3. Unscented Kalman Filter (UKF)\n",
    "\n",
    "The **Unscented Kalman Filter** uses a clever trick to handle nonlinearity without explicit Jacobian computation. It uses \"sigma points\" (carefully chosen sample points) to capture the mean and covariance of a nonlinear transformation.\n",
    "\n",
    "### Key Insight: The Unscented Transform\n",
    "\n",
    "Instead of linearizing (approximating the function), we **sample the function**:\n",
    "\n",
    "1. Generate 2n+1 sigma points around the current estimate\n",
    "2. Propagate each sigma point through the nonlinear function\n",
    "3. Compute mean and covariance from the transformed sigma points\n",
    "\n",
    "### Why This Works Better\n",
    "\n",
    "- **Second-order accuracy**: Captures up to 2nd-order Taylor terms (vs 1st for EKF)\n",
    "- **No Jacobians needed**: Purely sampling-based, numerical derivative-free\n",
    "- **More stable**: Works well with highly nonlinear systems\n",
    "- **Same computational cost** as EKF (both O(n³) due to matrix operations)\n",
    "\n",
    "### UKF Algorithm Structure\n",
    "\n",
    "```\n",
    "For each time step:\n",
    "  1. Generate sigma points from (x̂, P)\n",
    "  2. Predict: Propagate each sigma point through f(·)\n",
    "     - Compute mean and covariance of predictions\n",
    "  3. Generate measurement sigma points\n",
    "  4. Update: Weight by measurement likelihood\n",
    "     - Blend predictions with measurements using Kalman gain\n",
    "```\n",
    "\n",
    "### When to Use UKF vs EKF\n",
    "\n",
    "| Scenario | Choice | Reason |\n",
    "|----------|--------|--------|\n",
    "| Radar/bearing measurements | UKF | Often nonlinear, avoids Jacobian errors |\n",
    "| Maneuvering targets | UKF | Handles sharp turns better |\n",
    "| Well-behaved linear systems | KF | Simpler, lower cost |\n",
    "| Real-time systems | EKF or UKF | Both feasible; UKF more robust |"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "UKF final position: (3.56, 15.05)\n"
     ]
    }
   ],
   "source": [
    "# Run UKF with radar measurements\n",
    "x_ukf = np.array([1.0, 0.0, 1.0, 0.0])\n",
    "P_ukf = np.eye(4) * 10.0\n",
    "\n",
    "ukf_estimates = [x_ukf.copy()]\n",
    "\n",
    "for z in radar_measurements:\n",
    "    # Predict\n",
    "    pred = ukf_predict(x_ukf, P_ukf, f_cv, Q)\n",
    "    x_pred, P_pred = pred.x, pred.P\n",
    "    \n",
    "    # Update\n",
    "    upd = ukf_update(x_pred, P_pred, z, h_radar, R_radar)\n",
    "    x_ukf, P_ukf = upd.x, upd.P\n",
    "    \n",
    "    ukf_estimates.append(x_ukf.copy())\n",
    "\n",
    "ukf_estimates = np.array(ukf_estimates)\n",
    "\n",
    "print(f\"UKF final position: ({ukf_estimates[-1, 0]:.2f}, {ukf_estimates[-1, 2]:.2f})\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.plotly.v1+json": {
       "config": {
        "plotlyServerURL": "https://plot.ly"
       },
       "data": [
        {
         "hovertemplate": "Time: %{x:.2f}s<br>Error: %{y:.4f}m<extra></extra>",
         "line": {
          "color": "#ff6b6b",
          "width": 2.5
         },
         "mode": "lines",
         "name": "EKF (RMSE: 0.3773 m)",
         "type": "scatter",
         "x": {
          "bdata": "AAAAAAAAAACamZmZmZm5P5qZmZmZmck/NDMzMzMz0z+amZmZmZnZPwAAAAAAAOA/NDMzMzMz4z9nZmZmZmbmP5qZmZmZmek/zczMzMzM7D8AAAAAAADwP5qZmZmZmfE/NDMzMzMz8z/NzMzMzMz0P2dmZmZmZvY/AAAAAAAA+D+amZmZmZn5PzQzMzMzM/s/zczMzMzM/D9nZmZmZmb+PwAAAAAAAABAzczMzMzMAECamZmZmZkBQGdmZmZmZgJANDMzMzMzA0AAAAAAAAAEQM3MzMzMzARAmpmZmZmZBUBnZmZmZmYGQDQzMzMzMwdAAAAAAAAACEDNzMzMzMwIQJqZmZmZmQlAZ2ZmZmZmCkA0MzMzMzMLQAAAAAAAAAxAzczMzMzMDECamZmZmZkNQGdmZmZmZg5ANDMzMzMzD0AAAAAAAAAQQGdmZmZmZhBAzczMzMzMEEAzMzMzMzMRQJqZmZmZmRFAAAAAAAAAEkBnZmZmZmYSQM3MzMzMzBJANDMzMzMzE0CamZmZmZkTQAAAAAAAABRAZ2ZmZmZmFEDNzMzMzMwUQDQzMzMzMxVAmpmZmZmZFUAAAAAAAAAWQGdmZmZmZhZAzczMzMzMFkA0MzMzMzMXQJqZmZmZmRdAAAAAAAAAGEBnZmZmZmYYQM3MzMzMzBhANDMzMzMzGUCamZmZmZkZQAAAAAAAABpAZ2ZmZmZmGkDNzMzMzMwaQDQzMzMzMxtAmpmZmZmZG0AAAAAAAAAcQGdmZmZmZhxAzczMzMzMHEA0MzMzMzMdQJqZmZmZmR1AAAAAAAAAHkBnZmZmZmYeQM3MzMzMzB5ANDMzMzMzH0CamZmZmZkfQAAAAAAAACBAMzMzMzMzIEBnZmZmZmYgQJqZmZmZmSBAzczMzMzMIEAAAAAAAAAhQDMzMzMzMyFAZ2ZmZmZmIUCamZmZmZkhQM3MzMzMzCFAAAAAAAAAIkAzMzMzMzMiQGdmZmZmZiJAmpmZmZmZIkDNzMzMzMwiQAAAAAAAACNANDMzMzMzI0BnZmZmZmYjQJqZmZmZmSNAzczMzMzMI0A=",
          "dtype": "f8"
         },
         "y": {
          "bdata": "NX/GLx455D8hOq5KtmnmP0jIJOtaIOI/PEQnbMev3z/LNYfEFzzKP9J2y7IRctU/XsG1/6fg1z9BP+aSnsncP6fn/6SL8uI/2L7X6w3t5D+c8EoBoarnP9ksWwNku+k/bP7jKMWX6T/+GKJs0cPqP5q0tc+Cfeo/oaPdocp07D/a4bl7eyvqP67vDB9jPOo/Zlh35Qpu5T+GK+PmhdvlP0EpGjCafuI/SGk79+/A3z+ejCPjrafbP5YiWRXNseI/wX4CdXOS4D9frcb1wGHRP/5/7wr848k/8kQCH8i2uz9PijUzXF3DPzYq9ThBRso/vIkSWwbExz8IHsl9guPKP6A+0y4y6cQ/DWjZwZ6mvj9uzle99ATBP3tVPwfjfbc/He/I7ryWuD/VhEx+XbDGP+rdYVqRPc0/Mq3qlAk0wT9ZcF5BP72gPx7i2AT5Oq8/q/EqOw/VxD/+k/liNgbCP3IzG1eMsKY/DeUJPlhmsz8WGGUnUWTDP4ji8Q/XDrE/mYsdIbn7vD8XPYJ0i4nEP3NDsRZ91rk/b8cCskWsyT8Ays5s51jQP6SfJFK0yNM/hVtmfSaI3j9NxJbsLa/aPwUrafYJENw/MB2LhLoC2z96m3UaYvDXP1HxVafsENg/1r4pd9+J0z/hnv81kPvYP0HIgrQBrNQ/qnAdJ7dN0T8WHI1WZ+/RP+mUMitiI9A/m524g4PL0T+rsdpXD5bIP8QtQClYIL4/0tb9W6BVzT9BQMc8GiC/PwA28aWJgbg/gorpqsoCxj+9NBpFfo3LP7cD0hNhFc4/pNr8E2wn0D/KXm28yNK9P8leRj43kMU/yCMDWjMSxj9g7nG5DObPP7un0dfcINE/g5SJ8vBwzj9TLx47y3vPP/jkV2VIA9Q/fnlh3WQ52T+ylckzIAjVP9/XRc3xPNg/i975YnJ1zj9DoGl+C7PDP9q1oaVQxsc/FEp9vPsRrT+AK2ZdyIG0PzfUDlw6X8E/pNCJ6L/5wj/VNaZqOeSoP5essfRborI/B1WXqxh/yT8nw6WLXR69P7wF9kBPBLI/lSGxZLyutz8=",
          "dtype": "f8"
         }
        },
        {
         "hovertemplate": "Time: %{x:.2f}s<br>Error: %{y:.4f}m<extra></extra>",
         "line": {
          "color": "#4ecdc4",
          "width": 2.5
         },
         "mode": "lines",
         "name": "UKF (RMSE: 1.0151 m)",
         "type": "scatter",
         "x": {
          "bdata": "AAAAAAAAAACamZmZmZm5P5qZmZmZmck/NDMzMzMz0z+amZmZmZnZPwAAAAAAAOA/NDMzMzMz4z9nZmZmZmbmP5qZmZmZmek/zczMzMzM7D8AAAAAAADwP5qZmZmZmfE/NDMzMzMz8z/NzMzMzMz0P2dmZmZmZvY/AAAAAAAA+D+amZmZmZn5PzQzMzMzM/s/zczMzMzM/D9nZmZmZmb+PwAAAAAAAABAzczMzMzMAECamZmZmZkBQGdmZmZmZgJANDMzMzMzA0AAAAAAAAAEQM3MzMzMzARAmpmZmZmZBUBnZmZmZmYGQDQzMzMzMwdAAAAAAAAACEDNzMzMzMwIQJqZmZmZmQlAZ2ZmZmZmCkA0MzMzMzMLQAAAAAAAAAxAzczMzMzMDECamZmZmZkNQGdmZmZmZg5ANDMzMzMzD0AAAAAAAAAQQGdmZmZmZhBAzczMzMzMEEAzMzMzMzMRQJqZmZmZmRFAAAAAAAAAEkBnZmZmZmYSQM3MzMzMzBJANDMzMzMzE0CamZmZmZkTQAAAAAAAABRAZ2ZmZmZmFEDNzMzMzMwUQDQzMzMzMxVAmpmZmZmZFUAAAAAAAAAWQGdmZmZmZhZAzczMzMzMFkA0MzMzMzMXQJqZmZmZmRdAAAAAAAAAGEBnZmZmZmYYQM3MzMzMzBhANDMzMzMzGUCamZmZmZkZQAAAAAAAABpAZ2ZmZmZmGkDNzMzMzMwaQDQzMzMzMxtAmpmZmZmZG0AAAAAAAAAcQGdmZmZmZhxAzczMzMzMHEA0MzMzMzMdQJqZmZmZmR1AAAAAAAAAHkBnZmZmZmYeQM3MzMzMzB5ANDMzMzMzH0CamZmZmZkfQAAAAAAAACBAMzMzMzMzIEBnZmZmZmYgQJqZmZmZmSBAzczMzMzMIEAAAAAAAAAhQDMzMzMzMyFAZ2ZmZmZmIUCamZmZmZkhQM3MzMzMzCFAAAAAAAAAIkAzMzMzMzMiQGdmZmZmZiJAmpmZmZmZIkDNzMzMzMwiQAAAAAAAACNANDMzMzMzI0BnZmZmZmYjQJqZmZmZmSNAzczMzMzMI0A=",
          "dtype": "f8"
         },
         "y": {
          "bdata": "cvpF/xYX3z/OL8yQfsPlP0LvdtaVNdg/r0SzmzPIuz9OEQPI1tvcPxwh0mX7QfY/ccj8sLd5BkCjVJuIABANQGEJNFmLpg5AhTUqSGhcDkAoRZtPDJkLQLXMRniY8glAJ/ccTFIwB0DEpe+gx0cCQCiHgdq6Wv0/gNKU0m089D/pLTUulpjvPzXCFJwYIeQ/6yQaZI7C3j+/caPMxZPDP93KOC553Ko/ykscquRQwT8D5mWi76zQP4cdMMnHx+I/7ZVy1Xil5D+YPnfNFp7fP1r1WeIoH+A/GX1ITuJG0D93UXbPtK/QPzWdVYExxM4/pIPi/HyS0j8vk6psvX7SP9AyEVT1adU/uIpzPlaW1T+MEtAfkFLiP7FS9Lgxodk/aojxTFvt2T/sg19GqCzgP3qaXRd+8+E/vWVUXFO52z/DFRLw3+jSP5jo/Ev3Z9M/Asbf+nQJ2D8Ev8UeYfXUP2qMhqggMso/6hNA7FzxyT+erutsiNnQPzyhfG/SlMQ/3hBsHOKmwj8QTGkirmjCP9jOBZXMpLs/Ek4gHOdHyz9UiVrUFJLRP8OxP/hDrdQ/2kAPO0Xl3j9dGimYQKnaP/g+eREetds/i0t1kLVe2j/1oEDqgBrXPzkxVIbHF9c/rXxXHi6Q0j+Vt341M+LXP0KYxFFSktM/DKFet0Yz0D+AB+unAfrQP4Qm7Zlu784/LxtaN82Z0T8KX0TH5DLJPyWixHj4UcA/019sCF78zj/kbyjtsKzAP0VS6MwWDro/f5ll9t7yxj+q73Brf2LMP6nei06Mmc4/vPbo8WhE0D+r4moZSKO9PwnQYe95QcU/bBlrkVD4xT+wiVTRNsrPPwJIRhiqFtE/ZpLntLp6zj+ZIuQLSH3PP4HHH1ZQAdQ/RE0S0ZY62T/yEO6EwwXVP1jaSl4ZOdg/yUrGb9Rbzj+85OELNrPDP0Exw7AK18c/hiCWtPvQrT88MsnUvMW0P9yAO9qjZ8E/dgCk39Xgwj+Q3RYlrj6oPyOkPAjSNLI/LrYDJOBeyT8rhXq/0EK9P5qZJTClJ7I/AkdpKAyytz8=",
          "dtype": "f8"
         }
        },
        {
         "fill": "tozeroy",
         "fillcolor": "rgba(76, 175, 80, 0.2)",
         "hoverinfo": "skip",
         "line": {
          "color": "rgba(78, 205, 196, 0)"
         },
         "name": "UKF advantage",
         "type": "scatter",
         "x": {
          "bdata": "AAAAAAAAAACamZmZmZm5P5qZmZmZmck/NDMzMzMz0z+amZmZmZnZPwAAAAAAAOA/NDMzMzMz4z9nZmZmZmbmP5qZmZmZmek/zczMzMzM7D8AAAAAAADwP5qZmZmZmfE/NDMzMzMz8z/NzMzMzMz0P2dmZmZmZvY/AAAAAAAA+D+amZmZmZn5PzQzMzMzM/s/zczMzMzM/D9nZmZmZmb+PwAAAAAAAABAzczMzMzMAECamZmZmZkBQGdmZmZmZgJANDMzMzMzA0AAAAAAAAAEQM3MzMzMzARAmpmZmZmZBUBnZmZmZmYGQDQzMzMzMwdAAAAAAAAACEDNzMzMzMwIQJqZmZmZmQlAZ2ZmZmZmCkA0MzMzMzMLQAAAAAAAAAxAzczMzMzMDECamZmZmZkNQGdmZmZmZg5ANDMzMzMzD0AAAAAAAAAQQGdmZmZmZhBAzczMzMzMEEAzMzMzMzMRQJqZmZmZmRFAAAAAAAAAEkBnZmZmZmYSQM3MzMzMzBJANDMzMzMzE0CamZmZmZkTQAAAAAAAABRAZ2ZmZmZmFEDNzMzMzMwUQDQzMzMzMxVAmpmZmZmZFUAAAAAAAAAWQGdmZmZmZhZAzczMzMzMFkA0MzMzMzMXQJqZmZmZmRdAAAAAAAAAGEBnZmZmZmYYQM3MzMzMzBhANDMzMzMzGUCamZmZmZkZQAAAAAAAABpAZ2ZmZmZmGkDNzMzMzMwaQDQzMzMzMxtAmpmZmZmZG0AAAAAAAAAcQGdmZmZmZhxAzczMzMzMHEA0MzMzMzMdQJqZmZmZmR1AAAAAAAAAHkBnZmZmZmYeQM3MzMzMzB5ANDMzMzMzH0CamZmZmZkfQAAAAAAAACBAMzMzMzMzIEBnZmZmZmYgQJqZmZmZmSBAzczMzMzMIEAAAAAAAAAhQDMzMzMzMyFAZ2ZmZmZmIUCamZmZmZkhQM3MzMzMzCFAAAAAAAAAIkAzMzMzMzMiQGdmZmZmZiJAmpmZmZmZIkDNzMzMzMwiQAAAAAAAACNANDMzMzMzI0BnZmZmZmYjQJqZmZmZmSNAzczMzMzMI0A=",
          "dtype": "f8"
         },
         "y": {
          "bdata": "cvpF/xYX3z/OL8yQfsPlP0LvdtaVNdg/r0SzmzPIuz/LNYfEFzzKP9J2y7IRctU/XsG1/6fg1z9BP+aSnsncP6fn/6SL8uI/2L7X6w3t5D+c8EoBoarnP9ksWwNku+k/bP7jKMWX6T/+GKJs0cPqP5q0tc+Cfeo/oaPdocp07D/a4bl7eyvqPzXCFJwYIeQ/6yQaZI7C3j+/caPMxZPDP93KOC553Ko/ykscquRQwT8D5mWi76zQP5YiWRXNseI/wX4CdXOS4D9frcb1wGHRP/5/7wr848k/8kQCH8i2uz9PijUzXF3DPzYq9ThBRso/vIkSWwbExz8IHsl9guPKP6A+0y4y6cQ/DWjZwZ6mvj9uzle99ATBP3tVPwfjfbc/He/I7ryWuD/VhEx+XbDGP+rdYVqRPc0/Mq3qlAk0wT9ZcF5BP72gPx7i2AT5Oq8/q/EqOw/VxD/+k/liNgbCP3IzG1eMsKY/DeUJPlhmsz8WGGUnUWTDP4ji8Q/XDrE/mYsdIbn7vD8QTGkirmjCP3NDsRZ91rk/b8cCskWsyT8Ays5s51jQP6SfJFK0yNM/hVtmfSaI3j9dGimYQKnaP/g+eREetds/i0t1kLVe2j/1oEDqgBrXPzkxVIbHF9c/rXxXHi6Q0j+Vt341M+LXP0KYxFFSktM/DKFet0Yz0D+AB+unAfrQP4Qm7Zlu784/LxtaN82Z0T+rsdpXD5bIP8QtQClYIL4/0tb9W6BVzT9BQMc8GiC/PwA28aWJgbg/gorpqsoCxj+9NBpFfo3LP7cD0hNhFc4/pNr8E2wn0D+r4moZSKO9PwnQYe95QcU/bBlrkVD4xT+wiVTRNsrPPwJIRhiqFtE/g5SJ8vBwzj9TLx47y3vPP4HHH1ZQAdQ/fnlh3WQ52T/yEO6EwwXVP1jaSl4ZOdg/yUrGb9Rbzj9DoGl+C7PDP9q1oaVQxsc/FEp9vPsRrT+AK2ZdyIG0PzfUDlw6X8E/dgCk39Xgwj+Q3RYlrj6oPyOkPAjSNLI/LrYDJOBeyT8nw6WLXR69P7wF9kBPBLI/lSGxZLyutz8=",
          "dtype": "f8"
         }
        }
       ],
       "layout": {
        "height": 500,
        "hovermode": "x unified",
        "legend": {
         "bgcolor": "rgba(0,0,0,0.5)",
         "x": 0.5,
         "xanchor": "center",
         "y": 1,
         "yanchor": "top"
        },
        "template": {
         "layout": {
          "font": {
           "color": "#e6edf3"
          },
          "paper_bgcolor": "#0d1117",
          "plot_bgcolor": "#0d1117",
          "xaxis": {
           "gridcolor": "#30363d",
           "zerolinecolor": "#30363d"
          },
          "yaxis": {
           "gridcolor": "#30363d",
           "zerolinecolor": "#30363d"
          }
         }
        },
        "title": {
         "text": "Extended Kalman Filter vs Unscented Kalman Filter"
        },
        "xaxis": {
         "title": {
          "text": "Time (s)"
         }
        },
        "yaxis": {
         "title": {
          "text": "Position Error (m)"
         }
        }
       }
      }
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\n",
      "=== FILTER COMPARISON ===\n",
      "EKF mean error: 0.3055 m, RMSE: 0.3773 m\n",
      "UKF mean error: 0.5632 m, RMSE: 1.0151 m\n",
      "UKF improvement: -169.0% better RMSE\n"
     ]
    }
   ],
   "source": [
    "# Compare EKF vs UKF\n",
    "ekf_error = np.sqrt((ekf_estimates[1:, 0] - true_states[1:, 0])**2 + \n",
    "                     (ekf_estimates[1:, 2] - true_states[1:, 2])**2)\n",
    "ukf_error = np.sqrt((ukf_estimates[1:, 0] - true_states[1:, 0])**2 + \n",
    "                     (ukf_estimates[1:, 2] - true_states[1:, 2])**2)\n",
    "\n",
    "ekf_rmse = np.sqrt(np.mean(ekf_error**2))\n",
    "ukf_rmse = np.sqrt(np.mean(ukf_error**2))\n",
    "\n",
    "# Plotly visualization\n",
    "fig = go.Figure()\n",
    "\n",
    "time_vec = np.arange(len(ekf_error)) * dt\n",
    "\n",
    "# Add EKF error\n",
    "fig.add_trace(go.Scatter(\n",
    "    x=time_vec, y=ekf_error,\n",
    "    mode='lines',\n",
    "    name=f'EKF (RMSE: {ekf_rmse:.4f} m)',\n",
    "    line=dict(color='#ff6b6b', width=2.5),\n",
    "    hovertemplate='Time: %{x:.2f}s<br>Error: %{y:.4f}m<extra></extra>'\n",
    "))\n",
    "\n",
    "# Add UKF error\n",
    "fig.add_trace(go.Scatter(\n",
    "    x=time_vec, y=ukf_error,\n",
    "    mode='lines',\n",
    "    name=f'UKF (RMSE: {ukf_rmse:.4f} m)',\n",
    "    line=dict(color='#4ecdc4', width=2.5),\n",
    "    hovertemplate='Time: %{x:.2f}s<br>Error: %{y:.4f}m<extra></extra>'\n",
    "))\n",
    "\n",
    "# Add UKF advantage region (where UKF performs better)\n",
    "ukf_better = np.where(ukf_error <= ekf_error, ukf_error, ekf_error)\n",
    "fig.add_trace(go.Scatter(\n",
    "    x=time_vec, y=ukf_better,\n",
    "    fill='tozeroy',\n",
    "    name='UKF advantage',\n",
    "    line=dict(color='rgba(78, 205, 196, 0)'),\n",
    "    fillcolor='rgba(76, 175, 80, 0.2)',\n",
    "    hoverinfo='skip'\n",
    "))\n",
    "\n",
    "fig.update_layout(\n",
    "    template=dark_template,\n",
    "    title='Extended Kalman Filter vs Unscented Kalman Filter',\n",
    "    xaxis_title='Time (s)',\n",
    "    yaxis_title='Position Error (m)',\n",
    "    height=500,\n",
    "    hovermode='x unified',\n",
    "    legend=dict(x=0.5, y=1.0, xanchor='center', yanchor='top', bgcolor='rgba(0,0,0,0.5)')\n",
    ")\n",
    "\n",
    "fig.show()\n",
    "\n",
    "print(f\"\\n=== FILTER COMPARISON ===\")\n",
    "print(f\"EKF mean error: {np.mean(ekf_error):.4f} m, RMSE: {ekf_rmse:.4f} m\")\n",
    "print(f\"UKF mean error: {np.mean(ukf_error):.4f} m, RMSE: {ukf_rmse:.4f} m\")\n",
    "print(f\"UKF improvement: {(1 - ukf_rmse/ekf_rmse)*100:.1f}% better RMSE\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 4. Square-Root Kalman Filter (SR-KF)\n",
    "\n",
    "The **Square-Root KF** works with the **Cholesky factor** $S$ where $P = S S^T$ instead of the covariance matrix $P$ directly.\n",
    "\n",
    "### Why Square-Root Form?\n",
    "\n",
    "**Standard KF Problem**: \n",
    "- Covariance matrix $P$ can become non-positive-definite due to numerical errors\n",
    "- Computing $P^{-1}$ amplifies round-off errors\n",
    "- Can diverge or produce negative variances\n",
    "\n",
    "**SR-KF Solution**:\n",
    "- Work with $S$ (Cholesky decomposition) instead\n",
    "- Guaranteed positive semi-definiteness: $P = SS^T > 0$ (always)\n",
    "- Better numerical conditioning\n",
    "- Smaller numerical errors propagate better\n",
    "\n",
    "### Mathematics\n",
    "\n",
    "Instead of updating $P_k$, update $S_k$ using specialized algorithms:\n",
    "- **QR-decomposition-based updates** (Givens rotations)\n",
    "- **Cholesky updates** (rank-1 and rank-2 updates)\n",
    "- Result: Same covariance but computed more reliably\n",
    "\n",
    "### When to Use SR Filters\n",
    "\n",
    "- **Ill-conditioned problems**: Stiff systems, large state dimensions\n",
    "- **Long-running simulations**: Numerical drift accumulates over time\n",
    "- **Baseline measurements**: Guarantees positive-definite estimate\n",
    "- **Production systems**: More robust than standard KF"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\n",
      "=== SQUARE-ROOT KF COMPARISON ===\n",
      "Standard KF position RMSE: 5.3819\n",
      "Square-Root KF position RMSE: 5.3819\n",
      "Difference: 0.000000 (should be very small - demonstrates equivalence)\n"
     ]
    }
   ],
   "source": [
    "# Run Square-Root KF\n",
    "# Note: SR-KF works with Cholesky factors of covariances, not covariances directly\n",
    "x_sr = np.array([0.0, 0.0, 0.0, 0.0])\n",
    "P_sr_init = np.eye(4) * 10.0\n",
    "S_sr = np.linalg.cholesky(P_sr_init)  # Cholesky factor of initial covariance\n",
    "\n",
    "# Compute Cholesky factors of Q and R\n",
    "S_Q = np.linalg.cholesky(Q)  # Cholesky factor of process noise\n",
    "S_R = np.linalg.cholesky(R)  # Cholesky factor of measurement noise\n",
    "\n",
    "sr_estimates = [x_sr.copy()]\n",
    "\n",
    "for z in measurements:\n",
    "    # Predict: Pass Cholesky factors instead of covariances\n",
    "    pred = srkf_predict(x_sr, S_sr, F, S_Q)\n",
    "    x_pred, S_pred = pred.x, pred.S\n",
    "    \n",
    "    # Update: Pass Cholesky factors instead of covariances\n",
    "    upd = srkf_update(x_pred, S_pred, z, H, S_R)\n",
    "    x_sr, S_sr = upd.x, upd.S\n",
    "    \n",
    "    sr_estimates.append(x_sr.copy())\n",
    "\n",
    "sr_estimates = np.array(sr_estimates)\n",
    "\n",
    "# Compare with standard KF\n",
    "kf_rmse = np.sqrt(np.mean((estimates[1:, :2] - true_states[1:, [0, 2]])**2))\n",
    "sr_rmse = np.sqrt(np.mean((sr_estimates[1:, :2] - true_states[1:, [0, 2]])**2))\n",
    "\n",
    "print(f\"\\n=== SQUARE-ROOT KF COMPARISON ===\")\n",
    "print(f\"Standard KF position RMSE: {kf_rmse:.4f}\")\n",
    "print(f\"Square-Root KF position RMSE: {sr_rmse:.4f}\")\n",
    "print(f\"Difference: {abs(kf_rmse - sr_rmse):.6f} (should be very small - demonstrates equivalence)\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Parameter Tuning: Interactive Exploration\n",
    "\n",
    "The Kalman filter's performance is highly sensitive to the noise covariances **Q** and **R**. Let's explore how they affect tracking quality."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.plotly.v1+json": {
       "config": {
        "plotlyServerURL": "https://plot.ly"
       },
       "data": [
        {
         "colorbar": {
          "title": {
           "text": "RMSE (m)"
          }
         },
         "colorscale": [
          [
           0,
           "rgb(0,104,55)"
          ],
          [
           0.1,
           "rgb(26,152,80)"
          ],
          [
           0.2,
           "rgb(102,189,99)"
          ],
          [
           0.3,
           "rgb(166,217,106)"
          ],
          [
           0.4,
           "rgb(217,239,139)"
          ],
          [
           0.5,
           "rgb(255,255,191)"
          ],
          [
           0.6,
           "rgb(254,224,139)"
          ],
          [
           0.7,
           "rgb(253,174,97)"
          ],
          [
           0.8,
           "rgb(244,109,67)"
          ],
          [
           0.9,
           "rgb(215,48,39)"
          ],
          [
           1,
           "rgb(165,0,38)"
          ]
         ],
         "customdata": [
          [
           "Q mult: 0.01<br>R mult: 0.1<br>RMSE: 0.4017m",
           "Q mult: 0.01<br>R mult: 0.5<br>RMSE: 0.4762m",
           "Q mult: 0.01<br>R mult: 1.0<br>RMSE: 0.4972m",
           "Q mult: 0.01<br>R mult: 2.0<br>RMSE: 0.5081m"
          ],
          [
           "Q mult: 0.1<br>R mult: 0.1<br>RMSE: 0.2543m",
           "Q mult: 0.1<br>R mult: 0.5<br>RMSE: 0.3658m",
           "Q mult: 0.1<br>R mult: 1.0<br>RMSE: 0.4017m",
           "Q mult: 0.1<br>R mult: 2.0<br>RMSE: 0.4357m"
          ],
          [
           "Q mult: 1.0<br>R mult: 0.1<br>RMSE: 0.1337m",
           "Q mult: 1.0<br>R mult: 0.5<br>RMSE: 0.2311m",
           "Q mult: 1.0<br>R mult: 1.0<br>RMSE: 0.2543m",
           "Q mult: 1.0<br>R mult: 2.0<br>RMSE: 0.3087m"
          ],
          [
           "Q mult: 10.0<br>R mult: 0.1<br>RMSE: 0.1391m",
           "Q mult: 10.0<br>R mult: 0.5<br>RMSE: 0.0557m",
           "Q mult: 10.0<br>R mult: 1.0<br>RMSE: 0.1337m",
           "Q mult: 10.0<br>R mult: 2.0<br>RMSE: 0.1924m"
          ]
         ],
         "hovertemplate": "%{customdata}<extra></extra>",
         "text": {
          "bdata": "7nw/NV662T8QWDm0yHbePwIrhxbZzt8/qMZLN4lB4D+oxks3iUHQPwaBlUOLbNc/7nw/NV662T+BlUOLbOfbP/T91HjpJsE/xSCwcmiRzT+oxks3iUHQPy2yne+nxtM/mG4Sg8DKwT956SYxCKysP/T91HjpJsE/+n5qvHSTyD8=",
          "dtype": "f8",
          "shape": "4, 4"
         },
         "textfont": {
          "size": 10
         },
         "texttemplate": "%{text:.3f}",
         "type": "heatmap",
         "x": [
          "0.1",
          "0.5",
          "1.0",
          "2.0"
         ],
         "y": [
          "0.01",
          "0.10",
          "1.00",
          "10.00"
         ],
         "z": {
          "bdata": "hIie/Nm12T9c0xbFqXrePzBa1Xzf0t8/1ud632hC4D88JEgcOUfQP8CfoG3Gadc/wMf3ADa22T/suDmrsOLbPzD0X9e8G8E/8N/bjUWVzT+oHO7BOEfQPwAbP3BNwdM/iPsKODnNwT9AM20LkYesP2jUYNe8G8E/OMCfQ4mfyD8=",
          "dtype": "f8",
          "shape": "4, 4"
         }
        }
       ],
       "layout": {
        "height": 450,
        "template": {
         "layout": {
          "font": {
           "color": "#e6edf3"
          },
          "paper_bgcolor": "#0d1117",
          "plot_bgcolor": "#0d1117",
          "xaxis": {
           "gridcolor": "#30363d",
           "zerolinecolor": "#30363d"
          },
          "yaxis": {
           "gridcolor": "#30363d",
           "zerolinecolor": "#30363d"
          }
         }
        },
        "title": {
         "text": "Kalman Filter Tuning: Final Position RMSE (lower is better)"
        },
        "width": 700,
        "xaxis": {
         "title": {
          "text": "Measurement Noise Multiplier (R)"
         }
        },
        "yaxis": {
         "title": {
          "text": "Process Noise Multiplier (Q)"
         }
        }
       }
      }
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\n",
      "=== TUNING GUIDANCE ===\n",
      "Best Q multiplier: 10.0\n",
      "Best R multiplier: 0.5\n",
      "Minimum RMSE: 0.0557 m\n",
      "\n",
      "Tuning Rule: Balance Q (trust model) vs R (trust sensors)\n",
      "Too high Q → filter ignores model → sluggish\n",
      "Too low Q → filter trusts model too much → diverges\n"
     ]
    }
   ],
   "source": [
    "# Parameter tuning: Grid search over Q and R values\n",
    "q_values = [0.01, 0.1, 1.0, 10.0]\n",
    "R_multiplier = [0.1, 0.5, 1.0, 2.0]\n",
    "\n",
    "results = np.zeros((len(q_values), len(R_multiplier)))\n",
    "\n",
    "for i, q_test in enumerate(q_values):\n",
    "    for j, r_mult in enumerate(R_multiplier):\n",
    "        # Adjust process and measurement noise\n",
    "        Q_test = q_test * Q / 0.1  # Normalize relative to default\n",
    "        R_test = R * r_mult\n",
    "        \n",
    "        # Run filter\n",
    "        x_tune = np.array([0.0, 0.0, 0.0, 0.0])\n",
    "        P_tune = np.eye(4) * 10.0\n",
    "        \n",
    "        for z in measurements:\n",
    "            pred = kf_predict(x_tune, P_tune, F, Q_test)\n",
    "            x_pred, P_pred = pred.x, pred.P\n",
    "            upd = kf_update(x_pred, P_pred, z, H, R_test)\n",
    "            x_tune, P_tune = upd.x, upd.P\n",
    "        \n",
    "        # Compute RMSE\n",
    "        pos_error = np.sqrt((np.array([x_tune[0], x_tune[2]]) - \n",
    "                             np.array([true_states[-1, 0], true_states[-1, 2]]))**2)\n",
    "        results[i, j] = np.mean(pos_error)\n",
    "\n",
    "# Create heatmap with Plotly\n",
    "hover_text = []\n",
    "for i in range(len(q_values)):\n",
    "    row = []\n",
    "    for j in range(len(R_multiplier)):\n",
    "        row.append(f\"Q mult: {q_values[i]}<br>R mult: {R_multiplier[j]}<br>RMSE: {results[i, j]:.4f}m\")\n",
    "    hover_text.append(row)\n",
    "\n",
    "fig = go.Figure(data=go.Heatmap(\n",
    "    z=results,\n",
    "    x=[f'{r:.1f}' for r in R_multiplier],\n",
    "    y=[f'{q:.2f}' for q in q_values],\n",
    "    colorscale='RdYlGn_r',\n",
    "    text=np.round(results, 3),\n",
    "    texttemplate='%{text:.3f}',\n",
    "    textfont={\"size\": 10},\n",
    "    hovertemplate='%{customdata}<extra></extra>',\n",
    "    customdata=hover_text,\n",
    "    colorbar=dict(title='RMSE (m)')\n",
    "))\n",
    "\n",
    "fig.update_layout(\n",
    "    template=dark_template,\n",
    "    title='Kalman Filter Tuning: Final Position RMSE (lower is better)',\n",
    "    xaxis_title='Measurement Noise Multiplier (R)',\n",
    "    yaxis_title='Process Noise Multiplier (Q)',\n",
    "    height=450,\n",
    "    width=700\n",
    ")\n",
    "\n",
    "fig.show()\n",
    "\n",
    "print(\"\\n=== TUNING GUIDANCE ===\")\n",
    "best_idx = np.unravel_index(np.argmin(results), results.shape)\n",
    "print(f\"Best Q multiplier: {q_values[best_idx[0]]}\")\n",
    "print(f\"Best R multiplier: {R_multiplier[best_idx[1]]}\")\n",
    "print(f\"Minimum RMSE: {results[best_idx]:.4f} m\")\n",
    "print(f\"\\nTuning Rule: Balance Q (trust model) vs R (trust sensors)\")\n",
    "print(f\"Too high Q → filter ignores model → sluggish\")\n",
    "print(f\"Too low Q → filter trusts model too much → diverges\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 5. Filter Comparison Summary\n",
    "\n",
    "### Performance Characteristics\n",
    "\n",
    "| Filter | Linearity | Order | Jacobian | Stability | Cost | Best For |\n",
    "|--------|-----------|-------|----------|-----------|------|----------|\n",
    "| **KF** | Linear | - | N/A | Good | Low | GPS, simple dynamics |\n",
    "| **EKF** | Nonlinear | 1st | Yes | Moderate | Low | Radar, mild nonlinearity |\n",
    "| **UKF** | Nonlinear | 2nd | No | Better | Low | Nonlinear sensors, maneuvering |\n",
    "| **SR-KF** | Linear | - | N/A | Excellent | Low | Long-running, ill-conditioned |\n",
    "| **SR-UKF** | Nonlinear | 2nd | No | Excellent | Low | Non-linear + numerical robustness |\n",
    "| **IMM** | Multi-model | Varies | Depends | Good | High | Target maneuvering modes |\n",
    "\n",
    "### Decision Tree for Filter Selection\n",
    "\n",
    "**Q: Are your system dynamics nonlinear?**\n",
    "- No → Use **Kalman Filter** ✓\n",
    "- Yes → Go to next question\n",
    "\n",
    "**Q: Do you need high accuracy or have ill-conditioned problems?**\n",
    "- Yes → Use **SR-UKF** (best robustness)\n",
    "- No → Go to next question\n",
    "\n",
    "**Q: Can you easily compute Jacobians?**\n",
    "- Yes → Use **EKF** (simpler, lower cost)\n",
    "- No → Use **UKF** (no Jacobians needed)\n",
    "\n",
    "### Key Tuning Parameters Across All Filters\n",
    "\n",
    "| Parameter | Effect | How to Tune |\n",
    "|-----------|--------|------------|\n",
    "| **Q** (Process noise) | ↑Q = trust model less | Start with 0.01 of state variance |\n",
    "| **R** (Measurement noise) | ↑R = trust sensors less | Use sensor specs, iterate if needed |\n",
    "| **P₀** (Initial covariance) | High = start uncertain | Use 10-100× expected error |\n",
    "| **α** (UKF spreading) | Affects sigma points | Typical: 0.001-0.01 for nearly linear |"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Exercises & Challenges\n",
    "\n",
    "### Exercise 1: Process Noise Sensitivity (level 1)\n",
    "**Objective**: Understand how process noise (Q) affects tracking performance\n",
    "\n",
    "**Task**: \n",
    "1. Increase `q` from 0.1 to 0.5, 1.0, and 5.0\n",
    "2. Run the KF for each value\n",
    "3. Plot convergence speed vs final estimate accuracy\n",
    "4. **Question**: What happens to the filter's responsiveness to target maneuvers?\n",
    "\n",
    "---\n",
    "\n",
    "### Exercise 2: Measurement Outlier Rejection (level 2)\n",
    "**Objective**: Make the filter robust to sensor failures\n",
    "\n",
    "**Task**:\n",
    "1. Inject 5 large outliers in `measurements` (e.g., 10× normal noise)\n",
    "2. Implement **Chi-squared gating**: Reject measurements where $(z - h(\\hat{x}))^T (HPH^T + R)^{-1} (z - h(\\hat{x})) > \\chi^2_{0.95}$\n",
    "3. Compare filter performance with/without gating\n",
    "4. **Challenge**: Can you make it adaptive (learn outlier statistics)?\n",
    "\n",
    "---\n",
    "\n",
    "### Exercise 3: Maneuvering Target Tracking (level 3)\n",
    "**Objective**: Handle nonlinear trajectories (circles, spirals)\n",
    "\n",
    "**Task**:\n",
    "1. Create ground truth that performs a **circular maneuver** (constant turn rate)\n",
    "2. Use coordinated turn rate model: $\\dot{x} = v \\cos(\\psi), \\dot{\\psi} = \\omega_z$\n",
    "3. Implement tracking with UKF vs EKF\n",
    "4. **Metric**: RMSE during turn vs straight-line segments\n",
    "5. **Deep dive**: Why does UKF outperform EKF here?\n",
    "\n",
    "---\n",
    "\n",
    "### Exercise 4: Consistency Check (NEES) (level 4)\n",
    "**Objective**: Verify the filter is consistent (uncertainty estimates match actual errors)\n",
    "\n",
    "**Task**:\n",
    "1. Compute the Normalized Estimation Error Squared (NEES): \n",
    "   $$\\epsilon_k = (x_k - \\hat{x}_k)^T P_k^{-1} (x_k - \\hat{x}_k)$$\n",
    "2. Ideally, $\\epsilon_k \\sim \\chi^2_4$ (4-dimensional)\n",
    "3. Run a Monte Carlo simulation (100 trials of the full scenario)\n",
    "4. Plot histogram of NEES values; check if it matches $\\chi^2$ distribution\n",
    "5. **Interpretation**: If filter is under-confident, NEES ≫ 4; if over-confident, NEES ≪ 4\n",
    "\n",
    "---\n",
    "\n",
    "### Exercise 5: Multi-Sensor Fusion (level 4)\n",
    "**Objective**: Combine multiple sensors with different noise profiles\n",
    "\n",
    "**Task**:\n",
    "1. Create a **GPS sensor** (slow, accurate): $R_{GPS} = 1.0$ (1m std)\n",
    "2. Create a **dead-reckoning system** (fast, drifts): $R_{DR} = 10.0$ (10m std)  \n",
    "3. Implement a filter that accepts measurements from both at different rates\n",
    "4. **Bonus**: Use an Interacting Multiple Model (IMM) filter to handle GPS outages\n",
    "5. **Real-world scenario**: GPS loss in tunnel → rely on dead-reckoning during gap\n",
    "\n",
    "---\n",
    "\n",
    "## Key Takeaways\n",
    "\n",
    "✅ **Kalman filters are optimal for linear-Gaussian systems**  \n",
    "✅ **EKF extends to nonlinear systems via linearization**  \n",
    "✅ **UKF better handles highly nonlinear systems without Jacobians**  \n",
    "✅ **Square-root variants improve numerical stability**  \n",
    "✅ **Filter tuning (Q, R) is an art—validate with real data**  \n",
    "✅ **Consistency checks (NEES) verify filter correctness**  \n",
    "\n",
    "## Next Steps\n",
    "\n",
    "→ Explore **covariance intersection** for decentralized fusion  \n",
    "→ Study **particle filters** for non-Gaussian distributions  \n",
    "→ Implement **IMM filters** for maneuvering target tracking"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## References & Further Reading\n",
    "\n",
    "### Core Textbooks\n",
    "1. **Bar-Shalom, Y., Li, X. R., & Kirubarajan, T.** (2001). *Estimation with Applications to Tracking and Navigation: Theory, Algorithms and Software*. Wiley. \n",
    "   - **Coverage**: The definitive reference, covers KF, EKF, measurement-to-track association\n",
    "   - **Best for**: Advanced practitioners, comprehensive theory\n",
    "\n",
    "2. **Simon, D.** (2006). *Optimal State Estimation: Kalman, H∞, and Nonlinear Approaches*. John Wiley & Sons.\n",
    "   - **Coverage**: KF, EKF, UKF, H∞ filtering, practical implementation\n",
    "   - **Best for**: Engineers wanting both theory and practical guidance\n",
    "\n",
    "3. **Welch, G., & Bishop, G.** (2006). *An Introduction to the Kalman Filter*. UNC Chapel Hill (free PDF).\n",
    "   - **Coverage**: Gentle introduction, intuitively explained\n",
    "   - **Best for**: Students, beginners\n",
    "\n",
    "### Advanced Topics\n",
    "4. **Sarkka, S.** (2013). *Bayesian Filtering and Smoothing*. Cambridge University Press.\n",
    "   - **Coverage**: Probabilistic interpretation, sequential filtering, GPU-friendly algorithms\n",
    "   - **Best for**: Researchers, particle filters, GPU implementation\n",
    "\n",
    "5. **Kleeman, L.** (1995). *Understanding and Applying Kalman Filtering*. \n",
    "   - **Coverage**: EKF applications, real-world tuning advice\n",
    "   - **Best for**: Practitioners in robotics/navigation\n",
    "\n",
    "### Key Papers\n",
    "- **Julier, S. J., & Uhlmann, J. K.** (2004). \"Unscented Filtering and Nonlinear Estimation.\" *Proceedings of the IEEE*, 92(3), 401-422.\n",
    "- **Bierman, G. J.** (1977). \"Factorization Methods for Discrete Sequential Estimation.\" Academic Press.\n",
    "  - **Note**: Foundational work on square-root filters\n",
    "\n",
    "### PyTCL Library Documentation\n",
    "- **Dynamic Estimation Module**: See `pytcl.dynamic_estimation` for available filter implementations\n",
    "- **API Reference**: Full documentation at [NRL Tracker GitHub](https://github.com/nedonatelli/TCL)\n",
    "- **Example Gallery**: Additional examples in `examples/` directory\n",
    "\n",
    "### Online Resources\n",
    "- **MIT OpenCourseWare**: 6.041 Probabilistic Systems Analysis (free lectures on Bayesian inference)\n",
    "- **Andrew Ng's Machine Learning Course**: Covers HMMs and Kalman filters intuitively\n",
    "- **MATLAB Documentation**: Excellent visual guides on Kalman filter concepts\n",
    "\n",
    "### Recommended Learning Path\n",
    "\n",
    "```\n",
    "1. Week 1: Linear Kalman Filter\n",
    "   → Read: Welch & Bishop introduction\n",
    "   → Code: Constant velocity 1D tracking\n",
    "   → Exercise 1: Tune Q and R by hand\n",
    "\n",
    "2. Week 2: Extended Kalman Filter\n",
    "   → Read: Simon Ch. 6 on EKF\n",
    "   → Code: Nonlinear radar measurements\n",
    "   → Exercise 3: Compare EKF vs UKF\n",
    "\n",
    "3. Week 3: Unscented Kalman Filter\n",
    "   → Read: Julier & Uhlmann paper\n",
    "   → Code: Highly nonlinear system\n",
    "   → Exercise: Verify NEES consistency\n",
    "\n",
    "4. Week 4: Applications\n",
    "   → Read: Real-world case studies\n",
    "   → Code: Multi-sensor fusion (Exercise 5)\n",
    "   → Project: Your own tracking application\n",
    "```"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.13.0"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}