├── .github └── workflows │ └── manual.yml ├── 4_2_Robot_Localization ├── 1_1. 1D Robot World, exercise.ipynb ├── 1_2. 1D Robot World, solution.ipynb ├── 2_1. Probability After Sense, exercise.ipynb ├── 2_2. Probability After Sense, solution.ipynb ├── 3_1. Sense Function, exercise.ipynb ├── 3_2. Sense Function, solution.ipynb ├── 4_1. Normalized Sense Function, exercise.ipynb ├── 4_2. Normalized Sense Function, solution.ipynb ├── 5_1. Multiple Measurements, exercise.ipynb ├── 5_2. Multiple Measurements, solution.ipynb ├── 6_1. Move Function, exercise.ipynb ├── 6_2. Move Function, solution.ipynb ├── 7_1. Inexact Move Function, exercise.ipynb ├── 7_2. Inexact Move Function, solution.ipynb ├── 8_1. Multiple Movements, exercise.ipynb ├── 8_2. Multiple Movements, solution.ipynb ├── 9_1. Sense and Move, exercise.ipynb ├── 9_2. Sense and Move, solution.ipynb └── images │ ├── 2D_trees.png │ ├── lidar.png │ ├── motion_1.png │ ├── numbered_grid.png │ ├── numbered_grid2.png │ ├── road_1.png │ ├── road_stop.png │ ├── robot_sensing.png │ ├── sense_move.png │ ├── stop_4.png │ └── uncertain_motion.png ├── 4_3_2D_Histogram_Filter ├── helpers.py ├── localizer.py ├── simulate.py └── writeup.ipynb ├── 4_4_Kalman_Filters ├── 1_1. Gaussian Calculations.ipynb ├── 2_1. New Mean and Variance, exercise.ipynb ├── 2_2. New Mean and Variance, solution.ipynb ├── 3_1. Predict Function, exercise.ipynb ├── 3_2. Predict Function, solution.ipynb ├── 4_1. 1D Kalman Filter, exercise.ipynb ├── 4_2. 1D Kalman Filter, solution.ipynb └── images │ ├── gaussian_updates.png │ ├── mean_var.png │ └── motion_update.png ├── 4_5_State_and_Motion ├── 1. Interacting with a Car Object.ipynb ├── 2. Car, playground.ipynb ├── 3. Turning Right.ipynb ├── 4. Multiple, Colorful Cars.ipynb ├── 4x4_path.png ├── 5. Create color playground.ipynb ├── 6. Add color playground.ipynb ├── 7. Matrix_Multiplication.ipynb ├── car.py ├── color.py └── matrix.py ├── 4_6_Matrices_and_Transformation_of_State ├── 1_vector_coding.ipynb ├── 1_vector_coding_solution.ipynb ├── 2_matrices_in_python.ipynb ├── 2_matrices_in_python_solution.ipynb ├── 3_matrix_addition.ipynb ├── 4_matrix_multiplication.ipynb ├── 5_matrix_transpose.ipynb ├── 6_inverse_matrix.ipynb ├── 7_identity_matrix.ipynb └── guide_to_mathematical_notation.ipynb ├── 4_7_SLAM ├── 1_1. Construct Omega and Xi, exercise.ipynb ├── 1_2. Construct Omega and Xi, solution.ipynb ├── 2_1. Include Landmarks, exercise.ipynb ├── 2_2. Include Landmarks, solution.ipynb ├── 3. Confident Measurements.ipynb └── images │ └── omega_xi.png ├── 4_8_Vehicle_Motion_and_Calculus ├── Approaching Instantaneous Speed.ipynb ├── Implement an Accelerometer.ipynb ├── Plotting Position vs Time.ipynb ├── Speed from Position Data.ipynb ├── Understanding the Derivative.ipynb ├── data_generator.ipynb ├── helpers.py ├── images │ └── vmc_l1_plot1.png ├── parallel_park.pickle └── sharp_left.pickle ├── CODEOWNERS ├── LICENSE ├── Project_Landmark Detection ├── 1. Robot Moving and Sensing.ipynb ├── 2. Omega and Xi, Constraints.ipynb ├── 3. Landmark Detection and Tracking.ipynb ├── 4. Zip Your Project Files and Submit.ipynb ├── filelist.txt ├── helpers.py ├── images │ ├── constraints2D.png │ ├── download_ex.png │ ├── initial_constraints.png │ ├── motion_constraint.png │ ├── omega_xi.png │ ├── omega_xi_constraints.png │ ├── robot_world.png │ └── solution.png └── robot_class.py └── README.md /.github/workflows/manual.yml: -------------------------------------------------------------------------------- 1 | # Workflow to ensure whenever a Github PR is submitted, 2 | # a JIRA ticket gets created automatically. 3 | name: Manual Workflow 4 | 5 | # Controls when the action will run. 6 | on: 7 | # Triggers the workflow on pull request events but only for the master branch 8 | pull_request_target: 9 | types: [opened, reopened] 10 | 11 | # Allows you to run this workflow manually from the Actions tab 12 | workflow_dispatch: 13 | 14 | jobs: 15 | test-transition-issue: 16 | name: Convert Github Issue to Jira Issue 17 | runs-on: ubuntu-latest 18 | steps: 19 | - name: Checkout 20 | uses: actions/checkout@master 21 | 22 | - name: Login 23 | uses: atlassian/gajira-login@master 24 | env: 25 | JIRA_BASE_URL: ${{ secrets.JIRA_BASE_URL }} 26 | JIRA_USER_EMAIL: ${{ secrets.JIRA_USER_EMAIL }} 27 | JIRA_API_TOKEN: ${{ secrets.JIRA_API_TOKEN }} 28 | 29 | - name: Create NEW JIRA ticket 30 | id: create 31 | uses: atlassian/gajira-create@master 32 | with: 33 | project: CONUPDATE 34 | issuetype: Task 35 | summary: | 36 | Github PR [Assign the ND component] | Repo: ${{ github.repository }} | PR# ${{github.event.number}} 37 | description: | 38 | Repo link: https://github.com/${{ github.repository }} 39 | PR no. ${{ github.event.pull_request.number }} 40 | PR title: ${{ github.event.pull_request.title }} 41 | PR description: ${{ github.event.pull_request.description }} 42 | In addition, please resolve other issues, if any. 43 | fields: '{"components": [{"name":"Github PR"}], "customfield_16449":"https://classroom.udacity.com/", "customfield_16450":"Resolve the PR", "labels": ["github"], "priority":{"id": "4"}}' 44 | 45 | - name: Log created issue 46 | run: echo "Issue ${{ steps.create.outputs.issue }} was created" 47 | -------------------------------------------------------------------------------- /4_2_Robot_Localization/1_1. 1D Robot World, exercise.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "# The Robot World\n", 8 | "\n", 9 | "A robot, much like you, perceives the world through its \"senses.\" For example, self-driving cars use video, radar, and Lidar, to observe the world around them. As cars gather data, they build up a 3D world of observations that tells the car where it is, where other objects (like trees, pedestrians, and other vehicles) are, and where it should be going! \n", 10 | "\n", 11 | "In this section, we'll be working with first a 1D then a 2D representation of the world for simplicity, and because two dimensions are often all you'll need to solve a certain problem. \n", 12 | "* You'll be given a set of quizzes to solve to build up your understanding of robot localization. \n", 13 | "* Try your best to solve these quizzes and consult the solution if you get stuck or want to confirm your answer.\n", 14 | "\n", 15 | "*The solution will always be in either 1. a notebook which can be accessed in the `jupyter` directory in the top-left or 2. the video that comes after a quiz.* \n", 16 | "\n", 17 | "\n", 18 | "\n", 19 | "\n", 20 | "These grid representations of the environment are known as **discrete** representations. Discrete just means a limited number of places a robot can be (ex. in one grid cell). That's because robots, and autonomous vehicles like self-driving cars, use maps to figure out where they are, and maps lend themselves to being divided up into grids and sections.\n", 21 | "\n", 22 | "You'll see **continuous** probability distributions when locating objects that are moving around the robot. Continuous means that these objects can be anywhere around the robot and their movement is smooth.\n", 23 | "\n", 24 | "So, let's start with the 1D case." 25 | ] 26 | }, 27 | { 28 | "cell_type": "markdown", 29 | "metadata": {}, 30 | "source": [ 31 | "### Robot World 1-D\n", 32 | "\n", 33 | "First, imagine you have a robot living in a 1-D world. You can think of a 1D world as a one-lane road. \n", 34 | "\n", 35 | "\n", 36 | "\n", 37 | "We can treat this road as an array, and break it up into grid cells for a robot to understand. In this case, the road is a 1D grid with 5 different spaces. The robot can only move forwards or backwards. If the robot falls off the grid, it will loop back around to the other side (this is known as a cyclic world).\n", 38 | "\n", 39 | "\n", 40 | "\n", 41 | "### Uniform Distribution\n", 42 | "\n", 43 | "The robot has a map so that it knows there are only 5 spaces in this 1D world. However, it hasn't sensed anything or moved. For a length of 5 cells (a list of 5 values), what is the probability distribution, `p`, that the robot is in any one of these locations?\n", 44 | "\n", 45 | "Since the robot does not know where it is at first, the probability of being in any space is the same! This is a probability distribution and so the sum of all these probabilities should be equal to 1, so `1/5 spaces = 0.2`. A distribution in which all the probabilities are the same (and we have maximum uncertainty) is called a **uniform distribution**.\n" 46 | ] 47 | }, 48 | { 49 | "cell_type": "code", 50 | "execution_count": null, 51 | "metadata": { 52 | "collapsed": true 53 | }, 54 | "outputs": [], 55 | "source": [ 56 | "# importing resources\n", 57 | "import matplotlib.pyplot as plt\n", 58 | "import numpy as np" 59 | ] 60 | }, 61 | { 62 | "cell_type": "code", 63 | "execution_count": null, 64 | "metadata": { 65 | "collapsed": true 66 | }, 67 | "outputs": [], 68 | "source": [ 69 | "# uniform distribution for 5 grid cells\n", 70 | "# we use \"p\" to represent probability\n", 71 | "p = [0.2, 0.2, 0.2, 0.2, 0.2]\n", 72 | "print(p)\n" 73 | ] 74 | }, 75 | { 76 | "cell_type": "markdown", 77 | "metadata": {}, 78 | "source": [ 79 | "I'll also include a helper function for visualizing this distribution. The below function, `display_map` will output a bar chart showing the probability that a robot is in each grid space. The y-axis has a range of 0 to 1 for the range of probabilities. For a uniform distribution, this will look like a flat line. You can choose the width of each bar to be <= 1 should you want to space these out. " 80 | ] 81 | }, 82 | { 83 | "cell_type": "code", 84 | "execution_count": null, 85 | "metadata": { 86 | "collapsed": true 87 | }, 88 | "outputs": [], 89 | "source": [ 90 | "def display_map(grid, bar_width=1):\n", 91 | " if(len(grid) > 0):\n", 92 | " x_labels = range(len(grid))\n", 93 | " plt.bar(x_labels, height=grid, width=bar_width, color='b')\n", 94 | " plt.xlabel('Grid Cell')\n", 95 | " plt.ylabel('Probability')\n", 96 | " plt.ylim(0, 1) # range of 0-1 for probability values \n", 97 | " plt.title('Probability of the robot being at each cell in the grid')\n", 98 | " plt.xticks(np.arange(min(x_labels), max(x_labels)+1, 1))\n", 99 | " plt.show()\n", 100 | " else:\n", 101 | " print('Grid is empty')\n", 102 | "\n", 103 | "# call function on grid, p, from before\n", 104 | "display_map(p)" 105 | ] 106 | }, 107 | { 108 | "cell_type": "markdown", 109 | "metadata": {}, 110 | "source": [ 111 | "Now, what about if the world was 8 grid cells in length instead of 5?\n", 112 | "\n", 113 | "### QUIZ: Write a function that takes in the number of spaces in the robot's world (in this case 8), and returns the initial probability distribution `p` that the robot is in each space. \n", 114 | "\n", 115 | "This function should store the probabilities in a list. So in this example, there would be a list with 8 probabilities." 116 | ] 117 | }, 118 | { 119 | "cell_type": "code", 120 | "execution_count": null, 121 | "metadata": { 122 | "collapsed": true 123 | }, 124 | "outputs": [], 125 | "source": [ 126 | "# ex. initialize_robot(5) = [0.2, 0.2, 0.2, 0.2, 0.2]\n", 127 | "def initialize_robot(grid_length):\n", 128 | " ''' Takes in a grid length and returns \n", 129 | " a uniform distribution of location probabilities'''\n", 130 | " p = []\n", 131 | " return p\n" 132 | ] 133 | }, 134 | { 135 | "cell_type": "code", 136 | "execution_count": null, 137 | "metadata": { 138 | "collapsed": true 139 | }, 140 | "outputs": [], 141 | "source": [ 142 | "p = initialize_robot(8)\n", 143 | "print(p)\n", 144 | "display_map(p)" 145 | ] 146 | }, 147 | { 148 | "cell_type": "markdown", 149 | "metadata": {}, 150 | "source": [ 151 | "Now that you know how a robot initially sees a simple 1D world, let's learn about how it can locate itself by moving around and sensing it's environment!" 152 | ] 153 | } 154 | ], 155 | "metadata": { 156 | "kernelspec": { 157 | "display_name": "Python [default]", 158 | "language": "python", 159 | "name": "python3" 160 | }, 161 | "language_info": { 162 | "codemirror_mode": { 163 | "name": "ipython", 164 | "version": 3 165 | }, 166 | "file_extension": ".py", 167 | "mimetype": "text/x-python", 168 | "name": "python", 169 | "nbconvert_exporter": "python", 170 | "pygments_lexer": "ipython3", 171 | "version": "3.6.4" 172 | } 173 | }, 174 | "nbformat": 4, 175 | "nbformat_minor": 2 176 | } 177 | -------------------------------------------------------------------------------- /4_2_Robot_Localization/3_1. Sense Function, exercise.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "# Sense Function\n", 8 | "\n", 9 | "In this notebook, let's go over the steps a robot takes to help localize itself from an initial, uniform distribution to sensing and updating that distribution.\n", 10 | "\n", 11 | "You know these steps well, and this time, you're tasked with writing a function `sense` that encompasses this behavior. \n", 12 | "\n", 13 | "1. The robot starts off knowing nothing; the robot is equally likely to be anywhere and so `p` is a uniform distribution.\n", 14 | "2. Then the robot senses a grid color: red or green, and updates this distribution `p` according to the values of pHit and pMiss.\n", 15 | "\n", 16 | "* The probability that it is sensing the color correctly is `pHit = 0.6`.\n", 17 | "* The probability that it is sensing the wrong color is `pMiss = 0.2`\n", 18 | "\n", 19 | "\n" 20 | ] 21 | }, 22 | { 23 | "cell_type": "code", 24 | "execution_count": null, 25 | "metadata": { 26 | "collapsed": true 27 | }, 28 | "outputs": [], 29 | "source": [ 30 | "# importing resources\n", 31 | "import matplotlib.pyplot as plt\n", 32 | "import numpy as np" 33 | ] 34 | }, 35 | { 36 | "cell_type": "markdown", 37 | "metadata": {}, 38 | "source": [ 39 | "A helper function for visualizing a distribution." 40 | ] 41 | }, 42 | { 43 | "cell_type": "code", 44 | "execution_count": null, 45 | "metadata": { 46 | "collapsed": true 47 | }, 48 | "outputs": [], 49 | "source": [ 50 | "def display_map(grid, bar_width=1):\n", 51 | " if(len(grid) > 0):\n", 52 | " x_labels = range(len(grid))\n", 53 | " plt.bar(x_labels, height=grid, width=bar_width, color='b')\n", 54 | " plt.xlabel('Grid Cell')\n", 55 | " plt.ylabel('Probability')\n", 56 | " plt.ylim(0, 1) # range of 0-1 for probability values \n", 57 | " plt.title('Probability of the robot being at each cell in the grid')\n", 58 | " plt.xticks(np.arange(min(x_labels), max(x_labels)+1, 1))\n", 59 | " plt.show()\n", 60 | " else:\n", 61 | " print('Grid is empty')\n" 62 | ] 63 | }, 64 | { 65 | "cell_type": "markdown", 66 | "metadata": { 67 | "collapsed": true 68 | }, 69 | "source": [ 70 | "### QUIZ: Complete the sense function so that this outputs an unnormalized distribution, `p`, after sensing. \n", 71 | "\n", 72 | "Use the previous exercise as a starting point. `q = [0.04, 0.12, 0.12, 0.04, 0.04]` should be exactly the distribution you get when the sensor measurement `Z= 'red'`. \n", 73 | "\n", 74 | "This complete function should also output the correct `q` for `Z= 'green'`.\n", 75 | "\n", 76 | "Note that `pHit` refers to the probability that the robot correctly senses the color of the square it is on, so if a robot senses red *and* is on a red square, we'll multiply the current location probability (0.2) with pHit. Same goes for if a robot senses green *and* is on a green square." 77 | ] 78 | }, 79 | { 80 | "cell_type": "code", 81 | "execution_count": null, 82 | "metadata": { 83 | "collapsed": true 84 | }, 85 | "outputs": [], 86 | "source": [ 87 | "# given initial variables\n", 88 | "p=[0.2, 0.2, 0.2, 0.2, 0.2]\n", 89 | "# the color of each grid cell in the 1D world\n", 90 | "world=['green', 'red', 'red', 'green', 'green']\n", 91 | "# Z, the sensor reading ('red' or 'green')\n", 92 | "Z = 'red'\n", 93 | "pHit = 0.6\n", 94 | "pMiss = 0.2\n", 95 | "\n", 96 | "## Complete this function\n", 97 | "def sense(p, Z):\n", 98 | " ''' Takes in a current probability distribution, p, and a sensor reading, Z.\n", 99 | " Returns an unnormalized distribution after the sensor measurement has been made, q.\n", 100 | " This should be accurate whether Z is 'red' or 'green'. '''\n", 101 | " \n", 102 | " q=[]\n", 103 | " \n", 104 | " return q\n", 105 | "\n", 106 | "q = sense(p,Z)\n", 107 | "print(q)\n", 108 | "display_map(q)" 109 | ] 110 | }, 111 | { 112 | "cell_type": "code", 113 | "execution_count": null, 114 | "metadata": { 115 | "collapsed": true 116 | }, 117 | "outputs": [], 118 | "source": [] 119 | } 120 | ], 121 | "metadata": { 122 | "kernelspec": { 123 | "display_name": "Python [default]", 124 | "language": "python", 125 | "name": "python3" 126 | }, 127 | "language_info": { 128 | "codemirror_mode": { 129 | "name": "ipython", 130 | "version": 3 131 | }, 132 | "file_extension": ".py", 133 | "mimetype": "text/x-python", 134 | "name": "python", 135 | "nbconvert_exporter": "python", 136 | "pygments_lexer": "ipython3", 137 | "version": "3.6.4" 138 | } 139 | }, 140 | "nbformat": 4, 141 | "nbformat_minor": 2 142 | } 143 | -------------------------------------------------------------------------------- /4_2_Robot_Localization/4_1. Normalized Sense Function, exercise.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "# Normalized Sense Function\n", 8 | "\n", 9 | "In this notebook, let's go over the steps a robot takes to help localize itself from an initial, uniform distribution to sensing and updating that distribution and finally normalizing that distribution.\n", 10 | "\n", 11 | "1. The robot starts off knowing nothing; the robot is equally likely to be anywhere and so `p` is a uniform distribution.\n", 12 | "2. Then the robot senses a grid color: red or green, and updates this distribution `p` according to the values of pHit and pMiss.\n", 13 | "3. **We normalize `p` such that its components sum to 1.**\n", 14 | "\n", 15 | "\n" 16 | ] 17 | }, 18 | { 19 | "cell_type": "code", 20 | "execution_count": null, 21 | "metadata": { 22 | "collapsed": true 23 | }, 24 | "outputs": [], 25 | "source": [ 26 | "# importing resources\n", 27 | "import matplotlib.pyplot as plt\n", 28 | "import numpy as np" 29 | ] 30 | }, 31 | { 32 | "cell_type": "markdown", 33 | "metadata": {}, 34 | "source": [ 35 | "A helper function for visualizing a distribution." 36 | ] 37 | }, 38 | { 39 | "cell_type": "code", 40 | "execution_count": null, 41 | "metadata": { 42 | "collapsed": true 43 | }, 44 | "outputs": [], 45 | "source": [ 46 | "def display_map(grid, bar_width=1):\n", 47 | " if(len(grid) > 0):\n", 48 | " x_labels = range(len(grid))\n", 49 | " plt.bar(x_labels, height=grid, width=bar_width, color='b')\n", 50 | " plt.xlabel('Grid Cell')\n", 51 | " plt.ylabel('Probability')\n", 52 | " plt.ylim(0, 1) # range of 0-1 for probability values \n", 53 | " plt.title('Probability of the robot being at each cell in the grid')\n", 54 | " plt.xticks(np.arange(min(x_labels), max(x_labels)+1, 1))\n", 55 | " plt.show()\n", 56 | " else:\n", 57 | " print('Grid is empty')\n" 58 | ] 59 | }, 60 | { 61 | "cell_type": "markdown", 62 | "metadata": { 63 | "collapsed": true 64 | }, 65 | "source": [ 66 | "### QUIZ: Modify your code so that it normalizes the output for the sense function. \n", 67 | "\n", 68 | "This means that the entries in `q` should sum to one.\n", 69 | "\n", 70 | "Note that `pHit` refers to the probability that the robot correctly senses the color of the square it is on, so if a robot senses red *and* is on a red square, we'll multiply the current location probability (0.2) with pHit. Same goes for if a robot senses green *and* is on a green square." 71 | ] 72 | }, 73 | { 74 | "cell_type": "code", 75 | "execution_count": null, 76 | "metadata": {}, 77 | "outputs": [], 78 | "source": [ 79 | "# given initial variables\n", 80 | "p=[0.2, 0.2, 0.2, 0.2, 0.2]\n", 81 | "# the color of each grid cell in the 1D world\n", 82 | "world=['green', 'red', 'red', 'green', 'green']\n", 83 | "# Z, the sensor reading ('red' or 'green')\n", 84 | "Z = 'red'\n", 85 | "pHit = 0.6\n", 86 | "pMiss = 0.2\n", 87 | "\n", 88 | "## Complete this function\n", 89 | "def sense(p, Z):\n", 90 | " ''' Takes in a current probability distribution, p, and a sensor reading, Z.\n", 91 | " Returns a *normalized* distribution after the sensor measurement has been made, q.\n", 92 | " This should be accurate whether Z is 'red' or 'green'. '''\n", 93 | " \n", 94 | " q=[]\n", 95 | " \n", 96 | " ##TODO: normalize q\n", 97 | " \n", 98 | " # loop through all grid cells\n", 99 | " for i in range(len(p)):\n", 100 | " # check if the sensor reading is equal to the color of the grid cell\n", 101 | " # if so, hit = 1\n", 102 | " # if not, hit = 0\n", 103 | " hit = (Z == world[i])\n", 104 | " q.append(p[i] * (hit * pHit + (1-hit) * pMiss))\n", 105 | " return q\n", 106 | "\n", 107 | "q = sense(p,Z)\n", 108 | "print(q)\n", 109 | "display_map(q)" 110 | ] 111 | }, 112 | { 113 | "cell_type": "code", 114 | "execution_count": null, 115 | "metadata": { 116 | "collapsed": true 117 | }, 118 | "outputs": [], 119 | "source": [] 120 | } 121 | ], 122 | "metadata": { 123 | "kernelspec": { 124 | "display_name": "Python [default]", 125 | "language": "python", 126 | "name": "python3" 127 | }, 128 | "language_info": { 129 | "codemirror_mode": { 130 | "name": "ipython", 131 | "version": 3 132 | }, 133 | "file_extension": ".py", 134 | "mimetype": "text/x-python", 135 | "name": "python", 136 | "nbconvert_exporter": "python", 137 | "pygments_lexer": "ipython3", 138 | "version": "3.6.4" 139 | } 140 | }, 141 | "nbformat": 4, 142 | "nbformat_minor": 2 143 | } 144 | -------------------------------------------------------------------------------- /4_2_Robot_Localization/5_1. Multiple Measurements, exercise.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "# Multiple Measurements\n", 8 | "\n", 9 | "In this notebook, let's go over the steps a robot takes to help localize itself from an initial, uniform distribution to sensing and updating that distribution and finally normalizing that distribution.\n", 10 | "\n", 11 | "1. The robot starts off knowing nothing; the robot is equally likely to be anywhere and so `p` is a uniform distribution.\n", 12 | "2. Then the robot senses a grid color: red or green, and updates this distribution `p` according to the values of pHit and pMiss.\n", 13 | "3. We normalize `p` such that its components sum to 1.\n", 14 | "4. **We repeat steps 2 and 3 for however many measurements are taken**\n", 15 | "\n", 16 | "\n" 17 | ] 18 | }, 19 | { 20 | "cell_type": "code", 21 | "execution_count": null, 22 | "metadata": { 23 | "collapsed": true 24 | }, 25 | "outputs": [], 26 | "source": [ 27 | "# importing resources\n", 28 | "import matplotlib.pyplot as plt\n", 29 | "import numpy as np" 30 | ] 31 | }, 32 | { 33 | "cell_type": "markdown", 34 | "metadata": {}, 35 | "source": [ 36 | "A helper function for visualizing a distribution." 37 | ] 38 | }, 39 | { 40 | "cell_type": "code", 41 | "execution_count": null, 42 | "metadata": { 43 | "collapsed": true 44 | }, 45 | "outputs": [], 46 | "source": [ 47 | "def display_map(grid, bar_width=1):\n", 48 | " if(len(grid) > 0):\n", 49 | " x_labels = range(len(grid))\n", 50 | " plt.bar(x_labels, height=grid, width=bar_width, color='b')\n", 51 | " plt.xlabel('Grid Cell')\n", 52 | " plt.ylabel('Probability')\n", 53 | " plt.ylim(0, 1) # range of 0-1 for probability values \n", 54 | " plt.title('Probability of the robot being at each cell in the grid')\n", 55 | " plt.xticks(np.arange(min(x_labels), max(x_labels)+1, 1))\n", 56 | " plt.show()\n", 57 | " else:\n", 58 | " print('Grid is empty')\n" 59 | ] 60 | }, 61 | { 62 | "cell_type": "markdown", 63 | "metadata": { 64 | "collapsed": true 65 | }, 66 | "source": [ 67 | "### QUIZ: Measure Twice\n", 68 | "\n", 69 | "Below is the normalized sense function, add code that can loop over muliple measurements, now in a *list* `measurements`. Add to this code so that it updates the probability twice and gives the posterior distribution after both measurements are incorporated. \n", 70 | "\n", 71 | "Make sure that your code allows for any sequence of measurements whether two measurements or more have been taken." 72 | ] 73 | }, 74 | { 75 | "cell_type": "code", 76 | "execution_count": null, 77 | "metadata": { 78 | "collapsed": true 79 | }, 80 | "outputs": [], 81 | "source": [ 82 | "# given initial variables\n", 83 | "p=[0.2, 0.2, 0.2, 0.2, 0.2]\n", 84 | "# the color of each grid cell in the 1D world\n", 85 | "world=['green', 'red', 'red', 'green', 'green']\n", 86 | "\n", 87 | "# measurements, now a *list* of sensor readings ('red' or 'green')\n", 88 | "measurements = ['red', 'green']\n", 89 | "pHit = 0.6\n", 90 | "pMiss = 0.2\n", 91 | "\n", 92 | "# sense function\n", 93 | "def sense(p, Z):\n", 94 | " ''' Takes in a current probability distribution, p, and a sensor reading, Z.\n", 95 | " Returns a *normalized* distribution after the sensor measurement has been made, q.\n", 96 | " This should be accurate whether Z is 'red' or 'green'. '''\n", 97 | " q=[]\n", 98 | " # loop through all grid cells\n", 99 | " for i in range(len(p)):\n", 100 | " # check if the sensor reading is equal to the color of the grid cell\n", 101 | " # if so, hit = 1\n", 102 | " # if not, hit = 0\n", 103 | " hit = (Z == world[i])\n", 104 | " q.append(p[i] * (hit * pHit + (1-hit) * pMiss))\n", 105 | " \n", 106 | " # sum up all the components\n", 107 | " s = sum(q)\n", 108 | " # divide all elements of q by the sum to normalize\n", 109 | " for i in range(len(p)):\n", 110 | " q[i] = q[i] / s\n", 111 | " return q\n", 112 | "\n", 113 | "## TODO: Add your code for accounting for 2 motion measurements, here\n", 114 | "## Grab and print out the resulting distribution, p\n", 115 | "p = []\n", 116 | "\n", 117 | "print(p)\n", 118 | "display_map(p)" 119 | ] 120 | } 121 | ], 122 | "metadata": { 123 | "kernelspec": { 124 | "display_name": "Python [default]", 125 | "language": "python", 126 | "name": "python3" 127 | }, 128 | "language_info": { 129 | "codemirror_mode": { 130 | "name": "ipython", 131 | "version": 3 132 | }, 133 | "file_extension": ".py", 134 | "mimetype": "text/x-python", 135 | "name": "python", 136 | "nbconvert_exporter": "python", 137 | "pygments_lexer": "ipython3", 138 | "version": "3.6.4" 139 | } 140 | }, 141 | "nbformat": 4, 142 | "nbformat_minor": 2 143 | } 144 | -------------------------------------------------------------------------------- /4_2_Robot_Localization/6_1. Move Function, exercise.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "# Move Function\n", 8 | "\n", 9 | "Now that you know how a robot uses sensor measurements to update its idea of its own location, let's see how we can incorporate motion into this location. In this notebook, let's go over the steps a robot takes to help localize itself from an initial, uniform distribution to sensing, moving and updating that distribution.\n", 10 | "\n", 11 | "We include the `sense` function that you've seen, which updates an initial distribution based on whether a robot senses a grid color: red or green. \n", 12 | "\n", 13 | "Next, you're tasked with writing a function `move` that incorporates motion into the distribution. As seen below, **one motion `U= 1` to the right, causes all values in a distribution to shift one grid cell to the right.**\n", 14 | "\n", 15 | "\n" 16 | ] 17 | }, 18 | { 19 | "cell_type": "markdown", 20 | "metadata": {}, 21 | "source": [ 22 | "First let's include our usual resource imports and display function." 23 | ] 24 | }, 25 | { 26 | "cell_type": "code", 27 | "execution_count": 3, 28 | "metadata": { 29 | "collapsed": true 30 | }, 31 | "outputs": [], 32 | "source": [ 33 | "# importing resources\n", 34 | "import matplotlib.pyplot as plt\n", 35 | "import numpy as np" 36 | ] 37 | }, 38 | { 39 | "cell_type": "markdown", 40 | "metadata": {}, 41 | "source": [ 42 | "A helper function for visualizing a distribution." 43 | ] 44 | }, 45 | { 46 | "cell_type": "code", 47 | "execution_count": 4, 48 | "metadata": { 49 | "collapsed": true 50 | }, 51 | "outputs": [], 52 | "source": [ 53 | "def display_map(grid, bar_width=1):\n", 54 | " if(len(grid) > 0):\n", 55 | " x_labels = range(len(grid))\n", 56 | " plt.bar(x_labels, height=grid, width=bar_width, color='b')\n", 57 | " plt.xlabel('Grid Cell')\n", 58 | " plt.ylabel('Probability')\n", 59 | " plt.ylim(0, 1) # range of 0-1 for probability values \n", 60 | " plt.title('Probability of the robot being at each cell in the grid')\n", 61 | " plt.xticks(np.arange(min(x_labels), max(x_labels)+1, 1))\n", 62 | " plt.show()\n", 63 | " else:\n", 64 | " print('Grid is empty')\n" 65 | ] 66 | }, 67 | { 68 | "cell_type": "markdown", 69 | "metadata": { 70 | "collapsed": true 71 | }, 72 | "source": [ 73 | "You are given the initial variables and the complete `sense` function, below." 74 | ] 75 | }, 76 | { 77 | "cell_type": "code", 78 | "execution_count": null, 79 | "metadata": { 80 | "collapsed": true 81 | }, 82 | "outputs": [], 83 | "source": [ 84 | "# given initial variables\n", 85 | "p=[0, 1, 0, 0, 0]\n", 86 | "# the color of each grid cell in the 1D world\n", 87 | "world=['green', 'red', 'red', 'green', 'green']\n", 88 | "# Z, the sensor reading ('red' or 'green')\n", 89 | "Z = 'red'\n", 90 | "pHit = 0.6\n", 91 | "pMiss = 0.2\n", 92 | "\n", 93 | "# You are given the complete sense function\n", 94 | "def sense(p, Z):\n", 95 | " ''' Takes in a current probability distribution, p, and a sensor reading, Z.\n", 96 | " Returns a *normalized* distribution after the sensor measurement has been made, q.\n", 97 | " This should be accurate whether Z is 'red' or 'green'. '''\n", 98 | " q=[]\n", 99 | " # loop through all grid cells\n", 100 | " for i in range(len(p)):\n", 101 | " # check if the sensor reading is equal to the color of the grid cell\n", 102 | " # if so, hit = 1\n", 103 | " # if not, hit = 0\n", 104 | " hit = (Z == world[i])\n", 105 | " q.append(p[i] * (hit * pHit + (1-hit) * pMiss))\n", 106 | " \n", 107 | " # sum up all the components\n", 108 | " s = sum(q)\n", 109 | " # divide all elements of q by the sum to normalize\n", 110 | " for i in range(len(p)):\n", 111 | " q[i] = q[i] / s\n", 112 | " return q\n", 113 | "\n", 114 | "# Commented out code for measurements\n", 115 | "# for k in range(len(measurements)):\n", 116 | "# p = sense(p, measurements)\n" 117 | ] 118 | }, 119 | { 120 | "cell_type": "markdown", 121 | "metadata": {}, 122 | "source": [ 123 | "### QUIZ: Program a function that returns a new distribution q, shifted to the right by the motion (U) units. \n", 124 | "\n", 125 | "This function should shift a distribution with the motion, U. Keep in mind that this world is cyclic and that if U=0, q should be the same as the given p. You should see all the values in `p` are moved to the right by 1, for U=1." 126 | ] 127 | }, 128 | { 129 | "cell_type": "code", 130 | "execution_count": null, 131 | "metadata": { 132 | "collapsed": true 133 | }, 134 | "outputs": [], 135 | "source": [ 136 | "\n", 137 | "## TODO: Complete this move function so that it shifts a probability distribution, p\n", 138 | "## by a given motion, U\n", 139 | "def move(p, U):\n", 140 | " q=[]\n", 141 | " # Your code here\n", 142 | " return q\n", 143 | "\n", 144 | "p = move(p,1)\n", 145 | "print(p)\n", 146 | "display_map(p)" 147 | ] 148 | }, 149 | { 150 | "cell_type": "code", 151 | "execution_count": null, 152 | "metadata": { 153 | "collapsed": true 154 | }, 155 | "outputs": [], 156 | "source": [] 157 | } 158 | ], 159 | "metadata": { 160 | "kernelspec": { 161 | "display_name": "Python [default]", 162 | "language": "python", 163 | "name": "python3" 164 | }, 165 | "language_info": { 166 | "codemirror_mode": { 167 | "name": "ipython", 168 | "version": 3 169 | }, 170 | "file_extension": ".py", 171 | "mimetype": "text/x-python", 172 | "name": "python", 173 | "nbconvert_exporter": "python", 174 | "pygments_lexer": "ipython3", 175 | "version": "3.6.4" 176 | } 177 | }, 178 | "nbformat": 4, 179 | "nbformat_minor": 2 180 | } 181 | -------------------------------------------------------------------------------- /4_2_Robot_Localization/7_1. Inexact Move Function, exercise.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "# Inexact Move Function\n", 8 | "\n", 9 | "Let's see how we can incorporate **uncertain** motion into our motion update. We include the `sense` function that you've seen, which updates an initial distribution based on whether a robot senses a grid color: red or green. \n", 10 | "\n", 11 | "Next, you're tasked with modifying the `move` function so that it incorporates uncertainty in motion.\n", 12 | "\n", 13 | "\n" 14 | ] 15 | }, 16 | { 17 | "cell_type": "markdown", 18 | "metadata": {}, 19 | "source": [ 20 | "First let's include our usual resource imports and display function." 21 | ] 22 | }, 23 | { 24 | "cell_type": "code", 25 | "execution_count": null, 26 | "metadata": { 27 | "collapsed": true 28 | }, 29 | "outputs": [], 30 | "source": [ 31 | "# importing resources\n", 32 | "import matplotlib.pyplot as plt\n", 33 | "import numpy as np" 34 | ] 35 | }, 36 | { 37 | "cell_type": "markdown", 38 | "metadata": {}, 39 | "source": [ 40 | "A helper function for visualizing a distribution." 41 | ] 42 | }, 43 | { 44 | "cell_type": "code", 45 | "execution_count": null, 46 | "metadata": { 47 | "collapsed": true 48 | }, 49 | "outputs": [], 50 | "source": [ 51 | "def display_map(grid, bar_width=1):\n", 52 | " if(len(grid) > 0):\n", 53 | " x_labels = range(len(grid))\n", 54 | " plt.bar(x_labels, height=grid, width=bar_width, color='b')\n", 55 | " plt.xlabel('Grid Cell')\n", 56 | " plt.ylabel('Probability')\n", 57 | " plt.ylim(0, 1) # range of 0-1 for probability values \n", 58 | " plt.title('Probability of the robot being at each cell in the grid')\n", 59 | " plt.xticks(np.arange(min(x_labels), max(x_labels)+1, 1))\n", 60 | " plt.show()\n", 61 | " else:\n", 62 | " print('Grid is empty')\n" 63 | ] 64 | }, 65 | { 66 | "cell_type": "markdown", 67 | "metadata": { 68 | "collapsed": true 69 | }, 70 | "source": [ 71 | "You are given the initial variables and the complete `sense` function, below." 72 | ] 73 | }, 74 | { 75 | "cell_type": "code", 76 | "execution_count": null, 77 | "metadata": { 78 | "collapsed": true 79 | }, 80 | "outputs": [], 81 | "source": [ 82 | "# given initial variables\n", 83 | "p=[0, 1, 0, 0, 0]\n", 84 | "# the color of each grid cell in the 1D world\n", 85 | "world=['green', 'red', 'red', 'green', 'green']\n", 86 | "# Z, the sensor reading ('red' or 'green')\n", 87 | "Z = 'red'\n", 88 | "pHit = 0.6\n", 89 | "pMiss = 0.2\n", 90 | "\n", 91 | "# You are given the complete sense function\n", 92 | "def sense(p, Z):\n", 93 | " ''' Takes in a current probability distribution, p, and a sensor reading, Z.\n", 94 | " Returns a *normalized* distribution after the sensor measurement has been made, q.\n", 95 | " This should be accurate whether Z is 'red' or 'green'. '''\n", 96 | " q=[]\n", 97 | " # loop through all grid cells\n", 98 | " for i in range(len(p)):\n", 99 | " # check if the sensor reading is equal to the color of the grid cell\n", 100 | " # if so, hit = 1\n", 101 | " # if not, hit = 0\n", 102 | " hit = (Z == world[i])\n", 103 | " q.append(p[i] * (hit * pHit + (1-hit) * pMiss))\n", 104 | " \n", 105 | " # sum up all the components\n", 106 | " s = sum(q)\n", 107 | " # divide all elements of q by the sum to normalize\n", 108 | " for i in range(len(p)):\n", 109 | " q[i] = q[i] / s\n", 110 | " return q\n", 111 | "\n", 112 | "# Commented out code for measurements\n", 113 | "# for k in range(len(measurements)):\n", 114 | "# p = sense(p, measurements)\n" 115 | ] 116 | }, 117 | { 118 | "cell_type": "markdown", 119 | "metadata": {}, 120 | "source": [ 121 | "### QUIZ: Modify the move function to accommodate the added probabilities of overshooting or undershooting the intended destination.\n", 122 | "\n", 123 | "This function should shift a distribution with the motion, U, with some probability of under/overshooting. For the given, initial `p`, you should see the result for U = 1 and incorporated uncertainties: `[0.0, 0.1, 0.8, 0.1, 0.0]`." 124 | ] 125 | }, 126 | { 127 | "cell_type": "code", 128 | "execution_count": null, 129 | "metadata": { 130 | "collapsed": true 131 | }, 132 | "outputs": [], 133 | "source": [ 134 | "## TODO: Modify the move function to accommodate the added robabilities of overshooting or undershooting \n", 135 | "pExact = 0.8\n", 136 | "pOvershoot = 0.1\n", 137 | "pUndershoot = 0.1\n", 138 | "\n", 139 | "# Complete the move function\n", 140 | "def move(p, U):\n", 141 | " q=[]\n", 142 | " # iterate through all values in p\n", 143 | " for i in range(len(p)):\n", 144 | " ## TODO: Modify this distribution code to incorporate values \n", 145 | " ## for over/undershooting the exact location\n", 146 | " \n", 147 | " # use the modulo operator to find the new location for a p value\n", 148 | " index = (i-U) % len(p)\n", 149 | " \n", 150 | " # append the correct, modified value of p to q\n", 151 | " q.append(p[index])\n", 152 | " return q\n", 153 | "\n", 154 | "## TODO: try this for U = 2 and see the result\n", 155 | "p = move(p,1)\n", 156 | "print(p)\n", 157 | "display_map(p)" 158 | ] 159 | } 160 | ], 161 | "metadata": { 162 | "kernelspec": { 163 | "display_name": "Python [default]", 164 | "language": "python", 165 | "name": "python3" 166 | }, 167 | "language_info": { 168 | "codemirror_mode": { 169 | "name": "ipython", 170 | "version": 3 171 | }, 172 | "file_extension": ".py", 173 | "mimetype": "text/x-python", 174 | "name": "python", 175 | "nbconvert_exporter": "python", 176 | "pygments_lexer": "ipython3", 177 | "version": "3.6.4" 178 | } 179 | }, 180 | "nbformat": 4, 181 | "nbformat_minor": 2 182 | } 183 | -------------------------------------------------------------------------------- /4_2_Robot_Localization/7_2. Inexact Move Function, solution.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "# Inexact Move Function\n", 8 | "\n", 9 | "Let's see how we can incorporate **uncertain** motion into our motion update. We include the `sense` function that you've seen, which updates an initial distribution based on whether a robot senses a grid color: red or green. \n", 10 | "\n", 11 | "Next, you're tasked with modifying the `move` function so that it incorporates uncertainty in motion.\n", 12 | "\n", 13 | "\n" 14 | ] 15 | }, 16 | { 17 | "cell_type": "markdown", 18 | "metadata": {}, 19 | "source": [ 20 | "First let's include our usual resource imports and display function." 21 | ] 22 | }, 23 | { 24 | "cell_type": "code", 25 | "execution_count": null, 26 | "metadata": { 27 | "collapsed": true 28 | }, 29 | "outputs": [], 30 | "source": [ 31 | "# importing resources\n", 32 | "import matplotlib.pyplot as plt\n", 33 | "import numpy as np" 34 | ] 35 | }, 36 | { 37 | "cell_type": "markdown", 38 | "metadata": {}, 39 | "source": [ 40 | "A helper function for visualizing a distribution." 41 | ] 42 | }, 43 | { 44 | "cell_type": "code", 45 | "execution_count": null, 46 | "metadata": { 47 | "collapsed": true 48 | }, 49 | "outputs": [], 50 | "source": [ 51 | "def display_map(grid, bar_width=1):\n", 52 | " if(len(grid) > 0):\n", 53 | " x_labels = range(len(grid))\n", 54 | " plt.bar(x_labels, height=grid, width=bar_width, color='b')\n", 55 | " plt.xlabel('Grid Cell')\n", 56 | " plt.ylabel('Probability')\n", 57 | " plt.ylim(0, 1) # range of 0-1 for probability values \n", 58 | " plt.title('Probability of the robot being at each cell in the grid')\n", 59 | " plt.xticks(np.arange(min(x_labels), max(x_labels)+1, 1))\n", 60 | " plt.show()\n", 61 | " else:\n", 62 | " print('Grid is empty')\n" 63 | ] 64 | }, 65 | { 66 | "cell_type": "markdown", 67 | "metadata": { 68 | "collapsed": true 69 | }, 70 | "source": [ 71 | "You are given the initial variables and the complete `sense` function, below." 72 | ] 73 | }, 74 | { 75 | "cell_type": "code", 76 | "execution_count": null, 77 | "metadata": { 78 | "collapsed": true 79 | }, 80 | "outputs": [], 81 | "source": [ 82 | "# given initial variables\n", 83 | "p=[0, 1, 0, 0, 0]\n", 84 | "# the color of each grid cell in the 1D world\n", 85 | "world=['green', 'red', 'red', 'green', 'green']\n", 86 | "# Z, the sensor reading ('red' or 'green')\n", 87 | "Z = 'red'\n", 88 | "pHit = 0.6\n", 89 | "pMiss = 0.2\n", 90 | "\n", 91 | "# You are given the complete sense function\n", 92 | "def sense(p, Z):\n", 93 | " ''' Takes in a current probability distribution, p, and a sensor reading, Z.\n", 94 | " Returns a *normalized* distribution after the sensor measurement has been made, q.\n", 95 | " This should be accurate whether Z is 'red' or 'green'. '''\n", 96 | " q=[]\n", 97 | " # loop through all grid cells\n", 98 | " for i in range(len(p)):\n", 99 | " # check if the sensor reading is equal to the color of the grid cell\n", 100 | " # if so, hit = 1\n", 101 | " # if not, hit = 0\n", 102 | " hit = (Z == world[i])\n", 103 | " q.append(p[i] * (hit * pHit + (1-hit) * pMiss))\n", 104 | " \n", 105 | " # sum up all the components\n", 106 | " s = sum(q)\n", 107 | " # divide all elements of q by the sum to normalize\n", 108 | " for i in range(len(p)):\n", 109 | " q[i] = q[i] / s\n", 110 | " return q\n", 111 | "\n", 112 | "# Commented out code for measurements\n", 113 | "# for k in range(len(measurements)):\n", 114 | "# p = sense(p, measurements)\n" 115 | ] 116 | }, 117 | { 118 | "cell_type": "markdown", 119 | "metadata": {}, 120 | "source": [ 121 | "### QUIZ: Modify the move function to accommodate the added probabilities of overshooting or undershooting the intended destination.\n", 122 | "\n", 123 | "This function should shift a distribution with the motion, U, with some probability of under/overshooting. For the given, initial `p`, you should see the result for U = 1 and incorporated uncertainties: `[0.0, 0.1, 0.8, 0.1, 0.0]`." 124 | ] 125 | }, 126 | { 127 | "cell_type": "code", 128 | "execution_count": null, 129 | "metadata": {}, 130 | "outputs": [], 131 | "source": [ 132 | "## TODO: Modify the move function to accommodate the added robabilities of overshooting or undershooting \n", 133 | "pExact = 0.8\n", 134 | "pOvershoot = 0.1\n", 135 | "pUndershoot = 0.1\n", 136 | "\n", 137 | "# Complete the move function\n", 138 | "def move(p, U):\n", 139 | " q=[]\n", 140 | " # iterate through all values in p\n", 141 | " for i in range(len(p)):\n", 142 | " # use the modulo operator to find the new location for a p value\n", 143 | " # this finds an index that is shifted by the correct amount\n", 144 | " index = (i-U) % len(p)\n", 145 | " nextIndex = (index+1) % len(p)\n", 146 | " prevIndex = (index-1) % len(p)\n", 147 | " s = pExact * p[index]\n", 148 | " s = s + pOvershoot * p[nextIndex]\n", 149 | " s = s + pUndershoot * p[prevIndex]\n", 150 | " # append the correct, modified value of p to q\n", 151 | " q.append(s)\n", 152 | " return q\n", 153 | "\n", 154 | "## TODO: try this for U = 2 and see the result\n", 155 | "p = move(p,1)\n", 156 | "print(p)\n", 157 | "display_map(p)" 158 | ] 159 | } 160 | ], 161 | "metadata": { 162 | "kernelspec": { 163 | "display_name": "Python [default]", 164 | "language": "python", 165 | "name": "python3" 166 | }, 167 | "language_info": { 168 | "codemirror_mode": { 169 | "name": "ipython", 170 | "version": 3 171 | }, 172 | "file_extension": ".py", 173 | "mimetype": "text/x-python", 174 | "name": "python", 175 | "nbconvert_exporter": "python", 176 | "pygments_lexer": "ipython3", 177 | "version": "3.6.4" 178 | } 179 | }, 180 | "nbformat": 4, 181 | "nbformat_minor": 2 182 | } 183 | -------------------------------------------------------------------------------- /4_2_Robot_Localization/9_1. Sense and Move, exercise.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "# Sense and Move\n", 8 | "\n", 9 | "In this notebook, let's put all of what we've learned together and see what happens to an initial probability distribution as a robot goes trough cycles of sensing then moving then sensing then moving, and so on! Recall that each time a robot senses (in this case a red or green color)it gains information about its environment, and everytime it moves, it loses some information due to motion uncertainty.\n", 10 | "\n", 11 | "\n", 12 | "\n" 13 | ] 14 | }, 15 | { 16 | "cell_type": "markdown", 17 | "metadata": {}, 18 | "source": [ 19 | "First let's include our usual resource imports and display function." 20 | ] 21 | }, 22 | { 23 | "cell_type": "code", 24 | "execution_count": 1, 25 | "metadata": { 26 | "collapsed": true 27 | }, 28 | "outputs": [], 29 | "source": [ 30 | "# importing resources\n", 31 | "import matplotlib.pyplot as plt\n", 32 | "import numpy as np" 33 | ] 34 | }, 35 | { 36 | "cell_type": "markdown", 37 | "metadata": {}, 38 | "source": [ 39 | "A helper function for visualizing a distribution." 40 | ] 41 | }, 42 | { 43 | "cell_type": "code", 44 | "execution_count": 2, 45 | "metadata": { 46 | "collapsed": true 47 | }, 48 | "outputs": [], 49 | "source": [ 50 | "def display_map(grid, bar_width=1):\n", 51 | " if(len(grid) > 0):\n", 52 | " x_labels = range(len(grid))\n", 53 | " plt.bar(x_labels, height=grid, width=bar_width, color='b')\n", 54 | " plt.xlabel('Grid Cell')\n", 55 | " plt.ylabel('Probability')\n", 56 | " plt.ylim(0, 1) # range of 0-1 for probability values \n", 57 | " plt.title('Probability of the robot being at each cell in the grid')\n", 58 | " plt.xticks(np.arange(min(x_labels), max(x_labels)+1, 1))\n", 59 | " plt.show()\n", 60 | " else:\n", 61 | " print('Grid is empty')\n" 62 | ] 63 | }, 64 | { 65 | "cell_type": "markdown", 66 | "metadata": { 67 | "collapsed": true 68 | }, 69 | "source": [ 70 | "### QUIZ: Given the list motions=[1,1], compute the posterior distribution if the robot first senses red, then moves right one, then senses green, then moves right again, starting with a uniform prior distribution, `p`.\n", 71 | "\n", 72 | "`motions=[1,1]` mean that the robot moves right one cell and then right again. You are given the initial variables and the complete `sense` and `move` function, below." 73 | ] 74 | }, 75 | { 76 | "cell_type": "code", 77 | "execution_count": null, 78 | "metadata": { 79 | "collapsed": true 80 | }, 81 | "outputs": [], 82 | "source": [ 83 | "# given initial variables\n", 84 | "p=[0.2, 0.2, 0.2, 0.2, 0.2]\n", 85 | "# the color of each grid cell in the 1D world\n", 86 | "world=['green', 'red', 'red', 'green', 'green']\n", 87 | "# Z, the sensor reading ('red' or 'green')\n", 88 | "measurements = ['red', 'green']\n", 89 | "pHit = 0.6\n", 90 | "pMiss = 0.2\n", 91 | "\n", 92 | "motions = [1,1]\n", 93 | "pExact = 0.8\n", 94 | "pOvershoot = 0.1\n", 95 | "pUndershoot = 0.1\n", 96 | "\n", 97 | "# You are given the complete sense function\n", 98 | "def sense(p, Z):\n", 99 | " ''' Takes in a current probability distribution, p, and a sensor reading, Z.\n", 100 | " Returns a *normalized* distribution after the sensor measurement has been made, q.\n", 101 | " This should be accurate whether Z is 'red' or 'green'. '''\n", 102 | " q=[]\n", 103 | " # loop through all grid cells\n", 104 | " for i in range(len(p)):\n", 105 | " # check if the sensor reading is equal to the color of the grid cell\n", 106 | " # if so, hit = 1\n", 107 | " # if not, hit = 0\n", 108 | " hit = (Z == world[i])\n", 109 | " q.append(p[i] * (hit * pHit + (1-hit) * pMiss))\n", 110 | " \n", 111 | " # sum up all the components\n", 112 | " s = sum(q)\n", 113 | " # divide all elements of q by the sum to normalize\n", 114 | " for i in range(len(p)):\n", 115 | " q[i] = q[i] / s\n", 116 | " return q\n", 117 | "\n", 118 | "\n", 119 | "# The complete move function\n", 120 | "def move(p, U):\n", 121 | " q=[]\n", 122 | " # iterate through all values in p\n", 123 | " for i in range(len(p)):\n", 124 | " # use the modulo operator to find the new location for a p value\n", 125 | " # this finds an index that is shifted by the correct amount\n", 126 | " index = (i-U) % len(p)\n", 127 | " nextIndex = (index+1) % len(p)\n", 128 | " prevIndex = (index-1) % len(p)\n", 129 | " s = pExact * p[index]\n", 130 | " s = s + pOvershoot * p[nextIndex]\n", 131 | " s = s + pUndershoot * p[prevIndex]\n", 132 | " # append the correct, modified value of p to q\n", 133 | " q.append(s)\n", 134 | " return q\n", 135 | "\n", 136 | "\n", 137 | "## TODO: Compute the posterior distribution if the robot first senses red, then moves \n", 138 | "## right one, then senses green, then moves right again, starting with a uniform prior distribution.\n", 139 | "\n", 140 | "## print/display that distribution" 141 | ] 142 | }, 143 | { 144 | "cell_type": "code", 145 | "execution_count": null, 146 | "metadata": { 147 | "collapsed": true 148 | }, 149 | "outputs": [], 150 | "source": [] 151 | }, 152 | { 153 | "cell_type": "markdown", 154 | "metadata": {}, 155 | "source": [ 156 | "### Clarification about Entropy\n", 157 | "\n", 158 | "The video mentions that entropy will go down after the update step and that entropy will go up after the measurement step. \n", 159 | "\n", 160 | "In general, **entropy measures the amount of uncertainty**. Since the update step increases uncertainty, then entropy should increase. The measurement step decreases uncertainty, so entropy should decrease.\n", 161 | "\n", 162 | "Let's look at our current example where the robot could be at five different positions. The maximum uncertainty occurs when all positions have equal probabilities $[0.2, 0.2, 0.2, 0.2, 0.2]$ \n", 163 | "\n", 164 | "Following the formula $$\\text{Entropy} = \\Sigma (-p \\times log(p))$$we get $$-5 \\times (.2)\\times log(0.2) = 0.699$$\n", 165 | "\n", 166 | "Taking a measurement should decrease uncertainty and thus decrease entropy. Let's say after taking a measurement, the probabilities become [0.05, 0.05, 0.05, 0.8, 0.05]. Now the entropy decreased to 0.338. Hence a measurement step should decrease entropy whereas an update step should increase entropy." 167 | ] 168 | } 169 | ], 170 | "metadata": { 171 | "kernelspec": { 172 | "display_name": "Python [default]", 173 | "language": "python", 174 | "name": "python3" 175 | }, 176 | "language_info": { 177 | "codemirror_mode": { 178 | "name": "ipython", 179 | "version": 3 180 | }, 181 | "file_extension": ".py", 182 | "mimetype": "text/x-python", 183 | "name": "python", 184 | "nbconvert_exporter": "python", 185 | "pygments_lexer": "ipython3", 186 | "version": "3.6.4" 187 | } 188 | }, 189 | "nbformat": 4, 190 | "nbformat_minor": 2 191 | } 192 | -------------------------------------------------------------------------------- /4_2_Robot_Localization/images/2D_trees.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/CVND_Localization_Exercises/f5d5e90e2548aba4a145dd31f352fa9da19244dc/4_2_Robot_Localization/images/2D_trees.png -------------------------------------------------------------------------------- /4_2_Robot_Localization/images/lidar.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/CVND_Localization_Exercises/f5d5e90e2548aba4a145dd31f352fa9da19244dc/4_2_Robot_Localization/images/lidar.png -------------------------------------------------------------------------------- /4_2_Robot_Localization/images/motion_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/CVND_Localization_Exercises/f5d5e90e2548aba4a145dd31f352fa9da19244dc/4_2_Robot_Localization/images/motion_1.png -------------------------------------------------------------------------------- /4_2_Robot_Localization/images/numbered_grid.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/CVND_Localization_Exercises/f5d5e90e2548aba4a145dd31f352fa9da19244dc/4_2_Robot_Localization/images/numbered_grid.png -------------------------------------------------------------------------------- /4_2_Robot_Localization/images/numbered_grid2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/CVND_Localization_Exercises/f5d5e90e2548aba4a145dd31f352fa9da19244dc/4_2_Robot_Localization/images/numbered_grid2.png -------------------------------------------------------------------------------- /4_2_Robot_Localization/images/road_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/CVND_Localization_Exercises/f5d5e90e2548aba4a145dd31f352fa9da19244dc/4_2_Robot_Localization/images/road_1.png -------------------------------------------------------------------------------- /4_2_Robot_Localization/images/road_stop.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/CVND_Localization_Exercises/f5d5e90e2548aba4a145dd31f352fa9da19244dc/4_2_Robot_Localization/images/road_stop.png -------------------------------------------------------------------------------- /4_2_Robot_Localization/images/robot_sensing.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/CVND_Localization_Exercises/f5d5e90e2548aba4a145dd31f352fa9da19244dc/4_2_Robot_Localization/images/robot_sensing.png -------------------------------------------------------------------------------- /4_2_Robot_Localization/images/sense_move.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/CVND_Localization_Exercises/f5d5e90e2548aba4a145dd31f352fa9da19244dc/4_2_Robot_Localization/images/sense_move.png -------------------------------------------------------------------------------- /4_2_Robot_Localization/images/stop_4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/CVND_Localization_Exercises/f5d5e90e2548aba4a145dd31f352fa9da19244dc/4_2_Robot_Localization/images/stop_4.png -------------------------------------------------------------------------------- /4_2_Robot_Localization/images/uncertain_motion.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/CVND_Localization_Exercises/f5d5e90e2548aba4a145dd31f352fa9da19244dc/4_2_Robot_Localization/images/uncertain_motion.png -------------------------------------------------------------------------------- /4_3_2D_Histogram_Filter/helpers.py: -------------------------------------------------------------------------------- 1 | def normalize(grid): 2 | """ 3 | Given a grid of unnormalized probabilities, computes the 4 | correspond normalized version of that grid. 5 | """ 6 | total = 0.0 7 | for row in grid: 8 | for cell in row: 9 | total += cell 10 | for i,row in enumerate(grid): 11 | for j,cell in enumerate(row): 12 | grid[i][j] = float(cell) / total 13 | return grid 14 | 15 | 16 | def blur(grid, blurring): 17 | """ 18 | Spreads probability out on a grid using a 3x3 blurring window. 19 | The blurring parameter controls how much of a belief spills out 20 | into adjacent cells. If blurring is 0 this function will have 21 | no effect. 22 | """ 23 | height = len(grid) 24 | width = len(grid[0]) 25 | 26 | center_prob = 1.0-blurring 27 | corner_prob = blurring / 12.0 28 | adjacent_prob = blurring / 6.0 29 | 30 | window = [ 31 | [corner_prob, adjacent_prob, corner_prob], 32 | [adjacent_prob, center_prob, adjacent_prob], 33 | [corner_prob, adjacent_prob, corner_prob] 34 | ] 35 | new = [[0.0 for i in range(width)] for j in range(height)] 36 | for i in range(height): 37 | for j in range(width): 38 | grid_val = grid[i][j] 39 | for dx in range(-1,2): 40 | for dy in range(-1,2): 41 | mult = window[dx+1][dy+1] 42 | new_i = (i + dy) % height 43 | new_j = (j + dx) % width 44 | new[new_i][new_j] += mult * grid_val 45 | return normalize(new) 46 | 47 | def is_robot_localized(beliefs, true_pos): 48 | """ 49 | Returns None if the robot has no "strong opininon" about 50 | its belief. The robot has a strong opinion when the 51 | size of it's best belief is greater than twice the size of 52 | its second best belief. 53 | 54 | If it DOES have a strong opinion then this function returns 55 | True if that opinion is correct and False if it is not. 56 | """ 57 | best_belief = 0.0 58 | best_pos = None 59 | second_best = 0.0 60 | for y, row in enumerate(beliefs): 61 | for x, belief in enumerate(row): 62 | if belief > best_belief: 63 | second_best = best_belief 64 | best_belief = belief 65 | best_pos = (y,x) 66 | elif belief > second_best: 67 | second_best = belief 68 | if second_best <= 0.00001 or best_belief / second_best > 2.0: 69 | # robot thinks it knows where it is 70 | localized = best_pos == true_pos 71 | return localized, best_pos 72 | else: 73 | # No strong single best belief 74 | return None, best_pos 75 | 76 | def close_enough(g1, g2): 77 | if len(g1) != len(g2): 78 | return False 79 | if len(g1) == 0 or len(g1[0]) != len(g2[0]): 80 | return False 81 | for r1, r2 in zip(g1,g2): 82 | for v1, v2 in zip(r1, r2): 83 | if abs(v1 - v2) > 0.001: 84 | print(v1, v2) 85 | return False 86 | return True -------------------------------------------------------------------------------- /4_3_2D_Histogram_Filter/localizer.py: -------------------------------------------------------------------------------- 1 | # import pdb 2 | from helpers import normalize, blur 3 | 4 | def initialize_beliefs(grid): 5 | height = len(grid) 6 | width = len(grid[0]) 7 | area = height * width 8 | belief_per_cell = 1.0 / area 9 | beliefs = [] 10 | for i in range(height): 11 | row = [] 12 | for j in range(width): 13 | row.append(belief_per_cell) 14 | beliefs.append(row) 15 | return beliefs 16 | 17 | def sense(color, grid, beliefs, p_hit, p_miss): 18 | new_beliefs = [] 19 | 20 | # 21 | # TODO - implement this in part 2 22 | # 23 | 24 | return new_beliefs 25 | 26 | def move(dy, dx, beliefs, blurring): 27 | height = len(beliefs) 28 | width = len(beliefs[0]) 29 | new_G = [[0.0 for i in range(width)] for j in range(height)] 30 | for i, row in enumerate(beliefs): 31 | for j, cell in enumerate(row): 32 | new_i = (i + dy ) % width 33 | new_j = (j + dx ) % height 34 | # pdb.set_trace() 35 | new_G[int(new_i)][int(new_j)] = cell 36 | return blur(new_G, blurring) -------------------------------------------------------------------------------- /4_3_2D_Histogram_Filter/simulate.py: -------------------------------------------------------------------------------- 1 | import localizer 2 | import random 3 | from copy import deepcopy 4 | from matplotlib import pyplot as plt 5 | 6 | class Simulation(object): 7 | def __init__(self, grid, blur, p_hit,start_pos=None): 8 | """ 9 | 10 | """ 11 | self.grid = grid 12 | self.beliefs = localizer.initialize_beliefs(self.grid) 13 | self.height = len(grid) 14 | self.width = len(grid[0]) 15 | self.blur = blur 16 | self.p_hit = p_hit 17 | self.p_miss = 1.0 18 | self.incorrect_sense_probability = self.p_miss / (p_hit + self.p_miss) 19 | self.colors = self.get_colors() 20 | self.num_colors = len(self.colors) 21 | if not start_pos: 22 | self.true_pose = (self.height/2, self.width/2) 23 | else: 24 | self.true_pose = start_pos 25 | self.prev_pose = self.true_pose 26 | self.prepare_visualizer() 27 | 28 | def prepare_visualizer(self): 29 | self.X = [] 30 | self.Y = [] 31 | self.P = [] 32 | 33 | def get_colors(self): 34 | all_colors = [] 35 | for row in self.grid: 36 | for cell in row: 37 | if cell not in all_colors: 38 | all_colors.append(cell) 39 | return all_colors 40 | 41 | def sense(self): 42 | color = self.get_observed_color() 43 | beliefs = deepcopy(self.beliefs) 44 | new_beliefs = localizer.sense(color, self.grid, beliefs, self.p_hit, self.p_miss) 45 | if not new_beliefs or len(new_beliefs) == 0: 46 | print "NOTE! The robot doesn't have a working sense function at this point." 47 | self.beliefs = beliefs 48 | else: 49 | self.beliefs = new_beliefs 50 | 51 | def move(self, dy, dx): 52 | new_y = (self.true_pose[0] + dy) % self.height 53 | new_x = (self.true_pose[1] + dx) % self.width 54 | self.prev_pose = self.true_pose 55 | self.true_pose = (new_y, new_x) 56 | beliefs = deepcopy(self.beliefs) 57 | new_beliefs = localizer.move(dy, dx, beliefs, self.blur) 58 | self.beliefs = new_beliefs 59 | 60 | 61 | def get_observed_color(self): 62 | y,x = self.true_pose 63 | true_color = self.grid[y][x] 64 | if random.random() < self.incorrect_sense_probability: 65 | possible_colors = [] 66 | for color in self.colors: 67 | if color != true_color and color not in possible_colors: 68 | possible_colors.append(color) 69 | color = random.choice(possible_colors) 70 | else: 71 | color = true_color 72 | return color 73 | 74 | def show_beliefs(self,past_turn=False): 75 | if past_turn: 76 | X = deepcopy(self.X) 77 | Y = deepcopy(self.Y) 78 | P = deepcopy(self.P) 79 | 80 | del(self.X[:]) 81 | del(self.Y[:]) 82 | del(self.P[:]) 83 | for y, row in enumerate(self.beliefs): 84 | for x, belief in enumerate(row): 85 | self.X.append(x) 86 | self.Y.append(self.height-y-1) # puts large y ABOVE small y 87 | self.P.append(5000.0 * belief) 88 | plt.figure() 89 | if past_turn: 90 | plt.scatter(X, Y, s=P, alpha=0.3,color="blue") 91 | plt.scatter([self.prev_pose[1]], [self.height-self.true_pose[0]-1], color='red', marker="*", s=200, alpha=0.3) 92 | plt.scatter(self.X,self.Y,s=self.P,color="blue") 93 | plt.scatter([self.true_pose[1]], [self.height-self.true_pose[0]-1], color='red', marker="*", s=200) 94 | plt.show() 95 | 96 | def random_move(self): 97 | dy = random.choice([-1,0,1]) 98 | dx = random.choice([-1,0,1]) 99 | return dy,dx 100 | 101 | def run(self, num_steps=1): 102 | for i in range(num_steps): 103 | self.sense() 104 | dy, dx = self.random_move() 105 | self.move(dy,dx) -------------------------------------------------------------------------------- /4_4_Kalman_Filters/2_1. New Mean and Variance, exercise.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "# New Mean and Variance\n", 8 | "\n", 9 | "Now let's take the formulas from the example below and use them to write a program that takes in two means and variances, and returns a *new*, updated mean and variance for a gaussian. This step is called the parameter or **measurement update** because it is the update that happens when an initial belief (represented by the blue Gaussian, below) is merged with a new piece of information, a measurement with some uncertainty (the orange Gaussian). \n", 10 | "\n", 11 | "As you've seen in the previous quizzes, the updated Gaussian will be a combination of these two Gaussians with a new mean that is in between both of theirs and a variance that is less than the smallest of the two given variances; this means that after a measurement, our new mean is more certain than that of the initial belief!\n", 12 | "\n" 13 | ] 14 | }, 15 | { 16 | "cell_type": "markdown", 17 | "metadata": {}, 18 | "source": [ 19 | "Below is our usual Gaussian equation and imports." 20 | ] 21 | }, 22 | { 23 | "cell_type": "code", 24 | "execution_count": null, 25 | "metadata": { 26 | "collapsed": true 27 | }, 28 | "outputs": [], 29 | "source": [ 30 | "# import math functions\n", 31 | "from math import *\n", 32 | "import matplotlib.pyplot as plt\n", 33 | "import numpy as np\n", 34 | "\n", 35 | "# gaussian function\n", 36 | "def f(mu, sigma2, x):\n", 37 | " ''' f takes in a mean and squared variance, and an input x\n", 38 | " and returns the gaussian value.'''\n", 39 | " coefficient = 1.0 / sqrt(2.0 * pi *sigma2)\n", 40 | " exponential = exp(-0.5 * (x-mu) ** 2 / sigma2)\n", 41 | " return coefficient * exponential\n", 42 | " " 43 | ] 44 | }, 45 | { 46 | "cell_type": "markdown", 47 | "metadata": {}, 48 | "source": [ 49 | "### QUIZ: Write an `update` function that performs the measurement update.\n", 50 | "\n", 51 | "This function should combine the given Gaussian parameters and return new values for the mean and squared variance.\n", 52 | "\n", 53 | "This function does not have to perform any exponential math, it simply has to follow the equations for the measurement update as seen in the image at the top of this notebook. You may assume that the given variances `var1` and `var2` are squared terms." 54 | ] 55 | }, 56 | { 57 | "cell_type": "code", 58 | "execution_count": null, 59 | "metadata": { 60 | "collapsed": true 61 | }, 62 | "outputs": [], 63 | "source": [ 64 | "# the update function\n", 65 | "def update(mean1, var1, mean2, var2):\n", 66 | " ''' This function takes in two means and two squared variance terms,\n", 67 | " and returns updated gaussian parameters.'''\n", 68 | " ## TODO: Calculate the new parameters\n", 69 | " new_mean = None\n", 70 | " new_var = None\n", 71 | " \n", 72 | " return [new_mean, new_var]" 73 | ] 74 | }, 75 | { 76 | "cell_type": "code", 77 | "execution_count": null, 78 | "metadata": { 79 | "collapsed": true 80 | }, 81 | "outputs": [], 82 | "source": [ 83 | "# test your implementation\n", 84 | "new_params = update(10, 4, 12, 4)\n", 85 | "print(new_params)" 86 | ] 87 | }, 88 | { 89 | "cell_type": "markdown", 90 | "metadata": {}, 91 | "source": [ 92 | "### Plot a Gaussian\n", 93 | "\n", 94 | "Plot a Gaussian by looping through a range of x values and creating a resulting list of Gaussian values, `g`, as shown below. You're encouraged to see what happens if you change the values of `mu` and `sigma2`." 95 | ] 96 | }, 97 | { 98 | "cell_type": "code", 99 | "execution_count": null, 100 | "metadata": { 101 | "collapsed": true 102 | }, 103 | "outputs": [], 104 | "source": [ 105 | "# display a gaussian over a range of x values\n", 106 | "# define the parameters\n", 107 | "mu = new_params[0]\n", 108 | "sigma2 = new_params[1]\n", 109 | "\n", 110 | "# define a range of x values\n", 111 | "x_axis = np.arange(0, 20, 0.1)\n", 112 | "\n", 113 | "# create a corresponding list of gaussian values\n", 114 | "g = []\n", 115 | "for x in x_axis:\n", 116 | " g.append(f(mu, sigma2, x))\n", 117 | "\n", 118 | "# plot the result \n", 119 | "plt.plot(x_axis, g)" 120 | ] 121 | } 122 | ], 123 | "metadata": { 124 | "kernelspec": { 125 | "display_name": "Python [conda env:cvnd_1]", 126 | "language": "python", 127 | "name": "conda-env-cvnd_1-py" 128 | }, 129 | "language_info": { 130 | "codemirror_mode": { 131 | "name": "ipython", 132 | "version": 3 133 | }, 134 | "file_extension": ".py", 135 | "mimetype": "text/x-python", 136 | "name": "python", 137 | "nbconvert_exporter": "python", 138 | "pygments_lexer": "ipython3", 139 | "version": "3.5.2" 140 | } 141 | }, 142 | "nbformat": 4, 143 | "nbformat_minor": 2 144 | } 145 | -------------------------------------------------------------------------------- /4_4_Kalman_Filters/3_1. Predict Function, exercise.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "# Predict Function\n", 8 | "\n", 9 | "After performing a parameter update, which is done after some new measurement is collected, the next step is to incorporate motion into our Gaussian calculations. Recall that, as we estimate the location of a robot or self-driving car:\n", 10 | "* the measurement update *increases* our estimation certainty\n", 11 | "* the motion update/prediction *decreases* our certainty\n", 12 | "\n", 13 | "That is because every motion has some chance of under or overshooting its goal, and since motion is not exact, we end up losing some certainty about our exact location after each motion.\n", 14 | "\n", 15 | "Let's take the formulas from the example below and use them to write a program that takes in a mean and a motion and squared variances for both of those quantities, and returns a *new*, updated mean and variance for a new gaussian. This step is called the **motion update** or the predict step.\n", 16 | "\n", 17 | "\n" 18 | ] 19 | }, 20 | { 21 | "cell_type": "markdown", 22 | "metadata": {}, 23 | "source": [ 24 | "Below is our usual Gaussian equation and imports." 25 | ] 26 | }, 27 | { 28 | "cell_type": "code", 29 | "execution_count": null, 30 | "metadata": { 31 | "collapsed": true 32 | }, 33 | "outputs": [], 34 | "source": [ 35 | "# import math functions\n", 36 | "from math import *\n", 37 | "import matplotlib.pyplot as plt\n", 38 | "import numpy as np\n", 39 | "\n", 40 | "# gaussian function\n", 41 | "def f(mu, sigma2, x):\n", 42 | " ''' f takes in a mean and squared variance, and an input x\n", 43 | " and returns the gaussian value.'''\n", 44 | " coefficient = 1.0 / sqrt(2.0 * pi *sigma2)\n", 45 | " exponential = exp(-0.5 * (x-mu) ** 2 / sigma2)\n", 46 | " return coefficient * exponential\n", 47 | " " 48 | ] 49 | }, 50 | { 51 | "cell_type": "markdown", 52 | "metadata": {}, 53 | "source": [ 54 | "For convenience, you've also been given the complete `update` code that performs a parameter update when an initial belief and new measurement information are merged." 55 | ] 56 | }, 57 | { 58 | "cell_type": "code", 59 | "execution_count": null, 60 | "metadata": { 61 | "collapsed": true 62 | }, 63 | "outputs": [], 64 | "source": [ 65 | "# the update function\n", 66 | "def update(mean1, var1, mean2, var2):\n", 67 | " ''' This function takes in two means and two squared variance terms,\n", 68 | " and returns updated gaussian parameters.'''\n", 69 | " # Calculate the new parameters\n", 70 | " new_mean = (var2*mean1 + var1*mean2)/(var2+var1)\n", 71 | " new_var = 1/(1/var2 + 1/var1)\n", 72 | " \n", 73 | " return [new_mean, new_var]" 74 | ] 75 | }, 76 | { 77 | "cell_type": "markdown", 78 | "metadata": {}, 79 | "source": [ 80 | "### QUIZ: Write a `predict` function that returns new values for the mean and squared variance of a Gaussian after a motion.\n", 81 | "\n", 82 | "This function should take in parameters for an initial belief and motion and perform the measurement update as seen in the image at the top of this notebook." 83 | ] 84 | }, 85 | { 86 | "cell_type": "code", 87 | "execution_count": null, 88 | "metadata": { 89 | "collapsed": true 90 | }, 91 | "outputs": [], 92 | "source": [ 93 | "# the motion update/predict function\n", 94 | "def predict(mean1, var1, mean2, var2):\n", 95 | " ''' This function takes in two means and two squared variance terms,\n", 96 | " and returns updated gaussian parameters, after motion.'''\n", 97 | " ## TODO: Calculate the new parameters\n", 98 | " new_mean = None\n", 99 | " new_var = None\n", 100 | " \n", 101 | " return [new_mean, new_var]" 102 | ] 103 | }, 104 | { 105 | "cell_type": "code", 106 | "execution_count": null, 107 | "metadata": {}, 108 | "outputs": [], 109 | "source": [ 110 | "# test your implementation\n", 111 | "new_params = predict(10, 4, 12, 4)\n", 112 | "print(new_params)" 113 | ] 114 | }, 115 | { 116 | "cell_type": "markdown", 117 | "metadata": {}, 118 | "source": [ 119 | "### Plot a Gaussian\n", 120 | "\n", 121 | "Plot a Gaussian by looping through a range of x values and creating a resulting list of Gaussian values, `g`, as shown below. You're encouraged to see what happens if you change the values of `mu` and `sigma2`." 122 | ] 123 | }, 124 | { 125 | "cell_type": "code", 126 | "execution_count": null, 127 | "metadata": {}, 128 | "outputs": [], 129 | "source": [ 130 | "# display a gaussian over a range of x values\n", 131 | "# define the parameters\n", 132 | "mu = new_params[0]\n", 133 | "sigma2 = new_params[1]\n", 134 | "\n", 135 | "# define a range of x values\n", 136 | "x_axis = np.arange(0, 40, 0.1)\n", 137 | "\n", 138 | "# create a corresponding list of gaussian values\n", 139 | "g = []\n", 140 | "for x in x_axis:\n", 141 | " g.append(f(mu, sigma2, x))\n", 142 | "\n", 143 | "# plot the result \n", 144 | "plt.plot(x_axis, g)" 145 | ] 146 | } 147 | ], 148 | "metadata": { 149 | "kernelspec": { 150 | "display_name": "Python [conda env:cvnd_1]", 151 | "language": "python", 152 | "name": "conda-env-cvnd_1-py" 153 | }, 154 | "language_info": { 155 | "codemirror_mode": { 156 | "name": "ipython", 157 | "version": 3 158 | }, 159 | "file_extension": ".py", 160 | "mimetype": "text/x-python", 161 | "name": "python", 162 | "nbconvert_exporter": "python", 163 | "pygments_lexer": "ipython3", 164 | "version": "3.5.2" 165 | } 166 | }, 167 | "nbformat": 4, 168 | "nbformat_minor": 2 169 | } 170 | -------------------------------------------------------------------------------- /4_4_Kalman_Filters/4_1. 1D Kalman Filter, exercise.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "# 1D Kalman Filter\n", 8 | "\n", 9 | "Now, you're ready to implement a 1D Kalman Filter by putting all these steps together. Let's take the case of a robot that moves through the world. As a robot moves through the world it locates itself by performing a cycle of:\n", 10 | "1. sensing and performing a measurement update and\n", 11 | "2. moving and performing a motion update\n", 12 | "\n", 13 | "You've programmed each of these steps individually, so now let's combine them in a cycle!\n", 14 | "\n", 15 | "After implementing this filter, you should see that you can go from a very uncertain location Gaussian to a more and more certain Gaussian, as pictured below. The code in this notebooks is really just a simplified version of the Kalman filter that runs in the Google self-driving car that is used to track surrounding vehicles and other objects.\n", 16 | "\n", 17 | "\n", 18 | "\n", 19 | "---" 20 | ] 21 | }, 22 | { 23 | "cell_type": "markdown", 24 | "metadata": {}, 25 | "source": [ 26 | "Below is our usual Gaussian equation and imports." 27 | ] 28 | }, 29 | { 30 | "cell_type": "code", 31 | "execution_count": null, 32 | "metadata": { 33 | "collapsed": true 34 | }, 35 | "outputs": [], 36 | "source": [ 37 | "# import math functions\n", 38 | "from math import *\n", 39 | "import matplotlib.pyplot as plt\n", 40 | "import numpy as np\n", 41 | "\n", 42 | "# gaussian function\n", 43 | "def f(mu, sigma2, x):\n", 44 | " ''' f takes in a mean and squared variance, and an input x\n", 45 | " and returns the gaussian value.'''\n", 46 | " coefficient = 1.0 / sqrt(2.0 * pi *sigma2)\n", 47 | " exponential = exp(-0.5 * (x-mu) ** 2 / sigma2)\n", 48 | " return coefficient * exponential\n", 49 | " " 50 | ] 51 | }, 52 | { 53 | "cell_type": "markdown", 54 | "metadata": {}, 55 | "source": [ 56 | "You've also been given the complete `update` code that performs a parameter update when an initial belief and new measurement information are merged. And the complete `predict` code that performs an update to a Gasuuain after a motion is incorporated." 57 | ] 58 | }, 59 | { 60 | "cell_type": "code", 61 | "execution_count": null, 62 | "metadata": { 63 | "collapsed": true 64 | }, 65 | "outputs": [], 66 | "source": [ 67 | "# the update function\n", 68 | "def update(mean1, var1, mean2, var2):\n", 69 | " ''' This function takes in two means and two squared variance terms,\n", 70 | " and returns updated gaussian parameters.'''\n", 71 | " # Calculate the new parameters\n", 72 | " new_mean = (var2*mean1 + var1*mean2)/(var2+var1)\n", 73 | " new_var = 1/(1/var2 + 1/var1)\n", 74 | " \n", 75 | " return [new_mean, new_var]\n", 76 | "\n", 77 | "\n", 78 | "# the motion update/predict function\n", 79 | "def predict(mean1, var1, mean2, var2):\n", 80 | " ''' This function takes in two means and two squared variance terms,\n", 81 | " and returns updated gaussian parameters, after motion.'''\n", 82 | " # Calculate the new parameters\n", 83 | " new_mean = mean1 + mean2\n", 84 | " new_var = var1 + var2\n", 85 | " \n", 86 | " return [new_mean, new_var]" 87 | ] 88 | }, 89 | { 90 | "cell_type": "markdown", 91 | "metadata": {}, 92 | "source": [ 93 | "### QUIZ: For the given measurements and motions, write complete 1D Kalman filter code that loops through all of these in order.\n", 94 | "\n", 95 | "Your complete code should look at sensor measurements then motions in that sequence until all updates are done!\n", 96 | "\n", 97 | "### Initial Uncertainty\n", 98 | "\n", 99 | "You'll see that you are given initial parameters below, and this includes and nitial location estimation, `mu` and squared variance, `sig`. Note that the initial estimate is set to the location 0, and the variance is extremely large; this is a state of high confusion much like the *uniform* distribution we used in the histogram filter. There are also values given for the squared variance associated with the sensor measurements and the motion, since neither of those readings are perfect, either.\n", 100 | "\n", 101 | "You should see that even though the initial estimate for location (the initial `mu`) is far from the first measurement, it should catch up fairly quickly as you cycle through measurements and motions." 102 | ] 103 | }, 104 | { 105 | "cell_type": "code", 106 | "execution_count": null, 107 | "metadata": { 108 | "collapsed": true 109 | }, 110 | "outputs": [], 111 | "source": [ 112 | "# measurements for mu and motions, U\n", 113 | "measurements = [5., 6., 7., 9., 10.]\n", 114 | "motions = [1., 1., 2., 1., 1.]\n", 115 | "\n", 116 | "# initial parameters\n", 117 | "measurement_sig = 4.\n", 118 | "motion_sig = 2.\n", 119 | "mu = 0.\n", 120 | "sig = 10000.\n", 121 | "\n", 122 | "## TODO: Loop through all measurements/motions\n", 123 | "## Print out and display the resulting Gaussian \n", 124 | "\n", 125 | "# your code here\n" 126 | ] 127 | }, 128 | { 129 | "cell_type": "code", 130 | "execution_count": null, 131 | "metadata": {}, 132 | "outputs": [], 133 | "source": [] 134 | }, 135 | { 136 | "cell_type": "markdown", 137 | "metadata": {}, 138 | "source": [ 139 | "### Plot a Gaussian\n", 140 | "\n", 141 | "Plot a Gaussian by looping through a range of x values and creating a resulting list of Gaussian values, `g`, as shown below. You're encouraged to see what happens if you change the values of `mu` and `sigma2`." 142 | ] 143 | }, 144 | { 145 | "cell_type": "code", 146 | "execution_count": null, 147 | "metadata": {}, 148 | "outputs": [], 149 | "source": [ 150 | "# display the *initial* gaussian over a range of x values\n", 151 | "# define the parameters\n", 152 | "mu = 0\n", 153 | "sigma2 = 10000\n", 154 | "\n", 155 | "# define a range of x values\n", 156 | "x_axis = np.arange(-20, 20, 0.1)\n", 157 | "\n", 158 | "# create a corresponding list of gaussian values\n", 159 | "g = []\n", 160 | "for x in x_axis:\n", 161 | " g.append(f(mu, sigma2, x))\n", 162 | "\n", 163 | "# plot the result \n", 164 | "plt.plot(x_axis, g)" 165 | ] 166 | } 167 | ], 168 | "metadata": { 169 | "kernelspec": { 170 | "display_name": "Python [conda env:cvnd_1]", 171 | "language": "python", 172 | "name": "conda-env-cvnd_1-py" 173 | }, 174 | "language_info": { 175 | "codemirror_mode": { 176 | "name": "ipython", 177 | "version": 3 178 | }, 179 | "file_extension": ".py", 180 | "mimetype": "text/x-python", 181 | "name": "python", 182 | "nbconvert_exporter": "python", 183 | "pygments_lexer": "ipython3", 184 | "version": "3.5.2" 185 | } 186 | }, 187 | "nbformat": 4, 188 | "nbformat_minor": 2 189 | } 190 | -------------------------------------------------------------------------------- /4_4_Kalman_Filters/images/gaussian_updates.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/CVND_Localization_Exercises/f5d5e90e2548aba4a145dd31f352fa9da19244dc/4_4_Kalman_Filters/images/gaussian_updates.png -------------------------------------------------------------------------------- /4_4_Kalman_Filters/images/mean_var.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/CVND_Localization_Exercises/f5d5e90e2548aba4a145dd31f352fa9da19244dc/4_4_Kalman_Filters/images/mean_var.png -------------------------------------------------------------------------------- /4_4_Kalman_Filters/images/motion_update.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/CVND_Localization_Exercises/f5d5e90e2548aba4a145dd31f352fa9da19244dc/4_4_Kalman_Filters/images/motion_update.png -------------------------------------------------------------------------------- /4_5_State_and_Motion/1. Interacting with a Car Object.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "# Interacting with a Car Object" 8 | ] 9 | }, 10 | { 11 | "cell_type": "markdown", 12 | "metadata": {}, 13 | "source": [ 14 | "In this notebook, you've been given some of the starting code for creating and interacting with a car object.\n", 15 | "\n", 16 | "Your tasks are to:\n", 17 | "1. Become familiar with this code. \n", 18 | " - Know how to create a car object, and how to move and turn that car.\n", 19 | "2. Constantly visualize.\n", 20 | " - To make sure your code is working as expected, frequently call `display_world()` to see the result!\n", 21 | "3. **Make the car move in a 4x4 square path.** \n", 22 | " - If you understand the move and turn functions, you should be able to tell a car to move in a square path. This task is a **TODO** at the end of this notebook.\n", 23 | "\n", 24 | "Feel free to change the values of initial variables and add functions as you see fit!\n", 25 | "\n", 26 | "And remember, to run a cell in the notebook, press `Shift+Enter`." 27 | ] 28 | }, 29 | { 30 | "cell_type": "code", 31 | "execution_count": null, 32 | "metadata": { 33 | "collapsed": true 34 | }, 35 | "outputs": [], 36 | "source": [ 37 | "import numpy as np\n", 38 | "import car\n", 39 | "\n", 40 | "%matplotlib inline" 41 | ] 42 | }, 43 | { 44 | "cell_type": "markdown", 45 | "metadata": {}, 46 | "source": [ 47 | "### Define the initial variables" 48 | ] 49 | }, 50 | { 51 | "cell_type": "code", 52 | "execution_count": null, 53 | "metadata": { 54 | "collapsed": true 55 | }, 56 | "outputs": [], 57 | "source": [ 58 | "# Create a 2D world of 0's\n", 59 | "height = 4\n", 60 | "width = 6\n", 61 | "world = np.zeros((height, width))\n", 62 | "\n", 63 | "# Define the initial car state\n", 64 | "initial_position = [0, 0] # [y, x] (top-left corner)\n", 65 | "velocity = [0, 1] # [vy, vx] (moving to the right)\n" 66 | ] 67 | }, 68 | { 69 | "cell_type": "markdown", 70 | "metadata": {}, 71 | "source": [ 72 | "### Create a car object" 73 | ] 74 | }, 75 | { 76 | "cell_type": "code", 77 | "execution_count": null, 78 | "metadata": { 79 | "collapsed": true 80 | }, 81 | "outputs": [], 82 | "source": [ 83 | "# Create a car object with these initial params\n", 84 | "carla = car.Car(initial_position, velocity, world)\n", 85 | "\n", 86 | "print('Carla\\'s initial state is: ' + str(carla.state))" 87 | ] 88 | }, 89 | { 90 | "cell_type": "markdown", 91 | "metadata": {}, 92 | "source": [ 93 | "### Move and track state" 94 | ] 95 | }, 96 | { 97 | "cell_type": "code", 98 | "execution_count": null, 99 | "metadata": { 100 | "collapsed": true 101 | }, 102 | "outputs": [], 103 | "source": [ 104 | "# Move in the direction of the initial velocity\n", 105 | "carla.move()\n", 106 | "\n", 107 | "# Track the change in state\n", 108 | "print('Carla\\'s state is: ' + str(carla.state))\n", 109 | "\n", 110 | "# Display the world\n", 111 | "carla.display_world()" 112 | ] 113 | }, 114 | { 115 | "cell_type": "markdown", 116 | "metadata": {}, 117 | "source": [ 118 | "## TODO: Move in a square path\n", 119 | "\n", 120 | "Using the `move()` and `turn_left()` functions, make carla traverse a 4x4 square path.\n", 121 | "\n", 122 | "The output should look like:\n", 123 | "" 124 | ] 125 | }, 126 | { 127 | "cell_type": "code", 128 | "execution_count": null, 129 | "metadata": { 130 | "collapsed": true 131 | }, 132 | "outputs": [], 133 | "source": [ 134 | "## TODO: Make carla traverse a 4x4 square path\n", 135 | "## Display the result\n" 136 | ] 137 | } 138 | ], 139 | "metadata": { 140 | "anaconda-cloud": {}, 141 | "kernelspec": { 142 | "display_name": "Python 3", 143 | "language": "python", 144 | "name": "python3" 145 | }, 146 | "language_info": { 147 | "codemirror_mode": { 148 | "name": "ipython", 149 | "version": 3 150 | }, 151 | "file_extension": ".py", 152 | "mimetype": "text/x-python", 153 | "name": "python", 154 | "nbconvert_exporter": "python", 155 | "pygments_lexer": "ipython3", 156 | "version": "3.6.3" 157 | } 158 | }, 159 | "nbformat": 4, 160 | "nbformat_minor": 1 161 | } 162 | -------------------------------------------------------------------------------- /4_5_State_and_Motion/2. Car, playground.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "# Car playground 1\n", 8 | "\n", 9 | "This notebook provides some initial variables and creates one car object, but, here, you are encouraged to play around with the car movement code!\n", 10 | "\n", 11 | "This playground is especially useful if you **modify the car.py file** and want to test out some new car property or function!\n", 12 | "\n", 13 | "So, your only tasks for this notebook are to use it as your own personal testing ground. Some ideas:\n", 14 | "1. Create multiple cars, with different initial states\n", 15 | " - And visualize them using `display_world()`\n", 16 | "2. Add a function in car.py (by navigating back to it clicking the orange Jupyter icon), and then call that function, here!\n", 17 | "3. Add a new initial variable to __init__ in car.py and use it!" 18 | ] 19 | }, 20 | { 21 | "cell_type": "code", 22 | "execution_count": null, 23 | "metadata": { 24 | "collapsed": true 25 | }, 26 | "outputs": [], 27 | "source": [ 28 | "import numpy as np\n", 29 | "import car\n", 30 | "\n", 31 | "%matplotlib inline\n", 32 | "\n", 33 | "# Auto-reload function so that this notebook keeps up with \n", 34 | "# changes in the class file \n", 35 | "%load_ext autoreload\n", 36 | "%autoreload 2" 37 | ] 38 | }, 39 | { 40 | "cell_type": "markdown", 41 | "metadata": {}, 42 | "source": [ 43 | "### Create a new car object" 44 | ] 45 | }, 46 | { 47 | "cell_type": "code", 48 | "execution_count": null, 49 | "metadata": { 50 | "collapsed": true 51 | }, 52 | "outputs": [], 53 | "source": [ 54 | "# Create a 2D world of 0's\n", 55 | "height = 4\n", 56 | "width = 6\n", 57 | "world = np.zeros((height, width))\n", 58 | "\n", 59 | "# Define the initial car state\n", 60 | "initial_position = [0, 0] # [y, x] (top-left corner)\n", 61 | "velocity = [0, 1] # [vy, vx] (moving to the right)\n", 62 | "\n", 63 | "# Create a car with initial params\n", 64 | "carla = car.Car(initial_position, velocity, world)\n", 65 | "carla.display_world()" 66 | ] 67 | }, 68 | { 69 | "cell_type": "code", 70 | "execution_count": null, 71 | "metadata": { 72 | "collapsed": true 73 | }, 74 | "outputs": [], 75 | "source": [ 76 | "## TODO: Create multiple cars and visualize them\n" 77 | ] 78 | } 79 | ], 80 | "metadata": { 81 | "anaconda-cloud": {}, 82 | "kernelspec": { 83 | "display_name": "Python 3", 84 | "language": "python", 85 | "name": "python3" 86 | }, 87 | "language_info": { 88 | "codemirror_mode": { 89 | "name": "ipython", 90 | "version": 3 91 | }, 92 | "file_extension": ".py", 93 | "mimetype": "text/x-python", 94 | "name": "python", 95 | "nbconvert_exporter": "python", 96 | "pygments_lexer": "ipython3", 97 | "version": "3.6.1" 98 | } 99 | }, 100 | "nbformat": 4, 101 | "nbformat_minor": 1 102 | } 103 | -------------------------------------------------------------------------------- /4_5_State_and_Motion/3. Turning Right.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "# Turning Right\n", 8 | "\n", 9 | "This notebook provides some initial variables and creates one car object!\n", 10 | "\n", 11 | "This time around, you are expected to **modify the car.py file** and test out some new functionality!\n", 12 | "\n", 13 | "Your tasks for this notebook are:\n", 14 | "1. Add a `turn_right()` function to `car.py`\n", 15 | " - There are a few ways to do this. I'd suggest looking at the code in `turn_left()` or even *using* this function.\n", 16 | "2. Don't forget to update the **state** as necessary, after a turn!\n", 17 | "3. Test out your `turn_right()` function in this notebook by visualizing the car as it moves, and printing out the state of the car to see if it matches what you expect!" 18 | ] 19 | }, 20 | { 21 | "cell_type": "code", 22 | "execution_count": 2, 23 | "metadata": { 24 | "collapsed": true 25 | }, 26 | "outputs": [], 27 | "source": [ 28 | "import numpy as np\n", 29 | "import car\n", 30 | "\n", 31 | "%matplotlib inline\n", 32 | "\n", 33 | "# Auto-reload function so that this notebook keeps up with \n", 34 | "# changes in the class file \n", 35 | "%load_ext autoreload\n", 36 | "%autoreload 2" 37 | ] 38 | }, 39 | { 40 | "cell_type": "markdown", 41 | "metadata": {}, 42 | "source": [ 43 | "### Create a new car object" 44 | ] 45 | }, 46 | { 47 | "cell_type": "code", 48 | "execution_count": 3, 49 | "metadata": { 50 | "collapsed": true 51 | }, 52 | "outputs": [], 53 | "source": [ 54 | "# Create a 2D world of 0's\n", 55 | "height = 4\n", 56 | "width = 6\n", 57 | "world = np.zeros((height, width))\n", 58 | "\n", 59 | "# Define the initial car state\n", 60 | "initial_position = [0, 0] # [y, x] (top-left corner)\n", 61 | "velocity = [0, 1] # [vy, vx] (moving to the right)\n", 62 | "\n", 63 | "# Create a car with initial params\n", 64 | "carla = car.Car(initial_position, velocity, world)\n" 65 | ] 66 | }, 67 | { 68 | "cell_type": "markdown", 69 | "metadata": {}, 70 | "source": [ 71 | "### Directory of Python files\n", 72 | "\n", 73 | "Remember, to go back to see and change all your files, click on the orange Jupyter icon at the top left of this notebook! There you'll see this notebook and the class file `car.py`, which you'll need to open and change.\n", 74 | "\n", 75 | "I recommend opening this notebook and the car.py file in new tabs so that you can easily look at both!" 76 | ] 77 | }, 78 | { 79 | "cell_type": "code", 80 | "execution_count": null, 81 | "metadata": { 82 | "collapsed": true 83 | }, 84 | "outputs": [], 85 | "source": [ 86 | "## TODO: Move carla around, using your new turn_right() function\n", 87 | "## Display the result and the state as it changes\n" 88 | ] 89 | } 90 | ], 91 | "metadata": { 92 | "anaconda-cloud": {}, 93 | "kernelspec": { 94 | "display_name": "Python 3", 95 | "language": "python", 96 | "name": "python3" 97 | }, 98 | "language_info": { 99 | "codemirror_mode": { 100 | "name": "ipython", 101 | "version": 3 102 | }, 103 | "file_extension": ".py", 104 | "mimetype": "text/x-python", 105 | "name": "python", 106 | "nbconvert_exporter": "python", 107 | "pygments_lexer": "ipython3", 108 | "version": "3.6.1" 109 | } 110 | }, 111 | "nbformat": 4, 112 | "nbformat_minor": 1 113 | } 114 | -------------------------------------------------------------------------------- /4_5_State_and_Motion/4. Multiple, Colorful Cars.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "# Creating colorful cars\n", 8 | "\n", 9 | "Now, the `car.py` file has been modified so that `__init__` takes in an optional color parameter!\n", 10 | "\n", 11 | "Your tasks for this notebook are:\n", 12 | "1. Create multiple cars of different colors\n", 13 | "2. Move them around and display the result\n", 14 | "3. (Optional) Add another variable to __init__ like maximum_speed or a boolean true or false depending on if the car has good speakers. It's up to you!\n", 15 | "\n", 16 | "Your options for color values include:\n", 17 | "* b: blue\n", 18 | "* g: green\n", 19 | "* r: red\n", 20 | "* c: cyan\n", 21 | "* m: magenta\n", 22 | "* y: yellow\n", 23 | "* k: black\n", 24 | "* w: white\n", 25 | "\n", 26 | "More color info can be found, [here](https://matplotlib.org/api/colors_api.html)." 27 | ] 28 | }, 29 | { 30 | "cell_type": "code", 31 | "execution_count": null, 32 | "metadata": {}, 33 | "outputs": [], 34 | "source": [ 35 | "import numpy as np\n", 36 | "import car\n", 37 | "\n", 38 | "%matplotlib inline\n", 39 | "\n", 40 | "# Auto-reload function so that this notebook keeps up with \n", 41 | "# changes in the class file \n", 42 | "%load_ext autoreload\n", 43 | "%autoreload 2" 44 | ] 45 | }, 46 | { 47 | "cell_type": "markdown", 48 | "metadata": {}, 49 | "source": [ 50 | "### Define some initial variables" 51 | ] 52 | }, 53 | { 54 | "cell_type": "code", 55 | "execution_count": null, 56 | "metadata": {}, 57 | "outputs": [], 58 | "source": [ 59 | "# Create a 2D world of 0's\n", 60 | "height = 4\n", 61 | "width = 6\n", 62 | "world = np.zeros((height, width))\n", 63 | "\n", 64 | "# Define the initial car state\n", 65 | "initial_position = [0, 0] # [y, x] (top-left corner)\n", 66 | "velocity = [0, 1] # [vy, vx] (moving to the right)\n" 67 | ] 68 | }, 69 | { 70 | "cell_type": "code", 71 | "execution_count": null, 72 | "metadata": { 73 | "collapsed": true 74 | }, 75 | "outputs": [], 76 | "source": [ 77 | "## TODO: Create two cars of different colors and display their different worlds\n" 78 | ] 79 | }, 80 | { 81 | "cell_type": "code", 82 | "execution_count": null, 83 | "metadata": { 84 | "collapsed": true 85 | }, 86 | "outputs": [], 87 | "source": [] 88 | } 89 | ], 90 | "metadata": { 91 | "anaconda-cloud": {}, 92 | "kernelspec": { 93 | "display_name": "Python 3", 94 | "language": "python", 95 | "name": "python3" 96 | }, 97 | "language_info": { 98 | "codemirror_mode": { 99 | "name": "ipython", 100 | "version": 3 101 | }, 102 | "file_extension": ".py", 103 | "mimetype": "text/x-python", 104 | "name": "python", 105 | "nbconvert_exporter": "python", 106 | "pygments_lexer": "ipython3", 107 | "version": "3.6.1" 108 | } 109 | }, 110 | "nbformat": 4, 111 | "nbformat_minor": 1 112 | } 113 | -------------------------------------------------------------------------------- /4_5_State_and_Motion/4x4_path.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/CVND_Localization_Exercises/f5d5e90e2548aba4a145dd31f352fa9da19244dc/4_5_State_and_Motion/4x4_path.png -------------------------------------------------------------------------------- /4_5_State_and_Motion/5. Create color playground.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "# Create a color object" 8 | ] 9 | }, 10 | { 11 | "cell_type": "code", 12 | "execution_count": null, 13 | "metadata": {}, 14 | "outputs": [], 15 | "source": [ 16 | "# Notice we are importing the color class!\n", 17 | "\n", 18 | "import numpy as np\n", 19 | "import color\n", 20 | "\n", 21 | "%matplotlib inline\n", 22 | "%load_ext autoreload\n", 23 | "%autoreload 2" 24 | ] 25 | }, 26 | { 27 | "cell_type": "markdown", 28 | "metadata": {}, 29 | "source": [ 30 | "### Define the color and print it out" 31 | ] 32 | }, 33 | { 34 | "cell_type": "code", 35 | "execution_count": null, 36 | "metadata": {}, 37 | "outputs": [], 38 | "source": [ 39 | "# r, g, b\n", 40 | "r = 200\n", 41 | "g = 0\n", 42 | "b = 200\n", 43 | "\n", 44 | "# Create the color object\n", 45 | "test_color = color.Color(r, g, b)\n", 46 | "\n", 47 | "# This will throw an error if the class code is incomplete\n", 48 | "print(test_color)" 49 | ] 50 | }, 51 | { 52 | "cell_type": "code", 53 | "execution_count": null, 54 | "metadata": { 55 | "collapsed": true 56 | }, 57 | "outputs": [], 58 | "source": [] 59 | } 60 | ], 61 | "metadata": { 62 | "anaconda-cloud": {}, 63 | "kernelspec": { 64 | "display_name": "Python 3", 65 | "language": "python", 66 | "name": "python3" 67 | }, 68 | "language_info": { 69 | "codemirror_mode": { 70 | "name": "ipython", 71 | "version": 3 72 | }, 73 | "file_extension": ".py", 74 | "mimetype": "text/x-python", 75 | "name": "python", 76 | "nbconvert_exporter": "python", 77 | "pygments_lexer": "ipython3", 78 | "version": "3.6.1" 79 | } 80 | }, 81 | "nbformat": 4, 82 | "nbformat_minor": 1 83 | } 84 | -------------------------------------------------------------------------------- /4_5_State_and_Motion/6. Add color playground.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "# Adding colors" 8 | ] 9 | }, 10 | { 11 | "cell_type": "code", 12 | "execution_count": null, 13 | "metadata": { 14 | "collapsed": true 15 | }, 16 | "outputs": [], 17 | "source": [ 18 | "# Notice we are importing the color class!\n", 19 | "\n", 20 | "import numpy as np\n", 21 | "import color\n", 22 | "\n", 23 | "%matplotlib inline\n", 24 | "\n", 25 | "# Auto-reload function so that this notebook keeps up with \n", 26 | "# changes in the class file \n", 27 | "%load_ext autoreload\n", 28 | "%autoreload 2" 29 | ] 30 | }, 31 | { 32 | "cell_type": "markdown", 33 | "metadata": {}, 34 | "source": [ 35 | "### Define colors and print them out" 36 | ] 37 | }, 38 | { 39 | "cell_type": "code", 40 | "execution_count": null, 41 | "metadata": { 42 | "collapsed": true 43 | }, 44 | "outputs": [], 45 | "source": [ 46 | "color1 = color.Color(250, 0, 0)\n", 47 | "print(color1)" 48 | ] 49 | }, 50 | { 51 | "cell_type": "code", 52 | "execution_count": null, 53 | "metadata": { 54 | "collapsed": true 55 | }, 56 | "outputs": [], 57 | "source": [ 58 | "color2 = color.Color(0, 50, 200)\n", 59 | "print(color2)" 60 | ] 61 | }, 62 | { 63 | "cell_type": "markdown", 64 | "metadata": {}, 65 | "source": [ 66 | "### Add the two colors and visualize the result!\n", 67 | "\n", 68 | "Once you've implemented the `__add__` function in the color class, you should be able to add colors with a `+` operator and display the result!\n", 69 | "\n", 70 | "Remember, to go back to all your files, click on the orange Jupyter icon at the top left of this notebook!\n" 71 | ] 72 | }, 73 | { 74 | "cell_type": "code", 75 | "execution_count": null, 76 | "metadata": { 77 | "collapsed": true 78 | }, 79 | "outputs": [], 80 | "source": [ 81 | "# Add the two colors to create a *new* color object\n", 82 | "new_color = color1 + color2\n", 83 | "print(new_color)" 84 | ] 85 | } 86 | ], 87 | "metadata": { 88 | "anaconda-cloud": {}, 89 | "kernelspec": { 90 | "display_name": "Python 3", 91 | "language": "python", 92 | "name": "python3" 93 | }, 94 | "language_info": { 95 | "codemirror_mode": { 96 | "name": "ipython", 97 | "version": 3 98 | }, 99 | "file_extension": ".py", 100 | "mimetype": "text/x-python", 101 | "name": "python", 102 | "nbconvert_exporter": "python", 103 | "pygments_lexer": "ipython3", 104 | "version": "3.6.1" 105 | } 106 | }, 107 | "nbformat": 4, 108 | "nbformat_minor": 1 109 | } 110 | -------------------------------------------------------------------------------- /4_5_State_and_Motion/7. Matrix_Multiplication.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "# Predict state\n", 8 | "\n", 9 | "Here is the current implementation of the `predict_state` function. It takes in a state (a Python list), and then separates those into position and velocity to calculate a new, predicted state. It uses a constant velocity motion model.\n", 10 | "\n", 11 | "**In this exercise, we'll be improving this function, and using matrix multiplication to efficiently calculate the predicted state!**" 12 | ] 13 | }, 14 | { 15 | "cell_type": "code", 16 | "execution_count": null, 17 | "metadata": { 18 | "collapsed": true 19 | }, 20 | "outputs": [], 21 | "source": [ 22 | "# The current predict state function\n", 23 | "# Predicts the next state based on a motion model\n", 24 | "def predict_state(state, dt):\n", 25 | " # Assumes a valid state had been passed in\n", 26 | " x = state[0]\n", 27 | " velocity = state[1]\n", 28 | " \n", 29 | " # Assumes a constant velocity model\n", 30 | " new_x = x + velocity*dt\n", 31 | " \n", 32 | " # Create and return the new, predicted state\n", 33 | " predicted_state = [new_x, velocity]\n", 34 | " return predicted_state\n" 35 | ] 36 | }, 37 | { 38 | "cell_type": "markdown", 39 | "metadata": {}, 40 | "source": [ 41 | "## Matrix operations\n", 42 | "\n", 43 | "You've been given a matrix class that can create new matrices and performs one operation: multiplication. In our directory this is called `matrix.py`.\n", 44 | "\n", 45 | "Similar to the Car class, we can use this to initialize matrix objects." 46 | ] 47 | }, 48 | { 49 | "cell_type": "code", 50 | "execution_count": null, 51 | "metadata": { 52 | "collapsed": true 53 | }, 54 | "outputs": [], 55 | "source": [ 56 | "# import the matrix file\n", 57 | "import matrix\n", 58 | "\n", 59 | "# Initialize a state vector\n", 60 | "initial_position = 0 # meters\n", 61 | "velocity = 50 # m/s\n", 62 | "\n", 63 | "# Notice the syntax for creating a state column vector ([ [x], [v] ])\n", 64 | "# Commas separate these items into rows and brackets into columns\n", 65 | "initial_state = matrix.Matrix([ [initial_position], \n", 66 | " [velocity] ])" 67 | ] 68 | }, 69 | { 70 | "cell_type": "markdown", 71 | "metadata": {}, 72 | "source": [ 73 | "### Transformation matrix\n", 74 | "\n", 75 | "Next, define the state transformation matrix and print it out!" 76 | ] 77 | }, 78 | { 79 | "cell_type": "code", 80 | "execution_count": null, 81 | "metadata": { 82 | "collapsed": true 83 | }, 84 | "outputs": [], 85 | "source": [ 86 | "# Define the state transformation matrix\n", 87 | "dt = 1\n", 88 | "tx_matrix = matrix.Matrix([ [1, dt], \n", 89 | " [0, 1] ])\n", 90 | "\n", 91 | "print(tx_matrix)" 92 | ] 93 | }, 94 | { 95 | "cell_type": "markdown", 96 | "metadata": {}, 97 | "source": [ 98 | "### TODO: Modify the predict state function to use matrix multiplication\n", 99 | "\n", 100 | "Now that you know how to create matrices, modify the `predict_state` function to work with them!\n", 101 | "\n", 102 | "Note: you can multiply a matrix A by a matrix B by writing `A*B` and it will return a new matrix.\n" 103 | ] 104 | }, 105 | { 106 | "cell_type": "code", 107 | "execution_count": null, 108 | "metadata": { 109 | "collapsed": true 110 | }, 111 | "outputs": [], 112 | "source": [ 113 | "# The current predict state function\n", 114 | "def predict_state_mtx(state, dt):\n", 115 | " ## TODO: Assume that the state passed in is a Matrix object\n", 116 | " ## Using a constant velocity model and a transformation matrix\n", 117 | " ## Create and return the new, predicted state!\n", 118 | " predicted_state = 0\n", 119 | " \n", 120 | " return predicted_state" 121 | ] 122 | }, 123 | { 124 | "cell_type": "markdown", 125 | "metadata": {}, 126 | "source": [ 127 | "### Test cell\n", 128 | "\n", 129 | "Here is an initial state vector and dt to test your function with!" 130 | ] 131 | }, 132 | { 133 | "cell_type": "code", 134 | "execution_count": null, 135 | "metadata": { 136 | "collapsed": true 137 | }, 138 | "outputs": [], 139 | "source": [ 140 | "# initial state variables\n", 141 | "initial_position = 10 # meters\n", 142 | "velocity = 30 # m/s\n", 143 | "\n", 144 | "# Initial state vector\n", 145 | "initial_state = matrix.Matrix([ [initial_position], \n", 146 | " [velocity] ])\n", 147 | "\n", 148 | "\n", 149 | "print('The initial state is: ' + str(initial_state))\n", 150 | "\n", 151 | "\n", 152 | "# after 2 seconds make a prediction using the new function\n", 153 | "state_est1 = predict_state_mtx(initial_state, 2)\n", 154 | "\n", 155 | "print('State after 2 seconds is: ' + str(state_est1))" 156 | ] 157 | }, 158 | { 159 | "cell_type": "code", 160 | "execution_count": null, 161 | "metadata": { 162 | "collapsed": true 163 | }, 164 | "outputs": [], 165 | "source": [ 166 | "# Make more predictions!\n", 167 | "\n", 168 | "# after 3 more\n", 169 | "state_est2 = predict_state_mtx(state_est1, 3)\n", 170 | "\n", 171 | "print('State after 3 more seconds is: ' + str(state_est2))\n", 172 | "\n", 173 | "# after 3 more\n", 174 | "state_est3 = predict_state_mtx(state_est2, 3)\n", 175 | "\n", 176 | "print('Final state after 3 more seconds is: ' + str(state_est3))" 177 | ] 178 | } 179 | ], 180 | "metadata": { 181 | "anaconda-cloud": {}, 182 | "kernelspec": { 183 | "display_name": "Python 3", 184 | "language": "python", 185 | "name": "python3" 186 | }, 187 | "language_info": { 188 | "codemirror_mode": { 189 | "name": "ipython", 190 | "version": 3 191 | }, 192 | "file_extension": ".py", 193 | "mimetype": "text/x-python", 194 | "name": "python", 195 | "nbconvert_exporter": "python", 196 | "pygments_lexer": "ipython3", 197 | "version": "3.6.3" 198 | } 199 | }, 200 | "nbformat": 4, 201 | "nbformat_minor": 1 202 | } 203 | -------------------------------------------------------------------------------- /4_5_State_and_Motion/car.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | 3 | """ The Car class defines a car's movement and keeps track of its state. 4 | 5 | The class includes init, move, and display functions. 6 | This class assumes a constant velocity motion model and the state 7 | of the car includes the car's position, and it's velocity. 8 | 9 | Attributes: 10 | state: A list of the car's current position [y, x] and velocity [vy, vx] 11 | world: The world that the car is moving within (a 2D list) 12 | """ 13 | 14 | class Car(object): 15 | 16 | 17 | # Car constructor 18 | # Called when you write car.Car(_, _, _) 19 | def __init__(self, position, velocity, world): 20 | """Initializes Car with some position, velocity, and a world to traverse.""" 21 | 22 | # Initialize the state 23 | # Position is a list [y, x] and so is velocity [vy, vx] 24 | self.state = [position, velocity] 25 | self.world = world # world is a 2D list of values that range from 0-1 26 | 27 | # Set the default color 28 | self.color = 'r' 29 | 30 | # Initalize the path 31 | self.path = [] 32 | self.path.append(position) 33 | 34 | 35 | # Move function 36 | def move(self, dt=1): 37 | """ The move function moves the car in the direction of the velocity and 38 | updates the state. 39 | It assumes a circular world and a default dt = 1 (though dt can be any 40 | non-negative integer). 41 | """ 42 | 43 | height = len(self.world) 44 | width = len(self.world[0]) 45 | 46 | position = self.state[0] 47 | velocity = self.state[1] 48 | 49 | # Predict the new position [y, x] based on velocity [vx, vy] and time, dt 50 | predicted_position = [ 51 | (position[0] + velocity[0]*dt) % height, # default dt = 1 52 | (position[1] + velocity[1]*dt) % width 53 | ] 54 | 55 | # Update the state 56 | self.state = [predicted_position, velocity] 57 | 58 | # Every time the robot moves, add the new position to the path 59 | self.path.append(predicted_position) 60 | 61 | 62 | # Turn left function 63 | def turn_left(self): 64 | """ Turning left "rotates" the velocity values, so vy = -vx, and vx = vy. 65 | 66 | For example, if a car is going right at 1 world cell/sec this means 67 | vy = 0, vx = 1, 68 | and if it turns left, then it should be moving upwards on the world grid 69 | at the same speed! 70 | And up is vy = -1 and vx = 0 71 | """ 72 | 73 | # Change the velocity 74 | velocity = self.state[1] 75 | 76 | predicted_velocity = [ 77 | -velocity[1], 78 | velocity[0] 79 | ] 80 | 81 | # Update the state velocity 82 | self.state[1] = predicted_velocity 83 | 84 | 85 | # Helper function for displaying the world + robot position 86 | # Assumes the world in a 2D numpy array and position is in the form [y, x] 87 | # path is a list of positions, and it's an optional argument 88 | def display_world(self): 89 | 90 | # Store the current position of the car 91 | position = self.state[0] 92 | 93 | # Plot grid of values + initial ticks 94 | plt.matshow(self.world, cmap='gray') 95 | 96 | # Set minor axes in between the labels 97 | ax=plt.gca() 98 | rows = len(self.world) 99 | cols = len(self.world[0]) 100 | 101 | ax.set_xticks([x-0.5 for x in range(1,cols)],minor=True ) 102 | ax.set_yticks([y-0.5 for y in range(1,rows)],minor=True) 103 | 104 | # Plot grid on minor axes in gray (width = 2) 105 | plt.grid(which='minor',ls='-',lw=2, color='gray') 106 | 107 | # Create a 'x' character that represents the car 108 | # ha = horizontal alignment, va = verical 109 | ax.text(position[1], position[0], 'x', ha='center', va='center', color=self.color, fontsize=30) 110 | 111 | # Draw path if it exists 112 | if(len(self.path) > 1): 113 | # loop through all path indices and draw a dot (unless it's at the car's location) 114 | for pos in self.path: 115 | if(pos != position): 116 | ax.text(pos[1], pos[0], '.', ha='center', va='baseline', color=self.color, fontsize=30) 117 | 118 | # Display final result 119 | plt.show() 120 | 121 | -------------------------------------------------------------------------------- /4_5_State_and_Motion/color.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | ''' 3 | The color class creates a color from 3 values, r, g, and b (red, green, and blue). 4 | 5 | attributes: 6 | r - a value between 0-255 for red 7 | g - a value between 0-255 for green 8 | b - a value between 0-255 for blue 9 | ''' 10 | 11 | class Color(object): 12 | 13 | # __init__ is called when a color is constructed using color.Color(_, _, _) 14 | def __init__(self, r, g, b): 15 | # Setting the r value 16 | self.r = r 17 | 18 | ## TODO: Set the other two color variables g and b 19 | 20 | 21 | # __repr__ is called when a color is printed using print(some_color) 22 | # It must return a string 23 | def __repr__(self): 24 | '''Display a color swatch and then return a text description of r,g,b values.''' 25 | 26 | plt.imshow([[(self.r/255, self.g/255, self.b/255)]]) 27 | 28 | ## TODO: Write a string representation for the color 29 | ## ex. "rgb = [self.r, self.g, self.b]" 30 | ## Right now this returns an empty string 31 | string = '' 32 | 33 | return string 34 | 35 | -------------------------------------------------------------------------------- /4_5_State_and_Motion/matrix.py: -------------------------------------------------------------------------------- 1 | 2 | 3 | def zeroes(height, width): 4 | """ Creates a matrix of zeroes. """ 5 | g = [[0.0 for _ in range(width)] for __ in range(height)] 6 | return Matrix(g) 7 | 8 | 9 | class Matrix(object): 10 | 11 | # Initializes a matrix from a 2D grid of values 12 | # Assumes and initializes a grid-specified number of rows and columns 13 | def __init__(self, grid): 14 | self.grid = grid 15 | self.rows = len(grid) 16 | self.cols = len(grid[0]) 17 | 18 | # Overloading the multiplication operator 19 | def __mul__(self, other): 20 | ''' self and other are Matrix objects. 21 | This function check the dimensions of the two matrices, 22 | multiplies them according to the rules of linear algebra, 23 | and returns a new matrix.''' 24 | 25 | if(self.cols == other.rows): 26 | # Then the dimensions match for multiplication! 27 | 28 | # Create a new matrix of zeroes of the appropriate size (self.rows x other.cols) 29 | self_times_other = zeroes(self.rows, other.cols) 30 | 31 | # iterate through the rows of self 32 | for i in range(self.rows): 33 | 34 | # iterate through the columns of other 35 | for j in range(other.cols): 36 | 37 | # iterate through rows of other and sum 38 | for k in range(other.rows): 39 | # multiply and sum step 40 | self_times_other[i][j] += (self[i][k] * other[k][j]) 41 | 42 | # for debugging 43 | #print(' i = ' + str(i) +', j = ' + str(j) + ', k = ' + str(k)) 44 | return self_times_other 45 | 46 | else: 47 | print('Invalid matrix dimensions!') 48 | print('A_cols = ' + str(self.cols) + ', and B_rows = ' + str(other.rows)) 49 | # return [] 50 | 51 | 52 | # Overloading indexing 53 | def __getitem__(self, index): 54 | return self.grid[index] 55 | 56 | # Overloading print output 57 | def __repr__(self): 58 | s = '[' 59 | for row in self.grid: 60 | s += '[' 61 | for item in row: 62 | s += str(item) +' ' 63 | s += ']\n' 64 | 65 | s += ']' 66 | 67 | return s 68 | 69 | 70 | 71 | 72 | -------------------------------------------------------------------------------- /4_6_Matrices_and_Transformation_of_State/1_vector_coding_solution.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": { 6 | "collapsed": true 7 | }, 8 | "source": [ 9 | "## Vector Coding Solutions\n", 10 | "Here are some examples of how you might have approached the previous exercises. " 11 | ] 12 | }, 13 | { 14 | "cell_type": "code", 15 | "execution_count": null, 16 | "metadata": { 17 | "collapsed": true 18 | }, 19 | "outputs": [], 20 | "source": [ 21 | "def add(vector_1, vector_2):\n", 22 | " if len(vector_1) != len(vector_2):\n", 23 | " print(\"error! vectors must be same size to add\")\n", 24 | " return \n", 25 | " \n", 26 | " new_vector = []\n", 27 | " for i in range(len(vector_1)):\n", 28 | " value_1 = vector_1[i]\n", 29 | " value_2 = vector_2[i]\n", 30 | " new_val = value_1 + value_2\n", 31 | " new_vector.append(new_val)\n", 32 | " \n", 33 | " return new_vector\n", 34 | "\n", 35 | "v1 = [1,2]\n", 36 | "v2 = [3,4]\n", 37 | "\n", 38 | "v_1_plus_2 = add(v1, v2)\n", 39 | "\n", 40 | "print(v1, \"plus\", v2, \"equals\", v_1_plus_2)" 41 | ] 42 | }, 43 | { 44 | "cell_type": "code", 45 | "execution_count": null, 46 | "metadata": { 47 | "collapsed": true 48 | }, 49 | "outputs": [], 50 | "source": [ 51 | "def multiply(scalar, vector):\n", 52 | " new_vector = []\n", 53 | " for value in vector:\n", 54 | " new_value = scalar * value\n", 55 | " new_vector.append(new_value)\n", 56 | " return new_vector\n", 57 | "\n", 58 | "vector = [1,2,3,4,5]\n", 59 | "number = 3\n", 60 | "\n", 61 | "product = multiply(number, vector)\n", 62 | "\n", 63 | "print(number, \"times\", vector, \"equals\", product)" 64 | ] 65 | }, 66 | { 67 | "cell_type": "code", 68 | "execution_count": null, 69 | "metadata": { 70 | "collapsed": true 71 | }, 72 | "outputs": [], 73 | "source": [ 74 | "def dot_product(v1, v2):\n", 75 | " if len(v1) != len(v2):\n", 76 | " print(\"error! Vectors must have same length\")\n", 77 | " \n", 78 | " result = 0\n", 79 | " for i in range(len(v1)):\n", 80 | " value_1 = v1[i]\n", 81 | " value_2 = v2[i]\n", 82 | " result += value_1 * value_2\n", 83 | " return result\n", 84 | "\n", 85 | "vector_1 = [7,2,3]\n", 86 | "vector_2 = [1, 10, 4]\n", 87 | "\n", 88 | "# should be 39 (7*1 + 2*10 + 3*4)\n", 89 | "v1_dot_v2 = dot_product(vector_1, vector_2)\n", 90 | "\n", 91 | "\n", 92 | "print(vector_1, \"dot\", vector_2, \"equals\", v1_dot_v2)\n", 93 | " " 94 | ] 95 | }, 96 | { 97 | "cell_type": "markdown", 98 | "metadata": {}, 99 | "source": [ 100 | "In mathematical notation, the above code could be described as follows:\n", 101 | "\n", 102 | "> Given two vectors $\\mathbf{a} = [a_1, a_2, \\ldots, a_n]$ and $\\mathbf{b} = [b_1, b_2, \\ldots, b_n]$, the **dot product** is defined as follows:\n", 103 | ">\n", 104 | "> $$\\mathbf{a} \\cdot \\mathbf{b} = \\sum_{i=1}^{n}a_ib_i = a_1b_1 + a_2b_2 + \\ldots + a_nb_n$$" 105 | ] 106 | } 107 | ], 108 | "metadata": { 109 | "kernelspec": { 110 | "display_name": "Python 3", 111 | "language": "python", 112 | "name": "python3" 113 | }, 114 | "language_info": { 115 | "codemirror_mode": { 116 | "name": "ipython", 117 | "version": 3 118 | }, 119 | "file_extension": ".py", 120 | "mimetype": "text/x-python", 121 | "name": "python", 122 | "nbconvert_exporter": "python", 123 | "pygments_lexer": "ipython3", 124 | "version": "3.6.1" 125 | } 126 | }, 127 | "nbformat": 4, 128 | "nbformat_minor": 2 129 | } 130 | -------------------------------------------------------------------------------- /4_6_Matrices_and_Transformation_of_State/2_matrices_in_python.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "## Coding Matrices\n", 8 | "\n", 9 | "Here are a few exercises to get you started with coding matrices. The exercises start off with vectors and then get more challenging\n", 10 | "\n", 11 | "### Vectors" 12 | ] 13 | }, 14 | { 15 | "cell_type": "code", 16 | "execution_count": null, 17 | "metadata": { 18 | "collapsed": true 19 | }, 20 | "outputs": [], 21 | "source": [ 22 | "### TODO: Assign the vector <5, 10, 2, 6, 1> to the variable v\n", 23 | "v = []" 24 | ] 25 | }, 26 | { 27 | "cell_type": "markdown", 28 | "metadata": {}, 29 | "source": [ 30 | "The v variable contains a Python list. This list could also be thought of as a 1x5 matrix with 1 row and 5 columns. How would you represent this list as a matrix?" 31 | ] 32 | }, 33 | { 34 | "cell_type": "code", 35 | "execution_count": null, 36 | "metadata": { 37 | "collapsed": true 38 | }, 39 | "outputs": [], 40 | "source": [ 41 | "### TODO: Assign the vector <5, 10, 2, 6, 1> to the variable mv\n", 42 | "### The difference between a vector and a matrix in Python is that\n", 43 | "### a matrix is a list of lists.\n", 44 | "\n", 45 | "### Hint: See the last quiz on the previous page\n", 46 | "\n", 47 | "mv = [[]]" 48 | ] 49 | }, 50 | { 51 | "cell_type": "markdown", 52 | "metadata": {}, 53 | "source": [ 54 | "How would you represent this vector in its vertical form with 5 rows and 1 column? When defining matrices in Python, each row is a list. So in this case, you have 5 rows and thus will need 5 lists.\n", 55 | "\n", 56 | "As an example, this is what the vector $$<5, 7>$$ would look like as a 1x2 matrix in Python: \n", 57 | "```python\n", 58 | "matrix1by2 = [\n", 59 | " [5, 7]\n", 60 | "]\n", 61 | "```\n", 62 | "\n", 63 | "And here is what the same vector would look like as a 2x1 matrix:\n", 64 | "```python\n", 65 | "matrix2by1 = [\n", 66 | " [5], \n", 67 | " [7]\n", 68 | "]\n", 69 | "```" 70 | ] 71 | }, 72 | { 73 | "cell_type": "code", 74 | "execution_count": null, 75 | "metadata": { 76 | "collapsed": true 77 | }, 78 | "outputs": [], 79 | "source": [ 80 | "### TODO: Assign the vector <5, 10, 2, 6, 1> to the variable vT\n", 81 | "### vT is a 5x1 matrix\n", 82 | "vT = []" 83 | ] 84 | }, 85 | { 86 | "cell_type": "markdown", 87 | "metadata": {}, 88 | "source": [ 89 | "### Assigning Matrices to Variables" 90 | ] 91 | }, 92 | { 93 | "cell_type": "code", 94 | "execution_count": null, 95 | "metadata": { 96 | "collapsed": true 97 | }, 98 | "outputs": [], 99 | "source": [ 100 | "### TODO: Assign the following matrix to the variable m\n", 101 | "### 8 7 1 2 3\n", 102 | "### 1 5 2 9 0\n", 103 | "### 8 2 2 4 1\n", 104 | "\n", 105 | "m = [[]]" 106 | ] 107 | }, 108 | { 109 | "cell_type": "markdown", 110 | "metadata": {}, 111 | "source": [ 112 | "### Accessing Matrix Values" 113 | ] 114 | }, 115 | { 116 | "cell_type": "code", 117 | "execution_count": null, 118 | "metadata": { 119 | "collapsed": true 120 | }, 121 | "outputs": [], 122 | "source": [ 123 | "### TODO: In matrix m, change the value \n", 124 | "### in the second row last column from 0 to 5\n", 125 | "### Hint: You do not need to rewrite the entire matrix\n" 126 | ] 127 | }, 128 | { 129 | "cell_type": "markdown", 130 | "metadata": {}, 131 | "source": [ 132 | "### Looping through Matrices to do Math\n", 133 | "\n", 134 | "Coding mathematical operations with matrices can be tricky. Because matrices are lists of lists, you will need to use a for loop inside another for loop. The outside for loop iterates over the rows and the inside for loop iterates over the columns.\n", 135 | "\n", 136 | "\n", 137 | "Here is some pseudo code\n", 138 | "```python\n", 139 | "for i in number of rows:\n", 140 | " for j in number of columns:\n", 141 | " mymatrix[i][j]\n", 142 | "```\n", 143 | "\n", 144 | "To figure out how many times to loop over the matrix, you need to know the number of rows and number of columns. \n", 145 | "\n", 146 | "\n", 147 | "If you have a variable with a matrix in it, how could you figure out the number of rows? How could you figure out the number of columns? The [len](https://docs.python.org/2/library/functions.html#len) function in Python might be helpful." 148 | ] 149 | }, 150 | { 151 | "cell_type": "markdown", 152 | "metadata": {}, 153 | "source": [ 154 | "### Scalar Multiplication" 155 | ] 156 | }, 157 | { 158 | "cell_type": "code", 159 | "execution_count": 2, 160 | "metadata": { 161 | "collapsed": true 162 | }, 163 | "outputs": [], 164 | "source": [ 165 | "### TODO: Use for loops to multiply each matrix element by 5\n", 166 | "### Store the answer in the r variable. This is called scalar\n", 167 | "### multiplication\n", 168 | "###\n", 169 | "### HINT: First write a for loop that iterates through the rows\n", 170 | "### one row at a time\n", 171 | "###\n", 172 | "### Then write another for loop within the for loop that\n", 173 | "### iterates through the columns\n", 174 | "###\n", 175 | "### If you used the variable i to represent rows and j\n", 176 | "### to represent columns, then m[i][j] would give you\n", 177 | "### access to each element in the matrix\n", 178 | "###\n", 179 | "### Because r is an empty list, you cannot directly assign\n", 180 | "### a value like r[i][j] = m[i][j]. You might have to\n", 181 | "### work on one row at a time and then use r.append(row).\n", 182 | "r = []" 183 | ] 184 | }, 185 | { 186 | "cell_type": "markdown", 187 | "metadata": {}, 188 | "source": [ 189 | "### Printing Out a Matrix" 190 | ] 191 | }, 192 | { 193 | "cell_type": "code", 194 | "execution_count": 4, 195 | "metadata": { 196 | "collapsed": true 197 | }, 198 | "outputs": [], 199 | "source": [ 200 | "### TODO: Write a function called matrix_print() \n", 201 | "### that prints out a matrix in\n", 202 | "### a way that is easy to read.\n", 203 | "### Each element in a row should be separated by a tab\n", 204 | "### And each row should have its own line\n", 205 | "### You can test our your results with the m matrix\n", 206 | "\n", 207 | "### HINT: You can use a for loop within a for loop\n", 208 | "### In Python, the print() function will be useful\n", 209 | "### print(5, '\\t', end = '') will print out the integer 5, \n", 210 | "### then add a tab after the 5. The end = '' makes sure that\n", 211 | "### the print function does not print out a new line if you do\n", 212 | "### not want a new line.\n", 213 | "\n", 214 | "### Your output should look like this\n", 215 | "### 8 7 1 2 3 \n", 216 | "### 1 5 2 9 5 \n", 217 | "### 8 2 2 4 1\n", 218 | "\n", 219 | "def matrix_print(matrix):\n", 220 | " return\n", 221 | "\n", 222 | "m = [\n", 223 | " [8, 7, 1, 2, 3],\n", 224 | " [1, 5, 2, 9, 5],\n", 225 | " [8, 2, 2, 4, 1]\n", 226 | "]\n", 227 | "\n", 228 | "matrix_print(m)" 229 | ] 230 | }, 231 | { 232 | "cell_type": "markdown", 233 | "metadata": {}, 234 | "source": [ 235 | "### Test Your Results" 236 | ] 237 | }, 238 | { 239 | "cell_type": "code", 240 | "execution_count": null, 241 | "metadata": { 242 | "collapsed": true 243 | }, 244 | "outputs": [], 245 | "source": [ 246 | "### You can run these tests to see if you have the expected\n", 247 | "### results. If everything is correct, this cell has no output\n", 248 | "\n", 249 | "assert v == [5, 10, 2, 6, 1]\n", 250 | "assert mv == [\n", 251 | " [5, 10, 2, 6, 1]\n", 252 | "]\n", 253 | "\n", 254 | "assert vT == [\n", 255 | " [5], \n", 256 | " [10], \n", 257 | " [2], \n", 258 | " [6], \n", 259 | " [1]]\n", 260 | "\n", 261 | "assert m == [\n", 262 | " [8, 7, 1, 2, 3], \n", 263 | " [1, 5, 2, 9, 5], \n", 264 | " [8, 2, 2, 4, 1]\n", 265 | "]\n", 266 | "\n", 267 | "assert r == [\n", 268 | " [40, 35, 5, 10, 15], \n", 269 | " [5, 25, 10, 45, 25], \n", 270 | " [40, 10, 10, 20, 5]\n", 271 | "]" 272 | ] 273 | }, 274 | { 275 | "cell_type": "markdown", 276 | "metadata": {}, 277 | "source": [ 278 | "### Print Out Your Results" 279 | ] 280 | }, 281 | { 282 | "cell_type": "code", 283 | "execution_count": null, 284 | "metadata": { 285 | "collapsed": true 286 | }, 287 | "outputs": [], 288 | "source": [ 289 | "### Run this cell to print out your answers\n", 290 | "print(v)\n", 291 | "print(mv)\n", 292 | "print(vT)\n", 293 | "print(m)\n", 294 | "print(r)" 295 | ] 296 | } 297 | ], 298 | "metadata": { 299 | "kernelspec": { 300 | "display_name": "Python 3", 301 | "language": "python", 302 | "name": "python3" 303 | }, 304 | "language_info": { 305 | "codemirror_mode": { 306 | "name": "ipython", 307 | "version": 3 308 | }, 309 | "file_extension": ".py", 310 | "mimetype": "text/x-python", 311 | "name": "python", 312 | "nbconvert_exporter": "python", 313 | "pygments_lexer": "ipython3", 314 | "version": "3.6.1" 315 | } 316 | }, 317 | "nbformat": 4, 318 | "nbformat_minor": 2 319 | } 320 | -------------------------------------------------------------------------------- /4_6_Matrices_and_Transformation_of_State/3_matrix_addition.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "# Matrix Addition\n", 8 | "\n", 9 | "In this exercises, you will write a function that accepts two matrices and outputs their sum. Think about how you could do this with a for loop nested inside another for loop." 10 | ] 11 | }, 12 | { 13 | "cell_type": "code", 14 | "execution_count": null, 15 | "metadata": { 16 | "collapsed": true 17 | }, 18 | "outputs": [], 19 | "source": [ 20 | "### TODO: Write a function called matrix_addition that \n", 21 | "### calculate the sum of two matrices\n", 22 | "###\n", 23 | "### INPUTS:\n", 24 | "### matrix A _ an m x n matrix\n", 25 | "### matrix B _ an m x n matrix\n", 26 | "###\n", 27 | "### OUPUT:\n", 28 | "### matrixSum _ sum of matrix A + matrix B\n", 29 | "\n", 30 | "def matrix_addition(matrixA, matrixB):\n", 31 | "\n", 32 | " # initialize matrix to hold the results\n", 33 | " matrixSum = []\n", 34 | " \n", 35 | " # matrix to hold a row for appending sums of each element\n", 36 | " row = []\n", 37 | " \n", 38 | " # TODO: write a for loop within a for loop to iterate over\n", 39 | " # the matrices\n", 40 | " \n", 41 | " # TODO: As you iterate through the matrices, add matching\n", 42 | " # elements and append the sum to the row variable\n", 43 | " \n", 44 | " # TODO: When a row is filled, append the row to matrixSum. \n", 45 | " # Then reinitialize row as an empty list\n", 46 | " \n", 47 | " return matrixSum\n", 48 | "\n", 49 | "### When you run this code cell, your matrix addition function\n", 50 | "### will run on the A and B matrix.\n", 51 | "\n", 52 | "A = [\n", 53 | " [2,5,1], \n", 54 | " [6,9,7.4], \n", 55 | " [2,1,1], \n", 56 | " [8,5,3], \n", 57 | " [2,1,6], \n", 58 | " [5,3,1]\n", 59 | "]\n", 60 | "\n", 61 | "B = [\n", 62 | " [7, 19, 5.1], \n", 63 | " [6.5,9.2,7.4], \n", 64 | " [2.8,1.5,12], \n", 65 | " [8,5,3], \n", 66 | " [2,1,6], \n", 67 | " [2,33,1]\n", 68 | "]\n", 69 | "\n", 70 | "matrix_addition(A, B)" 71 | ] 72 | }, 73 | { 74 | "cell_type": "markdown", 75 | "metadata": {}, 76 | "source": [ 77 | "### Vectors versus Matrices\n", 78 | "\n", 79 | "What happens if you run the cell below? Here you are adding two vectors together. Does your code still work?" 80 | ] 81 | }, 82 | { 83 | "cell_type": "code", 84 | "execution_count": null, 85 | "metadata": { 86 | "collapsed": true 87 | }, 88 | "outputs": [], 89 | "source": [ 90 | "matrix_addition([4, 2, 1], [5, 2, 7])" 91 | ] 92 | }, 93 | { 94 | "cell_type": "markdown", 95 | "metadata": {}, 96 | "source": [ 97 | "Why did this error occur? Because your code assumes that a matrix is a two-dimensional grid represented by a list of lists. But a horizontal vector, which can also be considered a matrix, is a one-dimensional grid represented by a single list.\n", 98 | "\n", 99 | "What happens if you store a vector as a list of lists like [[4, 2, 1]] and [[5, 2, 7]]? Does your function work? Run the code cell below to find out." 100 | ] 101 | }, 102 | { 103 | "cell_type": "code", 104 | "execution_count": null, 105 | "metadata": { 106 | "collapsed": true 107 | }, 108 | "outputs": [], 109 | "source": [ 110 | "matrix_addition([[4, 2, 1]], [[5, 2, 7]])" 111 | ] 112 | }, 113 | { 114 | "cell_type": "markdown", 115 | "metadata": {}, 116 | "source": [ 117 | "### Test your Code\n", 118 | "Run the cell below. If there is no output, then your results are as expected." 119 | ] 120 | }, 121 | { 122 | "cell_type": "code", 123 | "execution_count": null, 124 | "metadata": { 125 | "collapsed": true 126 | }, 127 | "outputs": [], 128 | "source": [ 129 | "assert matrix_addition([\n", 130 | " [1, 2, 3]], \n", 131 | " [[4, 5, 6]]) == [[5, 7, 9]]\n", 132 | "\n", 133 | "assert matrix_addition([\n", 134 | " [4]], [\n", 135 | " [5]]) == [[9]]\n", 136 | "\n", 137 | "assert matrix_addition([[1, 2, 3], \n", 138 | " [4, 5, 6]], \n", 139 | " [[7, 8, 9], \n", 140 | " [10, 11, 12]]) == [[8, 10, 12], \n", 141 | " [14, 16, 18]]" 142 | ] 143 | } 144 | ], 145 | "metadata": { 146 | "kernelspec": { 147 | "display_name": "Python 3", 148 | "language": "python", 149 | "name": "python3" 150 | }, 151 | "language_info": { 152 | "codemirror_mode": { 153 | "name": "ipython", 154 | "version": 3 155 | }, 156 | "file_extension": ".py", 157 | "mimetype": "text/x-python", 158 | "name": "python", 159 | "nbconvert_exporter": "python", 160 | "pygments_lexer": "ipython3", 161 | "version": "3.6.1" 162 | } 163 | }, 164 | "nbformat": 4, 165 | "nbformat_minor": 2 166 | } 167 | -------------------------------------------------------------------------------- /4_6_Matrices_and_Transformation_of_State/5_matrix_transpose.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "# Transpose of a Matrix\n", 8 | "\n", 9 | "In this set of exercises, you will work with the transpose of a matrix.\n", 10 | "\n", 11 | "Your first task is to write a funciton that takes the transpose of a matrix. Think about how to use nested for loops efficiently.\n", 12 | "\n", 13 | "The second task will be to write a new matrix multiplication function that takes advantage of your matrix transposition function." 14 | ] 15 | }, 16 | { 17 | "cell_type": "code", 18 | "execution_count": 1, 19 | "metadata": { 20 | "collapsed": true 21 | }, 22 | "outputs": [], 23 | "source": [ 24 | "### TODO: Write a function called transpose() that \n", 25 | "### takes in a matrix and outputs the transpose of the matrix\n", 26 | "\n", 27 | "def transpose(matrix):\n", 28 | " matrix_transpose = []\n", 29 | " \n", 30 | " return matrix_transpose" 31 | ] 32 | }, 33 | { 34 | "cell_type": "code", 35 | "execution_count": null, 36 | "metadata": { 37 | "collapsed": true 38 | }, 39 | "outputs": [], 40 | "source": [ 41 | "### TODO: Run the code in the cell below. If there is no \n", 42 | "### output, then your answers were as expected\n", 43 | "\n", 44 | "assert transpose([[5, 4, 1, 7], [2, 1, 3, 5]]) == [[5, 2], [4, 1], [1, 3], [7, 5]]\n", 45 | "assert transpose([[5]]) == [[5]]\n", 46 | "assert transpose([[5, 3, 2], [7, 1, 4], [1, 1, 2], [8, 9, 1]]) == [[5, 7, 1, 8], [3, 1, 1, 9], [2, 4, 2, 1]]\n" 47 | ] 48 | }, 49 | { 50 | "cell_type": "markdown", 51 | "metadata": {}, 52 | "source": [ 53 | "### Matrix Multiplication\n", 54 | "\n", 55 | "Now that you have your transpose function working, write a matrix multiplication function that takes advantage of the transpose. \n", 56 | "\n", 57 | "As part of the matrix multiplication code, you might want to re-use your dot product function from the matrix multiplication exercises. But you won't need your get_row and get_column functions anymore because the tranpose essentially takes care of turning columns into row vectors.\n", 58 | "\n", 59 | "Remember that if matrix A is mxn and matrix B is nxp, then the resulting product will be mxp." 60 | ] 61 | }, 62 | { 63 | "cell_type": "code", 64 | "execution_count": 1, 65 | "metadata": { 66 | "collapsed": true 67 | }, 68 | "outputs": [], 69 | "source": [ 70 | "### TODO: Write a function called matrix_multiplication() that\n", 71 | "### takes in two matrices and outputs the product of the two\n", 72 | "### matrices\n", 73 | "\n", 74 | "### TODO: Copy your dot_product() function here so that you can\n", 75 | "### use it in your matrix_multiplication function\n", 76 | "\n", 77 | "def matrix_multiplication(matrixA, matrixB):\n", 78 | " product = []\n", 79 | " \n", 80 | " \n", 81 | " ## TODO: Take the transpose of matrixB and store the result\n", 82 | " ## in a new variable\n", 83 | " \n", 84 | " \n", 85 | " ## TODO: Use a nested for loop to iterate through the rows\n", 86 | " ## of matrix A and the rows of the tranpose of matrix B\n", 87 | " \n", 88 | " ## TODO: Calculate the dot product between each row of matrix A\n", 89 | " ## with each row in the transpose of matrix B\n", 90 | " \n", 91 | " ## TODO: As you calculate the results inside your for loops,\n", 92 | " ## store the results in the product variable\n", 93 | " \n", 94 | " \n", 95 | " ## TODO:\n", 96 | " return product" 97 | ] 98 | }, 99 | { 100 | "cell_type": "code", 101 | "execution_count": null, 102 | "metadata": { 103 | "collapsed": true 104 | }, 105 | "outputs": [], 106 | "source": [ 107 | "### TODO: Run the code in the cell below. If there is no \n", 108 | "### output, then your answers were as expected\n", 109 | "\n", 110 | "assert matrix_multiplication([[5, 3, 1], \n", 111 | " [6, 2, 7]], \n", 112 | " [[4, 2], \n", 113 | " [8, 1], \n", 114 | " [7, 4]]) == [[51, 17], \n", 115 | " [89, 42]]\n", 116 | "\n", 117 | "assert matrix_multiplication([[5]], [[4]]) == [[20]]\n", 118 | "\n", 119 | "assert matrix_multiplication([[2, 8, 1, 2, 9],\n", 120 | " [7, 9, 1, 10, 5],\n", 121 | " [8, 4, 11, 98, 2],\n", 122 | " [5, 5, 4, 4, 1]], \n", 123 | " [[4], \n", 124 | " [2], \n", 125 | " [17], \n", 126 | " [80], \n", 127 | " [2]]) == [[219], [873], [8071], [420]]\n", 128 | "\n", 129 | "\n", 130 | "assert matrix_multiplication([[2, 8, 1, 2, 9],\n", 131 | " [7, 9, 1, 10, 5],\n", 132 | " [8, 4, 11, 98, 2],\n", 133 | " [5, 5, 4, 4, 1]], \n", 134 | " [[4, 1, 2], \n", 135 | " [2, 3, 1], \n", 136 | " [17, 8, 1], \n", 137 | " [1, 3, 0], \n", 138 | " [2, 1, 4]]) == [[61, 49, 49], [83, 77, 44], [329, 404, 39], [104, 65, 23]]" 139 | ] 140 | } 141 | ], 142 | "metadata": { 143 | "kernelspec": { 144 | "display_name": "Python 3", 145 | "language": "python", 146 | "name": "python3" 147 | }, 148 | "language_info": { 149 | "codemirror_mode": { 150 | "name": "ipython", 151 | "version": 3 152 | }, 153 | "file_extension": ".py", 154 | "mimetype": "text/x-python", 155 | "name": "python", 156 | "nbconvert_exporter": "python", 157 | "pygments_lexer": "ipython3", 158 | "version": "3.6.1" 159 | } 160 | }, 161 | "nbformat": 4, 162 | "nbformat_minor": 2 163 | } 164 | -------------------------------------------------------------------------------- /4_6_Matrices_and_Transformation_of_State/6_inverse_matrix.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "# Matrix Inverse\n", 8 | "\n", 9 | "In this exercise, you will write a function to calculate the inverse of either a 1x1 or a 2x2 matrix." 10 | ] 11 | }, 12 | { 13 | "cell_type": "code", 14 | "execution_count": 1, 15 | "metadata": { 16 | "collapsed": true 17 | }, 18 | "outputs": [], 19 | "source": [ 20 | "### TODO: Write a function called inverse_matrix() that \n", 21 | "### receives a matrix and outputs the inverse\n", 22 | "### \n", 23 | "### You are provided with start code that checks\n", 24 | "### if the matrix is square and if not, throws an error\n", 25 | "###\n", 26 | "### You will also need to check the size of the matrix.\n", 27 | "### The formula for a 1x1 matrix and 2x2 matrix are different,\n", 28 | "### so your solution will need to take this into account.\n", 29 | "###\n", 30 | "### If the user inputs a non-invertible 2x2 matrix or a matrix\n", 31 | "### of size 3 x 3 or greater, the function should raise an\n", 32 | "### error. A non-invertible\n", 33 | "### 2x2 matrix has ad-bc = 0 as discussed in the lesson\n", 34 | "###\n", 35 | "### Python has various options for raising errors\n", 36 | "### raise RuntimeError('this is the error message')\n", 37 | "### raise NotImplementedError('this functionality is not implemented')\n", 38 | "### raise ValueError('The denominator of a fraction cannot be zero')\n", 39 | "\n", 40 | "def inverse_matrix(matrix):\n", 41 | " \n", 42 | " inverse = []\n", 43 | " \n", 44 | " if len(matrix) != len(matrix[0]):\n", 45 | " raise ValueError('The matrix must be square')\n", 46 | " \n", 47 | " ## TODO: Check if matrix is larger than 2x2.\n", 48 | " ## If matrix is too large, then raise an error\n", 49 | " \n", 50 | " ## TODO: Check if matrix is 1x1 or 2x2.\n", 51 | " ## Depending on the matrix size, the formula for calculating\n", 52 | " ## the inverse is different. \n", 53 | " # If the matrix is 2x2, check that the matrix is invertible\n", 54 | "\n", 55 | " \n", 56 | "\n", 57 | " ## TODO: Calculate the inverse of the square 1x1 or 2x2 matrix.\n", 58 | " \n", 59 | " return inverse\n", 60 | " \n", 61 | " " 62 | ] 63 | }, 64 | { 65 | "cell_type": "code", 66 | "execution_count": null, 67 | "metadata": { 68 | "collapsed": true 69 | }, 70 | "outputs": [], 71 | "source": [ 72 | "## TODO: Run this cell to check your output. If this cell does \n", 73 | "## not output anything your answers were as expected.\n", 74 | "\n", 75 | "assert inverse_matrix([[100]]) == [[0.01]]\n", 76 | "assert inverse_matrix([[4, 5], [7, 1]]) == [[-0.03225806451612903, 0.16129032258064516],\n", 77 | " [0.22580645161290322, -0.12903225806451613]]" 78 | ] 79 | }, 80 | { 81 | "cell_type": "code", 82 | "execution_count": null, 83 | "metadata": { 84 | "collapsed": true 85 | }, 86 | "outputs": [], 87 | "source": [ 88 | "### Run this line of code and see what happens. Because ad = bc, this\n", 89 | "### matrix does not have an inverse\n", 90 | "inverse_matrix([[4, 2], [14, 7]])" 91 | ] 92 | }, 93 | { 94 | "cell_type": "code", 95 | "execution_count": null, 96 | "metadata": { 97 | "collapsed": true 98 | }, 99 | "outputs": [], 100 | "source": [ 101 | "### Run this line of code and see what happens. This is a 3x3 matrix\n", 102 | "inverse_matrix([[4, 5, 1], [2, 9, 7], [6, 3, 9]])" 103 | ] 104 | } 105 | ], 106 | "metadata": { 107 | "kernelspec": { 108 | "display_name": "Python 3", 109 | "language": "python", 110 | "name": "python3" 111 | }, 112 | "language_info": { 113 | "codemirror_mode": { 114 | "name": "ipython", 115 | "version": 3 116 | }, 117 | "file_extension": ".py", 118 | "mimetype": "text/x-python", 119 | "name": "python", 120 | "nbconvert_exporter": "python", 121 | "pygments_lexer": "ipython3", 122 | "version": "3.6.3" 123 | } 124 | }, 125 | "nbformat": 4, 126 | "nbformat_minor": 2 127 | } 128 | -------------------------------------------------------------------------------- /4_6_Matrices_and_Transformation_of_State/7_identity_matrix.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": { 6 | "collapsed": true 7 | }, 8 | "source": [ 9 | "# Identity Matrix\n", 10 | "\n", 11 | "Write a function called identity_matrix that outputs an identity matrix of size n.\n", 12 | "\n", 13 | "INPUT\n", 14 | "* n - size of the Identity matrix\n", 15 | "\n", 16 | "OUPUT\n", 17 | "* identity matrix as a list of lists\n", 18 | "\n", 19 | "\n", 20 | "HINTS\n", 21 | "* nested for loops will be helpful\n", 22 | "* the one values are always on the diagonal. To access diagonal values in a list of lists will occur where i = j\n", 23 | "* whenever i does not equal j, the value in the matrix should be 0" 24 | ] 25 | }, 26 | { 27 | "cell_type": "code", 28 | "execution_count": null, 29 | "metadata": { 30 | "collapsed": true 31 | }, 32 | "outputs": [], 33 | "source": [ 34 | "def identity_matrix(n):\n", 35 | " \n", 36 | " identity = []\n", 37 | " \n", 38 | " # TODO: Write a nested for loop to iterate over the rows and\n", 39 | " # columns of the identity matrix. Remember that identity\n", 40 | " # matrices are square so they have the same number of rows\n", 41 | " # and columns\n", 42 | " \n", 43 | " # Make sure to assign 1 to the diagonal values and 0 everywhere\n", 44 | " # else\n", 45 | " \n", 46 | " return identity" 47 | ] 48 | }, 49 | { 50 | "cell_type": "code", 51 | "execution_count": null, 52 | "metadata": { 53 | "collapsed": true 54 | }, 55 | "outputs": [], 56 | "source": [ 57 | "# TODO: Run this cell to see if your answers are as expected\n", 58 | "\n", 59 | "assert identity_matrix(1) == [[1]]\n", 60 | "\n", 61 | "assert identity_matrix(2) == [[1, 0], \n", 62 | " [0, 1]]\n", 63 | "\n", 64 | "assert identity_matrix(3) == [[1, 0, 0],\n", 65 | " [0, 1, 0],\n", 66 | " [0, 0, 1]]\n", 67 | "\n", 68 | "assert identity_matrix(4) == [[1, 0, 0, 0],\n", 69 | " [0, 1, 0, 0],\n", 70 | " [0, 0, 1, 0],\n", 71 | " [0, 0, 0, 1]]" 72 | ] 73 | }, 74 | { 75 | "cell_type": "markdown", 76 | "metadata": {}, 77 | "source": [ 78 | "# Multiplication with the Identity Matrix\n", 79 | "\n", 80 | "Copy your matrix multiplication function in the code cell below. Try multiplying a matrix with an identity matrix to prove to yourself that the identity matrix is analogous to multiplyin a scalar by one." 81 | ] 82 | }, 83 | { 84 | "cell_type": "code", 85 | "execution_count": null, 86 | "metadata": { 87 | "collapsed": true 88 | }, 89 | "outputs": [], 90 | "source": [ 91 | "# TODO: Copy your matrix multiplication function and any other helper\n", 92 | "# funcitons here from the previous exercises\n", 93 | "\n", 94 | "def matrix_multiplication(matrixA, matrixB):\n", 95 | " product = []\n", 96 | " \n", 97 | " return product" 98 | ] 99 | }, 100 | { 101 | "cell_type": "code", 102 | "execution_count": null, 103 | "metadata": { 104 | "collapsed": true 105 | }, 106 | "outputs": [], 107 | "source": [ 108 | "# TODO: Run this cell to see if your results are as expected.\n", 109 | "\n", 110 | "m = [[5, 9, 2, 4],\n", 111 | " [3, 8, 5, 6],\n", 112 | " [1, 0, 0, 15]]\n", 113 | "\n", 114 | "assert matrix_multiplication(m, identity_matrix(4)) == m\n", 115 | "assert matrix_multiplication(identity_matrix(3), m) == m" 116 | ] 117 | } 118 | ], 119 | "metadata": { 120 | "kernelspec": { 121 | "display_name": "Python 3", 122 | "language": "python", 123 | "name": "python3" 124 | }, 125 | "language_info": { 126 | "codemirror_mode": { 127 | "name": "ipython", 128 | "version": 3 129 | }, 130 | "file_extension": ".py", 131 | "mimetype": "text/x-python", 132 | "name": "python", 133 | "nbconvert_exporter": "python", 134 | "pygments_lexer": "ipython3", 135 | "version": "3.6.1" 136 | } 137 | }, 138 | "nbformat": 4, 139 | "nbformat_minor": 2 140 | } 141 | -------------------------------------------------------------------------------- /4_6_Matrices_and_Transformation_of_State/guide_to_mathematical_notation.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "# Becoming \"Wikipedia proficient\"\n", 8 | "\n", 9 | "The goal of this course is **not** for you to memorize how to calculate a dot product or multiply matrices. The goal is for you to be able to do something useful with a wikipedia page like their [article on Kalman Filters](https://en.wikipedia.org/wiki/Kalman_filter), even if requires some additional research and review from you.\n", 10 | "\n", 11 | "But these pages are usually written in the notation of **linear algebra** and not the notation of computer programming. \n", 12 | "\n", 13 | "In this notebook you will learn something about how to navigate the notation of linear algebra and how to translate it into computer code. " 14 | ] 15 | }, 16 | { 17 | "cell_type": "markdown", 18 | "metadata": {}, 19 | "source": [ 20 | "## Analyzing The Dot Product Equation\n", 21 | "At the time I'm writing this, the wikipedia article on the [dot product](https://en.wikipedia.org/wiki/Dot_product) begins with a section called **Algebraic Definition**, which starts like this:\n", 22 | "\n", 23 | "> The dot product of two vectors $\\mathbf{a} = [a_1, a_2, \\ldots, a_n]$ and $\\mathbf{b} = [b_1, b_2, \\ldots, b_n]$ is defined as: \n", 24 | "> \n", 25 | "> $$\\mathbf{a} \\cdot \\mathbf{b} = \\sum _{i=1}^{n}a_{i}b_{i}=a_{1}b_{1}+a_{2}b_{2}+\\cdots +a_{n}b_{n}$$\n", 26 | "\n", 27 | "If you don't know what to look for, this can be pretty unhelfpul. Let's take a look at three features of this equation which can be helpful to understand...\n", 28 | "\n", 29 | "### Feature 1 - Lowercase vs uppercase variables\n", 30 | "This equation only uses lowercase variables. In general, lowercase variables are used when discussing **vectors** or **scalars** (regular numbers like 3, -2.5, etc...) while UPPERCASE variables are reserved for matrices. \n", 31 | "\n", 32 | "### Feature 2 - Bold vs regular typeface for variables\n", 33 | "A variable in **bold** typeface indicates a vector or a matrix. A variable in regular typeface is a scalar.\n", 34 | "\n", 35 | "\n", 36 | "### Feature 3 - \"...\" in equations\n", 37 | "When you see three dots $\\ldots$ in an equation it means \"this pattern could continue any number of times\"\n", 38 | "\n", 39 | "#### EXAMPLE 1 - APPLYING FEATURES 1, 2, and 3\n", 40 | "When you see something like $\\mathbf{a} = [a_1, a_2, \\ldots, a_n]$ you can infer the following:\n", 41 | "\n", 42 | "1. **$\\mathbf{a}$ is a vector**: since a is bold it's either a vector OR a matrix, but since it's also lowercase, we know it can only be a vector.\n", 43 | "\n", 44 | "2. **$\\mathbf{a}$ can have any length**: since there's a $\\ldots$ in the definition for $\\mathbf{a}$, we know that in addition to $a_1$ and $a_2$ there could also be $a_3$, $a_4$, and so on... \n", 45 | "\n", 46 | "3. **The values in the $\\mathbf{a}$ vector are scalars**: since $a_1$ is lowercase and non-bold we know that it must be a scalar (regular number) as opposed to being a vector or matrix." 47 | ] 48 | }, 49 | { 50 | "cell_type": "markdown", 51 | "metadata": {}, 52 | "source": [ 53 | "### Feature 4 - $\\Sigma$ Notation\n", 54 | "The symbol $\\Sigma$ is the uppercase version of the greek letter \"sigma\" and it is an instruction to perform a sum.\n", 55 | "\n", 56 | "**When you see a $\\Sigma$ you should think \"for loop!\"**\n", 57 | "\n", 58 | "In the case of the dot product, the sigma instructs us to sum $a_ib_i$ for $i=1,2, \\ldots, n$. And in this case $n$ is just the length of the $\\mathbf{a}$ and $\\mathbf{b}$ vectors.\n", 59 | "\n", 60 | "How this for loop works is best explained with an example. Take a look at the `dot_product` function defined below. Try to read through the comments and really understand how the code connects to math.\n" 61 | ] 62 | }, 63 | { 64 | "cell_type": "markdown", 65 | "metadata": {}, 66 | "source": [ 67 | "**The MATH**\n", 68 | "\n", 69 | "The dot product of two vectors $\\mathbf{a} = [a_1, a_2, \\ldots, a_n]$ and $\\mathbf{b} = [b_1, b_2, \\ldots, b_n]$ is defined as: \n", 70 | "\n", 71 | "$$\\mathbf{a} \\cdot \\mathbf{b} = \\sum _{i=1}^{n}a_{i}b_{i}=a_{1}b_{1}+a_{2}b_{2}+\\cdots +a_{n}b_{n}$$" 72 | ] 73 | }, 74 | { 75 | "cell_type": "code", 76 | "execution_count": 4, 77 | "metadata": { 78 | "collapsed": true 79 | }, 80 | "outputs": [], 81 | "source": [ 82 | "# The CODE\n", 83 | "\n", 84 | "def dot_product(a, b):\n", 85 | " # start by checking that a and b have the same length.\n", 86 | " # I know they SHOULD have the same length because they\n", 87 | " # each are DEFINED (in the first line above) to have n \n", 88 | " # elements. Even though n isn't specified, the fact that \n", 89 | " # a goes from 0 to n AND b does the same (instead of going\n", 90 | " # from 0 to m for example) implies that these vectors \n", 91 | " # always should have the same length.\n", 92 | " if len(a) != len(b):\n", 93 | " print(\"Error! Vectors must have the same length!\")\n", 94 | " return None\n", 95 | " \n", 96 | " # let's call the length of these vectors \"n\" so we can\n", 97 | " # be consistent with the mathematical notation\n", 98 | " n = len(a)\n", 99 | " \n", 100 | " # Since we want to add up a bunch of terms, we should\n", 101 | " # start by setting the total to zero and then add to \n", 102 | " # this total n times.\n", 103 | " total = 0\n", 104 | " \n", 105 | " # now we are going to perform the multiplication!\n", 106 | " # note that the algebraic version goes from 1 to n. \n", 107 | " # The Python version of this indexing will go from \n", 108 | " # 0 to n-1 (recall that range(3) returns [0,1,2] for example).\n", 109 | " for i in range(n): \n", 110 | " a_i = a[i]\n", 111 | " b_i = b[i]\n", 112 | " total = total + a_i * b_i\n", 113 | " \n", 114 | " return total\n", 115 | "\n" 116 | ] 117 | }, 118 | { 119 | "cell_type": "code", 120 | "execution_count": 3, 121 | "metadata": {}, 122 | "outputs": [ 123 | { 124 | "name": "stdout", 125 | "output_type": "stream", 126 | "text": [ 127 | "52\n" 128 | ] 129 | } 130 | ], 131 | "source": [ 132 | "# let's see if it works\n", 133 | "a = [3,2,4]\n", 134 | "b = [2,5,9]\n", 135 | "\n", 136 | "# a*b should be 3*2 + 2*5 + 4*9\n", 137 | "# or... 6 + 10 + 36 \n", 138 | "# 52\n", 139 | "\n", 140 | "a_dot_b = dot_product(a,b)\n", 141 | "print(a_dot_b)" 142 | ] 143 | }, 144 | { 145 | "cell_type": "code", 146 | "execution_count": null, 147 | "metadata": { 148 | "collapsed": true 149 | }, 150 | "outputs": [], 151 | "source": [] 152 | } 153 | ], 154 | "metadata": { 155 | "kernelspec": { 156 | "display_name": "Python 3", 157 | "language": "python", 158 | "name": "python3" 159 | }, 160 | "language_info": { 161 | "codemirror_mode": { 162 | "name": "ipython", 163 | "version": 3 164 | }, 165 | "file_extension": ".py", 166 | "mimetype": "text/x-python", 167 | "name": "python", 168 | "nbconvert_exporter": "python", 169 | "pygments_lexer": "ipython3", 170 | "version": "3.6.1" 171 | } 172 | }, 173 | "nbformat": 4, 174 | "nbformat_minor": 2 175 | } 176 | -------------------------------------------------------------------------------- /4_7_SLAM/1_1. Construct Omega and Xi, exercise.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "## Omega and Xi\n", 8 | "\n", 9 | "To implement Graph SLAM, a matrix and a vector (omega and xi, respectively) are introduced. The matrix is square and labelled with all the robot poses (xi) and all the landmarks (Li). Every time you make an observation, for example, as you move between two poses by some distance `dx` and can relate those two positions, you can represent this as a numerical relationship in these matrices.\n", 10 | "\n", 11 | "Below you can see a matrix representation of omega and a vector representation of xi.\n", 12 | "\n", 13 | "\n", 14 | "\n", 15 | "\n", 16 | "### Solving for x, L\n", 17 | "\n", 18 | "To \"solve\" for all these poses and landmark positions, we can use linear algebra; all the positional values are in the vector `mu` which can be calculated as a product of the inverse of omega times xi.\n", 19 | "\n", 20 | "---\n" 21 | ] 22 | }, 23 | { 24 | "cell_type": "markdown", 25 | "metadata": {}, 26 | "source": [ 27 | "### Quiz: Construct constraints for 3 motions and return `mu`\n", 28 | "\n", 29 | "In the following example, you will complete the function call `mu_from_positions(-3, 5, 3)`, which takes in 3 robot poses/moves:\n", 30 | "* initial pose: -3\n", 31 | "* moves by 5\n", 32 | "* moves by 3\n", 33 | "\n", 34 | "In this function, you should construct the constraint matrices `omega` and `xi` and calculate `mu`. The final call should result in a `mu` of:\n", 35 | "```\n", 36 | "[[-3.0],\n", 37 | " [2.0],\n", 38 | " [5.0]]\n", 39 | " ```" 40 | ] 41 | }, 42 | { 43 | "cell_type": "markdown", 44 | "metadata": {}, 45 | "source": [ 46 | "## Constraint Updates\n", 47 | "\n", 48 | "We will not consider landmark sensor measurements in this example, only robot poses.\n", 49 | "\n", 50 | "#### Motion\n", 51 | "When your robot moves by some amount `dx` update the constraint matrices as follows:\n", 52 | "* Add `[[1, -1], [-1, 1]]` to omega at the indices for the intersection of `xt` and `xt+1`\n", 53 | "* Add `-dx` and `dx` to xi at the rows for `xt` and `xt+1`" 54 | ] 55 | }, 56 | { 57 | "cell_type": "code", 58 | "execution_count": null, 59 | "metadata": { 60 | "collapsed": true 61 | }, 62 | "outputs": [], 63 | "source": [ 64 | "import numpy as np\n", 65 | "\n", 66 | "\n", 67 | "def mu_from_positions(initial_pos, move1, move2):\n", 68 | " \n", 69 | " ## TODO: construct constraint matrices\n", 70 | " ## and add each position/motion constraint to them\n", 71 | " \n", 72 | " # Your code here\n", 73 | " \n", 74 | " # display final omega and xi\n", 75 | " print('Omega: \\n', omega)\n", 76 | " print('\\n')\n", 77 | " print('Xi: \\n', xi)\n", 78 | " print('\\n')\n", 79 | " \n", 80 | " ## TODO: calculate mu as the inverse of omega * xi\n", 81 | " ## recommended that you use: np.linalg.inv(np.matrix(omega)) to calculate the inverse\n", 82 | " mu = 0\n", 83 | " return mu\n" 84 | ] 85 | }, 86 | { 87 | "cell_type": "code", 88 | "execution_count": null, 89 | "metadata": {}, 90 | "outputs": [], 91 | "source": [ 92 | "# call function and print out `mu`\n", 93 | "mu = mu_from_positions(-3, 5, 3)\n", 94 | "print('Mu: \\n', mu)" 95 | ] 96 | }, 97 | { 98 | "cell_type": "code", 99 | "execution_count": null, 100 | "metadata": { 101 | "collapsed": true 102 | }, 103 | "outputs": [], 104 | "source": [] 105 | } 106 | ], 107 | "metadata": { 108 | "kernelspec": { 109 | "display_name": "Python 3", 110 | "language": "python", 111 | "name": "python3" 112 | }, 113 | "language_info": { 114 | "codemirror_mode": { 115 | "name": "ipython", 116 | "version": 3 117 | }, 118 | "file_extension": ".py", 119 | "mimetype": "text/x-python", 120 | "name": "python", 121 | "nbconvert_exporter": "python", 122 | "pygments_lexer": "ipython3", 123 | "version": "3.6.3" 124 | } 125 | }, 126 | "nbformat": 4, 127 | "nbformat_minor": 2 128 | } 129 | -------------------------------------------------------------------------------- /4_7_SLAM/1_2. Construct Omega and Xi, solution.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "## Omega and Xi\n", 8 | "\n", 9 | "To implement Graph SLAM, a matrix and a vector (omega and xi, respectively) are introduced. The matrix is square and labelled with all the robot poses (xi) and all the landmarks (Li). Every time you make an observation, for example, as you move between two poses by some distance `dx` and can relate those two positions, you can represent this as a numerical relationship in these matrices.\n", 10 | "\n", 11 | "Below you can see a matrix representation of omega and a vector representation of xi.\n", 12 | "\n", 13 | "\n", 14 | "\n", 15 | "\n", 16 | "### Solving for x, L\n", 17 | "\n", 18 | "To \"solve\" for all these poses and landmark positions, we can use linear algebra; all the positional values are in the vector `mu` which can be calculated as a product of the inverse of omega times xi.\n", 19 | "\n", 20 | "---\n" 21 | ] 22 | }, 23 | { 24 | "cell_type": "markdown", 25 | "metadata": {}, 26 | "source": [ 27 | "### Quiz: Construct constraints for 3 motions and return `mu`\n", 28 | "\n", 29 | "In the following example, you will complete the function call `mu_from_positions(-3, 5, 3)`, which takes in 3 robot poses/moves:\n", 30 | "* initial pose: -3\n", 31 | "* moves by 5\n", 32 | "* moves by 3\n", 33 | "\n", 34 | "In this function, you should construct the constraint matrices `omega` and `xi` and calculate `mu`. The final call should result in a `mu` of:\n", 35 | "```\n", 36 | "[[-3.0],\n", 37 | " [2.0],\n", 38 | " [5.0]]\n", 39 | " ```" 40 | ] 41 | }, 42 | { 43 | "cell_type": "markdown", 44 | "metadata": {}, 45 | "source": [ 46 | "## Constraint Updates\n", 47 | "\n", 48 | "We will not consider landmark sensor measurements in this example, only robot poses.\n", 49 | "\n", 50 | "#### Motion\n", 51 | "When your robot moves by some amount `dx` update the constraint matrices as follows:\n", 52 | "* Add `[[1, -1], [-1, 1]]` to omega at the indices for the intersection of `xt` and `xt+1`\n", 53 | "* Add `-dx` and `dx` to xi at the rows for `xt` and `xt+1`" 54 | ] 55 | }, 56 | { 57 | "cell_type": "code", 58 | "execution_count": 29, 59 | "metadata": {}, 60 | "outputs": [], 61 | "source": [ 62 | "import numpy as np\n", 63 | "\n", 64 | "\n", 65 | "def mu_from_positions(initial_pos, move1, move2):\n", 66 | " \n", 67 | " ## TODO: construct constraint matrices\n", 68 | " ## and add each position/motion constraint to them\n", 69 | " \n", 70 | " # initialize constraint matrices with 0's\n", 71 | " omega = np.zeros((3,3))\n", 72 | " xi = np.zeros((3,1))\n", 73 | " \n", 74 | " # add initial pose constraint\n", 75 | " omega[0][0] = 1\n", 76 | " xi[0] = initial_pos\n", 77 | " \n", 78 | " # account for the first motion, dx = move1\n", 79 | " omega += [[1., -1., 0.],\n", 80 | " [-1., 1., 0.],\n", 81 | " [0., 0., 0.]]\n", 82 | " xi += [[-move1],\n", 83 | " [move1],\n", 84 | " [0.0]]\n", 85 | " \n", 86 | " # account for the second motion\n", 87 | " omega += [[0., 0., 0.],\n", 88 | " [0., 1., -1.],\n", 89 | " [0., -1., 1.]]\n", 90 | " xi += [[0.],\n", 91 | " [-move2],\n", 92 | " [move2]]\n", 93 | " \n", 94 | " # display final omega and xi\n", 95 | " print('Omega: \\n', omega)\n", 96 | " print('\\n')\n", 97 | " print('Xi: \\n', xi)\n", 98 | " print('\\n')\n", 99 | " \n", 100 | " ## TODO: calculate mu as the inverse of omega * xi\n", 101 | " ## recommended that you use: np.linalg.inv(np.matrix(omega)) to calculate the inverse\n", 102 | " omega_inv = np.linalg.inv(np.matrix(omega))\n", 103 | " mu = omega_inv*xi\n", 104 | " return mu\n" 105 | ] 106 | }, 107 | { 108 | "cell_type": "code", 109 | "execution_count": 30, 110 | "metadata": {}, 111 | "outputs": [ 112 | { 113 | "name": "stdout", 114 | "output_type": "stream", 115 | "text": [ 116 | "Omega: \n", 117 | " [[ 2. -1. 0.]\n", 118 | " [-1. 2. -1.]\n", 119 | " [ 0. -1. 1.]]\n", 120 | "\n", 121 | "\n", 122 | "Xi: \n", 123 | " [[-8.]\n", 124 | " [ 2.]\n", 125 | " [ 3.]]\n", 126 | "\n", 127 | "\n", 128 | "Mu: \n", 129 | " [[-3.]\n", 130 | " [ 2.]\n", 131 | " [ 5.]]\n" 132 | ] 133 | } 134 | ], 135 | "source": [ 136 | "# call function and print out `mu`\n", 137 | "mu = mu_from_positions(-3, 5, 3)\n", 138 | "print('Mu: \\n', mu)" 139 | ] 140 | }, 141 | { 142 | "cell_type": "code", 143 | "execution_count": null, 144 | "metadata": { 145 | "collapsed": true 146 | }, 147 | "outputs": [], 148 | "source": [] 149 | } 150 | ], 151 | "metadata": { 152 | "kernelspec": { 153 | "display_name": "Python 3", 154 | "language": "python", 155 | "name": "python3" 156 | }, 157 | "language_info": { 158 | "codemirror_mode": { 159 | "name": "ipython", 160 | "version": 3 161 | }, 162 | "file_extension": ".py", 163 | "mimetype": "text/x-python", 164 | "name": "python", 165 | "nbconvert_exporter": "python", 166 | "pygments_lexer": "ipython3", 167 | "version": "3.6.3" 168 | } 169 | }, 170 | "nbformat": 4, 171 | "nbformat_minor": 2 172 | } 173 | -------------------------------------------------------------------------------- /4_7_SLAM/2_1. Include Landmarks, exercise.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "## Updating Constraint Matrices\n", 8 | "\n", 9 | "To implement Graph SLAM, a matrix and a vector (omega and xi, respectively) are introduced. The matrix is square and labelled with all the robot poses (xi) and all the landmarks (Li). Every time you make an observation, for example, as you move between two poses by some distance `dx` and can relate those two positions, you can represent this as a numerical relationship in these matrices.\n", 10 | "\n", 11 | "Below you can see a matrix representation of omega and a vector representation of xi.\n", 12 | "\n", 13 | "\n", 14 | "\n", 15 | "\n", 16 | "### Solving for x, L\n", 17 | "\n", 18 | "To \"solve\" for all these poses and landmark positions, we can use linear algebra; all the positional values are in the vector `mu` which can be calculated as a product of the inverse of omega times xi.\n", 19 | "\n", 20 | "---\n" 21 | ] 22 | }, 23 | { 24 | "cell_type": "markdown", 25 | "metadata": {}, 26 | "source": [ 27 | "## Constraint Updates\n", 28 | "\n", 29 | "In the below code, we construct `omega` and `xi` constraint matrices, and update these according to landmark sensor measurements and motion.\n", 30 | "\n", 31 | "#### Sensor Measurements\n", 32 | "\n", 33 | "When you sense a distance, `dl`, between a pose and a landmark, l, update the constraint matrices as follows:\n", 34 | "* Add `[[1, -1], [-1, 1]]` to omega at the indices for the intersection of `xt` and `l`\n", 35 | "* Add `-dl` and `dl` to xi at the rows for `xt` and `l`\n", 36 | "\n", 37 | "The values 2 instead of 1 indicate the \"strength\" of the measurement.\n", 38 | "\n", 39 | "You'll see three new `dl`'s as new inputs to our function `Z0, Z1, Z2`, below.\n", 40 | "\n", 41 | "#### Motion\n", 42 | "When your robot moves by some amount `dx` update the constraint matrices as follows:\n", 43 | "* Add `[[1, -1], [-1, 1]]` to omega at the indices for the intersection of `xt` and `xt+1`\n", 44 | "* Add `-dx` and `dx` to xi at the rows for `xt` and `xt+1`\n", 45 | "\n", 46 | "## QUIZ: Include three new sensor measurements for a single landmark, L." 47 | ] 48 | }, 49 | { 50 | "cell_type": "code", 51 | "execution_count": null, 52 | "metadata": { 53 | "collapsed": true 54 | }, 55 | "outputs": [], 56 | "source": [ 57 | "import numpy as np\n", 58 | "\n", 59 | "\n", 60 | "def mu_from_positions(initial_pos, move1, move2, Z0, Z1, Z2):\n", 61 | " \n", 62 | " ## TODO: construct constraint matrices\n", 63 | " ## and add each position/motion constraint to them\n", 64 | " \n", 65 | " # initialize constraint matrices with 0's\n", 66 | " # Now these are 4x4 because of 3 poses and a landmark\n", 67 | " omega = np.zeros((4,4))\n", 68 | " xi = np.zeros((4,1))\n", 69 | " \n", 70 | " # add initial pose constraint\n", 71 | " omega[0][0] = 1\n", 72 | " xi[0] = initial_pos\n", 73 | " \n", 74 | " # account for the first motion, dx = move1\n", 75 | " omega += [[1., -1., 0., 0.],\n", 76 | " [-1., 1., 0., 0.],\n", 77 | " [0., 0., 0., 0.],\n", 78 | " [0., 0., 0., 0.]]\n", 79 | " xi += [[-move1],\n", 80 | " [move1],\n", 81 | " [0.],\n", 82 | " [0.]]\n", 83 | " \n", 84 | " # account for the second motion\n", 85 | " omega += [[0., 0., 0., 0.],\n", 86 | " [0., 1., -1., 0.],\n", 87 | " [0., -1., 1., 0.],\n", 88 | " [0., 0., 0., 0.]]\n", 89 | " xi += [[0.],\n", 90 | " [-move2],\n", 91 | " [move2],\n", 92 | " [0.]]\n", 93 | " \n", 94 | " \n", 95 | " ## TODO: Include three new sensor measurements for the landmark, L\n", 96 | " \n", 97 | " # Your code here\n", 98 | " \n", 99 | " # display final omega and xi\n", 100 | " print('Omega: \\n', omega)\n", 101 | " print('\\n')\n", 102 | " print('Xi: \\n', xi)\n", 103 | " print('\\n')\n", 104 | " \n", 105 | " ## TODO: calculate mu as the inverse of omega * xi\n", 106 | " ## recommended that you use: np.linalg.inv(np.matrix(omega)) to calculate the inverse\n", 107 | " omega_inv = np.linalg.inv(np.matrix(omega))\n", 108 | " mu = omega_inv*xi\n", 109 | " return mu\n" 110 | ] 111 | }, 112 | { 113 | "cell_type": "code", 114 | "execution_count": null, 115 | "metadata": {}, 116 | "outputs": [], 117 | "source": [ 118 | "# call function and print out `mu`\n", 119 | "mu = mu_from_positions(-3, 5, 3, 10, 5, 2)\n", 120 | "print('Mu: \\n', mu)" 121 | ] 122 | }, 123 | { 124 | "cell_type": "code", 125 | "execution_count": null, 126 | "metadata": { 127 | "collapsed": true 128 | }, 129 | "outputs": [], 130 | "source": [] 131 | } 132 | ], 133 | "metadata": { 134 | "kernelspec": { 135 | "display_name": "Python 3", 136 | "language": "python", 137 | "name": "python3" 138 | }, 139 | "language_info": { 140 | "codemirror_mode": { 141 | "name": "ipython", 142 | "version": 3 143 | }, 144 | "file_extension": ".py", 145 | "mimetype": "text/x-python", 146 | "name": "python", 147 | "nbconvert_exporter": "python", 148 | "pygments_lexer": "ipython3", 149 | "version": "3.6.3" 150 | } 151 | }, 152 | "nbformat": 4, 153 | "nbformat_minor": 2 154 | } 155 | -------------------------------------------------------------------------------- /4_7_SLAM/2_2. Include Landmarks, solution.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "## Updating Constraint Matrices\n", 8 | "\n", 9 | "To implement Graph SLAM, a matrix and a vector (omega and xi, respectively) are introduced. The matrix is square and labelled with all the robot poses (xi) and all the landmarks (Li). Every time you make an observation, for example, as you move between two poses by some distance `dx` and can relate those two positions, you can represent this as a numerical relationship in these matrices.\n", 10 | "\n", 11 | "Below you can see a matrix representation of omega and a vector representation of xi.\n", 12 | "\n", 13 | "\n", 14 | "\n", 15 | "\n", 16 | "### Solving for x, L\n", 17 | "\n", 18 | "To \"solve\" for all these poses and landmark positions, we can use linear algebra; all the positional values are in the vector `mu` which can be calculated as a product of the inverse of omega times xi.\n", 19 | "\n", 20 | "---\n" 21 | ] 22 | }, 23 | { 24 | "cell_type": "markdown", 25 | "metadata": {}, 26 | "source": [ 27 | "## Constraint Updates\n", 28 | "\n", 29 | "In the below code, we construct `omega` and `xi` constraint matrices, and update these according to landmark sensor measurements and motion.\n", 30 | "\n", 31 | "#### Sensor Measurements\n", 32 | "\n", 33 | "When you sense a distance, `dl`, between a pose and a landmark, l, update the constraint matrices as follows:\n", 34 | "* Add `[[1, -1], [-1, 1]]` to omega at the indices for the intersection of `xt` and `l`\n", 35 | "* Add `-dl` and `dl` to xi at the rows for `xt` and `l`\n", 36 | "\n", 37 | "The values 2 instead of 1 indicate the \"strength\" of the measurement.\n", 38 | "\n", 39 | "You'll see three new `dl`'s as new inputs to our function `Z0, Z1, Z2`, below.\n", 40 | "\n", 41 | "#### Motion\n", 42 | "When your robot moves by some amount `dx` update the constraint matrices as follows:\n", 43 | "* Add `[[1, -1], [-1, 1]]` to omega at the indices for the intersection of `xt` and `xt+1`\n", 44 | "* Add `-dx` and `dx` to xi at the rows for `xt` and `xt+1`\n", 45 | "\n", 46 | "## QUIZ: Include three new sensor measurements for a single landmark, L." 47 | ] 48 | }, 49 | { 50 | "cell_type": "code", 51 | "execution_count": 3, 52 | "metadata": { 53 | "collapsed": true 54 | }, 55 | "outputs": [], 56 | "source": [ 57 | "import numpy as np\n", 58 | "\n", 59 | "\n", 60 | "def mu_from_positions(initial_pos, move1, move2, Z0, Z1, Z2):\n", 61 | " \n", 62 | " ## TODO: construct constraint matrices\n", 63 | " ## and add each position/motion constraint to them\n", 64 | " \n", 65 | " # initialize constraint matrices with 0's\n", 66 | " # Now these are 4x4 because of 3 poses and a landmark\n", 67 | " omega = np.zeros((4,4))\n", 68 | " xi = np.zeros((4,1))\n", 69 | " \n", 70 | " # add initial pose constraint\n", 71 | " omega[0][0] = 1\n", 72 | " xi[0] = initial_pos\n", 73 | " \n", 74 | " # account for the first motion, dx = move1\n", 75 | " omega += [[1., -1., 0., 0.],\n", 76 | " [-1., 1., 0., 0.],\n", 77 | " [0., 0., 0., 0.],\n", 78 | " [0., 0., 0., 0.]]\n", 79 | " xi += [[-move1],\n", 80 | " [move1],\n", 81 | " [0.],\n", 82 | " [0.]]\n", 83 | " \n", 84 | " # account for the second motion\n", 85 | " omega += [[0., 0., 0., 0.],\n", 86 | " [0., 1., -1., 0.],\n", 87 | " [0., -1., 1., 0.],\n", 88 | " [0., 0., 0., 0.]]\n", 89 | " xi += [[0.],\n", 90 | " [-move2],\n", 91 | " [move2],\n", 92 | " [0.]]\n", 93 | " \n", 94 | " \n", 95 | " ## TODO: Include three new sensor measurements for the landmark, L\n", 96 | " # incorporate first sense\n", 97 | " omega += [[1., 0., 0., -1.],\n", 98 | " [0., 0., 0., 0.],\n", 99 | " [0., 0., 0., 0.], \n", 100 | " [-1., 0., 0., 1.]]\n", 101 | " xi += [[-Z0],\n", 102 | " [0.0],\n", 103 | " [0.0],\n", 104 | " [Z0]]\n", 105 | "\n", 106 | " # incorporate second sense\n", 107 | " omega += [[0., 0., 0., 0.],\n", 108 | " [0., 1., 0., -1.],\n", 109 | " [0., 0., 0., 0.], \n", 110 | " [0., -1., 0., 1.]]\n", 111 | " xi += [[0.],\n", 112 | " [-Z1],\n", 113 | " [0.],\n", 114 | " [Z1]]\n", 115 | " \n", 116 | " # incorporate third sense\n", 117 | " omega += [[0., 0., 0., 0.],\n", 118 | " [0., 0., 0., 0.],\n", 119 | " [0., 0., 1., -1.], \n", 120 | " [0., 0., -1., 1.]]\n", 121 | " xi += [[0.],\n", 122 | " [0.],\n", 123 | " [-Z2],\n", 124 | " [Z2]]\n", 125 | " \n", 126 | " # display final omega and xi\n", 127 | " print('Omega: \\n', omega)\n", 128 | " print('\\n')\n", 129 | " print('Xi: \\n', xi)\n", 130 | " print('\\n')\n", 131 | " \n", 132 | " ## TODO: calculate mu as the inverse of omega * xi\n", 133 | " ## recommended that you use: np.linalg.inv(np.matrix(omega)) to calculate the inverse\n", 134 | " omega_inv = np.linalg.inv(np.matrix(omega))\n", 135 | " mu = omega_inv*xi\n", 136 | " return mu\n" 137 | ] 138 | }, 139 | { 140 | "cell_type": "code", 141 | "execution_count": 5, 142 | "metadata": {}, 143 | "outputs": [ 144 | { 145 | "name": "stdout", 146 | "output_type": "stream", 147 | "text": [ 148 | "Omega: \n", 149 | " [[ 3. -1. 0. -1.]\n", 150 | " [-1. 3. -1. -1.]\n", 151 | " [ 0. -1. 2. -1.]\n", 152 | " [-1. -1. -1. 3.]]\n", 153 | "\n", 154 | "\n", 155 | "Xi: \n", 156 | " [[-18.]\n", 157 | " [ -3.]\n", 158 | " [ 1.]\n", 159 | " [ 17.]]\n", 160 | "\n", 161 | "\n", 162 | "Mu: \n", 163 | " [[-3.]\n", 164 | " [ 2.]\n", 165 | " [ 5.]\n", 166 | " [ 7.]]\n" 167 | ] 168 | } 169 | ], 170 | "source": [ 171 | "# call function and print out `mu`\n", 172 | "mu = mu_from_positions(-3, 5, 3, 10, 5, 2)\n", 173 | "print('Mu: \\n', mu)" 174 | ] 175 | }, 176 | { 177 | "cell_type": "code", 178 | "execution_count": null, 179 | "metadata": { 180 | "collapsed": true 181 | }, 182 | "outputs": [], 183 | "source": [] 184 | } 185 | ], 186 | "metadata": { 187 | "kernelspec": { 188 | "display_name": "Python 3", 189 | "language": "python", 190 | "name": "python3" 191 | }, 192 | "language_info": { 193 | "codemirror_mode": { 194 | "name": "ipython", 195 | "version": 3 196 | }, 197 | "file_extension": ".py", 198 | "mimetype": "text/x-python", 199 | "name": "python", 200 | "nbconvert_exporter": "python", 201 | "pygments_lexer": "ipython3", 202 | "version": "3.6.3" 203 | } 204 | }, 205 | "nbformat": 4, 206 | "nbformat_minor": 2 207 | } 208 | -------------------------------------------------------------------------------- /4_7_SLAM/3. Confident Measurements.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "## Updating Constraint Matrices\n", 8 | "\n", 9 | "To implement Graph SLAM, a matrix and a vector (omega and xi, respectively) are introduced. The matrix is square and labelled with all the robot poses (xi) and all the landmarks (Li). Every time you make an observation, for example, as you move between two poses by some distance `dx` and can relate those two positions, you can represent this as a numerical relationship in these matrices.\n", 10 | "\n", 11 | "Below you can see a matrix representation of omega and a vector representation of xi.\n", 12 | "\n", 13 | "\n", 14 | "\n", 15 | "\n", 16 | "### Solving for x, L\n", 17 | "\n", 18 | "To \"solve\" for all these poses and landmark positions, we can use linear algebra; all the positional values are in the vector `mu` which can be calculated as a product of the inverse of omega times xi.\n", 19 | "\n", 20 | "---\n" 21 | ] 22 | }, 23 | { 24 | "cell_type": "markdown", 25 | "metadata": {}, 26 | "source": [ 27 | "## Constraint Updates\n", 28 | "\n", 29 | "In the below code, we construct `omega` and `xi` constraint matrices, and update these according to landmark sensor measurements and motion.\n", 30 | "\n", 31 | "#### Sensor Measurements\n", 32 | "\n", 33 | "When you sense a distance, `dl`, between a pose and a landmark, l, update the constraint matrices as follows:\n", 34 | "* Add `[[1, -1], [-1, 1]]` to omega at the indices for the intersection of `xt` and `l`\n", 35 | "* Add `-dl` and `dl` to xi at the rows for `xt` and `l`\n", 36 | "\n", 37 | "The values 2 instead of 1 indicate the \"strength\" of the measurement.\n", 38 | "\n", 39 | "You'll see three new `dl`'s as new inputs to our function `Z0, Z1, Z2`, below.\n", 40 | "\n", 41 | "#### Motion\n", 42 | "When your robot moves by some amount `dx` update the constraint matrices as follows:\n", 43 | "* Add `[[1, -1], [-1, 1]]` to omega at the indices for the intersection of `xt` and `xt+1`\n", 44 | "* Add `-dx` and `dx` to xi at the rows for `xt` and `xt+1`\n", 45 | "\n", 46 | "## Change the code so that the last sensor measurement (Z2) is extremely confident.\n", 47 | "\n", 48 | "Multiple the \"strength\" of that measurement by a factor of 5." 49 | ] 50 | }, 51 | { 52 | "cell_type": "code", 53 | "execution_count": 1, 54 | "metadata": {}, 55 | "outputs": [], 56 | "source": [ 57 | "import numpy as np\n", 58 | "\n", 59 | "\n", 60 | "def mu_from_positions(initial_pos, move1, move2, Z0, Z1, Z2):\n", 61 | " \n", 62 | " ## TODO: construct constraint matrices\n", 63 | " ## and add each position/motion constraint to them\n", 64 | " \n", 65 | " # initialize constraint matrices with 0's\n", 66 | " # Now these are 4x4 because of 3 poses and a landmark\n", 67 | " omega = np.zeros((4,4))\n", 68 | " xi = np.zeros((4,1))\n", 69 | " \n", 70 | " # add initial pose constraint\n", 71 | " omega[0][0] = 1\n", 72 | " xi[0] = initial_pos\n", 73 | " \n", 74 | " # account for the first motion, dx = move1\n", 75 | " omega += [[1., -1., 0., 0.],\n", 76 | " [-1., 1., 0., 0.],\n", 77 | " [0., 0., 0., 0.],\n", 78 | " [0., 0., 0., 0.]]\n", 79 | " xi += [[-move1],\n", 80 | " [move1],\n", 81 | " [0.],\n", 82 | " [0.]]\n", 83 | " \n", 84 | " # account for the second motion\n", 85 | " omega += [[0., 0., 0., 0.],\n", 86 | " [0., 1., -1., 0.],\n", 87 | " [0., -1., 1., 0.],\n", 88 | " [0., 0., 0., 0.]]\n", 89 | " xi += [[0.],\n", 90 | " [-move2],\n", 91 | " [move2],\n", 92 | " [0.]]\n", 93 | " \n", 94 | " \n", 95 | " ## TODO: Include three new sensor measurements for the landmark, L\n", 96 | " # incorporate first sense\n", 97 | " omega += [[1., 0., 0., -1.],\n", 98 | " [0., 0., 0., 0.],\n", 99 | " [0., 0., 0., 0.], \n", 100 | " [-1., 0., 0., 1.]]\n", 101 | " xi += [[-Z0],\n", 102 | " [0.0],\n", 103 | " [0.0],\n", 104 | " [Z0]]\n", 105 | "\n", 106 | " # incorporate second sense\n", 107 | " omega += [[0., 0., 0., 0.],\n", 108 | " [0., 1., 0., -1.],\n", 109 | " [0., 0., 0., 0.], \n", 110 | " [0., -1., 0., 1.]]\n", 111 | " xi += [[0.],\n", 112 | " [-Z1],\n", 113 | " [0.],\n", 114 | " [Z1]]\n", 115 | " \n", 116 | " ## This third sense is now *very confident* and\n", 117 | " ## we multiply everything by a strength factor of 5 instead of 1\n", 118 | " # incorporate third sense\n", 119 | " omega += [[0., 0., 0., 0.],\n", 120 | " [0., 0., 0., 0.],\n", 121 | " [0., 0., 5., -5.], \n", 122 | " [0., 0., -5., 5.]]\n", 123 | " xi += [[0.],\n", 124 | " [0.],\n", 125 | " [-Z2],\n", 126 | " [Z2]]\n", 127 | " \n", 128 | " # display final omega and xi\n", 129 | " print('Omega: \\n', omega)\n", 130 | " print('\\n')\n", 131 | " print('Xi: \\n', xi)\n", 132 | " print('\\n')\n", 133 | " \n", 134 | " ## TODO: calculate mu as the inverse of omega * xi\n", 135 | " ## recommended that you use: np.linalg.inv(np.matrix(omega)) to calculate the inverse\n", 136 | " omega_inv = np.linalg.inv(np.matrix(omega))\n", 137 | " mu = omega_inv*xi\n", 138 | " return mu\n" 139 | ] 140 | }, 141 | { 142 | "cell_type": "code", 143 | "execution_count": 10, 144 | "metadata": {}, 145 | "outputs": [ 146 | { 147 | "name": "stdout", 148 | "output_type": "stream", 149 | "text": [ 150 | "Omega: \n", 151 | " [[ 3. -1. 0. -1.]\n", 152 | " [-1. 3. -1. -1.]\n", 153 | " [ 0. -1. 6. -5.]\n", 154 | " [-1. -1. -5. 7.]]\n", 155 | "\n", 156 | "\n", 157 | "Xi: \n", 158 | " [[-18.]\n", 159 | " [ -3.]\n", 160 | " [ 2.]\n", 161 | " [ 16.]]\n", 162 | "\n", 163 | "\n", 164 | "Mu: \n", 165 | " [[-3. ]\n", 166 | " [ 2.32142857]\n", 167 | " [ 6.28571429]\n", 168 | " [ 6.67857143]]\n" 169 | ] 170 | } 171 | ], 172 | "source": [ 173 | "# call function and print out `mu`\n", 174 | "mu = mu_from_positions(-3, 5, 3, 10, 5, 1)\n", 175 | "print('Mu: \\n', mu)" 176 | ] 177 | }, 178 | { 179 | "cell_type": "code", 180 | "execution_count": null, 181 | "metadata": { 182 | "collapsed": true 183 | }, 184 | "outputs": [], 185 | "source": [] 186 | } 187 | ], 188 | "metadata": { 189 | "kernelspec": { 190 | "display_name": "Python 3", 191 | "language": "python", 192 | "name": "python3" 193 | }, 194 | "language_info": { 195 | "codemirror_mode": { 196 | "name": "ipython", 197 | "version": 3 198 | }, 199 | "file_extension": ".py", 200 | "mimetype": "text/x-python", 201 | "name": "python", 202 | "nbconvert_exporter": "python", 203 | "pygments_lexer": "ipython3", 204 | "version": "3.6.3" 205 | } 206 | }, 207 | "nbformat": 4, 208 | "nbformat_minor": 2 209 | } 210 | -------------------------------------------------------------------------------- /4_7_SLAM/images/omega_xi.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/CVND_Localization_Exercises/f5d5e90e2548aba4a145dd31f352fa9da19244dc/4_7_SLAM/images/omega_xi.png -------------------------------------------------------------------------------- /4_8_Vehicle_Motion_and_Calculus/Implement an Accelerometer.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "# Implement an Accelerometer\n", 8 | "In this notebook you will define your own `get_derivative_from_data` function and use it to differentiate position data ONCE to get velocity information and then again to get acceleration information.\n", 9 | "\n", 10 | "In part 1 I will demonstrate what this process looks like and then in part 2 you'll implement the function yourself." 11 | ] 12 | }, 13 | { 14 | "cell_type": "markdown", 15 | "metadata": {}, 16 | "source": [ 17 | "-----" 18 | ] 19 | }, 20 | { 21 | "cell_type": "markdown", 22 | "metadata": {}, 23 | "source": [ 24 | "## Part 1 - Reminder and Demonstration" 25 | ] 26 | }, 27 | { 28 | "cell_type": "code", 29 | "execution_count": null, 30 | "metadata": {}, 31 | "outputs": [], 32 | "source": [ 33 | "# run this cell for required imports\n", 34 | "\n", 35 | "from helpers import process_data\n", 36 | "from helpers import get_derivative_from_data as solution_derivative\n", 37 | "from matplotlib import pyplot as plt" 38 | ] 39 | }, 40 | { 41 | "cell_type": "code", 42 | "execution_count": null, 43 | "metadata": {}, 44 | "outputs": [], 45 | "source": [ 46 | "# load the parallel park data\n", 47 | "PARALLEL_PARK_DATA = process_data(\"parallel_park.pickle\")\n", 48 | "\n", 49 | "# get the relevant columns\n", 50 | "timestamps = [row[0] for row in PARALLEL_PARK_DATA]\n", 51 | "displacements = [row[1] for row in PARALLEL_PARK_DATA]\n", 52 | "\n", 53 | "# calculate first derivative\n", 54 | "speeds = solution_derivative(displacements, timestamps)\n", 55 | "\n", 56 | "# plot\n", 57 | "plt.title(\"Position and Velocity vs Time\")\n", 58 | "plt.xlabel(\"Time (seconds)\")\n", 59 | "plt.ylabel(\"Position (blue) and Speed (orange)\")\n", 60 | "plt.scatter(timestamps, displacements)\n", 61 | "plt.scatter(timestamps[1:], speeds)\n", 62 | "plt.show()" 63 | ] 64 | }, 65 | { 66 | "cell_type": "markdown", 67 | "metadata": {}, 68 | "source": [ 69 | "But you just saw that acceleration is the derivative of velocity... which means we can use the same derivative function to calculate acceleration!" 70 | ] 71 | }, 72 | { 73 | "cell_type": "code", 74 | "execution_count": null, 75 | "metadata": {}, 76 | "outputs": [], 77 | "source": [ 78 | "# calculate SECOND derivative \n", 79 | "accelerations = solution_derivative(speeds, timestamps[1:])\n", 80 | "\n", 81 | "# plot (note the slicing of timestamps from 2 --> end)\n", 82 | "plt.scatter(timestamps[2:], accelerations)\n", 83 | "plt.show()" 84 | ] 85 | }, 86 | { 87 | "cell_type": "markdown", 88 | "metadata": {}, 89 | "source": [ 90 | "As you can see, this parallel park motion consisted of four segments with different (but constant) acceleration. We can plot all three quantities at once like this:\n", 91 | "\n" 92 | ] 93 | }, 94 | { 95 | "cell_type": "code", 96 | "execution_count": null, 97 | "metadata": {}, 98 | "outputs": [], 99 | "source": [ 100 | "plt.title(\"x(t), v(t), a(t)\")\n", 101 | "plt.xlabel(\"Time (seconds)\")\n", 102 | "plt.ylabel(\"x (blue), v (orange), a (green)\")\n", 103 | "plt.scatter(timestamps, displacements)\n", 104 | "plt.scatter(timestamps[1:], speeds)\n", 105 | "plt.scatter(timestamps[2:], accelerations)\n", 106 | "plt.show()" 107 | ] 108 | }, 109 | { 110 | "cell_type": "markdown", 111 | "metadata": {}, 112 | "source": [ 113 | "----" 114 | ] 115 | }, 116 | { 117 | "cell_type": "markdown", 118 | "metadata": {}, 119 | "source": [ 120 | "## Part 2 - Implement it yourself!" 121 | ] 122 | }, 123 | { 124 | "cell_type": "code", 125 | "execution_count": null, 126 | "metadata": {}, 127 | "outputs": [], 128 | "source": [ 129 | "def get_derivative_from_data(position_data, time_data):\n", 130 | " # TODO - try your best to implement this code yourself!\n", 131 | " # if you get really stuck feel free to go back\n", 132 | " # to the previous notebook for a hint.\n", 133 | " return " 134 | ] 135 | }, 136 | { 137 | "cell_type": "code", 138 | "execution_count": null, 139 | "metadata": {}, 140 | "outputs": [], 141 | "source": [ 142 | "# Testing part 1 - visual testing of first derivative\n", 143 | "# compare this output to the corresponding graph above.\n", 144 | "speeds = get_derivative_from_data(displacements, timestamps)\n", 145 | "\n", 146 | "plt.title(\"Position and Velocity vs Time\")\n", 147 | "plt.xlabel(\"Time (seconds)\")\n", 148 | "plt.ylabel(\"Position (blue) and Speed (orange)\")\n", 149 | "plt.scatter(timestamps, displacements)\n", 150 | "plt.scatter(timestamps[1:], speeds)\n", 151 | "plt.show()" 152 | ] 153 | }, 154 | { 155 | "cell_type": "code", 156 | "execution_count": null, 157 | "metadata": {}, 158 | "outputs": [], 159 | "source": [ 160 | "# Testing part 2 - visual testing of second derivative\n", 161 | "# compare this output to the corresponding graph above.\n", 162 | "speeds = get_derivative_from_data(displacements, timestamps)\n", 163 | "accelerations = get_derivative_from_data(speeds, timestamps[1:])\n", 164 | "\n", 165 | "plt.title(\"x(t), v(t), a(t)\")\n", 166 | "plt.xlabel(\"Time (seconds)\")\n", 167 | "plt.ylabel(\"x (blue), v (orange), a (green)\")\n", 168 | "plt.scatter(timestamps, displacements)\n", 169 | "plt.scatter(timestamps[1:], speeds)\n", 170 | "plt.scatter(timestamps[2:], accelerations)\n", 171 | "plt.show()" 172 | ] 173 | } 174 | ], 175 | "metadata": { 176 | "kernelspec": { 177 | "display_name": "Python 3", 178 | "language": "python", 179 | "name": "python3" 180 | }, 181 | "language_info": { 182 | "codemirror_mode": { 183 | "name": "ipython", 184 | "version": 3 185 | }, 186 | "file_extension": ".py", 187 | "mimetype": "text/x-python", 188 | "name": "python", 189 | "nbconvert_exporter": "python", 190 | "pygments_lexer": "ipython3", 191 | "version": "3.6.3" 192 | } 193 | }, 194 | "nbformat": 4, 195 | "nbformat_minor": 2 196 | } 197 | -------------------------------------------------------------------------------- /4_8_Vehicle_Motion_and_Calculus/Plotting Position vs Time.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "# Plotting Position vs Time\n", 8 | "In this notebook you will plot a position vs time graph of the data you just saw.\n", 9 | "\n", 10 | "First, I will demonstrate such a plot by following these steps:\n", 11 | "\n", 12 | "1. Importing `pyplot`, Python's most popular plotting library.\n", 13 | "2. Storing data to be plotted in variables named `X` and `Y`\n", 14 | "3. Creating a scatter plot of this data using pyplot's `scatter()` function.\n", 15 | "4. Adding a line connecting two data points using pyplot's `plot()` function.\n", 16 | "4. Adding axis labels and a title to the graph." 17 | ] 18 | }, 19 | { 20 | "cell_type": "code", 21 | "execution_count": null, 22 | "metadata": {}, 23 | "outputs": [], 24 | "source": [ 25 | "# Step 1.\n", 26 | "# we import pyplot as plt so we can refer to the pyplot\n", 27 | "# succinctly. This is a standard convention for this library.\n", 28 | "\n", 29 | "from matplotlib import pyplot as plt" 30 | ] 31 | }, 32 | { 33 | "cell_type": "markdown", 34 | "metadata": {}, 35 | "source": [ 36 | "Initially, I only told you the mileage at 2:00 and 3:00. The data looked like this.\n", 37 | "\n", 38 | "| Time | Odometer
(miles) |\n", 39 | "|:----:|:--------------------------------:|\n", 40 | "| 2:00 | 30 |\n", 41 | "| 3:00 | 80 |\n", 42 | "\n", 43 | "I'd like to make a scatter plot of this data and I want my **horizontal** axis to show time and my **vertical** axis to show mileage.\n", 44 | "\n", 45 | "In this notebook (and those that follow), we are going to use a capital `X` to store horizontal axis data and a capital `Y` to store vertical axis data. In this case:" 46 | ] 47 | }, 48 | { 49 | "cell_type": "code", 50 | "execution_count": null, 51 | "metadata": {}, 52 | "outputs": [], 53 | "source": [ 54 | "# Step 2.\n", 55 | "# get the data into variables called X and Y. This naming pattern\n", 56 | "# is a convention. You could use any variables you like.\n", 57 | "\n", 58 | "X = [2,3]\n", 59 | "Y = [30,80]" 60 | ] 61 | }, 62 | { 63 | "cell_type": "code", 64 | "execution_count": null, 65 | "metadata": {}, 66 | "outputs": [], 67 | "source": [ 68 | "# Step 3.\n", 69 | "# create a scatter plot using plt.scatter. Note that you NEED\n", 70 | "# to call plt.show() to actually see the plot. Forgetting to \n", 71 | "# call plt.show() is a common source of problems for people \n", 72 | "# new to this library\n", 73 | "\n", 74 | "plt.scatter(X,Y)\n", 75 | "plt.show()" 76 | ] 77 | }, 78 | { 79 | "cell_type": "markdown", 80 | "metadata": {}, 81 | "source": [ 82 | "This isn't a very exciting scatter plot since it only has two data points. Let's add a line connecting these data points as well." 83 | ] 84 | }, 85 | { 86 | "cell_type": "code", 87 | "execution_count": null, 88 | "metadata": {}, 89 | "outputs": [], 90 | "source": [ 91 | "# Step 4.\n", 92 | "# add lines connecting adjacent points\n", 93 | "\n", 94 | "plt.scatter(X,Y)\n", 95 | "plt.plot(X,Y)\n", 96 | "plt.show()" 97 | ] 98 | }, 99 | { 100 | "cell_type": "markdown", 101 | "metadata": {}, 102 | "source": [ 103 | "Let's add a title and labels to the X and Y axes " 104 | ] 105 | }, 106 | { 107 | "cell_type": "code", 108 | "execution_count": null, 109 | "metadata": {}, 110 | "outputs": [], 111 | "source": [ 112 | "plt.scatter(X,Y)\n", 113 | "plt.plot(X,Y)\n", 114 | "plt.title(\"Position vs. Time on a Roadtrip\")\n", 115 | "plt.xlabel(\"Time (in hours)\")\n", 116 | "plt.ylabel(\"Odometer Reading (in miles)\")\n", 117 | "plt.show()" 118 | ] 119 | }, 120 | { 121 | "cell_type": "markdown", 122 | "metadata": {}, 123 | "source": [ 124 | "## Twenty minute resolution\n", 125 | "When looking at the odometer every *20* minutes, the data looks like this:\n", 126 | "\n", 127 | "| Time | Odometer
(miles) |\n", 128 | "|:----:|:--------------------------------:|\n", 129 | "| 2:00 | 30 |\n", 130 | "| 2:20 | 40 |\n", 131 | "| 2:40 | 68 |\n", 132 | "| 3:00 | 80 |\n", 133 | "\n", 134 | "But a better way to think about it for plotting is like this (note the difference in how time is represented):\n", 135 | "\n", 136 | "| Time | Odometer
(miles) |\n", 137 | "|:----:|:--------------------------------:|\n", 138 | "| 2.000 | 30 |\n", 139 | "| 2.333 | 40 |\n", 140 | "| 2.667 | 68 |\n", 141 | "| 3.000 | 80 |\n", 142 | "\n", 143 | "### EXERCISE 1 - Make a position vs time graph of the data shown above with lines connecting adjacent dots.\n", 144 | "\n", 145 | "Reproduce the demonstration from before using the data shown above." 146 | ] 147 | }, 148 | { 149 | "cell_type": "code", 150 | "execution_count": null, 151 | "metadata": {}, 152 | "outputs": [], 153 | "source": [ 154 | "# TODO - your code for exercise 1 here\n", 155 | "\n", 156 | "\n", 157 | "\n", 158 | "\n" 159 | ] 160 | }, 161 | { 162 | "cell_type": "markdown", 163 | "metadata": {}, 164 | "source": [ 165 | "#### Exercise 1 - Solution Check (full solution code at end of notebook)\n", 166 | "You'll know you're correct when you've generated a plot that looks something like the following:\n", 167 | "\n", 168 | "![](https://d17h27t6h515a5.cloudfront.net/topher/2017/December/5a2ee74f_vmc-l1-20-min-plot/vmc-l1-20-min-plot.png)\n" 169 | ] 170 | }, 171 | { 172 | "cell_type": "markdown", 173 | "metadata": {}, 174 | "source": [ 175 | "### EXERCISE 2 - Reflect\n", 176 | "Look at the graph above and think about the following questions (we will talk about them more in the video that follows) \n", 177 | "\n", 178 | "1. How can you tell which of these time intervals had the highest average speed (without looking at the actual data?)\n", 179 | "\n", 180 | "2. In the car stopped from 3:00 - 4:00 and you were to plot that data, what would the **slope** of that line look like? " 181 | ] 182 | }, 183 | { 184 | "cell_type": "code", 185 | "execution_count": null, 186 | "metadata": {}, 187 | "outputs": [], 188 | "source": [ 189 | "# \n", 190 | "\n", 191 | "#\n", 192 | "\n", 193 | "#\n", 194 | "\n", 195 | "# SOLUTION CODE BELOW\n", 196 | "\n", 197 | "#\n", 198 | "\n", 199 | "#\n", 200 | "\n", 201 | "#\n", 202 | "\n", 203 | "# Exercise 1 - Solution\n", 204 | "\n", 205 | "X = [\n", 206 | " 2.000,\n", 207 | " 2.333,\n", 208 | " 2.667,\n", 209 | " 3.000\n", 210 | "]\n", 211 | "\n", 212 | "Y = [\n", 213 | " 30,\n", 214 | " 40,\n", 215 | " 68,\n", 216 | " 80\n", 217 | "]\n", 218 | "\n", 219 | "plt.scatter(X,Y)\n", 220 | "plt.plot(X,Y)\n", 221 | "plt.title(\"Position vs. Time on a Roadtrip\")\n", 222 | "plt.xlabel(\"Time (in hours)\")\n", 223 | "plt.ylabel(\"Odometer Reading (in miles)\")\n", 224 | "plt.show()" 225 | ] 226 | }, 227 | { 228 | "cell_type": "code", 229 | "execution_count": null, 230 | "metadata": {}, 231 | "outputs": [], 232 | "source": [] 233 | } 234 | ], 235 | "metadata": { 236 | "kernelspec": { 237 | "display_name": "Python 3", 238 | "language": "python", 239 | "name": "python3" 240 | }, 241 | "language_info": { 242 | "codemirror_mode": { 243 | "name": "ipython", 244 | "version": 3 245 | }, 246 | "file_extension": ".py", 247 | "mimetype": "text/x-python", 248 | "name": "python", 249 | "nbconvert_exporter": "python", 250 | "pygments_lexer": "ipython3", 251 | "version": "3.6.3" 252 | } 253 | }, 254 | "nbformat": 4, 255 | "nbformat_minor": 2 256 | } 257 | -------------------------------------------------------------------------------- /4_8_Vehicle_Motion_and_Calculus/helpers.py: -------------------------------------------------------------------------------- 1 | import pickle 2 | 3 | def process_data(filename): 4 | with open(filename, 'rb') as f: 5 | data_list = pickle.load(f) 6 | return list(data_list) 7 | 8 | def get_column(table, column_num): 9 | return [r[column_num] for r in table] 10 | 11 | def get_derivative_from_data(position_data, time_data): 12 | """ 13 | Calculates a list of speeds from position_data and 14 | time_data. 15 | 16 | Arguments: 17 | position_data - a list of values corresponding to 18 | vehicle position 19 | 20 | time_data - a list of values (equal in length to 21 | position_data) which give timestamps for each 22 | position measurement 23 | 24 | Returns: 25 | speeds - a list of values (which is shorter 26 | by ONE than the input lists) of speeds. 27 | """ 28 | # 1. Check to make sure the input lists have same length 29 | if len(position_data) != len(time_data): 30 | raise(ValueError, "Data sets must have same length") 31 | 32 | # 2. Prepare empty list of speeds 33 | speeds = [] 34 | 35 | # 3. Get first values for position and time 36 | previous_position = position_data[0] 37 | previous_time = time_data[0] 38 | 39 | # 4. Begin loop through all data EXCEPT first entry 40 | for i in range(1, len(position_data)): 41 | 42 | # 5. get position and time data for this timestamp 43 | position = position_data[i] 44 | time = time_data[i] 45 | 46 | # 6. Calculate delta_x and delta_t 47 | delta_x = position - previous_position 48 | delta_t = time - previous_time 49 | 50 | # 7. Speed is slope. Calculate it and append to list 51 | speed = delta_x / delta_t 52 | speeds.append(speed) 53 | 54 | # 8. Update values for next iteration of the loop. 55 | previous_position = position 56 | previous_time = time 57 | 58 | return speeds -------------------------------------------------------------------------------- /4_8_Vehicle_Motion_and_Calculus/images/vmc_l1_plot1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/CVND_Localization_Exercises/f5d5e90e2548aba4a145dd31f352fa9da19244dc/4_8_Vehicle_Motion_and_Calculus/images/vmc_l1_plot1.png -------------------------------------------------------------------------------- /4_8_Vehicle_Motion_and_Calculus/parallel_park.pickle: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/CVND_Localization_Exercises/f5d5e90e2548aba4a145dd31f352fa9da19244dc/4_8_Vehicle_Motion_and_Calculus/parallel_park.pickle -------------------------------------------------------------------------------- /4_8_Vehicle_Motion_and_Calculus/sharp_left.pickle: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/CVND_Localization_Exercises/f5d5e90e2548aba4a145dd31f352fa9da19244dc/4_8_Vehicle_Motion_and_Calculus/sharp_left.pickle -------------------------------------------------------------------------------- /CODEOWNERS: -------------------------------------------------------------------------------- 1 | * @udacity/active-public-content -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2018 Udacity 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /Project_Landmark Detection/2. Omega and Xi, Constraints.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "## Omega and Xi\n", 8 | "\n", 9 | "To implement Graph SLAM, a matrix and a vector (omega and xi, respectively) are introduced. The matrix is square and labelled with all the robot poses (xi) and all the landmarks (Li). Every time you make an observation, for example, as you move between two poses by some distance `dx` and can relate those two positions, you can represent this as a numerical relationship in these matrices.\n", 10 | "\n", 11 | "It's easiest to see how these work in an example. Below you can see a matrix representation of omega and a vector representation of xi.\n", 12 | "\n", 13 | "\n", 14 | "\n", 15 | "Next, let's look at a simple example that relates 3 poses to one another. \n", 16 | "* When you start out in the world most of these values are zeros or contain only values from the initial robot position\n", 17 | "* In this example, you have been given constraints, which relate these poses to one another\n", 18 | "* Constraints translate into matrix values\n", 19 | "\n", 20 | "\n", 21 | "\n", 22 | "If you have ever solved linear systems of equations before, this may look familiar, and if not, let's keep going!\n", 23 | "\n", 24 | "### Solving for x\n", 25 | "\n", 26 | "To \"solve\" for all these x values, we can use linear algebra; all the values of x are in the vector `mu` which can be calculated as a product of the inverse of omega times xi.\n", 27 | "\n", 28 | "\n", 29 | "\n", 30 | "---\n", 31 | "**You can confirm this result for yourself by executing the math in the cell below.**\n" 32 | ] 33 | }, 34 | { 35 | "cell_type": "code", 36 | "execution_count": null, 37 | "metadata": { 38 | "collapsed": true 39 | }, 40 | "outputs": [], 41 | "source": [ 42 | "import numpy as np\n", 43 | "\n", 44 | "# define omega and xi as in the example\n", 45 | "omega = np.array([[1,0,0],\n", 46 | " [-1,1,0],\n", 47 | " [0,-1,1]])\n", 48 | "\n", 49 | "xi = np.array([[-3],\n", 50 | " [5],\n", 51 | " [3]])\n", 52 | "\n", 53 | "# calculate the inverse of omega\n", 54 | "omega_inv = np.linalg.inv(np.matrix(omega))\n", 55 | "\n", 56 | "# calculate the solution, mu\n", 57 | "mu = omega_inv*xi\n", 58 | "\n", 59 | "# print out the values of mu (x0, x1, x2)\n", 60 | "print(mu)" 61 | ] 62 | }, 63 | { 64 | "cell_type": "markdown", 65 | "metadata": {}, 66 | "source": [ 67 | "## Motion Constraints and Landmarks\n", 68 | "\n", 69 | "In the last example, the constraint equations, relating one pose to another were given to you. In this next example, let's look at how motion (and similarly, sensor measurements) can be used to create constraints and fill up the constraint matrices, omega and xi. Let's start with empty/zero matrices.\n", 70 | "\n", 71 | "\n", 72 | "\n", 73 | "This example also includes relationships between poses and landmarks. Say we move from x0 to x1 with a displacement `dx` of 5. Then we have created a motion constraint that relates x0 to x1, and we can start to fill up these matrices.\n", 74 | "\n", 75 | "\n", 76 | "\n", 77 | "In fact, the one constraint equation can be written in two ways. So, the motion constraint that relates x0 and x1 by the motion of 5 has affected the matrix, adding values for *all* elements that correspond to x0 and x1.\n", 78 | "\n", 79 | "---\n", 80 | "\n", 81 | "### 2D case\n", 82 | "\n", 83 | "In these examples, we've been showing you change in only one dimension, the x-dimension. In the project, it will be up to you to represent x and y positional values in omega and xi. One solution could be to create an omega and xi that are 2x larger that the number of robot poses (that will be generated over a series of time steps) and the number of landmarks, so that they can hold both x and y values for poses and landmark locations. I might suggest drawing out a rough solution to graph slam as you read the instructions in the next notebook; that always helps me organize my thoughts. Good luck!" 84 | ] 85 | } 86 | ], 87 | "metadata": { 88 | "kernelspec": { 89 | "display_name": "Python 3", 90 | "language": "python", 91 | "name": "python3" 92 | }, 93 | "language_info": { 94 | "codemirror_mode": { 95 | "name": "ipython", 96 | "version": 3 97 | }, 98 | "file_extension": ".py", 99 | "mimetype": "text/x-python", 100 | "name": "python", 101 | "nbconvert_exporter": "python", 102 | "pygments_lexer": "ipython3", 103 | "version": "3.6.3" 104 | } 105 | }, 106 | "nbformat": 4, 107 | "nbformat_minor": 2 108 | } 109 | -------------------------------------------------------------------------------- /Project_Landmark Detection/4. Zip Your Project Files and Submit.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "## Project Submission\n", 8 | "\n", 9 | "When you are ready to submit your project, meaning you have checked the [rubric](https://review.udacity.com/#!/rubrics/1428/view) and made sure that you have completed all tasks and answered all questions. Then you are ready to compress your files and submit your solution!\n", 10 | "\n", 11 | "The following steps assume:\n", 12 | "1. All cells have been *run* in Notebook 3 (and that progress has been saved).\n", 13 | "2. All questions in Notebook 3 have been answered.\n", 14 | "3. Your robot `sense` function in `robot_class.py` is complete.\n", 15 | "\n", 16 | "Please make sure all your work is saved before moving on. You do not need to change any code in the following cells; this code is to help you submit your project, only.\n", 17 | "\n", 18 | "---\n", 19 | "\n", 20 | "The first thing we'll do, is convert your notebooks into `.html` files; these files will save the output of each cell and any code/text that you have modified and saved in those notebooks. Note that the second notebook is not included because its completion does not affect whether you pass this project." 21 | ] 22 | }, 23 | { 24 | "cell_type": "code", 25 | "execution_count": null, 26 | "metadata": {}, 27 | "outputs": [], 28 | "source": [ 29 | "!jupyter nbconvert \"1. Robot Moving and Sensing.ipynb\"\n", 30 | "!jupyter nbconvert \"3. Landmark Detection and Tracking.ipynb\"" 31 | ] 32 | }, 33 | { 34 | "cell_type": "markdown", 35 | "metadata": {}, 36 | "source": [ 37 | "### Zip the project files\n", 38 | "\n", 39 | "Next, we'll zip these notebook files and your `robot_class.py` file into one compressed archive named `project3.zip`.\n", 40 | "\n", 41 | "After completing this step you should see this zip file appear in your home directory, where you can download it as seen in the image below, by selecting it from the list and clicking **Download**.\n", 42 | "\n", 43 | "\n" 44 | ] 45 | }, 46 | { 47 | "cell_type": "code", 48 | "execution_count": null, 49 | "metadata": {}, 50 | "outputs": [], 51 | "source": [ 52 | "!!apt-get -y update && apt-get install -y zip\n", 53 | "!zip project3.zip -r . -i@filelist.txt" 54 | ] 55 | }, 56 | { 57 | "cell_type": "markdown", 58 | "metadata": {}, 59 | "source": [ 60 | "### Submit Your Project\n", 61 | "\n", 62 | "After creating and downloading your zip file, click on the `Submit` button and follow the instructions for submitting your `project3.zip` file. Congratulations on completing this project and I hope you enjoyed it!" 63 | ] 64 | } 65 | ], 66 | "metadata": { 67 | "kernelspec": { 68 | "display_name": "Python 3", 69 | "language": "python", 70 | "name": "python3" 71 | }, 72 | "language_info": { 73 | "codemirror_mode": { 74 | "name": "ipython", 75 | "version": 3 76 | }, 77 | "file_extension": ".py", 78 | "mimetype": "text/x-python", 79 | "name": "python", 80 | "nbconvert_exporter": "python", 81 | "pygments_lexer": "ipython3", 82 | "version": "3.6.3" 83 | } 84 | }, 85 | "nbformat": 4, 86 | "nbformat_minor": 2 87 | } 88 | -------------------------------------------------------------------------------- /Project_Landmark Detection/filelist.txt: -------------------------------------------------------------------------------- 1 | 1. Robot Moving and Sensing.html 2 | 3. Landmark Detection and Tracking.html 3 | robot_class.py -------------------------------------------------------------------------------- /Project_Landmark Detection/helpers.py: -------------------------------------------------------------------------------- 1 | from robot_class import robot 2 | from math import * 3 | import random 4 | import numpy as np 5 | import matplotlib.pyplot as plt 6 | import seaborn as sns 7 | 8 | 9 | # -------- 10 | # this helper function displays the world that a robot is in 11 | # it assumes the world is a square grid of some given size 12 | # and that landmarks is a list of landmark positions(an optional argument) 13 | def display_world(world_size, position, landmarks=None): 14 | 15 | # using seaborn, set background grid to gray 16 | sns.set_style("dark") 17 | 18 | # Plot grid of values 19 | world_grid = np.zeros((world_size+1, world_size+1)) 20 | 21 | # Set minor axes in between the labels 22 | ax=plt.gca() 23 | cols = world_size+1 24 | rows = world_size+1 25 | 26 | ax.set_xticks([x for x in range(1,cols)],minor=True ) 27 | ax.set_yticks([y for y in range(1,rows)],minor=True) 28 | 29 | # Plot grid on minor axes in gray (width = 1) 30 | plt.grid(which='minor',ls='-',lw=1, color='white') 31 | 32 | # Plot grid on major axes in larger width 33 | plt.grid(which='major',ls='-',lw=2, color='white') 34 | 35 | # Create an 'o' character that represents the robot 36 | # ha = horizontal alignment, va = vertical 37 | ax.text(position[0], position[1], 'o', ha='center', va='center', color='r', fontsize=30) 38 | 39 | # Draw landmarks if they exists 40 | if(landmarks is not None): 41 | # loop through all path indices and draw a dot (unless it's at the car's location) 42 | for pos in landmarks: 43 | if(pos != position): 44 | ax.text(pos[0], pos[1], 'x', ha='center', va='center', color='purple', fontsize=20) 45 | 46 | # Display final result 47 | plt.show() 48 | 49 | 50 | # -------- 51 | # this routine makes the robot data 52 | # the data is a list of measurements and movements: [measurements, [dx, dy]] 53 | # collected over a specified number of time steps, N 54 | # 55 | def make_data(N, num_landmarks, world_size, measurement_range, motion_noise, 56 | measurement_noise, distance): 57 | 58 | # check that data has been made 59 | try: 60 | check_for_data(num_landmarks, world_size, measurement_range, motion_noise, measurement_noise) 61 | except ValueError: 62 | print('Error: You must implement the sense function in robot_class.py.') 63 | return [] 64 | 65 | complete = False 66 | 67 | r = robot(world_size, measurement_range, motion_noise, measurement_noise) 68 | r.make_landmarks(num_landmarks) 69 | 70 | while not complete: 71 | 72 | data = [] 73 | 74 | seen = [False for row in range(num_landmarks)] 75 | 76 | # guess an initial motion 77 | orientation = random.random() * 2.0 * pi 78 | dx = cos(orientation) * distance 79 | dy = sin(orientation) * distance 80 | 81 | for k in range(N-1): 82 | 83 | # collect sensor measurements in a list, Z 84 | Z = r.sense() 85 | 86 | # check off all landmarks that were observed 87 | for i in range(len(Z)): 88 | seen[Z[i][0]] = True 89 | 90 | # move 91 | while not r.move(dx, dy): 92 | # if we'd be leaving the robot world, pick instead a new direction 93 | orientation = random.random() * 2.0 * pi 94 | dx = cos(orientation) * distance 95 | dy = sin(orientation) * distance 96 | 97 | # collect/memorize all sensor and motion data 98 | data.append([Z, [dx, dy]]) 99 | 100 | # we are done when all landmarks were observed; otherwise re-run 101 | complete = (sum(seen) == num_landmarks) 102 | 103 | print(' ') 104 | print('Landmarks: ', r.landmarks) 105 | print(r) 106 | 107 | 108 | return data 109 | 110 | 111 | def check_for_data(num_landmarks, world_size, measurement_range, motion_noise, measurement_noise): 112 | # make robot and landmarks 113 | r = robot(world_size, measurement_range, motion_noise, measurement_noise) 114 | r.make_landmarks(num_landmarks) 115 | 116 | 117 | # check that sense has been implemented/data has been made 118 | test_Z = r.sense() 119 | if(test_Z is None): 120 | raise ValueError 121 | -------------------------------------------------------------------------------- /Project_Landmark Detection/images/constraints2D.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/CVND_Localization_Exercises/f5d5e90e2548aba4a145dd31f352fa9da19244dc/Project_Landmark Detection/images/constraints2D.png -------------------------------------------------------------------------------- /Project_Landmark Detection/images/download_ex.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/CVND_Localization_Exercises/f5d5e90e2548aba4a145dd31f352fa9da19244dc/Project_Landmark Detection/images/download_ex.png -------------------------------------------------------------------------------- /Project_Landmark Detection/images/initial_constraints.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/CVND_Localization_Exercises/f5d5e90e2548aba4a145dd31f352fa9da19244dc/Project_Landmark Detection/images/initial_constraints.png -------------------------------------------------------------------------------- /Project_Landmark Detection/images/motion_constraint.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/CVND_Localization_Exercises/f5d5e90e2548aba4a145dd31f352fa9da19244dc/Project_Landmark Detection/images/motion_constraint.png -------------------------------------------------------------------------------- /Project_Landmark Detection/images/omega_xi.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/CVND_Localization_Exercises/f5d5e90e2548aba4a145dd31f352fa9da19244dc/Project_Landmark Detection/images/omega_xi.png -------------------------------------------------------------------------------- /Project_Landmark Detection/images/omega_xi_constraints.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/CVND_Localization_Exercises/f5d5e90e2548aba4a145dd31f352fa9da19244dc/Project_Landmark Detection/images/omega_xi_constraints.png -------------------------------------------------------------------------------- /Project_Landmark Detection/images/robot_world.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/CVND_Localization_Exercises/f5d5e90e2548aba4a145dd31f352fa9da19244dc/Project_Landmark Detection/images/robot_world.png -------------------------------------------------------------------------------- /Project_Landmark Detection/images/solution.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/CVND_Localization_Exercises/f5d5e90e2548aba4a145dd31f352fa9da19244dc/Project_Landmark Detection/images/solution.png -------------------------------------------------------------------------------- /Project_Landmark Detection/robot_class.py: -------------------------------------------------------------------------------- 1 | from math import * 2 | import random 3 | 4 | 5 | ### ------------------------------------- ### 6 | # Below, is the robot class 7 | # 8 | # This robot lives in 2D, x-y space, and its motion is 9 | # pointed in a random direction, initially. 10 | # It moves in a straight line until it comes close to a wall 11 | # at which point it stops. 12 | # 13 | # For measurements, it senses the x- and y-distance 14 | # to landmarks. This is different from range and bearing as 15 | # commonly studied in the literature, but this makes it much 16 | # easier to implement the essentials of SLAM without 17 | # cluttered math. 18 | # 19 | class robot: 20 | 21 | # -------- 22 | # init: 23 | # creates a robot with the specified parameters and initializes 24 | # the location (self.x, self.y) to the center of the world 25 | # 26 | def __init__(self, world_size = 100.0, measurement_range = 30.0, 27 | motion_noise = 1.0, measurement_noise = 1.0): 28 | self.measurement_noise = 0.0 29 | self.world_size = world_size 30 | self.measurement_range = measurement_range 31 | self.x = world_size / 2.0 32 | self.y = world_size / 2.0 33 | self.motion_noise = motion_noise 34 | self.measurement_noise = measurement_noise 35 | self.landmarks = [] 36 | self.num_landmarks = 0 37 | 38 | 39 | # returns a positive, random float 40 | def rand(self): 41 | return random.random() * 2.0 - 1.0 42 | 43 | 44 | # -------- 45 | # move: attempts to move robot by dx, dy. If outside world 46 | # boundary, then the move does nothing and instead returns failure 47 | # 48 | def move(self, dx, dy): 49 | 50 | x = self.x + dx + self.rand() * self.motion_noise 51 | y = self.y + dy + self.rand() * self.motion_noise 52 | 53 | if x < 0.0 or x > self.world_size or y < 0.0 or y > self.world_size: 54 | return False 55 | else: 56 | self.x = x 57 | self.y = y 58 | return True 59 | 60 | 61 | # -------- 62 | # sense: returns x- and y- distances to landmarks within visibility range 63 | # because not all landmarks may be in this range, the list of measurements 64 | # is of variable length. Set measurement_range to -1 if you want all 65 | # landmarks to be visible at all times 66 | # 67 | 68 | ## TODO: paste your complete the sense function, here 69 | ## make sure the indentation of the code is correct 70 | def sense(self): 71 | ''' This function does not take in any parameters, instead it references internal variables 72 | (such as self.landamrks) to measure the distance between the robot and any landmarks 73 | that the robot can see (that are within its measurement range). 74 | This function returns a list of landmark indices, and the measured distances (dx, dy) 75 | between the robot's position and said landmarks. 76 | This function should account for measurement_noise and measurement_range. 77 | One item in the returned list should be in the form: [landmark_index, dx, dy]. 78 | ''' 79 | 80 | measurements = None 81 | 82 | ## TODO: iterate through all of the landmarks in a world 83 | 84 | ## TODO: For each landmark 85 | ## 1. compute dx and dy, the distances between the robot and the landmark 86 | ## 2. account for measurement noise by *adding* a noise component to dx and dy 87 | ## - The noise component should be a random value between [-1.0, 1.0)*measurement_noise 88 | ## - Feel free to use the function self.rand() to help calculate this noise component 89 | ## 3. If either of the distances, dx or dy, fall outside of the internal var, measurement_range 90 | ## then we cannot record them; if they do fall in the range, then add them to the measurements list 91 | ## as list.append([index, dx, dy]), this format is important for data creation done later 92 | 93 | ## TODO: return the final, complete list of measurements 94 | return measurements 95 | 96 | 97 | # -------- 98 | # make_landmarks: 99 | # make random landmarks located in the world 100 | # 101 | def make_landmarks(self, num_landmarks): 102 | self.landmarks = [] 103 | for i in range(num_landmarks): 104 | self.landmarks.append([round(random.random() * self.world_size), 105 | round(random.random() * self.world_size)]) 106 | self.num_landmarks = num_landmarks 107 | 108 | 109 | # called when print(robot) is called; prints the robot's location 110 | def __repr__(self): 111 | return 'Robot: [x=%.5f y=%.5f]' % (self.x, self.y) 112 | 113 | 114 | 115 | ####### END robot class ####### -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Computer Vision Nanodegree Program, Localization Exercises 2 | 3 | This repository contains code exercises and materials for Udacity's [Computer Vision Nanodegree](https://www.udacity.com/course/computer-vision-nanodegree--nd891) program. It consists of tutorial notebooks that demonstrate, or challenge you to complete, object motion and localization methods. These notebooks depend on a number of software packages to run, and so, we suggest that you create a local environment with these dependencies by following the instructions in the [linked set of instructions](https://github.com/udacity/CVND_Exercises/blob/master/README.md). 4 | --------------------------------------------------------------------------------