Python Control Systems Library

The Python Control Systems Library (python-control) is a Python package that implements basic operations for analysis and design of feedback control systems.

Features

  • Linear input/output systems in state-space and frequency domain

  • Block diagram algebra: serial, parallel, and feedback interconnections

  • Time response: initial, step, impulse

  • Frequency response: Bode and Nyquist plots

  • Control analysis: stability, reachability, observability, stability margins

  • Control design: eigenvalue placement, LQR, H2, Hinf

  • Model reduction: balanced realizations, Hankel singular values

  • Estimator design: linear quadratic estimator (Kalman filter)

Documentation

Introduction

Welcome to the Python Control Systems Toolbox (python-control) User’s Manual. This manual contains information on using the python-control package, including documentation for all functions in the package and examples illustrating their use.

Overview of the toolbox

The python-control package is a set of python classes and functions that implement common operations for the analysis and design of feedback control systems. The initial goal is to implement all of the functionality required to work through the examples in the textbook Feedback Systems by Astrom and Murray. A MATLAB compatibility module is available that provides many of the common functions corresponding to commands available in the MATLAB Control Systems Toolbox.

Some differences from MATLAB

The python-control package makes use of NumPy and SciPy. A list of general differences between NumPy and MATLAB can be found here.

In terms of the python-control package more specifically, here are some thing to keep in mind:

  • You must include commas in vectors. So [1 2 3] must be [1, 2, 3].

  • Functions that return multiple arguments use tuples.

  • You cannot use braces for collections; use tuples instead.

Installation

The python-control package can be installed using pip, conda or the standard distutils/setuptools mechanisms. The package requires numpy and scipy, and the plotting routines require matplotlib. In addition, some routines require the slycot library in order to implement more advanced features (including some MIMO functionality).

To install using pip:

pip install slycot   # optional
pip install control

Many parts of python-control will work without slycot, but some functionality is limited or absent, and installation of slycot is recommended.

Note: the slycot library only works on some platforms, mostly linux-based. Users should check to insure that slycot is installed correctly by running the command:

python -c "import slycot"

and verifying that no error message appears. It may be necessary to install slycot from source, which requires a working FORTRAN compiler and either the lapack or openplas library. More information on the slycot package can be obtained from the slycot project page.

For users with the Anaconda distribution of Python, the following commands can be used:

conda install numpy scipy matplotlib    # if not yet installed
conda install -c conda-forge control

This installs slycot and python-control from conda-forge, including the openblas package.

Alternatively, to use setuptools, first download the source and unpack it. To install in your home directory, use:

python setup.py install --user

or to install for all users (on Linux or Mac OS):

python setup.py build
sudo python setup.py install

Getting started

There are two different ways to use the package. For the default interface described in Function reference, simply import the control package as follows:

>>> import control

If you want to have a MATLAB-like environment, use the MATLAB compatibility module:

>>> from control.matlab import *

Library conventions

The python-control library uses a set of standard conventions for the way that different types of standard information used by the library.

LTI system representation

Linear time invariant (LTI) systems are represented in python-control in state space, transfer function, or frequency response data (FRD) form. Most functions in the toolbox will operate on any of these data types and functions for converting between compatible types is provided.

State space systems

The StateSpace class is used to represent state-space realizations of linear time-invariant (LTI) systems:

\frac{dx}{dt} &= A x + B u \\ y &= C x + D u

where u is the input, y is the output, and x is the state.

To create a state space system, use the StateSpace constructor:

sys = StateSpace(A, B, C, D)

State space systems can be manipulated using standard arithmetic operations as well as the feedback(), parallel(), and series() function. A full list of functions can be found in Function reference.

Transfer functions

The TransferFunction class is used to represent input/output transfer functions

G(s) = \frac{\text{num}(s)}{\text{den}(s)} = \frac{a_0 s^m + a_1 s^{m-1} + \cdots + a_m} {b_0 s^n + b_1 s^{n-1} + \cdots + b_n},

where n is generally greater than or equal to m (for a proper transfer function).

To create a transfer function, use the TransferFunction constructor:

sys = TransferFunction(num, den)

Transfer functions can be manipulated using standard arithmetic operations as well as the feedback(), parallel(), and series() function. A full list of functions can be found in Function reference.

FRD (frequency response data) systems

The FrequencyResponseData (FRD) class is used to represent systems in frequency response data form.

The main data members are omega and fresp, where omega is a 1D array with the frequency points of the response, and fresp is a 3D array, with the first dimension corresponding to the output index of the FRD, the second dimension corresponding to the input index, and the 3rd dimension corresponding to the frequency points in omega.

FRD systems have a somewhat more limited set of functions that are available, although all of the standard algebraic manipulations can be performed.

Discrete time systems

A discrete time system is created by specifying a nonzero ‘timebase’, dt. The timebase argument can be given when a system is constructed:

  • dt = None: no timebase specified (default)

  • dt = 0: continuous time system

  • dt > 0: discrete time system with sampling period ‘dt’

  • dt = True: discrete time with unspecified sampling period

Only the StateSpace, TransferFunction, and InputOutputSystem classes allow explicit representation of discrete time systems.

Systems must have compatible timebases in order to be combined. A system with timebase None can be combined with a system having a specified timebase; the result will have the timebase of the latter system. Similarly, a discrete time system with unspecified sampling time (dt = True) can be combined with a system having a specified sampling time; the result will be a discrete time system with the sample time of the latter system. For continuous time systems, the sample_system() function or the StateSpace.sample() and TransferFunction.sample() methods can be used to create a discrete time system from a continuous time system. See Utility functions and conversions. The default value of ‘dt’ can be changed by changing the values of control.config.defaults['statesp.default_dt'] and control.config.defaults['xferfcn.default_dt'].

Conversion between representations

LTI systems can be converted between representations either by calling the constructor for the desired data type using the original system as the sole argument or using the explicit conversion functions ss2tf() and tf2ss().

Time series data

A variety of functions in the library return time series data: sequences of values that change over time. A common set of conventions is used for returning such data: columns represent different points in time, rows are different components (e.g., inputs, outputs or states). For return arguments, an array of times is given as the first returned argument, followed by one or more arrays of variable values. This convention is used throughout the library, for example in the functions forced_response(), step_response(), impulse_response(), and initial_response().

Note

The convention used by python-control is different from the convention used in the scipy.signal library. In Scipy’s convention the meaning of rows and columns is interchanged. Thus, all 2D values must be transposed when they are used with functions from scipy.signal.

Types:

  • Arguments can be arrays, matrices, or nested lists.

  • Return values are arrays (not matrices).

The time vector is either 1D, or 2D with shape (1, n):

T = [[t1,     t2,     t3,     ..., tn    ]]

Input, state, and output all follow the same convention. Columns are different points in time, rows are different components. When there is only one row, a 1D object is accepted or returned, which adds convenience for SISO systems:

U = [[u1(t1), u1(t2), u1(t3), ..., u1(tn)]
     [u2(t1), u2(t2), u2(t3), ..., u2(tn)]
     ...
     ...
     [ui(t1), ui(t2), ui(t3), ..., ui(tn)]]

Same for X, Y

So, U[:,2] is the system’s input at the third point in time; and U[1] or U[1,:] is the sequence of values for the system’s second input.

The initial conditions are either 1D, or 2D with shape (j, 1):

X0 = [[x1]
      [x2]
      ...
      ...
      [xj]]

As all simulation functions return arrays, plotting is convenient:

t, y = step_response(sys)
plot(t, y)

The output of a MIMO system can be plotted like this:

t, y, x = forced_response(sys, u, t)
plot(t, y[0], label='y_0')
plot(t, y[1], label='y_1')

The convention also works well with the state space form of linear systems. If D is the feedthrough matrix of a linear system, and U is its input (matrix or array), then the feedthrough part of the system’s response, can be computed like this:

ft = D * U

Package configuration parameters

The python-control library can be customized to allow for different default values for selected parameters. This includes the ability to set the style for various types of plots and establishing the underlying representation for state space matrices.

To set the default value of a configuration variable, set the appropriate element of the control.config.defaults dictionary:

control.config.defaults['module.parameter'] = value

The ~control.config.set_defaults function can also be used to set multiple configuration parameters at the same time:

control.config.set_defaults('module', param1=val1, param2=val2, ...]

Finally, there are also functions available set collections of variables based on standard configurations.

Selected variables that can be configured, along with their default values:

  • bode.dB (False): Bode plot magnitude plotted in dB (otherwise powers of 10)

  • bode.deg (True): Bode plot phase plotted in degrees (otherwise radians)

  • bode.Hz (False): Bode plot frequency plotted in Hertz (otherwise rad/sec)

  • bode.grid (True): Include grids for magnitude and phase plots

  • freqplot.number_of_samples (None): Number of frequency points in Bode plots

  • freqplot.feature_periphery_decade (1.0): How many decades to include in the frequency range on both sides of features (poles, zeros).

  • statesp.use_numpy_matrix (True): set the return type for state space matrices to numpy.matrix (verus numpy.ndarray)

  • statesp.default_dt and xferfcn.default_dt (None): set the default value of dt when constructing new LTI systems

  • statesp.remove_useless_states (True): remove states that have no effect on the input-output dynamics of the system

Additional parameter variables are documented in individual functions

Functions that can be used to set standard configurations:

reset_defaults()

Reset configuration values to their default (initial) values.

use_fbs_defaults()

Use Feedback Systems (FBS) compatible settings.

use_matlab_defaults()

Use MATLAB compatible configuration settings.

use_numpy_matrix([flag, warn])

Turn on/off use of Numpy matrix class for state space operations.

use_legacy_defaults(version)

Sets the defaults to whatever they were in a given release.

Function reference

The Python Control Systems Library control provides common functions for analyzing and designing feedback control systems.

System creation

ss(A, B, C, D[, dt])

Create a state space system.

tf(num, den[, dt])

Create a transfer function system.

frd(d, w)

Construct a frequency response data model

rss([states, outputs, inputs])

Create a stable continuous random state space object.

drss([states, outputs, inputs])

Create a stable discrete random state space object.

System interconnections

append(sys1, sys2, …, sysn)

Group models by appending their inputs and outputs

connect(sys, Q, inputv, outputv)

Index-based interconnection of an LTI system.

feedback(sys1[, sys2, sign])

Feedback interconnection between two I/O systems.

negate(sys)

Return the negative of a system.

parallel(sys1, *sysn)

Return the parallel connection sys1 + sys2 (+ .

series(sys1, *sysn)

Return the series connection (sysn * .

See also the Input/output systems module, which can be used to create and interconnect nonlinear input/output systems.

Frequency domain plotting

bode_plot(syslist[, omega, plot, …])

Bode plot for a system

nyquist_plot(syslist[, omega, plot, …])

Nyquist plot for a system

gangof4_plot(P, C[, omega])

Plot the “Gang of 4” transfer functions for a system

nichols_plot(sys_list[, omega, grid])

Nichols plot for a system

nichols_grid([cl_mags, cl_phases, line_style])

Nichols chart grid

Note: For plotting commands that create multiple axes on the same plot, the individual axes can be retrieved using the axes label (retrieved using the get_label method for the matplotliib axes object). The following labels are currently defined:

  • Bode plots: control-bode-magnitude, control-bode-phase

  • Gang of 4 plots: control-gangof4-s, control-gangof4-cs, control-gangof4-ps, control-gangof4-t

Time domain simulation

forced_response(sys[, T, U, X0, transpose, …])

Simulate the output of a linear system.

impulse_response(sys[, T, X0, input, …])

Impulse response of a linear system

initial_response(sys[, T, X0, input, …])

Initial condition response of a linear system

input_output_response(sys, T[, U, X0, …])

Compute the output response of a system to a given input.

step_response(sys[, T, X0, input, output, …])

Step response of a linear system

phase_plot(odefun[, X, Y, scale, X0, T, …])

Phase plot for 2D dynamical systems

Block diagram algebra

series(sys1, *sysn)

Return the series connection (sysn * .

parallel(sys1, *sysn)

Return the parallel connection sys1 + sys2 (+ .

feedback(sys1[, sys2, sign])

Feedback interconnection between two I/O systems.

negate(sys)

Return the negative of a system.

Control system analysis

dcgain(sys)

Return the zero-frequency (or DC) gain of the given system

evalfr(sys, x)

Evaluate the transfer function of an LTI system for a single complex number x.

freqresp(sys, omega)

Frequency response of an LTI system at multiple angular frequencies.

margin(sysdata)

Calculate gain and phase margins and associated crossover frequencies

stability_margins(sysdata[, returnall, epsw])

Calculate stability margins and associated crossover frequencies.

phase_crossover_frequencies(sys)

Compute frequencies and gains at intersections with real axis in Nyquist plot.

pole(sys)

Compute system poles.

zero(sys)

Compute system zeros.

pzmap(sys[, plot, grid, title])

Plot a pole/zero map for a linear system.

root_locus(sys[, kvect, xlim, ylim, …])

Root locus plot

sisotool(sys[, kvect, xlim_rlocus, …])

Sisotool style collection of plots inspired by MATLAB’s sisotool.

Matrix computations

care(A, B, Q[, R, S, E, stabilizing])

(X, L, G) = care(A, B, Q, R=None) solves the continuous-time algebraic Riccati equation

dare(A, B, Q, R[, S, E, stabilizing])

(X, L, G) = dare(A, B, Q, R) solves the discrete-time algebraic Riccati equation

lyap(A, Q[, C, E])

X = lyap(A, Q) solves the continuous-time Lyapunov equation

dlyap(A, Q[, C, E])

dlyap(A,Q) solves the discrete-time Lyapunov equation

ctrb(A, B)

Controllabilty matrix

obsv(A, C)

Observability matrix

gram(sys, type)

Gramian (controllability or observability)

Control system synthesis

acker(A, B, poles)

Pole placement using Ackermann method

h2syn(P, nmeas, ncon)

H_2 control synthesis for plant P.

hinfsyn(P, nmeas, ncon)

H_{inf} control synthesis for plant P.

lqr(A, B, Q, R[, N])

Linear quadratic regulator design

lqe(A, G, C, QN, RN, [, N])

Linear quadratic estimator design (Kalman filter) for continuous-time systems.

mixsyn(g[, w1, w2, w3])

Mixed-sensitivity H-infinity synthesis.

place(A, B, p)

Place closed loop eigenvalues

Model simplification tools

minreal(sys[, tol, verbose])

Eliminates uncontrollable or unobservable states in state-space models or cancelling pole-zero pairs in transfer functions.

balred(sys, orders[, method, alpha])

Balanced reduced order model of sys of a given order.

hsvd(sys)

Calculate the Hankel singular values.

modred(sys, ELIM[, method])

Model reduction of sys by eliminating the states in ELIM using a given method.

era(YY, m, n, nin, nout, r)

Calculate an ERA model of order r based on the impulse-response data YY.

markov(Y, U[, m, transpose])

Calculate the first m Markov parameters [D CB CAB …] from input U, output Y.

Nonlinear system support

find_eqpt(sys, x0[, u0, y0, t, params, iu, …])

Find the equilibrium point for an input/output system.

linearize(sys, xeq[, ueq, t, params])

Linearize an input/output system at a given state and input.

input_output_response(sys, T[, U, X0, …])

Compute the output response of a system to a given input.

ss2io(*args, **kw)

Create an I/O system from a state space linear system.

tf2io(*args, **kw)

Convert a transfer function into an I/O system

flatsys.point_to_point(sys, x0, u0, xf, uf, Tf)

Compute trajectory between an initial and final conditions.

Utility functions and conversions

augw(g[, w1, w2, w3])

Augment plant for mixed sensitivity problem.

canonical_form(xsys[, form])

Convert a system into canonical form

damp(sys[, doprint])

Compute natural frequency, damping ratio, and poles of a system

db2mag(db)

Convert a gain in decibels (dB) to a magnitude

isctime(sys[, strict])

Check to see if a system is a continuous-time system

isdtime(sys[, strict])

Check to see if a system is a discrete time system

issiso(sys[, strict])

Check to see if a system is single input, single output

issys(obj)

Return True if an object is a system, otherwise False

mag2db(mag)

Convert a magnitude to decibels (dB)

observable_form(xsys)

Convert a system into observable canonical form

pade(T[, n, numdeg])

Create a linear system that approximates a delay.

reachable_form(xsys)

Convert a system into reachable canonical form

reset_defaults()

Reset configuration values to their default (initial) values.

sample_system(sysc, Ts[, method, alpha, …])

Convert a continuous time system to discrete time

ss2tf(sys)

Transform a state space system to a transfer function.

ssdata(sys)

Return state space data objects for a system

tf2ss(sys)

Transform a transfer function to a state space system.

tfdata(sys)

Return transfer function data objects for a system

timebase(sys[, strict])

Return the timebase for an LTI system

timebaseEqual(sys1, sys2)

Check to see if two systems have the same timebase

unwrap(angle[, period])

Unwrap a phase angle to give a continuous curve

use_fbs_defaults()

Use Feedback Systems (FBS) compatible settings.

use_matlab_defaults()

Use MATLAB compatible configuration settings.

use_numpy_matrix([flag, warn])

Turn on/off use of Numpy matrix class for state space operations.

Control system classes

The classes listed below are used to represent models of linear time-invariant (LTI) systems. They are usually created from factory functions such as tf() and ss(), so the user should normally not need to instantiate these directly.

TransferFunction(num, den[, dt])

A class for representing transfer functions

StateSpace(A, B, C, D[, dt])

A class for representing state-space models

FrequencyResponseData(d, w)

A class for models defined by frequency response data (FRD)

InputOutputSystem([inputs, outputs, states, …])

A class for representing input/output systems.

Input/output system subclasses

Input/output systems are accessed primarily via a set of subclasses that allow for linear, nonlinear, and interconnected elements:

LinearIOSystem(linsys[, inputs, outputs, …])

Input/output representation of a linear (state space) system.

NonlinearIOSystem(updfcn[, outfcn, inputs, …])

Nonlinear I/O system.

InterconnectedSystem(syslist[, connections, …])

Interconnection of a set of input/output systems.

MATLAB compatibility module

The control.matlab module contains a number of functions that emulate some of the functionality of MATLAB. The intent of these functions is to provide a simple interface to the python control systems library (python-control) for people who are familiar with the MATLAB Control Systems Toolbox (tm).

Creating linear models

tf(num, den[, dt])

Create a transfer function system.

ss(A, B, C, D[, dt])

Create a state space system.

frd(d, w)

Construct a frequency response data model

rss([states, outputs, inputs])

Create a stable continuous random state space object.

drss([states, outputs, inputs])

Create a stable discrete random state space object.

Utility functions and conversions

mag2db(mag)

Convert a magnitude to decibels (dB)

db2mag(db)

Convert a gain in decibels (dB) to a magnitude

c2d(sysc, Ts[, method, prewarp_frequency])

Return a discrete-time system

ss2tf(sys)

Transform a state space system to a transfer function.

tf2ss(sys)

Transform a transfer function to a state space system.

tfdata(sys)

Return transfer function data objects for a system

System interconnections

series(sys1, *sysn)

Return the series connection (sysn * .

parallel(sys1, *sysn)

Return the parallel connection sys1 + sys2 (+ .

feedback(sys1[, sys2, sign])

Feedback interconnection between two I/O systems.

negate(sys)

Return the negative of a system.

connect(sys, Q, inputv, outputv)

Index-based interconnection of an LTI system.

append(sys1, sys2, …, sysn)

Group models by appending their inputs and outputs

System gain and dynamics

dcgain(*args)

Compute the gain of the system in steady state.

pole(sys)

Compute system poles.

zero(sys)

Compute system zeros.

damp(sys[, doprint])

Compute natural frequency, damping ratio, and poles of a system

pzmap(sys[, plot, grid, title])

Plot a pole/zero map for a linear system.

Time-domain analysis

step(sys[, T, X0, input, output, return_x])

Step response of a linear system

impulse(sys[, T, X0, input, output, return_x])

Impulse response of a linear system

initial(sys[, T, X0, input, output, return_x])

Initial condition response of a linear system

lsim(sys[, U, T, X0])

Simulate the output of a linear system.

Frequency-domain analysis

bode(syslist[, omega, dB, Hz, deg, …])

Bode plot of the frequency response

nyquist(syslist[, omega, plot, label_freq, …])

Nyquist plot for a system

nichols(sys_list[, omega, grid])

Nichols plot for a system

margin(sysdata)

Calculate gain and phase margins and associated crossover frequencies

freqresp(sys, omega)

Frequency response of an LTI system at multiple angular frequencies.

evalfr(sys, x)

Evaluate the transfer function of an LTI system for a single complex number x.

Compensator design

rlocus(sys[, kvect, xlim, ylim, plotstr, …])

Root locus plot

sisotool(sys[, kvect, xlim_rlocus, …])

Sisotool style collection of plots inspired by MATLAB’s sisotool.

place(A, B, p)

Place closed loop eigenvalues

lqr(A, B, Q, R[, N])

Linear quadratic regulator design

State-space (SS) models

rss([states, outputs, inputs])

Create a stable continuous random state space object.

drss([states, outputs, inputs])

Create a stable discrete random state space object.

ctrb(A, B)

Controllabilty matrix

obsv(A, C)

Observability matrix

gram(sys, type)

Gramian (controllability or observability)

Model simplification

minreal(sys[, tol, verbose])

Eliminates uncontrollable or unobservable states in state-space models or cancelling pole-zero pairs in transfer functions.

hsvd(sys)

Calculate the Hankel singular values.

balred(sys, orders[, method, alpha])

Balanced reduced order model of sys of a given order.

modred(sys, ELIM[, method])

Model reduction of sys by eliminating the states in ELIM using a given method.

era(YY, m, n, nin, nout, r)

Calculate an ERA model of order r based on the impulse-response data YY.

markov(Y, U[, m, transpose])

Calculate the first m Markov parameters [D CB CAB …] from input U, output Y.

Time delays

pade(T[, n, numdeg])

Create a linear system that approximates a delay.

Matrix equation solvers and linear algebra

lyap(A, Q[, C, E])

X = lyap(A, Q) solves the continuous-time Lyapunov equation

dlyap(A, Q[, C, E])

dlyap(A,Q) solves the discrete-time Lyapunov equation

care(A, B, Q[, R, S, E, stabilizing])

(X, L, G) = care(A, B, Q, R=None) solves the continuous-time algebraic Riccati equation

dare(A, B, Q, R[, S, E, stabilizing])

(X, L, G) = dare(A, B, Q, R) solves the discrete-time algebraic Riccati equation

Additional functions

gangof4(P, C[, omega])

Plot the “Gang of 4” transfer functions for a system

unwrap(angle[, period])

Unwrap a phase angle to give a continuous curve

Functions imported from other modules

linspace(start, stop[, num, endpoint, …])

Return evenly spaced numbers over a specified interval.

logspace(start, stop[, num, endpoint, base, …])

Return numbers spaced evenly on a log scale.

ss2zpk(A, B, C, D[, input])

State-space representation to zero-pole-gain representation.

tf2zpk(b, a)

Return zero, pole, gain (z, p, k) representation from a numerator, denominator representation of a linear filter.

zpk2ss(z, p, k)

Zero-pole-gain representation to state-space representation

zpk2tf(z, p, k)

Return polynomial transfer function representation from zeros and poles

Differentially flat systems

The control.flatsys package contains a set of classes and functions that can be used to compute trajectories for differentially flat systems.

A differentially flat system is defined by creating an object using the FlatSystem class, which has member functions for mapping the system state and input into and out of flat coordinates. The point_to_point() function can be used to create a trajectory between two endpoints, written in terms of a set of basis functions defined using the BasisFamily class. The resulting trajectory is return as a SystemTrajectory object and can be evaluated using the eval() member function.

Overview of differential flatness

A nonlinear differential equation of the form

\dot x = f(x, u), \qquad x \in R^n, u \in R^m

is differentially flat if there exists a function \alpha such that

z = \alpha(x, u, \dot u\, \dots, u^{(p)})

and we can write the solutions of the nonlinear system as functions of z and a finite number of derivatives

(1)x &= \beta(z, \dot z, \dots, z^{(q)}) \\ u &= \gamma(z, \dot z, \dots, z^{(q)}).

For a differentially flat system, all of the feasible trajectories for the system can be written as functions of a flat output z(\cdot) and its derivatives. The number of flat outputs is always equal to the number of system inputs.

Differentially flat systems are useful in situations where explicit trajectory generation is required. Since the behavior of a flat system is determined by the flat outputs, we can plan trajectories in output space, and then map these to appropriate inputs. Suppose we wish to generate a feasible trajectory for the the nonlinear system

\dot x = f(x, u), \qquad x(0) = x_0,\, x(T) = x_f.

If the system is differentially flat then

x(0) &= \beta\bigl(z(0), \dot z(0), \dots, z^{(q)}(0) \bigr) = x_0, \\ x(T) &= \gamma\bigl(z(T), \dot z(T), \dots, z^{(q)}(T) \bigr) = x_f,

and we see that the initial and final condition in the full state space depends on just the output z and its derivatives at the initial and final times. Thus any trajectory for z that satisfies these boundary conditions will be a feasible trajectory for the system, using equation (1) to determine the full state space and input trajectories.

In particular, given initial and final conditions on z and its derivatives that satisfy the initial and final conditions any curve z(\cdot) satisfying those conditions will correspond to a feasible trajectory of the system. We can parameterize the flat output trajectory using a set of smooth basis functions \psi_i(t):

z(t) = \sum_{i=1}^N \alpha_i \psi_i(t), \qquad \alpha_i \in R

We seek a set of coefficients \alpha_i, i = 1, \dots, N such that z(t) satisfies the boundary conditions for x(0) and x(T). The derivatives of the flat output can be computed in terms of the derivatives of the basis functions:

\dot z(t) &= \sum_{i=1}^N \alpha_i \dot \psi_i(t) \\ &\,\vdots \\ \dot z^{(q)}(t) &= \sum_{i=1}^N \alpha_i \psi^{(q)}_i(t).

We can thus write the conditions on the flat outputs and their derivatives as

\begin{bmatrix} \psi_1(0) & \psi_2(0) & \dots & \psi_N(0) \\ \dot \psi_1(0) & \dot \psi_2(0) & \dots & \dot \psi_N(0) \\ \vdots & \vdots & & \vdots \\ \psi^{(q)}_1(0) & \psi^{(q)}_2(0) & \dots & \psi^{(q)}_N(0) \\[1ex] \psi_1(T) & \psi_2(T) & \dots & \psi_N(T) \\ \dot \psi_1(T) & \dot \psi_2(T) & \dots & \dot \psi_N(T) \\ \vdots & \vdots & & \vdots \\ \psi^{(q)}_1(T) & \psi^{(q)}_2(T) & \dots & \psi^{(q)}_N(T) \\ \end{bmatrix} \begin{bmatrix} \alpha_1 \\ \vdots \\ \alpha_N \end{bmatrix} = \begin{bmatrix} z(0) \\ \dot z(0) \\ \vdots \\ z^{(q)}(0) \\[1ex] z(T) \\ \dot z(T) \\ \vdots \\ z^{(q)}(T) \\ \end{bmatrix}

This equation is a linear equation of the form

M \alpha = \begin{bmatrix} \bar z(0) \\ \bar z(T) \end{bmatrix}

where \bar z is called the flat flag for the system. Assuming that M has a sufficient number of columns and that it is full column rank, we can solve for a (possibly non-unique) \alpha that solves the trajectory generation problem.

Module usage

To create a trajectory for a differentially flat system, a FlatSystem object must be created. This is done by specifying the forward and reverse mappings between the system state/input and the differentially flat outputs and their derivatives (“flat flag”).

The forward() method computes the flat flag given a state and input:

zflag = sys.forward(x, u)

The reverse() method computes the state and input given the flat flag:

x, u = sys.reverse(zflag)

The flag \bar z is implemented as a list of flat outputs z_i and their derivatives up to order q_i:

zflag[i][j] = z_i^{(j)}

The number of flat outputs must match the number of system inputs.

For a linear system, a flat system representation can be generated using the LinearFlatSystem class:

flatsys = control.flatsys.LinearFlatSystem(linsys)

For more general systems, the FlatSystem object must be created manually

flatsys = control.flatsys.FlatSystem(nstate, ninputs, forward, reverse)

In addition to the flat system descriptionn, a set of basis functions \phi_i(t) must be chosen. The FlatBasis class is used to represent the basis functions. A polynomial basis function of the form 1, t, t^2, … can be computed using the PolyBasis class, which is initialized by passing the desired order of the polynomial basis set:

polybasis = control.flatsys.PolyBasis(N)

Once the system and basis function have been defined, the point_to_point() function can be used to compute a trajectory between initial and final states and inputs:

traj = control.flatsys.point_to_point(x0, u0, xf, uf, Tf, basis=polybasis)

The returned object has class SystemTrajectory and can be used to compute the state and input trajectory between the initial and final condition:

xd, ud = traj.eval(T)

where T is a list of times on which the trajectory should be evaluated (e.g., T = numpy.linspace(0, Tf, M).

Example

To illustrate how we can use a two degree-of-freedom design to improve the performance of the system, consider the problem of steering a car to change lanes on a road. We use the non-normalized form of the dynamics, which are derived Feedback Systems by Astrom and Murray, Example 3.11.

import control.flatsys as fs

# Function to take states, inputs and return the flat flag
def vehicle_flat_forward(x, u, params={}):
    # Get the parameter values
    b = params.get('wheelbase', 3.)

    # Create a list of arrays to store the flat output and its derivatives
    zflag = [np.zeros(3), np.zeros(3)]

    # Flat output is the x, y position of the rear wheels
    zflag[0][0] = x[0]
    zflag[1][0] = x[1]

    # First derivatives of the flat output
    zflag[0][1] = u[0] * np.cos(x[2])  # dx/dt
    zflag[1][1] = u[0] * np.sin(x[2])  # dy/dt

    # First derivative of the angle
    thdot = (u[0]/b) * np.tan(u[1])

    # Second derivatives of the flat output (setting vdot = 0)
    zflag[0][2] = -u[0] * thdot * np.sin(x[2])
    zflag[1][2] =  u[0] * thdot * np.cos(x[2])

    return zflag

# Function to take the flat flag and return states, inputs
def vehicle_flat_reverse(zflag, params={}):
    # Get the parameter values
    b = params.get('wheelbase', 3.)

    # Create a vector to store the state and inputs
    x = np.zeros(3)
    u = np.zeros(2)

    # Given the flat variables, solve for the state
    x[0] = zflag[0][0]  # x position
    x[1] = zflag[1][0]  # y position
    x[2] = np.arctan2(zflag[1][1], zflag[0][1])  # tan(theta) = ydot/xdot

    # And next solve for the inputs
    u[0] = zflag[0][1] * np.cos(x[2]) + zflag[1][1] * np.sin(x[2])
    u[1] = np.arctan2(
        (zflag[1][2] * np.cos(x[2]) - zflag[0][2] * np.sin(x[2])), u[0]/b)

    return x, u

vehicle_flat = fs.FlatSystem(
    3, 2, forward=vehicle_flat_forward, reverse=vehicle_flat_reverse)

To find a trajectory from an initial state x_0 to a final state x_\text{f} in time T_\text{f} we solve a point-to-point trajectory generation problem. We also set the initial and final inputs, which sets the vehicle velocity v and steering wheel angle \delta at the endpoints.

# Define the endpoints of the trajectory
x0 = [0., -2., 0.]; u0 = [10., 0.]
xf = [100., 2., 0.]; uf = [10., 0.]
Tf = 10

# Define a set of basis functions to use for the trajectories
poly = fs.PolyFamily(6)

# Find a trajectory between the initial condition and the final condition
traj = fs.point_to_point(vehicle_flat, x0, u0, xf, uf, Tf, basis=poly)

# Create the trajectory
t = np.linspace(0, Tf, 100)
x, u = traj.eval(t)

Module classes and functions

Flat systems classes

BasisFamily(N)

Base class for implementing basis functions for flat systems.

FlatSystem(forward, reverse[, updfcn, …])

Base class for representing a differentially flat system.

LinearFlatSystem(linsys[, inputs, outputs, …])

PolyFamily(N)

Polynomial basis functions.

SystemTrajectory(sys, basis[, coeffs, flaglen])

Class representing a system trajectory.

Flat systems functions

point_to_point(sys, x0, u0, xf, uf, Tf[, …])

Compute trajectory between an initial and final conditions.

Input/output systems

The iosys module contains the InputOutputSystem class that represents (possibly nonlinear) input/output systems. The InputOutputSystem class is a general class that defines any continuous or discrete time dynamical system. Input/output systems can be simulated and also used to compute equilibrium points and linearizations.

Module usage

An input/output system is defined as a dynamical system that has a system state as well as inputs and outputs (either inputs or states can be empty). The dynamics of the system can be in continuous or discrete time. To simulate an input/output system, use the input_output_response() function:

t, y = input_output_response(io_sys, T, U, X0, params)

An input/output system can be linearized around an equilibrium point to obtain a StateSpace linear system. Use the find_eqpt() function to obtain an equilibrium point and the linearize() function to linearize about that equilibrium point:

xeq, ueq = find_eqpt(io_sys, X0, U0)
ss_sys = linearize(io_sys, xeq, ueq)

Input/output systems can be created from state space LTI systems by using the LinearIOSystem class`:

io_sys = LinearIOSystem(ss_sys)

Nonlinear input/output systems can be created using the NonlinearIOSystem class, which requires the definition of an update function (for the right hand side of the differential or different equation) and and output function (computes the outputs from the state):

io_sys = NonlinearIOSystem(updfcn, outfcn, inputs=M, outputs=P, states=N)

More complex input/output systems can be constructed by using the InterconnectedSystem class, which allows a collection of input/output subsystems to be combined with internal connections between the subsystems and a set of overall system inputs and outputs that link to the subsystems:

steering = ct.InterconnectedSystem(
    (plant, controller), name='system',
    connections=(('controller.e', '-plant.y')),
    inplist=('controller.e'), inputs='r',
    outlist=('plant.y'), outputs='y')

Interconnected systems can also be created using block diagram manipulations such as the series(), parallel(), and feedback() functions. The InputOutputSystem class also supports various algebraic operations such as * (series interconnection) and + (parallel interconnection).

Example

To illustrate the use of the input/output systems module, we create a model for a predator/prey system, following the notation and parameter values in FBS2e.

We begin by defining the dynamics of the system

import control
import numpy as np
import matplotlib.pyplot as plt

def predprey_rhs(t, x, u, params):
    # Parameter setup
    a = params.get('a', 3.2)
    b = params.get('b', 0.6)
    c = params.get('c', 50.)
    d = params.get('d', 0.56)
    k = params.get('k', 125)
    r = params.get('r', 1.6)

    # Map the states into local variable names
    H = x[0]
    L = x[1]

    # Compute the control action (only allow addition of food)
    u_0 = u if u > 0 else 0

    # Compute the discrete updates
    dH = (r + u_0) * H * (1 - H/k) - (a * H * L)/(c + H)
    dL = b * (a * H *  L)/(c + H) - d * L

    return [dH, dL]

We now create an input/output system using these dynamics:

io_predprey = control.NonlinearIOSystem(
    predprey_rhs, None, inputs=('u'), outputs=('H', 'L'),
    states=('H', 'L'), name='predprey')

Note that since we have not specified an output function, the entire state will be used as the output of the system.

The io_predprey system can now be simulated to obtain the open loop dynamics of the system:

X0 = [25, 20]                 # Initial H, L
T = np.linspace(0, 70, 500)   # Simulation 70 years of time

# Simulate the system
t, y = control.input_output_response(io_predprey, T, 0, X0)

# Plot the response
plt.figure(1)
plt.plot(t, y[0])
plt.plot(t, y[1])
plt.legend(['Hare', 'Lynx'])
plt.show(block=False)

We can also create a feedback controller to stabilize a desired population of the system. We begin by finding the (unstable) equilibrium point for the system and computing the linearization about that point.

eqpt = control.find_eqpt(io_predprey, X0, 0)
xeq = eqpt[0]                         # choose the nonzero equilibrium point
lin_predprey = control.linearize(io_predprey, xeq, 0)

We next compute a controller that stabilizes the equilibrium point using eigenvalue placement and computing the feedforward gain using the number of lynxes as the desired output (following FBS2e, Example 7.5):

K = control.place(lin_predprey.A, lin_predprey.B, [-0.1, -0.2])
A, B = lin_predprey.A, lin_predprey.B
C = np.array([[0, 1]])                # regulated output = number of lynxes
kf = -1/(C @ np.linalg.inv(A - B @ K) @ B)

To construct the control law, we build a simple input/output system that applies a corrective input based on deviations from the equilibrium point. This system has no dynamics, since it is a static (affine) map, and can constructed using the ~control.ios.NonlinearIOSystem class:

io_controller = control.NonlinearIOSystem(
  None,
  lambda t, x, u, params: -K @ (u[1:] - xeq) + kf * (u[0] - xeq[1]),
  inputs=('Ld', 'u1', 'u2'), outputs=1, name='control')

The input to the controller is u, consisting of the vector of hare and lynx populations followed by the desired lynx population.

To connect the controller to the predatory-prey model, we create an InterconnectedSystem:

io_closed = control.InterconnectedSystem(
  (io_predprey, io_controller),       # systems
  connections=(
    ('predprey.u', 'control.y[0]'),
    ('control.u1',  'predprey.H'),
    ('control.u2',  'predprey.L')
  ),
  inplist=('control.Ld'),
  outlist=('predprey.H', 'predprey.L', 'control.y[0]')
)

Finally, we simulate the closed loop system:

# Simulate the system
t, y = control.input_output_response(io_closed, T, 30, [15, 20])

# Plot the response
plt.figure(2)
plt.subplot(2, 1, 1)
plt.plot(t, y[0])
plt.plot(t, y[1])
plt.legend(['Hare', 'Lynx'])
plt.subplot(2, 1, 2)
plt.plot(t, y[2])
plt.legend(['input'])
plt.show(block=False)

Module classes and functions

Input/output system classes

InputOutputSystem([inputs, outputs, states, …])

A class for representing input/output systems.

InterconnectedSystem(syslist[, connections, …])

Interconnection of a set of input/output systems.

LinearIOSystem(linsys[, inputs, outputs, …])

Input/output representation of a linear (state space) system.

NonlinearIOSystem(updfcn[, outfcn, inputs, …])

Nonlinear I/O system.

Input/output system functions

find_eqpt(sys, x0[, u0, y0, t, params, iu, …])

Find the equilibrium point for an input/output system.

linearize(sys, xeq[, ueq, t, params])

Linearize an input/output system at a given state and input.

input_output_response(sys, T[, U, X0, …])

Compute the output response of a system to a given input.

ss2io(*args, **kw)

Create an I/O system from a state space linear system.

tf2io(*args, **kw)

Convert a transfer function into an I/O system

Examples

The source code for the examples below are available in the examples/ subdirecory of the source code distribution. The can also be accessed online via the [python-control GitHub repository](https://github.com/python-control/python-control/tree/master/examples).

Python scripts

The following Python scripts document the use of a variety of methods in the Python Control Toolbox on examples drawn from standard control textbooks and other sources.

Secord order system (MATLAB module example)

This example computes time and frequency responses for a second-order system using the MATLAB compatibility module.

Code
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
# secord.py - demonstrate some standard MATLAB commands
# RMM, 25 May 09

import os
import matplotlib.pyplot as plt   # MATLAB plotting functions
from control.matlab import *  # MATLAB-like functions

# Parameters defining the system
m = 250.0           # system mass
k = 40.0            # spring constant
b = 60.0            # damping constant

# System matrices
A = [[0, 1.], [-k/m, -b/m]]
B = [[0], [1/m]]
C = [[1., 0]]
sys = ss(A, B, C, 0)

# Step response for the system
plt.figure(1)
yout, T = step(sys)
plt.plot(T.T, yout.T)
plt.show(block=False)

# Bode plot for the system
plt.figure(2)
mag, phase, om = bode(sys, logspace(-2, 2), Plot=True)
plt.show(block=False)

# Nyquist plot for the system
plt.figure(3)
nyquist(sys, logspace(-2, 2))
plt.show(block=False)

# Root lcous plot for the system
rlocus(sys)

if 'PYCONTROL_TEST_EXAMPLES' not in os.environ:
    plt.show()
Notes

1. The environment variable PYCONTROL_TEST_EXAMPLES is used for testing to turn off plotting of the outputs.

Inner/outer control design for vertical takeoff and landing aircraft

This script demonstrates the use of the python-control package for analysis and design of a controller for a vectored thrust aircraft model that is used as a running example through the text Feedback Systems by Astrom and Murray. This example makes use of MATLAB compatible commands.

Code
  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
# pvtol-nested.py - inner/outer design for vectored thrust aircraft
# RMM, 5 Sep 09
#
# This file works through a fairly complicated control design and
# analysis, corresponding to the planar vertical takeoff and landing
# (PVTOL) aircraft in Astrom and Murray, Chapter 11.  It is intended
# to demonstrate the basic functionality of the python-control
# package.
#

from __future__ import print_function

import os
import matplotlib.pyplot as plt  # MATLAB plotting functions
from control.matlab import *    # MATLAB-like functions
import numpy as np

# System parameters
m = 4               # mass of aircraft
J = 0.0475          # inertia around pitch axis
r = 0.25            # distance to center of force
g = 9.8             # gravitational constant
c = 0.05            # damping factor (estimated)

# Transfer functions for dynamics
Pi = tf([r], [J, 0, 0])  # inner loop (roll)
Po = tf([1], [m, c, 0])  # outer loop (position)

#
# Inner loop control design
#
# This is the controller for the pitch dynamics.  Goal is to have
# fast response for the pitch dynamics so that we can use this as a 
# control for the lateral dynamics
#

# Design a simple lead controller for the system
k, a, b = 200, 2, 50
Ci = k*tf([1, a], [1, b])  # lead compensator
Li = Pi*Ci

# Bode plot for the open loop process
plt.figure(1) 
bode(Pi)

# Bode plot for the loop transfer function, with margins
plt.figure(2)
bode(Li)

# Compute out the gain and phase margins
#! Not implemented
# gm, pm, wcg, wcp = margin(Li)

# Compute the sensitivity and complementary sensitivity functions
Si = feedback(1, Li)
Ti = Li*Si

# Check to make sure that the specification is met
plt.figure(3)
gangof4(Pi, Ci)

# Compute out the actual transfer function from u1 to v1 (see L8.2 notes)
# Hi = Ci*(1-m*g*Pi)/(1+Ci*Pi)
Hi = parallel(feedback(Ci, Pi), -m*g*feedback(Ci*Pi, 1))

plt.figure(4)
plt.clf()
plt.subplot(221)
bode(Hi)

# Now design the lateral control system
a, b, K = 0.02, 5, 2
Co = -K*tf([1, 0.3], [1, 10])  # another lead compensator
Lo = -m*g*Po*Co

plt.figure(5)
bode(Lo)  # margin(Lo)

# Finally compute the real outer-loop loop gain + responses
L = Co*Hi*Po
S = feedback(1, L)
T = feedback(L, 1)

# Compute stability margins
gm, pm, wgc, wpc = margin(L)
print("Gain margin: %g at %g" % (gm, wgc))
print("Phase margin: %g at %g" % (pm, wpc))

plt.figure(6)
plt.clf()
bode(L, np.logspace(-4, 3))

# Add crossover line to the magnitude plot
#
# Note: in matplotlib before v2.1, the following code worked:
#
#   plt.subplot(211); hold(True);
#   loglog([1e-4, 1e3], [1, 1], 'k-')
#
# In later versions of matplotlib the call to plt.subplot will clear the
# axes and so we have to extract the axes that we want to use by hand.
# In addition, hold() is deprecated so we no longer require it.
#
for ax in plt.gcf().axes:
    if ax.get_label() == 'control-bode-magnitude':
        break
ax.semilogx([1e-4, 1e3], 20*np.log10([1, 1]), 'k-')

#
# Replot phase starting at -90 degrees
#
# Get the phase plot axes
for ax in plt.gcf().axes:
    if ax.get_label() == 'control-bode-phase':
        break

# Recreate the frequency response and shift the phase
mag, phase, w = freqresp(L, np.logspace(-4, 3))
phase = phase - 360

# Replot the phase by hand
ax.semilogx([1e-4, 1e3], [-180, -180], 'k-')
ax.semilogx(w, np.squeeze(phase), 'b-')
ax.axis([1e-4, 1e3, -360, 0])
plt.xlabel('Frequency [deg]')
plt.ylabel('Phase [deg]')
# plt.set(gca, 'YTick', [-360, -270, -180, -90, 0])
# plt.set(gca, 'XTick', [10^-4, 10^-2, 1, 100])

#
# Nyquist plot for complete design
#
plt.figure(7)
plt.clf()
nyquist(L, (0.0001, 1000))
plt.axis([-700, 5300, -3000, 3000])

# Add a box in the region we are going to expand
plt.plot([-400, -400, 200, 200, -400], [-100, 100, 100, -100, -100], 'r-')

# Expanded region  
plt.figure(8)
plt.clf()
plt.subplot(231)
nyquist(L)
plt.axis([-10, 5, -20, 20])

# set up the color
color = 'b'

# Add arrows to the plot
# H1 = L.evalfr(0.4); H2 = L.evalfr(0.41);
# arrow([real(H1), imag(H1)], [real(H2), imag(H2)], AM_normal_arrowsize, \
#  'EdgeColor', color, 'FaceColor', color);

# H1 = freqresp(L, 0.35); H2 = freqresp(L, 0.36);
# arrow([real(H2), -imag(H2)], [real(H1), -imag(H1)], AM_normal_arrowsize, \
#  'EdgeColor', color, 'FaceColor', color);

plt.figure(9)
Yvec, Tvec = step(T, np.linspace(0, 20))
plt.plot(Tvec.T, Yvec.T)

Yvec, Tvec = step(Co*S, np.linspace(0, 20))
plt.plot(Tvec.T, Yvec.T)

plt.figure(10)
plt.clf()
P, Z = pzmap(T, plot=True, grid=True)
print("Closed loop poles and zeros: ", P, Z)

# Gang of Four
plt.figure(11)
plt.clf()
gangof4(Hi*Po, Co)

if 'PYCONTROL_TEST_EXAMPLES' not in os.environ:
    plt.show()
Notes

1. Importing print_function from __future__ in line 11 is only required if using Python 2.7.

2. The environment variable PYCONTROL_TEST_EXAMPLES is used for testing to turn off plotting of the outputs.

LQR control design for vertical takeoff and landing aircraft

This script demonstrates the use of the python-control package for analysis and design of a controller for a vectored thrust aircraft model that is used as a running example through the text Feedback Systems by Astrom and Murray. This example makes use of MATLAB compatible commands.

Code
  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
# pvtol_lqr.m - LQR design for vectored thrust aircraft
# RMM, 14 Jan 03
#
# This file works through an LQR based design problem, using the
# planar vertical takeoff and landing (PVTOL) aircraft example from
# Astrom and Murray, Chapter 5.  It is intended to demonstrate the
# basic functionality of the python-control package.
#

import os
import numpy as np
import matplotlib.pyplot as plt  # MATLAB plotting functions
from control.matlab import *  # MATLAB-like functions

#
# System dynamics
#
# These are the dynamics for the PVTOL system, written in state space
# form.
#

# System parameters
m = 4       # mass of aircraft
J = 0.0475  # inertia around pitch axis
r = 0.25    # distance to center of force
g = 9.8     # gravitational constant
c = 0.05    # damping factor (estimated)

# State space dynamics
xe = [0, 0, 0, 0, 0, 0]  # equilibrium point of interest
ue = [0, m*g]  # (note these are lists, not matrices)

# TODO: The following objects need converting from np.matrix to np.array
# This will involve re-working the subsequent equations as the shapes
# See below.

# Dynamics matrix (use matrix type so that * works for multiplication)
A = np.matrix(
    [[0, 0, 0, 1, 0, 0],
     [0, 0, 0, 0, 1, 0],
     [0, 0, 0, 0, 0, 1],
     [0, 0, (-ue[0]*np.sin(xe[2]) - ue[1]*np.cos(xe[2]))/m, -c/m, 0, 0],
     [0, 0, (ue[0]*np.cos(xe[2]) - ue[1]*np.sin(xe[2]))/m, 0, -c/m, 0],
     [0, 0, 0, 0, 0, 0]]
)

# Input matrix
B = np.matrix(
    [[0, 0], [0, 0], [0, 0],
     [np.cos(xe[2])/m, -np.sin(xe[2])/m],
     [np.sin(xe[2])/m, np.cos(xe[2])/m],
     [r/J, 0]]
)

# Output matrix 
C = np.matrix([[1, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0]])
D = np.matrix([[0, 0], [0, 0]])

#
# Construct inputs and outputs corresponding to steps in xy position
#
# The vectors xd and yd correspond to the states that are the desired
# equilibrium states for the system.  The matrices Cx and Cy are the 
# corresponding outputs.
#
# The way these vectors are used is to compute the closed loop system
# dynamics as
#
#   xdot = Ax + B u  =>  xdot = (A-BK)x + K xd
#      u = -K(x - xd)       y = Cx
#
# The closed loop dynamics can be simulated using the "step" command, 
# with K*xd as the input vector (assumes that the "input" is unit size,
# so that xd corresponds to the desired steady state.
#

xd = np.matrix([[1], [0], [0], [0], [0], [0]])
yd = np.matrix([[0], [1], [0], [0], [0], [0]])

#
# Extract the relevant dynamics for use with SISO library
#
# The current python-control library only supports SISO transfer
# functions, so we have to modify some parts of the original MATLAB
# code to extract out SISO systems.  To do this, we define the 'lat' and
# 'alt' index vectors to consist of the states that are are relevant
# to the lateral (x) and vertical (y) dynamics.
#

# Indices for the parts of the state that we want
lat = (0, 2, 3, 5)
alt = (1, 4)

# Decoupled dynamics
Ax = (A[lat, :])[:, lat]  # ! not sure why I have to do it this way
Bx = B[lat, 0]
Cx = C[0, lat]
Dx = D[0, 0]

Ay = (A[alt, :])[:, alt]  # ! not sure why I have to do it this way
By = B[alt, 1]
Cy = C[1, alt]
Dy = D[1, 1]

# Label the plot
plt.clf()
plt.suptitle("LQR controllers for vectored thrust aircraft (pvtol-lqr)")

#
# LQR design
#

# Start with a diagonal weighting
Qx1 = np.diag([1, 1, 1, 1, 1, 1])
Qu1a = np.diag([1, 1])
K, X, E = lqr(A, B, Qx1, Qu1a)
K1a = np.matrix(K)

# Close the loop: xdot = Ax - B K (x-xd)
# Note: python-control requires we do this 1 input at a time
# H1a = ss(A-B*K1a, B*K1a*concatenate((xd, yd), axis=1), C, D);
# (T, Y) = step(H1a, T=np.linspace(0,10,100));

# TODO: The following equations will need modifying when converting from np.matrix to np.array
# because the results and even intermediate calculations will be different with numpy arrays
# For example:
# Bx = B[lat, 0]
# Will need to be changed to:
# Bx = B[lat, 0].reshape(-1, 1)
# (if we want it to have the same shape as before)

# For reference, here is a list of the correct shapes of these objects:
# A: (6, 6)
# B: (6, 2)
# C: (2, 6)
# D: (2, 2)
# xd: (6, 1)
# yd: (6, 1)
# Ax: (4, 4)
# Bx: (4, 1)
# Cx: (1, 4)
# Dx: ()
# Ay: (2, 2)
# By: (2, 1)
# Cy: (1, 2)

# Step response for the first input
H1ax = ss(Ax - Bx*K1a[0, lat], Bx*K1a[0, lat]*xd[lat, :], Cx, Dx)
Yx, Tx = step(H1ax, T=np.linspace(0, 10, 100))

# Step response for the second input
H1ay = ss(Ay - By*K1a[1, alt], By*K1a[1, alt]*yd[alt, :], Cy, Dy)
Yy, Ty = step(H1ay, T=np.linspace(0, 10, 100))

plt.subplot(221)
plt.title("Identity weights")
# plt.plot(T, Y[:,1, 1], '-', T, Y[:,2, 2], '--')
plt.plot(Tx.T, Yx.T, '-', Ty.T, Yy.T, '--')
plt.plot([0, 10], [1, 1], 'k-')

plt.axis([0, 10, -0.1, 1.4])
plt.ylabel('position')
plt.legend(('x', 'y'), loc='lower right')

# Look at different input weightings
Qu1a = np.diag([1, 1])
K1a, X, E = lqr(A, B, Qx1, Qu1a)
H1ax = ss(Ax - Bx*K1a[0, lat], Bx*K1a[0, lat]*xd[lat, :], Cx, Dx)

Qu1b = (40 ** 2)*np.diag([1, 1])
K1b, X, E = lqr(A, B, Qx1, Qu1b)
H1bx = ss(Ax - Bx*K1b[0, lat], Bx*K1b[0, lat]*xd[lat, :], Cx, Dx)

Qu1c = (200 ** 2)*np.diag([1, 1])
K1c, X, E = lqr(A, B, Qx1, Qu1c)
H1cx = ss(Ax - Bx*K1c[0, lat], Bx*K1c[0, lat]*xd[lat, :], Cx, Dx)

[Y1, T1] = step(H1ax, T=np.linspace(0, 10, 100))
[Y2, T2] = step(H1bx, T=np.linspace(0, 10, 100))
[Y3, T3] = step(H1cx, T=np.linspace(0, 10, 100))

plt.subplot(222)
plt.title("Effect of input weights")
plt.plot(T1.T, Y1.T, 'b-')
plt.plot(T2.T, Y2.T, 'b-')
plt.plot(T3.T, Y3.T, 'b-')
plt.plot([0, 10], [1, 1], 'k-')

plt.axis([0, 10, -0.1, 1.4])

# arcarrow([1.3, 0.8], [5, 0.45], -6)
plt.text(5.3, 0.4, 'rho')

# Output weighting - change Qx to use outputs
Qx2 = C.T*C
Qu2 = 0.1*np.diag([1, 1])
K, X, E = lqr(A, B, Qx2, Qu2)
K2 = np.matrix(K)

H2x = ss(Ax - Bx*K2[0, lat], Bx*K2[0, lat]*xd[lat, :], Cx, Dx)
H2y = ss(Ay - By*K2[1, alt], By*K2[1, alt]*yd[alt, :], Cy, Dy)

plt.subplot(223)
plt.title("Output weighting")
[Y2x, T2x] = step(H2x, T=np.linspace(0, 10, 100))
[Y2y, T2y] = step(H2y, T=np.linspace(0, 10, 100))
plt.plot(T2x.T, Y2x.T, T2y.T, Y2y.T)
plt.ylabel('position')
plt.xlabel('time')
plt.ylabel('position')
plt.legend(('x', 'y'), loc='lower right')

#
# Physically motivated weighting
#
# Shoot for 1 cm error in x, 10 cm error in y.  Try to keep the angle
# less than 5 degrees in making the adjustments.  Penalize side forces
# due to loss in efficiency.
#

Qx3 = np.diag([100, 10, 2*np.pi/5, 0, 0, 0])
Qu3 = 0.1*np.diag([1, 10])
(K, X, E) = lqr(A, B, Qx3, Qu3)
K3 = np.matrix(K)

H3x = ss(Ax - Bx*K3[0, lat], Bx*K3[0, lat]*xd[lat, :], Cx, Dx)
H3y = ss(Ay - By*K3[1, alt], By*K3[1, alt]*yd[alt, :], Cy, Dy)
plt.subplot(224)
# step(H3x, H3y, 10)
[Y3x, T3x] = step(H3x, T=np.linspace(0, 10, 100))
[Y3y, T3y] = step(H3y, T=np.linspace(0, 10, 100))
plt.plot(T3x.T, Y3x.T, T3y.T, Y3y.T)
plt.title("Physically motivated weights")
plt.xlabel('time')
plt.legend(('x', 'y'), loc='lower right')

if 'PYCONTROL_TEST_EXAMPLES' not in os.environ:
    plt.show()
Notes

1. The environment variable PYCONTROL_TEST_EXAMPLES is used for testing to turn off plotting of the outputs.

Balanced model reduction examples

Code
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
#!/usr/bin/env python

import os

import numpy as np
import control.modelsimp as msimp
import control.matlab as mt
from control.statesp import StateSpace
import matplotlib.pyplot as plt

plt.close('all')

# controllable canonical realization computed in MATLAB for the
# transfer function: num = [1 11 45 32], den = [1 15 60 200 60]
A = np.array([
    [-15., -7.5, -6.25, -1.875],
    [8., 0., 0., 0.],
    [0., 4., 0., 0.],
    [0., 0., 1., 0.]
])
B = np.array([
    [2.],
    [0.],
    [0.],
    [0.]
])
C = np.array([[0.5, 0.6875, 0.7031, 0.5]])
D = np.array([[0.]])

# The full system
fsys = StateSpace(A, B, C, D)

# The reduced system, truncating the order by 1
n = 3
rsys = msimp.balred(fsys, n, method='truncate')

# Comparison of the step responses of the full and reduced systems
plt.figure(1)
y, t = mt.step(fsys)
yr, tr = mt.step(rsys)
plt.plot(t.T, y.T)
plt.plot(tr.T, yr.T)

# Repeat balanced reduction, now with 100-dimensional random state space
sysrand = mt.rss(100, 1, 1)
rsysrand = msimp.balred(sysrand, 10, method='truncate')

# Comparison of the impulse responses of the full and reduced random systems
plt.figure(2)
yrand, trand = mt.impulse(sysrand)
yrandr, trandr = mt.impulse(rsysrand)
plt.plot(trand.T, yrand.T, trandr.T, yrandr.T)

if 'PYCONTROL_TEST_EXAMPLES' not in os.environ:
    plt.show()
Notes

1. The environment variable PYCONTROL_TEST_EXAMPLES is used for testing to turn off plotting of the outputs.

Phase plot examples

Code
  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
# phaseplots.py - examples of phase portraits
# RMM, 24 July 2011
#
# This file contains examples of phase portraits pulled from "Feedback
# Systems" by Astrom and Murray (Princeton University Press, 2008).

import os

import numpy as np
import matplotlib.pyplot as plt
from control.phaseplot import phase_plot
from numpy import pi

# Clear out any figures that are present
plt.close('all')

#
# Inverted pendulum
#

# Define the ODEs for a damped (inverted) pendulum
def invpend_ode(x, t, m=1., l=1., b=0.2, g=1):
    return x[1], -b/m*x[1] + (g*l/m)*np.sin(x[0])


# Set up the figure the way we want it to look
plt.figure()
plt.clf()
plt.axis([-2*pi, 2*pi, -2.1, 2.1])
plt.title('Inverted pendulum')

# Outer trajectories
phase_plot(
    invpend_ode,
    X0=[[-2*pi, 1.6], [-2*pi, 0.5], [-1.8, 2.1],
        [-1, 2.1], [4.2, 2.1], [5, 2.1],
        [2*pi, -1.6], [2*pi, -0.5], [1.8, -2.1],
        [1, -2.1], [-4.2, -2.1], [-5, -2.1]],
    T=np.linspace(0, 40, 200),
    logtime=(3, 0.7)
)

# Separatrices
phase_plot(invpend_ode, X0=[[-2.3056, 2.1], [2.3056, -2.1]], T=6, lingrid=0)

#
# Systems of ODEs: damped oscillator example (simulation + phase portrait)
#

def oscillator_ode(x, t, m=1., b=1, k=1):
    return x[1], -k/m*x[0] - b/m*x[1]


# Generate a vector plot for the damped oscillator
plt.figure()
plt.clf()
phase_plot(oscillator_ode, [-1, 1, 10], [-1, 1, 10], 0.15)
#plt.plot([0], [0], '.')
# a=gca; set(a,'FontSize',20); set(a,'DataAspectRatio',[1,1,1])
plt.xlabel('$x_1$')
plt.ylabel('$x_2$')
plt.title('Damped oscillator, vector field')

# Generate a phase plot for the damped oscillator
plt.figure()
plt.clf()
plt.axis([-1, 1, -1, 1])  # set(gca, 'DataAspectRatio', [1, 1, 1]);
phase_plot(
    oscillator_ode,
    X0=[
        [-1, 1], [-0.3, 1], [0, 1], [0.25, 1], [0.5, 1], [0.75, 1], [1, 1],
        [1, -1], [0.3, -1], [0, -1], [-0.25, -1], [-0.5, -1], [-0.75, -1], [-1, -1]
    ],
    T=np.linspace(0, 8, 80),
    timepts=[0.25, 0.8, 2, 3]
)
plt.plot([0], [0], 'k.')  # 'MarkerSize', AM_data_markersize*3)
# set(gca, 'DataAspectRatio', [1,1,1])
plt.xlabel('$x_1$')
plt.ylabel('$x_2$')
plt.title('Damped oscillator, vector field and stream lines')

#
# Stability definitions
#
# This set of plots illustrates the various types of equilibrium points.
#


def saddle_ode(x, t):
    """Saddle point vector field"""
    return x[0] - 3*x[1], -3*x[0] + x[1]


# Asy stable
m = 1
b = 1
k = 1  # default values
plt.figure()
plt.clf()
plt.axis([-1, 1, -1, 1])  # set(gca, 'DataAspectRatio', [1 1 1]);
phase_plot(
    oscillator_ode,
    X0=[
        [-1, 1], [-0.3, 1], [0, 1], [0.25, 1], [0.5, 1], [0.7, 1], [1, 1], [1.3, 1],
        [1, -1], [0.3, -1], [0, -1], [-0.25, -1], [-0.5, -1], [-0.7, -1], [-1, -1],
        [-1.3, -1]
    ],
    T=np.linspace(0, 10, 100),
    timepts=[0.3, 1, 2, 3],
    parms=(m, b, k)
)
plt.plot([0], [0], 'k.')  # 'MarkerSize', AM_data_markersize*3)
# plt.set(gca,'FontSize', 16)
plt.xlabel('$x_1$')
plt.ylabel('$x_2$')
plt.title('Asymptotically stable point')

# Saddle
plt.figure()
plt.clf()
plt.axis([-1, 1, -1, 1])  # set(gca, 'DataAspectRatio', [1 1 1])
phase_plot(
    saddle_ode,
    scale=2,
    timepts=[0.2, 0.5, 0.8],
    X0=[
        [-1, -1], [1, 1],
        [-1, -0.95], [-1, -0.9], [-1, -0.8], [-1, -0.6], [-1, -0.4], [-1, -0.2],
        [-0.95, -1], [-0.9, -1], [-0.8, -1], [-0.6, -1], [-0.4, -1], [-0.2, -1],
        [1, 0.95], [1, 0.9], [1, 0.8], [1, 0.6], [1, 0.4], [1, 0.2],
        [0.95, 1], [0.9, 1], [0.8, 1], [0.6, 1], [0.4, 1], [0.2, 1],
        [-0.5, -0.45], [-0.45, -0.5], [0.5, 0.45], [0.45, 0.5],
        [-0.04, 0.04], [0.04, -0.04]
    ],
    T=np.linspace(0, 2, 20)
)
plt.plot([0], [0], 'k.')  # 'MarkerSize', AM_data_markersize*3)
# set(gca,'FontSize', 16)
plt.xlabel('$x_1$')
plt.ylabel('$x_2$')
plt.title('Saddle point')

# Stable isL
m = 1
b = 0
k = 1  # zero damping
plt.figure()
plt.clf()
plt.axis([-1, 1, -1, 1])  # set(gca, 'DataAspectRatio', [1 1 1]);
phase_plot(
    oscillator_ode,
    timepts=[pi/6, pi/3, pi/2, 2*pi/3, 5*pi/6, pi, 7*pi/6,
             4*pi/3, 9*pi/6, 5*pi/3, 11*pi/6, 2*pi],
    X0=[[0.2, 0], [0.4, 0], [0.6, 0], [0.8, 0], [1, 0], [1.2, 0], [1.4, 0]],
    T=np.linspace(0, 20, 200),
    parms=(m, b, k)
)
plt.plot([0], [0], 'k.')  # 'MarkerSize', AM_data_markersize*3)
# plt.set(gca,'FontSize', 16)
plt.xlabel('$x_1$')
plt.ylabel('$x_2$')
plt.title('Undamped system\nLyapunov stable, not asympt. stable')

if 'PYCONTROL_TEST_EXAMPLES' not in os.environ:
    plt.show()
Notes

1. The environment variable PYCONTROL_TEST_EXAMPLES is used for testing to turn off plotting of the outputs.

SISO robust control example (SP96, Example 2.1)

Code
  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
"""robust_siso.py

Demonstrate mixed-sensitivity H-infinity design for a SISO plant.

Based on Example 2.11 from Multivariable Feedback Control, Skogestad
and Postlethwaite, 1st Edition.
"""

import os

import numpy as np
import matplotlib.pyplot as plt

from control import tf, mixsyn, feedback, step_response

s = tf([1, 0], 1)
# the plant
g = 200/(10*s + 1) / (0.05*s + 1)**2
# disturbance plant
gd = 100/(10*s + 1)

# first design
# sensitivity weighting
M = 1.5
wb = 10
A = 1e-4
ws1 = (s/M + wb) / (s + wb*A)
# KS weighting
wu = tf(1, 1)

k1, cl1, info1 = mixsyn(g, ws1, wu)

# sensitivity (S) and complementary sensitivity (T) functions for
# design 1
s1 = feedback(1, g*k1)
t1 = feedback(g*k1, 1)

# second design
# this weighting differs from the text, where A**0.5 is used; if you use that,
# the frequency response doesn't match the figure.  The time responses
# are similar, though.
ws2 = (s/M ** 0.5 + wb)**2 / (s + wb*A)**2
# the KS weighting is the same as for the first design

k2, cl2, info2 = mixsyn(g, ws2, wu)

# S and T for design 2
s2 = feedback(1, g*k2)
t2 = feedback(g*k2, 1)

# frequency response
omega = np.logspace(-2, 2, 101)
ws1mag, _, _ = ws1.freqresp(omega)
s1mag, _, _ = s1.freqresp(omega)
ws2mag, _, _ = ws2.freqresp(omega)
s2mag, _, _ = s2.freqresp(omega)

plt.figure(1)
# text uses log-scaled absolute, but dB are probably more familiar to most control engineers
plt.semilogx(omega, 20*np.log10(s1mag.flat), label='$S_1$')
plt.semilogx(omega, 20*np.log10(s2mag.flat), label='$S_2$')
# -1 in logspace is inverse
plt.semilogx(omega, -20*np.log10(ws1mag.flat), label='$1/w_{P1}$')
plt.semilogx(omega, -20*np.log10(ws2mag.flat), label='$1/w_{P2}$')

plt.ylim([-80, 10])
plt.xlim([1e-2, 1e2])
plt.xlabel('freq [rad/s]')
plt.ylabel('mag [dB]')
plt.legend()
plt.title('Sensitivity and sensitivity weighting frequency responses')

# time response
time = np.linspace(0, 3, 201)
_, y1 = step_response(t1, time)
_, y2 = step_response(t2, time)

# gd injects into the output (that is, g and gd are summed), and the
# closed loop mapping from output disturbance->output is S.
_, y1d = step_response(s1*gd, time)
_, y2d = step_response(s2*gd, time)

plt.figure(2)
plt.subplot(1, 2, 1)
plt.plot(time, y1, label='$y_1(t)$')
plt.plot(time, y2, label='$y_2(t)$')

plt.ylim([-0.1, 1.5])
plt.xlim([0, 3])
plt.xlabel('time [s]')
plt.ylabel('signal [1]')
plt.legend()
plt.title('Tracking response')

plt.subplot(1, 2, 2)
plt.plot(time, y1d, label='$y_1(t)$')
plt.plot(time, y2d, label='$y_2(t)$')

plt.ylim([-0.1, 1.5])
plt.xlim([0, 3])
plt.xlabel('time [s]')
plt.ylabel('signal [1]')
plt.legend()
plt.title('Disturbance response')

if 'PYCONTROL_TEST_EXAMPLES' not in os.environ:
    plt.show()
Notes

1. The environment variable PYCONTROL_TEST_EXAMPLES is used for testing to turn off plotting of the outputs.

MIMO robust control example (SP96, Example 3.8)

Code
  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
"""robust_mimo.py

Demonstrate mixed-sensitivity H-infinity design for a MIMO plant.

Based on Example 3.8 from Multivariable Feedback Control, Skogestad and Postlethwaite, 1st Edition.
"""

import os

import numpy as np
import matplotlib.pyplot as plt

from control import tf, ss, mixsyn, step_response


def weighting(wb, m, a):
    """weighting(wb,m,a) -> wf
    wb - design frequency (where |wf| is approximately 1)
    m - high frequency gain of 1/wf; should be > 1
    a - low frequency gain of 1/wf; should be < 1
    wf - SISO LTI object
    """
    s = tf([1, 0], [1])
    return (s/m + wb) / (s + wb*a)


def plant():
    """plant() -> g
    g - LTI object; 2x2 plant with a RHP zero, at s=0.5.
    """
    den = [0.2, 1.2, 1]
    gtf = tf([[[1], [1]],
              [[2, 1], [2]]],
             [[den, den],
              [den, den]])
    return ss(gtf)


# as of this writing (2017-07-01), python-control doesn't have an
# equivalent to Matlab's sigma function, so use a trivial stand-in.
def triv_sigma(g, w):
    """triv_sigma(g,w) -> s
    g - LTI object, order n
    w - frequencies, length m
    s - (m,n) array of singular values of g(1j*w)"""
    m, p, _ = g.freqresp(w)
    sjw = (m*np.exp(1j*p)).transpose(2, 0, 1)
    sv = np.linalg.svd(sjw, compute_uv=False)
    return sv


def analysis():
    """Plot open-loop responses for various inputs"""
    g = plant()

    t = np.linspace(0, 10, 101)
    _, yu1 = step_response(g, t, input=0)
    _, yu2 = step_response(g, t, input=1)

    yu1 = yu1
    yu2 = yu2

    # linear system, so scale and sum previous results to get the
    # [1,-1] response
    yuz = yu1 - yu2

    plt.figure(1)
    plt.subplot(1, 3, 1)
    plt.plot(t, yu1[0], label='$y_1$')
    plt.plot(t, yu1[1], label='$y_2$')
    plt.xlabel('time')
    plt.ylabel('output')
    plt.ylim([-1.1, 2.1])
    plt.legend()
    plt.title('o/l response\nto input [1,0]')

    plt.subplot(1, 3, 2)
    plt.plot(t, yu2[0], label='$y_1$')
    plt.plot(t, yu2[1], label='$y_2$')
    plt.xlabel('time')
    plt.ylabel('output')
    plt.ylim([-1.1, 2.1])
    plt.legend()
    plt.title('o/l response\nto input [0,1]')

    plt.subplot(1, 3, 3)
    plt.plot(t, yuz[0], label='$y_1$')
    plt.plot(t, yuz[1], label='$y_2$')
    plt.xlabel('time')
    plt.ylabel('output')
    plt.ylim([-1.1, 2.1])
    plt.legend()
    plt.title('o/l response\nto input [1,-1]')


def synth(wb1, wb2):
    """synth(wb1,wb2) -> k,gamma
    wb1: S weighting frequency
    wb2: KS weighting frequency
    k: controller
    gamma: H-infinity norm of 'design', that is, of evaluation system
    with loop closed through design
    """
    g = plant()
    wu = ss([], [], [], np.eye(2))
    wp1 = ss(weighting(wb=wb1, m=1.5, a=1e-4))
    wp2 = ss(weighting(wb=wb2, m=1.5, a=1e-4))
    wp = wp1.append(wp2)
    k, _, info = mixsyn(g, wp, wu)
    return k, info[0]


def step_opposite(g, t):
    """reponse to step of [-1,1]"""
    _, yu1 = step_response(g, t, input=0)
    _, yu2 = step_response(g, t, input=1)
    return yu1 - yu2


def design():
    """Show results of designs"""
    # equal weighting on each output
    k1, gam1 = synth(0.25, 0.25)
    # increase "bandwidth" of output 2 by moving crossover weighting frequency 100 times higher
    k2, gam2 = synth(0.25, 25)
    # now weight output 1 more heavily
    # won't plot this one, just want gamma
    _, gam3 = synth(25, 0.25)

    print('design 1 gamma {:.3g} (Skogestad: 2.80)'.format(gam1))
    print('design 2 gamma {:.3g} (Skogestad: 2.92)'.format(gam2))
    print('design 3 gamma {:.3g} (Skogestad: 6.73)'.format(gam3))

    # do the designs
    g = plant()
    w = np.logspace(-2, 2, 101)
    I = ss([], [], [], np.eye(2))
    s1 = I.feedback(g*k1)
    s2 = I.feedback(g*k2)

    # frequency response
    sv1 = triv_sigma(s1, w)
    sv2 = triv_sigma(s2, w)

    plt.figure(2)

    plt.subplot(1, 2, 1)
    plt.semilogx(w, 20*np.log10(sv1[:, 0]), label=r'$\sigma_1(S_1)$')
    plt.semilogx(w, 20*np.log10(sv1[:, 1]), label=r'$\sigma_2(S_1)$')
    plt.semilogx(w, 20*np.log10(sv2[:, 0]), label=r'$\sigma_1(S_2)$')
    plt.semilogx(w, 20*np.log10(sv2[:, 1]), label=r'$\sigma_2(S_2)$')
    plt.ylim([-60, 10])
    plt.ylabel('magnitude [dB]')
    plt.xlim([1e-2, 1e2])
    plt.xlabel('freq [rad/s]')
    plt.legend()
    plt.title('Singular values of S')

    # time response

    # in design 1, both outputs have an inverse initial response; in
    # design 2, output 2 does not, and is very fast, while output 1
    # has a larger initial inverse response than in design 1
    time = np.linspace(0, 10, 301)
    t1 = (g*k1).feedback(I)
    t2 = (g*k2).feedback(I)

    y1 = step_opposite(t1, time)
    y2 = step_opposite(t2, time)

    plt.subplot(1, 2, 2)
    plt.plot(time, y1[0], label='des. 1 $y_1(t))$')
    plt.plot(time, y1[1], label='des. 1 $y_2(t))$')
    plt.plot(time, y2[0], label='des. 2 $y_1(t))$')
    plt.plot(time, y2[1], label='des. 2 $y_2(t))$')
    plt.xlabel('time [s]')
    plt.ylabel('response [1]')
    plt.legend()
    plt.title('c/l response to reference [1,-1]')


if __name__ == "__main__":
    analysis()
    design()
    if 'PYCONTROL_TEST_EXAMPLES' not in os.environ:
        plt.show()
Notes

1. The environment variable PYCONTROL_TEST_EXAMPLES is used for testing to turn off plotting of the outputs.

Cruise control design example (as a nonlinear I/O system)

Code
  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
# cruise-control.py - Cruise control example from FBS
# RMM, 16 May 2019
#
# The cruise control system of a car is a common feedback system encountered
# in everyday life. The system attempts to maintain a constant velocity in the
# presence of disturbances primarily caused by changes in the slope of a
# road. The controller compensates for these unknowns by measuring the speed
# of the car and adjusting the throttle appropriately.
#
# This file explore the dynamics and control of the cruise control system,
# following the material presenting in Feedback Systems by Astrom and Murray.
# A full nonlinear model of the vehicle dynamics is used, with both PI and
# state space control laws.  Different methods of constructing control systems
# are show, all using the InputOutputSystem class (and subclasses).

import numpy as np
import matplotlib.pyplot as plt
from math import pi
import control as ct

#
# Section 4.1: Cruise control modeling and control
#

# Vehicle model: vehicle()
#
# To develop a mathematical model we start with a force balance for
# the car body. Let v be the speed of the car, m the total mass
# (including passengers), F the force generated by the contact of the
# wheels with the road, and Fd the disturbance force due to gravity,
# friction, and aerodynamic drag.

def vehicle_update(t, x, u, params={}):
    """Vehicle dynamics for cruise control system.

    Parameters
    ----------
    x : array
         System state: car velocity in m/s
    u : array
         System input: [throttle, gear, road_slope], where throttle is
         a float between 0 and 1, gear is an integer between 1 and 5,
         and road_slope is in rad.

    Returns
    -------
    float
        Vehicle acceleration

    """
    from math import copysign, sin
    sign = lambda x: copysign(1, x)         # define the sign() function
    
    # Set up the system parameters
    m = params.get('m', 1600.)
    g = params.get('g', 9.8)
    Cr = params.get('Cr', 0.01)
    Cd = params.get('Cd', 0.32)
    rho = params.get('rho', 1.3)
    A = params.get('A', 2.4)
    alpha = params.get(
        'alpha', [40, 25, 16, 12, 10])      # gear ratio / wheel radius

    # Define variables for vehicle state and inputs
    v = x[0]                           # vehicle velocity
    throttle = np.clip(u[0], 0, 1)     # vehicle throttle
    gear = u[1]                        # vehicle gear
    theta = u[2]                       # road slope

    # Force generated by the engine

    omega = alpha[int(gear)-1] * v      # engine angular speed
    F = alpha[int(gear)-1] * motor_torque(omega, params) * throttle

    # Disturbance forces
    #
    # The disturbance force Fd has three major components: Fg, the forces due
    # to gravity; Fr, the forces due to rolling friction; and Fa, the
    # aerodynamic drag.

    # Letting the slope of the road be \theta (theta), gravity gives the
    # force Fg = m g sin \theta.
    
    Fg = m * g * sin(theta)

    # A simple model of rolling friction is Fr = m g Cr sgn(v), where Cr is
    # the coefficient of rolling friction and sgn(v) is the sign of v (+/- 1) or
    # zero if v = 0.
    
    Fr  = m * g * Cr * sign(v)

    # The aerodynamic drag is proportional to the square of the speed: Fa =
    # 1/\rho Cd A |v| v, where \rho is the density of air, Cd is the
    # shape-dependent aerodynamic drag coefficient, and A is the frontal area
    # of the car.

    Fa = 1/2 * rho * Cd * A * abs(v) * v
    
    # Final acceleration on the car
    Fd = Fg + Fr + Fa
    dv = (F - Fd) / m
    
    return dv

# Engine model: motor_torque
#
# The force F is generated by the engine, whose torque is proportional to
# the rate of fuel injection, which is itself proportional to a control
# signal 0 <= u <= 1 that controls the throttle position. The torque also
# depends on engine speed omega.
    
def motor_torque(omega, params={}):
    # Set up the system parameters
    Tm = params.get('Tm', 190.)             # engine torque constant
    omega_m = params.get('omega_m', 420.)   # peak engine angular speed
    beta = params.get('beta', 0.4)          # peak engine rolloff

    return np.clip(Tm * (1 - beta * (omega/omega_m - 1)**2), 0, None)

# Define the input/output system for the vehicle
vehicle = ct.NonlinearIOSystem(
    vehicle_update, None, name='vehicle',
    inputs = ('u', 'gear', 'theta'), outputs = ('v'), states=('v'))

# Figure 1.11: A feedback system for controlling the speed of a vehicle. In
# this example, the speed of the vehicle is measured and compared to the
# desired speed.  The controller is a PI controller represented as a transfer
# function.  In the textbook, the simulations are done for LTI systems, but
# here we simulate the full nonlinear system.

# Construct a PI controller with rolloff, as a transfer function
Kp = 0.5                        # proportional gain
Ki = 0.1                        # integral gain
control_tf = ct.tf2io(
    ct.TransferFunction([Kp, Ki], [1, 0.01*Ki/Kp]),
    name='control', inputs='u', outputs='y')

# Construct the closed loop control system
# Inputs: vref, gear, theta
# Outputs: v (vehicle velocity)
cruise_tf = ct.InterconnectedSystem(
    (control_tf, vehicle), name='cruise',
    connections = (
        ('control.u', '-vehicle.v'),
        ('vehicle.u', 'control.y')),
    inplist = ('control.u', 'vehicle.gear', 'vehicle.theta'),
    inputs = ('vref', 'gear', 'theta'),
    outlist = ('vehicle.v', 'vehicle.u'),
    outputs = ('v', 'u'))

# Define the time and input vectors
T = np.linspace(0, 25, 101)
vref = 20 * np.ones(T.shape)
gear = 4 * np.ones(T.shape)
theta0 = np.zeros(T.shape)

# Now simulate the effect of a hill at t = 5 seconds
plt.figure()
plt.suptitle('Response to change in road slope')
vel_axes = plt.subplot(2, 1, 1)
inp_axes = plt.subplot(2, 1, 2)
theta_hill = np.array([
    0 if t <= 5 else
    4./180. * pi * (t-5) if t <= 6 else
    4./180. * pi for t in T])

for m in (1200, 1600, 2000):
    # Compute the equilibrium state for the system
    X0, U0 = ct.find_eqpt(
        cruise_tf, [0, vref[0]], [vref[0], gear[0], theta0[0]], 
        iu=[1, 2], y0=[vref[0], 0], iy=[0], params={'m':m})

    t, y = ct.input_output_response(
        cruise_tf, T, [vref, gear, theta_hill], X0, params={'m':m})

    # Plot the velocity
    plt.sca(vel_axes)
    plt.plot(t, y[0])

    # Plot the input
    plt.sca(inp_axes)
    plt.plot(t, y[1])

# Add labels to the plots
plt.sca(vel_axes)
plt.ylabel('Speed [m/s]')
plt.legend(['m = 1000 kg', 'm = 2000 kg', 'm = 3000 kg'], frameon=False)

plt.sca(inp_axes)
plt.ylabel('Throttle')
plt.xlabel('Time [s]')

# Figure 4.2: Torque curves for a typical car engine. The graph on the
# left shows the torque generated by the engine as a function of the
# angular velocity of the engine, while the curve on the right shows
# torque as a function of car speed for different gears.

plt.figure()
plt.suptitle('Torque curves for typical car engine')

# Figure 4.2a - single torque curve as function of omega
omega_range = np.linspace(0, 700, 701)
plt.subplot(2, 2, 1)
plt.plot(omega_range, [motor_torque(w) for w in omega_range])
plt.xlabel('Angular velocity $\omega$ [rad/s]')
plt.ylabel('Torque $T$ [Nm]')
plt.grid(True, linestyle='dotted')

# Figure 4.2b - torque curves in different gears, as function of velocity
plt.subplot(2, 2, 2)
v_range = np.linspace(0, 70, 71)
alpha = [40, 25, 16, 12, 10]
for gear in range(5):
    omega_range = alpha[gear] * v_range
    plt.plot(v_range, [motor_torque(w) for w in omega_range],
             color='blue', linestyle='solid')

# Set up the axes and style
plt.axis([0, 70, 100, 200])
plt.grid(True, linestyle='dotted')

# Add labels
plt.text(11.5, 120, '$n$=1')
plt.text(24, 120, '$n$=2')
plt.text(42.5, 120, '$n$=3')
plt.text(58.5, 120, '$n$=4')
plt.text(58.5, 185, '$n$=5')
plt.xlabel('Velocity $v$ [m/s]')
plt.ylabel('Torque $T$ [Nm]')

plt.show(block=False)

# Figure 4.3: Car with cruise control encountering a sloping road

# PI controller model: control_pi()
#
# We add to this model a feedback controller that attempts to regulate the
# speed of the car in the presence of disturbances. We shall use a
# proportional-integral controller

def pi_update(t, x, u, params={}):
    # Get the controller parameters that we need
    ki = params.get('ki', 0.1)
    kaw = params.get('kaw', 2)  # anti-windup gain

    # Assign variables for inputs and states (for readability)
    v = u[0]                    # current velocity
    vref = u[1]                 # reference velocity
    z = x[0]                    # integrated error

    # Compute the nominal controller output (needed for anti-windup)
    u_a = pi_output(t, x, u, params)

    # Compute anti-windup compensation (scale by ki to account for structure)
    u_aw = kaw/ki * (np.clip(u_a, 0, 1) - u_a) if ki != 0 else 0

    # State is the integrated error, minus anti-windup compensation
    return (vref - v) + u_aw

def pi_output(t, x, u, params={}):
    # Get the controller parameters that we need
    kp = params.get('kp', 0.5)
    ki = params.get('ki', 0.1)

    # Assign variables for inputs and states (for readability)
    v = u[0]                    # current velocity
    vref = u[1]                 # reference velocity
    z = x[0]                    # integrated error

    # PI controller
    return kp * (vref - v) + ki * z

control_pi = ct.NonlinearIOSystem(
    pi_update, pi_output, name='control',
    inputs = ['v', 'vref'], outputs = ['u'], states = ['z'],
    params = {'kp':0.5, 'ki':0.1})

# Create the closed loop system
cruise_pi = ct.InterconnectedSystem(
    (vehicle, control_pi), name='cruise',
    connections=(
        ('vehicle.u', 'control.u'),
        ('control.v', 'vehicle.v')),
    inplist=('control.vref', 'vehicle.gear', 'vehicle.theta'),
    outlist=('control.u', 'vehicle.v'), outputs=['u', 'v'])

# Figure 4.3b shows the response of the closed loop system.  The figure shows
# that even if the hill is so steep that the throttle changes from 0.17 to
# almost full throttle, the largest speed error is less than 1 m/s, and the
# desired velocity is recovered after 20 s.

# Define a function for creating a "standard" cruise control plot
def cruise_plot(sys, t, y, t_hill=5, vref=20, antiwindup=False,
                linetype='b-', subplots=[None, None]):
    # Figure out the plot bounds and indices
    v_min = vref-1.2; v_max = vref+0.5; v_ind = sys.find_output('v')
    u_min = 0; u_max = 2 if antiwindup else 1; u_ind = sys.find_output('u')

    # Make sure the upper and lower bounds on v are OK
    while max(y[v_ind]) > v_max: v_max += 1
    while min(y[v_ind]) < v_min: v_min -= 1

    # Create arrays for return values
    subplot_axes = list(subplots)

    # Velocity profile
    if subplot_axes[0] is None:
        subplot_axes[0] = plt.subplot(2, 1, 1)
    else:
        plt.sca(subplots[0])
    plt.plot(t, y[v_ind], linetype)
    plt.plot(t, vref*np.ones(t.shape), 'k-')
    plt.plot([t_hill, t_hill], [v_min, v_max], 'k--')
    plt.axis([0, t[-1], v_min, v_max])
    plt.xlabel('Time $t$ [s]')
    plt.ylabel('Velocity $v$ [m/s]')

    # Commanded input profile
    if subplot_axes[1] is None:
        subplot_axes[1] = plt.subplot(2, 1, 2)
    else:
        plt.sca(subplots[1])
    plt.plot(t, y[u_ind], 'r--' if antiwindup else linetype)
    plt.plot([t_hill, t_hill], [u_min, u_max], 'k--')
    plt.axis([0, t[-1], u_min, u_max])
    plt.xlabel('Time $t$ [s]')
    plt.ylabel('Throttle $u$')

    # Applied input profile
    if antiwindup:
        # TODO: plot the actual signal from the process?
        plt.plot(t, np.clip(y[u_ind], 0, 1), linetype)
        plt.legend(['Commanded', 'Applied'], frameon=False)

    return subplot_axes

# Define the time and input vectors
T = np.linspace(0, 30, 101)
vref = 20 * np.ones(T.shape)
gear = 4 * np.ones(T.shape)
theta0 = np.zeros(T.shape)

# Compute the equilibrium throttle setting for the desired speed (solve for x
# and u given the gear, slope, and desired output velocity)
X0, U0, Y0 = ct.find_eqpt(
    cruise_pi, [vref[0], 0], [vref[0], gear[0], theta0[0]],
    y0=[0, vref[0]], iu=[1, 2], iy=[1], return_y=True)

# Now simulate the effect of a hill at t = 5 seconds
plt.figure()
plt.suptitle('Car with cruise control encountering sloping road')
theta_hill = [
    0 if t <= 5 else
    4./180. * pi * (t-5) if t <= 6 else
    4./180. * pi for t in T]
t, y = ct.input_output_response(cruise_pi, T, [vref, gear, theta_hill], X0)
cruise_plot(cruise_pi, t, y)

#
# Example 7.8: State space feedback with integral action
#

# State space controller model: control_sf_ia()
#
# Construct a state space controller with integral action, linearized around
# an equilibrium point.  The controller is constructed around the equilibrium
# point (x_d, u_d) and includes both feedforward and feedback compensation.
#
# Controller inputs: (x, y, r)    system states, system output, reference
# Controller state:  z            integrated error (y - r)
# Controller output: u            state feedback control
#
# Note: to make the structure of the controller more clear, we implement this
# as a "nonlinear" input/output module, even though the actual input/output
# system is linear.  This also allows the use of parameters to set the
# operating point and gains for the controller.

def sf_update(t, z, u, params={}):
    y, r = u[1], u[2]
    return y - r

def sf_output(t, z, u, params={}):
    # Get the controller parameters that we need
    K = params.get('K', 0)
    ki = params.get('ki', 0)
    kf = params.get('kf', 0)
    xd = params.get('xd', 0)
    yd = params.get('yd', 0)
    ud = params.get('ud', 0)

    # Get the system state and reference input
    x, y, r = u[0], u[1], u[2]

    return ud - K * (x - xd) - ki * z + kf * (r - yd)

# Create the input/output system for the controller
control_sf = ct.NonlinearIOSystem(
    sf_update, sf_output, name='control',
    inputs=('x', 'y', 'r'),
    outputs=('u'),
    states=('z'))

# Create the closed loop system for the state space controller
cruise_sf = ct.InterconnectedSystem(
    (vehicle, control_sf), name='cruise',
    connections=(
        ('vehicle.u', 'control.u'),
        ('control.x', 'vehicle.v'),
        ('control.y', 'vehicle.v')),
    inplist=('control.r', 'vehicle.gear', 'vehicle.theta'),
    outlist=('control.u', 'vehicle.v'), outputs=['u', 'v'])

# Compute the linearization of the dynamics around the equilibrium point

# Y0 represents the steady state with PI control => we can use it to
# identify the steady state velocity and required throttle setting.
xd = Y0[1]
ud = Y0[0]
yd = Y0[1]

# Compute the linearized system at the eq pt
cruise_linearized = ct.linearize(vehicle, xd, [ud, gear[0], 0])

# Construct the gain matrices for the system
A, B, C = cruise_linearized.A, cruise_linearized.B[0, 0], cruise_linearized.C
K = 0.5
kf = -1 / (C * np.linalg.inv(A - B * K) * B)

# Response of the system with no integral feedback term
plt.figure()
plt.suptitle('Cruise control with proportional and PI control')
theta_hill = [
    0 if t <= 8 else
    4./180. * pi * (t-8) if t <= 9 else
    4./180. * pi for t in T]
t, y = ct.input_output_response(
    cruise_sf, T, [vref, gear, theta_hill], [X0[0], 0],
    params={'K':K, 'kf':kf, 'ki':0.0, 'kf':kf, 'xd':xd, 'ud':ud, 'yd':yd})
subplots = cruise_plot(cruise_sf, t, y, t_hill=8, linetype='b--')

# Response of the system with state feedback + integral action
t, y = ct.input_output_response(
    cruise_sf, T, [vref, gear, theta_hill], [X0[0], 0],
    params={'K':K, 'kf':kf, 'ki':0.1, 'kf':kf, 'xd':xd, 'ud':ud, 'yd':yd})
cruise_plot(cruise_sf, t, y, t_hill=8, linetype='b-', subplots=subplots)

# Add a legend
plt.legend(['Proportional', 'PI control'], frameon=False)

# Example 11.5: simulate the effect of a (steeper) hill at t = 5 seconds
#
# The windup effect occurs when a car encounters a hill that is so steep (6
# deg) that the throttle saturates when the cruise controller attempts to
# maintain speed.

plt.figure()
plt.suptitle('Cruise control with integrator windup')
T = np.linspace(0, 70, 101)
vref = 20 * np.ones(T.shape)
theta_hill = [
    0 if t <= 5 else
    6./180. * pi * (t-5) if t <= 6 else
    6./180. * pi for t in T]
t, y = ct.input_output_response(
    cruise_pi, T, [vref, gear, theta_hill], X0,
    params={'kaw':0})
cruise_plot(cruise_pi, t, y, antiwindup=True)

# Example 11.6: add anti-windup compensation
#
# Anti-windup can be applied to the system to improve the response. Because of
# the feedback from the actuator model, the output of the integrator is
# quickly reset to a value such that the controller output is at the
# saturation limit.

plt.figure()
plt.suptitle('Cruise control with integrator anti-windup protection')
t, y = ct.input_output_response(
    cruise_pi, T, [vref, gear, theta_hill], X0,
    params={'kaw':2.})
cruise_plot(cruise_pi, t, y, antiwindup=True)

# If running as a standalone program, show plots and wait before closing
import os
if __name__ == '__main__' and 'PYCONTROL_TEST_EXAMPLES' not in os.environ:
    plt.show()
else:
    plt.show(block=False)
Notes

1. The environment variable PYCONTROL_TEST_EXAMPLES is used for testing to turn off plotting of the outputs.

Gain scheduled control for vehicle steeering (I/O system)

Code
  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
# steering-gainsched.py - gain scheduled control for vehicle steering
# RMM, 8 May 2019
#
# This file works through Example 1.1 in the "Optimization-Based Control"
# course notes by Richard Murray (avaliable at http://fbsbook.org, in the
# optimization-based control supplement).  It is intended to demonstrate the
# functionality for nonlinear input/output systems in the python-control
# package.

import numpy as np
import control as ct
from cmath import sqrt
import matplotlib.pyplot as mpl

#
# Vehicle steering dynamics
#
# The vehicle dynamics are given by a simple bicycle model.  We take the state
# of the system as (x, y, theta) where (x, y) is the position of the vehicle
# in the plane and theta is the angle of the vehicle with respect to
# horizontal.  The vehicle input is given by (v, phi) where v is the forward
# velocity of the vehicle and phi is the angle of the steering wheel.  The
# model includes saturation of the vehicle steering angle.
#
# System state: x, y, theta
# System input: v, phi
# System output: x, y
# System parameters: wheelbase, maxsteer
#
def vehicle_update(t, x, u, params):
    # Get the parameters for the model
    l = params.get('wheelbase', 3.)         # vehicle wheelbase
    phimax = params.get('maxsteer', 0.5)    # max steering angle (rad)

    # Saturate the steering input
    phi = np.clip(u[1], -phimax, phimax)

    # Return the derivative of the state
    return np.array([
        np.cos(x[2]) * u[0],            # xdot = cos(theta) v
        np.sin(x[2]) * u[0],            # ydot = sin(theta) v
        (u[0] / l) * np.tan(phi)        # thdot = v/l tan(phi)
    ])

def vehicle_output(t, x, u, params):
    return x                            # return x, y, theta (full state)

# Define the vehicle steering dynamics as an input/output system
vehicle = ct.NonlinearIOSystem(
    vehicle_update, vehicle_output, states=3, name='vehicle',
    inputs=('v', 'phi'),
    outputs=('x', 'y', 'theta'))

#
# Gain scheduled controller
#
# For this system we use a simple schedule on the forward vehicle velocity and
# place the poles of the system at fixed values.  The controller takes the
# current vehicle position and orientation plus the velocity velocity as
# inputs, and returns the velocity and steering commands.
#
# System state: none
# System input: ex, ey, etheta, vd, phid
# System output: v, phi
# System parameters: longpole, latpole1, latpole2
#
def control_output(t, x, u, params):
    # Get the controller parameters
    longpole = params.get('longpole', -2.)
    latpole1 = params.get('latpole1', -1/2 + sqrt(-7)/2)
    latpole2 = params.get('latpole2', -1/2 - sqrt(-7)/2)
    l = params.get('wheelbase', 3)
    
    # Extract the system inputs
    ex, ey, etheta, vd, phid = u

    # Determine the controller gains
    alpha1 = -np.real(latpole1 + latpole2)
    alpha2 = np.real(latpole1 * latpole2)

    # Compute and return the control law
    v = -longpole * ex          # Note: no feedfwd (to make plot interesting)
    if vd != 0:
        phi = phid + (alpha1 * l) / vd * ey + (alpha2 * l) / vd * etheta
    else:
        # We aren't moving, so don't turn the steering wheel
        phi = phid
    
    return  np.array([v, phi])

# Define the controller as an input/output system
controller = ct.NonlinearIOSystem(
    None, control_output, name='controller',        # static system
    inputs=('ex', 'ey', 'etheta', 'vd', 'phid'),    # system inputs
    outputs=('v', 'phi')                            # system outputs
)

#
# Reference trajectory subsystem
#
# The reference trajectory block generates a simple trajectory for the system
# given the desired speed (vref) and lateral position (yref).  The trajectory
# consists of a straight line of the form (vref * t, yref, 0) with nominal
# input (vref, 0).
#
# System state: none
# System input: vref, yref
# System output: xd, yd, thetad, vd, phid
# System parameters: none
#
def trajgen_output(t, x, u, params):
    vref, yref = u
    return np.array([vref * t, yref, 0, vref, 0])

# Define the trajectory generator as an input/output system
trajgen = ct.NonlinearIOSystem(
    None, trajgen_output, name='trajgen',
    inputs=('vref', 'yref'),
    outputs=('xd', 'yd', 'thetad', 'vd', 'phid'))

#
# System construction
#
# The input to the full closed loop system is the desired lateral position and
# the desired forward velocity.  The output for the system is taken as the
# full vehicle state plus the velocity of the vehicle.  The following diagram
# summarizes the interconnections:
#
#                        +---------+       +---------------> v
#                        |         |       |
# [ yref ]               |         v       |
# [      ] ---> trajgen -+-+-> controller -+-> vehicle -+-> [x, y, theta]
# [ vref ]                 ^                            |
#                          |                            |
#                          +----------- [-1] -----------+
#
# We construct the system using the InterconnectedSystem constructor and using
# signal labels to keep track of everything.  

steering = ct.InterconnectedSystem(
    # List of subsystems
    (trajgen, controller, vehicle), name='steering',

    # Interconnections between  subsystems
    connections=(
        ('controller.ex', 'trajgen.xd', '-vehicle.x'),
        ('controller.ey', 'trajgen.yd', '-vehicle.y'),
        ('controller.etheta', 'trajgen.thetad', '-vehicle.theta'),
        ('controller.vd', 'trajgen.vd'),
        ('controller.phid', 'trajgen.phid'),
        ('vehicle.v', 'controller.v'),
        ('vehicle.phi', 'controller.phi')
    ),

    # System inputs
    inplist=['trajgen.vref', 'trajgen.yref'],
    inputs=['yref', 'vref'],

    #  System outputs
    outlist=['vehicle.x', 'vehicle.y', 'vehicle.theta', 'controller.v',
             'controller.phi'],
    outputs=['x', 'y', 'theta', 'v', 'phi']
)

# Set up the simulation conditions
yref = 1
T = np.linspace(0, 5, 100)

# Set up a figure for plotting the results
mpl.figure();

# Plot the reference trajectory for the y position
mpl.plot([0, 5], [yref, yref], 'k--')

# Find the signals we want to plot
y_index = steering.find_output('y')
v_index = steering.find_output('v')

# Do an iteration through different speeds
for vref in [8, 10, 12]:
    # Simulate the closed loop controller response
    tout, yout = ct.input_output_response(
        steering, T, [vref * np.ones(len(T)), yref * np.ones(len(T))])

    # Plot the reference speed
    mpl.plot([0, 5], [vref, vref], 'k--')

    # Plot the system output
    y_line, = mpl.plot(tout, yout[y_index, :], 'r')  # lateral position
    v_line, = mpl.plot(tout, yout[v_index, :], 'b')  # vehicle velocity

# Add axis labels
mpl.xlabel('Time (s)')
mpl.ylabel('x vel (m/s), y pos (m)')
mpl.legend((v_line, y_line), ('v', 'y'), loc='center right', frameon=False)
Notes

Differentially flat system - kinematic car

This example demonstrates the use of the flatsys module for generating trajectories for differentially flat systems. The example is drawn from Chapter 8 of FBS2e.

Code
  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
# kincar-flatsys.py - differentially flat systems example
# RMM, 3 Jul 2019
#
# This example demonstrates the use of the `flatsys` module for generating
# trajectories for differnetially flat systems by computing a trajectory for a
# kinematic (bicycle) model of a car changing lanes.

import os
import numpy as np
import matplotlib.pyplot as plt
import control as ct
import control.flatsys as fs


# Function to take states, inputs and return the flat flag
def vehicle_flat_forward(x, u, params={}):
    # Get the parameter values
    b = params.get('wheelbase', 3.)

    # Create a list of arrays to store the flat output and its derivatives
    zflag = [np.zeros(3), np.zeros(3)]

    # Flat output is the x, y position of the rear wheels
    zflag[0][0] = x[0]
    zflag[1][0] = x[1]

    # First derivatives of the flat output
    zflag[0][1] = u[0] * np.cos(x[2])  # dx/dt
    zflag[1][1] = u[0] * np.sin(x[2])  # dy/dt

    # First derivative of the angle
    thdot = (u[0]/b) * np.tan(u[1])

    # Second derivatives of the flat output (setting vdot = 0)
    zflag[0][2] = -u[0] * thdot * np.sin(x[2])
    zflag[1][2] =  u[0] * thdot * np.cos(x[2])

    return zflag


# Function to take the flat flag and return states, inputs
def vehicle_flat_reverse(zflag, params={}):
    # Get the parameter values
    b = params.get('wheelbase', 3.)

    # Create a vector to store the state and inputs
    x = np.zeros(3)
    u = np.zeros(2)

    # Given the flat variables, solve for the state
    x[0] = zflag[0][0]  # x position
    x[1] = zflag[1][0]  # y position
    x[2] = np.arctan2(zflag[1][1], zflag[0][1])  # tan(theta) = ydot/xdot

    # And next solve for the inputs
    u[0] = zflag[0][1] * np.cos(x[2]) + zflag[1][1] * np.sin(x[2])
    thdot_v = zflag[1][2] * np.cos(x[2]) - zflag[0][2] * np.sin(x[2])
    u[1] = np.arctan2(thdot_v, u[0]**2 / b)

    return x, u


# Function to compute the RHS of the system dynamics
def vehicle_update(t, x, u, params):
    b = params.get('wheelbase', 3.)             # get parameter values
    dx = np.array([
        np.cos(x[2]) * u[0],
        np.sin(x[2]) * u[0],
        (u[0]/b) * np.tan(u[1])
    ])
    return dx


# Create differentially flat input/output system
vehicle_flat = fs.FlatSystem(
    vehicle_flat_forward, vehicle_flat_reverse, vehicle_update,
    inputs=('v', 'delta'), outputs=('x', 'y', 'theta'),
    states=('x', 'y', 'theta'))

# Define the endpoints of the trajectory
x0 = [0., -2., 0.]; u0 = [10., 0.]
xf = [40., 2., 0.]; uf = [10., 0.]
Tf = 4

# Define a set of basis functions to use for the trajectories
poly = fs.PolyFamily(6)

# Find a trajectory between the initial condition and the final condition
traj = fs.point_to_point(vehicle_flat, x0, u0, xf, uf, Tf, basis=poly)

# Create the desired trajectory between the initial and final condition
T = np.linspace(0, Tf, 500)
xd, ud = traj.eval(T)

# Simulation the open system dynamics with the full input
t, y, x = ct.input_output_response(
    vehicle_flat, T, ud, x0, return_x=True)

# Plot the open loop system dynamics
plt.figure()
plt.suptitle("Open loop trajectory for kinematic car lane change")

# Plot the trajectory in xy coordinates
plt.subplot(4, 1, 2)
plt.plot(x[0], x[1])
plt.xlabel('x [m]')
plt.ylabel('y [m]')
plt.axis([x0[0], xf[0], x0[1]-1, xf[1]+1])

# Time traces of the state and input
plt.subplot(2, 4, 5)
plt.plot(t, x[1])
plt.ylabel('y [m]')

plt.subplot(2, 4, 6)
plt.plot(t, x[2])
plt.ylabel('theta [rad]')

plt.subplot(2, 4, 7)
plt.plot(t, ud[0])
plt.xlabel('Time t [sec]')
plt.ylabel('v [m/s]')
plt.axis([0, Tf, u0[0] - 1, uf[0] + 1])

plt.subplot(2, 4, 8)
plt.plot(t, ud[1])
plt.xlabel('Ttime t [sec]')
plt.ylabel('$\delta$ [rad]')
plt.tight_layout()

# Show the results unless we are running in batch mode
if 'PYCONTROL_TEST_EXAMPLES' not in os.environ:
    plt.show()
Notes

1. The environment variable PYCONTROL_TEST_EXAMPLES is used for testing to turn off plotting of the outputs.

Jupyter notebooks

The examples below use python-control in a Jupyter notebook environment. These notebooks demonstrate the use of modeling, anaylsis, and design tools using running examples in FBS2e.

Cruise control

Richard M. Murray and Karl J. Åström
17 Jun 2019

The cruise control system of a car is a common feedback system encountered in everyday life. The system attempts to maintain a constant velocity in the presence of disturbances primarily caused by changes in the slope of a road. The controller compensates for these unknowns by measuring the speed of the car and adjusting the throttle appropriately.

This notebook explores the dynamics and control of the cruise control system, following the material presenting in Feedback Systems by Astrom and Murray. A nonlinear model of the vehicle dynamics is used, with both state space and frequency domain control laws. The process model is presented in Section 1, and a controller based on state feedback is discussed in Section 2, where we also add integral action to the controller. In Section 3 we explore the behavior with PI control including the effect of actuator saturation and how it is avoided by windup protection. Different methods of constructing control systems are shown, all using the InputOutputSystem class (and subclasses).

[1]:

import numpy as np
import matplotlib.pyplot as plt
from math import pi
import control as ct
Process Model
Vehicle Dynamics

To develop a mathematical model we start with a force balance for the car body. Let v be the speed of the car, m the total mass (including passengers), F the force generated by the contact of the wheels with the road, and F_d the disturbance force due to gravity, friction, and aerodynamic drag.

[2]:

def vehicle_update(t, x, u, params={}):
    """Vehicle dynamics for cruise control system.

    Parameters
    ----------
    x : array
         System state: car velocity in m/s
    u : array
         System input: [throttle, gear, road_slope], where throttle is
         a float between 0 and 1, gear is an integer between 1 and 5,
         and road_slope is in rad.

    Returns
    -------
    float
        Vehicle acceleration

    """
    from math import copysign, sin
    sign = lambda x: copysign(1, x)         # define the sign() function

    # Set up the system parameters
    m = params.get('m', 1600.)              # vehicle mass, kg
    g = params.get('g', 9.8)                # gravitational constant, m/s^2
    Cr = params.get('Cr', 0.01)             # coefficient of rolling friction
    Cd = params.get('Cd', 0.32)             # drag coefficient
    rho = params.get('rho', 1.3)            # density of air, kg/m^3
    A = params.get('A', 2.4)                # car area, m^2
    alpha = params.get(
        'alpha', [40, 25, 16, 12, 10])      # gear ratio / wheel radius

    # Define variables for vehicle state and inputs
    v = x[0]                           # vehicle velocity
    throttle = np.clip(u[0], 0, 1)     # vehicle throttle
    gear = u[1]                        # vehicle gear
    theta = u[2]                       # road slope

    # Force generated by the engine

    omega = alpha[int(gear)-1] * v      # engine angular speed
    F = alpha[int(gear)-1] * motor_torque(omega, params) * throttle

    # Disturbance forces
    #
    # The disturbance force Fd has three major components: Fg, the forces due
    # to gravity; Fr, the forces due to rolling friction; and Fa, the
    # aerodynamic drag.

    # Letting the slope of the road be \theta (theta), gravity gives the
    # force Fg = m g sin \theta.

    Fg = m * g * sin(theta)

    # A simple model of rolling friction is Fr = m g Cr sgn(v), where Cr is
    # the coefficient of rolling friction and sgn(v) is the sign of v (±1) or
    # zero if v = 0.

    Fr  = m * g * Cr * sign(v)

    # The aerodynamic drag is proportional to the square of the speed: Fa =
    # 1/2 \rho Cd A |v| v, where \rho is the density of air, Cd is the
    # shape-dependent aerodynamic drag coefficient, and A is the frontal area
    # of the car.

    Fa = 1/2 * rho * Cd * A * abs(v) * v

    # Final acceleration on the car
    Fd = Fg + Fr + Fa
    dv = (F - Fd) / m

    return dv
Engine model

The force F is generated by the engine, whose torque is proportional to the rate of fuel injection, which is itself proportional to a control signal 0 <= u <= 1 that controls the throttle position. The torque also depends on engine speed omega.

[3]:

def motor_torque(omega, params={}):
    # Set up the system parameters
    Tm = params.get('Tm', 190.)             # engine torque constant
    omega_m = params.get('omega_m', 420.)   # peak engine angular speed
    beta = params.get('beta', 0.4)          # peak engine rolloff

    return np.clip(Tm * (1 - beta * (omega/omega_m - 1)**2), 0, None)

Torque curves for a typical car engine. The graph on the left shows the torque generated by the engine as a function of the angular velocity of the engine, while the curve on the right shows torque as a function of car speed for different gears.

[4]:

# Figure 4.2a - single torque curve as function of omega
omega_range = np.linspace(0, 700, 701)
plt.subplot(2, 2, 1)
plt.plot(omega_range, [motor_torque(w) for w in omega_range])
plt.xlabel('Angular velocity $\omega$ [rad/s]')
plt.ylabel('Torque $T$ [Nm]')
plt.grid(True, linestyle='dotted')

# Figure 4.2b - torque curves in different gears, as function of velocity
plt.subplot(2, 2, 2)
v_range = np.linspace(0, 70, 71)
alpha = [40, 25, 16, 12, 10]
for gear in range(5):
    omega_range = alpha[gear] * v_range
    plt.plot(v_range, [motor_torque(w) for w in omega_range],
             color='blue', linestyle='solid')

# Set up the axes and style
plt.axis([0, 70, 100, 200])
plt.grid(True, linestyle='dotted')

# Add labels
plt.text(11.5, 120, '$n$=1')
plt.text(24, 120, '$n$=2')
plt.text(42.5, 120, '$n$=3')
plt.text(58.5, 120, '$n$=4')
plt.text(58.5, 185, '$n$=5')
plt.xlabel('Velocity $v$ [m/s]')
plt.ylabel('Torque $T$ [Nm]')

plt.tight_layout()
plt.suptitle('Torque curves for typical car engine');
_images/cruise_7_0.png
Input/ouput model for the vehicle system

We now create an input/output model for the vehicle system that takes the throttle input u, the gear and the angle of the road \theta as input. The output of this model is the current vehicle velocity v.

[5]:

vehicle = ct.NonlinearIOSystem(
    vehicle_update, None, name='vehicle',
    inputs = ('u', 'gear', 'theta'), outputs = ('v'), states=('v'))

# Define a generator for creating a "standard" cruise control plot
def cruise_plot(sys, t, y, t_hill=5, vref=20, antiwindup=False, linetype='b-',
               subplots=[None, None]):
    # Figure out the plot bounds and indices
    v_min = vref-1.2; v_max = vref+0.5; v_ind = sys.find_output('v')
    u_min = 0; u_max = 2 if antiwindup else 1; u_ind = sys.find_output('u')

    # Make sure the upper and lower bounds on v are OK
    while max(y[v_ind]) > v_max: v_max += 1
    while min(y[v_ind]) < v_min: v_min -= 1

    # Create arrays for return values
    subplot_axes = subplots.copy()

    # Velocity profile
    if subplot_axes[0] is None:
        subplot_axes[0] = plt.subplot(2, 1, 1)
    else:
        plt.sca(subplots[0])
    plt.plot(t, y[v_ind], linetype)
    plt.plot(t, vref*np.ones(t.shape), 'k-')
    plt.plot([t_hill, t_hill], [v_min, v_max], 'k--')
    plt.axis([0, t[-1], v_min, v_max])
    plt.xlabel('Time $t$ [s]')
    plt.ylabel('Velocity $v$ [m/s]')

    # Commanded input profile
    if subplot_axes[1] is None:
        subplot_axes[1] = plt.subplot(2, 1, 2)
    else:
        plt.sca(subplots[1])
    plt.plot(t, y[u_ind], 'r--' if antiwindup else linetype)
    plt.plot([t_hill, t_hill], [u_min, u_max], 'k--')
    plt.axis([0, t[-1], u_min, u_max])
    plt.xlabel('Time $t$ [s]')
    plt.ylabel('Throttle $u$')

    # Applied input profile
    if antiwindup:
        plt.plot(t, np.clip(y[u_ind], 0, 1), linetype)
        plt.legend(['Commanded', 'Applied'], frameon=False)

    return subplot_axes
State space controller

Construct a state space controller with integral action, linearized around an equilibrium point. The controller is constructed around the equilibrium point (x_d, u_d) and includes both feedforward and feedback compensation.

  • Controller inputs - (x, y, r): system states, system output, reference

  • Controller state - z: integrated error (y - r)

  • Controller output - u: state feedback control

Note: to make the structure of the controller more clear, we implement this as a “nonlinear” input/output module, even though the actual input/output system is linear. This also allows the use of parameters to set the operating point and gains for the controller.

[6]:

def sf_update(t, z, u, params={}):
    y, r = u[1], u[2]
    return y - r

def sf_output(t, z, u, params={}):
    # Get the controller parameters that we need
    K = params.get('K', 0)
    ki = params.get('ki', 0)
    kf = params.get('kf', 0)
    xd = params.get('xd', 0)
    yd = params.get('yd', 0)
    ud = params.get('ud', 0)

    # Get the system state and reference input
    x, y, r = u[0], u[1], u[2]

    return ud - K * (x - xd) - ki * z + kf * (r - yd)

# Create the input/output system for the controller
control_sf = ct.NonlinearIOSystem(
    sf_update, sf_output, name='control',
    inputs=('x', 'y', 'r'),
    outputs=('u'),
    states=('z'))

# Create the closed loop system for the state space controller
cruise_sf = ct.InterconnectedSystem(
    (vehicle, control_sf), name='cruise',
    connections=(
        ('vehicle.u', 'control.u'),
        ('control.x', 'vehicle.v'),
        ('control.y', 'vehicle.v')),
    inplist=('control.r', 'vehicle.gear', 'vehicle.theta'),
    outlist=('control.u', 'vehicle.v'), outputs=['u', 'v'])

# Define the time and input vectors
T = np.linspace(0, 25, 501)
vref = 20 * np.ones(T.shape)
gear = 4 * np.ones(T.shape)
theta0 = np.zeros(T.shape)

# Find the equilibrium point for the system
Xeq, Ueq = ct.find_eqpt(
    vehicle, [vref[0]], [0, gear[0], theta0[0]], y0=[vref[0]], iu=[1, 2])
print("Xeq = ", Xeq)
print("Ueq = ", Ueq)

# Compute the linearized system at the eq pt
cruise_linearized = ct.linearize(vehicle, Xeq, [Ueq[0], gear[0], 0])

Xeq =  [20.]
Ueq =  [0.16874874 4.         0.        ]

[7]:

# Construct the gain matrices for the system
A, B, C = cruise_linearized.A, cruise_linearized.B[0, 0], cruise_linearized.C
K = 0.5
kf = -1 / (C * np.linalg.inv(A - B * K) * B)

# Compute the steady state velocity and throttle setting
xd = Xeq[0]
ud = Ueq[0]
yd = vref[-1]

# Response of the system with no integral feedback term
plt.figure()
theta_hill = [
    0 if t <= 5 else
    4./180. * pi * (t-5) if t <= 6 else
    4./180. * pi for t in T]
t, y_sfb = ct.input_output_response(
    cruise_sf, T, [vref, gear, theta_hill], [Xeq[0], 0],
    params={'K':K, 'ki':0.0, 'kf':kf, 'xd':xd, 'ud':ud, 'yd':yd})
subplots = cruise_plot(cruise_sf, t, y_sfb, t_hill=5, linetype='b--')

# Response of the system with state feedback + integral action
t, y_sfb_int = ct.input_output_response(
    cruise_sf, T, [vref, gear, theta_hill], [Xeq[0], 0],
    params={'K':K, 'ki':0.1, 'kf':kf, 'xd':xd, 'ud':ud, 'yd':yd})
cruise_plot(cruise_sf, t, y_sfb_int, t_hill=5, linetype='b-', subplots=subplots)

# Add title and legend
plt.suptitle('Cruise control with state feedback, integral action')
import matplotlib.lines as mlines
p_line = mlines.Line2D([], [], color='blue', linestyle='--', label='State feedback')
pi_line = mlines.Line2D([], [], color='blue', linestyle='-', label='w/ integral action')
plt.legend(handles=[p_line, pi_line], frameon=False, loc='lower right');
_images/cruise_12_0.png
Pole/zero cancellation

The transfer function for the linearized dynamics of the cruise control system is given by P(s) = b/(s+a). A simple (but not necessarily good) way to design a PI controller is to choose the parameters of the PI controller as k_\text{i}=ak_\text{p}. The controller transfer function is then C(s)=k_\text{p}+k_\text{i}/s=k_\text{i}(s+a)/s. It has a zero at s = -k_\text{i}/k_\text{p}=-a that cancels the process pole at s = -a. We have P(s)C(s)=k_\text{i}/s giving the transfer function from reference to vehicle velocity as G_{yr}(s)=b k_\text{p}/(s + b k_\text{p}), and control design is then simply a matter of choosing the gain k_\text{p}. The closed loop system dynamics are of first order with the time constant 1/(b k_\text{p}).

[8]:

# Get the transfer function from throttle input + hill to vehicle speed
P = ct.ss2tf(cruise_linearized[0, 0])

# Construction a controller that cancels the pole
kp = 0.5
a = -P.pole()[0]
b = np.real(P(0)) * a
ki = a * kp
C = ct.tf2ss(ct.TransferFunction([kp, ki], [1, 0]))
control_pz = ct.LinearIOSystem(C, name='control', inputs='u', outputs='y')
print("system: a = ", a, ", b = ", b)
print("pzcancel: kp =", kp, ", ki =", ki, ", 1/(kp b) = ", 1/(kp * b))
print("sfb_int: K = ", K, ", ki = 0.1")

# Construct the closed loop system and plot the response
# Create the closed loop system for the state space controller
cruise_pz = ct.InterconnectedSystem(
    (vehicle, control_pz), name='cruise_pz',
    connections = (
        ('control.u', '-vehicle.v'),
        ('vehicle.u', 'control.y')),
    inplist = ('control.u', 'vehicle.gear', 'vehicle.theta'),
    inputs = ('vref', 'gear', 'theta'),
    outlist = ('vehicle.v', 'vehicle.u'),
    outputs = ('v', 'u'))

# Find the equilibrium point
X0, U0 = ct.find_eqpt(
    cruise_pz, [vref[0], 0], [vref[0], gear[0], theta0[0]],
    iu=[1, 2], y0=[vref[0], 0], iy=[0])

# Response of the system with PI controller canceling process pole
t, y_pzcancel = ct.input_output_response(
    cruise_pz, T, [vref, gear, theta_hill], X0)
subplots = cruise_plot(cruise_pz, t, y_pzcancel, t_hill=5, linetype='b-')
cruise_plot(cruise_sf, t, y_sfb_int, t_hill=5, linetype='b--', subplots=subplots);

system: a =  0.010124405669387215 , b =  1.3203061238159202
pzcancel: kp = 0.5 , ki = 0.005062202834693608 , 1/(kp b) =  1.5148002148317266
sfb_int: K =  0.5 , ki = 0.1
_images/cruise_14_1.png
PI Controller

In this example, the speed of the vehicle is measured and compared to the desired speed. The controller is a PI controller represented as a transfer function. In the textbook, the simulations are done for LTI systems, but here we simulate the full nonlinear system.

Parameter design through pole placement

To illustrate the design of a PI controller, we choose the gains k_\text{p} and k_\text{i} so that the characteristic polynomial has the form

s^2 + 2 \zeta \omega_0 s + \omega_0^2

[9]:

# Values of the first order transfer function P(s) = b/(s + a) are set above

# Define the input that we want to track
T = np.linspace(0, 40, 101)
vref = 20 * np.ones(T.shape)
gear = 4 * np.ones(T.shape)
theta_hill = np.array([
    0 if t <= 5 else
    4./180. * pi * (t-5) if t <= 6 else
    4./180. * pi for t in T])

# Fix \omega_0 and vary \zeta
w0 = 0.5
subplots = [None, None]
for zeta in [0.5, 1, 2]:
    # Create the controller transfer function (as an I/O system)
    kp = (2*zeta*w0 - a)/b
    ki = w0**2 / b
    control_tf = ct.tf2io(
        ct.TransferFunction([kp, ki], [1, 0.01*ki/kp]),
        name='control', inputs='u', outputs='y')

    # Construct the closed loop system by interconnecting process and controller
    cruise_tf = ct.InterconnectedSystem(
    (vehicle, control_tf), name='cruise',
    connections = [('control.u', '-vehicle.v'), ('vehicle.u', 'control.y')],
    inplist = ('control.u', 'vehicle.gear', 'vehicle.theta'),
        inputs = ('vref', 'gear', 'theta'),
    outlist = ('vehicle.v', 'vehicle.u'), outputs = ('v', 'u'))

    # Plot the velocity response
    X0, U0 = ct.find_eqpt(
        cruise_tf, [vref[0], 0], [vref[0], gear[0], theta_hill[0]],
        iu=[1, 2], y0=[vref[0], 0], iy=[0])

    t, y = ct.input_output_response(cruise_tf, T, [vref, gear, theta_hill], X0)
    subplots = cruise_plot(cruise_tf, t, y, t_hill=5, subplots=subplots)
_images/cruise_17_0.png 
[10]:

# Fix \zeta and vary \omega_0
zeta = 1
subplots = [None, None]
for w0 in [0.2, 0.5, 1]:
    # Create the controller transfer function (as an I/O system)
    kp = (2*zeta*w0 - a)/b
    ki = w0**2 / b
    control_tf = ct.tf2io(
        ct.TransferFunction([kp, ki], [1, 0.01*ki/kp]),
        name='control', inputs='u', outputs='y')

    # Construct the closed loop system by interconnecting process and controller
    cruise_tf = ct.InterconnectedSystem(
    (vehicle, control_tf), name='cruise',
    connections = [('control.u', '-vehicle.v'), ('vehicle.u', 'control.y')],
    inplist = ('control.u', 'vehicle.gear', 'vehicle.theta'),
        inputs = ('vref', 'gear', 'theta'),
    outlist = ('vehicle.v', 'vehicle.u'), outputs = ('v', 'u'))

    # Plot the velocity response
    X0, U0 = ct.find_eqpt(
        cruise_tf, [vref[0], 0], [vref[0], gear[0], theta_hill[0]],
        iu=[1, 2], y0=[vref[0], 0], iy=[0])

    t, y = ct.input_output_response(cruise_tf, T, [vref, gear, theta_hill], X0)
    subplots = cruise_plot(cruise_tf, t, y, t_hill=5, subplots=subplots)
_images/cruise_18_0.png
Robustness to change in mass
[11]:

# Nominal controller design for remaining analyses
# Construct a PI controller with rolloff, as a transfer function
Kp = 0.5                        # proportional gain
Ki = 0.1                        # integral gain
control_tf = ct.tf2io(
    ct.TransferFunction([Kp, Ki], [1, 0.01*Ki/Kp]),
    name='control', inputs='u', outputs='y')

cruise_tf = ct.InterconnectedSystem(
    (vehicle, control_tf), name='cruise',
    connections = [('control.u', '-vehicle.v'), ('vehicle.u', 'control.y')],
    inplist = ('control.u', 'vehicle.gear', 'vehicle.theta'), inputs = ('vref', 'gear', 'theta'),
    outlist = ('vehicle.v', 'vehicle.u'), outputs = ('v', 'u'))

[12]:

# Define the time and input vectors
T = np.linspace(0, 25, 101)
vref = 20 * np.ones(T.shape)
gear = 4 * np.ones(T.shape)
theta0 = np.zeros(T.shape)

# Now simulate the effect of a hill at t = 5 seconds
plt.figure()
plt.suptitle('Response to change in road slope')
theta_hill = np.array([
    0 if t <= 5 else
    4./180. * pi * (t-5) if t <= 6 else
    4./180. * pi for t in T])

subplots = [None, None]
linecolor = ['red', 'blue', 'green']
handles = []
for i, m in enumerate([1200, 1600, 2000]):
    # Compute the equilibrium state for the system
    X0, U0 = ct.find_eqpt(
        cruise_tf, [vref[0], 0], [vref[0], gear[0], theta0[0]],
        iu=[1, 2], y0=[vref[0], 0], iy=[0], params={'m':m})

    t, y = ct.input_output_response(
        cruise_tf, T, [vref, gear, theta_hill], X0, params={'m':m})

    subplots = cruise_plot(cruise_tf, t, y, t_hill=5, subplots=subplots,
                           linetype=linecolor[i][0] + '-')
    handles.append(mlines.Line2D([], [], color=linecolor[i], linestyle='-',
                                 label="m = %d" % m))

# Add labels to the plots
plt.sca(subplots[0])
plt.ylabel('Speed [m/s]')
plt.legend(handles=handles, frameon=False, loc='lower right');

plt.sca(subplots[1])
plt.ylabel('Throttle')
plt.xlabel('Time [s]');
_images/cruise_21_0.png
PI controller with antiwindup protection

We now create a more complicated feedback controller that includes anti-windup protection.

[13]:

def pi_update(t, x, u, params={}):
    # Get the controller parameters that we need
    ki = params.get('ki', 0.1)
    kaw = params.get('kaw', 2)  # anti-windup gain

    # Assign variables for inputs and states (for readability)
    v = u[0]                    # current velocity
    vref = u[1]                 # reference velocity
    z = x[0]                    # integrated error

    # Compute the nominal controller output (needed for anti-windup)
    u_a = pi_output(t, x, u, params)

    # Compute anti-windup compensation (scale by ki to account for structure)
    u_aw = kaw/ki * (np.clip(u_a, 0, 1) - u_a) if ki != 0 else 0

    # State is the integrated error, minus anti-windup compensation
    return (vref - v) + u_aw

def pi_output(t, x, u, params={}):
    # Get the controller parameters that we need
    kp = params.get('kp', 0.5)
    ki = params.get('ki', 0.1)

    # Assign variables for inputs and states (for readability)
    v = u[0]                    # current velocity
    vref = u[1]                 # reference velocity
    z = x[0]                    # integrated error

    # PI controller
    return kp * (vref - v) + ki * z

control_pi = ct.NonlinearIOSystem(
    pi_update, pi_output, name='control',
    inputs = ['v', 'vref'], outputs = ['u'], states = ['z'],
    params = {'kp':0.5, 'ki':0.1})

# Create the closed loop system
cruise_pi = ct.InterconnectedSystem(
    (vehicle, control_pi), name='cruise',
    connections=(
        ('vehicle.u', 'control.u'),
        ('control.v', 'vehicle.v')),
    inplist=('control.vref', 'vehicle.gear', 'vehicle.theta'),
    outlist=('control.u', 'vehicle.v'), outputs=['u', 'v'])
Response to a small hill

Figure 4.3b shows the response of the closed loop system. The figure shows that even if the hill is so steep that the throttle changes from 0.17 to almost full throttle, the largest speed error is less than 1 m/s, and the desired velocity is recovered after 20 s.

[14]:

# Compute the equilibrium throttle setting for the desired speed
X0, U0, Y0 = ct.find_eqpt(
    cruise_pi, [vref[0], 0], [vref[0], gear[0], theta0[0]],
    y0=[0, vref[0]], iu=[1, 2], iy=[1], return_y=True)

# Now simulate the effect of a hill at t = 5 seconds
plt.figure()
plt.suptitle('Car with cruise control encountering sloping road')
theta_hill = [
    0 if t <= 5 else
    4./180. * pi * (t-5) if t <= 6 else
    4./180. * pi for t in T]
t, y = ct.input_output_response(
    cruise_pi, T, [vref, gear, theta_hill], X0)
cruise_plot(cruise_pi, t, y);
_images/cruise_25_0.png
Effect of Windup

The windup effect occurs when a car encounters a hill that is so steep (6^\circ) that the throttle saturates when the cruise controller attempts to maintain speed.

[15]:

plt.figure()
plt.suptitle('Cruise control with integrator windup')
T = np.linspace(0, 50, 101)
vref = 20 * np.ones(T.shape)
theta_hill = [
    0 if t <= 5 else
    6./180. * pi * (t-5) if t <= 6 else
    6./180. * pi for t in T]
t, y = ct.input_output_response(
    cruise_pi, T, [vref, gear, theta_hill], X0,
    params={'kaw':0})
cruise_plot(cruise_pi, t, y, antiwindup=True);
_images/cruise_27_0.png
PI controller with anti-windup compensation

Anti-windup can be applied to the system to improve the response. Because of the feedback from the actuator model, the output of the integrator is quickly reset to a value such that the controller output is at the saturation limit.

[16]:

plt.figure()
plt.suptitle('Cruise control with integrator anti-windup protection')
t, y = ct.input_output_response(
    cruise_pi, T, [vref, gear, theta_hill], X0,
    params={'kaw':2.})
cruise_plot(cruise_pi, t, y, antiwindup=True);
_images/cruise_29_0.png 
[ ]:

Vehicle steering

Karl J. Astrom and Richard M. Murray
23 Jul 2019

This notebook contains the computations for the vehicle steering running example in Feedback Systems.

RMM comments to Karl, 27 Jun 2019 * I’m using this notebook to walk through all of the vehicle steering examples and make sure that all of the parameters, conditions, and maximum steering angles are consitent and reasonable. * Please feel free to send me comments on the contents as well as the bulletted notes, in whatever form is most convenient. * Once we have sorted out all of the settings we want to use, I’ll copy over the changes into the MATLAB files that we use for creating the figures in the book. * These notes will be removed from the notebook once we have finalized everything.

[1]:

import numpy as np
import matplotlib.pyplot as plt
import control as ct
ct.use_fbs_defaults()
ct.use_numpy_matrix(False)
Vehicle steering dynamics (Example 3.11)

The vehicle dynamics are given by a simple bicycle model. We take the state of the system as (x, y, \theta) where (x, y) is the position of the reference point of the vehicle in the plane and \theta is the angle of the vehicle with respect to horizontal. The vehicle input is given by (v, \delta) where v is the forward velocity of the vehicle and \delta is the angle of the steering wheel. We take as parameters the wheelbase b and the offset a between the rear wheels and the reference point. The model includes saturation of the vehicle steering angle (maxsteer).

  • System state: x, y, theta

  • System input: v, delta

  • System output: x, y

  • System parameters: wheelbase, refoffset, maxsteer

Assuming no slipping of the wheels, the motion of the vehicle is given by a rotation around a point O that depends on the steering angle \delta. To compute the angle \alpha of the velocity of the reference point with respect to the axis of the vehicle, we let the distance from the center of rotation O to the contact point of the rear wheel be r_\text{r} and it the follows from Figure 3.17 in FBS that b = r_\text{r} \tan \delta and a = r_\text{r} \tan \alpha, which implies that \tan \alpha = (a/b) \tan \delta.

Reasonable limits for the steering angle depend on the speed. The physical limit is given in our model as 0.5 radians (about 30 degrees). However, this limit is rarely possible when the car is driving since it would cause the tires to slide on the pavement. We us a limit of 0.1 radians (about 6 degrees) at 10 m/s (\approx 35 kph) and 0.05 radians (about 3 degrees) at 30 m/s (\approx 110 kph). Note that a steering angle of 0.05 rad gives a cross acceleration of (v^2/b) \tan \delta \approx (100/3) 0.05 = 1.7 \text{m/s}^2 at 10 m/s and 15 \text{m/s}^2 at 30 m/s (\approx 1.5 times the force of gravity).

[2]:

def vehicle_update(t, x, u, params):
    # Get the parameters for the model
    a = params.get('refoffset', 1.5)        # offset to vehicle reference point
    b = params.get('wheelbase', 3.)         # vehicle wheelbase
    maxsteer = params.get('maxsteer', 0.5)  # max steering angle (rad)

    # Saturate the steering input
    delta = np.clip(u[1], -maxsteer, maxsteer)
    alpha = np.arctan2(a * np.tan(delta), b)

    # Return the derivative of the state
    return np.array([
        u[0] * np.cos(x[2] + alpha),    # xdot = cos(theta + alpha) v
        u[0] * np.sin(x[2] + alpha),    # ydot = sin(theta + alpha) v
        (u[0] / b) * np.tan(delta)      # thdot = v/l tan(phi)
    ])

def vehicle_output(t, x, u, params):
    return x[0:2]

# Default vehicle parameters (including nominal velocity)
vehicle_params={'refoffset': 1.5, 'wheelbase': 3, 'velocity': 15,
                'maxsteer': 0.5}

# Define the vehicle steering dynamics as an input/output system
vehicle = ct.NonlinearIOSystem(
    vehicle_update, vehicle_output, states=3, name='vehicle',
    inputs=('v', 'delta'), outputs=('x', 'y'), params=vehicle_params)
Vehicle driving on a curvy road (Figure 8.6a)

To illustrate the dynamics of the system, we create an input that correspond to driving down a curvy road. This trajectory will be used in future simulations as a reference trajectory for estimation and control.

RMM notes, 27 Jun 2019: * The figure below appears in Chapter 8 (output feedback) as Example 8.3, but I’ve put it here in the notebook since it is a good way to demonstrate the dynamics of the vehicle. * In the book, this figure is created for the linear model and in a manner that I can’t quite understand, since the linear model that is used is only for the lateral dynamics. The original file is OutputFeedback/figures/steering_obs.m. * To create the figure here, I set the initial vehicle angle to be \theta(0) = 0.75 rad and then used an input that gives a figure approximating Example 8.3 To create the lateral offset, I think subtracted the trajectory from the averaged straight line trajectory, shown as a dashed line in the xy figure below. * I find the approach that we used in the MATLAB version to be confusing, but I also think the method of creating the lateral error here is a hart to follow. We might instead consider choosing a trajectory that goes mainly vertically, with the 2D dynamics being the x, \theta dynamics instead of the y, \theta dynamics.

KJA comments, 1 Jul 2019:

  1. I think we should point out that the reference point is typically the projection of the center of mass of the whole vehicle.

  2. The heading angle \theta must be marked in Figure 3.17b.

  3. I think it is useful to start with a curvy road that you have done here but then to specialized to a trajectory that is essentially horizontal, where y is the deviation from the nominal horizontal x axis. Assuming that \alpha and \theta are small we get the natural linearization of (3.26) \dot x = v and \dot y =v(\alpha + \theta)

RMM response, 16 Jul 2019: * I’ve changed the trajectory to be about the horizontal axis, but I am ploting things vertically for better figure layout. This corresponds to what is done in Example 9.10 in the text, which I think looks OK.

KJA response, 20 Jul 2019: Fig 8.6a is fine

[3]:

# System parameters
wheelbase = vehicle_params['wheelbase']
v0 = vehicle_params['velocity']

# Control inputs
T_curvy = np.linspace(0, 7, 500)
v_curvy = v0*np.ones(T_curvy.shape)
delta_curvy = 0.1*np.sin(T_curvy)*np.cos(4*T_curvy) + 0.0025*np.sin(T_curvy*np.pi/7)
u_curvy = [v_curvy, delta_curvy]
X0_curvy = [0, 0.8, 0]

# Simulate the system + estimator
t_curvy, y_curvy, x_curvy = ct.input_output_response(
    vehicle, T_curvy, u_curvy, X0_curvy, params=vehicle_params, return_x=True)

# Configure matplotlib plots to be a bit bigger and optimize layout
plt.figure(figsize=[9, 4.5])

# Plot the resulting trajectory (and some road boundaries)
plt.subplot(1, 4, 2)
plt.plot(y_curvy[1], y_curvy[0])
plt.plot(y_curvy[1] - 9/np.cos(x_curvy[2]), y_curvy[0], 'k-', linewidth=1)
plt.plot(y_curvy[1] - 3/np.cos(x_curvy[2]), y_curvy[0], 'k--', linewidth=1)
plt.plot(y_curvy[1] + 3/np.cos(x_curvy[2]), y_curvy[0], 'k-', linewidth=1)

plt.xlabel('y [m]')
plt.ylabel('x [m]');
plt.axis('Equal')

# Plot the lateral position
plt.subplot(2, 2, 2)
plt.plot(t_curvy, y_curvy[1])
plt.ylabel('Lateral position $y$ [m]')

# Plot the steering angle
plt.subplot(2, 2, 4)
plt.plot(t_curvy, delta_curvy)
plt.ylabel('Steering angle $\\delta$ [rad]')
plt.xlabel('Time t [sec]')
plt.tight_layout()
_images/steering_7_0.png
Linearization of lateral steering dynamics (Example 6.13)

We are interested in the motion of the vehicle about a straight-line path (\theta = \theta_0) with constant velocity v_0 \neq 0. To find the relevant equilibrium point, we first set \dot\theta = 0 and we see that we must have \delta = 0, corresponding to the steering wheel being straight. The motion in the xy plane is by definition not at equilibrium and so we focus on lateral deviation of the vehicle from a straight line. For simplicity, we let \theta_\text{e} = 0, which corresponds to driving along the x axis. We can then focus on the equations of motion in the y and \theta directions with input u = \delta.

[4]:

# Define the lateral dynamics as a subset of the full vehicle steering dynamics
lateral = ct.NonlinearIOSystem(
    lambda t, x, u, params: vehicle_update(
        t, [0., x[0], x[1]], [params.get('velocity', 1), u[0]], params)[1:],
    lambda t, x, u, params: vehicle_output(
        t, [0., x[0], x[1]], [params.get('velocity', 1), u[0]], params)[1:],
    states=2, name='lateral', inputs=('phi'), outputs=('y')
)

# Compute the linearization at velocity v0 = 15 m/sec
lateral_linearized = ct.linearize(lateral, [0, 0], [0], params=vehicle_params)

# Normalize dynamics using state [x1/b, x2] and timescale v0 t / b
b = vehicle_params['wheelbase']
v0 = vehicle_params['velocity']
lateral_transformed = ct.similarity_transform(
    lateral_linearized, [[1/b, 0], [0, 1]], timescale=v0/b)

# Set the output to be the normalized state x1/b
lateral_normalized = lateral_transformed * (1/b)
print("Linearized system dynamics:\n")
print(lateral_normalized)

# Save the system matrices for later use
A = lateral_normalized.A
B = lateral_normalized.B
C = lateral_normalized.C

Linearized system dynamics:

A = [[0. 1.]
 [0. 0.]]

B = [[0.5]
 [1. ]]

C = [[1. 0.]]

D = [[0.]]
Eigenvalue placement controller design (Example 7.4)

We want to design a controller that stabilizes the dynamics of the vehicle and tracks a given reference value r of the lateral position of the vehicle. We use feedback to design the dynamics of the system to have the characteristic polynomial p(s) = s^2 + 2 \zeta_\text{c} \omega_\text{c} + \omega_\text{c}^2.

To find reasonable values of \omega_\text{c} we observe that the initial response of the steering angle to a unit step change in the steering command is \omega_\text{c}^2 r, where r is the commanded lateral transition. Recall that the model is normalized so that the length unit is the wheelbase b and the time unit is the time b/v_0 to travel one wheelbase. A typical car has a wheelbase of about 3 m and, assuming a speed of 30 m/s, a normalized time unit corresponds to 0.1 s. To determine a reasonable steering angle when making a gentle lane change, we assume that the turning radius is R = 600 m. For a wheelbase of 3 m this corresponds to a steering angle \delta \approx 3/600 = 0.005 rad and a lateral acceleration of v^2/R = 302/600 = 1.5 m/s^2. Assuming that a lane change corresponds to a translation of one wheelbase we find \omega_\text{c} = \sqrt{0.005} = 0.07 rad/s.

The unit step responses for the closed loop system for different values of the design parameters are shown below. The effect of \omega_c is shown on the left, which shows that the response speed increases with increasing \omega_\text{c}. All responses have overshoot less than 5% (15 cm), as indicated by the dashed lines. The settling times range from 30 to 60 normalized time units, which corresponds to about 3–6 s, and are limited by the acceptable lateral acceleration of the vehicle. The effect of \zeta_\text{c} is shown on the right. The response speed and the overshoot increase with decreasing damping. Using these plots, we conclude that a reasonable design choice is \omega_\text{c} = 0.07 and \zeta_\text{c} = 0.7.

RMM note, 27 Jun 2019: * The design guidelines are for v_0 = 30 m/s (highway speeds) but most of the examples below are done at lower speed (typically 10 m/s). Also, the eigenvalue locations above are not the same ones that we use in the output feedback example below. We should probably make things more consistent.

KJA comment, 1 Jul 2019: * I am all for maikng it consist and choosing e.g. v0 = 30 m/s

RMM comment, 17 Jul 2019: * I’ve updated the examples below to use v0 = 30 m/s for everything except the forward/reverse example. This corresponds to ~105 kph (freeway speeds) and a reasonable bound for the steering angle to avoid slipping is 0.05 rad.

[5]:

# Utility function to place poles for the normalized vehicle steering system
def normalized_place(wc, zc):
    # Get the dynamics and input matrices, for later use
    A, B = lateral_normalized.A, lateral_normalized.B

    # Compute the eigenvalues from the characteristic polynomial
    eigs = np.roots([1, 2*zc*wc, wc**2])

    # Compute the feedback gain using eigenvalue placement
    K = ct.place_varga(A, B, eigs)

    # Create a new system representing the closed loop response
    clsys = ct.StateSpace(A - B @ K, B, lateral_normalized.C, 0)

    # Compute the feedforward gain based on the zero frequency gain of the closed loop
    kf = np.real(1/clsys.evalfr(0))

    # Scale the input by the feedforward gain
    clsys *= kf

    # Return gains and closed loop system dynamics
    return K, kf, clsys

# Utility function to plot simulation results for normalized vehicle steering system
def normalized_plot(t, y, u, inpfig, outfig):
    plt.sca(outfig)
    plt.plot(t, y)
    plt.sca(inpfig)
    plt.plot(t, u[0])

# Utility function to label plots of normalized vehicle steering system
def normalized_label(inpfig, outfig):
    plt.sca(inpfig)
    plt.xlabel('Normalized time $v_0 t / b$')
    plt.ylabel('Steering angle $\delta$ [rad]')

    plt.sca(outfig)
    plt.ylabel('Lateral position $y/b$')
    plt.plot([0, 20], [0.95, 0.95], 'k--')
    plt.plot([0, 20], [1.05, 1.05], 'k--')

# Configure matplotlib plots to be a bit bigger and optimize layout
plt.figure(figsize=[9, 4.5])

# Explore range of values for omega_c, with zeta_c = 0.7
outfig = plt.subplot(2, 2, 1)
inpfig = plt.subplot(2, 2, 3)
zc = 0.7
for wc in [0.5, 0.7, 1]:
    # Place the poles of the system
    K, kf, clsys = normalized_place(wc, zc)

    # Compute the step response
    t, y, x = ct.step_response(clsys, np.linspace(0, 20, 100), return_x=True)

    # Compute the input used to generate the control response
    u = -K @ x + kf * 1

    # Plot the results
    normalized_plot(t, y, u, inpfig, outfig)

# Add labels to the figure
normalized_label(inpfig, outfig)
plt.legend(('$\omega_c = 0.5$', '$\omega_c = 0.7$', '$\omega_c = 0.1$'))

# Explore range of values for zeta_c, with omega_c = 0.07
outfig = plt.subplot(2, 2, 2)
inpfig = plt.subplot(2, 2, 4)
wc = 0.7
for zc in [0.5, 0.7, 1]:
    # Place the poles of the system
    K, kf, clsys = normalized_place(wc, zc)

    # Compute the step response
    t, y, x = ct.step_response(clsys, np.linspace(0, 20, 100), return_x=True)

    # Compute the input used to generate the control response
    u = -K @ x + kf * 1

    # Plot the results
    normalized_plot(t, y, u, inpfig, outfig)

# Add labels to the figure
normalized_label(inpfig, outfig)
plt.legend(('$\zeta_c = 0.5$', '$\zeta_c = 0.7$', '$\zeta_c = 1$'))
plt.tight_layout()
_images/steering_12_0.png

RMM notes, 17 Jul 2019 * These step responses are very slow. Note that the steering wheel angles are about 10X less than a resonable bound (0.05 rad at 30 m/s). A consequence of these low gains is that the tracking controller in Example 8.4 has to use a different set of gains. We could update, but the gains listed here have a rationale that we would have to update as well. * Based on the discussion below, I think we should make \omega_\text{c} range from 0.5 to 1 (10X faster).

KJA response, 20 Jul 2019: Makes a lot of sense to make \omega_\text{c} range from 0.5 to 1 (10X faster). The plots were still in the range 0.05 to 0.1 in the note you sent me.

RMM response: 23 Jul 2019: Updated \omega_\text{c} to 10X faster. Note that this makes size of the inputs for the step response quite large, but that is in part because a unit step in the desired position produces an (instantaneous) error of b = 3 m \implies quite a large error. A lateral error of 10 cm with \omega_c = 0.7 would produce an (initial) input of 0.015 rad.

Eigenvalue placement observer design (Example 8.3)

We construct an estimator for the (normalized) lateral dynamics by assigning the eigenvalues of the estimator dynamics to desired value, specifified in terms of the second order characteristic equation for the estimator dynamics.

[6]:

# Find the eigenvalue from the characteristic polynomial
wo = 1          # bandwidth for the observer
zo = 0.7        # damping ratio for the observer
eigs = np.roots([1, 2*zo*wo, wo**2])

# Compute the estimator gain using eigenvalue placement
L = np.transpose(
    ct.place(np.transpose(A), np.transpose(C), eigs))
print("L = ", L)

# Create a linear model of the lateral dynamics driving the estimator
est = ct.StateSpace(A - L @ C, np.block([[B, L]]), np.eye(2), np.zeros((2,2)))

L =  [[1.4]
 [1. ]]
Linear observer applied to nonlinear system output

A simulation of the observer for a vehicle driving on a curvy road is shown below. The first figure shows the trajectory of the vehicle on the road, as viewed from above. The response of the observer is shown on the right, where time is normalized to the vehicle length. We see that the observer error settles in about 4 vehicle lengths.

RMM note, 27 Jun 2019: * As an alternative, we can attempt to estimate the state of the full nonlinear system using a linear estimator. This system does not necessarily converge to zero since there will be errors in the nominal dynamics of the system for the linear estimator. * The limits on the x axis for the time plots are different to show the error over the entire trajectory. * We should decide whether we want to keep the figure above or the one below for the text.

KJA comment, 1 Jul 2019: * I very much like your observation about the nonlinear system. I think it is a very good idea to use your new simulation

RMM comment, 17 Jul 2019: plan to use this version in the text.

KJA comment, 20 Jul 2019: I think this is a big improvement we show that an observer based on a linearized model works on a nonlinear simulation, If possible we could add a line telling why the linear model works and that this is standard procedure in control engineering.

[7]:

# Convert the curvy trajectory into normalized coordinates
x_ref = x_curvy[0] / wheelbase
y_ref = x_curvy[1] / wheelbase
theta_ref = x_curvy[2]
tau = v0 * T_curvy / b

# Simulate the estimator, with a small initial error in y position
t, y_est, x_est = ct.forced_response(est, tau, [delta_curvy, y_ref], [0.5, 0])

# Configure matplotlib plots to be a bit bigger and optimize layout
plt.figure(figsize=[9, 4.5])

# Plot the actual and estimated states
ax = plt.subplot(2, 2, 1)
plt.plot(t, y_ref)
plt.plot(t, x_est[0])
ax.set(xlim=[0, 10])
plt.legend(['actual', 'estimated'])
plt.ylabel('Lateral position $y/b$')

ax = plt.subplot(2, 2, 2)
plt.plot(t, x_est[0] - y_ref)
ax.set(xlim=[0, 10])
plt.ylabel('Lateral error')

ax = plt.subplot(2, 2, 3)
plt.plot(t, theta_ref)
plt.plot(t, x_est[1])
ax.set(xlim=[0, 10])
plt.xlabel('Normalized time $v_0 t / b$')
plt.ylabel('Vehicle angle $\\theta$')

ax = plt.subplot(2, 2, 4)
plt.plot(t, x_est[1] - theta_ref)
ax.set(xlim=[0, 10])
plt.xlabel('Normalized time $v_0 t / b$')
plt.ylabel('Angle error')
plt.tight_layout()
_images/steering_18_0.png
Output Feedback Controller (Example 8.4)

RMM note, 27 Jun 2019 * The feedback gains for the controller below are different that those computed in the eigenvalue placement example (from Ch 7), where an argument was given for the choice of the closed loop eigenvalues. Should we choose a single, consistent set of gains in both places? * This plot does not quite match Example 8.4 because a different reference is being used for the laterial position. * The transient in \delta is quiet large. This appears to be due to the error in \theta(0), which is initialized to zero intead of to theta_curvy.

KJA comment, 1 Jul 2019: 1. The large initial errors dominate the plots.

  1. There is somehing funny happening at the end of the simulation, may be due to the small curvature at the end of the path?

RMM comment, 17 Jul 2019: * Updated to use the new trajectory * We will have the issue that the gains here are different than the gains that we used in Chapter 7. I think that what we need to do is update the gains in Ch 7 (they are too sluggish, as noted above). * Note that unlike the original example in the book, the errors do not converge to zero. This is because we are using pure state feedback (no feedforward) => the controller doesn’t apply any input until there is an error.

KJA comment, 20 Jul 2019: We may add that state feedback is a proportional controller which does not guarantee that the error goes to zero for example by changing the line “The tracking error …” to “The tracking error can be improved by adding integral action (Section7.4), later in this chapter “Disturbance Modeling” or feedforward (Section 8,5). Should we do an exercises?

[8]:

# Compute the feedback gains
# K, kf, clsys = normalized_place(1, 0.707)     # Gains from MATLAB
# K, kf, clsys = normalized_place(0.07, 0.707)  # Original gains
K, kf, clsys = normalized_place(0.7, 0.707)       # Final gains

# Print out the gains
print("K = ", K)
print("kf = ", kf)

# Construct an output-based controller for the system
clsys = ct.StateSpace(
    np.block([[A, -B@K], [L@C, A - B@K - L@C]]),
    np.block([[B], [B]]) * kf,
    np.block([[C, np.zeros(C.shape)], [np.zeros(C.shape), C]]),
    np.zeros((2,1)))

# Simulate the system
t, y, x = ct.forced_response(clsys, tau, y_ref, [0.4, 0, 0.0, 0])

# Calcaluate the input used to generate the control response
u_sfb = kf * y_ref - K @ x[0:2]
u_ofb = kf * y_ref - K @ x[2:4]

# Configure matplotlib plots to be a bit bigger and optimize layout
plt.figure(figsize=[9, 4.5])

# Plot the actual and estimated states
ax = plt.subplot(1, 2, 1)
plt.plot(t, x[0])
plt.plot(t, x[2])
plt.plot(t, y_ref, 'k-.')
ax.set(xlim=[0, 30])
plt.legend(['state feedback', 'output feedback', 'reference'])
plt.xlabel('Normalized time $v_0 t / b$')
plt.ylabel('Lateral position $y/b$')

ax = plt.subplot(2, 2, 2)
plt.plot(t, x[1])
plt.plot(t, x[3])
plt.plot(t, theta_ref, 'k-.')
ax.set(xlim=[0, 15])
plt.ylabel('Vehicle angle $\\theta$')

ax = plt.subplot(2, 2, 4)
plt.plot(t, u_sfb[0])
plt.plot(t, u_ofb[0])
plt.plot(t, delta_curvy, 'k-.')
ax.set(xlim=[0, 15])
plt.xlabel('Normalized time $v_0 t / b$')
plt.ylabel('Steering angle $\\delta$')
plt.tight_layout()

K =  [[0.49   0.7448]]
kf =  [[0.49]]
_images/steering_21_1.png
Trajectory Generation (Example 8.8)

To illustrate how we can use a two degree-of-freedom design to improve the performance of the system, consider the problem of steering a car to change lanes on a road. We use the non-normalized form of the dynamics, which were derived in Example 3.11.

KJA comment, 1 Jul 2019: 1. I think the reference trajectory is too much curved in the end compare with Example 3.11

In summary I think it is OK to change the reference trajectories but we should make sure that the curvature is less than \rho=600 m not to have too high acceleratarion.

RMM response, 16 Jul 2019: * Not sure if the comment about the trajectory being too curved is referring to this example. The steering angles (and hence radius of curvature/acceleration) are quite low. ??

KJA response, 20 Jul 2019: You are right the curvature is not too small. We could add the sentence “The small deviations can be eliminated by adding feedback.”

RMM response, 23 Jul 2019: I think the small deviation you are referring to is in the velocity trace. This occurs because I gave a fixed endpoint in time and so the velocity had to be adjusted to hit that exact point at that time. This doesn’t show up in the book, so it won’t be a problem (\implies no additional explanation required).

[9]:

import control.flatsys as fs

# Function to take states, inputs and return the flat flag
def vehicle_flat_forward(x, u, params={}):
    # Get the parameter values
    b = params.get('wheelbase', 3.)

    # Create a list of arrays to store the flat output and its derivatives
    zflag = [np.zeros(3), np.zeros(3)]

    # Flat output is the x, y position of the rear wheels
    zflag[0][0] = x[0]
    zflag[1][0] = x[1]

    # First derivatives of the flat output
    zflag[0][1] = u[0] * np.cos(x[2])  # dx/dt
    zflag[1][1] = u[0] * np.sin(x[2])  # dy/dt

    # First derivative of the angle
    thdot = (u[0]/b) * np.tan(u[1])

    # Second derivatives of the flat output (setting vdot = 0)
    zflag[0][2] = -u[0] * thdot * np.sin(x[2])
    zflag[1][2] =  u[0] * thdot * np.cos(x[2])

    return zflag

# Function to take the flat flag and return states, inputs
def vehicle_flat_reverse(zflag, params={}):
    # Get the parameter values
    b = params.get('wheelbase', 3.)

    # Create a vector to store the state and inputs
    x = np.zeros(3)
    u = np.zeros(2)

    # Given the flat variables, solve for the state
    x[0] = zflag[0][0]  # x position
    x[1] = zflag[1][0]  # y position
    x[2] = np.arctan2(zflag[1][1], zflag[0][1])  # tan(theta) = ydot/xdot

    # And next solve for the inputs
    u[0] = zflag[0][1] * np.cos(x[2]) + zflag[1][1] * np.sin(x[2])
    thdot_v = zflag[1][2] * np.cos(x[2]) - zflag[0][2] * np.sin(x[2])
    u[1] = np.arctan2(thdot_v, u[0]**2 / b)

    return x, u

vehicle_flat = fs.FlatSystem(vehicle_flat_forward, vehicle_flat_reverse, inputs=2, states=3)

To find a trajectory from an initial state x_0 to a final state x_\text{f} in time T_\text{f} we solve a point-to-point trajectory generation problem. We also set the initial and final inputs, which sets the vehicle velocity v and steering wheel angle \delta at the endpoints.

[10]:

# Define the endpoints of the trajectory
x0 = [0., 2., 0.]; u0 = [15, 0.]
xf = [75, -2., 0.]; uf = [15, 0.]
Tf = xf[0] / uf[0]

# Define a set of basis functions to use for the trajectories
poly = fs.PolyFamily(6)

# Find a trajectory between the initial condition and the final condition
traj = fs.point_to_point(vehicle_flat, x0, u0, xf, uf, Tf, basis=poly)

# Create the trajectory
t = np.linspace(0, Tf, 100)
x, u = traj.eval(t)

# Configure matplotlib plots to be a bit bigger and optimize layout
plt.figure(figsize=[9, 4.5])

# Plot the trajectory in xy coordinate
plt.subplot(1, 4, 2)
plt.plot(x[1], x[0])
plt.xlabel('y [m]')
plt.ylabel('x [m]')

# Add lane lines and scale the axis
plt.plot([-4, -4], [0, x[0, -1]], 'k-', linewidth=1)
plt.plot([0, 0], [0, x[0, -1]], 'k--', linewidth=1)
plt.plot([4, 4], [0, x[0, -1]], 'k-', linewidth=1)
plt.axis([-10, 10, -5, x[0, -1] + 5])

# Time traces of the state and input
plt.subplot(2, 4, 3)
plt.plot(t, x[1])
plt.ylabel('y [m]')

plt.subplot(2, 4, 4)
plt.plot(t, x[2])
plt.ylabel('theta [rad]')

plt.subplot(2, 4, 7)
plt.plot(t, u[0])
plt.xlabel('Time t [sec]')
plt.ylabel('v [m/s]')
plt.axis([0, Tf, u0[0] - 1, uf[0] +1])

plt.subplot(2, 4, 8)
plt.plot(t, u[1]);
plt.xlabel('Time t [sec]')
plt.ylabel('$\delta$ [rad]')
plt.tight_layout()
_images/steering_26_0.png
Vehicle transfer functions for forward and reverse driving (Example 10.11)

The vehicle steering model has different properties depending on whether we are driving forward or in reverse. The figures below show step responses from steering angle to lateral translation for a the linearized model when driving forward (dashed) and reverse (solid). In this simulation we have added an extra pole with the time constant T=0.1 to approximately account for the dynamics in the steering system.

With rear-wheel steering the center of mass first moves in the wrong direction and the overall response with rear-wheel steering is significantly delayed compared with that for front-wheel steering. (b) Frequency response for driving forward (dashed) and reverse (solid). Notice that the gain curves are identical, but the phase curve for driving in reverse has non-minimum phase.

RMM note, 27 Jun 2019: * I cannot recreate the figures in Example 10.11. Since we are looking at the lateral velocity, there is a differentiator in the output and this takes the step function and creates an offset at t = 0 (intead of a smooth curve). * The transfer functions are also different, and I don’t quite understand why. Need to spend a bit more time on this one.

KJA comment, 1 Jul 2019: The reason why you cannot recreate figures i Example 10.11 is because the caption in figure is wrong, sorry my fault, the y-axis should be lateral position not lateral velocity. The approximate expression for the transfer functions

G_{y\delta}=\frac{av_0s+v_0^2}{bs} = \frac{1.5 s + 1}{3s^2}=\frac{0.5s + 0.33}{s}

are quite close to the values that you get numerically

In this case I think it is useful to have v=1 m/s because we do not drive to fast backwards.

RMM response, 17 Jul 2019 * Updated figures below use the same parameters as the running example (the current text uses different parameters) * Following the material in the text, a pole is added at s = -1 to approximate the dynamics of the steering system. This is not strictly needed, so we could decide to take it out (and update the text)

KJA comment, 20 Jul 2019: I have been oscillating a bit about this example. Of course it does not make sense to drive in reverse in 30 m/s but it seems a bit silly to change parameters just in this case (if we do we have to motivate it). On the other hand what we are doing is essentially based on transfer functions and a RHP zero. My current view which has changed a few times is to keep the standard parameters. In any case we should eliminate the extra time constant. A small detail, I could not see the time response in the file you sent, do not resend it!, I will look at the final version.

RMM comment, 23 Jul 2019: I think it is OK to have the speed be different and just talk about this in the text. I have removed the extra time constant in the current version.

[11]:

# Magnitude of the steering input (half maximum)
Msteer = vehicle_params['maxsteer'] / 2

# Create a linearized model of the system going forward at 2 m/s
forward_lateral = ct.linearize(lateral, [0, 0], [0], params={'velocity': 2})
forward_tf = ct.ss2tf(forward_lateral)[0, 0]
print("Forward TF = ", forward_tf)

# Create a linearized model of the system going in reverise at 1 m/s
reverse_lateral = ct.linearize(lateral, [0, 0], [0], params={'velocity': -2})
reverse_tf = ct.ss2tf(reverse_lateral)[0, 0]
print("Reverse TF = ", reverse_tf)

Forward TF =
          s + 1.333
-----------------------------
s^2 + 7.828e-16 s - 1.848e-16

Reverse TF =
          -s + 1.333
-----------------------------
s^2 - 7.828e-16 s - 1.848e-16


[12]:

# Configure matplotlib plots to be a bit bigger and optimize layout
plt.figure()

# Forward motion
t, y = ct.step_response(forward_tf * Msteer, np.linspace(0, 4, 500))
plt.plot(t, y, 'b--')

# Reverse motion
t, y = ct.step_response(reverse_tf * Msteer, np.linspace(0, 4, 500))
plt.plot(t, y, 'b-')

# Add labels and reference lines
plt.axis([0, 4, -0.5, 2.5])
plt.legend(['forward', 'reverse'], loc='upper left')
plt.xlabel('Time $t$ [s]')
plt.ylabel('Lateral position [m]')
plt.plot([0, 4], [0, 0], 'k-', linewidth=1)

# Plot the Bode plots
plt.figure()
plt.subplot(1, 2, 2)
ct.bode_plot(forward_tf[0, 0], np.logspace(-1, 1, 100), color='b', linestyle='--')
ct.bode_plot(reverse_tf[0, 0], np.logspace(-1, 1, 100), color='b', linestyle='-')
plt.legend(('forward', 'reverse'));
_images/steering_30_0.png
_images/steering_30_1.png
Feedforward Compensation (Example 12.6)

For a lane transfer system we would like to have a nice response without overshoot, and we therefore consider the use of feedforward compensation to provide a reference trajectory for the closed loop system. We choose the desired response as F_\text{m}(s) = a^22/(s + a)^2, where the response speed or aggressiveness of the steering is governed by the parameter a.

RMM note, 27 Jun 2019: * a was used in the original description of the dynamics as the reference offset. Perhaps choose a different symbol here? * In current version of Ch 12, the y axis is labeled in absolute units, but it should actually be in normalized units, I think. * The steering angle input for this example is quite high. Compare to Example 8.8, above. Also, we should probably make the size of the “lane change” from this example match whatever we use in Example 8.8

KJA comments, 1 Jul 2019: Chosen parameters look good to me

RMM response, 17 Jul 2019 * I changed the time constant for the feedforward model to give something that is more reasonable in terms of turning angle at the speed of v_0 = 30 m/s. Note that this takes about 30 body lengths to change lanes (= 9 seconds at 105 kph). * The time to change lanes is about 2X what it is using the differentially flat trajectory above. This is mainly because the feedback controller applies a large pulse at the beginning of the trajectory (based on the input error), whereas the differentially flat trajectory spreads the turn over a longer interval. Since are living the steering angle, we have to limit the size of the pulse => slow down the time constant for the reference model.

KJA response, 20 Jul 2019: I think the time for lane change is too long, which may depend on the small steering angles used. The largest steering angle is about 0.03 rad, but we have admitted larger values in previous examples. I suggest that we change the design so that the largest sterring angel is closer to 0.05, see the remark from Bjorn O a lane change could take about 5 s at 30m/s.

RMM response, 23 Jul 2019: I reset the time constant to 0.2, which gives something closer to what we had for trajectory generation. It is still slower, but this is to be expected since it is a linear controller. We now finish the trajectory in 20 body lengths, which is about 6 seconds.

[13]:

# Define the desired response of the system
a = 0.2
P = ct.ss2tf(lateral_normalized)
Fm = ct.TransferFunction([a**2], [1, 2*a, a**2])
Fr = Fm / P

# Compute the step response of the feedforward components
t, y_ffwd = ct.step_response(Fm, np.linspace(0, 25, 100))
t, delta_ffwd = ct.step_response(Fr, np.linspace(0, 25, 100))

# Scale and shift to correspond to lane change (-2 to +2)
y_ffwd = 0.5 - 1 * y_ffwd
delta_ffwd *= 1

# Overhead view
plt.subplot(1, 2, 1)
plt.plot(y_ffwd, t)
plt.plot(-1*np.ones(t.shape), t, 'k-', linewidth=1)
plt.plot(0*np.ones(t.shape), t, 'k--', linewidth=1)
plt.plot(1*np.ones(t.shape), t, 'k-', linewidth=1)
plt.axis([-5, 5, -2, 27])

# Plot the response
plt.subplot(2, 2, 2)
plt.plot(t, y_ffwd)
# plt.axis([0, 10, -5, 5])
plt.ylabel('Normalized position y/b')

plt.subplot(2, 2, 4)
plt.plot(t, delta_ffwd)
# plt.axis([0, 10, -1, 1])
plt.ylabel('$\\delta$ [rad]')
plt.xlabel('Normalized time $v_0 t / b$');

plt.tight_layout()
_images/steering_33_0.png
Fundamental Limits (Example 14.13)

Consider a controller based on state feedback combined with an observer where we want a faster closed loop system and choose \omega_\text{c} = 10, \zeta_\text{c} = 0.707, \omega_\text{o} = 20, and \zeta_\text{o} = 0.707.

KJA comment, 20 Jul 2019: This is a really troublesome case. If we keep it as a vehicle steering problem we must have an order of magnitude lower valuer for \omega_c and \omega_o and then the zero will not be slow. My recommendation is to keep it as a general system with the transfer function. P(s)=(s+1)/s^2. The text then has to be reworded.

RMM response, 23 Jul 2019: I think the way we have it is OK. Our current value for the controller and observer is \omega_\text{c} = 0.7 and \omega_\text{o} = 1. Here we way we want something faster and so we got to \omega_\text{c} = 7 (10X) and \omega_\text{o} = 10 (10X).

[14]:

# Compute the feedback gain using eigenvalue placement
wc = 10
zc = 0.707
eigs = np.roots([1, 2*zc*wc, wc**2])
K = ct.place(A, B, eigs)
kr = np.real(1/clsys.evalfr(0))
print("K = ", np.squeeze(K))

# Compute the estimator gain using eigenvalue placement
wo = 20
zo = 0.707
eigs = np.roots([1, 2*zo*wo, wo**2])
L = np.transpose(
    ct.place(np.transpose(A), np.transpose(C), eigs))
print("L = ", np.squeeze(L))

# Construct an output-based controller for the system
C1 = ct.ss2tf(ct.StateSpace(A - B@K - L@C, L, K, 0))
print("C(s) = ", C1)

# Compute the loop transfer function and plot Nyquist, Bode
L1 = P * C1
plt.figure(); ct.nyquist_plot(L1, np.logspace(0.5, 3, 500))
plt.figure(); ct.bode_plot(L1, np.logspace(-1, 3, 500));

K =  [100.   -35.86]
L =  [ 28.28 400.  ]
C(s) =
-1.152e+04 s + 4e+04
--------------------
s^2 + 42.42 s + 6658
_images/steering_36_1.png
_images/steering_36_2.png 
[15]:

# Modified control law
wc = 10
zc = 2.6
eigs = np.roots([1, 2*zc*wc, wc**2])
K = ct.place(A, B, eigs)
kr = np.real(1/clsys.evalfr(0))
print("K = ", np.squeeze(K))

# Construct an output-based controller for the system
C2 = ct.ss2tf(ct.StateSpace(A - B@K - L@C, L, K, 0))
print("C(s) = ", C2)

K =  [100.   2.]
C(s) =
    3628 s + 4e+04
---------------------
s^2 + 80.28 s + 156.6


[16]:

# Plot the gang of four for the two designs
ct.gangof4(P, C1, np.logspace(-1, 3, 100))
ct.gangof4(P, C2, np.logspace(-1, 3, 100))
_images/steering_38_0.png 
[ ]:

Vertical takeoff and landing aircraft

This notebook demonstrates the use of the python-control package for analysis and design of a controller for a vectored thrust aircraft model that is used as a running example through the text Feedback Systems by Astrom and Murray. This example makes use of MATLAB compatible commands.

Additional information on this system is available at

http://www.cds.caltech.edu/~murray/wiki/index.php/Python-control/Example:_Vertical_takeoff_and_landing_aircraft

System Description

This example uses a simplified model for a (planar) vertical takeoff and landing aircraft (PVTOL), as shown below:

PVTOL diagram

PVTOL dynamics

The position and orientation of the center of mass of the aircraft is denoted by (x,y,\theta), m is the mass of the vehicle, J the moment of inertia, g the gravitational constant and c the damping coefficient. The forces generated by the main downward thruster and the maneuvering thrusters are modeled as a pair of forces F_1 and F_2 acting at a distance r below the aircraft (determined by the geometry of the thrusters).

Letting z=(x,y,\theta, \dot x, \dot y, \dot\theta), the equations can be written in state space form as:

\frac{dz}{dt} = \begin{bmatrix} z_4 \\ z_5 \\ z_6 \\ -\frac{c}{m} z_4 \\ -g- \frac{c}{m} z_5 \\ 0 \end{bmatrix} + \begin{bmatrix} 0 \\ 0 \\ 0 \\ \frac{1}{m} \cos \theta F_1 + \frac{1}{m} \sin \theta F_2 \\ \frac{1}{m} \sin \theta F_1 + \frac{1}{m} \cos \theta F_2 \\ \frac{r}{J} F_1 \end{bmatrix}

LQR state feedback controller

This section demonstrates the design of an LQR state feedback controller for the vectored thrust aircraft example. This example is pulled from Chapter 6 (Linear Systems, Example 6.4) and Chapter 7 (State Feedback, Example 7.9) of Astrom and Murray. The python code listed here are contained the the file pvtol-lqr.py.

To execute this example, we first import the libraries for SciPy, MATLAB plotting and the python-control package:

[1]:

from numpy import *             # Grab all of the NumPy functions
from matplotlib.pyplot import * # Grab MATLAB plotting functions
from control.matlab import *    # MATLAB-like functions
%matplotlib inline

The parameters for the system are given by

[2]:

m = 4                         # mass of aircraft
J = 0.0475                    # inertia around pitch axis
r = 0.25                      # distance to center of force
g = 9.8                       # gravitational constant
c = 0.05                      # damping factor (estimated)

Choosing equilibrium inputs to be u_e = (0, mg), the dynamics of the system \frac{dz}{dt}, and their linearization A about equilibrium point z_e = (0, 0, 0, 0, 0, 0) are given by

\frac{dz}{dt} = \begin{bmatrix} z_4 \\ z_5 \\ z_6 \\ -g \sin z_3 -\frac{c}{m} z_4 \\ g(\cos z_3 - 1)- \frac{c}{m} z_5 \\ 0 \end{bmatrix} \qquad A = \begin{bmatrix} 0 & 0 & 0 &1&0&0\\ 0&0&0&0&1&0 \\ 0&0&0&0&0&1 \\ 0&0&-g&-c/m&0&0 \\ 0&0&0&0&-c/m&0 \\ 0&0&0&0&0&0 \end{bmatrix}

[3]:

# State space dynamics
xe = [0, 0, 0, 0, 0, 0]         # equilibrium point of interest
ue = [0, m*g]                   # (note these are lists, not matrices)

[4]:

# Dynamics matrix (use matrix type so that * works for multiplication)
# Note that we write A and B here in full generality in case we want
# to test different xe and ue.
A = matrix(
   [[ 0,    0,    0,    1,    0,    0],
    [ 0,    0,    0,    0,    1,    0],
    [ 0,    0,    0,    0,    0,    1],
    [ 0, 0, (-ue[0]*sin(xe[2]) - ue[1]*cos(xe[2]))/m, -c/m, 0, 0],
    [ 0, 0, (ue[0]*cos(xe[2]) - ue[1]*sin(xe[2]))/m, 0, -c/m, 0],
    [ 0,    0,    0,    0,    0,    0 ]])

# Input matrix
B = matrix(
   [[0, 0], [0, 0], [0, 0],
    [cos(xe[2])/m, -sin(xe[2])/m],
    [sin(xe[2])/m,  cos(xe[2])/m],
    [r/J, 0]])

# Output matrix
C = matrix([[1, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0]])
D = matrix([[0, 0], [0, 0]])

To compute a linear quadratic regulator for the system, we write the cost function as

J = \int_0^\infty (\xi^T Q_\xi \xi + v^T Q_v v) dt,

where \xi = z - z_e and v = u - u_e represent the local coordinates around the desired equilibrium point (z_e, u_e). We begin with diagonal matrices for the state and input costs:

[5]:

Qx1 = diag([1, 1, 1, 1, 1, 1])
Qu1a = diag([1, 1])
(K, X, E) = lqr(A, B, Qx1, Qu1a); K1a = matrix(K)

This gives a control law of the form v = -K \xi, which can then be used to derive the control law in terms of the original variables:

u = v + u_e = - K(z - z_d) + u_d. where :math:`u_e = (0, mg)` and :math:`z_d = (x_d, y_d, 0, 0, 0, 0)`

The way we setup the dynamics above, A is already hardcoding u_d, so we don’t need to include it as an external input. So we just need to cascade the -K(z-z_d) controller with the PVTOL aircraft’s dynamics to control it. For didactic purposes, we will cheat in two small ways:

  • First, we will only interface our controller with the linearized dynamics. Using the nonlinear dynamics would require the NonlinearIOSystem functionalities, which we leave to another notebook to introduce.

  1. Second, as written, our controller requires full state feedback (K multiplies full state vectors z), which we do not have access to because our system, as written above, only returns x and y (because of C matrix). Hence, we would need a state observer, such as a Kalman Filter, to track the state variables. Instead, we assume that we have access to the full state.

The following code implements the closed loop system:

[6]:

# Our input to the system will only be (x_d, y_d), so we need to
# multiply it by this matrix to turn it into z_d.
Xd = matrix([[1,0,0,0,0,0],
             [0,1,0,0,0,0]]).T

# Closed loop dynamics
H = ss(A-B*K,B*K*Xd,C,D)

# Step response for the first input
x,t = step(H,input=0,output=0,T=linspace(0,10,100))
# Step response for the second input
y,t = step(H,input=1,output=1,T=linspace(0,10,100))

[7]:

plot(t,x,'-',t,y,'--')
plot([0, 10], [1, 1], 'k-')
ylabel('Position')
xlabel('Time (s)')
title('Step Response for Inputs')
legend(('Yx', 'Yy'), loc='lower right')
show()
_images/pvtol-lqr-nested_12_0.png

The plot above shows the x and y positions of the aircraft when it is commanded to move 1 m in each direction. The following shows the x motion for control weights \rho = 1, 10^2, 10^4. A higher weight of the input term in the cost function causes a more sluggish response. It is created using the code:

[8]:

# Look at different input weightings
Qu1a = diag([1, 1])
K1a, X, E = lqr(A, B, Qx1, Qu1a)
H1ax = H = ss(A-B*K1a,B*K1a*Xd,C,D)

Qu1b = (40**2)*diag([1, 1])
K1b, X, E = lqr(A, B, Qx1, Qu1b)
H1bx = H = ss(A-B*K1b,B*K1b*Xd,C,D)

Qu1c = (200**2)*diag([1, 1])
K1c, X, E = lqr(A, B, Qx1, Qu1c)
H1cx = ss(A-B*K1c,B*K1c*Xd,C,D)

[Y1, T1] = step(H1ax, T=linspace(0,10,100), input=0,output=0)
[Y2, T2] = step(H1bx, T=linspace(0,10,100), input=0,output=0)
[Y3, T3] = step(H1cx, T=linspace(0,10,100), input=0,output=0)

[9]:

plot(T1, Y1.T, 'b-', T2, Y2.T, 'r-', T3, Y3.T, 'g-')
plot([0 ,10], [1, 1], 'k-')
title('Step Response for Inputs')
ylabel('Position')
xlabel('Time (s)')
legend(('Y1','Y2','Y3'),loc='lower right')
axis([0, 10, -0.1, 1.4])
show()
_images/pvtol-lqr-nested_15_0.png
Lateral control using inner/outer loop design

This section demonstrates the design of loop shaping controller for the vectored thrust aircraft example. This example is pulled from Chapter 11 (Frequency Domain Design) of Astrom and Murray.

To design a controller for the lateral dynamics of the vectored thrust aircraft, we make use of a “inner/outer” loop design methodology. We begin by representing the dynamics using the block diagram

8f98d0f8ae1f4d27a817dff392c80bec where 9dbd995d12914555954c46878ca7f5b6
The controller is constructed by splitting the process dynamics and controller into two components: an inner loop consisting of the roll dynamics P_i and control C_i and an outer loop consisting of the lateral position dynamics P_o and controller C_o. 8e35deed606a425fa3a7e363a8776c23 The closed inner loop dynamics H_i control the roll angle of the aircraft using the vectored thrust while the outer loop controller C_o commands the roll angle to regulate the lateral position.

The following code imports the libraries that are required and defines the dynamics:

[10]:

from matplotlib.pyplot import * # Grab MATLAB plotting functions
from control.matlab import *    # MATLAB-like functions

[12]:

# System parameters
m = 4                         # mass of aircraft
J = 0.0475                    # inertia around pitch axis
r = 0.25                      # distance to center of force
g = 9.8                       # gravitational constant
c = 0.05                      # damping factor (estimated)

[13]:

# Transfer functions for dynamics
Pi = tf([r], [J, 0, 0])       # inner loop (roll)
Po = tf([1], [m, c, 0])       # outer loop (position)

For the inner loop, use a lead compensator

[14]:

k = 200
a = 2
b = 50
Ci = k*tf([1, a], [1, b])     # lead compensator
Li = Pi*Ci

The closed loop dynamics of the inner loop, H_i, are given by

[15]:

Hi = parallel(feedback(Ci, Pi), -m*g*feedback(Ci*Pi, 1))

Finally, we design the lateral compensator using another lead compenstor

[16]:

# Now design the lateral control system
a = 0.02
b = 5
K = 2
Co = -K*tf([1, 0.3], [1, 10])         # another lead compensator
Lo = -m*g*Po*Co

The performance of the system can be characterized using the sensitivity function and the complementary sensitivity function:

[17]:

L = Co*Hi*Po
S = feedback(1, L)
T = feedback(L, 1)

[18]:

t, y = step(T, T=linspace(0,10,100))
plot(y, t)
title("Step Response")
grid()
xlabel("time (s)")
ylabel("y(t)")
show()
_images/pvtol-lqr-nested_28_0.png

The frequency response and Nyquist plot for the loop transfer function are computed using the commands

[19]:

bode(L)
show()
_images/pvtol-lqr-nested_30_0.png 
[20]:

nyquist(L, (0.0001, 1000))
show()
_images/pvtol-lqr-nested_31_0.png 
[21]:

gangof4(Hi*Po, Co)
_images/pvtol-lqr-nested_32_0.png

Development

You can check out the latest version of the source code with the command:

git clone https://github.com/python-control/python-control.git

You can run the unit tests with pytest to make sure that everything is working correctly. Inside the source directory, run:

pytest -v

or to test the installed package:

pytest --pyargs control -v

Your contributions are welcome! Simply fork the GitHub repository and send a pull request.

Links