sample-lecture.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# MA-MOROB-M02: Trajectory Estimation (Semester 1)\n",
    "\n",
    "*part of the Programme \"Mobile Robotics\" (M.Sc.) at Bonn University*\n",
    "\n",
    "## Methods for high precision trajectory estimation of mobile mapping vehicles on crop fields"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "## Practicalities\n",
    "\n",
    "* All materials for students are available at https://github.com/marc-hanheide/trajectory_estimation_lecture\n",
    "* In this module we'll be using ROS2 and Jupyter Notebooks extensively (all lecture materials)\n",
    "* Students will work with provided DevContainers (Docker) for the exercises, with ROS2 and Jupyter pre-installed (familiarise yourself with these technologies)\n",
    "\n",
    "### Today\n",
    "\n",
    "* Slides available at https://bonn.zrok.lcas.group/sample-lecture.slides.html#/ for your own browsing\n",
    "* You can open examples in Google Colab\n",
    "\n",
    "  <a target=\"_blank\" href=\"https://colab.research.google.com/github/marc-hanheide/trajectory_estimation_lecture\">\n",
    "    <img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/>\n",
    "  </a>\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "## The Need for High Precision Trajectory Estimation (in Agricultural Mapping)\n",
    "\n",
    "<div align=\"center\">\n",
    "    <img src=\"./assets/trajectory-need-illustration.svg\" width=\"40%\"/>\n",
    "</div>\n",
    "\n",
    "- **Positional Accuracy**: Even small deviations from planned trajectories can lead to significant consequences:\n",
    "  - **Overlapping treatment**: Wasting resources (fertilizer, pesticides, seeds)\n",
    "  - **Missed areas**: Reducing yield and creating uneven crop development\n",
    "  - **Crop damage**: Physical damage from vehicles moving off designated paths\n",
    "\n",
    "- **Mapping Quality**: Poor trajectory estimation directly impacts the quality of collected data, affecting:\n",
    "  - Precise crop monitoring (e.g. tracking individual crops over time)\n",
    "  - Accurate field mapping\n",
    "  - Reliable yield prediction models\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "source": [
    "## A Concrete Introductory Problem to look at\n",
    "\n",
    "<!--\n",
    "* Add robot data recording video here\n",
    "* explain to want to come back multiple times, monitor crop growth\n",
    "* introduce the problem of robot localisation\n",
    "* ask about other scenarios where precision localisation is needed\n",
    "* explain the simplicity of a row (1D) in the strawberry tunnel case\n",
    "-->\n",
    "\n",
    "<div align=\"middle\">\n",
    "<video width=\"80%\" controls autoplay loop muted>\n",
    "      <source src=\"assets/thorvald-data.mp4\" type=\"video/mp4\">\n",
    "</video></div>\n",
    "\n",
    "<center>An Example of Crop Monitoring with a Mobile Robot</center>\n",
    "\n",
    "**Large UK Strawberry Producer: \"I want to monitor the growth of my individual strawberry plants over time, identifying underperforming ones\"**\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "source": [
    "## A Crop Monitoring Robot\n",
    "\n",
    "<div align=\"center\">\n",
    "    <img src=\"./assets/thorvald-cameras.jpg\" width=\"25%\" style=\"float: right;\"/>\n",
    "</div>\n",
    "\n",
    "<div>\n",
    "\n",
    "### Robot Specifications\n",
    "* 4 wheel drive, 4 wheel steering\n",
    "* Multiple Cameras, including Hyperspectral\n",
    "* LiDAR\n",
    "* GPS\n",
    "* IMU\n",
    "* Wheel Odometry\n",
    "\n",
    "### Simple?\n",
    "* drive down a row\n",
    "* use sensors to detect and memorise 3D position of each crop\n",
    "* go back and relocate the crops again\n",
    "\n",
    "### (Some) Challenges:\n",
    "* reliable execution of \"driving down a row\"\n",
    "* GPS accuracy?\n",
    "* relocate a crop again in a changed environment,\n",
    "* basically: **How is our sensor (platform) moving??**\n",
    "\n",
    "</div>\n",
    "\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "source": [
    "## The problem of not moving as one thinks\n",
    "\n",
    "<div align=\"middle\">\n",
    "<video width=\"100%\" controls autoplay loop muted>\n",
    "      <source src=\"assets/odom-failure.mp4\" type=\"video/mp4\">\n",
    "</video></div>\n",
    "\n",
    "<img src=\"assets/question.svg\" align=\"right\" width=\"100px\">\n",
    "\n",
    "**What could be the reason here?**\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "## Module \"Trajectory Estimation\" -- Learning Objectives\n",
    "\n",
    "1. **Understand and Apply**: Demonstrate a comprehensive understanding of theoretical principles underlying trajectory estimation, including coordinate systems, sensor models, and state estimation techniques.\n",
    "2. **Analyse and Evaluate**: Critically analyse various trajectory estimation methods and evaluate their suitability for different environments and applications.\n",
    "3. **Design and Implement**: Design and implement appropriate sensor fusion algorithms for accurate trajectory estimation in challenging environments.\n",
    "4. **Evaluate Performance**: Assess the performance of trajectory estimation systems using appropriate metrics and validation techniques.\n",
    "5. **Problem Solve**: Develop solutions for trajectory estimation in environments with specific challenges, such as GPS-denied areas, dynamic obstacles, or varying terrain.\n",
    "6. **Research and Innovate**: Demonstrate awareness of current research trends and innovations in trajectory estimation for mobile sensing platforms.\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "source": [
    "## Draft Module Syllabus\n",
    "\n",
    "### Fundamentals of Trajectory Estimation and Problem Introduction\n",
    "- Coordinate systems and transformations (global vs. local)\n",
    "- Introduction to state representation and rigid body kinematics ($SE(3)$)\n",
    "- Applications and challenges in trajectory estimation (example Crop Monitoring and Precision Agriculture)\n",
    "- Precision vs. accuracy in different application domains\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "source": [
    "### Global Localisation for Trajectory Estimation\n",
    "- GNSS/GPS principles and error sources\n",
    "- RTK and PPK techniques for high-precision positioning\n",
    "- GNSS limitations in various environments\n",
    "\n",
    "### Local Sensors for Trajectory Estimation\n",
    "- Proprioception: \n",
    "  - Inertial Measurement Units (IMUs): principles and types\n",
    "  - Odometry sensors (wheel encoders, visual odometry)\n",
    "- Exteroception: Environmental sensors (LiDAR, cameras, radar)\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "source": [
    "\n",
    "### Sensor Fusion & Estimation Theory\n",
    "- Probabilistic state estimation fundamentals\n",
    "- Kalman filtering: theory and implementation\n",
    "- Kalman Filter variants (EKF, UKF)\n",
    "- Particle filters and Monte Carlo methods\n",
    "- Smoothing techniques vs. filtering techniques\n",
    "\n",
    "### Multi-Sensor Calibration\n",
    "- Calibration of sensor arrays\n",
    "- Online calibration techniques\n",
    "- Sensor degradation detection and handling\n",
    "- Quality metrics and validation approaches\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "source": [
    "### Advanced Topics in Trajectory Estimation\n",
    "- Deep learning approaches for state estimation\n",
    "- Place recognition and loop closure techniques\n",
    "- Long-term autonomy \n",
    "- Distributed (multi-agent) trajectory estimation\n",
    "\n",
    "### Real World Applications and Limitations\n",
    "- Real-time implementation considerations and computational complexity\n",
    "- Methods for GPS-denied environments\n",
    "- Dynamic (slow and fast) changing environments\n",
    "- Example Applications\n",
    "    - Infrastructure inspection\n",
    "    - Autonomous agricultural vehicles and precision farming\n",
    "    - Indoor mobile robots and warehouse automation\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "source": [
    "## Assessment\n",
    "\n",
    "\n",
    "<img width=\"40%\" src=\"./assets/gazebo.png\" style=\"float: right; margin: 5px;\">\n",
    "\n",
    "* Oral Examination 50%\n",
    "    - 20 minutes oral examination on the theoretical principles of trajectory estimation\n",
    "    - indicative topics: coordinate systems, sensor models, state estimation techniques, sensor fusion, calibration, and validation\n",
    "* Coursework, 50%\n",
    "    - Handling different real-world sensor data sets (IMU, LiDAR, camera, GPS)\n",
    "    - Implementing a selection of trajectory estimation algorithms\n",
    "    - Comparative scientific evaluation of their performance\n",
    "    - Implementing a trajectory estimation system in ROS2 (simulation in devcontainer)\n",
    "    - Presentation of results and discussion of findings (assessed)\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "## Foundations: Mathematical Representation of a 3D Trajectory\n",
    "\n",
    "**A robot trajectory refers to the path that a rigid body (robot) follows through space over time**\n",
    "\n",
    "Includes both the position and orientation, referred to as \"Poses\" (position + orientation).\n",
    "\n",
    "\n",
    "<div align=\"middle\">\n",
    "<img width=\"80%\" src=\"./assets/trajectory-illustration.svg\">\n",
    "</div>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "source": [
    "## Special Euclidean Group SE(3)\n",
    "\n",
    "The most comprehensive representation of any rigid body trajectory is through the Special Euclidean Group in 3 dimensions, denoted as $SE(3)$. \n",
    "\n",
    "This representation captures both the position (translation) and orientation (rotation) of a rigid body in 3D space.\n",
    "\n",
    "Mathematically, SE(3) can be represented as a 4×4 matrix:\n",
    "\n",
    "$$\n",
    "\\begin{pmatrix} \n",
    "R & \\vec{t} \\\\\n",
    "0 & 1 \n",
    "\\end{pmatrix}\n",
    "$$\n",
    "\n",
    "Where:\n",
    "- $R$ is a 3×3 rotation matrix that belongs to the Special Orthogonal Group SO(3)\n",
    "- $\\vec{t}$ is a 3×1 translation vector\n",
    "- The bottom row completes the homogeneous transformation\n",
    "\n",
    "*Note:* A Rotation can be represented in many other forms (Euler angles, RPY, Quaternions, etc.)\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "source": [
    "# Some Important Properties of Special Euclidean Groups SE(N)\n",
    "\n",
    "- **3D Rigid Body Transformations**: SE(3) represents all possible rigid body transformations (rotations and translations) in 3D space\n",
    "  \n",
    "- **Structure**:\n",
    "  - Degrees of Freedom of $SE(n)$: $DoF_{SE(n)} = DoF_{SO(n)} + n = [n(n-1)/2] + n = n(n+1)/2$\n",
    "    - In 1D: 1 degree of freedom (0 for rotation, 1 for translation)\n",
    "    - In 2D: 3 degrees of freedom (2 for rotation, 1 for translation)\n",
    "    - In 3D: 6 degrees of freedom (3 for rotation, 3 for translation) \n",
    "  \n",
    "- **Key Properties**:\n",
    "  - **Group Structure**: Closed under composition (combining two transformations yields another valid transformation)\n",
    "  - **Invertibility**: Every transformation has an inverse\n",
    "  - **Non-commutativity**: Order matters ($T_1 \\cdot T_2 \\neq T_2 \\cdot T_1$)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "source": [
    "## Time Parameterisation\n",
    "\n",
    "A complete trajectory representation includes time as a parameter. This can be written as:\n",
    "\n",
    "$$T(t) \\in SE(3)$$\n",
    "\n",
    "Where $T(t)$ represents the pose (position and orientation) of the robot at time $t$.\n",
    "\n",
    "\n",
    "\n",
    "In practical robotics applications, trajectories are often represented as:\n",
    "\n",
    "1. **Discrete Waypoints**: A sequence of poses $(T_1, T_2, ..., T_n)$ that the robot should pass through at specific times $(t_1, t_2, ..., t_n)$. Often also displayed as arrows (quivers) in 3D space.\n",
    "2. **Parametric Curves**: Such as splines or polynomials that smoothly interpolate between waypoints.\n",
    "3. **Joint Space Trajectories**: For robot manipulators, trajectories are often represented in joint space rather than Cartesian space, as a time-varying vector of joint angles $θ(t)$.\n",
    "\n",
    "<div align=\"middle\">\n",
    "<video width=\"30%\" controls autoplay loop muted>\n",
    "      <source src=\"assets/odom-failure.mp4\" type=\"video/mp4\">\n",
    "</video></div>\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "source": [
    "## Relevance for Robotics more generally\n",
    "\n",
    "* This representation allows us to transform poses (and points) from one coordinate frame to another\n",
    "* In Robotics often need to relate poses between different reference frames (e.g., robot base frame, world frame, end-effector frame)\n",
    "    * trajectory estimation\n",
    "    * calibration\n",
    "\n",
    "<img src=\"./assets/frames.svg\" width=\"90%\"/>\n",
    "\n",
    "In ROS, rigid body transformation are represented in the **`tf2` framwork**. \n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "source": [
    "## Workshop Session Exercises:\n",
    "1. Familiarise yousself with the use of the ROS2 DevContainer setup.\n",
    "1. Start the provided simulation and display the \"tf tree\". What does each frame represent? Identify the transformation between the \"base_link\" and the \"front_camera\" frame.\n",
    "1. You are given a partial \"tf tree\": `odom -> base_link -> front_camera_link`. Assume the transformation between `odom` and `base_link` is known as $T_{o\\_bl}$. \n",
    "   And the transformation between `base_link` and `front_camera_link` has been determined through calibration as\n",
    "   \n",
    "    $$\n",
    "    \\begin{pmatrix} \n",
    "    1 & 0 & 0 & 2 \\\\\n",
    "    0 & 0 & -1 & 0 \\\\\n",
    "    0 & 1 & 0 & 1 \\\\ \n",
    "    0 & 0 & 0 & 1\n",
    "    \\end{pmatrix}\n",
    "    $$\n",
    "\n",
    "    (in homogneous coordinates). Now assume a a fruit has been detected which the RGBD sensor at position $\\vec{p}=(0.3\\ 0.4\\ 2.1)^T$. How could you determine the fruit's position in the `odom` frame?\n",
    "    Work on paper and then with ROS2 code.\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# Trajectory Estimation as a State Estimation Problem\n",
    "\n",
    "As seen before, we may not know where we actually are (acurately enough), i.e., the *true state* is not known. It is *hidden*. \n",
    "\n",
    "Typically, this is framing it as a *State Estimation Problem*.\n",
    "\n",
    "- **Definition**: Trajectory estimation can be formulated as a state estimation problem, where we recursively estimate the vehicle's state (position, orientation, velocities) over time\n",
    "- **Uncertainty Propagation**: The robot's motion inherently introduces uncertainty (e.g. control errors, wheel slip or uneven terrain in agricultural environments); likewise each sensor will have uncertainty\n",
    "- **Recursive Bayesian Estimation**: The problem can be approached through probabilistic frameworks that maintain a belief distribution over possible states\n",
    "- **Sensor Fusion Opportunity**: Multiple sensor modalities (GNSS, IMU, LiDAR, visual odometry) can be integrated to improve accuracy and robustness\n",
    "- **Computational Constraints**: Real-time operation on mobile platforms requires efficient algorithms that balance accuracy with computational resources\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "source": [
    "\n",
    "## Bayesian Filtering for State Estimation\n",
    "\n",
    "- **Bayesian Approach**: Maintains probability distributions over possible states, handling uncertainty in both:\n",
    "\n",
    "  <div align=\"center\">\n",
    "    <img width=\"30%\" src=\"./assets/pgm-models.svg\" style=\"float: right; margin: 5px;\">\n",
    "  </div>\n",
    "\n",
    "  - **Process Model**: How the vehicle moves (kinematic equations)\n",
    "  - **Measurement Model**: What sensors observe (GNSS, IMU, visual features)\n",
    "\n",
    "- **Recursive Structure**: Each update incorporates:\n",
    "  - **Prediction Step**: Project state forward using motion model\n",
    "  - **Correction Step**: Update state belief using sensor measurements\n",
    "\n",
    "- **Kalman Filter**: Optimal solution when:\n",
    "  - Systems are linear (or linearizable)\n",
    "  - Noise is Gaussian\n",
    "\n",
    "- **Particle Filter**: Non-parametric approach when:\n",
    "  - Non-linear dynamics are more significant\n",
    "  - Non-Gaussian noise may exist\n",
    "\n",
    "- These algorithms compute $P(x_t | y_{1:t})$ - the probability of the current state $x_t$ given all observations $y_{1:t}$ up to now ($t$)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "source": [
    "### State Estimation Process\n",
    "\n",
    "State estimation typically follows these steps:\n",
    "\n",
    "1. **Prediction Step (Motion Model)**: When a robot moves, uncertainty *increases*.\n",
    "   \n",
    "   $$p(x_t|u_t,x_{t-1}) $$\n",
    "   \n",
    "   This transition model describes how state $x$ changes when control $u$ is applied. $x_{t-1}$ being the previous state.\n",
    "\n",
    "2. **Update Step (Measurement Model)**: When sensors provide data (information!), uncertainty *decreases*.\n",
    "   \n",
    "   $$p(z_t|x_t)$$\n",
    "   \n",
    "   This *sensor model* describes the probability of getting measurement $z_t$ in the current (hidden!) state $x_t$.\n",
    "\n",
    "3. **Belief Update**: Combining the above using Bayes' rule.\n",
    "   \n",
    "   $$bel(x_t) = \\eta \\cdot p(z_t|x_t) \\int p(x_t|u_t,x_{t-1}) \\cdot bel(x_{t-1}) \\, dx_{t-1}$$\n",
    "   \n",
    "   Where $\\eta$ is a normalizing constant and $bel(x_t)$ is the belief at time $t$.\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "notes"
    }
   },
   "source": [
    "### Bayes Rule Reminder\n",
    "\n",
    "$$\n",
    "p(x|z) = \\frac{p(z|x) \\cdot p(x)}{p(z)}\n",
    "$$\n",
    "\n",
    "Where:\n",
    "- $p(x)$ is the prior belief about state $x$\n",
    "- $p(z|x)$ is the likelihood of getting measurement $z$ given state $x$\n",
    "- $p(x|z)$ is the posterior belief after incorporating measurement $z$\n",
    "- $p(z)$ is a normalizing factor\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "source": [
    "## Simple Example: Robot Localization\n",
    "\n",
    "Let's consider a simple 1D robot moving along a line, with:\n",
    "- A motion model that attempts to move forward e.g 1m (in a tunnel) each step but has noise\n",
    "- A sensor that can detect a pole (as a landmark) in the tunnel with some uncertainty\n",
    "\n",
    "### Initial Belief\n",
    "The robot starts with a uniform belief about its position - it could be anywhere with equal probability.\n",
    "\n",
    "\n",
    "<div align=\"middle\">\n",
    "<img width=\"80%\" src=\"./assets/bayesian-filtering-illustration_0.svg\">\n",
    "</div>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "source": [
    "\n",
    "### Prediction Step\n",
    "The robot attempts to move forward 1 unit by applying motor controls. Due to uncertainty in motion:\n",
    "- If it's at position $x$ now, it might end up at $x+0.8$, $x+1.0$, or $x+1.2$ with different probabilities\n",
    "- This \"smears\" the belief forward and increases uncertainty: $bel_{pred}(x_t) = \\int p(x_t|u_t,x_{t-1}) \\cdot bel(x_{t-1}) \\, dx_{t-1}$\n",
    "\n",
    "<div align=\"middle\">\n",
    "<img width=\"80%\" src=\"./assets/bayesian-filtering-illustration_1.svg\">\n",
    "</div>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "source": [
    "\n",
    "### Update Step\n",
    "The robot takes a sensor measurement and detects a pole 2 metres away, but with some uncertainty: $bel(x_t) = \\eta \\cdot p(z_t|x_t) \\cdot bel_{pred}(x_t)$\n",
    "\n",
    "If the sensor is perfect, positions exactly 2 units from the pole would have probability 1.0, and all others 0.0. But with noise:\n",
    "- Positions 1.8-2.2 units from the pole might have high probability\n",
    "- Positions further away have lower probability\n",
    "\n",
    "\n",
    "<div align=\"middle\">\n",
    "<img width=\"80%\" src=\"./assets/bayesian-filtering-illustration.svg\">\n",
    "</div>\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "source": [
    "## Particle Filter Implementation Example\n",
    "\n",
    "A particle filter would represent this belief using discrete samples (particles):\n",
    "\n",
    "\n",
    "This simple example demonstrates how a robot can:\n",
    "1. Start with high uncertainty\n",
    "2. Move and increase uncertainty further\n",
    "3. Take measurements to reduce uncertainty\n",
    "4. Combine these to arrive at a better state estimate\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "2bcc826855044b5eb84a3664102a7bc7",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "HBox(children=(VBox(children=(IntSlider(value=5000, description='Particles:', max=10000, min=100, step=100), F…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "import ipywidgets as widgets\n",
    "from IPython.display import display\n",
    "\n",
    "\n",
    "# Function to calculate estimated position (weighted average)\n",
    "def calculate_estimated_position(particles, weights):\n",
    "    return np.sum(particles * weights)\n",
    "\n",
    "# Modify the run_simulation function to return and display the estimated position\n",
    "def run_simulation(num_particles, move_distance, move_noise, measurement_noise, pole_distance, measurement):\n",
    "    # Initialize particles (positions) with uniform distribution around 2 intervals: 0-1 and 5-6,\n",
    "    # illustrating a bimodal distribution (a Kalman filter wouldn't be able to handle)\n",
    "    particles = np.random.uniform(0, 1, num_particles//2)  # half the particles from 0-1\n",
    "    particles = np.concatenate([\n",
    "        particles,  # First half from 0-1\n",
    "        np.random.uniform(5, 6, num_particles//2)  # the other half of particles from 5-6\n",
    "    ])\n",
    "    \n",
    "    # Create figure for plotting\n",
    "    plt.figure(figsize=(10, 6))\n",
    "    \n",
    "    # Initial belief\n",
    "    plt.subplot(3, 1, 1)\n",
    "    plt.hist(particles, bins=100, color='b', density=True)\n",
    "    plt.title(\"Initial Belief (Uniform between 0-10)\")\n",
    "    plt.xlim(0, 20)\n",
    "    \n",
    "    # After movement (prediction step)\n",
    "    particles = particles + move_distance + np.random.normal(0, move_noise, len(particles))\n",
    "    plt.subplot(3, 1, 2)\n",
    "    plt.hist(particles, bins=100, density=True, color='y')\n",
    "    plt.title(f\"After Movement (Prediction Step): Move {move_distance} units with noise {move_noise:.2f}\")\n",
    "    plt.xlim(0, 20)\n",
    "    # Add lines to show the pole position and measured distance\n",
    "    plt.axvline(x=pole_distance, color='k', linestyle='-', label=f'Pole at {pole_distance}')\n",
    "    plt.axvline(x=pole_distance - measurement, color='m', linestyle=':', label=f'Expected robot from meaurement: {pole_distance - measurement:.2f}')\n",
    "    plt.legend()\n",
    "\n",
    "\n",
    "    # Calculate weights based on measurement (update step)\n",
    "    expected_measurements = pole_distance - particles\n",
    "    weights = np.exp(-0.5 * ((expected_measurements - measurement) / measurement_noise)**2)\n",
    "    weights = weights / sum(weights)  # Normalize weights\n",
    "    \n",
    "    # Measures to capture the \"compatibility\" of our measurement given the prediction:\n",
    "    # Calculate effective sample size (ESS)\n",
    "    ess = 1.0 / np.sum(weights**2)\n",
    "    ess_percentage = 100 * ess / num_particles\n",
    "    print(f\"Effective Sample Size: {ess:.1f}/{num_particles} ({ess_percentage:.1f}%)\")\n",
    "\n",
    "    # Calculate entropy of the weights\n",
    "    entropy = -np.sum(weights * np.log(weights + 1e-10))  # Adding small epsilon to avoid log(0)\n",
    "    entropy_normalized = entropy / np.log(num_particles)  # Normalize by maximum possible entropy\n",
    "    print(f\"Normalized Entropy: {entropy_normalized:.3f} (higher = more uncertain)\")\n",
    "\n",
    "    # Calculate estimated position\n",
    "    estimated_pos = calculate_estimated_position(particles, weights)\n",
    "    \n",
    "    # Resample particles based on weights (low weight particles will be replaced by high weight particles)\n",
    "    resampled_indices = np.random.choice(range(num_particles), num_particles, p=weights)\n",
    "    particles = particles[resampled_indices]\n",
    "    \n",
    "    plt.subplot(3, 1, 3)\n",
    "    plt.hist(particles, bins=100, color='g', density=True)\n",
    "    plt.title(f\"After Sensor Update: ESS={ess_percentage:.1f}%; Entropy={entropy_normalized:.3f}\")\n",
    "    plt.axvline(x=estimated_pos, color='r', linestyle='--', label=f'Estimated Position: {estimated_pos:.2f}')\n",
    "    plt.xlim(0, 20)\n",
    "    plt.legend()\n",
    "\n",
    "\n",
    "    plt.tight_layout()\n",
    "    plt.show()\n",
    "\n",
    "    \n",
    "    return estimated_pos\n",
    "\n",
    "# Create interactive widgets\n",
    "num_particles_slider = widgets.IntSlider(value=5000, min=100, max=10000, step=100, description='Particles:')\n",
    "move_distance_slider = widgets.FloatSlider(value=1.0, min=0.0, max=20.0, step=0.5, description='Move:')\n",
    "move_noise_slider = widgets.FloatSlider(value=0.5, min=0.1, max=5.0, step=0.1, description='Move Noise:')\n",
    "measurement_noise_slider = widgets.FloatSlider(value=0.2, min=0.001, max=5.0, step=0.1, description='Meas. Noise:')\n",
    "pole_distance_slider = widgets.FloatSlider(value=8.0, min=5.0, max=20.0, step=1.0, description='Pole Pos:')\n",
    "measurement_slider = widgets.FloatSlider(value=1.5, min=0.0, max=20.0, step=0.5, description='Meas. Dist:')\n",
    "\n",
    "# Create the interactive output\n",
    "interactive_output = widgets.interactive_output(run_simulation, {\n",
    "    'num_particles': num_particles_slider,\n",
    "    'move_distance': move_distance_slider,\n",
    "    'move_noise': move_noise_slider,\n",
    "    'measurement_noise': measurement_noise_slider,\n",
    "    'pole_distance': pole_distance_slider,\n",
    "    'measurement': measurement_slider\n",
    "})\n",
    "\n",
    "# Display widgets and output\n",
    "controls = widgets.VBox([\n",
    "    num_particles_slider, \n",
    "    move_distance_slider, \n",
    "    move_noise_slider, \n",
    "    measurement_noise_slider, \n",
    "    pole_distance_slider, \n",
    "    measurement_slider\n",
    "])\n",
    "\n",
    "display(widgets.HBox([controls, interactive_output]))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "## Next Steps\n",
    "\n",
    "* Read Chapter 2 of Probabilistic Robotics (Thrun et al., 2005) for a more detailed introduction to Bayesian filtering\n",
    "* Explore the Particle Filter implementation and the simple Kalman Filter example on Colab\n",
    "* Expand the initial implementation to handle 2D motion and sensor measurements in $SE(2)$\n",
    "* Try out the ROS DevContainer, start the simulation, and explore the tf tree\n",
    "* Work on the workshop exercises to apply the concepts learned today"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": ".venv",
   "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.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}