{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# EE16B: Homework 9"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#Run these to import all of the necessary files\n",
    "import numpy as np\n",
    "import scipy as sp\n",
    "import scipy.io as spio\n",
    "import scipy.signal as sig\n",
    "import scipy.io.wavfile as wf\n",
    "import matplotlib as plt\n",
    "from pylab import *\n",
    "import os\n",
    "import re\n",
    "\n",
    "# Plots graphs in the browser window\n",
    "%matplotlib inline"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "##Problem 1: Simulating State Space Models"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "###Part 1b: Stable Systems"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Simulate the given system as stated in the problem."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# YOUR CODE HERE #"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "###Part 1d: Unstable Systems"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Simulate the given system as stated in the problem."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# YOUR CODE HERE #"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "##Problem 2: Open-Loop UAV Control"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Based on the parameters given for the problem, write code below to simulate the UAV in response to a (noisy) step input of 4.9N."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "def step_uav(A,B,u_eff,x0):\n",
    "    ''' Steps the state of the UAV by one sample.\n",
    "    Inputs:\n",
    "        A: A 2x2 numpy array representing the update matrix A of the UAV state.\n",
    "        B: A 2x1 numpy array representing the update matrix B of the UAV state.\n",
    "        u_eff: A value representing the net force on the UAV (including gravity) for this timestep.\n",
    "        x0: A 2x1 numpy array representing the initial state of the UAV.  The first element\n",
    "            is position y, and the second elemement is velocity v.\n",
    "    Returns:\n",
    "        x1: A 2x1 numpy array representing the updated state of the UAV.  The first element\n",
    "            is position y, and the second elemement is velocity v.\n",
    "    '''\n",
    "    # YOUR CODE HERE #\n",
    "    \n",
    "def sim_uav(A,B,u_eff,x0,n):\n",
    "    ''' Iterates the state of the UAV and measures its position and velocity.\n",
    "    Inputs:\n",
    "        A: A 2x2 numpy array representing the update matrix A of the UAV state.\n",
    "        B: A 2x1 numpy array representing the update matrix B of the UAV state.\n",
    "        u_eff: A numpy array representing the net force on the UAV (including gravity)\n",
    "            for each timestep.  u_eff must be at least length n.\n",
    "        x0: A 2x1 numpy array representing the initial state of the UAV.  The first element\n",
    "            is position y, and the second elemement is velocity v.\n",
    "        n: The desired number of iterations.\n",
    "    Returns:\n",
    "        x: A 2xn numpy array representing time-series values of the UAV states.\n",
    "    '''\n",
    "    # YOUR CODE HERE #"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now call your functions and plot the results to simulate the behavior of the UAV."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "iter = 10000                                    # Simulate for 10 seconds\n",
    "init = np.array([[1],[0]])                      # Inital conditions: 1m height, 0m/s velocity\n",
    "u = np.full(iter,4.9) + 2*np.random.randn(iter) # Input: 4.9N, plus some unwanted noise\n",
    "u_eff = u - 4.9                                 # Subtract gravity\n",
    "\n",
    "Ts = 0.001\n",
    "\n",
    "# Define the A and B matrices\n",
    "# YOUR CODE HERE #\n",
    "# A = \n",
    "# B = \n",
    "\n",
    "# Call your function\n",
    "# YOUR CODE HERE #\n",
    "x = np.zeros((2,iter+1))\n",
    "\n",
    "# Plot the result\n",
    "t = np.linspace(0.0,iter*Ts,num=iter+1)\n",
    "plt.figure(1)\n",
    "plt.subplot(211)\n",
    "plt.plot(t,x[0,:])\n",
    "plt.xlabel('Time (s)'); plt.ylabel('Height (m)')\n",
    "plt.subplot(212)\n",
    "plt.plot(t,x[1,:])\n",
    "plt.xlabel('Time (s)'); plt.ylabel('Velocity (m/s)')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**<font color=\"red\">How well does the UAV maintain its initial position?**\n",
    "\n",
    "YOUR ANSWER HERE"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "##Problem 3: Closed-Loop UAV Control"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Based on the parameters given for the problem, write code below to simulate the UAV in closed-loop configuration."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "def step_uav_CL(A_CL,B_CL,u,x0):\n",
    "    ''' Steps the state of the UAV by one sample.\n",
    "    Inputs:\n",
    "        A_CL: A 2x2 numpy array representing the update matrix A of the UAV state.\n",
    "        B_CL: A 2x2 numpy array representing the update matrix B of the UAV state.\n",
    "        u: A 2x1 numpy array representing the input for this timestep.  The first element is\n",
    "            desired velocity v_d, and the second is the uncontrolled force mg (plus any noise).\n",
    "        x0: A 2x1 numpy array representing the initial state of the UAV.  The first element\n",
    "            is position y, and the second elemement is velocity v.\n",
    "    Returns:\n",
    "        x1: A 2x1 numpy array representing the updated state of the UAV.  The first element\n",
    "            is position y, and the second elemement is velocity v.\n",
    "    '''\n",
    "    # YOUR CODE HERE #\n",
    "    \n",
    "def sim_uav_CL(A_CL,B_CL,u,x0,n):\n",
    "    ''' Iterates the state of the UAV and measures its position and velocity.\n",
    "    Inputs:\n",
    "        A_CL: A 2x2 numpy array representing the update matrix A of the UAV state.\n",
    "        B_CL: A 2x2 numpy array representing the update matrix B of the UAV state.\n",
    "        u: A 2-row numpy array representing the inputs for each timestep.  u must be at least length n.\n",
    "        x0: A 2x1 numpy array representing the initial state of the UAV.  The first element\n",
    "            is position y, and the second elemement is velocity v.\n",
    "        n: The desired number of iterations.\n",
    "    Returns:\n",
    "        x: A 2xn numpy array representing time-series values of the UAV states.\n",
    "    '''\n",
    "    # YOUR CODE HERE #"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now call your functions and plot the results to simulate the behavior of the UAV."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "phase1 = 5000; phase2 = 200; phase3 = 4800      # Define step function for input vd\n",
    "vd = np.concatenate((np.zeros(phase1),np.full(phase2,5),np.zeros(phase3)))\n",
    "\n",
    "init = np.array([[1],[0]])                      # Inital conditions: 1m height, 0m/s velocity\n",
    "iter = phase1+phase2+phase3                     # Simulate for 10 seconds\n",
    "\n",
    "mg = np.full(iter,4.9) + 2*np.random.randn(iter) # Uncontrolled force: 4.9N, plus some unwanted noise\n",
    "\n",
    "u = np.vstack((vd,mg))                          # The input vector u is made up of vd and mg\n",
    "\n",
    "Ts = 0.001\n",
    "\n",
    "# Define the A_CL and B_CL matrices; use h = 200 N/(m/s)\n",
    "# YOUR CODE HERE #\n",
    "# A_CL = \n",
    "# B_CL = \n",
    "\n",
    "# Call your function\n",
    "# YOUR CODE HERE #\n",
    "x = np.zeros((2,iter+1))\n",
    "\n",
    "# Plot the result\n",
    "t = np.linspace(0.0,iter*Ts,num=iter+1)\n",
    "plt.figure(1,figsize=(12,6))\n",
    "plt.subplot(321)\n",
    "plt.plot(t[0:iter],vd)\n",
    "plt.xlabel('Time (s)'); plt.ylabel('Desired Velocity (m/s)')\n",
    "plt.subplot(323)\n",
    "plt.plot(t,x[0,:])\n",
    "plt.xlabel('Time (s)'); plt.ylabel('Height (m)')\n",
    "plt.subplot(325)\n",
    "plt.plot(t,x[1,:])\n",
    "plt.xlabel('Time (s)'); plt.ylabel('Velocity (m/s)')\n",
    "zoom_x=[4900,5300]\n",
    "plt.subplot(322)\n",
    "plt.plot(t[zoom_x[0]:zoom_x[1]],vd[zoom_x[0]:zoom_x[1]])\n",
    "plt.xlabel('Time (s)'); plt.ylabel('Desired Velocity (m/s)')\n",
    "plt.subplot(324)\n",
    "plt.plot(t[zoom_x[0]:zoom_x[1]],x[0,:][zoom_x[0]:zoom_x[1]])\n",
    "plt.xlabel('Time (s)'); plt.ylabel('Height (m)')\n",
    "plt.subplot(326)\n",
    "plt.plot(t[zoom_x[0]:zoom_x[1]],x[1,:][zoom_x[0]:zoom_x[1]])\n",
    "plt.xlabel('Time (s)'); plt.ylabel('Velocity (m/s)')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now repeat the experiment, but use a different value of $h$ to define new state matrices."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# Define the new A_CL and B_CL matrices; use h = 800 N/(m/s)\n",
    "# YOUR CODE HERE #\n",
    "# A_CL = \n",
    "# B_CL = \n",
    "\n",
    "# Call your function\n",
    "# YOUR CODE HERE #\n",
    "x = np.zeros((2,iter+1))\n",
    "\n",
    "# Plot the result\n",
    "t = np.linspace(0.0,iter*Ts,num=iter+1)\n",
    "plt.figure(1,figsize=(12,6))\n",
    "plt.subplot(321)\n",
    "plt.plot(t[0:iter],vd)\n",
    "plt.xlabel('Time (s)'); plt.ylabel('Desired Velocity (m/s)')\n",
    "plt.subplot(323)\n",
    "plt.plot(t,x[0,:])\n",
    "plt.xlabel('Time (s)'); plt.ylabel('Height (m)')\n",
    "plt.subplot(325)\n",
    "plt.plot(t,x[1,:])\n",
    "plt.xlabel('Time (s)'); plt.ylabel('Velocity (m/s)')\n",
    "zoom_x=[4900,5300]\n",
    "plt.subplot(322)\n",
    "plt.plot(t[zoom_x[0]:zoom_x[1]],vd[zoom_x[0]:zoom_x[1]])\n",
    "plt.xlabel('Time (s)'); plt.ylabel('Desired Velocity (m/s)')\n",
    "plt.subplot(324)\n",
    "plt.plot(t[zoom_x[0]:zoom_x[1]],x[0,:][zoom_x[0]:zoom_x[1]])\n",
    "plt.xlabel('Time (s)'); plt.ylabel('Height (m)')\n",
    "plt.subplot(326)\n",
    "plt.plot(t[zoom_x[0]:zoom_x[1]],x[1,:][zoom_x[0]:zoom_x[1]])\n",
    "plt.xlabel('Time (s)'); plt.ylabel('Velocity (m/s)')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**<font color=\"red\">How well does the closed-loop system maintain a velocity of 0m/s and a constant height for the first five seconds?**\n",
    "\n",
    "YOUR ANSWER HERE\n",
    "\n",
    "**<font color=\"red\">The same magnitude of unwanted noise was applied to both the open-loop and closed-loop system.  How does the response of the two systems to noise differ?**\n",
    "\n",
    "YOUR ANSWER HERE\n",
    "\n",
    "**<font color=\"red\">How responsive is the closed-loop system to the step change in desired velocity at $t=5$ seconds?**\n",
    "\n",
    "YOUR ANSWER HERE\n",
    "\n",
    "**<font color=\"red\">How does the UAV behavior change with different values of $h$?**\n",
    "\n",
    "YOUR ANSWER HERE"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.4.3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 0
}