├── .gitignore ├── LICENSE ├── README ├── bicycle.py ├── congctrl.py ├── cruise.py ├── example-3.15-queuing_systems.py ├── example-3.17-consensus.py ├── example-3.18-repressilator.py ├── example-3.19-fitzhugh_nagumo.py ├── example-3.4-predator_prey.py ├── example-5.13-genetic_switch.py ├── example-5.14-invpend_stabilized.py ├── example-5.16-predprey_bif.py ├── example-5.17-bicycle_stability.py ├── example-5.18-noise_cancel.py ├── example-5.8-tanker_stabilty.py ├── example-5.9-limit_cycle_stability.py ├── example-6.10-compartment_response.py ├── example-6.8-opamp_bandpass.py ├── example-6.9-afm_freqresp.py ├── example-7.4-steering_place.py ├── example-8.10-steering_gainsched.py ├── example-template.py ├── fbs.py ├── figure-1.11-cruise_robustness.py ├── figure-1.18-airfuel_selectors.py ├── figure-2.11-2dof_stepresp.py ├── figure-2.12,14-static_nlsys.py ├── figure-2.19-posfbk_saturation.py ├── figure-2.8-PI_step_responses.py ├── figure-2.9-secord_stepresp.py ├── figure-3.11-spring_mass_simulation.py ├── figure-3.12-frequency_response.py ├── figure-3.2-state_model.py ├── figure-3.4-io_response.py ├── figure-4.12-congctrl_eqplot.py ├── figure-4.13-congctrl_tcpsim.py ├── figure-4.20-predprey_ctstime.py ├── figure-5-6-lyapunov_stability.py ├── figure-5.1-damposc_response.py ├── figure-5.10-congctrl_dynamics.py ├── figure-5.11-invpend_linearized.py ├── figure-5.3-phase_portraits.py ├── figure-5.4-invpend_phaseplot.py ├── figure-5.5-limit_cycle.py ├── figure-5.7-stable_eqpt.py ├── figure-5.8-asystable_eqpt.py ├── figure-5.9-unstable_eqpt.py ├── figure-6.1-superposition.py ├── figure-6.14-cruise_linearized.py ├── figure-6.5-modes.py ├── predprey.py ├── run_all.sh ├── springmass.py ├── steering.py └── template.py /.gitignore: -------------------------------------------------------------------------------- 1 | *~ 2 | *.png 3 | *.eps 4 | __pycache__ 5 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2021, Richard Murray 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /README: -------------------------------------------------------------------------------- 1 | README for Feedback Systems 2e Python files 2 | RMM, 20 Jun 2021 3 | 4 | This directory contains Python code for the examples and selected figures 5 | in the second edition of Feedback Systems by Astrom and Murray. 6 | 7 | The file corresponding to each example (or figure, if there is not a 8 | corresponding example) has the form 9 | 10 | example-N.m-short_title.py 11 | figure-N.m-short_title.py 12 | 13 | where 'N' is the chapter number and 'm' is the example number. If you 14 | run the file in Python, it will produce the figures that appear in the 15 | text. Some figures may look slightly different due to Matplotlib 16 | formatting, and depending on your environment you may need to enter 17 | `plt.show()` to tell matplotlib to show the figure. 18 | 19 | These examples use the Python Control Systems Library (python-control), 20 | which is available via PIP, Conda, or GitHub. See python-control.org 21 | for more information. 22 | 23 | Several modules are included that are imported in multiple examples and 24 | figures: 25 | 26 | * cruise.py: vehicle dynamics and PI controller 27 | * fbs.py: FBS plotting customizations (to match the style of the text) 28 | 29 | Release notes 30 | ------------- 31 | 16 Nov 2024, RMM: updated to use control-0.10.1 32 | -------------------------------------------------------------------------------- /bicycle.py: -------------------------------------------------------------------------------- 1 | # bicycle.py - bicycle dynamics 2 | # RMM, 24 Nov 2024 (from KJA) 3 | # 4 | # Bicyle dynamics 5 | # 6 | # This model describes the dynamics of a bicycle with the feature that one 7 | # of its key properties is due to a feedback mechanism that is created by 8 | # the design of the front fork. This model is described in more detail in 9 | # Section 4.2 of FBS2e. 10 | # 11 | # MATLAB header (bicycle_stabplot.m) 12 | # % Linearized 4th order model and analysis of eigenvalues 13 | # % Equations based on Schwab et al 2004 14 | # % Run bicycleparameters first 15 | # % kja 040611 16 | # % Parameters of a bicycle model 17 | # % kja 040613 18 | # % Basic data is given by 26 parameters 19 | 20 | import numpy as np 21 | import control as ct 22 | from math import pi 23 | 24 | import numpy as np 25 | 26 | # Acceleration of gravity [m/s^2] 27 | g = 9.81 28 | 29 | # Wheel base [m] 30 | b = 1.00 31 | 32 | # Trail [m] 33 | c = 0.08 34 | 35 | # Wheel radii 36 | Rrw = 0.35 37 | Rfw = 0.35 38 | 39 | # Head angle [radians] 40 | lambda_angle = np.pi * 70 / 180 41 | 42 | # Rear frame mass [kg], center of mass [m], and inertia tensor [kgm^2] 43 | mrf = 87 44 | xrf = 0.491586 45 | zrf = 1.028138 46 | Jxxrf = 3.283666 47 | Jxzrf = 0.602765 48 | Jyyrf = 3.8795952 49 | Jzzrf = 0.565929 50 | 51 | # Front frame mass [kg], center of mass [m], and inertia tensor [kgm^2] 52 | mff = 2 53 | xff = 0.866 54 | zff = 0.676 55 | Jxxff = 0.08 56 | Jxzff = -0.02 57 | Jyyff = 0.07 58 | Jzzff = 0.02 59 | 60 | # Rear wheel mass [kg], center of mass [m], and inertia tensor [kgm^2] 61 | mrw = 1.5 62 | Jxxrw = 0.07 63 | Jyyrw = 0.14 64 | 65 | # Front wheel mass [kg], center of mass [m], and inertia tensor [kgm^2] 66 | mfw = 1.5 67 | Jxxfw = 0.07 68 | Jyyfw = 0.14 69 | 70 | # Auxiliary variables 71 | xrw = 0 72 | zrw = Rrw 73 | xfw = b 74 | zfw = Rfw 75 | Jzzrw = Jxxrw 76 | Jzzfw = Jxxfw 77 | 78 | # Total mass 79 | mt = mrf + mrw + mff + mfw 80 | 81 | # Center of mass 82 | xt = (mrf * xrf + mrw * xrw + mff * xff + mfw * xfw) / mt 83 | zt = (mrf * zrf + mrw * zrw + mff * zff + mfw * zfw) / mt 84 | 85 | # Inertia tensor components 86 | Jxxt = ( 87 | Jxxrf + mrf * zrf**2 + 88 | Jxxrw + mrw * zrw**2 + 89 | Jxxff + mff * zff**2 + 90 | Jxxfw + mfw * zfw**2 91 | ) 92 | Jxzt = ( 93 | Jxzrf + mrf * xrf * zrf + 94 | mrw * xrw * zrw + 95 | Jxzff + mff * xff * zff + 96 | mfw * xfw * zfw 97 | ) 98 | Jzzt = ( 99 | Jzzrf + mrf * xrf**2 + 100 | Jzzrw + mrw * xrw**2 + 101 | Jzzff + mff * xff**2 + 102 | Jzzfw + mfw * xfw**2 103 | ) 104 | 105 | # Front frame parameters 106 | mf = mff + mfw 107 | xf = (mff * xff + mfw * xfw) / mf 108 | zf = (mff * zff + mfw * zfw) / mf 109 | 110 | Jxxf = ( 111 | Jxxff + mff * (zff - zf)**2 + 112 | Jxxfw + mfw * (zfw - zf)**2 113 | ) 114 | Jxzf = ( 115 | Jxzff + mff * (xff - xf) * (zff - zf) + 116 | mfw * (xfw - xf) * (zfw - zf) 117 | ) 118 | Jzzf = ( 119 | Jzzff + mff * (xff - xf)**2 + 120 | Jzzfw + mfw * (xfw - xf)**2 121 | ) 122 | 123 | # Auxiliary variables 124 | d = (xf - b - c) * np.sin(lambda_angle) + zf * np.cos(lambda_angle) 125 | Fll = ( 126 | mf * d**2 + 127 | Jxxf * np.cos(lambda_angle)**2 + 128 | 2 * Jxzf * np.sin(lambda_angle) * np.cos(lambda_angle) + 129 | Jzzf * np.sin(lambda_angle)**2 130 | ) 131 | Flx = mf * d * zf + Jxxf * np.cos(lambda_angle) + Jxzf * np.sin(lambda_angle) 132 | Flz = mf * d * xf + Jxzf * np.cos(lambda_angle) + Jzzf * np.sin(lambda_angle) 133 | gamma = c * np.sin(lambda_angle) / b 134 | Sr = Jyyrw / Rrw 135 | Sf = Jyyfw / Rfw 136 | St = Sr + Sf 137 | Su = mf * d + gamma * mt * xt 138 | 139 | # Matrices for the linearized fourth-order model 140 | M = np.array([ 141 | [Jxxt, -Flx - gamma * Jxzt], 142 | [-Flx - gamma * Jxzt, Fll + 2 * gamma * Flz + gamma**2 * Jzzt] 143 | ]) 144 | 145 | K0 = np.array([ 146 | [-mt * g * zt, g * Su], 147 | [g * Su, -g * Su * np.cos(lambda_angle)] 148 | ]) 149 | 150 | K2 = np.array([ 151 | [0, -(St + mt * zt) * np.sin(lambda_angle) / b], 152 | [0, (Su + Sf * np.cos(lambda_angle)) * np.sin(lambda_angle) / b] 153 | ]) 154 | 155 | c12 = gamma * St + Sf * np.sin(lambda_angle) \ 156 | + Jxzt * np.sin(lambda_angle) / b + gamma * mt * zt 157 | c22 = Flz * np.sin(lambda_angle) / b \ 158 | + gamma * (Su + Jzzt * np.sin(lambda_angle) / b) 159 | 160 | C = np.array([ 161 | [0, -c12], 162 | [gamma * St + Sf * np.sin(lambda_angle), c22] 163 | ]) 164 | 165 | def whipple_A(v0): 166 | return np.block([ 167 | [np.zeros((2, 2)), np.eye(2)], 168 | [-np.linalg.inv(M) @ (K0 + K2 * v0**2), -np.linalg.inv(M) @ C * v0] 169 | ]) 170 | -------------------------------------------------------------------------------- /congctrl.py: -------------------------------------------------------------------------------- 1 | # congctrl.py - Congestion control dynamics and control 2 | # RMM, 11 Jul 2006 (from MATLAB) 3 | # 4 | # Congestion control dynamics 5 | # 6 | # This model implements the congestion control dynamics described in 7 | # the text. We assume that we have N identical sources and 1 router. 8 | # 9 | # To allow the model to be used in a variety of ways, we 10 | # allow the number of states and the number of sources to be set 11 | # independently. The length of the state vector, M+1, is used to determine 12 | # the number of simulated sources, with each source representing N/M 13 | # sources. 14 | 15 | import numpy as np 16 | import control as ct 17 | 18 | # Congestion control dynamics 19 | def _congctrl_update(t, x, u, params): 20 | # Number of sources per state of the simulation 21 | M = x.size - 1 22 | 23 | # Remaining parameters 24 | N = params.get('N', M) # number of sources 25 | rho = params.get('rho', 2e-4) # RED parameter = pbar / (bupper-blower) 26 | c = params.get('c', 10) # link capacity (Mp/ms) 27 | 28 | # Compute the derivative (last state = bdot) 29 | return np.append( 30 | c / x[M] - (rho * c) * (1 + (x[:-1]**2) / 2), 31 | N/M * np.sum(x[:-1]) * c / x[M] - c) 32 | 33 | 34 | # Function to define an I/O system 35 | def create_iosystem(M, N=60, rho=2e-4, c=10): 36 | #! TODO: Check to make sure M and N are compatible 37 | return ct.nlsys( 38 | _congctrl_update, None, states=M+1, 39 | params={'N': N, 'rho': rho, 'c': c}) 40 | -------------------------------------------------------------------------------- /cruise.py: -------------------------------------------------------------------------------- 1 | # cruise.py - Cruise control dynamics and control 2 | # RMM, 20 Jun 2021 3 | 4 | import numpy as np 5 | import matplotlib.pyplot as plt 6 | from math import pi 7 | import control as ct 8 | 9 | # 10 | # Vehicle model 11 | # 12 | # The dynamics for this system are implemented using the I/O systems module 13 | # in python-control. This model is described in detail in Section 4.1 of 14 | # FBS2e. 15 | # 16 | 17 | # Engine model 18 | def motor_torque(omega, params={}): 19 | # Set up the system parameters 20 | Tm = params.get('Tm', 190.) # engine torque constant 21 | omega_m = params.get('omega_m', 420.) # peak engine angular speed 22 | beta = params.get('beta', 0.4) # peak engine rolloff 23 | 24 | return np.clip(Tm * (1 - beta * (omega/omega_m - 1)**2), 0, None) 25 | 26 | 27 | # Vehicle dynamics 28 | def vehicle_update(t, x, u, params={}): 29 | """Vehicle dynamics for cruise control system. 30 | 31 | Parameters 32 | ---------- 33 | x : array 34 | System state: car velocity in m/s 35 | u : array 36 | System input: [throttle, gear, road_slope], where throttle is 37 | a float between 0 and 1, gear is an integer between 1 and 5, 38 | and road_slope is in rad. 39 | 40 | Returns 41 | ------- 42 | float 43 | Vehicle acceleration 44 | 45 | """ 46 | from math import copysign, sin 47 | sign = lambda x: copysign(1, x) # define the sign() function 48 | 49 | # Set up the system parameters 50 | m = params.get('m', 1600.) # vehicle mass, kg 51 | g = params.get('g', 9.8) # gravitational constant, m/s^2 52 | Cr = params.get('Cr', 0.01) # coefficient of rolling friction 53 | Cd = params.get('Cd', 0.32) # drag coefficient 54 | rho = params.get('rho', 1.3) # density of air, kg/m^3 55 | A = params.get('A', 2.4) # car area, m^2 56 | alpha = params.get( 57 | 'alpha', [40, 25, 16, 12, 10]) # gear ratio / wheel radius 58 | 59 | # Define variables for vehicle state and inputs 60 | v = x[0] # vehicle velocity 61 | throttle = np.clip(u[0], 0, 1) # vehicle throttle 62 | gear = u[1] # vehicle gear 63 | theta = u[2] # road slope 64 | 65 | # Force generated by the engine 66 | 67 | omega = alpha[int(gear)-1] * v # engine angular speed 68 | F = alpha[int(gear)-1] * motor_torque(omega, params) * throttle 69 | 70 | # Disturbance forces 71 | # 72 | # The disturbance force Fd has three major components: Fg, the forces due 73 | # to gravity; Fr, the forces due to rolling friction; and Fa, the 74 | # aerodynamic drag. 75 | 76 | # Letting the slope of the road be \theta (theta), gravity gives the 77 | # force Fg = m g sin \theta. 78 | 79 | Fg = m * g * sin(theta) 80 | 81 | # A simple model of rolling friction is Fr = m g Cr sgn(v), where Cr is 82 | # the coefficient of rolling friction and sgn(v) is the sign of v (±1) or 83 | # zero if v = 0. 84 | 85 | Fr = m * g * Cr * sign(v) 86 | 87 | # The aerodynamic drag is proportional to the square of the speed: Fa = 88 | # 1/2 \rho Cd A |v| v, where \rho is the density of air, Cd is the 89 | # shape-dependent aerodynamic drag coefficient, and A is the frontal area 90 | # of the car. 91 | 92 | Fa = 1/2 * rho * Cd * A * abs(v) * v 93 | 94 | # Final acceleration on the car 95 | Fd = Fg + Fr + Fa 96 | dv = (F - Fd) / m 97 | 98 | return dv 99 | 100 | # Vehicle input/output model 101 | vehicle_dynamics = ct.nlsys( 102 | vehicle_update, None, name='vehicle', 103 | inputs = ['u', 'gear', 'theta'], outputs = ['v'], states=['v']) 104 | 105 | # 106 | # PI controller 107 | # 108 | # The controller for the system is designed in Example XX, but is included 109 | # here since it is needed for some earlier examples. 110 | # 111 | 112 | # Nominal controller design for remaining analyses 113 | # Construct a PI controller with rolloff, as a transfer function 114 | Kp = 0.5 # proportional gain 115 | Ki = 0.1 # integral gain 116 | PI_control = ct.tf( 117 | [Kp, Ki], [1, 0.01*Ki/Kp], name='control', inputs='u', outputs='y') 118 | 119 | cruise_PI = ct.interconnect( 120 | (vehicle_dynamics, PI_control), name='cruise', 121 | connections = [['control.u', '-vehicle.v'], ['vehicle.u', 'control.y']], 122 | inplist = ['control.u', 'vehicle.gear', 'vehicle.theta'], 123 | inputs = ['vref', 'gear', 'theta'], 124 | outlist = ['vehicle.v', 'vehicle.u'], outputs = ['v', 'u']) 125 | -------------------------------------------------------------------------------- /example-3.15-queuing_systems.py: -------------------------------------------------------------------------------- 1 | # example-3.15-queuing_systems.py - Queuing system modeling 2 | # RMM, 29 Aug 2021 3 | # 4 | # Figure 3.22: Queuing dynamics. (a) The steady-state queue length as a 5 | # function of $\lambda/\mu_{max}$. (b) The behavior of the queue length when 6 | # there is a temporary overload in the system. The solid line shows a 7 | # realization of an event-based simulation, and the dashed line shows the 8 | # behavior of the flow model (3.33). The maximum service rate is $\mu_{max} 9 | # = 1$, and the arrival rate starts at $\lambda = 0.5$. The arrival rate is 10 | # increased to $\lambda = 4$ at time 20, and it returns to $\lambda =0.5$ at 11 | # time 25. 12 | # 13 | 14 | import control as ct 15 | import numpy as np 16 | import matplotlib.pyplot as plt 17 | 18 | # Queing parameters 19 | 20 | # Queuing system model (KJA, 2006) 21 | def queuing_model(t, x, u, params={}): 22 | # Define default parameters 23 | mu = params.get('mu', 1) 24 | 25 | # Get the current load 26 | lambda_ = u 27 | 28 | # Return the change in queue size 29 | return np.array(lambda_ - mu * x[0] / (1 + x[0])) 30 | 31 | # Create I/O system representation 32 | queuing_sys = ct.nlsys( 33 | updfcn=queuing_model, inputs=1, outputs=1, states=1) 34 | 35 | # Set up the plotting grid to match the layout in the book 36 | fig = plt.figure(constrained_layout=True) 37 | gs = fig.add_gridspec(3, 2) 38 | 39 | # 40 | # (a) The steady-state queue length as a function of $\lambda/\mu_{max}$. 41 | # 42 | 43 | fig.add_subplot(gs[0, 0]) # first row, first column 44 | 45 | # Steady state queue length 46 | x = np.linspace(0.01, 0.99, 100) 47 | plt.plot(x, x / (1 - x), 'b-') 48 | 49 | # Label the plot 50 | plt.xlabel(r"Service rate excess $\lambda/\mu_{max}$") 51 | plt.ylabel(r"Queue length $x_{e}$") 52 | plt.title("Steady-state queue length") 53 | 54 | # 55 | # (b) The behavior of the queue length when there is a temporary overload 56 | # in the system. The solid line shows a realization of an event-based 57 | # simulation, and the dashed line shows the behavior of the flow model 58 | # (3.33). The maximum service rate is $\mu_{max} = 1$, and the arrival 59 | # rate starts at $\lambda = 0.5$. The arrival rate is increased to $\lambda 60 | # = 4$ at time 20, and it returns to $\lambda =0.5$ at time 25. 61 | # 62 | 63 | fig.add_subplot(gs[0, 1]) # first row, first column 64 | 65 | # Construct the loading condition 66 | t = np.linspace(0, 80, 100) 67 | 68 | u =np.ones_like(t) * 0.5 69 | u[t <= 25] = 4 70 | u[t < 20] = 0.5 71 | 72 | # Simulate the system dynamics 73 | response = ct.input_output_response(queuing_sys, t, u) 74 | 75 | # Plot the results 76 | plt.plot(response.time, response.outputs, 'b-') 77 | 78 | # Label the plot 79 | plt.xlabel("Time $t$ [s]") 80 | plt.ylabel(r"Queue length $x_{e}$") 81 | plt.title("Overload condition") 82 | 83 | # Save the figure 84 | plt.savefig("figure-3.22-queuing_dynamics.png", bbox_inches='tight') 85 | -------------------------------------------------------------------------------- /example-3.17-consensus.py: -------------------------------------------------------------------------------- 1 | # example-3.17-consensus.py - consensus protocols 2 | # RMM, 30 Aug 2021 3 | 4 | # Figure 3.24: Consensus protocols for sensor networks. (a) A simple sensor 5 | # net- work with five nodes. In this network, node 1 communicates with node 6 | # 2 and node 2 communicates with nodes 1, 3, 4, 5, etc. (b) A simulation 7 | # demonstrating the convergence of the consensus protocol (3.35) to the 8 | # average value of the initial conditions. 9 | 10 | import control as ct 11 | import numpy as np 12 | import matplotlib.pyplot as plt 13 | 14 | # 15 | # System dynamcis 16 | # 17 | 18 | # Construct the Laplacian corresponding to our network 19 | L = np.array([ 20 | [ 1, -1, 0, 0, 0], 21 | [-1, 4, -1, -1, -1], 22 | [ 0, -1, 2, -1, 0], 23 | [ 0, -1, -1, 2, 0], 24 | [ 0, -1, 0, 0, 1] 25 | ]) 26 | 27 | # Now generate the discrete time dynamics matrix 28 | gamma = 0.1 29 | A = np.eye(5) - gamma * L 30 | 31 | # Initial set of measurements for the system 32 | x0 = [10, 15, 25, 35, 40] 33 | 34 | # Create a discrete time system 35 | sys = ct.ss(A, np.zeros(5), np.zeros(5), 0, dt=True) 36 | 37 | # Set up the plotting grid to match the layout in the book 38 | fig = plt.figure(constrained_layout=True) 39 | gs = fig.add_gridspec(3, 2) 40 | 41 | # 42 | # (b) A simulation demonstrating the convergence of the consensus protocol 43 | # (3.35) to the average value of the initial conditions. 44 | # 45 | 46 | fig.add_subplot(gs[0, 1]) # first row, first column 47 | 48 | # Simulate the system 49 | response = ct.initial_response(sys, 40, x0) 50 | 51 | # Plot th3 results 52 | for i in range(response.nstates): 53 | plt.plot(response.time, response.states[i], 'b-') 54 | 55 | # Label the figure 56 | plt.xlabel("Iteration") 57 | plt.ylabel("Agent states $x_i$") 58 | plt.title("Consensus convergence") 59 | 60 | # Save the figure 61 | plt.savefig("figure-3.24-consensus_dynamics.png", bbox_inches='tight') 62 | -------------------------------------------------------------------------------- /example-3.18-repressilator.py: -------------------------------------------------------------------------------- 1 | # example-3.18-repressilator.py - Transcriptional regulation 2 | # RMM, 29 Aug 2021 3 | # 4 | # Figure 3.26: The repressilator genetic regulatory network. (a) A schematic 5 | # diagram of the repressilator, showing the layout of the genes in the 6 | # plasmid that holds the circuit as well as the circuit diagram 7 | # (center). (b) A simulation of a simple model for the repressilator, 8 | # showing the oscillation of the individual protein concentrations. 9 | # 10 | 11 | import control as ct 12 | import numpy as np 13 | import matplotlib.pyplot as plt 14 | 15 | # 16 | # Repressilator dynamics 17 | # 18 | # This function implements the basic model of the repressilator All 19 | # parameter values were taken from Nature. 2000 Jan 20; 403(6767):335-8. 20 | # 21 | # This model was developed by members of the 2003 Synthetic Biology Class 22 | # on Engineered Blinkers. 23 | # 24 | 25 | # Dynamics for the repressilator 26 | def repressilator(t, x, u, params): 27 | # store the state variables under more meaningful names 28 | mRNA_cI = x[0] 29 | mRNA_lacI = x[1] 30 | mRNA_tetR = x[2] 31 | protein_cI = x[3] 32 | protein_lacI = x[4] 33 | protein_tetR = x[5] 34 | 35 | # 36 | # set the parameter values 37 | # 38 | 39 | # set the max transcription rate in transcripts per second 40 | k_transcription_cI = params.get('k_transcription_cI', 0.5) 41 | k_transcription_lacI = params.get('k_transcription_lacI', 0.5) 42 | k_transcription_tetR = params.get('k_transcription_tetR', 0.5) 43 | 44 | # set the leakage transcription rate (ie transcription rate if 45 | # promoter region bound by repressor) in transcripts per second 46 | k_transcription_leakage = params.get('k_transcription_leakage', 5e-4) 47 | 48 | # Set the mRNA and protein degradation rates (per second) 49 | mRNA_half_life = params.get('mRNA_half_life', 120) # in seconds 50 | k_mRNA_degradation = np.log(2)/mRNA_half_life 51 | protein_half_life = params.get('protein_half_life', 600) # in seconds 52 | k_protein_degradation = np.log(2)/protein_half_life 53 | 54 | # proteins per transcript lifespan 55 | translation_efficiency = params.get('translation_efficiency', 20) 56 | average_mRNA_lifespan = 1/k_mRNA_degradation 57 | 58 | # proteins per transcript per sec 59 | k_translation = translation_efficiency/average_mRNA_lifespan 60 | 61 | # set the Hill coefficients of the repressors 62 | n_tetR = params.get('n_tetR', 2) 63 | n_cI = params.get('n_cI', 2) 64 | n_lacI = params.get('n_lacI', 2) 65 | 66 | # Set the dissociation constant for the repressors to their target promoters 67 | # in per molecule per second 68 | KM_tetR = params.get('KM_tetR', 40) 69 | KM_cI = params.get('KM_cI', 40) 70 | KM_lacI = params.get('KM_lacI', 40) 71 | 72 | # the differential equations governing the state variables: 73 | # mRNA concentration = transcription given repressor concentration - 74 | # mRNA degradation + transcription leakage 75 | dxdt = np.empty(6) 76 | dxdt[0] = k_transcription_cI/(1 + (protein_tetR / KM_tetR) ** n_tetR) - \ 77 | k_mRNA_degradation * mRNA_cI + k_transcription_leakage 78 | dxdt[1] = k_transcription_lacI/(1 + (protein_cI / KM_cI)**n_cI) - \ 79 | k_mRNA_degradation * mRNA_lacI + k_transcription_leakage 80 | dxdt[2] = k_transcription_tetR/(1 + (protein_lacI / KM_lacI) ** n_lacI) - \ 81 | k_mRNA_degradation * mRNA_tetR + k_transcription_leakage 82 | 83 | # protein concentration = translation - protein degradation 84 | dxdt[3] = k_translation*mRNA_cI - k_protein_degradation*protein_cI 85 | dxdt[4] = k_translation*mRNA_lacI - k_protein_degradation*protein_lacI 86 | dxdt[5] = k_translation*mRNA_tetR - k_protein_degradation*protein_tetR 87 | 88 | return dxdt 89 | 90 | # Define the system as an I/O system 91 | sys = ct.nlsys( 92 | updfcn=repressilator, outfcn=lambda t, x, u, params: x[3:], 93 | states=6, inputs=0, outputs=3) 94 | 95 | # Set up the plotting grid to match the layout in the book 96 | fig = plt.figure(constrained_layout=True) 97 | gs = fig.add_gridspec(2, 2) 98 | 99 | # 100 | # (b) A simulation of a simple model for the repressilator, showing the 101 | # oscillation of the individual protein concentrations. 102 | # 103 | 104 | fig.add_subplot(gs[0, 1]) # first row, second column 105 | 106 | # Initial conditions and time 107 | t = np.linspace(0, 20000, 1000) 108 | x0 = [1, 0, 0, 200, 0, 0] 109 | 110 | # Integrate the differential equation 111 | response = ct.input_output_response(sys, t, 0, x0) 112 | 113 | # Plot the results (protein concentrations) 114 | plt.plot(response.time/60, response.outputs[0], '-') 115 | plt.plot(response.time/60, response.outputs[1], '--') 116 | plt.plot(response.time/60, response.outputs[2], '-.') 117 | 118 | plt.axis([0, 300, 0, 5000]) 119 | plt.legend(("cI", "lacI", "tetR"), loc='upper right') 120 | 121 | plt.xlabel("Time [min]") # Axis labels 122 | plt.ylabel("Proteins per cell") 123 | plt.title("Repressilator simulation") # Plot title 124 | 125 | # Save the figure 126 | plt.savefig("figure-3.26-repressilator_dynamics.png", bbox_inches='tight') 127 | -------------------------------------------------------------------------------- /example-3.19-fitzhugh_nagumo.py: -------------------------------------------------------------------------------- 1 | # example-3.19-fitzhugh_nagumo.py - nerve cell dynamics 2 | # RMM, 30 Aug 2021 3 | # 4 | # Figure 3.28: Response of a neuron to a current input. The current 5 | # input is shown in (a) and the neuron voltage V in (b). The 6 | # simulation was done using the FitzHugh–Nagumo model (Exercise 3.11). 7 | 8 | import control as ct 9 | import numpy as np 10 | import matplotlib.pyplot as plt 11 | ct.use_fbs_defaults() 12 | 13 | # 14 | # System dynamics 15 | # 16 | 17 | # FitzHugh-Nagumo dynamics (from KJA) 18 | def fitzhugh_nagumo_dynamics(t, x, u, params): 19 | dx = np.zeros(3) 20 | 21 | # Get the system state 22 | V = x[0] 23 | R = x[1] 24 | 25 | # Compute the dim derivative 26 | dx[0] = 10 * (V - (V**3) / 3 - R + u[0]) 27 | dx[1] = 0.8 * (-R + 1.25 * V + 1.5) 28 | dx[2] = 1 29 | 30 | return dx 31 | 32 | # Set up an input/output system 33 | sys = ct.nlsys( 34 | updfcn=fitzhugh_nagumo_dynamics, states=3, inputs=1, outputs=3) 35 | 36 | # Set up the plotting grid to match the layout in the book 37 | fig = plt.figure(constrained_layout=True) 38 | gs = fig.add_gridspec(3, 2) 39 | 40 | # 41 | # (a) Current input 42 | # 43 | 44 | fig.add_subplot(gs[0, 0]) # first row, first column 45 | 46 | # Set up input stimulation 47 | t = np.linspace(0, 50, 500) 48 | u = np.zeros_like(t) 49 | u[t >= 5] = 1.5 # start of short input pulse 50 | u[t >= 6] = 0 # end of short input pulse 51 | u[t >= 30] = 1.5 # longer input pulse 52 | 53 | # Initial state 54 | x0 = [-1.5, -3/8, 0] 55 | 56 | response = ct.input_output_response(sys, t, u, x0) 57 | 58 | plt.plot(response.time, response.inputs[0]) 59 | plt.xlabel("Time $t$ [ms]") 60 | plt.ylabel("Current $I$ [mA]") 61 | plt.title("Input stimulation") 62 | 63 | # 64 | # (b) Neuron response 65 | # 66 | 67 | fig.add_subplot(gs[0, 1]) # first row, second column 68 | 69 | plt.plot(response.time, response.states[0]) 70 | plt.xlabel("Time $t$ [ms]") 71 | plt.ylabel("Voltage $V$ [mV]") 72 | plt.title("Neuron response") 73 | 74 | # Save the figure 75 | plt.savefig("figure-3.19-fitzhugh_nagumo.png", bbox_inches='tight') 76 | -------------------------------------------------------------------------------- /example-3.4-predator_prey.py: -------------------------------------------------------------------------------- 1 | # example-3.4-predator_prey.py - discrete-time simulation of predator–prey model 2 | # RMM, 15 May 2019 3 | # 4 | # Figure 3.8: Discrete-time simulation of the predator–prey model 5 | # (3.13). Using the parameters a = c = 0.014, bh(u) = 0.6, and dl = 6 | # 0.7 in equation (3.13), the period and magnitude of the lynx and 7 | # hare population cycles approximately match the data in Figure 3.7. 8 | 9 | import control as ct 10 | import numpy as np 11 | import matplotlib.pyplot as plt 12 | 13 | # Define the dynamics of the predator prey system 14 | def predprey(t, x, u, params): 15 | # Parameter setup 16 | a = params.get('a', 0.014) / 365. 17 | bh0 = params.get('bh0', 0.6) / 365. 18 | c = params.get('c', 0.014) / 365. 19 | dl = params.get('dl', 0.7) / 365. 20 | 21 | # Map the states into local variable names 22 | H = x[0] 23 | L = x[1] 24 | 25 | # Compute the input 26 | bhu = bh0 + u[0] 27 | 28 | # Compute the discrete updates 29 | dH = H + bhu * H - a * L * H 30 | dL = L + c * L * H - dl * L 31 | 32 | return [dH, dL] 33 | 34 | # Create a nonlinear I/O system (dt = days) 35 | io_predprey = ct.nlsys( 36 | predprey, None, inputs=['u'], outputs=['H', 'L'], 37 | states=['H', 'L'], name='predprey', dt=1/365) 38 | 39 | X0 = [10, 10] # Initial H, L 40 | T = np.linspace(1845, 1935, 90*365 + 1) # 90 years 41 | 42 | # Simulate the system 43 | response = ct.input_output_response(io_predprey, T, 0, X0) 44 | t, y = response.time, response.outputs 45 | 46 | # Downsample the responses to individual years 47 | yrs = t[::365] 48 | pop = y[:, ::365] 49 | 50 | # Plot the response 51 | plt.subplot(2, 1, 1) # Set the aspect ration to match the text 52 | 53 | plt.plot(yrs, pop[0], 'b-o', markersize=3) 54 | plt.plot(yrs, pop[1], 'r--o', markersize=3) 55 | plt.legend(["Hare", "Lynx"]) 56 | 57 | plt.xlabel("Year") 58 | plt.ylabel("Population") 59 | 60 | # Save the figure 61 | plt.savefig("figure-3.8-predator_prey.png", bbox_inches='tight') 62 | -------------------------------------------------------------------------------- /example-5.13-genetic_switch.py: -------------------------------------------------------------------------------- 1 | # example-5.13-genetic_switch.py - Genetic switch dynamics 2 | # RMM, 17 Nov 2024 3 | # 4 | # Figure 5.15: Stability of a genetic switch. The circuit diagram in (a) 5 | # represents two proteins that are each repressing the production of the 6 | # other. The inputs u1 and u2 interfere with this repression, allowing the 7 | # circuit dynamics to be modified. The equilibrium points for this circuit 8 | # can be determined by the intersection of the two curves shown in (b). 9 | # 10 | # Figure 5.16: Dynamics of a genetic switch. The phase portrait on the left 11 | # shows that the switch has three equilibrium points, corresponding to 12 | # protein A having a concentration greater than, equal to, or less than 13 | # protein B. The equilibrium point with equal protein concentrations is 14 | # unstable, but the other equilibrium points are stable. The simulation on 15 | # the right shows the time response of the system starting from two 16 | # different initial conditions. The initial portion of the curve 17 | # corresponds to initial concentrations z(0) = (1, 5) and converges to the 18 | # equilibrium point where z1e < z2e. At time t = 10, the concentrations are 19 | # perturbed by +2 in z1 and −2 in z2, moving the state into the region of 20 | # the state space whose solutions converge to the equilibrium point where 21 | # z2e < z1e. 22 | 23 | import control as ct 24 | import numpy as np 25 | import matplotlib.pyplot as plt 26 | from scipy.integrate import solve_ivp 27 | from scipy.optimize import fsolve 28 | ct.use_fbs_defaults() 29 | 30 | # 31 | # System dynamics 32 | # 33 | 34 | # Switch parameters 35 | genswitch_params = {'mu': 4, 'n': 2} 36 | 37 | def genswitch_dynamics(t, x, u, params): 38 | mu = params.get('mu') 39 | n = params.get('n') 40 | z1, z2 = x 41 | dz1 = mu / (1 + z2**n) - z1 42 | dz2 = mu / (1 + z1**n) - z2 43 | return [dz1, dz2] 44 | 45 | genswitch_model = ct.nlsys( 46 | genswitch_dynamics, None, states=2, inputs=None, params=genswitch_params) 47 | 48 | # 49 | # 5.15 (b) Equilibrium points 50 | # 51 | 52 | # Set up the plotting grid to match the layout in the book 53 | fig = plt.figure(constrained_layout=True) 54 | gs = fig.add_gridspec(2, 2) 55 | ax = fig.add_subplot(gs[0, 1]) 56 | 57 | # Generate nullcline 58 | mu, n = genswitch_params['mu'], genswitch_params['n'] 59 | u = np.linspace(0, 5, 100) 60 | f = mu / (1 + u**n) 61 | 62 | # Find equilibrium points 63 | def eq_func(z): 64 | return mu / (1 + z**2) - z 65 | 66 | eqpts = [fsolve(eq_func, 2)[0], fsolve(eq_func, 0)[0], fsolve(eq_func, -2)[0]] 67 | 68 | ax.plot(u, f, 'b-', label='$z_1, f(z_1)$') 69 | ax.plot(f, u, 'r--', label='$z_2, f(z_2)$') 70 | ax.plot([0, 3], [0, 3], 'k-', linewidth=0.5) 71 | ax.scatter(eqpts, eqpts[::-1], color='k') 72 | ax.axis([0, 5, 0, 5]) 73 | ax.set_xlabel('$z_1, f(z_2)$') 74 | ax.set_ylabel('$z_2, f(z_1)$') 75 | ax.legend() 76 | ax.set_title('(b) Equilibrium Curves') 77 | 78 | # Save the first figure 79 | plt.savefig("figure-5.15-genswitch_nullclines.png", bbox_inches='tight') 80 | 81 | # 82 | # 5.16 (a) Phase portrait 83 | # 84 | 85 | fig = plt.figure(constrained_layout=True) 86 | gs = fig.add_gridspec(2, 2) 87 | ax = fig.add_subplot(gs[0, 0]) # first row, first column 88 | 89 | ct.phase_plane_plot( 90 | genswitch_model, [0, 5, 0, 5], 10, gridspec=[7, 7], ax=ax, 91 | plot_separatrices=False) 92 | 93 | ax.axis('scaled') 94 | ax.axis([0, 5, 0, 5]) 95 | ax.set_xticks([0, 1, 2, 3, 4, 5]) # defaults are different than y axis (?) 96 | ax.set_xlabel("Protein A [scaled]") 97 | ax.set_ylabel("Protein B [scaled]") 98 | ax.set_title('(a) Phase portrait') 99 | 100 | # 101 | # 5.16 (b) Time traces 102 | # 103 | 104 | ax = fig.add_subplot(gs[0, 1]) # first row, second column 105 | 106 | # Solve the ODE for the first segment 107 | sol1 = ct.input_output_response( 108 | genswitch_model, np.linspace(0, 10, 1000), 0, [1, 5]) 109 | 110 | # Solve the ODE for the second segment 111 | sol2 = ct.input_output_response( 112 | genswitch_model, np.linspace(11, 25, 1000), 0, 113 | [sol1.states[0, -1] + 2, sol1.states[1, -1] - 2]) 114 | 115 | # Second plot: Time traces 116 | ax.plot(sol1.time, sol1.outputs[0], 'b-', label='$z_1$') 117 | ax.plot(sol1.time, sol1.outputs[1], 'r--', label='$z_2$') 118 | ax.plot(sol2.time, sol2.outputs[0], 'b-') 119 | ax.plot(sol2.time, sol2.outputs[1], 'r--') 120 | ax.plot( 121 | [sol1.time[-1], sol2.time[0]], [sol1.outputs[0, -1], sol2.outputs[0, -0]], 122 | 'k:') 123 | ax.plot( 124 | [sol1.time[-1], sol2.time[0]], [sol1.outputs[1, -1], sol2.outputs[1, -0]], 125 | 'k:') 126 | ax.scatter( 127 | [sol1.time[-1], sol2.time[0]], [sol1.outputs[0, -1], sol2.outputs[0, 0]], 128 | edgecolors='b', facecolors='none', marker='o') 129 | ax.scatter( 130 | [sol1.time[-1], sol2.time[0]], [sol1.outputs[1, -1], sol2.outputs[1, 0]], 131 | edgecolors='r', facecolors='none', marker='o') 132 | ax.axis([0, 25, 0, 5]) 133 | ax.set_xlabel('Time [scaled]') 134 | ax.set_ylabel('Protein concentrations [scaled]') 135 | ax.legend() 136 | ax.set_title('(b) Simulation time traces') 137 | 138 | # Save the second figure 139 | plt.savefig("figure-5.16-genswitch_dynamics.png", bbox_inches='tight') 140 | -------------------------------------------------------------------------------- /example-5.14-invpend_stabilized.py: -------------------------------------------------------------------------------- 1 | # example-5.14-invpend_stabilized.py - stabilized inverted pendulum 2 | # RMM, 17 Nov 2024 3 | 4 | # Figure 5.17: Stabilized inverted pendulum. A control law applies a 5 | # force u at the bottom of the pendulum to stabilize the inverted 6 | # position (a). The phase portrait (b) shows that the equilibrium 7 | # point corresponding to the vertical position is stabilized. The 8 | # shaded region indicates the set of initial conditions that converge 9 | # to the origin. The ellipse corresponds to a level set of a Lyapunov 10 | # function V (x) for which V (x) > 0 and V ̇ (x) < 0 for all points 11 | # inside the ellipse. This can be used as an estimate of the region of 12 | # attraction of the equilibrium point. The actual dynamics of the 13 | # system evolve on a manifold (c). 14 | 15 | import control as ct 16 | import numpy as np 17 | import matplotlib.pyplot as plt 18 | from math import pi, sin, cos, sqrt 19 | ct.use_fbs_defaults() 20 | 21 | # 22 | # System dynamics 23 | # 24 | 25 | # Stablized (and normalized) inverted pendulum dynamics 26 | def balpend_update(t, x, v, params): 27 | a = params.get('a', 2) 28 | u = -2 * a * sin(x[0]) - x[1] * cos(x[0]) 29 | return [x[1], sin(x[0]) + u * cos(x[0])] 30 | balpend = ct.nlsys( 31 | balpend_update, states=2, inputs=0, name='inverted pendulum') 32 | 33 | 34 | # Set up the plotting grid to match the layout in the book 35 | fig = plt.figure(constrained_layout=True) 36 | gs = fig.add_gridspec(2, 4) 37 | ax = fig.add_subplot(gs[0, 1:3]) 38 | 39 | # Generate the phase plot (easy part) 40 | cplt = ct.phase_plane_plot( 41 | balpend, [-2*pi, 2*pi, -4, 4], 4.5, gridspec=[10, 10], 42 | plot_separatrices={'timedata': np.linspace(0, 20, 500), 'arrows': 1}, 43 | ax=ax) 44 | 45 | # 46 | # Next we need to shade the region of attraction containing the origin. We 47 | # do this by extracting out the bounding separatrices and then filling in 48 | # the region between them, using some messy NumPy/Matplotlib commands... 49 | # 50 | 51 | # Get the separatrices on either side of the origin 52 | for line in cplt.lines[0]: 53 | xdata, ydata = line.get_data() 54 | if line.get_linestyle() != '--' or abs(ydata[-1]) > 0.1: 55 | continue # Not the line we are looking for 56 | 57 | # Extract out the separatrices for the middle region 58 | if xdata[-1] < 0 and xdata[-1] > -2 and ydata[-1] > 0: 59 | ul = (xdata[::-1], ydata[::-1]) # Sort along increasing y 60 | elif xdata[-1] < 0 and xdata[-1] > -2 and ydata[-1] < 0: 61 | ll = (xdata, ydata) 62 | elif xdata[-1] > 0 and xdata[-1] < 2 and ydata[-1] > 0: 63 | ur = (xdata[::-1], ydata[::-1]) # Sort along increasing y 64 | elif xdata[-1] > 0 and xdata[-1] < 2 and ydata[-1] < 0: 65 | lr = (xdata, ydata) 66 | 67 | # Shade the region between the separatrices (including the middle) 68 | ax.fill_betweenx(ul[1], ul[0], np.interp(ul[1], ur[1], ur[0]), color='0.95') 69 | ax.fill_betweenx(lr[1], np.interp(lr[1], ll[1], ll[0]), lr[0], color='0.95') 70 | ax.fill_betweenx( 71 | [ul[1][0], ll[1][-1]], [ul[0][0], ll[0][-1]], [ur[0][0], lr[0][-1]], 72 | color='0.95') 73 | 74 | # Add the Lypaunov level set 75 | A = balpend.linearize(0, 0).A # Linearized dynamics matrix 76 | P = ct.lyap(A.T, np.eye(2)) # Solve Lyapunov equation for P 77 | 78 | # Figure out the states along the level set 79 | rho = 1.2 # value of the level set 80 | xval, yval = [], [] 81 | for theta in np.linspace(0, 2*pi, 100): 82 | # Find the length of state vector at this angle that gives V(x) = 1 83 | r = 1 / sqrt(np.array([cos(theta), sin(theta)]).T @ P @ 84 | np.array([cos(theta), sin(theta)])) 85 | xval.append(rho * r * cos(theta)) 86 | yval.append(rho * r * sin(theta)) 87 | ax.plot(xval, yval, 'r-', linewidth=2) 88 | 89 | # Label the plot 90 | ax.set_xticks([-2*pi, -pi, 0, pi, 2*pi]) 91 | ax.set_xticklabels([r'$-2\pi$', r'$-pi$', r'$0$', r'$\pi$', r'$2\pi$']) 92 | ax.set_xlabel('$x_1$') 93 | ax.set_ylabel('$x_2$', rotation=0) 94 | ax.set_title("(b) Phase portrait") 95 | 96 | plt.savefig('figure-5.17-balpend_phaseplot.png', bbox_inches='tight') 97 | -------------------------------------------------------------------------------- /example-5.16-predprey_bif.py: -------------------------------------------------------------------------------- 1 | # example-5.16-predprey_bif.py - predator-prey stability analysis 2 | # RMM, 18 Nov 2024 3 | # 4 | # Figure 5.18: Bifurcation analysis of the predator–prey system. (a) 5 | # Parametric stability diagram showing the regions in parameter space for 6 | # which the system is stable. (b) Bifurcation diagram showing the location 7 | # and stability of the equilib- rium point as a function of a. The solid 8 | # line represents a stable equilibrium point, and the dashed line 9 | # represents an unstable equilibrium point. The dash-dotted lines indicate 10 | # the upper and lower bounds for the limit cycle at that parameter value 11 | # (computed via simulation). The nominal values of the parameters in the 12 | # model are a = 3.2, b = 0.6, c = 50, d = 0.56, k = 125, and r = 1.6. 13 | 14 | import control as ct 15 | import numpy as np 16 | import scipy 17 | import matplotlib.pyplot as plt 18 | ct.use_fbs_defaults() 19 | 20 | # 21 | # System dynamics 22 | # 23 | 24 | from predprey import predprey, predprey_params 25 | 26 | # Create a function to compute the real part of the largest eigenvalue 27 | def maxeig(a, c): 28 | # Initialize parameter values 29 | r, d, b, k = map(predprey_params.get, ['r', 'd', 'b', 'k']) 30 | params = {'a': a, 'c': c} 31 | 32 | # Equilibrium point from equations (4.33) and (4.34) 33 | xeq = [(c*d) / (a*b - d), (b*c*r)*(a*b*k - c*d - d*k)/(k * (a*b - d)**2)] 34 | 35 | # Linearization 36 | A = ct.linearize(predprey, xeq, 0, params=params).A 37 | 38 | return np.max(np.linalg.eig(A).eigenvalues.real) 39 | 40 | # Set up the plotting grid to match the layout in the book 41 | fig = plt.figure(constrained_layout=True) 42 | gs = fig.add_gridspec(2, 2) 43 | 44 | # 45 | # (a) Stability diagram 46 | # 47 | # To find the boundaries of stability in terms of the parameters $a$ and 48 | # $c$, we scan over the $a$ parameter and find the values of $c$ that cause 49 | # the real part of one of the eigenvalues to be zero. 50 | # 51 | 52 | ax = fig.add_subplot(gs[0, 0]) # first row, first column 53 | ax.set_title("(a) Stability diagram") 54 | 55 | avals = np.linspace(1.3, 4, 50) 56 | cvals_lower, cvals_upper = [], [] 57 | last_lower, last_upper = 5, 100 58 | for a in avals: 59 | sol1 = scipy.optimize.root( 60 | lambda c: maxeig(a, c), last_lower, method='broyden1') 61 | if sol1.success: 62 | last_lower = sol1.x.item() 63 | cvals_lower.append(last_lower) 64 | else: 65 | print(sol1.message) 66 | cvals_lower.append(np.nan) 67 | 68 | sol2 = scipy.optimize.root(lambda c: maxeig(a, c), last_upper) 69 | if sol2.success: 70 | last_upper = sol2.x.item() 71 | cvals_upper.append(last_upper) 72 | else: 73 | cvals_upper.append(np.nan) 74 | 75 | ax.plot(avals, cvals_lower, 'k', linewidth=0.5) 76 | ax.plot(avals, cvals_upper, 'k', linewidth=0.5) 77 | ax.fill_between(avals, cvals_lower, cvals_upper, color='0.9') 78 | 79 | ax.set_xlabel("$a$") 80 | ax.set_ylabel("$c$", rotation=0) 81 | ax.text(1.4, 160, "Unstable") 82 | ax.text(2.2, 100, "Stable") 83 | ax.text(3, 25, "Unstable") 84 | ax.axis('tight') 85 | ax.axis([1.35, 4, 0, 200]) 86 | 87 | # 88 | # (b) Bifurcation diagram 89 | # 90 | 91 | ax = fig.add_subplot(gs[0, 1]) # first row, second column 92 | ax.set_title("(b) Bifurcation diagram") 93 | 94 | # Create lists to hold the values of the different branches on the plot 95 | stable_H, unstable_H = [], [] # Equilibrium point 96 | lower_H, upper_H = [], [] # Limit cycle bounds 97 | 98 | # Set the values of 'a' to be denser near the bifurcation point 99 | avals = np.hstack( 100 | [np.linspace(1.35, 2, 10), np.linspace(2, 4, 100), np.linspace(4, 8, 20)]) 101 | 102 | # Set up the remaining parameters for the simulation 103 | timepts = np.linspace(0, 300, 5000) 104 | params = predprey_params 105 | 106 | # Compute the branches of the bifurcation diagram 107 | for a in avals: 108 | # Set the parameter values 109 | params['a'] = a 110 | 111 | # Equilibrium point from equations (4.33) and (4.34) 112 | r, d, b, k, c = map(predprey_params.get, ['r', 'd', 'b', 'k', 'c']) 113 | xeq = [(c*d) / (a*b - d), (b*c*r)*(a*b*k - c*d - d*k)/(k * (a*b - d)**2)] 114 | 115 | # Check stability 116 | if maxeig(a, params['c']) < 0: 117 | # Stable branch 118 | stable_H.append(xeq[0]) 119 | [vlist.append(np.nan) for vlist in [unstable_H, lower_H, upper_H]] 120 | else: 121 | # Unstable branch 122 | unstable_H.append(xeq[0]) 123 | stable_H.append(np.nan) 124 | 125 | # Run a simulation to figure out size of the limit cycle 126 | resp = ct.input_output_response( 127 | predprey, timepts, X0=np.array(xeq) + 0.1, params=params) 128 | lower_H.append(np.min(resp.outputs[0, -500:])) 129 | upper_H.append(np.max(resp.outputs[0, -500:])) 130 | 131 | # Plot the different branches 132 | ax.plot(avals, stable_H, 'b-') 133 | ax.plot(avals, unstable_H, 'r--') 134 | ax.plot(avals, lower_H, 'k-.') 135 | ax.plot(avals, upper_H, 'k-.') 136 | 137 | # Label the plot 138 | ax.set_xlabel("$a$") 139 | ax.set_ylabel("$H$", rotation=0) 140 | ax.axis('tight') 141 | ax.axis([1.35, 8, 0, 150]) 142 | 143 | # Save the figure 144 | plt.savefig("figure-5.18-predprey_bif.png", bbox_inches='tight') 145 | -------------------------------------------------------------------------------- /example-5.17-bicycle_stability.py: -------------------------------------------------------------------------------- 1 | # example-5.17-bicycle_stability.py - Root locus diagram for a bicycle model 2 | # RMM, 24 Nov 2024 3 | # 4 | # Figure 5.19: Stability plots for a bicycle moving at constant 5 | # velocity. The plot in (a) shows the real part of the system eigenvalues 6 | # as a function of the bicycle velocity v0. The system is stable when all 7 | # eigenvalues have negative real part (shaded region). The plot in (b) 8 | # shows the locus of eigenvalues on the complex plane as the velocity v is 9 | # varied and gives a different view of the stability of the system. This 10 | # type of plot is called a root locus diagram. 11 | # 12 | # Notes: 13 | # 14 | # 1. The line styles used in this plot are slightly different than in the 15 | # book. Solid lines are used for real-valued eigenvalues and dashed 16 | # lines are used for the real part of complex-valued eigenvalues. 17 | # 18 | # 2. This code relies on features on python-control-0.10.2, which is 19 | # currently under development. 20 | 21 | import control as ct 22 | import numpy as np 23 | import matplotlib.pyplot as plt 24 | from math import isclose 25 | ct.use_fbs_defaults() 26 | 27 | # 28 | # System dynamics 29 | # 30 | 31 | from bicycle import whipple_A 32 | 33 | # Set up the plotting grid to match the layout in the book 34 | fig = plt.figure(constrained_layout=True) 35 | gs = fig.add_gridspec(2, 2) 36 | 37 | # 38 | # (a) Stability diagram 39 | # 40 | 41 | ax = fig.add_subplot(gs[0, 0]) # first row, first column 42 | ax.set_title("(a) Stability diagram") 43 | 44 | # Compute the eigenvalues as a function of velocity 45 | v0_vals = np.linspace(-15, 15, 500) 46 | eig_vals = [] 47 | for v0 in v0_vals: 48 | A = whipple_A(v0) 49 | eig_vals.append(np.sort(np.linalg.eig(A).eigenvalues)) 50 | 51 | # Initialize lists to categorize eigenvalues 52 | eigs_real_stable = [] 53 | eigs_complex_stable = [] 54 | eigs_real_unstable = [] 55 | eigs_complex_unstable = [] 56 | 57 | # Keep track of region in which all eigenvalues are stable 58 | stable_beg = stable_end = None 59 | 60 | # Process each set of eigenvalues 61 | for i, eig_set in enumerate(eig_vals): 62 | # Create arrays filled with NaN for each category 63 | real_stable = np.full(eig_set.shape, np.nan) 64 | complex_stable = np.full(eig_set.shape, np.nan) 65 | real_unstable = np.full(eig_set.shape, np.nan) 66 | complex_unstable = np.full(eig_set.shape, np.nan) 67 | 68 | # Classify eigenvalues 69 | for j, eig in enumerate(eig_set): 70 | if isclose(eig.imag, 0): # Real eigenvalue 71 | if eig.real < 0: 72 | real_stable[j] = eig.real 73 | else: 74 | real_unstable[j] = eig.real 75 | else: # Complex eigenvalue 76 | if eig.real < 0: 77 | complex_stable[j] = eig.real 78 | else: 79 | complex_unstable[j] = eig.real 80 | 81 | # Append categorized arrays to respective lists 82 | eigs_real_stable.append(real_stable) 83 | eigs_complex_stable.append(complex_stable) 84 | eigs_real_unstable.append(real_unstable) 85 | eigs_complex_unstable.append(complex_unstable) 86 | 87 | # Look for regions where everything is stable 88 | if stable_beg is None and all(eig_set.real < 0): 89 | stable_beg = i 90 | elif stable_beg and stable_end is None and any(eig_set.real > 0): 91 | stable_end = i 92 | 93 | # Plot the stability diagram 94 | ax.plot(v0_vals, eigs_real_stable, 'b-') 95 | ax.plot(v0_vals, eigs_real_unstable, 'r-') 96 | ax.plot(v0_vals, eigs_complex_stable, 'b--') 97 | ax.plot(v0_vals, eigs_complex_unstable, 'r--') 98 | 99 | # Add in the coordinate axes 100 | ax.axhline(color='k', linewidth=0.5) 101 | ax.axvline(color='k', linewidth=0.5) 102 | 103 | # Label and shade stable and unstable regions 104 | ax.text(-12, 8, "Unstable") 105 | ax.fill_betweenx( 106 | [-15, 15], [v0_vals[stable_beg], v0_vals[stable_beg]], 107 | [v0_vals[stable_end], v0_vals[stable_end]], color='0.9') 108 | ax.text(7.2, 6, "Stable", rotation=90) 109 | ax.text(11.7, 5, "Unstable", rotation=90) 110 | 111 | # Label the axes 112 | ax.set_xlabel(r"Velocity $v_0$ [m/s]") 113 | ax.set_ylabel(r"$\text{Re}\,\lambda$") 114 | ax.axis('scaled') 115 | ax.axis([-15, 15, -15, 15]) 116 | 117 | # 118 | # (b) Root locus diagram 119 | # 120 | 121 | ax = fig.add_subplot(gs[0, 1]) # first row, second column 122 | ax.set_title("(b) Root locus diagram") 123 | 124 | # Generate the root locus diagram via the root_locus_plot functionality 125 | pos_idx = np.argmax(v0_vals >= 0) 126 | poles = eig_vals[pos_idx] 127 | loci = np.array(eig_vals[pos_idx:]) 128 | rl_map = ct.PoleZeroData(poles, [], v0_vals[pos_idx:], loci) 129 | rl_map.plot(ax=ax) 130 | 131 | # Add in the coordinate axes 132 | ax.axhline(color='k', linewidth=0.5) 133 | ax.axvline(color='k', linewidth=0.5) 134 | 135 | # Label the real axes of the plot 136 | ax.text(-12.5, -2, r"$\leftarrow v_0$") 137 | ax.text(-3.5, -2, r"$v_0 \rightarrow$") 138 | 139 | # Label the crossover points 140 | xo_idx = np.argmax(rl_map.loci[:, 3].real < 0) 141 | ax.plot(0, rl_map.loci[xo_idx, 2].imag, 'bo', markersize=3) 142 | ax.plot(0, rl_map.loci[xo_idx, 3].imag, 'bo', markersize=3) 143 | ax.text(1, rl_map.loci[xo_idx, 2].imag, r"$v_0 = 6.1$") 144 | ax.text(1, rl_map.loci[xo_idx, 3].imag, r"$v_0 = 6.1$") 145 | 146 | # Label the axes 147 | ax.set_xlabel(r"$\text{Re}\,\lambda$") 148 | ax.set_ylabel(r"$\text{Im}\,\lambda$") 149 | ax.set_box_aspect(1) 150 | ax.axis([-15, 15, -10, 10]) 151 | 152 | # Save the figure 153 | plt.savefig("figure-5.19-bicycle_stability.png", bbox_inches='tight') 154 | -------------------------------------------------------------------------------- /example-5.18-noise_cancel.py: -------------------------------------------------------------------------------- 1 | # example-5.18-noise_cancel.py - Noise cancellation 2 | # RMM, 24 Nov 2024 3 | # 4 | # Figure 5.21: Simulation of noise cancellation. The upper left figure 5 | # shows the headphone signal without noise cancellation, and the lower left 6 | # figure shows the signal with noise cancellation. The right figures show 7 | # the parameters a and b of the filter. 8 | 9 | import control as ct 10 | import numpy as np 11 | import matplotlib.pyplot as plt 12 | from math import pi 13 | ct.use_fbs_defaults() 14 | 15 | # 16 | # System dynamics 17 | # 18 | 19 | # Headphone dynamics 20 | headphone_params = {'a0': -0.75, 'b0': 0.9} 21 | def headphone_update(t, z, n, params): 22 | return params['a0'] * z[0] + params['b0'] * n[0] 23 | headphone = ct.nlsys( 24 | headphone_update, inputs='n', states='z', params=headphone_params, 25 | name='headphone') 26 | 27 | # Filter dynamics 28 | def filter_update(t, w, u, params): 29 | n, a, b = u 30 | return a * w + b * n 31 | filter = ct.nlsys( 32 | filter_update, inputs=['n', 'a', 'b'], states='w', name='filter') 33 | 34 | # Controller dynamics 35 | control_params = {'alpha': 1} 36 | def control_update(t, x, u, params): 37 | n, e, w = u 38 | a, b = x 39 | return [ 40 | params['alpha'] * w * e, 41 | params['alpha'] * n * e 42 | ] 43 | control = ct.nlsys( 44 | control_update, inputs=['n', 'e', 'w'], states=['a', 'b'], name='control', 45 | params=control_params) 46 | 47 | # Create summing junction to add all of the signal together 48 | summer = ct.summing_junction(inputs=['z', 'S', '-w'], outputs='e') 49 | 50 | # Interconnected system 51 | sys = ct.interconnect( 52 | [headphone, filter, control, summer], name='noise_cancel', 53 | inputs=['S', 'n'], outputs=['e', 'a', 'b']) 54 | 55 | # Create the signal and noise 56 | timepts = np.linspace(0, 200, 2000) 57 | signal = np.sin(0.1 * 2 * pi * timepts) # sinewave with frequency 0.1 Hz 58 | noise = ct.white_noise(timepts, 5) # white noise with covariance 5 59 | 60 | # Set up the plotting grid to match the layout in the book 61 | fig = plt.figure(constrained_layout=True) 62 | gs = fig.add_gridspec(3, 2) 63 | 64 | # No noise cancellation 65 | resp_off = ct.input_output_response( 66 | sys, timepts, [signal, noise], params={'alpha': 0}) 67 | 68 | ax = fig.add_subplot(gs[0, 0]) 69 | ax.plot(resp_off.time, resp_off.outputs[0]) 70 | ax.axis('tight') 71 | ax.axis([0, 200, -5, 5]) 72 | ax.set_ylabel("No cancellation") 73 | 74 | resp_on = ct.input_output_response( 75 | sys, timepts, [signal, noise], params={'alpha': 1e-2}) 76 | 77 | ax = fig.add_subplot(gs[1, 0]) 78 | ax.plot(resp_on.time, resp_on.outputs[0], label='e') 79 | # ax.plot(resp_on.time, signal, label='S') 80 | ax.axis('tight') 81 | ax.axis([0, 200, -5, 5]) 82 | ax.set_ylabel("Cancellation") 83 | ax.set_xlabel("Time $t$ [s]") 84 | # ax.legend() 85 | 86 | ax = fig.add_subplot(gs[0, 1]) 87 | ax.plot(resp_on.time, resp_on.outputs[1]) 88 | ax.axis('tight') 89 | ax.axis([0, 200, -1.1, 0]) 90 | ax.set_ylabel("$a$", rotation=0) 91 | 92 | ax = fig.add_subplot(gs[1, 1]) 93 | ax.plot(resp_on.time, resp_on.outputs[2]) 94 | ax.axis('tight') 95 | ax.axis([0, 200, 0, 1.1]) 96 | ax.set_ylabel("$b$", rotation=0) 97 | ax.set_xlabel("Time $t$ [s]") 98 | 99 | # Save the figure 100 | plt.savefig("figure-5.21-noise_cancel.png", bbox_inches='tight') 101 | -------------------------------------------------------------------------------- /example-5.8-tanker_stabilty.py: -------------------------------------------------------------------------------- 1 | # example-5.8-tanker_stability.py - Stability of a tanker 2 | # RMM, 16 Nov 2024 3 | # 4 | # Figure 3.15: Stability analysis for a tanker. The rudder characteristics 5 | # are shown in (a), where the equilibrium points are marked by circles, and 6 | # the tanker trajec- tories are shown in (b). 7 | 8 | import control as ct 9 | import numpy as np 10 | import matplotlib.pyplot as plt 11 | ct.use_fbs_defaults() 12 | 13 | # 14 | # System dynamics 15 | # 16 | 17 | def tanker_dynamics(t, x, u, params): 18 | # Parameter values 19 | a1 = params.get('a1', -0.6) 20 | a2 = params.get('a2', -0.3) 21 | a3 = params.get('a3', -5) 22 | a4 = params.get('a4', -2) 23 | alpha = params.get('alpha', -2) 24 | b1 = params.get('b1', 0.1) 25 | b2 = params.get('b2', -0.8) 26 | 27 | v, r = x[0], x[1] # velocity and turning rate 28 | delta = u[0] # rudder angle 29 | 30 | return [ 31 | a1 * v + a2 * r + alpha * v * abs(v) + b1 * delta, 32 | a3 * v + a4 * r + b2 * delta 33 | ] 34 | tanker_model = ct.nlsys( 35 | tanker_dynamics, None, inputs='delta', states=['v', 'r']) 36 | 37 | 38 | # Set up the plotting grid to match the layout in the book 39 | fig = plt.figure(constrained_layout=True) 40 | gs = fig.add_gridspec(2, 2) 41 | 42 | # 43 | # (a) Rudder curve 44 | # 45 | ax = fig.add_subplot(gs[0, 0]) # first row, first column 46 | 47 | # Compute the input for each turning rate 48 | rvec = np.linspace(-0.4, 0.4, 50) 49 | delta_list = [] 50 | for r in rvec: 51 | # Solve for the different equilibrium solutions 52 | eqpt = ct.find_operating_point( 53 | tanker_model, [0, 0], 0, 54 | y0=[0, r], iy=[1], # Look for the desired turning rate 55 | ) 56 | delta_list.append(eqpt.inputs[0]) 57 | dvec = np.array(delta_list) 58 | ax.plot(dvec, rvec) 59 | 60 | # Add the equilibrium points at zero 61 | for x0 in [[0.1, 0.1], [0, 0], [-0.1, -0.1]]: 62 | eqpt = ct.find_operating_point(tanker_model, x0, 0) 63 | ax.scatter( 64 | eqpt.inputs[0], eqpt.outputs[1], facecolors='none', edgecolors='b') 65 | 66 | # Add labels and axis lines 67 | ax.set_title("(a) Rudder curve") 68 | ax.set_xlabel(r"Rudder angle $\delta$") 69 | ax.set_ylabel(r"Noramlized turning rate $r$") 70 | ax.plot([-0.1, 0.1], [0, 0], 'k', linewidth=0.5) 71 | ax.plot([0, 0], [-0.4, 0.4], 'k', linewidth=0.5) 72 | ax.axis([-0.1, 0.1, -0.4, 0.4]) 73 | 74 | # 75 | # (b) Tanker trajectories 76 | # 77 | from math import sin, cos 78 | ax = fig.add_subplot(gs[0, 1]) # first row, second column 79 | 80 | # Create a full tanker model, including position and orientation 81 | def full_tanker_dynamics(t, x, u, params): 82 | vdot, rdot = tanker_dynamics(t, x[3:], u, params) 83 | theta, v, r = x[2], x[3], x[4] 84 | return [ 85 | cos(theta) + v * sin(theta), -sin(theta) + v * cos(theta), 86 | r, vdot, rdot] 87 | full_tanker_model = ct.nlsys( 88 | full_tanker_dynamics, None, inputs='delta', 89 | states=['x', 'y', 'theta', 'v', 'r']) 90 | 91 | # Create simulations and plot them 92 | timepts = np.linspace(0, 100, 100) 93 | for r0, linestyle in zip([0.1, 0, -0.1], ['b-', 'b--', 'b-']): 94 | response = ct.input_output_response( 95 | full_tanker_model, timepts, 0, [0, 0, 0, 0, r0]) 96 | ax.plot(response.outputs[0], response.outputs[1], linestyle) 97 | 98 | # Add labels and axis lines 99 | ax.set_title("(b) Tanker trajectories") 100 | ax.set_xlabel("$x$") 101 | ax.set_ylabel("$y$") 102 | ax.axis('scaled') 103 | ax.axis([0, 40, -20, 20]) 104 | 105 | # Save the figure 106 | plt.savefig("figure-5.12-tanker_stability.png", bbox_inches='tight') 107 | -------------------------------------------------------------------------------- /example-5.9-limit_cycle_stability.py: -------------------------------------------------------------------------------- 1 | # example-5.9-limit_cycle_stability.py - Solution curves for stable limit cycle 2 | # RMM, 16 Nov 2024 (with initial converstion from MATLAB by ChatGPT) 3 | # 4 | # Figure 5.13: Solution curves for a stable limit cycle. The phase portrait 5 | # on the left shows that the trajectory for the system rapidly converges to 6 | # the stable limit cycle. The starting points for the trajectories are 7 | # marked by circles in the phase portrait. The time domain plots on the 8 | # right show that the states do not converge to the solution but instead 9 | # maintain a constant phase error. 10 | 11 | import control as ct 12 | import numpy as np 13 | import matplotlib.pyplot as plt 14 | from scipy.integrate import solve_ivp 15 | ct.use_fbs_defaults() 16 | 17 | # 18 | # System dynamics 19 | # 20 | 21 | def limcyc(t, x): 22 | E = x[0]**2 + x[1]**2 23 | xdot1 = x[1] + x[0] * (1 - E) 24 | xdot2 = -x[0] + x[1] * (1 - E) 25 | return [xdot1, xdot2] 26 | 27 | # Set up the plotting grid to match the layout in the book 28 | fig = plt.figure(constrained_layout=True) 29 | gs = fig.add_gridspec(8, 10) 30 | 31 | # Simulation settings 32 | ts = 20 33 | tspan = (0, ts) 34 | x01 = [0, 2] 35 | x02 = [1/np.sqrt(2), 1/np.sqrt(2)] 36 | 37 | # 38 | # (a) Phase plane plot 39 | # 40 | 41 | ax = fig.add_subplot(gs[1:5, :4]) # left plot 42 | 43 | # Solve the differential equations 44 | sol1 = solve_ivp(limcyc, tspan, x01, t_eval=np.linspace(*tspan, 1000)) 45 | sol2 = solve_ivp(limcyc, tspan, x02, t_eval=np.linspace(*tspan, 1000)) 46 | 47 | # Plot phase plane 48 | ax.plot(sol1.y[0], sol1.y[1], 'r--') 49 | ax.plot(sol2.y[0], sol2.y[1], 'b-') 50 | ax.scatter(0, 2, facecolors='none', edgecolors='r') 51 | ax.scatter(1/np.sqrt(2), 1/np.sqrt(2), facecolors='none', edgecolors='b') 52 | 53 | ax.axhline(0, color='k') 54 | ax.axvline(0, color='k') 55 | ax.axis('square') 56 | ax.axis([-1.2, 2.2, -1.2, 2.2]) 57 | ax.set_xlabel('$x_1$') 58 | ax.set_ylabel('$x_2$', rotation=0) 59 | 60 | # 61 | # (b) Time traces 62 | # 63 | 64 | ax = fig.add_subplot(gs[1:3, 5:]) # upper right plot 65 | ax.plot(sol1.t, sol1.y[0], 'r--') 66 | ax.plot(sol2.t, sol2.y[0], 'b-') 67 | ax.set_xlim([0, 20]) 68 | ax.set_ylim([-1.2, 2]) 69 | ax.set_ylabel('$x_1$', rotation=0) 70 | 71 | ax = fig.add_subplot(gs[3:5, 5:]) # upper right plot 72 | ax.plot(sol1.t, sol1.y[1], 'r--') 73 | ax.plot(sol2.t, sol2.y[1], 'b-') 74 | ax.set_xlim([0, 20]) 75 | ax.set_ylim([-1.2, 2]) 76 | ax.set_ylabel('$x_2$', rotation=0) 77 | ax.set_xlabel('Time $t$') 78 | 79 | plt.savefig('figure-5.13-limit_cycle_stability.png', bbox_inches='tight') 80 | -------------------------------------------------------------------------------- /example-6.10-compartment_response.py: -------------------------------------------------------------------------------- 1 | # example-6.7-compartment_response.py - Compartment model response 2 | # RMM, 24 Nov 2024 3 | # 4 | # Figure 6.10: Response of a compartment model to a constant drug 5 | # infusion. A simple diagram of the system is shown in (a). The step 6 | # response (b) shows the rate of concentration buildup in compartment 2. In 7 | # (c) a pulse of initial concentration is used to speed up the response. 8 | 9 | import control as ct 10 | import numpy as np 11 | import matplotlib.pyplot as plt 12 | ct.use_fbs_defaults() 13 | 14 | # 15 | # System dynamics 16 | # 17 | 18 | # Parameter settings for the model 19 | k0 = 0.1 20 | k1 = 0.1 21 | k2 = 0.5 22 | b0 = 1.5 23 | 24 | # Compartment model definition 25 | Ada = np.array([[-k0 - k1, k1], [k2, -k2]]) 26 | Bda = np.array([[b0], [0]]) 27 | Cda = np.array([[0, 1]]) 28 | compartment = ct.ss(Ada, Bda, Cda, 0); 29 | 30 | # Set up the plotting grid to match the layout in the book 31 | fig = plt.figure(constrained_layout=True) 32 | gs = fig.add_gridspec(4, 3) 33 | 34 | # 35 | # (a) Step input 36 | # 37 | 38 | timepts = np.linspace(0, 50) 39 | input = np.ones(timepts.size) * 0.1 40 | response = ct.forced_response(compartment, timepts, input) 41 | 42 | ax = fig.add_subplot(gs[0, 1]) 43 | ax.set_title("(b) Step input") 44 | 45 | ax.plot(response.time, response.outputs) 46 | ax.axhline(response.outputs[-1], color='k', linewidth=0.5) 47 | ax.set_ylabel("Concentration $C_2$") 48 | ax.axis('tight') 49 | ax.axis([0, 50, 0, 2]) 50 | 51 | ax = fig.add_subplot(gs[1, 1]) 52 | ax.plot(response.time, response.inputs) 53 | ax.set_xlabel("Time $t$ [min]") 54 | ax.set_ylabel("Input dosage") 55 | ax.axis('tight') 56 | ax.axis([0, 50, 0, 0.4]) 57 | 58 | # 59 | # (b) Pulse input 60 | # 61 | 62 | timepts = np.linspace(0, 50, 200) 63 | input = np.ones(timepts.size) * 0.1 64 | input[:20] = 0.3 # Increase value for first 5 seconds 65 | response = ct.forced_response(compartment, timepts, input) 66 | 67 | ax = fig.add_subplot(gs[0, 2]) 68 | ax.set_title("(c) Pulse input") 69 | 70 | ax.plot(response.time, response.outputs) 71 | ax.axhline(response.outputs[-1], color='k', linewidth=0.5) 72 | ax.set_ylabel("Concentration $C_2$") 73 | ax.axis('tight') 74 | ax.axis([0, 50, 0, 2]) 75 | 76 | ax = fig.add_subplot(gs[1, 2]) 77 | ax.plot(response.time, response.inputs) 78 | ax.set_xlabel("Time $t$ [min]") 79 | ax.set_ylabel("Input dosage") 80 | ax.axis('tight') 81 | ax.axis([0, 50, 0, 0.4]) 82 | 83 | # Save the figure 84 | fig.align_ylabels() 85 | plt.savefig("figure-6.10-compartment_response.png", bbox_inches='tight') 86 | -------------------------------------------------------------------------------- /example-6.8-opamp_bandpass.py: -------------------------------------------------------------------------------- 1 | # example-6.8-opamp_bandpass.py - 2 | # RMM, 28 Nov 2028 3 | # 4 | # Figure 6.12: Active band-pass filter. The circuit diagram (a) shows an op 5 | # amp with two RC filters arranged to provide a band-pass filter. The plot 6 | # in (b) shows the gain and phase of the filter as a function of frequency. 7 | # Note that the phase starts at -90 deg due to the negative gain of the 8 | # operational amplifier. 9 | 10 | import control as ct 11 | import numpy as np 12 | import matplotlib.pyplot as plt 13 | ct.use_fbs_defaults() 14 | 15 | # 16 | # System dynamics 17 | # 18 | 19 | # Filter parameters 20 | R1 = 100 21 | R2 = 5000 22 | C1 = 100e-6 23 | C2 = 100e-6 24 | 25 | # State-space form of the solution 26 | A = np.array([[-1 / (R1 * C1), 0], 27 | [1 / (R1 * C2), -1 / (R2 * C2)]]) 28 | B = np.array([[1 / (R1 * C1)], 29 | [-1 / (R1 * C2)]]) 30 | C = np.array([[0, 1]]) 31 | D = np.array([[0]]) 32 | 33 | # Create the state-space system 34 | sys = ct.ss(A, B, C, D) 35 | 36 | # 37 | # (b) Frequency response 38 | # 39 | 40 | # Generate the Bode plot data 41 | omega = np.logspace(-1, 3, 100) # Frequency range from 0.1 to 1000 rad/s 42 | cplt = ct.frequency_response(sys, omega).plot( 43 | initial_phase=-90, title="(b) Frequency response") 44 | 45 | # Plot the unit gain line 46 | cplt.axes[0, 0].axhline(1, color='k', linestyle='-', linewidth=0.5) 47 | 48 | # Print system poles for bandwidth computation 49 | print("System poles (for computing bandwidth):") 50 | print(sys.poles()) 51 | 52 | # Save the figure 53 | plt.savefig("figure-6.8-opamp_bandpass.png", bbox_inches='tight') 54 | -------------------------------------------------------------------------------- /example-6.9-afm_freqresp.py: -------------------------------------------------------------------------------- 1 | # example-6.9-afm_freqresp.py - 2 | # RMM, 28 Nov 2024 3 | # 4 | # Figure 6.13: AFM frequency response. (a) A block diagram for the vertical 5 | # dynamics of an atomic force microscope in contact mode. The plot in (b) 6 | # shows the gain and phase for the piezo stack. The response contains two 7 | # frequency peaks at resonances of the system, along with an antiresonance 8 | # at ω = 268 krad/s. The combination of a resonant peak followed by an 9 | # antiresonance is common for systems with multiple lightly damped 10 | # modes. The dashed horizontal line represents the gain equal to the zero 11 | # frequency gain divided by sqrt(2). 12 | 13 | import control as ct 14 | import numpy as np 15 | import matplotlib.pyplot as plt 16 | from math import sqrt 17 | ct.use_fbs_defaults() 18 | 19 | # 20 | # System dynamics 21 | # 22 | 23 | # System parameters 24 | m1 = 0.15e-3 25 | m2 = 1e-3 26 | f1 = 40.9e3 27 | f2 = 41.6e3 28 | f3 = 120e3 29 | 30 | # Derived quantities 31 | w1 = 2 * np.pi * f1 32 | w2 = 2 * np.pi * f2 33 | w3 = 2 * np.pi * f3 34 | z1 = 0.1 35 | z3 = 0.1 36 | k2 = w2**2 * m2 37 | c2 = m2 * z1 * w1 38 | 39 | # System matrices 40 | A = np.array([[0, 1, 0, 0], 41 | [-k2 / (m1 + m2), -c2 / (m1 + m2), 1 / m2, 0], 42 | [0, 0, 0, 1], 43 | [0, 0, -w3**2, -2 * z3 * w3]]) 44 | B = np.array([[0], [0], [0], [w3**2]]) 45 | C = (m2 / (m1 + m2)) * np.array([m1 * k2 / (m1 + m2), m1 * c2 / (m1 + m2), 1, 0]) 46 | D = np.array([[0]]) 47 | 48 | # Create the state-space system 49 | sys = ct.ss(A, B, C, D) 50 | 51 | # 52 | # (b) Frequency response 53 | # 54 | 55 | # Generate the frequency response 56 | omega = np.logspace(4, 7, 10000) # Frequency range from 10^4 to 10^7 rad/s 57 | freqresp = ct.frequency_response(sys, omega) 58 | mag, phase = freqresp.magnitude[0, 0], freqresp.phase[0, 0] 59 | 60 | # Create the Bode plot 61 | cplt = freqresp.plot() 62 | mag_ax, phase_ax = cplt.axes[:, 0] 63 | cplt.set_plot_title("(b) Frequency response") 64 | 65 | # Locate peaks and valleys 66 | # Find the first peak 67 | max_mag = 0 68 | omega1 = None 69 | for i, m in enumerate(mag): 70 | if m > max_mag: 71 | max_mag = m 72 | omega1 = omega[i] 73 | elif m < max_mag: 74 | break 75 | 76 | # Find the first valley 77 | min_mag = max_mag 78 | omega2 = None 79 | for i, m in enumerate(mag): 80 | if m < min_mag: 81 | min_mag = m 82 | omega2 = omega[i] 83 | elif m > min_mag: 84 | break 85 | 86 | # Find the second peak (must be higher than first) 87 | omega3 = None 88 | for i, m in enumerate(mag): 89 | if m > max_mag: 90 | max_mag = m 91 | omega3 = omega[i] 92 | elif m < max_mag and omega3 is not None: 93 | break 94 | 95 | # Print peaks and valley frequencies 96 | print(f"Peaks at {omega1:.2e}, {omega3:.2e}; valley at {omega2:.2e}") 97 | 98 | # Add lines to mark the frequencies 99 | mag_ax.axhline(1 / sqrt(2), color='r', linestyle='--', linewidth=0.5) 100 | 101 | mag_ax.axvline(omega1, color='k', linestyle='--', linewidth=0.5) 102 | mag_ax.text(omega1 * 1.1, 2, r"$M_{r1}$") 103 | mag_ax.text(omega1 * 1.1, 0.07, r"$\omega = \omega_{r1}$") 104 | 105 | mag_ax.axvline(omega3, color='k', linestyle='--', linewidth=0.5) 106 | mag_ax.text(omega3 * 1.2, 3, r"$M_{r2}$") 107 | mag_ax.text(omega3 * 1.1, 0.07, r"$\omega = \omega_{r2}$") 108 | 109 | phase_ax.axvline(omega1, color='k', linestyle='--', linewidth=0.5) 110 | phase_ax.axvline(omega3, color='k', linestyle='--', linewidth=0.5) 111 | 112 | # Save the figure 113 | plt.savefig("figure-6.9-afm_freqresp.png", bbox_inches='tight') 114 | -------------------------------------------------------------------------------- /example-7.4-steering_place.py: -------------------------------------------------------------------------------- 1 | # example-7.4-steering_place.py - 2 | # RMM, 28 Nov 2024 3 | # 4 | # Figure 7.6: State feedback control of a steering system. Unit step 5 | # responses (from zero initial condition) obtained with controllers 6 | # designed with zeta_c = 0.7 and omega_c = 0.5, 0.7, and 1 [rad/s] are 7 | # shown in (a). The dashed lines indicate ±5% deviations from the 8 | # setpoint. Notice that response speed increases with increasing omega_c, 9 | # but that large omega_c also give large initial control actions. Unit step 10 | # responses obtained with a controller designed with omega_c = 0.7 and 11 | # zeta_c = 0.5, 0.7, and 1 are shown 2in (b). 12 | 13 | import control as ct 14 | import numpy as np 15 | import matplotlib.pyplot as plt 16 | ct.use_fbs_defaults() 17 | 18 | # 19 | # System dynamics 20 | # 21 | 22 | # Get the normalized linear dynamics 23 | from steering import linearize_lateral 24 | sys = linearize_lateral(normalize=True, output_full_state=True) 25 | 26 | # Function to place the poles at desired values 27 | def steering_place(sys, omega, zeta): 28 | # Get the pole locations based on omega and zeta 29 | desired_poly = np.polynomial.Polynomial([omega**2, 2 * zeta * omega, 1]) 30 | return ct.place(sys.A, sys.B, desired_poly.roots()) 31 | 32 | # Set up the plotting grid to match the layout in the book 33 | fig = plt.figure(constrained_layout=True) 34 | gs = fig.add_gridspec(3, 14) # allow some space in the middle 35 | left = slice(0, 6) 36 | right = slice(7, 13) 37 | 38 | # 39 | # (a) Unit step response for varying omega_c 40 | # 41 | 42 | ax_pos = fig.add_subplot(gs[0, left]) 43 | ax_delta = fig.add_subplot(gs[1, left]) 44 | ax_pos.set_title( 45 | r"(a) Unit step response for varying $\omega_c$", size='medium') 46 | 47 | timepts = np.linspace(0, 20) 48 | zeta_c = 0.7 49 | for omega_c in [0.5, 0.7, 1]: 50 | # Compute the gains for the controller 51 | K = steering_place(sys, omega_c, zeta_c) 52 | kf = omega_c**2 53 | 54 | # Compute the closed loop system 55 | ctrl, clsys = ct.create_statefbk_iosystem( 56 | sys, K, kf, feedfwd_pattern='refgain') 57 | 58 | # Simulate the closed loop dynamics 59 | response = ct.forced_response(clsys, timepts, 1, X0=0) 60 | 61 | ax_pos.plot(response.time, response.outputs['y'], 'b') 62 | ax_delta.plot(response.time, response.outputs['delta'], 'b') 63 | 64 | # Label the plot 65 | ax_pos.set_ylabel(r"Lateral position $y/b$") 66 | ax_delta.set_xlabel(r"Normalized time $v_0 t/b$") 67 | ax_delta.set_ylabel(r"Steering angle $\delta$ [rad]") 68 | 69 | ax_pos.axhline(0.95, color='k', linestyle='--', linewidth=0.5) 70 | ax_pos.axhline(1.05, color='k', linestyle='--', linewidth=0.5) 71 | ax_pos.annotate( 72 | "", xy=(3.5, 0.3), xytext=[0, 0.8], arrowprops={'arrowstyle': '<-'}) 73 | ax_pos.text(3.6, 0.25, r"$\omega_c$") 74 | 75 | ax_delta.annotate( 76 | "", xy=(4, 0.1), xytext=(0, -0.2), arrowprops={'arrowstyle': '<-'}) 77 | ax_delta.text(3.5, 0.15, r"$\omega_c$") 78 | 79 | # 80 | # (b) Unit step response for varying zeta_c 81 | # 82 | 83 | ax_pos = fig.add_subplot(gs[0, right], sharey=ax_pos) 84 | ax_delta = fig.add_subplot(gs[1, right], sharey=ax_delta) 85 | ax_pos.set_title( 86 | r"(b) Unit step response for varying $\zeta_c$", size='medium') 87 | 88 | timepts = np.linspace(0, 20) 89 | omega_c = 0.7 90 | for zeta_c in [0.5, 0.7, 1]: 91 | # Compute the gains for the controller 92 | K = steering_place(sys, omega_c, zeta_c) 93 | kf = omega_c**2 94 | 95 | # Compute the closed loop system 96 | ctrl, clsys = ct.create_statefbk_iosystem( 97 | sys, K, kf, feedfwd_pattern='refgain') 98 | 99 | # Simulate the closed loop dynamics 100 | response = ct.forced_response(clsys, timepts, 1, X0=0) 101 | 102 | ax_pos.plot(response.time, response.outputs['y'], 'b') 103 | ax_delta.plot(response.time, response.outputs['delta'], 'b') 104 | 105 | # Label the plot 106 | ax_pos.set_ylabel(r"Lateral position $y/b$") 107 | ax_delta.set_xlabel(r"Normalized time $v_0 t/b$") 108 | ax_delta.set_ylabel(r"Steering angle $\delta$ [rad]") 109 | 110 | ax_pos.axhline(0.95, color='k', linestyle='--', linewidth=0.5) 111 | ax_pos.axhline(1.05, color='k', linestyle='--', linewidth=0.5) 112 | ax_pos.annotate( 113 | "", xy=(1.7, 1.15), xytext=(5.2, 0.6), arrowprops={'arrowstyle': '<-'}) 114 | ax_pos.text(5.5, 0.5, r"$\zeta_c$") 115 | 116 | ax_delta.annotate( 117 | "", xy=(5.5, -0.2), xytext=(3.5, 0.15), arrowprops={'arrowstyle': '<-'}) 118 | ax_delta.text(3, 0.2, r"$\zeta_c$") 119 | 120 | # Save the figure 121 | fig.align_ylabels() 122 | plt.savefig("figure-7.6-steering_place.png", bbox_inches='tight') 123 | -------------------------------------------------------------------------------- /example-8.10-steering_gainsched.py: -------------------------------------------------------------------------------- 1 | # example-8.10-steering_gainsched.py - gain scheduling for vehicle steering 2 | # RMM, 8 May 2019 3 | 4 | import numpy as np 5 | import control as ct 6 | from cmath import sqrt 7 | import matplotlib.pyplot as plt 8 | import fbs # FBS plotting customizations 9 | 10 | # 11 | # Vehicle steering dynamics 12 | # 13 | # The vehicle dynamics are given by a simple bicycle model. We take the state 14 | # of the system as (x, y, theta) where (x, y) is the position of the vehicle 15 | # in the plane and theta is the angle of the vehicle with respect to 16 | # horizontal. The vehicle input is given by (v, phi) where v is the forward 17 | # velocity of the vehicle and phi is the angle of the steering wheel. The 18 | # model includes saturation of the vehicle steering angle. 19 | # 20 | # System state: x, y, theta 21 | # System input: v, phi 22 | # System output: x, y 23 | # System parameters: wheelbase, maxsteer 24 | # 25 | def vehicle_update(t, x, u, params): 26 | # Get the parameters for the model 27 | l = params.get('wheelbase', 3.) # vehicle wheelbase 28 | phimax = params.get('maxsteer', 0.5) # max steering angle (rad) 29 | 30 | # Saturate the steering input 31 | phi = np.clip(u[1], -phimax, phimax) 32 | 33 | # Return the derivative of the state 34 | return np.array([ 35 | np.cos(x[2]) * u[0], # xdot = cos(theta) v 36 | np.sin(x[2]) * u[0], # ydot = sin(theta) v 37 | (u[0] / l) * np.tan(phi) # thdot = v/l tan(phi) 38 | ]) 39 | 40 | def vehicle_output(t, x, u, params): 41 | return x # return x, y, theta (full state) 42 | 43 | # Define the vehicle steering dynamics as an input/output system 44 | vehicle = ct.nlsys( 45 | vehicle_update, vehicle_output, states=3, name='vehicle', 46 | inputs=('v', 'phi'), 47 | outputs=('x', 'y', 'theta')) 48 | 49 | # 50 | # Gain scheduled controller 51 | # 52 | # For this system we use a simple schedule on the forward vehicle velocity and 53 | # place the poles of the system at fixed values. The controller takes the 54 | # current and desired vehicle position and orientation plus the velocity 55 | # velocity as inputs, and returns the velocity and steering commands. 56 | # 57 | # System state: none 58 | # System input: x, y, theta, xd, yd, thetad, vd, phid 59 | # System output: v, phi 60 | # System parameters: longpole, latomega_c, latzeta_c 61 | # 62 | def control_output(t, x, u, params): 63 | # Get the controller parameters 64 | longpole = params.get('longpole', -2.) 65 | latomega_c = params.get('latomega_c', 2) 66 | latzeta_c = params.get('latzeta_c', 0.5) 67 | l = params.get('wheelbase', 3) 68 | 69 | # Extract the system inputs and compute the errors 70 | x, y, theta, xd, yd, thetad, vd, phid = u 71 | ex, ey, etheta = x - xd, y - yd, theta - thetad 72 | 73 | # Determine the controller gains 74 | lambda1 = -longpole 75 | a1 = 2 * latzeta_c * latomega_c 76 | a2 = latomega_c**2 77 | 78 | # Compute and return the control law 79 | v = -lambda1 * ex # leave off feedforward to generate transient 80 | if vd != 0: 81 | phi = phid - ((a2 * l) / vd**2) * ey - ((a1 * l) / vd) * etheta 82 | else: 83 | # We aren't moving, so don't turn the steering wheel 84 | phi = phid 85 | 86 | return np.array([v, phi]) 87 | 88 | # Define the controller as an input/output system 89 | controller = ct.nlsys( 90 | None, control_output, name='controller', # static system 91 | inputs=('x', 'y', 'theta', 'xd', 'yd', 'thetad', # system inputs 92 | 'vd', 'phid'), 93 | outputs=('v', 'phi') # system outputs 94 | ) 95 | 96 | # 97 | # Reference trajectory subsystem 98 | # 99 | # The reference trajectory block generates a simple trajectory for the system 100 | # given the desired speed (vref) and lateral position (yref). The trajectory 101 | # consists of a straight line of the form (vref * t, yref, 0) with nominal 102 | # input (vref, 0). 103 | # 104 | # System state: none 105 | # System input: vref, yref 106 | # System output: xd, yd, thetad, vd, phid 107 | # System parameters: none 108 | # 109 | def trajgen_output(t, x, u, params): 110 | vref, yref = u 111 | return np.array([vref * t, yref, 0, vref, 0]) 112 | 113 | # Define the trajectory generator as an input/output system 114 | trajgen = ct.nlsys( 115 | None, trajgen_output, name='trajgen', 116 | inputs=('vref', 'yref'), 117 | outputs=('xd', 'yd', 'thetad', 'vd', 'phid')) 118 | 119 | # 120 | # System construction 121 | # 122 | # The input to the full closed loop system is the desired lateral position and 123 | # the desired forward velocity. The output for the system is taken as the 124 | # full vehicle state plus the velocity of the vehicle. 125 | # 126 | # We construct the system using the interconnect function and using signal 127 | # labels to keep track of everything. 128 | 129 | steering = ct.interconnect( 130 | # List of subsystems 131 | (trajgen, controller, vehicle), name='steering', 132 | 133 | # System inputs 134 | inplist=['trajgen.vref', 'trajgen.yref'], 135 | inputs=['yref', 'vref'], 136 | 137 | # System outputs 138 | outlist=['vehicle.x', 'vehicle.y', 'vehicle.theta', 'controller.v', 139 | 'controller.phi'], 140 | outputs=['x', 'y', 'theta', 'v', 'phi'] 141 | ) 142 | 143 | # Set up the simulation conditions 144 | yref = 1 145 | T = np.linspace(0, 5, 100) 146 | 147 | # Set up a figure for plotting the results 148 | fbs.figure('mlh') 149 | 150 | # Plot the reference trajectory for the y position 151 | plt.plot([0, 5], [yref, yref], 'k-', linewidth=0.6) 152 | 153 | # Find the signals we want to plot 154 | y_index = steering.find_output('y') 155 | v_index = steering.find_output('v') 156 | 157 | # Do an iteration through different speeds 158 | for vref in [8, 10, 12]: 159 | # Simulate the closed loop controller response 160 | tout, yout = ct.input_output_response( 161 | steering, T, [vref * np.ones(len(T)), yref * np.ones(len(T))]) 162 | 163 | # Plot the reference speed 164 | plt.plot([0, 5], [vref, vref], 'k-', linewidth=0.6) 165 | 166 | # Plot the system output 167 | y_line, = plt.plot(tout, yout[y_index, :], 'r-') # lateral position 168 | v_line, = plt.plot(tout, yout[v_index, :], 'b--') # vehicle velocity 169 | 170 | # Add axis labels 171 | plt.xlabel('Time [s]') 172 | plt.ylabel(r'$\dot x$ [m/s], $y$ [m]') 173 | plt.legend((v_line, y_line), (r'$\dot x$', '$y$'), 174 | loc='center right', frameon=False) 175 | 176 | # Save the figure 177 | fbs.savefig('figure-8.13-steering_gainsched.png') # PNG for web 178 | fbs.savefig('steering-gainsched.eps') # EPS for book 179 | -------------------------------------------------------------------------------- /example-template.py: -------------------------------------------------------------------------------- 1 | # example-N.mm-short_title.py - 2 | # RMM, dd MMM yyyy 3 | # 4 | # Figure 3.15: 5 | 6 | import control as ct 7 | import numpy as np 8 | import matplotlib.pyplot as plt 9 | ct.use_fbs_defaults() 10 | 11 | # 12 | # System dynamics 13 | # 14 | 15 | # Set up the plotting grid to match the layout in the book 16 | fig = plt.figure(constrained_layout=True) 17 | gs = fig.add_gridspec(2, 2) 18 | 19 | # 20 | # (a) Description 21 | # 22 | 23 | ax = fig.add_subplot(gs[0, 0]) # first row, first column 24 | ax.set_title("(a) Description") 25 | 26 | # 27 | # (b) Description 28 | # 29 | 30 | ax = fig.add_subplot(gs[0, 1]) # first row, second column 31 | ax.set_title("(b) Description") 32 | 33 | # Save the figure 34 | plt.savefig("figure-N.mm-short_title.png", bbox_inches='tight') 35 | -------------------------------------------------------------------------------- /fbs.py: -------------------------------------------------------------------------------- 1 | # fbs.py - FBS customization 2 | # RMM, 8 Oct 2021 3 | 4 | import matplotlib.pyplot as plt 5 | 6 | # Set the fonts to match the main text 7 | plt.rc('font', family='Times New Roman', weight='normal', size=11) 8 | plt.rcParams['mathtext.fontset'] = 'cm' 9 | 10 | 11 | # Create a new figure of a specified size 12 | def figure(size='mlh'): 13 | if size == 'mlf' or size == '111': 14 | plt.figure(figsize=[3.4, 5.1]) 15 | elif size == 'mlh' or size == '221': 16 | plt.figure(figsize=[3.4, 2.55]) 17 | elif size == '211': 18 | plt.figure(figsize=[6.8, 2.55]) 19 | elif size == 'mlt' or size == '321': 20 | plt.figure(figsize=[3.4, 1.7]) 21 | else: 22 | raise ValueError("unknown figure size") 23 | return plt.gca() 24 | 25 | 26 | # Print a figure 27 | def savefig(name, pad=0.1, **kwargs): 28 | plt.tight_layout(pad=pad) # clean up plots 29 | plt.savefig(name) # save to file 30 | -------------------------------------------------------------------------------- /figure-1.11-cruise_robustness.py: -------------------------------------------------------------------------------- 1 | # figure-1.11-cruise_robustness.py - Cruise control w/ range of vehicle masses 2 | # RMM, 20 Jun 2021 3 | # 4 | # This script generates the response of the system to a 4 deg hill with 5 | # different vehicles masses. The figure shows how the velocity changes when 6 | # the car travels on a horizontal road and the slope of the road changes to 7 | # a constant uphill slope. The three different curves correspond to 8 | # differing masses of the vehicle, between 1200 and 2000 kg, demonstrating 9 | # that feedback can indeed compensate for the changing slope and that the 10 | # closed loop system is robust to a large change in the vehicle 11 | # characteristics. 12 | 13 | # Package import 14 | import numpy as np 15 | import matplotlib.pyplot as plt 16 | import control as ct 17 | import cruise # vehicle dynamics, PI controller 18 | 19 | # Define the time and input vectors 20 | T = np.linspace(0, 25, 101) 21 | vref = 20 * np.ones(T.shape) 22 | gear = 4 * np.ones(T.shape) 23 | theta0 = np.zeros(T.shape) 24 | 25 | # Now simulate the effect of a hill at t = 5 seconds 26 | theta_hill = np.array([ 27 | 0 if t <= 5 else 28 | 4./180. * np.pi * (t-5) if t <= 6 else 29 | 4./180. * np.pi for t in T]) 30 | 31 | # Create the plot and add a line at the reference speed 32 | plt.subplot(2, 1, 1) 33 | plt.axis([0, T[-1], 18.75, 20.25]) 34 | plt.plot([T[0], T[-1]], [vref[0], vref[-1]], 'k-') # reference velocity 35 | plt.plot([5, 5], [18.75, 20.25], 'k:') # disturbance start 36 | 37 | masses = [1200, 1600, 2000] 38 | for i, m in enumerate(masses): 39 | # Compute the equilibrium state for the system 40 | X0, U0 = ct.find_eqpt( 41 | cruise.cruise_PI, 42 | [vref[0], 0], 43 | [vref[0], gear[0], theta0[0]], 44 | iu=[1, 2], y0=[vref[0], 0], iy=[0], params={'m': m}) 45 | 46 | # Simulate the effect of a hill 47 | t, y = ct.input_output_response( 48 | cruise.cruise_PI, T, 49 | [vref, gear, theta_hill], 50 | X0, params={'m':m}) 51 | 52 | # Plot the response for this mass 53 | plt.plot(t, y[cruise.cruise_PI.find_output('v')], label='m = %d' % m) 54 | 55 | # Add labels and legend to the plot 56 | plt.xlabel('Time [s]') 57 | plt.ylabel('Velocity [m/s]') 58 | plt.legend() 59 | -------------------------------------------------------------------------------- /figure-1.18-airfuel_selectors.py: -------------------------------------------------------------------------------- 1 | # figure-1.18-airfuel_selectors.py - Air-fuel control example 2 | # RMM, 20 Jun 2021 3 | # 4 | # Air–fuel controller based on selectors. The right figure shows a 5 | # simulation where the power reference r is changed stepwise at t = 1 and t 6 | # = 15. Notice that the normalized air flow is larger than the normalized 7 | # fuel flow both for increasing and decreasing reference steps. 8 | 9 | # Package import 10 | import numpy as np 11 | import matplotlib.pyplot as plt 12 | import control as ct 13 | import cruise 14 | 15 | # 16 | # Air and fuel (oil) dynamics and controllers 17 | # 18 | # These dynamics come from Karl Astrom and are embedded in a SIMULINK 19 | # diagram used for the initial part of the book. The basic structure for 20 | # both the air and fuel controllers is a PI controller with output feedback 21 | # for the proportional term and integral feedback on the error. This cuts 22 | # the feedthrough term for the proportional feedback and gives a smoother 23 | # step response (see Figure 11.1b for the basic structure). 24 | # 25 | 26 | # Min selector for oil PI controller input 27 | min_block = ct.nlsys( 28 | updfcn=None, outfcn=lambda t, x, u, params: min(u), 29 | name='min', inputs=['u1', 'u2'], outputs='y') 30 | 31 | # Max selector for air PI controller input 32 | max_block = ct.nlsys( 33 | updfcn=None, outfcn=lambda t, x, u, params: max(u), 34 | name='max', inputs=['u1', 'u2'], outputs='y') 35 | 36 | # Oil and air flow dynamics (from KJA SIMULINK diagram) 37 | Po = ct.tf([1], [1, 1]) 38 | Pa = ct.tf([4], [1, 4]) 39 | 40 | # PI controller for oil flow 41 | kpo = 2; kio = 4 42 | Cio = ct.tf([kio], [1, 0]) 43 | Cpo = kpo 44 | oil_block = ct.tf( 45 | Po * Cio / (1 + Po * (Cio + Cpo)), 46 | name="oil", inputs='r', outputs='y') 47 | 48 | # PI controller for air flow 49 | kpa = 1; kia = 1 50 | Cia = ct.tf([kia], [1, 0]) 51 | Cpa = kpa 52 | air_block = ct.tf( 53 | Pa * Cia / (1 + Pa * (Cia + Cpa)), 54 | name="air", inputs='r', outputs='y') 55 | 56 | # 57 | # Air-fuel selector dynamics 58 | # 59 | # The selector dynamics are based on the diagram Figure 1.18a, where we have 60 | # already pre-computing the transfer function around the process/controller 61 | # pairs (so the air and oil blocks have input 'R' and output 'Y' from the 62 | # diagram). We use the interconnect function along with named signals to 63 | # set everything up. 64 | # 65 | 66 | airfuel = ct.interconnect( 67 | [min_block, max_block, oil_block, air_block], 68 | connections = ( 69 | ['oil.r', 'min.y'], 70 | ['air.r', 'max.y'], 71 | ['min.u2', 'air.y'], 72 | ['max.u1', 'oil.y']), 73 | inplist = [['min.u1', 'max.u2']], inputs='ref', 74 | outlist = ['air.y', 'oil.y'], outputs=['air', 'oil']) 75 | 76 | # 77 | # Input/output response 78 | # 79 | # Finally, we simulate the dynamics with an input singla as showin in Figure 80 | # 1.18b, consisting of a step increase from 0 to 1 at time t = 1 sec and 81 | # then a decrease from 1 to 0.5 at time t = 15 sec. 82 | # 83 | 84 | T = np.linspace(0, 30, 101) 85 | ref = np.array([ 86 | 0 if t <= 1 else 87 | 1 if t <= 15 else 88 | 0.5 for t in T]) 89 | t, y = ct.input_output_response(airfuel, T, ref) 90 | 91 | # Plot the results 92 | plt.subplot(2, 2, 1) 93 | plt.plot(t, ref, t, y[0], t, y[1]) 94 | 95 | plt.legend(['ref', 'air', 'fuel']) 96 | plt.xlabel('Time $t$ [sec]') 97 | plt.ylabel('Normalized signals') 98 | -------------------------------------------------------------------------------- /figure-2.11-2dof_stepresp.py: -------------------------------------------------------------------------------- 1 | # figure-2.11-2dof_stepresp.py - step responses for two DOF system 2 | # RMM, 21 Jun 2021 3 | # 4 | # Response to a step change in the reference signal for a system with a PI 5 | # controller having two degrees of freedom. The process transfer function is 6 | # P(s) = 1/s and the controller gains are kp = 1.414, ki = 1, and β = 0, 7 | # 0.5, and 1. 8 | # 9 | 10 | import numpy as np 11 | import matplotlib as mpl 12 | import matplotlib.pyplot as plt 13 | import control as ct 14 | 15 | s = ct.TransferFunction.s # define the differentiation operator 16 | mpl.rcParams['text.usetex'] = True # use LaTeX for formatting legend strings 17 | 18 | # Process model: integrator dynamics 19 | P = ct.tf([1], [1, 0]) 20 | 21 | # Set the simulation time vector 22 | time = np.linspace(0, 10, 100) 23 | 24 | # 25 | # Beta sweep 26 | # 27 | 28 | # Choose gains to use 29 | beta_list = [0, 0.5, 1] 30 | kp = 1.414 31 | ki = 1 32 | 33 | for beta in beta_list: 34 | C1 = beta * kp + ki / s 35 | C2 = (1 - beta) * kp 36 | 37 | Gyr = P * C1 / (1 + P * (C1 + C2)) 38 | Gur = C1 / (1 + P * (C1 + C2)) 39 | 40 | t, y = ct.step_response(Gyr, time) 41 | t, u = ct.step_response(Gur, time) 42 | 43 | if 'w_y_ax' not in locals(): 44 | w_y_ax = plt.subplot(3, 2, 1) 45 | plt.plot([time[0], time[-1]], [1, 1], 'k-') 46 | plt.xlabel('Time $t$') 47 | plt.ylabel('Output $y$') 48 | w_y_ax.plot(t, y) 49 | 50 | if 'w_u_ax' not in locals(): 51 | w_u_ax = plt.subplot(3, 2, 2) 52 | plt.xlabel('Time $t$') 53 | plt.ylabel('Input $u$') 54 | w_u_ax.plot(t, u, label=r"$\beta = %g$" % beta) 55 | 56 | # Label the omega sweep curves 57 | w_u_ax.legend(loc="upper right") 58 | 59 | # Overalll figure formating 60 | plt.tight_layout() 61 | -------------------------------------------------------------------------------- /figure-2.12,14-static_nlsys.py: -------------------------------------------------------------------------------- 1 | # figure-2.12,14-static_nlsys.py - static nonlinear feedback system 2 | # RMM, 21 Jun 2021 3 | # 4 | # Figure 2.12: Responses of a static nonlinear system. The left figure shows 5 | # the in- put/output relations of the open loop systems and the right figure 6 | # shows responses to the input signal (2.38). The ideal response is shown 7 | # with solid bold lines. The nominal response of the nonlinear system is 8 | # shown using dashed bold lines and the responses for different parameter 9 | # values are shown using thin lines. Notice the large variability in the 10 | # responses. 11 | # 12 | # Figure 2.14: Responses of the systems with integral feedback (ki = 13 | # 1000). The left figure shows the input/output relationships for the closed 14 | # loop systems, and the center figure shows responses to the input signal 15 | # (2.38) (compare to the corresponding responses in Figure 2.12. The right 16 | # figure shows the individual errors (solid lines) and the approximate error 17 | # given by equation (2.42) (dashed line). 18 | # 19 | # Intial code contributed by Adam Matic, 26 May 2021. 20 | # 21 | 22 | import numpy as np 23 | import matplotlib.pyplot as plt 24 | import control as ct 25 | 26 | # Static nonlinearity 27 | def F(u, alpha, beta): 28 | return alpha * (u + beta * (u ** 3)) 29 | 30 | # Reference signal 31 | t = np.linspace(0, 6, 300) 32 | r = np.sin(t) + np.sin(np.pi * t) + np.sin((np.pi**2) * t) 33 | 34 | # 35 | # Open loop response 36 | # 37 | 38 | # Set up the plots for Figure 2.12 39 | fig, [ax1, ax2] = plt.subplots(1, 2, figsize=(6, 3)) 40 | 41 | # Generate the input/output curves and system responses 42 | for a in [0.1, 0.2, 0.5]: 43 | for b in [0, 0.5, 1, 2]: 44 | y = F(r, a, b) 45 | ax1.plot(r, y, 'r', linewidth=0.5); 46 | ax2.plot(t, y, 'r', linewidth=0.5) 47 | 48 | # Generate the nominal response 49 | y = F(r, 0.2, 1) 50 | ax1.plot(r, y, 'b--', linewidth=1.5); 51 | ax2.plot(t, y, 'b--', linewidth=1.5) 52 | 53 | # Left plot labels 54 | ax1.set_title("I/O relationships") 55 | ax1.set_xlabel("Input $u$") 56 | ax1.set_ylabel("Output $y$") 57 | 58 | # Draw reference line, set axis limits 59 | ax1.plot(y, y, 'k-', linewidth=1.5) 60 | ax1.set_ylim(-3, 3) 61 | ax1.set_xlim(-2.5, 2.5) 62 | 63 | # Right plot labels 64 | ax2.set_title("Output signals") 65 | ax2.set_xlabel("Time $t$") 66 | ax2.set_ylabel("Output $y$") 67 | 68 | # Draw reference line, set axis limits 69 | ax2.plot(t, r, 'k-', linewidth=1.5) 70 | ax2.set_ylim(-1, 5) 71 | ax2.set_xlim(0, 2) 72 | 73 | plt.tight_layout() 74 | 75 | # 76 | # Closed loop response 77 | # 78 | 79 | # Create an I/O system representing the static nonlinearity 80 | P = ct.nlsys( 81 | updfcn=None, 82 | outfcn=lambda t, x, u, params: F(u, params['a'], params['b']), 83 | inputs=['u'], outputs=['y'], name='P') 84 | 85 | # Integral controller 86 | ki = 1000 87 | C = ct.tf([ki], [1, 0]) 88 | 89 | # Closed loop system 90 | sys = ct.feedback(P * C, 1) 91 | 92 | # Set up the plots for Figure 2.12 93 | fig, [ax1, ax2, ax3] = plt.subplots(1, 3, figsize=(6, 3)) 94 | 95 | # Generate the input/output curves and system responses 96 | for a in [0.1, 0.2, 0.5]: 97 | for b in [0, 0.5, 1, 2]: 98 | # Simulate the system dynamics 99 | t, y = ct.input_output_response(sys, t, r, params={'a':a, 'b':b}) 100 | 101 | ax1.plot(r, y, 'r', linewidth=0.5); 102 | ax2.plot(t, y, 'r', linewidth=0.5) 103 | ax3.plot(t, r-y, 'r', linewidth=0.5) 104 | 105 | # Left plot labels 106 | ax1.set_title("I/O relationships") 107 | ax1.set_xlabel("Input $u$") 108 | ax1.set_ylabel("Output $y$") 109 | 110 | # Draw reference line, set axis limits 111 | ax1.plot(y, y, 'k-', linewidth=1.5) 112 | ax1.set_ylim(-3, 3) 113 | ax1.set_xlim(-2.5, 2.5) 114 | 115 | # Center plot labels 116 | ax2.set_title("Output signals") 117 | ax2.set_xlabel("Time $t$") 118 | ax2.set_ylabel("Output $y$") 119 | 120 | # Draw reference line, set axis limits 121 | ax2.plot(t, r, 'k-', linewidth=1) 122 | ax2.set_ylim(-1, 5) 123 | ax2.set_xlim(0, 2) 124 | 125 | # Right plot labels 126 | ax3.set_title("Error") 127 | ax3.set_xlabel("Time $t$") 128 | ax3.set_ylabel("Error $e$") 129 | 130 | # Draw bounding line, set axis limits 131 | rdot = np.diff(r)/(t[1] - t[0]) # Approximation of derivative 132 | bmin = 0.1 # See FBS2e, below equation (2.40) 133 | ax3.plot(t[:-1], rdot/(bmin * ki), 'b--', linewidth=1.5) 134 | ax3.set_xlim(0, 2) 135 | 136 | plt.tight_layout() 137 | -------------------------------------------------------------------------------- /figure-2.19-posfbk_saturation.py: -------------------------------------------------------------------------------- 1 | # figure-2.19-posfbk_saturation.py - positive feedback with saturation 2 | # RMM, 22 Jun 2021 3 | # 4 | # Figure 2.19: System with positive feedback and saturation. (a) For a 5 | # fixed reference value r, the intersections with the curve r = G(y) 6 | # corresponds to equilibrium points for the system. Equilibrium points 7 | # at selected values of r are shown by circles (note that for some 8 | # reference values there are multiple equilibrium points). Arrows 9 | # indicate the sign of the derivative of y away from the equilibrium 10 | # points, with the solid portions of r = G(y) representing stable 11 | # equilibrium points and dashed portions representing unstable 12 | # equilibrium points. (b) The hysteretic input/output map given by y = 13 | # G+(r), showing that some values of r have single equilibrium points 14 | # while others have two possible (stable) steady-state output val- 15 | # ues. (c) Simulation of the system dynamics showing the reference r 16 | # (dashed curve) and the output y (solid curve). 17 | 18 | import numpy as np 19 | import matplotlib.pyplot as plt 20 | import control as ct 21 | 22 | # Nonlinearity: forward and reverse 23 | def F(x): 24 | return x / (1 + np.abs(x)) 25 | 26 | def Finv(y): 27 | return y / (1 - np.abs(y)) 28 | 29 | # Equilibrium calculation 30 | a = 1; b = 4; # parameters for the system dynamics 31 | def G(y, a=a, b=b): 32 | return a * Finv(y) / b - y 33 | 34 | # 35 | # Stable and unstable equilibrium points 36 | # 37 | plt.subplot(2, 2, 1) 38 | plt.title('Stable and unstable eq points') 39 | plt.xlabel('$y$') 40 | plt.ylabel('$r = G(y)$') 41 | 42 | # Define the stable and unstable branches 43 | ymax = -1 + np.sqrt(a/b) # First maximum (negative value) 44 | y_stable = np.linspace(-ymax, 0.85, 100) 45 | y_unstable = np.linspace(ymax, -ymax, 100) 46 | 47 | # Plot the locus of equilibrium piots 48 | plt.plot(-y_stable, G(-y_stable), 'b-') 49 | plt.plot(y_unstable, G(y_unstable), 'b--') 50 | plt.plot(y_stable, G(y_stable), 'b-') 51 | 52 | # Plot individual equlibrium points 53 | for r in [0, G(ymax), 0.5]: 54 | # Find the values intersecting this value of r 55 | y_left_arg = np.argwhere(G(-y_stable) >= r) 56 | if y_left_arg.size > 0: 57 | y = -y_stable[y_left_arg[-1].item()] 58 | plt.plot(y, r, 'o', fillstyle='none') 59 | 60 | y_center_arg = np.argwhere(G(y_unstable) >= r) 61 | if y_center_arg.size > 0: 62 | y = y_unstable[y_center_arg[-1].item()] 63 | plt.plot(y, r, 'o', fillstyle='none') 64 | 65 | y_right_arg = np.argwhere(G(y_stable) <= r) 66 | if y_right_arg.size > 0: 67 | y = y_stable[y_right_arg[-1].item()] 68 | plt.plot(y, r, 'o', fillstyle='none') 69 | 70 | # 71 | # Hysteretic input/output map y=G+(r) 72 | # 73 | plt.subplot(2, 2, 2) 74 | plt.title('Hysteretic input/output map') 75 | plt.xlabel('$r$') 76 | plt.ylabel(r'$y = G^\dagger(r)$') 77 | 78 | # Plot y versus r (multi-valued) 79 | plt.plot(G(y_stable), y_stable, 'b-') # Upper branch 80 | plt.plot(G(-y_stable), -y_stable, 'b-') # Lower branch 81 | 82 | # Transition lines (look for intersection on opposite branch) 83 | plt.plot( 84 | [G(y_stable[0]), G(y_stable[0])], 85 | [y_stable[0], 86 | -y_stable[np.argwhere(G(-y_stable) > G(y_stable[0]))[-1].item()]], 87 | 'b--') 88 | plt.plot( 89 | [G(-y_stable[0]), G(-y_stable[0])], 90 | [-y_stable[0], 91 | y_stable[np.argwhere(G(y_stable) < G(-y_stable[0]))[-1].item()]], 92 | 'b--') 93 | 94 | # 95 | # Input/output behavior 96 | # 97 | plt.subplot(2, 1, 2) 98 | plt.title('Input/output behavior') 99 | 100 | # Closed loop dynamics 101 | linsys = ct.tf([b], [1, a]) 102 | nonlin = ct.nlsys( 103 | updfcn=None, outfcn=lambda t, x, u, params: F(u), 104 | inputs=1, outputs=1) 105 | posfbk = ct.feedback(nonlin * linsys, 1, 1) 106 | 107 | # Simulate and plot 108 | t = np.linspace(0, 100, 100) 109 | r = 4 * np.sin(0.2 * t) + np.sin(0.1 * t) 110 | t, y = ct.input_output_response(posfbk, t, r) 111 | plt.plot(t, r, 'b--') 112 | plt.plot(t, y, 'r') 113 | plt.plot(t, np.zeros_like(t), 'k') 114 | 115 | plt.tight_layout() 116 | -------------------------------------------------------------------------------- /figure-2.8-PI_step_responses.py: -------------------------------------------------------------------------------- 1 | # figure-2.8-PI_step_reesponses.py - step responses for P/PI controllers 2 | # RMM, 21 Jun 2021 3 | # 4 | # Step responses for a first-order, closed loop system with proportional 5 | # control and PI control. The process transfer function is P = 2/(s + 1). 6 | # The controller gains for proportional control are k_p = 0, 0.5, 1, and 7 | # 2. The PI controller is designed using equation (2.28) with zeta_c = 0.707 8 | # and omega_c = 0.707, 1, and 2, which gives the controller parameters k_p = 9 | # 0, 0.207, and 0.914 and k_i = 0.25, 0.50, and 2. 10 | # 11 | 12 | import numpy as np 13 | import matplotlib.pyplot as plt 14 | import control as ct 15 | 16 | # Process model 17 | b = 2; a = 1 18 | P = ct.tf([b], [1, a]) 19 | 20 | # Set the simulation time vector 21 | time = np.linspace(0, 8, 100) 22 | 23 | # 24 | # Proportional control 25 | # 26 | 27 | # Choose gains to use 28 | kp_gains = [0, 0.5, 1, 2] 29 | 30 | for kp in kp_gains: 31 | Gyv = ct.tf([b], [1, a + b*kp]) 32 | Guv = ct.tf([-b*kp], [1, a + b*kp], dt=0) # force kp=0 to be cts time 33 | 34 | t, y = ct.step_response(Gyv, time) 35 | t, u = ct.step_response(Guv, time) 36 | 37 | if 'p_y_ax' not in locals(): 38 | p_y_ax = plt.subplot(3, 2, 1) 39 | plt.ylabel('Output $y$') 40 | plt.title('Proportional control') 41 | p_y_ax.plot(t, y) 42 | 43 | if 'p_u_ax' not in locals(): 44 | p_u_ax = plt.subplot(3, 2, 3) 45 | plt.ylabel('Input $u$') 46 | plt.xlabel('Normalized time $at$') 47 | p_u_ax.plot(t, u, label="kp = %0.3g" % kp) 48 | 49 | # Label proportional control curves 50 | p_u_ax.legend() 51 | 52 | # 53 | # PI control 54 | # 55 | 56 | # Figure out frequency of critical damping 57 | zeta = 0.707 58 | wc = a / 2 / zeta 59 | 60 | # Plot results for different resonate frequencies 61 | wc_list = [wc, 1, 2] 62 | for wc in wc_list: 63 | kp = (2 * zeta * wc - a) / b 64 | ki = wc**2 / b 65 | 66 | Gyv = ct.tf([b, 0], [1, a + b*kp, b*ki]) 67 | Guv = -ct.tf([b*kp, b*ki], [1, a + b*kp, b*ki], dt=0) 68 | 69 | t, y = ct.step_response(Gyv, time) 70 | t, u = ct.step_response(Guv, time) 71 | 72 | if 'pi_y_ax' not in locals(): 73 | pi_y_ax = plt.subplot(3, 2, 2) 74 | plt.ylabel('Output $y$') 75 | plt.title('Proportional-integral control') 76 | pi_y_ax.plot(t, y) 77 | 78 | if 'pi_u_ax' not in locals(): 79 | pi_u_ax = plt.subplot(3, 2, 4) 80 | plt.ylabel('Input $u$') 81 | plt.xlabel('Normalized time $at$') 82 | pi_u_ax.plot(t, u, label="wc = %0.3g" % wc) 83 | 84 | # Label PI curves 85 | pi_u_ax.legend() 86 | 87 | # Overall figure labeling 88 | plt.tight_layout() 89 | -------------------------------------------------------------------------------- /figure-2.9-secord_stepresp.py: -------------------------------------------------------------------------------- 1 | # figure-2.9-secord_stepresp.py - step responses for second order systems 2 | # RMM, 21 Jun 2021 3 | # 4 | # Responses to a unit step change in the reference signal for different 5 | # values of the design parameters \omega_c and \zeta_c. The left column 6 | # shows responses for fixed \zeta_c = 0.707 and \omega_c = 1, 2, and 5. The 7 | # right figure column responses for \omega_c = 2 and \zeta_c = 0.5, 0.707, 8 | # and 1. The process parameters are a = b = 1. The initial value of the 9 | # control signal is kp. 10 | # 11 | 12 | import numpy as np 13 | import matplotlib.pyplot as plt 14 | import control as ct 15 | 16 | # Process model 17 | b = 1; a = 1 18 | P = ct.tf([b], [1, a]) 19 | 20 | # Set the simulation time vector 21 | time = np.linspace(0, 6, 100) 22 | 23 | # 24 | # Omega sweep 25 | # 26 | 27 | # Choose gains to use 28 | wc_list = [1, 2, 5] 29 | zc = 0.707 30 | 31 | for wc in wc_list: 32 | kp = (2 * zc * wc - a) / b 33 | ki = wc**2 34 | C = ct.tf([kp, ki], [1, 0]) 35 | 36 | Gyr = P*C / (1 + P*C) 37 | Gur = C / (1 + P*C) 38 | 39 | t, y = ct.step_response(Gyr, time) 40 | t, u = ct.step_response(Gur, time) 41 | 42 | if 'w_y_ax' not in locals(): 43 | w_y_ax = plt.subplot(3, 2, 1) 44 | plt.ylabel('Output $y$') 45 | plt.title(r"Sweep $\omega_c$, $\zeta_c = %g$" % zc) 46 | w_y_ax.plot(t, y) 47 | 48 | if 'w_u_ax' not in locals(): 49 | w_u_ax = plt.subplot(3, 2, 3) 50 | plt.ylabel('Input $u$') 51 | plt.xlabel(r'Normalized time $\omega_c t$') 52 | w_u_ax.plot(t, u, label=r"$\omega_c = %g$" % wc) 53 | 54 | # Label the omega sweep curves 55 | w_u_ax.legend(loc="upper right") 56 | 57 | # 58 | # Zeta sweep 59 | # 60 | 61 | # Figure out frequency of critical damping 62 | wc = 2 63 | zc_list = [0.5, 0.707, 1] 64 | 65 | # Plot results for different resonate frequencies 66 | for zc in zc_list: 67 | kp = (2 * zc * wc - a) / b 68 | ki = wc**2 69 | C = ct.tf([kp, ki], [1, 0]) 70 | 71 | Gyr = P*C / (1 + P*C) 72 | Gur = C / (1 + P*C) 73 | 74 | t, y = ct.step_response(Gyr, time) 75 | t, u = ct.step_response(Gur, time) 76 | 77 | if 'z_y_ax' not in locals(): 78 | z_y_ax = plt.subplot(3, 2, 2) 79 | plt.ylabel('Output $y$') 80 | plt.title(r"Sweep $\zeta_c$, $\omega_c = %g$" % wc) 81 | z_y_ax.plot(t, y) 82 | 83 | if 'z_u_ax' not in locals(): 84 | z_u_ax = plt.subplot(3, 2, 4) 85 | plt.ylabel('Input $u$') 86 | plt.xlabel(r'Normalized time $\omega_c t$') 87 | z_u_ax.plot(t, u, label=r"$\zeta_c = %g$" % zc) 88 | 89 | # Label the zeta sweep curves 90 | z_u_ax.legend(loc="upper right") 91 | 92 | # Overalll figure labeling 93 | plt.tight_layout() 94 | -------------------------------------------------------------------------------- /figure-3.11-spring_mass_simulation.py: -------------------------------------------------------------------------------- 1 | # figure-3.11-spring_mass_simulation.py - forced spring–mass simulation approx 2 | # RMM, 28 Aug 2021 3 | # 4 | # Figure 3.11: Simulation of the forced spring–mass system with 5 | # different simulation time constants. The solid line represents the 6 | # analytical solution. The dashed lines represent the approximate 7 | # solution via the method of Euler integration, using decreasing step 8 | # sizes. 9 | # 10 | 11 | import numpy as np 12 | import matplotlib.pyplot as plt 13 | import control as ct 14 | ct.use_fbs_defaults() 15 | 16 | # Parameters defining the system 17 | m = 250 # system mass 18 | k = 40 # spring constant 19 | b = 60 # damping constant 20 | 21 | # System matrices 22 | A = [[0, 1], [-k/m, -b/m]] 23 | B = [0, 1/m] 24 | C = [1, 0] 25 | sys = ct.ss(A, B, C, 0) 26 | 27 | # 28 | # Discrete time simulation 29 | # 30 | # This section explores what happens when we discretize the ODE 31 | # and convert it to a discrete time simulation. 32 | # 33 | Af = 20 # forcing amplitude 34 | omega = 0.5 # forcing frequency 35 | 36 | # Sinusoidal forcing function 37 | t = np.linspace(0, 100, 1000) 38 | u = Af * np.sin(omega * t) 39 | 40 | # Simulate the system using standard MATLAB routines 41 | response = ct.forced_response(sys, t, u) 42 | ts, ys = response.time, response.outputs 43 | 44 | # 45 | # Now generate some simulations manually 46 | # 47 | 48 | # Time increments for discrete approximations 49 | hvec = [1, 0.5, 0.1] # h must be a multiple of 0.1 50 | max_len = int(t[-1] / min(hvec)) # maximum number of time steps 51 | 52 | # Create arrays for storing results 53 | td = np.zeros((len(hvec), max_len)) # discrete time instants 54 | yd = np.zeros((len(hvec), max_len)) # output at discrete time instants 55 | 56 | # Discrete time simulations 57 | maxi = [] # list to store maximum index 58 | for iter, h in enumerate(hvec): 59 | maxi.append(round(t[-1] / h)) # save maximum index for this h 60 | x = np.zeros((2, maxi[-1] + 1)) # create an array to store the state 61 | 62 | # Compute the discrete time Euler approximation of the dynamics 63 | for i in range(maxi[-1]): 64 | offset = int(h/0.1 * i) # input offset 65 | x[:, i+1] = x[:, i] + h * (sys.A @ x[:, i] + (sys.B * u[offset])[:, 0]) 66 | td[iter, i] = (i-1) * h 67 | yd[iter, i] = (sys.C @ x[:, i]).item() 68 | 69 | # Plot the results 70 | plt.subplot(2, 1, 1) 71 | 72 | simh = plt.plot( 73 | td[0, 0:maxi[0]], yd[0, 0:maxi[0]], 'g+--', 74 | td[1, 0:maxi[1]], yd[1, 0:maxi[1]], 'ro--', 75 | td[2, 0:maxi[2]], yd[2, 0:maxi[2]], 'b--', 76 | markersize=4, linewidth=1 77 | ) 78 | analh = plt.plot(ts, ys, 'k-', linewidth=1) 79 | 80 | plt.xlabel("Time [s]") 81 | plt.ylabel("Position $q$ [m]") 82 | plt.axis([0, 50, -2, 2]) 83 | plt.legend(["$h = %g$" % h for h in hvec] + ["analytical"], loc="lower left") 84 | 85 | # Save the figure 86 | plt.savefig("figure-3.11-spring_mass_simulation.png", bbox_inches='tight') 87 | -------------------------------------------------------------------------------- /figure-3.12-frequency_response.py: -------------------------------------------------------------------------------- 1 | # figure-3.12-frequency_response.py - frequency response computed by simulation 2 | # 3 | # Figure 3.12: A frequency response (gain only) computed by measuring 4 | # the response of individual sinusoids. The figure on the left shows 5 | # the response of the system as a function of time to a number of 6 | # different unit magnitude inputs (at different frequencies). The 7 | # figure on the right shows this same data in a different way, with 8 | # the magnitude of the response plotted as a function of the input 9 | # frequency. The filled circles correspond to the particular 10 | # frequencies shown in the time responses. 11 | # 12 | 13 | import numpy as np 14 | import matplotlib.pyplot as plt 15 | import control as ct 16 | ct.use_fbs_defaults() 17 | 18 | # System definition - third order, state space system 19 | A = [[-0.2, 2, 0], [-0.5, -0.2, 4], [0, 0, -10]] 20 | B = [0, 0, 1] 21 | C = [2.6, 0, 0] 22 | sys = ct.ss(A, B, C, 0) * 1.4 # state space object (with tweaked scale) 23 | 24 | # Set up the plotting grid to match the layout in the book 25 | fig = plt.figure(constrained_layout=True) 26 | gs = fig.add_gridspec(2, 2) 27 | 28 | # 29 | # (a) The response of the system as a function of time to a number of 30 | # different unit magnitude inputs (at different frequencies). 31 | # 32 | 33 | fig.add_subplot(gs[0, 0]) # first row, first column 34 | 35 | # List of frequencies for the time simulations (and frequency response points) 36 | omega_time = [0.1, 0.4, 1, 3] 37 | mag_time = [] # list to store magnitude of responses 38 | 39 | # Manual computation of the frequency response 40 | for omega in omega_time: 41 | # Compute out the time vector and inputs 42 | t = np.linspace(0, 50, 1000) 43 | u = np.sin(omega * t) 44 | 45 | # Simulate the system 46 | response = ct.forced_response(sys, t, u) 47 | 48 | # Plot the output 49 | plt.plot(response.time, response.outputs, 'b-') 50 | 51 | # Compute the magnitude of the response (avoiding initial transient) 52 | mag_time.append(max(response.outputs[500:])) 53 | 54 | # Add grid lines 55 | plt.xticks([0, 10, 20, 30, 40, 50]) 56 | plt.grid(which='major') 57 | 58 | # Label the plot 59 | plt.xlabel("Time [s]") 60 | plt.ylabel("Output, $y$") 61 | plt.title("Time domain simulations") 62 | 63 | # 64 | # (b) The same data in a different way, with the magnitude of the 65 | # response plotted as a function of the input frequency. The filled 66 | # circles correspond to the particular frequencies shown in the time 67 | # responses. 68 | # 69 | 70 | fig.add_subplot(gs[0, 1]) # first row, second column 71 | 72 | # List of frequencies to compute the frequency response 73 | omega_freq = [0.1, 0.2, 0.4, 1, 1.6, 3, 8, 10] 74 | mark_index = [1, 4, 6] # frequencies to mark on the plot 75 | mag_freq = [] # list to store magnitude of responses 76 | 77 | # Manual computation of the frequency response 78 | for omega in omega_freq: 79 | # Compute out the time vector and inputs 80 | t = np.linspace(0, 50, 1000) 81 | u = np.sin(omega * t) 82 | 83 | # Simulate the system 84 | response = ct.forced_response(sys, t, u) 85 | 86 | # Compute the magnitude of the response (avoiding initial transient) 87 | mag_freq.append(max(response.outputs[500:])) 88 | 89 | # Figure out which frequency points to mark 90 | omega_mark = np.array(omega_freq)[mark_index] 91 | mag_mark = np.array(mag_freq)[mark_index] 92 | 93 | # Plot the results 94 | plt.loglog(omega_freq, mag_freq, 'b-') 95 | plt.loglog(omega_mark, mag_mark, 'bo', markerfacecolor='none') 96 | plt.loglog(omega_time, mag_time, 'bo') 97 | 98 | plt.grid(axis='x', which='minor') 99 | plt.grid(axis='y', which='major') 100 | 101 | # Label the plot 102 | plt.xlabel("Frequency [rad/s]") 103 | plt.ylabel("Gain (log scale)") 104 | plt.title("Frequency response") 105 | 106 | plt.savefig("figure-3.12-frequency_response.png", bbox_inches='tight') 107 | -------------------------------------------------------------------------------- /figure-3.2-state_model.py: -------------------------------------------------------------------------------- 1 | # figure-3.2-state_mode.py - illustration of a state model 2 | # RMM, 2 Jul 2021 3 | # 4 | # Figure 3.2: Illustration of a state model. A state model gives the rate of 5 | # change of the state as a function of the state. The plot on the left shows 6 | # the evolution of the state as a function of time. The plot on the right, 7 | # called a phase portrait, shows the evolution of the states relative to 8 | # each other, with the velocity of the state denoted by arrows. 9 | # 10 | 11 | import numpy as np 12 | import scipy as sp 13 | import matplotlib.pyplot as plt 14 | import control as ct 15 | 16 | # 17 | # Spring mass system with nonlinear dampling 18 | # 19 | # This function gives the dynamics for a dampled oscillator with nonlinear 20 | # damping. The states of the system are 21 | # 22 | # x[0] position 23 | # x[1] velocity 24 | # 25 | # The nonlinear damping is implemented as a change in the linear damping 26 | # coefficient at a small velocity. This is intended to roughly correspond 27 | # to some sort of stiction (and give an interesting phase portrait). The 28 | # default parameters for the system are given by 29 | # 30 | # m = 1 mass, kg 31 | # k = 1 spring constant, N/m 32 | # b1 = 1 damping constant near origin, N-sec/m 33 | # b2 = 0.01 damping constant away from origin, N-sec/m 34 | # dth = 0.5 threshold for switching between damping 35 | # 36 | # This corresponds to a fairly lightly damped oscillator away from the origin. 37 | 38 | def _nlspringmass(t, x, u, params): 39 | m = params.get('m', 1) 40 | k = params.get('k', 1) 41 | b1 = params.get('b1', 2) 42 | b2 = params.get('b2', 0.01) 43 | dth = params.get('dth', 0.2) 44 | 45 | # Compute the friction force 46 | if abs(x[1]) < dth: 47 | Fb = b1 * x[1]; 48 | elif x[1] < 0: 49 | Fb = -b1 * dth \ 50 | + b2 * (x[1] + dth); 51 | else: 52 | Fb = b1 * dth \ 53 | + b2 * (x[1] - dth); 54 | 55 | # Return the time derivative of the state 56 | return np.array([x[1], -k/m * x[0] - Fb/m]) 57 | nlspringmass = ct.nlsys(_nlspringmass, None, states=2, inputs=0, outputs=2) 58 | 59 | # 60 | # (a) Simulation of the nonlinear spring mass system 61 | # 62 | plt.subplot(2, 2, 1) 63 | 64 | t = np.linspace(0, 16, 100) 65 | resp = ct.input_output_response(nlspringmass, t, 0, [2, 0]) 66 | y = resp.outputs 67 | 68 | plt.plot(t, y[0], '-', t, y[1], '--') 69 | plt.xlabel('Time $t$ [s]') 70 | plt.ylabel(r'Position $q$ [m], velocity $\dot q$̇ [m/s]') 71 | plt.title('Time plot') 72 | plt.legend(['Position $q$', 'Velocity $v$']) 73 | 74 | # 75 | # (b) Generate a phase plot for the damped oscillator 76 | # 77 | ax = plt.subplot(2, 2, 2) 78 | 79 | cplt = ct.phase_plane_plot( 80 | nlspringmass, # dynamics 81 | [-1, 1, -1, 1], # bounds of the plot 82 | gridspec=[8, 8], # number of points for vectorfield 83 | plot_vectorfield=True, # plot vectorfield 84 | plot_streamlines=False, # plot streamlines separately 85 | plot_separatrices=False, # leave off separatrices 86 | ax=ax 87 | ) 88 | ct.phaseplot.streamlines( # Plot streamlines from selected points 89 | nlspringmass, 90 | np.array([[-1, 0.4], [0.1, 1], [1, -0.4], [-0.1, -1]]), 91 | 10, ax=ax 92 | ) 93 | 94 | plt.xlabel('Position $q$ [m]') 95 | plt.ylabel(r'Velocity $\dot q$ [m/s]') 96 | plt.title('Phase portrait') 97 | plt.axis([-1, 1, -1, 1]) 98 | 99 | plt.tight_layout() 100 | -------------------------------------------------------------------------------- /figure-3.4-io_response.py: -------------------------------------------------------------------------------- 1 | # figure-3-1-io_response.py - input/output response of a linear system 2 | # RMM, 28 Aug 2021 3 | # 4 | # Figure 3.4: Input/output response of a linear system. The step 5 | # response (a) shows the output of the system due to an input that 6 | # changes from 0 to 1 at time t = 5 s. The frequency response (b) 7 | # shows the amplitude gain and phase change due to a sinusoidal input 8 | # at different frequencies. 9 | # 10 | 11 | import numpy as np 12 | import scipy as sp 13 | import matplotlib.pyplot as plt 14 | import control as ct 15 | ct.use_fbs_defaults() # Use settings to match FBS 16 | 17 | # System definition - third order, state space system 18 | A = [[-0.2, 2, 0], [-0.5, -0.2, 4], [0, 0, -10]] 19 | B = [0, 0, 1] 20 | C = [2.6, 0, 0] 21 | sys = ct.ss(A, B, C, 0) # state space object 22 | 23 | # Set up the plotting grid to match the layout in the book 24 | fig = plt.figure(constrained_layout=True) 25 | gs = fig.add_gridspec(4, 2) 26 | 27 | # 28 | # (a) Step response showing the output of the system due to an input 29 | # that changes from 0 to 1 at time t = 5 s 30 | # 31 | 32 | fig.add_subplot(gs[0:2, 0]) # first column 33 | 34 | # Create an input signal that is zero until time t = 5 35 | t = np.linspace(0, 30, 100) 36 | u = np.ones_like(t) 37 | u[t < 5] = 0 38 | 39 | # Compute the response 40 | response = ct.forced_response(sys, t, u) 41 | y = response.outputs 42 | 43 | # Plot the response 44 | plt.plot(t, u, 'b--', label="Input") 45 | plt.plot(t, y, 'r-', label="Output") 46 | plt.xlabel("Time (sec)") 47 | plt.ylabel("Input, output") 48 | plt.title("Step response") 49 | plt.legend() 50 | 51 | # 52 | # (b) Frequency` response showing the amplitude gain and phase change 53 | # due to a sinusoidal input at different frequencies 54 | # 55 | 56 | # Set up the axes for plotting (labels are recognized by bode_plot()) 57 | mag = fig.add_subplot(gs[0, 1], label='control-bode-magnitude') 58 | phase = fig.add_subplot(gs[1, 1], label='control-bode-phase') 59 | 60 | # Generate the Bode plot 61 | ct.bode_plot(sys) 62 | 63 | # Adjust the appearance to match the book 64 | mag.xaxis.set_ticklabels([]) 65 | mag.set_title("Frequency response") 66 | -------------------------------------------------------------------------------- /figure-4.12-congctrl_eqplot.py: -------------------------------------------------------------------------------- 1 | # congctrl_eqplot.py - congestion control equilibrium point plot 2 | # RMM, 29 Jun 2007 (converted from MATLAB) 3 | # 4 | # The equilibrium buffer size be for a set of N identical computers sending 5 | # packets through a single router with drop probability ρb. 6 | # 7 | 8 | import matplotlib.pyplot as plt 9 | import numpy as np 10 | import scipy.optimize 11 | import fbs # FBS plotting customizations 12 | 13 | # Range of values to plot (\alpha = 1/(2\rho^2 N^2) 14 | alpha_vals = np.logspace(-2, 4) 15 | 16 | # Solve for the equilibrium value of \rho b_e 17 | bratio_vals = [] 18 | for alpha in alpha_vals: 19 | # Define a function for the equilibrium point (equation (4.22)) 20 | def equilibrium(bratio): 21 | return alpha * bratio**3 + bratio - 1 22 | 23 | bratio = scipy.optimize.fsolve(equilibrium, 0) 24 | bratio_vals.append(bratio) 25 | 26 | # Set up a figure for plotting the results 27 | fbs.figure('mlh') 28 | 29 | # Plot the equilibrium buffer length 30 | plt.semilogx(alpha_vals, bratio_vals) 31 | plt.xlabel(r"$1/(2 \rho^2 N^2)$ (log scale)") 32 | plt.ylabel(r"$\rho b_{e}$") 33 | plt.title("Operating point") 34 | 35 | # Save the figure 36 | fbs.savefig('figure-4.12-congctrl_eqplot.png') # PNG for web 37 | -------------------------------------------------------------------------------- /figure-4.13-congctrl_tcpsim.py: -------------------------------------------------------------------------------- 1 | # tcpsim.m - congestion control simulation 2 | # RMM, 9 Sep 2006 (from MATLAB) 3 | 4 | import matplotlib.pyplot as plt 5 | import numpy as np 6 | import random 7 | import control as ct 8 | import congctrl 9 | import fbs # FBS plotting customizations 10 | 11 | # Create an I/O system for simulation 12 | M = 6 # simulate 6 aggregated sources 13 | N = 60 # modeling 60 independent sources 14 | congctrl_sys = congctrl.create_iosystem(M, N=N) 15 | 16 | # Find the equilibrium point 17 | xeq, ueq = ct.find_eqpt(congctrl_sys, np.ones(M+1), 0) 18 | weq, beq = xeq[0], xeq[-1] # first N states are identical 19 | 20 | # Compute a perturbation for the initial condition 21 | random.seed(6) # for repeatability 22 | w0 = np.array([ 23 | weq + 2 * (random.random() - 0.5) * weq for i in range(M)]) 24 | b0 = beq/2 25 | 26 | # Run a simulation 27 | tvec = np.linspace(0, 500, 100) 28 | resp = ct.input_output_response( 29 | congctrl_sys, tvec, U=0, X0=[w0, b0]) 30 | 31 | # Plot the results 32 | # Set up a figure for plotting the results 33 | fbs.figure('mlh') 34 | 35 | for i in range(M): 36 | plt.plot(resp.time, resp.states[i], 'k') 37 | plt.plot(resp.time, resp.states[-1] / 20, 'b') 38 | 39 | # Now change the number of sources and rerun from the old initial condition 40 | M = 4 # simulate 4 aggregated sources 41 | N = 40 # modeling 40 independent sources 42 | congctrl_pert = congctrl.create_iosystem(M, N=N) 43 | 44 | # Run a simulation starting from where we left off 45 | tvec = np.linspace(500, 1000, 100) 46 | pert = ct.input_output_response( 47 | congctrl_pert, tvec, U=0, X0=[resp.states[0:4, -1], resp.states[-1, -1]]) 48 | 49 | # Plot the results 50 | for i in range(M): 51 | plt.plot(pert.time, pert.states[i], 'k') 52 | plt.plot(pert.time, pert.states[-1] / 20, 'b') 53 | 54 | # Label the plots and make them pretty 55 | plt.axis([0, 1000, 0, 20]) 56 | plt.xlabel("Time $t$ [ms]") 57 | plt.ylabel("States $w_i$ [pkts/ms], $b$ [pkts]") 58 | plt.title("Time response") 59 | plt.text(250, 3, "$w_1$ - $w_{60}$") 60 | plt.text(700, 4, "$w_1$ - $w_{40}$") 61 | plt.text(700, 10, "$b$") 62 | 63 | # Save the figure 64 | fbs.savefig('figure-4.13-congctrl_tcpsim.png') # PNG for web 65 | -------------------------------------------------------------------------------- /figure-4.20-predprey_ctstime.py: -------------------------------------------------------------------------------- 1 | # predprey_ctstime.py - Predator-prey model in continuous time 2 | # RMM, 28 May 2023 3 | 4 | import matplotlib.pyplot as plt 5 | import numpy as np 6 | import control as ct 7 | import fbs # FBS plotting customizations 8 | 9 | # Define the dynamis for the predator-prey system (no input) 10 | def predprey_update(t, x, u, params={}): 11 | """Predator prey dynamics""" 12 | r = params.get('r', 1.6) 13 | d = params.get('d', 0.56) 14 | b = params.get('b', 0.6) 15 | k = params.get('k', 125) 16 | a = params.get('a', 3.2) 17 | c = params.get('c', 50) 18 | 19 | # Dynamics for the system 20 | dx0 = r * x[0] * (1 - x[0]/k) - a * x[1] * x[0]/(c + x[0]) 21 | dx1 = b * a * x[1] * x[0] / (c + x[0]) - d * x[1] 22 | 23 | return np.array([dx0, dx1]) 24 | 25 | # Create a nonlinear I/O system 26 | predprey_sys = ct.nlsys(predprey_update, states=2) 27 | 28 | # Simulate a trajectory leading to a limit cycle 29 | timepts = np.linspace(0, 70, 500) 30 | sim = ct.input_output_response(predprey_sys, timepts, 0, [25, 20]) 31 | 32 | # Plot the results 33 | fbs.figure('mlh') # FBS conventions 34 | plt.plot(sim.time, sim.states[0], 'b-', label="Hare") 35 | plt.plot(sim.time, sim.states[1], 'r--', label="Lynx") 36 | plt.legend() 37 | plt.xlabel("Time $t$ [years]") 38 | plt.ylabel("Population") 39 | plt.title("Time response") 40 | 41 | # Save the figure 42 | fbs.savefig('figure-4.20-predprey_ctstime-sim.png') # PNG for web 43 | 44 | # Generate a phase portrait 45 | fbs.figure('mlh') 46 | ct.phaseplot.equilpoints(predprey_sys, [-5, 126, -5, 100]) 47 | ct.phaseplot.streamlines( 48 | predprey_sys, np.array([ 49 | [0, 100], [1, 0], 50 | ]), 10, color='b') 51 | ct.phaseplot.streamlines( 52 | predprey_sys, np.array([[124, 1]]), np.linspace(0, 10, 500), color='b') 53 | ct.phaseplot.streamlines( 54 | predprey_sys, np.array([[125, 25], [125, 50], [125, 75]]), 3, color='b') 55 | ct.phaseplot.streamlines(predprey_sys, np.array([2, 8]), 6, color='b') 56 | ct.phaseplot.streamlines( 57 | predprey_sys, np.array([[20, 30]]), np.linspace(0, 65, 500), 58 | gridtype='circlegrid', gridspec=[2, 1], arrows=10, color='r') 59 | ct.phaseplot.vectorfield(predprey_sys, [5, 125, 5, 100], gridspec=[20, 20]) 60 | 61 | # Add the limit cycle 62 | resp1 = ct.initial_response(predprey_sys, np.linspace(0, 100), [20, 75]) 63 | resp2 = ct.initial_response( 64 | predprey_sys, np.linspace(0, 20, 500), resp1.states[:, -1]) 65 | plt.plot(resp2.states[0], resp2.states[1], color='k') 66 | 67 | # Legacy code 68 | # def pp_ode(x, t): 69 | # return predprey_update(t, x, 0, {}) 70 | # ct.phase_plot(pp_ode, [0, 60, 7], [0, 50, 6]) 71 | # ct.phase_plot(pp_ode, [0, 60, 7], [60, 100, 4]) 72 | # ct.phase_plot(pp_ode, [70, 120, 6], [0, 50, 6]) 73 | # ct.phase_plot(pp_ode, [70, 120, 6], [60, 100, 4]) 74 | 75 | # # Plot the limit cycle 76 | # ct.phase_plot(pp_ode, X0=sim.states[:, -1:].T, T=20) # limit cycle 77 | # ct.phase_plot(pp_ode, X0=[[120, 32], [120, 60]], T=20) # outside trajectories 78 | # ct.phase_plot(pp_ode, X0=[[19, 30]], T=75) # inside trajectories 79 | 80 | # Label the plot 81 | plt.xlabel("Hares") 82 | plt.ylabel("Lynxes") 83 | plt.title("Phase portrait") 84 | fbs.savefig('figure-4.20-predprey_ctstime-pp.png') # PNG for web 85 | -------------------------------------------------------------------------------- /figure-5-6-lyapunov_stability.py: -------------------------------------------------------------------------------- 1 | # lyapunov_stability.py - illustration of Lyapunov stability 2 | # RMM, 6 Apr 2024 3 | 4 | import matplotlib.pyplot as plt 5 | import numpy as np 6 | import control as ct 7 | import fbs # FBS plotting customizations 8 | 9 | t = np.linspace(0, 6) 10 | x0 = np.sin(t) + 0.8 * np.cos(2.2 * t) + 2.5 11 | 12 | # Plot the centerline and bounds 13 | fbs.figure('211') 14 | plt.plot(t, x0, 'k') 15 | plt.plot(t, x0 + 0.5, 'r', t, x0 - 0.5, 'r') 16 | 17 | # Plot the signal 18 | x = x0 - 0.4 * np.sin(t - 0.3) 19 | plt.plot(t, x, 'b--') 20 | 21 | # Label the axes 22 | plt.xlabel("Time $t$") 23 | plt.ylabel("State $x$") 24 | 25 | # Add some arrows and label the range of stability 26 | plt.arrow(1.5, 1.3, 0, 0.8, width=0.01, head_width=0.05) 27 | plt.arrow(1.5, 4.15, 0, -0.8, width=0.01, head_width=0.05) 28 | plt.text(1.6, 1.4, "$\\epsilon$") 29 | 30 | fbs.savefig('figure-5.6-lyapunov_stability.png') 31 | -------------------------------------------------------------------------------- /figure-5.1-damposc_response.py: -------------------------------------------------------------------------------- 1 | # damposc_response.py - dampled oscillator response 2 | # RMM, 28 May 2023 3 | 4 | import matplotlib.pyplot as plt 5 | import numpy as np 6 | import control as ct 7 | import fbs # FBS plotting customizations 8 | 9 | # Oscillator parameters 10 | damposc_params = {'m': 1, 'b': 0.2, 'k': 1} 11 | 12 | # System model (as ODE) 13 | def damposc_update(t, x, u, params): 14 | m, b, k = params['m'], params['b'], params['k'] 15 | return np.array([x[1], -k/m * x[0] - b/m * x[1]]) 16 | damposc = ct.nlsys(damposc_update, params=damposc_params) 17 | 18 | # Simulate the response 19 | tvec = np.linspace(0, 20, 100) 20 | response = ct.input_output_response(damposc, tvec, 0, X0=[1, 0]) 21 | 22 | # Plot the states 23 | fbs.figure('211') 24 | plt.plot(response.time, response.states[0], 'b-') 25 | plt.plot(response.time, response.states[1], 'b--') 26 | plt.plot([response.time[0], response.time[-1]], [0, 0], 'k-', linewidth=0.75) 27 | plt.xlabel('Time $t$ [s]') 28 | plt.ylabel('States $x_1$, $x_2$') 29 | plt.title( 30 | "Response of the damped oscillator to the initial condition x0 = (1, 0)") 31 | fbs.savefig('figure-5.1-damposc_response-time.png') 32 | -------------------------------------------------------------------------------- /figure-5.10-congctrl_dynamics.py: -------------------------------------------------------------------------------- 1 | # congctrl-dynamics.py - phase plots for congestion control dynamis 2 | # RMM, 7 Apr 2024 3 | 4 | import matplotlib.pyplot as plt 5 | import numpy as np 6 | import control as ct 7 | import fbs # FBS plotting customizations 8 | 9 | # Define the system dynamics 10 | def _congctrl_update(t, x, u, params): 11 | # Number of sources per state of the simulation 12 | M = x.size - 1 # general case 13 | assert M == 1 # make sure nothing funny happens here 14 | 15 | # Remaining parameters 16 | N = params.get('N', M) # number of sources 17 | rho = params.get('rho', 2e-4) # RED parameter = pbar / (bupper-blower) 18 | c = params.get('c', 10) # link capacity (Mp/ms) 19 | 20 | # Compute the derivative (last state = bdot) 21 | return np.append( 22 | c / x[M] - (rho * c) * (1 + (x[:-1]**2) / 2), 23 | N/M * np.sum(x[:-1]) * c / x[M] - c) 24 | 25 | congctrl = ct.nlsys( 26 | _congctrl_update, states=2, inputs=0, 27 | params={'N': 60, 'rho': 2e-4, 'c': 10}) 28 | 29 | fbs.figure() 30 | ct.phase_plane_plot(congctrl, [0, 10, 10, 500], 100) 31 | plt.axis([0, 10, 0, 500]) 32 | plt.suptitle("") 33 | plt.title("$\\rho = 2 \\times 10^{-4}$, $c = 10$ pkts/msec") 34 | plt.xlabel("Window size, $w$ [pkts]") 35 | plt.ylabel("Buffer size, $b$ [pkts]") 36 | fbs.savefig('figure-5.10-congctrl_dynamics-pp1.png') 37 | 38 | fbs.figure() 39 | ct.phase_plane_plot( 40 | congctrl, [0, 10, 10, 500], 100, 41 | params={'rho': 4e-4, 'c': 20}) 42 | plt.axis([0, 10, 0, 500]) 43 | plt.suptitle("") 44 | plt.title("$\\rho = 4 \\times 10^{-4}$, $c = 20$ pkts/msec") 45 | plt.xlabel("Window size, $w$ [pkts]") 46 | plt.ylabel("Buffer size, $b$ [pkts]") 47 | fbs.savefig('figure-5.10-congctrl_dynamics-pp2.png') 48 | -------------------------------------------------------------------------------- /figure-5.11-invpend_linearized.py: -------------------------------------------------------------------------------- 1 | # invepend_linearized.py - nonlinear vs linear inverted pendulum dynamics 2 | # RMM, 7 Apr 2024 3 | 4 | import matplotlib.pyplot as plt 5 | import numpy as np 6 | from math import pi 7 | import control as ct 8 | import fbs # FBS plotting customizations 9 | 10 | def invpend_update(t, x, u, params): 11 | m, l, b, g = params['m'], params['l'], params['b'], params['g'] 12 | return [x[1], -b/m * x[1] + (g * l / m) * np.sin(x[0])] 13 | invpend = ct.nlsys( 14 | invpend_update, states=2, inputs=0, name='inverted pendulum', 15 | params={'m': 1, 'l': 1, 'b': 0.5, 'g': 1}) 16 | 17 | fbs.figure() 18 | ct.phase_plane_plot( 19 | invpend, [0, 2*pi, -2, 2], 6, gridspec=[6, 5], 20 | plot_separatrices={'timedata': 20, 'arrows': 4}) 21 | fbs.savefig('figure-5.10-invpend_linearized-nl.png') 22 | 23 | # Create a linearized model 24 | linsys = invpend.linearize([pi, 0], 0) 25 | 26 | fbs.figure() 27 | ct.phase_plane_plot( 28 | linsys, [-pi, pi, -2, 2], 10, gridspec=[5, 2], 29 | plot_separatrices={'timedata': 20, 'arrows': 4}) 30 | fbs.savefig('figure-5.10-invpend_linearized-ln.png') 31 | -------------------------------------------------------------------------------- /figure-5.3-phase_portraits.py: -------------------------------------------------------------------------------- 1 | # phase_portraits.py - phase portrait examples 2 | # RMM, 6 Apr 2024 3 | 4 | import matplotlib.pyplot as plt 5 | import numpy as np 6 | import control as ct 7 | import fbs # FBS plotting customizations 8 | 9 | # Oscillator parameters 10 | damposc_params = {'m': 1, 'b': 1, 'k': 1} 11 | 12 | # System model (as ODE) 13 | def damposc_update(t, x, u, params): 14 | m, b, k = params['m'], params['b'], params['k'] 15 | return np.array([x[1], -k/m * x[0] - b/m * x[1]]) 16 | damposc = ct.nlsys(damposc_update, states=2, params=damposc_params) 17 | 18 | # Set the limits for the plot 19 | limits = [-1, 1, -1, 1] 20 | 21 | # Vector field 22 | fbs.figure('mlh') 23 | ct.phase_plane_plot( 24 | damposc, limits, plot_streamlines=False, 25 | plot_vectorfield=True, gridspec=[15, 12]) 26 | plt.suptitle(""); plt.title("Vector field") 27 | fbs.savefig('figure-5.3-phase_portraits-vf.png') 28 | 29 | # Streamlines 30 | fbs.figure('mlh') 31 | ct.phase_plane_plot(damposc, limits, 8) 32 | plt.suptitle(""); plt.title("Phase portrait") 33 | fbs.savefig('figure-5.3-phase_portraits-sl.png') 34 | -------------------------------------------------------------------------------- /figure-5.4-invpend_phaseplot.py: -------------------------------------------------------------------------------- 1 | # invepend_phaseplot.py - inverted pendulum phase plots 2 | # RMM, 6 Apr 2024 3 | 4 | import matplotlib.pyplot as plt 5 | import numpy as np 6 | from math import pi 7 | import control as ct 8 | import fbs # FBS plotting customizations 9 | 10 | def invpend_update(t, x, u, params): 11 | m, l, b, g = params['m'], params['l'], params['b'], params['g'] 12 | return [x[1], -b/m * x[1] + (g * l / m) * np.sin(x[0])] 13 | invpend = ct.nlsys( 14 | invpend_update, states=2, inputs=0, name='inverted pendulum', 15 | params={'m': 1, 'l': 1, 'b': 0.2, 'g': 1}) 16 | 17 | fbs.figure() 18 | ct.phase_plane_plot( 19 | invpend, [-2*pi, 2*pi, -2, 2], 4, gridspec=[6, 6], 20 | plot_separatrices={'timedata': 20, 'arrows': 4}) 21 | fbs.savefig('figure-5.4-invpend_phaseplot.png') 22 | -------------------------------------------------------------------------------- /figure-5.5-limit_cycle.py: -------------------------------------------------------------------------------- 1 | # limit_cycle.py - nonlinear oscillator (limit cycle) phase plot 2 | # RMM, 6 Apr 2024 3 | 4 | import matplotlib.pyplot as plt 5 | import numpy as np 6 | from math import pi 7 | import control as ct 8 | import control.phaseplot as pp 9 | import fbs # FBS plotting customizations 10 | 11 | def oscillator_update(t, x, u, params): 12 | return [ 13 | x[1] + x[0] * (1 - x[0]**2 - x[1]**2), 14 | -x[0] + x[1] * (1 - x[0]**2 - x[1]**2) 15 | ] 16 | oscillator = ct.nlsys(oscillator_update, states=2, inputs=0, name='oscillator') 17 | 18 | fbs.figure() 19 | ct.phase_plane_plot(oscillator, [-1.5, 1.5, -1.5, 1.5], 0.9) 20 | pp.streamlines( 21 | oscillator, np.array([[0, 0]]), 1.5, 22 | gridtype='circlegrid', gridspec=[0.5, 6], dir='both') 23 | pp.streamlines( 24 | oscillator, np.array([[1, 0]]), 2*pi, arrows=6, color='b') 25 | plt.gca().set_aspect('equal') 26 | plt.suptitle("") 27 | fbs.savefig('figure-5.5-limit_cycle-pp.png') 28 | 29 | fbs.figure() 30 | plt.axis([0, 30, -2, 2]) 31 | timepts = np.linspace(0, 30) 32 | response = ct.input_output_response(oscillator, timepts, 0, [0.1, 1]) 33 | plt.plot(response.time, response.outputs[0], 'b', label="$x_1$") 34 | plt.plot(response.time, response.outputs[1], 'r--', label="$x_2$") 35 | plt.xlabel("Time $t$") 36 | plt.ylabel("$x_1, x_2$") 37 | plt.legend(loc='upper right', ncols=2, frameon=False) 38 | fbs.savefig('figure-5.5-limit_cycle-time.png') 39 | -------------------------------------------------------------------------------- /figure-5.7-stable_eqpt.py: -------------------------------------------------------------------------------- 1 | # stable_eqpt.py - plots for stable equlibrium point 2 | # RMM, 6 Apr 2024 3 | 4 | import matplotlib.pyplot as plt 5 | import numpy as np 6 | from math import pi 7 | import control as ct 8 | import control.phaseplot as pp 9 | import fbs # FBS plotting customizations 10 | 11 | m, b, k = 1, 0, 2 12 | linsys = ct.ss([[0, 1], [-k/m, -b/m]], [[0], [1]], np.eye(2), 0) 13 | 14 | # Draw the phase portrait 15 | fbs.figure() 16 | ct.phase_plane_plot(linsys, [-1, 1, -1, 1], 1, plot_streamlines=False) 17 | pp.streamlines( 18 | linsys, np.array([[0.2, 0], [0.4, 0], [0.6, 0], [0.8, 0], [1, 0]]), 19 | 4.5, arrows=6) 20 | plt.gca().set_aspect('equal') 21 | plt.suptitle("") 22 | 23 | # Add some level sets 24 | theta = np.linspace(0, 2*pi) 25 | plt.plot(0.2 * np.sin(theta), 0.2 * np.cos(theta), 'r--') 26 | plt.plot(0.3 * np.sin(theta), 0.3 * np.cos(theta), 'r--') 27 | 28 | fbs.savefig('figure-5.7-stable_eqpt-pp.png') 29 | 30 | fbs.figure('321') 31 | plt.axis([0, 10, -2.5, 2.5]) 32 | timepts = np.linspace(0, 10) 33 | response = ct.input_output_response(linsys, timepts, 0, [1, 0]) 34 | plt.plot(response.time, response.outputs[0], 'b', label="$x_1$") 35 | plt.plot(response.time, response.outputs[1], 'r--', label="$x_2$") 36 | plt.xlabel("Time $t$") 37 | plt.ylabel("$x_1, x_2$") 38 | plt.legend(loc='upper right', ncols=2, frameon=False) 39 | fbs.savefig('figure-5.7-stable_eqpt-time.png') 40 | -------------------------------------------------------------------------------- /figure-5.8-asystable_eqpt.py: -------------------------------------------------------------------------------- 1 | # asystable_eqpt.py - plots for stable equlibrium point 2 | # RMM, 7 Apr 2024 3 | 4 | import matplotlib.pyplot as plt 5 | import numpy as np 6 | import control as ct 7 | import fbs # FBS plotting customizations 8 | 9 | m, b, k = 1, 1, 1 10 | linsys = ct.ss([[0, 1], [-k/m, -b/m]], [[0], [1]], np.eye(2), 0) 11 | 12 | # Draw the phase portrait 13 | fbs.figure() 14 | ct.phase_plane_plot(linsys, [-1, 1, -1, 1], 5) 15 | plt.gca().set_aspect('equal') 16 | plt.suptitle("") 17 | fbs.savefig('figure-5.8-asystable_eqpt-pp.png') 18 | 19 | fbs.figure('321') 20 | plt.axis([0, 10, -0.6, 1]) 21 | timepts = np.linspace(0, 10) 22 | response = ct.input_output_response(linsys, timepts, 0, [1, 0]) 23 | plt.plot(response.time, response.outputs[0], 'b', label="$x_1$") 24 | plt.plot(response.time, response.outputs[1], 'r--', label="$x_2$") 25 | plt.xlabel("Time $t$") 26 | plt.ylabel("$x_1, x_2$") 27 | plt.legend(loc='upper right', ncols=2, frameon=False) 28 | fbs.savefig('figure-5.8-asystable_eqpt-time.png') 29 | -------------------------------------------------------------------------------- /figure-5.9-unstable_eqpt.py: -------------------------------------------------------------------------------- 1 | # unstable_eqpt.py - plots for stable equlibrium point 2 | # RMM, 7 Apr 2024 3 | 4 | import matplotlib.pyplot as plt 5 | import numpy as np 6 | import control as ct 7 | import fbs # FBS plotting customizations 8 | 9 | saddle = ct.ss([[1, -3], [-3, 1]], [[0], [1]], np.eye(2), 0) 10 | 11 | # Draw the phase portrait 12 | fbs.figure() 13 | ct.phase_plane_plot( 14 | saddle, [-1, 1, -1, 1], 0.4, 15 | gridtype='meshgrid', gridspec=[6, 6]) 16 | plt.gca().set_aspect('equal') 17 | plt.suptitle("") 18 | fbs.savefig('figure-5.9-unstable_eqpt-pp.png') 19 | 20 | fbs.figure('321') 21 | plt.axis([0, 3, -100, 100]) 22 | timepts = np.linspace(0, 3) 23 | response = ct.input_output_response(saddle, timepts, 0, [1, 0]) 24 | plt.plot(response.time, response.outputs[0], 'b', label="$x_1$") 25 | plt.plot(response.time, response.outputs[1], 'r--', label="$x_2$") 26 | plt.xlabel("Time $t$") 27 | plt.ylabel("$x_1, x_2$") 28 | plt.legend(loc='upper right', ncols=1, frameon=False) 29 | fbs.savefig('figure-5.9-unstable_eqpt-time.png') 30 | -------------------------------------------------------------------------------- /figure-6.1-superposition.py: -------------------------------------------------------------------------------- 1 | # superposition.py - superposition of homogeneous and particular solutions 2 | # RMM, 19 Apr 2024 3 | 4 | import matplotlib.pyplot as plt 5 | import numpy as np 6 | import control as ct 7 | import fbs # FBS plotting customizations 8 | 9 | # Spring mass system 10 | from springmass import springmass # use spring mass dynamics 11 | sys = springmass / springmass(0).real # normalize the response to 1 12 | X0 = [2, -1] # initial condition 13 | 14 | # Create input vectors 15 | tvec = np.linspace(0, 60, 100) 16 | u1 = 0 * tvec 17 | u2 = np.hstack([tvec[0:50]/tvec[50], 1 - tvec[0:50]/tvec[50]]) 18 | 19 | # Run simulations for the different cases 20 | homogeneous = ct.forced_response(sys, tvec, u1, X0=X0) 21 | particular = ct.forced_response(sys, tvec, u2) 22 | complete = ct.forced_response(sys, tvec, u1 + u2, X0=X0) 23 | 24 | # Plot results 25 | fig, axs = plt.subplots(3, 3, figsize=[8, 4], layout='tight') 26 | for i, resp in enumerate([homogeneous, particular, complete]): 27 | axs[i, 0].plot(resp.time, resp.inputs) 28 | axs[0, 0].set_title("Input $u$") 29 | axs[i, 0].set_ylim(-2, 2) 30 | 31 | axs[i, 1].plot( 32 | resp.time, resp.states[0], 'b', 33 | resp.time, resp.states[1], 'r--') 34 | axs[0, 1].set_title("States $x_1$, $x_2$") 35 | axs[i, 1].set_ylim(-2, 2) 36 | 37 | axs[i, 2].plot(resp.time, resp.outputs) 38 | axs[0, 2].set_title("Output $y$") 39 | axs[i, 2].set_ylim(-2, 2) 40 | 41 | # Label the plots 42 | axs[0, 0].set_ylabel("Homogeneous") 43 | axs[1, 0].set_ylabel("Particular") 44 | axs[2, 0].set_ylabel("Complete") 45 | for i in range(3): 46 | axs[2, i].set_xlabel("Time $t$ [s]") 47 | 48 | # Save the figure 49 | fbs.savefig('figure-6.1-superposition.png') 50 | -------------------------------------------------------------------------------- /figure-6.14-cruise_linearized.py: -------------------------------------------------------------------------------- 1 | # cruise_linearized.py - linear versus nonlinear response, cruise w/ PI 2 | # RMM, 20 Apr 2024 3 | 4 | import matplotlib.pyplot as plt 5 | import numpy as np 6 | import control as ct 7 | import fbs # FBS plotting customizations 8 | 9 | # System definition 10 | from cruise import vehicle_dynamics as vehicle 11 | 12 | # Figure out the equilibrium point for the system at 20 m/s 13 | xe, ue = ct.find_eqpt(vehicle, 20, u0=[0, 4, 0], iu=[1, 2], y0=20, iy=[0]) 14 | 15 | # Linearized dynamics 16 | vehicle_lin = vehicle.linearize(xe, ue) 17 | 18 | # Controller: PI + antiwindup 19 | ctrl_params = {'kp': 0.5, 'ki': 0.1, 'kaw': 2} 20 | 21 | def ctrl_update(t, x, u, params): 22 | e = u[1] - u[0] # v - vref 23 | v_nom = -params['kp'] * e + x[0] # nominal control input (PI) 24 | v_sat = np.clip(v_nom, 0, 1) # clipped control input 25 | return -params['ki'] * e + params['kaw'] * (v_sat - v_nom) 26 | 27 | def ctrl_output(t, x, u, params): 28 | e = u[1] - u[0] # v - vref 29 | v_nom = -params['kp'] * e + x[0] # nominal control input (PI) 30 | v_sat = np.clip(v_nom, 0, 1) # clipped control input 31 | return v_sat 32 | 33 | ctrl = ct.nlsys( 34 | ctrl_update, ctrl_output, states=1, name='ctrl', 35 | inputs=['vref', 'v'], outputs='u', params=ctrl_params) 36 | 37 | # Figure out the equilibrium point for the system at 20 m/s 38 | xe, ue = ct.find_eqpt(vehicle, 20, u0=[0, 4, 0], iu=[1, 2], y0=20, iy=[0]) 39 | 40 | # Linearized dynamics 41 | vehicle_lin = vehicle.linearize( 42 | xe, ue, inputs=vehicle.input_labels, outputs=vehicle.output_labels) 43 | 44 | # Controller: PI + antiwindup 45 | ctrl_params = {'kp': 0.5, 'ki': 0.1, 'kaw': 2} 46 | 47 | def ctrl_update(t, x, u, params): 48 | e = u[1] - u[0] # v - vref 49 | v_nom = -params['kp'] * e + x[0] # nominal control input (PI) 50 | v_sat = np.clip(v_nom, 0, 1) # clipped control input 51 | return -params['ki'] * e + params['kaw'] * (v_sat - v_nom) 52 | 53 | def ctrl_output(t, x, u, params): 54 | e = u[1] - u[0] # v - vref 55 | v_nom = -params['kp'] * e + x[0] # nominal control input (PI) 56 | v_sat = np.clip(v_nom, 0, 1) # clipped control input 57 | return v_sat 58 | 59 | ctrl = ct.nlsys( 60 | ctrl_update, ctrl_output, states=1, name='ctrl', 61 | inputs=['vref', 'v'], outputs='u', params=ctrl_params) 62 | 63 | # Full system (linear and nonlinear) 64 | nlsys = ct.interconnect( 65 | [vehicle, ctrl], inputs=['vref', 'gear', 'theta'], outputs=['v', 'u']) 66 | 67 | lnsys = ct.interconnect( 68 | [vehicle_lin, ctrl], inputs=['vref', 'gear', 'theta'], outputs=['v', 'u']) 69 | 70 | # Compute system response: flat then a hill 71 | T1 = np.linspace(0, 5, 40) # Flat section 72 | T2 = np.linspace(5, 30) # Hill section 73 | 74 | # Nonlinear response 75 | nl_resp1 = ct.input_output_response(nlsys, T1, [20, 4, 0], X0=[20, ue[0]]) 76 | nl_resp2a = ct.input_output_response( 77 | nlsys, T2, [20, 4, 0.07], X0=nl_resp1.states[:, -1]) 78 | nl_resp2b = ct.input_output_response( 79 | nlsys, T2, [20, 4, 0.105], X0=nl_resp1.states[:, -1]) 80 | 81 | # Linear response 82 | ln_resp1 = ct.input_output_response(lnsys, T1, [20, 4, 0], X0=[20, ue[0]]) 83 | ln_resp2a = ct.input_output_response( 84 | lnsys, T2, [20, 4, 0.07], X0=ln_resp1.states[:, -1]) 85 | ln_resp2b = ct.input_output_response( 86 | lnsys, T2, [20, 4, 0.105], X0=ln_resp1.states[:, -1]) 87 | 88 | # Plot the velocity response 89 | fig, axs = plt.subplots(2, 1, figsize=[3.4, 3.4], sharex=True) 90 | 91 | axs[0].plot(nl_resp1.time, nl_resp1.outputs[0], 'b') 92 | axs[0].plot(nl_resp2a.time, nl_resp2a.outputs[0], 'b') 93 | axs[0].plot(nl_resp2b.time, nl_resp2b.outputs[0], 'b') 94 | 95 | axs[0].plot(ln_resp2a.time, ln_resp2a.outputs[0], 'r--') 96 | axs[0].plot(nl_resp2b.time, ln_resp2b.outputs[0], '--') 97 | 98 | axs[0].axis([0, 30, 18.5, 20.5]) 99 | axs[0].set_yticks([19, 20]) 100 | axs[0].set_ylabel("Velocity $v$ [m/s]") 101 | 102 | # Plot the throttle command 103 | axs[1].plot(nl_resp1.time, nl_resp1.outputs[1], 'b') 104 | axs[1].plot(nl_resp2a.time, nl_resp2a.outputs[1], 'b') 105 | axs[1].plot(nl_resp2b.time, nl_resp2b.outputs[1], 'b') 106 | 107 | axs[1].plot(ln_resp2a.time, ln_resp2a.outputs[1], 'r--') 108 | axs[1].plot(nl_resp2b.time, ln_resp2b.outputs[1], '--') 109 | 110 | axs[1].set_title(" ") # Hack to adjust spacing between plots 111 | axs[1].set_ylim([0, 1.25]) 112 | axs[1].set_yticks([0, 0.5, 1]) 113 | axs[1].set_yticklabels(["0", "0.5", "1"]) 114 | axs[1].set_ylabel("Throttle $u$") 115 | axs[1].set_xlabel("Time $t$ [s]") 116 | 117 | fbs.savefig('figure-6.14-cruise_linearized.png') 118 | -------------------------------------------------------------------------------- /figure-6.5-modes.py: -------------------------------------------------------------------------------- 1 | # modes.py - illustration of modes for a second order system 2 | # RMM, 19 Apr 2024 3 | 4 | import matplotlib.pyplot as plt 5 | import numpy as np 6 | import control as ct 7 | import fbs # FBS plotting customizations 8 | 9 | # System definition 10 | k0, k1, k2, b0 = 0.1, 0.1, 0.5, 1.5 11 | A = [[-k0 - k1, k1], [k2, -k2]] 12 | B = [[b0], [0]] 13 | C = [[1, 0], [0, 1]] 14 | sys = ct.ss(A, B, C, 0) 15 | 16 | # Generate a phase plot for the system 17 | fig, ax = plt.subplots(1, 1, figsize=[3.4, 3.4]) 18 | ct.phase_plane_plot(sys, [-1, 1, -1, 1], 5, gridspec=[7, 4]) 19 | ax.set_aspect('equal') 20 | 21 | # Label the figure 22 | plt.suptitle("") 23 | plt.text(-1, -0.8, "Slow") 24 | plt.text(-0.2, 0.8, "Fast") 25 | fbs.savefig('figure-6.5-modes-pp.png') 26 | 27 | # Time domain simulations 28 | evals, evecs = np.linalg.eig(A) 29 | X0s, X0f = evecs[:, 0], evecs[:, 1] # Fast and slow modes 30 | tvec = np.linspace(0, 50, endpoint=True) 31 | 32 | # Slow mode 33 | ax = fbs.figure('321') 34 | resp_slow = ct.initial_response(sys, tvec, X0=X0s) 35 | 36 | ax.set_xlim([0, 50]) 37 | ax.set_ylim([0, 1]) 38 | ax.plot(tvec, resp_slow.states[0], 'b', label="$x_1$") 39 | ax.plot(tvec, resp_slow.states[1], 'r--', label="$x_2$") 40 | ax.text(10, 0.75, "Slow mode") 41 | ax.set_xlabel("Time $t$ [s]") 42 | ax.set_ylabel("$x_1, x_2$") 43 | ax.legend(frameon=False) 44 | 45 | plt.tight_layout() 46 | fbs.savefig('figure-6.5-modes-slow.png') 47 | 48 | # Fast mode 49 | ax = fbs.figure('321') 50 | resp_fast = ct.initial_response(sys, tvec, X0=X0f) 51 | 52 | ax.set_xlim([0, 50]) 53 | ax.set_ylim([-0.25, 1]) 54 | ax.plot(tvec, resp_fast.states[0], 'b', label="$x_1$") 55 | ax.plot(tvec, resp_fast.states[1], 'r--', label="$x_2$") 56 | ax.text(10, 0.75, "Fast mode") 57 | ax.set_xlabel("Time $t$ [s]") 58 | ax.set_ylabel("$x_1, x_2$") 59 | 60 | plt.tight_layout() 61 | fbs.savefig('figure-6.5-modes-fast.png') 62 | -------------------------------------------------------------------------------- /predprey.py: -------------------------------------------------------------------------------- 1 | # predprey.py - predator-prey dynamics 2 | # RMM, 21 Apr 2024 3 | # 4 | # Predator-prey dynamics 5 | # 6 | # This model implements the dynamics of a predator-prey system, as 7 | # described in Section 2.7 of FBS. 8 | 9 | import numpy as np 10 | import control as ct 11 | 12 | # Define the dynamis for the predator-prey system (no input) 13 | predprey_params = {'r': 1.6, 'd': 0.56, 'b': 0.6, 'k': 125, 'a': 3.2, 'c': 50} 14 | def predprey_update(t, x, u, params): 15 | """Predator prey dynamics""" 16 | r, d, b, k, a, c = map(params.get, ['r', 'd', 'b', 'k', 'a', 'c']) 17 | u = np.atleast_1d(u) # Fix python-control bug 18 | u = np.clip(u, 0, 4*r) # constraints used in FBS 2e 19 | # u = np.clip(u, -r, 3*r) # constrain the input to keep r_eff >= 0 20 | 21 | # Dynamics for the system 22 | dH = (r + u[0]) * x[0] * (1 - x[0]/k) - a * x[1] * x[0]/(c + x[0]) 23 | dL = b * a * x[1] * x[0] / (c + x[0]) - d * x[1] 24 | 25 | return np.array([dH, dL]) 26 | 27 | # Create a nonlinear I/O system 28 | predprey = ct.nlsys( 29 | predprey_update, name='predprey', params=predprey_params, 30 | states=['H', 'L'], inputs='u', outputs=['H', 'L']) 31 | -------------------------------------------------------------------------------- /run_all.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # Run the examples. Doesn't run Jupyter notebooks. 3 | 4 | # The examples must cooperate: they must have no operations that wait 5 | # on user action, like matplotlib.pyplot.show, or starting a GUI event 6 | # loop. Environment variable PYCONTROL_TEST_EXAMPLES is set when the 7 | # examples are being tested; existence of this variable should be used 8 | # to prevent such user-action-waiting operations. 9 | 10 | export PYCONTROL_TEST_EXAMPLES=1 11 | 12 | example_errors="" 13 | 14 | for example in *.py; do 15 | echo "Running ${example}" 16 | if ! python ${example}; then 17 | example_errors="${example_errors} ${example}" 18 | fi 19 | done 20 | 21 | # Get rid of the output files 22 | rm *.log 23 | 24 | # List any files that generated errors 25 | if [ -n "${example_errors}" ]; then 26 | echo These examples had errors: 27 | echo "${example_errors}" 28 | exit 1 29 | else 30 | echo All examples ran successfully 31 | fi 32 | -------------------------------------------------------------------------------- /springmass.py: -------------------------------------------------------------------------------- 1 | # springmass.py - Spring mass dynamics 2 | # RMM, 19 Apr 2024 3 | 4 | import control as ct 5 | 6 | m, k, b = 250, 40, 60 7 | A = [[0, 1], [-k/m, -b/m]] 8 | B = [[0], [1/m]] 9 | C = [[1, 0]] 10 | 11 | springmass = ct.ss(A, B, C, 0) 12 | -------------------------------------------------------------------------------- /steering.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import control as ct 3 | 4 | # Define the nonlinear dynamics for the vehicle steering system 5 | def steering_update(t, x, u, params): 6 | """ 7 | Nonlinear dynamics of the vehicle steering system. 8 | 9 | Parameters: 10 | t : float 11 | Current time (not used in the dynamics, but included for compatibility). 12 | x : array_like 13 | Current state vector: [x1, x2, x3]. 14 | u : array_like 15 | Input vector: [velocity, steering angle]. 16 | params : dict 17 | Dictionary of parameters (a, b, maxsteer). 18 | 19 | Returns: 20 | dx : array_like 21 | State derivatives: [dx1, dx2, dx3]. 22 | """ 23 | a = params.get('a', 1.5) # Offset to vehicle reference point [m] 24 | b = params.get('b', 3.0) # Vehicle wheelbase [m] 25 | maxsteer = params.get('maxsteer', 0.5) # Max steering angle [rad] 26 | 27 | # Saturate the steering input 28 | delta = np.clip(u[1], -maxsteer, maxsteer) 29 | 30 | # System dynamics 31 | alpha = np.arctan2(a * np.tan(delta), b) 32 | dx1 = u[0] * np.cos(x[2] + alpha) # x velocity 33 | dx2 = u[0] * np.sin(x[2] + alpha) # y velocity 34 | dx3 = (u[0] / b) * np.tan(delta) # Angular velocity 35 | return [dx1, dx2, dx3] 36 | 37 | # Create a nonlinear input/output system 38 | steering = ct.nlsys( 39 | steering_update, # Update function for system dynamics 40 | inputs=['v', 'delta'], # Inputs: velocity and steering angle 41 | outputs=None, # Outputs are the same as the states 42 | states=['x', 'y', 'theta'], # States: x, y, and theta (angle) 43 | name='steering', 44 | params={'a': 1.5, 'b': 3.0, 'maxsteer': 0.5} # Default parameters 45 | ) 46 | 47 | # Generate the linearization at a given velocity 48 | def linearize_lateral(v0=10, normalize=False, output_full_state=False): 49 | # Compute the linearization at the given velocity 50 | linsys = ct.linearize(steering, 0, [v0, 0])[1, 1] 51 | 52 | # Extract out the lateral dynamics 53 | latsys = ct.model_reduction( 54 | linsys, [0], method='truncate', warn_unstable=False) 55 | 56 | # Normalize coordinates if desired 57 | if normalize: 58 | b = steering.params['b'] 59 | latsys = ct.similarity_transform( 60 | latsys, np.array([[1/b, 0], [0, 1]]), timescale=v0/b) 61 | 62 | C = np.eye(2) if output_full_state else np.array([[1, 0]]) 63 | 64 | # Normalized system with (normalized) lateral offset as output 65 | return ct.ss( 66 | latsys.A, latsys.B, C, 0, inputs='delta', 67 | outputs=['y', 'theta'] if output_full_state else 'y') 68 | -------------------------------------------------------------------------------- /template.py: -------------------------------------------------------------------------------- 1 | # figure_name.py - short description 2 | # RMM, date 3 | 4 | import matplotlib.pyplot as plt 5 | import numpy as np 6 | import control as ct 7 | import fbs # FBS plotting customizations 8 | 9 | # System definition 10 | 11 | # Compute system response 12 | 13 | # Plot results 14 | fbs.figure('mlh') # Create figure using FBS defaults 15 | 16 | plt.tight_layout() 17 | fbs.savefig('figure-N.m-figure_name-panel.png') 18 | --------------------------------------------------------------------------------