{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# EE120 Minilab 4: The Smith Predictor"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Phillip Sandborn, Instructor: Ron Fearing"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<b>Some control systems have very long delays.</b> We've been studying systems with feedback, and examining the stability of such systems by understanding phase margin. In many control systems that we might build in the lab or in the field, we may have a system that is a long way away from the controller, introducing a significant delay in the feedback. We sometimes call this \"dead time\", and it can cause significant problems for a control system. In this lab, we'll work through an example of control with dead time, and try to accommodate it using a control architecture known as the \"Smith Predictor\". "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "print 4 + 5"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "%matplotlib inline\n",
    "from numpy import *\n",
    "import numpy as np\n",
    "import matplotlib.pyplot as plt"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Helper Function: Define Discrete-Time LPF Matrix Functions\n",
    "A low-pass filter's system function for continuous-time takes on the form: \n",
    "\n",
    "$H_{LPF}(s) = \\frac{k_p}{1+s\\tau}$\n",
    "\n",
    "It describes the differential equation relation:\n",
    "\n",
    "$ y(t) + \\tau \\frac{dy(t)}{dt} = k_p x(t)$\n",
    "\n",
    "In order to implement this relation in discrete-time, we implement $\\frac{dy(t)}{dt}$ as a <b>first-difference</b> relation, so that\n",
    "\n",
    "$ y[n] + \\tau \\frac{y[n]-y[n-1]}{T_s} = k_p x[n]$\n",
    "\n",
    "The \"constructLPF\" function takes arguments kp, tau, Ts, and N (number of samples in the input signal), and creates a matrix, F. If we have some input array, x, F operated on x should yield y. \n",
    "\n",
    "$ y = Fx$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "#Construct LPF FUNCTION\n",
    "def constructLPF(kp,tau,Ts,N):\n",
    "    ## STUDENT: construct F, low-pass filter matrix, using kp, tau, Ts, and N\n",
    "    \n",
    "    F = ;\n",
    "    ## End Student edits\n",
    "    return F\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Helper Function: Define Discrete-Time PI-controller Matrix Function\n",
    "A PI-controller's system function for continuous-time takes on the form: \n",
    "\n",
    "$C(s) = k_p + \\frac{k_pi}{s}$\n",
    "\n",
    "It describes the differential equation relation:\n",
    "\n",
    "$y(t) = k_p x(t) + k_i \\int x(t) dt$\n",
    "\n",
    "In order to implement this relation in discrete-time, we can use the trapezoidal rule to approximate the integral:\n",
    "\n",
    "$ y[n] = k_p x[n] + k_i \\frac{x[n]+x[n-1]}{2}T_s$\n",
    "\n",
    "The \"constructPIcontroller\" function takes arguments Kp, Ki, Ts, and N (number of samples in the input signal), and creates a matrix, C. If we have some input array, x, C operated on x should yield y. \n",
    "\n",
    "$ y = Cx$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "def constructPIcontroller(Kp1,Ki1,Ts,N):\n",
    "    ## STUDENT: construct C, PI controller matrix, using kp, tau, Ts, and N\n",
    "    C = ;\n",
    "    ## end student edits\n",
    "    return C"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Helper Function: Define Discrete-Time Delay Matrix Function\n",
    "A delay can be implemented in discrete time simply by padding a signal with zeros. In this code, we will apply the delay to the transfer functions, so that the output of a block will be delayed by a certain number of samples. \n",
    "\n",
    "The function \"matrixDelay\" should take an input transfer function, M, and pad the first some rows with zeros (truncating the last some rows). "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "def matrixDelay(M,delay_samples):\n",
    "    ## Student: construct D, a matrix that performs the same operation as M but with a sample delay [delay_samples]\n",
    "    D = ;\n",
    "    ## end student edits\n",
    "    return D"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Fundamentals: Simple Model of a Rover's Velocity"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The forward velocity of a robotic lunar module is modeled by the following differential equation: \n",
    "$v(t) = k_p u(t) - \\tau \\frac{dv(t)}{dt}$\n",
    "\n",
    "where we can specify $\\tau = 0.3s$ (a system time-constant), $u(t)$ is the input signal, and $k_p$ is the proportional gain associated with the input. \n",
    "\n",
    "Using $\\tau = 0.3s$ and $k_p = 0.5 m/s$, and the helper functions above, generate the unit-step response of the rover's velocity. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# Step response of non-delayed system\n",
    "# Construct a time-domain model and plot the step-response of this system. Use k_p=0.5, ?=0.3s. \n",
    "\n",
    "tau_r = 0.3; # Rover velocity time constant [s]\n",
    "k_r = 0.5; # Rover velocity gain [m/s]\n",
    "\n",
    "## STUDENT: choose appropriate values for Ts, N, \n",
    "\n",
    "Ts = ; # Sampling period [s]\n",
    "N = ; # Total number of samples to generate\n",
    "\n",
    "## end student edit\n",
    "\n",
    "t = np.linspace(0,(N-1)*Ts,N); # time vector\n",
    "# Create rover velocity transfer matrix:\n",
    "G1 = constructLPF(k_r,tau_r,Ts,N);\n",
    "\n",
    "# Construct unit step at t=0.\n",
    "unitStep = np.ones((N,1));\n",
    "roverSequence = np.concatenate([0.5*np.ones((np.round(N/5),1)),np.ones((np.round(N/5),1)),2.0*np.ones((np.round(N/5),1)),0*np.ones((np.round(N/5),1)),1.5*np.ones((np.round(N/5),1))]);\n",
    "\n",
    "## STUDENT: find rover velocity with unit step input or the input sequence above (or your own input sequence)\n",
    "\n",
    "# Model step response by multiplying unit step by rover velocity transfer\n",
    "roverVelocity1 = ; # Output velocity sequence\n",
    "\n",
    "## end student edit\n",
    "\n",
    "# Plot Rover velocity step response\n",
    "plt.plot(t,roverVelocity1,'g');\n",
    "plt.xlabel('Time (s)'); \n",
    "plt.ylabel('Rover Velocity (m/s)');\n",
    "plt.title('Step Response - Open Loop');\n",
    "plt.axis([0, t[-1], 0, 3])\n",
    "plt.grid()\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now, we can introduce a PI-controller for this system, and close the loop to control the system. Select values for Kp1 and Ki1 for the controller, so that the closed-loop system has a time constant of 0.1s.\n",
    "\n",
    "Simulate the step response of the closed-loop system by using the matrices constructed using the helper functions. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# PI CONTROLLER\n",
    "\n",
    "w_BW = 7; # desired open loop BW\n",
    "\n",
    "## STUDENT: choose appropriate values for PI controller gain, Kp1 and Ki1. \n",
    "Ki1 = ;\n",
    "Kp1 = ;\n",
    "## end student edit\n",
    "\n",
    "# Create PI-controller transfer matrix:\n",
    "C1 = constructPIcontroller(Kp1,Ki1,Ts,N);\n",
    "\n",
    "## STUDENT: find rover velocity with unit step input or the input sequence above (or your own input sequence)\n",
    "# Model step response by multiplying unit step by rover velocity transfer\n",
    "roverVelocity2 = ; # Output velocity sequence for PI controller closed loop\n",
    "## end student edit\n",
    "\n",
    "# Plot Rover velocity step response\n",
    "plt.plot(t,roverVelocity1,'g');\n",
    "plt.plot(t,roverVelocity2,'b');\n",
    "plt.xlabel('Time (s)'); \n",
    "plt.ylabel('Rover Velocity (m/s)');\n",
    "plt.title('Step Response - Closed Loop');\n",
    "plt.axis([0,t[-1],0,3]);\n",
    "plt.grid()\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# Examine the frequency response for the closed-loop system with PI controller.\n",
    "omega = np.logspace(-2,3,100);\n",
    "\n",
    "## STUDENT: construct a vector that represents the closed-loop system response, as a function of the variable, omega (above)\n",
    "## Use the variables, Kp1, Ki1, k_r, tau_r, from before\n",
    "C1_omega = ;\n",
    "G1_omega = ;\n",
    "H1_omega = ;\n",
    "## end student edit\n",
    "\n",
    "plt.semilogx(omega,20*np.log10(np.abs(H1_omega)));\n",
    "plt.xlabel('Frequency (rad/s)');\n",
    "plt.ylabel('Magnitude (dB)');\n",
    "plt.axis([min(omega),max(omega),-20,10]);\n",
    "plt.title('Frequency Response')\n",
    "plt.grid();\n",
    "plt.show();\n",
    "\n",
    "plt.semilogx(omega,np.angle(H1_omega)*180/np.pi);\n",
    "plt.xlabel('Frequency (rad/s)');\n",
    "plt.ylabel('Phase (deg)');\n",
    "plt.axis([min(omega),max(omega),-180,180]);\n",
    "plt.title('Phase Response')\n",
    "plt.grid();\n",
    "plt.show();"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now, let's introduce a delay into our rover system. \n",
    "\n",
    "The moon is ~384,400km from Earth. The speed of light in vaccuum is ~300,000 km/s. Use this information to calculate the time delay introduced into the lunar rover system that includes a link with Earth. \n",
    "\n",
    "Convert this time-delay into a sample-delay, and use the helper functions above to introduce delay into the rover system function, and plot the rover's step response. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# Step response including Earth-moon delay\n",
    "\n",
    "## STUDENT: calculate the correct sample delay and construct the \"delayed\" rover velocity response to unit step or rover sequence.\n",
    "delay_seconds = ;\n",
    "delay_samples = ;\n",
    "G2 = matrixDelay(G1,delay_samples);\n",
    "roverVelocity3 = ; # output response\n",
    "## end student edits\n",
    "\n",
    "plt.plot(t,roverVelocity1,'g');\n",
    "plt.plot(t,roverVelocity3,'r');\n",
    "plt.xlabel('Time (s)'); \n",
    "plt.ylabel('Rover Velocity (m/s)');\n",
    "plt.title('Step Response - Open Loop');\n",
    "plt.axis([0,t[-1],0,3]);\n",
    "plt.axis([0,t[-1],0,3]);\n",
    "plt.grid()\n",
    "plt.show()\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now, design a <b>new</b> PI-controller, for controlling the system with delay, in closed-loop operation. This implementation will be identical to the closed-loop simulation above, except for a different specification for the PI controller. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "## Using Controller\n",
    "\n",
    "## STUDENT: select an appropriate bandwidth, w_BW, and find new values Kp1_delay and Ki1_delay so that the system is stable \n",
    "## (it may have a less desirable response than the closed-loop system before)\n",
    "\n",
    "w_BW = ;\n",
    "# Design new values for Kp1 and Ki1\n",
    "Ki1_delay = ;\n",
    "Kp1_delay = ;\n",
    "\n",
    "# Create PI-controller transfer matrix:\n",
    "C1_delay = constructPIcontroller(Kp1_delay,Ki1_delay,Ts,N);\n",
    "\n",
    "# Model step response by multiplying matrices...\n",
    "roverVelocity4 = ;\n",
    "\n",
    "## end student edits\n",
    "\n",
    "# Plot Rover velocity step response\n",
    "plt.plot(t,roverVelocity2,'b');\n",
    "plt.plot(t,roverVelocity4,'b-.');\n",
    "plt.xlabel('Time (s)'); \n",
    "plt.ylabel('Rover Velocity (m/s)');\n",
    "plt.title('Step Response - Closed Loop');\n",
    "#plt.axis([0,t[-1],0,3]);\n",
    "plt.grid()\n",
    "plt.show()\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# Examine the frequency response for the closed-loop system with delay and PI controller.\n",
    "\n",
    "omega = np.logspace(-2,2,100);\n",
    "\n",
    "## STUDENT: construct a vector that represents the closed-loop system response, as a function of the variable, omega (above)\n",
    "C1_omega = ;\n",
    "G2_omega = ;\n",
    "H2_omega = ;\n",
    "## end student edits\n",
    "\n",
    "plt.semilogx(omega,20*np.log10(np.abs(H2_omega)),'b-.');\n",
    "plt.xlabel('Frequency (rad/s)');\n",
    "plt.ylabel('Magnitude (dB)');\n",
    "plt.axis([min(omega),max(omega),-20,10]);\n",
    "plt.title('Frequency Response')\n",
    "plt.grid();\n",
    "plt.show();\n",
    "\n",
    "plt.semilogx(omega,np.angle(H2_omega)*180/np.pi,'b-.');\n",
    "plt.xlabel('Frequency (rad/s)');\n",
    "plt.ylabel('Phase (deg)');\n",
    "plt.axis([min(omega),max(omega),-180,180]);\n",
    "plt.title('Phase Response')\n",
    "plt.grid();\n",
    "plt.show();"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Try different values of Kp1 to see what happens to the time- and the frequency-responses above."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## The Smith Predictor\n",
    "\n",
    "Now, we’ll construct a controller that implements an architecture known as the “Smith predictor,” which allows us to increase stability while maintaining a high system function bandwidth. The block diagram for the Smith predictor in our case is shown below. Our new control system requires knowledge about G2 without delay (which we have already modeled as G1), and the delay."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#Construct Smith Predictor architecture\n",
    "def constructSmith(delay_seconds_model,delay_seconds_system,kp_model,tau_model,kp_system,tau_system,kp_filt,tau_filt,Ts,N):\n",
    "    ## STUDENT: construct Smith Predictor matrix, using kp, tau, Ts, and N\n",
    "    \n",
    "    delay_samples_system = round(delay_seconds_system/Ts); # real delay\n",
    "    delay_samples_model = round(delay_seconds_model/Ts); # model of real delay\n",
    "    \n",
    "    G1 = constructLPF(kp_model,tau_model,Ts,N); # model of real system without delay\n",
    "    G2 = matrixDelay(constructLPF(kp_system,tau_system,Ts,N),delay_samples_system); # real system with delay\n",
    "    \n",
    "    w_BW = 7;\n",
    "    # Select values for Kp1 and Ki1 based on the internal model parameters kp_model and tau_model.\n",
    "    # This can be the same design as the closed loop PI controller with non-delayed system. \n",
    "    Ki1 = ;\n",
    "    Kp1 = ;\n",
    "    C1 = constructPIcontroller(Kp1,Ki1,Ts,N); # reconstruct PI controller\n",
    "    \n",
    "    F = constructLPF(kp_filt,tau_filt,Ts,N); # outer loop filter\n",
    "\n",
    "    smithMatrix = ; # Smith matrix response using C1, G1, G2, and F. \n",
    "    ## end student edits\n",
    "    \n",
    "    return smithMatrix"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "## Construct smith predictor in time domain\n",
    "# Case 0: matched moon delay, matched system parameters\n",
    "delay_seconds_system = delay_seconds; # delay in the real system\n",
    "delay_seconds_model = delay_seconds; # delay in the model\n",
    "kp_system = 0.5;\n",
    "tau_system = 0.3;\n",
    "kp_model = kp_system; # match the model to the real system\n",
    "tau_model = tau_system; # match the model to the real system\n",
    "\n",
    "## STUDENT: use the helper function for the smith predictor to create the smith matrix and find the output response to the unit step.\n",
    "S0 = constructSmith(delay_seconds_model,delay_seconds_system,kp_model,tau_model,kp_system,tau_system,1,0.1,Ts,N);\n",
    "roverVelocity0 = ;\n",
    "# end student edits\n",
    "\n",
    "plt.plot(t,roverVelocity0,'g');\n",
    "plt.xlabel('Time (s)'); \n",
    "plt.ylabel('Rover Velocity (m/s)');\n",
    "plt.title('Step Response - Closed Loop Smith Predictor');\n",
    "plt.axis([0,15,0.95,1.05]);\n",
    "plt.grid();\n",
    "plt.show();"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "## Case 1: overestimate moon delay by 1%, overestimate kp by 1%\n",
    "## STUDENT: try different over- and under-estimating delay_seconds_model and kp_model below\n",
    "\n",
    "## STUDENT: model the over- and under-estimates for this case below.\n",
    "delay_seconds_system = delay_seconds; # delay in the real system\n",
    "delay_seconds_model = delay_seconds_system*1.01; # delay in the model, overestimate by 1%\n",
    "\n",
    "kp_system = 0.5;\n",
    "tau_system = 0.3;\n",
    "kp_model = kp_system*1.01; # over-estimate the real kp of the system by 1%\n",
    "tau_model = tau_system; # match the model to the real system\n",
    "\n",
    "S1 = constructSmith(delay_seconds_model,delay_seconds_system,kp_model,tau_model,kp_system,tau_system,1,0.1,Ts,N);\n",
    "roverVelocity1 = ; # output velocity for case 1.\n",
    "## end student edits\n",
    "\n",
    "plt.plot(t,roverVelocity0,'g');\n",
    "plt.plot(t,roverVelocity1,'r');\n",
    "plt.xlabel('Time (s)'); \n",
    "plt.ylabel('Rover Velocity (m/s)');\n",
    "plt.title('Step Response - Closed Loop Smith Predictor');\n",
    "plt.axis([0,15,0.95,1.05]);\n",
    "plt.grid();\n",
    "plt.show();\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "To examine the inner loop's stability, we examine the open-loop frequency response in the next snippet."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "## Bode plot for inner loop (open loop response)\n",
    "\n",
    "omega = np.logspace(-1,2,1000);\n",
    "\n",
    "## STUDENT: create innerLoopOpen, vector over frequency representing the open-loop transfer function for the inner loop \n",
    "## Use a matched model for this bode plot (i.e. use kp_system and tau_system from before)\n",
    "C1_omega = ;\n",
    "G1_omega = ;\n",
    "innerLoopOpen = ;\n",
    "## end student edits\n",
    "\n",
    "plt.semilogx(omega,20*np.log10(np.abs(innerLoopOpen)));\n",
    "plt.xlabel('Frequency (rad/s)');\n",
    "plt.ylabel('Magnitude (dB)');\n",
    "plt.axis([min(omega),max(omega),-20,40]);\n",
    "plt.title('Frequency Response')\n",
    "plt.grid();\n",
    "plt.show();\n",
    "\n",
    "plt.semilogx(omega,np.angle(innerLoopOpen)*180/np.pi);\n",
    "plt.xlabel('Frequency (rad/s)');\n",
    "plt.ylabel('Phase (deg)');\n",
    "plt.axis([min(omega),max(omega),-100,-80]);\n",
    "plt.title('Phase Response')\n",
    "plt.grid();\n",
    "plt.show();"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "To examine the outer-loop's stability, examine the open-loop response (input: x, output: output of filter F)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "## Bode plot for outer loop (matched model)\n",
    "\n",
    "# outer loop system parameters\n",
    "kp_system = 0.5;\n",
    "tau_system = 0.3;\n",
    "kp_model = kp_system; # match the real system\n",
    "tau_model = tau_system; # match the real system\n",
    "delay_system_seconds = delay_seconds;\n",
    "delay_model_seconds = delay_system_seconds; # match the real system\n",
    "\n",
    "omega = np.logspace(-1,2,1000); # frequency vector\n",
    "kp_filt = 1; # filter gain\n",
    "tau_filt = 0.1; # filter time constant\n",
    "F = np.divide(kp_filt,(1+np.multiply(tau_filt,np.multiply(1j,omega)))); # filter F(jw)\n",
    "\n",
    "## STUDENT: create outerLoopOpen, vector over frequency representing the open-loop transfer function for the outer loop with inner loop closed.\n",
    "## Use the parameter specifications above and the filter, F. \n",
    "C1 = ; # PI Controller\n",
    "G1 = ; # internal model\n",
    "G2 = ; # real system with delay\n",
    "D_model = ; # Internal model delay\n",
    "outerLoopOpen = ; # outer loop, open transfer function\n",
    "## end student edits\n",
    "\n",
    "plt.semilogx(omega,20*np.log10(np.abs(outerLoopOpen)));\n",
    "plt.xlabel('Frequency (rad/s)');\n",
    "plt.ylabel('Magnitude (dB)');\n",
    "#plt.axis([min(omega),max(omega),-20,10]);\n",
    "plt.title('Frequency Response')\n",
    "plt.grid();\n",
    "plt.show();\n",
    "\n",
    "plt.semilogx(omega,np.angle(outerLoopOpen)*180/np.pi);\n",
    "plt.xlabel('Frequency (rad/s)');\n",
    "plt.ylabel('Phase (deg)');\n",
    "plt.axis([min(omega),max(omega),-180,180]);\n",
    "plt.title('Phase Response')\n",
    "plt.grid();\n",
    "plt.show();"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "## Bode plot for outer loop (un-matched model)\n",
    "\n",
    "## mismatch Case: underestimate moon delay by 1%, overestimate kp by 1%\n",
    "kp_system = 0.5;\n",
    "tau_system = 0.3;\n",
    "\n",
    "tau_model = tau_system; # match the real system\n",
    "delay_system_seconds = delay_seconds;\n",
    "\n",
    "omega = np.logspace(-1,2,1000);\n",
    "\n",
    "kp_filt = 1;\n",
    "tau_filt = 0.1;\n",
    "F = np.divide(kp_filt,(1+np.multiply(tau_filt,np.multiply(1j,omega))));\n",
    "\n",
    "## STUDENT: select model gain and model delay to overestimate gain by 1% and underestimate moon delay by 1%\n",
    "kp_model = kp_system; # overestimate the real kp\n",
    "delay_model_seconds = delay_system_seconds; # underestimate the real delay\n",
    "\n",
    "## STUDENT: create outerLoopOpen, vector over frequency representing the open-loop transfer function for the outer loop with inner loop closed.\n",
    "## (This would be the same as your previous implementation, but with the new kp_model and new delay_model_seconds)\n",
    "\n",
    "C1 = ; # PI Controller\n",
    "G1 = ; # internal model\n",
    "G2 = ; # real system with delay\n",
    "D_model = ; # Internal model delay\n",
    "outerLoopOpen = ; # outer loop, open transfer function\n",
    "\n",
    "## end student edits\n",
    "\n",
    "plt.semilogx(omega,20*np.log10(np.abs(outerLoopOpen)));\n",
    "plt.xlabel('Frequency (rad/s)');\n",
    "plt.ylabel('Magnitude (dB)');\n",
    "#plt.axis([min(omega),max(omega),-20,10]);\n",
    "plt.title('Frequency Response')\n",
    "plt.grid();\n",
    "plt.show();\n",
    "\n",
    "plt.semilogx(omega,np.angle(outerLoopOpen)*180/np.pi);\n",
    "plt.xlabel('Frequency (rad/s)');\n",
    "plt.ylabel('Phase (deg)');\n",
    "plt.axis([min(omega),max(omega),-180,180]);\n",
    "plt.title('Phase Response')\n",
    "plt.grid();\n",
    "plt.show();"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Try different values of tau_filt to improve the gain margin of this system. Use a good value of tau_filt in the next part."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "## What is the system response with a modified filter?\n",
    "## mismatch case: overestimate moon delay by 1%, overestimate kp by 1%\n",
    "\n",
    "delay_seconds = 384.4e6/3e8;\n",
    "delay_seconds_system = delay_seconds;\n",
    "delay_seconds_model = delay_seconds_system*1.01; # overestimate by 1%\n",
    "\n",
    "kp_system = 0.5;\n",
    "tau_system = 0.3;\n",
    "kp_model = kp_system*1.01; # overestimate by 1%\n",
    "tau_model = tau_system;\n",
    "\n",
    "## STUDENT: modify tau for the outer loop filter to change the output response\n",
    "kp_filt = 1;\n",
    "tau_filt = 0.1; # modify this to match what you used in the last part.\n",
    "S5 = constructSmith(delay_seconds_model,delay_seconds_system,kp_model,tau_model,kp_system,tau_system,kp_filt,tau_filt,Ts,N);\n",
    "\n",
    "roverVelocity5 = ;\n",
    "\n",
    "## end student edits\n",
    "plt.plot(t,roverVelocity0,'g');\n",
    "plt.plot(t,roverVelocity5,'r');\n",
    "plt.xlabel('Time (s)'); \n",
    "plt.ylabel('Rover Velocity (m/s)');\n",
    "plt.title('Step Response - Closed Loop Smith Predictor');\n",
    "plt.axis([0,15,0.95,1.05]);\n",
    "plt.grid();\n",
    "plt.show();\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "You can try other inputs other than the unit step. One is specified early in the code, called \"roverSequence\". You can replace \"unitStep\" in the code with \"roverSequence\" to see what happens. (You might also need to adjust axis limits on some plots to see the velocity input sequence properly.) You can also make your own input to see what happens. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "anaconda-cloud": {},
  "kernelspec": {
   "display_name": "Python [Root]",
   "language": "python",
   "name": "Python [Root]"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 0
}