diff --git a/labs/bonus2/kalmanfilter.ipynb b/labs/bonus2/kalmanfilter.ipynb new file mode 100644 index 0000000..f426503 --- /dev/null +++ b/labs/bonus2/kalmanfilter.ipynb @@ -0,0 +1,616 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# Bonus Lab: Kalman Filtering\n", + "\n", + "Parameter estimation for robotics can be challenging due to noisy sensor data and inexact motion models. Kalman filtering is a technique that uses properties of the **Gaussian distribution** to minimize error in the state of an agent. The Kalman filter uses a motion model to predict the future state of the agent, and corrects these predictions based on sensor measurements with **Bayes Theorem**.\n", + "\n", + "In this notebook, we will learn how to use kalman filtering to more effectively estimate the state of the RACECAR. Here, you will estimate the distance of an orange cone, as you did in Lab 3b.\n", + "\n", + "Throughout this notebook, **text in bold red** indicates a change you must make to the following code block before running it.\n", + "\n", + "\n", + "## Table of Contents\n", + "1. [Getting Started](#GettingStarted)\n", + "2. [Depth and Color Images](#DepthAndColorImages)\n", + "3. [Uncertainty](#Uncertainty)\n", + "4. [Prediction](#Prediction)\n", + "5. [Implementation](#Implementation)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "\n", + "## 1. Getting Started\n", + "\n", + "**If you are running the car in RacecarSim, set `isSimulation` to `True`**. Leave `isSimulation` `False` if you are using a physical car." + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": {}, + "outputs": [], + "source": [ + "# TODO: Update isSimulation if necessary\n", + "isSimulation = True" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Next, we will import the necessary libraries for this notebook, including Python libraries (`cv`, `numpy`, etc.) and the Racecar library (`racecar_core`)." + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": {}, + "outputs": [], + "source": [ + "# Import Python libraries\n", + "import cv2 as cv\n", + "import numpy as np\n", + "import matplotlib.pyplot as plt\n", + "import ipywidgets as widgets\n", + "import statistics\n", + "from nptyping import NDArray\n", + "from typing import Any, Tuple, List, Optional\n", + "\n", + "# Import Racecar library\n", + "import sys\n", + "sys.path.append(\"../../library\")\n", + "import racecar_core\n", + "import racecar_utils as rc_utils" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "The following functions will help us throughout this notebook." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [], + "source": [ + "def show_depth_image(\n", + " depth_image: NDArray[(Any, Any), np.float32],\n", + " max_depth: int = 400,\n", + " points: List[Tuple[int, int]] = []\n", + ") -> None:\n", + " \"\"\"\n", + " Displays a color image in the Jupyter Notebook.\n", + " \n", + " Args:\n", + " depth_image: The image to display.\n", + " max_depth: The farthest depth to show in the image in cm. Anything past this depth is shown as black.\n", + " points: A list of points in (pixel row, pixel column) format to show on the image colored dots.\n", + " \"\"\"\n", + " # Clip anything above max_depth\n", + " np.clip(depth_image, None, max_depth, depth_image)\n", + "\n", + " # Shift down slightly so that 0 (no data) becomes the \"farthest\" color\n", + " depth_image = (depth_image - 1) % max_depth\n", + "\n", + " # Convert depth values to colors\n", + " color_image = cv.applyColorMap(-cv.convertScaleAbs(depth_image, alpha=255/max_depth), cv.COLORMAP_INFERNO)\n", + " \n", + " # Draw a dot at each point in points\n", + " for point in points:\n", + " cv.circle(color_image, (point[1], point[0]), 6, (0, 255, 0), -1)\n", + "\n", + " # Show the image with Matplotlib\n", + " plt.imshow(cv.cvtColor(color_image, cv.COLOR_BGR2RGB))\n", + " plt.show()\n", + "\n", + "\n", + "def add_noise(\n", + " depth_image: NDArray[(Any, Any), np.float32],\n", + " error_percent = 0.1,\n", + " null_percent: float = 0.005\n", + ") -> NDArray[(Any, Any), np.float32]:\n", + " \"\"\"\n", + " Adds noise to a depth image.\n", + " \n", + " Args:\n", + " depth_image: The original image to which to add noise.\n", + " error_percent: The error percentage to introduce to each measurement.\n", + " null_percent: The percentage of pixels to set to zero.\n", + " \n", + " Returns:\n", + " A copy of the provided depth_image with noise added.\n", + " \"\"\"\n", + " # Copy the original image\n", + " image = np.copy(depth_image) \n", + " \n", + " # Apply error_percent to each measurement (gaussian error)\n", + " gauss = np.random.normal(1, error_percent, image.shape)\n", + " image *= gauss\n", + " \n", + " # Add null (zero) values\n", + " num_nulls = int(image.size * null_percent)\n", + " coords = [np.random.randint(0, i - 1, num_nulls) for i in image.shape]\n", + " image[tuple(coords)] = 0.0\n", + " \n", + " return image\n", + "\n", + "\n", + "def crop(\n", + " image: NDArray[(Any, ...), Any],\n", + " top_left_inclusive: Tuple[float, float],\n", + " bottom_right_exclusive: Tuple[float, float]\n", + ") -> NDArray[(Any, ...), Any]:\n", + " \"\"\"\n", + " Crops an image to a rectangle based on the specified pixel points.\n", + "\n", + " Args:\n", + " image: The color or depth image to crop.\n", + " top_left_inclusive: The (row, column) of the top left pixel of the crop rectangle.\n", + " bottom_right_exclusive: The (row, column) of the pixel one past the bottom right corner of the crop rectangle.\n", + "\n", + " Returns:\n", + " A cropped version of the image.\n", + "\n", + " Note:\n", + " The top_left_inclusive pixel is included in the crop rectangle, but the\n", + " bottom_right_exclusive pixel is not.\n", + " \n", + " If bottom_right_exclusive exceeds the bottom or right edge of the image, the\n", + " full image is included along that axis.\n", + " \"\"\"\n", + " # Extract the minimum and maximum pixel rows and columns from the parameters\n", + " r_min, c_min = top_left_inclusive\n", + " r_max, c_max = bottom_right_exclusive\n", + "\n", + " # Shorten the array to the specified row and column ranges\n", + " return image[r_min:r_max, c_min:c_max]" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Finally, we will create a racecar object. If this step fails, make sure that `isSimulation` has the correct value." + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\u001b[95m>> Racecar created with the following options:\n", + " Simulation (-s): [True]\n", + " Headless (-h): [False]\n", + " Initialize with display (-d): [False]\u001b[00m\n" + ] + } + ], + "source": [ + "# Create Racecar\n", + "rc = racecar_core.create_racecar(isSimulation)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "\n", + "## 2. Depth and Color Images\n", + "The following code has been provided for idenitfying an orange contour and calculating its distance." + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "metadata": { + "scrolled": true + }, + "outputs": [], + "source": [ + "# The HSV range for the color orange\n", + "ORANGE = ((90, 50, 50), (120, 255, 255))\n", + "\n", + "# Calculate the center of the contour\n", + "def find_contour_center():\n", + " contour_center = None\n", + " \n", + " image = rc.camera.get_color_image_async()\n", + " \n", + " # Find all of the orange contours\n", + " contours = rc_utils.find_contours(image, ORANGE[0], ORANGE[1])\n", + "\n", + " # Select the largest contour\n", + " contour = rc_utils.get_largest_contour(contours, 30)\n", + " \n", + " if contour is not None:\n", + " # Calculate contour information\n", + " contour_center = rc_utils.get_contour_center(contour)\n", + " return (contour_center[1],contour_center[0])" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "\n", + "## 3. Uncertainty\n", + "\n", + "While it is easy to estimate state based on raw sensor, these measurements are inherently noisy. Oftentimes, this noise follows a Gaussian, or normal distribution. The Kalman filter uses properties of this distribution to optimally estimate the state of an agent.\n", + "\n", + "Probability distributions have an associated variance, or spread of the values (you may remember this as the square of the standard deviation). The more noisy sensor data, the greater its variance. The Kalman filter uses known information about the variance, or uncertainty, of sensor measurements to make accurate estimates.\n", + "\n", + "The variance of a sample of data can be computed using:\n", + "$\\sigma^2 = \\frac{\\sum (x_i -\\overline{x})^2}{n-1}$\n", + "\n", + "Here, you will calculate the variance of a sample of depth measurements on an orange cone:\n", + "\n", + "**Finish writing `get_sample_variance` in the following code block.**\n", + "\n", + "Note: you might have seen the that sample variance has $n-1$ in the denominator, as opposed to $n$. This has to do with the fact that the values in a sample will inherently be closer to the sample mean than the actual mean. Read up on **sampling** if you're further curious on why this value takes on $n-1$ specifically." + ] + }, + { + "cell_type": "code", + "execution_count": 7, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "104.035675\n", + "105.42071\n", + "106.33837\n", + "105.067\n", + "105.92013\n", + "103.50241\n", + "105.27675\n", + "103.56479\n", + "110.32463\n", + "103.72267\n", + "110.124626\n", + "104.4324\n", + "105.0116\n", + "101.140976\n", + "103.96829\n", + "103.28456\n", + "106.45598\n", + "100.99677\n", + "104.54126\n", + "103.37908\n", + "107.57448\n", + "104.58403\n", + "104.43553\n", + "101.70653\n", + "109.26831\n", + "103.62463\n", + "104.971886\n", + "107.06133\n", + "102.84172\n", + "105.26291\n", + "sample variance: 3\n" + ] + } + ], + "source": [ + "depth_data = []\n", + "\n", + "#collect data\n", + "for i in range(30):\n", + " \n", + " #find contour\n", + " contour_center = find_contour_center()\n", + " \n", + " if contour_center is not None:\n", + " #compute distance\n", + " depth_image = rc.camera.get_depth_image_async()\n", + " z = rc_utils.get_pixel_average_distance(depth_image, contour_center)\n", + " print(z)\n", + " depth_data.append(z)\n", + " \n", + "def get_sample_variance(data):\n", + " \n", + " #TODO: calculate the sample variance on the depth data\n", + " \n", + "sample_variance = get_sample_variance(depth_data)\n", + "print(\"sample variance: \", sample_variance)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "You will notice that the depth measurements vary even thought the actual distance does not change" + ] + }, + { + "cell_type": "code", + "execution_count": 8, + "metadata": {}, + "outputs": [ + { + "data": { + "image/png": "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\n", + "text/plain": [ + "
" + ] + }, + "metadata": { + "needs_background": "light" + }, + "output_type": "display_data" + } + ], + "source": [ + "plt.plot(depth_data)\n", + "plt.show()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "\n", + "## 4. Prediction\n", + "\n", + "The Kalman filter makes **predictions** of the RACECAR's state each timestep based on sensor measurements and their **uncertainty**. \n", + "\n", + "There is uncertainty in this prediction ($\\sigma_{\\hat{x}}$), as well as the sensor measurements ($\\sigma_{\\hat{z}}$). However, while the sensor measurements maintain a consistent uncertainty inherent to the sensor, the uncertainty of the prediction will decrease as more and more measurements are made.\n", + "\n", + "Here, you will use Kalman filtering to estimate the distance of the orange cone while stationary ($x$). You will need to implement the following formulas:\n", + "\n", + "**Kalman gain**: $K=\\frac{\\hat{\\sigma}_{\\hat{x}}^2}{\\hat{\\sigma}_{\\hat{x}}^2+\\sigma_z^2}$\n", + "\n", + "The Kalman gain capture the relative accuracy of the estimate, and is calculated by dividing the estimate uncertainty ($\\hat{\\sigma}_{\\hat{x}}^2$) by itself added to the measurement uncertainty ($\\hat{\\sigma}_{\\hat{x}}^2+\\sigma_z^2$). This value will always be less than 1, so every timestep, as the RACECAR gains more and more data, the Kalman gain should go down. \n", + "\n", + "**Distance estimate**: $x=\\hat{x}+K*(z-\\hat{x})$\n", + "\n", + "This is how far the RACECAR thinks the cone is. It adjusts the prediction $\\hat{x}$ based on the sensor measurement ($z$).\n", + "\n", + "Updated **estimate uncertainty**: $ \\sigma_{\\hat{x}}^2 = (1-K) * \\sigma_{\\hat{x}}^2 $\n", + "\n", + "This is how uncertain the RACECAR is about its estimate. Every timestep, uncertainty should decrease ($0 \\leq K \\leq 1$).\n", + "\n", + "**Estimate the distance of an orange cone while the robot is stationary by computing the Kalman gain, distance estimate, and estimate uncertainty**\n", + "\n", + "NOTE: because the RACECAR is stationary, your prediction ($\\hat{x}$) will be the same as your previous estimate ($x$).\n", + "\n", + "Hint: You calculated uncertainty in the sensor measurement during the previous section" + ] + }, + { + "cell_type": "code", + "execution_count": 10, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "105.05846\n", + "102.84233\n", + "108.74164\n", + "105.1827\n", + "105.37468\n", + "105.865036\n", + "105.91574\n", + "108.13771\n", + "104.39417\n", + "100.8343\n", + "106.121254\n", + "109.21776\n", + "104.27721\n", + "108.25184\n", + "103.85033\n", + "108.18071\n", + "104.00426\n", + "101.685486\n", + "100.67201\n", + "103.40814\n", + "107.57622\n", + "104.60262\n", + "103.373795\n", + "105.02775\n", + "100.17612\n", + "107.28107\n", + "106.34128\n", + "105.4633\n", + "104.74989\n", + "103.12167\n" + ] + } + ], + "source": [ + "# Initial prediction\n", + "predicted_distance = 100\n", + "# Initial uncertainty\n", + "estimate_variance = 400\n", + "\n", + "depth_data = []\n", + "estimates = []\n", + "\n", + "for i in range(30):\n", + " \n", + " #find contour\n", + " contour_center = find_contour_center()\n", + " \n", + " if contour_center is not None:\n", + " #compute distance\n", + " depth_image = rc.camera.get_depth_image_async()\n", + " z = rc_utils.get_pixel_average_distance(depth_image, contour_center)\n", + " print(z)\n", + " depth_data.append(z)\n", + " \n", + " \n", + " #TODO: compute the Kalman gain\n", + " K = \n", + " \n", + " #TODO: estimate the distance from the cone\n", + " estimate = \n", + " \n", + " estimates.append(estimate)\n", + " \n", + " #Estimate uncertainty\n", + " estimate_uncertainty = " + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Your program should stabilize the distance predictions over time!" + ] + }, + { + "cell_type": "code", + "execution_count": 12, + "metadata": {}, + "outputs": [ + { + "data": { + "image/png": "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\n", + "text/plain": [ + "
" + ] + }, + "metadata": { + "needs_background": "light" + }, + "output_type": "display_data" + } + ], + "source": [ + "signal = plt.plot(estimates)\n", + "signal = plt.plot(depth_data)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "\n", + "## 5. Implementation\n", + "\n", + "Congrats, You've completed this Jupyter Notebook!\n", + "\n", + "This section will outline how to transform the Kalman filter you designed to work while the RACECAR is in motion. Unfortunately, Jupyter Notebook doesn't have a way to measure $\\Delta$time, so **the provided starter code is only an example of what you might write in your IDE/text editor. The code will not work in Jupyter Notebook**. Try implementing this in one of your previous programs, or test it out as is.\n", + "\n", + "Most of the time when you want to estimate the state of the RACECAR it will be in motion. This means the Kalman filter prediction will need to incorporate **velocity** and **acceleration** which both have their own measurement uncertainties.\n", + "\n", + "You can calculate calculate these values using sampling in the same way you did for distance. Here they are provided for you to save time/effort." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "acceleration_variance = 112\n", + "velocity_variance = 112" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Now, you will incorporate implement Kalman filter while the RACECAR is accelerating. Here, you will have to incorporate a **motion model** to predict the new state of the RACECAR each timestep. All requisite equations are provided: \n", + "\n", + "\n", + "**predicted state**: $\\hat{x}=x + \\Delta t v + \\frac{1}{2}\\Delta t^2 a$\n", + "\n", + "This equation is **new**. Unlike in the previous example, the RACECAR is accelerating, so you will need to predict a new location for the RACECAR each timestep based on its velocity and acceleration.\n", + "\n", + "**estimate uncertainty**: $\\hat{\\sigma}_{\\hat{x}}^2=\\sigma_x^2+\\Delta t \\sigma_v^2 + \\frac{1}{2}\\Delta t^2 \\sigma_a^2$\n", + "\n", + "**kalman gain**: $K=\\frac{\\hat{\\sigma}_{\\hat{x}}^2}{\\hat{\\sigma}_{\\hat{x}}^2+\\sigma_z^2}$ \n", + "\n", + "**estimate**: $x=\\hat{x}+K(z-\\hat{x})$\n", + "\n", + "\n", + "**Estimate the distance of an orange cone while the robot is accelerating**\n", + "\n", + "**Use controls in the sim to accelerate and decelerate while keeping the cone in view**\n", + "\n", + "Hint: compute velocity and acceleration using rc.get_delta_time()" + ] + }, + { + "cell_type": "code", + "execution_count": 13, + "metadata": {}, + "outputs": [], + "source": [ + "# Initial prediction\n", + "predicted_distance = 100\n", + "# Initial uncertainty\n", + "estimate_variance = 400\n", + "\n", + "depth_data = []\n", + "estimates = []\n", + "\n", + "for i in range(20):\n", + " #find contour\n", + " contour_center = find_contour_center()\n", + " \n", + " if contour_center is not None:\n", + " #compute distance\n", + " depth_image = rc.camera.get_depth_image_async()\n", + " z = rc_utils.get_pixel_average_distance(depth_image, contour_center)\n", + " \n", + " depth_data.append(z)\n", + " \n", + " #TODO: predict the distance of the cone based on previous estimate\n", + " v = \n", + " a = \n", + " x = \n", + " \n", + " #TODO: estimate the distance from the cone\n", + " estimate = \n", + " \n", + " estimates.append(estimate)\n", + " \n", + " #TODO: estimate uncertainty\n", + " estimate_uncertainty = \n", + " \n", + " #TODO: compute a new Kalman gain\n", + " K = " + ] + } + ], + "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.7.6" + } + }, + "nbformat": 4, + "nbformat_minor": 4 +}