Introduction to

Feedback Control
USING D ESIGN S TUDIES

R ANDAL W. B EARD
WITH

T IMOTHY W. M C L AIN
C AMMY P ETERSON
M ARC K ILLPACK

Revised: December 9, 2021

© 2016 Randal W. Beard
All rights reserved.

This work may not be distributed and/or modified without written consent of the authors.
Contents

Page
1 Introduction 1
Design Study A: Single Link Robot Arm 5
Design Study B: Pendulum on a Cart 6
Design Study C: Satellite Attitude Control 7

I Simulation Models 8
2 Kinetic Energy 11
3 Euler-Lagrange 33

II Design Models 49
4 Linearization 51
5 Transfer Functions 61
6 State Space Models 73

III PID Control Design 86

7 Second Order Systems 91
8 Second Order Design 99
9 Integrators 127
10 Digital PID 145

IV Observer Based Control Design 158

11 Full State Feedback 161
12 Full State with Integrators 185
13 Observers 205
14 Disturbance Observers 235

V Loopshaping Control Design 258

15 Frequency Response 259
16 Frequency Specifications 281
17 Stability and Robustness Margins 301
18 Compensator Design 321

VI Homework Problems 378

D Design Study: Mass Spring Damper 379
E Design Study: Ball on Beam 387
F Design Study: Planar VTOL 395

Appendices 407
P.1 Simulating Control Systems in Python 409
P.2 Simulating Control Systems in Matlab 423
P.3 Simulating Control Systems in Simulink 437
P.4 Numerical ODE Solvers 449
P.5 Differential Equations 461
P.6 Root Locus 469
P.7 Linear Algebra 479

Bibliography 488

Index 488
 Preface

Does the world really need another introductory textbook on feedback control?
For many years we have felt that the many quality textbooks that currently exist
are more than sufficient. However, several years ago the first author (RWB) had
an experience that changed my mind, and the minds of my colleagues. I had
just finished teaching the introductory feedback control course at BYU. I had a
need for a new master’s student in my research group and hired one of the top
students in the class. We had designed a gimbal system for a small unmanned air
vehicle, and we needed a control system implemented on a microcontroller for
the gimbal. I tasked this new master’s student to model the gimbal and design
the control system. I was surprised by how much the student struggled with this
task, especially understanding where to begin and how to model the gimbal. If
I had given him a transfer function and asked him to design a PID controller, or
if I had given him a state space model and asked him to design an observer and
controller for the system, he would not have had any difficulty. But he did not
know how to do an end-to-end design that required developing models for the
system, including physical constraints. It was this experience that convinced me
that my current approach to teaching feedback control was inadequate.
The next time that I taught the course, rather than focusing on the theory of
feedback design, I focused the lectures on the end-to-end design process. Accord-
ingly, I spent significantly more time talking about the modeling process, adding
several lectures on the Euler-Lagrange method. I also added several lectures on
linearization about set points, and on developing linear models. When I taught
feedback design methods, I focused on design specifications and the need to ac-
count for saturation, sensor noise, model uncertainty, and external disturbances
on the system. Over the next several years, we developed several complete design
studies and then used these design studies throughout the course to illustrate the
material. That course reorganization and the realization that existing textbooks on
feedback control did not fit the pedagogy were the genesis of this textbook.
Therefore, this textbook is unique in several ways. Most importantly, it is fo-
cused, in its organization, in its examples, and in its homework problems, on a
particular, but fairly general, end-to-end design strategy and methodology. Topics
were included in the book only if they aided in the design process. For example,
the book does not include a chapter on the root locus (although it is discussed in an
appendix). The second unique feature is that the examples and homework prob-
lems follow a small set of design studies throughout the book. The text provides
complete worked examples for three design studies: a single link robot arm, a
pendulum on a cart, and a satellite attitude control problem. The homework prob-
lems parallel the examples with three additional design studies: a mass-spring-
damper, a ball on beam, and a planar vertical take-off and landing aircraft, similar
to a quadrotor. A third unique feature of the book is that complete solutions to
the three example design studies are provided in Matlab/Simulink and Python at
http://controlbook.byu.edu.
The design methodology culminates in computer code that implements the
controllers. In the examples provided in the book, the computer code interacts
with a simulation model of the plant composed of coupled nonlinear differential
equations, rather than the physical plant. However, the example computer code is
written in a way that can be directly implemented on a microcontroller, or other
computing devices. We have used Matlab/Simulink because many students are
already familiar with these tools. But we also provide Python examples because
Python is free of charge and therefore accessible to everyone, and because for
many applications, Python can be used to implement the controller in hardware.
One criticism of the book might be that all of the design problems are mechan-
ical systems, and that they are fairly similar to each other. The student may come
away with the impression that feedback control and the design methods discussed
in the book are only applicable to mechanical systems. Of course this is not true.
The tools taught in the book are widely used in many applications including elec-
trical power systems, disk drives, aerospace systems, chemical processing, biolog-
ical systems, queueing systems, and many more. Although this book is focused
on mechanical systems, we believe that the student who masters the material will
be able to easily transition to other application domains. Mechanical systems, like
those covered in this book, have a compelling pedagogical advantage because the
control objectives are intuitive, and the behavior of the system can be easily visu-
alized. Therefore, we feel that they offer the best setting for developing intuition
behind feedback design.
We had two other motivations for writing this textbook. The first is the ex-
orbitant prices currently being charged for textbooks. Accordingly, we are pro-
viding an electronic version of this book to students free of charge at http:
//controlbook.byu.edu. The book has required significant time and en-
ergy to develop, and so we hope that “free” does not equate to “low quality.” An
Amazon version of the book is also available for those desiring a hard copy. Given
the amount of high quality open source software, students may get the wrong im-
pression about monetary compensation for technical work. Good work should be
compensated. However, since our day jobs provide adequate financial remunera-
tion, we ask to be compensated by your feedback about your educational experi-
ence with the book, including typos and other errors. This will allow us to update
and improve the book. We also hope that you will understand that we will proba-
bly not be very responsive to email asking for help with the homework problems.
Another motivation for writing the book is our observation that this generation
of students seems to prefer to learn using electronic resources. The book has
therefore been written with the assumption that it will be studied electronically.
Therefore, hyperlinks and other electronic aids are embedded throughout the text.
We hope that these enhance the educational experience.

RWB
 Introduction
1
Overview
The objective of this book is to prepare the reader to design feedback control sys-
tems for dynamic systems and to lay the groundwork for further studies in the area.
The design philosophy that we follow throughout the book is illustrated schemat-
ically in Fig. 1-1. The objective is to design a control system for the “Physical
System” shown in Fig. 1-1. The physical system includes actuators like motors
and propellers, and sensors like accelerometers, gyros, GPS, cameras, and pres-
sure sensors. The first step in the design process is to model the physical system
using nonlinear differential equations. While approximations and simplifications
will be necessary at this step, the hope is to capture, in mathematics, all of the
important characteristics of the physical system. While there are many different
methods for developing the equations of motion of the physical systems, in this
book we will introduce one specific method, the Euler-Lagrange method, that is
applicable to a wide variety of mechanical systems. We will, for the most part,
abstract the actuators to applied forces and torques on the system. Similarly, the
sensors will be abstracted to general measurements of certain physical quantities
like position and velocity. The resulting model is called the Simulation Model
as shown in Fig. 1-1. In the design process, the simulation model is used for
the high fidelity computer simulation of the physical system. Every control de-
sign will be tested thoroughly on the simulation model. However, the simulation
model is only a mathematical approximation of the physical system, and simply
because a design is effective on the simulation model, we should not assume that
it will function properly on the physical system. Part I of the book introduces the
Euler-Lagrange method and demonstrates how to derive simulation models for
mechanical systems using this method.
The simulation model is typically nonlinear and high order and is too mathe-
matically complex to be useful for control design. Therefore, to facilitate design,

TOC
CHAPTER 1. INTRODUCTION 2

PID
Control
Transfer Function Linearization,
Design Model Model Reduction,
Loopshaping Simplification, etc.
Control

Observer Based State Space

Control Design Model

Simulation
Model
Test in Simulation

Physical
System
Implement on Physical System Euler-Lagrange Equations

Figure 1-1: The design process. Using principles of physics, the physical sys-
tem is modeled mathematically, resulting in the simulation model. The simulation
model is simplified to create design models that are used for the control design.
The control design is then tested and debugged in simulation and finally imple-
mented on the physical system.

TOC
CHAPTER 1. INTRODUCTION 3

the simulation model is simplified and usually linearized to create lower-order de-
sign models. For any physical system, there may be multiple design models that
capture certain aspects of the system that are important for a particular design. In
this book we will introduce the two most common design models used for control
systems analysis and design: transfer-function models and state-space models. In
both cases we will linearize the system about a particular operating condition.
The advantage of transfer function models is that they capture the frequency de-
pendence of the input-output relationship in dynamic systems. The advantage of
state space models is that they are more intuitive and are better adapted to numer-
ical implementation. Part II of the book shows how to linearize the equations of
motion and how to create transfer function and state space design models.
The next three parts, Part III–V, introduce three different control design meth-
ods. Part III shows how to design Proportional-Integral-Derivative (PID) con-
trollers for second order systems. The PID method is the most common control
technique used in industry. It can be understood from both a time-domain and
frequency-domain perspective. The advantage of PID is its simplicity in design
and implementation and the fact that it can be used to achieve high performance
for a surprising variety of systems. However, the disadvantage of PID control is
that stability and performance can typically only be guaranteed for second order
systems. Therefore, Part III is focused exclusively on second order systems.
In Part IV we introduce the observer-based control method. Observer based
methods can be thought of as a natural extension of PID controllers to high or-
der systems. However, observer based methods are much more general than PID
and can be used to obtain higher performance. In addition, most observer based
techniques extend to nonlinear and time-varying systems. The disadvantages of
observer-based methods are that they require a higher level of mathematical so-
phistication to understand and use and they are primarily time-domain methods.
The final design technique, which is covered in Part V, is the loopshaping
control design method. The loopshaping method is a frequency domain technique
that explicitly addresses robustness of the system. The loopshaping method is also
a natural extension of PID control to higher order systems. One of the advantages
of loopshaping techniques is that they can be used when only a frequency response
model of the system is available.
The book is organized around several design studies. For each new concept
that is introduced, we will apply that concept to three specific design problems.
The design problems are introduced in the following three sections and include a
single link robot arm, a pendulum on a cart, and a simple satellite attitude control
problem. The homework problems, which are provided in Part VI, will follow
the same format with problems that are identical to those worked in the book,
but they will be applied to different physical systems. Our hope is to illustrate
the control design process with specific, easy-to-understand problems. We note,
however, that the concepts introduced in this book are applicable to a much wider
variety of problems than those illustrated in the book. We focus on mechanical
systems, but a similar process (with the exception of the modeling section) can be
followed for any system that can be modeled using ordinary differential equations.

TOC
CHAPTER 1. INTRODUCTION 4

Examples include aerodynamic systems, electrical systems, chemical processes,

large structures like buildings and bridges, biological feedback systems, economic
systems, queuing systems, and many more.

TOC
CHAPTER 1. INTRODUCTION 5

1.1 Design Studies

A Design Study: Single Link Robot Arm
Figure 1-2 shows a single link robot
arm system. The robot arm has mass
m and length `. The angle of the robot
is given by θ as measured from hor-
izontal to the ground. The angular
speed of the arm is θ̇. There is an
applied torque τ at the joint. There
is also a damping torque that opposes
the rotation of the joint of magnitude, Figure 1-2: Single Link Robot Arm
which can be modeled as −bθ̇.
The actual physical parameters of
the system are m = 0.5 kg, ` = 0.3 m, g = 9.8 m/s2 , b = 0.01 Nms. The torque
is limited by |τ | ≤ 1 Nm.

Examples (with solutions):

A.2 Kinetic energy. A.10 Digital PID.

A.3 Equations of motion. A.11 Full state feedback.
A.4 Linearize equations of motion. A.12 Full state with integrator.
A.5 Transfer function model. A.13 Observer based control.
A.6 State space model. A.14 Disturbance observer.
A.7 Pole placement using PD. A.15 Frequency Response.
A.8 Second order design. A.16 Loop gain.
A.9 Integrators and system type. A.17 Stability margins.
A.P.6 Root locus. A.18 Loopshaping design.

Note: The notation identifies the design configuration and the chapter in which
it is used: Example A.4 refers to the application of approximations in Chapter 4
to linearize design configuration A. Similarly, Example A.P.6 refers to the root
locus assignment associated with Appendix A.P.6.

TOC
CHAPTER 1. INTRODUCTION 6

B Design Study: Pendulum on a Cart

Figure 1-3 shows the pendulum on a
cart system. The position of the cart
measured from the origin is z and the
linear speed of the cart is ż. The an-
gle of the pendulum from straight up
is given by θ, and the angular velocity
is θ̇. The rod is of length `, and has
mass m1 , and is approximated as be-
ing infinitely thin. The cart has mass
m2 . Gravity acts in the down direc-
tion. The only applied force is F , Figure 1-3: Pendulum on a cart.
which acts in the direction of z. The
cart slides on a frictionless surface, but air friction produces a damping force equal
to −bż.
The physical constants are m1 = 0.25 kg, m2 = 1.0 kg, ` = 1.0 m, g =
9.8 m/s2 , b = 0.05 Ns. The force is limited by |F | ≤ 5 N.

Examples (with solutions):

B.2 Kinetic energy. B.11 Full state feedback.

B.3 Equations of Motion. B.12 Full state with integrator.
B.4 Linearize equations of motion. B.13 Observer based control.
B.5 Transfer function model. B.14 Disturbance observer.
B.6 State space model. B.15 Frequency Response.
B.8 Successive loop closure. B.16 Loop gain.
B.9 Integrators and system type. B.17 Stability margins.
B.P.6 Root locus. B.18 Loopshaping design.
B.10 Digital PID.

TOC
CHAPTER 1. INTRODUCTION 7

C Design Study: Satellite Attitude Control

Js
✓
Rotational spring k
and damper b at joint

Jp

Figure 1-4: Satellite with flexible solar panels.

Figure 1-4 shows a simplified version of a satellite with flexible solar panels.
We will model the flexible panels using a rigid sheet with moment of inertia Jp
connected to the main satellite body by a torsional compliant element with spring
constant k and damping constant b. The moment of inertia of the satellite is Js .
The angle of the satellite body from the inertial reference is θ and the angle of the
panel from the inertial reference is denoted as φ. Thrusters are used to command
an external torque of τ about the axis of the satellite body.
The physical constants are Js = 5 kg m2 , Jp = 1 kg m2 , k = 0.15 N m,
b = 0.05 Nms. The torque is limited by |τ | ≤ 5 Nm.

Examples (with solutions):

C.2 Kinetic energy. C.10 Digital PID.

C.2 Animate the satellite. C.11 Full state feedback.
C.3 Equations of motion. C.12 Full state with integrator.
C.4 Equilibria. C.13 Observer based control.
C.5 Transfer function model. C.14 Disturbance observer.
C.6 State space model. C.15 Frequency Response.
C.8 Successive loop closure. C.16 Loop gain.
C.9 Integrators and system type. C.17 Stability margins.
C.P.6 Root locus. C.18 Loopshaping design.

TOC
 Part I
Simulation Models

To design efficient feedback control systems, we use mathematical models to

understand the system, and then we design controllers that achieve the desired
specifications. Developing suitable mathematical models is problem specific and
can be very complicated requiring specialized knowledge. However, there are
some well known techniques for certain classes of systems. This book only covers
one general technique that is useful for a large class of rigid body mechanical
systems. The technique is known as the Euler-Lagrange method and requires a
mathematical description of the kinetic and potential energy of the system, given
its (generalized) position and velocity. The theory behind Euler-Lagrange method
is deep and well beyond the scope of this book. However, we will give a brief
introduction that will enable the reader to apply Euler-Lagrange methods to a
surprisingly large class of problems.
As motivation for the Euler-Lagrange equations, consider a particle of mass
m in a gravity field, as shown in Fig. 1-5, where g is the force of gravity at sea
level, fa is an externally applied force, and the friction due to air resistance is
modeled by a gain b times the velocity ẏ. Since all of the forces are in the vertical
direction, the motion of the vehicle can be completely described by the vertical
position of particle y(t). From Newton’s law, the equations of motion are

mÿ = fa − mg − bẏ, (1.1)

where dots over the variable will be used throughout the book to denote differen-
4 dy 4 d2 y
tiation with respect to time, i.e., ẏ =
dt and ÿ =dt2 . Rearranging (1.1) gives

mÿ + mg = fa − bẏ. (1.2)

The kinetic energy for the particle is given by K(ẏ) = 12 mv 2 = 12 mẏ 2 . The po-
tential energy, which is induced by gravitational pull proportional to the distance
from the center of the earth, is given by P (y) = P0 + mgy, where P0 is the po-
tential energy of the particle when y = 0. Note that if the velocity ẏ is thought of
 y
fa

bẏ mg

x
Figure 1-5: Point mass in a gravity field.

as independent of y, then
∂K(ẏ)
= mẏ
∂ ẏ
and  
d ∂K(ẏ)
= mÿ.
dt ∂ ẏ
Also note that
∂P (y)
= mg.
∂y
Therefore, Equation (1.2) can be written as
 
d ∂K(ẏ) ∂P (y)
+ = fa − bẏ. (1.3)
dt ∂ ẏ ∂y

Equation (1.3) is a particular form of the so-called Euler-Lagrange equation. The

generalization to a more complicated system takes the form
   
d ∂K(y, ẏ) ∂P (y) ∂K(y, ẏ) ∂P (y)
− − − = fa − bẏ, (1.4)
dt ∂ ẏ ∂ ẏ ∂y ∂y

which reduces to Equation (1.3) for our particular case since K is only a function
of ẏ. To simplify the notation, we define the so-called Lagrangian as
4
L(y, ẏ) = K(y, ẏ) − P (y),

and write Equation (1.4) as

   
d ∂L(y, ẏ) ∂L(y, ẏ)
− = fa − bẏ. (1.5)
dt ∂ ẏ ∂y

The position of the system can be generalized to both positions and angles, and
the velocity can be generalized to both linear and angular velocities. Throughout
the book we will use the notation q = (q1 , q2 , . . . , qn )> to denote the general
configuration of the mechanical system that we are modeling. The vector q is
called the generalized coordinates. The kinetic energy of the system is generally
a function of both the generalized coordinates q and the generalized velocity q̇.
Therefore, we denote the kinetic energy as K(q, q̇). On the other hand, in this
book we will only consider potential fields that are conservative, and that only
depend on the configuration variable q. Under these assumptions, the general
form of the Euler-Lagrange equations is given by
 
d ∂L(q, q̇) ∂L(q)
− = τ1 − b11 q̇1 − · · · − b1n q̇n
dt ∂ q̇1 ∂q1
 
d ∂L(q, q̇) ∂L(q)
− = τ2 − b21 q̇1 − · · · − b2n q̇n (1.6)
dt ∂ q̇2 ∂q2
..
. (1.7)
 
d ∂L(q, q̇) ∂L(q)
− = τn − bn1 q̇1 − · · · − bnn q̇n ,
dt ∂ q̇n ∂qn

where τi are the generalized forces and torques that are applied to the system. Or
in vector form we can write
 
d ∂L(q, q̇) ∂L(q)
− = τ − B q̇, (1.8)
dt ∂ q̇ ∂q

where B is a matrix with positive elements and τ = (τ1 , . . . , τn )> are the gen-
eralized forces. The Euler-Lagrange equation will be discussed in more detail in
Chapter 3. However, before showing how to use Equation (1.8) to derive the equa-
tions of motion for practical systems, we need to show how to compute the kinetic
and potential energy for general (holonomic) rigid body systems.

Important Concepts:
• The equations of motion for many mechanical systems can be derived
using the Euler-Lagrange method.
• The Euler-Lagrange method utilizes the scalar quantities of kinetic
and potential energy.
• The Lagrangian is the difference between the system’s kinetic and
potential energy.
• The Euler-Lagrange method produces equations of motion that are
equivalent to those derived using Newton’s 2nd law of motion.
 Kinetic Energy of Mechanical Systems
2
Learning Objectives:
• Compute the kinetic energy for a translating and rotating point mass.
• Calculate the inertia matrix for rigid bodies.
• Derive the kinetic energy for rotating and translating rigid bodies.

2.1 Theory
Consider a point mass with mass m, traveling at velocity v, as shown in Fig. 2-1.
If the velocity is resolved with respect to a coordinate axis, then v = (vx , vy , vz )> .
The kinetic energy of the point mass is given by

1 2
K= m kvk
2
1
= mv> v
2
1
= m(vx2 + vy2 + vz2 ).
2

m v
Figure 2-1: Point mass moving at velocity v.

Now consider two point masses as shown in Fig. 2-2. The total kinetic energy

TOC
CHAPTER 2. KINETIC ENERGY 12

v1

L1
m1
L2 ✓
m2 !

v2
Figure 2-2: Two constrained point masses spinning about a point on an adjoining
axis.

of the system is given by summing the kinetic energy of each of the point masses
as

1 1
K= m1 v1> v1 + m2 v2> v2 . (2.1)
2 2

If the two masses are connected by a massless rod and the particles are spinning
about a point on the rod, as shown in Fig. 2-2, where the angular velocity vector
ω points out of the page, and the magnitude is given by θ̇, i.e., ω = (0, 0, θ̇)> ,
then from elementary physics, the velocity of point i is given by

vi = ω × pi ,

where in our case p1 = (L1 cos θ, L1 sin θ, 0)> , and p2 = (−L2 cos θ, −L2 sin θ, 0)> .
Therefore

     
0 L1 cos θ −L1 θ̇ sin θ
v1 = 0 ×  L1 sin θ  =  L1 θ̇ cos θ 
θ̇ 0 0
     
0 −L2 cos θ L2 θ̇ sin θ
v2 = 0 ×  −L2 sin θ  = −L2 θ̇ cos θ .
θ̇ 0 0

An alternative method for computing the velocities v1 and v2 is to differentiate

TOC
CHAPTER 2. KINETIC ENERGY 13

p1 and p2 with respect to time to obtain

4 dp1 d
v1 = ṗ1 = = (L1 cos θ, L1 sin θ, 0)>
dt dt



>
= −L1 dθ dθ
dt sin θ, L1 dt cos θ, 0

>
= −L1 θ̇ sin θ, L1 θ̇ cos θ, 0
4 dp2 d
v2 = ṗ2 = = (−L2 cos θ, −L2 sin θ, 0)>
dt dt



>
= L2 dθ dθ
dt sin θ, −L2 dt cos θ, 0

>
= L2 θ̇ sin θ, −L2 θ̇ cos θ, 0 .

Therefore, using Equation (2.1) the kinetic energy is

1 1
K= m1 v1> v1 + m2 v2> v2
2 2
1 h i
= m1 (−L1 θ̇ sin θ)2 + (L1 θ̇ cos θ)2
2
1 h i
+ m2 (L2 θ̇ sin θ)2 + (−L2 θ̇ cos θ)2
2
1 h i 1 h i
= m1 L21 θ̇2 sin2 θ + L21 θ̇2 cos2 θ + m2 L22 θ̇2 sin2 θ + L22 θ̇2 cos2 θ
2 2
1
= (m1 L21 + m2 L22 )θ̇2 .
2
Now consider the case where there are N point masses spinning about a gen-
eral angular velocity vector ω, where the position of the ith point mass, i =
1, . . . , N in a specified coordinate system is given by pi , as shown in Fig. 2-3.
We will see in the discussion below that it is most convenient to pick the coordi-
vi
! mi
pi
mN pN
v
vN

p1 v1
m1
Figure 2-3: Many masses in a rigid body spinning about a common axis and
moving at velocity v.

nate system to be at the center of mass. The kinetic energy of the ith particle is

TOC
CHAPTER 2. KINETIC ENERGY 14

Ki = 12 mi vi> vi . Therefore, the kinetic energy of the total system is given by

N
X 1
K= mi vi> vi .
i=1
2

Using the fact that vi = ω × pi gives

N
X 1 >
K= mi (ω × pi ) (ω × pi ) .
i=1
2

Recalling the cross product rule

(a × b)> (c × d) = (a> c)(b> d) − (a> d)(b> c)

gives

N
1X  
K= mi (ω > ω)(p> > >
i pi ) − (ω pi )(pi ω)
2 i=1
N
1X  
= mi ω > (p> >
i pi )I3 − pi pi ω
2 i=1
"N #
1 > X > >


= ω mi (pi pi )I3 − pi pi ω
2 i=1
4 1 >
= ω Jω.
2

where I3 is the 3 × 3 identity matrix. The matrix

N
X

J= mi (p> >
i pi )I3 − pi pi
i=1

is called the inertia matrix of the mass system. If the mass system is solid then
there is an infinite number of particles, and the sum becomes an integral resulting
in
Z


J= (p> p)I3 − pp> dm(p),
p∈body

where dm(p) is the elemental mass located at position p and the integral is over
all elemental masses in the system.
Suppose that p = (x, y, z)> is expressed with respect to a coordinate system

TOC
CHAPTER 2. KINETIC ENERGY 15

fixed in a rigid body. Then

    
Z "

x 1 0 0 x

#
J= x y z y  0 1 0 − y  x y z dm(x, y, z)
(x,y,z)∈body z 0 0 1 z
   2 
Z " 1 0 0 x xy xz #
= (x2 + y 2 + z 2 ) 0 1 0 − yx y 2 yz  dm(x, y, z)
(x,y,z)∈body 0 0 1 zx zy z 2
 2
y + z2

Z −xy −xz
=  −yx x2 + z 2 −yz  dm(x, y, z).
(x,y,z)∈body −zx −zy x + y2
2

As an example, to compute the inertia matrix of the thin rod of mass m about
its center of mass, as shown in Fig. 2-4, the first step is to define a coordinate
system at the center of mass, as shown in Fig. 2-4, where î and ĵ are orthogo-
nal unit vectors pointing parallel and perpendicular to the rod, respectively. The
unit vector k̂ forms a right-handed coordinate system and points out of the page.
Assuming that the rod is infinitely thin and that the mass is evenly distributed

ĵ dx dm(x)

î
x
` `
2 2
Figure 2-4: Computing the inertia of a thin rod.

throughout the rod, then the elemental mass is given by

m
dm(x) = dx.
`
Since the rod is infinitely thin in the ĵ and k̂ directions, the inertia matrix is given
by
 2 
Z `/2 Z 0 Z 0 y + z2 −xy −xz
 −yx m
J= x2 + z 2 −yz  dz dy dx
`
x=−`/2 y=0 z=0 −zx −zy x2 + y 2
 
Z 0 0 0
m `/2 
= 0 x2 0  dx
` x=−`/2
0 0 x2
 
0 0 0  
3`
0 0 0

m 0 x  2 
= 3 x=− 2` 0  = 0 m`2 0 . (2.2)
`   `
x3  2
 12
m`2
0 0 `
0 0 12
3 x=− 2

TOC
CHAPTER 2. KINETIC ENERGY 16

Note that the infinitely thin rod assumption implies that there is only inertia, or
rotational mass, about the ĵ and k̂ axes.
As another example, to compute the inertia matrix of the thin plate of mass m
about its center of mass, as shown in Fig. 2-5, the first step is to define a coordinate
system at the center of mass, as shown in Fig. 2-5, where î and ĵ are orthogonal
unit vectors pointing along the length and width of the plate, respectively, and
k̂ forms a right-handed coordinate system and points perpendicular to the plate.
Assuming that the plate is infinitely thin, and that the mass is evenly distributed

k̂ ĵ
w
î dm(x, y) y
dy
dx
`
x
Figure 2-5: Computing the inertia of a thin plate.

throughout the plate, then the elemental mass is given by

m
dm(x, y) = dx dy.
`w

Since the plate is infinitely thin in the k̂ direction, the inertia matrix is given by
 2 
Z `/2 Z w/2 Z 0 y + z2 −xy −xz
 −yx m
J= x2 + z 2 −yz  dz dy dx
x=−`/2 y=−w/2 z=0 2 2 `w
−zx −zy x +y
 2 
Z `/2 Z w/2 y −xy 0
m −yx x2
= 0  dy dx
`w x=−`/2 y=−w/2
0 0 x2 + y 2
 w3 
Z 0 0
m `/2  12  dx
= 0 x2 w 0
`w x=−`/2 2 w3
0 0 x w + 12
 3   2 
w mw
12 ` 0 0 12 0 0
m  `3   m`2 
=  0 12 w 0 = 0 12 0 .
`w `3 w3 m`2 mw2
0 0 12 w + 12 ` 0 0 12 + 12

The diagonal elements of J are called the moments of inertia and represent
the rotational mass about the î, ĵ, and k̂ axes respectively.

TOC
CHAPTER 2. KINETIC ENERGY 17

We have shown that if the motion of the system of N masses is caused purely
by spinning about the angular velocity vector ω, then the kinetic energy is given
by K = 12 ω > Jω. If the system of masses is also translating with velocity v, then
the velocity of the ith particle is given by vi = v +ω ×pi . In that case, the kinetic
energy is given by
N N
1X 1X
K= mi vi> vi = mi (v + ω × pi )> (v + ω × pi )
2 i=1 2 i=1

1X  > 
N
= mi v v + v> (ω × pi ) + (ω × pi )> v + (ω × pi )> (ω × pi )
2 i=1
N
! N
1 X > 1X
= mi v v + mi (ω × pi )> (ω × pi )
2 i=1 2 i=1
N
1X  
+ mi v> (ω × pi ) + (ω × pi )> v .
2 i=1
Using the cross product property
a> (b × c) = b> (c × a) = c> (a × b)
we get that
N
X  
mi v> (ω × pi ) + (ω × pi )> v =
i=1
XN N
X
>
( mi pi ) (v × ω) + ( mi pi )> (v × ω).
i=1 i=1

If
Pwe choose the center of the coordinate system P to be the center of mass, then
N N
i=1 m i pi = 0. Defining the total mass as m = i=1 mi , the kinetic energy is
given by
N
! N
1 X > 1X
K= mi vcm vcm + mi (ω × pi )> (ω × pi )
2 i=1 2 i=1
1 > 1
= mvcm vcm + ω > Jcm ω.
2 2
Therefore, the kinetic energy of a rigid body moving with velocity vcm and
spinning at angular velocity ω is therefore
1 > 1
K = mvcm vcm + ω > Jcm ω. (2.3)
2 2
If the system consists of n rigid bodies, then the total kinetic energy is computed
by summing the kinetic energy for each body as
Xn  
1 > 1 >
K= mj vcm,j vcm,j + ω j Jcm,j ω j .
j=1
2 2

TOC
CHAPTER 2. KINETIC ENERGY 18

2.1.1 Example: Mass Spring System

As an example, consider the mass spring system shown in Fig. 2-6 where two
masses are moving along a surface and are connected by springs with constants
k1 and k2 and dampers with constants b1 and b2 . The kinetic energy of the system
is determined by the velocity of the two masses and is therefore given by

1 1
K= m1 ż12 + m2 ż22 .
2 2

k1 k2
f
m1 m2
b1 b2

z1 z2

Figure 2-6: Mass spring system with two masses connected by springs and
dampers.

2.1.2 Example: Spinning Dumbbell

As another simple example, consider the spinning dumbbell system shown in
Fig. 2-7, where two masses are spinning about a point on the adjoining line be-
tween the masses. The angular velocity vector is pointing out of the page with
magnitude θ̇ and the dumbbell system is moving to the right at velocity ż.

v1

ĵ z `1 m1
✓
`2 ż
î m2 ✓˙

v2
Figure 2-7: Spinning dumbbell. The system is spinning about the axis pointing
out of the page and moving to the right at velocity ż.

The first step in computing the kinetic energy is to define an inertial coordinate
system as shown in Fig. 2-7 where î is a unit vector along the direction of travel, ĵ
is perpendicular to the direction of travel, and where k̂ forms a right-handed coor-
dinate system and points out of the page. In this coordinate system, the position of
the point of rotation is given by p(t) = (z(t), 0, 0)> . Accordingly, the positions

TOC
CHAPTER 2. KINETIC ENERGY 19

of m1 and m2 are given by

p1 (t) = (z(t) + `1 cos θ(t), `1 sin θ(t), 0)> ,

p2 (t) = (z(t) − `2 cos θ(t), −`2 sin θ(t), 0)> .

Since z and θ are functions of time, using the chain rule

d ∂f dg
f (g(t)) =
dt ∂x dt
and differentiating with respect to time, we get the velocities of the masses as

v1 = (ż − `1 θ̇ sin θ, `1 θ̇ cos θ, 0)> ,

v2 = (ż + `2 θ̇ sin θ, −`2 θ̇ cos θ, 0)> .

The kinetic energy is therefore given by

1 1
K= m1 v1> v1 + m2 v2> v2
2 2
1 h i
= m1 (ż − `1 θ̇ sin θ)2 + (`1 θ̇ cos θ)2
2
1 h i
+ m2 (ż + `2 θ̇ sin θ)2 + (−`2 θ̇ cos θ)2
2
1 h i
= m1 ż 2 − 2`1 ż θ̇ sin θ + `21 θ̇2 sin2 θ + `21 θ̇2 cos2 θ
2
1 h i
+ m2 ż 2 + 2`2 ż θ̇ sin θ + `22 θ̇2 sin2 θ + `22 θ̇2 cos2 θ
2
1 1
= (m1 + m2 )ż 2 + (m1 `21 + m2 `22 )θ̇2 + (m2 `2 − m1 `1 )ż θ̇ sin θ.
2 2
Note that if the pivot point is located at the center of mass, then m1 `1 = m2 `2 ,
and the kinetic energy is simply given by

1 1
K= (m1 + m2 )ż 2 + (m1 `21 + m2 `22 )θ̇2 ,
2 2
which is in the form
1 > 1
K= mvcm vcm + ω > Jω,
2 2
where the total mass is m = m1 + m2 , the velocity of the center of mass is
vcm = (ż, 0, 0)> , the angular velocity vector is ω = (0, 0, θ̇)> and the (3, 3)
element of J is given by Jz = m1 `21 + m2 `22 .

2.2 Design Study A: Single Link Robot Arm

A definition of the single link robot arm is given in Design Study A.

TOC
CHAPTER 2. KINETIC ENERGY 20

Example Problem A.2

(a) Using the configuration variable θ, write an expression for the kinetic energy
of the system.
(b) Referring to Appendices P.1, P.2, and P.3, write a Python or Matlab class,
or a Matlab function that creates an animation of the single link robot arm.
Simulate the animation to display a sinusoidal variation on the configuration
variable q = θ.

Solution

vcm
ĵ
pcm
î
`
` 2
2

Figure 2-8: Single link robot arm. Compute the velocity of the center of mass and
the angular velocity about the center of mass.

The first step is to define an inertial coordinate system as shown in Figure 2-8.
If the pivot point is the origin of the coordinate system as shown in Figure 2-8,
then the position of the center of mass is
` 
2 cos θ(t)
pcm (t) =  2` sin θ(t)  .
0
Differentiating to obtain the velocity we get
 
− sin θ
` 
vcm = θ̇ cos θ  .
2
0
The angular velocity about the center of mass is given by
 
0
ω = 0 .
θ̇
Observe that
  

Jx −Jxy −Jxz 0
ω > Jω = 0 0 θ̇ −Jxy Jy −Jyz  0 = Jz θ̇2 ,
−Jxz −Jyz Jz θ̇

TOC
 CHAPTER 2. KINETIC ENERGY 21

therefore we only need the moment of inertia about the k̂ axis Jz . From the inertia
2
matrix for a thin rod given in Equation (2.2), we have that Jz = m` 12 .
Therefore, the kinetic energy of the single link robot arm is

1 > 1
K= mvcm vcm + ω > Jcm ω
2 2
1 `2 2 1 m`2
= m θ̇ (sin2 θ + cos2 θ) + θ̇2
2 4 2 12
1 m`2 2
= θ̇ .
2 3
Throughout the book, we will demonstrate how to simulate the single link
robot arm using Python object-oriented code. The Python class that animates the
single link robot arm is shown below.
1 import sys
2 sys.path.append(’..’) # add parent directory
3 import matplotlib.pyplot as plt
4 import matplotlib.patches as mpatches
5 import numpy as np
6 import armParam as P
7
8
9 class armAnimation:
10 ’’’
11 Create arm animation
12 ’’’
13 def __init__(self):
14 # Used to indicate initialization
15 self.flagInit = True
16
17 # Initializes a figure and axes object
18 self.fig, self.ax = plt.subplots()
19
20 # Initializes a list object that will be used to
21 # contain handles to the patches and line objects.
22 self.handle = []
23
24 self.length=P.length
25 self.width=P.width
26
27 # Change the x,y axis limits
28 plt.axis([-2.0*P.length, 2.0*P.length, -2.0*P.length,
29 2.0*P.length])
30
31 # Draw a base line
32 plt.plot([0, P.length], [0, 0],’k--’)
33
34
35 # Draw pendulum is the main function that will call the
36 # functions:
37 # drawCart, drawCircle, and drawRod to create the animation.
38
39 def update(self, u):

TOC
 CHAPTER 2. KINETIC ENERGY 22

40 # Process inputs to function

41 theta = u[0] # angle of arm, rads
42
43 X = [0, self.length*np.cos(theta)] # X data points
44 Y = [0, self.length*np.sin(theta)] # Y data points
45
46 # When the class is initialized, a line object will be
47 # created and added to the axes. After initialization, the
48 # line object will only be updated.
49 if self.flagInit == True:
50 # Create the line object and append its handle
51 # to the handle list.
52 line, =self.ax.plot(X, Y, lw=5, c=’blue’)
53 self.handle.append(line)
54 self.flagInit=False
55 else:
56 self.handle[0].set_xdata(X) # Update the line
57 self.handle[0].set_ydata(Y)
58
59
60 plt.draw()

Listing 2.1: armAnimation.py

The Python code that implements the animation is given below.

1 import matplotlib.pyplot as plt
2 import numpy as np
3 import sys
4 sys.path.append(’..’) # add parent directory
5 import armParam as P
6 from signalGenerator import signalGenerator
7 from armAnimation import armAnimation
8 from dataPlotter import dataPlotter
9
10 # instantiate reference input classes
11 reference = signalGenerator(amplitude=0.5, frequency=0.1)
12 thetaRef = signalGenerator(amplitude=2.0*np.pi, frequency=0.1)
13 tauRef = signalGenerator(amplitude=5, frequency=.5)
14
15 # instantiate the simulation plots and animation
16 dataPlot = dataPlotter()
17 animation = armAnimation()
18
19 t = P.t_start # time starts at t_start
20 while t < P.t_end: # main simulation loop
21 # set variables
22 r = reference.square(t)
23 theta = thetaRef.sin(t)
24 tau = tauRef.sawtooth(t)
25 # update animation
26
27 state = np.array([[theta], [0.0]])
28 animation.update(state)
29 dataPlot.update(t, r, state, tau)
30
31 #plt.show()

TOC
 CHAPTER 2. KINETIC ENERGY 23

32 t = t + P.t_plot # advance time by t_plot

33 plt.pause(0.1)
34
35 # Keeps the program from closing until the user presses a button.
36 print(’Press key to close’)
37 plt.waitforbuttonpress()
38 plt.close()

Listing 2.2: armSim.py

Complete simulation code for Matlab, Python, and Simulink can be down-
loaded at http://controlbook.byu.edu.

2.3 Design Study B. Pendulum on Cart

A definition of the inverted pendulum is given in Design Study B.

Example Problem B.2

(a) Using the configuration variables z and θ, write an expression for the kinetic
energy of the system.

(b) Referring to Appendices P.1, P.2, and P.3, write a Python or Matlab class,
or a Matlab function that creates an animation of the inverted pendululm.
Simulate the animation to display sinusoidal variations on the configuration
variables q = (z, θ)> .

Solution

v1
ĵ
v2
î

z
Figure 2-9: Pendulum on a cart. Compute the velocity of the pendulum and of the
cart.

Define the inertial coordinate frame as in Figure 2-9, with k̂ out of the page.

TOC
 CHAPTER 2. KINETIC ENERGY 24

The position of the center of mass of the rod with mass m1 is given by
 
z(t) + 2` sin θ(t)
p1 =  2` cos θ(t)  ,
0

and the horizontal position of the cart with mass m2 is given by

 
z(t)
p2 =  0  .
0

Differentiating to obtain the velocities of m1 and m2 we obtain

   
ż + 2` θ̇ cos θ ż
v1 =  − 2` θ̇ sin θ  , v2 =  0  .
0 0

We are modeling the cart as a point mass and the rod as being infinitely thin.
Therefore the kinetic energy of the rod is the summation of its translational and
rotational kinetic energy about the center of mass. The total kinetic energy of the
system is given by
1 1 1
K= m1 v1> v1 + ω > Jrod,cm ω + m2 v2> v2
2  2 2 
1 ` ` 1 m1 `2 1
= m1 (ż + θ̇ cos θ) + (− θ̇ sin θ) + θ̇2
2 2
+ m2 ż 2
2 2 2 2 12 2
 
1 2 ` `2 2 2 `2 2 2
= m1 ż + 2 ż θ̇ cos θ + θ̇ cos θ + θ̇ sin θ
2 2 4 4
2
1 m1 ` 1
+ θ̇2 + m2 ż 2
2 12 2
1 1 `2 `
= (m1 + m2 )ż 2 + m1 θ̇2 + m1 ż θ̇ cos θ. (2.4)
2 2 3 2
Throughout the book, we will demonstrate how to simulate the pendulum on
a cart using Python code. The Python class that animates the system is shown
below.
1 import matplotlib.pyplot as plt
2 import matplotlib.patches as mpatches
3 import numpy as np
4 import pendulumParam as P
5
6 class pendulumAnimation:
7 def __init__(self):
8 self.flag_init = True # Used to indicate initialization
9 # Initialize a figure and axes object
10 self.fig, self.ax = plt.subplots()
11 # Initializes a list of objects (patches and lines)

TOC
 CHAPTER 2. KINETIC ENERGY 25

12 self.handle = []
13 # Specify the x,y axis limits
14 plt.axis([-3*P.ell, 3*P.ell, -0.1, 3*P.ell])
15 # Draw line for the ground
16 plt.plot([-2*P.ell, 2*P.ell], [0, 0], ’b--’)
17 # label axes
18 plt.xlabel(’z’)
19
20 def update(self, state):
21 z = state.item(0) # Horizontal position of cart, m
22 theta = state.item(1) # Angle of pendulum, rads
23 # draw plot elements: cart, bob, rod
24 self.draw_cart(z)
25 self.draw_bob(z, theta)
26 self.draw_rod(z, theta)
27 self.ax.axis(’equal’)
28 # Set initialization flag to False after first call
29 if self.flag_init == True:
30 self.flag_init = False
31
32 def draw_cart(self, z):
33 # specify bottom left corner of rectangle
34 x = z-P.w/2.0
35 y = P.gap
36 corner = (x, y)
37 # create rectangle on first call, update on subsequent calls
38 if self.flag_init is True:
39 # Create the Rectangle patch and append its handle
40 # to the handle list
41 self.handle.append(
42 mpatches.Rectangle(corner, P.w, P.h,
43 fc=’blue’, ec=’black’))
44 # Add the patch to the axes
45 self.ax.add_patch(self.handle[0])
46 else:
47 self.handle[0].set_xy(corner) # Update patch
48
49 def draw_bob(self, z, theta):
50 # specify center of circle
51 x = z+(P.ell+P.radius)*np.sin(theta)
52 y = P.gap+P.h+(P.ell+P.radius)*np.cos(theta)
53 center = (x, y)
54 # create circle on first call, update on subsequent calls
55 if self.flag_init is True:
56 # Create the CirclePolygon patch and append its handle
57 # to the handle list
58 self.handle.append(
59 mpatches.CirclePolygon(center, radius=P.radius,
60 resolution=15, fc=’limegreen’, ec=’black’))
61 # Add the patch to the axes
62 self.ax.add_patch(self.handle[1])
63 else:
64 self.handle[1].xy = center
65
66 def draw_rod(self, z, theta):
67 # specify x-y points of the rod
68 X = [z, z+P.ell*np.sin(theta)]

TOC
 CHAPTER 2. KINETIC ENERGY 26

69 Y = [P.gap+P.h, P.gap+P.h+P.ell*np.cos(theta)]
70 # create rod on first call, update on subsequent calls
71 if self.flag_init is True:
72 # Create the line object and append its handle
73 # to the handle list.
74 line, = self.ax.plot(X, Y, lw=1, c=’black’)
75 self.handle.append(line)
76 else:
77 self.handle[2].set_xdata(X)
78 self.handle[2].set_ydata(Y)

Listing 2.3: pendulumAnimation.py

The Python code that implements the animation functionality is given below.
1 import matplotlib.pyplot as plt
2 import numpy as np
3 import sys
4 sys.path.append(’..’) # add parent directory
5 import pendulumParam as P
6 from signalGenerator import signalGenerator
7 from pendulumAnimation import pendulumAnimation
8 from dataPlotter import dataPlotter
9
10
11 # instantiate reference input classes
12 reference = signalGenerator(amplitude=0.5, frequency=0.1)
13 zRef = signalGenerator(amplitude=0.5, frequency=0.1)
14 thetaRef = signalGenerator(amplitude=.25*np.pi, frequency=.5)
15 fRef = signalGenerator(amplitude=5, frequency=.5)
16
17 # instantiate the simulation plots and animation
18 dataPlot = dataPlotter()
19 animation = pendulumAnimation()
20
21 t = P.t_start # time starts at t_start
22 while t < P.t_end: # main simulation loop
23 # set variables
24 r = reference.square(t)
25 z = zRef.sin(t)
26 theta = thetaRef.square(t)
27 f = fRef.sawtooth(t)
28 # update animation
29 state = np.array([[z], [theta], [0.0], [0.0]])
30 animation.update(state)
31 dataPlot.update(t, r, state, f)
32
33 t = t + P.t_plot # advance time by t_plot
34 plt.pause(0.05)
35
36 # Keeps the program from closing until the user presses a button.
37 print(’Press key to close’)
38 plt.waitforbuttonpress()
39 plt.close()

Listing 2.4: pendulumSim.py

TOC
 CHAPTER 2. KINETIC ENERGY 27

Complete simulation code for Matlab, Python, and Simulink can be down-
loaded at http://controlbook.byu.edu.

2.4 Design Study C. Satellite Attitude Control

A definition of the satellite attitude control problem is given in Design Study C.

Example Problem C.2

(a) Using the configuration variables θ and φ, write an expression for the kinetic
energy of the system.

(b) Referring to Appendices P.1, P.2, and P.3, write a Python or Matlab class,
or a Matlab function that creates an animation of the satellite. Simulate
the animation to display sinusoidal variations on the configuration variables
q = (θ, φ)> .

Solution
Since the satellite consists of two rotational masses, the kinetic energy is the sum
of the rotational kinetic energy of each mass. Therefore the kinetic energy is given
by
1 1
K = Js θ̇2 + Jp φ̇2 . (2.5)
2 2
Throughout the book, we will demonstrate how to simulate the satellite using
Python code. The code that animates the satellite and implements the animation
is given below.
1 import matplotlib.pyplot as plt
2 import matplotlib.patches as mpatches
3 import numpy as np
4 import satelliteParam as P
5
6
7 class satelliteAnimation:
8 ’’’
9 Create satellite animation
10 ’’’
11 def __init__(self):
12 # Used to indicate initialization
13 self.flagInit = True
14
15 # Initializes a figure and axes object
16 self.fig, self.ax = plt.subplots()
17
18 # Initializes a list object that will be used to contain
19 # handles to the patches and line objects.
20 self.handle = []

TOC
 CHAPTER 2. KINETIC ENERGY 28

21
22 plt.axis([-2.0*P.length, 2.0*P.length, -2.0*P.length,
23 2.0*P.length])
24 plt.plot([-2.0*P.length, 2.0*P.length], [0, 0], ’b--’)
25 self.length = P.length
26 self.width = P.width
27
28 def update(self, u):
29 # Process inputs to function
30 theta = u.item(0) # Angle of base, rad
31 phi = u.item(1) # angle of panel, rad
32
33 self.drawBase(theta)
34 self.drawPanel(phi)
35
36 # This will cause the image to not distort
37 # self.ax.axis(’equal’)
38
39 # After each function has been called, initialization is
40 # over.
41 if self.flagInit == True:
42 self.flagInit = False
43
44 def drawBase(self, theta):
45 # points that define the base
46 pts =np.matrix([
47 [self.width/2.0, -self.width/2.0],
48 [self.width/2.0, -self.width/6.0],
49 [self.width/2.0 + self.width/6.0, -self.width/6.0],
50 [self.width/2.0 + self.width/6.0, self.width/6.0],
51 [self.width/2.0, self.width/6.0],
52 [self.width/2.0, self.width/2.0],
53 [-self.width/2.0, self.width/2.0],
54 [-self.width/2.0, self.width/6.0],
55 [-self.width/2.0 - self.width/6.0, self.width/6.0],
56 [- self.width/2.0 - self.width/6.0, -self.width/6.0],
57 [- self.width/2.0, -self.width/6.0],
58 [-self.width/2.0, -self.width/2.0]]).T
59 R = np.matrix([[np.cos(theta), np.sin(theta)],
60 [-np.sin(theta), np.cos(theta)]])
61 pts = R*pts
62 xy = np.array(pts.T)
63
64 # When the class is initialized, a polygon patch object
65 # will be created and added to the axes. After
66 # initialization, the polygon patch object will only be
67 # updated.
68 if self.flagInit == True:
69
70 # Create the Rectangle patch and append its handle
71 # to the handle list
72 self.handle.append(mpatches.Polygon(xy,
73 facecolor=’blue’,
74 edgecolor=’black’))
75 # Add the patch to the axes
76 self.ax.add_patch(self.handle[0])
77

TOC
 CHAPTER 2. KINETIC ENERGY 29

78 else:
79
80 # Update polygon
81 self.handle[0].set_xy(xy)
82
83 def drawPanel(self, phi):
84 # points that define the base
85 pts = np.matrix([
86 [-self.length, -self.width/6.0],
87 [self.length, -self.width/6.0],
88 [self.length, self.width/6.0],
89 [-self.length, self.width/6.0]]).T
90 R = np.matrix([[np.cos(phi), np.sin(phi)],
91 [-np.sin(phi), np.cos(phi)]])
92 pts = R * pts
93 xy = np.array(pts.T)
94
95 # When the class is initialized, a polygon patch
96 # object will be created and added to the
97 # axes. After initialization, the polygon patch
98 # object will only be updated.
99 if self.flagInit == True:
100
101 # Create the Rectangle patch and append its
102 # handle to the handle list
103 self.handle.append(mpatches.Polygon(xy,
104 facecolor=’green’,
105 edgecolor=’black’))
106
107 # Add the patch to the axes
108 self.ax.add_patch(self.handle[1])
109
110 else:
111
112 # Update polygon
113 self.handle[1].set_xy(xy)

Listing 2.5: satelliteAnimation.py

The Python code that implements the animation is given below.

1 import matplotlib.pyplot as plt
2 import numpy as np
3 import sys
4 sys.path.append(’..’) # add parent directory
5 import satelliteParam as P
6 from signalGenerator import signalGenerator
7 from satelliteAnimation import satelliteAnimation
8 from dataPlotter import dataPlotter
9
10
11 # instantiate reference input classes
12 reference = signalGenerator(amplitude=0.5, frequency=0.1)
13 thetaRef = signalGenerator(amplitude=2.0*np.pi, frequency=0.1)
14 phiRef = signalGenerator(amplitude=0.5, frequency=0.1)
15 tauRef = signalGenerator(amplitude=5, frequency=.5)
16

TOC
 CHAPTER 2. KINETIC ENERGY 30

17 # instantiate the simulation plots and animation

18 dataPlot = dataPlotter()
19 animation = satelliteAnimation()
20
21 t = P.t_start # time starts at t_start
22 while t < P.t_end: # main simulation loop
23
24 # set variables
25 r = reference.square(t)
26 theta = thetaRef.sin(t)
27 phi = phiRef.sin(t)
28 tau = tauRef.sawtooth(t)
29
30 # update animation
31 state = np.array([[theta], [phi], [0.0], [0.0]])
32 animation.update(state)
33 dataPlot.update(t, r, state, tau)
34
35 # advance time by t_plot
36 t = t + P.t_plot
37 plt.pause(0.1)
38
39 # Keeps the program from closing until the user presses a button.
40 print(’Press key to close’)
41 plt.waitforbuttonpress()
42 plt.close()

Listing 2.6: satelliteSim.py

Complete simulation code for Matlab, Python, and Simulink can be down-
loaded at http://controlbook.byu.edu.

Important Concepts:
• A rigid body’s total kinetic energy is equal to the summation of (a)
the translational velocity of its center of mass and (b) the rotational
kinetic energy spinning about its center of mass.
• The kinetic energy of a system is comprised of the summation of each
component’s kinetic energy.
• The inertia matrix is a mass-like value that plays a similar role in
rotational kinetic energy as mass does in translational kinetic energy.
• The inertia matrix for a rigid body is an integral with respect to the
elemental mass of the object.
• The diagonal elements of the inertia matrix represent the rotational
mass about the î, ĵ, and k̂ axes.

Notes and References

The inertia matrix in Equation (2.3) is computed about the center of mass of the
object. When an object is composed of several rigid bodies connected to each

TOC
CHAPTER 2. KINETIC ENERGY 31

other, where the inertia matrix may be known for each rigid body, to find the total
inertia about the center of mass, it is convenient to use the parallel axis theorem
which states that if Jcm is the inertia matrix about the center of mass, then the
inertia matrix about a point that is located at vector r from the center of mass is


J(r) = Jcm + m r> rI3 − rr> .

For the moment of inertia about a single axis, where the axis is shifted from the
center of mass by distance d, the parallel axis theorem implies that

I(d) = Icm + md2 .

TOC
CHAPTER 2. KINETIC ENERGY 32

TOC
 The Euler-Lagrange Equations
3
Learning Objectives:
• Compute the potential energy of a system due to gravity and springs
(rotational or translational).
• Identify a set of generalized coordinates and forces of a system.
• Use the potential and kinetic energy to calculate the Lagrangian of a
system.
• Derive the equations of motion for a system in terms of its general-
ized coordinates.

3.1 Theory
In this chapter we show how to define the potential energy for simple mechanical
systems and how to define the generalized forces. The Euler-Lagrange equations
given in Equation (1.8) can then be used to derive the equations of motion for the
system.

3.1.1 Potential Energy

In this book we will be concerned with two sources of potential energy: potential
energy due to gravity and potential energy due to springs. Consider a point mass
m in a gravity field that is at position y as shown in Fig. 3-1. The potential energy
is given by
P = mgy + P0 ,
where P0 is the potential energy when y = 0. Typically we desire an expression
for potential energy that is zero when the configuration variables are zero.
The second source of potential energy is from translational and rotational
springs. The spring shown in Fig. 3-2 is stretched from rest by a distance of z.

TOC
CHAPTER 3. EULER-LAGRANGE 34

gravity
y

y=0
Figure 3-1: The potential energy of a unit mass m in gravity is given by P = mgy.

k
m

z
Figure 3-2: The potential energy of a spring stretched by distance z is P = 12 kz 2 .

The potential energy of a linear stretched/compressed spring is given by

1 2
P = kz ,
2

where k is the spring constant. Similarly, for a rotational spring, the potential
energy is given by P = 21 kθ2 , where θ is the rotational angle and θ = 0 is the
angle at which the rotational spring is at rest.

3.1.2 Generalized Coordinates and Generalized Forces

The generalized coordinates are the minimum set of configuration variables. For
example, for the mass-spring-damper system shown in Fig. 2-6, the generalized
coordinates are q = (z1 , z2 )> . For the single link robot arm shown in Fig. 2-8,
the generalized coordinate is q = θ. For the spinning dumbbell system shown in
Fig. 2-7, and the pendulum on a cart system shown in Fig. 2-9, the generalized
coordinates are q = (z, θ)> . Finally, for the satellite system shown in Fig. 1-4,
the generalized coordinates are q = (θ, φ)> .
The generalized forces are the nonconservative forces and torques acting along
each generalized coordinate. In other words, they are the applied forces and
torques on the system. For example, for the mass-spring-damper system shown
in Fig. 2-6, the generalized force acting along z1 is zero and the generalized
force acting along z2 is the applied force F . Therefore the generalized force is
τ = (0, F )> .

TOC
CHAPTER 3. EULER-LAGRANGE 35

3.1.3 Damping Forces and Torques

Friction and applied dampers also exert forces on the system. Damping forces are
externally applied forces that are proportional to the velocity or angular velocity of
the system. For example, in the mass-spring-damper system shown in Fig. 2-6, the
damping force acting along z1 is due to the first damper and the damper between
the two masses and is given by −b1 ż1 − b2 (ż1 − ż2 ). The damping acting along
z2 is given by −b2 (ż2 − ż1 ). Therefore the damping force on the system is given
by
    
b + b2 −b2 ż1 −b1 ż1 − b2 (ż1 − ż2 )
−B q̇ = − 1 = .
−b2 b2 ż2 −b2 (ż2 − ż1 )

3.1.4 Euler-Lagrange Equations

The equations of motion are given by the Euler-Lagrange equations. Let the La-
grangian be given by
L(q, q̇) = K(q, q̇) − P (q),
where K(q, q̇) is the kinetic energy of the system and is, in general, a function
of the generalized coordinates and the generalized velocities, and P (q) is the
potential energy which is, in general, a function of the generalized coordinates.
As shown in Equation (1.8) the Euler-Lagrange equations are given in vector form
as  
d ∂L(q, q̇) ∂L(q)
− = τ − B q̇.
dt ∂ q̇ ∂q

3.2 Design Study A. Single Link Robot Arm

Example Problem A.3

(a) Find the potential energy for the system.

(b) Define the generalized coordinates.

(c) Find the generalized forces and damping forces.

(d) Derive the equations of motion using the Euler-Lagrange equations.

(e) Referring to Appendices P.1, P.2, and P.3, write a class or s-function that im-
plements the equations of motion. Simulate the system using a variable
torque input. The output should connect to the animation function devel-
oped in homework A.2.

TOC
CHAPTER 3. EULER-LAGRANGE 36

`
2

⌧ mg

Figure 3-3: Energy calculation for the single link robot arm

Solution
Let P0 be the potential when θ = 0. The height of the center of mass is `/2 sin θ,
which is zero when θ = 0. Accordingly, the potential energy is given by
`
P = P0 + mg sin θ.
2
The generalized coordinate is
q1 = θ.
The generalized force is
τ1 = τ,
and the generalized damping is
−B q̇ = −bθ̇.
From Homework A.1 we found that the kinetic energy is given by
 
1 m`2
K= θ̇2 .
2 3
Therefore the Lagrangian is
 
1 m`2 `
L=K −P = θ̇2 − P0 − mg sin θ.
2 3 2
For this case, the Euler-Lagrange equation is given by
 
d ∂L ∂L
− = τ1 − bθ̇
dt ∂ θ̇ ∂θ
where
   
d ∂L d m`2 m`2
= θ̇ = θ̈
dt ∂ θ̇ dt 3 3
∂L `
= −mg cos θ.
∂θ 2
Therefore, the Euler-Lagrange equation becomes
m`2 `
θ̈ + mg cos θ = τ − bθ̇. (3.1)
3 2
A Python class that implements the dynamics of the single link robot arm is
shown below.

TOC
 CHAPTER 3. EULER-LAGRANGE 37

1 import numpy as np
2 import random
3 import armParam as P
4
5
6 class armDynamics:
7 def __init__(self, alpha=0.0):
8 # Initial state conditions
9 self.state = np.array([
10 [P.theta0], # initial angle
11 [P.thetadot0]
12 ]) # initial angular rate
13
14 # Mass of the arm, kg
15 self.m = P.m * (1.+alpha*(2.*np.random.rand()-1.))
16
17 # Length of the arm, m
18 self.ell = P.ell * (1.+alpha*(2.*np.random.rand()-1.))
19
20 # Damping coefficient, Ns
21 self.b = P.b * (1.+alpha*(2.*np.random.rand()-1.))
22
23 # the gravity constant is well known, so we don’t change it.
24 self.g = P.g
25
26 # sample rate at which the dynamics are propagated
27 self.Ts = P.Ts
28 self.torque_limit = P.tau_max
29
30 def update(self, u):
31 # This is the external method that takes the input u at time
32 # t and returns the output y at time t.
33 # saturate the input torque
34 u = self.saturate(u, self.torque_limit)
35
36 self.rk4_step(u) # propagate the state by one time sample
37 y = self.h() # return the corresponding output
38
39 return y
40
41 def f(self, state, tau):
42 # Return xdot = f(x,u), the system state update equations
43 # re-label states for readability
44 theta = state.item(0)
45 thetadot = state.item(1)
46 thetaddot = (3.0/self.m/self.ell**2) * \
47 (tau - self.b*thetadot \
48 - self.m*self.g*self.ell/2.0*np.cos(theta))
49 xdot = np.array([[thetadot],
50 [thetaddot]])
51
52 return xdot
53
54 def h(self):
55 # return the output equations
56 # could also use input u if needed

TOC
 CHAPTER 3. EULER-LAGRANGE 38

57 theta = self.state.item(0)
58 y = np.array([[theta]])
59
60 return y
61
62 def rk4_step(self, u):
63 # Integrate ODE using Runge-Kutta RK4 algorithm
64 F1 = self.f(self.state, u)
65 F2 = self.f(self.state + self.Ts / 2 * F1, u)
66 F3 = self.f(self.state + self.Ts / 2 * F2, u)
67 F4 = self.f(self.state + self.Ts * F3, u)
68 self.state += self.Ts / 6 * (F1 + 2 * F2 + 2 * F3 + F4)
69
70 def saturate(self, u, limit):
71 if abs(u) > limit:
72 u = limit*np.sign(u)
73
74 return u

Listing 3.1: armDynamics.py

Code that simulates the dynamics is given below.

1 import matplotlib.pyplot as plt
2 import sys
3 sys.path.append(’..’) # add parent directory
4 import armParam as P
5 from hw2.signalGenerator import signalGenerator
6 from hw2.armAnimation import armAnimation
7 from hw2.dataPlotter import dataPlotter
8 from armDynamics import armDynamics
9
10 # instantiate arm, controller, and reference classes
11 arm = armDynamics()
12 reference = signalGenerator(amplitude=0.01, frequency=0.02)
13 torque = signalGenerator(amplitude=0.2, frequency=0.05)
14
15 # instantiate the simulation plots and animation
16 dataPlot = dataPlotter()
17 animation = armAnimation()
18
19 t = P.t_start # time starts at t_start
20 while t < P.t_end: # main simulation loop
21 # Propagate dynamics in between plot samples
22 t_next_plot = t + P.t_plot
23
24 # updates control and dynamics at faster simulation rate
25 while t < t_next_plot:
26
27 # Get referenced inputs from signal generators
28 r = reference.square(t)
29 u = torque.square(t)
30 y = arm.update(u) # Propagate the dynamics
31 t = t + P.Ts # advance time by Ts
32 # update animation and data plots
33 animation.update(arm.state)
34 dataPlot.update(t, r, arm.state, u)

TOC
 CHAPTER 3. EULER-LAGRANGE 39

35
36 # the pause causes the figure to be displayed during the
37 # simulation
38 plt.pause(0.0001)
39
40 # Keeps the program from closing until the user presses a button.
41 print(’Press key to close’)
42 plt.waitforbuttonpress()
43 plt.close()

Listing 3.2: armSim.py

Complete simulation code for Matlab, Python, and Simulink can be down-
loaded at http://controlbook.byu.edu.

3.3 Design Study B. Inverted Pendulum

Example Problem B.3

(a) Find the potential energy for the system.

(b) Define the generalized coordinates.

(c) Find the generalized forces and damping forces.

(d) Derive the equations of motion using the Euler-Lagrange equations.

(e) Referring to Appendices P.1, P.2, and P.3, write a class for s-function that
implements the equations of motion. Simulate the system using a variable
force on the cart as an input. The output should connect to the animation
function developed in homework B.2.

Solution

Figure 3-4: Pendulum on a cart.

TOC
CHAPTER 3. EULER-LAGRANGE 40

Let P0 be the potential energy when θ = 0 and z = 0. Then the potential

energy of the pendulum system is given by
`
P = P0 + m1 g (cos θ − 1),
2
where 2` cos θ is the height of the center of mass of the rod. Note that the potential
energy decreases as θ increases.
The generalized coordinates for the system are the horizontal position z and
the angle θ. Therefore, let q = (z, θ)> .
The external force acting in the direction of z is τ1 = F , and the external
torque acting in the direction of θ is τ2 = 0. Therefore, the generalized forces
are τ = (F, 0)> . A damping term acts in the direction of z, which implies that
−B q̇ = (−bż, 0)> .
Using Equation (2.4), the kinetic energy can be written in terms of the gener-
alized coordinates as
1 1 `2 `
K(q, q̇) = (m1 + m2 )ż 2 + m1 θ̇2 + m1 ż θ̇ cos θ
2 2 3 2
1 1 `2 `
= (m1 + m2 )q̇12 + m1 q̇22 + m1 q̇1 q̇2 cos q2 ,
2 2 3 2
and the potential energy can be written as
`
P (q) = P0 + m1 g (cos θ − 1)
2
`
= P0 + m1 g (cos q2 − 1).
2
The Lagrangian is therefore given by
1 1 `2 ` `
L(q, q̇) = (m1 +m2 )q̇12 + m1 q̇22 +m1 q̇1 q̇2 cos q2 −P0 −m1 g (cos q2 −1).
2 2 3 2 2
Therefore
!
∂L (m1 + m2 )ż + m1 2` θ̇ cos θ
= 2
∂ q̇ m1 `3 θ̇ + m1 2` ż cos θ
 
∂L 0
= .
∂q −m1 2` ż θ̇ sin θ + m1 g 2` sin θ
∂L
Differentiating ∂ q̇ with respect to time gives
  !
d ∂L (m1 + m2 )z̈ + m1 2` θ̈ cos θ − m1 2` θ̇2 sin θ
= 2 .
dt ∂ q̇ m1 `3 θ̈ + m1 2` z̈ cos θ − m1 2` ż θ̇ sin θ

Therefore the Euler-Lagrange equation

 
d ∂L ∂L
− = τ − B q̇
dt ∂ q̇ ∂q

TOC
 CHAPTER 3. EULER-LAGRANGE 41

gives

` `
(m1 + m2 )z̈ + m1 θ̈ cos θ − m1 θ̇2 sin θ = F − bż
2 2
`2 ` ` ` `
m1 θ̈ + m1 z̈ cos θ − m1 ż θ̇ sin θ + m1 ż θ̇ sin θ − m1 g sin θ = 0.
3 2 2 2 2
Simplifying and moving all second order derivatives to the left-hand side, and all
other terms to the right-hand side gives
!  
(m1 + m2 )z̈ + m1 2` θ̈ cos θ m1 2` θ̇2 sin θ + F − bż
2 = .
m1 `3 θ̈ + m1 2` z̈ cos θ m1 g 2` sin θ

Using matrix notation, this equation can be rearranged to isolate the second order
derivatives on the left-hand side as
    ` 2 
(m1 + m2 ) m1 2` cos θ z̈ m1 2 θ̇ sin θ + F − bż
2 = . (3.2)
m1 2` cos θ m1 `3 θ̈ m1 g 2` sin θ

Equation (3.2) represents the simulation model for the pendulum on a cart system.
A Python class that implements the dynamics of the pendulum is shown below
1 import numpy as np
2 import pendulumParam as P
3
4 class pendulumDynamics:
5 def __init__(self, alpha=0.0):
6 # Initial state conditions
7 self.state = np.array([
8 [P.z0], # z initial position
9 [P.theta0], # Theta initial orientation
10 [P.zdot0], # zdot initial velocity
11 [P.thetadot0], # Thetadot initial velocity
12 ])
13
14 # simulation time step
15 self.Ts = P.Ts
16
17 # Mass of the pendulum, kg
18 self.m1 = P.m1 * (1.+alpha*(2.*np.random.rand()-1.))
19
20 # Mass of the cart, kg
21 self.m2 = P.m2 * (1.+alpha*(2.*np.random.rand()-1.))
22
23 # Length of the rod, m
24 self.ell = P.ell * (1.+alpha*(2.*np.random.rand()-1.))
25
26 # Damping coefficient, Ns
27 self.b = P.b * (1.+alpha*(2.*np.random.rand()-1.))
28
29 # gravity constant is well known, don’t change.
30 self.g = P.g
31 self.force_limit = P.F_max

TOC
 CHAPTER 3. EULER-LAGRANGE 42

32
33 def update(self, u):
34 # This is the external method that takes the input u at time
35 # t and returns the output y at time t.
36 # saturate the input force
37 u = self.saturate(u, self.force_limit)
38
39 self.rk4_step(u) # propagate the state by one time sample
40 y = self.h() # return the corresponding output
41
42 return y
43
44 def f(self, state, u):
45 # Return xdot = f(x,u)
46 z = state.item(0)
47 theta = state.item(1)
48 zdot = state.item(2)
49 thetadot = state.item(3)
50 F = u
51
52 # The equations of motion.
53 M = np.array([[self.m1 + self.m2,
54 self.m1 * (self.ell/2.0) * np.cos(theta)],
55 [self.m1 * (self.ell/2.0) * np.cos(theta),
56 self.m1 * (self.ell**2/3.0)]])
57 C = np.array([[self.m1 * (self.ell/2.0)
58 * thetadot**2 * np.sin(theta)
59 + F - self.b*zdot],
60 [self.m1 * self.g * (self.ell/2.0)
61 * np.sin(theta)]])
62 tmp = np.linalg.inv(M) @ C
63 zddot = tmp.item(0)
64 thetaddot = tmp.item(1)
65
66 # build xdot and return
67 xdot = np.array([[zdot], [thetadot], [zddot], [thetaddot]])
68
69 return xdot
70
71 def h(self):
72 # return y = h(x)
73 z = self.state.item(0)
74 theta = self.state.item(1)
75 y = np.array([[z],[theta]])
76
77 return y
78
79 def rk4_step(self, u):
80 # Integrate ODE using Runge-Kutta RK4 algorithm
81 F1 = self.f(self.state, u)
82 F2 = self.f(self.state + self.Ts / 2 * F1, u)
83 F3 = self.f(self.state + self.Ts / 2 * F2, u)
84 F4 = self.f(self.state + self.Ts * F3, u)
85 self.state += self.Ts / 6 * (F1 + 2 * F2 + 2 * F3 + F4)
86
87 def saturate(self, u, limit):
88 if abs(u) > limit:

TOC
 CHAPTER 3. EULER-LAGRANGE 43

89 u = limit*np.sign(u)
90
91 return u

Listing 3.3: pendulumDynamics.py

The Python code that simulates the dynamics is given below.

1 import matplotlib.pyplot as plt

2 import sys
3 sys.path.append(’..’) # add parent directory
4 import pendulumParam as P
5 from hw2.signalGenerator import signalGenerator
6 from hw2.pendulumAnimation import pendulumAnimation
7 from hw2.dataPlotter import dataPlotter
8 from pendulumDynamics import pendulumDynamics
9
10 # instantiate pendulum, controller, and reference classes
11 pendulum = pendulumDynamics(alpha=0.0)
12 reference = signalGenerator(amplitude=0.5, frequency=0.02)
13 force = signalGenerator(amplitude=1, frequency=1)
14
15 # instantiate the simulation plots and animation
16 dataPlot = dataPlotter()
17 animation = pendulumAnimation()
18
19 t = P.t_start # time starts at t_start
20 while t < P.t_end: # main simulation loop
21
22 # Propagate dynamics at rate Ts
23 t_next_plot = t + P.t_plot
24 while t < t_next_plot:
25 r = reference.square(t)
26 u = force.sin(t)
27 y = pendulum.update(u) # Propagate the dynamics
28 t = t + P.Ts # advance time by Ts
29
30 # update animation and data plots at rate t_plot
31 animation.update(pendulum.state)
32 dataPlot.update(t, r, pendulum.state, u)
33
34 # the pause causes figure to be displayed during simulation
35 plt.pause(0.0001)
36
37 # Keeps the program from closing until the user presses a button.
38 print(’Press key to close’)
39 plt.waitforbuttonpress()
40 plt.close()

Listing 3.4: pendulumSim.py

Complete simulation code for Matlab, Python, and Simulink can be down-
loaded at http://controlbook.byu.edu.

TOC
CHAPTER 3. EULER-LAGRANGE 44

3.4 Design Study C. Satellite Attitude Control

Example Problem C.3

(a) Find the potential energy for the system.

(b) Define the generalized coordinates.

(c) Find the generalized forces and damping forces.

(d) Derive the equations of motion using the Euler-Lagrange equations.

(e) Referring to Appendices P.1, P.2, and P.3, write a class or s-function that im-
plements the equations of motion. Simulate the system using a variable
torque on the body as an input. The output should connect to the animation
function developed in homework C.2.

Solution

Js
✓
Rotational spring k
and damper b at joint

Jp

Figure 3-5: Satellite with flexible solar panels.

The generalized coordinates for the satellite attitude control problem are the
angles θ and φ. Therefore, we define the generalized coordinates as q = (θ, φ)> .
We will assume the ability to apply torque on the main body (θ) of the satellite,
but not on the solar panel. Therefore, the generalized force is τ = (τ, 0)> . We
will model the dissipation (friction) forces as proportional to the relative motion
between the main body and the solar panel. Therefore, the damping term is given
by
    
−b(θ̇ − φ̇) b −b θ̇
−B q̇ = =− .
−b(φ̇ − θ̇) −b b φ̇

TOC
CHAPTER 3. EULER-LAGRANGE 45

Using Equation (2.5), the kinetic energy can be written in terms of the gener-
alized coordinates as
1 1
K(q, q̇) = Js θ̇2 + Jp φ̇2
2 2
1 1
= Js q̇1 + Jp q̇22 .
2
2 2
The potential energy of the system is due to the spring force between the base and
the solar panel, and is given by
1
P (q) = k(φ − θ)2
2
1
= k(q2 − q1 )2 ,
2
where k is the spring constant. The Lagrangian is therefore given by
1 1 1
L(q, q̇) = Js q̇12 + Jp q̇22 − k(q2 − q1 )2 .
2 2 2
Therefore
 
∂L Js θ̇
=
∂ q̇ Jp φ̇
 
∂L k(φ − θ)
= .
∂q −k(φ − θ)
∂L
Differentiating ∂ q̇ with respect to time gives
   
d ∂L Js θ̈
= .
dt ∂ q̇ Jp φ̈
Therefore the Euler-Lagrange equation
 
d ∂L ∂L
− = τ − B q̇
dt ∂ q̇ ∂q
gives      
Js θ̈ − k(φ − θ) τ −b(θ̇ − φ̇)
= + .
Jp φ̈ + k(φ − θ) 0 −b(φ̇ − θ̇)
Simplifying and moving all second order derivatives to the left-hand side, and all
other terms to the right-hand side gives
   
Js θ̈ τ − b(θ̇ − φ̇) − k(θ − φ)
= .
Jp φ̈ −b(φ̇ − θ̇) − k(φ − θ)
Using matrix notation, this equation can be rearranged to isolate the second order
derivatives on the left-hand side as
    
Js 0 θ̈ τ − b(θ̇ − φ̇) − k(θ − φ)
= . (3.3)
0 Jp φ̈ −b(φ̇ − θ̇) − k(φ − θ)

TOC
 CHAPTER 3. EULER-LAGRANGE 46

Equation (3.3) represents the simulation model for the simplified satellite system.
A Python class that implements the dynamics of the satellite is shown below
1 import numpy as np
2 import random
3 import satelliteParam as P
4
5 class satelliteDynamics:
6 def __init__(self, alpha=0.0):
7 # Initial state conditions
8 self.state = np.array([
9 [P.theta0], # initial base angle
10 [P.phi0], # initial panel angle
11 [P.thetadot0], # initial angular velocity of base
12 [P.phidot0], # initial angular velocity of panel
13 ])
14
15 # simulation time step
16 self.Ts = P.Ts
17
18 # inertia of base
19 self.Js = P.Js * (1.+alpha*(2.*np.random.rand()-1.))
20
21 # inertia of panel
22 self.Jp = P.Jp * (1.+alpha*(2.*np.random.rand()-1.))
23
24 # spring coefficient
25 self.k = P.k * (1.+alpha*(2.*np.random.rand()-1.))
26
27 # Damping coefficient, Ns
28 self.b = P.b * (1.+alpha*(2.*np.random.rand()-1.))
29 self.torque_limit = P.tau_max
30
31 def update(self, u):
32 # This is the external method that takes the input u at time
33 # t and returns the output y at time t.
34 # saturate the input torque
35 u = self.saturate(u, self.torque_limit)
36
37 self.rk4_step(u) # propagate the state by one time sample
38 y = self.h() # return the corresponding output
39
40 return y
41
42 def f(self, state, u):
43 # Return xdot = f(x,u)
44 theta = state.item(0)
45 phi = state.item(1)
46 thetadot = state.item(2)
47 phidot = state.item(3)
48 tau = u
49 # The equations of motion.
50 M = np.array([[self.Js, 0],
51 [0, self.Jp]])
52 C = np.array([[tau -
53 self.b*(thetadot-phidot)-self.k*(theta-phi)],
54 [-self.b*(phidot-thetadot)-self.k*(phi-theta)

TOC
 CHAPTER 3. EULER-LAGRANGE 47

55 ]])
56 tmp = np.linalg.inv(M) @ C
57 thetaddot = tmp.item(0)
58 phiddot = tmp.item(1)
59
60 # build xdot and return
61 xdot = np.array([[thetadot], [phidot], [thetaddot],
62 [phiddot]])
63 return xdot
64
65 def h(self):
66 # return y = h(x)
67 theta = self.state.item(0)
68 phi = self.state.item(1)
69 y = np.array([[theta], [phi]])
70
71 return y
72
73 def rk4_step(self, u):
74 # Integrate ODE using Runge-Kutta RK4 algorithm
75 F1 = self.f(self.state, u)
76 F2 = self.f(self.state + self.Ts / 2 * F1, u)
77 F3 = self.f(self.state + self.Ts / 2 * F2, u)
78 F4 = self.f(self.state + self.Ts * F3, u)
79 self.state += self.Ts / 6 * (F1 + 2 * F2 + 2 * F3 + F4)
80
81 def saturate(self, u, limit):
82 if abs(u) > limit:
83 u = limit*np.sign(u)
84 return u

Listing 3.5: pendulumDynamics.py

Code that simulates the dynamics is given below.

1 import matplotlib.pyplot as plt
2 import sys
3 sys.path.append(’..’) # add parent directory
4 import satelliteParam as P
5 from hw2.signalGenerator import signalGenerator
6 from hw2.satelliteAnimation import satelliteAnimation
7 from hw2.dataPlotter import dataPlotter
8 from satelliteDynamics import satelliteDynamics
9
10 # instantiate satellite, controller, and reference classes
11 satellite = satelliteDynamics()
12 reference = signalGenerator(amplitude=0.5, frequency=0.1)
13 torque = signalGenerator(amplitude=0.1, frequency=0.1)
14
15 # instantiate the simulation plots and animation
16 dataPlot = dataPlotter()
17 animation = satelliteAnimation()
18
19 t = P.t_start # time starts at t_start
20 while t < P.t_end: # main simulation loop
21
22 # Propagate dynamics in between plot samples

TOC
 CHAPTER 3. EULER-LAGRANGE 48

23 t_next_plot = t + P.t_plot
24
25 # updates control and dynamics at faster simulation rate
26 while t < t_next_plot:
27 r = reference.square(t)
28 u = torque.sin(t)
29 y = satellite.update(u) # Propagate the dynamics
30 t = t + P.Ts # advance time by Ts
31
32 # update animation and data plots
33 animation.update(satellite.state)
34 dataPlot.update(t, r, satellite.state, u)
35
36 # the pause causes the figure to be displayed during the
37 # simulation
38 plt.pause(0.0001)
39
40 # Keeps the program from closing until the user presses a button.
41 print(’Press key to close’)
42 plt.waitforbuttonpress()
43 plt.close()

Listing 3.6: pendulumSim.py

Complete simulation code for Matlab, Python, and Simulink can be down-
loaded at http://controlbook.byu.edu.

Important Concepts:
• Generalized coordinates are the minimum number of independent co-
ordinates that define the system’s configuration.
• Generalized forces are nonconservative forces and torques that act on
generalized coordinates.
• Damping forces/torques are proportional to the linear/angular veloc-
ity of the generalized coordinates.
• The Lagrangian is defined as the difference between a system’s ki-
netic and potential energy.
• A system’s equations of motion can be derived using the Euler-
Lagrange equation.

Notes and References

Derivation of the Lagrange-Euler equations can be found in many sources includ-
ing [1, 2]. A simple derivation using only calculus can be found in [3].

TOC
 Part II
Design Models

The equations of motion that we derived in the previous section are often too
complicated to facilitate effective engineering design. Accordingly, the standard
approach is to simplify the models into so-called design models that capture the
essential features of the system. The design techniques studied in this class will
require that the design models are linear and time-invariant. In addition, the PID
method described in Part III will require that the design models are second-order
systems.
Our approach to developing linear time-invariant design models will be to lin-
earize the simulation models developed in Part I and then to convert the linearized
models into two standard forms, namely
• Transfer function models and
• State space models.

Both forms will have advantages and disadvantages that will be explained through-
out the remaining parts of the book. In Chapter 4 we will define the important
concept of equilibrium and show how the equations of motion can be linearized
about the equilibrium. Chapter 5 will show how to transform the linearized equa-
tions of motion into transfer function models, and Chapter 6 will show how to put
the equations of motion in state space form.
The remaining parts of the book will use the design models to develop feed-
back controllers for the system of interest. In Part III we will describe the
Proportional-Integral-Derivative (PID) controllers based on second-order transfer
function models. Part IV derives observer based controllers using state space mod-
els. Finally, Part V describes loopshaping design strategies using general transfer
function models.
CHAPTER 3. EULER-LAGRANGE 50

TOC
 Equilibria and Linearization
4
Learning Objectives:
• Identify the equilibrium point(s) of a dynamical system.
• Compute the Jacobian linearization of a system about its equilibrium
point(s).
• Feedback linearize a dynamical system.

4.1 Theory
In this chapter we will describe two methods for linearizing the system, namely
Jacobian linearization and feedback linearization. Jacobian linearization is based
on the idea of approximating the nonlinear terms in the equations of motion by the
first two terms in their Taylor series expansion. Feedback linearization is based
on the idea of using the input signal to artificially remove the nonlinear terms.

4.1.1 Jacobian Linearization

If the function g : R → R is a nonlinear function that can be differentiated an
infinite number of times, then its Taylor series expansion about the point w0 is
given by
 
∂g  1 ∂ 2 g 
g(w) = g(w0 ) + (w − w ) + (w − w0 )2 + · · · .
∂w w0 2! ∂w2 w0
0

A pictorial representation of Taylor series expansion is shown in Fig. 4-1, where

it can be seen that in a small neighborhood around w = w0 , the function g(w) is

TOC
CHAPTER 4. LINEARIZATION 52

well approximated by the first two terms in the Taylor series expansion, namely

∂g 
g(w) ≈ g(w0 ) + (w − w0 ).
∂w w0

@g
Step Response
1.4 (w w0 )
@w w0
1.2

g(w0 ) 1

0.8
Amplitude

0.6

0.4

0.2

0
0 2 4 6 8
w
0
10
Time (seconds)
12 14 16 18

Figure 4-1: Taylor series expansion of g(w) about w = w0 .

If g is a scalar function of several variables, then the Taylor series about w1 =

w10 , w2 = w20 , · · · , wm = wm0 is given by

∂g 
g(w1 , w2 , . . . , wm ) = g(w10 , w20 , . . . , wm0 )+ (w1 −w10 )
∂w1 w10 ,w20 ,...,wm0
 
∂g  ∂g 
+ (w2 −w20 )+· · ·+ (wm −wm0 )+· · · .
∂w2 w10 ,w20 ,...,wm0 ∂wm w10 ,w20 ,...,wm0
(4.1)

For example, given the function g(v, w) = v cos w, the constant and linear terms
in the Taylor series, expanded around v0 and w0 are
 
∂g  ∂g 
g(v, w) ≈ g(v0 , w0 ) + (v − v0 ) + (w − w0 )
∂v v0 ,w0 ∂w v0 ,w0
= v0 cos w0 + (cos w)|v0 ,w0 (v − v0 ) + (−v sin w)|v0 ,w0 (w − w0 )
= v0 cos w0 + (cos w0 )(v − v0 ) − (v0 sin w0 )(w − w0 ).

Since approximating a nonlinear function by the first two terms in its Taylor
series is only a good approximation in a small neighborhood of w0 , it is important
to select w0 judiciously. In particular, we will be interested in linearizing about
equilibrium points of differential equations.
Definition. Given the differential equation ẋ = f (x, u), the pair (xe , ue ) is an
equilibrium point if
f (xe , ue ) = 0.

TOC
CHAPTER 4. LINEARIZATION 53

In other words, an equilibrium point is a state-input pair where there is not any
motion in the system.
For example, suppose that the nonlinear differential equation of a second-order
system is given by
ÿ + aẏ + by + g(y, ẏ) = u, (4.2)
where a and b are constants and g(y, ẏ) is a known nonlinear function of y and ẏ,
and u is the input signal. Defining x = (y, ẏ)> , we get
   
ẏ ẏ 4
ẋ = = = f (x, u).
ÿ u − aẏ − by − g(y, ẏ)
The equilibrium is when f (xe , ue ) = 0 or in other words, when
ye = anything , ẏe = 0, ue = bye + g(ye , 0).
Equivalently, at equilibrium there is no motion in the system, which implies that
ÿe = ẏe = 0, which from Equation (4.2) implies that ue = bye + g(ye , 0).
Jacobian linearization then proceeds by replacing each term in the nonlinear
differential equations describing the system by the first two terms in the Taylor’s
series expansion about the equilibrium point.
For example, since the first term in Equation (4.2) is already linear, it can be
expanded exactly as
∂ ÿ  ¨
ÿ = ÿe +  (ÿ − ÿe ) = ỹ,
∂ ÿ e
4
where we have defined the linearized quantity ỹ = y − ye , and we have used the
fact that ÿe = 0. Similarly, the remaining terms in Equation (4.2) are linearized as
∂ ẏ 
aẏ = aẏe + a  (ẏ − ẏe ) = aỹ˙
∂ ẏ e
∂y 
by = bye + b  (y − ye ) = bye + bỹ
∂y e
∂g  ∂g 
g(y, ẏ) ≈ g(ye , ẏe ) +  (y − ye ) +  (ẏ − ẏe )
∂y e ∂ ẏ e
∂g  ∂g  ˙
= g(ye , 0) +  ỹ +  ỹ
∂y e ∂ ẏ e
∂u 
u = ue +  (u − ue ) = ue + ũ.
∂u e
Substituting these expressions into Equation (4.2) the resulting linearized equa-
tions of motion are
   
˙ + [bye + bỹ] + g(ye , 0) + ∂g  ỹ + ∂g  ỹ˙ = [ue + ũ]
¨ + [aỹ]
[ỹ]
∂y e ∂ ẏ e
Using the equilibrium ue = bye + g(ye , 0) gives
   
¨ ∂g  ˙ ∂g 
ỹ + a +  ỹ + b +  ỹ = ũ.
∂ ẏ e ∂y e

TOC
CHAPTER 4. LINEARIZATION 54

A block diagram that shows the design model for Equation (4.2) using Jacobian
linearization is shown in Fig. 4-2.
ye

bye + g(ye , 0)

ue ỹ
u y
ũ ÿ = u aẏ by g(y, ẏ)

modeled⇣as ⌘ ⇣ ⌘
ỹ¨ = ũ a+ @g
@ ẏ ỹ˙ b+ @g
@y ỹ
e e

Figure 4-2: Linear model using Jacobian linearization.

4.1.2 Feedback Linearization

It is often the case that the nonlinearities in the system can be directly canceled by
a judicious choice for the input variable. Consider again the nonlinear equations
of motion in Equation (4.2). If u is selected as

u = g(y, ẏ) + ũ

then the resulting equation of motion is

ÿ + aẏ + by = ũ,

which is linear. This technique is called feedback linearization because the feed-
back signal u has been used to linearize the system by directly canceling the non-
linearities. The advantage of this method is that the equations of motion are still
globally defined and are not restricted to a small neighborhood of an equilibrium
point. The disadvantage is that g(y, ẏ) may not be precisely known, and therefore
may not be completely canceled by u. A block diagram that shows the design
model for Equation (4.2) using feedback linearization is shown in Fig. 4-3.

4.2 Design Study A. Single Link Robot Arm

Example Problem A.4

For the single link robot arm:

(a) Find the equilibria of the system.

TOC
CHAPTER 4. LINEARIZATION 55

g(y, ẏ)

ũ u y
ÿ = u aẏ by g(y, ẏ)

modeled as
ÿ = ũ aẏ by

Figure 4-3: Linear model using feedback linearization.

(b) Linearize the system about the equilibria using Jacobian linearization.

(c) Linearize the system using feedback linearization.

Solution
The differential equation describing the single link robot arm derived in HW A.3
is
m`2 mg`
θ̈ + cos θ = τ − bθ̇. (4.3)
3 2

Defining x = (θ, θ̇), and u = τ we get

   
θ̇ θ̇ 4
ẋ = = 3 3b 3g = f (x, u).
θ̈ m`2 τ − m` θ̇ −
2 2` cos θ

The equilibrium is when f (xe , ue ) = 0, or in other words, when

mg`
θe = anything , θ̇e = 0, τe = cos θe . (4.4)
2

Equivalently, at equilibrium there is no motion in the system, which implies that

θ̈e = θ̇e = 0, which from Equation (4.3) implies that τe = mg` 2 cos θe .
Therefore, any pair (θe , τe ) satisfying Equation (4.4) is an equilibria. Jaco-
bian linearization then proceeds by replacing each term in the nonlinear differen-
tial equations describing the system by the first two terms in the Taylor’s series
4 ˙ 4
expansion about the equilibrium point. Defining θ̃ = θ − θe , θ̃ = θ̇ − θ̇e = θ̇,
¨
θ̃ = θ̈ − θ̈e = θ̈, and τ̃ = τ − τe , each term in Equation (4.3) can be expanded

TOC
CHAPTER 4. LINEARIZATION 56

about the equilibrium as follows:


m`2 m`2 m`2 ∂ θ̈  m`2 ¨
θ̈ = θ̈e +  (θ̈ − θ̈e ) = θ̃,
3 3 3 ∂ θ̈  3
e
mg` mg` mg` ∂ 

cos θ ≈ cos θe + (cos θ) (θ − θe )
2 2 2 ∂θ θe
mg` mg`
= cos θe − sin θe θ̃
2 2
∂τ 
τ = τe +  (τ − τe ) = τe + τ̃
∂τ e
∂ θ̇  ˙
bθ̇ = bθ̇e + b  (θ̇ − θ̇e ) = bθ̃.
∂ θ̇ e
Substituting into Equation (4.3) gives
m`2 h ¨
i mg` h i h
˙
i
θ̈e + θ̃ + cos θe − sin θe θ̃ = [τe + τ̃ ] − b θ̇e + θ̃ .
3 2
Simplifying this expression using Equation (4.4) gives
m`2 ¨ mg` ˙
θ̃ − (sin θe )θ̃ = τ̃ − bθ̃, (4.5)
3 2
which are the linearized equations of motion using Jacobian linearization.
Feedback linearization proceeds by using the feedback linearizing control
mg`
τ= cos θ + τ̃ (4.6)
2
in Equation (4.3) to obtain the feedback linearized equations of motion
m`2
θ̈ = τ̃ − bθ̇, (4.7)
3
where we emphasize that the equations of motion are valid for any θ and θ̇ and
not just in a small region about an equilibrium. It is interesting to compare Equa-
tion (4.6) to the control signal using Jacobian linearization which is
mg`
τ = τe + τ̃ = cos θe + τ̃ . (4.8)
2
The control signal in (4.8) uses the equilibrium angle θe whereas the control signal
in (4.6) uses the actual angle θ. For the single link robot arm we will use the design
model obtained using feedback linearization.

4.3 Design Study B. Inverted Pendulum

Example Problem B.4

For the inverted pendulum,

TOC
CHAPTER 4. LINEARIZATION 57

(a) Find the equilibria of the system.

(b) Linearize the system about the equilibria using Jacobian linearization.

Note that for the inverted pendulum, since the nonlinearities are not all contained
in the same channel as the control force F , the system cannot be feedback lin-
earized.

Solution

The differential equations describing the inverted pendulum derived in HW B.3

are

` `
(m1 + m2 )z̈ + m1 θ̈ cos θ = m1 θ̇2 sin θ − bż + F (4.9)
2 2
` `2 `
m1 z̈ cos θ + m1 θ̈ = m1 g sin θ.
2 3 2

The equilibria values (ze , θe , Fe ) are found by setting ż = z̈ = θ̇ = θ̈ = 0 in

Equation (4.9) to obtain

Fe = 0 (4.10)
`
m1 g sin θe = 0. (4.11)
2

Therefore, any triple (ze , θe , Fe ) satisfying Equations (4.10) and (4.11) is an equi-
libria, or in other words ze can be any value, Fe = 0 and θe = kπ, where k is an
integer.

TOC
CHAPTER 4. LINEARIZATION 58

To linearize around (ze , θe , Fe ) where k is an even integer, note that

∂  ∂ 
 
θ̈ cos θ ≈ θ̈e cos θe + (θ̈ cos θ) (θ − θe ) + (θ̈ cos θ) (θ̈ − θ̈e )
∂θ (θ̈e ,θe ) ∂ θ̈ (θ̈e ,θe )
¨
= θ̈e cos θe − (θ̈e sin θe )θ̃ + (cos θe )θ̃
¨
= θ̃
∂ 2  ∂ ˙2 
 
θ̇2 sin θ ≈ θ̇e2 sin θe + (θ̇ sin θ) (θ − θe ) + (θ sin θ) (θ̇ − θ̇e )
∂θ (θe ,θ̇e ) ∂ θ̇ (θe ,θ̇e )
˙
= θ̇e2 sin θe + θ̇e2 cos θe θ̃ + 2θ̇e sin θe θ̃
= 0,
∂  ∂ 
 
z̈ cos θ ≈ z̈e cos θe + (z̈ cos θ) (θ − θe ) + (z̈ cos θ) (z̈ − z̈e )
∂θ (z̈e ,θe ) ∂ z̈ (z̈e ,θe )

= z̈e cos θe − (z̈e sin θe )θ̃ + (cos θe )z̃¨

= z̃¨
∂ 

sin θ ≈ sin θe + (sin θ) (θ − θe )
∂θ θe

= sin θe + (cos θe )θ̃

= θ̃,
4 4 4
where we have defined θ̃ = θ − θe and z̃ = z − ze . Also defining F̃ = F − Fe ,
and noting that
θ = θe + θ̃ = θ̃
˙ ˙
θ̇ = θ̇e + θ̃ = θ̃
z = ze + z̃
ż = że + z̃˙ = z̃˙
z̈ = z̈e + z̃¨ = z̃¨
F = Fe + F̃ = F̃ ,
we can write Equation (4.9) in its linearized form as

¨ + m1 ` [θ̃]
(m1 + m2 )[z̈e + z̃]
¨ ` ˙ + [Fe + F̃ ]
= m1 [0] − b[że + z̃]
2 2
` ¨ `2 ¨ `
m1 [z̃] + m1 [θ̈e + θ̃] = m1 g [θ̃],
2 3 2
which simplifies to
  ¨!  
(m1 + m2 ) m1 2` z̃ −bz̃˙ + F̃
2
¨ = , (4.12)
m1 2` m1 `3 θ̃ m1 g 2` θ̃

which are the linearized equations of motion.

TOC
CHAPTER 4. LINEARIZATION 59

4.4 Design Study C. Satellite Attitude Control

Example Problem C.4

For the satellite system:

(a) Find the equilibria of the system.

Solution

The differential equations describing the motion of the satellite system derived in
HW C.3 are

Js θ̈ + b(θ̇ − φ̇) + k(θ − φ) = τ (4.13)

Jp φ̈ + b(φ̇ − θ̇) + k(φ − θ) = 0. (4.14)

Defining x = (θ, φ, θ̇, φ̇), and u = τ we get

   
θ̇ θ̇
φ̇  φ̇ 
ẋ =  = 4
= f (x, u).
 θ̈  − b (θ̇ − φ̇) − k (θ − φ) + 1 τ 

Js Js Js 
φ̈ − Jbp (φ̇ − θ̇) − Jkp (φ − θ)

The equilibrium is when f (xe , ue ) = 0, or in other words when

θe = φe = anything , θ̇e = φ̇e = 0, τe = 0. (4.15)

The equations of motion for this system are linear and do not require lineariza-
tion. With that said, the information about the system equilibria can be helpful for
designing the control system. In this case, understanding that the commanded an-
gular position of the satellite and the panel should be the same to keep the system
in equilibrium will influence our control design.

TOC
CHAPTER 4. LINEARIZATION 60

Important Concepts:
• The equilibrium point of a differential equation is where it is station-
ary, i.e. f (xe , ue ) = 0, where ẋ = f (x, u).
• Jacobian linearization uses the first two terms of a Taylor series ex-
pansion to create a linear representation of a system about its equi-
librium point.
• Feedback linearization uses the input or control variable u to directly
cancel out nonlinear terms in the differential equations.
• Feedback linearization is valid globally, unlike Jacobian linearization
which is only accurate in a region near the equilibrium point.
• The disadvantages of feedback linearization include (a) it assumes
perfect knowledge of the terms that are being canceled out, and (b) it
is not always feasible or may require a transformation of the system
before being able to cancel out all the nonlinearities.

Notes and References

Jacobian linearization is a standard technique that is well documented in most
introductory textbooks on control. For example, see [4, 5, 6, 7]. Feedback lin-
earization is a much deeper topic than can be covered in an introductory textbook
on control. A discussion that is similar to ours, that assumes that the nonlinearity
is in the input channel, is contained in [5]. Feedback linearization is a fairly stan-
dard technique for fully actuated mechanical systems modeled by Euler-Lagrange
equations, where the model of the system can be reduced to
M (q)q̈ + c(q, q̇)q̇ + g(q) = τ,
where q ∈ Rm is the generalized coordinate and τ ∈ Rm is the generalized force,
and where M (q) is an invertible matrix. In that case we can write
q̈ = M −1 (q) (τ − c(q, q̇)q̇ − g(q)) .
Selecting τ as
τ = c(q, q̇)q̇ + g(q) + M (q)ν, (4.16)
results in the linear system
q̈ = ν,
where the new control variable ν can be designed to stabilize the system. The
control law in Equation (4.16) is the feedback linearizing control. This is a stan-
dard technique for robot manipulators and is often called the method of computed
torque [8, 9, 10].
When the nonlinearity is not in the input channel, the system can still often be
feedback linearized through a suitable change of variables. A good introduction to
the topic is contained in [11]. More advanced coverage of the topic can be found
in [12, 13, 14, 15].

TOC
 Transfer Function Models
5
Learning Objectives:
• Compute the transfer function of dynamical systems using the
Laplace transform.
• Approximate high-order transfer functions with a cascade of lower-
order transfer functions.

5.1 Theory

The (one-sided) Laplace transform of a time domain signal y(t) is defined as

Z ∞
4
L{y(t)} = Y (s) = y(t)e−st dt.
0

In essence, the Laplace transform indicates the content of the complex signal e−st
that is contained in y(t). The Laplace transform of the time derivative of y(t) is
given by
Z ∞
L{ẏ} = ẏe−st dt.
0

R R
Using integration by parts udv = uv − vdu, where u = e−st and dv = ẏdt,
and assuming that limt→∞ y(t)e−st = 0, or that s is in the region of convergence

TOC
CHAPTER 5. TRANSFER FUNCTIONS 62

of the Laplace transform, we get

Z ∞
L{ẏ} = ẏe−st dt
0
∞ Z ∞


= y(t)e−st  − y(t) −se−st dt
0 0
Z ∞
−st
= lim y(t)e − y(0) + s y(t)e−st dt
t→∞ 0
= sY (s) − y(0).

Similarly, the Laplace transform of the second derivative of y(t) is given by

L{ÿ} = sL{ẏ} − ẏ(0)

= s (sY (s) − y(0)) − ẏ(0)
= s2 Y (s) − sy(0) − ẏ(0).

The transfer function is typically defined as the relationship between the input
and the output of the system. In that setting the transfer function is found when
all initial conditions are set to zero. This is important to remember in the context
of modeling. We will see that state space models explicitly account for initial
conditions, but that transfer functions do not.
Given the nth order differential equation

y (n) + an−1 y (n−1) + · · · + a1 ẏ + a0 y

= bm u(m) + bm−1 u(m−1) + · · · + b1 u̇ + b0 u,

the transfer function from u to y is found by taking the Laplace transform of both
sides of the equation and setting all initial conditions to zero to obtain

sn Y (s) + an−1 sn−1 Y (s) + · · · + a1 sY (s) + a0 Y (s)

= bm sm U (s) + bm−1 sm−1 U (s) + · · · + b1 sU (s) + b0 U (s).

Factoring Y (s) and U (s) we get

sn + an−1 sn−1 + · · · + a1 s + a0 Y (s)


= bm sm + bm−1 sm−1 + · · · + b1 s + b0 U (s).

Solving for Y (s) gives

 
bm sm + bm−1 sm−1 + · · · + b1 s + b0
Y (s) = U (s),
sn + an−1 sn−1 + · · · + a1 s + a0
where
bm sm + bm−1 sm−1 + · · · + b1 s + b0
H(s) =
sn + an−1 sn−1 + · · · + a1 s + a0

TOC
CHAPTER 5. TRANSFER FUNCTIONS 63

is the transfer function from u to y.

Given the transfer function H(s), the poles of the transfer function are the
roots of the denominator polynomial

sn + an−1 sn−1 + · · · + a1 s + a0 = 0.

Similarly, the zeros of the transfer function are the roots of the numerator polyno-
mial
bm sm + bm−1 sm−1 + · · · + b1 s + b0 = 0.
As a concrete example, consider the differential equation given by

y (3) + 2ÿ + 3ẏ + 4y = 5u̇ + 6u.

Taking the Laplace transform of both sides, setting the initial conditions to zero,
and factoring Y (s) and U (s) gives

(s3 + 2s2 + 3s + 4)Y (s) = (5s + 6)U (s).

Solving for Y (s) gives

5s + 6
Y (s) = U (s).
s3 + 2s2 + 3s + 4
The transfer function from u to y is therefore
5s + 6
H(s) = .
s3 + 2s2 + 3s + 4
The poles of the transfer function are the roots of s3 + 2s2 + 3s + 4 = 0 or
p1 = −1.6506, p2 = −0.1747 + j1.5469, and p3 = −0.1747 − j1.5469. The
zeros of the transfer function are the roots of 5s + 6 = 0 or z1 = −6/5.

5.2 Design Study A. Single Link Robot Arm

Example Problem A.5

For the single link robot arm, find the transfer function of the system from the
torque τ to the angle θ.

Solution
As shown in Section 4.2, the feedback linearized model for the single link robot
arm is given by Equation (4.7) as

m`2
θ̈ = τ̃ − bθ̇. (5.1)
3

TOC
CHAPTER 5. TRANSFER FUNCTIONS 64

Taking the Laplace transform of Equation (5.1) and setting all initial conditions to
zero we get
m`2 2
s Θ(s) + bsΘ(s) = τ̃ (s).
3
Solving for Θ(s) gives
!
1
Θ(s) = m`2 τ̃ (s).
2
3 s + bs

The canonical form for transfer functions is for the leading coefficient in the de-
nominator polynomial to be unity. This is called monic form. Putting the transfer
function in monic form results in
!
3
m`2
Θ(s) = 3b
τ̃ (s), (5.2)
s2 + m` 2s

where the expression in the parenthesis is the transfer function from τ̃ to θ, where
τ̃ indicates that we are working with the feedback linearized control in Equa-
tion (4.6). The block diagram associated with Equation (5.2) is shown in Fig. 5-1.

⌧˜(s)
<latexit sha1_base64="q8ZezRutJMSaNrbHzHDxyunGTsk=">AAAEY3icfZPdbtMwFIC9tcDo+NnG7hBSRIU0JDQ1aPxJgCbBBZdDWrehppoc56Sx5p9gO12rkKfgFh6MB+A9OEnHlrmII0U6Od9nOz6O41xw6waDXyurne6Nm7fWbvfW79y9d39jc+vI6sIwGDIttDmJqQXBFQwddwJOcgNUxgKO47MPNT+egrFcq0M3z2Es6UTxlDPqsPQlOszA0R379HSjP9gdNBEsJ+FF0t/fTps4ON3sfIwSzQoJyjFBrR2Fg9yNS2ocZwKqXlRYyCk7oxMYYaqoBDsumy+ugidYSYJUG3yUC5pqe0RJpbVzGaMpqcusz+riv9iocOnrcclVXjhQbLFQWojA6aDefpBwA8yJOSaUGY7fGrCMGsocNql3fRkx0WhkkrNnlznuS8E501JSlZSR0kZWo3BcRgJSF33rh5HhkwwzT6SxvfIuNd/iudHJlff2r/fe8+oE+96Y9Rt2r4xFAVU/rCrchYG27WDmznmCHSvfcOVNlcdpVUZ1I+O0zCuPJpDCV+TWYVMMCJzMcKomeMDlO1/O2lNlPpVtKn06bdPp0tiiwbEWSf1XaFGXfAmEWLKwtqS1VwKfqjZVPjVtanw6a9PZ/w/iRX0QGHjvQv+WLSdHz3fDl7t7n/EC7pFFrJGH5DHZISF5RfbJJ3JAhoQRSb6TH+Rn53d3vbvV3V6oqysXYx6Qa9F99AcXv4sH</latexit>

3 ⇥(s)
m`2
3b
s2 + m` 2s

Figure 5-1: A block diagram of the single link robot arm.

5.3 SIMO Systems and Cascade Approximations

There are many systems that have multiple inputs and/or multiple outputs. In
this case, a transfer function exists from each input to each output. For example,
suppose that the system is given by the coupled differential equations

ÿ1 + a11 ẏ1 + ÿ2 + a12 ẏ2 = b11 u1 + b12 u2 + b13 u̇2
ÿ2 + a22 y2 = b21 u2 .

Taking the Laplace transform and arranging in matrix notation gives

 2     
s + a11 s s2 + a12 s Y1 (s) b11 (b13 s + b12 ) U1 (s)
= .
0 s2 + a22 Y2 (s) 0 b21 U2 (s)

To find the relevant transfer functions, we need to isolate the outputs on the left-
hand side by inverting the matrix on the left. Using the inverse formula for a 2 × 2

TOC
CHAPTER 5. TRANSFER FUNCTIONS 65

matrix derived in Appendix P.7

 
d −b
 −1
a b −c a
=
c d ad − bc
we get
−1 
s2 + a11 s s2 + a12 s
    
Y1 b11 b13 s + b12 U1
=
Y2 0 s2 + a22 0 b21 U2
s2 + a22 −s2 − a12 s
 

0 s2 + a11 s
  
b11 b13 s + b12 U1
=
(s + a11 s)(s2 + a22 )
2 0 b21 U2
b11 (s2 + a22 ) (s2 + a22 )(b13 s + b12 ) − b21 (s2 + a12 s)
 
2
0 b21 (s + a11 s)
 
U1
=
(s2 + a11 s)(s2 + a22 ) U2
3 2 ! 
b11 b13 s +(b12 −b21 )s +(b13 a22 −b21 a12 )s+a22 b12
s2 +a11 s (s2 +a11 s)(s2 +a22 ) U1
= b21
.
0 2
U2
s +a22

b11
Therefore, the transfer function from u1 to y1 is s2 +a11 s , the transfer function
b13 s3 +(b12 −b21 )s2 +(b13 a22 −b21 a12 )s+a22 b12
from u2 to y1 is (s2 +a11 s)(s2 +a22 ) , the transfer function
from u1 to y2 is 0, and the transfer function from u2 to y2 is s2b+a
21
22
.
For many of the case studies that we will look at in this book, there is a sin-
gle input, but we are also interested in controlling two or more measurements or
outputs. State-space control techniques are able to create control laws to control
multiple outputs in a single design step, as we will see in later chapters. However,
we also want to be able to create single-input, multiple-output controllers using
more classical transfer-function based approaches. Doing so will require more
intuition about the systems we are controlling, as well as a clear understanding
of our control objectives so that we can make appropriate design decisions and
simplifying assumptions.
The majority of our design studies (specifically studies B, C, E, and F) have
coupled, higher-order dynamics (greater than order two) with multiple outputs.
Instead of coming up with a single feedback loop to control the multiple outputs
of interest, we will form a cascade of two lower-order transfer functions and de-
sign two independent control loops. Forming the cascade of lower-order transfer
functions requires insights into the dynamic behavior of the system, because it
may require simplifying assumptions about the dynamic coupling between the
two subsystems. Instead of a two-way coupling, the cascade may make the dy-
namic coupling one-way. To reduce the complexity of the system, we may make
simplifying assumptions about the dynamics of one of the subsystems. It is essen-
tial for these simplifications to be justifiable given the parameters of the system
and our control objectives. For example, in design study C, we form a cascade
of two transfer functions: the first is from the torque input to the satellite angular
position and the second is from the satellite angular position to the solar panel

TOC
CHAPTER 5. TRANSFER FUNCTIONS 66

angular position. Because the satellite is more massive than the solar panels, we
will make a valid assumption that while the satellite significantly influences the
dynamic behavior of the solar panels, the solar panels do not significantly influ-
ence the dynamic behavior of the satellite. As we will see, this will simplify the
design of the control system significantly and result in a viable control design that
provides good performance.
In forming cascades of transfer functions, other factors may influence the form
of the cascade. Our control priorities may play a significant role. For example,
in design study B, the balancing of the inverted pendulum is the highest priority
and therefore, we will use the force input to control the angle of the pendulum. Of
importance, but lower priority, is controlling the cart. These considerations will
lead us to form the cascade using the transfer functions from force to pendulum
angle and from pendulum angle to cart position. In other situations, the causality
of the dynamic behavior is more transparent. In the ball-beam system, the force
on the beam causes the beam angle to change and the change in beam angle causes
the ball to roll and translate. We will form the transfer function cascade for the
ball-beam system accordingly. In the planar VTOL aircraft system, torque from
the differential thrust causes the aircraft to pitch, while thrust combined with non-
zero pitch causes the aircraft to translate. It is natural, therefore, to think of torque
causing pitch, and pitch causing translational motion. We will thus form a cascade
of two transfer functions: from torque to pitch angle and from pitch angle to
horizontal position.
In forming a cascade of two (or more) transfer functions to represent the dy-
namics of a single system, it is critically important that we consider the physics
of the system and our control objectives as we make simplifying assumptions.
Important questions to consider include:

• What is the control input to the system?

• What outputs of the system are to be controlled?

• What outputs can be measured?

• What physically justifiable simplifications can be made about the dynamic

coupling between subsystems?

• What physically justifiable simplifying assumptions can be made about the

dynamics of the system?

Asking and answering these questions is fundamental to the design of successful

control systems. The more insight you have into the physical behavior of the
system you are controlling, the better your control system designs will be.

TOC
CHAPTER 5. TRANSFER FUNCTIONS 67

5.4 Design Study B. Inverted Pendulum

Example Problem B.5

For the inverted pendulum:
(a) Start with the linearized equations and use the Laplace transform to convert
the equations of motion to the s-domain.
(b) Find the transfer functions from the input F̃ (s) to the outputs Z̃(s) and Θ̃(s).
Assume that the damping constant is negligible. How does this simplify the
transfer functions? Is this a reasonable assumption?
(c) From the simplified Z̃(s)/F̃ (s) and Θ̃(s)/F̃ (s) transfer functions, compute
the Z̃(s)/Θ̃(s) transfer function. Draw a block diagram of the system as a
series of two transfer functions from F̃ (s) to Θ̃(s) and from Θ̃(s) to Z̃(s).
(d) Describe how this transfer-function cascade makes sense physically. How
does force influence the pendulum angle? How does the pendulum angle
influence the cart position?

Solution
From HW B.4, the linearized equations of motion are given by
  ¨!  
(m1 + m2 ) m1 2` z̃ −bz̃˙ + F̃
2
¨ = .
m1 2` m1 `3 θ̃ m1 g 2` θ̃

The second equation in this matrix formulation can be simplified by dividing both
sides of the second equation by m1 2` to give
  ¨!  
(m1 + m2 ) m1 2` z̃ −bz̃˙ + F̃
2` ¨ = .
1 3 θ̃ g θ̃

We can write these equations with states and state derivatives on the left and inputs
on the right as
`¨
(m1 + m2 )z̃¨ + m1 θ̃ + bz̃˙ = F̃
2
2` ¨
z̃¨ + θ̃ − g θ̃ = 0.
3
Taking the Laplace transform gives
  `
(m1 + m2 )s2 + bs Z̃(s) + m1 s2 Θ̃(s) = F̃ (s)
 2 
2 2` 2
s Z̃(s) + s − g Θ̃(s) = 0.
3

TOC
CHAPTER 5. TRANSFER FUNCTIONS 68

These equations can be expressed in matrix form as

    
(m1 + m2 )s2 + bs m1 2` s2 Z̃(s) F̃ (s)
2` 2 = .
s2 3 s −g Θ̃(s) 0

Inverting the matrix on the left hand side and solving for the transfer functions
gives
!
2` 2
3 s −g
Z̃(s) = F̃ (s)
(m1 6` + m2 2` 4 2` 3 2
3 )s + b 3 s − (m1 + m2 )gs − bgs
!
−s2
Θ̃(s) = F̃ (s).
(m1 6` + m2 2` 4 2` 3 2
3 )s + b 3 s − (m1 + m2 )gs − bgs

If we make the assumption that b ≈ 0, then these transfer functions will sim-
plify further and be even easier to work with from a control design perspective.
This is a reasonable and conservative assumption because the damping force bż is
small relative to the other forces acting on the system. Furthermore, by underesti-
mating the damping in the system, our control design will be conservative because
it assumes there is no damping provided by the physics of the system and thus all
the damping in the system must come from the feedback control. Assuming b = 0,
we get
!
2` 2
3 s −g
Z̃(s) =   F̃ (s)
s2 (m1 6` + m2 2` 2
3 )s − (m1 + m2 )g
!
−1
Θ̃(s) = F̃ (s).
(m1 6` + m2 2` 2
3 )s − (m1 + m2 )g

From a controls perspective, we will be interested in how the pendulum angle

θ influences the cart position z, and so we want to find the transfer function from
Θ̃(s) to Z̃(s). This can be easily calculated by recognizing that

Z̃(s) F̃ (s)
Z̃(s) = Θ̃(s).
F̃ (s) Θ̃(s)

Accordingly, !
− 2` 2
3 s +g
Z̃(s) = 2
Θ̃(s).
s
The block diagram for the approximate system is shown in Figure 5-2.
This transfer function cascade makes sense physically. With the pendulum
balanced vertically, a positive force on the cart would cause the pendulum to fall
in the negative direction as indicated by the minus sign in the numerator. The
pendulum falls in an unstable motion due to the right-half-plane pole in the trans-
fer function. The angle of the rod influences the position of the cart as shown

TOC
CHAPTER 5. TRANSFER FUNCTIONS 69

Figure 5-2: The inverted pendulum dynamics are approximated by a cascade of

fast and slow subsystems. The fast subsystem is the transfer function from the
force to the angle, and the slow subsystem is the transfer function from the angle
to the position.

in the second transfer function. If the pendulum were balanced vertically with
no input force applied and a small disturbance caused the pendulum to fall in the
positive direction, the cart would shoot off in the negative direction. Again, this
can be seen from the negative sign in the numerator of the transfer function and
the right-half-plane pole in the denominator. Keep in mind that these equations
are linearized about the vertical position of the pendulum. When the pendulum is
hanging down, the equations are different. How are they different?
The PID and loopshaping techniques that we will discuss in Parts III and V
will use the approximate transfer function models of the plant derived above. The
state space methods discussed in Part IV will use a state space model, which does
not depend on neglecting the coupling between subsystems and does not depend
on a separation into fast and slow subsystems.

5.5 Design Study C. Satellite Attitude Control

Example Problem C.5

For the satellite attitude control problem:

(a) Start with the linearized equations for the satellite attitude problem and use
the Laplace transform to convert the equations of motion to the s-domain.

(b) Find the full transfer matrix from the input τ (s) to the outputs Φ(s) and Θ(s).

(c) From the transfer matrix, find the second-order transfer function from Θ(s) to
Φ(s).

(d) Under the assumption that the panel moment of inertia Jp is significantly
smaller than the spacecraft moment of inertia Js (specifically, (Js +Jp )/Js ≈
1), find the second-order approximation for the transfer function from τ (s)
to Θ(s).

TOC
CHAPTER 5. TRANSFER FUNCTIONS 70

(e) From your results on parts (c) and (d), form the approximate transfer func-
tion cascade for the satellite/panel system and justify why it makes sense
physically.

Solution
From HW C.3, the linearized equations of motion are given by
  !
θ̈ − Jbs (θ̇ − φ̇) − Jks (θ − φ) + J1s τ
=
φ̈ − Jbp (φ̇ − θ̇) − Jkp (φ − θ)

or in other words, the coupled differential equations

b k b k 1
θ̈ + θ̇ + θ = φ̇ + φ + τ
Js Js Js Js Js
b k b k
φ̈ + φ̇ + φ = θ̇ + θ.
Jp Jp Jp Jp

Taking the Laplace transform with initial conditions set to zero and rearranging
gives

b k b k 1
(s2 + s+ )Θ(s) = ( s + )Φ(s) + τ (s) (5.3)
Js Js Js Js Js
b k b k
(s2 + s + )Φ(s) = ( s + )Θ(s). (5.4)
Jp Jp Jp Jp

To find the transfer matrix from τ to (Θ, Φ)> , write Equation (5.3) and (5.4)
in matrix form as
!  1
s2 + Jbs s + Jks − Jbs s − Jks Θ(s)
= Js τ (s),
− Jbp s − Jkp s2 + Jbp s + Jkp Φ(s) 0

and invert the matrix on the left hand side to obtain

 1 2 b k

J s + J J s+ Js Jp
   s b(J +Js )p
 s2 s2 + Jss Jp p s+ k(JJss+J p) 

Θ(s)
=  b k
Jp  τ (s).

Φ(s) 
Js Jp s+ Js Jp

b(Js +Jp ) k(J +J )
s2 s2 + Js Jp s+ Jss Jp p

By dividing the bottom transfer function of the transfer matrix Φ(s)/τ (s) by
the top transfer function of the transfer matrix Θ(s)/τ (s), we can find the transfer
function from the satellite angular position to the panel angular position
b k
Φ(s) J s + Jp
= 2 p b . (5.5)
Θ(s) s + Jp s + Jkp

TOC
CHAPTER 5. TRANSFER FUNCTIONS 71

From the transfer matrix, the transfer function from τ (s) to Θ(s) is given by

1 2 b k
Θ(s) J s + Js Jp s + Js Jp
=  s  (5.6)
τ (s) b(J +J )
s2 s2 + Jss Jp p s +
k(Js +Jp )
Js Jp

Under the assumption that the moment of inertia of the panel is significantly
smaller than the inertia of the satellite (which is true for this problem), we can
infer that (Js + Jp )/Js ≈ 1. Taking this into account, we can simplify Θ(s)/τ (s)
as

 
1 2 b k
Θ(s) Js s + Jp s + Jp
=   
τ (s) J +J
s2 sJs p Js 2 b k
Js +Jp s + Jp s + Jp
1
≈ .
(Js + Jp )s2

The block diagram for the approximate system is shown in Figure 5-3

b k
⌧ (s) 1 ⇥(s) Jp s + Jp (s)
(Js + Jp )s2 s2 + Jbp s + Jkp

satellite subsystem solar panel subsystem

Figure 5-3: The satellite attitude dynamics are approximated by a cascade of the
satellite and panel subsystems.

The cascade approximation has the exact transfer function for the panel sub-
system, but it has an approximate transfer function for the satellite subsystem.
The approximation implies that the dynamics of the satellite affects the dynam-
ics of the panel, but the dynamics of the panel do not affect the dynamics of the
satellite. Notice that the transfer function for the satellite has the dynamics of a
rigid body with a moment of inertia of Js + Jp . As long as the moment of iner-
tia of the satellite is significantly greater than the moment of inertia of the panel,
this assumption is reasonable and approximates the fully coupled dynamics of the
system with acceptable accuracy.

TOC
CHAPTER 5. TRANSFER FUNCTIONS 72

Important Concepts:
• A transfer function captures the relationship between the input and
output of the system in the Laplace domain.
• Transfer functions assume that all the initial conditions are set to zero.
• The roots of the transfer function’s denominator polynomial are the
poles of the dynamical system.
• The roots of the transfer function’s numerator polynomial are the ze-
ros of the dynamical system.
• For multiple-input and/or multiple-output systems, there will be a
matrix of transfer functions representing the relationship between ev-
ery input-output pairing.
• Some higher-order systems may be decoupled and represented as a
cascade of two or more lower-order transfer functions.

Notes and References

The Laplace transform can be found in introductory textbooks on signals and sys-
tems like [16] and in introductory textbooks on differential equations. The transfer
function of a linear time-invariant system is also covered in introductory textbooks
on signals and systems, and in most introductory textbooks on control like [4].

TOC
 State Space Models
6
Learning Objectives:
• Use Jacobian linearization to put dynamical systems into a state space
model.
• Convert state space models to their transfer function representation.

6.1 Theory

The transfer function models discussed in the previous chapter are a frequency
domain representation of the system because they describe how the system re-
sponds to sinusoidal inputs at specific frequencies. In contrast, differential equa-
tion models are time-domain representations of the systems that directly model
the time-evolution of the system. The state space model is also a time-domain
representation that is essentially a reformatting of the differential equations. In
essence, the state space model represents the system by an input, an output, and
a memory element, called the state. In this book, the input will always be de-
noted as u(t), the output as y(t), and the state as x(t). The state represents all
of the memory elements in the system. For example, the state may represent a
storage register, the altitude of an aircraft, the velocity of a car, the voltage across
a capacitor, or the current through an inductor.
The state space model is composed of two equations. The first equation is
called the state evolution equation and represents how the memory elements, or
states, change as a function of the current state and the inputs to the system. The
second equation is called the output equation and describes how the current output
of the system depends on the current state and the current input. The general state

TOC
CHAPTER 6. STATE SPACE MODELS 74

space equations for a continuous time system are written as

ẋ = f (x, u) (6.1)
y = h(x, u), (6.2)
where Equation (6.1) is the state evolution equation and Equation (6.2) is the out-
put equation. Note that for continuous time systems the state evolution equation is
a system of coupled first order nonlinear differential equations. The general state
space equations for a discrete time system are written as
x[k + 1] = f (x[k], u[k]) (6.3)
y[k] = h(x[k], u[k]), (6.4)
where again Equation (6.3) is the state evolution equation, this time given by a
system of coupled first order difference equations, and Equation (6.4) is the output
equation.
Any nonlinear nth order differential equation can be transformed into state
space form. For example, consider the nonlinear differential equation
...
y + y 2 ÿ + sin(y ẏ) + ey = u,

where u is the input and y is the output. Define the state as

   
x1 y
4
x = x2  = ẏ  .
x3 ÿ
Then the state evolution equation can be derived as
     
ẋ1 ẏ ẏ
ẋ = ẋ2  = ...ÿ  =  ÿ 
y 2 y
ẋ3 −y ÿ − sin(y ẏ) − e + u
 
x2
4
= x3 = f (x, u),
−x21 x3 − sin(x1 x2 ) − ex1 + u
and the output equation is
4
y = x1 = h(x, u).
As another example, consider the coupled differential equations
ÿ1 + ẏ1 y2 + y2 = u
ÿ2 + ẏ2 cos(y1 ) + y˙1 = 0.

Defining the output as y = (y1 , y2 )> and the state as

   
x1 y1
x2  4 y2 
x=   
x3  = ẏ1  ,
x4 ẏ2

TOC
CHAPTER 6. STATE SPACE MODELS 75

the state evolution equation can be derived as

     
ẋ1 ẏ1 ẏ1
ẋ2  ẏ2   ẏ2 
ẋ =     
ẋ3  = ÿ1  =  −ẏ1 y2 − y2 + u 


ẋ4 ÿ2 −ẏ2 cos(y1 ) − y˙1

 
x3
 x4 4
=  = f (x, u),
−x3 x2 − x2 + u 
−x4 cos(x1 ) − x3

and the output equation is given by

 
x1 4
y= = h(x, u).
x2

6.1.1 Jacobian Linearization Revisited

Jacobian linearization of the system is more straightforward when the equations

are in state space form. Given the nonlinear state space equations

ẋ = f (x, u) (6.5)
y = h(x, u) (6.6)

an equilibrium point is defined as any (xe , ue ) such that f (xe , ue ) = 0. In other

words, equilibrium points are combinations of states and inputs where the state
stops evolving in time. To linearize about an equilibrium point, Equations (6.5)
and (6.6) can be expanded in a Taylor series about (xe , ue ) as

 
∂f  ∂f 
f (x, u) ≈ f (xe , ue ) + (x − xe ) + (u − ue ) + H.O.T. (6.7)
∂x e ∂u e
 
∂h  ∂h 
h(x, u) ≈ h(xe , ue ) + (x − xe ) + (u − ue ) + H.O.T., (6.8)
∂x e ∂u e

TOC
CHAPTER 6. STATE SPACE MODELS 76

where the Jacobian matrices are defined as

 ∂f1 ∂f1 
∂x1 ... ∂xn
∂f 4  . .. 
=  .. . 
∂x ∂f ∂fn
∂x1
n
... ∂xn
 ∂f1 ∂f1 
∂u1 ... ∂um
∂f 4  . .. 
=  .. . 
∂u ∂f ∂fn
∂u1
n
... ∂um
 ∂h1 ∂h1 
∂x1 ... ∂xn
∂h 4  . .. 
=  .. . 
∂x ∂h ∂hp
p
∂x1 ... ∂xn
 ∂h1 ∂h1 
∂u1 ... ∂um
∂h 4  . .. 
=  .. . .
∂u ∂h ∂hp
p
∂u1 ... ∂um

Therefore ∂f
∂x is an n × n matrix,
∂f
∂u is an n × m matrix, ∂h
∂x is a p × n matrix,
∂h
and ∂u is a p × m matrix.
 
4  ∂f 
Defining x̃ = x − xe and ũ = u − ue and letting A = ∂f ∂x  and B = ∂u  ,
e e
and noting that x̃˙ = ẋ − ẋe = ẋ, results in the linearized state evolution equation

x̃˙ = Ax̃ + B ũ.

At the equilibria, the output may not necessarily be zeros. If we define the equal-
ibrium output to be ye = h(xe , ue ) and the linearized output as ỹ = y − ye , and
4  4 
define C = ∂h  and D = ∂h  , then using Equation (6.8) in (6.6) gives
∂x e ∂u e

ỹ = y − ye
≈ h(xe , ue ) + C x̃ + Dũ − h(xe , ue )
= C x̃ + Dũ.

The linearized state space equations are therefore

x̃˙ = Ax̃ + B ũ
ỹ = C x̃ + Dũ.

TOC
CHAPTER 6. STATE SPACE MODELS 77

6.1.2 Converting State Space Models to Transfer Function Mod-

els
Linear state space models can be converted to transfer function models using the
following technique. Suppose that the linear state space model is given by

ẋ = Ax + Bu (6.9)
y = Cx + Du. (6.10)

Defining the Laplace transform of a vector to be the Laplace transform of each

element, i.e.,    
 x1 
  L {x1 }
   
L  ...  =  ...  ,

 

xn L {xn }
and noting that the Laplace transform is a linear operator, which implies that

L{Ax} = AL{x},

taking the Laplace transform of Equations (6.9) and (6.10), and setting the initial
condition to zeros, gives

sX(s) = AX(s) + BU (s) (6.11)

Y (s) = CX(s) + DU (s). (6.12)

Solving (6.11) for X(s) gives

sX(s) − AX(s) = BU (s)

=⇒ (sI − A)X(s) = BU (s)
=⇒ X(s) = (sI − A)−1 BU (s), (6.13)

where I is the n × n identity matrix. Substituting (6.13) into (6.12) gives

Y (s) = C(sI − A)−1 BU (s) + DU (s)

 
=⇒ Y (s) = C(sI − A)−1 B + D U (s).

The transfer function from u to y is therefore

H(s) = C(sI − A)−1 B + D. (6.14)

As an example, suppose that the state space equations are given by

   
0 1 0
ẋ = x+ u
−4 −3 2


y = 1 0 x + (0)u,

TOC
CHAPTER 6. STATE SPACE MODELS 78

then the transfer function from u to y is given by

H(s) = C(sI − A)−1 B + D

    −1  

1 0 0 1 0
= 1 0 s − +0
0 1 −4 −3 2
   

s −1 −1 0
= 1 0
4 s+3 2
 
s+3 1
 

−4 s 0
= 1 0 2
s + 3s + 4 2

 
s+3 1 0
= 2
s + 3s + 4 2
2
= 2 .
s + 3s + 4

The form of the transfer function in Equation (6.14) allows us to derive a

simple relationship between the poles of the transfer function H(s) and the eigen-
values of A. Recall (see Appendix P.7) that for any square invertible matrix M ,
we have that
adj(M )
M −1 = ,
det(M )
where adj(M ) is the adjugate of M defined as the transpose of the cofactors of
M , and det(M ) is the determinant of M . Therefore

H(s) = C(sI − A)−1 B + D

Cadj(sI − A)B
= +D
det(sI − A)
Cadj(sI − A)B + Ddet(sI − A)
= .
det(sI − A)

This expression implies that the zeros of H(s) are given by the roots of the poly-
nomial
Cadj(sI − A)B + Ddet(sI − A) = 0,
and that the poles of H(s) are given by the roots of the polynomial

det(sI − A) = 0. (6.15)

Recall from linear algebra (see Appendix P.7) that Equation (6.15) also defines the
eigenvalues of A. Therefore, the poles of H(s) are equivalent to the eigenvalues
of A.
In Chapters 11 and 13 we will show two general techniques for converting
SISO transfer function models into state space models.

TOC
CHAPTER 6. STATE SPACE MODELS 79

6.2 Design Study A. Single Link Robot Arm

Example Problem A.6

For the feedback linearized equations of motion for the single link robot arm given
in Equation (4.7), define the states as x = (θ, θ̇)> , the input as ũ = τ̃ , and the
measured output as y = θ. Find the linear state space equations in the form

ẋ = Ax + B ũ
y = Cx + Dũ.

Solution
The linear state space equations can be derived in two different ways: (1) directly
from the linearized equations of motion and (2) by linearizing the nonlinear equa-
tions of motion.
Starting with the feedback linearized equation in Equation (4.7) and solving
for θ̈ gives
3 3b
θ̈ = 2
τ̃ − θ̇.
m` m`2
Therefore,
       
4 ẋ1 θ̇ θ̇ x2
ẋ = = = 3 3b = 3 3b
m`2 ũ − m`2 x2
ẋ2 θ̈ 2 τ̃ − 2 θ̇
   m`  m` 
0 1 x1 0
= 3b + 3 ũ.
0 − m` 2 x2 m`2

Assuming that the measured output of the system is y = θ, the linearized output
is given by  

x1
y = θ = x1 = 1 0 + (0)ũ.
x2
Therefore, the linearized state space equations are given by
   
0 1 0
ẋ = 3b x + 3 ũ
0 − m` 2 m`2


y = 1 0 x. (6.16)

Alternatively, the state space equations can be found directly from the nonlin-
ear equations of motion given in Equation (3.1), by forming the nonlinear state
space model as
     
4 ẋ1 θ̇ θ̇ 4
ẋ = = = 3 3b 3g = f (x, u).
ẋ2 θ̈ m` 2 τ − m` 2 θ̇ − 2` cos θ

TOC
CHAPTER 6. STATE SPACE MODELS 80

Evaluating the Jacobians at the equilibrium (θe , θ̇e , τe ) = (0, 0, mg`

2 ) gives
!  
∂f  ∂f1
∂θ
∂f1
∂ θ̇

 0 1 
A=  = ∂f2 ∂f2  = 3g 3b 
∂x e ∂θ e 2` sin θ − m` 2 e
∂ θ̇
 
0 1
= 3b
0 − m` 2

  ∂f1    
∂f  ∂τ  0 
B=  = ∂f2  = 3 
∂u e e m`2 e
  ∂τ
0
= 3 .
m`2
4
Similarly, the output is given by y = θ = x1 = h(x, u), which implies that
∂h  
∂h1 


C=  = ∂h ∂θ
1
∂ θ̇
 = 1 0 
∂x e
e e
= 1 0
∂h 
D=  = 0.
∂u e
The linearized state space equations are therefore given by
   
0
˙x̃ = 3g sin 1 0
θe 3b x̃ + 3 ũ
2` − m` 2 m`2


ỹ = 1 0 x̃,
which is similar to Equation (6.16) but with linearized state and output.
The transfer function can be found from the linearized state space equations
using Equation (6.14) as
H(s) = C(sI − A)−1 B + D
    −1  

1 0 0 1 0
= 1 0 s − 3b 3
0 1 0 − m` 2 m`2
 −1  

s −1 0
= 1 0 3b 3
0 s + m` 2 m`2
   

s + 3b2 1 0
1 0 m`
3
0 s m`2
= 3b
s2 + m` 2s
 
3b

0
s + m`2 1 3
m`2
= 3b
s2 + m` 2s

3
m`2
= 3b
,
s2 + m` 2s

which is identical to Equation (5.2).

TOC
CHAPTER 6. STATE SPACE MODELS 81

6.3 Design Study B. Inverted Pendulum

Example Problem B.6

˙ >
˙ θ̃)
Defining the states as x̃ = (z̃, θ̃, z̃, , the input as ũ = F̃ , and the measured
>
output as ỹ = (z̃, θ̃) , find the linear state space equations in the form
x̃˙ = Ax̃ + B ũ
ỹ = C x̃ + Dũ.

Solution
The linear state space equations can be derived in two different ways: (1) directly
from the linearized equations of motion and (2) by linearizing the nonlinear equa-
tions of motion.
Starting with the linear state space equation in Equation (4.12) and solving for
¨>
¨ θ̃)
(z̃, gives
 2 
! m1 `3 −m1 2`
 
z̃¨ −m1 2` (m1 + m2 ) −bz̃˙ + F̃
¨ =
θ̃
2 2
m21 `3 + m1 m2 `3 − m21 `4
2
m1 g 2` θ̃
 3

4 m1 g
− 1 m b+m z̃˙ + 1 m 1+m F̃ − 1 m θ̃
= 4 1 2 4 1 2 4 1 +m2
3(m1 +m2 )g
.
1
3b ˙
z̃ − 1
3
F̃ + 1 θ̃
2( 4 m1 +m2 )` 2( 4 m1 +m2 )` 2( 4 m1 +m2 )`

4 ˙ >
˙ θ̃) 4
Therefore, defining x̃ = (x̃1 , x̃2 , x̃3 , x̃4 )> = (z̃, θ̃, z̃, , and ũ = F̃ gives
 ˙  z̃˙   x̃3

x̃1
˙ 2 
4 x̃
˙ 
θ̃  
x̃4 
x̃˙ =  =   =  b 1
3
4 m1 g


x̃˙ 3  z̃¨  − 1 m +m x̃3 + 1 m +m ũ − 1 m +m x̃2 
4 1 2 4 1 2 4 1 2

x̃˙ 4 ¨
θ̃ 1
3b
x̃ 3 − 1
3
ũ + 3(m1 +m2 )g
1 x̃ 2
2( 4 m1 +m2 )` 2( 4 m1 +m2 )` 2( 4 m1 +m2 )`
    
0 0 1 0 0
x̃1
0 0 0 1    0 
  x̃2   
= 0 − 43 m1 g − b
0    + 1 1  ũ.
 1
4 m 1 +m 2
1
4 m 1 +m 2  x̃ 3  4 m 1 +m 2 
3(m1 +m2 )g −3
0 2( 1 m +m )` 2( 1 m +m )` 03b x̃ 4 1
2( 4 m1 +m2 )`
4 1 2 4 1 2

Assuming that the measured output of the system is ỹ = (z̃, θ̃)> , the linearized
output is given by
 
      x̃1  
z̃ x̃1 1 0 0 0   x̃2 
 0
ỹ = = = + ũ.
θ̃ x̃2 0 1 0 0 x̃3  0
x̃4

TOC
CHAPTER 6. STATE SPACE MODELS 82

Therefore, the linearized state space equations are given by

   
0 0 1 0 0
0 0 0 1  0 
   
x̃˙ = 0 − 34 m1 g − 1 m b+m 0 x̃ +  1  ũ
 1
4 m1 +m2 4 1 2  14 m1 +m2 
3(m1 +m2 )g 3b −3
0 2( 1 m +m )` 2( 14 m1 +m2 )`
0 2( 14 m1 +m2 )`
4 1 2
 
1 0 0 0
ỹ = x̃. (6.17)
0 1 0 0

Alternatively, the linearized state space equations can be derived from the non-
linear equations of motion. Starting with Equation (3.2) and solving for (z̈, θ̈)>
gives

 2 
m1 `3 −m1 2` cos θ
−m1 2` cos θ (m1 + m2 ) m1 2` θ̇2 sin θ − bż + F
   
z̈
= 2
θ̈ m1 m2 `3 + m21 `2 ( 31 − 14 cos2 θ) m1 g` sin θ
3 2 2 2
!
m21 `6 θ̇2 sin θ − bm1 `3 ż + m1 `3 F − m21 `4 g sin θ cos θ
2 `2 2
−m1 4 θ̇ sin θ cos θ + bm1 2` ż cos θ − m1 2` cos θF + (m1 + m2 )m1 g 2` sin θ
= 2 .
m1 m2 `3 + m21 `2 ( 31 − 1
4
cos2 θ)

4 4 4
Defining x = (z, θ, ż, θ̇)> , u = F , and y = (z, θ)> results in the nonlinear state
space equations

 
  x3
ẋ1  x4 
ẋ2  
 2 `3 2 `2 `2 2 `2


 = m 1 6 4x sin x 2 −bm 1 3 3x +m 1 3 u−m 1 4 g sin x 2 cos x 2

ẋ3   2
m1 m2 `3 +m21 `2 ( 13 − 41 cos2 x2 ) 
ẋ4  −m2 `2 x2 sin x cos x +bm ` x cos x −m ` cos x u+(m +m )m g ` sin x 
1 4 4 2 2 12 3 2 12 2 1 2 1 2 2
2
m1 m2 `3 +m21 `2 ( 13 − 41 cos2 x2 )
4
= f (x, u)
 
x1 4
y= = h(x, u).
x2

TOC
CHAPTER 6. STATE SPACE MODELS 83

Taking the Jacobians about the equilibrium xe = (ze , 0, 0, 0)> and ue = 0 gives

∂f 
4
A=
∂x 
 e 
0 0 1 0 

0 0 0 1 
 
 −bm1 `3
2 3
2m21 `6 x4 sin x2 
= 0 ∂f3 
 ∂x2 2 2
m1 m2 `4 +m1 `4 sin2 x2 m1 m2 `3 +m21 `2 ( 13 − 41 cos2 x2 ) 
2
 2
−2m21 `4 x4 sin x2 cos x2

0 ∂x∂f4 bm1 2` cos x2 
2
2
m1 m2 `3 +m21 `2 ( 13 − 41 cos2 x2 )
2
m1 m2 `3 +m21 `2 ( 13 − 41 cos2 x2 )

e
 
0 0 1 0
0 0 0 1
 
= 0 − 43 m1 g − 1 m +m b
0
 1
4 m 1 +m 2 4 1 2 
0 2(3(m1
1 +m2 )g
1
3b
0
4 m1 +m2 )` 2( 4 m1 +m2 )`
   
0 
 0
  0   
4 ∂f    0
 =  
2
B=  = m1 `3
 1 
∂u e  m m `2 +m2 `2 ( 1 − 1 cos2 x ) 
 1 23 1 3 4 2   1
4 m 1 +m 2 
m1 2` cos x2  −3
− 2  1
2( 4 m1 +m2 )`
m1 m2 `3 +m21 `2 ( 31 − 14 cos2 x2 ) e
  
4 ∂h 
C=  = 1 0 0 0
∂x e 0 1 0 0
  
4 ∂h 
D=  = 0 ,
∂u e 0

resulting in the linearized state space equations given in Equation (6.17).

6.4 Design Study C. Satellite Attitude Control

Example Problem C.6

Suppose that a star tracker is used to measure θ and a strain gauge is used to
approximate φ − θ. Defining the states as x = (θ, φ, θ̇, φ̇)> , the input as u = τ ,
and the measured output as y = (θ, φ − θ)> , find the linear state space equations
in the form

ẋ = Ax + Bu
y = Cx + Du.

TOC
CHAPTER 6. STATE SPACE MODELS 84

Solution

The equations of motion from HW C.3 are given by

1 b b k k
θ̈ = τ− θ̇ + φ̇ − θ + φ
Js Js Js Js Js
b b k k
φ̈ = θ̇ − φ̇ + θ − φ.
Jp Jp Jp Jp

4 4
Defining the state x = (x1 , x2 , x3 , x4 )> = (θ, φ, θ̇, φ̇)> , the input u = τ , and the
4
output y = (y1 , y2 )> = (θ, φ − θ)> , the state space equations are given by

     
ẋ1 θ̇ x3
ẋ2  φ̇  x4 
ẋ =      
ẋ3  =  θ̈  =  J1 u − Jb x3 + Jb x4 − Jk x1 + Jk x2 
s s s s s
b b k k
ẋ4 φ̈ Jp x3 − Jp x4 + Jp x1 − Jp x2
    
0 0 1 0 x1 0
 0 0 0 1  x2   0 
=− Jk k
− Jbs b     
 x3  +  1  u.
s Js Js Js
k k b b
Jp − Jp Jp − Jp x 4 0

Similarly

   
θ x1
y= =
φ−θ x2 − x1
 
  x1  
1 0 0 0  
x2  + 0 u.
=
−1 1 0 0 x3  0
x4

Therefore, the state space model is

ẋ = Ax + Bu
y = Cx + Du,

TOC
CHAPTER 6. STATE SPACE MODELS 85

where
 
0 0 1 0
 0 0 0 1 
A=
− Jk k
− Jbs b 
s Js Js
k
Jp − Jkp b
Jp
b
− Jp
 
0
0
B=1

Js
0
 
1 0 0 0
C=
−1 1 0 0
 
0
D= .
0

Important Concepts:
• State space models are composed of the state evolution equation and
an output equation.
• The state evolution equation describes how the states change over
time as a function of the current state and the input.
• The output equation describes how the variables that can be seen or
measured are related to the states and input.
• Any nth order differential equation can be written as n first order
state evolution equations.
• Jacobian linearization will linearize nonlinear state space equations.
• The transfer function of a state space realization is H(s) = C(sI −
A)−1 B + D.

Notes and References

There are numerous resources that derive state space models from differential
equations and transfer functions. For example [4, 17, 6, 5].

TOC
 Part III
PID Control Design

In this part of the book we will introduce feedback control by studying the
most commonly used industrial control law, the so-called proportional-integral-
derivative control, or PID control. PID controllers are widely used because they
are intuitive and easy to understand, they can be designed without any under-
standing of the underlying physics of the system, and a model of the system
is not required, which is especially important in the chemical processing indus-
try. However, the stability and performance of the closed-loop system can only
be guaranteed when using PID control on second order systems. In Part IV on
observer-based control and Part V on loopshaping, we will extend the basic ideas
covered in this Part to higher-order systems.
Fig. 6-1 shows a basic proportional control strategy. The output y of the phys-
ical system is subtracted from the commanded reference output yr to produce the
error e = yr − y. Proportional control determines the input to the system u to
be proportional to the error, i.e., u = kP e. The constant gain kP is called the
proportional gain. Proportional control can be augmented with integral control

yr u Physical
kP system

Figure 6-1: Proportional Control. The feedback control is a gain kp multiplied by

the error.

to produce PI control, as shown in Fig. 6-2. The basic idea is to integrate the error
so that the system responds to error accrued in the past. The constant gain kI is
called the integral gain. Alternatively, proportional control can be augmented
with a derivative control as shown in Fig. 6-3. The idea is to respond to how fast
 R
kI

yr u Physical
kP
system

Figure 6-2: Proportional plus Integral (PI) Control. The feedback control is a
linear combination of proportional and integral control.

the error is changing, or the derivative of the error. The constant gain kD is called
the derivative gain. When all three elements are used, as shown in Fig. 6-4, the

yr u Physical
kP system

d kD
dt

Figure 6-3: Proportional plus Derivative (PD) Control. The feedback control is a
linear combination of proportional and derivative control.

resulting controller is called a PID control. In Part III of this book, we will ex-
plore various aspects of PID control. Many of these concepts can be generalized
to other control schemes in later chapters, and so our study of PID control will
also help to motivate and understand more advanced concepts.
Intuition for PID control can be gained by considering the step response shown
in Fig. 6-5. At the beginning of the response, there is significant error between the
current output y(t) and the reference command yr (t). The proportional control
term will tend to push the system so as to reduce the gap between y(t) and yr (t).
As y(t) begins to change, a fast response may lead to significant overshoot due to
the momentum in the system. Therefore, if it is moving too quickly, we may want
to slow down the response, or if it is moving too slowly, we may want to speed
up the response. Derivative control is used to affect these changes. After the
response has settled into steady state, there may be significant steady state error,
as shown in Fig. 6-5. Integral control adds up this steady error and will eventually
act to reduce the steady state error. A good example is an aircraft in a constant
crosswind. An integrator can be used to correct the response of the aircraft so that
it crabs into the wind to maintain its desired flight path.
Part III of the book is organized as follows. In Chapter 7 we will show how
PD control can be used to specify the desired closed-loop poles of a second order
system. In Chapter 8 we introduce the concepts of rise time, natural frequency,
 R
kI

yr u Physical
kP system

d kD
dt

Figure 6-4: Proportional plus Integral plus Derivative (PID) Control. The feed-
back control is a linear combination of proportional and integral and derivative
control.

and damping ratio for second order systems and show how these concepts relate
to the location of the closed-loop poles. We also show how these quantities can
be used to design PD controllers for second order systems and cascades of second
order systems. In Chapter 9 we use the final value theorem to show the effect of
integrators on the closed-loop system. We introduce the notion of system type in
the context of reference tracking and disturbance rejection. Finally, in Chapter 10
we show how to write computer code to implement PID controllers.

Important Concepts:
• PID controllers are widely used for a variety of applications. They
act on the error between the reference input and the system’s output.
• A proportional controller sets the input to the system to be propor-
tional to the error.
• Integral control corrects for accrued, or integrated, error in the past.
• Derivative control changes the input based upon the error’s slope.
• Combinations of control types, such as proportional (P),
proportional-integral (PI), proportional-derivative (PD), and
proportional-integral-derivative (PID) are used in feedback control
systems.
CHAPTER 6. STATE SPACE MODELS 89

Step Response
1.6

1.4 Integral control

acts on accrued
error, looking back
1.2
over the past.

1 reference command
Amplitude

0.8

Derivative control
Proportional control 0.6
acts on the slope
acts on the current
of the response,
distance to the
0.4 anticipating future
reference command
position.
0.2

0
0 1 2 3 4 5 6 7 8 9
Time (seconds)

Figure 6-5: Intuitive interpretation of PID control. Proportional control acts on

the current error, integral control acts on accrued past error, and derivative control
acts on anticipated future error.

TOC
CHAPTER 6. STATE SPACE MODELS 90

TOC
 Pole Placement for Second Order Systems
7
Learning Objectives:
• Gain intuition on how PD control works.
• Choose proportional and derivative gains that will place the closed-
loop poles of a 2nd order system at user desired locations.

7.1 Theory
Suppose that the model for the linearized physical system is given by the second
order transfer function
b0
P (s) = 2 . (7.1)
s + a1 s + a0

The open loop poles of the plant P (s) are defined to be the roots of the open loop
characteristic polynomial

∆ol (s) = s2 + a1 s + a0 ,

which are given by

r
a1 a1 2
pol = − ± − a0 .
2 2
Note that the open loop poles are determined by the physical parameters and that
the poles may be in the right half plane. If we use PD control to regulate the output
y to the reference command yr , then the block diagram is shown in Fig. 7-1. The
transfer function for the closed-loop system can be derived by noting from the

TOC
CHAPTER 7. SECOND ORDER SYSTEMS 92

Yr (s) E(s) Y (s)

U (s)

Figure 7-1: PD control of a second order system.

block diagram that

 
b0
Y (s) = 2
(kP + kD s) (Yr (s) − Y (s))
s + a1 s + a0
=⇒ (s2 + a1 s + a0 )Y (s) = (b0 kP + b0 kD s) (Yr (s) − Y (s))
 
=⇒ (s2 + a1 s + a0 ) + (b0 kP + b0 kD s) Y (s) (7.2)
= (b0 kP + b0 kD s) Yr (s)
=⇒ (s2 + (a1 + b0 kD )s + (a0 + b0 kP ))Y (s) (7.3)
= (b0 kP + b0 kD s) Yr (s)
b0 kD s + b0 kP
=⇒ Y (s) = 2 Yr (s). (7.4)
s + (a1 + b0 kD )s + (a0 + b0 kP )

Equation (7.4) constitutes the closed-loop transfer function for the system under
the influence of PD control. The closed-loop poles are given by the roots of the
closed-loop characteristic polynomial

∆cl (s) = s2 + (a1 + b0 kD )s + (a0 + b0 kP ),

which are
s 2
a1 + b0 kD (a1 + b0 kD )
pcl = − ± − (a0 + b0 kP ).
2 2

Note that the closed-loop poles can be specified by the designer by selecting kP
and kD . The basic idea is to select desired closed-loop poles −pd1 and −pd2 and
then to form the desired characteristic polynomial
4
∆dcl (s) = (s + pd1 )(s + pd2 ) = s2 + α1 s + α0 .

By setting ∆cl (s) = ∆dcl (s) we can equate the leading coefficients of each term
of the polynomial and solve for the gains kP and kD as
α0 − a0
kP =
b0
α1 − a1
kD = .
b0

TOC
CHAPTER 7. SECOND ORDER SYSTEMS 93

One of the disadvantages to the PD architecture shown in Fig. 7-1 is the in-
troduction of a zero in the closed-loop transfer function. Note that while the open
loop system (7.1) does not have a zero, the closed-loop system (7.4) has a zero at
kP
zcl = − .
kD
The presence of the zero will modify the closed-loop response of the system. A
simple trick that removes the zero is to use the control structure shown in Fig. 7-2.
In this particular case, the differentiator acts only on the output y and not on the
error e. Notice the change of sign on the signal coming from the differentiator. In

Yr (s) E(s) Y (s)

U (s)

Figure 7-2: PD control of a second order system, where the derivative control only
differentiates the output y and not the error e.

this case the closed-loop transfer function is calculated as

 
b0
Y (s) = (kP (Yr (s) − Y (s)) − kD sY (s))
s2 + a1 s + a0
=⇒ (s2 + a1 s + a0 )Y (s) = (b0 kP (Yr (s) − Y (s)) − b0 kD sY (s))
 
=⇒ (s2 + a1 s + a0 ) + (b0 kP + b0 kD s) Y (s) = b0 kP Yr (s)
=⇒ (s2 + (a1 + b0 kD )s + (a0 + b0 kP ))Y (s) = b0 kP Yr (s)
b0 k P
=⇒ Y (s) = 2 Yr (s). (7.5)
s + (a1 + b0 kD )s + (a0 + b0 kP )
Note that the closed-loop transfer function (7.5) has the same poles as Equa-
tion (7.4), but that the zero has been removed. An added benefit to the structure
shown in Fig. 7-2 is that when yr (t) is a step input, the derivative can introduce a
large signal spike in u(t). These large spikes are removed by only differentiating
y(t) instead of e(t) = yr (t) − y(t).

7.2 Design Study A. Single Link Robot Arm

Example Problem A.7

(a) Given the open loop transfer function found in problem A.6, find the open
loop poles of the system, when the equilibrium angle is θe = 0.

TOC
CHAPTER 7. SECOND ORDER SYSTEMS 94

(b) Using the PD control architecture shown in Fig. 7-2, find the closed loop
transfer function from θr to θ and find the closed loop poles as a function
of kP and kD .
(c) Select kP and kD to place the closed loop poles at p1 = −3 and p2 = −4.
(d) Using the gains from part (c), implement the PD control for the single link
robot arm in simulation and plot the step response.

Solution
The open loop transfer function from homework A.6 is
!
3
m`2
Θ(s) = 3b
τ̃ (s).
s2 + m` 2s

Using the parameters from Section A gives

 
66.67
Θ(s) = τ̃ (s).
s2 + 0.667s

The open loop poles are therefore the roots of the open loop polynomial

∆ol (s) = s2 + 0.667s,

which are given by

pol = 0, −0.667.
Using PD control, the closed loop system is therefore shown in Fig. 7-3. The

Yr (s) E(s) Y (s)

U (s) 66.67
s(s + 0.667)

Figure 7-3: PD control of the single link robot arm.

transfer function of the closed loop system is given by

 
66.67
Θ(s) = (kP (Θc (s) − Θ(s)) − kD sΘ(s))
s2 + 0.667s
=⇒ (s2 + 0.667s)Θ(s) = (66.67kP (Θc (s) − Θ(s)) − 66.67kD sΘ(s))
=⇒ (s2 + (0.667 + 66.67kD )s + 66.67kP )Θ(s) = 66.67kP Θc (s)
66.67kP
=⇒ Θ(s) = 2 Θc (s).
s + (0.667 + 66.67kD )s + 66.67kP

TOC
 CHAPTER 7. SECOND ORDER SYSTEMS 95

Therefore, the closed loop poles are given by the roots of the closed loop charac-
teristic polynomial

∆cl (s) = s2 + (0.667 + 66.67kD )s + 66.67kP

which are given by

s 2
(0.667 + 66.67kD ) (0.667 + 66.67kD )
pcl = − ± − 66.67kP
2 2

If the desired closed loop poles are at −3 and −4, then the desired closed loop
characteristic polynomial is

∆dcl = (s + 3)(s + 4)
= s2 + 7s + 12.

Equating the actual closed loop characteristic polynomial ∆cl with the desired
characteristic polynomial ∆dcl gives

s2 + (0.667 + 66.67kD )s + 66.67kP = s2 + 7s + 12,

or by equating each term we get

0.667 + 66.67kD = 7
66.67kP = 12.

Solving for kP and kD gives

kP = 0.18
kD = 0.095.

A Python class that implements a PD controller for the single link robot arm
is shown below.
1 import numpy as np
2 import armParamHW7 as P
3 import sys
4 sys.path.append(’..’) # add parent directory
5 import armParam as P0
6
7 class armController:
8 def __init__(self):
9
10 # Instantiates the PD object
11 self.kp = P.kp
12 self.kd = P.kd
13 self.limit = P0.tau_max
14
15 def update(self, theta_r, x):
16 theta = x.item(0)

TOC
 CHAPTER 7. SECOND ORDER SYSTEMS 96

17 thetadot = x.item(1)
18
19 # feedback linearized torque
20 tau_fl = P0.m * P0.g * (P0.ell / 2.0) * np.cos(theta)
21
22 # equilibrium torque around theta_e = 0
23 theta_e = 0.0
24 tau_e = P0.m * P0.g * P0.ell/2.0 * np.cos(theta_e)
25
26 # compute the linearized torque using PD control
27 tau_tilde = self.kp * (theta_r - theta) - self.kd * thetadot
28
29 # compute total torque
30 #tau = tau_fl + tau_tilde
31
32 tau = tau_e + tau_tilde
33 # always saturate to protect hardware
34 tau = self.saturate(tau)
35
36 return tau
37
38 def saturate(self, u):
39 if abs(u) > self.limit:
40 u = self.limit*np.sign(u)
41 return u

Listing 7.1: armController.py

Code that simulates closed-loop system is given below.

1 import sys
2 sys.path.append(’..’) # add parent directory
3 import matplotlib.pyplot as plt
4 import numpy as np
5 import armParam as P
6 from hw3.armDynamics import armDynamics
7 from armController import armController
8 from hw2.signalGenerator import signalGenerator
9 from hw2.armAnimation import armAnimation
10 from hw2.dataPlotter import dataPlotter
11
12 # instantiate arm, controller, and reference classes
13 arm = armDynamics()
14 controller = armController()
15 reference = signalGenerator(amplitude=5*np.pi/180.0, frequency=0.05)
16 disturbance = signalGenerator(amplitude=0.01)
17 noise = signalGenerator(amplitude=0.01)
18
19 # instantiate the simulation plots and animation
20 dataPlot = dataPlotter()
21 animation = armAnimation()
22
23 t = P.t_start # time starts at t_start
24 y = arm.h() # output of system at start of simulation
25 while t < P.t_end: # main simulation loop
26 # Get referenced inputs from signal generators
27 # Propagate dynamics in between plot samples

TOC
 CHAPTER 7. SECOND ORDER SYSTEMS 97

28 t_next_plot = t + P.t_plot
29
30 # updates control and dynamics at faster simulation rate
31 while t < t_next_plot:
32 r = reference.square(t)
33 d = 0#disturbance.step(t) # input disturbance
34 n = noise.random(t) # simulate sensor noise
35 x = arm.state
36 u = controller.update(r, x) # update controller
37 y = arm.update(u + d) # propagate system
38 t = t + P.Ts # advance time by Ts
39
40 # update animation and data plots
41 animation.update(arm.state)
42 dataPlot.update(t, r, arm.state, u)
43
44 # the pause causes the figure to be displayed for simulation
45 plt.pause(0.0001)
46
47 # Keeps the program from closing until the user presses a button.
48 print(’Press key to close’)
49 plt.waitforbuttonpress()
50 plt.close()

Listing 7.2: armSim.py

Complete simulation code for Matlab, Python, and Simulink can be down-
loaded at http://controlbook.byu.edu.

Important Concepts:
• The PD controller gains can place the poles of a second order closed-
loop characteristic equation to a desired location.
• PD gains can be chosen to move unstable poles, ones in the right side
of the complex plane, to stable locations in the closed-loop feedback
system.
• A PD controller introduces a zero into the closed-loop system. It may
be eliminated by modifying the derivative portion of the controller so
that it acts on the output (rather than the error).

Notes and References

The concept of using feedback to place the poles of the system at desired locations
is a much deeper topic than is covered in this chapter. While the method discussed
in this chapter is only applicable to second order systems, it is usually possible to
arbitrarily assign the poles for an nth order system. In general, to assign n poles
will require n gains. We will discuss pole placement in more detail in Chapter 11
of this book.

TOC
CHAPTER 7. SECOND ORDER SYSTEMS 98

TOC
 Design Strategies for Second Order Systems
8
Learning Objectives:
• Understand how pole locations affect the step response of a system.
• Choose pole locations (for second order systems ) that satisfy desired
design requirements for the rise time, natural frequency, and damping
ratio.

8.1 Theory
In the previous section we saw that for second order systems, PD control could be
used to exactly specify the pole location of the closed-loop system. This chapter
examines the design problem of how to select the pole locations to achieve desired
behavior. In Section 8.1.1 we derive the relationship between a single pole loca-
tion and the step response of the system. In Section 8.1.2 we show that the time
domain response for a second order system can be understood in terms of natural
frequency and damping ratio. Finally in Section 8.1.3 we show the effect that a
zero has on the step response of a second order system.

8.1.1 First Order Response

Consider a first order system given by
p
Y (s) = U (s),
s+p
A
where p > 0. The pole location of the first order system is at −p. If U (s) = s is
a step of magnitude A, then
Ap
Y (s) = .
s(s + p)

TOC
CHAPTER 8. SECOND ORDER DESIGN 100

As shown in Appendix P.5, the inverse Laplace transform is given by

(
A(1 − e−pt ), t ≥ 0
y(t) = . (8.1)
0, otherwise

The response is shown in Fig. 8-1, which shows the response to steps of size
A = 1, A = 2, and A = 3, respectively. If we define the rise time tr of the

Slope is Ap

Step Response
3

.9A A=3

2.5

.9A A=2
Amplitude

1.5

1
.9A A=1

0.5

0
0 1 2 3 4 5 6 7 8 9
Time (seconds)

1 tr
p

Figure 8-1: Step response of a first order system with pole p. Three step responses
are shown, where the step size is A = 1, A = 2, and A = 3, respectively.

system to be the time after the step when the response reaches 90% of its final
value, then we note from Fig. 8-1 that the rise time is independent of the step size
A. Also, by differentiating Equation (8.1) at time t = 0, it is straightforward to
show that the slope of the response at t = 0 is equal to Ap. This is also shown
graphically in Fig. 8-1.
When A = 1, the step response for different pole locations is shown in
Fig. 8-2. Pole locations corresponding to p = 1, p = 2, and p = 4 are shown.
Note that as the pole location moves further into the left half of the complex plane,
the rise time decreases.
An important concept for the steady state response of a system is the DC-gain,
which is defined as follows.
Definition The DC-gain of a transfer function H(s) is
H(s)|DC-gain = lim H(s).
s→0

An important fact is that if the poles of H(s) are in the open left half plane,
then the response of H(s) to a step of size A approaches A H(s)|DC-gain as t → ∞.

TOC
CHAPTER 8. SECOND ORDER DESIGN 101

Step Response
1

0.9

0.8

p=4
0.7

0.6
p=2
Amplitude

0.5

0.4
p=1
0.3

0.2

0.1

0
0 1 2 3 4 5 6 7 8 9
Time (seconds)

Figure 8-2: Step response of a first order system with poles p = 1, p = 2, and
p = 4. The rise time decreases as the pole location moves further into the left half
of the complex plane.

This is true for first order systems, as well as general H(s). For example, the DC-
q
gain of the first order system H(s) = s+p is pq , as long as p > 0.

8.1.2 Second Order Response

Suppose that the closed-loop system has a second order transfer function with two
real poles and no zeros:
K
Y (s) = Yr (s).
(s + p1 )(s + p2 )
To find the output of the system when the input is a step of magnitude A, we find
the inverse Laplace transform of
K A
Y (s) = .
(s + p1 )(s + p2 ) s
Using partial fraction expansion we get
KA KA KA
p1 p2 (−p1 )(p2 −p1 ) (−p2 )(p1 −p2 )
Y (s) = + + .
s s + p1 s + p2
Taking the inverse Laplace transform gives
(
KA KA KA
+ (−p1 )(p 2 −p1 )
e−p1 t + (−p2 )(p1 −p2 ) e
−p2 t
, t≥0
y(t) = p1 p2 .
0, otherwise

TOC
 CHAPTER 8. SECOND ORDER DESIGN 102

Note that as t → ∞ that y(t) → pKA1 p2

which is equal to the step size A times the
DC-gain of the system.
Now suppose that the closed-loop system has two complex poles and no zeros.
We will write a generic second order system with poles in the left half plane using
the notation
Kωn2
H(s) = 2 ,
s + 2ζωn s + ωn2
where K is the DC-gain of the system (typically K = 1), ωn > 0 is called the
natural frequency, and ζ > 0 is called the damping ratio. The poles are given by
the roots of the characteristic polynomial

∆(s) = s2 + 2ζωn s + ωn2 = 0,

in other words, the poles are

p
p1,2 = −ζωn ± (ζωn )2 − ωn2
p
= −ζωn ± ωn ζ 2 − 1.

If 0 < ζ < 1, then the roots are complex and given by

p
p1,2 = −ζωn ± jωn 1 − ζ 2 . (8.2)

Using the Python code

1 import control as cnt
2 import matplotlib.pyplot as plt
3
4 #start by varying the natural frequency
5 plt.figure(1), plt.clf()
6 zeta = 0.707
7
8 for wn in [0.5, 1.0, 2.0, 5.0]:
9 G = cnt.tf(wn**2, [1, 2*zeta*wn, wn**2])
10 t,y = cnt.step_response(G)
11 plt.plot(t, y)
12
13 plt.plot([0, 20.0] , [1, 1], ’k--’)
14 plt.xlabel(’Time (seconds)’)
15 plt.ylabel(’Amplitude’)
16 plt.title(’Step Response’)
17 plt.legend((’wn=0.5’, ’wn=1.0’, ’wn=2.0’, ’wn=5.0’))
18
19 #now we’ll see the effect of varying zeta
20 plt.figure(2), plt.clf()
21 wn = 1.0
22
23 for zeta in [0.2, 0.4, 0.707, 1.0, 2.0]:
24 G = cnt.tf(wn**2, [1, 2*zeta*wn, wn**2])
25 t,y = cnt.step_response(G)
26 plt.plot(t, y)
27
28 plt.plot([0, 35.0] , [1, 1], ’k--’)

TOC
 CHAPTER 8. SECOND ORDER DESIGN 103

29 plt.xlabel(’Time (seconds)’)
30 plt.ylabel(’Amplitude’)
31 plt.title(’Step Response’)
32 plt.legend((’zeta=0.2’, ’zeta=0.4’, ’zeta=0.707’,
33 ’zeta=1.0’, ’zeta=2.0’))
34
35 plt.show() # necessary to make plots appear

we obtain second order response plots similar to the ones shown in Figures 8-3
and 8-4. Note from Fig. 8-3 that as the natural frequency increases, the rise time
decreases. We should note that a similar phenomena to Fig. 8-1 also occurs, where
the rise time is independent of the size of the step. Note from Fig. 8-4 that the
damping ratio affects the amount of ringing in the system. For a small damping
ratio, e.g., ζ = 0.2 there is a large overshoot and significant ringing in the system.
At the other extreme, when ζ is larger than one, the poles are real and the response
is highly damped. The sweet spot is when ζ = 0.707 where the rise time is small,
but the overshoot is minimal.

Step Response
1.2

0.8 !n = 0.5
Amplitude

0.6 !n = 1

0.4 !n = 2

0.2 !n = 5

0
0 2 4 6 8 10 12 14 16 18

Time (seconds)

Figure 8-3: Second order response for ωn = 0.5, 1, 2, 5, when the damping ratio
is fixed at ζ = 0.707.

Note from Equation (8.2) that when ζ = 0.707 = √1 , the poles are at
2
ωn ωn
p1,2 = − √ ± j √ .
2 2
In the complex plane, the pole locations are shown in Fig. 8-5, where ωn is the
distance from the origin to the poles. When ζ = √12 the angle θ equals 45 degrees.

In rectangular coordinates, the poles are given by

p1,2 = −σ ± jωd , (8.3)

TOC
CHAPTER 8. SECOND ORDER DESIGN 104

Figure 8-4: Second order response ζ = 0.2, 0.4, 0.707, 1, 2 when the natural fre-
quency is fixed at ωn = 1.

as shown in Fig. 8-5. Equations (8.2) and (8.3) give

σ = ζωn
p
ωd = ωn 1 − ζ 2 .
From Fig. 8-5 it is also clear that
σ
sin θ = = ζ.
ωn
Therefore
θ = sin−1 ζ.
p
Since the pole locations are at −ζωn ± ωn ζ 2 − 1, the poles are both real if
ζ ≥ 1. When ζ = 1 the poles are both located at −ωn . When 0 < ζ < 1 the
poles are imaginary and occur in complex conjugate pairs. As shown in Fig. 8-6,
we say that the system is over damped when ζ > 1, we say that the system is
critically damped when ζ = 1, and we say that the system is under damped when
0 < ζ < 1.
The response of H(s) to a step of size A is the inverse Laplace transform of
Aωn2
Y (s) = ,
s(s2 + 2ζωn s + ωn2 )
which, as shown in Appendix P.5, when the poles are complex results in
"   #
1 −σt −1 σ
y(t) = A 1 − p e cos ωd t − tan . (8.4)
1 − ζ2 ωd

TOC
CHAPTER 8. SECOND ORDER DESIGN 105

x j!d
✓
!n
<

x j!d

Figure 8-5: Complex conjugate poles in the left half plane.

A plot of this signal is shown in Fig. 8-7.

The rise time is defined to be the time that it takes the output to transition from
10% to 90% of its final value A. A suitable approximation is that the rise time is
one half of the peak time, which is the time when y(t) reaches its first peak. To
find the peak time tp , differentiate y(t) in Equation (8.4) and then solve for the
first instance when the derivative is zero. The result is
π π
tp = = p .
ωd ωn 1 − ζ 2
Therefore, the rise time is approximately
1 1 π
tr ≈ tp = p .
2 2 ωn 1 − ζ 2

When ζ = 0.707 we get

2.2
tr ≈ . (8.5)
ωn
Specifications for second order systems are usually given either in terms of the
rise time tr and the damping ratio ζ, or in terms of the natural frequency ωn and
the damping ratio ζ.

8.1.3 Effect of a Zero on the Step Response

In the previous section, we looked at the step response when there were two com-
plex poles and no zeros. In this section we will briefly discuss the effect of zeros
on the step response of the system. This is a topic that we could treat at a much
deeper level.

TOC
CHAPTER 8. SECOND ORDER DESIGN 106

Figure 8-6: Second order response as a function of the damping ratio ζ.

A transfer function is called rational if it is the ratio of two polynomials in s.

For example,
s+1
H(s) = 3
s + 3s2 + s + 4
is a rational transfer function, whereas

(s + 1)e−s
H(s) =
s3 + 3s2 + s + 4
is not. A rational transfer function can be written as
N (s)
H(s) = ,
D(s)

where N (s) is the numerator polynomial and D(s) is the denominator polyno-
mial. The zeros of the transfer function are roots of the equation N (s) = 0, and
the poles of the system are the roots of the equation D(s) = 0.
One way to understand the effect of zeros is to consider the effect that zeros
have on the partial fraction expansion of a transfer function. For a review of
partial fraction expansion, see Appendix P.5. First consider a two pole system
with characteristic equation given by ∆(s) = (s+2)(s+3). The transfer function
with DC-gain equal to one and without zeros is given by

6
H(s) = .
(s + 2)(s + 3)

TOC
CHAPTER 8. SECOND ORDER DESIGN 107

Step Response
1.4

envelope is p 1 e t
1.2 1 ⇣2

0.8
Amplitude

1
cos(!d t tan !d )
0.6

0.4

0.2

0
0 2 4 6 8 10 12 14 16 18
Time (seconds)

Figure 8-7: Second order response. The steady state value is A. The envelope
 is
1 −σt
√ 2 e , and the frequency and phase shift are given by ωd and tan −1 σ
1−ζ ω d

respectively.

The response to a unit step is given by

 
6
y(t) = L−1
s(s + 2)(s + 3)
 
−1 1 −3 2
=L + +
s s+2 s+3
= 1 − 3e−2t + 2e−3t , t ≥ 0. (8.6)
Now consider the step response when a zero has been added at z = −2.1 and the
gain adjusted so that the DC-gain is still one, i.e., when
6
2.1 (s+ 2.1)
H(s) = .
(s + 2)(s + 3)
The response to a unit step is given by
 6 
−1 2.1 (s + 2.1)
y(t) = L
s(s + 2)(s + 3)
 
−1 1 −0.1429 −0.8571
=L + +
s s+2 s+3
= 1 − 0.1429e−2t − 0.8571e−3t , t ≥ 0.
Note that y(t) still settles to one, but the response of the pole at −2 has almost
been removed. If however, the zero is far away from the poles, then the effect is

TOC
CHAPTER 8. SECOND ORDER DESIGN 108

much less. Suppose that the zero is at z = −60 and the gain is adjusted so that the
DC-gain is still one, i.e.,
6
60 (s+ 60)
H(s) = .
(s + 2)(s + 3)

The response to a unit step is given by

 6 
60 (s
+ 60)
y(t) = L−1
s(s + 2)(s + 3)
 
1 −2.9 1.9
= L−1 + +
s s+2 s+3
= 1 − 2.9e−2t + 1.9e−3t , t ≥ 0.

In this case, the response is similar to Equation (8.6).

Another way to understand the effect of zeros on the system is to recall that an
s in the numerator is essentially a differentiator. Therefore

ωn2 (τ s + 1)
H(s) =
s2
+ 2ζωn s + ωn2
ωn2 s ωn2
=τ 2 2
+ 2 .
s + 2ζωn s + ωn s + 2ζωn s + ωn2

Therefore the step response is given by

 
−1 ωn2 s ωn2
y(t) = L τ +
s(s2 + 2ζωn s + ωn2 ) s(s2 + 2ζωn s + ωn2 )
   
d −1 ωn2 −1 ωn2
=τ L +L .
dt s(s2 + 2ζωn s + ωn2 ) s(s2 + 2ζωn s + ωn2 )

Therefore, the effect of the zero is to add τ times the derivative of the step response
to what the step response would have been without the zero. Fig. 8-8 shows the
step response for different values of τ , where it can be seen that adding a zero has
a strong effect on the rise time and the damping in the response. Note also that
when τ < 0, i.e., when zeros are in the right-half plane, that the system response
first goes negative, before going positive. Right half plane zeros are often called
nonminimum phase zeros and the associated response is often called nonminimum
phase response. Physical systems that have right-half plane zeros include the in-
verted pendulum and aircraft altitude control. For the inverted pendulum, in order
to move the cart to the right from a stopped position the cart must first move left,
so that the rod leans right, before cart can move to its desired position. Similarly,
aircraft pitch up to climb altitude. However, when the aircraft pitches up it loses
speed which causes it to lose lift, which causes the altitude to drop. When the
altitude drops it gains speed and produces the lift necessary to increase altitude.

TOC
CHAPTER 8. SECOND ORDER DESIGN 109

Figure 8-8: Step response of a second order system to a single zero.

8.1.4 Successive Loop Closure

The natural frequency ωn and the damping ratio ζ are only defined for second
order systems, and therefore, the PD design technique based on ωn and ζ are
only valid for second order systems. However, these concepts can be extended to
higher-order systems through a technique called successive loop closure, or inner-
loop, outer-loop design. When we derived the Laplace transform in Chapter 5,
we saw that for many systems, the transfer functions can be arranged in cascade
form as shown in Fig. 8-9 where the input to the system is u1 and the output is
y2 , but where y1 and u2 are intermediary values that represent the natural physical
coupling between internal variables. Many physical systems can be represented
by a cascade of systems.

u1 y2

Figure 8-9: Open-loop transfer function modeled as a cascade of two transfer

functions.

When the system is represented as a cascade, the control strategy can be de-
signed using successive loop closure. The basic idea is to close feedback loops
in succession around the open-loop plant dynamics rather than designing a single
control system. To illustrate this approach, consider the open-loop system shown
in Fig. 8-9. The open-loop dynamics are given by the product of two transfer func-
tions in series: P (s) = P1 (s)P2 (s). We assume that each of the transfer functions
has an output (y1 , y2 ) that can be measured and used for feedback. Typically, each
of the transfer functions, P1 (s) and P2 (s), is of relatively low order – usually first
or second order. In this case, we are interested in controlling the output y2 . Instead
of closing a single feedback loop around y2 , we will close feedback loops around
y1 and y2 in succession as shown in Fig. 8-10. We will design the compensators
C1 (s) and C2 (s) in succession. A necessary condition in the design process is that

TOC
CHAPTER 8. SECOND ORDER DESIGN 110

y2

Figure 8-10: Successive loop closure design.

the inner loop must be much faster than each successive loop. As a rule of thumb,
if the rise time of the inner loop is tr1 , the rise time of the next loop should be
5 to 10 times longer, i.e., tr2 > W1 tr1 , where W1 is a design parameter, usually
on the order of 5 − 10. In part V of this book, we will talk about the frequency
response of the system. At that time, we will provide a deeper explanation that
can be understood in terms of the bandwidth of each successive loop.
Examining the inner loop shown in Fig. 8-10, the goal is to design a closed-
loop system from r1 to y1 having a rise time of tr1 . If the rise time of the inner
loop is significantly faster than the rise time of the outer loop, and if the DC-gain
of the inner loop is kDC1 , then relative to the outer loop the inner loop can be
effectively modeled as the DC-gain. This is depicted schematically in Fig. 8-11.
With the inner-loop transfer function modeled as its DC-gain, design of the outer
loop is simplified because it only includes the plant transfer function P2 (s) and
the compensator C2 (s) and the DC-gain kDC1 . Because each of the plant models

y2
⇡ kDC1

Figure 8-11: Successive loop closure design with inner loop modeled as a unity
gain.

P1 (s) and P2 (s) are first or second order, conventional PID compensators can be
employed. The loopshaping techniques discussed in Part V can also be used for
successive loop closure of higher-order systems.

8.1.5 Input Saturation and Limits of Performance

A natural question is to ask if the rise time can be made arbitrarily small. Unfortu-
nately, saturation limits on the actuators place a lower bound on the achievable rise
time. All physical systems have saturation limits on their actuators. For example,
in the robot arm problem, the torque that can be applied to the arm is limited by
the motor that is used to apply that torque. Force and torque will always be limited

TOC
CHAPTER 8. SECOND ORDER DESIGN 111

by physical constraints like current limits. Similarly, for an airplane, the rudder
command is limited by how far the rudder can physically move, which is typically
on the order of ±40 degrees. A natural way to model input constraints is to add a
saturation block preceding the physical plant. Fig. 8-12 shows a saturation block
in conjunction with a PD control scheme. Mathematically, the saturation block is

yr usat

Figure 8-12: Control system with input saturation. The input saturation places a
lower bound on the achievable rise time.

given by 

umax , if u ≥ umax
usat = u, if − umax ≤ u ≤ umax ,


−umax , otherwise
where the input to the block is u, the output is usat , and the saturation limit is
umax . Note that the saturation block is non-linear. Therefore, one design strategy
is to select the feedback gains so that the system remains linear by keeping the
input out of saturation.
To see how this might be done, consider the second-order system shown in
Fig. 8-12 with proportional feedback on the output error and derivative feedback
on the output. When the input is not saturated, the closed-loop transfer function is
 
b0 kp
Y (s) = Yr (s). (8.7)
s2 + (a1 + b0 kd )s + (a0 + b0 kp )

We can see that the closed-loop poles of the system are defined by the selection of
the control gains kp and kd . Note also that the actuator effort u can be expressed
as u = kp e − kd ẏ. When ẏ is zero or small, the size of the actuator effort u
is primarily governed by the size of the control error e and the control gain kp .
If the system is stable, the largest control effort in response to a step input will
occur immediately after the step when ẏ = 0. If the system is to be kept just
out of saturation, then immediately after the step we have |u| = umax = kp emax .
Rearranging this expression, we find that the proportional control gain can be
determined from the maximum anticipated output error and the saturation limits
of the actuator as
umax
kp = ± , (8.8)
emax
where umax is the maximum control effort the system can provide, emax is the
step error that results from a step input of nominal size, and the sign of kp is
determined by the physics of the system.

TOC
CHAPTER 8. SECOND ORDER DESIGN 112

Suppose that the desired closed-loop characteristic polynomial is given by

∆dcl (s) = s2 + 2ζωn s + ωn2 ,
then by comparing with the actual closed-loop characteristic polynomial
∆cl (s) = s2 + (a1 + b0 kd )s + (a0 + b0 kp )
we have that
r
p umax
ωn = a0 + b0 kp ≤ a0 + b0 ,
emax
which is an upper limit on the natural frequency of the closed-loop system, en-
suring that saturation of the actuator is avoided. The corresponding rise time con-
straint is
2.2 2.2
tr ≥ =q ,
ωn a0 + b0 uemax
max

which is the smallest possible rise time for an error step of emax .
If the input has an equilibrium value, then the saturation constraint used in the
above calculation must be modified to account for the equilibrium. For example,
suppose that u = ue + ũ where ue is the equilibrium value and ũ is the output of
the PID controller. Also suppose that the input saturation constraint is given by
umin ≤ u ≤ umax . The objective is to find a value for ũmax where |ũ| ≤ ũmax
guarantees that umin ≤ u ≤ umax . We have that
umin ≤ u ≤ umax
=⇒ umin ≤ ue + ũ ≤ umax
=⇒ umin − ue ≤ ũ ≤ umax − ue .
The desired value for ũmax is therefore the minimum (in absolute value) of the
right and left hand sides of this expression. The situation is shown in Fig. 8-13,
where it is clear that the value for ũmax can be written as
(
|umax − ue | , if ue ≥ umax +u 2
min
ũmax = umax +umin
|ue − umin | , if ue ≤ 2 .

The takeaway from this discussion is that the rise time is limited by the input
saturation constraint and cannot be made arbitrarily small. In practice, the control
parameters are tuned so that the closed-loop system has the fastest possible rise
time without driving the plant input into saturation.

8.2 Design Study A. Single Link Robot Arm

Example Problem A.8

For the single link robot arm, do the following:

TOC
CHAPTER 8. SECOND ORDER DESIGN 113

u
umax
ue ũmax

umin

u
umax

ue
ũmax
umin

Figure 8-13: The saturation constraint for ũ depends on the value of the equilib-
rium input ue .

(a) Suppose that the design requirements are that the rise time is tr ≈ 0.8 sec-
onds, with a damping ratio of ζ = 0.707. Find the desired closed loop
characteristic polynomial ∆dcl (s) and the associated pole locations. Find
the proportional and derivative gains kP and kD to achieve these specifi-
cations, and modify the simulation from HW A.8 to verify that the step
response satisfies the requirements.

(b) Suppose that the input torque for the robot arm is limited to |τ | ≤ τmax =
1 Nm. Modify the simulation to include a saturation on the torque τ . Using
the rise time tr and damping ratio ζ as tuning parameters, tune the PD
control law so that the input just saturates when a step of size 50 degrees is
placed on θ̃r .

Solution
A rise time of tr ≈ 0.8 seconds, implies that the natural frequency is

ωn = 2.2/0.8 = 2.75.

Therefore, the desired characteristic polynomial is

∆dcl = s2 + 2ζωn s + ωn2 = s2 + 3.889s + 7.5625. (8.9)

TOC
 CHAPTER 8. SECOND ORDER DESIGN 114

From HW A.7 the actual closed loop characteristic polynomial is

∆cl (s) = s2 + (0.667 + 66.67kD )s + 66.67kP . (8.10)

Equating Equations (8.9) and (8.10) and solving for the gains gives

kP = 0.1134
kD = 0.0483.

A Python class that implements a PD controller for the single link robot arm
is shown below.
1 import numpy as np
2 import armParamHW8 as P
3 import sys
4 sys.path.append(’..’) # add parent directory
5 import armParam as P0
6
7
8 class armController:
9
10 def __init__(self):
11 # Instantiates the PD object
12 self.kp = P.kp
13 self.kd = P.kd
14 self.tau_max = P.tau_max # Maximum torque
15
16 def update(self, theta_r, state):
17 theta = state.item(0)
18 thetadot = state.item(1)
19
20 # compute feedback linearizing torque tau_fl
21 tau_fl = P0.m * P0.g * (P0.ell / 2.0) * np.cos(theta)
22
23 # compute the linearized torque using PD
24 tau_tilde = self.kp * (theta_r - theta) - self.kd * thetadot
25
26 # compute total torque
27 tau = tau_fl + tau_tilde
28 tau = self.saturate(tau, self.tau_max)
29 return tau
30
31 def saturate(self, u, limit):
32 if abs(u) > limit:
33 u = limit*np.sign(u)
34 return u

Listing 8.1: armController.py

Code that computes the control gains is given below.

1 import sys
2 sys.path.append(’..’) # add parent directory
3 import matplotlib.pyplot as plt
4 import numpy as np

TOC
 CHAPTER 8. SECOND ORDER DESIGN 115

5 import armParam as P
6 from hw3.armDynamics import armDynamics
7 from armController import armController
8 from hw2.signalGenerator import signalGenerator
9 from hw2.armAnimation import armAnimation
10 from hw2.dataPlotter import dataPlotter
11
12 # instantiate arm, controller, and reference classes
13 arm = armDynamics()
14 controller = armController()
15 reference = signalGenerator(amplitude=30*np.pi/180.0, frequency=0.05)
16 disturbance = signalGenerator(amplitude=0.0)
17
18 # instantiate the simulation plots and animation
19 dataPlot = dataPlotter()
20 animation = armAnimation()
21
22 t = P.t_start # time starts at t_start
23 y = arm.h() # output of system at start of simulation
24 while t < P.t_end: # main simulation loop
25 # Get referenced inputs from signal generators
26 # Propagate dynamics in between plot samples
27 t_next_plot = t + P.t_plot
28
29 # updates control and dynamics at faster simulation rate
30 while t < t_next_plot:
31 r = reference.square(t)
32 d = disturbance.step(t) # input disturbance
33 n = 0.0 #noise.random(t) # simulate sensor noise
34 x = arm.state
35 u = controller.update(r, x) # update controller
36 y = arm.update(u + d) # propagate system
37 t = t + P.Ts # advance time by Ts
38
39 # update animation and data plots
40 animation.update(arm.state)
41 dataPlot.update(t, r, arm.state, u)
42
43 # the pause causes the figure to be displayed for simulation
44 plt.pause(0.0001)
45
46 # Keeps the program from closing until the user presses a button.
47 print(’Press key to close’)
48 plt.waitforbuttonpress()
49 plt.close()

Listing 8.2: armSim.py

Complete simulation code for Matlab, Python, and Simulink can be down-
loaded at http://controlbook.byu.edu.

TOC
CHAPTER 8. SECOND ORDER DESIGN 116

8.3 Design Study B. Inverted Pendulum

Example Problem B.8

For the inverted pendulum, do the following:
(a) Using the principle of successive loop closure, draw a block diagram that uses
PD control for both inner loop control and outer loop control. The input to
the outer loop controller is the desired cart position z d and the output of
the controller is the desired pendulum angle θd . The input to the inner loop
controller is the desired pendulum angle θd and the output is the force F on
the cart.
(b) Focusing on the inner loop, find the PD gains kPθ and kDθ so that the rise time
of the inner loop is trθ = 0.5 seconds, and the damping ratio is ζθ = 0.707.
(c) Find the DC gain kDCθ of the inner loop.
(d) Replace the inner loop by its DC-gain and consider the outer loop. Add a
low-pass filter that cancels the left-half plane zero, and then design a PD
control strategy to stabilize the position of the cart.
(e) Implement the control design to control on the inverted pendulum. Modify
the simulation to include saturation on the force F to limit the size of the
input force on the cart to Fmax = 5 N. Give the pendulum angle an initial
condition of 10 deg and simulate the response of the system for 10 seconds
to verify that the cart is able to balance the pendulum.

Solution
The block diagram for the inner loop is shown in Fig. 8-14. The closed loop

Figure 8-14: Block diagram for inner loop of inverted pendulum control

transfer function from Θ̃r to Θ̃ is given by

k Pθ
− m ` +m 2`
16 2 3
Θ̃(s) =   Θ̃r (s).
kDθ (m1 +m2 )g kPθ
s2 − 2` s −
m1 6` +m2 3 m ` +m 2`
+ m1 6` +m2 2`
16 2 3 3

Hw_pendulum_PD_inner TOC
CHAPTER 8. SECOND ORDER DESIGN 117

Therefore the closed loop characteristic equation is

!
2 kDθ (m1 + m2 )g kPθ
∆cl (s) = s − s− +
m1 6 + m2 2`
`
3
` 2`
m1 6 + m2 3 m1 6 + m2 2`
`
3

The desired closed loop characteristic equation is

∆dcl (s) = s2 + 2ζθ ωnθ s + ωn2 θ ,

where
2.2
ωnθ = = 4.4
trθ
ζθ = 0.707.

Therefore
` 2`
kPθ = −(m1 + m2 )g − (m1 + m2 )ωn2 θ = −25.96
6 3
` 2`
kDθ = −2ζθ ωnθ (m1 + m2 ) = −4.407.
6 3
The DC gain of the inner loop is given by
kPθ
kDCθ = = 1.893.
(m1 + m2 )g + kPθ
Replacing the inner loop by its DC gain, the block diagram for the outer loop
is shown in Fig. 8-15. The closed loop transfer function from Z̃r to Z̃ is given by

Figure 8-15: Block diagram for outer loop of inverted pendulum control.

kPz 3g
kDz (s2 − 2` )
Z̃(s) = kPz 3kPz g
Z̃r .
s3 − s2 ( 2kDC3 kD + ) − s 3g
2` −
θ z` kDz 2kDz `

Note that the DC gain for the outer loop is equal to one. The closed-loop charac-
teristic equation is
Hw_pendulum_PD_outer
3 kPz 3g 3kPz g
∆cl (s) = s − s2 (
3
+ )−s − .
2kDCθ kDz ` kDz 2` 2kDz `

TOC
CHAPTER 8. SECOND ORDER DESIGN 118

Since the desired characteristic equation is

∆dcl (s) = s2 + 2ζz ωnz s + ωn2 z , (8.11)

it is not possible to pick PD gain values that will create a match between the actual
characteristic equation and the desired characteristic equation.
To resolve this problem, note that the transfer function of the outer loop can
be written as
q q
2` 3g 3g
2` 2
−3s +g − 3 (s + 2` )(s − 2l )
H(s) = = ,
s2 s2
where we see that the system has two zeros, one in the right-half plane and one in
the left-half plane. The strategy for making the numerator polynomial have degree
one is to add a low-pass filter after the PD controller that attempts to cancel the
left-half plane zero, as shown in Figure 8-16. After accounting for the cancelations

Figure 8-16: Block diagram for outer loop of inverted pendulum control, with
low-pass filter to cancel the LHP zero.

introduced by the low-pass filter, the equivalent closed-loop systems is shown in

Figure 8-17. In this case, the closed-loop characteristic equation from Z̃r to Z̃ is

Figure 8-17: Block diagram for the equivalent outer loop of inverted pendulum
control with low-pass filter.

given by  
  q
kpz 3g
 1+kdz (s − 2` ) 
Z̃(s) = 
  √ 3g  √ 3g 
 Z̃r .
kpz −kdz kpz
s2 − 1+kdz
2`
s− 2`
1+kdz

TOC
 CHAPTER 8. SECOND ORDER DESIGN 119

Setting the closed-loop characteristic equation equal to the desire closed-loop

characteristic equation in Equation (8.11) gives
s
k pz 2 2` 4
= −ωnz =a (8.12)
1 + kdz 3g
 s
kdz kpz 2` 4
= − 2ζz ωnz = b. (8.13)
1 + kdz 1 + kdz 3g

Then
b
kdz =
1−b
kpz = a(1 + kdz ).

A Python class that implements a PD controller for the inverted pendulum is

shown below.
1 import numpy as np
2 import pendulumParam as P
3 import pendulumParamHW8 as P8
4
5 class pendulumController:
6 def __init__(self):
7 self.kp_z = P8.kp_z
8 self.kd_z = P8.kd_z
9 self.kp_th = P8.kp_th
10 self.kd_th = P8.kd_th
11 self.filter = zeroCancelingFilter()
12
13 def update(self, z_r, state):
14 z = state.item(0)
15 theta = state.item(1)
16 zdot = state.item(2)
17 thetadot = state.item(3)
18
19 # the reference angle for theta comes from the
20 # outer loop PD control
21 tmp = self.kp_z * (z_r - z) - self.kd_z * zdot
22
23 # low pass filter the outer loop to cancel
24 # left-half plane zero and DC-gain
25 theta_r = self.filter.update(tmp)
26
27 # the force applied to the cart comes from the
28 # inner loop PD control
29 F = self.kp_th * (theta_r - theta) - self.kd_th * thetadot
30 return F
31
32 class zeroCancelingFilter:
33 def __init__(self):
34 self.a = -3.0/(2.0*P.ell*P8.DC_gain)
35 self.b = np.sqrt(3.0*P.g/(2.0*P.ell))
36 self.state = 0.0

TOC
 CHAPTER 8. SECOND ORDER DESIGN 120

37
38 def update(self, input):
39 # integrate using RK1
40 self.state = self.state \
41 + P.Ts * (-self.b*self.state + self.a*input)
42 return self.state

Listing 8.3: pendulumController.py

Python code that implements the closed loop system is given below.

1 import sys
2 sys.path.append(’..’) # add parent directory
3 import matplotlib.pyplot as plt
4 import pendulumParam as P
5 from hw3.pendulumDynamics import pendulumDynamics
6 from pendulumController import pendulumController
7 from hw2.signalGenerator import signalGenerator
8 from hw2.pendulumAnimation import pendulumAnimation
9 from hw2.dataPlotter import dataPlotter
10
11 # instantiate pendulum, controller, and reference classes
12 pendulum = pendulumDynamics()
13 controller = pendulumController()
14 reference = signalGenerator(amplitude=0.5, frequency=0.02)
15 disturbance = signalGenerator(amplitude=0)
16
17 # instantiate the simulation plots and animation
18 dataPlot = dataPlotter()
19 animation = pendulumAnimation()
20
21 t = P.t_start # time starts at t_start
22 y = pendulum.h() # output of system at start of simulation
23
24 while t < P.t_end: # main simulation loop
25 # Propagate dynamics in between plot samples
26 t_next_plot = t + P.t_plot
27
28 while t < t_next_plot:
29 r = reference.sin(t) # reference input
30 d = disturbance.step(t) # input disturbance
31 n = 0.0 #noise.random(t) # simulate sensor noise
32 x = pendulum.state # use state instead of output
33 u = controller.update(r, x) # update controller
34 y = pendulum.update(u + d) # propagate system
35 t = t + P.Ts # advance time by Ts
36
37 # update animation and data plots
38 animation.update(pendulum.state)
39 dataPlot.update(t, r, pendulum.state, u)
40 plt.pause(0.0001)
41
42 # Keeps the program from closing until the user presses a button.
43 print(’Press key to close’)
44 plt.waitforbuttonpress()
45 plt.close()

TOC
CHAPTER 8. SECOND ORDER DESIGN 121

Listing 8.4: pendulumSim.py

Complete simulation code for Matlab, Python, and Simulink can be down-
loaded at http://controlbook.byu.edu.

8.4 Design Study C. Satellite Attitude Control

Example Problem C.8

(a) For this problem and future problems, change the satellite spring constant
to k = 0.1 N m. For the simple satellite system, using the principle of
successive loop closure, draw a block diagram that uses PD control for the
inner and the outer loop control. The input to the outer loop controller is
the desired angle of the solar panel φr and the output is the desired angle of
the satellite θr . The input to the inner loop controller is θr and the output is
the torque on the satellite τ .

(b) Focusing on the inner loop, find the PD gains kPθ and kDθ so that the rise
time of the inner loop is trθ = 1 second, and the damping ratio is ζθ = 0.9.

(c) Find the DC gain kDCθ of the inner loop.

(d) Replacing the inner loop by its DC-gain, find the PD gains kPφ and kDφ
so that the rise time of the outer loop is trφ = 10trθ with damping ratio
ζφ = 0.9.

(f) Implement the successive loop closure design for the satellite system in sim-
ulation where the commanded solar panel angle is given by a square wave
with magnitude 15 degrees and frequency 0.015 Hz.

(g) Suppose that the size of the input torque on the satellite is limited to τmax =
5 Nm. Modify the simulation to include saturation on the torque τ . Using
the rise time of the outer loop as a tuning parameter, tune the PD control
law to get the fastest possible response without input saturation when a step
of size 30 degrees is placed on φr .

Solution
The block diagram for the inner loop is shown in Fig. 8-18. The closed loop
transfer function from Θd to Θ is given by
kPθ
Js +Jp
Θ(s) =     Θd (s).
kDθ kPθ
s2 + Js +Jp s+ Js +Jp

TOC
CHAPTER 8. SECOND ORDER DESIGN 122

Figure 8-18: Block diagram for inner loop of satellite control

Therefore the closed loop characteristic equation is

   
kDθ kPθ
∆cl (s) = s2 + s+ .
Js + Jp Js + Jp

The desired closed loop characteristic equation is

∆dcl (s) = s2 + 2ζθ ωnθ s + ωn2 θ ,

where

1 π
ω nθ = p
2 trθ 1 − ζθ2
ζθ = 0.9.

Therefore

kPθ = ωn2 θ (Js + Jp ) = 77.9

kDθ = 2ζθ ωnθ (Js + Jp ) = 38.9.

The DC gain of the inner loop is given by

kDCθ = 1.

Replacing the inner loop by its DC gain, the block diagram for the outer loop
is shown in Fig. 8-19. To find the closed loop transfer function from φd to φ,

⇣ ⌘
r ✓d b k
kP kDC✓ Jp s + Jp
b k
s2 + Jp s + Jp
kD s

Figure 8-19: Block diagram for outer loop of satellite attitude control

TOC
CHAPTER 8. SECOND ORDER DESIGN 123

follow the loop backwards from φ to obtain

b k
!
Jp s + Jp  
Φ(s) = 2 b k
kDCθ kPφ (Φr − Φ) − kDCθ kDφ sΦ .
s + Jp s + Jp

After some manipulation we get

 
(Jp + bkDCθ kDφ )s2 + (b + bkDCθ kPφ + kkDCθ kDφ )s + (k + kkDCθ kPφ ) Φ
 
= bkDCθ kPφ s + kkDCθ kPφ Φr , (8.14)

which results in the monic transfer function

 bk   kk 
DCθ kPφ DCθ kPφ
Jp +bkDCθ kDφ s + Jp +bkDCθ kDφ
Φ(s) =  b+bk   k+kk  Φr .
DCθ kPφ +kkDCθ kDφ DCθ kPφ
s2 + Jp +bkDC kD s + Jp +bkDC kD
θ φ θ φ

The desired closed loop characteristic equation for the outer loop is

∆dcl (s) = s2 + 2ζφ ωnφ s + ωn2 φ ,

where

trφ = 10 trθ = 10
1 π
ω nφ = q
2t 1 − ζ2
rφ φ

ζφ = 0.9.

Therefore the gains kPφ and kPφ satisfy

k + kkDCθ kPφ
= ωn2 φ
Jp + bkDCθ kDφ
b + bkDCθ kPφ + kkDCθ kDφ
= 2ζφ ωnφ .
Jp + bkDCθ kDφ

Expressing these equations in matrix form gives

    
kkDCθ −bkDCθ ωn2 φ kPφ −k + Jp ωn2 φ
= ,
bkDCθ kkDCθ − 2bkDCθ ζφ ωnφ kDφ −b + 2Jp ζφ ωnφ

implying that
   −1  
kPφ kkDCθ −bkDCθ ωn2 φ −k + Jp ωn2 φ
=
kDφ bkDCθ kkDCθ − 2bkDCθ ζφ ωnφ −b + 2Jp ζφ ωnφ
 
0.835
=
8.254

TOC
 CHAPTER 8. SECOND ORDER DESIGN 124

The DC gain of the outer loop is given by

kkDCθ kPφ
kDCφ = = 0.455.
k + kkDCθ kPφ

Note that the DC gain of the outer loop is not equal to one, so there will be signif-
icant steady state error. To remedy this, a feedforward term can be used to ensure
that θ and φ are made to be equal in steady state, resulting in an overall DC gain
of one. This configuration is depicted in Fig. 8-20. Feedforward control will be
discussed in a later chapter.

Figure 8-20: Block diagram for outer loop of satellite attitude control with a feed-
forward term.

A Python class that implements a PD controller using state feedback is shown

below.
1 import satelliteParamHW8 as P
2
3 class satelliteController:
4 def __init__(self):
5 self.kp_phi = P.kp_phi
6 self.kd_phi = P.kd_phi
7 self.kp_th = P.kp_th
8 self.kd_th = P.kd_th
9
10 def update(self, phi_r, state):
11
12 theta = state.item(0)
13 phi = state.item(1)
14 thetadot = state.item(2)
15 phidot = state.item(3)
16
17 # outer loop: outputs the reference angle for theta
18 # note that book recommends a feed forward term because
19 # of poor DC gain on the outer loop which
20 # is why we add an addition "phi_r" at the end
21 theta_r = self.kp_phi * (phi_r - phi) \
22 - self.kd_phi * phidot + phi_r
23
24 # inner loop: outputs the torque applied to the base
25 tau = self.kp_th * (theta_r - theta) \
26 - self.kd_th * thetadot
27
28 return tau

TOC
 CHAPTER 8. SECOND ORDER DESIGN 125

Listing 8.5: satelliteController.py

Python code that implements the closed loop system is given below.
1 import sys
2 sys.path.append(’..’) # add parent directory
3 import matplotlib.pyplot as plt
4 import numpy as np
5 import satelliteParam as P
6 from hw3.satelliteDynamics import satelliteDynamics
7 from satelliteController import satelliteController
8 from hw2.signalGenerator import signalGenerator
9 from hw2.satelliteAnimation import satelliteAnimation
10 from hw2.dataPlotter import dataPlotter
11
12 # instantiate satellite, controller, and reference classes
13 satellite = satelliteDynamics()
14 controller = satelliteController()
15 reference = signalGenerator(amplitude=15.0*np.pi/180.0,
16 frequency=0.02)
17 disturbance = signalGenerator(amplitude=1.0)
18
19 # instantiate the simulation plots and animation
20 dataPlot = dataPlotter()
21 animation = satelliteAnimation()
22
23 t = P.t_start # time starts at t_start
24 y = satellite.h() # output of system at start of simulation
25
26 while t < P.t_end: # main simulation loop
27 # Propagate dynamics in between plot samples
28 t_next_plot = t + P.t_plot
29
30 # updates control and dynamics at faster simulation rate
31 while t < t_next_plot:
32 r = reference.square(t) # reference input
33 d = disturbance.step(t) # input disturbance
34 n = 0.0 #noise.random(t) # simulate sensor noise
35 x = satellite.state
36 u = controller.update(r, x) # update controller
37 y = satellite.update(u + d) # propagate system
38 t = t + P.Ts # advance time by Ts
39
40 # update animation and data plots
41 animation.update(satellite.state)
42 dataPlot.update(t, r, satellite.state, u)
43
44 # the pause causes the figure to display for the simulation.
45 plt.pause(0.0001)
46
47 # Keeps the program from closing until the user presses a button.
48 print(’Press key to close’)
49 plt.waitforbuttonpress()
50 plt.close()

Listing 8.6: satelliteSim.py

TOC
CHAPTER 8. SECOND ORDER DESIGN 126

Complete simulation code for Matlab, Python, and Simulink can be down-
loaded at http://controlbook.byu.edu.

Important Concepts:
• The DC-gain of a transfer function is equal to the limit as s → 0 of
the transfer function.
• If a transfer function contains only stable poles (those in the left half
plane) then the steady-state response to a unit step input will equal
the DC-gain.
• The time response y(t) to a step input is found by taking the inverse
Laplace transform of the transfer function multiplied by the step in-
put. It is useful to use partial fraction expansion to get the combined
system into a form that is easily invertible to the time domain.
• The characteristic equation of a second order system can be written
in terms of its natural frequency and damping ratio.
• The natural frequency most strongly affects the system’s rise time,
while the damping ratio mainly affects how much overshoot and os-
cillation occur in the response.
• Zeros can affect the system’s overshoot and rise time. They will most
strongly affect the poles that are positioned nearby.

Notes and References

TOC
 System Type and Integrators
9
Learning Objectives:
• Compute a system’s type with respect to reference tracking, distur-
bances, and noise.
• Understand how the system type will affect the system’s ability to
track different types of inputs such as steps, ramps, and parabolas.

9.1 Theory
In the previous chapter we explored design techniques for second order systems
where the feedback controller was a PD-type control. Since second order systems
have two degrees of freedom, their transient response can be completely deter-
mined by the proportional and derivative gains. However, from the simulation
results it can easily be observed that if there is a disturbance on the system, or if
the controller does not know the system parameters exactly, then there will be a
steady state error in the output. The steady state error can be effectively removed
using an integrator in the control law. In this chapter we will explore these ideas
in more detail and explain when and how integral control can be used to enhance
steady state tracking and to reduce the effect of disturbances.

9.1.1 Final Value Theorem

The key theoretical tool to understanding the steady state error and the effect of
integrators is the final value theorem, which we state and prove below.
Final Value Theorem.
Suppose that z(t) and Z(s) are Laplace transform pairs, and suppose that limt→∞ z(t)
is finite, then
lim z(t) = lim sZ(s).
t→∞ s→0

TOC
CHAPTER 9. INTEGRATORS 128

Proof.
From the fundamental theorem of calculus we have that for t ≥ 0
Z t
z(t) − z(0) = dz.
0

Therefore
Z t
dz
lim z(t) = z(0) + lim dτ
t→∞ t→∞ 0 dτ
Z t
= z(0) + lim ż(τ )dτ
t→∞ 0
Z t
= z(0) + lim lim ż(τ )e−sτ dτ
t→∞ 0 s→0
Z t
= z(0) + lim lim ż(τ )e−sτ dτ
s→0 t→∞ 0
Z ∞
= z(0) + lim ż(τ )e−sτ dτ
s→0 0
= z(0) + lim (sZ(s) − z(0))
s→0
= lim sZ(s),
s→0

which completes the proof.

9.1.2 System Type for Reference Tracking

r(t) e(t) u(t) y(t)

C(s) P (s)

Figure 9-1: Block diagram for reference tracking.

Consider the closed-loop system shown in Fig. 9-1. The transfer function from
the reference r to the error e = r − y is computed as

E(s) = R(s) − P (s)C(s)E(s)

1
=⇒ E(s) = R(s)
1 + P (s)C(s)
Step Input.
1
Suppose that r(t) is a step of size one, then R(s) = s and

1 1
E(s) = .
1 + P (s)C(s) s

TOC
CHAPTER 9. INTEGRATORS 129

Assuming that the closed-loop system is stable and therefore that limt→∞ e(t) is
finite, then the final value theorem gives
lim e(t) = lim sE(s)
t→∞ s→0
1 1
= lim s
s→0 1 + P (s)C(s) s
1
= lim
s→0 1 + P (s)C(s)
1
= .
1 + lims→0 P (s)C(s)
Let
4
Mp = lim P (s)C(s),
s→0
then
1
lim e(t) = .
t→∞ 1 + Mp
Now suppose that
K(s + z1 )(s + z2 ) · · · (s + zm )
P (s)C(s) = ,
(s + p1 )(s + p2 ) · · · (s + pn )
then
Kz1 z2 · · · zm
Mp = lim P (s)C(s) = .
s→0 p1 p2 · · · pn
1
In this case, the steady state error is 1+Mp which is finite. Note also that the
DC-gain of the closed-loop system is
1
kDC = .
1 + Mp
Now suppose that P (s)C(s) contains a pole at the origin, or in other words,
one free integrator, i.e.,
K(s + z1 )(s + z2 ) · · · (s + zm )
P (s)C(s) = .
s(s + p1 )(s + p2 ) · · · (s + pn )
In this case we have
Mp = lim P (s)C(s) = ∞,
s→0
and the steady state error is
1
lim e(t) = = 0.
t→∞ 1+∞
Therefore, one free integrator in P (s)C(s) implies that the steady state error to a
step is zero. Note that multiple free integrators, i.e.,
K(s + z1 )(s + z2 ) · · · (s + zm )
P (s)C(s) = ,
sq (s + p1 )(s + p2 ) · · · (s + pn )

TOC
CHAPTER 9. INTEGRATORS 130

gives that same result for q ≥ 1.

Ramp Input.
1
Now suppose that r(t) is a unit ramp, or in other words that R(s) = s2 . In
this case the error is given by
1 1
E(s) = .
1 + P (s)C(s) s2
Again assuming that the closed-loop system is stable, the final value theorem gives
lim e(t) = lim sE(s)
t→∞ s→0
1 1
= lim s
s→01 + P (s)C(s) s2
1
= lim
s→0 s + sP (s)C(s)
1
= .
lims→0 sP (s)C(s)
Defining
Mv = lim sP (s)C(s),
s→0
gives
1
lim e(t) = .
t→∞ Mv
If P (s)C(s) has no free integrators, i.e.,
K(s + z1 )(s + z2 ) · · · (s + zm )
P (s)C(s) =
(s + p1 )(s + p2 ) · · · (s + pn )
then
K(s + z1 )(s + z2 ) · · · (s + zm )
Mv = lim s = 0.
s→0 (s + p1 )(s + p2 ) · · · (s + pn )
Therefore
1
lim e(t) = = ∞.
t→∞ 0
If on the other hand P (s)C(s) has one free integrator, i.e.,
K(s + z1 )(s + z2 ) · · · (s + zm )
P (s)C(s) =
s(s + p1 )(s + p2 ) · · · (s + pn )
then
K(s + z1 )(s + z2 ) · · · (s + zm ) Kz1 zd · · · zm
Mv = lim s = ,
s→0 s(s + p1 )(s + p2 ) · · · (s + pn ) p1 p2 · · · pn
therefore the steady state error is finite. If P (s)C(s) has two or more free integra-
tors, then
K(s + z1 )(s + z2 ) · · · (s + zm )
Mv = lim s q = ∞,
s→0 s (s + p1 )(s + p2 ) · · · (s + pn )

TOC
CHAPTER 9. INTEGRATORS 131

when q ≥ 2, therefore
1
lim e(t) = = 0.
t→∞ ∞
Therefore, the number of free integrators in P (s)C(s) is the key parameter for
reference tracking with zero steady state error.
For the reference tracking problem, we say that the system is type q if the
1
steady state error to input R(s) = sq+1 is finite and if the steady state error to
1
input R(s) = sp is zero for 1 ≤ p ≤ q. In general, for the feedback system shown
1
in Fig. 9-1, the steady state tracking error when R(s) = sq+1 is given by
  
1 1
lim e(t) = lim sE(s) = lim .
t→∞ s→0 s→0 1 + P (s)C(s) sq
Table 9-1 summarizes the tracking error for different inputs as a function of system
type, where

Mp = lim P (s)C(s)
s→0
Mv = lim sP (s)C(s)
s→0
Ma = lim s2 P (s)C(s).
s→0

Table 9-1: Reference tracking verse system type.

System Type Step ( 1s ) Ramp ( s12 ) Parabola ( s13 ) ···

1
0 1+Mp ∞ ∞ ···
1
1 0 Mv ∞ ···
1
2 0 0 Ma ···
3 0 0 0 ···
.. .. .. ..
. . . .

As an example, consider the feedback system shown in Fig. 9-2. The open

r(t) 1 y(t)
kP s(s + 2)

Figure 9-2: The system type for reference tracking for this system is type 1.

loop system
kP
P (s)C(s) =
s(s + 2)

TOC
CHAPTER 9. INTEGRATORS 132

has one free integrator (pole at zero) and therefore the system is type 1. From
Table 9-1 the tracking error when the input is a step is zero, and the tracking error
when the input is a ramp of slope one is
1 1 1 2
lim e(t) = = = kP = .
t→∞ Mv lims→0 sP (s)C(s) 2
k P

The tracking error when the input is a parabola, or higher-order polynomial, is ∞,

meaning that y(t) and r(t) diverge as t → ∞.
As another example, consider the system shown in Fig. 9-3.

r(t) 1 y(t)
kD s + kP (s + 3)(s + 4)

Figure 9-3: The system type for reference tracking for this system is type 0.

In this case, the open loop system

kD s + kP
P (s)C(s) =
(s + 3)(s + 4)
does not have any free integrators (poles at zero) and therefore the system is type 0.
From Table 9-1 the tracking error when the input is a step is
1 1 1 12
lim e(t) = = = k
= .
t→∞ 1 + Mp 1 + lims→0 P (s)C(s) 1 + 12 P 12 + kP

The tracking error when the input is a ramp, or higher-order polynomial, is ∞,

meaning that y(t) and r(t) diverge as t → ∞.
If on the other hand, a PID controller is used as shown in Fig. 9-4 where
   
kI kD s2 + kP s + kI
U (s) = kP + + kD s E(s) = E(s),
s s
then the open loop system

r(t) kD s 2 + kP s + kI 1 y(t)
s (s + 3)(s + 4)

Figure 9-4: The system type for reference tracking for this system is type 1.

kD s2 + kP s + kI
P (s)C(s) =
s(s + 3)(s + 4)

TOC
CHAPTER 9. INTEGRATORS 133

has one free integrator (pole at zero) and therefore the system becomes type 1 by
design. From Table 9-1 the tracking error when the input is a step is zero, and the
tracking error when the input is a ramp is
1 1 1 12
lim e(t) = = = kI = .
t→∞ Mv lims→0 sP (s)C(s) 12
kI

The tracking error when the input is a parabola, or higher-order polynomial, is ∞,

meaning that y(t) and r(t) diverge as t → ∞.

9.1.3 System Type for Input Disturbances

Consider the general feedback loop shown in Fig. 9-5. There are in general four
inputs to the system, namely the reference input r(t), the input disturbance din (t),
the output disturbance dout (t), and the sensor noise n(t). In general r(t), din (t),
and dout (t) are signals with low frequency content, and the sensor noise n(t) has
high frequency content. The negative sign on the disturbance and noise terms is
for notational convenience.

din (t) dout (t)

u(t) y(t)
r(t) e(t)
C(s) P (s)

n(t)

Figure 9-5: General feedback loop with reference input r(t), input disturbance
din (t), output disturbance dout (t), and sensor noise n(t).

We can compute the transfer function from each input to the error signal by
following the signal flow to obtain

E(s) = R(s) + N (s) + Dout (s) + P (s)Din (s) − P (s)C(s)E(s).

Solving for E(s) gives

1 P (s)C(s)
E(s) = R(s) + N (s)
1 + P (s)C(s) 1 + P (s)C(s)
1 P (s)
+ Dout (s) + Din (s).
1 + P (s)C(s) 1 + P (s)C(s)

Since the transfer function from N (s) and Dout (s) to E(s) is identical to the
transfer function from R(s) to E(s), the system type as defined in the previous
section for reference inputs, also characterizes the steady state response to output
disturbances and noise. However, the transfer function from the input disturbance

TOC
CHAPTER 9. INTEGRATORS 134

to the error is different, and so we need to further analyze system type with respect
to input disturbances.
1
Suppose that the input disturbance is given by Din (s) = sq+1 , then by the
final value theorem, the steady state error due to the input disturbance is

  
P (s) P (s) 1
lim e(t) = lim s Din (s) = lim .
t→∞ s→0 1 + P (s)C(s) s→0 1 + P (s)C(s) sq

The system type with respect to input disturbance is the value of q such that the
steady state error is finite, and the steady state error is zero for Din (s) = s1p for
1 ≤ p ≤ q.
For example, consider the system shown in Fig. 9-6. Recall from the discus-
sion above, that the system type with respect to the reference input is type 1. For

din (t)

r(t) e(t) 1 y(t)

kD s + kP
s(s + 2)

Figure 9-6: The system type for input disturbances for this system is type 0.

the input disturbance we have

  
P (s) 1
lim e(t) = lim
t→∞ s→0 1 + P (s)C(s) sq
 
1  
s(s+2) 1
= lim    
s→0
1+ 1
(kD s + kP ) sq
s(s+2)
  
1 1
= lim
s→0 s(s + 2) + (kD s + kP ) sq
  
1 1
= lim
s→0 kP sq

which is finite when q = 0. Therefore, the system type with respect to input
disturbances is type 0 and the steady state error to a step input disturbance is k1P
whereas the steady state error with respect to a ramp and higher-order polynomials
is infinite.

TOC
CHAPTER 9. INTEGRATORS 135

If on the other hand, we add an integrator as in Fig. 9-7, then

  
P (s) 1
lim e(t) = lim
t→∞ s→0 1 + P (s)C(s) sq
 
1  
 s(s+2)
 1
= lim  
s→0 1
1 + s(s+2)
2
( kD s +ks P s+kI ) sq
  
s 1
= lim
s→0 s2 (s + 2) + (kD s2 + kP s + kI ) sq
  
1 1
= lim
s→0 kI sq−1

which is finite when q = 1. Therefore, the system type with respect to input dis-
turbances is type 1 and the steady state error to a step on the input disturbance is
zero. The steady state error with respect to a ramp on the input disturbance is k1I
whereas the steady state error with respect to a parabola and higher-order polyno-
mials on the input disturbance is infinite. Therefore, the system type with respect

din (t)

r(t) e(t) kD s 2 + kP s + kI 1 y(t)

s s(s + 2)

Figure 9-7: The system type for input disturbances for this system is type 1.

to input disturbances is related to the number of free integrators in the controller

C(s) and not in the open loop system P (s)C(s). As a consequence, while an
integrator may not be necessary for steady state tracking, it is often necessary to
reject constant input disturbances.

9.2 Design Study A. Single Link Robot Arm

Example Problem A.9

(a) When the controller for the single link robot arm is PD control, what is the
system type? Characterize the steady state error when the reference input
is a step, a ramp, and a parabola. How does this change if you add an
integrator?

TOC
CHAPTER 9. INTEGRATORS 136

(b) Consider the case where a constant disturbance acts at the input to the plant
(for example gravity in this case). What is the steady state error to a constant
input disturbance when the integrator is not present and when it is present?

Solution
The closed loop system is shown in Fig. 9-8. Without the integrator, the open-

d(t)

✓r e(t) kD s2 + kP s + kI 3 ✓
m`2
s s(s + 3b
m`2 )

Figure 9-8: Closed loop system for problem HW A.9.

loop transfer function is given by

!
3
m`2
P (s)C(s) = 3b
(kD s + kP ) .
s(s + m`2 )

The system has one free integrator and is therefore type 1, which, from Table 9-1
implies that the tracking error when the input is a step is zero, and the tracking
error when the input is a ramp is

1 1 1 b
lim e(t) = = = kP = .
t→∞ Mv lims→0 sP (s)C(s) b
kP

The tracking error when the input is a parabola, or higher order polynomial, is ∞,
meaning that θ(t) and θr (t) diverge as t → ∞.
With the integrator, the open loop transfer function is
! 
3
m`2 kD s2 + kP s + kI
P (s)C(s) = 3b
.
s(s + m` 2) s

which has two free integrators and is therefore type 2. Therefore, from Table 9-1
the tracking error when the input is either a step or a ramp is zero, and the tracking
error when the input is a parabola is

1 1 1 b
lim e(t) = = 2
= kI = .
t→∞ Ma lims→0 s P (s)C(s) b
kI

The tracking error when the input is t3 or a higher order polynomial, is ∞.

TOC
CHAPTER 9. INTEGRATORS 137

For the input disturbance, the transfer function from D(s) to E(s) is given by
P (s)
E(s) = D(s).
1 + P (s)C(s)
A
Without the integrator, and when D(s) = sq+1 , the steady state error is given by
P A
lim e(t) = lim
t→∞ s→0 1 + P C sq
  3
 
m`2  
 3b
s(s+ m` 2 )  A
= lim   
s→0  3
(kD s+kP )  sq
m`2
1+ 3b
s(s+ m` 2)
! 
3
m`2 A
= lim 3b 3
s→0 s(s + m`2 ) + m` 2 (kD s + kP ) sq
3
A m` 2 A
= 3kP
= ,
m`2
kP
if q = 0. Therefore, to an input disturbance the system is type 0. The steady state
error when a constant step of size A is placed on d(t) is kAP .
A
With the integrator, and when D(s) = sq+1 , the steady state error is given by
P A
lim e(t) = lim
t→∞ s→0 1 + P C sq
  3
 
m`2  
 3b
s(s+ m` 2 )  A
= lim    
s→0  3
kD s2 +kP s+kI
 sq
m`2
1+ 3b
s(s+ m` s
2)
! 
3
m`2 A
= lim s 3b 3
s→0 s2 (s + m`2 ) + m`2 (kD s
2 + kP s + kI ) sq
3
A m` 2 A
= 3kI
= ,
m`2
kI
if q = 1. Therefore, to an input disturbance the system is type 1. The steady state
error when d(t) is a constant step is zero, and the steady state error when d(t) is a
ramp of slope size A is kAI .

9.3 Design Study B. Inverted Pendulum

Example Problem B.9

TOC
CHAPTER 9. INTEGRATORS 138

(a) When the inner loop controller for the inverted pendulum is PD control, what
is the system type with respect to tracking of the inner loop? Characterize
the steady state error when θ̃r is a step, a ramp, and a parabola. What is the
system type with respect to an input disturbance?

(b) When the outer loop controller for the inverted pendulum is PD control, what
is the system type of the outer loop with respect to tracking? Characterize
the steady state error when z r is a step, a ramp, and a parabola. How does
this change if you add an integrator? What is the system type with respect
to an input disturbance for both PD and PID control?

Solution

The block diagram for the inner loop is shown in Fig. 9-9. The open loop system

Figure 9-9: Inner loop system for problem HW B.9.

is given by
 
1
−m ` 2`
1 6 +m2 3
P (s)C(s) =  (m1 +m2 )g
 (kD s + kP ) .
s2 − m1 6` +m2 2`
3

Since there are no free integrators, the system is type 0, and from Table 9-1 the
hw_pendulum_system_type_inner
tracking error when the input is a step is

1 1 1
lim e(t) = = = kP
.
t→∞ 1 + Mp 1 + lims→0 P (s)C(s) 1+ (m1 +m2 )g

The tracking error when the input is a ramp, or higher order polynomial, is ∞.

TOC
CHAPTER 9. INTEGRATORS 139

For the disturbance input, the steady state error to a step on d(t) is

P (s) 1
lim e(t) = lim s
t→∞ s→0 1 + P (s)C(s) s
1
!
−
m1 ` +m2 2`
6 3
(m1 +m2 )g
s2 −
m1 ` +m2 2`
= lim 1
6
! 3

s→0 −
m1 ` +m2 2`
1+ 6 3
(m1 +m2 )g (kD s + kP )
s2 −
m1 +m2 2`
`
6 3
 
1
− m ` +m 2`
= lim   1 6 2 3 
s→0 (m +m )g 1
s2 − m ` +m 2` + − m ` +m
1 2
2` (kD s + kP )
1 2 3 16 2 3
 6 
1
− m ` +m 2` 1
16
=   2 3 = .
(m1 +m2 )g
− m ` +m 2` + − m ` +m 2`
kp (m 1 + m 2 )g + kP
16 2 3 16 2 3

The system is type 0 with respect to the input disturbance.

The block diagram for the outer loop is shown in Fig. 9-10. The open loop

Figure 9-10: Outer loop system for problem HW B.9.

system is given by
! 
−2`s2
3 + g kD s2 + kP s + kI
P (s)C(s) = 2
.
s s

When kI = 0 there are two free integrators in P (s)C(s) and the system is type 2,
and from Table 9-1 the tracking
hw_pendulum_system_type_outer
error when the input is a parabola is

1 1 1
lim e(t) = = 2
= .
t→∞ Ma lims→0 s P (s)C(s) −kP g

When kI 6= 0, there are three free integrators in P (s)C(s) and the system is
type 3, with zero tracking error for a step, ramp, and parabola input.

TOC
CHAPTER 9. INTEGRATORS 140

1
For the disturbance input, the steady state error when D(s) = sq+1 is

P (s) 1
lim e(t) = lim s q+1
t→∞ s→0 1 + P (s)C(s) s
 2 
2`s
3 −g
s2 1
= lim    sq .
s→0 2`s2
3 −g kD s2 +kP s+kI
1+ s2 s

Without the integrator, i.e., when kI = 0 we have

2`s2
3 −g 1 g 1
lim e(t) = lim 2`s2

q
= lim
t→∞ s→0 s2 + 3 − g (kD s + kP ) s s→0 g (kP ) sq

which is finite when q = 0. Therefore, the system is type 0 with respect to the
input disturbance and the steady state error when d2 (t) is a unit step is 1/kP .
When kI 6= 0, we have
2
s( 2`s − g) 1 g 1
lim e(t) = lim 2`s2

3 q
= lim q−1
t→∞ s→0 s3 + 3
2
− g (kD s + kP s + kI ) s s→0 g (k I ) s

which is finite when q = 1. Therefore, the system is type 1 with respect to the
input disturbance and the steady state error when d2 (t) is a unit ramp is 1/kI .

9.4 Design Study C. Satellite Attitude Control

Example Problem C.9

(a) When the inner loop controller of the satellite system is PD control, what is
the system type of the inner loop with respect to the reference input and
with respect to the disturbance input? Characterize the steady state error
when the reference input is a step, a ramp, and a parabola. Characterize
the steady state error when the input disturbance is a step, a ramp, and a
parabola.

(b) With PD control for the outer loop, what is the system type of the outer loop
with respect to the reference input and with respect to the disturbance input?
Characterize the steady state error when the reference input and disturbance
input is a step, a ramp, and a parabola. How does this change if you add an
integrator?

TOC
CHAPTER 9. INTEGRATORS 141

Figure 9-11: Inner loop system for problem HW C.9.

Solution
The block diagram for the inner loop is shown in Fig. 9-11. The open loop system
is given by  
1
P (s)C(s) = (kD s + kP ) .
(Js + Jp )s2
Since there are two free integrators, the system is type 2, and from Table 9-1 the
tracking error when the input is a step is

1 1
lim e(t) = = = 0.
t→∞ 1 + Mp 1 + lims→0 P (s)C(s)

The tracking error when the input is a ramp is

1 1
lim e(t) = = = 0.
t→∞ Mv lims→0 sP (s)C(s)

Finally, the tracking error when the input is a parabola is

1 1 Jp + Js
lim e(t) = = = .
t→∞ Ma lims→0 s2 P (s)C(s) kp

The tracking error for higher order polynomials is ∞. For a disturbance input of
D(s) = 1/sq+1 , the steady state error is

P (s) 1 1 1
lim e(t) = lim s = lim
t→∞ s→0 1 + P (s)C(s) sq+1 s→0 (Js + Jp )s2 + kd s + kp sq

which is finite and equal to 1/kP when q = 0. Therefore, the system is type 0
with respect to the input disturbance, and the steady state error to a step on d(t) is
1/kP which is infinite to a ramp and higher order polynomials.
The block diagram for the outer loop is shown in Fig. 9-12. The open loop
system is given by
b k
! 
JP s + Jp kD s2 + kP s + kI
P (s)C(s) = .
s2 + Jb s + Jk
p p
s

TOC
CHAPTER 9. INTEGRATORS 142

Figure 9-12: Outer loop system for problem HW C.9.

When kI = 0 there are no free integrators in P (s)C(s) and the system is type 0,
and from Table 9-1 the tracking error when the input is a step is

1 1 1
lim e(t) = = = .
t→∞ 1 + Mp 1 + lims→0 P (s)C(s) 1 + kP

When the reference is a ramp or higher order polynomial, the tracking error is
infinite. When kI 6= 0, there is one free integrator in P (s)C(s) and the system is
type 1. From Table 9-1 the tracking error when the input is a ramp is

1 1 1
lim e(t) = = = .
t→∞ Mv lims→0 sP (s)C(s) kI

The tracking error when φr is a step is zero, and it is infinite when φr is a parabola
or higher order polynomial.
1
For the disturbance input, the steady state error when D(s) = sq+1 is

P (s) 1
lim e(t) = lim s q+1
t→∞ s→0 1 + P (s)C(s) s
 b k

JP s+ Jp
s2 + Jbp s+ Jkp 1
= lim  b
  sq .
s→0 JP s+ Jkp kD s2 +kP s+kI
1+ s2 + Jbp s+ Jkp s

Without the integrator, i.e., when kI = 0 we have

b k
Jp s + Jp 1
lim e(t) = lim b k
t→∞ s→0 (s2 + Jp s + Jp ) + ( JbP s+ k
Jp )(kD s + kP ) sq
1 1
= lim
s→0 1 + kP sq

which is finite when q = 0. Therefore, the system is type 0 with respect to the
input disturbance and the steady state error when d2 (t) is a unit step is 1/(1+kP ).

TOC
CHAPTER 9. INTEGRATORS 143

When kI 6= 0, we have
 
b k
s Jp s + Jp 1
lim e(t) = lim     q
t→∞ s→0
s s2 + b
Jp s + k
Jp + b
Jp s + k
Jp (kD s2 + kP s + kI ) s
1 1
= lim
s→0 kI sq−1
which is finite when q = 1. Therefore, the system is type 1 with respect to the
input disturbance and the steady state error when d2 (t) is a unit ramp is 1/kI . It
is zero when d2 is a step, and it is infinite when d2 is a parabola or higher order
polynomial.

Important Concepts:
• The final value theorem relates a transfer function’s frequency-
domain behavior to its steady-state time-domain behavior.
• The system type describes the type of input (step, ramp, parabola)
that produces a finite steady-state error. For input types of higher-
order the steady-state error will be infinite. For input types of lower-
order the steady-state error will be zero.
• In reference tracking the system type is equal to the number of free
integrators in the open loop.

Notes and References

The PID structure analyzed in this chapter uses a differentiator on the error as
shown in Fig. 7-1 instead of differentiating the output variable as shown in Fig. 7-2.
It is straightforward to show, using the final value theorem, that the system type
does not change under the two configurations. However, the value of the steady
state error does change and is not necessarily given by the simple formulas in
Table 9-1.

TOC
CHAPTER 9. INTEGRATORS 144

TOC
 Digital Implementation of PID Controllers
10
Learning Objectives:
• Implement a discrete PID controller.

10.1 Theory
In this book we work primarily with continuous time systems and continuous
time controllers. However, almost all modern control strategies are implemented
digitally on a micro-controller or a micro-processor, typically implemented using
C-code. In this chapter, we describe how PID controllers can be implemented in
a sampled data system using computer code.
A general PID control signal is given by
Z t
de
u(t) = kP e(t) + kI e(τ )dτ + kD (t),
−∞ dt

where e(t) = yr (t) − y(t) is the error between the desired reference output yr (t)
Rt
and the current output y(t). Define uP (t) = e(t), uI (t) = −∞ e(τ )dτ , and
uD (t) = de dt (t). The PID controller is therefore given by u(t) = kp uP (t) +
kI uI (t) + kD uD (t). In the following paragraphs we will derive discrete sampled-
time equivalents for uP (t), uI (t), and uD (t).
First consider a sampled time implementation of uP (t). When t = nTs , where
Ts is the sample period and n is the sample time, we have uP (nTs ) = e(nTs ), or
using discrete time notation
uP [n] = e[n].

TOC
CHAPTER 10. DIGITAL PID 146

To derive the sample-time implementation of an integrator, note that

Z nTs
uI (nTs ) = e(τ )dτ
−∞
Z (n−1)Ts Z nTs
= e(τ )dτ + e(τ )dτ
−∞ (n−1)Ts
Z nTs
= uI ((n − 1)Ts ) + e(τ )dτ.
(n−1)Ts

As shown in Fig. 10-1 a trapezoidal approximation of the integral between (n −

1)Ts and nTs is given by
Z nTs
Ts
e(τ )dτ ≈ (e(nTs ) + e((n − 1)Ts )) .
(n−1)Ts 2

Therefore

e(nTs )
e((n 1)Ts )
e(t) ✓ ◆
e(nTs ) + e((n 1)Ts )
Ts
2

t
Ts

Figure 10-1: Trapezoidal rule for digital integration.

Ts
uI (nTs ) ≈ uI ((n − 1)Ts ) + (e(nTs ) + e((n − 1)Ts )) .
2
Taking the z-transform we get

Ts
UI (z) = z −1 UI (z) + (E(z) + z −1 E(z)).
2
Solving for UI (z) gives
 
Ts 1 + z −1
UI (z) = E(z). (10.1)
2 1 − z −1

Since in the Laplace domain we have

1
UI (s) = E(s),
s

TOC
CHAPTER 10. DIGITAL PID 147

we have derived the so-called Tustin approximation where s in the Laplace domain
is replaced with the z-transform approximation
 
2 1 − z −1
s 7→ , (10.2)
Ts 1 + z −1

where Ts is the sample period [18]. Taking the inverse z-transform of Equa-
tion (10.1) gives the discrete time implementation of an integrator as
Ts
uI [n] = uI [n − 1] + (e[n] + e[n − 1]) . (10.3)
2

In the Laplace domain, a pure differentiator is given by UD (s) = sE(s).

However, since a pure differentiator is not causal, the standard approach is to use
the band-limited, or dirty derivative
s
UD (s) = E(s),
σs + 1
where σ is small, and σ1 defines the bandwidth of the differentiator. In practical
terms, the dirty derivative differentiates signals with frequency content less than
1
σ radians per second. Using the Tustin approximation (10.2), the dirty derivative
in the z-domain becomes
   
2 1−z −1 2 −1
Ts 1+z −1 2σ+Ts (1 − z )
UD (z) =   E(z) =   E(z).
2σ 1−z −1
Ts 1+z −1 + 1 1 − 2σ−T
2σ+Ts z
s −1

Transforming to the time domain, we have

   
2σ − Ts 2
uD [n] = uD [n − 1] + (e[n] − e[n − 1]) . (10.4)
2σ + Ts 2σ + Ts

10.1.1 Integrator Anti-Windup

A potential problem with a straight-forward implementation of PID controllers is
integrator wind up. When the error yr − y is large and a large error persists for
an extended period of time, the value of the integrator can become large, or “wind
up.” A large integrator will cause u to saturate, which will cause the system to
push with maximum effort in the direction needed to correct the error. Since the
value of the integrator will continue to wind up until the error signal changes sign,
the control signal may not come out of saturation until well after the error has
changed sign, which can cause a large overshoot and may potentially destabilize
the system.
Since integrator wind up can destabilize the system, it is important to add an
anti-windup scheme. A number of different anti-windup schemes are possible. In

TOC
 CHAPTER 10. DIGITAL PID 148

this section we will discuss two schemes that are commonly used in practice. The
first scheme is based on the fact that integrators are typically used to correct for
steady state error and the fact that overshoot is caused by integrating error during
the transients of the step response. Therefore, the integrator can be turned off
when the derivative of the error is large. This anti-windup scheme is implemented
by an if-statement conditioned on the size of ė.
The second anti-windup scheme is to subtract from the integrator exactly the
amount needed to keep u at the saturation bound. In particular, let

uunsat = kP e + kD uD + kI uI

denote the unsaturated control value before applying the anti-windup scheme. Af-
ter the saturation block we get

u = sat(uunsat ) = sat(kP e + kD uD + kI uI ).

The idea of anti-windup is to modify the output of the integrator from uI to u+

I so
that
kP e + kD uD + kI u+I = sat(kP e + kD uD + kI uI ) = u.

Subtracting uunsat from u gives

u − uunsat = (kP e + kD uD + kI u+
I ) − (kP e + kD uD + kI uI )
= kI (u+
I − uI ).

Solving for u+
I gives
1
u+
I = uI + (u − uunsat ),
kI
which is the updated value of the integrator that ensures that the control signal
remains just out of saturation. Note that if the control value is not in saturation
then u = uunsat and the integrator value remains unchanged.

10.1.2 Implementation
Python code that implements a general class for a PID loop is shown below.
1 import numpy as np
2
3 class PIDControl:
4 def __init__(self, kp, ki, kd, limit, sigma, Ts, flag=True):
5 self.kp = kp # Proportional control gain
6 self.ki = ki # Integral control gain
7 self.kd = kd # Derivative control gain
8 self.limit = limit # The output saturates at this limit
9 self.sigma = sigma # dirty derivative bandwidth is 1/sigma
10 self.beta = (2.0*sigma-Ts)/(2.0*sigma+Ts)
11 self.Ts = Ts # sample rate
12 self.flag = flag
13 # if flag == True, then returns
14 # u = kp * error + ki * integral(error) + kd * error_dot.

TOC
 CHAPTER 10. DIGITAL PID 149

15 # else returns
16 # u = kp * error + ki * integral(error) - kd * y_dot.
17 self.y_dot = 0.0 # estimated derivative of y
18 self.y_d1 = 0.0 # Signal y delayed by one sample
19 self.error_dot = 0.0 # estimated derivative of error
20 self.error_d1 = 0.0 # Error delayed by one sample
21 self.integrator = 0.0 # integrator
22
23 def PID(self, y_r, y):
24 # Compute the current error
25 error = y_r - y
26 # integrate error using trapazoidal rule
27 self.integrator = self.integrator \
28 + (self.Ts/2) * (error + self.error_d1)
29 # PID Control
30 if flag is True:
31 # differentiate error
32 self.error_dot = self.beta * self.error_dot \
33 + (1-self.beta)/self.Ts * (error - self.error_d1)
34 # PID control
35 u_unsat = self.kp*error \
36 + self.ki*self.integrator \
37 + self.kd*self.error_dot
38 else:
39 # differentiate y
40 self.y_dot = self.beta * self.y_dot \
41 + (1-self.beta)/self.Ts * (y - self.y_d1)
42 # PID control
43 u_unsat = self.kp*error \
44 + self.ki*self.integrator \
45 - self.kd*self.y_dot
46 # return saturated control signal
47 u_sat = self.saturate(u_unsat)
48 # integrator anti - windup
49 if self.ki != 0.0:
50 self.integrator = self.integrator \
51 + 1.0 / self.ki * (u_sat - u_unsat)
52 # update delayed variables
53 self.error_d1 = error
54 self.y_d1 = y
55 return u_sat
56
57 def PD(self, y_r, y):
58 # Compute the current error
59 error = y_r - y
60 # PD Control
61 if flag is True:
62 # differentiate error
63 self.error_dot = self.beta * self.error_dot \
64 + (1-self.beta)/self.Ts * (error - self.error_d1)
65 # PD control
66 u_unsat = self.kp*error \
67 + self.kd*self.error_dot
68 else:
69 # differentiate y
70 self.y_dot = self.beta * self.y_dot \
71 + (1-self.beta)/self.Ts * (y - self.y_d1)

TOC
 CHAPTER 10. DIGITAL PID 150

72 # PD control
73 u_unsat = self.kp*error \
74 - self.kd*self.y_dot
75 # return saturated control signal
76 u_sat = self.saturate(u_unsat)
77 # update delayed variables
78 self.error_d1 = error
79 self.y_d1 = y
80 return u_sat
81
82 def saturate(self,u):
83 if abs(u) > self.limit:
84 u = self.limit*np.sign(u)
85 return u

Listing 10.1: PIDControl.py

The PID controller class is initialized on lines 4–21, where kp, ki, and kd
are the control gains, limit is the saturation limit for the control signal, sigma
is the dirty derivative gain, Ts is the sample rate, and flag is a flag that indicates
whether the controller differentiates the error or the output. The internal mem-
ory used to implement the differentiators and integrators are initialized to zero in
lines 17–21, where y_d1 is the signal y delayed by one sample and error_d1
is the error delayed by one signal. PID control is implemented in lines 23–55
and PID control is implemented in lines 57–80. The integrator update is shown in
lines 50–51.

10.1.3 Gain Selection for PID control

Gain selection for PID control usually proceeds in the following manner. The
proportional and derivative gains kP and kD are first selected according to the
methods discussed in Chapter 8. The integral gain kI is then tuned by starting with
zero and then slowly increasing the gain until the steady state error is removed.
The parameters associated with the anti-windup scheme, especially v̄, are also
adjusted during this phase. The proportional and derivative gains can then be
adjusted if necessary by retuning ωn and ζ. In the case of successive loop closure,
integrators are only added on the outer loop.
The addition of an integrator has a destabilizing effect on the feedback system.
This effect is most easily understood by using the root locus method described in
Appendix P.6, where it can be seen that for second order systems, a large kI will
always result in closed-loop poles in the right half plane. The root locus can also
be used to aid in the selection of the integral gain kI , where the value of kI is
selected so that integrator has minimal impact on the closed-loop poles designed
using PD control. A more effective method for selecting integral gains will be
discussed in Chapter 12.

TOC
 CHAPTER 10. DIGITAL PID 151

10.2 Design Study A. Single Link Robot Arm

Example Problem A.10

The objective of this problem is to implement the PID controller using only mea-
sured outputs of the system.

(a) Modify the system dynamics file so that the parameters vary by up to 20%
each time they are run (uncertainty parameter = 0.2).

(b) Change the simulation files so that the input to the controller is the output and
not the state. The controller should only assume knowledge of the angle θ
and the reference angle θr .

(c) Implement the PID controller designed in Problems A.8. Use the dirty deriva-
tive gain of σ = 0.05. Tune the integrator to remove the steady state error
caused by the uncertain parameters.

Solution
A Python class that implements a PID controller for the single link robot arm is
shown below.
1 import numpy as np
2 import armParamHW10 as P
3 import sys
4 sys.path.append(’..’) # add parent directory
5 import armParam as P0
6 from PIDControl import PIDControl
7
8
9 class armController:
10
11 def __init__(self):
12 # Instantiates the PD object
13 self.thetaCtrl = PIDControl(P.kp, P.ki, P.kd,
14 P0.tau_max, P.sigma, P.Ts)
15 self.limit = P0.tau_max
16
17 def update(self, theta_r, y):
18 theta = y.item(0)
19
20 # compute feedback linearized torque tau_fl
21 #tau_e = P0.m * P0.g * (P0.ell / 2.0) * np.cos(0.0)
22
23 # compute feedback linearized torque tau_fl
24 tau_fl = P0.m * P0.g * (P0.ell / 2.0) * np.cos(theta)
25
26 # compute the linearized torque using PD
27 tau_tilde = self.thetaCtrl.PID(theta_r, theta, False)
28

TOC
 CHAPTER 10. DIGITAL PID 152

29 # compute total torque

30 tau = tau_fl + tau_tilde
31 tau = self.saturate(tau)
32
33 return tau
34
35 def saturate(self, u):
36 if abs(u) > self.limit:
37 u = self.limit*np.sign(u)
38
39 return u

Listing 10.2: armController.py

Python code that implements the complete simulation is shown below.

1 import sys
2 sys.path.append(’..’) # add parent directory
3 import matplotlib.pyplot as plt
4 import numpy as np
5 import armParam as P
6 from hw3.armDynamics import armDynamics
7 from armController import armController
8 from hw2.signalGenerator import signalGenerator
9 from hw2.armAnimation import armAnimation
10 from hw2.dataPlotter import dataPlotter
11
12 # instantiate arm, controller, and reference classes
13 arm = armDynamics(alpha=0.2)
14 controller = armController()
15 reference = signalGenerator(amplitude=30*np.pi/180.0,
16 frequency=0.05)
17 disturbance = signalGenerator(amplitude=0)
18
19 # instantiate the simulation plots and animation
20 dataPlot = dataPlotter()
21 animation = armAnimation()
22
23 t = P.t_start # time starts at t_start
24 y = arm.h() # output of system at start of simulation
25
26 while t < P.t_end: # main simulation loop
27
28 # Get referenced inputs from signal generators
29 # Propagate dynamics in between plot samples
30 t_next_plot = t + P.t_plot
31
32 # updates control and dynamics at faster simulation rate
33 while t < t_next_plot:
34 r = reference.square(t)
35 d = disturbance.step(t) # input disturbance
36 n = 0.0 #noise.random(t) # simulate sensor noise
37 u = controller.update(r, y + n) # update controller
38 y = arm.update(u + d) # propagate system
39 t = t + P.Ts # advance time by Ts
40
41 # update animation and data plots

TOC
 CHAPTER 10. DIGITAL PID 153

42 animation.update(arm.state)
43 dataPlot.update(t, r, arm.state, u)
44
45 # the pause causes the figure to display during simulation
46 plt.pause(0.0001)
47
48 # Keeps the program from closing until the user presses a button.
49 print(’Press key to close’)
50 plt.waitforbuttonpress()
51 plt.close()

Listing 10.3: armSim.py

Complete simulation code for Matlab, Python, and Simulink can be down-
loaded at http://controlbook.byu.edu.

10.3 Design Study B. Inverted Pendulum

Example Problem B.10

The objective of this problem is to implement the PID controller using only mea-
sured outputs of the system.

(a) Modify the system dynamics file so that the parameters m1 , m2 , ` and b vary
by up to 20% of their nominal value each time they are run (uncertainty
parameter = 0.2).

(b) Change the simulation files so that the input to the controller is the output and
not the state. The controller should only assume knowledge of the position
z and the angle θ, as well as the reference position zr .

(c) Implement the nested PID loops designed in Problems B.8. Use the dirty
derivative gain of σ = 0.05. Tune the integrator to remove the steady state
error caused by the uncertain parameters.

Solution
A Python class that implements a PID controller for the inverted pendulum is
shown below.
1 import numpy as np
2 import pendulumParam as P
3 import pendulumParamHW10 as P10
4 from PIDControl import PIDControl
5
6 class pendulumController:
7 def __init__(self):
8 # Instantiates the SS_ctrl object
9 self.zCtrl = PIDControl(P10.kp_z, P10.ki_z, P10.kd_z,

TOC
 CHAPTER 10. DIGITAL PID 154

10 P10.theta_max, P10.sigma, P10.Ts)

11 self.thetaCtrl = PIDControl(P10.kp_th, 0.0, P10.kd_th,
12 P10.F_max, P10.sigma, P10.Ts)
13 self.filter = zeroCancelingFilter()
14
15 def update(self, z_r, y):
16 z = y.item(0)
17 theta = y.item(1)
18
19 # the reference angle for theta comes from
20 # the outer loop PID control
21 theta_r = self.zCtrl.PID(z_r, z, flag=False)
22
23 # low pass filter the outer loop to cancel
24 # left-half plane zero and DC-gain
25 theta_r = self.filter.update(theta_r)
26
27 # the force applied to the cart comes from
28 # the inner loop PD control
29 F = self.thetaCtrl.PD(theta_r, theta, flag=False)
30 return F
31
32 class zeroCancelingFilter:
33 def __init__(self):
34 self.a = -3.0/(2.0*P.ell*P10.DC_gain)
35 self.b = np.sqrt(3.0*P.g/(2.0*P.ell))
36 self.state = 0.0
37
38 def update(self, input):
39 # integrate using RK1
40 self.state = self.state \
41 + P.Ts * (-self.b*self.state + self.a*input)
42 return self.state

Listing 10.4: pendulumController.py

Python code for the closed loop system is shown below.

1 import sys
2 sys.path.append(’..’) # add parent directory
3 import matplotlib.pyplot as plt
4 import pendulumParam as P
5 from hw3.pendulumDynamics import pendulumDynamics
6 from pendulumController import pendulumController
7 from hw2.signalGenerator import signalGenerator
8 from hw2.pendulumAnimation import pendulumAnimation
9 from hw2.dataPlotter import dataPlotter
10
11 # instantiate pendulum, controller, and reference classes
12 pendulum = pendulumDynamics()
13 controller = pendulumController()
14 reference = signalGenerator(amplitude=0.5, frequency=0.06)
15 disturbance = signalGenerator(amplitude=0.1)
16 noise = signalGenerator(amplitude=0.01)
17
18 # instantiate the simulation plots and animation
19 dataPlot = dataPlotter()

TOC
 CHAPTER 10. DIGITAL PID 155

20 animation = pendulumAnimation()
21
22 t = P.t_start # time starts at t_start
23 y = pendulum.h() # output of system at start of simulation
24
25 while t < P.t_end: # main simulation loop
26 t_next_plot = t + P.t_plot
27
28 while t < t_next_plot:
29 r = reference.square(t) # reference input
30 d = disturbance.step(t) # input disturbance
31 n = noise.random(t) # simulate sensor noise
32 u = controller.update(r, y + n) # update controller
33 y = pendulum.update(u + d) # propagate system
34 t = t + P.Ts # advance time by Ts
35
36 # update animation and data plots
37 animation.update(pendulum.state)
38 dataPlot.update(t, r, pendulum.state, u)
39 plt.pause(0.0001)
40
41 # Keeps the program from closing until the user presses a button.
42 print(’Press key to close’)
43 plt.waitforbuttonpress()
44 plt.close()

Listing 10.5: pendulymSim.py

Complete simulation code for Matlab, Python, and Simulink can be down-
loaded at http://controlbook.byu.edu.

10.4 Design Study C. Satellite Attitude Control

Example Problem C.10

The objective of this problem is to implement the PID controller using only mea-
sured outputs of the system.

(a) Modify the system dynamics file so that the parameters Js , Jp , k and b vary
by up to 20% of their nominal value each time they are run (uncertainty
parameter = 0.2).

(b) Change the simulation files so that the input to the controller is the output and
not the state. Assume that the controller only has knowledge of the angles
φ and θ as well as the reference angle φr .

(c) Implement the nested PID loops designed in Problems C.8. Use the dirty
derivative gain of σ = 0.05. Tune the integrator to remove the steady state
error caused by the uncertain parameters.

TOC
 CHAPTER 10. DIGITAL PID 156

Solution
A Python class that implements a PID controller for the satellite is shown below.
1 import satelliteParamHW10 as P
2 from PIDControl import PIDControl
3
4 class satelliteController:
5 def __init__(self):
6 # Instantiates the SS_ctrl object
7 self.phiCtrl = PIDControl(P.kp_phi, P.ki_phi, P.kd_phi,
8 P.theta_max, P.sigma, P.Ts)
9 self.thetaCtrl = PIDControl(P.kp_th, 0.0, P.kd_th,
10 P.tau_max, P.sigma, P.Ts)
11
12 def update(self, phi_r, y):
13 theta = y.item(0)
14 phi = y.item(1)
15
16 # the reference angle for theta comes from
17 # the outer loop PD control
18 theta_r = self.phiCtrl.PID(phi_r, phi, flag=False)
19
20 # the torque applied to the base comes from
21 # the inner loop PD control
22 tau = self.thetaCtrl.PD(theta_r, theta, flag=False)
23
24 return tau

Listing 10.6: satelliteController.py

Python code that implements the complete simulation is shown below

1 import sys
2 sys.path.append(’..’) # add parent directory
3 import matplotlib.pyplot as plt
4 import numpy as np
5 import satelliteParam as P
6 from hw3.satelliteDynamics import satelliteDynamics
7 from satelliteController import satelliteController
8 from hw2.signalGenerator import signalGenerator
9 from hw2.satelliteAnimation import satelliteAnimation
10 from hw2.dataPlotter import dataPlotter
11
12 # instantiate satellite, controller, and reference classes
13 satellite = satelliteDynamics(alpha=0.2)
14 controller = satelliteController()
15 reference = signalGenerator(amplitude=15.0*np.pi/180.0,
16 frequency=0.02)
17 disturbance = signalGenerator(amplitude=0.0)
18
19 # instantiate the simulation plots and animation
20 dataPlot = dataPlotter()
21 animation = satelliteAnimation()
22
23 t = P.t_start # time starts at t_start
24 y = satellite.h() # output of system at start of simulation

TOC
 CHAPTER 10. DIGITAL PID 157

25
26 while t < P.t_end: # main simulation loop
27 # Propagate dynamics in between plot samples
28 t_next_plot = t + P.t_plot
29
30 # updates control and dynamics at faster simulation rate
31 while t < t_next_plot:
32 r = reference.square(t) # reference input
33 d = disturbance.step(t) # input disturbance
34 n = 0.0 #noise.random(t) # simulate sensor noise
35 u = controller.update(r, y + n) # update controller
36 y = satellite.update(u + d) # propagate system
37 t = t + P.Ts # advance time by Ts
38 # update animation and data plots
39 animation.update(satellite.state)
40 dataPlot.update(t, r, satellite.state, u)
41
42 # the pause causes the figure to display during simulation
43 plt.pause(0.0001)
44
45 # Keeps the program from closing until the user presses a button.
46 print(’Press key to close’)
47 plt.waitforbuttonpress()
48 plt.close()

Listing 10.7: satelliteSim.py

Complete simulation code for Matlab, Python, and Simulink can be down-
loaded at http://controlbook.byu.edu.

Important Concepts:
• The Tustin approximation provides a relationship between the
Laplace s-domain and the Z-domain.
• The trapezoid rule can be used to approximate the integrator.
• The band-limited or dirty derivative is used in place of a pure dif-
ferentiator to avoid non-causal implementations (i.e. current outputs
depend on future inputs).
• Integrator anti-windup prevents overshoot and destabilization that
may arise when large integrator errors exist.

Notes and References

A standard reference for digital implementation of PID controllers is [18]. Simple
anti-wind-up schemes are discussed in [18, 4].

TOC
 Part IV
Observer Based Control
Design

In Part III we focused on designing feedback control systems for second order
systems. Since second order systems have two poles, we saw that it is possible to
use feedback control with two gains to place the closed-loop poles at any desired
location. In Chapter 11 of this book we will see that for an nth order system
it is possible to arbitrarily place the closed-loop poles, but that this will require
n feedback gains. In that sense, the full state feedback controllers that we will
derive in Chapter 11 are a generalization of PD control to higher order systems.
This generalization is most easily accomplished using the state space model of
the system derived in Chapter 6. In fact, the state space model leads to efficient
numerical routines for designing and implementing the feedback controller.
In Chapter 12 we show how integral control can be added to the state feedback
controllers designed in Chapter 11. The method introduced in Chapter 12 has a
significant advantage over the integrator tuning method discussed in Chapter 10 in
the sense that tuning the integral gain will not impact the other closed-loop poles
and therefore allows for greater design flexibility.
We saw in Chapter 10 that in order to implement PD/PID control, the deriva-
tive of the output needs to be reconstructed using a digital differentiator. The
generalization of this idea to state feedback control is that the full state of the
system must be reconstructed. The tool that we will use to reconstruct the sys-
tem state is called an observer. The observer design problem will be discussed in
Chapter 13.
It turns out that observers can be used in a variety of different contexts outside
of feedback control. In particular, observers can be used to solve target tracking
problems and to estimate the system parameters of complex dynamic systems. In
the setting of feedback control where unknown input and output disturbances act
on the system, observers can be used to estimate the unknown disturbances. In
Chapter 14 we will show how to design the observer to estimate both the system
state and the unknown constant input disturbance. We will also show that estimat-
ing the input disturbance is mathematically and functionally similar to adding an
integrator.
The design tools developed in Part IV of this book use state space models
and require the use of linear algebra and matrix analysis. A brief review of linear
algebra concepts that will be used in Part IV are given in Appendix P.7.
CHAPTER 10. DIGITAL PID 160

TOC
 Full State Feedback
11
Learning Objectives:
• Design a full-state feedback controller.
• Evaluate the controllability of a system.
• Compute the feedforward gain that ensures that the DC-gain is equal
to one.

11.1 Theory
In this chapter we will show how to use full state feedback to stabilize a linear
time-invariant system represented in state space form. Recall from Section 6.1.2,
that for state space equations given by

ẋ = Ax + Bu (11.1)
y = Cx + Du, (11.2)

the poles of the system are the eigenvalues of A, or in other words, the roots of
the open-loop characteristic equation

∆ol (s) = det(sI − A) = 0.

The matrix A is called the state-interaction matrix because it specifies how the
states interact with each other. The basic idea behind full state feedback is to
use the feedback signal to change the interaction matrix so that the poles are in
specified locations. A full state feedback controller is given by

u = −Kx + ν, (11.3)

where K ∈ Rm×n are the feedback gains, and ν will be a signal that is speci-
fied later. Using (11.3) in Equation (11.1) results in the closed-loop state space

TOC
CHAPTER 11. FULL STATE FEEDBACK 162

equations

ẋ = Ax + B(−Kx + ν) = (A − BK)x + Bν (11.4)

y = Cx, (11.5)

where the interaction matrix is now A − BK, and so the closed-loop poles will be
given by the eigenvalues of A−BK, or in other words, the roots of the closed-loop
characteristic equation

∆cl (s) = det(sI − (A − BK)) = 0.

A natural question that will be addressed in the remainder of this section, is how
to select K so that the closed-loop characteristic equation is equal to a desired
closed-loop characteristic ∆dcl (s), where the designer selects the roots of ∆dcl (s) =
0 to achieve desired closed-loop performance.
Section 6.1.2 showed that the state space model given by Equations (11.1)–
(11.2) is equivalent to the transfer function model given by

Y (s) = C(sI − A)−1 BU (s). (11.6)

While the mapping from state-space model to transfer function model is unique,
the reverse is not true. In fact, there are an infinite number of state space models
that correspond to a particular transfer function model. Accordingly, we say that
a state space model is a realization of the transfer function model, if it produces
the same input-output behavior when the initial conditions are zero.
In particular, define the change of variables

z = P −1 x, (11.7)

where P is an invertible matrix. Then

ż = P −1 ẋ = P −1 (Ax + Bu) = P −1 AP z + P −1 Bu (11.8)

y = Cx = CP z (11.9)

is an alternative state space representation of system (11.1)-(11.2). To show that

these two representations have the same input-output behavior, or in other words,
the same transfer function, note from Equation (11.6) that the transfer function of
system (11.8)-(11.9) is


Y (s) = CP (sI − P −1 AP )−1 P −1 B U (s)


= CP (sP −1 P − P −1 AP )−1 P −1 B U (s)


= CP (P −1 (sI − A)P )−1 P −1 B U (s)


= CP P −1 (sI − A)−1 P P −1 B U (s)


= C(sI − A)−1 B U (s),

which is identical to Equation (11.6). Therefore Equations (11.1)-(11.2) and

Equations (11.8)-(11.9) are realizations of the same system.

TOC
CHAPTER 11. FULL STATE FEEDBACK 163

11.1.1 State Space Realization in Control Canonic Form

In this section, we will derive one specific state space realization for single-input
single-output transfer functions, where the state feedback problem is particularly
simple. The realization that we derive below is called the control canonic form of
the system.
Consider the general SISO monic transfer function model given by

bm sm + bm−1 sm−1 + · · · + b1 s + b0
Y (s) = U (s), (11.10)
sn + an−1 sn−1 + · · · + a1 s + a0

where m < n. The block diagram of the system is shown in Fig. 11-1. The first

U (s) Y (s)
bm s m + bm 1 s m 1 + · · · + b1 s + b0
s n + an 1 s n 1 + · · · + a1 s + a0

Figure 11-1: General transfer function for a SISO system.

step in deriving the control canonic state space equations is to artificially decom-
pose the transfer function as shown in Fig. 11-2, where the numerator polynomial
and the denominator polynomial appear in separate cascaded blocks, and where
Z(s) is an artificial intermediate variable. In the s-domain, the equations corre-

U (s) Y (s)
1 Z(s)
bm s m + bm 1s
m 1
+ · · · + b1 s + b0
s n + an 1s
n 1 + ··· + a s + a
1 0

Figure 11-2: General transfer function for a SISO system decomposed into state
dynamics and output dynamics.

sponding to Fig. 11-2 can be written as

1
Z(s) = U (s)
sn + an−1 sn−1 + · · · + a1 s + a0
Y (s) = (bm sm + bm−1 sm−1 + · · · + b1 s + b0 )Z(s).

Taking the inverse Laplace transform of these equations gives

dn z dn−1 z
= u(t) − an−1 − · · · − a1 ż − a0 z(t) (11.11)
dtn dtn−1
m m−1
d z d z
y(t) = bm m + bm−1 m−1 + · · · + b1 ż + b0 z(t). (11.12)
dt dt
The next step in the derivation of the state space equations is to draw the analog
computer implementation of Equations (11.11) and (11.12), which is shown in
Fig. 11-3. Labeling the output of the integrators as the state variables x1 , . . . , xn ,

TOC
CHAPTER 11. FULL STATE FEEDBACK 164

bm
...
b1
dn z dn 1 z
u(t) dtn dtn 1 z̈ z y(t)
1 1 ... 1 ż 1
b0
s x1 s x2 s xn 1 s xn

an
... 1

a1

a0

Figure 11-3: Block diagram showing the analog computer implementation of

Equations (11.11) and (11.12).

as shown in Fig. 11-3, we can write the state space equations as

 
  −an−1 −an−2 ... −a1 −a0    
ẋ1  1 x1 1
 ẋ2   0 ... 0 0    x2  0
   0 1 ... 0 0     
 ..  =    .  +  .  u (11.13)
 .   .. .. ..   ..   .. 
 . . . 
ẋn xn 0
0 0 ... 1 0
 
x1


 x2 
y= 0 ... 0 bm . . . b1 b0  .  + 0 · u.
 .. 
xn

In the case when m = n it is straightforward to show that the output equation

becomes


x1

 
 x2 
y = bn−1 − bn an−1 , ... b1 − bn a1 , b0 − bn a0  .  + bn · u.
 .. 
xn

We will define the control canonic realization as

ẋc = Ac xc + Bc u
y = Cc xc + Du,

TOC
CHAPTER 11. FULL STATE FEEDBACK 165

where
 
−an−1 −an−2 ... −a1 −a0
 1 0 ... 0 0 
 
4  0 
Ac =  0 1 ... 0  (11.14)
 .. .. .. 
 . . . 
0 0 ... 1 0
 
1
0
4  
Bc =  .  (11.15)
 .. 
0
 
 0 . . . 0 bm . . . b1 b0 m<n
4
Cc =  
 bn−1 − bn an−1 , . . . b1 − bn a1 , b0 − bn a0 m=n
(11.16)
(
4 0 m<n
D= . (11.17)
bn m=n

Note that the negative of the coefficients of the denominator of (11.10) consti-
tute the first row of Ac , and that the remainder of Ac has ones on the lower off
diagonal and zeros in all other elements. Matrices with this structure are said to
be in companion form. The Bc vector has a single one as its first element, with
the remaining elements equal to zero. The coefficients of the numerator of Equa-
tion (11.10) are the last p-elements of Cc .
As an example, suppose that the transfer function model of the system is given
by
9s + 20
Y (s) = 3 U (s),
s + 6s2 − 11s + 8
then the control canonic realization of the system is given by
      
ẋ1 −6 11 −8 x1 1
ẋ2  =  1 0 0  x2  + 0 u (11.18)
ẋ3 0 1 0 x3 0
 

x1
y = 0 9 20 x2  .
x3

11.1.2 Full State Feedback using Control Canonic Form

The design of a state feedback controller is particularly easy when the state space
equations are in control canonic form. The key insight is that there is a direct
relationship between matrices in companion form, as shown in Equation (11.14)

TOC
CHAPTER 11. FULL STATE FEEDBACK 166

and their characteristic polynomial

∆ol (s) = sn + an−1 sn−1 + · · · + a1 s + a0 .

Suppose that we let

 
x1
 
x2




..
u = −Kc xc + ν = − k1 k2 ... kn−1 kn   + ν.
. (11.19)
 
xn−1 
xn

Substituting into Equation (11.13) and rearranging gives

−(a
n−1 + k1 ) −(an−2 + k2 ) ... −(a1 + kn−1 ) −(a0 + kn )

ẋ1
  1 0 ... 0 0   x  1
0 1 ... 0 0  x1  0
 
 ẋ2  
  2  
  
. .

 .  = 
  . 
. + .  ν.
  
 .   . . .  .  .
 .  
 . . . 
 .  .
ẋn  xn 0
 
 .
.
0 0 . 1 0

Since the state interaction matrix is still in companion form, the characteristic
polynomial of the closed-loop system is given by

∆cl (s) = sn + (an−1 + k1 )sn−1 + · · · + (a1 + kn−1 )s + (a0 + kn ).

If the desired characteristic polynomial is given by

∆dcl (s) = sn + αn−1 sn−1 + · · · + α1 s + α0 , (11.20)

then we have that

an−1 + k1 = αn−1
an−2 + k2 = αn−2
..
.
a1 + kn−1 = α1
a0 + kn = α0 .

Defining the row vectors

aA = (an−1 , an−2 , . . . , a1 , a0 )
α = (αn−1 , αn−2 , . . . , α1 , α0 )
Kc = (k1 , k2 , . . . , kn−1 , kn ),

we have that the feedback gains satisfy

Kc = α − aA , (11.21)

TOC
CHAPTER 11. FULL STATE FEEDBACK 167

where the subscript “c” is used to emphasize that the gains are when the state
equations are in control canonic form.
As an example, consider the state equations given in Equation (11.18). The
open-loop characteristic polynomial is given by

∆ol (s) = s3 + 6s2 − 11s + 8,

with roots at −7.5885, and 0.7942 ± j0.6507. Suppose that the desired closed-
loop characteristic polynomial is

∆dcl (s) = s3 + 5s2 + 12s + 8,

with roots at −1 and −2 ± j2, then aA = (6, −11, 8), and α = (5, 12, 8). The
feedback gains that place the poles at the desired locations are therefore given by

Kc = α − aA = (5, 12, 8) − (6, −11, 8) = (−1, 23, 0).

11.1.3 Full State Feedback: General Case

The formula given in Equation (11.21) assumes that the state space equations
are in control canonic form. It turns out that the closed-loop eigenvalues cannot
always be arbitrarily assigned to desired locations. When they can, the system ẋ =
Ax + Bu is said to be controllable. In this section we will derive an expression
for the state feedback gains, and we will simultaneously derive a simple test for
determining when the system is controllable.
Recall from Equations (11.7)–(11.9) that state space equations can be trans-
formed using the change of variables in Equation (11.7) without modifying the
input-output properties of the system. This implies that the change of variables
does not change the characteristic polynomial. We can verify this fact explicitly
as follows:

det(sI − P −1 AP ) = det(sP −1 P − P −1 AP )


= det P −1 (sI − A)P
= det(P −1 ) det(sI − A) det(P )
= det(sI − A).

Therefore, to derive full state feedback gains for the general state space equa-
tions
ẋ = Ax + Bu, (11.22)
the idea is to find a state transformation matrix P so that the transformed system
is in control canonic form, i.e., find P so that

xc = P −1 x, (11.23)

and
ẋc = P −1 AP xc + P −1 Bu = Ac xc + Bc u, (11.24)

TOC
CHAPTER 11. FULL STATE FEEDBACK 168

where Ac and Bc have the form given by Equations (11.14) and (11.15), re-
spectively. Therefore, we need to find P so that P −1 B = Bc , and P −1 AP =
Ac , which are satisfied if and only if B = P Bc and AP = P Ac . Let P =
(p1 , p2 , . . . , pn ), where pi ∈ Rn is the ith column of P . The requirement that
B = P Bc implies that
 
1

0

B = p1 p2 . . . pn  .  = p1 .
 .. 
0

Therefore, p1 = B. The requirement that P Ac = AP implies that

 
−an−1 −an−2 ... −a1 −a0
 1 0 ... 0 0 
 
 0 

 0 1 ... 0 
p1 p2 ... pn  .
 .. .. .. 
 . . 

 0 0 ... 0 0 
 
..
0 0 . 1 0


= A p1 p2 ... pn .

After multiplication, the first n − 1 columns can be equated to get

−an−1 p1 + p2 = Ap1 (11.25)

−an−2 p1 + p3 = Ap2 (11.26)
..
. (11.27)
−a1 p1 + pn = Apn−1 . (11.28)

Equation (11.25), and the previous conclusion that p1 = B, implies that

p2 = an−1 B + AB.

Using this results in Equation (11.26) gives

p3 = an−2 B + an−1 AB + A2 B.

We can continue to iteratively solve for each column of P until the (n − 1)th
column, which from Equation (11.28) is

pn = a1 B + a2 AB + · · · + an−2 An−2 B + An−1 B.

Notice that each column in P is a linear combination of the elements B, AB,

A2 B, . . . , An−1 B. Accordingly, defining


CA,B = B AB A2 B . . . An−1 B , (11.29)

TOC
CHAPTER 11. FULL STATE FEEDBACK 169

gives

>
p1 = CA,B 1, 0, . . . 0, 0 ,
 >
p2 = CA,B an−1 , 1, ..
. 0, 0 ,
.. ..
. .
 >
pn = CA,B a1 , a2 , ... an−1 , 1 .

Defining the matrix

 
1 an−1 an−2 ... a2 a1
 .. 
0 1 an−1 . a3 a2 
 
 .. 
 a3 
AA = 0 0 1 . a4 , (11.30)
. .. .. 
 .. . . 
 
0 0 0 ... 1 an−1 
0 0 0 ... 0 1

gives that the transformation matrix that converts the system into control canonic
form is
P = CA,B AA .
Notice that since AA is an upper triangular matrix with ones on the diagonal, that
det(AA ) = 1, implying that A−1A always exists. It is clear that the matrix P is
invertible with
P −1 = A−1 −1
A CA,B
−1
if and only if CA,B exists. The matrix CA,B in Equation (11.29) is called the
controllability matrix, and a single input system is said to be controllable if C
is invertible. Therefore, when the original system (11.22) is controllable, there
exists an invertible state transformation matrix P so that the change of variables
in Equation (11.23) is well defined and that results in the transformed equations
being in control canonic form via Equations (11.24).
In control canonic form, the state feedback equation is

u = −Kc xc + ν = −(α − aA )xc + ν.

Using Equation (11.23) the state feedback control can be expressed in the original
states x as

u = −(α − aA )P −1 x + ν
= −(α − aA )A−1 −1
A CA,B x + ν
= −Kx + ν, (11.31)

TOC
CHAPTER 11. FULL STATE FEEDBACK 170

where
4
K = (α − aA )A−1 −1
A CA,B . (11.32)
Equation (11.32) is called Ackermann’s formula for the feedback gains. Since
CA,B is only square in the single-input case, Ackermann’s formula only holds for
single input systems.
Using the full state feedback control in Equation (11.31) in the state equa-
tions (11.1)-(11.2) results in the closed-loop system

ẋ = (A − BK)x + Bν
y = Cx.

The resulting transfer function from ν to y is given by

Y (s) = C(sI − (A − BK))−1 B ν(s). (11.33)

A block diagram of the resulting closed-loop system is shown in Fig. 11-4

where the double line associated with x is meant to imply that the state x is not re-
ally available. The block diagram also shows the new input ν. Since the objective
is for the output y to track the reference input r, we let ν(t) = kr r(t), as shown
in Fig. 11-4. The feedforward gain kr is selected so that the DC-gain from r to y
is equal to one.

Figure 11-4: Full state feedback with reference input r.

From Equation (11.33), the transfer function from R to Y is given by

Y (s) = C(sI − (A − BK))−1 Bkr R(s), (11.34)

and the DC-gain is

 
kDC = lim C(sI − (A − BK))−1 Bkr = −C(A − BK)−1 Bkr . (11.35)
s→0

Therefore, the DC-gain is equal to one if

1
kr = − .
C(A − BK)−1 B

The matrix A − BK will be invertible when there are no eigenvalues at zero.

Since K is selected to place the eigenvalues in the open left half plane, the matrix
will always be invertible, and kr exists. Note that in many applications, the output

TOC
CHAPTER 11. FULL STATE FEEDBACK 171

measured by the sensors is not the same as the desired reference output. In other
words, the state space equations may be given by

ẋ = Ax + Bu
yr = Cr x
ym = Cm x,

where yr is the reference output and ym is the measured output. In this case, the
reference gain kr is selected to make the DC-gain from r to yr equal to one, or in
other words
1
kr = − . (11.36)
Cr (A − BK)−1 B
Chapter 13 will describe observers that use the measured output ym to reconstruct
the state x.
The feedforward gain computed in Equation (11.36) is for single input sys-
tems. More generally, if the size of the reference output yr is equal to the size
of the input vector u ∈ Rm , then Cr ∈ Rm×n and B ∈ Rn×m , which implies
that the transfer matrix in Equation (11.34) is square and has dimension m × m.
Similarly, the resulting matrix gain Kr ∈ Rm×m will also be square and from
Equation (11.35) will be equal to
 −1
Kr = − Cr (A − BK)−1 B .

11.1.4 State Feedback for Linearized Systems

Suppose that the state space model is from a linearized system, i.e, suppose that
x ∈ Rn is the state and that xe ∈ Rn is the equilibrium state, and that x̃ = x − xe
is the state linearized around the equilibrium. Similarly, suppose that u ∈ Rm
is the input, that ue ∈ Rm is the input at equilibrium, and that ũ = u − ue is
the input linearized around equilibrium, and that yr ∈ Rp is the reference output,
yre = Cr xe ∈ Rp is the reference output at equilibrium, and ỹr = yr − yre is
the reference output linearized about equilibrium, then the linearized state space
model is given by

x̃˙ = Ax̃ + B ũ
ỹr = Cr x̃.

Now suppose that the state feedback design process is used to find the state feed-
back controller
ũ = −K x̃ + kr r̃ (11.37)
where r̃ = r − yre is the reference input around the equilibrium output, and where
the eigenvalues of (A − BK) are in the open left half plane, and the DC-gain from
r̃ to ỹr is one. The controller input to the plant is therefore

u = ue − K(x − xe ) + kr (r − yre ). (11.38)

TOC
CHAPTER 11. FULL STATE FEEDBACK 172

11.2 Summary of Design Process - State Feedback

The state feedback design process can be summarized as follows.
Given the plant
ẋ = Ax + Bu
yr = Cr x
and the desired pole locations p1 , p2 , . . . , pn , find the gains K and kr so that the
feedback controller
u = −Kx + kr r
places the poles at the desired location and results in a DC-gain equal to one.
Step 1. Check to see if the system is controllable by computing the controllability
matrix
CA,B = [B, AB, . . . , An−1 B]
and checking to see if rank(C) = n.
Step 2. Find the open-loop characteristic polynomial
∆ol (s) = det(sI − A) = sn + an−1 sn−1 + · · · + a1 s + a0 ,
and construct the row vector
aA = (an−1 , an−2 , . . . , a1 , a0 )
and the matrix
 
1 an−1 an−2 ··· a2 a1
0 1 an−1 ··· a3 a2 
 
 .. .. 
AA = . . . . . 
 
0 0 ··· 0 1 an−1 
0 0 ··· 0 0 1

Step 3. Select the desired closed-loop polynomial

∆dcl (s) = (s − p1 )(s − p2 ) · · · (s − pn )
= sn + αn−1 sn−1 + · · · + α1 s + α0 ,
and construct the row vector
α = (αn−1 , αn−2 , . . . , α1 , α0 ).

Step 4. Compute the desired gains as

K = (α − aA )A−1 −1
A CA,B
−1
kr = .
Cr (A − BK)−1 B

TOC
 CHAPTER 11. FULL STATE FEEDBACK 173

Using Python, these steps can be implemented as follows:

1 import control as cnt
2 import numpy as np
3
4 CC = cnt.ctrb(A,B)
5
6 # check for controllability
7 if np.linalg.matrix_rank(CC) == np.size(A,1):
8 print(’the system is controllable’)
9 else:
10 print(’the system is not controllable’)
11
12 # place the poles at p1, p2, ..., pn
13 K = cnt.place(A, B, [p1, p2, p3])
14
15 # compute reference gain
16 kr = -1/(C @ np.linalg.inv(A - B @ K) @ B)

11.2.1 Simple example

Given the system
   
0 1 0
ẋ = x+ u
2 3 4


yr = 5 0 x
find the feedback gain K to place the closed-loop poles at −1±j, and the reference
gain kr so that the DC-gain is one.
Step 1. The controllability matrix is given by
 
0 4
CA,B = [B, AB] = .
4 12
The determinant is det(C) = −16 6= 0, therefore the system is controllable.
Step 2. The open-loop characteristic polynomial is
 
s −1
∆ol (s) = det(sI − A) = det = s2 − 3s − 2,
−2 s − 3
which implies that
aA = (−3, −2)
 
1 −3
AA =
0 1

Step 3. The desired closed-loop polynomial is

∆dcl (s) = (s + 1 − j)(s + 1 + j) = s2 + 2s + 2,
which implies that

α = 2, 2 .

TOC
CHAPTER 11. FULL STATE FEEDBACK 174

Step 4. The gains are therefore given as

K = (α − aA )A−1 −1
A CA,B
  3 1

1 3 −4 4
= ((2, 2) − (−3, −2)) 1
0 1 4 0
 

0 1
= 5 4 1 4
0
5

4
= 1 4

−1
kr =
Cr (A − BK)−1 B
−1
=       

0 1 0
−1 0
5 0 − 1 45
2 3 4 4
−1
=    −1  

0 1 0 0 0
5 0 −
2 3 4 5 4
−1
=  −1  

0 1 0
5 0
−2 −2 4
−1 1
= = .
−10 10

11.3 Design Study A. Single Link Robot Arm

Example Problem A.11

The objective of this problem is to implement a state feedback controller using the
full state. Start with the simulation files developed in Homework A.10.
(a) Select the closed loop poles as the roots of the equation s2 + 2ζωn + ωn2 = 0
where tr = 0.4890, and ζ = 0.707, which may be different than parameters
found in Homework A.8.
(b) Add the state space matrices A, B, C, D derived in Homework A.6 to your
param file.
(c) Verify that the state space system is controllable by checking that rank(CA,B ) =
n.
(d) Find the feedback gain K so that the eigenvalues of (A−BK) are equal to the
desired closed loop poles. Find the reference gain kr so that the DC-gain

TOC
CHAPTER 11. FULL STATE FEEDBACK 175

from θr to θ is equal to one. Note that if you used the exact same pole values
from Homework A.8, K = (kp , kd ) where kp and kd are the proportional
and derivative gains found in that problem. Why?

(e) Modify the control code to implement the state feedback controller. To con-
struct the state x = (θ, θ̇)> use a digital differentiator to estimate θ̇.

Solution
From HW A.6, the state space equations for the single link robot arm are given by
   
0 1.0000 0
ẋ = x+ u
0 −0.667 66.667


y = 1 0 x.

Step 1. The controllability matrix is therefore

 
0 66.6667
CA,B = [B, AB] = .
66.6667 −44.44440

The determinant is det(CA,B ) = −4444 6= 0, therefore the system is con-

trollable.

Step 2. The open loop characteristic polynomial is

 
s −1
∆ol (s) = det(sI − A) = det = s2 + 0.667s,
0 s + 0.667

which implies that

aA = (0.667, 0)
 
1 0.667
AA =
0 1

Step 3. The desired closed loop polynomial is

∆dcl (s) = s2 + 2ζωn s + ωn2 = s2 + 6.3616s + 20.2408

which implies that

α = (6.3616, 20.2408).

Step 4. The gains are therefore given as

K = (α − aA )A−1 −1
A CA,B = 0.3036 0.0854
−1
kr = = 0.3036.
C(A − BK)−1 B

TOC
 CHAPTER 11. FULL STATE FEEDBACK 176

A Python class that implements state feedback control for the single link robot
arm is shown below.
1 import numpy as np
2 import armParamHW11 as P
3
4 class armController:
5 # dirty derivatives to estimate thetadot
6 def __init__(self):
7 self.K = P.K # state feedback gain
8 self.kr = P.kr # Input gain
9 self.limit = P.tau_max # Maximum torque
10 self.Ts = P.Ts # sample rate of controller
11
12 def update(self, theta_r, x):
13 theta = x.item(0)
14 thetadot = x.item(1)
15
16 # compute feedback linearizing torque tau_fl
17 tau_fl = P.m * P.g * (P.ell / 2.0) * np.cos(theta)
18
19 # Compute the state feedback controller
20 tau_tilde = -self.K @ x + self.kr * theta_r
21
22 # compute total torque
23 tau = self.saturate(tau_fl + tau_tilde)
24 return tau
25
26 def saturate(self,u):
27 if abs(u) > self.limit:
28 u = self.limit*np.sign(u)
29
30 return u

Listing 11.1: armController.py

Python code that computes the control gains is given below.

1 # Single link arm Parameter File
2 import numpy as np
3 import control as cnt
4 import sys
5 sys.path.append(’..’) # add parent directory
6 import armParam as P
7
8 Ts = P.Ts # sample rate of the controller
9 tau_max = P.tau_max # limit on control signal
10 m = P.m
11 ell = P.ell
12 g = P.g
13
14 # tuning parameters
15 tr = 0.4
16 zeta = 0.707
17
18 # State Space Equations
19 # xdot = A*x + B*u

TOC
 CHAPTER 11. FULL STATE FEEDBACK 177

20 # y = C*x
21 A = np.array([[0.0, 1.0],
22 [0.0, -1.0*P.b/P.m/(P.ell**2)]])
23
24 B = np.array([[0.0],
25 [3.0/P.m/(P.ell**2)]])
26
27 C = np.array([[1.0, 0.0]])
28
29 # gain calculation
30 wn = 2.2/tr # natural frequency
31 des_char_poly = [1, 2*zeta*wn, wn**2]
32 des_poles = np.roots(des_char_poly)
33
34 # Compute the gains if the system is controllable
35 if np.linalg.matrix_rank(cnt.ctrb(A, B)) != 2:
36 print("The system is not controllable")
37
38 else:
39 K = (cnt.acker(A, B, des_poles))
40 kr = -1.0/(C @ np.linalg.inv(A - B @ K) @ B)
41
42 print(’K: ’, K)
43 print(’kr: ’, kr)
44 print(des_poles)

Listing 11.2: armParamHW11.py

Complete simulation code for Matlab, Python, and Simulink can be down-
loaded at http://controlbook.byu.edu.

11.4 Design Study B. Inverted Pendulum

Example Problem B.11

The objective of this problem is to implement a state feedback controller using the
full state. Start with the simulation files developed in Homework B.10.

(a) Find the desired closed loop poles using the values for ζz , ωnθ , and ζθ selected
in Homework B.8 and an ωnz that gives tz = 3tθ .

(b) Add the state space matrices A, B, C, D derived in Homework B.6 to your
param file.

(c) Verify that the state space system is controllable by checking that: rank(C) =
n.

(d) Find the feedback gain K so that the eigenvalues of (A−BK) are equal to the
desired closed loop poles. Find the reference gain kr so that the DC-gain
from zr to z is equal to one.

TOC
CHAPTER 11. FULL STATE FEEDBACK 178

(e) Implement the state feedback scheme and tune the closed loop poles to get a
good response. You should be able to get a much faster response using state
space methods.

Solution
From HW B.6, the state space equations for the inverted pendulum are given by
   
0 0 1.0000 0 0
0 0 0 1.0000  0 
ẋ = 
0 −1.7294 −0.0471
x +  
 0.9412  u
0 
0 34.5882 0.1412 0 −2.8235
 
1 0 0 0
y= x, (11.39)
0 1 0 0
where Equation (12.2) represents the measured outputs. The reference output is
the position z, which implies that


yr = 1 0 0 0 x.
Step 1. The controllability matrix is therefore
CA,B = [B, AB, A2 B, A3 B]
 
0 0.9412 −0.0443 4.8851
 0 −2.8235 0.1329 −97.6672
= 0.9412 −0.0443
.
4.8851 −0.4597 
−2.8235 0.1329 −97.6672 5.2855
The determinant is det(CA,B ) = −6104.1 6= 0, implying that the system is
controllable.
Step 2. The open loop characteristic polynomial is
∆ol (s) = det(sI − A) = s4 + 0.0471s3 − 34.59s2 − 1.384s
which implies that
aA = (0.0471, −34.59, −1.384, 0)
 
1 0.0471 −34.59 −1.384
0 1 0.0471 −34.59
AA = 
0
.
0 1 0.0471 
0 0 0 1

Step 3. The desired closed loop polynomial is

∆dcl (s) = (s2 + 2ζθ ωnθ s + ωn2 θ )(s2 + 2ζz ωnz s + ωn2 z )
= s4 + 8.2955s3 + 34.4139s2 + 53.5334s + 41.6455
which implies that
α = (8.2955, 34.4139, 53.5334, 41.6455).

TOC
 CHAPTER 11. FULL STATE FEEDBACK 179

Step 4. The gains are therefore given as

K = (α − aA )A−1 −1
A CA,B = −1.5050, −24.9399, −1.9847, −3.5829

The feedforward reference gain kr = −1/Cr (A − BK)−1 B is computed

using Cr = (1, 0, 0, 0), which gives
−1
kr = = −1.5050.
Cr (A − BK)−1 B

A Python class that implements a state feedback controller for the inverted
pendulum is shown below.
1 import numpy as np
2 import pendulumParamHW11 as P
3
4 class pendulumController:
5 def __init__(self):
6 self.K = P.K # state feedback gain
7 self.kr = P.kr # Input gain
8 self.limit = P.F_max # Maxiumum force
9 self.Ts = P.Ts # sample rate of controller
10
11 def update(self, z_r, x):
12 # Compute the state feedback controller
13 F_unsat = -self.K @ x + self.kr * z_r
14 F_sat = self.saturate(F_unsat)
15 return F_sat.item(0)
16
17 def saturate(self,u):
18 if abs(u) > self.limit:
19 u = self.limit*np.sign(u)
20 return u

Listing 11.3: pendulumController.py

The gains are computed with the following Python script.

1 # Inverted Pendulum Parameter File
2 import numpy as np
3 import control as cnt
4 import sys
5 sys.path.append(’..’) # add parent directory
6 import pendulumParam as P
7
8 # sample rate of the controller
9 Ts = P.Ts
10
11 # saturation limits
12 F_max = 5.0 # Max Force, N
13
14 ####################################################
15 # State Space
16 ####################################################
17 # tuning parameters

TOC
 CHAPTER 11. FULL STATE FEEDBACK 180

18 tr_z = 1.5 # rise time for position

19 tr_theta = 0.5 # rise time for angle
20 zeta_z = 0.707 # damping ratio position
21 zeta_th = 0.707 # damping ratio angle
22
23 # State Space Equations
24 # xdot = A*x + B*u
25 # y = C*x
26 A = np.array([[0.0, 0.0, 1.0, 0.0],
27 [0.0, 0.0, 0.0, 1.0],
28 [0.0, -3*P.m1*P.g/4/(.25*P.m1+P.m2),
29 -P.b/(.25*P.m1+P.m2), 0.0],
30 [0.0, 3*(P.m1+P.m2)*P.g/2/(.25*P.m1+P.m2)/P.ell,
31 3*P.b/2/(.25*P.m1+P.m2)/P.ell, 0.0]])
32 B = np.array([[0.0],
33 [0.0],
34 [1/(.25*P.m1+P.m2)],
35 [-3.0/2/(.25*P.m1+P.m2)/P.ell]])
36
37 C = np.array([[1.0, 0.0, 0.0, 0.0],
38 [0.0, 1.0, 0.0, 0.0]])
39
40 # gain calculation
41 wn_th = 2.2/tr_theta # natural frequency for angle
42 wn_z = 2.2/tr_z # natural frequency for position
43 des_char_poly = np.convolve([1, 2*zeta_z*wn_z, wn_z**2],
44 [1, 2*zeta_th*wn_th, wn_th**2])
45 des_poles = np.roots(des_char_poly)
46
47 # Compute the gains if the system is controllable
48 if np.linalg.matrix_rank(cnt.ctrb(A, B)) != 4:
49 print("The system is not controllable")
50 else:
51 K = cnt.acker(A, B, des_poles)
52 Cr = np.array([[1.0, 0.0, 0.0, 0.0]])
53 kr = -1.0/(Cr @ np.linalg.inv(A - B @ K) @ B)
54
55 print(’K: ’, K)
56 print(’kr: ’, kr)

Listing 11.4: pendulumParamHW11.py

Complete simulation code for Matlab, Python, and Simulink can be down-
loaded at http://controlbook.byu.edu.

11.5 Design Study C. Satellite Attitude Control

Example Problem C.11

The objective of this problem is to implement a state feedback controller using the
full state. Start with the simulation files developed in Homework C.10.

TOC
CHAPTER 11. FULL STATE FEEDBACK 181

(a) Using the values for ωnφ , ζφ , ωnθ , and ζθ selected in Homework C.8, find the
desired closed loop poles.

(b) Add the state space matrices A, B, C, D derived in Homework C.6 to your
param file.

(c) Verify that the state space system is controllable by checking that rank(CA,B ) =
n.

(d) Find the feedback gain K so that the eigenvalues of (A−BK) are equal to the
desired closed loop poles. Find the reference gain kr so that the DC-gain
from φr to φ is equal to one.

(e) Implement the state feedback scheme and tune the closed loop poles to get a
good response. You should be able to get a much faster response using state
space methods.

Solution
From HW C.6, the state space equations for the satellite are given by
   
0 0 1.0000 0 0
 0 0 0 1.0000  0
ẋ = 
−0.0300 0.0300 −0.0100 0.0100  x + 0.2 u
  

0.1500 −0.1500 0.0500 −0.0500 0

 
1 0 0 0
y= x (11.40)
−1 1 0 0

where Equation (11.40) represents the measured outputs. The reference output is
the angle φ, which implies that


yr = 0 1 0 0 x.

Step 1. The controllability matrix is

CA,B = [B, AB, A2 B, A3 B]

 
0 0.2000 −0.0020 −0.0059
 0 0 0.0100 0.0294 
=0.2000 −0.0020 −0.0059
.
0.0007 
0 0.0100 0.0294 −0.0036

The determinant is det(CA,B ) = −36, 000 6= 0, therefore the system is

controllable.

Step 2. The open loop characteristic polynomial is

∆ol (s) = det(sI − A) = s4 + 0.0600s3 + 0.18s2

TOC
 CHAPTER 11. FULL STATE FEEDBACK 182

which implies that

aA = (0.06, 0.18, 0, 0)
 
1 0.06 0.18 0
0 1 0.06 0.18
AA = 
0
.
0 1 0.06
0 0 0 1

Step 3. The desired closed loop polynomial is

∆dcl (s) = (s2 + 2ζθ ωnθ s + ωn2 θ )(s2 + 2ζφ ωnφ s + ωn2 φ )
= s4 + 4.9275s3 + 12.1420s2 + 14.6702s + 8.8637,
when ωθ = 1.9848, ζθ = 0.707, ωφ = 1.5, ζφ = 0.707, which implies that
α = (4.9275, 12.1420, 14.6702, 8.8637).

Step 4. The gains are therefore given by

K = (α − aA )A−1 −1
A CA,B = (40.2842, 255.1732, 24.3375, 366.1825)

The feedforward reference gain kr = −1/Cr (A − BK)−1 B is computed

using Cr = (0, 1, 0, 0), which gives
−1
kr = = 295.4573.
Cr (A − BK)−1 B

A Python class that implements a PID controller for the satellite is shown
below.
1 import numpy as np
2 import satelliteParamHW11 as P
3
4 class satelliteController:
5 def __init__(self):
6 self.K = P.K # state feedback gain
7 self.kr = P.kr # Input gain
8 self.limit = P.tau_max # Maxiumum torque
9 self.Ts = P.Ts # sample rate of controller
10
11 def update(self, phi_r, x):
12 # Compute the state feedback controller
13 tau_unsat = -self.K @ x + self.kr * phi_r
14 tau = self.saturate(tau_unsat.item(0))
15 return tau
16
17 def saturate(self,u):
18 if abs(u) > self.limit:
19 u = self.limit*np.sign(u)
20 return u

Listing 11.5: satelliteController.py

TOC
 CHAPTER 11. FULL STATE FEEDBACK 183

The gains are computed with the following Python script.

1 # satellite Parameter File
2 import numpy as np
3 import control as cnt
4 import sys
5 sys.path.append(’..’) # add parent directory
6 import satelliteParam as P
7
8 # import variables from satelliteParam
9 Ts = P.Ts
10 tau_max = P.tau_max
11
12 # tuning parameters
13 tr_phi = 2
14 tr_th = 3.66
15 zeta_phi = 0.707 # damping ratio position
16 zeta_th = 0.707 # damping ratio angle
17
18 # State Space Equations
19 # xdot = A*x + B*u
20 # y = C*x
21 A = np.array([[0.0, 0.0, 1.0, 0.0],
22 [0.0, 0.0, 0.0, 1.0],
23 [-P.k/P.Js, P.k/P.Js, -P.b/P.Js, P.b/P.Js],
24 [P.k/P.Jp, -P.k/P.Jp, P.b/P.Jp, -P.b/P.Jp]])
25
26 B = np.array([[0.0],
27 [0.0],
28 [1.0/P.Js],
29 [0.0]])
30
31 C = np.array([[1.0, 0.0, 0.0, 0.0],
32 [0.0, 1.0, 0.0, 0.0]])
33
34 # gain calculation
35 wn_th = 2.2/tr_th
36 wn_phi = 2.2/tr_phi
37 des_char_poly = np.convolve([1, 2*zeta_th*wn_th, wn_th**2],
38 [1, 2*zeta_phi*wn_phi, wn_phi**2])
39 des_poles = np.roots(des_char_poly)
40
41 # Compute the gains if the system is controllable
42 if np.linalg.matrix_rank(cnt.ctrb(A, B)) != 4:
43 print("The system is not controllable")
44 else:
45 K = cnt.acker(A, B, des_poles)
46 Cr = np.array([[1.0, 0.0, 0.0, 0.0]])
47 kr = -1.0/(Cr @ np.linalg.inv(A - B @ K) @ B)
48
49 print(’K: ’, K)
50 print(’kr: ’, kr)

Listing 11.6: satelliteParamHW11.py

Complete simulation code for Matlab, Python, and Simulink can be down-
loaded at http://controlbook.byu.edu.

TOC
CHAPTER 11. FULL STATE FEEDBACK 184

Important Concepts:
• Ackerman’s formula will put the closed-loop poles of a controllable
SISO system at any desired locations.
• The columns of the controllability matrix are equal to Ai B with i
ranging from zero to n − 1, where n is equal to the number of states.
• If a system is controllable then the associated controllability matrix
will be full rank.
• Control canonical form is a specific state-space representation where
the A matrix contains the negative coefficients of the characteristic
polynomial in the top row. The B vector has a single one in its first
row and all other entries as zeros. And the C matrix contains the
coefficients of the transfer function’s numerator.
• In closed-loop control canonical form each control gain directly mod-
ifies one of the characteristic polynomial coefficients. Since it is
possible to individually modify every coefficient in the characteristic
polynomial, the system’s poles may be moved to any desired loca-
tion.

Notes and References

Controllability is typically defined as follows. The state space system

ẋ = Ax + Bu

is said to be controllable from initial state x0 , if there exists a control function u(t)
defined on some interval t ∈ [0, T ] that transitions the state to the origin. It can be
shown that for linear time-invariant systems, the system is controllable if and only
if the rank of the controllability matrix CA,B is equal to the size of the state n. For
single input systems, this is equivalent to CA,B being invertible. Controllability
is an interesting and deep topic that extends nicely to nonlinear and multi-input
systems. For further information see [5, 6, 12].
Ackermann’s formula given in Equation (11.32) is only valid for single input
systems. In the multi-input case, the controllability matrix CA,B is not square and
therefore cannot be inverted. However, a more general formula can be derived
if CA,B has rank n. The more general formula is implemented by the place
command in both Matlab and Python. For additional information on the derivation
of the feedback gain in the MIMO case see [6].

TOC
 Integrator with Full State Feedback
12
Learning Objectives:
• Eliminate the steady-state error by incorporating an integrator into
the full-state feedback controller.

12.1 Theory
In the previous chapter we derived a full state feedback controller that stabilized
the system and ensured that the DC-gain from the reference r(t) to the reference
output yr (t) is equal to one. If the model is precise meaning that we know all of
the parameters exactly and we have not neglected any dynamics, or if there are
no disturbances on the system, then the output will track the reference as time
goes to infinity. However, models are never precise, and there are almost always
disturbances on the system, which implies that there will be steady state error in
the response. In Chapter 9 we showed that adding an integrator to the controller is
an effective way of eliminating steady state error caused by model mismatch and
input/output disturbances. In this section we will show how to add an integrator
to the full state feedback controller derived in Chapter 11.
A block diagram of the system with full state feedback and an integrator is
shown in Fig. 12-1. Let the state of the integrator be defined as xI , as shown in
Fig. 12-1, then
ẋI = r − yr = r − Cr x.
In Appendix P.6 we argue using the root locus, that adding an integrator after
the other gains are selected, will change all of the pole locations. Since the pole
placement design methods described in Part III were limited to second order sys-
tems, we did not have an effective method for placing the system poles and the
integrator pole simultaneously. On the other hand, the pole placement technique

TOC
 CHAPTER 12. FULL STATE WITH INTEGRATORS 186

Figure 12-1: Full state feedback with integrator.

described in Chapter 11 was not limited to second order systems. As such, there
should be a way to use the methods of Chapter 11 to simultaneously place both
the system poles and the integrator pole. To do so, we augment the states of the
original system with the integrator state xI . Since the dynamics of the original
system are given by ẋ = Ax + Bu, the dynamics of the augmented system are
given by         
ẋ A 0 x B 0
= + u+ r, (12.1)
ẋI −Cr 0 xI 0 1
where the state of the augmented system is (x> , xI )> , the interaction matrix, or
the “A-matrix,” of the augmented system is
 
A 0
A1 = ,
−Cr 0

and the “B-matrix” of the augmented system is

 
B
B1 = .
0

The first question that might be asked is whether the augmented system (A1 , B1 )
is controllable when the original system (A, B) is controllable. Since the original
system is controllable, we know that the controllability matrix


CA,B = B AB . . . An−1 B

has rank n. For the augmented system, the controllability matrix is

A2 B An−1 B An B
 
B AB ...
CA1 ,B1 = ,
0 −Cr B −Cr AB ... −Cr An−2 B −Cr An−1 B

which is guaranteed to have rank n + 1 if Cr An−1 B 6= 0. When the aug-

mented system is controllable, we can use the pole placement technique described
in Chapter 11 to place the system poles and the pole of the integrator simultane-
ously using the augmented system. For example, letting A1 and B1 represent the
augmented system, and using the Python place command
1 import control as cnt
2 K1 = cnt.place(A1,B1, [p1,...,pn,pI])

TOC
CHAPTER 12. FULL STATE WITH INTEGRATORS 187

where pI is the integrator pole. The resulting controller is

   
x
x
u(t) = −K1 = − K kI = −Kx − kI xI
xI xI
Z t
= −Kx(t) − kI (r(τ ) − yr (τ ))dτ .
| {z }
state feedback | 0 {z }
integral feedback

We can show that adding the integrator ensures that the DC-gain of the closed-
loop system is equal to one. Indeed from Equation (12.1) the augmented closed-
loop system is
        
ẋ A 0 x B
0
= − K kI + r
ẋI −Cr 0 xI 0 1
    
A − BK −BkI x 0
= + r,
−Cr 0 xI 1

where the reference output equation is

 

x
yr = Cr 0 .
xI

Therefore, the transfer function from R(s) to Yr (s) is

  −1  

A − BK −BkI 0
Yr (s) = Cr 0 sI − R(s),
−Cr 0 1

which implies that the DC-gain is

 −1  

A − BK −BkI 0
kDC = − Cr 0 .
−Cr 0 1

4
Defining M = (A − BK)−1 , and using the Matrix inversion formula for block
matrices given in Appendix P.7 Equation (P.7.2) we get

M − M BkI (Cr M BkI )−1 Cr M −M BkI (Cr M BkI )−1
  
0
kDC = − Cr 0
−(Cr M BkI )−1 Cr M (Cr M BkI )−1 1
= Cr M BkI (Cr M BkI )−1
= 1.

12.1.1 Integral Feedback for Linearized Systems

Suppose that the state space model is from a linearized system, i.e, suppose that
x ∈ Rn is the state, xe ∈ Rn is the equilibrium state, and that x̃ = x − xe is

TOC
CHAPTER 12. FULL STATE WITH INTEGRATORS 188

the state linearized around the equilibrium. Similarly, suppose that u ∈ Rm is

the input, that ue ∈ Rm is the input at equilibrium, and that ũ = u − ue is the
input linearized around equilibrium, and that yr ∈ Rp is the reference output,
yre = Cr xe ∈ Rp is the reference output at equilibrium, ỹr = yr − yre is the
reference output linearized about equilibrium, and the referenceRinput is given by
r̃ = r − yre . In this case the integral state is selected as x̃I = (r̃ − ỹr )dτ , and
the augmented system is given by
        
x̃˙ A 0 x̃ B 0
= + ũ + r̃.
x̃˙ I −Cr 0 x̃I 0 1
The pole placement design proceeds as in the previous section with the resulting
controller being Z t
ũ = −K x̃ − kI ẽ(τ )dτ.
−∞
Noting that
ẽ = r̃ − ỹ = (r − yre ) − (yr − yre ) = r − yr
we get that the controller for the linearized system is given by
Z t
u = ue − K(x − xe ) − kI (r(τ ) − yr (τ ))dτ.
−∞

12.2 Summary of Design Process - State Feedback

with Integrator
The design process for adding an integrator to state feedback can be summarized
as follows. Given the plant
ẋ = Ax + Bu
yr = Cr x,
where we desire that yr tracks the reference r using integral action using the con-
troller Z t
u(t) = −Kx(t) − kI (r(τ ) − yr (τ ))dτ,
−∞
with desired closed-loop gains at p1 , p2 , . . . , pn , pI .
Step 1. Augment the state evolution equation as
        
ẋ A 0 x B 0
= + u+ r,
ẋI −Cr 0 xI 0 I
and let
 
A 0
A1 =
−Cr 0
 
B
B1 =
0

TOC
 CHAPTER 12. FULL STATE WITH INTEGRATORS 189

Step 2. Follow the pole placement procedure outlined in Chapter 11 to find K1

that places the poles of the augmented system at p1 , p2 , . . . , pn , and pI .

Step 3. Find the gains K and kI as

K = K1 (1 : n)
kI = K1 (n + 1).

Using Python, these steps can be implemented using the following script.

1 import numpy as np
2 import control as cnt
3
4 # augment the system matrices
5 A1 = np.vstack((
6 np.hstack((A, np.zeros((np.size(A,1),1)))),
7 np.hstack((-Cr, 0)) ))
8
9 B1 = np.vstack((
10 B, 0))
11
12 # Compute the gains if the system is controllable
13 if np.linalg.matrix_rank(cnt.ctrb(A1, B1)) != np.size(A1,1):
14 print("The system is not controllable")
15 else:
16 K1 = cnt.acker(A1, B1, [p1, p2, ..., pn, pI])
17
18 #extract the state feedback gains and the integral gain
19 K = K1[0,0:-1]
20 ki = K1[0,-1]
21
22 print(’K: ’, K)
23 print(’ki: ’, ki)

12.2.1 Simple example

Given the system

   
0 1 0
ẋ = x+ u
2 3 4


yr = 5 0 x

find the feedback gain K to place the closed-loop poles at −1±j, and the reference
gain kr so that the DC-gain is one, and add an integrator with gain at pI = −0.1.

TOC
CHAPTER 12. FULL STATE WITH INTEGRATORS 190

Step 1. Form the augmented system

 
  0 1 0
A 0
A1 = = 2 3 0
−Cr0
−5 0 0
 
  0
B
B1 = = 4
0
0

Step 2. To place the poles at p1 = −1 + j, p2 = −1 − j, pI = −0.1, we first

form the controllability matrix
 
0 4 12
CA1 ,B1 = [B1 , A1 B1 , A21 B1 ] = 4 12 44  .
0 0 −20
The determinant is det(CA1 ,B1 ) = 320 6= 0, therefore the system is con-
trollable.
The open-loop characteristic polynomial
 
s −1 0
∆ol (s) = det(sI − A1 ) = det −2 s−3 0
5 0 s
= s3 − 3s2 − 2s,
which implies that


aA1 = −3, −2, 0
 
1 −3 −2
AA1 = 0 1 −3 .
0 0 1

The desired closed-loop polynomial

∆dcl (s) = (s + 1 − j)(s + 1 + j)(s + 0.1)
= s3 + 2.1s2 + 2.2s + 0.2,
which implies that

α = 2.1, 2.2, 0.2 .
The augmented gains are therefore given as
K1 = (α − aA1 )A−1 −1
A1 CA1 ,B1
= ((2.1, 2.2, 0.2) − (−3, −2, 0)) ·
 −1  −1
1 −3 −2 0 4 12
0 1 −3 4 12 44 
0 0 1 0 0 −20


= 1.05, 1.275, −0.01

TOC
 CHAPTER 12. FULL STATE WITH INTEGRATORS 191

Step 3. The feedback gains are therefore given by

K = K1 (1 : 2) = 1.05, 1.275
kI = K1 (3) = −0.01

Alternatively, we could have used the following Python script

1 import numpy as np
2 import control as cnt
3
4 # define original state space system and desired poles
5 A = np.array([[0.0, 1.0], [2.0, 3.0]])
6 B = np.array([[0.0], [4.0]])
7 Cr = np.array([5.0, 0])
8
9 p1 = -1.0 - 1.j
10 p2 = -1.0 + 1.j
11 pI = -0.1
12
13 # augment the system matrices
14 A1 = np.vstack((
15 np.hstack((A, np.zeros((np.size(A,1),1)))),
16 np.hstack((-Cr, 0)) ))
17 B1 = np.vstack((
18 B, 0))
19
20 # Compute the gains if the system is controllable
21 if np.linalg.matrix_rank(cnt.ctrb(A1, B1)) != np.size(A1,1):
22 print("The system is not controllable")
23 else:
24 K1 = cnt.acker(A1, B1, [p1, p2, pI])
25
26 #extract the state feedback gains and the integral gain
27 K = K1[0,0:-1]
28 ki = K1[0,-1]

12.3 Design Study A. Single Link Robot Arm

Example Problem A.12

(a) Modify the state feedback solution developed in Homework A.11 to add an
integrator with anti-windup.
(b) Add a disturbance to the system and allow the system parameters to change
up to 20%.
(c) Tune the integrator pole (and other gains if necessary) to get good tracking
performance.

TOC
CHAPTER 12. FULL STATE WITH INTEGRATORS 192

Solution
Step 1. The original state space equations are
   
0 1.0000 0
ẋ = x+ u
0 −0.667 66.667


y = 1 0 x,

therefore the augmented system is

 
  0 1.0000 0
0A
A1 = =  0 −0.667 0
−C
0
−1 0 0
 
  0
B
B1 = = 66.667
0
0

Step 2. After the design in HW A.11, the closed loop poles were located at p1,2 =
−3.1191 ± j3.1200. We will add the integrator pole at pI = −5. The new
controllability matrix is

CA1 ,B1 = [B1 , A1 B1 , A21 B1 ]

 
0 66.6670 −44.4669
= 66.6670 −44.4669 29.6594  .
0 0 −66.6670

The determinant is det(CA1 ,B1 ) = 2.9630e + 05 6= 0, therefore the system

is controllable.
The open loop characteristic polynomial
 
s −1.0000 0
∆ol (s) = det(sI − A1 ) = det 0 s + 0.667 0
1 0 s
= s3 + 0.6174s2 ,

which implies that

aA1 = 0.6174, 0, 0
 
1 0.6174 0
AA1 = 0 1 0.6174 .
0 0 1

The desired closed loop polynomial

∆dcl (s) = (s + 3.8885 − j3.8897)(s + 3.8885 + j3.8897)(s + 5)

= s3 + 12.7770s2 + 69.1350s + 151.2500,

TOC
 CHAPTER 12. FULL STATE WITH INTEGRATORS 193

which implies that

α = 12.7770, 69.1350, 151.2500 .

The augmented gains are therefore given as

K1 = (α − aA1 )A−1 −1
A1 CA1 ,B1


= 1.0370, 0.1817, −2.2687

Step 3. The feedback gains are therefore given by

K = K1 (1 : 2) = 1.0370, 0.1817
kI = K1 (3) = −2.2687

A Python class that implements state feedback control with an integrator for
the single link robot arm is shown below.
1 import numpy as np
2 import armParamHW12 as P
3
4 class armController:
5 def __init__(self):
6 self.integrator = 0.0 # integrator
7 self.error_d1 = 0.0 # error signal delayed by 1 sample
8 self.K = P.K # state feedback gain
9 self.ki = P.ki # Input gain
10 self.limit = P.tau_max # Maximum torque
11 self.Ts = P.Ts # sample rate of controller
12
13 def update(self, theta_r, x):
14 theta = x.item(0)
15
16 # integrate error
17 error = theta_r - theta
18 self.integrateError(error)
19
20 # compute feedback linearizing torque tau_fl
21 tau_fl = P.m * P.g * (P.ell / 2.0) * np.cos(theta)
22
23 # Compute the state feedback controller
24 tau_tilde = -self.K @ x - self.ki*self.integrator
25
26 # compute total torque
27 tau = self.saturate(tau_fl + tau_tilde)
28 return tau
29
30 def integrateError(self, error):
31 self.integrator = self.integrator \
32 + (self.Ts/2.0)*(error + self.error_d1)
33 self.error_d1 = error
34
35 def saturate(self,u):
36 if abs(u) > self.limit:

TOC
 CHAPTER 12. FULL STATE WITH INTEGRATORS 194

37 u = self.limit*np.sign(u)
38 return u

Listing 12.1: armController.py

Python code that computes the control gains is given below.

1 # Single link arm Parameter File
2 import numpy as np
3 import control as cnt
4 import sys
5 sys.path.append(’..’) # add parent directory
6 import armParam as P
7
8 Ts = P.Ts # sample rate of the controller
9 #beta = P.beta # dirty derivative gain
10 tau_max = P.tau_max # limit on control signal
11 m = P.m
12 ell = P.ell
13 g = P.g
14
15 # tuning parameters
16 tr = 0.4
17 zeta = 0.707
18 integrator_pole = np.array([-0.5])
19
20 # State Space Equations
21 # xdot = A*x + B*u
22 # y = C*x
23 A = np.array([[0.0, 1.0],
24 [0.0, -1.0*P.b/P.m/(P.ell**2)]])
25
26 B = np.array([[0.0],
27 [3.0/P.m/(P.ell**2)]])
28
29 C = np.array([[1.0, 0.0]])
30
31 # form augmented system
32 A1 = np.array([[0.0, 1.0, 0.0],
33 [0.0, -1.0*P.b/P.m/(P.ell**2), 0.0],
34 [-1.0, 0.0, 0.0]])
35
36 B1 = np.array([[0.0],
37 [3.0/P.m/(P.ell**2)],
38 [0.0]])
39
40 # gain calculation
41 #wn = 2.2/tr # natural frequency if zeta = 0.707
42 wn = 0.5*np.pi/(tr*np.sqrt(1-zeta**2)) # natural frequency
43
44 des_char_poly = np.convolve(
45 [1, 2*zeta*wn, wn**2],
46 np.poly(integrator_pole))
47 des_poles = np.roots(des_char_poly)
48
49 # Compute the gains if the system is controllable
50 if np.linalg.matrix_rank(cnt.ctrb(A1, B1)) != 3:

TOC
 CHAPTER 12. FULL STATE WITH INTEGRATORS 195

51 print("The system is not controllable")

52 else:
53 K1 = cnt.place(A1, B1, des_poles)
54 K = np.array([[K1.item(0), K1.item(1)]])
55 ki = K1.item(2)
56
57 print(’K: ’, K)
58 print(’ki: ’, ki)

Listing 12.2: armParamHW12.py

Complete simulation code for Matlab, Python, and Simulink can be down-
loaded at http://controlbook.byu.edu.

12.4 Design Study B. Inverted Pendulum

Example Problem B.12

(a) Modify the state feedback solution developed in Homework B.11 to add an
integrator with anti-windup to the position feedback.

(b) Add a constant input disturbance of 0.5 Newtons to the input of the plant and
allow the system parameters to change up to 20%.

(c) Tune the integrator pole (and other gains if necessary) to get good tracking
performance.

Solution
Step 1. The original state space equations are
   
0 0 1.0000 0 0
0 0 0 1.0000  0 
ẋ = 
0 −1.7294 −0.0471
x +  
 0.9412  u
0 
0 34.5882 0.1412 0 −2.8235
 
1 0 0 0
y= x, (12.2)
0 1 0 0

The integrator will only be on z = x1 , therefore

Cr = 1 0 0 0 .

TOC
CHAPTER 12. FULL STATE WITH INTEGRATORS 196

The augmented system is therefore

 
A 0
A1 =
−Cr 0
 
0 0 1.0000 0 0
 0 0 0 1.0000 0
 
=  0 −1.7294 −0.0471 0 0

 0 34.5882 0.1412 0 0
−1.0000 0 0 0 0
 
0
   0 
B  
B1 = =  0.9412 

0 −2.8235
0

Step 2. After the design in HW B.11, the closed loop poles were located at p1,2 =
−1.4140 ± j1.4144, p3,4 = −0.8000 ± j0.6000. We will add the integrator
pole at pI = −10. The new controllability matrix
CA1 ,B1 = [B1 , A1 B1 , A21 B1 , A31 B1 , A41 B1 ]
 
00.94118 −0.044291 4.8851 −0.45968
 0 −2.8235 0.13287 −97.667 5.2855 
 

=  0.94118 −0.044291 4.8851 −0.45968 168.93 .
 −2.8235 0.13287 −97.667 5.2855 −3378.2
0 0 −0.94118 0.044291 −4.8851
The determinant is nonzero, therefore the system is controllable.
The open loop characteristic polynomial
∆ol (s) = det(sI − A1 )
 
s 0 −1 0 0
0 s 0 −1 0
 
= det 
0 1.7294 s + 0.0471 0 0

0 −34.588 −0.14118 s 0
1 0 0 0 s
= s5 + 0.0471s4 − 34.588s3 − 1.3835s2 ,
which implies that


aA1 = 0.0471, −34.588, 1.3835, 0, 0
 
1 0.0471 −34.588 −1.3835 0
0 1 0.0471 −34.588 −1.3835
 
AA1 = 
 0 0 1 0.0471 −34.588.
0 0 0 1 0.0471 
0 0 0 0 1

TOC
 CHAPTER 12. FULL STATE WITH INTEGRATORS 197

The desired closed loop polynomial

∆dcl (s) = (s + 1.0369 − j1.0372)(s + 1.0369 + j1.0372) . . .

(s + 3.1108 − j3.1117)(s + 3.1108 + j3.1117)(s + 2)
= s5 + 10.2955s4 + 51.0048s3 + 122.3612s2
+ 148.7123s + 83.2910,

which implies that

α = 10.2955, 51.0048, 122.3612, 148.7123, 83.2910 .

The augmented gains are therefore given as

K1 = (α − aA1 )A−1 −1
A1 CA1 ,B1


= −5.3744, −32.1057, −4.5745, −5.1545, 3.0101

Step 3. The feedback gains are therefore given by

K = K1 (1 : 4) = −5.3744, −32.1057, −4.5745, −5.1545
kI = K1 (5) = 3.0101

A Python class that implements a state feedback controller with integrator for
the inverted is shown below.
1 import numpy as np
2 import pendulumParamHW12 as P
3
4 class pendulumController:
5 def __init__(self):
6 self.integrator = 0.0 # integrator
7 self.error_d1 = 0.0 # error signal delayed by 1 sample
8 self.K = P.K # state feedback gain
9 self.ki = P.ki # integrator gain
10 self.limit = P.F_max # Maximum force
11 self.Ts = P.Ts # sample rate of controller
12
13 def update(self, z_r, x):
14 z = x.item(0)
15 # integrate error
16 error = z_r - z
17 self.integrateError(error)
18 # Compute the state feedback controller
19 F_unsat = -self.K @ x - self.ki*self.integrator
20 F_sat = self.saturate(F_unsat)
21 return F_sat.item(0)
22
23 def integrateError(self, error):
24 self.integrator = self.integrator \
25 + (self.Ts/2.0)*(error + self.error_d1)

TOC
 CHAPTER 12. FULL STATE WITH INTEGRATORS 198

26 self.error_d1 = error
27
28 def saturate(self, u):
29 if abs(u) > self.limit:
30 u = self.limit*np.sign(u)
31 return u

Listing 12.3: pendulumController.py

The gains are computed with the following Python script.

1 # Inverted Pendulum Parameter File
2 import numpy as np
3 import control as cnt
4 import sys
5 sys.path.append(’..’) # add parent directory
6 import pendulumParam as P
7
8 # sample rate of the controller
9 Ts = P.Ts
10
11 # saturation limits
12 F_max = 5.0 # Max Force, N
13 theta_max = 30.0*np.pi/180.0 # Max theta, rads
14
15 ####################################################
16 # State Space
17 ####################################################
18 # tuning parameters
19 tr_z = 1.5 # rise time for position
20 tr_theta = 0.5 # rise time for angle
21 zeta_z = 0.707 # damping ratio position
22 zeta_th = 0.707 # damping ratio angle
23 integrator_pole = -2 # integrator pole
24
25 # State Space Equations
26 # xdot = A*x + B*u
27 # y = C*x
28 A = np.array([[0.0, 0.0, 1.0, 0.0],
29 [0.0, 0.0, 0.0, 1.0],
30 [0.0, -3*P.m1*P.g/4/(.25*P.m1+P.m2),
31 -P.b/(.25*P.m1+P.m2), 0.0],
32 [0.0, 3*(P.m1+P.m2)*P.g/2/(.25*P.m1+P.m2)/P.ell,
33 3*P.b/2/(.25*P.m1+P.m2)/P.ell, 0.0]])
34
35 B = np.array([[0.0],
36 [0.0],
37 [1/(.25*P.m1+P.m2)],
38 [-3.0/2/(.25*P.m1+P.m2)/P.ell]])
39
40 C = np.array([[1.0, 0.0, 0.0, 0.0],
41 [0.0, 1.0, 0.0, 0.0]])
42
43 # form augmented system
44 Cout = np.array([[1.0, 0.0, 0.0, 0.0]])
45 A1 = np.array([[0.0, 0.0, 1.0, 0.0, 0.0],
46 [0.0, 0.0, 0.0, 1.0, 0.0],

TOC
 CHAPTER 12. FULL STATE WITH INTEGRATORS 199

47 [0.0, -3*P.m1*P.g/4/(.25*P.m1+P.m2),
48 -P.b/(.25*P.m1+P.m2), 0.0, 0.0],
49 [0.0, 3*(P.m1+P.m2)*P.g/2/(.25*P.m1+P.m2)/P.ell,
50 3*P.b/2/(.25*P.m1+P.m2)/P.ell, 0.0, 0.0],
51 [-1.0, 0.0, 0.0, 0.0, 0.0]])
52 B1 = np.array([[0.0],
53 [0.0],
54 [1/(.25*P.m1+P.m2)],
55 [-3.0/2/(.25*P.m1+P.m2)/P.ell],
56 [0.0]])
57
58 # gain calculation
59 wn_th = 2.2/tr_theta # natural frequency for angle
60 wn_z = 2.2/tr_z # natural frequency for position
61 des_char_poly = np.convolve(
62 np.convolve([1, 2*zeta_z*wn_z, wn_z**2],
63 [1, 2*zeta_th*wn_th, wn_th**2]),
64 np.poly([integrator_pole]))
65 des_poles = np.roots(des_char_poly)
66
67 # Compute the gains if the system is controllable
68 if np.linalg.matrix_rank(cnt.ctrb(A1, B1)) != 5:
69 print("The system is not controllable")
70 else:
71 K1 = cnt.acker(A1, B1, des_poles)
72 K = np.array([[K1.item(0), K1.item(1),
73 K1.item(2), K1.item(3)]])
74 ki = K1.item(4)
75
76 print(’K: ’, K)
77 print(’ki: ’, ki)

Listing 12.4: pendulumParamHW12.py

Complete simulation code for Matlab, Python, and Simulink can be down-
loaded at http://controlbook.byu.edu.

12.5 Design Study C. Satellite Attitude Control

Example Problem C.12

(a) Modify the state feedback solution developed in Homework C.11 to add an
integrator with anti-windup to the feedback loop for φ.

(b) Add a constant input disturbance of 1 Newtons-meters to the input of the plant
and allow the plant parameters to vary up to 20%.

(c) Tune the integrator pole (and other gains if necessary) to get good tracking
performance.

TOC
CHAPTER 12. FULL STATE WITH INTEGRATORS 200

Solution
Step 1. The original state space equations are
   
0 0 1 0 0
 0 0 0 1   0 
ẋ =    
−0.03 0.03 −0.01 0.01  x + 0.21 u
0.14 −0.14 0.05 −0.05 0
 
1 0 0 0
y= x.
0 1 0 0

The integrator will only be on φ = x2 , therefore

Cr = 0 1 0 0 .

The augmented system is therefore

 
A 0
A1 =
−Cr 0
 
0 0 1 0 0
 0 0 0 1 0
 
=  −0.03 0.03 −0.01 0.01 0

 0.14 −0.14 0.05 −0.05 0
0 −1 0 0 0
 
0
   0 
B  
B1 = = 0.21
0  0 
0

Step 2. After the design in HW C.11, the closed loop poles were located at p1,2 =
−1.0605 ± j1.0608, p3,4 = −0.7016 ± j0.7018. We will add the integrator
pole at pI = −1. The new controllability matrix

CA1 ,B1 = [B1 , A1 B1 , A21 B1 , A31 B1 , A41 B1 ]

 
0 0.21 −0.0023 −0.0062 0.0008
 0 0 0.011 0.0279 −0.0034
 
= 0.21 −0.0023 −0.0062 0.0008 0.0010 
.
 0 0.011 0.0279 −0.0034 −0.0044
0 0 0 0.0105 0.0279
The determinant is nonzero, therefore the system is controllable.
The open loop characteristic polynomial

∆ol (s) = det(sI − A1 )

= s5 + 0.0611s4 + 0.1657s3 ,

TOC
 CHAPTER 12. FULL STATE WITH INTEGRATORS 201

which implies that

aA1 = 0.0611, 0.1657, 0, 0, 0
 
1 0.0611 0.1657 0 0
0 1 0.0611 0.1657 0 
 

AA1 = 0 0 1 0.0611 0.1657.
0 0 0 1 0.0611
0 0 0 0 1

The desired closed loop polynomial

∆dcl (s) = (s + 1.0605 − j1.0608)(s + 1.0605 + j1.0608) . . .

(s + 0.7016 − j0.7018)(s + 0.7016 + j0.7018)(s + 1)
= s5 + 3.4038s4 + 5.2934s3 + 4.4761s2
+ 2.0221s + 0.4356,

which implies that

α = 3.4038, 5.2934 4.4761, 2.0221, 0.4356 .

The augmented gains are therefore given as

K1 = (α − aA1 )A−1 −1
A1 CA1 ,B1


= 19.15, 43.41, 16.72, 111.63, −14.52 .

Step 3. The feedback gains are therefore given by

K = K1 (1 : 4) = 19.15, 43.41, 16.72, 111.63
kI = K1 (5) = −14.5200.

A Python class that implements a PID controller for the satellite is shown
below.
1 import numpy as np
2 import satelliteParamHW12 as P
3
4 class satelliteController:
5 def __init__(self):
6 self.integrator = 0.0 # integrator
7 self.error_d1 = 0.0 # error delayed by 1 sample
8 self.K = P.K # state feedback gain
9 self.ki = P.ki # integrator gain
10 self.limit = P.tau_max # Maxiumum torque
11 self.Ts = P.Ts # sample rate of controller
12
13 def update(self, phi_r, x):
14 theta = x.item(0)
15 phi = x.item(1)

TOC
 CHAPTER 12. FULL STATE WITH INTEGRATORS 202

16
17 # integrate error
18 error = phi_r - phi
19 self.integrateError(error)
20
21 # Compute the state feedback controller
22 tau_unsat = -self.K @ x - self.ki*self.integrator
23 tau = self.saturate(tau_unsat.item(0))
24
25 return tau
26
27 def integrateError(self, error):
28 self.integrator = self.integrator + (self.Ts / 2.0) \
29 * (error + self.error_d1)
30
31 self.error_d1 = error
32
33 def saturate(self,u):
34 if abs(u) > self.limit:
35 u = self.limit*np.sign(u)
36 return u

Listing 12.5: satelliteController.py

The gains are computed with the following Python script.

1 # satellite Parameter File
2 import numpy as np
3 import control as cnt
4 import sys
5 sys.path.append(’..’) # add parent directory
6 import satelliteParam as P
7
8 # import variables from satelliteParam
9 Ts = P.Ts
10 sigma = P.sigma
11 beta = P.beta
12 tau_max = P.tau_max
13
14 # tuning parameters
15 wn_th = 0.6
16 wn_phi = 1.1 # rise time for angle
17 zeta_phi = 0.707 # damping ratio position
18 zeta_th = 0.707 # damping ratio angle
19 integrator_pole = np.array([-1])
20
21
22 # State Space Equations
23 # xdot = A*x + B*u
24 # y = C*x
25 A = np.array([[0.0, 0.0, 1.0, 0.0],
26 [0.0, 0.0, 0.0, 1.0],
27 [-P.k/P.Js, P.k/P.Js, -P.b/P.Js, P.b/P.Js],
28 [P.k/P.Jp, -P.k/P.Jp, P.b/P.Jp, -P.b/P.Jp]])
29
30 B = np.array([[0.0],
31 [0.0],

TOC
 CHAPTER 12. FULL STATE WITH INTEGRATORS 203

32 [1.0/P.Js],
33 [0.0]])
34
35 C = np.array([[1.0, 0.0, 0.0, 0.0],
36 [0.0, 1.0, 0.0, 0.0]])
37
38 # form augmented system
39 Cout = np.array([[0.0, 1.0, 0.0, 0.0]])
40 A1 = np.array([[0.0, 0.0, 1.0, 0.0, 0.0],
41 [0.0, 0.0, 0.0, 1.0, 0.0],
42 [-P.k/P.Js, P.k/P.Js, -P.b/P.Js, P.b/P.Js, 0.0],
43 [P.k/P.Jp, -P.k/P.Jp, P.b/P.Jp, -P.b/P.Jp, 0.0],
44 [0.0, -1.0, 0.0, 0.0, 0.0]])
45
46 B1 = np.array([[0.0],
47 [0.0],
48 [1.0/P.Js],
49 [0.0],
50 [0.0]])
51
52
53 # gain calculation
54 des_char_poly = np.convolve(
55 np.convolve([1, 2*zeta_phi*wn_phi, wn_phi**2],
56 [1, 2*zeta_th*wn_th, wn_th**2]),
57 np.poly(integrator_pole))
58 des_poles = np.roots(des_char_poly)
59
60 # Compute the gains if the system is controllable
61 if np.linalg.matrix_rank(cnt.ctrb(A1, B1)) != 5:
62 print("The system is not controllable")
63 else:
64 K1 = cnt.place(A1, B1, des_poles)
65 K = np.array([K1.item(0), K1.item(1), K1.item(2), K1.item(3)])
66 ki = K1.item(4)
67
68 print(’K: ’, K)
69 print(’ki: ’, ki)

Listing 12.6: satelliteParamHW12.py

Complete simulation code for Matlab, Python, and Simulink can be down-
loaded at http://controlbook.byu.edu.

Important Concepts:
• Including an integrator will eliminate steady-state error.
• Integrator gains should be selected simultaneously with the controller
gains to avoid changing the closed-loop pole locations.
• The state-space equations can be augmented to include the integra-
tor as an extra state. The techniques for pole placement may then be
used on the augmented state-space system to find all the gains con-
currently.

TOC
CHAPTER 12. FULL STATE WITH INTEGRATORS 204

Notes and References

Integral control for linear state space systems is a well established technique. Ad-
ditional examples can be found in [19, 5, 4]. A nice introduction to integral control
for nonlinear systems is given in [20].

TOC
 Observers
13
Learning Objectives:
• Create an observer to estimate unknown states.
• Determine when the system is observable.
• Choose observer gains that ensure that the state estimation error con-
verges exponentially fast.

13.1 Theory
In the previous two chapters we assumed that the full state x was available to the
controller for full state feedback. In this chapter we remove that assumption and
show how the state can be reconstructed given the measurements. We will assume
that the state space model is given by

ẋ = Ax + Bu (13.1)
y = Cx (13.2)
yr = Cr x,

where yr ∈ Rr is the reference output as discussed in the previous two chapters,

and y ∈ Rp is the measured output, or the signals measured by the sensors. While
the objective of the controller is to force yr (t) to converge to the reference r(t),
the objective of the observer is to estimate the state x(t) given a history of the
input u(t) and the measured output y(t). In this book, the estimate of the state
will be denoted as x̂(t).
A block diagram of the closed-loop system with observer based control is
shown in Fig. 13-1. Comparing Fig. 13-1 to the case of full state feedback in
Fig. 11-4 note that the controller uses the estimated state x̂ instead of the full state

TOC
CHAPTER 13. OBSERVERS 206

x. In other words, the controller in Equation (11.37) becomes

u = −K x̂ + kr r.

Figure 13-1: Closed-loop system with observer based feedback.

In this chapter we will design a so-called Luenberger observer that has the
structure given by
x̂˙ = Ax̂ + Bu + L (y − C x̂) . (13.3)
| {z } | {z }
predictor corrector

The first term in Equation (13.3), i.e., x̂˙ = Ax̂ + Bu is a copy of the equations of
motion for the plant given in Equation (13.1). The second term given by L(y−C x̂)
is a correction term, where L is the observer gain, and (y − C x̂) is the difference
between the measured output y = Cx and the predicted value of the measured
output C x̂. The term y − C x̂ is called the innovation signal.
Defining the observer error as e = x − x̂ and differentiating with respect to
time gives

ė = ẋ − x̂˙
= Ax + Bu − (Ax̂ + Bu + Ly − LC x̂)
= A(x − x̂) − LCx + LC x̂
= (A − LC)(x − x̂)
= (A − LC)e.

Therefore, the poles that govern the time-evolution of the observation error are
given by the eigenvalues of A − LC. The observer design problem is therefore to
select L to place the eigenvalues of A − LC at locations specified by the control
engineer. The derivation of a formula for the observer gain L proceeds in a manner
similar to the derivation of the feedback gain K in Chapter 11.

13.1.1 Observer Design using Observer Canonic Form

Given the general SISO monic transfer function

bm sm + bm−1 sm−1 + · · · + b1 s + b0
Y (s) = U (s), (13.4)
sn + an−1 sn−1 + · · · + a1 s + a0

TOC
CHAPTER 13. OBSERVERS 207

where n > m, and taking the inverse Laplace transform gives

dn y dn−1 y
+ a n−1 + · · · + a1 ẏ + a0 y(t)
dtn dtn−1
dm u dm−1 u
= bm m + bm−1 m−1 + · · · + b1 u̇ + b0 u(t).
dt dt

Solving for the nth derivative of y and collecting terms with similar order deriva-
tives gives

dn y dn−1 y dm+1 y
n
= −an−1 n−1 − am+1 m+1
dt dt
 dt 
dm u dm y
+ bm m − am m + · · · + (b1 u̇ − a1 ẏ) + (b0 u − a0 y) .
dt dt

Integrating n times gives

Z h
dn−1 y dm+1 y
y(t) = − an−1 n−1
− · · · − −am+1 m+1
dt dt
Z h 
dm u dm y
+ bm m − am m + . . .
dt dt
Z h Z i i i
+ (b1 u̇ − a1 ẏ) + (b0 u − a0 y) . . . . . . . (13.5)

The analog computer implementation of Equation (13.5) is shown in Fig. 13-2.

Labeling the states as the output of the integrators, as shown in Fig. 13-2 and

u(t) ...
b0 b1 bm

1 1 1 y(t)
... 1
s xn s xn 1 s x2 s x1

a0 a1 an 1

...
Figure 13-2: Block diagram showing the analog computer implementation of
Equation (13.5).

TOC
CHAPTER 13. OBSERVERS 208

writing the equations in linear state space form gives

 
0
     . 
ẋ1 −an−1 1 ... 0 0 x1  .. 
 ẋ2 −an−2 0 . . . 0 0  x2   
      0
 ..  .. .. 
..   ..  + b  
 = . . .   m u (13.6)
 .   .    . 
ẋn−1   −a1 0 . . . 0 1 xn−1   .. 
 
ẋn −a0 0 ... 0 0 xn  b1 
b0
 
x1
 x2 





= 1 0 ... 0 0  ...  . (13.7)
 
xn−1 
xn
We define the observer canonic realization as
ẋo = Ao xo + Bo u
y = C o xo ,
where
 
−an−1 1 ... 0 0
−an−2 0 ... 0 0
 
4  .. 
Ao =  ... ..
. . (13.8)
 
 −a1 0 ... 0 1
−a0 0 ... 0 0
 
0
 .. 
 . 
 
0
4  
Bo = 
bm 
 (13.9)
 . 
 .. 
 
 b1 
b0
4

Co = 1 0 ... 0 0 . (13.10)
Comparing the state space equations in observer canonic form in Equations (13.8)–
(13.10) with the state space equations for control canonic form in Equations (11.14)-
(11.16), we see that
Ao = A>
c

Bo = Cc>
Co = Bc> .

TOC
CHAPTER 13. OBSERVERS 209

Comparing the state equation in Equation (13.6) with the transfer function in
Equation (13.4) we see that the eigenvalues of Ao are the roots of the open-loop
characteristic polynomial

∆ol (s) = det(sI − Ao ) = sn + an−1 sn−1 + · · · + a1 s + a0 .

Since Co in Equation (13.7) has the structure of one in the first element followed
by zeros, we see that the structure of Ao − Lo Co is given by
 
−(an−1 + `1 ) 1 ... 0 0
−(an−2 + `2 ) 0 ... 0 0
 
 .. .. ..  .
Ao − Lo Co =  . . .
 
−(a1 + `n−1 ) 0 ... 0 1
−(a0 + `n ) 0 ... 0 0

The characteristic polynomial for the closed-loop observation error is therefore

given by

∆cl,obs (s) = det(sI − (Ao − Lo Co ))

= sn + (an−1 + `1 )sn−1 + · · · + (a1 + `n−1 )s + (a0 + `n ).

If the desired characteristic polynomial for the closed-loop observation error is

∆dcl,obs (s) = sn + βn−1 sn−1 + · · · + β1 s + β0 , (13.11)

then we have that

an−1 + `1 = βn−1
an−2 + `2 = βn−2
..
.
a1 + `n−1 = β1
a0 + `n = β0 .

Defining the vectors

4
aA = (an−1 , an−2 , . . . , a1 , a0 )
4
β = (βn−1 , βn−2 , . . . , β1 , β0 )
4
Lo = (`1 , `2 , . . . , `n−1 , `n )> ,

we have that the observer gain satisfies

Lo = (β − aA )> , (13.12)

TOC
CHAPTER 13. OBSERVERS 210

where the subscript “o” is used to emphasize what the gains are when the state
equations are in observer canonic form. As an example, suppose that the transfer
function model of the system is given by
9s + 20
Y (s) = U (s),
s3 + 6s2 − 11s + 8
then the state space equations in observer canonic form are given by
   
−6 1 0 0
ẋo =  11 0 1 xo +  9  u
−8 0 0 20


y = 1 0 0 xo .

The open-loop characteristic polynomial is

∆ol (s) = s3 + 6s2 − 11s + 8,

with roots at −7.5885, and 0.7942 ± j0.6507. Suppose that the desired poles
for the observation error are at −10 and −20 ± j20, then the desired closed-loop
polynomial for the observation error is

∆dcl,obs (s) = (s + 10)(s + 20 − j20)(s + 20 + j20)

= s3 + 50s2 + 1200s + 8000,

then aAo = (6, −11, 8), β = (50, 1200, 8000), and the observer gain that places
the eigenvalues of Ao − Lo Co at the desired locations is given by
     
50 6 44
Lo = (β − aAo )> = 1200 − −11 = 1211 .
8000 8 7992

13.1.2 Observer Design: General State Space Equations

The formula given in Equation (13.12) assumes that the state space equations are
in observer canonic form. Similar to the general full state feedback case discussed
in Section 11.1.3, a formula for the observer gains can be derived for general state
space equations. Following the method used in Section 11.1.3, the strategy is to
find a state transformation matrix that converts the general state space equations
into observer canonic form. Rather than repeating the steps in Section 11.1.3 we
will derive the formula for the observer gain by noting the similarities between
the design problem for full state feedback and the design problem for the observer
gain.
For full state feedback, the feedback gains K that place the eigenvalues of A−
BK at the roots of the desired characteristic equation given in Equation (11.20)
are given by Equation (11.32) or

K = (α − aA )A−1 −1
A CA,B , (13.13)

TOC
CHAPTER 13. OBSERVERS 211

where
 
1 an−1 an−2 ... a2 a1
 .. 
0 1 an−1 . a2 
a3
 
 .. 
 a3 
AA = 0 0 1 . a4 ,
. .. .. 
 .. . . 
 
0 0 0 . . . 1 an−1 
0 0 0 ... 0 1


CA,B = B AB A2 B · · · An−1 B ,

and where the subscripts are used to indicate that AA is a function of the matrix
A, and CA,B is a function of the matrices A and B.
The observer design problem is to find the observer gain L that places the
eigenvalues of A − LC at the roots of Equation (13.11). Recalling from Ap-
pendix P.7 that for a general square matrix M , the eigenvalues of M equal the
eigenvalues of M > , the observer problem can be posed as finding L that places
the eigenvalues of A> − C > L> at the roots of Equation (13.11). This problem
is identical to the full state feedback gain problem with A replaced by A> , B
replaced by C > , and K replaced by L> . Making the appropriate substitutions in
Ackermann’s formula (13.13) gives

L> = (β − aAp )A−1 C −1

A> A> ,C >
(13.14)

Since det(sI −A) = det(sI −A> ), the characteristic equations for A and A> are
identical, which implies that aA = aA> and AA = AA> . Taking the transpose of
Equation (13.14) gives
−> −> >
L = CA> ,C > AA (β − aA ) , (13.15)

where we have used the notation

4
>
−1
M −> = M −1 = M> .

Defining the observability matrix

4 >
OA,C = CA> ,C >

>
= C> A> C > (A> )2 C > ··· (A> )n−1 C >
 
C
 CA 
 
 2 
=  CA  ,
 .. 
 . 
CAn−1

TOC
 CHAPTER 13. OBSERVERS 212

the observer gain (13.15) can be written as

−1
L = OA,C A−> >
A (β − aA ) , (13.16)

where L is well defined if the observability matrix OA,C is invertible. Accord-

ingly, the system (A, C) is said to be observable if rank(OA,C ) = n. Equa-
tion (13.16) is Ackermann’s formula for the observer gain L. Ackermann’s for-
mula is only valid for single output systems. For multi-output systems there are
more advanced methods that are implemented in the Matlab and Python place
commands. The observer gain L that places the eigenvalues of A−LC at locations
q1 , q2 , . . . , qn can be found using the following Python commands
1 import numpy as np
2 import control as cnt
3
4 if np.linalg.matrix_rank(cnt.obsv(A,C)) != n:
5 print(’System Not Observable’)
6 else: # if so, compute observer gains
7 L = cnt.place(A.T,C.T,[q1,q2,...,qn]).T

where the first operation checks to see if the system is observable by checking
whether rank(OA,C ) = n. When finding the gain L it is important to note the
three transposes which correspond to the transposes in Equation (13.14).
If an integrator is used in the control law, as discussed in Chapter 12, then
the complete observer based controller is shown in Fig. 13-3. Note that integral
feedback uses the reference output yr , whereas the observer uses the measured
output y. There are many systems for which the measured output is identical to
the reference output.

Figure 13-3: closed-loop system with observer based feedback, including integral
feedback.

13.1.3 Separation Principle

The observer based feedback design consists of two steps:

• Find the state feedback gains K to place the eigenvalues of A − BK at

specified locations p1 , . . . , pn ,

TOC
CHAPTER 13. OBSERVERS 213

• Find the observer gains L to place the eigenvalues of A − LC at specified

locations q1 , . . . , qn .
Since the closed-loop system includes both the observer and the state feedback, it
is important to analyze whether the closed-loop system is stable and to determine
the closed-loop poles. If the integrator is excluded in Fig. 13-3 then the closed-
loop system has 2n states: n states associated with the open-loop plant x and n
states associated with the observer x̂. Since the reference input doesn’t affect the
internal stability of the systems, we can set r = 0 and the equations of motion for
the closed-loop system can be written as
ẋ = Ax + Bu
x̂˙ = Ax̂ + Bu + L(y − C x̂)
u = −K x̂
y = Cx,
which implies that
    
ẋ A −BK x
= .
x̂˙ LC A − BK − LC x̂
Recalling that a change of variables does not change the eigenvalues of the system,
and defining the observation error as e = x − x̂, and noting that
    
x I 0 x
= ,
e I −I x̂
we have that
    
ẋ I 0 ẋ
=
ė I −I x̂˙
   
I 0 A −BK x
=
I −I LC A − BK − LC x̂
   −1  
I 0 A −BK I 0 x
= ,
I −I LC A − BK − LC I −I e
and noting that
 −1  
I 0 I 0
=
I −I I −I
gives     
ẋ A − BK BK x
= .
ė 0 A − LC e
Using the determinant properties for block diagonal matrices discussed in Ap-
pendix P.7, it is straightforward to show that
 
A − BK BK
eig = eig(A − BK) ∪ eig(A − LC).
0 A − LC

TOC
CHAPTER 13. OBSERVERS 214

Therefore the poles of the closed-loop system are precisely the poles selected
during the design of the feedback gains K in addition to the poles selected during
the design of the observer gains L. This happy circumstance is called the separa-
tion principle, which in essence implies that the controller and the observer can be
designed separately and then combined without affecting the stability of the sys-
tem. Unfortunately, the separation principle does not hold in general for nonlinear
systems, and even more unfortunately, while the separation principle guarantees
stability, it turns out that the addition of an observer can negatively impact the
robustness of a state feedback design.

13.1.4 Observer Design for Linearized Systems

Suppose that the state space model is from a linearized system, i.e, suppose that
x ∈ Rn is the state, xe ∈ Rn is the equilibrium state, and that x̃ = x − xe
is the state linearized around the equilibrium. Similarly, suppose that u ∈ Rm
is the input, that ue ∈ Rm is the input at equilibrium, and that ũ = u − ue is
the input linearized around equilibrium, and that y ∈ Rp is the measured output,
yme = Cxe ∈ Rp is the measured output at equilibrium, and ỹ = y − ye is
the measured output linearized about equilibrium, then the linearized state space
model is given by

x̃˙ = Ax̃ + B ũ
ỹ = C x̃.

The observer design problem then finds an observer gain so that the eigenvalues
of A − LC are at specified locations, and the resulting observer is

ˆ˙ = Ax̃
x̃ ˆ + B ũ + L(ỹ − C x̃),
ˆ

ˆ = x̂ − xe . Writing the observer in terms of x̂ gives

where x̃

x̂˙ − ẋe = A(x̂ − xe ) + B(u − ue ) + L ((y − ye ) − C(x̂ − xe )) .

Noting that ẋe = 0 and that ye = Cxe gives

x̂˙ = A(x̂ − xe ) + B(u − ue ) + L (y − C x̂) .

13.2 Summary of Design Process - Observer Design

The procedure for designing an observer is summarized as follows.
Given the plant

ẋ = Ax + Bu
y = Cx

TOC
 CHAPTER 13. OBSERVERS 215

find the observer gain L so that the state observer

x̂˙ = Ax̂ + Bu + L(y − C x̂)
has stable estimation error dynamics with poles located at q1 , q2 , . . . , qn .
Step 1. Check to see if the system is observable by computing the observability
matrix  
C
 CA 
 
 2 
OA,C =  CA 
 .. 
 . 
CAn−1
and checking to see if rank(OA,C ) = n.
Step 2. Find the open-loop characteristic polynomial
∆ol (s) = det(sI − A) = sn + an−1 sn−1 + · · · + a1 s + a0 ,
and construct the row vector


aA = an−1 , an−2 , ..., a1 , a0
and the matrix
 
1 an−1 an−2 ··· a2 a1
0 1 an−1 ··· a3 a2 
 
 .. .. 
AA = . . . . . 
 
0 0 ··· 0 1 an−1 
0 0 ··· 0 0 1

Step 3. Find the desired characteristic polynomial for the observation error
∆dobs (s) = (s − q1 )(s − q2 ) · · · (s − qn ) = sn + βn−1 sn−1 + · · · + β1 s + β0 ,
and construct the row vector


β = βn−1 , βn−2 , ..., β1 , β0 .

Step 4. Compute the desired observer gains as

−1
L = OA,C (A>
A)
−1
(β − aA )>

Using Python, these steps can be implemented using the following script.
1 import numpy as np
2 import control as cnt
3
4 if np.linalg.matrix_rank(cnt.obsv(A,C)) != n:
5 print(’System Not Observable’)
6 else: # if so, compute observer gains
7 L = cnt.place(A.T,C.T,[q1,q2,...,qn]).T

TOC
CHAPTER 13. OBSERVERS 216

13.2.1 Simple example

Given the system
   
0 1 0
ẋ = x+ u
2 3 4


y= 5 0 x

find the observer gain L so that the state observer

x̂˙ = Ax̂ + Bu + L(y − C x̂)

has stable estimation error dynamics with poles located at −10 ± j10.
Step 1. The observability matrix is given by
   
C 5 0
O= = .
CA 0 5

The determinant is det(O) = 25 6= 0, therefore the system is observable.

Step 2. The open-loop characteristic polynomial
 
s −1
∆ol (s) = det(sI − A) = det = s2 − 3s − 2,
−2 s − 3

implies that


a = −3, −2
 
1 −3
A=
0 1

Step 3. The desired characteristic polynomial for the observation error is

∆dobs (s) = (s + 10 − j10)(s + 10 + j10) = s2 + 20s + 200,

which implies that

β = 20, 200 .

Step 4. The observation gains are therefore given as

L = O−1 (A> )−1 (β − a)>

1  
0 1 0 >
= 5 1 ((20, 200) − (−3, −2))
0 5 3 1
    
1/5 0 23 23/5
= =
3/5 1/5 202 271/5

Alternatively, we could have used the following Python script

TOC
 CHAPTER 13. OBSERVERS 217

1 import numpy as np
2 import control as cnt
3
4 # original state space system
5 A = np.array([[0.0, 1.0], [2.0, 3.0]])
6 B = np.array([[0.0], [4.0]])
7 C = np.array([[5.0, 0.0]])
8
9 # desired poles for observer
10 q1 = -10 + 10.j
11 q2 = -10 - 10.j
12
13 # is the system observable?
14 if np.linalg.matrix_rank(cnt.obsv(A,C)) != np.size(A,1):
15 print(’System Not Observable’)
16 else: # if so, compute observer gains
17 L = cnt.place(A.T,C.T,[q1,q2]).T

The corresponding observer is given by

     
0 1 0 23/5


x̂˙ = x̂ + u+ y− 5 0 x̂ , (13.17)
2 3 4 271/5

and can be implemented as member functions within a Python class as follows (see
the solutions for case studies A-C for class-specific implementation examples):

1 def update_observer(self, y_m):

2 # update the observer using RK4 integration
3 F1 = self.observer_f(self.x_hat, y_m)
4 F2 = self.observer_f(self.x_hat + self.Ts / 2 * F1, y_m)
5 F3 = self.observer_f(self.x_hat + self.Ts / 2 * F2, y_m)
6 F4 = self.observer_f(self.x_hat + self.Ts * F3, y_m)
7
8 self.x_hat += self.Ts / 6 * (F1 + 2 * F2 + 2 * F3 + F4)
9
10 return self.x_hat
11
12 def observer_f(self, x_hat, y_m):
13 # xhatdot = A*xhat + B*u + L(y-C*xhat)
14 # assumes that all matrices below are np.arrays
15 xhat_dot = self.A @ x_hat \
16 + self.B @ self.u \
17 + self.L @ (y_m-self.C @ x_hat)
18
19 return xhat_dot

Digital implementation of the observer in Equation (13.17) requires two per-

sistent (or member) variables, one for x̂ and one for u. The observer is updated
using the RK4 algorithm in lines 3–8. The differential equation that describes the
observer is given on lines 15–17.

TOC
 CHAPTER 13. OBSERVERS 218

13.3 Design Study A. Single Link Robot Arm

Example Problem A.13

The objective of this problem is to design an observer that estimates the state of the
system and uses the estimated state in the controller designed in Homework A.12.

(a) For the sake of understanding the function of the observer, for this problem
we will use exact parameters, without an input disturbance. Modify the arm
dynamics so that the parameters known to the controller are the actual plant
parameters (uncertainty parameter α = 0).

(b) Verify that the state space system is observable by checking that rank(OA,C ) =
n.

(c) Modify the controller to add an observer to estimate the state x̂, and then use
the estimate of the state x̂ in your feedback controller. Tune the poles of the
controller and observer to obtain good performance.

(d) Modify the simulation files so that the controller outputs both u and x̂. Add a
plotting routine to plot both the state and the estimated state of the system
on the same graph.

(e) As motivation for the next chapter, add an input disturbance to the system of
0.01 and observe that there is steady state error in the response even though
there is an integrator. This is caused by a steady state error in the observation
error. In the next chapter we will show how to remove the steady state error
in the observation error.

Solution
The single link robot arm is simulated using the following Python code.
1 import sys
2 sys.path.append(’..’) # add parent directory
3 import matplotlib.pyplot as plt
4 import numpy as np
5 import armParam as P
6 from hw3.armDynamics import armDynamics
7 from armController import armController
8 from hw2.signalGenerator import signalGenerator
9 from hw2.armAnimation import armAnimation
10 from hw2.dataPlotter import dataPlotter
11 from dataPlotterObserver import dataPlotterObserver
12
13 # instantiate arm, controller, and reference classes
14 arm = armDynamics(alpha=0.0)

TOC
 CHAPTER 13. OBSERVERS 219

15 controller = armController()
16 reference = signalGenerator(amplitude=30*np.pi/180.0,
17 frequency=0.05)
18 disturbance = signalGenerator(amplitude=0.25)
19 noise = signalGenerator(amplitude=0.1)
20
21 # instantiate the simulation plots and animation
22 dataPlot = dataPlotter()
23 dataPlotObserver = dataPlotterObserver()
24 animation = armAnimation()
25
26 t = P.t_start # time starts at t_start
27 y = arm.h() # output of system at start of simulation
28
29 while t < P.t_end: # main simulation loop
30 # Get referenced inputs from signal generators
31 # Propagate dynamics in between plot samples
32 t_next_plot = t + P.t_plot
33
34 # updates control and dynamics at faster simulation rate
35 while t < t_next_plot:
36 r = reference.square(t)
37 d = disturbance.step(t) # input disturbance
38 n = noise.random(t) # simulate sensor noise
39 u, xhat = controller.update(r, y + n) # update controller
40 y = arm.update(u + d) # propagate system
41 t = t + P.Ts # advance time by Ts
42
43 # update animation and data plots
44 animation.update(arm.state)
45 dataPlot.update(t, r, arm.state, u)
46 dataPlotObserver.update(t, arm.state, xhat, d, 0.0)
47
48 # the pause causes the figure to display during simulation
49 plt.pause(0.0001)
50
51 # Keeps the program from closing until user presses a button.
52 print(’Press key to close’)
53 plt.waitforbuttonpress()
54 plt.close()

Listing 13.1: armSim.py

Note that the input to the controller is the output y corrupted by noise n and that
the input disturbance d has been added to u. The dataPlotObserver plots the state
x and the estimated state x̂, as well as the disturbance d and estimated disturbance
ˆ as will be discussed in the next chapter.
d,
A Python class that implements observer-based control with an integrator for
the single link robot arm is shown below.
1 import numpy as np
2 import armParamHW13 as P
3
4 class armController:
5 def __init__(self):
6 self.x_hat = np.array([

TOC
 CHAPTER 13. OBSERVERS 220

7 [0.0], # theta_hat_0
8 [0.0], # thetadot_hat_0
9 ])
10 self.tau_d1 = 0.0 # control torque, delayed 1 sample
11 self.integrator = 0.0 # integrator
12 self.error_d1 = 0.0 # error signal, delayed 1 sample
13 self.K = P.K # state feedback gain
14 self.ki = P.ki # Input gain
15 self.L = P.L # observer gain
16 self.A = P.A # system model
17 self.B = P.B
18 self.C = P.C
19 self.limit = P.tau_max # Maximum torque
20 self.Ts = P.Ts # sample rate of controller
21
22 def update(self, theta_r, y):
23 # update the observer and extract theta_hat
24 x_hat = self.update_observer(y)
25 theta_hat = x_hat.item(0)
26
27 # integrate error
28 error = theta_r - theta_hat
29 self.integrateError(error)
30
31 # feedback linearizing torque tau_fl
32 tau_fl = P.m * P.g * (P.ell / 2.0) * np.cos(theta_hat)
33
34 # Compute the state feedback controller
35 tau_tilde = -self.K @ x_hat - self.ki * self.integrator
36
37 # compute total torque
38 tau = self.saturate(tau_fl + tau_tilde.item(0))
39 self.tau_d1 = tau
40 return tau, x_hat
41
42 def integrateError(self, error):
43 self.integrator = self.integrator \
44 + (self.Ts/2.0)*(error + self.error_d1)
45 self.error_d1 = error
46
47 def update_observer(self, y_m):
48 # update the observer using RK4 integration
49 F1 = self.observer_f(self.x_hat, y_m)
50 F2 = self.observer_f(self.x_hat + self.Ts / 2 * F1, y_m)
51 F3 = self.observer_f(self.x_hat + self.Ts / 2 * F2, y_m)
52 F4 = self.observer_f(self.x_hat + self.Ts * F3, y_m)
53 self.x_hat += self.Ts / 6 * (F1 + 2 * F2 + 2 * F3 + F4)
54
55 return self.x_hat
56
57 def observer_f(self, x_hat, y_m):
58 # compute feedback linearizing torque tau_fl
59 theta_hat = x_hat.item(0)
60 tau_fl = P.m * P.g * (P.ell / 2.0) * np.cos(theta_hat)
61
62 # xhatdot = A*(xhat-xe) + B*(u-ue) + L(y-C*xhat)
63 xhat_dot = self.A @ x_hat\

TOC
 CHAPTER 13. OBSERVERS 221

64 + self.B * (self.tau_d1 - tau_fl)\

65 + self.L * (y_m - self.C @ x_hat)
66 return xhat_dot
67
68 def saturate(self,u):
69 if abs(u) > self.limit:
70 u = self.limit*np.sign(u)
71 return u

Listing 13.2: armController.py

Notice that we must update the observer dynamics and that x̂ is used in the con-
troller instead of x, but otherwise this controller is identical to the controller in
Chapter 12. The observer is updated using the RK4 algorithm (see line 48). The
differential equations for the observer are also specified on lines 64–66.
Python code that computes the control gains is given below.
1 # Single link arm Parameter File
2 import numpy as np
3 import control as cnt
4 import sys
5 sys.path.append(’..’) # add parent directory
6 import armParam as P
7
8 # tuning parameters
9 tr = 0.4
10 zeta = 0.707
11 integrator_pole = 9
12 wn_obs = 10 # natural frequency for observer
13 zeta_obs = 0.707 # damping ratio for observer
14
15 Ts = P.Ts # sample rate of the controller
16 tau_max = P.tau_max # limit on control signal
17 m = P.m
18 ell = P.ell
19 g = P.g
20
21
22 # State Space Equations
23 A = np.array([[0.0, 1.0],
24 [0.0, -1.0*P.b/P.m/(P.ell**2)]])
25 n = A.shape[0]
26
27 B = np.array([[0.0],
28 [3.0/P.m/(P.ell**2)]])
29
30 C = np.array([[1.0, 0.0]])
31
32 # control design
33 # form augmented system
34 A1 = np.array([[0.0, 1.0, 0.0],
35 [0.0, -1.0*P.b/P.m/(P.ell**2), 0.0],
36 [-1.0, 0.0, 0.0]])
37
38 B1 = np.array([[0.0],
39 [3.0/P.m/(P.ell**2)],

TOC
 CHAPTER 13. OBSERVERS 222

40 [0.0]])
41
42 # gain calculation
43 wn = 2.2/tr # natural frequency
44 des_char_poly = np.convolve(
45 [1, 2*zeta*wn, wn**2],
46 [1, integrator_pole])
47 des_poles = np.roots(des_char_poly)
48
49 # Compute the gains if the system is controllable
50 if np.linalg.matrix_rank(cnt.ctrb(A1, B1)) != 3:
51 print("The system is not controllable")
52 else:
53 K1 = cnt.acker(A1, B1, des_poles)
54 K = np.array([K1.item(0), K1.item(1)])
55 ki = K1.item(2)
56
57 # observer design
58 #des_obsv_char_poly = [1, 2*zeta_obs*wn_obs, wn_obs**2]
59 des_obsv_poles = des_poles[0:n]*2. #np.roots(des_obsv_char_poly)
60
61 # Compute the gains if the system is controllable
62 if np.linalg.matrix_rank(cnt.ctrb(A.T, C.T)) != 2:
63 print("The system is not observerable")
64 else:
65 L = cnt.acker(A.T, C.T, des_obsv_poles).T
66
67 print(’K: ’, K)
68 print(’ki: ’, ki)
69 print(’L^T: ’, L.T)

Listing 13.3: armParamHW13.py

Complete simulation code for Matlab, Python, and Simulink can be down-
loaded at http://controlbook.byu.edu.

13.4 Design Study B. Inverted Pendulum

Example Problem B.13

The objective of this problem is to design an observer that estimates the state
of the system and to use the estimated state in the controller designed in Home-
work B.12.

(a) For the sake of understanding the function of the observer, for this problem
we will use exact parameters, without an input disturbance. Modify the
pendulum dynamics so that the parameters known to the controller are the
actual plant parameters (uncertainty parameter α = 0).

TOC
 CHAPTER 13. OBSERVERS 223

(b) Verify that the state space system is observable by checking that rank(OA,C ) =
n.

(c) In the control block, add an observer to estimate the state x̂, and use the esti-
mate of the state in your feedback controller. Tune the poles of the controller
and observer to obtain good performance.

(d) Modify the simulation files so that the controller outputs both u and x̂. Add a
plotting routine to plot both the state and the estimated state of the system
on the same graph.

(e) As motivation for the next chapter, add an input disturbance to the system of
0.05 and observe that there is steady state error in the response even though
there is an integrator. This is caused by a steady state error in the observation
error. In the next chapter we will show how to remove the steady state error
in the observation error.

Solution
The inverted pendulum is simulated using the following Python code.
1 import sys
2 sys.path.append(’..’) # add parent directory
3 import matplotlib.pyplot as plt
4 import numpy as np
5 import pendulumParam as P
6 from hw3.pendulumDynamics import pendulumDynamics
7 from pendulumController import pendulumController
8 from hw2.signalGenerator import signalGenerator
9 from hw2.pendulumAnimation import pendulumAnimation
10 from hw2.dataPlotter import dataPlotter
11 from dataPlotterObserver import dataPlotterObserver
12
13 # instantiate pendulum, controller, and reference classes
14 pendulum = pendulumDynamics(alpha=0.0)
15 controller = pendulumController()
16 reference = signalGenerator(amplitude=0.5, frequency=0.05)
17 disturbance = signalGenerator(amplitude=0.5)
18 noise_z = signalGenerator(amplitude=0.01)
19 noise_th = signalGenerator(amplitude=0.01)
20
21 # instantiate the simulation plots and animation
22 dataPlot = dataPlotter()
23 dataPlotObserver = dataPlotterObserver()
24 animation = pendulumAnimation()
25
26 t = P.t_start # time starts at t_start
27 y = pendulum.h() # output of system at start of simulation
28
29 while t < P.t_end: # main simulation loop
30 # Get referenced inputs from signal generators
31 # Propagate dynamics in between plot samples
32 t_next_plot = t + P.t_plot

TOC
 CHAPTER 13. OBSERVERS 224

33
34 while t < t_next_plot:
35 r = reference.square(t)
36 d = disturbance.step(t)
37 n = np.array([[0.0], [0.0]])
38 #n = np.array([[noise_z.random(t)], [noise_th.random(t)]])
39 u, xhat = controller.update(r, y + n)
40 y = pendulum.update(u + d) # propagate system
41 t = t + P.Ts # advance time by Ts
42
43 # update animation and data plots
44 animation.update(pendulum.state)
45 dataPlot.update(t, r, pendulum.state, u)
46 dataPlotObserver.update(t, pendulum.state, xhat, d, 0.0)
47 plt.pause(0.0001)
48
49 # Keeps the program from closing until the user presses a button.
50 print(’Press key to close’)
51 plt.waitforbuttonpress()
52 plt.close()

Listing 13.4: pendulumSim.py

Note in line 38, that the input to the controller is the output y corrupted by noise
n and that the input disturbance d has been added to u in line 39. The dataPlotO-
bserver plots the state x and the estimated state x̂, as well as the disturbance d and
estimated disturbance d,ˆ as will be discussed in the next chapter.
A Python class that implements observer-based control with an integrator for
the inverted pendulum is shown below.
1 import numpy as np
2 import pendulumParamHW13 as P
3
4 class pendulumController:
5 def __init__(self):
6 self.x_hat = np.array([
7 [0.0], # initial estimate for z_hat
8 [0.0], # initial estimate for theta_hat
9 [0.0], # initial estimate for z_hat_dot
10 [0.0]]) # initial estimate for theta_hat_dot
11 self.F_d1 = 0.0 # Computed Force, delayed by one sample
12 self.integrator = 0.0 # integrator
13 self.error_d1 = 0.0 # error signal delayed by 1 sample
14 self.K = P.K # state feedback gain
15 self.ki = P.ki # integrator gain
16 self.L = P.L # observer gain
17 self.A = P.A # system model
18 self.B = P.B
19 self.C = P.C
20 self.limit = P.F_max # Maximum force
21 self.Ts = P.Ts # sample rate of controller
22
23 def update(self, z_r, y):
24 # update the observer and extract z_hat
25 x_hat = self.update_observer(y)
26 z_hat = x_hat.item(0)

TOC
 CHAPTER 13. OBSERVERS 225

27
28 # integrate error
29 error = z_r - z_hat
30 self.integrate_error(error)
31
32 # Compute the state feedback controller
33 F_unsat = -self.K @ x_hat - self.ki * self.integrator
34 F_sat = self.saturate(F_unsat.item(0))
35 self.F_d1 = F_sat
36
37 return F_sat, x_hat
38
39 def update_observer(self, y_m):
40 # update the observer using RK4 integration
41 F1 = self.observer_f(self.x_hat, y_m)
42 F2 = self.observer_f(self.x_hat + self.Ts / 2 * F1, y_m)
43 F3 = self.observer_f(self.x_hat + self.Ts / 2 * F2, y_m)
44 F4 = self.observer_f(self.x_hat + self.Ts * F3, y_m)
45 self.x_hat += self.Ts / 6 * (F1 + 2 * F2 + 2 * F3 + F4)
46 return self.x_hat
47
48 def observer_f(self, x_hat, y_m):
49 # xhatdot = A*xhat + B*u + L(y-C*xhat)
50 xhat_dot = self.A @ x_hat \
51 + self.B * self.F_d1 \
52 + self.L @ (y_m-self.C @ x_hat)
53
54 return xhat_dot
55
56 def integrate_error(self, error):
57 self.integrator = self.integrator \
58 + (self.Ts/2.0)*(error + self.error_d1)
59 self.error_d1 = error
60
61 def saturate(self, u):
62 if abs(u) > self.limit:
63 u = self.limit*np.sign(u)
64 return u

Listing 13.5: pendulumController.py

The observer is updated on line 25, and x̂ is used in the controller instead of x but
otherwise is identical to the controller in Chapter 12. The observer is updated on
lines 40–48 using the RK4 algorithm. The differential equations for the observer
are specified on lines 50–56.
Python code that computes the observer and control gains is given below.
1 # Inverted Pendulum Parameter File
2 import sys
3 sys.path.append(’..’) # add parent directory
4 import pendulumParam as P
5 import numpy as np
6 from scipy import signal
7 import control as cnt
8
9 # sample rate of the controller

TOC
 CHAPTER 13. OBSERVERS 226

10 Ts = P.Ts
11
12 # saturation limits
13 F_max = 5.0 # Max Force, N
14
15 ####################################################
16 # State Space
17 ####################################################
18 # tuning parameters
19 tr_z = 1.5 # rise time for position
20 tr_theta = 0.5 # rise time for angle
21 zeta_z = 0.707 # damping ratio position
22 zeta_th = 0.707 # damping ratio angle
23 integrator_pole = -2.0 # integrator pole
24 tr_z_obs = tr_z/5.0 # rise time for observer - position
25 tr_theta_obs = tr_theta/5.0 # rise time for observer - angle
26
27 # State Space Equations
28 # xdot = A*x + B*u
29 # y = C*x
30
31 A = np.matrix([[0.0, 0.0, 1.0, 0.0],
32 [0.0, 0.0, 0.0, 1.0],
33 [0.0, -3*P.m1*P.g/4/(.25*P.m1+P.m2),
34 -P.b/(.25*P.m1+P.m2), 0.0],
35 [0.0, 3*(P.m1+P.m2)*P.g/2/(.25*P.m1+P.m2)/P.ell,
36 3*P.b/2/(.25*P.m1+P.m2)/P.ell, 0.0]])
37
38 B = np.matrix([[0.0],
39 [0.0],
40 [1/(.25*P.m1+P.m2)],
41 [-3.0/2/(.25*P.m1+P.m2)/P.ell]])
42
43 C = np.matrix([[1.0, 0.0, 0.0, 0.0],
44 [0.0, 1.0, 0.0, 0.0]])
45
46 # form augmented system
47 Cr = C[0, :]
48
49 # Augmented Matrices
50 A1 = np.concatenate((
51 np.concatenate((A, np.zeros((4, 1))), axis=1),
52 np.concatenate((-Cr, np.matrix([[0.0]])), axis=1)),
53 axis=0)
54 B1 = np.concatenate((B, np.matrix([[0.0]])), axis=0)
55
56 # computer control gains
57 wn_th = 2.2/tr_theta # natural frequency for angle
58 wn_z = 2.2/tr_z # natural frequency for position
59 des_char_poly = np.convolve(
60 np.convolve([1, 2*zeta_z*wn_z, wn_z**2],
61 [1, 2*zeta_th*wn_th, wn_th**2]),
62 np.poly([integrator_pole]))
63 des_poles = np.roots(des_char_poly)
64
65 # Compute the gains if the system is controllable
66 if np.linalg.matrix_rank(cnt.ctrb(A1, B1)) != 5:

TOC
 CHAPTER 13. OBSERVERS 227

67 print("The system is not controllable")

68 else:
69 K1 = cnt.acker(A1, B1, des_poles)
70 K = K1[0, 0:4]
71 ki = K1[0, 4]
72
73 # computer observer gains
74 wn_z_obs = 2.2/tr_z_obs
75 wn_th_obs = 2.2/tr_theta_obs
76 des_obs_char_poly = np.convolve(
77 [1, 2*zeta_z*wn_z_obs, wn_z_obs**2],
78 [1, 2*zeta_th*wn_th_obs, wn_th_obs**2])
79 des_obs_poles = np.roots(des_obs_char_poly)
80
81 # Compute the gains if the system is observable
82 if np.linalg.matrix_rank(cnt.ctrb(A.T, C.T)) != 4:
83 print("The system is not observable")
84 else:
85 L = signal.place_poles(A.T, C.T, des_obs_poles).gain_matrix.T
86
87 print(’K: ’, K)
88 print(’ki: ’, ki)
89 print(’L^T: ’, L.T)

Listing 13.6: pendulumParamHW13.py

Complete simulation code for Matlab, Python, and Simulink can be down-
loaded at http://controlbook.byu.edu.

13.5 Design Study C. Satellite Attitude Control

Example Problem C.13

The objective of this problem is to design an observer that estimates the state
of the system and to use the estimated state in the controller designed in Home-
work C.12.

(a) For the sake of understanding the function of the observer, for this problem we
will use exact parameters, without an input disturbance. Modify the satellite
dynamics so that the parameters known to the controller are the actual plant
parameters (uncertainty parameter α = 0).

(b) Verify that the state space system is observable by checking that rank(OA,C ) =
n.

(c) In the control block, add an observer to estimate the state x̂, and use the esti-
mate of the state in your feedback controller. Tune the poles of the controller
and observer to obtain good performance.

TOC
 CHAPTER 13. OBSERVERS 228

(d) Modify the simulation files so that the controller outputs both u and x̂. Add a
plotting routine to plot both the state and the estimated state of the system
on the same graph.

(e) As motivation for the next chapter, add an input disturbance to the system of
1.0 and observe that there is steady state error in the response even though
there is an integrator. This is caused by a steady state error in the observation
error. In the next chapter we will show how to remove the steady state error
in the observation error.

Solution
The satellite is simulated using the following Python code.
1 import sys
2 sys.path.append(’..’) # add parent directory
3 import matplotlib.pyplot as plt
4 import numpy as np
5 import satelliteParam as P
6 from hw3.satelliteDynamics import satelliteDynamics
7 from satelliteController import satelliteController
8 from hw2.signalGenerator import signalGenerator
9 from hw2.satelliteAnimation import satelliteAnimation
10 from hw2.dataPlotter import dataPlotter
11 from dataPlotterObserver import dataPlotterObserver
12
13 # instantiate satellite, controller, and reference classes
14 satellite = satelliteDynamics(alpha=0.0)
15 controller = satelliteController()
16 reference = signalGenerator(amplitude=15.0*np.pi/180.0,
17 frequency=0.03)
18 disturbance = signalGenerator(amplitude=1.0)
19 noise_phi = signalGenerator(amplitude=0.01)
20 noise_th = signalGenerator(amplitude=0.01)
21
22 # instantiate the simulation plots and animation
23 dataPlot = dataPlotter()
24 dataPlotObserver = dataPlotterObserver()
25 animation = satelliteAnimation()
26
27 t = P.t_start # time starts at t_start
28 y = satellite.h() # output of system at start of simulation
29
30 while t < P.t_end: # main simulation loop
31 # Propagate dynamics in between plot samples
32 t_next_plot = t + P.t_plot
33
34 # updates control and dynamics at faster simulation rate
35 while t < t_next_plot:
36 r = reference.square(t) # reference input
37 d = disturbance.step(t) # input disturbance
38
39 # simulate sensor noise -
40 # #np.array([[noise_phi.random(t)],[noise_th.random(t)]])

TOC
 CHAPTER 13. OBSERVERS 229

41 n = np.array([[noise_phi.random(t)],[noise_th.random(t)]])
42
43 u, xhat = controller.update(r, y + n) # update controller
44 y = satellite.update(u + d) # propagate system
45 t = t + P.Ts # advance time by Ts
46
47 # update animation and data plots
48 animation.update(satellite.state)
49 dataPlot.update(t, r, satellite.state, u)
50 dataPlotObserver.update(t, satellite.state, xhat, d, 0.0)
51
52 # the pause causes the figure to display during simulation
53 plt.pause(0.0001)
54
55 # Keeps the program from closing until the user presses a button.
56 print(’Press key to close’)
57 plt.waitforbuttonpress()
58 plt.close()

Listing 13.7: satelliteSim.py

Note in line 43, that the input to the controller is the output y corrupted by noise
n and that the input disturbance d has been added to u in line 44. The dataPlotO-
bserver plots the state x and the estimated state x̂, as well as the disturbance d and
estimated disturbance dˆ which will be discussed in the next chapter.
A Python class that implements observer-based control with an integrator for
the satellite is shown below.
1 import numpy as np
2 import satelliteParamHW13 as P
3
4 class satelliteController:
5 def __init__(self):
6 self.x_hat = np.array([
7 [0.0], # initial estimate for theta_hat
8 [0.0], # initial estimate for phi_hat
9 [0.0], # initial estimate for theta_hat_dot
10 [0.0], # initial estimate for phi_hat_dot
11 ])
12 self.tau_d1 = 0.0 # Computed torque delayed 1 sample
13 self.integrator = 0.0 # integrator
14 self.error_d1 = 0.0 # error signal delayed 1 sample
15 self.K = P.K # state feedback gain
16 self.ki = P.ki # integrator gain
17 self.L = P.L # observer gain
18 self.A = P.A # system model
19 self.B = P.B
20 self.C = P.C
21 self.limit = P.tau_max # Maxiumum torque
22 self.Ts = P.Ts # sample rate of controller
23
24 def update(self, phi_r, y):
25 # update the observer and extract z_hat
26 x_hat = self.update_observer(y)
27 phi_hat = x_hat.item(1)
28

TOC
 CHAPTER 13. OBSERVERS 230

29 # integrate error
30 error = phi_r - phi_hat
31 self.integrateError(error)
32
33 # Compute the state feedback controller
34 tau_unsat = -self.K @ x_hat \
35 - self.ki*self.integrator
36 tau = self.saturate(tau_unsat.item(0))
37 self.tau_d1 = tau
38
39 return tau, x_hat
40
41 def update_observer(self, y_m):
42 # update the observer using RK4 integration
43 F1 = self.observer_f(self.x_hat, y_m)
44 F2 = self.observer_f(self.x_hat + self.Ts / 2 * F1, y_m)
45 F3 = self.observer_f(self.x_hat + self.Ts / 2 * F2, y_m)
46 F4 = self.observer_f(self.x_hat + self.Ts * F3, y_m)
47 self.x_hat += self.Ts / 6 * (F1 + 2 * F2 + 2 * F3 + F4)
48
49 return self.x_hat
50
51 def observer_f(self, x_hat, y_m):
52 # xhatdot = A*xhat + B*u + L(y-C*xhat)
53 xhat_dot = self.A @ x_hat \
54 + self.B * self.tau_d1 \
55 + self.L @ (y_m - self.C @ x_hat)
56
57 return xhat_dot
58
59 def integrateError(self, error):
60 self.integrator = self.integrator + (self.Ts / 2.0) * \
61 (error + self.error_d1)
62 self.error_d1 = error
63
64 def saturate(self,u):
65 if abs(u) > self.limit:
66 u = self.limit*np.sign(u)
67 return u

Listing 13.8: satelliteController.py

The observer is updated on line 26, and x̂ is used in the controller instead of x but
otherwise is identical to the controller in Chapter 12. The observer is updated on
lines 41–49 using the RK4 algorithm. The differential equations for the observer
are specified on lines 51–57.
Python code that computes the observer and control gains is given below.
1 # satellite Parameter File
2 import numpy as np
3 import control as cnt
4 from scipy import signal
5 import sys
6 sys.path.append(’..’) # add parent directory
7 import satelliteParam as P
8

TOC
 CHAPTER 13. OBSERVERS 231

9 Ts = P.Ts
10 #sigma = P.sigma
11 #beta = P.beta
12 tau_max = P.tau_max
13
14 # tuning parameters
15 wn_th = 0.6
16 wn_phi = 1.1 # rise time for angle
17 zeta_phi = 0.707 # damping ratio position
18 zeta_th = 0.707 # damping ratio angle
19 integrator_pole = [-1.0]
20 # pick observer poles
21 wn_th_obs = 10.0*wn_th
22 wn_phi_obs = 10.0*wn_phi
23
24
25 # State Space Equations
26 # xdot = A*x + B*u
27 # y = C*x
28 A = np.array([[0.0, 0.0, 1.0, 0.0],
29 [0.0, 0.0, 0.0, 1.0],
30 [-P.k/P.Js, P.k/P.Js, -P.b/P.Js, P.b/P.Js],
31 [P.k/P.Jp, -P.k/P.Jp, P.b/P.Jp, -P.b/P.Jp]])
32
33 B = np.array([[0.0],
34 [0.0],
35 [1.0/P.Js],
36 [0.0]])
37
38 C = np.array([[1.0, 0.0, 0.0, 0.0],
39 [0.0, 1.0, 0.0, 0.0]])
40
41 # form augmented system
42 Cout = np.array([[0.0, 1.0, 0.0, 0.0]])
43 A1 = np.array([[0.0, 0.0, 1.0, 0.0, 0.0],
44 [0.0, 0.0, 0.0, 1.0, 0.0],
45 [-P.k/P.Js, P.k/P.Js, -P.b/P.Js, P.b/P.Js, 0.0],
46 [P.k/P.Jp, -P.k/P.Jp, P.b/P.Jp, -P.b/P.Jp, 0.0],
47 [0.0, -1.0, 0.0, 0.0, 0.0]])
48
49 B1 = np.array([[0.0],
50 [0.0],
51 [1.0/P.Js],
52 [0.0],
53 [0.0]])
54
55 # gain calculation
56 des_char_poly = np.convolve(
57 np.convolve([1, 2*zeta_phi*wn_phi, wn_phi**2],
58 [1, 2*zeta_th*wn_th, wn_th**2]),
59 np.poly(integrator_pole))
60 des_poles = np.roots(des_char_poly)
61
62 # Compute the gains if the system is controllable
63 if np.linalg.matrix_rank(cnt.ctrb(A1, B1)) != 5:
64 print("The system is not controllable")
65 else:

TOC
 CHAPTER 13. OBSERVERS 232

66 K1 = cnt.acker(A1, B1, des_poles)

67 K = np.matrix([K1.item(0), K1.item(1), K1.item(2), K1.item(3)])
68 ki = K1.item(4)
69
70 # computer observer gains
71 des_obs_char_poly = np.convolve([1, 2*zeta_phi*wn_phi_obs,
72 wn_phi_obs**2],
73 [1, 2*zeta_th*wn_th_obs,
74 wn_th_obs**2])
75 des_obs_poles = np.roots(des_obs_char_poly)
76
77 # Compute the gains if the system is observable
78 if np.linalg.matrix_rank(cnt.ctrb(A.T, C.T)) != 4:
79 print("The system is not observable")
80 else:
81 # place_poles returns an object with various properties.
82 # The gains are accessed through .gain_matrix
83 # .T transposes the matrix
84 L = cnt.place(A.T, C.T, des_obs_poles).T
85
86 print(’K: ’, K)
87 print(’ki: ’, ki)
88 print(’L^T: ’, L.T)

Listing 13.9: satelliteParamHW13.py

Complete simulation code for Matlab, Python, and Simulink can be down-
loaded at http://controlbook.byu.edu.

Important Concepts:
• Typically only a subset of the states are known. To implement a state
feedback controller the unknown states must be estimated.
• An observer estimates the system states using the measured outputs.
A typical observer will have a predictor term, that is based upon the
state-space equation, and a corrector term that modifies the predic-
tion based on the difference between the actual and expected mea-
surements.
• When a system is observable, we are able to (1) estimate all of the
states using measured outputs, (2) transform the state space equations
into observer canonical form, (3) invert the Observability matrix (i.e.
the Observability matrix is full rank), and (4) place the observer poles
in any desired location.
• There is a duality relationship between controllability and observ-
ability that allows for analogous steps in finding the observer gains
as was shown in finding the controller gains.
• The separation principle indicates that the controller gains and ob-
server gains may be chosen independently without affecting the
closed-loop stability.

TOC
CHAPTER 13. OBSERVERS 233

Notes and References

The original source for Luenberger observers is in [21]. Observer design using
pole placement is covered in most introductory textbooks on control, see for ex-
ample [4, 22, 23], as well as introductions to state space design [5, 6]. The most
common definition of observability is that a dynamic system is said to be observ-
able if the initial state x0 can be reconstructed by observing the output over any
finite interval. Linear time-invariant systems are observable if and only if the ob-
servability matrix is full rank [5]. Observer theory and design is an interesting
topic in its own right. The design of nonlinear observers is currently an active
area of research. The most commonly used observer for nonlinear systems is the
extended Kalman filter [24, 25].

TOC
CHAPTER 13. OBSERVERS 234

TOC
 Disturbance Observers
14
Learning Objectives:
• Use the observer to estimate the input disturbance in addition to the
state.

14.1 Theory
In previous sections we have discussed that modeling errors and other real-world
effects result in an input disturbance to the system as shown in Fig. 14-1. The

Figure 14-1: Input disturbance to plant modeled by state space equations.

objective of this section is to explain the effect of the disturbance on an observer.

It turns out that the disturbance creates a steady state bias in the observer states.
We will also describe how the observer can be augmented, the dual of adding an
integrator, to estimate the disturbance and to remove the steady state observation
error.
To understand the effect of the disturbance on the observer, note that when the
disturbance is present, the state space equations for the system are

ẋ = Ax + B(u + d)
y = Cx.

TOC
CHAPTER 14. DISTURBANCE OBSERVERS 236

The standard equation for the observer is

x̂˙ = Ax̂ + Bu + L(y − C x̂). (14.1)
Defining the observation error as e = x − x̂ and taking the derivative with respect
to time gives
ė = ẋ − x̂˙ (14.2)
= Ax + Bu + Bd − Ax̂ − Bu − LCx + LC x̂
= (A − LC)e + Bd. (14.3)
Taking the Laplace transform of both sides while accounting for the initial condi-
tion, and solving for E(s) gives
E(s) = (sI − (A − LC))−1 e(0) + (sI − (A − LC))−1 BL{d(t)}.
Therefore, the error is due to two terms. The first term involves the initial condi-
tions e(0) and decays to zero if the eigenvalues of A − LC are in the open left
half of the complex plane. The second term is driven by the disturbance and for
a constant bounded disturbance, it will result in a constant bounded observation
error.
If the disturbance d(t) were perfectly known, then we could modify the ob-
server in Equation (14.1) as
x̂˙ = Ax̂ + B(u + d) + L(y − C x̂), (14.4)
so that the evolution of the observation error becomes
ė = ẋ − x̂˙
= Ax + B(u + d) − Ax̂ − B(u + d) − LCx + LC x̂
= (A − LC)e,
which, in contrast to Equation (14.3) is not driven by the disturbance d. However,
since d is not known it needs to be estimated. The basic idea in this chapter is to
estimate d using an augmented observer.
If we assume that d is constant, then the differential equation that governs the
evolution of d is given by d˙ = 0. Therefore, if we augment the state x of the
system with the disturbance, then the equations of motion can be written as
      
ẋ A B x B
= + u
d˙ 0 0 d 0
 

x
y= C 0 .
d
Define the augmented state space matrices as
 
A B
A2 =
0 0


C2 = C 0 .

TOC
 CHAPTER 14. DISTURBANCE OBSERVERS 237

If (A2 , C2 ) are observable, then using the techniques discussed in Chapter 13 we

can design an observer for the augmented system, where the augmented observer
equations are
!         
x̂˙ A B x̂ L
x̂ B
˙ = 0 0 ˆ + y− C 0 ˆ + u
dˆ d Ld d 0

or in component form
 
x̂˙ = Ax̂ + B u + dˆ + L(y − C x̂)
˙
dˆ = Ld (y − C x̂),

where the gain L2 = (L> , Ld )> is obtained using the Ackerman formula
−1
L2 = OA 2 ,C2
(A>
A2 )
−1
(β d − aA2 )> ,

where aA2 is obtained from the characteristic polynomial of A2 , and β d is ob-

tained from the desired characteristic polynomial for A2 − L2 C2 . Alternatively,
L2 can be found using the Python place command:
1 import control as cnt
2 L2 = cnt.place(A2.T,C2.T,desired_observer_poles).T

14.1.1 Simple Example

Consider the example in Sections 11.2.1, 12.2.1, and 13.2.1. A block diagram
for the system is shown in Fig. 14-2, where a constant input disturbance has been
added to the system.
Suppose that a standard feedback controller without an integrator and without
a disturbance observer is designed for this system. An example of potential Python
code is
1 import numpy as np
2 import control as cnt
3
4 P.A = np.array([[0., 1.0], [2.0, 3.0]])
5 P.B = np.array([[0.], [4.0]])
6 P.C = np.array([[5.0, 0]])
7 P.D = 0
8
9 P.Ts = 0.01 #sampling time
10
11 # pick poles for the controller
12 wn_ctrl = 1
13 zeta_ctrl = 0.707
14 charpoly = [1., 2.0*zeta_ctrl*wn_ctrl, wn_ctrl**2]
15 des_ctrl_poles = np.roots(charpoly)
16
17 # pick poles for the observer

TOC
 CHAPTER 14. DISTURBANCE OBSERVERS 238

Figure 14-2: Block diagram for a simple example, where a constant input distur-
bance has been added to the system.

18 wn_obsv = 10.0
19 zeta_obsv = 0.707
20 charpoly = [1., 2.0*zeta_obsv*wn_obsv, wn_obsv**2]
21 des_obsv_poles = np.roots(charpoly)
22
23 # is the system controllable?
24 if np.linalg.matrix_rank(cnt.ctrb(P.A, P.B)) != np.size(P.A, 1):
25 print(’System Not Controllable’)
26 else:
27 P.K = cnt.place(P.A, P.B, des_ctrl_poles)
28 P.kr = -1.0/(P.C*np.linalg.inv(P.A-P.B*P.K)*P.B)
29
30 # is the system observable?
31 if np.linalg.matrix_rank(cnt.obsv(P.A, P.C)) != np.size(P.A, 1):
32 print(’System Not Observable’)
33 else:
34 P.L = cnt.place(P.A.T, P.C.T, des_obsv_poles).T

The associated control code is

1 # variables ’xhat’ and ’u’ must be declared as global, or
2 # class member variables for the sake of persistence, and the
3 # measurement ’y’ and reference input ’r’ must be passed in.
4 N = 10
5
6 # solve the observer differential equations using N steps of

TOC
 CHAPTER 14. DISTURBANCE OBSERVERS 239

7 # RK1 for i = 1 to N
8 for i in range(N):
9 self.xhat = self.xhat + P.Ts/N*(P.A*self.xhat + \
10 P.B*self.u + \
11 P.L*(y-P.C*self.xhat))
12
13 # observer-based update with feedback controller
14 self.u = P.kr*r - P.K*self.xhat

The step response for this controller is shown in Fig. 14-3, where it is obvious that
the input disturbance is causing a large steady state error in the system response,
as well as in the estimation error. If an integrator is added to the system using the

System Response Estimation Error

Figure 14-3: Step response for standard observer based control without integrator
and without disturbance observer.

following design file

1 # design file with integrator
2 integrator_pole = -1.0
3
4 # augment system to add integrator
5 P.A1 = np.block([[P.A, np.zeros((2,1))], [-P.C, 0]])
6 P.B1 = np.block([[P.B], [0]])
7
8 # is this system controllable?
9 if np.linalg.matrix_rank(cnt.ctrb(P.A1, P.B1)) != np.size(P.A1, 1):
10 print(’System Not Controllable’)
11 else:
12 K1 = cnt.place(P.A1, P.B1, np.concatenate((des_ctrl_poles,
13 [integrator_pole])))
14 P.K = K1[0,0:-1]
15 P.ki = K1[0, -1]

TOC
 CHAPTER 14. DISTURBANCE OBSERVERS 240

and the control code listed below:

1 # variables ’xhat,’ ’error_d1, ’integrator,’ and ’u’ must be
2 # declared as global, or class member variables for the sake
3 # of persistence, and the measurement ’y’ and reference input
4 # ’r’ must be passed in.
5 N = 10
6
7 # solve the observer differential equations using N steps of
8 # RK1 for i = 1 to N
9 for i in range(N):
10 self.xhat = self.xhat + P.Ts/N*(P.A*self.xhat + \
11 P.B*self.u + \
12 P.L*(y-P.C*self.xhat))
13
14 # implement the integrator
15 error = r-y
16 self.integrator = self.integrator + \
17 (P.Ts/2.0)*(error+self.error_d1)
18 self.error_d1 = error
19
20 # observer-based feedback controller
21 self.u = -P.K*self.xhat - P.ki*self.integrator

The response of the system with the integrator is shown in Fig. 14-4, where it is
clear that the steady state error in the response has been removed, but where there
is now a steady state response in the estimation error.

System Response Estimation Error

Figure 14-4: Step response for observer based control with integrator but without
disturbance observer.

Now consider the case where the integrator is not present, but we add a distur-

TOC
 CHAPTER 14. DISTURBANCE OBSERVERS 241

bance observer. The design code is listed below.

1 # is the system controllable?

2 if np.linalg.matrix_rank(cnt.ctrb(P.A, P.B)) != np.size(P.A, 1):
3 print(’System Not Controllable’)
4 else:
5 P.K = cnt.place(P.A, P.B, des_ctrl_poles)
6 P.kr = -1.0/(P.C*np.linalg.inv(P.A-P.B*P.K)*P.B)
7
8 # augment system for disturbance observer
9 disturb_obsv_pole = -2.0
10 P.A2 = np.block([[P.A, P.B], [np.zeros((1,2)), 0]])
11 P.B2 = np.block([[P.B], [0]])
12 P.C2 = np.block([P.C, 0])
13
14 # is the system observable?
15 if np.linalg.matrix_rank(cnt.obsv(P.A2, P.C2)) != np.size(P.A2, 1):
16 print(’System Not Observable’)
17 else:
18 P.L2 = cnt.place(P.A2.T, P.C2.T,
19 np.concatenate((des_obsv_poles,
20 [disturb_obsv_pole]))).T

The associated control code is listed below.

1 # variables ’x_obsv’ and ’u’ must be declared as global, or

2 # class member variables for the sake of persistence, and the
3 # measurement ’y’ and reference input ’r’ must be passed in.
4 N = 10
5
6 # solve the observer differential equations using N steps of
7 # RK1 for i = 1 to N
8 for i in range(N):
9 self.x_obsv = self.x_obsv + P.Ts/N*(P.A2*self.x_obsv + \
10 P.B2*self.u + \
11 P.L2*(y-P.C2*self.x_obsv))
12 xhat = self.x_obsv[0, 0:-1]
13 dhat = self.x_obsv[0, -1]
14
15 # observer-based feedback controller with disturbance obsverver
16 self.u = P.kr*r - P.K*self.xhat - dhat

Note the presence of dˆ on Line 13. The disturbance estimate is subtracted from
the control input on Line 16. The resulting system response is shown in Fig. 14-5.
Note that in this case, the steady state error in both the system response and the
estimation error has been removed. At this point, the integrator can be added back
into the system to remove the effect of model mismatch and other disturbances on
the system.

TOC
 CHAPTER 14. DISTURBANCE OBSERVERS 242

System Response Estimation Error

Figure 14-5: Step response for observer based control with a disturbance observer
rather than an integrator.

14.2 Design Study A. Single Link Robot Arm

Example Problem A.14

(a) Modify the simulation from HW A.13 so that the uncertainty parameter in
arm_dynamics.m is α = 0.2, representing 20% inaccuracy in the knowl-
edge of the system parameters and so that the input disturbance is 0.5 Newton-
meters. Also, add noise to the output channel θm with a standard deviation
of 0.001. Before adding the disturbance observer, run the simulation and
note that the controller is not robust enough to handle the large input distur-
bance.
(b) Add a disturbance observer to the controller, and verify that the steady state
error in the estimator has been removed. Tune the system to get good re-
sponse.

Solution
The single link robot arm is simulated using the following Python code.
1 import sys
2 sys.path.append(’..’) # add parent directory
3 import matplotlib.pyplot as plt
4 import numpy as np

TOC
 CHAPTER 14. DISTURBANCE OBSERVERS 243

5 import armParam as P
6 from hw3.armDynamics import armDynamics
7 from armController import armController
8 from hw2.signalGenerator import signalGenerator
9 from hw2.armAnimation import armAnimation
10 from hw2.dataPlotter import dataPlotter
11 from hw13.dataPlotterObserver import dataPlotterObserver
12
13 # instantiate arm, controller, and reference classes
14 arm = armDynamics(alpha=0.2)
15 controller = armController()
16 reference = signalGenerator(amplitude=30*np.pi/180.0,
17 frequency=0.05)
18 disturbance = signalGenerator(amplitude=0.25)
19 noise = signalGenerator(amplitude=0.01)
20
21 # instantiate the simulation plots and animation
22 dataPlot = dataPlotter()
23 dataPlotObserver = dataPlotterObserver()
24 animation = armAnimation()
25
26 t = P.t_start # time starts at t_start
27 y = arm.h() # output of system at start of simulation
28
29 while t < P.t_end: # main simulation loop
30 # Get referenced inputs from signal generators
31 # Propagate dynamics in between plot samples
32 t_next_plot = t + P.t_plot
33
34 # updates control and dynamics at faster simulation rate
35 while t < t_next_plot:
36 r = reference.square(t)
37 d = disturbance.step(t) # input disturbance
38 n = noise.random(t) # simulate sensor noise
39 # update controller
40 u, xhat, dhat = controller.update(r, y + n)
41 y = arm.update(u + d) # propagate system
42 t = t + P.Ts # advance time by Ts
43
44 # update animation and data plots
45 animation.update(arm.state)
46 dataPlot.update(t, r, arm.state, u)
47 dataPlotObserver.update(t, arm.state, xhat, d, dhat)
48 plt.pause(0.0001)
49
50 # Keeps the program from closing until the user presses a button.
51 print(’Press key to close’)
52 plt.waitforbuttonpress()
53 plt.close()

Listing 14.1: armSim.py

Note that in this simulation, the uncertainty parameter is α = 0.2 implying 20%
variation in the plant parameters, and that the noise and disturbance are not zero.
A Python class that implements observer-based control with an integrator and
disturbance observer for the single link robot arm is shown below.

TOC
 CHAPTER 14. DISTURBANCE OBSERVERS 244

1 import numpy as np
2 import armParamHW14 as P
3
4 class armController:
5 def __init__(self):
6 self.observer_state = np.array([
7 [0.0], # estimate of theta
8 [0.0], # estimate of theta_hat
9 [0.0], # estimate of disturbance
10 ])
11 self.tau_d1 = 0.0 # control torque, delayed 1 sample
12 self.integrator = 0.0 # integrator
13 self.error_d1 = 0.0 # error signal delayed by 1 sample
14 self.K = P.K # state feedback gain
15 self.ki = P.ki # Input gain
16 self.L = P.L # observer gain
17 self.Ld = P.Ld
18 self.L2 = P.L2
19 self.A2 = P.A2 # system model
20 self.B2 = P.B2
21 self.C2 = P.C2
22 self.limit = P.tau_max # Maximum torque
23 self.Ts = P.Ts # sample rate of controller
24
25 def update(self, theta_r, y_m):
26 # update the observer and extract theta_hat
27 x_hat, d_hat = self.update_observer(y_m)
28 theta_hat = x_hat.item(0)
29
30 # integrate error
31 error = theta_r - theta_hat
32 self.integrateError(error)
33
34 # compute feedback linearizing torque tau_fl
35 tau_fl = P.m * P.g * (P.ell / 2.0) * np.cos(theta_hat)
36
37 # Compute the state feedback controller
38 tau_tilde = -self.K @ x_hat \
39 - self.ki * self.integrator - d_hat
40
41 # compute total torque
42 tau = self.saturate(tau_fl + tau_tilde.item(0))
43 self.tau_d1 = tau
44
45 return tau, x_hat, d_hat
46
47 def update_observer(self, y_m):
48 # update the observer using RK4 integration
49 F1 = self.observer_f(self.observer_state, y_m)
50 F2 = self.observer_f(self.observer_state \
51 + self.Ts / 2 * F1, y_m)
52 F3 = self.observer_f(self.observer_state \
53 + self.Ts / 2 * F2, y_m)
54 F4 = self.observer_f(self.observer_state \
55 + self.Ts * F3, y_m)
56 self.observer_state += self.Ts / 6 * \

TOC
 CHAPTER 14. DISTURBANCE OBSERVERS 245

57 (F1 + 2 * F2 + 2 * F3 + F4)
58 x_hat = np.array([[self.observer_state.item(0)],
59 [self.observer_state.item(1)]])
60 d_hat = self.observer_state.item(2)
61
62 return x_hat, d_hat
63
64 def observer_f(self, x_hat, y_m):
65 # compute feedback linearizing torque tau_fl
66 theta_hat = x_hat.item(0)
67 tau_fl = P.m * P.g * (P.ell / 2.0) * np.cos(theta_hat)
68 # xhatdot = A*xhat + B*(u-ue) + L(y-C*xhat)
69 xhat_dot = self.A2 @ x_hat\
70 + self.B2 * (self.tau_d1 - tau_fl)\
71 + self.L2 * (y_m - self.C2 @ x_hat)
72
73 return xhat_dot
74
75 def integrateError(self, error):
76 self.integrator = self.integrator \
77 + (self.Ts/2.0)*(error + self.error_d1)
78 self.error_d1 = error
79
80 def saturate(self, u):
81 if abs(u) > self.limit:
82 u = self.limit*np.sign(u)
83 return u

Listing 14.2: armController.py

Python code that computes the control gains is given below.

1 # Single link arm Parameter File
2 import numpy as np
3 import control as cnt
4 import sys
5 sys.path.append(’..’) # add parent directory
6 import armParam as P
7
8 # tuning parameters
9 tr = 0.4
10 zeta = 0.707
11 integrator_pole = -5
12 wn_obs = 10 # natural frequency for observer
13 zeta_obs = 0.707 # damping ratio for observer
14 dist_obsv_pole = -5.5 # pole for disturbance observer
15
16
17 Ts = P.Ts # sample rate of the controller
18 tau_max = P.tau_max # limit on control signal
19 m = P.m
20 ell = P.ell
21 g = P.g
22
23
24 # State Space Equations
25 A = np.array([[0.0, 1.0],

TOC
 CHAPTER 14. DISTURBANCE OBSERVERS 246

26 [0.0, -1.0*P.b/P.m/(P.ell**2)]])
27
28 B = np.array([[0.0],
29 [3.0/P.m/(P.ell**2)]])
30
31 C = np.array([[1.0, 0.0]])
32
33 # control design
34 # form augmented system
35 A1 = np.array([[0.0, 1.0, 0.0],
36 [0.0, -1.0*P.b/P.m/(P.ell**2), 0.0],
37 [-1.0, 0.0, 0.0]])
38
39 B1 = np.array([[0.0],
40 [3.0/P.m/(P.ell**2)],
41 [0.0]])
42
43 # gain calculation
44 wn = 2.2/tr # natural frequency
45
46 #des_char_poly = np.convolve(
47 # [1, 2*zeta*wn, wn**2],
48 # np.poly(integrator_pole))
49 #des_poles = np.roots(des_char_poly)
50 des_char = np.array([1., 2.*zeta*wn_obs, wn_obs**2.])
51 des_char_augmented = np.convolve(des_char,
52 np.poly([integrator_pole]))
53 des_poles = np.roots(des_char_augmented)
54
55 # Compute the gains if the system is controllable
56 if np.linalg.matrix_rank(cnt.ctrb(A1, B1)) != 3:
57 print("The system is not controllable")
58 else:
59 K1 = cnt.acker(A1, B1, des_poles)
60 K = np.array([K1.item(0), K1.item(1)])
61 ki = K1.item(2)
62
63 # observer design
64 # Augmented Matrices
65 A2 = np.concatenate((
66 np.concatenate((A, B), axis=1),
67 np.zeros((1, 3))),
68 axis=0)
69 B2 = np.concatenate((B, np.zeros((1, 1))), axis=0)
70 C2 = np.concatenate((C, np.zeros((1, 1))), axis=1)
71
72 des_char_est = np.array([1., 2.*zeta*wn_obs, wn_obs**2.])
73 des_char_est_aug = np.convolve(des_char_est,
74 np.poly([dist_obsv_pole]))
75 des_obsv_poles = np.roots(des_char_est_aug)
76
77 # Compute the gains if the system is controllable
78 if np.linalg.matrix_rank(cnt.ctrb(A2.T, C2.T)) != 3:
79 print("The system is not observable")
80 else:
81 L2 = cnt.acker(A2.T, C2.T, des_obsv_poles).T
82 L = np.array([[L2.item(0)], [L2.item(1)]])

TOC
 CHAPTER 14. DISTURBANCE OBSERVERS 247

83 Ld = L2.item(0)
84
85 print(’K: ’, K)
86 print(’ki: ’, ki)
87 print(’L^T: ’, L.T)
88 print(’Ld: ’, Ld)

Listing 14.3: armParamHW14.py

Complete simulation code for Matlab, Python, and Simulink can be down-
loaded at http://controlbook.byu.edu.

14.3 Design Study B. Inverted Pendulum

Example Problem B.14

(a) Modify the simulation from HW B.13 so that the uncertainty parameter in
pendulum_dynamics.m is α = 0.2, representing 20% inaccuracy in
the knowledge of the system parameters, and so that the input disturbance
is 0.5. Also, add noise to the output channels zm and θm with a standard de-
viation of 0.001. Before adding the disturbance observer, run the simulation
and note that the controller is not robust to the large input disturbance.
(b) Add a disturbance observer to the controller, and verify that the steady state
error in the estimator has been removed. Tune the system to get good re-
sponse.

Solution
The inverted pendulum is simulated using the following Python code.
1 import sys
2 sys.path.append(’..’) # add parent directory
3 import matplotlib.pyplot as plt
4 import numpy as np
5 import pendulumParam as P
6 from hw3.pendulumDynamics import pendulumDynamics
7 from pendulumController import pendulumController
8 from hw2.signalGenerator import signalGenerator
9 from hw2.pendulumAnimation import pendulumAnimation
10 from hw2.dataPlotter import dataPlotter
11 from hw13.dataPlotterObserver import dataPlotterObserver
12
13 # instantiate pendulum, controller, and reference classes
14 pendulum = pendulumDynamics(alpha = 0.2)
15 controller = pendulumController()
16 reference = signalGenerator(amplitude=0.5, frequency=0.05)
17 disturbance = signalGenerator(amplitude=0.5, frequency=.1)
18 noise_z = signalGenerator(amplitude=0.01)
19 noise_th = signalGenerator(amplitude=0.01)

TOC
 CHAPTER 14. DISTURBANCE OBSERVERS 248

20
21 # instantiate the simulation plots and animation
22 dataPlot = dataPlotter()
23 dataPlotObserver = dataPlotterObserver()
24 animation = pendulumAnimation()
25
26 t = P.t_start # time starts at t_start
27 y = pendulum.h() # output of system at start of simulation
28
29 while t < P.t_end: # main simulation loop
30 # Get referenced inputs from signal generators
31 # Propagate dynamics in between plot samples
32 t_next_plot = t + P.t_plot
33
34 while t < t_next_plot:
35 r = reference.square(t)
36 d = disturbance.sin(t) # input disturbance
37 #n = np.array([[0.], [0.]])
38 n = np.array([[noise_z.random(t)], [noise_th.random(t)]])
39 u, xhat, dhat = controller.update(r, y + n)
40 y = pendulum.update(u + d) # propagate system
41 t = t + P.Ts # advance time by Ts
42
43 # update animation and data plots
44 animation.update(pendulum.state)
45 dataPlot.update(t, r, pendulum.state, u)
46 dataPlotObserver.update(t, pendulum.state, xhat, d, dhat)
47 plt.pause(0.0001)
48
49 # Keeps the program from closing until the user presses a button.
50 print(’Press key to close’)
51 plt.waitforbuttonpress()
52 plt.close()

Listing 14.4: pendulumSim.py

Note that in this simulation, the uncertainty parameter is α = 0.2 implying 20%
variation in the plant parameters, and that the noise and disturbance are not zero.
A Python class that implements observer-based control with an integrator and
disturbance observer for the inverted pendulum is shown below.
1 import numpy as np
2 import pendulumParamHW14 as P
3
4 class pendulumController:
5 def __init__(self):
6 self.obsv_state = np.array([
7 [0.0], # initial estimate for z_hat
8 [0.0], # initial estimate for theta_hat
9 [0.0], # initial estimate for z_hat_dot
10 [0.0], # initial estimate for theta_hat_dot
11 [0.0], # estimate of the disturbance
12 ])
13 self.F_d1 = 0.0 # Computed Force, delayed by 1 sample
14 self.integrator = 0.0 # integrator
15 self.error_d1 = 0.0 # error signal delayed by 1 sample

TOC
 CHAPTER 14. DISTURBANCE OBSERVERS 249

16 self.K = P.K # state feedback gain

17 self.ki = P.ki # integrator gain
18 self.L = P.L2 # observer gain
19 self.A = P.A2 # system model
20 self.B = P.B1
21 self.C = P.C2
22 self.limit = P.F_max # Maximum force
23 self.Ts = P.Ts # sample rate of controller
24
25 def update(self, z_r, y):
26 # update the observer and extract z_hat
27 x_hat, d_hat = self.update_observer(y)
28 z_hat = x_hat.item(0)
29
30 # integrate error
31 error = z_r - z_hat
32 self.integrateError(error)
33
34 # Compute the state feedback controller
35 F_unsat = -self.K @ x_hat \
36 - self.ki * self.integrator \
37 - d_hat
38 F = self.saturate(F_unsat.item(0))
39 self.F_d1 = F
40 return F, x_hat, d_hat
41
42 def update_observer(self, y):
43 # update the observer using RK4 integration
44 F1 = self.obsv_f(self.obsv_state, y)
45 F2 = self.obsv_f(self.obsv_state + self.Ts / 2 * F1, y)
46 F3 = self.obsv_f(self.obsv_state + self.Ts / 2 * F2, y)
47 F4 = self.obsv_f(self.obsv_state + self.Ts * F3, y)
48 self.obsv_state += self.Ts / 6 * (F1 + 2 * F2 + 2 * F3 + F4)
49 x_hat = np.array([[self.obsv_state.item(0)],
50 [self.obsv_state.item(1)],
51 [self.obsv_state.item(2)],
52 [self.obsv_state.item(3)]])
53 d_hat = self.obsv_state.item(4)
54
55 return x_hat, d_hat
56
57 def obsv_f(self, x_hat, y_m):
58 # xhatdot = A*xhat + B*(u-ue) + L(y-C*xhat)
59 xhat_dot = self.A @ x_hat \
60 + self.B * self.F_d1 \
61 + self.L @ (y_m - self.C @ x_hat)
62
63 return xhat_dot
64
65 def integrateError(self, error):
66 self.integrator = self.integrator \
67 + (self.Ts/2.0)*(error + self.error_d1)
68 self.error_d1 = error
69
70 def saturate(self,u):
71 if abs(u) > self.limit:
72 u = self.limit*np.sign(u)

TOC
 CHAPTER 14. DISTURBANCE OBSERVERS 250

73 return u

Listing 14.5: pendulumController.py

Python code that computes the observer and control gains is given below.
1 # Inverted Pendulum Parameter File
2 import sys
3 sys.path.append(’..’) # add parent directory
4 import pendulumParam as P
5 import numpy as np
6 from scipy import signal
7 import control as cnt
8
9 # sample rate of the controller
10 Ts = P.Ts
11
12 # saturation limits
13 F_max = P.F_max # Max Force, N
14
15 # tuning parameters
16 tr_z = 1.5 # rise time for position
17 tr_theta = 0.5 # rise time for angle
18 zeta_z = 0.707 # damping ratio position
19 zeta_th = 0.707 # damping ratio angle
20 integrator_pole = -10.0 # integrator pole
21 tr_z_obs = tr_z/10.0 # rise time for observer - position
22 tr_theta_obs = tr_theta/10.0 # rise time for observer - angle
23 dist_obsv_pole = -1.0 # pole for disturbance observer
24
25 # State Space Equations
26 # xdot = A*x + B*u
27 # y = C*x
28 A = np.array([[0.0, 0.0, 1.0, 0.0],
29 [0.0, 0.0, 0.0, 1.0],
30 [0.0, -3*P.m1*P.g/4/(.25*P.m1+P.m2),
31 -P.b/(.25*P.m1+P.m2), 0.0],
32 [0.0, 3*(P.m1+P.m2)*P.g/2/(.25*P.m1+P.m2)/P.ell,
33 3*P.b/2/(.25*P.m1+P.m2)/P.ell, 0.0]])
34 B = np.array([[0.0],
35 [0.0],
36 [1/(.25*P.m1+P.m2)],
37 [-3.0/2/(.25*P.m1+P.m2)/P.ell]])
38 C = np.array([[1.0, 0.0, 0.0, 0.0],
39 [0.0, 1.0, 0.0, 0.0]])
40
41 # form augmented system
42 Cr = np.array([[1.0, 0.0, 0.0, 0.0]])
43
44 # Augmented Matrices
45 A1 = np.concatenate((
46 np.concatenate((A, np.zeros((4, 1))), axis=1),
47 np.concatenate((-Cr, np.array([[0.0]])), axis=1)),
48 axis=0)
49 B1 = np.concatenate((B, np.array([[0.0]])), axis=0)
50
51 # computer control gains

TOC
 CHAPTER 14. DISTURBANCE OBSERVERS 251

52 wn_th = 2.2/tr_theta # natural frequency for angle

53 wn_z = 2.2/tr_z # natural frequency for position
54 des_char_poly = np.convolve(
55 np.convolve([1, 2*zeta_z*wn_z, wn_z**2],
56 [1, 2*zeta_th*wn_th, wn_th**2]),
57 np.poly([integrator_pole]))
58 des_poles = np.roots(des_char_poly)
59
60 # Compute the gains if the system is controllable
61 if np.linalg.matrix_rank(cnt.ctrb(A1, B1)) != 5:
62 print("The system is not controllable")
63 else:
64 K1 = cnt.acker(A1, B1, des_poles)
65 K = K1[0, 0:4]
66 ki = K1[0, 4]
67
68 # compute observer gains
69 # Augmented Matrices
70 A2 = np.concatenate((
71 np.concatenate((A, B), axis=1),
72 np.zeros((1, 5))),
73 axis=0)
74 C2 = np.concatenate((C, np.zeros((2, 1))), axis=1)
75
76 wn_z_obs = 2.2/tr_z_obs
77 wn_th_obs = 2.2/tr_theta_obs
78 des_obs_char_poly = np.convolve(
79 np.convolve([1, 2*zeta_z*wn_z_obs, wn_z_obs**2],
80 [1, 2*zeta_th*wn_th_obs, wn_th_obs**2]),
81 np.poly([dist_obsv_pole]))
82 des_obs_poles = np.roots(des_obs_char_poly)
83
84 # Compute the gains if the system is observable
85 if np.linalg.matrix_rank(cnt.ctrb(A2.T, C2.T)) != 5:
86 print("The system is not observable")
87 else:
88 L2 = signal.place_poles(A2.T, C2.T, des_obs_poles).gain_matrix.T
89
90 print(’K: ’, K)
91 print(’ki: ’, ki)
92 print(’L^T: ’, L2.T)

Listing 14.6: pendulumParamHW14.py

Complete simulation code for Matlab, Python, and Simulink can be down-
loaded at http://controlbook.byu.edu.

TOC
 CHAPTER 14. DISTURBANCE OBSERVERS 252

14.4 Design Study C. Satellite Attitude Control

Example Problem C.14

(a) Modify the simulation from HW C.13 so that the uncertainty parameter in
satellite_dynamics.m is α = 0.2, representing 20% inaccuracy in
the knowledge of the system parameters and so that the input disturbance is
1.0 Newton-meters. Also, add noise to the output channels φm and θm with
a standard deviation of 0.001. Before adding the disturbance observer, run
the simulation and note that the controller is not robust to the large input
disturbance.

(b) Add a disturbance observer to the controller, and verify that the steady state
error in the estimator has been removed. Tune the system to get good re-
sponse.

Solution
The satellite is simulated using the following Python code.
1 import sys
2 sys.path.append(’..’) # add parent directory
3 import matplotlib.pyplot as plt
4 import numpy as np
5 import satelliteParam as P
6 from hw3.satelliteDynamics import satelliteDynamics
7 from satelliteController import satelliteController
8 from hw2.signalGenerator import signalGenerator
9 from hw2.satelliteAnimation import satelliteAnimation
10 from hw2.dataPlotter import dataPlotter
11 from hw13.dataPlotterObserver import dataPlotterObserver
12
13 # instantiate satellite, controller, and reference classes
14 satellite = satelliteDynamics(alpha=0.2)
15 controller = satelliteController()
16 reference = signalGenerator(amplitude=15.0*np.pi/180.0,
17 frequency=0.03)
18 disturbance = signalGenerator(amplitude=1.0)
19 noise_phi = signalGenerator(amplitude=0.01)
20 noise_th = signalGenerator(amplitude=0.01)
21
22 # instantiate the simulation plots and animation
23 dataPlotObserver = dataPlotterObserver()
24 dataPlot = dataPlotter()
25 animation = satelliteAnimation()
26
27 t = P.t_start # time starts at t_start
28 y = satellite.h() # output of system at start of simulation
29
30 while t < P.t_end: # main simulation loop
31 # Propagate dynamics in between plot samples

TOC
 CHAPTER 14. DISTURBANCE OBSERVERS 253

32 t_next_plot = t + P.t_plot
33
34 # updates control and dynamics at faster simulation rate
35 while t < t_next_plot:
36 r = reference.square(t) # reference input
37 d = disturbance.step(t) # input disturbance
38
39 # simulate sensor noise
40 n = np.array([[noise_phi.random(t)],
41 [noise_th.random(t)]])
42
43 # update controller
44 u, xhat, dhat = controller.update(r, y + n)
45 y = satellite.update(u + d) # propagate system
46 t = t + P.Ts # advance time by Ts
47
48 # update animation and data plots
49 animation.update(satellite.state)
50 dataPlot.update(t, r, satellite.state, u)
51 dataPlotObserver.update(t, satellite.state, xhat, d, dhat)
52 plt.pause(0.0001)
53
54 # Keeps the program from closing until user presses a button.
55 print(’Press key to close’)
56 plt.waitforbuttonpress()
57 plt.close()

Listing 14.7: satelliteSim.py

Note that in this simulation, the uncertainty parameter is α = 0.2 implying 20%
variation in the plant parameters, and that the noise and disturbance are not zero.
A Python class that implements observer-based control with an integrator and
disturbance observer for the satellite is shown below.
1 import numpy as np
2 import satelliteParamHW14 as P
3
4 class satelliteController:
5 def __init__(self):
6 self.observer_state = np.array([
7 [0.0], # initial estimate for theta_hat
8 [0.0], # initial estimate for phi_hat
9 [0.0], # initial estimate for theta_hat_dot
10 [0.0], # initial estimate for phi_hat_dot
11 [0.0], # estimate of the disturbance
12 ])
13 self.tau_d1 = 0.0 # Computed torque delayed 1 sample
14 self.integrator = 0.0 # integrator
15 self.error_d1 = 0.0 # error signal delayed by 1 sample
16 self.K = P.K # state feedback gain
17 self.ki = P.ki # integrator gain
18 self.L = P.L2 # observer gain
19 self.Ld = P.Ld # gain for disturbance observer
20 self.A = P.A2 # system model
21 self.B = P.B1
22 self.C = P.C2

TOC
 CHAPTER 14. DISTURBANCE OBSERVERS 254

23 self.limit = P.tau_max # Maxiumum torque

24 self.Ts = P.Ts # sample rate of controller
25
26 def update(self, phi_r, y):
27 # update the observer and extract z_hat
28 x_hat, d_hat = self.update_observer(y)
29 phi_hat = x_hat.item(1)
30
31 # integrate error
32 error = phi_r - phi_hat
33 self.integrateError(error)
34
35 # Compute the state feedback controller
36 tau_unsat = -self.K @ x_hat \
37 - self.ki*self.integrator \
38 - d_hat
39 tau = self.saturate(tau_unsat.item(0))
40 self.tau_d1 = tau
41 return tau, x_hat, d_hat
42
43 def update_observer(self, y_m):
44 # update the observer using RK4 integration
45 F1 = self.observer_f(self.observer_state, y_m)
46 F2 = self.observer_f(self.observer_state \
47 + self.Ts / 2 * F1, y_m)
48 F3 = self.observer_f(self.observer_state \
49 + self.Ts / 2 * F2, y_m)
50 F4 = self.observer_f(self.observer_state \
51 + self.Ts * F3, y_m)
52 self.observer_state += self.Ts / 6 \
53 * (F1 + 2 * F2 + 2 * F3 + F4)
54 x_hat = np.array([[self.observer_state.item(0)],
55 [self.observer_state.item(1)],
56 [self.observer_state.item(2)],
57 [self.observer_state.item(3)]])
58 d_hat = self.observer_state.item(4)
59 return x_hat, d_hat
60
61 def observer_f(self, x_hat, y_m):
62 # xhatdot = A*xhat + B*(u-ue) + L(y-C*xhat)
63 xhat_dot = self.A @ x_hat \
64 + self.B * self.tau_d1 \
65 + self.L @ (y_m - self.C @ x_hat)
66 return xhat_dot
67
68 def integrateError(self, error):
69 self.integrator = self.integrator \
70 + (self.Ts / 2.0) * (error + self.error_d1)
71 self.error_d1 = error
72
73 def saturate(self,u):
74 if abs(u) > self.limit:
75 u = self.limit*np.sign(u)
76 return u

Listing 14.8: satelliteController.py

TOC
 CHAPTER 14. DISTURBANCE OBSERVERS 255

Python code that computes the control gains is given below.

1 # satellite Parameter File
2 import numpy as np
3 import control as cnt
4 from scipy import signal
5 import sys
6 sys.path.append(’..’) # add parent directory
7 import satelliteParam as P
8
9 Ts = P.Ts
10 sigma = P.sigma
11 beta = P.beta
12 tau_max = P.tau_max
13
14 # tuning parameters
15 wn_th = 0.6
16 wn_phi = 1.1 # rise time for angle
17 zeta_phi = 0.707 # damping ratio position
18 zeta_th = 0.707 # damping ratio angle
19 integrator_pole = np.array([-1.0])
20 # pick observer poles
21 wn_th_obs = 10.0*wn_th
22 wn_phi_obs = 10.0*wn_phi
23 dist_obsv_pole = np.array([-1.0]) # disturbance observer pole
24
25 # State Space Equations
26 # xdot = A*x + B*u
27 # y = C*x
28 A = np.array([[0.0, 0.0, 1.0, 0.0],
29 [0.0, 0.0, 0.0, 1.0],
30 [-P.k/P.Js, P.k/P.Js, -P.b/P.Js, P.b/P.Js],
31 [P.k/P.Jp, -P.k/P.Jp, P.b/P.Jp, -P.b/P.Jp]])
32
33 B = np.array([[0.0],
34 [0.0],
35 [1.0/P.Js],
36 [0.0]])
37
38 C = np.array([[1.0, 0.0, 0.0, 0.0],
39 [0.0, 1.0, 0.0, 0.0]])
40
41 # form augmented system
42 Cout = np.array([[0.0, 1.0, 0.0, 0.0]])
43 A1 = np.array([[0.0, 0.0, 1.0, 0.0, 0.0],
44 [0.0, 0.0, 0.0, 1.0, 0.0],
45 [-P.k/P.Js, P.k/P.Js, -P.b/P.Js, P.b/P.Js, 0.0],
46 [P.k/P.Jp, -P.k/P.Jp, P.b/P.Jp, -P.b/P.Jp, 0.0],
47 [0.0, -1.0, 0.0, 0.0, 0.0]])
48
49 B1 = np.array([[0.0],
50 [0.0],
51 [1.0/P.Js],
52 [0.0],
53 [0.0]])
54
55

TOC
 CHAPTER 14. DISTURBANCE OBSERVERS 256

56 # gain calculation
57 des_char_poly = np.convolve(
58 np.convolve([1, 2*zeta_phi*wn_phi, wn_phi**2],
59 [1, 2*zeta_th*wn_th, wn_th**2]),
60 np.poly(integrator_pole))
61 des_poles = np.roots(des_char_poly)
62
63 # Compute the gains if the system is controllable
64 if np.linalg.matrix_rank(cnt.ctrb(A1, B1)) != 5:
65 print("The system is not controllable")
66 else:
67 K1 = cnt.acker(A1, B1, des_poles)
68 K = np.array([K1.item(0),
69 K1.item(1),
70 K1.item(2),
71 K1.item(3)])
72 ki = K1.item(4)
73
74 # compute observer gains
75 # Augmented Matrices
76 A2 = np.concatenate((
77 np.concatenate((A, B), axis=1),
78 np.zeros((1, 5))),
79 axis=0)
80 C2 = np.concatenate((C, np.zeros((2, 1))), axis=1)
81 des_obs_char_poly = np.convolve(
82 np.convolve([1, 2*zeta_phi*wn_phi_obs, wn_phi_obs**2],
83 [1, 2*zeta_th*wn_th_obs, wn_th_obs**2]),
84 np.poly(dist_obsv_pole))
85 des_obs_poles = np.roots(des_obs_char_poly)
86
87 # Compute the gains if the system is observable
88 if np.linalg.matrix_rank(cnt.ctrb(A2.T, C2.T)) != 5:
89 print("The system is not observable")
90 else:
91 # place_poles returns an object with various properties.
92 # The gains are accessed through .gain_matrix
93 # .T transposes the matrix
94 L2 = signal.place_poles(A2.T,
95 C2.T,
96 des_obs_poles).gain_matrix.T
97 L = L2[0:4, 0:2]
98 Ld = L2[4:5, 0:2]
99
100 print(’K: ’, K)
101 print(’ki: ’, ki)
102 print(’L^T: ’, L.T)
103 print(’Ld: ’, Ld)

Listing 14.9: satelliteParamHW14.py

Simulation code for Matlab, Python, and Simulink can be downloaded at

http://controlbook.byu.edu.

TOC
CHAPTER 14. DISTURBANCE OBSERVERS 257

Important Concepts:
• Input disturbances create a steady-state bias in the observer states.
• Augmenting the observer with a disturbance state provides a method
to estimate and remove the disturbance from the control input.

Notes and References

Disturbance observers are one example of how observers can be used to estimate
a variety of different parameters in the system. In general, observers can be used
to estimate model parameters like mass and inertia, but in this case, nonlinear
observers are required. The most commonly used nonlinear observer is the ex-
tended Kalman filter. Derivations of the extended Kalman filter can be found
in [25, 24, 26]. In robotics, observers are used to estimate the system state, but
also the biases in gyroscopes and accelerometers [27], which are a form of input
disturbances.

TOC
 Part V
Loopshaping Control Design

This part of the book introduces the loopshaping design method based on the
frequency response characteristics of the system. In Part II we derived transfer
function and state space models based on the linearized system. In Part III we
showed that PID controllers can be used to regulate the output of second order
systems. For higher order systems, PID can be used if the original system can be
written as a cascade, or series, of second order systems. In Part IV we showed that
controllers can be derived for higher order systems using state space models. The
state space design method is a time-domain method that focuses on shaping the
time-response of the system. In this part of the book we extend PID control using
frequency domain ideas where we shape the frequency response of the system.
When the transfer function is available, it provides a compact representation of
the frequency response of the open loop system. Therefore, in this part we will
focus on transfer function models of the system. The methods in this part are not
constrained to second order systems and do not depend on a cascade structure.
In fact, they do not even require a transfer function of the system, as long as the
frequency response, or Bode plot, is known.
Chapter 15 is a review of frequency response models of linear time-invariant
systems and develops intuition concerning the Bode plot of the input-output re-
sponse. In Chapter 16 we show how performance specifications for reference
tracking, disturbance rejection, and noise attenuation can be related to the fre-
quency response of the open loop control-plant pair. Chapter 17 addresses stabil-
ity of the closed-loop system from a frequency response point of view. Chapter 18
represents the culminating chapter in this part of the book and describes how to
design the controller to achieve closed-loop stability and to satisfy design specifi-
cations on reference tracking, disturbance rejection, and noise attenuation.
 Frequency Response of LTI Systems
15
Learning Objectives:
• Convert complex numbers between rectangular coordinates and polar
form.
• Identify how transfer functions change the magnitude and phase of
an input signal.
• Draw straight-line approximations for Bode plots.

15.1 Theory
15.1.1 Manipulating Complex Numbers
Any complex number z can be represented in rectangular coordinates as
z = <{z} + j={z},
where <{z} is the real part of z, and ={z} is the imaginary part of z. Similarly, z
can be represented in polar form as
z = |z| ej∠z , (15.1)
where |z| denotes the magnitude of z and ∠z denotes the angle or phase of z. The
relationship between the real and imaginary parts, and the magnitude and phase
of z, are depicted in Fig. 15-1. From the geometry shown in Fig. 15-1 we can see
that
p
|z| = <{z}2 + ={z}2
={z}
∠z = tan−1 .
<{z}

TOC
CHAPTER 15. FREQUENCY RESPONSE 260

z
={z}
|z|

\z <
<{z}

Figure 15-1: Relationship between the real and imaginary parts of z, and the
magnitude and phase of z.

Similarly, from Euler’s relationship

ejθ = cos θ + j sin θ,

we have from Equation (15.1) that

z = |z| ej∠z = |z| cos ∠z + j |z| sin ∠z

which implies that

<{z} = |z| cos ∠z

={z} = |z| sin ∠z.

The conjugate of z, denoted z̄, is given by

z̄ = <{z} − j={z} = |z| e−j∠z .

2
Note that z z̄ = |z| .
Suppose that z1 = |z1 | ej∠z1 and z2 = |z2 | ej∠z2 are two complex numbers,
then

z1 z2 = |z1 | ej∠z1 |z2 | ej∠z2

= |z1 | |z2 | ej∠z1 ej∠z2
= |z1 | |z2 | ej(∠z1 +∠z2 ) ,

therefore when multiplying complex numbers, their magnitudes multiply, but their
phases add. Similarly, we have

z1 |z1 | ej∠z1 |z1 | ej∠z1 |z1 | j(∠z1 −∠z2 )

= j∠z
= = e ,
z2 |z2 | e 2 |z2 | ej∠z2 |z2 |

therefore when dividing complex numbers, their magnitudes divide, but their phases
subtract.

TOC
CHAPTER 15. FREQUENCY RESPONSE 261

A transfer function H(s) is a complex number for any specific value of s.

Therefore H(s) can be expressed in both rectangular and polar forms as

H(s) = <{H(s)} + j={H(s)} = |H(s)| ej∠H(s) .

1
For example, if H(s) = s+1 , then when s = 2 + j3

1 1 1 3 − j3
H(2 + j3) = = =
2 + j3 + 1 3 + j3 3 + j3 3 − j3
r
1 1 1 −j π
= −j = e 4.
6 6 18
If H(s) has multiple poles and zeros, then the magnitude and phase of H(s) can
be represented in terms of the magnitude and phase of the poles and zeros. For
example, suppose that
K(s + z1 )(s + z2 ) . . . (s + zm )
H(s) = .
(s + p1 )(s + p2 ) . . . (s + pn )
Since (s + a) is a complex number for any s, it can be written in polar form as
s + a = |s + a| ej∠(s+a) , therefore
K(s + z1 )(s + z2 ) . . . (s + zm )
H(s) =
(s + p1 )(s + p2 ) . . . (s + pn )
|K| ej∠K |s + z1 | ej∠(s+z1 ) |s + z2 | ej∠(s+z2 ) . . . |s + zm | ej∠(s+zm )
=
|s + p1 | ej∠(s+p1 ) |s + p2 | ej∠(s+p2 ) . . . |s + pn | ej∠(s+pn )
|K| |s + z1 | |s + z2 | . . . |s + zm | ej∠K ej∠(s+z1 ) ej∠(s+z2 ) . . . ej∠(s+zm )
=
|s + p1 | |s + p2 | . . . |s + pn | ej∠(s+p1 ) ej∠(s+p2 ) . . . ej∠(s+pn )
|K| |s + z1 | |s + z2 | . . . |s + zm | j (∠K+Pm i=1 ∠(s+zi )−
Pn
i=1 ∠(s+pi )) .
= e
|s + p1 | |s + p2 | . . . |s + pn |
Therefore
|K| |s + z1 | |s + z2 | . . . |s + zm |
|H(s)| = (15.2)
|s + p1 | |s + p2 | . . . |s + pn |
Xm Xn
∠H(s) = ∠K + ∠(s + zi ) − ∠(s + pi ). (15.3)
i=1 i=1

15.1.2 Frequency Response of LTI Systems

In systems theory, the magnitude and phase representations of the transfer func-
tion H(s) are used because they have important physical meaning. In particular,
suppose that H(s) represents a physical system with input u(t) and output y(t) as
shown in Fig. 15-2. If the input to the system is given by a sinusoid of magnitude
A and frequency ω0 , i.e.,
u(t) = A sin(ω0 t),

TOC
CHAPTER 15. FREQUENCY RESPONSE 262

u(t) y(t)
H(s)

Figure 15-2: LTI system represented by the transfer function H(s) with input u(t)
and output y(t).

then the output is given by

y(t) = A |H(jω0 )| sin(ω0 t + ∠H(jω0 )). (15.4)

Therefore, the transfer function H(s) is an elegant and compact representation

of the frequency response of the system, in that the magnitude of H(jω0 ) de-
scribes the gain of the system for input signals with frequency ω0 , and the phase
of H(jω0 ) describes the phase shift imposed by the system on input signals with
frequency ω0 . For example, Fig. 15-3 shows the input-output response of an LTI
1
system with transfer function H(s) = s+1 . The input signal, which is shown in
grey, is u(t) = sin(ω0 t) where ω0 changes in each subplot. The output signal is
shown in blue and is given by Equation (15.4). Note that in subplot (a) the time
◦
scale is different than the other subplots. When ω0 = 0.1, H(jω0 ) ≈ 0.995ej5 .
Accordingly, in subplot (a), the output is only slightly attenuated, with very little
◦
phase shift. In contrast, when ω0 = 5, H(jω0 ) ≈ 0.2ej80 , and in subplot (f) we
see that the output is approximately 20% of the input with a phase shift approach-
ing 90 degrees.
Therefore, the response of an LTI system to inputs at different frequencies can
be visualized by plotting the magnitude and phase of H(jω) as a function of ω.
The frequency response of H(s) = 1/(s + 1) is plotted in two different ways in
Fig. 15-4. In subfigure (a) a linear scale is used for both |H(jω)| and ∠H(jω), as
well as the frequency scale ω. In subfigure (b), the scale for magnitude is in dB:
20 log |H(jω)| whereas the scale for ∠H(jω) is linear, however, in both cases a
log scale for ω is used on the x-axis. The plot in subfigure (b) is called a Bode plot.
It should be clear from Fig. 15-4 that the Bode plot provides much more useful
information than a linear plot since it more effectively reveals the behavior of the
system at lower frequencies. For this reason, Bode plots are the most common
method to display the frequency response of the system.
As a note, since the magnitude of H(jω) is displayed on the Bode plot as
20 log |H(jω)| in units of dB, note that 20 log |H(jω)| = 0 dB implies that
|H(jω)| = 1. Similarly 20 log |H(jω)| = 20 dB implies that |H(jω)| = 10 and
20 log |H(jω)| = 40 dB implies that |H(jω)| = 100. Going in the other direc-
tion, 20 log |H(jω)| = −20 dB implies that |H(jω)| = 0.1, and 20 log |H(jω)| =
−40 dB implies that |H(jω)| = 0.01. In general,

20 log |H(jω)| = 20n dB =⇒ |H(jω)| = 10n .

TOC
CHAPTER 15. FREQUENCY RESPONSE 263

ω 0 =0.1 ω 0 =0.5
1 1

0.5 0.5
Amplitude

Amplitude
0 0

-0.5 -0.5

-1 -1
0 50 100 150 200 0 5 10 15 20
(a) (seconds) (b) (seconds)

ω 0 =1 ω 0 =2
1 1

0.5 0.5
Amplitude

Amplitude
0 0

-0.5 -0.5

-1 -1
0 5 10 15 20 0 5 10 15 20
(c) (seconds) (d) (seconds)

ω 0 =3 ω 0 =5
1 1

0.5 0.5
Amplitude

Amplitude

0 0

-0.5 -0.5

-1 -1
0 5 10 15 20 0 5 10 15 20
(e) (seconds) (f) (seconds)

Figure 15-3: Frequency response to LTI system with transfer function H(s) =
1/(s + 1).

Bode Diagram
1
0

0.8
-10
Magnitude (dB)

0.6
|H(jω)|

0.4
-20

0.2
-30
0
0 10 20 30 40 50 60 70 80 90 100
ω
-40
0
0

-20
Phase (deg)
H(jω)

-40
-45
-60

-80

-100 -90
0 10 20 30 40 50 60 70 80 90 100
10 -2 10 -1 10 0 10 1 10 2
ω Frequency (rad/s)

(a) (b)

Figure 15-4: Frequency response of H(s) = 1/(s + 1) on a linear scale (a), and a
logarithmic scale (b).

TOC
CHAPTER 15. FREQUENCY RESPONSE 264

15.1.3 Straight-line Approximations for Bode Plots

While the Bode plot for a system can be easily generated using the Matlab or
Python bode command, it is still useful to understand how to approximately draw
a Bode plot by hand. The reason for this is that in the loopshaping design method-
ology, we add elements to the control transfer function C(s) to "shape" the Bode
plot of the loop gain P (s)C(s) where P (s) is the transfer function of the plant.
Learning to approximately draw the Bode plot by hand will provide helpful in-
sights into this process. Therefore, in this section we provide a brief tutorial on
how to approximate the Bode plot for a transfer function H(s).
We start by putting the transfer function into Bode canonical form by factoring
out the poles and zeros as follows:

K(s + z1 )(s + z2 ) . . . (s + zm )
H(s) =
(s + p1 )(s + p2 ) . . . (s + pn )
Kz1 z2 . . . zm (1 + s/z1 )(1 + s/z2 ) . . . (1 + s/zm )
= .
p1 p2 . . . pn (1 + s/p1 )(1 + s/p2 ) . . . (1 + s/pn )
Therefore, following Equations (15.2) and (15.3) we have
 
 Kz1 z2 . . . zm  |1 + jω/z1 | |1 + jω/z2 | . . . |1 + jω/zm |
|H(jω)| =   (15.5)
p1 p2 . . . pm  |1 + jω/p1 | |1 + jω/p2 | . . . |1 + jω/pn )|
  X m n
Kz1 z2 . . . zm
∠H(jω) = ∠ ∠(1 + jω/zi ) − ∠(1 + jω/pi ).
X
+ (15.6)
p1 p2 . . . pm i=1 i=1

Since 20 log |AB| = 20 log |A| + 20 log |B|, Equation (15.5) gives
 
 Kz1 z2 . . . zm 

20 log |H(jω)| = 20 log  
p1 p2 . . . pm 
m
X n
X
+ 20 log |1 + jω/zi | − 20 log |1 + jω/pi | . (15.7)
i=1 i=1

Drawing the Bode plot for a general transfer function with real poles and zeros,
can be decomposed into drawing the Bode plot for each pole and zero, and then
graphically adding them to get the general Bode plot. Note first that
 
 Kz1 z2 . . . zm 
20 log  
p1 p2 . . . pm 
is not a function of ω and is therefore a constant line on the Bode plot. Also note
that  
Kz1 z2 . . . zm
∠
p1 p2 . . . pm
 
is also a constant and is either 0 degrees if Kz 1 z2 ...zm
p1 p2 ...pm > 0, or 180 degrees if
 
Kz1 z2 ...zm
p1 p2 ...pm < 0.

TOC
CHAPTER 15. FREQUENCY RESPONSE 265

We start by drawing the Bode plot when H(s) = s + z has a single zero at
s = −z and z > 0. The first step is to put the transfer function in Bode canonical
form as
ω
H(jω) = z(1 + j ).
z
Therefore, from Equation (15.7) we have
 ω 

20 log |H(jω)| = 20 log |z| + 20 log 1 + j 
p z
= 20 log |z| + 20 log 1 + (ω/z)2 .
p
When ω/z  1 we have that 1 + (ω/z)2 ≈ 1, implying that

20 log |H(jω)| ≈ 20 log |z| .

Note that since this term is not a function of ω, it is a constant

p value on the Bode
plot. On the other hand, when ω/z  1 we have that 1 + (ω/z)2 ≈ |ω/z|,
giving
20 log |H(jω)| ≈ 20 log |z| + 20 log |ω/z| .
Note that the second term is a straight line on a Bode plot that intersects the 0 dB
axis at ω = z and increases 20 dB for every factor of 10 increase in ω. Therefore,
on the Bode plot, the slope of the second term is +20 dB/decade. A straight-
line approximation is shown in Fig. 15-5, along with the Bode plot produced by
Matlab. Note that the largest mismatch between the straight-line approximation

Bode Diagram
50

40
20 log10 2 + 20 log10 (!/2)
Magnitude (dB)

30

20
20 log10 2 = 6.02 slope is 20 dB/decade
10

0
90
Phase (deg)

45

0
10 -1 z/10 10 0 z=2 10 1 10z 10 2
Frequency (rad/s)

Figure 15-5: Straight-line approximation for a single zero.

and the actual Bode plot is at ω = z. In that case we have

√
20 log |H(jz)| ≈ 20 log |z| + 20 log 2 = 20 log |z| + 3 dB.

TOC
CHAPTER 15. FREQUENCY RESPONSE 266

Therefore, at the corner frequency z the Bode plot is 3 dB above the low frequency
response.
To approximate the phase plot, note that
ω
∠H(jω) = ∠z(1 + jω/z) = tan−1 . (15.8)
z
When ω = z we have that tan−1 1 = 45 degrees. When ω  z, we have
that tan−1 (ω/z) ≈ 0 degrees, and when ω  z, we have that tan−1 (ω/z) ≈
90 degrees. The phase plot as calculated by Matlab is shown in Fig. 15-5. An
adequate straight-line approximation is obtained by setting the phase to zero when
ω < z/10, i.e., one decade less than z, and setting the phase to 90 degrees when
ω > 10z, i.e., one decade greater than z, and drawing a straight line between those
points so that the phase at z is 45 degrees. The straight-line approximation for the
phase is also shown in black in Fig. 15-5.
The straight-line approximation for phase as described above assumes that
z > 0, i.e, that the zero is in the left half of the complex plane. For right half
plane zeros when z < 0, the phase as calculated in Equation (15.6) is
ω
∠H(jω) = ∠z + ∠(1 + jω/z) = 180◦ − tan−1 .
|z|

Therefore, the phase starts at 180 degrees and decreases by 90 degrees as ω gets
large rather than increases. Fig. 15-6 shows the Bode plot and its straight-line
approximation when the zero is in the right half plane. When a system has a zero
in the right half plane, it is called non-minimum phase because of the 180 degree
phase increase at low frequencies.

Bode Diagram
50

40 20 log10 2 + 20 log10 (!/2)

Magnitude (dB)

30

20
20 log10 2 = 6.02 slope is 20 dB/decade
10

0
180
Phase (deg)

135

90
10 -1 z/10 10 0 z=2 10 1 10z 10 2
Frequency (rad/s)

Figure 15-6: Straight-line approximation for a zero in the right half of the complex
plane.

TOC
CHAPTER 15. FREQUENCY RESPONSE 267

p
Now consider drawing the Bode plot for a single pole, i.e., H(s) = s+p ,
which has a single pole at s = −p which is in the left half plane when p > 0. In
Bode canonical form H(s) becomes

1
H(jω) = .
1 + j ωp

Therefore, from Equation (15.7) we have

 
 ω
20 log |H(jω)| = 20 log |1| − 20 log 1 + j 
p
p
= −20 log 1 + (ω/p)2 .
p
When ω/p  1, we have that 1 + (ω/p)2 ≈ 1, implying that

20 log |H(jω)| ≈ 0.

Note that since this term is not a function of ω, it is a constant value

p at 0 dB on
the Bode plot. On the other hand, when ω/p  1, we have that 1 + (ω/p)2 ≈
|ω/p|, giving
20 log |H(jω)| ≈ −20 log |ω/p| .
Note that the second term is a straight line on a Bode plot that intersects the 0 dB
axis when ω = p and decreases at −20 dB for every factor of 10 increase in
ω. Therefore, on the Bode plot, the slope of the second term is −20 dB/decade.
A straight-line approximation is shown in Fig. 15-7, along with the Bode plot
produced by Matlab. Note that the largest mismatch between the straight-line

20 log10 1 = 0 dB
Bode Diagram
0

-5

-10 slope is 20 dB/decade

Magnitude (dB)

-15
20 log10 (!/2)
-20

-25

-30

-35
0
Phase (deg)

-45

-90
10 -1 10 0 10 1 10 2
Frequency (rad/s)
p/10 10p
p=2

Figure 15-7: Straight-line approximation for a single pole.

TOC
CHAPTER 15. FREQUENCY RESPONSE 268

approximation and the actual Bode is at ω = p. In that case, we have

√
20 log |H(jp)| ≈ −20 log 2 = −3 dB.

Therefore, at the corner frequency p the Bode plot is 3 dB below the low frequency
response.
To approximate the phase plot, note that
ω ω
∠H(jω) = −∠(1 + j ) = − tan−1 . (15.9)
p p

When ω = p we have that − tan−1 1 = −45 degrees. When ω  p, we have

that − tan−1 (ω/p) ≈ 0 degrees, and when ω  p, − tan−1 (ω/p) ≈ −90 de-
grees. The phase plot as calculated by Matlab is shown in Fig. 15-7. An ade-
quate straight-line approximation is obtained by setting the phase to zero when
ω < p/10, i.e., one decade less than p, and setting the phase to −90 degrees when
ω > 10p, i.e., one decade greater than p, and drawing a straight line between those
points so that the phase at p is −45 degrees. The straight-line approximation for
the phase is also shown in Fig. 15-7.
The straight-line approximation for phase as described above assumes that
p > 0, i.e, that the pole is in the left half of the complex plane. For right half
plane poles when p < 0, the phase as calculated in Equation (15.6) is
ω
∠H(jω) = −∠(1 + jω/p) = tan−1 .
|p|

Therefore, the phase starts at 0 degrees and increases by 90 degrees as ω gets

large. Fig. 15-8 shows the Bode plot and its straight-line approximation when the
pole is in the right half plane. Note that stability and instability are not directly
evident from the shapes of the magnitude and phase plots.
Suppose now that the poles are complex. Let

ωn2
H(s) = .
s2 + 2ζωn s + ωn2

In Bode canonical form we have

1 ωn2
H(jω) =  
ωn2 (jω)2
2 + 2ζωn jω
2 +1
ωn ωn
1
=  2   
1 − ωωn + j2ζ ωωn
 
2ζ ω
ωn
−j tan−1 
1 2

1− ω2
= r 2 e ωn
.
ω2 2
1− 2
ωn + 4ζ 2 ωω2
n

TOC
CHAPTER 15. FREQUENCY RESPONSE 269

20 log10 1 = 0 dB
Bode Diagram
0

-5

-10 slope is 20 dB/decade

Magnitude (dB)
-15
20 log10 (!/2)
-20

-25

-30

-35
90
Phase (deg)

45

0
10 -1 10 0 10 1 10 2
Frequency (rad/s)
p/10 10p
p=2

Figure 15-8: Straight-line approximation for a pole in the right half of the complex
plane.

Therefore
1
|H(jω)| = r 2
ω2 2
1− 2
ωn + 4ζ 2 ωω2
n

ω
!
2ζ ωn
∠H(jω) = − tan−1 ω2
.
1− 2
ωn

When ω  ωn we have that 20 log10 |H(jω)| ≈ 20 log10 |1| = 0 dB. Also,

∠H(jω) ≈ − tan−1 01 = 0 degrees. Alternatively, when ω  ωn we have
s 
2
ω2 2
ω2
20 log10 |H(jω)| ≈ −20 log + 4ζ
ωn2 ωn2
s 
2
ω2
≈ −20 log
ωn2
 2
ω 
≈ −20 log  
ωn
 
ω 
= −40 log   ,
ωn
which is a straight line that crosses the 0 dB axis at ωn withpa slope of −40 dB/decade.
When ω = ωn we have 20 log 10 |H(jω)| = −20 log 10 4ζ 2 = −20 log10 |2ζ|.
√
2
√
When ζ = 0.707 = 2 , then 20 log10 |H(jω)| = −20 log10 1/ 2 = −3 dB. For

TOC
CHAPTER 15. FREQUENCY RESPONSE 270

the angle plot, when ω  ωn we have ∠H(jω)
≈ tan−1 01 = 0 degrees. When

ω  ωn we have ∠H(jω) ≈ tan−1 2ζ ωωn ≈ −180 degrees, and when ω = ωn
we have ∠H(jω) ≈ − tan−1 2ζ 0 = −90 degrees.
The Bode plot for different values of ζ are shown in Fig. 15-9

Bode Diagram
20
⇣ = 0.1
0 ⇣ = 0.4
⇣ = 0.707
Magnitude (dB)

-20
⇣=1
⇣=2
-40

-60

-80
0
⇣ = 0.1
-45 ⇣=1 ⇣ = 0.4
⇣ = 0.707
Phase (deg)

⇣=2
-90

-135

-180
!-2n !n-1 10 0 1 2
10 10 !n
Frequency (rad/s)
10
10! n
10
100! n
100 10
Frequency (rad/s)

Figure 15-9: The Bode plot for a second order system for various values of ζ.

15.1.4 Example
Find the straight-line approximation for the Bode plot of the transfer function
20
H(s) = . (15.10)
s(s + 10)
In Bode canonical form we have
20 1
H(jω) = ω .
10 jω(1 + j 10 )

Therefore

20 log10 |H(jω)| =
r  ω 2
20 log10 2 − 20 log10 |ω| − 20 log10 1+ . (15.11)
10
Note that the term due to the pole at zero, namely −20 log10 |ω| is a straight line
that intercepts the 0 dB line at ω = 1, and has a slope of −20 dB per decade.
Therefore, the Bode plot for magnitude will be the graphical addition of three
terms:
1. A constant term at 20 log10 2 = 2 dB,

TOC
CHAPTER 15. FREQUENCY RESPONSE 271

2. A straight line with slope of −20 dB/decade, passing through the 0 dB line
at ω = 1, and

3. A first order low-pass filter with a cut-off frequency at ω = 10.

Similarly, the phase is given by

ω
∠H(jω) = ∠2 − ∠jω − ∠(1 + j )
10
ω ω
= 0 − tan−1 − tan−1
0 10
◦ −1 ω
= −90 − tan .
10

The straight-line approximation as well as the Bode plot generated by Matlab are
shown in Fig. 15-10.

Bode Diagram
40

20
6 = 20 log10 2
0

20 log10 |1 + j!/10|
Magnitude (dB)

-20
20 log10 |!|
-40

-60

-80

-100
0

\(1 + j!/10)
-45

\j!
Phase (deg)

-90

-135

-180
10 -1 10 0 10 1 10 2 10 3
Frequency (rad/s)

Figure 15-10: Bode plot for the transfer function given in Equation (15.10).

15.1.5 Example
Find the straight-line approximation for the Bode plot of the transfer function

8000(s + 1)(s + 10)

H(s) = . (15.12)
(s + 2)(s + 20)(s + 200)

TOC
CHAPTER 15. FREQUENCY RESPONSE 272

In Bode canonical form we have

ω
8000 · 10 (1 + jω)(1 + j 10 )
H(jω) = ω ω ω
2 · 20 · 200 (1 + j 2 )(1 + j 20 )(1 + j 200 )
ω
(1 + jω)(1 + j 10 )
= 10 ω .
(1 + j ω2 )(1 + j 20
ω
)(1 + j 200 )

Therefore

20 log10 |H(jω)| = 20 log10 10 + 20 log10 |1 + jω| +

 ω   ω 
 
20 log10 1 + j  − 20 log10 1 + j 
10
 2
 ω   ω 
− 20 log10 1 + j  − 20 log10 1 + j  . (15.13)
20 200

Therefore, the Bode plot for magnitude will be the graphical addition of a constant
gain, two zeros, and three poles. Similarly, the phase is given by

ω
∠H(jω) = ∠10 + ∠(1 + jω) + ∠(1 + j )
10
ω ω ω
− ∠(1 + j ) − ∠(1 + j ) − ∠(1 + j ).
2 20 200

The straight-line approximation as well as the Bode plot generated by Matlab are
shown in Fig. 15-11.

Bode Diagram

40
20 log10 |1 + j!|
30
20 log10 10 20 log10 |1 + j!/10|
20
Magnitude (dB)

10

-10 20 log10 |1 + j!/2|

-20 20 log10 |1 + j!/20|
20 log10 |1 + j!/200|
-30

90
\(1 + j!)
\(1 + j!/10)
45 \(1 + j!/2)

\(1 + j!/20)
Phase (deg)

\(1 + j!/200)
-45

-90

10 -1 10 0 10 1 10 2 10 3 10 4
Frequency (rad/s)

Figure 15-11: Bode plot for the transfer function given in Equation (15.12).

TOC
 CHAPTER 15. FREQUENCY RESPONSE 273

15.2 Design Study A. Single Link Robot Arm

Example Problem A.15

Draw by hand the Bode plot of the single link robot arm from torque τ̃ to angle θ̃
given that the equilibrium angle is θe = 0. Use the bode command (from Matlab
or Python) and compare your results.

Solution
From HW A.5, the transfer function for the single link robot arm is

3/m`2 44.44
P (s) = 2
= . (15.14)
s(s + 3b/m` ) s(s + 0.4444)
In Bode canonical form we have
100
P (jω) = ω
(jω)(1 + j 0.4444 )

Therefore

20 log10 |P (jω)| = 20 log10 100

 ω 

− 20 log10 |jω| − 20 log10 1 + j  . (15.15)
0.4444
Therefore, the Bode plot for magnitude will be the graphical addition of a constant
gain, an integrator, and a pole. Similarly, the phase is given by
ω
∠P (jω) = ∠100 − ∠(jω) − ∠(1 + j ).
0.4444

Python commands to generate the Bode plots are:

1 # Single link arm Parameter File
2 import sys
3 sys.path.append(’..’) # add parent directory
4 import armParam as P
5 from control import tf, bode
6 import matplotlib.pyplot as plt
7
8 # flag to define if using dB or absolute scale for M(omega)
9 dB_flag = True
10
11 # Compute plant transfer functions
12 th_e = 0
13 Plant = tf([3.0/P.m/P.ell**2],
14 [1, 3.0*P.b/P.m/P.ell**2, 0.0])
15

TOC
 CHAPTER 15. FREQUENCY RESPONSE 274

Bode Diagram
100

80
Magnitude (dB)

60
20 log10 100 = 40 dB
20 log10 |P (j!)|
40

20
20 log10 |!|
0
20 log10 |1 + j!/0.4444|
-20
0

\(1 + j!/0.4444)
-45

\j!
Phase (deg)

-90

-135
\P (j!)

-180
10 -2 10 -1 ! = 0.4444 10 0 10 1
Frequency (rad/s)

Figure 15-12: Bode plot for the transfer function given in Equation (15.14).

16
17 if __name__=="__main__":
18
19 # Bode plot for the plant
20 fig = plt.figure()
21 bode(Plant, dB=dB_flag, margins=False)
22 fig.axes[0].set_title(’P(s) for arm’)
23
24 # if you want specific values at specific frequencies, you can
25 # do the following (but the magnitudes are absolute, not dB)
26 mag, phase, omega = bode(Plant, plot=False,
27 omega = [0.3, 10.0, 1000.0])
28
29 print(’Close window to end program’)
30 plt.show()

Listing 15.1: armParamHW15.py

TOC
CHAPTER 15. FREQUENCY RESPONSE 275

15.3 Design Study B. Inverted Pendulum

Example Problem B.15

(a) Draw by hand the Bode plot of the inner loop transfer function from force
F to angle θ for the inverted pendulum. Use the bode command (from
Matlab or Python) and compare your results.
(b) Draw by hand the Bode plot of the outer loop transfer function from angle
θ to position z for the inverted pendulum. Use the bode (from Matlab or
Python) command and compare your results.

Solution
From HW B.5, the transfer function for the inner loop of the inverted pendulum is
−1
(m1 6` +m2 2`
3 )
−2.823 −2.823
Pin (s) = = = .
s2 − (m (m1 +m2 )g
` 2`
s2 − 34.58 (s + 5.881)(s − 5.881)
1 6 +m2 3 )
(15.16)
In Bode canonical form we have
0.0816
Pin (jω) = ω ω
(1 + j 5.881 )(1 − j 5.881 )

Therefore

20 log10 |Pin (jω)| = 20 log10 0.0816

 ω   ω 
 
− 20 log10 1 + j  − 20 log10 1 − j  . (15.17)
5.881 5.881
Therefore, the Bode plot for magnitude will be the graphical addition of a constant
gain, a right half plane pole, and a left half plane pole. Similarly, the phase is given
by
ω ω
∠Pin (jω) = ∠0.0816 − ∠(1 + j ) − ∠(1 − j ).
5.881 5.881
The straight-line approximation as well as the Bode plot generated by Matlab are
shown in Fig. 15-13.
!
− 2`
3 s 2
+ g
Z̃(s) = Θ̃(s).
s2
From HW B.5, the transfer function for the outer loop of the inverted pendu-
lum is
2` 2 2 g3 2
3 s − g `(s2 − ` 2) (s − 3.83)(s + 3.83)
Pout (s) = − =−3 =−3 . (15.18)
s2 s2 s2

TOC
 CHAPTER 15. FREQUENCY RESPONSE 276

Bode Diagram
10
20 log10 |1 + j!/7|
0
20 log10 |1 j!/7|
-10
Magnitude (dB)

-20

-30

-40
20 log10 |0.0816|
-50

-60

-70
90

45 \(1 j!/7)
Phase (deg)

-45 \(1 + j!/7)

-90
10 -1 10 0 10 1 10 2
Frequency (rad/s)

Figure 15-13: Bode plot for the transfer function given in Equation (15.17).

In Bode canonical form we have

0.174(jω/3.83 + 1)(−jω/3.83 + 1)
Pout (jω) = .
(jω)2
Therefore

20 log10 |Pout (jω)| =

 ω 

20 log10 0.174 + 20 log10 1 + j 
 3.83
 ω  2
+ 20 log10 1 + j − 20 log10 |ω| . (15.19)
−3.83 
Similarly, the phase is given by
ω ω
∠Pout (jω) = ∠0.174 + ∠(1 + j ) + ∠(1 − j ) − ∠(jω) − ∠(jω).
3.83 3.83
The Bode plot generated by Python is shown in Fig. 15-14.
Python code to generate Bode plots for the previous two figures is as follows:
1 # Inverted Pendulum Parameter File
2 import sys
3 sys.path.append(’..’) # add parent directory
4 import pendulumParam as P
5 #from control.matlab import *
6 from control import tf, bode
7 import matplotlib.pyplot as plt

TOC
 CHAPTER 15. FREQUENCY RESPONSE 277

Pout(s)
Magnitude (dB) 60
40
20
0
10 1 100 101 102
Phase (deg)

180

10 1 100 101 102

Frequency (rad/sec)

Figure 15-14: Bode plot for the transfer function given in Equation (15.19).

8
9 # flag to define if using dB or absolute scale for M(omega)
10 dB_flag = True
11
12 # Compute inner and outer open-loop transfer functions
13 temp = (P.m1*P.ell/6.0+P.m2*2*P.ell/3.0)
14 P_in = tf([-1/temp],
15 [1, 0, -(P.m1+P.m2)*P.g/temp])
16 P_out = tf([-2*P.ell/3.0, 0, P.g], [1, 0, 0])
17
18 if __name__=="__main__":
19
20 # Plot the open loop bode plots for the inner loop
21 fig1 = plt.figure()
22 bode(P_in, dB=dB_flag)
23 fig1.axes[0].set_title(’$P_{in}(s)$’)
24
25 fig2 = plt.figure()
26 bode(P_out, dB=dB_flag)
27 fig2.axes[0].set_title(’$P_{out}(s)$’)
28
29 print(’Close window to end program’)
30 plt.show()

Listing 15.2: pendulumParamHW15.py

TOC
CHAPTER 15. FREQUENCY RESPONSE 278

15.4 Design Study C. Satellite Attitude Control

Example Problem C.15

(a) Draw by hand the Bode plot of the inner loop transfer function from torque τ
to angle θ for the satellite attitude problem. Use the bode command (from
Matlab or Python) and compare your results.

(b) Draw by hand the Bode plot of the outer loop transfer function from body
angle θ to panel angle φ for the satellite attitude problem. Use the bode
command (from Python or Matlab) and compare your results.

Solution
From HW C.5, the transfer function for the inner loop of the satellite attitude
problem is
1 1
Pin (s) = 2
= 2. (15.20)
(Js + Jp )s 6s
In Bode canonical form we have
1 1
Pin (jω) = .
6 (jω)(jω)

Therefore,

20 log10 |Pin (jω)| = 20 log10 1/6 − 20 log10 |ω| − 20 log10 |ω| (15.21)

Therefore, the Bode plot for magnitude will be the graphical addition of a constant
gain and two straight lines passing through the 0 dB line at ω = 1 with a slope of
-20 dB/decade. Similarly, the phase is given by

∠Pin (jω) = ∠1/6 − ∠jω − ∠jω.

The Bode plot generated by Matlab is shown in Fig. 15-15. As can be seen from
the figure, the straight-line approximation corresponds directly with the Bode plot
in this case.
From HW C.5, the transfer function for the outer loop of the satellite is
b k
Jp s + Jp 0.05s + 0.15
Pout (s) = = . (15.22)
s2 + Jbp s + Jkp s2 + 0.05s + 0.15

In Bode canonical form we have

1 + j ω3
Pout (jω) =
2
1 + j ω3 + j 0.3873
ω

TOC
 CHAPTER 15. FREQUENCY RESPONSE 279

Figure 15-15: Bode plot for the transfer function given in Equation (15.21).

Therefore

20 log10 |Pout (jω)| = 20 log10 |1 + jω/3|

  
 ω 2 
− 20 log10 1 + jω/3 + j (15.23)
0.3873 
Therefore, the Bode plot for magnitude will be the graphical addition of a zero,
and a complex pole. Similarly, the phase is given by
  
ω 2
∠Pout (jω) = ∠ (1 + jω/3) − ∠ 1 + jω/3 + j .
0.3873
The straight-line approximation as well as the Bode plot generated by Matlab are
shown in Fig. 15-16.
The Python code to generate similar Bode plots is
1 # Inverted Pendulum Parameter File
2 import sys
3 sys.path.append(’..’) # add parent directory
4 import satelliteParam as P
5 from control import tf, bode
6 import matplotlib.pyplot as plt
7
8 # flag to define if using dB or absolute scale for M(omega)
9 dB_flag = True
10
11 # Compute inner and outer open-loop transfer functions
12 P_in = tf([1],[P.Js+P.Jp,0,0])
13 P_out = tf([P.b/P.Jp, P.k/P.Jp], [1, P.b/P.Jp, P.k/P.Jp])
14

TOC
 CHAPTER 15. FREQUENCY RESPONSE 280

Bode Diagram
40

20
Magnitude (dB)

0 20 log10 |1 + j!/3|

-20

-40
20 log10 |1 + (j!/.3873)2 |
-60

-80
90
\(1 + j!/3)
45
Phase (deg)

-45

-90

-135
\(1 + (j!/0.3873)2 )
-180
10 -2 10 -1 10 0 10 1 10 2
Frequency (rad/s)

Figure 15-16: Bode plot for the transfer function given in Equation (15.23).

15 if __name__=="__main__":
16
17 # Plot the open loop bode plots for the inner loop
18 fig1 = plt.figure()
19 bode(P_in, dB=dB_flag)
20 fig1.axes[0].set_title(’$P_{in}(s)$’)
21
22 fig2 = plt.figure()
23 bode(P_out, dB=dB_flag)
24 fig2.axes[0].set_title(’$P_{out}(s)$’)
25
26 print(’Close window to end program’)
27 plt.show()

Listing 15.3: satelliteParamHW15.py

Notes and References

A good introduction to frequency response for linear time-invariant system is [16].
A good overview of the Bode plot and straight-line approximations to the Bode
plot is contained in [4] and many other introductory textbooks on feedback con-
trol.

TOC
 Frequency Domain Specifications
16
Learning Objectives:
• Use the open-loop gain at low frequencies to evaluate a system’s abil-
ity to track reference signals and reject output disturbances.
• Use the open-loop gain at high frequencies to evaluate a system’s
ability to attenuate noise.
• Use the open-loop gain and plant at low frequencies to evaluate a
system’s ability to reject input disturbances.
• Identify a system’s type based upon the open-loop gain.

16.1 Theory

In this chapter we show how the magnitude of the frequency response of the open
loop system can be used to determine the performance of the closed-loop feed-
back system. A general feedback loop is shown in Fig. 16-1. In this feedback
system, r(t) is the reference input to be tracked by y(t), din (t) is an unknown in-
put disturbance, dout is an unknown output disturbance, and n(t) is an unknown
noise signal. Note that because of the presence of n, the error signal e = r − y is
no longer the input to C(s). The input to C(s) is now r − y − n. Note that this
is a different definition than was used in Chapter 9 and will result in a different
transfer function between the error and the noise. To find the transfer function
from the inputs r, din , dout , and n to the error e(t) we start by finding the transfer
functions to y. Starting at Y (s) and following the loop backward until returning

TOC
CHAPTER 16. FREQUENCY SPECIFICATIONS 282

to Y we get
Y (s) = −Dout (s) + P (−Din (s) + C (R(s) − N (s) − Y (s)))
=⇒ (1 + P C)Y (s) = P CR(s) − P CN (s) − Dout (s) − P Din (s)
PC PC 1 P
Y (s) = R(s) − N (s) − Dout (s) − Din (s).
1 + PC 1 + PC 1 + PC 1 + PC
(16.1)

4
Defining the error to be E(s) = R(s) − Y (s) we get

E(s) = R(s) − Y (s) (16.2)

1 PC 1 P
= R(s) + N (s) + Dout (s) + Din (s),
1 + PC 1 + PC 1 + PC 1 + PC
(16.3)

Therefore,
1
1. The transfer function from the reference r to the error e is 1+P (s)C(s) ,

P (s)C(s)
2. The transfer function from the noise n to the error e is 1+P (s)C(s) ,

3. The transfer function from the output disturbance dout to the error e is
1
1+P (s)C(s) , and

P (s)
4. The transfer function from the input disturbance din to the error e is 1+P (s)C(s) .

din (t) dout (t)

u(t) y(t)
r(t)
C(s) P (s)

n(t)

Figure 16-1: General feedback loop.

For typical applications, we have that

1. The reference signal r(t) is a low frequency signal with frequency content
below ωr ,

2. The noise signal n(t) is a high frequency signal with frequency content
above ωno ,

3. The output disturbance signal dout (t) is a low frequency signal with fre-
quency content below ωdout ,

TOC
CHAPTER 16. FREQUENCY SPECIFICATIONS 283

4. The input disturbance signal din (t) is a low frequency signal with frequency
content below ωdin .

The objective for the control design is to find C(s) so that the following spec-
ifications are satisfied:

1. Tracking: Track reference signals r(t) with frequency content below ωr so

that the error due to the reference signal satisfies

|e(t)| ≤ γr |r(t)| ,

2. Noise Attenuation: Attenuate noise signals n(t) with frequency content

above ωno so that the error due to the noise signal satisfies

|e(t)| ≤ γn |n(t)| ,

3. Reject Output Disturbances: Reject output disturbance signals dout (t)

with frequency content below ωdout so that the error due to the output dis-
turbance signal satisfies

|e(t)| ≤ γdout |dout (t)| ,

4. Reject Input Disturbances: Reject input disturbance signals din (t) with
frequency content below ωdin so that the error due to the input disturbance
signal satisfies
|e(t)| ≤ γdin |din (t)| .

The objective in the remainder of this chapter is to show how each of these speci-
fications can be translated into a requirement on the magnitude of the (open) loop
gain 20 log10 |P (jω)C(jω)|.

16.1.1 Tracking
The transfer function from the reference signal r(t) to the error e(t) is

1
E(s) = R(s).
1 + P (s)C(s)

Therefore,  
 1 
|E(jω)| ≤   |R(jω)| .
1 + P (jω)C(jω) 
The objective is to track reference signals r(t) with frequency content below ωr
so that the error due to the reference signal satisfies

|e(t)| ≤ γr |r(t)| .

TOC
CHAPTER 16. FREQUENCY SPECIFICATIONS 284

Therefore, the controller C(s) needs to be designed so that

 
 1 
 
 1 + P (jω)C(jω)  ≤ γr ,

or equivalently so that
1
|1 + P (jω)C(jω)| ≥ .
γr
If |P (jω)C(jω)|  1, then a sufficient condition is that
1
|P (jω)C(jω)| ≥ ,
γr
or equivalently
20 log10 |P (jω)C(jω)| ≥ 20 log10 1/γr , (16.4)
which needs to be satisfied for all ω ≤ ωr . On a Bode plot, the constraint repre-
sented by Equation (16.4) is shown graphically in Fig. 16-2.

Bode Diagram
60

20 log10 |1/ r | 40
20 log10 |P (j!)C(⌘!)|
20
Magnitude (dB)

-20

-40

-60

-80
-90
!r

Figure 16-2: Bode plot representation of the loopshaping constraint corresponding

to tracking r(t) so that the corresponding error satisfies |e(t)| ≤ γr |r(t)|.
Phase (deg)

-135

Alternatively, from the Bode plot of P C we can compute the tracking charac-
teristics of the closed-loop system. For example, suppose that on the Bode plot
-180
10 -2 10 -1 10 0 10 1 10 2
Frequency (rad/s)
20 log10 |P (jω)C(jω)| > Br dB

for all ω < ωr , then

|P (jω)C(jω)| > 10Br /20 ,
for all ω < ωr . If |P (jω)C(jω)|  1 for all ω < ωr , then
 
 1 
 
 1 + P (jω)C(jω)  / 10
−Br /20
,

which implies that

|E(jω)| ≤ 10−Br /20 |R(jω)|

TOC
CHAPTER 16. FREQUENCY SPECIFICATIONS 285

for all ω < ωr , or in other words that

|e(t)| ≤ 10−Br /20 |r(t)| , (16.5)

for all r(t) with frequency content below ωr , which implies that

γr = 10−Br /20 .

16.1.2 Noise Attenuation

The transfer function from the noise signal n(t) to the error e(t) is

P (s)C(s)
E(s) = N (s).
1 + P (s)C(s)

Therefore,  
 P (jω)C(jω) 
|E(jω)| ≤   |N (jω)| .
1 + P (jω)C(jω) 
The objective is to attenuate noise signals n(t) with frequency content above ωno
so that the error due to the noise signal satisfies

|e(t)| ≤ γn |n(t)| .

Therefore, the controller C(s) needs to be designed so that

 
 P (jω)C(jω) 
 
 1 + P (jω)C(jω)  ≤ γn .

If |P (jω)C(jω)|  1, then a sufficient condition is that

|P (jω)C(jω)| ≤ γn ,

or equivalently
20 log10 |P (jω)C(jω)| ≤ 20 log10 γn , (16.6)
which needs to be satisfied for all ω ≥ ωno . On a Bode plot, the constraint
represented by Equation (16.6) is shown graphically in Fig. 16-3.
Alternatively, from the Bode plot of P C we can compute the noise attenuation
characteristics of the closed-loop system. For example, suppose that on the Bode
plot
20 log10 |P (jω)C(jω)| < −Bn dB
for all ω > ωno , then
|P (jω)C(jω)| < 10−Bn /20 ,
for all ω > ωno . If P (jω)C(jω)  1 for all ω > ωno , then
 
 P (jω)C(jω) 
 
 1 + P (jω)C(jω)  / 10
−Bn /20
,

TOC
CHAPTER 16. FREQUENCY SPECIFICATIONS 286

Figure 16-3: Bode plot representation of the loopshaping constraint corresponding

to attenuating the noise n(t) so that the corresponding error satisfies |e(t)| ≤
γn |n(t)|.

which implies that

|E(jω)| ≤ 10−Bn /20 |N (jω)|
for all ω > ωno , or in other words that

|e(t)| ≤ 10−Bn /20 |n(t)| ,

for all n(t) with frequency content above ωno , which implies that

γn = 10−Bn /20 .

16.1.3 Reject Output Disturbances

The transfer function from the output disturbance signals dout (t) to the error e(t)
is
1
E(s) = Dout (s).
1 + P (s)C(s)
Therefore,  
 1 

|E(jω)| ≤   |Dout (jω)| .
1 + P (jω)C(jω) 
The objective is to attenuate output disturbance signals dout (t) with frequency
content below ωdout so that the error due to the output disturbance signal satisfies

|e(t)| ≤ γdout |dout (t)| .

Therefore, the controller C(s) needs to be designed so that

 
 1 
 
 1 + P (jω)C(jω)  ≤ γdout ,

TOC
CHAPTER 16. FREQUENCY SPECIFICATIONS 287

or equivalently so that
1
|1 + P (jω)C(jω)| ≥ .
γdout
If |P (jω)C(jω)|  1, then a sufficient condition is that
1
|P (jω)C(jω)| ≥ ,
γdout
or equivalently
20 log10 |P (jω)C(jω)| ≥ 20 log10 1/γdout , (16.7)
which needs to be satisfied for all ω ≤ ωdout . On a Bode plot, the constraint
represented by Equation (16.7) is shown graphically in Fig. 16-4.

Bode Diagram
60
20 log10 |1/ dout |
40
20 log10 |P (j!)C(⌘!)|
20
Magnitude (dB)

-20

-40

-60

-80
-90
!dout

Figure 16-4: Bode plot representation of the loopshaping constraint corresponding

to rejecting the output disturbance dout (t) so that the corresponding error satisfies
Phase (deg)

-135

|e(t)| ≤ γdout |dout (t)|.

Alternatively, from the Bode plot of P C we can compute the output distur-
bance rejection characteristics of the closed-loop system. For example, suppose
-180
10 -2 10 -1 10 0 10 1 10 2
Frequency (rad/s)
that on the Bode plot
20 log10 |P (jω)C(jω)| > Bdout dB
for all ω < ωdout , then
|P (jω)C(jω)| > 10Bdout /20 ,
for all ω < ωdout . If |P (jω)C(jω)|  1 for all ω < ωdout , then
 
 1 
 
 1 + P (jω)C(jω)  / 10
−Bdout /20
,

which implies that

|E(jω)| ≤ 10−Bdout /20 |Dout (jω)|

TOC
CHAPTER 16. FREQUENCY SPECIFICATIONS 288

for all ω < ωdout , or in other words that

|e(t)| ≤ 10−Bdout /20 |dout (t)| ,
for all dout (t) with frequency content below ωdout , which implies that
γdout = 10−Bdout /20 .

16.1.4 Reject Input Disturbances

The transfer function from the input disturbance signals din (t) to the error e(t) is
P (s)
E(s) = Din (s).
1 + P (s)C(s)
Therefore,  
 P (jω) 

|E(jω)| ≤   |Din (jω)| .
1 + P (jω)C(jω) 
The objective is to attenuate input disturbance signals din (t) with frequency con-
tent below ωdin so that the error due to the input disturbance signal satisfies
|e(t)| ≤ γdin |din (t)| .
Therefore, the controller C(s) needs to be designed so that
 
 P (jω) 
 
 1 + P (jω)C(jω)  ≤ γdin ,

or equivalently so that
|1 + P (jω)C(jω)| 1
≥ .
|P (jω)| γdin
If |P (jω)C(jω)|  1, then a sufficient condition is that
|P (jω)C(jω)| 1
≥ ,
|P (jω)| γdin
or equivalently
20 log10 |P (jω)C(jω)| − 20 log10 |P (jω)| ≥ 20 log10 1/γdin , (16.8)
which needs to be satisfied for all ω ≤ ωdin . On a Bode plot, the constraint
represented by Equation (16.8) is shown graphically in Fig. 16-5, where it can be
seen that the loop gain |P C| must be above the magnitude of the plant |P | by the
specified amount.
Alternatively, from the Bode plot of P C we can compute the input disturbance
rejection characteristics of the closed-loop system. For example, suppose that on
the Bode plot
20 log10 |P (jω)C(jω)| − 20 log10 |P (jω)| > Bdin dB

TOC
CHAPTER 16. FREQUENCY SPECIFICATIONS 289

Bode Diagram
120

100
20 log10 |1/ din |
20 log10 |P (j!)C(⌘!)|
80

60
Magnitude (dB)

40

20

-20 20 log10 |P (j!)|

-40

-60

-80
-90
!din

Figure 16-5: Bode plot representation of the loopshaping constraint corresponding

to rejecting the input disturbance din (t) so that the corresponding error satisfies
-120
Phase (deg)

|e(t)| ≤ γdin |din (t)|.

-150

for all ω < ωdin , then

-180
10
-4
10
-3
|P (jω)C(jω)|
10
-2
10
-1

Frequency (rad/s) Bd
10
0
10
1
10
2

> 10 in /20 ,
|P (jω)|

for all ω < ωdin . If |P (jω)C(jω)|  1 for all ω < ωdin , then
 
 P (jω) 
 
 1 + P (jω)C(jω)  / 10
−Bdin /20
,

which implies that

|E(jω)| ≤ 10−Bdin /20 |Din (jω)|
for all ω < ωdin , or in other words that

|e(t)| ≤ 10−Bdin /20 |din (t)| , (16.9)

for all din (t) with frequency content below ωdin , which implies that

γdin = 10−Bdin /20 .

16.1.5 Frequency Response Characterization of System Type

Recall that the loop gain P (s)C(s) is said to be

• Type 0 if there are no free integrators,

• Type 1 if there is one free integrator,

• Type 2 if there are two free integrators, etc..

TOC
CHAPTER 16. FREQUENCY SPECIFICATIONS 290

Type 0 System
Note that for a type 0 system

lim P (s)C(s) = constant

s→0
=⇒ lim |P (jω)C(jω)| = constant
ω→0
=⇒ lim 20 log10 |P (jω)C(jω)| = constant.
ω→0

In other words, at low frequency, the Bode magnitude plot is as shown in Fig. 16-6.

Bode Diagram
40

30 Slope is 0 dB/decade as ! ! 0

20
Magnitude (dB)

10 20 log10 Mp

-10

-20
0 !

Figure 16-6: Bode plot of a type 0 system.

Phase (deg)

Recall
-45 that for a step input to a type 0 system where R(s) = A/s, the steady
state error is
A A
ess = lim e(t) = lim sE(s) = lim = ,
-90
t→∞ s→0 s→0 1 + P (s)C(s) 1 + Mp
10 -2 10 -1 10 0 10 1 10 2
Frequency (rad/s)
where
Mp = lim P (s)C(s) = lim |P (jω)C(jω)| .
s→0 ω→0

As shown in Fig. 16-6, 20 log10 Mp is the constant limit of the loop gain as ω → 0.
Therefore, from the Bode plot of P C for a type 0 system, we can compute the
tracking error for a step input of size A. Suppose that on the Bode plot

lim 20 log10 |P (jω)C(jω)| = B0 dB.

ω→0

Then
Mp = 10B0 /20 ,
and
1
lim |e(t)| ≤ A. (16.10)
t→∞ 1 + Mp

TOC
CHAPTER 16. FREQUENCY SPECIFICATIONS 291

Type 1 System
For a type 1 system since P (s)C(s) has one free integrator

lim sP (s)C(s) = constant

s→0
=⇒ lim |(jω)P (jω)C(jω)| = constant
ω→0
|P (jω)C(jω)|
=⇒ lim   = constant
ω→0 1
 jω 
  
 1 
=⇒ lim 20 log10 |P (jω)C(jω)| − 20 log10   = constant.
ω→0 jω

In other words, at low frequency, the Bode magnitude plot of 20 log10 |P (jω)C(jω)|
has a slope of −20 dB/decade as ω → 0, as shown in Fig. 16-7.

Bode Diagram
80

60
Slope is 20 dB/decade as ! ! 0

40 20 log10 Mv
Magnitude (dB)

20

0
1
20 log10
j!
-20

-40 20 log10 |P (j!)C(j!)|

-60
-90
!=1

Figure 16-7: Bode plot of a type 1 system.

-120
Phase (deg)

Recall that for a ramp input to a type 1 system where R(s) = A/s2 , the steady
state error is -150

A A
ess = lim e(t) = lim sE(s) = lim = ,
-180
t→0 s→0 s→0 s + sP (s)C(s) Mv
10 -2 10 -1 10 0 10 1 10 2
Frequency (rad/s)
where
Mv = lim sP (s)C(s) = lim |(jω)P (jω)C(jω)| .
s→0 ω→0

As shown in Fig. 16-7, 20 log10 Mv is the amount that the loop gain exceeds the
Bode plot of 20 log10 |1/jω| as ω → 0.
Therefore, from the Bode plot of P C for a type 1 system, we can compute the
tracking error for a ramp input with slope A. Suppose that on the Bode plot
  
 1 
lim 20 log10 |P (jω)C(jω)| − 20 log10   = B1 dB,
ω→0 jω

TOC
CHAPTER 16. FREQUENCY SPECIFICATIONS 292

then
Mv = 10B1 /20 ,
and
1
lim |e(t)| ≤ A. (16.11)
t→∞ Mv

Type 2 System
For a type 2 system where P (s)C(s) has two free integrators we have
lim s2 P (s)C(s) = constant
s→0
 
=⇒ lim (jω)2 P (jω)C(jω) = constant
ω→0
|P (jω)C(jω)|
=⇒ lim  2 = constant
ω→0 1
 jω 
"  2 #
 1 
=⇒ lim 20 log10 |P (jω)C(jω)| − 20 log10   = constant.
ω→0 jω

In other words, at low frequency, the Bode magnitude plot of 20 log10 |P (jω)C(jω)|
has a slope of −40 dB/decade as ω → 0, as shown in Fig. 16-8.

Bode Diagram
150

Slope is 40 dB/decade as ! ! 0
100

20 log10 Ma
Magnitude (dB)

50

0
2
1
20 log10
j!
-50
20 log10 |P (j!)C(j!)|

-100
-180
!=1

Figure 16-8: Bode plot of a type 2 system.

-210
Phase (deg)

Recall that for a parabolic input to a type 2 system where R(s) = A/s3 , the
steady state error is -240

A A
ess = lim e(t) = lim sE(s) = lim = ,
-270
t→0 s→0 s→0 s2 + s2 P (s)C(s) Ma
10 -2 10 -1 10 0 10 1 10 2
where 
Frequency (rad/s)

Ma = lim s2 P (s)C(s) = lim (jω)2 P (jω)C(jω) .
s→0 ω→0
As shown in Fig. 16-8, 20 log10 Ma is the amount that the loop gain exceeds the
2
Bode plot of 20 log10 |1/jω| as ω → 0.

TOC
CHAPTER 16. FREQUENCY SPECIFICATIONS 293

Therefore, from the Bode plot of P C for a type 2 system, we can compute the
tracking error for a parabola input with curvature A. Suppose that on the Bode
plot "  2 #
 1 
lim 20 log10 |P (jω)C(jω)| − 20 log10   = B2 dB.
ω→0 jω
Then
Ma = 10B2 /20 ,
and
1
lim |e(t)| ≤ A. (16.12)
t→∞ Ma

16.2 Design Study A. Single Link Robot Arm

Example Problem A.16

For the single link robot arm, use the bode command (from Matlab or Python)
to create a graph that simultaneously displays the Bode plots for (1) the plant, (2)
the plant under PID control, using the control gains calculated in HW A.10.

(a) To what percent error can the closed loop system under PID control track
the desired input if all of the frequency content of θr (t) is below ωr =
0.4 radians per second?

(b) If the reference input is θr (t) = 5t2 for t ≥ 0, what will the steady state
tracking error to this input be when using PID control?

(c) If all of the frequency content of the input disturbance din (t) is below ωdin =
0.01 radians per second, what percentage of the input disturbance shows up
in the output θ under PID control?

(d) If all of the frequency content of the noise n(t) is greater than ωno = 100
radians per second, what percentage of the noise shows up in the output
signal θ?

Solution
(a) The Bode plot of the plant P (s), and the loop gain with PID control P (s)CP ID (s)
is shown in Fig. 16-9. From Fig. 16-9 we see that below ωr = 0.4 rad/sec, the
loop gain is above Br = 44.5 dB. Therefore, from Equation (16.5) we have that

|e(t)| ≤ γr |r(t)| ,

where γr = 10−44.5/20 = 0.006, which implies that the tracking error will be
0.60% of the magnitude of the input.

TOC
CHAPTER 16. FREQUENCY SPECIFICATIONS 294

Figure 16-9: Bode plot for single link robot arm, plant only and under PID control.

(b) Suppose now that the desired reference input is θr (t) = 5t2 . Under PID
control, the slope of the loop gain as ω → 0 is −40 dB/dec, which implies that
the system is type 2 and will track a step and a ramp with zero steady state error.
For a parabola, there will be a finite error. Since the loop gain under PID control
is B2 = 28 dB above the 1/s2 line, from Equation (16.12) the steady state error
satisfies
A
lim |e(t)| ≤ = 5 · 10−28/20 = 0.2.
t→∞ Ma

(c) If the input disturbance is below ωdin = 0.01 rad/s, then the difference
between the loop gain and the plant is Bdin = 28 dB at ωdin = 0.01 rad/s,
therefore, from Equation (16.9) we see that

γdin = 10−28/20 = 0.04,

implying that 4% of the input disturbance will show up in the output.

(d) For noise greater than ωno = 100 rad/sec, we see from Fig. 16-9 that
Bn = 32.6 dB. Therefore, γn = 10−32.6/20 = 0.023 which implies that 2.3% of
the noise will show up in the output signal.
Python code to generate the relevant plots can be downloaded at http://
controlbook.byu.edu.

TOC
CHAPTER 16. FREQUENCY SPECIFICATIONS 295

16.3 Design Study B. Inverted Pendulum

Example Problem B.16

For the inner loop of the inverted pendulum, use the bode command (in Python
or Matlab) to create a graph that simultaneously displays the Bode plots for (1)
the plant, and (2) the plant under PD control, using the control gains calculated in
Homework B.10.
(a) To what percent error can the closed loop system track the desired input if all
of the frequency content of θr (t) is below ωr = 1.0 radians per second?
(b) If all of the frequency content of the sensor noise on the inner n(t) is greater
than ωno = 200 radians per second, what percentage of the noise shows up
in the output signal θ?
For the outer loop of the inverted pendulum, use the Matlab bode command
to create a graph that simultaneously displays the Bode plots for the plant, and the
plant under PID control, using the control gains calculated in Homework B.10.
(c) If the reference signal yr (t) has frequency content below ωr = 0.001 radian-
s/second, what is the tracking error under PID control if |r(t)| ≤ 50?

Solution
(a) The Bode plot of the plant P (s), and the loop gain with PD control P (s)CP D (s)
are shown in Fig. 16-10. From Fig. 16-10 we see that below ωr = 1.0 rad/sec,
the loop gain is above Br = 6.5 dB. Therefore, from Equation (16.5) we have that

|e(t)| ≤ γr |r(t)| ,

where γr = 10−6.5/20 = 0.47, which implies that the tracking error will be 47%
of the magnitude of the input.
(b) For noise greater than ωno = 200 rad/sec, we see from Fig. 16-10 that
Bn = 32.2 dB. Therefore, γn = 10−32.2/20 = 0.0245 which implies that 2.45%
of the noise will show up in the output signal.
(c) The Bode plot of the outer loop P (s), and the loop gain with PID control
P (s)CP ID (s) is shown in Fig. 16-11. From Fig. 16-11 it can be seen that the
loop gain under PID control for the outer loop is above Br = 154 dB for all
ω < ωr = 0.001 radians/second. Therefore, from Equation (16.5) the tracking
error satisfies
|e(t)| ≤ γr |r(t)| ,
where γr = 10−154/20 = 2.0e − 08. Therefore, if |r(t)| ≤ 50, then |e(t)| ≤
100e − 08.
Python code to generate the relevant plots can be downloaded at http://
controlbook.byu.edu.

TOC
CHAPTER 16. FREQUENCY SPECIFICATIONS 296

Figure 16-10: Bode plot for the inner loop of the inverted pendulum for (1) plant
only (blue), and (2) under PD control (orange).

16.4 Design Study C. Satellite Attitude Control

Example Problem C.16

For the inner loop of the satellite attitude control, use the bode command to create
a graph that simultaneously displays the Bode plots for (1) the plant, and (2) the
plant under PD control, using the control gains calculated in Homework C.10.

(a) To what percent error can the closed loop system under PD control track a
step in θr (t) = 20t2 for t ≥ 0?

(b) If the input disturbance has frequency content below ωdin = 0.1 radians per
second, what percentage of the input disturbance appears in the output?

For the outer loop of the satellite attitude control, use the bode command to
create a graph that simultaneously displays the Bode plots for (1) the plant, (2) the
plant under PID control using the control gains calculated in Homework C.10.

(c) If all of the frequency content of the noise n(t) is greater than ωno = 10
radians per second, what percentage of the noise shows up in the output
signal φ, using PI control?

TOC
CHAPTER 16. FREQUENCY SPECIFICATIONS 297

Figure 16-11: Bode plot for the outer loop of the inverted pendulum for (1) plant
only (blue), and (2) under PID control (orange).

Solution
(a)
The Bode plot of the plant P (s) and the loop gain with PD control P (s)CP D (s)
are shown in Fig. 16-12. Suppose that the desired reference input is θr (t) = 20t2 .
Under PD control, the slope of the loop gain as ω → 0 is −40 dB/dec, which im-
plies that the system is type 2 and will track a step and a ramp with zero steady
state error. For a parabola, there will be a finite error. Since the loop gain under
PID control is B2 = 20 dB above the 1/s2 line, from Equation (16.12) the steady
state error satisfies
A
lim |e(t)| ≤ = 20 · 10−22.3/20 = 1.53.
t→∞ Ma
(b) If the input disturbance is below ωdin = 0.1 rad/s, then the difference be-
tween the loop gain and the plant is Bdin = 38 dB, therefore, from Equation (16.9)
we see that
γdin = 10−38/20 = 0.0126,
implying that 1.26% of the input disturbance will show up in the output.
(c) The Bode plot of outer loop Pout (s), and the loop gain with PI control
Pout (s)CP I (s) are shown in Fig. 16-13. For noise greater than ωno = 10 rad/sec,
we see from Fig. 16-13 that Bn = 8.4 dB. Therefore, γn = 10−8.4/20 = 0.380
which implies that 38% of the noise will show up in the output signal.

TOC
CHAPTER 16. FREQUENCY SPECIFICATIONS 298

Figure 16-12: Bode plot for the inner loop of the satellite attitude control for (a)
plant only (blue), and (b) under PD control (orange).

Python code to generate the relevant plots can be downloaded at http://

controlbook.byu.edu.

Important Concepts:
• The Bode magnitude plot is used to identify the error bounds due to
tracking, disturbances, and noise.
• The system type can be determined from the magnitude slope of the
open-loop gain at low frequencies.
• The steady-state error when tracking an input signal is calculated by
measuring the dB difference, at low frequencies, between the open-
loop gain and the gain for system’s number of free integrators.

Notes and References

TOC
CHAPTER 16. FREQUENCY SPECIFICATIONS 299

Figure 16-13: Bode plot for the outer loop of the satellite attitude control for (a)
plant only (blue), and (b) PI control (orange).

TOC
CHAPTER 16. FREQUENCY SPECIFICATIONS 300

TOC
 Stability and Robustness Margins
17
Learning Objectives:
• Calculate the gain margin and phase margin for a system.

17.1 Theory
17.1.1 Phase and Gain Margins
To achieve the objectives discussed in Chapter 16, the closed-loop system must be
stable. Recall that tracking performance and disturbance rejection characteristics
are determined by the loopshape at low frequencies when |P (jω)C(jω)|  1.
Similarly, noise attenuation properties are determined by the loopshape at high
frequencies when |P (jω)C(jω)|  1. In both cases we ignored the phase of
P (jω)C(jω). It turns out that stability of the closed-loop system is determined
by the phase of P (jω)C(jω) when P (jω)C(jω) = 1, as depicted in Fig. 17-1.
Define the crossover frequency ωco to be the frequency where
|P (jωco )C(jωco )| = 1,
or equivalently where
20 log10 |P (jωco )C(jωco )| = 0 dB.
The phase ∠P (jωco )C(jωco ) at the crossover frequency plays a critical role in
determining the stability of the system. To better understand why this is the case,
consider the transfer functions from R, Din , Dout , and N , to E in Equation (16.3).
The denominator in each transfer function is 1+P (s)C(s). Therefore, the closed-
loop poles are given by the closed-loop characteristic equation
1 + P (s)C(s) = 0,

TOC
CHAPTER 17. STABILITY AND ROBUSTNESS MARGINS 302

Bode Diagram
60

Tracking and disturbance rejection

40
Magnitude (dB)

20

Noise attenuation
0

-20
Stability
-40
-30
Phase (deg)

-60

-90
10 -2 10 -1 10 0 10 1 10 2 10 3
Frequency (rad/s)

Figure 17-1: The shape of the magnitude plot primarily determines the tracking
and disturbance rejection characteristics at low frequencies and the noise atten-
uation at high frequencies. Stability is primarily determined by the phase at the
crossover frequency.

or equivalently where
P (s)C(s) = −1.

Recall that for any s, P (s)C(s) is a complex number with magnitude and
phase. Therefore, given any s0 we can draw 1 + P (s0 )C(s0 ) in the complex plane
as shown in Fig. 17-2. Since the physical response of the system is determined
by P (s)C(s) evaluated on the jω-axis, there are stability problems if there exists
an ω0 such that P (jω0 )C(jω0 ) = −1, or in other words when

|P (jω0 )C(jω0 )| = 1 and ∠P (jω0 )C(jω0 ) = −180◦ ,

as shown in Fig. 17-3. Since the magnitude of the loop gain is equal to one at
the crossover frequency ωco , it turns out that the quality of stability is determined
by the phase of the loop gain at the crossover frequency. Define the phase margin
P M as
P M = ∠P (jωco )C(jωco ) − (−180)◦ .
Fig. 17-4 depicts the phase margin on the Bode plot. Another way to understand
the phase margin is shown in Fig. 17-5. The phase margin indicates the angular
distance that P (jωco )C(jωco ) is from −1.

TOC
CHAPTER 17. STABILITY AND ROBUSTNESS MARGINS 303

)
C (s 0
s 0) length is |P (s0 )C(s0 )|
P(
1+
\P (s0 )C(s0 )

1 + j0 <

Figure 17-2: The plot of 1 + P (s0 )C(s0 ) in the complex plane, is the sum of the
complex number 1 + j0 and P (s0 )C(s0 ).

P (j!0 )C(j!0 ) = 1

1 + j0 <

Figure 17-3: The plot of 1 + P (jω0 )C(jω0 ) in the complex plane when
P (jω0 )C(jω0 ) = −1.

TOC
CHAPTER 17. STABILITY AND ROBUSTNESS MARGINS 304

Bode Diagram
50

0
Magnitude (dB)

GM
-50

-100

-150
0

-45
Phase (deg)

-90
PM
-135

-180

-225
!co
-270
-2 -1 0 1 2 3 4
10 10 10 10 10 10 10
Frequency (rad/s)

Figure 17-4: The phase margin P M is the difference between the phase of
P (jω)C(jω) and −180 degrees at the crossover frequency ωco . The gain mar-
gin GM is the magnitude of P (jω)C(jω) when the phase crosses −180 degrees.

A related quantity is the Gain Margin GM , which measures how far the mag-
nitude |P (jω)C(jω)| is from unity when the phase ∠P (jω)C(jω) = −180 de-
grees. The gain margin is depicted on the Bode plot in Fig. 17-4. An alternative
view is shown in Fig. 17-6. A good control design will have large phase and gain
margins. A phase margin of P M = 60 degrees is considered to be very good
and is optimal for many applications. A gain margin of GM = 10 dB would be
considered very good for most applications.

17.1.2 Open and Closed Loop Frequency Response

From Equation (16.1), the transfer function from the reference R(s) to the output
Y (s) is
P (s)C(s)
Y (s) = R(s)
1 + P (s)C(s)
Note that
 
 P (jω)C(jω) 
• when |P (jω)C(jω)|  1, then  1+P (jω)C(jω)  ≈ 1,

 
 P (jω)C(jω) 
• when |P (jω)C(jω)|  1, then  1+P (jω)C(jω)  ≈ |P (jω)C(jω)|.

TOC
CHAPTER 17. STABILITY AND ROBUSTNESS MARGINS 305

=
P (j!co )C(j!co )

PM

1 2 <

Figure 17-5: Alternative graphical view of the phase margin P M .

P (j!)C(j!)

1 2 <
GM

Figure 17-6: Alternative graphical view of the gain margin GM .

 
 P (jωco )C(jωco ) 
When |P (jω)C(jω)| = 1, i.e., when ω = ωco , then  1+P (jωco )C(jωco )  is a com-
plex number divided by a complex number, and the magnitude will depend on the
phase of the P (jωco )C(jωco ), or in other words the phase margin P M . As shown
in Figures 17-5, P M will determine the magnitude of 1+P  (jωco )C(jωco ).  When
 P (jωco )C(jωco ) 
P M is small, |1 + P (jωco )C(jωco )| will be small and  1+P (jωco )C(jωco )  will be
larger than one. When P M is larger than 90 degrees, |1 + P (jωco )C(jωco )| will
 P (jωco )C(jωco ) 
be larger than |P (jωco )C(jωco )| and  1+P (jωco )C(jωco )  will be less than one.
Fig. 17-7 shows the closed-loop Bode plot superimposed on the open loop
Bode plot for different values of the phase margin. Note that when the phase mar-
gin is P M = 60 degrees, the closed-loop frequency response looks very similar
to the closed-loop frequency response for a second order system when ζ = 0.707,
as shown in Fig. 15-9. Phase margins smaller than 60 degrees have a peaking
response similar to second order system with ζ < 0.707. Note also that the band-
width of the closed-loop system is approximately the crossover frequency of the
open loop system. In general, the crossover frequency ωco plays a similar role for
general systems that the natural frequency ωn plays for second order systems.
Fig. 17-7 also shows the corresponding step response for the closed-loop sys-
tem. The step response looks similar to the step response for second order sys-
tems shown in Fig. 8-3, where again, the step response for P M = 60 degrees in
Fig. 17-7 is roughly equivalent to the step response for ζ = 0.707 in Fig. 8-4.
For a given reference signal, the size of the control signal u(t) is determined
by the crossover frequency. A large crossover frequency will result in a larger

TOC
CHAPTER 17. STABILITY AND ROBUSTNESS MARGINS 306

Bode Diagram
Gm = -Inf dB (at 0 rad/s) , Pm = 65.6 deg (at 3.75 rad/s) Closed Loop Step Response
Magnitude (dB) 100 1.2

1
0
0.8

Amplitude
-100 0.6
0
Phase (deg)

0.4
-90
0.2

-180 0
10 -2 10 -1 10 0 10 1 10 2 10 3 0 2 4 6 8 10 12
Frequency (rad/s) Time (seconds)

Bode Diagram
Gm = -Inf dB (at 0 rad/s) , Pm = 60.3 deg (at 1.69 rad/s) Closed Loop Step Response
100 1.5
Magnitude (dB)

0
1

Amplitude
-100
0
Phase (deg)

0.5
-90

-180 0
10 -2 10 -1 10 0 10 1 10 2 10 3 0 1 2 3 4 5 6 7
Frequency (rad/s) Time (seconds)

Bode Diagram
Gm = -Inf dB (at 0 rad/s) , Pm = 30.4 deg (at 1.11 rad/s) Closed Loop Step Response
50 1.5
Magnitude (dB)

-50 1
Amplitude

-100
0
Phase (deg)

0.5
-90

-180 0
10 -1 10 0 10 1 10 2 10 3 0 5 10 15 20 25
Frequency (rad/s) Time (seconds)

Figure 17-7: closed-loop Bode plot superimposed on the open loop Bode plot,
together with the closed-loop step response, for different phase margins.

control signal u(t). Fig. 17-8 shows the open and closed-loop transfer functions
for a plant equal to P (s) = 1/s(s + 1) and for three different controllers that are
tuned for equivalent phase margins but with cross over frequency at 2, 20, and
200 respectively. The corresponding control signal when the reference is a unit
step is also shown in Fig. 17-8. Note that as ωco increases, the magnitude of the
control signal increases and the speed of the response also increases. In fact, the
integral of the control signals will be the same for each ωco . Therefore, saturation
constraints on u(t) will necessarily limit the size of the cross over frequency ωco .

TOC
CHAPTER 17. STABILITY AND ROBUSTNESS MARGINS 307

Bode Diagram
Gm = -Inf dB (at 0 rad/s) , Pm = 60.3 deg (at 1.69 rad/s) Control Signal
100 20
Magnitude (dB)

15
0

10

Amplitude
-100
0 5
Phase (deg)

-45
-90 0
-135
-180 -5
-2 0 2 4 0 1 2 3 4 5 6 7
10 10 10 10
Frequency (rad/s) Time (seconds)

Bode Diagram
Gm = -Inf dB (at 0 rad/s) , Pm = 60.3 deg (at 16.9 rad/s) Control Signal
100 2000
Magnitude (dB)

1500
0
Amplitude

1000
-100
0 500
Phase (deg)

-45
-90 0
-135
-180 -500
10 -2 10 0
10 2
10 4 0 0.05 0.1 0.15 0.2 0.25 0.3 0.35
Frequency (rad/s) Time (seconds)

Bode Diagram
Gm = -Inf dB (at 0 rad/s) , Pm = 60.3 deg (at 169 rad/s) ×10 4 Control Signal
100 20
Magnitude (dB)

15
0
Amplitude

10
-100
0 5
Phase (deg)

-45
-90 0
-135
-180 -5
10 -2 10 0 10 2 10 4 0 0.005 0.01 0.015 0.02 0.025 0.03 0.035
Frequency (rad/s) Time (seconds)

Figure 17-8: closed-loop Bode plot superimposed on the open loop Bode plot, to-
gether with the corresponding control signal, for different cross-over frequencies.

TOC
CHAPTER 17. STABILITY AND ROBUSTNESS MARGINS 308

17.1.3 Bode Phase Gain Relationship

In Bode’s original work, he proved the following theorem that relates the slope of
the magnitude plot to the phase.
Bode’s Phase-Gain Theorem. For any stable, minimum phase (i.e., zeros in the
LHP), linear time-invariant system G(s), the phase of G(jω) is uniquely related
to 20 log10 |G(jω)| by
Z  
1 ∞ dM
∠G(jω0 ) = W (u) du, (17.1)
π −∞ du
where
M = 20 log10 |G(jω)| ,
 
ω
u = log10   ,
ω0
 
|u|
W (u) = log10 coth .
2

The term dMdu in Equation (17.1) is the slope of the magnitude of G(jω) on the
Bode plot. A plot of W (u) is shown in Fig. 17-9, where it can be seen that W (u)
heavily weights the slope of the Bode magnitude around ω0 , but also includes
bleed-over from the slope for roughly a decade before and after ω0 . This implies
that the phase of P (jω)C(jω) at ω = ω0 is determined by the slope of the Bode
magnitude plot in a region about ω0 . Recall that the phase for an integrator
3

2.5

1.5

0.5

0
10 -2 10 -1 10 0 10 1 10 2

!0 /10 !0 10!0

Figure 17-9: The weighting function W (u) in the Bode gain-phase relationship
heavily weights the slope around ω0 .

1/s is −90 degrees and that the phase for a differentiator is 90 degrees. Also,
recall that the phase for a constant is 0 degrees. Therefore, we can approximate
Equation (17.1) as follows:

TOC
CHAPTER 17. STABILITY AND ROBUSTNESS MARGINS 309

• Slope of 20 log10 |G(jω0 )| ≈ +20 dB/dec

=⇒ ∠G(jω0 ) ≈ +90 deg,

• Slope of 20 log10 |G(jω0 )| ≈ 0 dB/dec

=⇒ ∠G(jω0 ) ≈ 0 deg,

• Slope of 20 log10 |G(jω0 )| ≈ −20 dB/dec

=⇒ ∠G(jω0 ) ≈ −90 deg,

• Slope of 20 log10 |G(jω0 )| ≈ −40 dB/dec

=⇒ ∠G(jω0 ) ≈ −180 deg,

• Slope of 20 log10 |G(jω0 )| ≈ −60 dB/dec

=⇒ ∠G(jω0 ) ≈ −270 deg.

Of course, these values are approximate and require that the slope around ω0 per-
sist for approximately a decade before and after ω0 .
While we do not directly use Equation (17.1) in practice, it has clear impli-
cations for feedback design. In particular, the takeaway message is that to have
a good phase margin, the slope of the Bode magnitude plot at the crossover fre-
quency ωco cannot be too steep, and that it needs to be reasonably shallow for
roughly a decade before and after crossover. As a rule of thumb, a phase margin
of P M = 60 degrees requires that the slope at crossover is between −20 dB/dec
and −40 dB/dec. In other words, the ideal loopshape will need to look something
like Fig. 17-10. It is also important to understand that the Bode phase-gain the-
orem requires that there is adequate frequency separation between the frequency
content of signals that we are tracking or rejecting and the frequency content of
the sensor noise that is to be attenuated.

17.2 Design Study A. Single Link Robot Arm

Example Problem A.17

For the single link robot arm, use the bode and margin commands (from Python
or Matlab) to find the phase and gain margin for the closed loop system under PID
control. On the same graph, plot the open loop Bode plot and the closed loop
Bode plot. What is the bandwidth of the closed loop system, and how does this
relate to the crossover frequency? Use the gains found in HW A.10.

Solution
Python code to generate the plots from this problem is included below. However,
similar Matlab code is available on the book website (see http://controlbook.
byu.edu).

TOC
 CHAPTER 17. STABILITY AND ROBUSTNESS MARGINS 310

Bode Diagram
Gm = 23.4 dB (at 9.74 rad/s) , Pm = 60.1 deg (at 1.41 rad/s)
60

40 Shallow slope at crossover Small gain at high frequency:

20 noise attenuation
0

-20
Magnitude (dB)

Large gain at low frequency:

tracking & disturbance rejection
-40

-60

-80
Crossover frequency:
-100
small enough to satisfy
-120 saturation constraint
-140
-90

-135

Phase margin
PM
~ 60 degrees
Phase (deg)

-180

-225

-270
10 -2 10 -1 10 0 10 1 10 2 10 3

!co Frequency (rad/s)

Figure 17-10: The ideal loopshape is large at low frequencies to achieve good
tracking and disturbance rejection, is small at high frequencies to attenuate sensor
noise, has a shallow slope at the crossover frequency to achieve a good phase
margin, and has a small enough crossover frequency to satisfy the input saturation
constraints.

1 # Single link arm Parameter File

2 import sys
3 sys.path.append(’..’) # add parent directory
4 sys.path.append(’../hw16’)
5 import armParamHW16 as P16
6 from control import bode, margin, mag2db
7 import matplotlib.pyplot as plt
8
9 # flag to define if using dB or absolute scale for M(omega)
10 dB_flag = P16.dB_flag
11
12 # assigning plant and controller from past HW (to make sure
13 # we don’t introduce additional errors)
14 Plant = P16.Plant
15 C_pid = P16.C_pid
16
17 if __name__=="__main__":
18 # Calculate the phase and gain margins
19 if dB_flag:

TOC
 CHAPTER 17. STABILITY AND ROBUSTNESS MARGINS 311

20 gm, pm, Wcg, Wcp = margin(Plant*C_pid)

21 gm = mag2db(gm)
22 print("Inner Loop:", "gm: ",gm,
23 " pm: ", pm," Wcg: ", Wcg, " Wcp: ", Wcp)
24
25 else:
26 gm, pm, Wcg, Wcp = margin(Plant*C_pid)
27 print("Inner Loop:", "gm: ", gm,
28 " pm: ", pm, " Wcg: ", Wcg, " Wcp: ", Wcp)
29
30 # display bode plots of transfer functions
31 fig1 = plt.figure()
32
33 # this makes two bode plots for open and closed loop
34 bode(Plant * C_pid, dB=dB_flag,
35 label=’C(s)P(s) - Open-loop’)
36 bode(Plant*C_pid/(1+Plant*C_pid), dB=dB_flag,
37 label=r’$\frac{P(s)C(s)}{1+P(s)C(s)}$ - Closed-loop’)
38
39 # now we can add lines to show where we calculated the GM and PM
40 gm_line = fig1.axes[0].plot([Wcg, Wcg],
41 plt.ylim(), ’k--’, label=’GM’)
42 gm_line[0].set_label(’GM’)
43 fig1.axes[0].legend()
44 fig1.axes[1].plot([Wcp, Wcp], plt.ylim(), ’b--’, label=’PM’)
45 plt.legend()
46
47 # setting axis title
48 fig1.axes[0].set_title(’Single Link Robot Arm - ’+
49 ’GM:’+str(round(gm, 2))+
50 ’, PM:’+str(round(pm, 2)))
51
52 plt.show()

Listing 17.1: armParamHW17.py

The transfer function for the plant is defined in Line 14. The transfer function
for the PID controller is

kI kD s s(σs + 1)kP + (σs + 1)kI + kD s2

CP ID (s) = kP + + =
s σs + 1 s(σs + 1)
2
(kD + σkP )s + (kP + σkI )s + kI
= ,
s(σs + 1)

and is defined in Line 15. In Python the margin command returns informa-
tion about the phase margin, gain margin, and frequencies where those are calcu-
lated (like the crossover frequency). The bode command can be used to make
plots of transfer functions (and will show the margin information if sent the "mar-
gin=True" argument). The results of the Python code is shown in Fig. 17-11. As
seen in Fig. 17-11 the bandwidth for PID control is approximately 18 rad/sec,
which is slightly larger than the cross over frequency of 10.8 rad/sec. The larger
bandwidth is due to the small phase margin of P M = 49.0 degrees.

TOC
CHAPTER 17. STABILITY AND ROBUSTNESS MARGINS 312

Single Link Robot Arm - GM:inf, PM:49.02

100 C(s)P(s) - Open-loop
P(s)C(s)- Closed-loop
Magnitude (dB)

1 + P(s)C(s)
50 GM
0
50
10 2 10 1 100 101 102 103
0
45 C(s)P(s) - Open-loop
Phase (deg)

P(s)C(s)- Closed-loop
90 1 + P(s)C(s)
PM
135
180
10 2 10 1 100 101 102 103
Frequency (rad/sec)

Figure 17-11: Bode plots of the open loop and closed loop system for the single
link robot arm under PID control.

17.3 Design Study B. Inverted Pendulum

Example Problem B.17

For this problem, use the gains found in HW B.10.

(a) For the inner loop of the inverted pendulum, use the Python or Matlab bode
and margin commands to find the phase and gain margin for the inner loop
system under PD control. On the same graph, plot the open loop Bode plot
and the closed loop Bode plot. What is the bandwidth of the inner loop, and
how does this relate to the crossover frequency?

(b) For the outer loop of the inverted pendulum, use the bode and margin
commands to find the phase and gain margin for the outer loop system under
PID control. Plot the open and closed loop Bode plots for the outer loop
on the same plot as the open and closed loop for the inner loop. What is
the bandwidth of the outer loop, and how does this relate to the crossover
frequency?

TOC
 CHAPTER 17. STABILITY AND ROBUSTNESS MARGINS 313

(c) What is the bandwidth separation between the inner (fast) loop, and the
outer (slow) loop? For this design, is successive loop closure justified?

Solution
The Python code that can be used to generate the plots for this solution is shown
below.
1 # Inverted Pendulum Parameter File
2 import sys
3 sys.path.append(’..’)
4 sys.path.append(’../hw16’)
5 import pendulumParam as P
6 import pendulumParamHW16 as P16
7 from control import bode, margin, mag2db
8 import matplotlib.pyplot as plt
9
10 # flag to define if using dB or absolute scale for M(omega)
11 dB_flag = P16.dB_flag
12
13 # Assign inner and outer open-loop transfer functions from
14 # previous HW solution
15 P_in = P16.P_in
16 P_out = P16.P_out
17
18 # Compute inner and outer closed-loop transfer functions
19 C_in = P16.C_in
20 C_out = P16.C_out
21
22 if __name__=="__main__":
23
24 ##################################################
25 ########### Inner loop ###########################
26 # Calculate the phase and gain margins
27 if dB_flag:
28 gm, pm, Wcg, Wcp = margin(P_in*C_in)
29 gm = mag2db(gm)
30 print("Inner Loop:", "gm: ",gm,
31 " pm: ", pm," Wcg: ", Wcg, " Wcp: ", Wcp)
32
33 else:
34 gm, pm, Wcg, Wcp = margin(P_in * C_in)
35 print("Inner Loop:", "gm: ", gm,
36 " pm: ", pm, " Wcg: ", Wcg, " Wcp: ", Wcp)
37
38 # display bode plots of transfer functions
39 fig1 = plt.figure()
40
41 # this makes two bode plots for open and closed loop
42 bode(P_in * C_in, dB=dB_flag,
43 label=’$C_{in}P_{in}$ - Open-loop’)
44 bode(P_in*C_in/(1+P_in*C_in), dB=dB_flag,
45 label=r’$\frac{P_{in}C_{in}}{1+P_{in}C_{in}}$’
46 +’- Closed-loop’)
47

TOC
 CHAPTER 17. STABILITY AND ROBUSTNESS MARGINS 314

48 # now we can add lines to show where we calculated the GM and PM

49 gm_line = fig1.axes[0].plot([Wcg, Wcg],
50 plt.ylim(), ’k--’, label=’GM’)
51 gm_line[0].set_label(’GM’)
52 fig1.axes[0].legend()
53 fig1.axes[1].plot([Wcp, Wcp], plt.ylim(), ’b--’, label=’PM’)
54 plt.legend()
55
56 # setting axis title
57 fig1.axes[0].set_title(’Pendulum Inner Loop - ’+
58 ’GM:’+str(round(gm, 2))+
59 ’, PM:’+str(round(pm, 2)))
60
61 ##################################################
62 ########### Outer loop ###########################
63 # Calculate the phase and gain margins
64 if dB_flag:
65 gm, pm, Wcg, Wcp = margin(P_out * C_out)
66 gm = mag2db(gm)
67 print("Outer Loop:", "gm: ", gm,
68 " pm: ", pm, " Wcg: ", Wcg, " Wcp: ", Wcp)
69
70 else:
71 gm, pm, Wcg, Wcp = margin(P_out * C_out)
72 print("Outer Loop:", "gm: ", gm,
73 " pm: ", pm, " Wcg: ", Wcg, " Wcp: ", Wcp)
74
75 # display bode plots of transfer functions
76 fig2 = plt.figure()
77
78 # this makes two bode plots for open and closed loop
79 bode(P_out * C_out, dB=dB_flag,
80 label=’$C_{out}P_{out}$ - Open-loop’)
81 bode(P_out*C_out/(1+P_out*C_out), dB=dB_flag,
82 label=r’$\frac{P_{out}C_{out}}{1+P_{out}C_{out}}$’
83 +’ - Closed-loop’)
84
85 # now we can add lines to show where we calculated the GM and PM
86 gm_line = fig2.axes[0].plot([Wcg, Wcg],
87 plt.ylim(), ’k--’, label=’GM’)
88 gm_line[0].set_label(’GM’)
89 fig2.axes[0].legend()
90 fig2.axes[1].plot([Wcp, Wcp], plt.ylim(), ’b--’, label=’PM’)
91 plt.legend()
92
93 # setting axis title
94 fig2.axes[0].set_title(’Pendulum Outer Loop - ’+
95 ’GM:’+str(round(gm, 2))+
96 ’, PM:’+str(round(pm, 2)))
97
98 print(’Close window to end program’)
99 plt.show()

Listing 17.2: pendulumParamHW17.py

The transfer functions for the inner and outer loop plants and controller are
defined in Lines 14–19. For this problem, we first calculate the gain and phase

TOC
CHAPTER 17. STABILITY AND ROBUSTNESS MARGINS 315

margins (see Lines 26–35). We do this so that we can then plot the open-loop and
closed-loop bode plots with stability margin information shown on the plots (see
Lines 38–56). If all we want is the open-loop Bode plot with stability margins, we
can simply pass the argument “margins=True” to the bode function. The results
of this code are shown in Fig. 17-12. We follow a similar process for the outer
loop with results shown in Fig. 17-13.

Pendulum Inner Loop - GM:-6.62, PM:32.88

0
Magnitude (dB)

50
100
CinPin - Open-loop
PinCin
150 1 + PinCin - Closed-loop
GM
10 1 100 101 102 103
0 CinPin - Open-loop
PinCin
45 1 + PinCin - Closed-loop
Phase (deg)

PM
90
135
180
10 1 100 101 102 103
Frequency (rad/sec)

Figure 17-12: Bode plots with stability margins for the open and closed-loop
systems of the inner loop of the inverted pendulum system.

As seen from Fig. 17-12, the bandwidth of the inner loop is approximately
19 rad/sec, which is slightly larger than the cross over frequency of 10 rad/sec.
Similarly, Fig. 17-13 indicates that the bandwidth of the outer loop is approx-
imately 1.3 rad/sec, which is slightly larger than the cross over frequency of
1.0 rad/sec with a phase margin of P M = 72 degrees (the plot says -108, but
this is off by 180, so that P M = 180 − 108).
For a second order system, making the step response 10 times faster implies
that tr is divided by 10, or equivalently that ωn is multiplied by 10. A similar
principle holds for high order systems. To be 10 times faster, the closed loop
bandwidth of the inner loop should be a decade higher on the Bode plot than the
closed loop bandwidth of the outer loop. The bandwidth separation between the
inner and outer loop is about one decade justifying the successive loop closure
design approach.

TOC
CHAPTER 17. STABILITY AND ROBUSTNESS MARGINS 316

Pendulum Outer Loop - GM:inf, PM:-108.52

CoutPout - Open-loop
80 PoutCout
1 + PoutCout - Closed-loop
Magnitude (dB)

60 GM
40
20
0
10 2 10 1 100 101 102 103
90
45
Phase (deg)

0
CoutPout - Open-loop
45 PoutCout
1 + PoutCout - Closed-loop

90 PM
10 2 10 1 100 101 102 103
Frequency (rad/sec)

Figure 17-13: Bode plots with stability margins for the open and closed-loop
systems of the outer loop of the inverted pendulum system.

17.4 Design Study C. Satellite Attitude Control

Example Problem C.17

For this problem, use the gains found in HW C.10.

(a) For the inner loop of the satellite attitude controller, use the bode and
margin commands (from Matlab or Python) to find the phase and gain
margin for the inner loop system under PD control. On the same graph, plot
the open loop Bode plot and the closed loop Bode plot. What is the band-
width of the closed loop system, and how does this relate to the crossover
frequency?

(b) For the outer loop of the satellite attitude controller, use the bode and
margin commands to find the phase and gain margin for the outer loop
system under PID control. Plot the open and closed loop Bode plots for the
outer loop on the same plot as the open and closed loop for the inner loop.

TOC
 CHAPTER 17. STABILITY AND ROBUSTNESS MARGINS 317

What is the bandwidth of the closed loop system, and how does this relate
to the crossover frequency?

(c) What is the bandwidth separation between the inner (fast) loop, and the
outer (slow) loop? For this design, is successive loop closure justified?

Solution
Python code to generate the plots in this solutions is included below.
1 # Satellite Parameter File
2 import sys
3 sys.path.append(’..’) # add parent directory
4 import satelliteParam as P
5 sys.path.append(’../hw16’) # add parent directory
6 import satelliteParamHW16 as P16
7 from control import *
8 import matplotlib.pyplot as plt
9
10 # flag to define if using dB or absolute scale for M(omega)
11 dB_flag = P16.dB_flag
12
13 # Compute inner and outer open-loop transfer functions
14 P_in = P16.P_in
15 P_out = P16.P_out
16
17 # Compute the controller transfer functions from HW10
18 # (to make sure we don’t introduce additional errors)
19 C_in = P16.C_in
20 C_out = P16.C_out
21
22 if __name__=="__main__":
23
24 # we have to define the frequencies, or we get non-smooth
25 # bode plots.
26 omegas = np.logspace(-2, 3, 1000)
27
28 ##################################################
29 ########### Inner loop ###########################
30 # Calculate the phase and gain margins
31 if dB_flag:
32 gm, pm, Wcg, Wcp = margin(P_in * C_in)
33 gm = mag2db(gm)
34 print("Inner Loop:", "gm: ", gm,
35 " pm: ", pm, " Wcg: ", Wcg, " Wcp: ", Wcp)
36
37 else:
38 gm, pm, Wcg, Wcp = margin(P_in * C_in)
39 print("Inner Loop:", "gm: ", gm,
40 " pm: ", pm, " Wcg: ", Wcg, " Wcp: ", Wcp)
41
42 # display bode plots of transfer functions
43 fig1 = plt.figure()
44
45 # this makes two bode plots for open and closed loop

TOC
 CHAPTER 17. STABILITY AND ROBUSTNESS MARGINS 318

46 bode(P_in * C_in, omega=omegas, dB=dB_flag,

47 label=’$C_{in}P_{in}$ - Open-loop’)
48 bode(P_in * C_in / (1 + P_in * C_in),
49 omega=omegas, dB=dB_flag,
50 label=r’$\frac{P_{in}C_{in}}{1+P_{in}C_{in}}$’
51 + ’- Closed-loop’)
52
53 # now we can add lines to show where we calculated the GM and PM
54 gm_line = fig1.axes[0].plot([Wcg, Wcg],
55 plt.ylim(), ’k--’, label=’GM’)
56 gm_line[0].set_label(’GM’)
57 fig1.axes[0].legend()
58 fig1.axes[1].plot([Wcp, Wcp], plt.ylim(), ’b--’, label=’PM’)
59 plt.legend()
60
61 # setting axis title
62 fig1.axes[0].set_title(r’Satellite Inner Loop ($\theta$) - ’ +
63 ’GM:’ + str(round(gm, 2)) +
64 ’, PM:’ + str(round(pm, 2)))
65
66 ##################################################
67 ########### Outer loop ###########################
68 # Calculate the phase and gain margins
69 if dB_flag:
70 gm, pm, Wcg, Wcp = margin(P_out * C_out)
71 gm = mag2db(gm)
72 print("Outer Loop:", "gm: ", gm,
73 " pm: ", pm, " Wcg: ", Wcg, " Wcp: ", Wcp)
74
75 else:
76 gm, pm, Wcg, Wcp = margin(P_out * C_out)
77 print("Outer Loop:", "gm: ", gm,
78 " pm: ", pm, " Wcg: ", Wcg, " Wcp: ", Wcp)
79
80 # display bode plots of transfer functions
81 fig2 = plt.figure()
82
83 # this makes two bode plots for open and closed loop
84 bode(P_out * C_out, omega=omegas, dB=dB_flag,
85 label=’$C_{out}P_{out}$ - Open-loop’)
86 bode(P_out * C_out / (1 + P_out * C_out),
87 omega=omegas, dB=dB_flag,
88 label=r’$\frac{P_{out}C_{out}}{1+P_{out}C_{out}}$’
89 + ’ - Closed-loop’)
90
91 # now we can add lines to show where we calculated the GM and PM
92 gm_line = fig2.axes[0].plot([Wcg, Wcg],
93 plt.ylim(), ’k--’, label=’GM’)
94 gm_line[0].set_label(’GM’)
95 fig2.axes[0].legend()
96 fig2.axes[1].plot([Wcp, Wcp], plt.ylim(), ’b--’, label=’PM’)
97 plt.legend()
98
99 # setting axis title
100 fig2.axes[0].set_title(r’Satellite Outer Loop ($\phi$) - ’ +
101 ’GM:’ + str(round(gm, 2)) +
102 ’, PM:’ + str(round(pm, 2)))

TOC
 CHAPTER 17. STABILITY AND ROBUSTNESS MARGINS 319

103
104 print(’Close window to end program’)
105 plt.show()

Listing 17.3: satelliteParamHW17.py

Similar to the last two case studies, the transfer functions for the inner and
outer loop plants are calculated first. Then we find the stability margins and gen-
erate bode plots for the inner loop (see Fig. 17-14). We follow the same procedure
for the outer loop plots shown in Fig. 17-15.

Satellite Inner Loop ( ) - GM:inf, PM:56.16

100 CinPin - Open-loop
PinCin
1 + PinCin - Closed-loop
Magnitude (dB)

50
GM
0
50

10 2 10 1 100 101 102 103

0 CinPin - Open-loop
PinCin
45 1 + PinCin - Closed-loop
Phase (deg)

PM
90
135
180
10 2 10 1 100 101 102 103
Frequency (rad/sec)

Figure 17-14: Bode plots with stability margins for the open and closed-loop
systems of the inner loop of the satellite system.

As seen from Fig. 17-14,the bandwidth of the inner loop is approximately 11

rad/sec, which is slightly larger than the cross over frequency of 7 rad/sec, with a
phase margin of P M = 56 degrees. Similarly, Fig. 17-15 indicates that the band-
width of the outer loop is approximately the cross over frequency of 1 rad/sec. The
bandwidth separation between the inner and outer loop is close to a decade and
the closed-loop Bode plot of the inner-loop is approximately one for frequencies
far beyond the cross over frequency of the outer loop.

TOC
CHAPTER 17. STABILITY AND ROBUSTNESS MARGINS 320

Satellite Outer Loop ( ) - GM:inf, PM:110.92

20 CoutPout - Open-loop
PoutCout
1 + PoutCout - Closed-loop
Magnitude (dB)

0 GM
20
40
10 2 10 1 100 101 102 103
45 CoutPout - Open-loop
PoutCout
0 1 + PoutCout - Closed-loop
Phase (deg)

PM
45

90
10 2 10 1 100 101 102 103
Frequency (rad/sec)

Figure 17-15: Bode plots with stability margins for the open and closed-loop
systems of the outer loop of the satellite system.

Important Concepts:
• The gain/phase margins describe the amount that the gain/phase can
be shifted before the system becomes unstable.
• The crossover frequency is the frequency where the gain is equal to
unity (zero dB).
• The gain margin measures how far the gain is from unity (zero dB)
when the phase equals −180 degrees.
• The phase margin measures how far the phase is from −180 degrees
when the gain is at the crossover frequency.
• The bandwidth of a system may be approximated by the crossover
frequency. A large crossover frequency will result in a large control
input.
• A good phase margin, requires that the slope of the Bode magnitude
plot be relatively shallow (between -20– -40 dB/decade for roughly a
decade before and after the crossover frequency.

Notes and References

TOC
 Compensator Design
18
Important Concepts:
• Design controllers that satisfy specified frequency criteria and result
in good stability margins.
• Understand how different controller building blocks help to satisfy
each design/stability specification.

18.1 Theory
The loopshaping design methodology is to add elements to the compensator C(s)
to achieve a loop shape that satisfies the design specifications and that results
in good stability margins, ideally P M ≈ 60 degrees, all while satisfying input
saturation constraints. The idea is that the controller C(s) will be composed of
several building blocks:

C(s) = C1 (s)C2 (s) · · · Cq (s).

Since the loop gain is P (s)C(s), on the Bode plot we have

q
X
20 log10 |P C| = 20 log10 |P | + 20 log10 |Ci |
i=1
q
X
∠P C = ∠P + ∠Ci .
i=1

Therefore, the total design can be realized by adding different elements to the
Bode plot until the design specifications are satisfied. In this chapter we will
discuss the following five basic building blocks for loopshaping design:

TOC
CHAPTER 18. COMPENSATOR DESIGN 322

• Proportional Gain. A proportional gain does not have phase (assuming

kP > 0) and only affects the magnitude plot, by moving it up and down on
the Bode magnitude plot. A proportional gain is used to affect the location
of the cross over frequency.

• Integral (PI) Control. An integrator adds −20 dB of slope and −90 de-
grees of phase at all frequencies. The addition of an integrator changes the
system type. A modified PI controller can be used to affect only the low
frequency loop gain, thereby enhancing reference tracking and input/output
disturbance rejection.

• Low-pass Filter. Low-pass filters are used to decrease the loop gain at high
frequencies to achieve noise attenuation.

• phase-lag Filter. A phase-lag filter can be used to increase the loop gain
at low frequencies thereby enhancing reference tracking and input/output
disturbance rejection. A phase-lag filter differs from integral control in that
it does not change the system type.

• phase-lead Filter. A phase-lead filter can be used to add phase at the cross
over frequency and can thereby stabilize the system and increase the phase
margin.

Each of these basic building blocks will be described in more detail in the sections
that follow.

18.1.1 Building Block #1: Proportional Gain

The first basic building block for loopshaping design is proportional control where

CP (s) = kP .

Proportional control is used to change the loop gain at all frequencies, without
affecting the phase. A Bode plot for proportional control is shown in Fig. 18-1 for
different values of kP .

18.1.2 Building Block #2: Integral Control

The second basic building block for loopshaping design is proportional-integral
(PI) control where kP = 1, i.e.,
kI s + kI
CP I (s) = 1 + = .
s s
The Bode plot of CP I for different values of kI is shown in Fig. 18-2. Integral
control is used to change the system type by changing the slope of the loop gain
by −20 dB/dec at low frequencies. The Bode plot for integral control is shown
in Fig. 18-2 for different values of kI . It is clear that integral control changes the

TOC
CHAPTER 18. COMPENSATOR DESIGN 323

Proportional Gain
50
kP = 100
40

30

Magnitude (dB)
20
kP = 10
10

0 kP = 1
-10

-20 kP = 0.1
-30
90

45
Phase (deg)

-45

-90
10 0 10 1
Frequency (rad/s)

Figure 18-1: The Bode plot for proportional gain at different values of kP . The
proportional gain adds a constant to the loopshape at all frequencies and does not
affect the phase at any frequency.

slope for frequencies ω < kI while not affecting the loop gain when ω > kI . Note
also that integral control decreases the phase at low frequencies, which negatively
impacts stability. Therefore, integral control is usually applied at low frequencies,
where kI  ωco .

18.1.3 Building Block #3: Low-Pass Filter

The third basic building block for loopshaping design is a low-pass filter where
p
CLP F (s) = .
s+p

Note that the low-pass filter is constructed so that the DC-gain is one. The Bode
plot for a low-pass filter is shown in Fig. 18-3 for different values of p. It is evident
from Fig. 18-3 that a low-pass filter decreases the loop gain when ω > p while
leaving the loop gain unaffected when ω < p. Low-pass filters are used at high
frequencies to enhance noise attenuation.

18.1.4 Building Block #4: Phase-Lag Filter

The fourth basic building block for loopshaping design is a phase-lag filter where

s+z
CLag (s) = , (18.1)
s+p

TOC
CHAPTER 18. COMPENSATOR DESIGN 324

Integral Control
140

120

100
Magnitude (dB)

80 kI = 10
60 kI = 1
40 kI = 0.1
20 kI = 0.01
0
0

kI = 0.01
-30
kI = 0.1
Phase (deg)

kI = 1
-60
kI = 10

-90
10 -4 10 -3 10 -2 10 -1 10 0 10 1 10 2 10 3
Frequency (rad/s)

Figure 18-2: The Bode plot for integral control at different values of kI . Inte-
gral control changes the system type by decreasing the slope of the loopshape by
−20 dB/dec for frequencies below kI , while leaving high frequencies unaffected.

Low Pass Filter

0

-10 p=1
p=5
-20
p = 10
p = 100
-30 p = 103
p = 104
Magnitude (dB)

-40

-50

-60

-70

-80

-90

-100
0

-30 p=1
Phase (deg)

p=5
p = 10
-60 p = 100
p = 103
p = 104
-90
10 -2 10 -1 10 0 10 1 10 2 10 3 10 4 10 5 10 6
Frequency (rad/s)

Figure 18-3: The Bode plot for a low-pass filter at different values of the pole
p. A low-pass filter decreasing the slope of the loopshape by −20 dB/dec for
frequencies above p, while leaving low frequencies unaffected.

TOC
CHAPTER 18. COMPENSATOR DESIGN 325

where z > p > 0. Note that the phase-lag filter is constructed so that

z
s+z 1+ s
lim = lim p = 1.
s→∞ s + p s→∞ 1 +
s

Therefore, phase-lag filters change the loop gain at low frequencies while leav-
ing high frequencies unaffected. The Bode plot for a phase-lag filter is shown in
Fig. 18-4 when z = 1 and when p = z/M for different values of pole-zero sepa-
ration M . Note that phase-lag filters increase the gain at low frequencies and that
the total amount of gain is equal to the separation between the zero and the pole.
Specifically, the increased gain on the Bode plot is equal to 20 log10 M . Phase-lag
filters are used to enhance tracking performance and input/output disturbance re-
jection. Unfortunately, phase-lag filters decrease the phase between the zero and
the pole and will therefore negatively affect stability if the zero is too close to the
crossover frequency. Therefore, phase-lag filters are typically added at low fre-
quencies and are characterized by the zero z where the effect of the lag ends and
the pole-zero separation M which indicates the maximum gain at low frequencies.
Accordingly, the lag filter (18.1) can be written as

s+z
CLag (s) = .
s + z/M

80 M = 104 Phase Lag Filter

70

60
M = 103
Magnitude (dB)

50

40
M = 100
30

20
M = 10
M =5
10
M =2
0
0

M =2
M =5
-30
Phase (deg)

M = 10
M = 100
-60
M = 103
M = 104
-90
10 -6 10 -5 10 -4 10 -3 10 -2 10 -1 10 0 10 1 10 2
Frequency (rad/s)

Figure 18-4: The Bode plot for a phase-lag filter when the zero is at z = 1 and
the pole is at p = z/M , for different values of M . A phase-lag filter increases the
loop gain at low frequencies while leaving high frequencies unaffected.

TOC
CHAPTER 18. COMPENSATOR DESIGN 326

18.1.5 Building Block #5: phase-lead Filter

The final basic building block for loopshaping design that we will discuss is a
phase-lead filter where

√
M s + ωLead
CLead (s) = √ , (18.2)
s + ωLead M

√
√ we note that the pole p = ωLead M is greater than the zero at z =
where
ωL/ M for M > 1. The phase-lead filter is constructed so that

√ 
 M jω 
 Lead + ωLead 
|CLead (s = jωLead )| =  √ 
 jωLead + ωLead M 
p
ω2 2
+ M ωLead
= p Lead
ω 2 M + ωLead
2
√ Lead
M +1
=√ = 1.
M +1

The Bode plot for a phase-lead filter is shown in Fig. 18-5 when ωLead = 1 for
different values of pole-zero separation M . Note that phase-lead filters increase
the phase at frequencies centered at ωLead . At high frequencies, the magnitude is
increased by

√
M s + ωLead
lim CLead (s) = lim √
s→∞ s + ω
Lead M
s→∞
√
M + ωLead /s
= lim √
s→∞ 1 + ω
Lead M /s
√
= M,

and at low frequencies, the magnitude is decreased by

√
M s + ωLead
lim CLead (s) = lim √
s→0 s→0 s + ωLead M
1
=√ .
M

On the Bode plot this corresponds to an increase of 12 20 log10 M at high frequen-

cies and a decrease of 21 20 log10 M at low frequencies.

TOC
CHAPTER 18. COMPENSATOR DESIGN 327

The maximum increase in phase, which occurs at ωLead is

φLead = ∠CLead (jωLead )

√
M jωLead + ωLead
=∠ √
jωLead + ωLead M
√ √
= ∠( M jωLead + ωLead ) − ∠(jωLead + ωLead M )
√ !  
−1 M ωLead −1 ωLead
= tan − tan √
ωLead ωLead M
√   
−1 −1 1
= tan M − tan √ .
M

Phase-lead filters are used to increase the phase margin and thereby enhance the
stability and robustness of the system. Typically ωLead is selected to be the
crossover frequency ωco to get the maximum benefit to the phase margin. Un-
fortunately, phase-lead filters also increase the loop gain at high frequencies as
shown in Fig. 18-5, which will make the closed-loop system more sensitive to
noise, and decrease the loop gain at low frequencies which decreases tracking and
disturbance rejection properties.

M = 104
M = 103
M = 100
M = 10
M =5
M =2

M = 104 M = 103
M = 100

M = 10
M =5

M =2

Figure 18-5: The Bode plot for a phase-lead filter when ωLead = 1, for differ-
ent values of M . A phase-lead filter increases the phase around ωLead . It also
increases the loop gain at high frequencies and decreases the loop gain at low
frequencies.

TOC
CHAPTER 18. COMPENSATOR DESIGN 328

18.1.6 Simple Example 1

Given the open loop plant
1
P (s) = ,
s+1
design a controller C(s) so that the closed-loop system satisfies the following
constraints:

(1) Tracks a ramp within γ1 = 0.03.

(2) Attenuate noise with frequency content above ωn = 10 rad/sec by γn = 0.1.

(3) Reject input disturbances with frequencies content below ωdin = 0.1 rad/sec
by γdin = 0.1.

(4) Phase margin that is approximately 60 degrees.

The first three design specifications can be displayed on the Bode magnitude
plot as shown in Fig. 18-6. To track a ramp within γ1 = 0.03 requires that the
loop gain be above
 
 1 
B1 = 20 log10   − 20 log10 |ω|
0.03 

as ω → 0. Therefore, to satisfy this specification, we need to find C so that the

Bode plot for P C rises above the green line on the top left of Fig. 18-6.
To attenuate noise with frequency content above γn = 10 rad/sec by γn = 0.1,
the loop gain should be below

Bn = 20 log10 |0.1| = −20 dB

for ω > 10 rad/sec. Therefore, we need to design C so that the Bode plot for P C
is below the green line in the bottom right of Fig. 18-6.
To reject input disturbances with frequency content below ωdin = 0.1 rad/sec
by γdin = 0.1, requires that the loop gain is above
 
 1 
20 log10   + 20 log10 |P (jωd )| = 20 − 0 = 20 dB
0.1  in

for ω < 0.1 rad/sec. Therefore we need to design C so that the Bode plot for P C
is above the green line in the left-middle of Fig. 18-6.
To increase the type of the system so that the slope of P C is −20 dB/dec as
ω → 0, integral control can be added. Fig. 18-7 shows the loop gain of P C when
 
s + 0.4
C(s) = CP (s)CP I (s) = (2) .
s

TOC
 CHAPTER 18. COMPENSATOR DESIGN 329

Bode Diagram
100

To track a ramp,
80
the loop gain should exceed
1
20 log10 | .03 | 20 log10 |!|
<latexit sha1_base64="NoKQ65eLZUxRXHvVr7FlnfdtK9s=">AAACknicbVFNbxMxEPVugZbw0VC4cbFIkDhA5E1VgTiUol44cChS01bKRtGsd3Zj1R8r24uINvuD+ne48W9wPg4kZSRLT+/NjGfeZJUUzjP2J4r3Hjx8tH/wuPPk6bPnh90XR1fO1JbjiBtp7E0GDqXQOPLCS7ypLILKJF5nt+dL/fonWieMvvTzCicKSi0KwcEHatq9SyuwqTZC56g9vTTUW+C3FKgFVb2nnW3dz5BKYypagtDUzUwtc4q/OGK+m9ofslSactokrF2kRejaJG0zYMft4sOWZBSWsOjvlIOj/bVEU28o60+7PTZgq6D3QbIBPbKJi2n3d5obXqvQjUtwbpywyk8asF5wiW0nrR1WYVcocRygBoVu0qwsbenbwOS0MDa8MM2K/beiAeXcXGUhU4GfuV1tSf5PG9e++DRphK5qj5qvPypqScOOy/vQXFjkXs4DAG5FmJXyGQT3fLhiJ5iQ7K58H1wNB8nx4OTHsHfGNnYckNfkDXlHEvKRnJFv5IKMCI8Oo5PoNPoSv4o/x1/j83VqHG1qXpKtiL//BRO6xkc=</latexit>
as ! ! 0
60
Magnitude (dB)

40

To reject input disturbances To attenuate noise,

the loop gain should exceed
20 1 the loop gain should be below
20 log10 | .1 |
20 log10 |0.1|
for !  0.1 rad/sec
<latexit sha1_base64="LK0Tpn6AEVEwM+jtoQKqM8uPNrs=">AAACk3icbVFNb9QwEHVSoGX5CkWcuFjsInEK8SIKEoeugAMXpCJ120qb1WriTLKmjh1sB3WVhh/Ez+HGv8Gb7oFumdPTmzee5zdZLYV1SfInCHdu3b6zu3d3cO/+g4ePosf7J1Y3huOUa6nNWQYWpVA4dcJJPKsNQpVJPM3OP677pz/QWKHVsVvVOK+gVKIQHJynFtGvtAaTKi1UjsrRY00NfkPuqFB142juHTQmA8XRDq5L3RKp1LqmJQhF7VI3Mqd4wRFzuiUdjZNU6nLRsqS7TAsDvGVdG7PucrSlLLSho1RXWEIq8TtNYjb6aSB/ZZEvomESJ33Rm4BtwJBs6mgR/U5zzZvKP8wlWDtjSe3mLRgnuMRukDYWa+DnUOLMQwUV2nnbZ9rRF57Jez+F9sZ69t+JFiprV1XmlRW4pd3urcn/9WaNK97N2z5eVPxqUdFI6jRdH8hHbnz+cuUBcCO8V8qX4DNz/owDHwLb/vJNcDKO2ev4zdfxcJJs4tgjz8hz8pIw8pZMyGdyRKaEB1FwEBwGk/Bp+D78EH66kobBZuYJuVbhl7801chP</latexit>

<latexit sha1_base64="4VFBeZ6cl1joOXyqXR3ZCe+EXwk=">AAACgnicbVFNTxsxEPVuodDQj9Aee7FIkCpULXYQoocekHrpkUoEkLJRNOud3Vh47a3tbRUt4X/wt7jxa1on5FBCn3x4evPGHr/JaiWdZ+whil9sbL7c2n7V2Xn95u277u77C2caK3AojDL2KgOHSmoceukVXtUWocoUXmbX3xb1y19onTT63M9qHFdQallIAT5Ik+5dWoNNtZE6R+3puaHgPeoGPNKgOvzceerwU6TKmJqWIDV1U9OonGYYjjK/6Zq5P2CpMuWk5Wx+wxJ+018zFMbSfmoqLCEt8SflrH9rIT90KCbdHkvYEvQ54SvSIyucTbr3aW5EU4V7hQLnRpzVftyC9VIonHfSxmEN4hpKHAWqoUI3bpcRzul+UPLlOIUJcy3VfztaqJybVVlwVuCnbr22EP9XGzW++DJupa6bEKt4fKhoFPWGLvZBc2lReDULBISVYVYqpmBB+LC1TgiBr3/5ObkYJPwoOf4x6J2yVRzb5CPZI58IJyfklHwnZ2RIBPkT7UdJdBhvxAcxj48erXG06vlAniD++hd9EMFb</latexit>
for ! 10 rad/sec
0

20 log10 |P (j!)|

-20

-40
10 -2 10 -1 10 0 10 1 10 2
Frequency (rad/s)

Figure 18-6: The Bode plot for the open loop plant in Example 1, together with
the design specifications.

To increase the loop gain at low frequencies to get above the 1/s line as ω →
0, a phase-lag filter can be added. Fig. 18-8 shows the loop gain of P C when
  
s + 0.4 s + 0.8
C(s) = CP (s)CP I (s)CLag (s) = (2) .
s s + 0.8/50

From Fig. 18-8 we see that the closed-loop system will satisfy the tracking and
input disturbance specifications.
To decrease the loop gain at high frequencies to achieve the noise attenuation
specification, a low-pass filter can be added at p = 5. Fig. 18-9 shows the loop
gain of P C when

C(s) = CP (s)CP I (s)CLag (s)CLP F (s)

   
s + 0.4 s + 0.8 5
= (2) .
s s + 0.8/50 s+5

The phase margin for this design is P M = 63 degrees and so all of the design
specifications are satisfied, as shown in Fig. 18-9. Python code that implements
this example is shown below.
1 from control import *
2 import matplotlib.pyplot as plt
3 import numpy as np
4
5 Plant = tf([1],[1,1]) #transfer function for plant
6
7 # initialize frequency response for plant only

TOC
 CHAPTER 18. COMPENSATOR DESIGN 330

Bode Diagram
din constraint
100 noise constraint
tracking error constraint
75 P(s)
P(s)CkpCPI
50
Magnitude (dB)

25

25

50

10 4 10 3 10 2 10 1 100 101 102 103

Frequency (rad/s)

Figure 18-7: The Bode gain plot of P and P C when C includes proportional and
integral control.

8 plt.figure()
9 plt.grid(which=’both’)
10
11 ###################################
12 # input disturbance specification #
13 ###################################
14 # reject dist below this frequency
15 omega_din = 10**-1
16 omega = np.logspace(-4, np.log10(omega_din))
17 Pmag, phase, _ = bode(Plant, omega, Plot=False)
18
19 # amount of input disturbance in output
20 gamma_din = 0.1
21 mag_din = 20*np.log10(1/gamma_din)*np.ones(omega.size)+\
22 20*np.log10(Pmag)
23 plt.semilogx(omega, mag_din, color=[0, 1, 0], ls=’:’,
24 label=’$d_{in}$ constraint’)
25
26 #######################
27 # noise specification #
28 #######################
29 # attenuate noise above this frequency
30 omega_n = 10^1
31
32 #attenuate noise by this amount
33 gamma_n = .1

TOC
 CHAPTER 18. COMPENSATOR DESIGN 331

Bode Diagram
din constraint
100 noise constraint
tracking error constraint
75 P(s)
P(s)CkpCPIClag
50
Magnitude (dB)

25

25

50

10 4 10 3 10 2 10 1 100 101 102 103

Frequency (rad/s)

Figure 18-8: The Bode gain plot of P and P C when C includes proportional,
integral, and lag filters.

34 omega = np.logspace(np.log10(omega_n),2+np.log10(omega_n))
35 plt.semilogx(omega, 20*np.log10(gamma_n)*np.ones(omega.size),
36 color=[0, 1, 0], ls=’--’, label=’noise constraint’)
37
38 #######################
39 # steady state errror #
40 #######################
41 #steady state tracking of ramp
42 gamma_1 = .03
43 omega = np.logspace(-4, 0)
44 plt.semilogx(omega, 20*np.log10(1/gamma_1)-20*np.log10(omega),
45 color=[0, 1, 0], ls=’-.’,
46 label=’tracking error constraint’)
47
48 # open-loop freq response for plant only
49 omega = np.logspace(-4, 3)
50 Pmag, phase, omega = bode(Plant,omega, Plot=False)
51 Pmag_dB = 20*np.log10(Pmag)
52 plt.semilogx(omega, Pmag_dB, label=’$P(s)$’)
53
54 ##################
55 # control design #
56 ##################
57 C = 1.0
58
59 #proportional control

TOC
 CHAPTER 18. COMPENSATOR DESIGN 332

Bode Diagram
din constraint
100 noise constraint
tracking error constraint
P(s)
P(s)CkpCPIClagClpf = P(s)Ctotal
50
Magnitude (dB)

50

100
10 4 10 3 10 2 10 1 100 101 102 103
Frequency (rad/s)

Figure 18-9: The Bode gain plot of P and P C when C includes proportional,
integral, lag, and low-pass filters.

60 C_kp = 2.0
61
62 #integral control
63 k_I = 0.4
64 C_PI = tf([1, k_I], [1, 0])
65
66 #phase lag
67 z = 0.8
68 M = 50.0
69 C_lag = tf([1, z], [1, z/M])
70
71 #low-pass filter
72 p = 5
73 C_lpf = tf([p], [1, p])
74
75 #total filter:
76 # for figure 18-7
77 C1 = C*C_kp*C_PI
78
79 # for figure 18-8
80 C2 = C*C_kp*C_PI*C_lag
81
82 # for figure 18-9
83 C3 = C*C_kp*C_PI*C_lag*C_lpf
84
85 # open-loop freq response of the controller + plant

TOC
 CHAPTER 18. COMPENSATOR DESIGN 333

86 omega = np.logspace(-4, 3)
87 PC_mag, phase, _ = bode(Plant*C3, omega, Plot=False)
88 plt.semilogx(omega, 20*np.log10(PC_mag),
89 label=’$P(s)C_{k_p}C_{PI}C_{lag}C_{lpf}= P(s)C(s)$’)
90
91 plt.title(’Bode Diagram’)
92 plt.xlabel(’Frequency (rad/s)’)
93 plt.ylabel(’Magnitude (dB)’)
94 plt.legend()
95
96 plt.show()

Listing 18.1: example_18_1_6.py

18.1.7 Controller Implementation

The loopshaping design procedure produces a controller C(s) that will have mul-
tiple elements including integrators, lead and lag filters, and low-pass filters. Each
of these elements are straightforward to implement using analog circuits. How-
ever, if the controller is to be implemented on digital hardware using computer
code, what is the best way to proceed? While there are many possibilities, one
option that is straightforward to implement and that fits well with the previous
chapters, is to implement the controller using state space equations. The con-
troller
U (s) = C(s)E(s)
is first converted to continuous time state space equations using, for example,
control canonical form, to produce the equations

żC = AC zC + BC e (18.3)
u = CC zC + DC e (18.4)

where e(t) = r(t) − y(t) is the input to the state space equations, and the control
signal u(t) is the output. In the example of the previous section we get

10s2 + 12s + 3.2

C(s) = .
s3 + 5.016s2 + 0.08s
Therefore, using control canonic form, the controller implementation is
   
−5.016 −0.08 0 1
żC =  1 0 0 zC + 0 e
0 1 0 0


u = 10 12 3.2 zC .

In Python, the command to find the state space equations from the controller trans-
fer function ‘C’ is given by

TOC
 CHAPTER 18. COMPENSATOR DESIGN 334

1 C_ss = tf2ss(C)
2 A_C = C_ss.A
3 B_C = C_ss.B
4 C_C = C_ss.C
5 D_C = C_ss.D

The controller is then implemented by integrating the differential equations

between sample times. If Ts is the sample time, and N = 10 Euler steps are used
between each sample, then Python code that implements the controller is given by
Python code that could be used for the design of the controller is shown below:
1 # assuming that self.z_C (the controller state) has been
2 # initialized elsewhere and that the controller ss equations
3 # are available in the Python class
4 def u = updateController(self, r, y):
5 # calc error signal
6 error = r - y
7
8 # Euler integration steps per control calculation
9 N = 10
10
11 # integrate differential equation for controller
12 for i in range(N):
13 self.z_C = self.z_C + Ts / N * (self.A_C * self.z_C \
14 + self.B_C * error)
15 u = self.C_C * self.z_C + self.D_C * error
16
17 return u

Listing 18.2: ss_control_example.py

The inputs to the controller are the reference signal r, the output y, and the cur-
rent time t. The error signal is computed on Line 5. The control dynamics (18.3)
are integrated using N Euler steps in Lines 11–13, and the control output equation
is computed in Line 14.

18.1.8 Prefilter Design

There are applications where it is very difficult to meet the design specifications
at both low and high frequencies and still achieve a phase margin that is close to
60 degrees. If for example, the phase margin is close to 45 degrees, then there
will be a peak in the closed-loop Bode plot. As shown in Fig. 17-7, the peak in
the closed-loop Bode plot produces overshoot and ringing in the closed-loop step
response. One method for reducing the overshoot in the closed-loop step response
is to add a prefilter F (s) as shown in Fig. 18-10. The role of the prefilter F (s)
is to attenuate frequencies in r(t) that resonate with the closed-loop system and
produce the overshoot. From a Bode plot perspective, the goal of the prefilter
is to flatten the closed-loop Bode plot so that the peak is removed. There are
several standard forms used for the prefilter. These include a notch filter and a
low-pass filter. For simplicity, we will discuss the use of a low-pass filter below.

TOC
CHAPTER 18. COMPENSATOR DESIGN 335

din (t) dout (t)

r(t) u(t) y(t)

F (s) C(s) P (s)

n(t)

Figure 18-10: The closed-loop system with a prefilter F (s) on the reference com-
mand r.

As explained previously, the transfer function for a low-pass filter is given by

p
F (s) = , (18.5)
s+p
where the DC-gain is equal to one.
A notch filter can be constructed by concatenating a phase-lag and a phase
-lead filter   
ps pe (s + zs )(s + ze )
F (s) = ,
zs ze (s + ps )(s + pe )
where zs > ps and ze < pe . If ωnotch is the desired center frequency of the notch,
D is the frequency separation between ps and zs , and between ze and pe , and M
is the frequency separation between zs and ze , then
1
ps = √ ωnotch
M
D
zs = √ ωnotch
M
√
pe = M ωnotch
√
M
ze = ωnotch ,
D
where we have assumed that M > D. Therefore the notch filter can be written as
 √ 
s2 + √DM + DM ωnotch s + ωnotch
2

F (s) =  √  , (18.6)
s2 + √1M + M ωnotch s + ωnotch2

where Fig. 18-11 shows the straight-line approximation for the notch. Note that

lim F (s) = lim F (s) = 1.

s→0 s→∞

Fig. 18-12 shows a notch filter when ωnotch = 1, D = 10, and M =

10, 50, 100, 200, 1000. Note that as M increases the depth of the notch increases
but does not flatten out in between the beginning and end of the notch.

TOC
CHAPTER 18. COMPENSATOR DESIGN 336

Bode Diagram
20

15

20 log10 |F (j!)| 10

5 D D
Magnitude (dB)

-5 M
-10

-15

-20
90

45
\F (j!)
Phase (deg)

-45

-90
10 -3 10 -2 10 -1 10 0 10 1 10 2 10 3
ps zs !notch
Frequency (rad/s) ze pe

Figure 18-11: Straight-line Bode approximation for a notch filter.

Notch Filter
0

-5
Magnitude (dB)

-10

-15

-20
90

45
Phase (deg)

-45

-90
10 -3 10 -2 10 -1 10 0 10 1 10 2 10 3
Frequency (rad/s)

Figure 18-12: The Bode plot for a notch filter when ωnotch = 1, D = 10, and
for different values of M . A notch is often used as a prefilter on the reference
command r.

TOC
 CHAPTER 18. COMPENSATOR DESIGN 337

Notch Filter
0
D = 1000
-5
D = 500

Magnitude (dB)
-10

-15 D = 200

-20 D = 50

-25 D = 10
90

45
Phase (deg)

D = 1000
0
D = 500
D = 200
-45 D = 50
D = 10
-90
10 -3 10 -2 10 -1 10 0 10 1 10 2 10 3
Frequency (rad/s)

Figure 18-13: The Bode plot for a notch filter when ωnotch = 1, M = 1000, and
for different values of D.

Alternatively, Fig. 18-13 shows a notch filter when ωnotch = 1, M = 1000,

and D = 10, 50, 100, 200, 1000. Note that as D decreases the depth of the notch
increases and flatten out in between the beginning and end of the notch.
Just as the controller C(s) was implemented using state space equations, the
prefilter can also be implemented using state space equations. The prefilter

Rf (s) = F (s)R(s)

is converted to continuous time state space equations using, for example, control
canonical form, to produce the equations

żF = AF zF + BF r (18.7)
rf = CF zF + DF r (18.8)

where the reference r(t) is the input to the state space equations, and the fil-
tered reference signal rf (t) is the output. For the transfer function given in Equa-
tion (18.6) we get

żF = −pzF + r
rf = pzF .

In Python, the command to find the state space equations are given by (where
F is the transfer function object representing the prefilter)
1 F_ss = tf2ss(F)
2 A_F = F_ss.A

TOC
 CHAPTER 18. COMPENSATOR DESIGN 338

3 B_F = F_ss.B
4 C_F = F_ss.C
5 D_F = F_ss.D

The prefilter is then implemented by integrating the differential equations between

sample times. If Ts is the sample time, and N = 10 Euler steps are used between
each sample, then Python code that implements the controller plus prefilter is
given by
1 # assuming that self.z_C and self.z_F have been initialized
2 # elsewhere and that the controller AND prefilter ss equations
3 # (e.g. A_C, B_C, C_C, D_C, A_F, B_F, etc.) are available from
4 # the Python class
5
6 def u = updateController(self, r, y):
7
8 # Euler integration steps per filter calculation
9 N = 10
10
11 # integrate differential equation for controller
12 for i in range(N):
13 # prefilter the reference command
14 self.z_F = self.z_F + Ts / N * (self.A_F * self.z_F \
15 + self.B_F * r)
16 r_filtered = self.C_F * self.z_F + self.D_F * r
17
18 # error signal uses filtered reference
19 error = r_filtered - y
20 self.z_C = self.z_C + Ts / N * (self.A_C * self.z_C \
21 + self.B_C * error)
22
23 u = self.C_C * self.z_C + self.D_C * error
24
25 return u

Listing 18.3: ss_prefilter_example.py

Prefilter Example
As an example, consider the open loop system P C shown in Fig. 18-14. The
phase margin is P M = 42.5 degrees resulting in a peak in the closed-loop fre-
quency response, as shown in Fig. 18-14. The corresponding step response is
shown in Fig. 18-15, where it can be seen that there is significant ringing in the
step response.
To remove the peak in the closed-loop frequency response, a prefilter is added
to the system as shown in Fig. 18-10, where the low-pass filter pole is p = 9,
resulting in
9
F (s) = .
s+9
The Bode magnitude of the prefilter F is shown in Fig. 18-14, as well as the pre-
filtered closed-loop response F P C/(1 + P C). As can be seen from Fig. 18-14,

TOC
CHAPTER 18. COMPENSATOR DESIGN 339

the prefilter removes the peak in the closed-loop frequency response. The cor-
responding step response is shown in Fig. 18-15, where it can be seen that the
ringing has been significantly reduced.

Bode Diagram
Gm = Inf dB (at Inf rad/s) , Pm = 41.5 deg (at 12.5 rad/s)

30

20

PC PC
10

1 + PC
Magnitude (dB)

-10

F
-20
PC
F
1 + PC
-30

10 0 10 1 10 2
Frequency (rad/s)

Figure 18-14: The loop gain for the open loop system P C, the closed-loop sys-
tem P C/(1 + P C), the prefilter F , and the closed-loop system with the prefilter
F P C/(1 + P C).

18.1.9 Successive Loop Closure Design

When loopshaping is used to design an inner and outer loop controller in a suc-
cessive loop closure design, the design of the outer loop can take into account
the complete dynamics of the inner loop. This is in contrast to the design of PID
controllers in Chapter 8 where the inner loop was replaced with the DC-gain.
Fig. 18-16 shows the system architecture for loopshaping design. The inner and
outer loop controllers are accompanied by the prefilter F (s) which is only on the
outer loop. The first step is to design the inner loop controller Cin (s) using the
plant Pin (s). However, for the design of the outer loop controller Cout (s), we can
use the outer loop plant in cascade with the inner closed-loop system. Therefore,
the plant for the outer loop design is given by
 
Pin Cin
P = Pout .
1 + Pin Cin

TOC
CHAPTER 18. COMPENSATOR DESIGN 340

Step Response
1.4

1.2 PC
1 + PC
1

PC
0.8 F
Amplitude

1 + PC
0.6

0.4

0.2

0
0 0.5 1 1.5 2 2.5 3
Time (seconds)

Figure 18-15: The step response of the closed-loop system P C/(1 + P C) and the
closed-loop system with the prefilter F P C/(1 + P C).

F Cout Cin Pin Pout

Figure 18-16: The control architecture for a successive loop closure design, with
prefilter.

TOC
CHAPTER 18. COMPENSATOR DESIGN 341

Examples will be given in the sections below.

18.2 Design Study A. Single Link Robot Arm

Example Problem A.18

For this homework assignment we will use loopshaping to improve the PID con-
trollers developed in HW A.10. Let Cpid (s) be the PID controller designed in
HW A.10. The final control will be C(s) = Cpid (s)Cl (s) where Cl is designed
using loopshaping techniques.

(a) Design Cl (s) to meet the following objectives:

(1) Improve tracking and disturbance rejection by a factor of 10 for refer-

ence signals and disturbances below 0.07 rad/sec,
(2) Improve noise attention by a factor of 10 for frequencies above 1000
radians/sec.
(3) Phase margin that is approximately P M = 60 degrees.

(b) Add zero mean Gaussian noise with standard deviation σ 2 = 0.01 to the
simulation developed in HW A.10.

(c) Implement the controller C(s) in simulation using its state space equivalent.

(d) Note that despite having a good phase margin, there is still significant over-
shoot, due in part to the windup effect in the phase-lag filter. This can be
mitigated by adding a prefilter, that essentially modifies the hard step in-
put into the system. Add a low pass filter for F (s) as a prefilter to flatten
out the closed loop Bode response and implement in simulation using its
discrete-time state space equivalent.

Solution
The first two design specifications can be displayed on the Bode magnitude plot as
shown in Fig. 18-17. To improve the tracking and disturbance rejection by a factor
of 10 for reference signals and disturbances below ωr = 0.07 rad/sec, the loop
gain must be 20 dB above P (s)CP ID (s), as shown by the green line in the top
left of Fig. 18-17. To improve noise attenuation by a factor of 10 for frequencies
above ωn = 1000 rad/sec, the loop gain must be 20 dB below P (s)CP ID (s), as
shown by the green line in the bottom right of Fig. 18-17.
To increase the loop gain below ωr = 0.07 rad/sec, a phase-lag filter can be
added with zero at z = 1.5 with a separation of M = 40. Fig. 18-18 shows the

TOC
CHAPTER 18. COMPENSATOR DESIGN 342

Bode Diagram
Gm = -Inf dB (at 0 rad/s) , Pm = 46.6 deg (at 6.21 rad/s)
200

150 Loop gain is 20 dB above

P CP ID for ! < 0.07.
<latexit sha1_base64="2Iz5xtiZrx+4v14PFqolKyiQBVI=">AAACQHicbVA9TyMxFPTyTY6PACWNRfYkqmg3CEFBgYACJIqcdCFI2Sh6630bLLz2yvYiRav8NBp+Ah01DcWdEC0VTkjB16tGM/P8xhPnghsbBA/e1PTM7Nz8wmLl19Lyymp1bf3CqEIzbDEllL6MwaDgEluWW4GXuUbIYoHt+Pp4pLdvUBuu5F87yLGbQV/ylDOwjupV21EOOpKKywSlpedK5bQPXFJuqN8IfJocUYjVDVJa+Wz1m8e9snl2MvRpqjT1I5VhHw6CerDn1yu9as2h8dDvIJyAGplMs1e9jxLFisw9zQQY0wmD3HZL0JYzgcNKVBjMgV1DHzsOSsjQdMtxAUP62zHJOEaqXLQx+3GjhMyYQRY7Zwb2ynzVRuRPWqew6X635DIvLEr2figtBLWKjtqkCdfIrBg4AExzl5WyK9DArOt8VEL49cvfwUWjHu7Ud/80aofBpI4Fskm2yDYJyR45JKekSVqEkVvySP6R/96d9+Q9ey/v1ilvsrNBPo33+gZFL6vQ</latexit>

100

50
Magnitude (dB)

-50

-100
Loop gain is 20 dB below
P CP ID for ! > 1000.
-150
<latexit sha1_base64="GFL0efVA+CH/QUNuCRLtPS9HfiM=">AAACQHicbVA9TxtBFNyD8BHz5UCZZhUfEpW1Z4SgihBQBCmFkTBG8lnWu713ZsXe7ml3L5F18k+j4SfQUaehAKG0qbI2LgLkVaOZeftmJymksI6x+2Bu/sPC4tLyx9rK6tr6Rv3T5oXVpeHY4Vpqc5mARSkUdpxwEi8Lg5AnErvJ9fFE7/5AY4VW525UYD+HoRKZ4OA8Nah34wJMrLRQKSpHv2td0CEIRYWlYYuFND2iCUr9k9Laa2vYPh5U7dOTcUgzbWgY6xyH8DVijIXN2qDeYE02HfoeRDPQILNpD+p3cap5mfunuQRrexErXL8C4wSXOK7FpcUC+DUMseehghxtv5oWMKbbnkmnMTLto03ZfzcqyK0d5Yl35uCu7FttQv5P65UuO+hXQhWlQ8VfDmWlpE7TSZs0FQa5kyMPgBvhs1J+BQa4851PSojefvk9uGg1o93m3lmrcchmdSyTz+QL2SER2SeH5Btpkw7h5Ib8Io/kKbgNHoLn4PeLdS6Y7WyRVxP8+QtWlKva</latexit>

-200
10 -2 10 0 10 2 10 4
Frequency (rad/s)

Figure 18-17: The Bode plot for the open loop plant in HW A.18, together with
the design specifications.

loop gain of P C when

C(s) = CP ID (s)CLag (s)
  
0.179s2 + 0.554s + 0.25 s + 1.5
= .
0.05s2 + s s + 0.0375
From Fig. 18-18 we see that the closed loop system will satisfy the tracking and
input disturbance specifications.
The phase margin after adding the lag filter is P M = 41.3 degrees. To
increase the phase margin, a phase-lead filter is added with center frequency
ωc = 40 rad/sec and a separation of M = 10. The center frequency for the
lead filter is selected to be above the cross over frequency so as not to change the
location of the cross over frequency, thereby keeping the closed loop bandwidth,
and therefore the control effort, roughly the same as with the PID controller. After
adding the lead filter, the controller is
C(s) = CP ID (s)CLag (s)CLead (s)
   
0.179s2 + 0.554s + 0.25 s + 1.5 10s + 126.5
= ,
0.05s2 + s s + 0.0375 s + 126.5

TOC
CHAPTER 18. COMPENSATOR DESIGN 343

Gm = -21.37 dB (at 2.01 rad/s), Pm = 41.28 deg (at 10.87 rad/s)

CpidP
100 noise spec
Magnitude (dB)

din spec
0 CpidClagP

100

10 2 100 102 104

225
180
135
90
Phase (deg)

45
0
45
90
135
180
10 2 100 102 104
Frequency (rad/sec)

Figure 18-18: The Bode plot for HW A.18 where C = CP ID Clag .

and the corresponding loop gain is shown in Fig. 18-19.

In order to satisfy the noise attenuation specification, we start by adding a low
pass filter at p = 50. The corresponding loop gain is shown in Fig. 18-20, from
which we see that the noise specification is not yet satisfied. Therefore, we add
an additional low pass filter at p = 150 to obtain the loop gain in Fig. 18-21. The
phase margin is P M = 64 degrees. The corresponding controller is
C(s) = CP ID (s)CLag (s)CLead (s)Clpf (s)Clpf 2 (s)
   
0.179s2 + 0.554s + 0.25 s + 1.5 s + 0.7
=
0.05s2 + s s + 0.0375 s + 0.07
   
10s + 126.5 50 150
· .
s + 126.5 s + 50 s + 150
Note that we have selected the filter values to leave the cross over frequency the
same as with the original PID controller.
The closed loop frequency response P C/(1 + P C) and the corresponding
step response and control effort are shown by the blue lines in Fig. 18-22. The
overshoot in the step response is caused by the small amount of peaking on the
closed loop bode plot. The peaking can be reduced by adding a prefilter, which in
this case is a low pass filter with pole p = 3. The prefiltered closed loop frequency
response F P C/(1 + P C) and the corresponding step response and control effort
are shown by the red lines in Fig. 18-22. As shown by Fig. 18-22, the prefilter
reduces the overshoot and lowers the control effort.

TOC
 CHAPTER 18. COMPENSATOR DESIGN 344

Gm = -24.10 dB (at 1.73 rad/s), Pm = 81.50 deg (at 15.00 rad/s)

CpidP
100 noise spec
Magnitude (dB)

din spec
CpidClagCleadP
0

100

10 2 100 102 104

270
225
180
135
Phase (deg)

90
45
0
45
90
135
180
10 2 100 102 104
Frequency (rad/sec)

Figure 18-19: The Bode plot for HW A.18 where C = CP ID Clag Clead .

Python code that could be used for the design of the controller is shown below:

1 import sys
2 sys.path.append(’..’) # add parent directory
3 sys.path.append(’../hw16’) # add parent directory
4 import armParamHW16 as P16
5 import matplotlib.pyplot as plt
6 from control import tf, bode, margin, step_response, mag2db
7 import numpy as np
8 import loopshape_tools as ls
9
10 # flag to define if using dB or absolute scale for M(omega)
11 dB_flag = P16.dB_flag
12
13 # assigning plant and controller from past HW
14 # (to make sure we don’t introduce additional errors)
15 Plant = P16.Plant
16 C_pid = P16.C_pid
17
18 ###################################################################
19 # Control Design
20 ###################################################################
21 C = C_pid * ls.lead(w_L=10.0, M=3) * ls.lag(z=1.3, M=25) * ls.lpf(p=100) * ls.lpf(p=350
22
23 ###########################################################

TOC
 CHAPTER 18. COMPENSATOR DESIGN 345

Gm = 20.30 dB (at 82.99 rad/s), Pm = 65.09 deg (at 14.31 rad/s)

CpidP
100 noise spec
Magnitude (dB)

din spec
0 CpidClagCleadClpfP

100
200
10 2 100 102 104
270
225
180
135
Phase (deg)

90
45
0
45
90
135
180
10 2 100 102 104
Frequency (rad/sec)

Figure 18-20: The Bode plot for HW A.18 where C = CP ID Clag Clead Clpf .

24 # add a prefilter to eliminate the overshoot

25 ###########################################################
26 F = tf(1, 1) * ls.lpf(p=1)
27
28
29 ##############################################
30 # Convert Controller to State Space Equations if following
31 # method in 18.1.7
32 ##############################################
33 # C_ss = tf2ss(C) # convert to state space
34 # F_ss = tf2ss(F) # convert to state space
35 C_num = np.asarray(C.num[0])
36 C_den = np.asarray(C.den[0])
37 F_num = np.asarray(F.num[0])
38 F_den = np.asarray(F.den[0])
39
40
41 if __name__=="__main__":
42 # calculate bode plot and gain and phase margin
43 # for original PID * plant dynamics
44 mag, phase, omega = bode(Plant * C_pid, dB=dB_flag,
45 omega=np.logspace(-3, 5),
46 plot=True, label="$C_{pid}P$")
47

TOC
 CHAPTER 18. COMPENSATOR DESIGN 346

Gm = 14.69 dB (at 56.29 rad/s), Pm = 59.69 deg (at 14.24 rad/s)

200
100
Magnitude (dB)

0
CpidP
100 noise spec
din spec
200 CpidClagCleadClpfClpf2P
10 2 100 102 104
270
225
180
135
Phase (deg)

90
45
0
45
90
135
180
10 2 100 102 104
Frequency (rad/sec)

Figure 18-21: The Bode plot for HW A.18 where C =

CP ID Clag Clead Clpf Clpf 2 .

48
49 gm, pm, Wcg, Wcp = margin(Plant * C_pid)
50 print("for original C_pid system:")
51 if dB_flag is True:
52 print(" pm: ", pm, " Wcp: ", Wcp,
53 "gm: ", mag2db(gm), " Wcg: ", Wcg)
54 elif dB_flag is False:
55 print(" pm: ", pm, " Wcp: ", Wcp,
56 "gm: ", gm, " Wcg: ", Wcg)
57
58
59 #########################################
60 # Define Design Specifications
61 #########################################
62 #----------- noise specification --------
63 omega_n = 1000 # attenuate noise above this frequency
64 mag, phase, omega = bode(Plant * C_pid, dB=dB_flag,
65 omega=[omega_n])
66 gamma_n = 0.1*mag[0] # attenuate noise by this amount
67 ls.spec_noise(gamma_n, omega_n, dB_flag)
68
69 #----------- general tracking specification --------
70 omega_d = 0.07 # track signals below this frequency

TOC
 CHAPTER 18. COMPENSATOR DESIGN 347

0
Closed-Loop Bode Plot

100
closed-loop YR - no pre-filter
200 closed-loop YR - with pre-filter
10 3 10 2 10 1 100 101 102 103 104
1.0
Step Response

0.5
response without prefilter
response with prefilter
0.0
0.00 0.25 0.50 0.75 1.00 1.25 1.50 1.75 2.00
6 control effort without prefilter
control effort with prefilter
Control Effort

4
2
0
2
0.00 0.25 0.50 0.75 1.00 1.25 1.50 1.75 2.00

Figure 18-22: On the left is the closed loop frequency response in blue, and the
prefiltered closed loop frequency response in red. In the middle is the associated
closed-loop step response. On the right is the associated control effort.

71 gamma_d = 0.1 # tracking improvement over original system

72 ls.spec_input_disturbance(gamma_d, omega_d,
73 Plant*C_pid, dB_flag)
74
75 ## plot the effect of adding the new compensator terms
76 mag, phase, omega = bode(Plant * C, dB=dB_flag,
77 omega=np.logspace(-3, 5),
78 plot=True, label="$C_{final}(s)P(s)$",
79 color=’orange’)
80
81 gm, pm, Wcg, Wcp = margin(Plant * C)
82 print("for final C*P:")
83 if dB_flag is True:
84 print(" pm: ", pm, " Wcp: ", Wcp,
85 "gm: ", mag2db(gm), " Wcg: ", Wcg)
86 elif dB_flag is False:
87 print(" pm: ", pm, " Wcp: ", Wcp,
88 "gm: ", gm, " Wcg: ", Wcg)
89
90 fig = plt.gcf()
91 fig.axes[0].legend()

TOC
 CHAPTER 18. COMPENSATOR DESIGN 348

92 #plt.show()
93
94
95 ############################################
96 # now check the closed-loop response with prefilter
97 ############################################
98 # Closed loop transfer function from R to Y - no prefilter
99 CLOSED_R_to_Y = (Plant * C / (1.0 + Plant * C))
100 # Closed loop transfer function from R to Y - with prefilter
101 CLOSED_R_to_Y_with_F = (F * Plant * C / (1.0 + Plant * C))
102 # Closed loop transfer function from R to U - no prefilter
103 CLOSED_R_to_U = (C / (1.0 + Plant * C))
104 # Closed loop transfer function from R to U - with prefilter
105 CLOSED_R_to_U_with_F = (F*C / (1.0 + Plant * C))
106
107 plt.figure(4)
108 plt.clf()
109 plt.grid(True)
110 plt.subplot(311)
111 mag, phase, omega = bode(CLOSED_R_to_Y, dB=dB_flag, plot=False)
112 if dB_flag:
113 plt.semilogx(omega, mag2db(mag), color=[0, 0, 1],
114 label=’closed-loop $\\frac{Y}{R}$ - no pre-filter’)
115 else:
116 plt.loglog(omega, mag, color=[0, 0, 1],
117 label=’closed-loop $\\frac{Y}{R}$ - no pre-filter’)
118 mag, phase, omega = bode(CLOSED_R_to_Y_with_F,
119 dB=dB_flag, plot=False)
120 if dB_flag:
121 plt.semilogx(omega, mag2db(mag), color=[0, 1, 0],
122 label=’closed-loop $\\frac{Y}{R}$ - with pre-filter’)
123 else:
124 plt.loglog(omega, mag, color=[0, 1, 0],
125 label=’closed-loop $\\frac{Y}{R}$ - with pre-filter’)
126 plt.ylabel(’Closed-Loop Bode Plot’)
127 plt.grid(True)
128 plt.legend()
129
130 plt.subplot(312), plt.grid(True)
131 T = np.linspace(0, 2, 100)
132 _, yout_no_F = step_response(CLOSED_R_to_Y, T)
133 _, yout_F = step_response(CLOSED_R_to_Y_with_F, T)
134 plt.plot(T, yout_no_F, color=[0,0,1],
135 label=’response without prefilter’)
136 plt.plot(T, yout_F, color=[0,1,0],
137 label=’response with prefilter’)
138 plt.legend()
139 plt.ylabel(’Step Response’)
140
141
142 plt.subplot(313)
143 plt.grid(True)
144 _, Uout = step_response(CLOSED_R_to_U, T)
145 _, Uout_F = step_response(CLOSED_R_to_U_with_F, T)
146 plt.plot(T, Uout, color=[0, 0, 1],
147 label=’control effort without prefilter’)
148 plt.plot(T, Uout_F, color=[0, 1, 0],

TOC
 CHAPTER 18. COMPENSATOR DESIGN 349

149 label=’control effort with prefilter’)

150 plt.ylabel(’Control Effort’)
151 plt.legend()
152
153 plt.show()

Listing 18.4: loopshape_arm.py

TOC
CHAPTER 18. COMPENSATOR DESIGN 350

18.3 Design Study B. Inverted Pendulum

Example Problem B.18

For this homework assignment we will use the loopshaping design technique to
design a successive loop closure controller for the inverted pendulum.

(a) First consider the inner loop, where Pin (s) is the transfer function of the in-
ner loop derived in HW B.5. Using a proportional and phase-lead controller,
design Cin (s) to stabilize the system with a phase margin close to 60 deg,
and to ensure that noise above ωno = 200 rad/s is rejected by γno = 0.1.
We want the closed-loop bandwidth of the inner loop to be approximately
40 rad/s. Note that since the gain on Pin (s) is negative, the proportional
gain will also need to be negative.

(b) Now consider the design of the controller for the outer loop system. The
’plant’ for the design of the outer loop controller is

Pin Cin
P = Pout .
1 + Pin Cin

Design the outer loop controller Cout so that the system is stable with
phase margin close to P M = 60 degrees, and so that reference signals
with frequency below ωr = 0.0032 radians/sec are tracked with error γr =
0.00001, and noise with frequency content above ωno = 1000 radians/sec
are rejected with γno = 0.0001. Design the crossover frequency so that the
rise time of the system is about 2 sec.

Solution
Fig. 18-23 shows the Bode plot of the plant Pin (s) together with the design spec-
ification on the noise attenuation.
The first step is to add a negative proportional gain of 1 to account for the
negative sign in the plant transfer function. Fig. 18-24 shows the corresponding
Bode plot for a proportional gain of K = −1. Once this is applied the phase of
the plant alone is −180 deg over all frequencies.
We want the bandwidth of the inner loop to be approximately 40 rad/s and
so we will set the crossover frequency to be 40 rad/s. To stabilize the system,
we will add lead compensation centered at 40 rad/s with a lead ratio of 13.9 that
corresponds to the desired phase margin of 60 deg. Fig. 18-25 shows the addition
of the phase-lead filter
 s 
10.72 + 1
Clead = −155.12 s ,
120 + 1

TOC
 CHAPTER 18. COMPENSATOR DESIGN 351

Inner Open-loop Bode Plot

50
Magnitude (dB)

100
150 P(s)
noise spec
10 2 100 102 104
Phase (deg)

10 2 100 102 104

Frequency (rad/sec)

Figure 18-23: The Bode plot for the inner loop plant in HW B.18, together with
the design specification.

which has a phase margin of P M = 60 degrees. The gain 155.12 sets the
crossover frequency to be 40 rad/s. The lead compensation alone also satisfies
the noise specification.
Notice that the DC-gain is not equal to one. The closed-loop magnitude re-
sponse for the inner-loop subsystem
Pin Cin
,
1 + Pin Cin
as well as the unit step response for the output and control signal are all shown in
Fig. 18-26.
The Python code used to design the inner loop is shown below.
1 import sys
2 sys.path.append(’..’) # add parent directory
3 sys.path.append(’../hw16’) # add parent directory
4 import pendulumParam as P
5 import pendulumParamHW16 as P16
6 import matplotlib.pyplot as plt
7 from control import tf, step_response, bode, tf2ss, margin, mag2db
8 import numpy as np
9 import helper_functions as hf
10
11
12 # flag to define if using dB or absolute scale for M(omega)

TOC
 CHAPTER 18. COMPENSATOR DESIGN 352

Inner Open-loop Bode Plot

50
Magnitude (dB)

100
P(s)
150 noise spec
1 P(s)
10 2 100 102 104
0
45
Phase (deg)

90
135
180
10 2 100 102 104
Frequency (rad/sec)

Figure 18-24: The Bode plot for the inner loop plant in HW B.18, with a gain of
−1 applied.

13 dB_flag = P16.dB_flag
14
15 # assigning plant and controller from past HW
16 # (to make sure we don’t introduce additional errors)
17 P_in = P16.P_in
18 Plant = P_in
19
20 #########################################
21 # Control Design
22 #########################################
23 C = tf([1], [1])
24
25 # Proportional control: correct for negative sign in plant
26 K_neg = hf.get_control_proportional(-1)
27 C = C*K_neg
28
29 # phase lead: increase PM (stability)
30 w_max = 40 #location of maximum frequency bump
31 phi_max = 60*np.pi/180
32 M = (1 + np.sin(phi_max))/(1 - np.sin(phi_max)) # lead ratio
33 Lead = hf.get_control_lead(w_max, M)
34 C = C*Lead
35
36 # find gain to set crossover at w_max
37 mag, phase, omega = bode(Plant*C*Lead, dB=False,
38 omega=[w_max], plot=False)

TOC
 CHAPTER 18. COMPENSATOR DESIGN 353

Inner Open-loop Bode Plot

0
Magnitude (dB)

50
100
P(s)
150 noise spec
Cfinal(s)P(s)
200
10 2 100 102 104
0
45
Phase (deg)

90
135
180
10 2 100 102 104
Frequency (rad/sec)

Figure 18-25: The Bode plot for the inner loop system in HW B.18, with propor-
tional gain and phase-lead compensation.

39
40 # Proportional control: correct for negative sign in plant
41 K = hf.get_control_proportional(1/mag[0])
42 C = C*K
43
44 ##############################################
45 # Convert Controller to State Space Equations
46 ##############################################
47 C_ss = tf2ss(C) # convert to state space
48
49
50 if __name__=="__main__":
51 # calculate bode plot and gain and phase margin
52 # for original PID * plant dynamics
53 mag, phase, omega = bode(Plant, dB=dB_flag,
54 omega=np.logspace(-3, 5),
55 plot=True, label="$P(s)$")
56
57 gm, pm, Wcg, Wcp = margin(Plant)
58 print("for original system:")
59 if dB_flag == True:
60 print(" pm: ", pm, " Wcp: ", Wcp,
61 "gm: ", mag2db(gm), " Wcg: ", Wcg)
62 elif dB_flag == False:
63 print(" pm: ", pm, " Wcp: ", Wcp,
64 "gm: ", gm, " Wcg: ", Wcg)

TOC
 CHAPTER 18. COMPENSATOR DESIGN 354

closed-loop magnitude ratio YR

0
Magnitude (dB)

50

100
10 1 100 101 102 103 104

Step Response
Amplitude

0
0.00 0.25 0.50 0.75 1.00 1.25 1.50 1.75 2.00

Control Effort
0
Amplitude

250
500
0.00 0.25 0.50 0.75 1.00 1.25 1.50 1.75 2.00

Figure 18-26: The closed loop bode response, the unit step response for the output,
and the unit step response for the input of the inner loop design in HW B.18.

65
66
67 #########################################
68 # Define Design Specifications
69 #########################################
70 # ----------- noise specification --------
71 omega_n = 200 # attenuate noise above this frequency
72 gamma_n = 0.1 # attenuate noise by this amount
73 hf.add_spec_noise(gamma_n, omega_n, dB_flag)
74
75 ## plot the effect of adding the new compensator terms
76 mag, phase, omega = bode(Plant * C, dB=dB_flag,
77 omega=np.logspace(-3, 5),
78 plot=True, label="$C_{final}(s)P(s)$",
79 color=’orange’)
80
81 gm, pm, Wcg, Wcp = margin(Plant * C)
82 print("for final C*P:")
83 if dB_flag == True:
84 print(" pm: ", pm, " Wcp: ", Wcp,
85 "gm: ", mag2db(gm), " Wcg: ", Wcg)
86 elif dB_flag == False:
87 print(" pm: ", pm, " Wcp: ", Wcp,

TOC
 CHAPTER 18. COMPENSATOR DESIGN 355

88 "gm: ", gm, " Wcg: ", Wcg)

89
90 fig = plt.gcf()
91 fig.axes[0].legend()
92 fig.axes[0].set_title(’Inner Open-loop Bode Plot’)
93 plt.show()
94
95 ############################################
96 # now check the closed-loop response
97 ############################################
98 # Open-loop transfer function
99 OPEN = Plant*C
100 # Closed loop transfer function from R to Y
101 CLOSED_R_to_Y = (Plant*C/(1.0+Plant*C))
102 # Closed loop transfer function from R to U
103 CLOSED_R_to_U = (C/(1.0+Plant*C))
104
105 plt.figure()
106 plt.subplot(311)
107 mag, phase, omega = bode(CLOSED_R_to_Y,
108 dB=dB_flag, plot=False)
109 if dB_flag:
110 plt.semilogx(omega, mag2db(mag), color=[0,0,1])
111 else:
112 plt.loglog(omega, mag, color=[0,0,1])
113 plt.grid(True)
114 plt.ylabel(’Magnitude (dB)’)
115 plt.title(’closed-loop magnitude ratio $\\frac{Y}{R}$’)
116
117 plt.subplot(312), plt.grid(True)
118 T = np.linspace(0, 2, 100)
119 _, yout = step_response(CLOSED_R_to_Y, T)
120 plt.plot(T, yout, color=[0,0,1])
121 plt.ylabel(’Amplitude’)
122 plt.title(’Step Response’)
123
124 plt.subplot(313), plt.grid(True)
125 _, Uout = step_response(CLOSED_R_to_U, T)
126 plt.plot(T, Uout, color=[0,0,1])
127 plt.ylabel(’Amplitude’)
128 plt.title(’Control Effort’)
129
130 plt.tight_layout()
131 plt.show()

Listing 18.5: loopshape_in.py

For the outer loop design, Fig. 18-27 shows the Bode plot of the plant P =
Pin Cin
Pout 1+P in Cin
together with the design specification on reference tracking and
noise attenuation. We want a rise time of 2 sec for the system, so this leads us to
select a crossover frequency of 1.1 rad/s. At 1.1 rad/s, the plant alone has a small
negative phase margin (remember that 180 deg and -180 deg are equivalent), so to
bring the phase margin up above 60 deg, we will use a lead compensator centered
at 1.1 rad/s with a lead ratio of 32 to add 70 deg of phase. (A lead ratio of 32
is large. An alternative would be to add two leads that each contribute 35 deg

TOC
CHAPTER 18. COMPENSATOR DESIGN 356

Outer Open-loop Bode Plot

107 P(s)
104 ref tracking spec
noise spec
Magnitude

101
10 2
10 5

10 3 10 1 101 103 105

180
135
Phase (deg)

90
45
0
10 3 10 1 101 103 105
Frequency (rad/sec)

Figure 18-27: The Bode plot for the outer loop plant in HW B.18, together with
the design specification.

of phase margin.) As shown in Fig. 18-28, with the lead applied, the crossover
frequency is pushed out beyond 100 rad/s. By applying a proportional gain of
0.0146, we set the crossover at 1.1 rad/s. As can be seen from Fig. 18-28, with
the lead applied and crossover set at 1.1 rad/s, neither the tracking constraint nor
the noise attenuation constraint is satisfied. We can satisfy the tracking constraint
by applying lag compensation to boost the low-frequency gain near the constraint.
Checking the gain of the lead compensated system at the tracking constraint fre-
quency (ωr = 0.0032 rad/s), we can see that the gain is too low by a factor of 4.8.
Applying a lag compensator with a lag ratio of 8 ensures that this tracking con-
straint is satisfied as shown in Fig. 18-29. Finally, we can apply a low-pass filter
to reduce the effects of sensor noise in the system and satisfy the noise attenuation
constraint. To satisfy the constraint, we need to drop the gain of the open-loop sys-
tem by a factor of 2 at ωno = 1000 rad/s. We can accomplish this with a low-pass
filter with a cut-off frequency of 100 rad/s. This is shown in Fig. 18-29 below.
The resulting compensator is
   
s + 0.194 s + 0.0256 100
Cout (s) = 0.469 .
s + 6.24 s + 0.0032 s + 100

The closed-loop response for both the inner and outer loop systems, as well as
the unit-step response for the output and control signal of the outer loops, are all
shows in Fig. 18-30. A prefilter

TOC
 CHAPTER 18. COMPENSATOR DESIGN 357

Gm = 9.97 dB (at 7.67 rad/s), Pm = 67.59 deg (at 1.10 rad/s)

Outer Open-loop Bode Plot
150
100
Magnitude (dB)

50
0 P(s)
50 ref tracking spec
noise spec
100 CleadP
CleadCkpP
150
10 3 10 1 101 103 105
180
135
90
45
0
Phase (deg)

45
90
135
180
225
270
315
360
10 3 10 1 101 103 105
Frequency (rad/sec)

Figure 18-28: The Bode plot for the outer loop system in HW B.18, with lead
compensation and proportional gain.

2
F (s) =
s+2
has been added to the system. Note the significant decrease in the control effort
required when prefiltering is used with the step input.
Python code that could be used to design the outer loop is shown below. The
Python code used to design the inner loop is shown below.
1 import sys
2 sys.path.append(’..’) # add parent directory
3 sys.path.append(’../hw16’) # add parent directory
4 import pendulumParamHW16 as P16
5 import matplotlib.pyplot as plt
6 from control import tf, step_response, bode, tf2ss, \
7 margin, mag2db, minreal
8 import numpy as np
9 import helper_functions as hf
10 import numpy as np
11 import loopshape_in as L_in
12
13 # flag to define if using dB or absolute scale for M(omega)

TOC
 CHAPTER 18. COMPENSATOR DESIGN 358

Gm = 9.85 dB (at 7.31 rad/s), Pm = 65.79 deg (at 1.10 rad/s)

Outer Open-loop Bode Plot
150
100
50
Magnitude (dB)

0 P(s)
ref tracking spec
50 noise spec
100 CleadP
CleadCkpP
150 CleadCkpClagP
200 CleadCkpClagClpfP
10 3 10 1 101 103 105
270
225
180
135
90
45
Phase (deg)

0
45
90
135
180
225
270
315
360
10 3 10 1 101 103 105
Frequency (rad/sec)

Figure 18-29: The Bode plot for the outer loop system in HW B.18, with lead,
and lag control.

14 dB_flag = P16.dB_flag
15
16 P_out = P16.P_out
17
18 # construct plant as cascade of P_out and closed inner loop
19 Plant = minreal(P_out* \
20 (L_in.P_in*L_in.C/(1+L_in.P_in*L_in.C)))
21
22 #########################################
23 # Control Design
24 #########################################
25 C = tf([1], [1])
26
27 # define specifications:
28 omega_r = 0.0032 # track signals below this frequency
29 gamma_r = 0.00001 # tracking error below this value
30
31 omega_n = 1000 # attenuate noise above this frequency
32 gamma_n = 0.0001 # attenuate noise by this amount
33

TOC
 CHAPTER 18. COMPENSATOR DESIGN 359

0
Closed-Loop Bode Plot

50
100
150 closed-loop YR - no pre-filter
200 closed-loop YR - with pre-filter
10 3 10 1 101 103
1.0
Step Response

0.5
response without prefilter
0.0 response with prefilter
0 1 2 3 4 5
0.4 control effort without prefilter
control effort with prefilter
Control Effort

0.2

0.0

0 1 2 3 4 5

Figure 18-30: The closed-loop bode response, the unit-step response for the out-
put, and the unit step response for the input of the inner loop design in HW B.18.

34
35 # phase lead: increase PM (stability)
36 # At desired crossover frequency, PM = -3
37 # Add 70 deg of PM with lead
38
39 #location of maximum frequency bump (desired crossover)
40 w_max = 1.1
41 M = 32 # lead ratio
42 Lead = hf.get_control_lead(w_max, M)
43 C = C*Lead
44
45 # find gain to set crossover at w_max = 1.1 rad/s
46 mag, phase, omega = bode(Plant*C, dB=False,
47 omega=[w_max], Plot=False)
48 K = hf.get_control_proportional(1.0/mag[0])
49 C = K*C
50
51 ## boost low-frequency gain
52 # Find gain increase needed at omega_r
53 mag, phase, omega = bode(Plant*C, omega=[omega_r],
54 Plot=False)
55 gain_increase_needed = 1/gamma_r/mag.item(0)
56
57 # minimum gain increase at low frequencies is 4.8 let lag ratio
58 # be 8.

TOC
 CHAPTER 18. COMPENSATOR DESIGN 360

59 M = 8
60 p = omega_r # set pole at omega_r
61 z = M*p # set zero at M*omega_r
62 Lag = hf.get_control_lag(z, M)
63 C = C*Lag
64
65
66 # Noise attenuation constraint not quite satisfied can be
67 # satisfied by reducing gain at 400 rad/s by a factor of 2 Use
68 # a low-pass filter
69 lpf = hf.get_control_lpf(100.0)
70 C = lpf*C
71
72 ############################################
73 # Prefilter Design
74 ############################################
75 F = tf([1], [1])
76
77 # low pass filter
78 p = 2
79 LPF = tf([p], [1, p])
80 F = F*LPF
81
82 ##############################################
83 # Convert Controller to State Space Equations
84 ##############################################
85 C_ss = tf2ss(C)
86 F_ss = tf2ss(F)
87
88
89 if __name__=="__main__":
90 # calculate bode plot and gain and phase margin
91 # for original PID * plant dynamics
92 mag, phase, omega = bode(Plant, dB=dB_flag,
93 omega=np.logspace(-3, 5),
94 plot=True, label="$P(s)$")
95
96 gm, pm, Wcg, Wcp = margin(Plant)
97 print("for original system:")
98 if dB_flag == True:
99 print(" pm: ", pm, " Wcp: ", Wcp,
100 "gm: ", mag2db(gm), " Wcg: ", Wcg)
101 elif dB_flag == False:
102 print(" pm: ", pm, " Wcp: ", Wcp,
103 "gm: ", gm, " Wcg: ", Wcg)
104
105 #########################################
106 # Define Design Specifications
107 #########################################
108 # ----------- general tracking specification --------
109 hf.add_spec_ref_tracking(gamma_r, omega_r, dB_flag)
110
111 # ----------- noise specification --------
112 hf.add_spec_noise(gamma_n, omega_n, dB_flag)
113
114 mag, phase, omega = bode(Plant * C, dB=dB_flag,
115 omega=np.logspace(-3, 5),

TOC
 CHAPTER 18. COMPENSATOR DESIGN 361

116 plot=True, label="$C_{final}(s)P(s)$",

117 margins=True, color=’orange’)
118
119 gm, pm, Wcg, Wcp = margin(Plant * C)
120 print("for final C*P:")
121 if dB_flag == True:
122 print(" pm: ", pm, " Wcp: ", Wcp,
123 "gm: ", mag2db(gm), " Wcg: ", Wcg)
124 elif dB_flag == False:
125 print(" pm: ", pm, " Wcp: ", Wcp,
126 "gm: ", gm, " Wcg: ", Wcg)
127
128 fig = plt.gcf()
129 fig.axes[0].legend()
130 fig.axes[0].set_title(’Outer Open-loop Bode Plot’)
131 fig.axes[0].grid(True)
132 fig.axes[1].grid(True)
133 plt.show()
134
135
136 ############################################
137 # now check the closed-loop response with prefilter
138 ############################################
139 # Closed loop transfer function from R to Y - no prefilter
140 CLOSED_R_to_Y = (Plant * C / (1.0 + Plant * C))
141 # Closed loop transfer function from R to Y - with prefilter
142 CLOSED_R_to_Y_with_F = (F * Plant * C / (1.0 + Plant * C))
143 # Closed loop transfer function from R to U - no prefilter
144 CLOSED_R_to_U = (C / (1.0 + Plant * C))
145 # Closed loop transfer function from R to U - with prefilter
146 CLOSED_R_to_U_with_F = (F*C / (1.0 + Plant * C))
147
148 plt.figure()
149 plt.grid(True)
150 plt.subplot(311)
151 mag, phase, omega = bode(CLOSED_R_to_Y,
152 dB=dB_flag, plot=False)
153 if dB_flag:
154 plt.semilogx(omega, mag2db(mag), color=[0,0,1],
155 label=’closed-loop $\\frac{Y}{R}$ - no pre-filter’)
156 else:
157 plt.loglog(omega, mag, color=[0,0,1],
158 label=’closed-loop $\\frac{Y}{R}$ - no pre-filter’)
159 mag, phase, omega = bode(CLOSED_R_to_Y_with_F,
160 dB=dB_flag, plot=False)
161 if dB_flag:
162 plt.semilogx(omega, mag2db(mag), color=[0,1,0],
163 label=’closed-loop $\\frac{Y}{R}$ - with pre-filter’)
164 else:
165 plt.loglog(omega, mag, color=[0,1,0],
166 label=’closed-loop $\\frac{Y}{R}$ - with pre-filter’)
167 plt.ylabel(’Closed-Loop Bode Plot’)
168 plt.grid(True)
169 plt.legend()
170
171 plt.subplot(312), plt.grid(True)
172 T = np.linspace(0, 5, 5*200)

TOC
 CHAPTER 18. COMPENSATOR DESIGN 362

173 _, yout_no_F = step_response(CLOSED_R_to_Y, T)

174 _, yout_F = step_response(CLOSED_R_to_Y_with_F, T)
175 plt.plot(T, yout_no_F, color=[0,0,1],
176 label=’response without prefilter’)
177 plt.plot(T, yout_F, color=[0,1,0],
178 label=’response with prefilter’)
179 plt.legend()
180 plt.ylabel(’Step Response’)
181
182
183 plt.subplot(313), plt.grid(True)
184 _, Uout = step_response(CLOSED_R_to_U, T)
185 _, Uout_F = step_response(CLOSED_R_to_U_with_F, T)
186 plt.plot(T, Uout, color=[0,0,1],
187 label=’control effort without prefilter’)
188 plt.plot(T, Uout_F, color=[0,1,0],
189 label=’control effort with prefilter’)
190 plt.ylabel(’Control Effort’)
191 plt.legend()
192
193 plt.show()

Listing 18.6: loopshape_out.py

See http://controlbook.byu.edu for the complete solution.

TOC
CHAPTER 18. COMPENSATOR DESIGN 363

18.4 Design Study C. Satellite Attitude Control

Example Problem C.18

For this homework assignment we will use the loopshaping design technique to
design a successive loop closure controller for the satellite attitude problem.
(a) First consider the inner loop, where Pin (s) is the transfer function of the
inner loop derived in HW C.5. The Bode plot for this system shows an
underdamped resonant mode. We have seen in previous chapters that this
resonate mode can be removed by introducing rate feedback of the form

u = −kD ẏ + u0 ,

where y is the output signal and u0 is the additional control signal to be

designed. Using the derivative gain kDθ found in HW C.10, find the revised
transfer function from τ 0 to θ, and use this transfer function for Pin (s).
Letting
τ 0 (s) = Cin (s)E(s),
where e(t) = θr (t) − θ(t), design Cin (s) to meet the following specifica-
tions:
– The inner loop will track θr with frequency content below ωr =
0.01 radians/sec to within γr = 0.01.
– The inner loop will attenuate noise on the measurement of θ for all
frequencies above ωn = 20 rad/sec by γn = 0.01.
– The phase margin of the inner loop should be around P M = 60 de-
grees.
(b) Now consider the design of the controller for the outer loop system. As
seen from HW C.5, the transfer function for the outer loop also has a strong
resonate mode. Similar to the inner loop, use the derivative gain found in
HW C.10 to introduce rate damping, and find the associated transfer func-
tion Pout (s). The ’plant’ for the design of the outer loop controller is
Pin Cin
P = Pout .
1 + Pin Cin
Design the outer loop controller Cout to meet the following specs:
– Track steps in φr with zero steady state error.
– Reject input disturbances with frequency content below ωdin = 0.01
by γdin = 0.1.
– Attenuate noise on the measurement of φ with frequency content above
ωn = 10 rad/sec by γn = 10−4 .

TOC
CHAPTER 18. COMPENSATOR DESIGN 364

– The phase margin of the outer loop should be around P M = 60 de-

grees.
– Use a prefilter to reduce any peaking in the closed loop response of
the outer loop.

Solution
From HW C.5, the transfer function model for the inner loop is given by
1
Js
Θ(s) = b k
τ (s).
s2 + Js s + Js

Rate damping is added by designing the control signal so that

s
τ (s) = −kD Θ(s) + τ 0 (s).
σs + 1
Solving for Θ(s) in terms of τ 0 (s) gives
σs + 1
Θ(s) = τ 0 (s).
σJs s3 + (σb + Js )s2 + (σk + b + kD )s + k
Therefore, for the inner loop we will set
σs + 1
Pin = .
σJs s3 + (σb + Js )s2 + (σk + b + kD )s + k
Fig. 18-31 shows the Bode plot of the plant Pin (s) together with the design spec-
ification on input tracking and noise attenuation. The first step is to add a propor-
tional gain to satisfy the tracking requirement. Fig. 18-32 shows the correspond-
ing Bode plot for a proportional gain of kP = 45. Since the phase margin is over
P M = 60 degrees, a low pass filter is added. Fig. 18-33 shows the loop gain with
the addition of the low pass filter
8
CLP F = ,
s+8
which has a phase margin of P M = 76.9 degrees, and satisfies the noise specifi-
cation. The resulting compensator is
 
8
Cin (s) = 45 .
s+8
Notice that the DC-gain is not equal to one. The closed loop response for the inner
subsystem
Pin Cin
,
1 + Pin Cin
as well as the unit step response for the output and control signal are all shown in
Fig. 18-34.
The Python code used to design the inner loop is shown below.

TOC
 CHAPTER 18. COMPENSATOR DESIGN 365

Bode Diagram
50
0
Magnitude (dB)

50
100
150 P , in(s)
ref tracking spec
200 noise spec
10 3 10 1 101 103 105
0
45
Phase (deg)

90
135
180
10 3 10 1 101 103 105
Frequency (rad/sec)

Figure 18-31: The Bode plot for the inner loop plant in HW C.18, together with
the design specification.

1 import sys
2 sys.path.append(’..’) # add parent directory
3 import satelliteParam as P
4 sys.path.append(’../hw10’) # add parent directory
5 sys.path.append(’../hw16’) # add parent directory
6 import satelliteParamHW10 as P10
7 import satelliteParamHW16 as P16
8 import matplotlib.pyplot as plt
9 from control import tf, margin, bode, tf2ss, step_response, mag2db
10 import numpy as np
11 import helper_functions as hf
12
13 # flag to define if using dB or absolute scale for M(omega)
14 dB_flag = P16.dB_flag
15
16 # Compute open-loop transfer functions as described in Chapter 18
17 Plant = tf([P.sigma, 1],
18 [P.sigma*P.Js, P.sigma*P.b+P.Js,
19 P.sigma*P.k+P.b+P10.kd_th, P.k])
20
21 #########################################
22 # Control Design
23 #########################################
24 C = tf([1], [1])
25

TOC
 CHAPTER 18. COMPENSATOR DESIGN 366

Gm = inf dB (at nan rad/s), Pm = 84.93 deg (at 1.15 rad/s)

Bode Diagram
50
0
Magnitude (dB)

50
100 P , in(s)
150 ref tracking spec
noise spec
200 CkpP , in
10 3 10 1 101 103 105
0

45
Phase (deg)

90

135

180
10 3 10 1 101 103 105
Frequency (rad/sec)

Figure 18-32: The Bode plot for the inner loop plant in HW C.18, with propor-
tional gain.

26 # -----proportional control: change cross over frequency----

27 kp = 45.0
28 C_k = hf.get_control_proportional(kp)
29
30 # -----low pass filter: decrease gain at high frequency (noise)----
31 p = 8.0
32 C_lpf = hf.get_control_lpf(p)
33
34 # calculating final controller
35 C = C * C_lpf * C_k
36
37 ##############################################
38 # Convert Controller to State Space Equations
39 ##############################################
40 C_ss = tf2ss(C) # convert to state space
41
42
43 if __name__=="__main__":
44
45 # calculate bode plot and gain and phase margin
46 # for original PID * plant dynamics

TOC
 CHAPTER 18. COMPENSATOR DESIGN 367

Gm = 22.17 dB (at 8.81 rad/s), Pm = 76.86 deg (at 1.14 rad/s)

Bode Diagram
50
0
Magnitude (dB)

50
100
150 P , in(s)
ref tracking spec
200 noise spec
250 CkpClpfP , in
10 3 10 1 101 103 105
0
45
90
Phase (deg)

135
180
225
270
10 3 10 1 101 103 105
Frequency (rad/sec)

Figure 18-33: The Bode plot for the inner loop system in HW C.18, with propor-
tional gain and low pass filter.

47 mag, phase, omega = bode(Plant, dB=dB_flag,

48 omega=np.logspace(-4, 5),
49 plot=True, label=r’$P_{\theta,in}(s)$’)
50
51 gm, pm, Wcg, Wcp = margin(Plant)
52 print("for original system:")
53 print(" pm: ", pm, " Wcp: ", Wcp, "gm: ", gm, " Wcg: ", Wcg)
54
55 #########################################
56 # Define Design Specifications
57 #########################################
58 #----------- general tracking specification --------
59 omega_r = 0.01 # track signals below this frequency
60 gamma_r = 0.01 # tracking error below this value
61 hf.add_spec_ref_tracking(gamma_r, omega_r, dB_flag)
62
63 #----------- noise specification --------
64 omega_n = 20 # attenuate noise above this frequency
65 gamma_n = 0.01 # attenuate noise by this amount
66 hf.add_spec_noise(gamma_n, omega_n, dB_flag)
67

TOC
 CHAPTER 18. COMPENSATOR DESIGN 368

closed-loop magnitude ratio YR

0
Magnitude (dB)

50
100

10 4 10 3 10 2 10 1 100 101 102 103

Step Response
Amplitude

0.5

0.0
0.00 0.25 0.50 0.75 1.00 1.25 1.50 1.75 2.00

Control Effort
40
Amplitude

20

0
0.00 0.25 0.50 0.75 1.00 1.25 1.50 1.75 2.00

Figure 18-34: The closed loop bode response, the unit step response for the output,
and the unit step response for the input of the inner loop design in HW C.18.

68 ## plot the effect of adding the new compensator terms

69 mag, phase, omega = bode(Plant * C, dB=dB_flag,
70 omega=np.logspace(-4, 5),
71 plot=True,
72 label=r"$C_{\theta,in}(s)$"+
73 r"$P_{\theta,in}(s)$",
74 margins=True)
75
76 gm, pm, Wcg, Wcp = margin(Plant * C)
77 print("for final C*P:")
78 print(" pm: ", pm, " Wcp: ", Wcp, "gm: ", gm, " Wcg: ", Wcg)
79
80 fig = plt.gcf()
81 fig.axes[0].legend()
82 fig.axes[0].grid(True)
83 fig.axes[1].grid(True)
84 fig.axes[0].set_title(’Bode Diagram’)
85 plt.show()
86

TOC
 CHAPTER 18. COMPENSATOR DESIGN 369

87 ############################################
88 # now check the closed-loop response
89 ############################################
90 # Closed loop transfer function from R to Y
91 CLOSED_R_to_Y = (Plant * C / (1.0 + Plant * C))
92 # Closed loop transfer function from R to U
93 CLOSED_R_to_U = (C / (1.0 + Plant * C))
94
95 plt.figure()
96 plt.subplot(311)
97 mag, phase, omega = bode(CLOSED_R_to_Y,
98 dB=dB_flag, plot=False)
99 if dB_flag:
100 plt.semilogx(omega, mag2db(mag), color=[0,0,1])
101 else:
102 plt.loglog(omega, mag, color=[0,0,1])
103 plt.grid(True)
104 plt.ylabel(’Magnitude (dB)’)
105 plt.title(’closed-loop magnitude ratio $\\frac{Y}{R}$’)
106
107 plt.subplot(312), plt.grid(True)
108 T = np.linspace(0, 2, 100)
109 _, yout = step_response(CLOSED_R_to_Y, T)
110 plt.plot(T, yout, color=[0,0,1])
111 plt.ylabel(’Amplitude’)
112 plt.title(’Step Response’)
113
114 plt.subplot(313), plt.grid(True)
115 _, Uout = step_response(CLOSED_R_to_U, T)
116 plt.plot(T, Uout, color=[0,0,1])
117 plt.ylabel(’Amplitude’)
118 plt.title(’Control Effort’)
119
120 plt.tight_layout()
121 plt.show()

Listing 18.7: loopshape_in.py

The transfer function model for the outer loop was derived in HW C.5 to be
b k
Jp s + Jp
Φ(s) = Θr (s).
s2 + Jbp s + Jkp
Rate damping is added by designing the control signal so that
s 0
Θr (s) = −kD Φ(s) + Θr (s).
σs + 1
0
Solving for Φ(s) in terms of Θr (s) gives
σbs2 + (σk + b)s + k 0
Φ(s) = Θr (s).
σJp s3 + (σb + Jp + bkD )s2 + (σk + b + kkD )s + k
The plant for the outer loop is therefore
σs + 1 Pin Cin
Pout = .
σJp s3 2
+ (σb + Jp )s + (σk + b + kD )s + k 1 + Pin Cin

TOC
CHAPTER 18. COMPENSATOR DESIGN 370

Fig. 18-35 shows the Bode plot for Pout together with the design specification
on rejecting input disturbances and attenuating the noise. The requirement that
steady state error to a step requires that the slope as s → 0 is −20 dB/dec, and is
not shown in Fig. 18-35.

Bode Diagram
0
Magnitude (dB)

100
200
P , in(s)
300 din spec
noise spec
10 3 10 1 101 103
0
45
90
135
Phase (deg)

180
225
270
315
360
405
450
10 3 10 1 101 103
Frequency (rad/sec)

Figure 18-35: The Bode plot for the outer loop plant in HW C.18, together with
the design specifications.

The first step is to add an integrator to meet the steady state tracking require-
ment. The PM was also too low (P M = 20.8 degrees). So a lead compensator is
added at 0.15 rad/sec with M = 120, at approximately the midpoint in frequency
between the disturbance and noise constraints. This causes the disturbance con-
straint to be violated. For this reason, a lag compensator is also added to boost
magnitude ratio at low frequencies. The lag compensator is defined with z = 2.0
and M = 60.0. Fig. 18-36 shows the loop gain after adding the integral control,
lead compensator, and lag compensator.
   
s + 0.1 120s + 1.64 s + 0.5
C = Cint Clead Clag = .
s s + 1.64 0.0083

Without adding an overall gain to shift the crossover frequency (i.e. to the
desired 0.15 rad/sec for which the lead compensator was designed), the phase
margin as shown in Fig. 18-36 is P M = −36 degrees. Therefore a proportional

TOC
CHAPTER 18. COMPENSATOR DESIGN 371

Gm = -6.65 dB (at 2.71 rad/s), Pm = -36.38 deg (at 4.03 rad/s)

Bode Diagram
100
0
Magnitude (dB)

100
200 P , in(s)
din spec
300 noise spec
CpiCleadClagP , out
10 3 10 1 101 103
0
45
90
135
Phase (deg)

180
225
270
315
360
405
450
10 3 10 1 101 103
Frequency (rad/sec)

Figure 18-36: The Bode plot for the outer loop system in HW C.18, with integral,
lead, and lag control.

gain of kP = 0.0275 is added to the compensator. Two low-pass filters with

p = 1.5 and p = 1.8 are also added to meet the noise constraint and the result
in shown in Fig. 18-37. The resulting system has P M = 52 degrees which is a
direct result of tradeoffs between the constraints in noise and tracking.
The final compensator is defined as:
Ctotal = Ckp Cpi Clead Clag Clpf 1 Clpf 2
     
s + 0.1 120s + 1.64 s + 0.5 1.5 1.8
= 0.0275 .
s s + 1.64 0.0083 s + 1.5 s + 1.8
The closed loop response for both the inner and outer loop systems, as well as
the unit step response for the output and control signal of the outer loops, are all
shown in Fig. 18-38, where the prefilter
0.1
F (s) =
s + 0.1
has been added to reduce overshoot in the closed-loop response.
The time response of the outer loop is fairly slow, but this is because of the
chosen crossover frequency.
The Python code used to design the outer loop is shown below.

TOC
 CHAPTER 18. COMPENSATOR DESIGN 372

Gm = 17.45 dB (at 1.04 rad/s), Pm = 51.75 deg (at 0.15 rad/s)

Bode Diagram
100
0
Magnitude (dB)

100
200
300 P , in(s)
din spec
400 noise spec
CpiCleadClagCkpClpf1Clpf2P , out
500
10 3 10 1 101 103
0
45
90
135
180
225
Phase (deg)

270
315
360
405
450
495
540
585
630
10 3 10 1 101 103
Frequency (rad/sec)

Figure 18-37: The Bode plot for the outer loop system in HW C.18, with the final
controller.

1 import sys
2 sys.path.append(’..’) # add parent directory
3 import satelliteParam as P
4 sys.path.append(’../hw10’) # add parent directory
5 sys.path.append(’../hw16’) # add parent directory
6 import satelliteParamHW10 as P10
7 import satelliteParamHW16 as P16
8 import matplotlib.pyplot as plt
9 from control import tf, margin, bode, tf2ss, step_response, mag2db
10 import numpy as np
11 import loopshape_in as L_in
12 import helper_functions as hf
13
14 # flag to define if using dB or absolute scale for M(omega)
15 dB_flag = P16.dB_flag
16
17 # Compute outer loop transfer function as described in Chapter 18
18 num_out = [P.sigma, 1.0]

TOC
 CHAPTER 18. COMPENSATOR DESIGN 373

Figure 18-38: The closed loop bode response, the unit step response for the output,
and the unit step response for the input of the inner loop design in HW C.18.

19 den_out = [P.sigma*P.Jp, P.sigma*P.b+P.Jp,

20 P.sigma*P.k+P.b+P10.kd_phi, P.k]
21 P_out = tf(num_out, den_out)
22
23 # construct plant as cascade of P_out and closed inner loop
24 Plant = P_out * (L_in.Plant * L_in.C /
25 (1 + L_in.Plant * L_in.C))
26
27 #########################################
28 # Control Design
29 #########################################
30 C = tf([1], [1])
31
32 # -----integral control: change low freq. slope----
33 ki = 0.1
34 C_int = hf.get_control_integral(ki)
35
36
37 # -----lead control: change the phase margin ----
38 omega_max = 0.15
39 M = 120.0
40 C_lead = hf.get_control_lead(omega_max, M)

TOC
 CHAPTER 18. COMPENSATOR DESIGN 374

41
42 # -----lag control: increase magnitude near required freq. ----
43 z = 2.0
44 M = 60.0
45 C_lag = hf.get_control_lag(0.5, 60)
46
47 # -----proportional control: change cross over frequency----
48 mag, phase, omega = bode(Plant*C_int*C_lead*C_lag,
49 omega=[omega_max],
50 Plot=False)
51 C_k = hf.get_control_proportional(1.0/mag[0])
52
53 # -----low pass filter: decrease gain at high frequency (noise)----
54 # we will have to use two to get the desired noise attenuation
55 p = 1.5
56 C_lpf1 = hf.get_control_lpf(p)
57 p = 1.8
58 C_lpf2 = hf.get_control_lpf(p)
59
60 # calculating final controller
61 C = C*C_int*C_lead*C_lag*C_k*C_lpf1*C_lpf2
62
63
64 ############################################
65 # Prefilter Design
66 ############################################
67 # low pass filter
68 p = 0.1
69 F = hf.get_control_lpf(p)
70
71
72 ##############################################
73 # Convert Controller to State Space Equations
74 ##############################################
75 C_ss = tf2ss(C)
76 F_ss = tf2ss(F)
77
78
79 if __name__=="__main__":
80
81 # calculate bode plot and gain and phase margin
82 # for original PID * plant dynamics
83 mag, phase, omega = bode(Plant, dB=dB_flag,
84 omega=np.logspace(-4, 4),
85 plot=True, label=r’$P_{\theta,in}(s)$’)
86
87 gm, pm, Wcg, Wcp = margin(Plant)
88 print("for original system:")
89 print(" pm: ", pm, " Wcp: ", Wcp, "gm: ", gm, " Wcg: ", Wcg)
90
91 #########################################
92 # Define Design Specifications
93 #########################################
94 #----------- input disturbance spec ---------
95 omega_din = 10 ** (-2.0) # reject input disturbances below freq.
96 gamma_din = 0.1 # amount of input disturbance in output
97 hf.add_spec_input_disturbance(gamma_din, omega_din,

TOC
 CHAPTER 18. COMPENSATOR DESIGN 375

98 Plant, dB_flag)
99
100 # ----------- noise specification --------
101 omega_n = 10 # attenuate noise above this frequency
102 gamma_n = 10.0 ** (-4.0) # attenuate noise by this amount
103 hf.add_spec_noise(gamma_n, omega_n, dB_flag)
104
105 ## plot the effect of adding the new compensator terms
106 mag, phase, omega = bode(Plant * C, dB=dB_flag,
107 omega=np.logspace(-4, 4),
108 plot=True, margins=True,
109 label=r"$C_{\phi,out}(s)"+
110 "P_{\phi,out}(s)$")
111
112 gm, pm, Wcg, Wcp = margin(Plant * C)
113 print("for final C*P:")
114 print(" pm: ", pm, " Wcp: ", Wcp, "gm: ", gm, " Wcg: ", Wcg)
115
116 fig = plt.gcf()
117 fig.axes[0].legend()
118 fig.axes[0].set_title(’Bode Diagram’)
119 fig.axes[0].grid(True)
120 fig.axes[1].grid(True)
121 plt.show()
122
123 ############################################
124 # now check the closed-loop response with prefilter
125 ############################################
126 # Closed loop transfer function from R to Y - no prefilter
127 CLOSED_R_to_Y = (Plant * C / (1.0 + Plant * C))
128 # Closed loop transfer function from R to Y - with prefilter
129 CLOSED_R_to_Y_with_F = (F * Plant * C / (1.0 + Plant * C))
130 # Closed loop transfer function from R to U - no prefilter
131 CLOSED_R_to_U = (C / (1.0 + Plant * C))
132 # Closed loop transfer function from R to U - with prefilter
133 CLOSED_R_to_U_with_F = (F*C / (1.0 + Plant * C))
134
135 plt.figure()
136 plt.grid(True)
137 plt.subplot(311)
138 mag, phase, omega = bode(CLOSED_R_to_Y,
139 dB=dB_flag, plot=False)
140 if dB_flag:
141 plt.semilogx(omega, mag2db(mag), color=[0,0,1],
142 label=’closed-loop $\\frac{Y}{R}$ - no pre-filter’)
143 else:
144 plt.loglog(omega, mag, color=[0,0,1],
145 label=’closed-loop $\\frac{Y}{R}$ - no pre-filter’)
146 mag, phase, omega = bode(CLOSED_R_to_Y_with_F,
147 dB=dB_flag, plot=False)
148 if dB_flag:
149 plt.semilogx(omega, mag2db(mag), color=[0,1,0],
150 label=’closed-loop $\\frac{Y}{R}$ - with pre-filter’)
151 else:
152 plt.loglog(omega, mag, color=[0,1,0],
153 label=’closed-loop $\\frac{Y}{R}$ - with pre-filter’)
154 plt.ylabel(’Closed-Loop Bode Plot’)

TOC
 CHAPTER 18. COMPENSATOR DESIGN 376

155 plt.grid(True)
156 plt.legend()
157
158 plt.subplot(312), plt.grid(True)
159 T = np.linspace(0, 50, 50*100)
160 _, yout_no_F = step_response(CLOSED_R_to_Y, T)
161 _, yout_F = step_response(CLOSED_R_to_Y_with_F, T)
162 plt.plot(T, yout_no_F, color=[0,0,1],
163 label=’response without prefilter’)
164 plt.plot(T, yout_F, color=[0,1,0],
165 label=’response with prefilter’)
166 plt.legend()
167 plt.ylabel(’Step Response’)
168
169
170 plt.subplot(313), plt.grid(True)
171 _, Uout = step_response(CLOSED_R_to_U, T)
172 _, Uout_F = step_response(CLOSED_R_to_U_with_F, T)
173 plt.plot(T, Uout, color=[0,0,1],
174 label=’control effort without prefilter’)
175 plt.plot(T, Uout_F, color=[0,1,0],
176 label=’control effort with prefilter’)
177 plt.ylabel(’Control Effort’)
178 plt.legend()
179
180 plt.show()

Listing 18.8: loopshape_outer.py

See http://controlbook.byu.edu for the complete solution.

TOC
CHAPTER 18. COMPENSATOR DESIGN 377

Important Concepts:
• Loopshaping design may be achieved by multiplying different build-
ing block controllers together. On the Bode plot, this will result in
the summation of the gain (dB) and phase of each control block.
• A proportional gain controller alters the open-loop system’s magni-
tude. It will increase/decrease the magnitude if the gain is larger/less
than one. If the proportional controller’s gain is positive, the phase
remains the same. Otherwise, 180 degrees of phase is added.
• At low frequencies, a modified integral controller will (1) increase
the system type, (2) increase the gain, and (3) decrease the phase.
• A low-pass filter will decrease the gain at high frequencies to improve
the system’s noise attenuation. It also decreases the phase.
• A phase-lag filter uses a pole and zero (with the magnitude of the
zero being greater than that of the pole) to increase the loop gain at
low frequencies. This also decreases the phase between the zero and
the pole. phase-lag filters are used to improve tracking and distur-
bance rejection at low frequencies without introducing a potentially
destabilizing integrator.
• A phase-lead filter also uses a pole and zero (with the pole magnitude
being greater than that of the zero) to increase the phase between the
zero and the pole.
• Prefilters attenuate frequencies that will resonate in the closed-loop
system.

Notes and References

TOC
 Part VI
Homework Problems
 Design Study: Mass Spring Damper
D
Figure 19-1 shows the mass-spring-damper system. The mass slides along a fric-
tionless level surface and is connected to a wall by a spring with spring constant
k and a damper with damping constant b. The position of the mass is given by
z, where z = 0 is the position where the spring is not stretched. The speed of
the mass is given by ż. An external force F is applied to the mass as shown in
Fig. 19-1.
Assume the following physical constants: m = 5 kg, k = 3 N/m, b = 0.5 N-
sec/m.

Figure 19-1: Mass Spring Damper

Homework Problems:

D.2 Kinetic energy. D.10 Digital PID.

D.3 Equations of Motion. D.11 Full state feedback.
D.4 Linearize equations of motion. D.12 Full state with integrator.
D.5 Transfer function model. D.13 Observer based control.
D.6 State space model. D.14 Disturbance observer.
D.7 Pole placement using PD. D.15 Frequency Response.
D.8 Second order design. D.16 Loop gain.
D.9 Integrators and system type. D.17 Stability margins.
D.P.6 Root locus PID. D.18 Loopshaping design.
TOC
D. DESIGN STUDY: MASS SPRING DAMPER 380

Homework D.2
(a) Using the configuration variable z, write an expression for the kinetic energy
of the system.
(b) Create an animation of the mass-spring-damper system. The input should be
a variable z. Turn in a screen capture of the animation.

Homework D.3
(a) Find the potential energy for the system.
(b) Define the generalized coordinates.
(c) Find the generalized forces and damping forces.
(d) Derive the equations of motion using the Euler-Lagrange equations.
(e) Referring to Appendices P.1, P.2, and P.3, write a class or s-function that im-
plements the equations of motion. Simulate the system using a variable
force input. The output should connect to the animation function developed
in homework D.2.

Homework D.4
For the mass spring damper:
(a) Find the equilibria of the system.
(b) Linearize the system about the equilibria using Jacobian linearization.
(c) Linearize the system using feedback linearization.

Homework D.5
For the equations of motion for the mass spring damper,

(a) Using the Laplace transform, convert from time domain to the s-domain.
(b) Find the transfer function from the input force F to the mass position z.
(c) Draw the associated block diagram.

TOC
D. DESIGN STUDY: MASS SPRING DAMPER 381

Homework D.6
For the equations of motion for the mass spring damper, define the states as x =
(z, ż)> , and the input as u = F , and the measured output as y = z. Find the
linear state space equations in the form

ẋ = Ax + Bu
y = Cx + Du.

Homework D.7
(a) Given the open loop transfer function from force to position in Homework D.5,
find the open loop poles of the system, when the equilibrium position is
ze = 0.

(b) Using PD control architecture shown in Figure 7-2, find the closed loop trans-
fer function from zr to z and find the closed loop poles as a function of kP
and kD .

(c) Select kP and kD to place the closed loop poles at p1 = −1 and p2 = −1.5.

(d) Using the gains from part (c), implement the PD control for the mass spring
damper in simulation and plot the response of the system to a 1 m step input.

Homework D.8
For the mass spring, do the following:

(a) Suppose that the design requirements are that the rise time is tr ≈ 2 seconds,
with a damping ratio of ζ = 0.7. Find the desired closed loop characteristic
polynomial ∆dcl (s), and the associated pole locations. Find the proportional
and derivative gains kP and kD to achieve these specifications, and modify
the simulation from HW D.7 to verify that the step response satisfies the
requirements.

(b) Suppose that the size of the input force is limited to Fmax = 6 N. Modify
the simulation to include a saturation block on the force F . Using the rise
time tr as a tuning parameter, tune the PD control gains so that the input
just saturates when a step of size of 1 meter is placed on z r . Plot the step
response showing that saturation does not occur for a 1 meter step input for
the new control gains.

TOC
D. DESIGN STUDY: MASS SPRING DAMPER 382

Homework D.9
(a) When the controller for the mass spring damper is PD control, what is the
system type? Characterize the steady state error when the reference input
is a step, a ramp, and a parabola. How does this change if you add an
integrator?
(b) Consider the case where a constant disturbance acts at the input to the plant
(for example an inaccurate knowledge of the spring constant in this case).
What is the steady state error to a constant input disturbance when the inte-
grator is not present, and when it is present?

Homework D.P.6
Adding an integrator to obtain PID control for the mass spring damper, put the
characteristic equation in Evan’s form and use the Matlab rlocus command to
plot the root locus verses the integrator gain kI . Select a value for kI that does not
significantly change the other locations of the closed loop poles.

Homework D.10
The objective of this problem is to implement the PID controller using only mea-
sured outputs of the system.
(a) Modify the system dynamics file so that the parameters m, k, and b vary by up
to 20% of their nominal value each time they are run (uncertainty parameter
= 0.2).
(b) Change the simulation files so that the input to the controller is the output and
not the state. The controller should only assume knowledge of the position
z and the reference position zr .
(c) Implement the PID controller designed in Problems D.8 in simulation. Use
the dirty derivative gain of σ = 0.05. Tune the integrator to remove the
steady state error caused by the uncertain parameters.

Homework D.11
The objective of this problem is to a implement state feedback controller for the
mass spring damper using the full state. Start with the simulation files developed

TOC
D. DESIGN STUDY: MASS SPRING DAMPER 383

in Homework D.10.

(a) Select the closed loop poles as the roots of the equations s2 + 2ζωn + ωn2 = 0
where ωn , and ζ were found in Homework D.8.

(b) Add the state space matrices A, B, C, D derived in Homework D.6 to your
param file.

(c) Verify that the state space system is controllable by checking that rank(CA,B ) =
n.

(d) Find the feedback gain K so that the eigenvalues of (A − BK) are equal to
the desired closed loop poles. Find the reference gain kr so that the DC-
gain from zr to z is equal to one. Note that K = (kp , kd ) where kp and kd
are the proportional and derivative gains found in Homework D.8. Why?

(e) Modify the control simulation code to implement the state feedback controller.
To construct the state x = (z, ż)> use a digital differentiator to estimate ż.

Homework D.12
(a) Modify the state feedback solution developed in Homework D.11 to add an
integrator with anti-windup to the feedback loop for z.

(b) Add a constant input disturbance of 0.25 Newtons to the input of the plant
and allow the plant parameters to vary up to 20%.

(c) Tune the integrator pole (and other gains if necessary) to get good tracking
performance.

Homework D.13
The objective of this problem is to design an observer that estimates the state
of the system and to use the estimated state in the controller designed in Home-
work D.12.

(a) For the sake of understanding the function of the observer, for this problem we
will use exact parameters, without an input disturbance. Modify the mass
dynamics so that the parameters known to the controller are the actual plant
parameters (uncertainty parameter α = 0).

(b) Verify that the state space system is observable by checking that rank(OA,C ) =
n.

TOC
D. DESIGN STUDY: MASS SPRING DAMPER 384

(c) In the control block, add an observer to estimate the state x̂, and use the esti-
mate of the state in your feedback controller. Tune the poles of the controller
and observer to obtain good performance.

(d) Modify the simulation files so that the controller outputs both u and x̂. Add a
plotting routine to plot both the state and the estimated state of the system
on the same graph.

(e) As motivation for the next chapter, add an input disturbance to the system of
0.25 and observe that there is steady state error in the response even though
there is an integrator. This is caused by a steady state error in the observation
error. In the next chapter we will show how to remove the steady state error
in the observation error.

Homework D.14
(a) Modify your solution from HW D.13 so that the uncertainty parameter is
α = 0.2, representing 20% inaccuracy in the knowledge of the system pa-
rameters, and so that the input disturbance is 0.25. Also, add noise to the
output channels zm and θm with standard deviation of 0.001.

(b) Add a disturbance observer to the controller, and verify that the steady state
error in the estimator has been removed. Tune the system to get good re-
sponse.

Homework D.15
Draw by hand the Bode plot of the mass spring damper from force F̃ to position
z̃. Use the bode command in Matlab or Python and compare your results.

Homework D.16
For the mass spring, use the bode command (from Python or Matlab) to create
a graph that simultaneously displays the Bode plots for (1) the plant, and (2) the
plant under PID control, using the control gains calculated in Homework D.10.
Use the dirty derivative when implementing the derivative portion of the PID con-
trol.

(a) What is the tracking error to a unit ramp under PID control?

TOC
D. DESIGN STUDY: MASS SPRING DAMPER 385

(b) If the frequency content of the input disturbance din (t) is below ωdin = 0.1
radians per second, what percentage of the input disturbance shows up in
the output z under PID control?
(c) If all of the frequency content of the noise n(t) is greater than ωno = 100
radians per second, what percentage of the noise shows up in the output
signal z?

Homework D.17
For the mass spring damper, use the bode and margin commands (from Matlab
or Python) to find the phase and gain margin for the closed loop system under
PID control. On the same graph, plot the open loop Bode plot and the closed loop
Bode plot. What is the bandwidth of the closed loop system, and how does this
relate to the crossover frequency? Use the gains found in HW D.10.

Homework D.18
For this homework assignment we will use the loopshaping design technique to
design a controller for the mass spring damper problem with dynamics given by
!
1
m
Z(s) = b k
F (s).
s2 + ms + m

The controller designed in this problem will take the form

F = C(s)E(s),
where the error signal is e = zfr− z, where zfr is the prefiltered reference com-
mand. Find the controller C(s) so that the closed loop system meets the following
specifications.
• Reject constant input disturbances.
• Track reference signals with frequency content below ωr = 0.1 rad/s to
within γr = 0.03.
• Attenuate noise on the measurement of z for all frequencies above ωn =
500 rad/s by γn = 0.001.
• The phase margin is close to P M = 60 degrees.
• Use a prefilter F (s) to reduce any peaking in the closed loop response.
• Implement a control strategy in simulation using the state space or digital
filter equivalents of C(s) and F (s).

TOC
D. DESIGN STUDY: MASS SPRING DAMPER 386

TOC
 Design Study: Ball on Beam
E
Figure 20-1 shows the system.
The position of the ball mea-
sured from the pivot point is z
and the speed of the ball along
the direction of the beam is
ż. The angle of the beam
from level is θ and the angular
speed of the beam is θ̇. Grav-
ity acts in the down direction.
The mass of the ball is m1 and
Figure 20-1: Ball on Beam Problem
the mass of the beam is m2 .
The length of the beam is `.
An external vertical force is applied at the end of the beam as shown in Fig. 20-1.
Use the following physical parameters: m1 = 0.35 kg, m2 = 2 kg, ` = 0.5 m,
g = 9.8 m/s2 .

Homework Problems:

E.2 Kinetic energy. E.11 Full state feedback.

E.3 Equations of Motion. E.12 Full state with integrator.
E.4 Linearize equations of motion. E.13 Observer based control.
E.5 Transfer function model. E.14 Disturbance observer.
E.6 State space model. E.15 Frequency Response.
E.8 Successive loop closure. E.16 Loop gain.
E.9 Integrators and system type. E.17 Stability margins.
E.P.6 Root locus PID. E.18 Loopshaping design.
E.10 Digital PID.

TOC
E. DESIGN STUDY: BALL ON BEAM 388

Homework E.2
(a) Using the configuration variables z and θ, write an expression for the kinetic
energy of the system. Assume that the ball is a point mass without rolling
inertia. In general, this term will be small and will not have a large effect
on the dynamics of the ball-beam system.

(b) Create an animation of the ball on beam system. The inputs should be z and
θ. Turn in a screen capture of the animation.

Homework E.3
(a) Find the potential energy for the system.

(b) Define the generalized coordinates.

(c) Find the generalized forces and damping forces.

(d) Derive the equations of motion using the Euler-Lagrange equations.

(e) Referring to Appendices P.1, P.2, and P.3, write a class or s-function that im-
plements the equations of motion. Simulate the system using a variable
force input. The output should connect to the animation function developed
in homework E.2.

Homework E.4
For the ball on beam system:

(a) Find the equilibria of the system.

(b) Linearize the system about the equilibria using Jacobian linearization.

(c) If possible, linearize the system using feedback linearization.

Homework E.5
For the ball on beam system:

(a) Start with the linearized equations and use the Laplace transform to convert
the equations of motion to the s-domain.

TOC
E. DESIGN STUDY: BALL ON BEAM 389

(b) Find the transfer functions from the input F̃ (s) to the outputs Z̃(s) and Θ̃(s).
Find the transfer function from the input Θ̃(s) to the output Z̃(s).

(c) Assume that deviations in the gravity torque away from the equilibrium torque
due to motions of the ball along the beam are small compared to the torque
required to hold up the beam. This will allow you to ignore the m1 gz̃ term
in the second equation of motion. How does this simplify your transfer
functions?

(d) Draw a block diagram of your simplified system transfer functions in a cas-
cade from the input F̃ (s) to the intermediate state Θ̃(s) and then to the
output Z̃(s) .

Homework E.6
˙ >
˙ θ̃)
Defining the states as x̃ = (z̃, θ̃, z̃, , the input as ũ = F̃ , and the measured
output as ỹ = (z̃, θ̃)> , find the linear state space equations in the form

x̃˙ = Ax̃ + B ũ
ỹ = C x̃ + Dũ.

Homework E.8
For the ball and beam problem, do the following:

(a) Using the principle of successive loop closure, draw a block diagram that uses
PD control for both inner loop control and outer loop control. For design
purposes, let the equilibrium position for the ball be ze = 2` . The input
to the outer loop controller is the desired ball position zr and the output
of the controller is the desired beam angle θ̃r . The input to the inner loop
controller is the desired beam angle θ̃r and the output is the force F̃ on the
end of the beam.

(b) Focusing on the inner loop, find the PD gains kPθ and kDθ so that the rise time
of the inner loop is trθ = 1 second, and the damping ratio is ζθ = 0.707.

(c) Find the DC gain kDCθ of the inner loop.

(d) Replacing the inner loop by its DC-gain, find the PD gains kPz and kDz so
that the rise time of the outer loop is trz = 10trθ and the damping ratio is
ζz = 0.707.

TOC
E. DESIGN STUDY: BALL ON BEAM 390

(e) Implement the successive loop closure design for the ball and beam in simu-
lation where the commanded ball position is given by a square wave with
magnitude 0.25 ± 0.15 meters and frequency 0.01 Hz. Use the actual posi-
tion in the ball as a feedback linearizing term for the equilibrium force.
(f) Suppose that the size of the input force on the beam is limited to Fmax = 15 N.
Modify the simulation to include a saturation block on the force F . Using
the rise time of the outer loop, tune the PD control law to get the fastest
possible response without input saturation when a step of size 0.25 meter is
placed on z̃ r .

Homework E.9
(a) When the inner loop controller for the ballbeam system is PD control, what is
the system type of the inner loop? Characterize the steady state error when
θ̃d is a step, a ramp, and a parabola. What is the system type with respect to
an input disturbance?
(b) When the outer loop controller for the ballbeam is PD control, what is the
system type of the outer loop? Characterize the steady state error when
z d is a step, a ramp, and a parabola. How does this change if you add an
integrator? What is the system type with respect to an input disturbance for
both PD and PID control?

Homework E.P.6
Adding an integrator to obtain PID control for the outer loop of the ballbeam
system, put the characteristic equation in Evan’s form and use the Matlab rlocus
command to plot the root locus verses the integrator gain kI . Note that since
the integrator gain will be negative, the transfer function in Evan’s form must be
negated since the Matlab rlocus command only plots the return for kI > 0.
Select a value for kI that does not significantly change the other locations of the
closed loop poles.

Homework E.10
The objective of this problem is to implement the PID controller using only mea-
sured outputs of the system.
(a) Modify the system dynamics file so that the parameters m1 , m2 , and ` vary
by up to 20% of their nominal value each time they are run (uncertainty
parameter = 0.2).

TOC
E. DESIGN STUDY: BALL ON BEAM 391

(b) Change the simulation files so that the input to the controller is the output and
not the state. Implement the nested PID loops in Problems E.8 in a separate
class. Use the dirty derivative gain of σ = 0.05. Tune the integrators so that
there is no steady state error. The controller should only assume knowledge
of the position z, the angle θ, and the reference position zr .
(c) Note that the integrator gain will need to be negative which will cause prob-
lems for the anti-windup scheme that we have been implementing. Remove
the old anti-windup scheme and implement a new scheme where the inte-
grator only winds up when |ż| is small.

Homework E.11
The objective of this problem is to implement a state feedback controller for
the ball and beam problem. Start with the simulation files developed in Home-
work E.10.
(a) Using the values for ωnz and ζz from Homework E.8, choose the desired
locations of the four closed-loop poles so that their natural frequency is
greater than ωnz and damping ratio is greater than ζz .
(b) Add the state space matrices A, B, C, D derived in Homework E.6 to your
param file.
(c) Verify that the state space system is controllable by checking that rank(CA,B ) =
n.
(d) Find the feedback gain K such that the eigenvalues of (A − BK) are equal to
the desired closed loop poles. Find the reference gain kr so that the DC-gain
from zr to z is equal to one.
(e) Implement the state feedback scheme in simulation and tune the closed-loop
poles to vary the response of the system. Notice the effect of pole locations
on the speed of response and the control effort required.

Homework E.12
(a) Modify the state feedback solution developed in Homework E.11 to add an
integrator with anti-windup to the feedback loop for z.
(b) Add a constant input disturbance of 1 Newtons to the input of the plant and
allow the plant parameters to vary up to 20%.
(c) Tune the integrator pole (and other gains if necessary) to get good tracking
performance.

TOC
E. DESIGN STUDY: BALL ON BEAM 392

Homework E.13
The objective of this problem is to design an observer that estimates the state of the
system and to use the estimated state in the controller designed in Homework E.12.
(a) For the sake of understanding the function of the observer, for this problem
we will use exact parameters, without an input disturbance. Modify the ball
beam dynamics so that the parameters known to the controller are the actual
plant parameters (uncertainty parameter α = 0).
(b) Verify that the state space system is observable by checking that rank(OA,C ) =
n.
(c) In the control block, add an observer to estimate the state x̂, and use the esti-
mate of the state in your feedback controller. Tune the poles of the controller
and observer to obtain good performance.
(d) Modify the simulation files so that the controller outputs both u and x̂. Add a
plotting routine to plot both the state and the estimated state of the system
on the same graph.
(e) As motivation for the next chapter, add an input disturbance to the system of
0.5 and observe that there is steady state error in the response even though
there is an integrator. This is caused by a steady state error in the observation
error. In the next chapter we will show how to remove the steady state error
in the observation error.

Homework E.14
(a) Modify your solution from HW E.13 so that the uncertainty parameter is
α = 0.2, representing 20% inaccuracy in the knowledge of the system pa-
rameters, and so that the input disturbance is 0.5. Also, add noise to the
output channels zm and θm with standard deviation of 0.001.
(b) Add a disturbance observer to the controller, and verify that the steady state
error in the estimator has been removed. Tune the system to get good re-
sponse.

Homework E.15
(a) Draw by hand the Bode plot of the inner loop transfer function from force F̃
to angle θ̃ for the ball beam system. Use the bode command from Matlab
or Python and compare your results.

TOC
E. DESIGN STUDY: BALL ON BEAM 393

(b) Draw by hand the Bode plot of the outer loop transfer function from angle
θ̃ to position z̃(t) for the ball beam system. Use the bode command and
compare your results.

Homework E.16
For the inner loop of the ball & beam system, use the Matlab or Python bode
command to create a graph that simultaneously displays the Bode plots for (1)
the plant, and (2) the plant under PD control, using the control gains calculated in
Homework E.8. Use the dirty derivative when implementing the derivative portion
of the PD control.

(a) To what percent error can the closed loop system under PD control track the
desired input if all of the frequency content of θr (t) is below ωr = 1.0 ra-
dians per second?

(b) If the frequency content of the input disturbance is all contained below ωdin =
0.8 radians per second, what percentage of the input disturbance shows up
in the output?

(c) If all of the frequency content of the noise n(t) is greater than ωno = 300
radians per second, what percentage of the noise shows up in the output
signal θ?

For the outer loop of the ball & beam, use the Matlab or Python bode com-
mand to create a graph that simultaneously displays the Bode plots for (1) the
plant, and (2) the plant under PID control using the control gains calculated in
Homework E.10. Use the dirty derivative when implementing the derivative por-
tion of the PID control.

(d) If the frequency content of an output disturbance is contained below ωdout =

0.1 radian/sec, what percentage of the output disturbance will be contained
in the output under PID control?

(e) If the reference signal is yr (t) = 2 sin(0.6t) what is the output error under
PID control?

Homework E.17
For this problem, use the gains found in HW E.10.

(a) For the inner loop of the ball & beam system, use the bode and margin
commands (from Matlab or Python) to find the phase and gain margin for
the inner loop system under PD control. On the same graph, plot the open

TOC
E. DESIGN STUDY: BALL ON BEAM 394

loop Bode plot and the closed loop Bode plot. What is the bandwidth of the
closed loop system, and how does this relate to the crossover frequency?
(b) For the outer loop of the ball beam system, use the bode and margin
commands to find the phase and gain margin for the outer loop system under
PID control. Plot the open and closed loop Bode plots for the outer loop
on the same plot as the open and closed loop for the inner loop. What is
the bandwidth of the closed loop system, and how does this relate to the
crossover frequency?
(c) What is the bandwidth separation between the inner (fast) loop, and the
outer (slow) loop? For this design, is successive loop closure justified?

Homework E.18
For this homework assignment we will use the loopshaping design technique to
design a successive loop closure controller for the ball and beam problem.
(a) First consider the inner loop, where Pin (s) is the transfer function of the
inner loop derived in HW E.5. Design the inner control system Cin (s) to
stabilize the system with a phase margin close to 60 degrees, and to en-
sure that reference inputs with frequency below ωr = 1 radians/sec have
tracking error γr = 0.0032, and to ensure that noise above ωn = 1000 ra-
dians/second is rejected by γn = 0.0032.
(b) Now consider the design of the controller for the outer loop system. The
’plant’ for the design of the outer loop controller is
Pin Cin
P = Pout .
1 + Pin Cin
Design the outer loop controller Cout so that the system is stable with phase
margin close to P M = 60 degrees, and so that constant input disturbances
are rejected, reference signals with frequency below ωr = 0.1 radians/sec
are tracked with error γr = 0.01, and noise with frequency content above
ωn = 100 radians/sec are rejected with γn = 0.001. Add a prefilter F (s)
to reduce peaking in the closed loop transfer function.
(c) Implement the developed control strategy in simulation using the state space
equivalents of C(s) and F (s).

TOC
 Design Study: Planar VTOL
F
In this design study we will
explore the control design for
d mr , fr
a simplified planar version
of a quadrotor following a Jc , mc ✓
ground target. In particu-
lar, we will constrain the dy- `
namics to be in two dimen-
sion plane comprising verti-
m` , f` gravity
cal and one dimension of hor-
izontal, as shown in Fig. 21-1. h
The planar vertical take-off
and landing (VTOL) system is
comprised of a center pod of
mass mc and inertia Jc , a right
motor/rotor that is modeled as zv zt
a point mass mr that exerts a
force fr at a distance d from Figure 21-1: Planar vertical take-off and landing
the center of mass, and a left (VTOL)
motor/rotor that is modeled as
a point mass m` that exerts a force f` at a distance −d from the center of mass.
The position of the center of mass of the planar VTOL system is given by horizon-
tal position zv and altitude h. The airflow through the rotor creates a change in the
direction of flow of air and causes what is called “momentum drag.” Momentum
drag can be modeled as a viscous drag force that is proportional to the horizontal
velocity żv . In other words, the drag force is Fdrag = −µżv . The target on the
ground will be modeled as an object with position zt and altitude h = 0. We will
not explicitly model the dynamics of the target.
Use the following physical parameters: mc = 1 kg, Jc = 0.0042 kg m2 ,
mr = 0.25 kg, m` = 0.25 kg, d = 0.3 m, µ = 0.1 kg/s, g = 9.81 m/s2 .

TOC
F. DESIGN STUDY: PLANAR VTOL 396

Homework Problems:

F.2 Kinetic energy. F.10 Digital PID.

F.3 Equations of Motion. F.11 Full state feedback.
F.4 Linearize equations of motion. F.12 Full state with integrator.
F.5 Transfer function model. F.13 Observer based control.
F.6 State space model. F.14 Disturbance observer.
F.7 Pole Placement using PD. F.15 Frequency Response.
F.8 Successive loop closure. F.16 Loop gain.
F.9 Integrators and system type. F.17 Stability margins.
F.P.6 Root locus. F.18 Loopshaping design.

TOC
F. DESIGN STUDY: PLANAR VTOL 397

Homework F.2
(a) Using the configuration variables zv , h, and θ, write an expression for the
kinetic energy of the system.
(b) Create an animation of the planar VTOL system. The inputs to the animation
should be zv , zt , h, and θ. Turn in a screen capture of the animation.

Homework F.3
(a) Find the potential energy for the system.
(b) Define the generalized coordinates and damping forces.
(c) Find the generalized forces. Note that the right and left forces are more easily
modeled as a total force on the center of mass, and a torque about the center
of mass.
(d) Derive the equations of motion for the planar VTOL system using the Euler-
Lagrange equations.
(e) Referring to Appendices P.1, P.2, and P.3, write a class or s-function that im-
plements the equations of motion. Simulate the system using a variable
force inputs fr and f` . The output should connect to the animation function
developed in homework F.2.

Homework F.4
Since fr and f` appear in the equations as either fr + f` or d(fr − f` ), it is easier
4
to think of the inputs as the total force F = fr +f` , and the torque τ = d(fr −f` ).
Note that since     
F 1 1 fr
= ,
τ d −d f`
that    −1    1  
1
fr 1 1 F F
= = 21 2d
1 .
f` d −d τ 2 − 2d τ
Therefore, in subsequent exercises, if we determine F and τ , then the right and
left forces are given by
1 1
fr = F+ τ
2 2d
1 1
f` = F − τ.
2 2d

TOC
F. DESIGN STUDY: PLANAR VTOL 398

(a) Find the equilibria of the system.

(b) Linearize the equations about the equilibria using Jacobian linearization.

(c) If possible, linearize the system using feedback linearization.

Homework F.5
For the planar VTOL system, note that the system naturally divides into so-called
longitudinal dynamics (up-down motion) with total force F̃ as the input and alti-
tude h̃ as the output, and lateral dynamics (side-to-side motion) with τ̃ as the input
and z̃ as the output, with θ̃ as an intermediate variable.

(a) Start with the linearized equations of motion and use the Laplace transform to
convert the equations of motion to the s-domain.

(b) For the longitudinal dynamics, find the transfer function from the input F̃ (s)
to the output H̃(s).

(c) For the lateral dynamics, find the transfer function from the input τ̃ (s) to the
intermediate state Θ̃(s) and the output Z̃(s). Find the transfer function from
Θ̃(s) to Z̃(s).

(d) Draw a block diagram of the open loop longitudinal and lateral systems.

Homework F.6
˙
(a) Defining the longitudinal states as x̃lon = (h̃, h̃)> and the longitudinal input
as ũlon = F̃ and the longitudinal output as ỹlon = h̃, find the linear state
space equations in the form

x̃˙ lon = Ax̃lon + B ũlon

ỹlon = C x̃lon + Dũlon .

˙ >
˙ θ̃)
(b) Defining the lateral states as x̃lat = (z̃, θ̃, z̃, and the lateral input as
ũlat = τ̃ and the lateral output as ỹlat = (z̃, θ̃)> , find the linear state
space equations in the form

x̃˙ lat = Ax̃lat + B ũlat

ỹlat = C x̃lat + Dũlat .

TOC
F. DESIGN STUDY: PLANAR VTOL 399

Homework F.7
For this problem we will only consider the longitudinal (up-down) dynamics of
the VTOL system.

(a) Given the open-loop transfer function from total force F̃ to altitude h̃ found
in homework F.5, find the open-loop poles of the system.

(b) Using PD control architecture shown in Figure 7-2, find the closed-loop trans-
fer function from h̃r to h̃ and find the closed-loop poles as a function of kP
and kD .

(c) Select kP and kD to place the closed-loop poles at p1 = −0.2 and p2 = −0.3.

(d) Using the gains from part (c), implement the PD control for the altitude con-
trol in simulation and plot the step response.

Homework F.8
For the VTOL system, do the following:

(a) For the longitudinal (altitude) dynamics of the VTOL, suppose that the de-
sign requirements are that the rise time is tr ≈ 8 seconds, with a damping
ratio of ζ = 0.707. Find the desired closed-loop characteristic polyno-
mial ∆dcl (s), and the associated pole locations. Find the proportional and
derivative gains kPh and kDh to achieve these specifications, and modify the
computer simulation from HW F.8 to verify that the step response satisfies
the requirements.

(b) The lateral dynamics (side-to-side) will be designed using successive loop
closure with the inner-loop using torque to regulate the roll angle, and the
outer-loop using roll angle to regulate the position z. Focusing on the inner
loop, find the PD gains kPθ and kDθ so that the rise time of the inner loop
is trθ = 0.8 seconds, and the damping ratio is ζθ = 0.707.

(c) Find the DC gain kDCθ of the inner loop.

(d) Replacing the inner loop by its DC-gain, find the PD gains kPz and kDz so
that the rise time of the outer loop is trz = 10trθ and the damping ratio is
ζz = 0.707.

(e) Implement the successive loop closure design for the lateral control of the
VTOL system where the commanded lateral position is given by a square
wave with magnitude 3 ± 2.5 meters and frequency 0.08 Hz.

TOC
F. DESIGN STUDY: PLANAR VTOL 400

(f) Suppose that the size of the force on each rotor is constrained by 0 ≤ f` ≤
fmax = 10 N, and 0 ≤ fr ≤ fmax = 10 N. Modify the simulation to
include saturations on the right and left forces f` and fr . Using the rise
time for the altitude controller, and the rise time for the outer loop of the
lateral controller as tuning parameters, tune the controller to achieve the
fastest possible response without input saturation.

Homework F.9
(a) When the longitudinal controller for the VTOL system is PD control, what is
the system type? Characterize the steady state error when hd is a step, a
ramp, and a parabola. How does this change if you add an integrator? What
is the system type with respect to an input disturbance for both PD and PID
control?

(b) When the inner loop of the lateral controller for the VTOL system is PD
control, what is the system type of the inner loop? Characterize the steady
state error when θ̃d is a step, a ramp, and a parabola. What is the system
type with respect to an input disturbance?

(c) When the outer loop of the lateral controller for the VTOL system is PD con-
trol, what is the system type of the outer loop? Characterize the steady state
error when z d is a step, a ramp, and a parabola. How does this change if
you add an integrator? What is the system type with respect to an input
disturbance for both PD and PID control?

Homework F.P.6
(a) Adding an integrator to obtain PID control for the longitudinal controller for
the VTOL system, put the characteristic equation in Evan’s form and use
the Matlab/Python rlocus command to plot the root locus verses the in-
tegrator gain kIh . Select a value for kIh that does not significantly change
the other locations of the closed loop poles.

(b) Adding an integrator to obtain PID control for the outer loop of the lateral
controller for the VTOL system, put the characteristic equation of the outer
loop in Evan’s form and use the Matlab/Python rlocus command to plot
the root locus verses the integrator gain kIz . Select a value for kIz that does
not significantly change the other locations of the closed loop poles.

TOC
F. DESIGN STUDY: PLANAR VTOL 401

Homework F.10
The objective of this problem is to implement the PID controller using only mea-
sured outputs of the system.

(a) Modify the system dynamics file so that the parameters mc , Jc , d, and µ vary
by up to 20% of their nominal value each time they are run (uncertainty
parameter = 0.2).

(b) Change the simulation files so that the input to the controller is the output
and not the state. Implement the nested PID loops in Problems F.8. Use
the dirty derivative gain of σ = 0.05. Tune the integrator so that there is
no steady state error. The controller should only assume knowledge of the
position z, the angle θ, the altitude h, the reference position zr , and the
reference altitude hr .

Homework F.11
The objective of this problem is to implement a state feedback controller for the
VTOL system. Start with the simulation files developed in Homework F.10.

(a) Using the values for ωnh , ζh , ωnz , and ζz from Homework F.8, choose the
closed-loop pole locations to ensure that the longitudinal poles have damp-
ing ratios greater than ζh and natural frequencies greater than ωnh . Sim-
ilarly choose the closed-loop poles for the lateral dynamics to ensure that
their damping ratios are greater than ζz and natural frequencies greater than
ωnz

(b) Add the state space matrices A, B, C, D derived in Homework F.6 to your
param file.

(c) Verify that the state space system is controllable by checking that rank(CA,B ) =
n.

(d) Find the feedback gain K such that the eigenvalues of (A − BK) are equal
to desired closed loop poles. Find the reference gains krh and krz so that
the DC-gain from hr to h is equal to one, and the DC-gain from zr to z is
equal to one.

(e) Implement the state feedback scheme and tune the closed-loop poles to get
acceptable response. What changes would you make to your closed-loop
pole locations to decrease the rise time of the system? How would you
change them to lower the overshoot in response to a step command?

TOC
F. DESIGN STUDY: PLANAR VTOL 402

Homework F.12

(a) Modify the state feedback solution developed in Homework F.11 to add an
integrator with anti-windup to the altitude feedback loop and to the position
feedback loop.

(b) Allow the plant parameters to vary up to 20% and add a constant input dis-
turbance of 0.1 Newtons to the input of position dynamics simulating wind.
Hint: The best place to add the wind force is in the class that implements
the dynamics. For example, one possibility is to modify the z dynamics as:
z̈ = (−(fr + fl )sin(θ) − µż + F _wind) / (mc + 2mr )

(c) Tune the integrator poles on both loops (and other gains if necessary) to get
good tracking performance.

Homework F.13
The objective of this problem is to design an observer that estimates the state of the
system and to use the estimated state in the controller designed in Homework F.12.

(a) For the sake of understanding the function of the observer, for this problem we
will use exact parameters, without an input disturbance. Modify the VTOL
dynamics so that the parameters known to the controller are the actual plant
parameters (uncertainty parameter α = 0).

(b) Verify that the state space system is observable by checking that rank(OA,C ) =
n.

(c) In the control block, add an observer to estimate the state x̂, and use the esti-
mate of the state in your feedback controller. Tune the poles of the controller
and observer to obtain good performance.

(d) Modify the simulation files so that the controller outputs both u and x̂. Add a
plotting routine to plot both the state and the estimated state of the system
on the same graph.

(e) As motivation for the next chapter, add an input force disturbance to the sys-
tem of 1.0 and an input torque disturbance of 0.1 and observe that there is
steady state error in the response even though there is an integrator. This is
caused by a steady state error in the observation error. In the next chapter
we will show how to remove the steady state error in the observation error.

TOC
F. DESIGN STUDY: PLANAR VTOL 403

Homework F.14
(a) Modify your solution from HW F.13 so that the uncertainty parameter is
α = 0.2, representing 20% inaccuracy in the knowledge of the system pa-
rameters. Modify VTOL_dynamics to add an altitude disturbance of 1.0
and a wind disturbance of 1.0 m/s. A Python code snippet that implements
these disturbances is given by
1 from math import sin
2 # disturbances
3 wind = 1.0
4 altitude_dist = 1.0
5
6 # add wind disturbance
7 zdot = zdot + wind
8
9 zddot = -(fr+fl)*sin(theta)/(mc+2*mr)
10 hddot = (-(mc+2*mr)*g \
11 + (fr+fl)*cos(theta))/(mc+2*mr) \
12 + altitude_dist
13 thetaddot = d*(fr-fl)/(Jc+2*mr*d**2)

Before adding the disturbance observer, run the simulation and note that the
controller is not robust to the input disturbance.

(b) Add a disturbance observer to both controllers, and verify that the steady state
error in the estimator has been removed and that the disturbances have been
adequately compensated. Tune the system to get good response.

Homework F.15

(a) Draw by hand the Bode plot of the altitude transfer function from force F̃ to
altitude h̃. Use the bode command (from Matlab or Python) and compare
your results.

(b) Draw by hand the Bode plot of the inner loop transfer function for the lateral
dynamics from torque τ to angle θ. Use the bode command and compare
your results.

(c) Draw by hand the Bode plot of the outer loop transfer function for the lat-
eral dynamics from angle θ to position z(t). Use the bode command and
compare your results.

TOC
F. DESIGN STUDY: PLANAR VTOL 404

Homework F.16
For the altitude hold loop of the VTOL system, use the bode command (from
Matlab or Python) to create a graph that simultaneously displays the Bode plots for
(1) the plant, and (2) the plant under PID control using the control gains calculated
in Homework F.10. Use the dirty derivative when implementing the derivative
portion of the PID control.

(a) What is the tracking error if the reference input is a parabola with curvature
5?
(b) If all of the frequency content of the noise n(t) is greater than ωno = 30
radians per second, what percentage of the noise shows up in the output
signal h?

For the inner loop of the lateral controller for the VTOL system, use the bode
command to create a graph that simultaneously displays the Bode plots for (1)
the plant, and (2) the plant under PD control, using the control gains calculated in
Homework F.8. Use the dirty derivative when implementing the derivative portion
of the PD control.

(c) If the frequency content of the input disturbance is all contained below ωdin =
2 radians per second, what percentage of the input disturbance shows up in
the output?
(d) If a sensor for θ is to be selected, what are the characteristics of the sensor
(frequency band and size) that would result in measurement noise for θ that
is less than 0.1 degrees?

For the outer loop of the lateral controller for the VTOL system, use the bode
command to create a graph that simultaneously displays the Bode plots for (1) the
plant, and (2) the plant under PID control, using the control gains calculated in
Homework F.10.

(e) To what percent error can the closed loop system track the desired input if all
of the frequency content of zr (t) is below ωr = 0.1 radians per second?
(f) If the frequency content of an output disturbance is contained below ωdout =
0.01 radian/sec, what percentage of the output disturbance will be contained
in the output?

Homework F.17
For this problem, use the gains found in Homework F.10.

TOC
F. DESIGN STUDY: PLANAR VTOL 405

(a) For the altitude hold loop of the VTOL system, use the bode and margin
commands (from Matlab or Python) to find the phase and gain margin for
the closed loop system under PID control. On the same graph, plot the open
loop Bode plot and the closed loop Bode plot. What is the bandwidth of the
closed loop system, and how does this relate to the crossover frequency?

(b) For the inner loop of the lateral control loop, use the bode and margin
commands to find the phase and gain margin for the inner loop system under
PD control. On the same graph, plot the open loop Bode plot and the closed
loop Bode plot. What is the bandwidth of the closed loop system, and how
does this relate to the crossover frequency?

(c) For the outer loop of the lateral control loop, use the bode and margin
commands to find the phase and gain margin for the outer loop system under
PD control. Plot the open and closed loop Bode plots for the outer loop
on the same plot as the open and closed loop for the inner loop. What is
the bandwidth of the closed loop system, and how does this relate to the
crossover frequency?

(d) What is the bandwidth separation between the inner (fast) loop, and the
outer (slow) loop? For this design, is successive loop closure justified?

Homework F.18
For this homework assignment we will use the loopshaping design technique to
design a controller for the VTOL flight control problem.

(a) First consider the longitudinal or altitude control problem. For this part, the
force command will be

F = Fe + Clon (s)Eh (s),

where Fe is the equilibrium force as found in other homework problems,

and the error signal is eh = hrf − hm , where hrf is the prefiltered reference
altitude command, and hm is the measured altitude. Find the longitudi-
nal controller Clon (s) so that the closed loop system meets the following
specifications.

• Reject constant input disturbances.

• Track reference signals with frequency content below ωr = 0.1 rad/s
to within γr = 0.01.
• Attenuate noise on the measurement of h for all frequencies above
ωn = 200 rad/s by γn = 10−4 .
• The phase margin is close to P M = 60 deg.

TOC
F. DESIGN STUDY: PLANAR VTOL 406

• Use a prefilter to improve the closed loop response.

(b) Now consider the inner loop of the lateral, or position, dynamics. Find the
inner loop controller Clatin (s) so that the inner loop is stable with phase
margin approximately P M = 60 deg, and with closed loop bandwidth
approximately ωco = 10 rad/s. Add a low pass filter to enhance noise
rejection.
(c) For the outer loop of the lateral system, find the controller Clatout (s) to meet
the following specifications:
• Cross over frequency is around ωco = 1 rad/s.
• Reject constant input disturbances.
• Attenuate noise on the measurement of z for all frequencies above
ωno = 100 rad/s by γno = 10−5 .
• The phase margin is close to P M = 60 deg.
• Use a prefilter to reduce overshoot in the step response.

(c) Implement the developed control strategy in simulation using the state space
or digital filter equivalents of C(s) and F (s).

TOC
F. DESIGN STUDY: PLANAR VTOL 407

Appendices

TOC
F. DESIGN STUDY: PLANAR VTOL 408

TOC
 Simulating Control Systems Using Python
P.1
In this appendix we will show how to simulate the feedback control of a dynamic
system using Python. Consider the general feedback control system shown in
Figure 1-1. The output of the physical system is the y(t), the input to the physical
system is u(t) plus an external disturbance signal d(t). The output of the system
is corrupted by zero-mean noise n(t). The input to the controller is the reference
signal r(t) as well as the output corrupted by noise. In Python, each block shown
in Figure 1-1 is implemented using a Python Class.

Figure 1-1: Block diagram of a general feedback system.

We will illustrate how to implement feedback control systems in Python by

simulating the physical system described by the differential equation
ÿ + 2ẏ + 3y = 4u, (P.1.1)
and using the PID controller
Z t
u(t) = kp (r(t) − y(t)) + ki (r(τ ) − y(τ ))dτ − kd ẏ. (P.1.2)
−∞

TOC
 APPENDIX P.1. SIMULATING CONTROL SYSTEMS IN PYTHON 410

The feedback control system system shown in Figure 1-1 is simulated in

systemSim.py which is listed below.
1 import matplotlib.pyplot as plt
2 from systemDynamics import systemDynamics
3 from systemController import systemController
4 from signalGenerator import signalGenerator
5 from systemAnimation import systemAnimation
6 from dataPlotter import dataPlotter
7
8 # simulation parameters
9 t_start = 0.0 # start time
10 t_end = 20.0 # end time
11 t_plot = 0.1 # sample rate for plotter and animation
12 Ts = 0.01 # sample rate for dynamics and controller
13
14 # instantiate system, controller, and reference classes
15 system = systemDynamics(sample_rate=Ts)
16 controller = pendulumController(sample_rate=Ts)
17 reference = signalGenerator(amplitude=0.5, frequency=0.05)
18 disturbance = signalGenerator(amplitude=1.0, frequency = 0.0)
19 noise = signalGenerator(amplitude=0.01)
20
21 # instantiate the simulation plots and animation
22 dataPlot = dataPlotter()
23 animation = systemAnimation()
24
25 t = t_start # time starts at t_start
26 y = system.h() # output of system at start of simulation
27 while t < t_end: # main simulation loop
28 # set time for next plot
29 t_next_plot = t + t_plot
30 # Propagate dynamics and controller at fast rate Ts
31 while t < t_next_plot:
32 r = reference.square(t) # assign reference
33 d = disturbance.step(t) # simulate input disturbance
34 n = noise.random(t) # simulate sensor noise
35 u = controller.update(r, y + n) # update controller
36 y = system.update(u + d) # Propagate the dynamics
37 t = t + Ts # advance time by Ts
38 # update animation and data plots
39 animation.update(system.state)
40 dataPlot.update(t, ref_input, pendulum.state, u)
41 # pause causes the figure to be displayed during the simulation
42 plt.pause(0.0001)
43
44 # Keeps the program from closing until the user presses a button.
45 print(’Press key to close’)
46 plt.waitforbuttonpress()
47 plt.close()

Listing P.1.1: systemSim.py

The physical system is implemented in a class contained in

systemDynamic.py, which will be discussed below, and which is imported on
line 2, and instantiated on line 15. The system is updated at sample rate Ts given

TOC
 APPENDIX P.1. SIMULATING CONTROL SYSTEMS IN PYTHON 411

the control input u and the input disturbance d on line 36.

The controller is implemented in a class contained in
systemController.py, which will be discussed below, and which is im-
ported on line 3, and instantiated on line 16. Given the reference input r, and the
system output y, the controller is updated at the fast sample rate Ts on line 35.
At the initial time, the controller needs the initial output at time t = 0, which is
computed on line 26 before the main loop begins.
The reference input is implemented using the signalGenerator class that
is contained in signalGenerator.py, which is described below, and which
is imported on line 4 and instantiated on line 17. The reference input at time t,
i.e., r(t) is assigned a value on line 32.
The external disturbance d(t) is also implemented using the
signalGenerator class and is instantiated on line 17, and assigned a value
on line 33. For this example, the external disturbance will be a constant value of
d(t) = 1.0. The disturbance is assigned on line 33, and is input to the system on
line 36.
In addition to the feedback loop as implemented above, the system will be
visualized through an animation, and also by plotting important variables like the
reference input, the system states, and the control input. The animation and data
plotting functions are implemented in systemAnimation.py and
dataPlotter.py, are imported on lines 5 and 6, instantiated on lines 22 and 23,
and updated on lines 39 and 40. The pause command on line 42 forces the ani-
mation and data plotting functions to update their displays each time through the
loop.

Appendix P.1.1 System Dynamics

This section will describe how to implement the system dynamics given in Equa-
tion (P.1.1). The first step is to convert the dynamics to the state space form

ẋ = f (x, u)
y = h(x, u).

For the differential equation in Equation (P.1.1) let x = (y, ẏ)> , which gives
       
ẋ1 ẏ ẏ x2 4
ẋ = = = = = f (x, u)
ẋ2 ÿ −2ẏ − 3y + 4u −2x2 − 3x1 + 4u
(P.1.3)
4
y = x1 = h(x). (P.1.4)

The dynamics of the system are implemented in systemDynamics.py

which is listed below.
1 import numpy as np
2

TOC
 APPENDIX P.1. SIMULATING CONTROL SYSTEMS IN PYTHON 412

3 class systemDynamics:
4 def __init__(self, sample_rate):
5 # Initial state conditions
6 y0 = 0.0
7 ydot0 = 0.0
8 self.state = np.array([
9 [y0], # initial condition for y
10 [ydot0], # initial condition for ydot
11 ])
12 self.Ts = sample_rate # sample rate of system
13 self.limit = 1.0 # input saturation limit
14 # system parameters
15 self.a0 = 3.0
16 self.a1 = 2.0
17 self.b0 = 4.0
18 # modify the system parameters by random value
19 alpha = 0.2 # Uncertainty parameter
20 self.a1 = self.a1 * (1.+alpha*(2.*np.random.rand()-1.))
21 self.a0 = self.a0 * (1.+alpha*(2.*np.random.rand()-1.))
22 self.b0 = self.b0 * (1.+alpha*(2.*np.random.rand()-1.))
23
24 def f(self, state, u):
25 # for system xdot = f(x,u), return f(x,u)
26 y = state.item(0)
27 ydot = state.item(1)
28 # The equations of motion.
29 yddot = -self.a1 * ydot - self.a0 * y + self.b0 * u
30 # build xdot and return
31 xdot = np.array([[ydot], [yddot]])
32 return xdot
33
34 def h(self):
35 # Returns the measured output y = h(x)
36 y = self.state.item(0)
37 # return output
38 return y
39
40 def update(self, u):
41 # This is the external method that takes the input u(t)
42 # and returns the output y(t).
43 u = self.saturate(u, self.limit) # saturate the input
44 self.rk4_step(u) # propagate the state by one time step
45 y = self.h() # compute the output at the current state
46 return y
47
48 def rk4_step(self, u):
49 # Integrate ODE using Runge-Kutta RK4 algorithm
50 F1 = self.f(self.state, u)
51 F2 = self.f(self.state + self.Ts / 2 * F1, u)
52 F3 = self.f(self.state + self.Ts / 2 * F2, u)
53 F4 = self.f(self.state + self.Ts * F3, u)
54 self.state += self.Ts / 6 * (F1 + 2 * F2 + 2 * F3 + F4)
55
56 def saturate(self, u, limit):
57 if abs(u) > limit:
58 u = limit*np.sign(u)
59 return u

TOC
 APPENDIX P.1. SIMULATING CONTROL SYSTEMS IN PYTHON 413

Listing P.1.2: systemDynamics.py

The system is initialized in lines 4–22. The state is initialized in lines 6–11
based on the initial conditions for y(0) and ẏ(0). The system parameters are ini-
tialized in lines 15–22. The parameters for any physical system are never known
exactly. Therefore, feedback systems need to be designed to be robust to this
uncertainty. In the simulation, we model uncertainty in our knowledge of the
physical parameters by changing the nominal value by a uniform random vari-
able. Let α be the amount of change allowed for each parameter, i.e., alpha =
0.2, means that the physical parameters may change by up to 20%. The function
np.random.rand() returns a uniform random variable in the interval [0, 1].
Therefore (1 + α(2 ∗ np.random.rand() − 1)) is a random variable in the
interval [−α, α]. The nominal value for the physical parameters that are known to
the designer, are specified in lines 15–17. The actual values used by the simula-
tion, are randomly selected in lines 19–22.
The system dynamics

ẋ = f (x, u)
y = h(x, u).

are specified in lines 24–38. Lines 24–32 define f (x, u), and lines 34–38 define
h(x, u), as given by Equations (P.1.3) and (P.1.4) respectively.
The main feedback loop calls the update routine in lines 40–46. The update
function first saturates the input in line 43, where the saturation function is
defined in lines 56–59. The system dynamics are then propagated forward by one
sample time Ts using the Runge-Kutta 4 algorithm in line 44, where the RK4 al-
gorithm is specified in lines 48–54, as described in Appendix P.4. The update
function finally computes and returns the output y in lines 45–46.

Appendix P.1.2 Controller

This section will describe how to implement the PID controller given in Equa-
tion (P.3.2). The controller is implemented in systemController.py, which
is listed below.
1 import numpy as np
2
3 class systemController:
4 def __init__(self, sample_rate):
5 # specify gains
6 self.kp = 1.0
7 self.kd = 1.0
8 self.ki = 1.0
9 self.Ts = sample_rate
10 self.sigma = 2.0 * self.Ts
11 self.beta = (2.0*sigma-self.Ts)/(2.0*sigma+self.Ts)

TOC
 APPENDIX P.1. SIMULATING CONTROL SYSTEMS IN PYTHON 414

12 self.limit = 1.0
13 # internal storage for differentiator
14 self.y_dot = 0.0
15 self.y_d1 = 0.0
16 # internal storage for integrator
17 self.integrator = 0
18 self.error_d1 = 0
19
20 def update(self, r, y):
21 e = r - y # define the error
22 I = self.integrate_e(e) # integrate error
23 ydot = self.differentiate_y(y) # differentiate y
24 # form PID controller, saturate, and return
25 u = self.kp * e + self.ki * I - self.kd * ydot
26 u = self.saturate(u)
27 return u
28
29 def integrate_e(self, e):
30 self.integrator = self.integrator \
31 + (self.Ts / 2) * (error + self.error_d1)
32 self.error_d1 = error
33 return self.integrator
34
35 def differentiate_y(self, y):
36 self.y_dot = self.beta*self.y_dot \
37 + (1-self.beta)/self.Ts * (y - self.y_d1)
38 self.y_d1 = y
39 return self.y_dot
40
41 def saturate(self, u):
42 if abs(u) > self.limit:
43 u = self.limit*np.sign(u)
44 return u

Listing P.1.3: systemController.py

The controller is instantiated in lines 4–18. The PID gains are specified in
lines 6–8. The sample rate of the controller is passed into the system at the time of
instantiation and is assigned an internal variable Ts in line 9. The dirty derivative
gain σ described in Chapter 10 is specified in line 10, and the intermediate gain
β = 2σ−T s
2σ+Ts is specified on line 11. Internal storage variables for the differentiator
and the integrator, as explained in Chapter 10 are given in lines 13–18. The PID
control update routine is specified in lines 20–27. The error e = r − y as shown
in Figure 1-1 is defined in line 21. The PID controller in Equations (P.3.2) uses
the integral of e and the derivative of y. The integral of e is computed on line 22
using the routine specified in lines 29–33. A derivation of the integral equations is
given in Chapter 10. The derivative of y is computed in line 23 using the routine
specified in lines 35–39. A derivation of the dirty derivative equations is given
in Chapter 10. Finally, the PID controller computed, the value is saturated, and
returned in lines 25, 26, and 27, respectively.

TOC
 APPENDIX P.1. SIMULATING CONTROL SYSTEMS IN PYTHON 415

Appendix P.1.3 Signal Generator

Python code for the signal generator class is listed below.
1 import numpy as np
2
3 class signalGenerator:
4 def __init__(self, amplitude=1.0, frequency=0.001, y_offset=0):
5 self.amplitude = amplitude # signal amplitude
6 self.frequency = frequency # signal frequency
7 self.y_offset = y_offset # signal y-offset
8
9 def square(self, t):
10 if t % (1.0/self.frequency) <= 0.5/self.frequency:
11 out = self.amplitude + self.y_offset
12 else:
13 out = - self.amplitude + self.y_offset
14 return out
15
16 def sawtooth(self, t):
17 tmp = t % (0.5/self.frequency)
18 out = 4 * self.amplitude * self.frequency*tmp \
19 - self.amplitude + self.y_offset
20 return out
21
22 def step(self, t):
23 if t >= 0.0:
24 out = self.amplitude + self.y_offset
25 else:
26 out = self.y_offset
27 return out
28
29 def random(self, t):
30 out = np.random.normal(self.y_offset, self.amplitude)
31 return out
32
33 def sin(self, t):
34 out = self.amplitude * np.sin(2*np.pi*self.frequency*t) \
35 + self.y_offset
36 return out

Listing P.1.4: signalGenerator.py

The class is instantiated by passing in the desired amplitude of the signal, the
frequency in Hertz, and a y-offset value. For random signals, the amplitude is
interpreted to be be the standard deviation of the signal. The signal generator can
then be used to produce a square wave, a sawtooth signal, a step, a random signal,
or a sinusoid, as specified in lines 9–36.

Appendix P.1.4 Data Plotter

The objective of the data plotter is to plot the relevant signals during the simula-
tion. For this simple example, we will plot the output y and the reference r on

TOC
 APPENDIX P.1. SIMULATING CONTROL SYSTEMS IN PYTHON 416

one plot, the velocity ẏ on another plot, and the control signal u on a third plot.
For plotting and animation, we will use the Python Matplotlib library. Python
code for the data plotter class is listed below.

1 import matplotlib.pyplot as plt

2 from matplotlib.lines import Line2D
3 import numpy as np
4
5 plt.ion() # enable interactive drawing
6
7 class dataPlotter:
8 def __init__(self):
9 # Number of subplots = num_of_rows*num_of_cols
10 self.num_rows = 3 # Number of subplot rows
11 self.num_cols = 1 # Number of subplot columns
12 # Crete figure and axes handles
13 self.fig, self.ax = plt.subplots(self.num_rows,
14 self.num_cols,
15 sharex=True)
16 # Instantiate lists to hold the time and data histories
17 self.time_history = [] # time
18 self.r_history = [] # reference r
19 self.y_history = [] # output y
20 self.ydot_history = [] # velocity ydot
21 self.u_history = [] # input u
22 # create a handle for every subplot.
23 self.handle = []
24 self.handle.append(subplotWindow(self.ax[0],
25 ylabel=’y’,
26 title=’Simple System’))
27 self.handle.append(subplotWindow(self.ax[1],
28 ylabel=’ydot’))
29 self.handle.append(subplotWindow(self.ax[2],
30 xlabel=’t(s)’,
31 ylabel=’u’))
32
33 def update(self, time, reference, state, control):
34 # update the time history of all plot variables
35 self.time_history.append(time)
36 self.r_history.append(reference)
37 self.y_history.append(state.item(0))
38 self.ydot_history.append(state.item(1))
39 self.u_history.append(control)
40
41 # update the plots with associated histories
42 self.handle[0].update(self.time_history,
43 [self.r_history, self.y_history])
44 self.handle[1].update(self.time_history,
45 [self.ydot_history])
46 self.handle[1].update(self.time_history,
47 [self.u_history])
48
49 class subplotWindow:
50 # Create each individual subplot.
51 def __init__(self, ax, xlabel=’’, ylabel=’’, title=’’,
52 legend=None):

TOC
 APPENDIX P.1. SIMULATING CONTROL SYSTEMS IN PYTHON 417

53 ’’’
54 ax - This is a handle to the axes of the figure
55 xlable - Label of the x-axis
56 ylable - Label of the y-axis
57 title - Plot title
58 legend - A tuple of strings that identify the data.
59 EX: ("data1","data2", ... , "dataN")
60 ’’’
61 self.legend = legend
62 self.ax = ax # Axes handle
63 self.colors = [’b’, ’g’, ’r’, ’c’, ’m’, ’y’, ’b’]
64 # A list of colors. The first color in the list corresponds
65 # to the first line object, etc.
66 # ’b’ - blue, ’g’ - green, ’r’ - red, ’c’ - cyan,
67 # ’m’ - magenta, ’y’ - yellow, ’k’ - black
68 self.line_styles = [’-’, ’-’, ’--’, ’-.’, ’:’]
69 # A list of line styles. The first line style in the list
70 # corresponds to the first line object.
71 # ’-’ solid, ’--’ dashed, ’-.’ dash_dot, ’:’ dotted
72 self.line = []
73 # Configure the axes
74 self.ax.set_ylabel(ylabel)
75 self.ax.set_xlabel(xlabel)
76 self.ax.set_title(title)
77 self.ax.grid(True)
78 # Keeps track of initialization
79 self.init = True
80
81 def update(self, time, data):
82 # Adds data to the plot.
83 # Initialize the plot the first time routine is called
84 if self.init==True:
85 for i in range(len(data)):
86 # Instantiate line object and add it to the axes
87 self.line.append(Line2D(time, data[i],
88 color=self.colors[np.mod(i,len(self.colors)-1)],
89 ls=self.line_styles
90 [np.mod(i, len(self.line_styles) - 1)],
91 label=self.legend
92 if self.legend!=None else None))
93 self.ax.add_line(self.line[i])
94 self.init = False
95 # add legend if one is specified
96 if self.legend!=None:
97 plt.legend(handles=self.line)
98 else: # Add new data to the plot
99 # Updates the x and y data of each line.
100 for i in range(len(self.line)):
101 self.line[i].set_xdata(time)
102 self.line[i].set_ydata(data[i])
103
104 # Adjusts the axis to fit all of the data
105 self.ax.relim()
106 self.ax.autoscale()

Listing P.1.5: dataPlotter.py

TOC
 APPENDIX P.1. SIMULATING CONTROL SYSTEMS IN PYTHON 418

The plotter is initialized in lines 9–31. Lines 10–11, specify that the plot
window will contain three plots arranged as a 3 × 1 array. The routine keeps
track of the time histories of all variables to be plotted. Lines 17-21 initialize time
histories for the time t, the reference signal r, the output y, and the velocity ẏ, and
the input u. Lines 23–31 set up the individual plots with labels, by instantiating
objects from the subplotWindow class, which will be described below.
The update method receives as input the current time,
reference, state, and control values. Lines 35–39 appends the cur-
rent values to the history lists maintained in memory for each of those values.
The plots are then updated with the new histories in lines 42–47 by calling the
subplotWindow.update method, which will be described below.
The subplotWindow class is instantiated in lines 50–79. If multiple data
streams are on a single plot, the color of the line correspond to the order specified
on line 63, and the style of the lines will be in the order specified on line 68. The
subplotWindow.update method is specified in lines 81–102. The first time
the subplotWindow.update method is called, the line objects are initialized
and plotted in lines 85–97. In subsequent calls to subplotWindow.update
the data lines are modified in lines 100–102.

Appendix P.1.5 Animation

In this section we will show how to create an animation for the inverted pendulum
case study. Consider the image of the inverted pendulum shown in Fig. 1-2, where
the configuration is completely specified by the position of the cart z, and the angle
of the rod from vertical θ. The physical parameters of the system are the rod length
`, the base width w, the base height h, and the gap between the base and the track
g.
The first step in developing the animation is to determine the position of points
that define the animation. The pendulum will require that we draw three objects:
the rectangular cart, the circular bob at the end of the rod, and the rod. We will
draw the cart as a rectangle, the bob as a circle, and the rod as a line. In Python,
rectangles are specified by their lower-left corner, their width, and their height.
The lower left corner of the rectangle is (z − w/2, g). In Python, circles are
specified by their center and radius. The center of the bob is (z +(`+R) sin θ, g +
h + (` + R) cos θ). In Python, lines are specified by the x and y locations of their
endpoints. The x locations of the endpoints of the rod are X = (z, z + ` sin θ),
and the y locations of the endpoints are Y = (g + h, g + h + ` cos θ).
Python code for the animation class is listed below.
1 import matplotlib.pyplot as plt
2 import matplotlib.patches as mpatches
3 import numpy as np
4 import pendulumParam as P
5
6 class pendulumAnimation:
7 def __init__(self):

TOC
 APPENDIX P.1. SIMULATING CONTROL SYSTEMS IN PYTHON 419

Figure 1-2: Drawing for inverted pendulum. The first step in developing an an-
imation is to draw a figure of the object to be animated and identify all of the
physical parameters.

8 self.flag_init = True # Used to indicate initialization

9 # Initialize a figure and axes object
10 self.fig, self.ax = plt.subplots()
11 # Initializes a list of objects (patches and lines)
12 self.handle = []
13 # Specify the x,y axis limits
14 plt.axis([-3*P.ell, 3*P.ell, -0.1, 3*P.ell])
15 # Draw line for the ground
16 plt.plot([-2*P.ell, 2*P.ell], [0, 0], ’b--’)
17 # label axes
18 plt.xlabel(’z’)
19
20 def update(self, state):
21 z = state.item(0) # Horizontal position of cart, m
22 theta = state.item(1) # Angle of pendulum, rads
23 # draw plot elements: cart, bob, rod
24 self.draw_cart(z)
25 self.draw_bob(z, theta)
26 self.draw_rod(z, theta)
27 self.ax.axis(’equal’)
28 # Set initialization flag to False after first call
29 if self.flag_init == True:
30 self.flag_init = False
31
32 def draw_cart(self, z):
33 # specify bottom left corner of rectangle
34 x = z-P.w/2.0
35 y = P.gap
36 corner = (x, y)
37 # create rectangle on first call, update on subsequent calls
38 if self.flag_init is True:
39 # Create the Rectangle patch and append its handle
40 # to the handle list

TOC
 APPENDIX P.1. SIMULATING CONTROL SYSTEMS IN PYTHON 420

41 self.handle.append(
42 mpatches.Rectangle(corner, P.w, P.h,
43 fc=’blue’, ec=’black’))
44 # Add the patch to the axes
45 self.ax.add_patch(self.handle[0])
46 else:
47 self.handle[0].set_xy(corner) # Update patch
48
49 def draw_bob(self, z, theta):
50 # specify center of circle
51 x = z+(P.ell+P.radius)*np.sin(theta)
52 y = P.gap+P.h+(P.ell+P.radius)*np.cos(theta)
53 center = (x, y)
54 # create circle on first call, update on subsequent calls
55 if self.flag_init is True:
56 # Create the CirclePolygon patch and append its handle
57 # to the handle list
58 self.handle.append(
59 mpatches.CirclePolygon(center, radius=P.radius,
60 resolution=15, fc=’limegreen’, ec=’black’))
61 # Add the patch to the axes
62 self.ax.add_patch(self.handle[1])
63 else:
64 self.handle[1].xy = center
65
66 def draw_rod(self, z, theta):
67 # specify x-y points of the rod
68 X = [z, z+P.ell*np.sin(theta)]
69 Y = [P.gap+P.h, P.gap+P.h+P.ell*np.cos(theta)]
70 # create rod on first call, update on subsequent calls
71 if self.flag_init is True:
72 # Create the line object and append its handle
73 # to the handle list.
74 line, = self.ax.plot(X, Y, lw=1, c=’black’)
75 self.handle.append(line)
76 else:
77 self.handle[2].set_xdata(X)
78 self.handle[2].set_ydata(Y)

Listing P.1.6: pendulumAnimation.py

The pendulumAnimation class is initialized in lines 7–18. The flag
self.flag_init is used to indicate the first time the animation function is
called. On the first call when self.flag_init==True, all of the plot ele-
ments are initialized and configured (see lines 38–45, lines 55–62, and lines 71–
75). After the first call, when when self.flag_init==False, each plot
element is simply updated (see lines 47, lines 64, and lines 77–78). Line 10 ini-
tializes the plot window and axes, line 12 initializes an empty list of handles to
plot objects, line 14 sets the size of the axes to be ±3` along the x-axes and −0.1
to 3` along the y-axis, line 16 draws the line representing the ground, and line 18
labels the x-axis as the configuration variable z.
The animation function is updated each time step using the update method
shown in lines 20–30. The input to the update method is the state of the
pendulum, of which the first element is z and the second element is θ (lines 21–

TOC
APPENDIX P.1. SIMULATING CONTROL SYSTEMS IN PYTHON 421

22). Lines 24–26 then call methods, that will be described below, that draw the
cart, bob, and rod. Line 27 ensures that the axes are not distorted. Lines 29–
30 set the initialization flag self.flat_init to False after the first call to
update.
The draw_cart method is listed in lines 32–47. The cart is drawn as a
rectangle patch, which is specified by its lower-left corner, and the width and
height of the rectangle. The lower-left corner or the rectangle are specified in
lines 34–35, and the width and height are specified in line 42. The rectangle
patch is created in lines 42–43 and appended to the list of handles in line 41. The
patch is added to the graph in line 45. After initialization, the only element of the
cart rectangle that changes with time is the lower-left corner, which is updated in
line 47.
The draw_bob method is listed in lines 49–64. The bob is represented as a
circle patch, which in Python is specified by the x − y coordinates of the center of
the circle, and by its radius. The x − y center coordinates are given in lines 51–53.
The circle patch is initialized in lines 59–60, where resolution is the angular
resolution of the circle patch, fc is the color of the bob, and ec is the color of
the outline. The circle patch is added to the handle list in line 58, and the handle
is added to figure in line 62. After initialization, the only element of the bob that
changes with time is it center, which is updated on line 64.
The draw_rod method is listed in lines 66–78. The rod is draw as a line,
which in Python is specified by the x and y-coordinates of the start and end of
the line, which are given in lines 68–69. The line object is created in line 74, and
added to the handle list in line 75. After initialization, both ends of the line change
with time and are updated on lines 77–78.

TOC
APPENDIX P.1. SIMULATING CONTROL SYSTEMS IN PYTHON 422

TOC
 Simulating Control Systems Using Matlab
P.2
In this appendix we will show how to simulate the feedback control of a dynamic
system using Matlab’s object oriented class structure. Consider the general feed-
back control system shown in Figure 2-1. The output of the physical system is the
y(t), the inputs to the physical system are u(t) and an external disturbance signal
d(t). The output of the system y(t) is corrupted by zero-mean noise n(t). The
input to the controller is the reference signal r(t) as well as the output corrupted
by noise. In Matlab, each block shown in Figure 2-1 can be implemented using
a Matlab Class.

Figure 2-1: Block diagram of a general feedback system.

We will illustrate how to implement feedback control systems in Matlab by

simulating the physical system described by the differential equation

ÿ + 2ẏ + 3y = 4u, (P.2.1)

TOC
 APPENDIX P.2. SIMULATING CONTROL SYSTEMS IN MATLAB 424

and using the PID controller

Z t
u(t) = kp (r(t) − y(t)) + ki (r(τ ) − y(τ ))dτ − kd ẏ. (P.2.2)
−∞

The feedback control system system shown in Figure 2-1 is simulated in

systemSim.m which is listed below.
1 t_start = 0; % start time
2 t_end = 20; % end time
3 t_plot = 0.1; % sample rate for plotter and animation
4 Ts =0.01; % sample rate for dynamics and controller
5
6 % instantiate system, controller, and reference classes
7 system = systemDynamics(Ts);
8 controller = systemController(Ts);
9 reference = signalGenerator(0.5, 0.05);
10 disturbance = signalGenerator(1.0, 0.0);
11 noise = signalGenerator(0.01);
12
13 % instantiate the data plots and animation
14 dataPlot = dataPlotter();
15 animation = systemAnimation();
16
17 % main simulation loop
18 t = t_start; % time starts at t_start
19 y = system.h(); % system output at start of simulation
20 while t < t_end
21 % set time for next plot
22 t_next_plot = t + P.t_plot;
23 % updates control and dynamics at faster simulation rate
24 while t < t_next_plot
25 r = reference.square(t); % assign reference
26 d = disturbance.step(t); % simulate input disturbance
27 n = noise.random(t); % simulate noise
28 u = controller.update(r, y + n); % update controller
29 y = system.update(u+d); % Propagate the dynamics
30 t = t + Ts; % advance time by Ts
31 end
32 % update animation and data plots
33 animation.update(system.state);
34 dataPlot.update(t, r, system.state, u);
35 end

Listing P.2.1: systemSim.m

The physical system is implemented in a class contained in systemDynamic.m,

which will be discussed below, and which is instantiated on line 7. The system is
updated at sample rate Ts given the control input u and the input disturbance d on
line 29.
The controller is implemented in a class contained in
systemController.m, which will be discussed below, and which is instanti-
ated on line 8. Given the reference input r, and the system output y, the controller
is updated at the fast sample rate Ts on line 28. At the initial time, the controller

TOC
 APPENDIX P.2. SIMULATING CONTROL SYSTEMS IN MATLAB 425

needs the initial output at time t = 0, which is computed on line 19 before the
main loop begins.
The reference input is implemented using the signalGenerator class that
is contained in signalGenerator.m, which is described below, and which is
instantiated on line 9. The reference input at time t, i.e., r(t) is assigned a value
on line 25.
The external disturbance d(t) is also implemented using the
signalGenerator class and is instantiated on line 10 and assigned a value
on line 26. For this example, the external disturbance will be a constant value of
d(t) = 1.0. The noise n(t) is also implemented using the signalGenerator
class and is instantiated on line 11 and assigned a value on line 27.
In addition to the feedback loop as implemented above, the system will be
visualized through an animation, and also by plotting important variables like the
reference input, the system states, and the control input. The animation and data
plotting functions are implemented in systemAnimation.m and
dataPlotter.m, which are instantiated on lines 14–15, and updated on lines 33–
34.

Appendix P.2.1 System Dynamics

This section will describe how to implement the system dynamics given in Equa-
tion (P.2.1). The first step is to convert the dynamics to the state space form

ẋ = f (x, u)
y = h(x, u).

For the differential equation in Equation (P.2.1) let x = (y, ẏ)> , which gives
       
ẋ1 ẏ ẏ x2 4
ẋ = = = = = f (x, u)
ẋ2 ÿ −2ẏ − 3y + 4u −2x2 − 3x1 + 4u
(P.2.3)
4
y = x1 = h(x). (P.2.4)

The dynamics of the system are implemented in systemDynamics.m which

is listed below.
1 classdef systemDynamics < handle
2 properties
3 state
4 Ts
5 limit
6 a0
7 a1
8 b0
9 end
10 methods
11 %---constructor-------------------------

TOC
 APPENDIX P.2. SIMULATING CONTROL SYSTEMS IN MATLAB 426

12 function self = systemDynamics(sample_rate)

13 y0 = 0; % initial condition for y
14 ydot0 = 0; % initial condition for ydot
15 self.state = [... % initial state
16 y0;...
17 ydot0;...
18 ];
19 self.Ts = sample_rate;
20 self.limit = 1.0; % saturation limit
21 % system parameters
22 self.a0 = 3.0;
23 self.a1 = 2.0;
24 self.b0 = 4.0;
25 % modify the system parameters by random value
26 alpha = 0.2; % Uncertainty parameter
27 self.a0 = self.a0 * (1+2*alpha*rand-alpha);
28 self.a1 = self.a1 * (1+2*alpha*rand-alpha);
29 self.b0 = self.b0 * (1+2*alpha*rand-alpha);
30 end
31
32 function xdot = f(self, state, u)
33 % Return xdot = f(x,u)
34 y = state(1);
35 ydot = state(2);
36 % The equations of motion.
37 yddot = -self.a1 * ydot - self.a0 * y + self.b0 * u;
38 % build xdot and return
39 xdot = [ydot; yddot];
40 end
41
42 function y = h(self)
43 % y = h(x)
44 y = self.state(1);
45 end
46
47 function y = update(self, u)
48 % This is the external method that takes input u
49 % and returns output y.
50 u = self.saturate(u, self.limit); % saturate input
51 self.rk4_step(u); % propagate state by one time step
52 y = self.h(); % compute the output at current time
53 end
54
55 function self = rk4_step(self, u)
56 % Integrate ODE using Runge-Kutta-4 algorithm
57 F1 = self.f(self.state, u);
58 F2 = self.f(self.state + self.Ts/2*F1, u);
59 F3 = self.f(self.state + self.Ts/2*F2, u);
60 F4 = self.f(self.state + self.Ts*F3, u);
61 self.state = self.state...
62 + self.Ts/6 * (F1 + 2*F2 + 2*F3 + F4);
63 end
64
65 function out = saturate(self, in, limit)
66 if abs(in) > limit
67 out = limit * sign(in);
68 else

TOC
 APPENDIX P.2. SIMULATING CONTROL SYSTEMS IN MATLAB 427

69 out = in;
70 end
71 end
72
73 end
74 end

Listing P.2.2: systemDynamics.m

In Matlab, the class definition classdef systemDynamics < handle

must contain the < handle to ensure that internal methods can modify the class
properties. The internal properties are specified in lines 2–9. The class methods
are specified in lines 10–66. The class constructor is given in lines 12–30 and is
called when a class object is instantiated. The state is initialized on lines 13–18
based on the initial conditions for y(0) and ẏ(0). The system parameters are ini-
tialized in lines 21–29. The parameters for any physical system are never known
exactly. Therefore, feedback systems need to be designed to be robust to this
uncertainty. In the simulation, we model uncertainty in our knowledge of the
physical parameters by changing the nominal value by a uniform random vari-
able. Let α be the amount of change allowed for each parameter, i.e., alpha =
0.2, means that the physical parameters may change by up to 20%. The func-
tion rand() returns a uniform random variable in the interval [0, 1]. Therefore
(1 + α(2 ∗ np.random.rand() − 1)) is a random variable in the interval
[−α, α]. The nominal value for the physical parameters that are known to the de-
signer, are specified in lines 22–24. The actual values used by the simulation, are
randomly selected in lines 27–29.
The system dynamics

ẋ = f (x, u)
y = h(x, u).

are specified in lines 32–45. Lines 32–40 define f (x, u), and lines 42–45 define
h(x, u), as given by Equations (P.1.3) and (P.1.4) respectively.
The main feedback loop calls the update method on lines 47–53. The
update function first saturates the input on line 50, where the saturation
function is defined in lines 64–70. The system dynamics are then propagated for-
ward by one sample time Ts using the Runge-Kutta 4 algorithm on line 51, where
the RK4 algorithm is specified on lines 55–62, as described in Appendix P.4. The
update function finally computes and returns the output y on line 52.

Appendix P.2.2 Controller

This section will describe how to implement the PID controller given in Equa-
tion (P.2.2). The controller is implemented in systemController.m, which
is listed below.
1 classdef systemController

TOC
 APPENDIX P.2. SIMULATING CONTROL SYSTEMS IN MATLAB 428

2 properties
3 kp
4 kd
5 ki
6 Ts
7 sigma
8 beta
9 limit
10 ydot
11 y_d1
12 integrator
13 error_d1
14 end
15 methods
16 %------constructor----------------------
17 function self = systemController(sample_rate)
18 % specify gains
19 self.kp = 1.0;
20 self.kd = 1.0;
21 self.ki = 1.0;
22 self.Ts = sample_rate;
23 self.sigma = 2.0 * self.Ts;
24 self.beta = (2.0*sigma-self.Ts)/(2.0*sigma+self.Ts);
25 self.limit = 1.0;
26 % internal storage for differentiator
27 self.ydot = 0.0;
28 self.y_d1 = 0.0;
29 % internal storage for integrator
30 self.integrator = 0;
31 self.error_d1 = 0;
32 end
33
34 function u = update(self, r, y)
35 e = r - y; % define the error
36 I = self.integrate_e(e); % integrate error
37 ydot = self.differentiate_y(y); %differentiate y
38 % form PID controller, saturate, and return
39 u = self.kp * e + self.ki * I - self.kd * ydot;
40 u = self.saturate(u);
41 end
42
43 function I = integrate_e(self, error)
44 self.integrator = self.integrator...
45 + (self.Ts / 2) * (error + self.error_d1);
46 self.error_d1 = error;
47 I = self.integrator;
48 end
49
50 function ydot = differentiate_y(self, y)
51 self.ydot = self.beta*self.ydot...
52 + (1-self.beta)/self.Ts * (y - self.y_d1);
53 self.y_d1 = y;
54 ydot = self.ydot;
55 end
56
57 function out = saturate(self, in):
58 if abs(in) > self.limit

TOC
 APPENDIX P.2. SIMULATING CONTROL SYSTEMS IN MATLAB 429

59 out = self.limit * sign(u);

60 else
61 out = in;
62 end
63 end
64
65 end
66 end

Listing P.2.3: systemController.m

The controller properties are specified on lines 2–14, and instantiated in the
constructor on lines 17–32. The PID gains are specified on lines 19–21. The sam-
ple rate of the controller is passed into the system at the time of instantiation and is
assigned an internal variable Ts in line 9. The dirty derivative gain σ described in
Chapter 10 is specified on line 22, and the intermediate gain β = 2σ−T s
2σ+Ts is speci-
fied on line 24. Internal storage variables for the differentiator and the integrator,
as explained in Chapter 10 are given in lines 27–31. The PID control update
routine is specified in lines 34–41. The error e = r − y as shown in Figure 1-1
is defined on line 35. The PID controller in Equations (P.2.2) uses the integral
of e and the derivative of y. The integral of e is computed on line 36 using the
routine specified on lines 43–48. A derivation of the integral equations is given in
Chapter 10. The derivative of y is computed in line 37 using the routine specified
in lines 50-53. A derivation of the dirty derivative equations is given in Chap-
ter 10. Finally, the PID controller computed, the value is saturated, and returned
on lines 39, and 40, respectively.

Appendix P.2.3 Signal Generator

Matlab code for the signal generator class is listed below.
1 classdef signalGenerator
2 % produces: square_wave, step, sawtooth_wave,
3 % random_wave, sin_wave.
4 properties
5 amplitude
6 frequency
7 y_offset
8 end
9 methods
10 %---constructor--------------------------------
11 function self = signalGenerator(...
12 amplitude, frequency, y_offset)
13 self.amplitude = amplitude;
14 if nargin>1
15 self.frequency = frequency;
16 else
17 self.frequency = 1;
18 end
19 if nargin>2
20 self.y_offset = y_offset;

TOC
 APPENDIX P.2. SIMULATING CONTROL SYSTEMS IN MATLAB 430

21 else
22 self.y_offset = 0;
23 end
24 end
25
26 function out = square(self, t)
27 if mod(t, 1/self.frequency)<=0.5/self.frequency
28 out = self.amplitude + self.y_offset;
29 else
30 out = -self.amplitude + self.y_offset;
31 end
32 end
33
34 function out = step(self, t)
35 if t>=0
36 out = self.amplitude + self.y_offset;
37 else
38 out = 0;
39 end
40 end
41
42 function out = sawtooth(self, t)
43 out = 4*self.amplitude*self.frequency...
44 * mod(t, 0.5/self.frequency)...
45 - self.amplitude + self.y_offset;
46 end
47
48 function out = random(self, t)
49 out = sqrt(self.amplitude)*randn + self.y_offset;
50 end
51
52 function out = sin(self, t)
53 out = self.amplitude*sin(2*pi*self.frequency*t)...
54 + self.y_offset;
55 end
56 end
57 end

Listing P.2.4: signalGenerator.m

The class is instantiated by passing in the desired amplitude of the signal, the
frequency in Hertz, and a y-offset value. For random signals, the amplitude is
interpreted to be be the standard deviation of the signal. The signal generator can
then be used to produce a square wave, a sawtooth signal, a step, a random signal,
or a sinusoid, as specified in lines 26–55.

Appendix P.2.4 Data Plotter

The objective of the data plotter is to plot the relevant signals during the simula-
tion. For this simple example, we will plot the output y and the reference r on
one plot, the velocity ẏ on another plot, and the control signal u on a third plot.
Matlab code for the data plotter class is listed below.

TOC
 APPENDIX P.2. SIMULATING CONTROL SYSTEMS IN MATLAB 431

1 classdef dataPlotter < handle

2 properties
3 % data histories
4 time_history
5 r_history
6 y_history
7 ydot_history
8 u_history
9 index
10 % figure handles
11 r_handle
12 y_handle
13 ydot_handle
14 u_handle
15 end
16 methods
17 %--constructor--------------------------
18 function self = dataPlotter(P)
19 % Instantiate lists to hold the time and data histories
20 self.time_history...
21 = NaN*ones(1,(P.t_end-P.t_start)/P.t_plot);
22 self.r_history...
23 = NaN*ones(1,(P.t_end-P.t_start)/P.t_plot);
24 self.y_history...
25 = NaN*ones(1,(P.t_end-P.t_start)/P.t_plot);
26 self.ydot_history...
27 = NaN*ones(1,(P.t_end-P.t_start)/P.t_plot);
28 self.u_history...
29 = NaN*ones(1,(P.t_end-P.t_start)/P.t_plot);
30 self.index = 1;
31 % Create figure and axes handles
32 figure(2), clf
33 subplot(311)
34 hold on
35 self.r_handle...
36 = plot(self.time_history,...
37 self.r_history, ’g’);
38 self.y_handle...
39 = plot(self.time_history,...
40 self.y_history, ’b’);
41 ylabel(’y’)
42 title(’System Data’)
43 subplot(312)
44 hold on
45 self.ydot_handle...
46 = plot(self.time_history,...
47 self.ydot_history, ’b’);
48 ylabel(’ydot’)
49 subplot(313)
50 hold on
51 self.u_handle...
52 = plot(self.time_history,...
53 self.u_history, ’b’);
54 ylabel(’u’)
55 end
56
57 function self=update(self, time, reference, states, control)

TOC
 APPENDIX P.2. SIMULATING CONTROL SYSTEMS IN MATLAB 432

58 % update the time history of all plot variables

59 self.time_history(self.index) = time;
60 self.r_history(self.index) = reference;
61 self.y_history(self.index) = states(1);
62 self.ydot_history(self.index) = states(2);
63 self.u_history(self.index) = control;
64 self.index = self.index + 1;
65 % update the plots with associated histories
66 set(self.r_handle,...
67 ’Xdata’, self.time_history,...
68 ’Ydata’, self.r_history)
69 set(self.y_handle,...
70 ’Xdata’, self.time_history,...
71 ’Ydata’, self.y_history)
72 set(self.ydot_handle,...
73 ’Xdata’, self.time_history,...
74 ’Ydata’, self.ydot_history)
75 set(self.u_handle,...
76 ’Xdata’, self.time_history,...
77 ’Ydata’, self.u_history)
78 end
79 end
80 end

Listing P.2.5: dataPlotter.m

The plotter constructor is listed on lines 18–55. Lines 20–29 initialize arrays
that will hold the data histories. Allocating memory for the arrays upon initializa-
tion speeds up the run-time execution of the data plotter. Lines 32–54 set up the
plot window with two subplots and plot the initial data. The Matlab plot routine
returns a Matlab handle to the line, which is a data structure the contains all data
associated with each line. The first subplot (311) will plot r(t) in green (’g’),
and y(t) in blue (’b’). The second subplot (312) will plot ẏ in blue, and the third
subplot (313) will plot u(t) in blue.
The update method receives as input the current time,
reference, state, and control values. Lines 59–63 appends the current
values to their histories. The plots are then updated with the new histories in
lines 66–77 by calling the set routine and passing in the associated handle.

Appendix P.2.5 Animation

In this section we will show how to create an animation for the inverted pendulum
case study. Consider the image of the inverted pendulum shown in Fig. 2-2, where
the configuration is completely specified by the position of the cart z, and the angle
of the rod from vertical θ. The physical parameters of the system are the rod length
`, the base width w, the base height h, and the gap between the base and the track
g.
The first step in developing the animation is to determine the position of points
that define the animation. The pendulum will require that we draw three objects:
the rectangular cart, the circular bob at the end of the rod, and the rod. We will

TOC
 APPENDIX P.2. SIMULATING CONTROL SYSTEMS IN MATLAB 433

Figure 2-2: Drawing for inverted pendulum. The first step in developing an an-
imation is to draw a figure of the object to be animated and identify all of the
physical parameters.

draw the cart as a rectangle, the bob as a circle, and the rod as a line. In Matlab,
rectangles are specified by their corners. The four corners of the rectangle are
given by  
z − w/2, g
 zw /2, g 
C= z + w/2, g + h .


z − w/2, g + h
In Matlab, lines are specified by the x and y locations of their endpoints. The x
locations of the endpoints of the rod are X = [z, z + ` sin θ], and the y locations
of the endpoints are Y = [g + h, g + h + ` cos θ]. In Matlab, circles are drawn as a
sequence of small line segments, that must be specified by the x and y locations.
The center of the bob is given by C = (z + (` + R) sin θ, g + h + (` + R) cos θ),
and so points on the edge of the bob will be given by X = [Cx + R cos θ] and
Y = [Cy + R sin θ]. Matlab code for the animation class is listed below.
1 classdef pendulumAnimation
2 properties
3 cart_handle
4 rod_handle
5 bob_handle
6 ell
7 width
8 height
9 gap
10 radius
11 end
12 methods
13 %------constructor-----------
14 function self = pendulumAnimation(P)
15 self.ell = P.ell;

TOC
 APPENDIX P.2. SIMULATING CONTROL SYSTEMS IN MATLAB 434

16 self.width = P.w;
17 self.height = P.h;
18 self.gap = P.gap;
19 self.radius = P.radius;
20
21 figure(1), clf
22 % draw ground track
23 plot([-2*self.ell, 2*self.ell],[0,0],’k’);
24 hold on
25 % initialize the cart, rod, and bob
26 self=self.draw_cart(P.z0);
27 self=self.draw_rod(P.z0, P.theta0);
28 self=self.draw_bob(P.z0, P.theta0);
29 % Set the x,y axis limits
30 axis([-3*self.ell, 3*self.ell, -0.1, 3*self.ell]);
31 xlabel(’z’); % label x-axis
32 end
33
34 function self=update(self, state)
35 % external method to update the animation
36 z= state(1); % Horizontal position of cart, m
37 theta = state(2); % Angle of pendulum, rads
38 self=self.draw_cart(z);
39 self=self.draw_rod(z, theta);
40 self=self.draw_bob(z, theta);
41 drawnow
42 end
43
44 function self=draw_cart(self, z)
45 % specify X-Y locations of corners of the cart
46 X = [z-self.width/2, z+self.width/2,...
47 z+self.width/2, z-self.width/2];
48 Y = [self.gap, self.gap,...
49 self.gap+self.height, self.gap+self.height];
50 % plot or update cart
51 if isempty(self.cart_handle)
52 self.cart_handle = fill(X,Y,’b’);
53 else
54 set(self.cart_handle,’XData’,X);
55 end
56 end
57
58 function self=draw_rod(self, z, theta)
59 % specify X-Y locations of ends or rod
60 X = [z, z+self.ell*sin(theta)];
61 Y = [...
62 self.gap+self.height,...
63 self.gap + self.height + self.ell*cos(theta)...
64 ];
65 % plot or update rod
66 if isempty(self.rod_handle)
67 self.rod_handle = plot(X, Y, ’k’);
68 else
69 set(self.rod_handle,’XData’, X, ’YData’, Y);
70 end
71 end
72

TOC
 APPENDIX P.2. SIMULATING CONTROL SYSTEMS IN MATLAB 435

73 function self=draw_bob(self, z, theta)

74 th = 0:2*pi/10:2*pi;
75 % center of bob
76 center = [z + (self.ell+self.radius)*sin(theta) ...
77 self.gap+self.height + ...
78 (self.ell+self.radius)*cos(theta)];
79 % points that define the bob
80 X = center(1) + self.radius*cos(th);
81 Y = center(2) + self.radius*sin(th);
82 % plot or update bob
83 if isempty(self.bob_handle)
84 self.bob_handle = fill(X,Y,’g’);
85 else
86 set(self.bob_handle,’XData’,X);
87 set(self.bob_handle,’YData’,Y);
88 end
89 end
90 end
91 end

Listing P.2.6: pendulumAnimation.m

The pendulumAnimation class is initialized on lines 14–32. Lines 15–
19 sets all of the plot parameters based on data passed through the structure P.
Line 21 initializes the plot window and line 23 draws the ground track, which will
not change during the animation. Lines 26–26 draws the base, bob, and line for
the first time, where a Matlab handle is returned to each object in the animation.
Line 30 sets the plot axes, and line 31 labels the plot.
The animation function is updated each time step using the update method
on lines 34–42. The input to the update method is the state of the pendulum,
of which the first element is z and the second element is θ (lines 36–37). Lines 38–
40 then call methods, that will be described below, that draw the cart, rod, and bob.
Line 41 forces Matlab to update the animation at each time step.
The draw_cart method is listed on lines 44–56. The cart is drawn using the
fill routine, where the x and y locations of each corner must be specified, as
shown on lines 46–49. The first time draw_cart is called, the cart_handle
will not exist, and so it is created through a call to fill on line 52. Upon sub-
sequent calls to draw_cart, the handle will exist and so the cart location is
updated through the set command on line 54. Since the y data of the cart doesn’t
change with time, only the x data is updated.
The draw_rod method is listed on lines 58–71. The rod is drawn using the
plot routine, where the x and y locations of the ends of the rod must be specified,
as shown on lines 60–64. The first time draw_rod is called, the rod_handle
will not exist, and so it is created through a call to plot on line 67. Upon subse-
quent calls to draw_rod, the handle will exist and so the rod points are updated
through the set command on line 69. Note that both the x and the y data of the
rod must be updated at each time step.
The draw_bob method is listed on lines 73–90. The bob is drawn using
the fill routine, where the x and y locations of each point on the bob must be
specified. The points are parametrized by ϑ which ranges from zero to 2π. Line 74

TOC
APPENDIX P.2. SIMULATING CONTROL SYSTEMS IN MATLAB 436

establishes an array of N = 10 angle points. The center of the bob is specified

on lines 76–79, and the x and y points along the outline of the bob are given on
lines 81–82. The first time draw_bob is called, the bob_handle will not exist,
and so it is created through a call to fill on line 85. Upon subsequent calls to
draw_bob, the handle will exist and so the cart location is updated through the
set command on lines 87–88. Note that both the x and the y data of the bob must
be updated at each time step.

TOC
 Simulating Control Systems Using Simulink
P.3
In this appendix we will show how to simulate the feedback control of a dynamic
system using Simulink. Consider the general feedback control system shown in
Figure 3-1. The output of the physical system is y(t), the inputs to the physical
system are u(t) and an external disturbance signal d(t). The output of the sys-
tem y(t) is corrupted by zero-mean noise n(t). The input to the controller is the
reference signal r(t) as well as the output corrupted by noise.

Figure 3-1: Block diagram of a general feedback system.

We will illustrate how to implement feedback control systems in Simulink

by simulating the physical system described by the differential equation

ÿ + 2ẏ + 3y = 4u, (P.3.1)

and using the PID controller

Z t
u(t) = kp (r(t) − y(t)) + ki (r(τ ) − y(τ ))dτ − kd ẏ. (P.3.2)
−∞

TOC
APPENDIX P.3. SIMULATING CONTROL SYSTEMS IN SIMULINK 438

The Simulink diagram that corresponds to Figure 3-1 is shown in Figure 3-2.
The system dynamics are implemented in the system block, the controller is

Figure 3-2: Simulink diagram implementing the feedback control loop in Fig-
ure 3-1.

implemented in the controller block, and the animation is implemented in the

animation block. The reference r(t), the disturbance d(t), and the noise n(t)
are all implemented using the Simulink built in Signal Generator block.
The built in Simulink Scope blocks are used to display the output y and the
reference r, as well as the input u.
System parameters can be loaded into Simulink at the beginning of a simu-
lation by clicking on the Model Properties menu, as shown in Figure 3-3,
and then selecting Callbacks/InitFcn, as shown in Figure 3-4. The com-
mands listed under Model initialization function: are executed at
the beginning of each Simulink simulation.

Appendix P.3.1 System Dynamics

In this section we will describe how to implement the system dynamics. Double
clicking on the system block shown in Figure 3-2 results in the Simulink sub-
diagram shown in Figure 3-5. Double clicking on the System Dynamics block
results in Figure 3-6, which shows an s-function dialog block. In Simulink,
system dynamics are implemented using s-functions. Simulink is essentially
a sophisticated tool for solving interconnected hybrid ordinary differential and

TOC
 APPENDIX P.3. SIMULATING CONTROL SYSTEMS IN SIMULINK 439

Figure 3-3: Simulink Model Properties menu.

difference equations. Each block in Simulink is assumed to have the structure

ẋc = f (t, xc , xd , u); xc (0) = xc0 (P.3.3)

xd [k + 1] = g(t, xc , xd , u); xd [0] = xd0 (P.3.4)
y = h(t, xc , xd , u), (P.3.5)

where xc ∈ Rnc is a continuous state with initial condition xc0 , xd ∈ Rnd is a

discrete state with initial condition xd0 , u ∈ Rm is the input to the block, y ∈ Rp
is the output of the block, and t is the elapsed simulation time. An s-function is
a Simulink tool for explicitly defining the functions f , g, and h and the initial
conditions xc0 and xd0 . As explained in the Matlab/Simulink documentation,
there are a number of methods for specifying an s-function. In this section, we
will overview the method of using a level-1 m-file s-function.
The system given in Equation (P.3.1) is implemented using the s-function
shown below.
1 function [sys,x0,str,ts,simStateCompliance]...
2 =system_dynamics(t,x,u,flag)
3 switch flag
4 case 0 % Initialize block
5 [sys,x0,str,ts,simStateCompliance]=mdlInitializeSizes;
6 case 1 % define xdot = f(t,x,u)
7 sys=mdlDerivatives(t,x,u);
8 case 3 % define y = h(t,x,u)
9 sys=mdlOutputs(t,x,u);
10 % GetTimeOfNextVarHit
11 otherwise
12 DAStudio.error(...
13 ’Simulink:blocks:unhandledFlag’,num2str(flag));
14 end

TOC
 APPENDIX P.3. SIMULATING CONTROL SYSTEMS IN SIMULINK 440

Figure 3-4: Simulink Callback/InitFcn script.

15 %==============================================
16 function [sys,x0,str,ts,simStateCompliance]=mdlInitializeSizes
17 sizes = simsizes;
18 sizes.NumContStates = 2;
19 sizes.NumDiscStates = 0;
20 sizes.NumOutputs = 1+2;
21 sizes.NumInputs = 1;
22 sizes.DirFeedthrough = 0;
23 sizes.NumSampleTimes = 1;
24 sys = simsizes(sizes);
25
26 % define initial conditions
27 y0 = 0; % initial condition for y
28 ydot0 = 0; % initial condition for ydot
29 x0 = [y0; ydot0];
30 str = []; % str is always an empty matrix
31 % initialize the array of sample times
32 ts = [0 0];
33 simStateCompliance = ’UnknownSimState’;
34
35 %==============================================
36 function sys=mdlDerivatives(t,x,u)
37 % Return xdot = f(x,u)
38 y = x(1);
39 ydot = x(2);
40
41 % system parameters
42 persistent a0
43 persistent a1
44 persistent b0
45 if t==0
46 a0 = 3.0;
47 a1 = 2.0;

TOC
 APPENDIX P.3. SIMULATING CONTROL SYSTEMS IN SIMULINK 441

Figure 3-5: Simulink system dynamics sub-diagram.

48 b0 = 4.0;
49 % modify the system parameters by random value
50 alpha = 0.2; % Uncertainty parameter
51 a0 = a0 * (1+2*alpha*rand-alpha);
52 a1 = a1 * (1+2*alpha*rand-alpha);
53 b0 = b0 * (1+2*alpha*rand-alpha);
54 end
55 % The equations of motion.
56 yddot = -a1 * ydot - a0 * y + b0 * u;
57 % build xdot and return
58 sys = [ydot; yddot];
59
60 %==============================================
61 function sys=mdlOutputs(t, x, u)
62 y = x(1);
63 sys = [y; x]; % return output and state

Listing P.3.1: system_dynamics.m

Lines 1-2 defines the main s-function file. The inputs to this function are
always the elapsed time t; the state x, which is a concatenation of the continu-
ous state and discrete state; the input u; and a flag, followed by user defined
input parameters, which are absent in this case. The Simulink engine calls the
s-function and passes the parameters t, x, u, and flag. When flag==0, the
Simulink engine expects the s-function to return the structure sys, which defines
the block; initial conditions x0; an empty string str; and an array ts that defines
the sample times of the block. When flag==1, the Simulink engine expects the
s-function to return the function f (t, x, u); and when flag==3 the Simulink en-
gine expects the s-function to return h(t, x, u). The switch statement that calls the
proper functions based on the value of flag is shown in Lines 3–13. The block

TOC
 APPENDIX P.3. SIMULATING CONTROL SYSTEMS IN SIMULINK 442

Figure 3-6: Simulinks-function dialog.

setup and the definition of the initial conditions are shown in Lines 16–33. The
number of continuous states, discrete states, outputs, and inputs are defined on
Lines 18-21, respectively. The direct feedthrough term on Line 22 is set to one if
the output depends explicitly on the input u. For example, if D 6= 0 in the linear
state-space output equation y = Cx + Du. The initial conditions are defined on
Lines 26–29. The sample times are defined on Line 32. The format for this line is
ts = [period offset], where period defines the sample period and is 0
for continuous time or −1 for inherited, and where offset is the sample time
offset, which is typically 0. The function f (t, x, u) is defined in Lines 36–58, and
the output function h(t, x, u) is defined in Lines 61–63.

Appendix P.3.2 Controller

In this section we will describe how to implement the system dynamics. Double
clicking on the controller block shown in Figure 3-2 results in the Simulink
sub-diagram shown in Figure 3-7, which also shows the pop-up window that re-
sults from clicking on the Interpreted Matlab Function block. This
block calls the Matlab function system_controller.m with inputs u, com-
ing into the Simulink block, and the parameter structure P, which must be defined
in the Matlab workspace. The m-file that defines the controller is listed below.
1 function u=system_controller(in, Ts)
2 r = in(1);
3 y = in(2);
4 t = in(3);
5 % controller gains
6 kp = 1;

TOC
 APPENDIX P.3. SIMULATING CONTROL SYSTEMS IN SIMULINK 443

Figure 3-7: Simulink controller sub-diagram, with pop-up window.

7 kd = 1;
8 ki = 1;
9 sigma = 2*Ts;
10 beta = (2*sigma-Ts)/(2*sigma+Ts);
11 limit = 1;
12
13 % define the error
14 error = r - y;
15
16 % set persistent variables at start of simulation
17 persistent integrator
18 persistent ydot
19 persistent error_d1
20 persistent y_d1
21 if t<Ts
22 integrator = 0;
23 ydot = 0;
24 error_d1 = error;
25 y_d1 = y;
26 end
27
28 % update the integrator
29 integrator = integrator + (Ts/2)*(error+error_d1);
30 % update the derivative of y
31 ydot = beta*ydot + (1-beta)*(y-y_d1)/Ts;
32 % form PID controller, saturate, and return
33 u_unsat = kp*error + ki*integrator - kd*ydot;
34 u = saturate(u_unsat, limit);
35 % update persistent delay variables
36 error_d1 = error;
37 y_d1 = y;
38 end
39
40 %-----------------------------------------------------------------

TOC
 APPENDIX P.3. SIMULATING CONTROL SYSTEMS IN SIMULINK 444

41 % saturation function
42 function out = sat(in,limit)
43 if in > limit, out = limit;
44 elseif in < -limit, out = -limit;
45 else out = in;
46 end
47 end

Listing P.3.2: system_controller.m

The input to system_controller.m are in and the sample rate Ts. The
input array is decomposed into the reference r, the system output y, and the time t
on lines 2–4. The controller gains are set on lines 5–11, and the error is calculated
on line 14. The persistent variables that must persist between function calls, are
defined and initialized on lines 16–26. The integrator is updated on line 29, the
differentiator is updated on line 31, and the PID controller is specified on line 33-
34. The delay variables are upated on lines 36–37.

Appendix P.3.3 Animation

In this section we will show how to create an animation for the inverted pendulum
case study in Simulink.
When a graphics function like plot is called in Matlab, the function returns
a handle to the plot. A graphics handle is similar to a pointer in C/C++ in the
sense that all of the properties of the plot can be accessed through the handle. For
example, the Matlab command
1 >> plot_handle=plot(t,sin(t))

returns a pointer, or handle, to the plot of sin(t). Properties of the plot can
be changed by using the handle, rather than reissuing the plot command. For
example, the Matlab command
1 >> set(plot_handle, ’YData’, cos(t))

changes the plot to cos(t) without redrawing the axes, title, label, or other ob-
jects that may be associated with the plot. If the plot contains drawings of several
objects, a handle can be associated with each object. For example,
1 >> plot_handle1 = plot(t,sin(t))
2 >> hold on
3 >> plot_handle2 = plot(t,cos(t))

draws both sin(t) and cos(t) on the same plot, with a handle associated with
each object. The objects can be manipulated separately without redrawing the
other object. For example, to change cos(t) to cos(2t), issue the command
1 >> set(plot_handle2, ’YData’, cos(2*t))

TOC
 APPENDIX P.3. SIMULATING CONTROL SYSTEMS IN SIMULINK 445

We can exploit this property to animate simulations in Simulink by redrawing

only the parts of the animation that change in time, and thereby significantly re-
ducing the simulation time. To show how handle graphics can be used to produce
animations in Simulink, we will show how to draw the inverted pendulum in a
2-D animation.
Consider the image of the inverted pendulum shown in Fig. 3-8, where the
configuration is completely specified by the position of the cart z, and the angle of
the rod from vertical θ. The physical parameters of the system are the rod length
`, the base width w, the base height h, and the gap between the base and the track
g.

Figure 3-8: Drawing for inverted pendulum. The first step in developing an an-
imation is to draw a figure of the object to be animated and identify all of the
physical parameters.

The first step in developing the animation is to determine the position of points
that define the animation. For example, for the inverted pendulum in Fig. 3-8, the
four corners of the base are

(z + w/2, g), (z + w/2, g + h), (z − w/2, g + h), and (z − w/2, g),

and the two ends of the rod are given by

(z, g + h) and (z + L sin θ, g + h + L cos θ).

Since the base and the rod can move independently, each will need its own
figure handle. The drawBase command can be implemented with the following
Matlab code:
1 function handle...
2 = drawBase(z, width, height, gap, handle)
3 X = [z-width/2,z+width/2,z+width/2,z-width/2];
4 Y = [gap, gap, gap+height, gap+height];
5 if isempty(handle),

TOC
 APPENDIX P.3. SIMULATING CONTROL SYSTEMS IN SIMULINK 446

6 handle = fill(X,Y,’m’);
7 else
8 set(handle,’XData’,X,’YData’,Y);
9 end

Lines 2 and 3 define the X and Y locations of the corners of the base. Note that in
Line 1, handle is both an input and an output. If an empty array is passed into
the function, then the fill command is used to plot the base in Line 5. On the
other hand, if a valid handle is passed into the function, then the base is redrawn
using the set command in Line 7.
The Matlab code for drawing the rod is similar and is listed below.
1 function handle...
2 = drawRod(z,theta,L,gap,height,handle)
3 X = [z, z+L*sin(theta)];
4 Y = [gap+height, gap + height + L*cos(theta)];
5 if isempty(handle),
6 handle = plot(X, Y, ’g’);
7 else
8 set(handle,’XData’,X,’YData’,Y);
9 end

The main routine for the pendulum animation is listed below.

1 function drawPendulum(u)
2 % process inputs to function
3 z = u(1);
4 theta = u(2);
5 t = u(3);
6
7 % drawing parameters
8 L = 1;
9 gap = 0.01;
10 width = 1.0;
11 height = 0.1;
12
13 % define persistent variables
14 persistent base_handle
15 persistent rod_handle
16
17 % first time function is called, initialize plot
18 % and persistent vars
19 if t==0,
20 figure(1), clf
21 track_width=3;
22 % plot track
23 plot([-track_width,track_width],[0,0],’k’);
24 hold on
25 base_handle...
26 =drawBase(z, width, height, gap, []);
27 rod_handle...
28 =drawRod(z, theta, L, gap, height, []);
29 axis([-track_width,track_width,...
30 -L,2*track_width-L]);
31 % at every other time step, redraw base and rod
32 else

TOC
 APPENDIX P.3. SIMULATING CONTROL SYSTEMS IN SIMULINK 447

33 drawBase(z, width, height, gap, base_handle);

34 drawRod(z, theta, L, gap, height, rod_handle);
35 end

The routine drawPendulum is called from the Simulink file shown above, where
there are three inputs: the position z, the angle θ, and the time t. Lines 3-5 rename
the inputs to z, θ, and t. Lines 8-11 define the drawing parameters. We require that
the handle graphics persist between function calls to drawPendulum. Since a
handle is needed for both the base and the rod, we define two persistent variables
in Lines 14 and 15. The if statement in Lines 19-32 is used to produce the
animation. Lines 20–27 are called once at the beginning of the simulation, and
draw the initial animation. Line 20 brings the figure 1 window to the front and
clears it. Lines 21 and 23 draw the ground along which the pendulum will move.
Line 25 calls the drawBase routine with an empty handle as input, and returns
the handle base_handle to the base. Line 26 calls the drawRod routine, and
Line 25 sets the axes of the figure. After the initial time step, all that needs to be
changed are the locations of the base and rod. Therefore, in Lines 30 and 31, the
drawBase and drawRod routines are called with the figure handles as inputs.

TOC
APPENDIX P.3. SIMULATING CONTROL SYSTEMS IN SIMULINK 448

TOC
 Numerical Solutions to Differential Equations
P.4
This appendix provides a brief overview of techniques for numerically solving
ordinary differential equations, when the initial value is specified. In particular,
we are interested in solving the differential equation
dx
ẋ = (t) = f (x(t), u(t)), x(t0 ), (P.4.1)
dt
over the time interval [t0 , tf ], where x(t) ∈ Rn , and u(t) ∈ Rm is assumed to
be known, and where we assume that f (·, ·) is sufficiently smooth and that unique
solutions to the differential equation exist for every x0 .
Integrating both sides of Equation (P.4.1) from t0 to t < tf gives
Z t
x(t) = x(t0 ) + f (x(τ ), u(τ )) dτ. (P.4.2)
t0

The difficulty with solving Equation (P.4.2) is that x(t) appears on both sides
of the equation, and cannot therefore be solved explicitly for x(t). All numeri-
cal solution techniques attempt to approximate the solution to Equation (P.4.2).
Most techniques break the solution into small time increments, usually equal to
the sample rate of the computer, which we denote by Ts . Accordingly we write
Equation (P.4.2) as
Z t−Ts Z t
x(t) = x(t0 ) + f (x(τ ), u(τ )) dτ + f (x(τ ), u(τ )) dτ
t0 t−Ts
Z t
= x(t − Ts ) + f (x(τ ), x(τ )) dτ. (P.4.3)
t−Ts

The most simple form of numerical approximation for the integral in Equa-
tion (P.4.3) is to use the left endpoint method where
Z t
f (x(τ ), u(τ )) dτ ≈ Ts f (x(t − Ts ), u(t − Ts )).
t−Ts

TOC
APPENDIX P.4. NUMERICAL ODE SOLVERS 450

4
Using the notation xk = x(t0 + kTs ) we get the numerical approximation to the
differential equation in Equation (P.4.1) as
xk = xk−1 + Ts f (xk , uk ), x0 = x(t0 ). (P.4.4)
Equation (P.4.4) is called the Euler method, or a Runge-Kutta first order method
(RK1).
While the RK1 method is often effective for numerically solving ODEs, it
typically requires a small sample size Ts . The advantage of the RK1 method is
that it only requires one evaluation of f (·, ·).
To improve the numerical accuracy of the solution, a second order method can
be derived by approximating the integral in Equation (P.4.3) using the trapezoidal
rule shown in Figure 10-1, where
Z b  
f (a) + f (b)
f (ξ)dξ ≈ (b − a) .
a 2
Accordingly,
Z t
Ts
f (x(τ ), u(τ ))dτ ≈ (f (x(t − Ts ), u(t − Ts )) + f (x(t), u(t))) .
t−Ts 2
Since x(t) is unknown, we use the RK1 method to approximate it as
x(t) ≈ x(t − Ts ) + Ts f (x(t − Ts ), u(t − Ts )).
In addition, we typically assume that u(t) is constant over the interval [t − Ts , Ts )
to get
Z t
Ts
f (x(τ ), u(τ ))dτ ≈ f (x(t − Ts ), u(t − Ts ))
t−Ts 2
Ts

+ f (x(t − Ts ) + f (x(t − Ts ), u(t − Ts )), u(t − Ts )) .
2
Accordingly, the numerical integration routine can be written as
x0 = x(t0 )
F1 = f (xk−1 , uk−1 ) (P.4.5)
F2 = f (xk−1 + Ts F1 , uk−1 )
Ts
xk = xk−1 + (F1 + F2 ) .
2
Equations (P.4.5) is the Runge-Kutta second order method or RK2. It requires two
function calls to f (·, ·) for every time sample.
The most common numerical method for solving ODEs is the Runge-Kutta
forth order method RK4. The RK4 method is derived using Simpson’s Rule
Z b     
b−a a+b
f (ξ)dξ ≈ f (a) + 4f + f (b) ,
a 6 2

TOC
APPENDIX P.4. NUMERICAL ODE SOLVERS 451

which is found by approximating the area under the integral using a parabola. The
standard strategy is to write the approximation as
Z b       
b−a a+b a+b
f (ξ)ξ ≈ f (a) + 2f + 2f + f (b) ,
a 6 2 2
and then to use

F1 = f (a) (P.4.6)
 
Ts a+b
F2 = f (a + F1 ) ≈ f (P.4.7)
2 2
 
Ts a+b
F3 = f (a + F2 ) ≈ f (P.4.8)
2 2
F4 = f (a + Ts F3 ) ≈ f (b), (P.4.9)

resulting in the RK4 numerical integration rule

x0 = x(t0 )
F1 = f (xk−1 , uk−1 )
Ts
F2 = f (xk−1 + F1 , uk−1 ) (P.4.10)
2
Ts
F3 = f (xk−1 + F2 , uk−1 )
2
F4 = f (xk−1 + Ts F3 , uk−1 )
Ts
xk = xk−1 + (F1 + 2F2 + 2F3 + F4 ) .
6
It can be shown that the RK4 method matches the Taylor series approximation
of the integral in Equation (P.4.3) up to the fourth order term. Higher order meth-
ods can be derived, but generally do not result in significantly better numerical
approximations to the ODE. The step size Ts must still be chosen to be sufficiently
small to result in good solutions. It is also possible to develop adaptive step size
solvers. For example, the ODE45 algorithm in Matlab solves the ODE using both
an RK4 and an RK5 algorithm. The two solutions are then compared and if they
are sufficiently different, then the step size is reduced. If the two solutions are
close, then the step size can be increased.

Appendix P.4.1 Python Implementation of Numeri-

cal ODE Solvers
In the following code example, we show how to implement a system that is de-
scribed by the second order differential equation

ÿ + a1 ẏ + a2 sin y = bu,

TOC
 APPENDIX P.4. NUMERICAL ODE SOLVERS 452

where a1 , a2 and b are constant coefficients and u is the system input. The first
step is to put the system into state space form. Selecting the states as
   
x1 4 y
x= =
x2 ẏ

and differentiating gives the state space equations

   
ẋ1 x2
=
ẋ2 −a1 x2 − a2 sin x1 + bu
y = x1 ,

which is in the form

ẋ = f (x, u)
y = h(x, u).

Python code that implements a class for this system is given below. The class
has two external properties: the sample rate self.Ts and the state self.state.
The constant coefficients are treated as internal properties known only to the sys-
tem and are therefore labeled as self.a1, self.a2, and self.b. When the
class is instantiated, the __init__ function is called, which is where the states
are initialized with their initial conditions, and the system coefficients a1 , a2 , and
b are initialized. The external method is update, and takes as its input the system
input u, and returns the system output y. The update function first propagates
the system dynamics ẋ = f (x, u) using one of the Runge-Kutta methods. In
the code below, the RK4 algorithm is used, but internal methods that implement
the RK1, RK2, and RK4 methods are all shown for educational purposes. The
RK methods each use the internal function self.f, which is where the system
dynamics ẋ = f (x, u) are specified. This function is system dependent. The
update function then computes the output y = h(x) by calling the internal
function self.h, which is also system dependent.
1 import numpy as np
2
3 class systemDynamics:
4 # Model the system yddot + a1*ydot + a2*sin(y) = b*u
5 def __init__(self, Ts=0.01):
6 self.Ts = Ts # sample rate
7 # Specify the initial condition
8 y0 = 0.0 # y at time zero
9 ydot0 = 0.0 # ydot at time zero
10 self.state = np.array([
11 [y0],
12 [ydot0],
13 ]) # state at time zero
14 self.a1 = 2.0
15 self.a2 = 2.0
16 self.b = 4.0
17

TOC
 APPENDIX P.4. NUMERICAL ODE SOLVERS 453

18 def update(self, u):

19 # This is the external method that takes the input u and
20 # returns the output y.
21 self.rk4_step(u) # propagate the state by one time sample
22 y = self.h() # return the corresponding output
23 return y
24
25 def rk1_step(self, u):
26 # Integrate ODE using Runge-Kutta RK1 algorithm
27 F1 = self.f(self.state, u)
28 self.state += self.Ts * F1
29
30 def rk2_step(self, u):
31 # Integrate ODE using Runge-Kutta RK2 algorithm
32 F1 = self.f(self.state, u)
33 F2 = self.f(self.state + self.Ts / 2 * F1, u)
34 self.state += self.Ts / 2 * (F1 + F2)
35
36 def _rk4_step(self, u):
37 # Integrate ODE using Runge-Kutta RK4 algorithm
38 F1 = self.f(self.state, u)
39 F2 = self.f(self.state + self.Ts / 2 * F1, u)
40 F3 = self.f(self.state + self.Ts / 2 * F2, u)
41 F4 = self.f(self.state + self.Ts * F3, u)
42 self.state += self.Ts / 6 * (F1 + 2 * F2 + 2 * F3 + F4)
43
44 def f(self, state, u):
45 # Return xdot = f(x,u)
46 x1 = state.item(0)
47 x2 = state.item(1)
48 xdot = np.array([
49 [x2],
50 [-self._a1 * x2 - self._a2 * np.sin(x1) + self._b * u],
51 ])
52 return xdot
53
54 def h(self):
55 # return the output equations y=h(x)
56 x1 = self.state.item(0)
57 y = np.array([
58 [x1],
59 ])
60 return y

Listing P.4.1: systemDynamics.py

The system is simulated using the following Python code. The system is in-
stantiated on line 13. The while loop in lines 20–28 update the internal system at
the sample rate T_s rather than at the plot rate T_plot. The input is specified
as a square wave on line 23. The system is propagated one step forward using the
input u on line 24.
1 import matplotlib.pyplot as plt
2 from signalGenerator import signalGenerator
3 from plotData import plotData
4 from systemDynamics import systemDynamics

TOC
 APPENDIX P.4. NUMERICAL ODE SOLVERS 454

5
6 # simulation parameters
7 Ts = 0.01 # sample rate of simulation
8 T_plot = 0.1 # plot update rate
9 t_start = 0.0 # start time of simulation
10 t_end = 20.0 # end time of simulation
11
12 # instantiate system and input classes
13 system = systemDynamics(Ts)
14 input = signalGenerator(amplitude=0.5, frequency=0.1)
15
16 # instantiate the simulation plots and animation
17 dataPlot = plotData()
18
19 t = t_start # time starts at t0=0
20 while t < t_end: # main simulation loop
21 t_next_plot = t + T_plot
22 while t < t_next_plot:
23 u = input.square(t)
24 y = system.update(u)
25 t = t + Ts # advance time by Ts
26 # update animation and data plots
27 dataPlot.update(t, system.state, u)
28 plt.pause(0.0001)
29
30 # Keeps the program from closing until the user presses a button.
31 print(’Press key to close’)
32 plt.waitforbuttonpress()
33 plt.close()

Listing P.4.2: systemSim.py

Appendix P.4.2 Matlab Implementation of Numeri-

cal ODE Solvers
Matlab code that implements a class for the system defined in the previous sec-
tion is given below. The class has five properties: the sample rate self.Ts,
the state self.state, and the constant coefficients self.a1, self.a2, and
self.b. When the class is instantiated, constructor systemDynamics(Ts)
is called, which is where the states are initialized with their initial conditions, and
the system coefficients a1 , a2 , and b are initialized. The class method update
and takes as its input the system input u and returns the system output y. The
update function first propagates the system dynamics ẋ = f (x, u) using one of
the Runge-Kutta methods. In the code below, the RK4 algorithm is used, but in-
ternal methods that implement the RK1, RK2, and RK4 methods are all shown for
educational purposes. The RK methods each use the internal function self.f,
which is where the system dynamics ẋ = f (x, u) are specified. This function is
system dependent. The update function then computes the output y = h(x) by
calling the internal function self.h, which is also system dependent.

TOC
 APPENDIX P.4. NUMERICAL ODE SOLVERS 455

1 classdef systemDynamics < handle

2 properties
3 Ts
4 state
5 a1
6 a2
7 b
8 end
9 methods
10 %---constructor-------------------------
11 function self = systemDynamics(Ts)
12 self.Ts = Ts; % sample rate
13 y0 = 0; % initial y
14 ydot0 = 0; % initial y_dot
15 self.state = [y0; ydot0]; % initial state
16 self.a1 = 2; % system parameters
17 self.a2 = 2;
18 self.b = 4;
19 end
20 %----------------------------
21 function y = update(self, u)
22 % external method that takes u as input
23 % and returns y
24 self.rk4_step(u);
25 y = self.h();
26 end
27 function self = rk1_step(self, u)
28 % Integrate ODE using Runge-Kutta RK1 algorithm
29 F1 = self.f(self.state, u);
30 self.state = self.state + self.Ts * F1;
31 end
32 function self = rk2_step(self, u)
33 % Integrate ODE using Runge-Kutta RK4 algorithm
34 F1 = self.f(self.state, u);
35 F2 = self.f(self.state + self.Ts/2*F1, u);
36 self.state = self.state + self.Ts/2 * (F1 + F2);
37 end
38 function self = rk4_step(self, u)
39 % Integrate ODE using Runge-Kutta RK4 algorithm
40 F1 = self.f(self.state, u);
41 F2 = self.f(self.state + self.Ts/2*F1, u);
42 F3 = self.f(self.state + self.Ts/2*F2, u);
43 F4 = self.f(self.state + self.Ts*F3, u);
44 self.state = self.state...
45 + self.Ts/6 * (F1 + 2*F2 + 2*F3 + F4);
46 end
47 function xdot = f(self, state, u)
48 % Return xdot = f(x,u),
49 x1 = state(1);
50 x2 = state(2);
51 xdot = [x2;...
52 -self.a1*x2 - self.a2*sin(x2) + self.b*u];
53 end
54 function y = h(self)
55 % return y = h(x)
56 x1 = self.state(1);

TOC
 APPENDIX P.4. NUMERICAL ODE SOLVERS 456

57 y = x1;
58 end
59 end
60 end

Listing P.4.3: systemDynamics.m

The system is simulated using the following Matlab code. The system is in-
stantiated on line 7. The while loop in lines 13–23 update the internal system at
the sample rate T_s rather than at the plot rate T_plot. The input is specified
as a square wave on line 17. The system is propagated one step forward using the
input u on line 18.
1 % simulation parameters
2 Ts = 0.01; % sample rate
3 T_plot = 0.1; % plot update rate
4 t_start = 0.0; % simulation start time
5 t_end = 10.0; % simulation end timm
6 % instantiate dynamics and plotter
7 system = systemDynamics(Ts);
8 input = signalGenerator(0.5, 0.2);
9 dataPlot = dataPlotter();
10
11 % main simulation loop
12 t = t_start;
13 while t < t_end
14 % Propagate dynamics in between plot samples
15 t_next_plot = t + P.t_plot;
16 while t < t_next_plot
17 u = input.square(t); % input is a square wave
18 y = system.update(u); % Propagate the dynamics
19 t = t + P.Ts; % advance time by Ts
20 end
21 % update data plots
22 dataPlot.update(t, system.state, u);
23 end

Listing P.4.4: systemSim.m

Appendix P.4.3 Simulink Implementation of Numer-

ical ODE Solvers
The Simulink engine implements a variety of different ODE solvers. The system
dynamics are specified in an s-function. An s-function that implements
the system described above is shown below.
1 function [sys,x0,str,ts,simStateCompliance]...
2 = system_dynamics(t,x,u,flag)
3 switch flag
4 % Initialization
5 case 0
6 [sys,x0,str,ts,simStateCompliance]...

TOC
 APPENDIX P.4. NUMERICAL ODE SOLVERS 457

7 =mdlInitializeSizes();
8 % Derivatives
9 case 1
10 sys=mdlDerivatives(t,x,u);
11 % Update
12 case 2
13 sys=mdlUpdate(t,x,u);
14 % Outputs
15 case 3
16 sys=mdlOutputs(t,x,u);
17 % GetTimeOfNextVarHit
18 case 4
19 sys=mdlGetTimeOfNextVarHit(t,x,u);
20 % Terminate %
21 case 9
22 sys=mdlTerminate(t,x,u);
23 % Unexpected flags %
24 otherwise
25 DAStudio.error(...
26 ’Simulink:blocks:unhandledFlag’,...
27 num2str(flag));
28
29 end
30
31 %
32 %==============================================
33 % mdlInitializeSizes
34 % Return the sizes, initial conditions, and
35 % sample times for the S-function.
36 %==============================================
37 %
38 function [sys,x0,str,ts,simStateCompliance]...
39 =mdlInitializeSizes()
40
41 sizes = simsizes;
42
43 sizes.NumContStates = 2;
44 sizes.NumDiscStates = 0;
45 sizes.NumOutputs = 1;
46 sizes.NumInputs = 1;
47 sizes.DirFeedthrough = 0;
48 sizes.NumSampleTimes = 1;
49
50 sys = simsizes(sizes);
51
52 % initialize the initial conditions%
53 y0 = 0;
54 ydot0 = 0;
55 x0 = [y0; ydot0];
56 % str is always an empty matrix
57 str = [];
58
59 % initialize the array of sample times
60 ts = [0 0];
61 simStateCompliance = ’UnknownSimState’;
62
63 %

TOC
 APPENDIX P.4. NUMERICAL ODE SOLVERS 458

64 %==============================================
65 % mdlDerivatives
66 % Return the derivatives for the continuous
67 % states.
68 %==============================================
69 %
70 function sys=mdlDerivatives(t,x,u,P)
71 x1 = x(1);
72 x2 = x(2);
73 xdot = [x2;...
74 -self.a1*x2 - self.a2*sin(x2) + self.b*u];
75 sys = xdot;
76 % end mdlDerivatives
77
78 %
79 %==============================================
80 % mdlUpdate
81 % Handle discrete state updates, sample time
82 % hits, and major time step requirements.
83 %==============================================
84 %
85 function sys=mdlUpdate(t,x,u)
86
87 sys = [];
88
89 % end mdlUpdate
90
91 %
92 %==============================================
93 % mdlOutputs
94 % Return the block outputs.
95 %==============================================
96 %
97 function sys=mdlOutputs(t,x,u)
98 x1 = x(1);
99 y = x1;
100 sys = y;
101
102 % end mdlOutputs
103
104 %
105 %==============================================
106 % mdlGetTimeOfNextVarHit
107 %==============================================
108 %
109 function sys=mdlGetTimeOfNextVarHit(t,x,u)
110
111 sampleTime = 1;
112 sys = t + sampleTime;
113
114 % end mdlGetTimeOfNextVarHit
115
116 %
117 %==============================================
118 % mdlTerminate
119 % Perform any end of simulation tasks.
120 %==============================================

TOC
 APPENDIX P.4. NUMERICAL ODE SOLVERS 459

121 %
122 function sys=mdlTerminate(t,x,u)
123
124 sys = [];
125
126 % end mdlTerminate

Listing P.4.5: systemDynamics.m

The state equation ẋ = f (x, u) are implemented in the function

mdlDerivatives on lines 70–75, and the output equation y = h(x) is imple-
mented in the function mdlOutputs on lines 97–102. The system is initialized
in mdlInitializeSizes on lines 38–61.
The numerical ODE solver can be specified by using the menu
Simulation > Model Configuration Parameters. Simulink has two
types of solvers: fixed-step solvers and variable-step solvers. The dialog menu
for fixed-step solvers is shown in Figure 4-1, where “ode4(Runge-Kutta)” corre-
sponds to an RK4 method, “ode2(Heun)” corresponds to an RK2 method, and
“ode1(Euler)” correponds to an RK1 method. Simulink also has variable step size
ODE solvers. The options for variable step size are shown in Figure 4-2. The
“ode45 (Dormand-Prince)” solver works by solving the ODE over the next time
step using both an ode4 and an ode5 method, comparing the two, and then reduc-
ing the step size if the difference is too large, and increasing the step size if the
difference is within a tolerance.

Figure 4-1: Dialog window for Simulink fixed-step size solver.

Notes and References

TOC
APPENDIX P.4. NUMERICAL ODE SOLVERS 460

Figure 4-2: Dialog window for Simulink variable-step size solver.

TOC
 Review of Ordinary Differential Equations
P.5
This appendix provides a refresher on how to solve linear ordinary differential
equations with constant coefficients. In particular, we focus on the step response
of such system.

Appendix P.5.1 First order ODE’s

This section provides a review of how to use an integrating factor to find the
solution to a scalar first order differential equations.
Consider the first order linear constant coefficient ordinary differential equa-
tion
dy
(t) = ay(t) + bu(t), (P.5.1)
dt
with initial condition y(0) = y0 , where a and b are constants. Equation (P.5.1)
can be rearranged as
dy
(t) − ay(t) = bu(t). (P.5.2)
dt
Multiplying both sides of Equation (P.5.2) by the integrating factor e−at gives
 
−at dy
e (t) − ay(t) = e−at bu(t). (P.5.3)
dt
Noting that the left hand side of Equation (P.5.3) is the derivative of e−at y(t) gives
d −at

e y(t) = e−at bu(t). (P.5.4)
dt
Integrating both sides of Equation (P.5.4) from 0 to t yields
Z t Z t
d −aτ

e y(τ ) dτ = e−aτ bu(τ )dτ,
0 dτ 0

TOC
APPENDIX P.5. DIFFERENTIAL EQUATIONS 462

which implies that

Z t
e−at y(t) − e−a·0 y(0) = e−aτ bu(τ )dτ.
0

Solving for y(t) gives

Z t
y(t) = eat y0 + ea(t−τ ) bu(τ )dτ , (P.5.5)
| {z } 0
homogeneous response | {z }
forced response

where the first term on the right is the homogenous response, or the response to
initial conditions, and the second term on the right is the forced response, or the
response to the input u(t), which should be noted is the convolution of u(t) with
the impulse response eat .
For the discussion to follow, it is important to note that if a is a complex
number, then Equation (P.5.5) still holds, but that y(t) will be a complex function
of time. To see this, assume that a = σ + jω, then

eat = e(σ+jω)t
= eσt ejωt
= eσt (cos(ωt) + j sin(ωt))
= eσt cos(ωt) + jeσt sin(ωt),

substituting into Equation (P.5.5) yields the complex function

 Z t 
y(t) = eσt cos(ωt)y0 + eσ(t−τ ) cos(ωτ )bu(τ )dτ
0
 Z t 
+ j eσt sin(ωt)y0 + eσ(t−τ ) sin(ω(t − τ ))bu(τ )dτ .
0

Note that if the real part of a is negative, or in other words if a is in the left half of
the complex plane, then the homogeneous response decays to zero and the forced
response is bounded if u(t) is bounded.

Appendix P.5.2 Inverse Laplace Transform

Taking the Laplace transform of Equation (P.5.1) gives

(sY (s) − y0 ) = aY (s) + bU (s).

Solving for Y (s) yields

   
1 b
Y (s) = y0 + U (s).
s−a s−a

TOC
APPENDIX P.5. DIFFERENTIAL EQUATIONS 463

Taking the inverse Laplace transform gives

   
1 b
y(t) = L−1 y0 + L−1 ∗ L−1 (U (s)), (P.5.6)
s−a s−a
where ∗ denotes the convolution operator. Comparing Equation (P.5.5) with Equa-
tion (P.5.6) implies that
 
b
L−1 = beat , t ≥ 0. (P.5.7)
s−a
In the limit, as a → 0 we get
 
b
L−1 = b, t ≥ 0. (P.5.8)
s
Again we note that Equations (P.5.7) and (P.5.8) are true even when a and b are
complex numbers.

Appendix P.5.3 Partial Fraction Expansion

The inverse Laplace transform for first order systems can be used to find the re-
sponse to step inputs, for first order and for higher order systems, through the use
of partial fraction expansion. For example, consider the task of finding the step
response for the first order system

ẏ = −ay + bu, (P.5.9)

where a and b are real positive constants, where u is a step input of size A, i.e.,
(
A t≥0
u(t) = .
0 otherwise

The Laplace transform of u(t) is U (s) = As . Similarly, taking the Laplace trans-
form of Equation (P.5.9) and solving for Y (s) gives
 
b bA
Y (s) = U (s) = . (P.5.10)
s+a s(s + a)
The partial fraction expansion of Y (s) expresses Y (s) as a linear combination of
each factor in the denominator, i.e.,
c1 c2
Y (s) = + . (P.5.11)
s s+a
To find the coefficients c1 and c2 , set Equation (P.5.11) equal to Equation (P.5.10)
to obtain
c1 c2 bA
+ = . (P.5.12)
s s+a s(s + a)

TOC
APPENDIX P.5. DIFFERENTIAL EQUATIONS 464

To find c1 multiply both sides of Equation (P.5.12) by s to obtain

sc2 bA
c1 + = . (P.5.13)
s+a s+a
To remove the term involving c2 in Equation (P.5.13), set s = 0 to obtain

bA
c1 = .
a
To find c2 multiply both sides of Equation (P.5.12) by s + a to obtain

c1 (s + a) bA
+ c2 = . (P.5.14)
s s
To remove the term involving c1 in Equation (P.5.14), set s = −a to obtain

bA
c2 = − .
a
Therefore,
bA bA
a a
Y (s) = − . (P.5.15)
s s+a
Since the inverse Laplace transform is a linear operator, we get
! !
bA bA
y(t) = L−1 a
− L−1 a
s s+a
bA bA −at
= − e , t≥0
a a
bA
= (1 − e−at ), t ≥ 0.
a
The same technique works for finding the step response for higher order sys-
tems. As an example, find the unit step response of the system described by

y (4) + 10y (3) + 35ÿ + 50ẏ + 24y = u.

Taking the Laplace transform and solving for Y (s), and using the fact that U (s) =
1
s gives
 
1
Y (s) = U (s)
s4 + 10s3 + 35s2 + 50s + 24
 
1 1
=
s4 + 10s3 + 35s2 + 50s + 24 s
    
1 1 1 1 1
= . (P.5.16)
s s+1 s+2 s+3 s+4

TOC
 APPENDIX P.5. DIFFERENTIAL EQUATIONS 465

Using partial fraction expansion we have that

c1 c2 c3 c4 c5
Y (s) = + + + + . (P.5.17)
s s+1 s+2 s+3 s+4

Setting Equation (P.5.17) equal to Equation (P.5.16) gives

c1 c2 c3 c4 c5
+ + + +
s s+1 s+2 s+3 s+4
    
1 1 1 1 1
= . (P.5.18)
s s+1 s+2 s+3 s+4

The coefficient c1 is found by multiplying both sides of Equation (P.5.18) by s

and then setting s = 0 to remove the coefficients c2 –c5 to obtain c1 = 1/24.
Following a similar procedure for each coefficient yields c2 = −1/6, c3 = 1/4,
c4 = −1/6, and c5 = 1/24. The step response is therefore given by

     
1/24 −1/6 1/4
y(t) = L−1 + L−1 + L−1
s s+1 s+2
   
−1/6 1/24
+ L−1 + L−1
s+3 s+4
1 1 1 1 1
= − e−t + e−2t − e−3t + e−4t , t≥0
24 6 4 6 24
1

= 1 − 4e−t + 6e−2t − 4e−3t + e−4t , t ≥ 0.
24

In Matlab, the coefficients can be found by using the residue command.

For the problem above we could use the matlab commands

1 num = 1; % numerator polynomial

2 % denominator polynomial
3 den = poly([0,-1,-2,-3,-4]);
4 % compute partial fraction expansion
5 [R,P,K] = residue(num,den);

The vector R will contain the coefficients c1 –c5 , the vector P contains the asso-
ciated roots, and K will be the remainder polynomial if the numerator is a higher
order polynomial than the denominator.
The method of partial fraction expansion is also valid if the poles of the system
are complex. For example, consider finding the response of the second order
differential equation

ÿ + 2ζωn ẏ + ωn2 y = ωn2 u,

TOC
APPENDIX P.5. DIFFERENTIAL EQUATIONS 466

to as step of size A. Taking the Laplace transform and letting U (s) = A/s gives

 
ωn2 A
Y (s) =
s2 + 2ζωn s + 2ωn2 s
!
Aωn2 1
= p (P.5.19)
s s + ζωn + jωn 1 − ζ2
!
1
· p . (P.5.20)
s + ζωn − jωn 1 − ζ2

Expanding Y (s) as a sum of partial fractions yields

!
c1 c2
Y (s) = + p
s s + ζωn + jωn 1 − ζ2
!
c3
+ p . (P.5.21)
s + ζωn − jωn 1 − ζ2

Setting Equation (P.5.21) equal to Equation (P.5.20) gives

! !
c1 c2 c3
+ p + p
s s + ζωn + jωn 1 − ζ2 s + ζωn − jωn 1 − ζ2
! !
Aωn2 1 1
= p p .
s s + ζωn + jωn 1 − ζ2 s + ζωn − jωn 1 − ζ2

Solving for c1 , c2 , and c3 gives

c1 = A
!
A ζ
c2 = − 1+jp
2 1 + ζ2
!
A ζ
c3 = − 1−jp .
2 1 − ζ2

Therefore,
   
1+j√ ζ 2
− A2 − A2 1 − j √ ζ 2
A 1+ζ 1−ζ
Y (s) = + p + p .
s s + ζωn + jωn 1 − ζ 2 s + ζωn − jωn 1 − ζ 2

TOC
APPENDIX P.5. DIFFERENTIAL EQUATIONS 467

Taking the inverse Laplace transform gives

√
!
A ζ 1−ζ 2 t
y(t) = A − 1+jp e−ζωn t−jωn
2 1 + ζ2
√
!
A ζ 2
− 1−jp e−ζωn t+jωn 1−ζ t
2 1 − ζ2
 
1 ζ
 
 −ζωn t
= A 1 − 1 + j p e
2 1 + ζ2 
 ! !
√ j∠ 1+j √ ζ √ 1−j √ ζ
!
j∠
−jω 1−ζ 2t 2 2
· e n
e 1+ζ
+ e+jωn 1−ζ t e 1+ζ 2 

1
=A 1− p e−ζωn t
1 − ζ2
 √ !!
√ !! 
−j ωn 1−ζ 2 t−tan−1 √ζ j ωn 1−ζ 2 t−tan−1 √ζ !
e 1−ζ 2 1−ζ 2
+e 
· 
 2 

!!!
1 ζ
e−ζωn t cos ωn 1 − ζ 2 t − tan−1
p
=A 1− p p ,
1−ζ 2 1 − ζ2

for t ≥ 0. In general, complex conjugate poles will always lead to complex

conjugate coefficients that end up canceling to produce a decaying exponential
multiplied by a cosine term with a phase shift. If the coefficients of the original
ordinary differential equation are real, then the response will always be real.

TOC
APPENDIX P.5. DIFFERENTIAL EQUATIONS 468

TOC
 Root Locus
P.6
The root locus has been an important pedagogical tool in control systems since
the 1940s. It was important in the design of feedback amplifiers for communi-
cation circuits where the classic design involved the tuning of a small number of
parameters. In this appendix we introduce the root locus and show how it can be
used to understand the effect of adding an integrator to a second order system with
PD control. However, we should note that root locus can be used in many other
settings. We are not including this appendix as a main chapter in the book because
in our experience root locus is rarely used for actual design.

Appendix P.6.1 Theory

We begin by noting that for the closed loop system shown in Figure 6-1, the trans-
fer function from Yr (s) to Y (s) can be computed as
Y (s) = P (s)C(s)(Yr (s) − Y (s))
P (s)C(s)
=⇒ Y (s) = Yr (s).
1 + P (s)C(s)
Therefore, the characteristic equation of the closed loop system is given by
∆cl = 1 + P (s)C(s) = 0.

We have seen in Chapter 7 that by using PD control for a second order system,
it is possible to precisely select the locations of the closed loop poles. When an
integrator is added for PID control, it is also possible to precisely select the loca-
tion of the three closed-loop poles. In this chapter we will introduce a simple and
convenient tool for visualizing how the closed loop poles change as a function of
a single parameter. As motivation, consider a first order plant under proportional
control, as shown in Fig. 6-2. The closed-loop transfer function is given by

TOC
APPENDIX P.6. ROOT LOCUS 470

yr
C(s) P (s)

Figure 6-1: General feedback system with plant P (s) and controller C(s).

yr
1
kP s+2

Figure 6-2: A first order plant under proportional control.

kP
Y (s) = Yr (s)
s + (2 + kP )
and the closed loop characteristic polynomial is given by

∆cl (s) = s + (2 + kP ).

Note that the location of the closed loop pole is

pcl = −(2 + kP ).

We can create a plot of the location of the poles in the complex plane as shown in
Fig. 6-3. When kP = 0, the closed loop pole is located at the open loop pole −2,
but as kP is increased greater than zero, the closed loop pole moves from −2 into
the left half plane in the direction of the arrow shown in Fig. 6-3. The plot of the
closed loop pole as a function of kP is an example of a root locus.
As a second example, consider the second order system under proportional
control shown in Fig. 6-4. In this case the closed loop transfer function is given
by
kP
Y (s) = 2 Yr (s)
s + 2s + kP
Note that the open loop poles are located at 0 and −2. The closed loop character-
istic polynomial is given by

∆cl (s) = s2 + 2s + kP (P.6.1)

and the closed loop poles are located at

p
pcl = −1 ± 1 − kP .

The position of the closed loop poles as a function of kP > 0 are shown in
Fig. 6-5. When kP = 0, the closed loop poles are equal to the open loop poles

TOC
APPENDIX P.6. ROOT LOCUS 471

kP > 0 kP = 0

<
x
2

Figure 6-3: A plot of the closed loop pole associated with Fig. 6-2 as a function
of kP > 0.

yr 1
kP s(s + 2)

Figure 6-4: A second order plant under proportional control.

kP > 1
0  kP  1

<
x x
2 1

kP = 0

Figure 6-5: A plot of the closed loop pole associated with Fig. 6-4 as a function
of kP > 0.

TOC
 APPENDIX P.6. ROOT LOCUS 472

at 0 and −2. When 0 < kP ≤ 1, the closed loop poles are both real and move
toward −1 with increasing kP . When kP = 1 both poles are located at −1. When
kP > 1 the two poles are complex with real part equal to −1 and increasing com-
plex part as kP increases. The poles move in the direction of the arrows shown in
Fig. 6-5.
The Matlab command rlocus will plot the root locus and can be used in-
teractively to find the gain associated with specific pole locations. To use the
rlocus command in Matlab, the characteristic equation must be put in Evan’s
form, which is
1 + kL(s) = 0,

were k is the gain of interest. For example, the characteristic equation associated
with the characteristic polynomial in Equation (P.6.1) is

∆cl (s) = s2 + 2s + kP = 0.

The characteristic equation can be put in Evan’s form by dividing both sides by
s2 + 2s and rearranging to obtain
 
1
1 + kP = 0,
s2 + 2s
1
where L(s) = s2 +2s . The Matlab command that draws the root locus for this
equation is
1 >> L = tf([1], [1, 2, 0]);
2 >> figure(1), clf, rlocus(L)

1
where the first line defines the transfer function L(s) = s2 +2s and the second line
invokes the root locus command using L.
The root locus tool is particularly useful for selecting the integrator gains after
PD gains have been selected. For example, suppose that a PD controller has been
designed for the second order plant shown in Fig. 6-6 so that the closed loop poles
are located at −2 ± j2, where kP = 8 and kD = 2. Now suppose that an

zr 1 z
kD s 2 + kP s + kI
s s(s + 2)

Figure 6-6: Closed loop system where the proportional gains have been selected
as kP = 8 and kD = 2.

integrator is to be added to reject disturbances and eliminate steady state error, but
that the integrator gain kI is to be selected so that the closed loop poles deviate
from −2 ± j2 by less than 10%. The characteristic equation for the closed loop

TOC
 APPENDIX P.6. ROOT LOCUS 473

system including the integrator is given by

  
1 kD s2 + kP s + kI
1+ =0
s2 + 2s s
(s3 + 2s2 ) + (kD s2 + kP s + kI )
=⇒ =0
s3 + 2s2
=⇒ s3 + (2 + kD )s2 + kP s + kI = 0
(s3 + (2 + kD )s2 + kP s) + kI
=⇒ =0
s3 + (2 + kD )s2 + kP s
 
1
=⇒ 1 + kI =0
s3 + (2 + kD )s2 + kP s
 
1
=⇒ 1 + kI =0
s3 + 4s2 + 8s
where the later equation is in Evan’s form. Therefore the Matlab command for
drawing the root locus is
1 >> L = tf([1], [1, 4, 8, 0]);
2 >> figure(1), clf, rlocus(L)

and the resulting plot is shown in Fig. 6-7. Note from Fig. 6-7 that the poles

Root Locus
10

System: L
Gain: 3.81
4 Pole: -1.67 + 1.74i
Damping: 0.694
Overshoot (%): 4.85
Imaginary Axis (seconds-1 )

Frequency (rad/s): 2.41

2

-2

-4

-6

-8

-10
-12 -10 -8 -6 -4 -2 0 2 4
Real Axis (seconds -1 )

Figure 6-7: The result of Matlab’s rlocus command, and by interactively se-
lecting pole locations close to −2 ± j2.

when kI = 0 are at 0, −2 ± j2 and are denoted by ’x’. The root locus shows what

TOC
APPENDIX P.6. ROOT LOCUS 474

happens to the closed loop poles as kI is increased from zero. Note from Fig. 6-7
that for large values of kI the closed-loop poles are in the right half plane and the
system will be unstable. However, for a gain of kI = 3.81 the closed loop poles
are located at −1.67 ± j1.74. This technique can be used to select reasonable
values for kI .

Appendix P.6.2 Design Study A. Single Link Robot

Arm

Example Problem A.P.6

For the single link robot arm, use the PD gains derived in HW A.8. Add an integra-
tor to the controller to get PID control. Put the resulting closed loop characteristic
equation in Evan’s form and use the Matlab rlocus command to plot the root
locus verses the integrator gain kI . Select a value for kI that does not significantly
change the other locations of the closed loop poles.

Solution
The closed loop block diagram including an integrator is shown in Fig. 6-8. The

kI
s
˜r
⇥ 3 ˜
⇥
m`2
kP 3b
s(s + m`2 )
kD s

Figure 6-8: PID control for the single link robot arm.

closed loop transfer function is given by

3kP 3kI
m`2 s + m`2
Θ̃(s) = 3b+3kD

3kP 3kI
Θ̃d (s).
s3 + m`2 s2 + m` 2s + m`2

The characteristic equation is therefore

 
3 3b + 3kD 3kP 3kI
s + 2
s2 + 2
s+ = 0.
m` m` m`2

TOC
 APPENDIX P.6. ROOT LOCUS 475

In Evan’s form we have

!
3
m`2
1 + kI 3b+3kD

3kP
= 0.
s3 + m`2 s2 + m`2 s

The appropriate Matlab command is therefore

1 >> L = tf([3/m/L^2],...
2 [1,(3*b+3*kd)/m/L^2,3*kp/m/L^2,0]);
3 >> figure(1), clf, rlocus(L);

Appendix P.6.3 Design Study B. Inverted Pendulum

Example Problem B.P.6

Adding an integrator to obtain PID control for the outer loop of the inverted pen-
dulum system, put the characteristic equation in Evan’s form and use the Matlab
rlocus command to plot the root locus verses the integrator gain kI . Select a
value for kI that does not significantly change the other locations of the closed
loop poles.

Solution
The closed loop block diagram for the outer loop of the inverted pendulum includ-
ing an integrator is shown in Fig. 6-9. The closed loop transfer function is given

Figure 6-9: PID control for the outer loop of the inverted pendulum.

by
(g − 32 `s2 ) (kPz s + kIz )
Z̃(s) =   Z̃r (s).
− 23 `kDz s4 + 1
kDC
− 23 `kPz s3 + (gkDz − 32 `kIz )s2 + gkPz s + gkIz
θ

The characteristic equation is therefore

 
2 4 1 2 2
− `kDz s + − `kPz s3 + (gkDz − `kIz )s2 + gkPz s + gkIz = 0.
3 kDCθ Hw_pendulum_PID_root_locus
3 3

TOC
 APPENDIX P.6. ROOT LOCUS 476

In Evan’s form we have

 
− 23 `s2 + g
1 + kIz     = 0.
− 23 `kDz s4 + 1
kDCθ − 32 `kPz s3 + gkDz s2 + gkPz s

The appropriate Matlab command is therefore

1 >> L = tf([-2/3*ell,0,g],...
2 [-2/3*ell*kd,1/kDCth-2/3*ell*kp,g*kd,g*kp,0]);
3 >> figure(1), clf, rlocus(L);

Appendix P.6.4 Design Study C. Satellite Attitude Con-

trol

Example Problem C.P.6

Adding an integrator to obtain PID control for the outer loop of the satellite sys-
tem, put the characteristic equation in Evan’s form and use the Matlab rlocus
command to plot the root locus verses the integrator gain kI . Select a value for kI
that does not significantly change the other locations of the closed loop poles.

Solution
The closed loop block diagram for the outer loop including an integrator is shown
in Fig. 6-10. The closed loop transfer function is given by

kI
s
⇣ ⌘
r ✓d b k
kP kDC✓ Jp s + Jp
b k
s2 + Jp s + Jp
kD s

Figure 6-10: PID control for the outer loop of the satellite system.

b2 s2 + b1 s + b0
Φ(s) = Φr (s),
a3 s3+ a2 s2 + a1 s + a0

TOC
 APPENDIX P.6. ROOT LOCUS 477

The appropriate Matlab command is therefore

1 >> L = tf([b*kDC/Jp, k*kDC/Jp],...
2 [1+b*kDC*kD/Jp,...
3 b/Jp+k*kDC*kD/Jp+b*kDC*kP/Jp,...
4 k/Jp+k*kDC*kP/Jp, 0]);
5 >> figure(1), clf, rlocus(L);

Notes and References

TOC
APPENDIX P.6. ROOT LOCUS 478

TOC
 Review of Linear Algebra
P.7
Let A be an m × n matrix of real numbers, denoted as A ∈ Rm×n . Then
 
a11 a12 . . . a1n
 a21 a22 . . . a2n 
 
A= . ..  .
 .. . 
am1 am2 ... amn
Matrix addition is defined element wise for matrices of compatible dimensions as
   
a11 a12 . . . a1n b11 b12 . . . b1n
 a21 a22 . . . a2n   b21 b22 . . . b2n 
   
 .. ..  +  .. .. 
 . .   . . 
am1 am2 . . . amn bm1 bm2 . . . bmn
| {z } | {z }
A B
 
a11 + b11 a12 + b12 . . . a1n + a1n
 a21 + b21 a22 + b22 . . . a2n + b2n 
 
= .. .. .
 . . 
am1 + bm1 am2 + bm2 . . . amn + bmn
| {z }
C

To multiply a matrix times a vector, the inner dimensions must match:

     Pn 
a11 . . . a1n x1 i=1 a1i xi
 ..   ..  =  
x =  ...
A |{z}
|{z} .   .  P ..
.
 = |{z}
b .
m×n n×1 n m×1
am1 . . . amn xn i=1 ami xi

For example,     
1 2 5 17
= .
3 4 6 39

TOC
APPENDIX P.7. LINEAR ALGEBRA 480

Multiplying two matrices also requires matching dimensions:

  
a11 . . . a1n b11 . . . b1n
 ..   .. .. 
|{z} B =  ...
A |{z} .  . . 
m×n n×p am1 . . . amn bm1 . . . bmn
 Pn Pn 
i=1 a1i bi1 ... i=1 a1i bip
 .. .. 
= C .
 = |{z}
Pn . Pn . m×p
i=1 ami bi1 . . . i=1 ami bip

Note that if A ∈ Rm×n and B ∈ Rn×p , then AB is a valid product, but BA is

not a valid product. Even if the dimensions are compatible, i.e., if A and B are
square, in general
AB 6= BA.

Appendix P.7 .1 Transpose

If A ∈ Rm×n , then the transpose of A, denoted as A> ∈ Rn×n is obtained by
interchanging the rows and the columns. For example
 >
x1

x2  = x1 x2 x3 ,
x3
 >  
1 2 1 3
= .
3 4 2 4

A square matrix A is said to be symmetric if A> = A. Similarly, A is said

to be skew-symmetric if A> = −A. Every square matrix A ∈ Rn×n can be
decomposed into the sum of a symmetric and a skew-symmetric matrix as

1 1 1 1
A= A + A + A> − A>
2
 2 2  2 
1 1 > 1 1 >
= A+ A + A− A
2 2 2 2
= As + Ass ,

where As = 12 (A+A> ) is symmetric, and Ass = 21 (A−A> ) is skew-symmetric.

The following properties are easily verified by direct manipulation:

(AB)> = B > A>

(A + B)> = A> + B > .

TOC
APPENDIX P.7. LINEAR ALGEBRA 481

Appendix P.7 .2 Trace

The trace operator is defined for square matrices. If A ∈ Rn×n , then
n
X
tr(A) = aii ,
i=1

or in other words, the sum of the diagonals of A. For example

 
1 2
tr = 1 + 4 = 5.
3 4

Appendix P.7 .3 Determinant

The determinant operator is defined for square matrices A ∈ Rn×n as
n
X
det(A) = aij γij ,
j=1

for any i = 1, . . . , n, where the co-factor

γij = (−1)(i+j) det(Mij )
and where the minor Mij is the (n − 1) × (n − 1) matrix obtained by removing
the ith row and the j th column of A. As an example,
 
1 2 3    
  5 0 4 0
det 4 5 0 = 1 · det − 2 · det
7 8 6 8
6 7 8
 
4 5
+ 3 · det
6 7
= 1 · (5 · 8 − 7 · 0) − 2 · (4 · 8 − 6 · 0)
+ 3 · (4 · 7 − 6 · 5)
= −30.
The adjugate of A is defined as the transpose of the co-factors of A:
 
γ11 . . . γn1
 ..  .
adj(A) =  ... . 
γ1n ... γnn
We have the following properties for the determinant:
det(A) = det(A> )
det(AB) = det(A) det(B)
1
det(A−1 ) = .
det(A)

TOC
APPENDIX P.7. LINEAR ALGEBRA 482

There are also well defined rules for finding the determinant of a block matrix.
Let  
A B
E=
C D

where A ∈ Rn×n , B ∈ Rn×p , C ∈ Rm×n and D ∈ Rm×m , then it can be shown

that if A is invertible, then


det(E) = det(A) det D − CA−1 B .

As a special case, if either B = 0 or C = 0, then

det(E) = det(A) det(D).

For example,
 
1 2 3 4 5  
2 3 4 5 6 1 2 3  
  8 9
det 
3 4 5 6 7 
 = det 2 3 
4 · det .
0 9 10
0 0 8 9 3 4 5
0 0 0 9 10

Appendix P.7 .4 Matrix Inverses

Definition. If A ∈ Rn×n , and there exists a matrix B ∈ Rn×n such that

AB = BA = I,

then B is said to be the inverse of A and is denoted as B = A−1 .

The inverse of any square matrix can be expressed in terms of its adjugate and
its determinant as
adj(A)
A−1 = .
det(A)
For a 2 × 2 matrix we have the simple formula
   
a b d −b
 −1 adj
a b c d −c a
=  = .
c d a b ad − bc
det
c d

If det(A) 6= 0, then A is called non-singular.

If A and B are nonsingular, then

(AB)−1 = B −1 A−1 .

TOC
APPENDIX P.7. LINEAR ALGEBRA 483

For block matrices it can be verified by direct substitution that

−1
(A − BD−1 C)−1 −(A − BD−1 C)−1 BD−1
  
A B
=
C D −D−1 C(A − BD−1 C)−1 −1 D−1 −1
+ D C(A − BD C) BD −1 −1

(P.7.1)
 −1
A + A−1 B(D − CA−1 B)−1 CA−1 −A−1 B(D − CA−1 B)−1

= ,
−(D − CA−1 B)−1 CA−1 (D − CA−1 B)−1
(P.7.2)
from which we get the famous matrix inversion lemma

(A − BD−1 C)−1 = A−1 + A−1 B(D − CA−1 B)−1 CA−1 . (P.7.3)

When C = 0 we get the simple formula

 −1  −1 
A B A −A−1 BD−1
= .
0 D 0 D−1

Appendix P.7 .5 Eigenvalues and Eigenvectors

The pair (λ, v) where λ ∈ C and 0 6= v ∈ Cn×1 is said to be an eigenvalue-
eigenvector pair of the matrix A ∈ Rn×n if

Av = λv.

The following statements are equivalent

1. (λ, v) are an eigenvalue-eigenvector pair of A,
2. Av = λv,
3. (λI − A)v = 0, where I is the n × n identity matrix,
4. v is in the null space of (λI − A),
5. det(λI − A) = 0.
For example, to find the eigenvalues and eigenvectors of
 
0 1
A= ,
−2 −3

first compute
 
λ −1
det(λI − A) = det
2 λ+3
= λ2 + 3λ + 2 = (λ + 2)(λ + 1).

Setting det(λI − A) = 0 implies that the eigenvalues are

λ1 = −2, λ2 = −1.

TOC
APPENDIX P.7. LINEAR ALGEBRA 484

To find the eigenvector associated with λ1 we need to find a vector v1 that is in

the null space of (λ1 I − A). Since
    
−2 −1 v11 0
(λ1 I − A)v1 = = ,
2 1 v12 0

we have that 2v11 = −v12 . Therefore v1 = (1, −2)> is in the null space of
(λ1 I − A) and is an eigenvector of A associated with eigenvalue λ1 .
To find the eigenvector associated with λ2 we need to find a vector v2 that is
in the null space of (λ2 I − A). Since
    
−1 −1 v21 0
(λ2 I − A)v2 = = ,
2 2 v22 0

we have that v21 = −v22 . Therefore v2 = (−1, 1)> is in the null space of
(λ2 I − A) and is an eigenvector of A associated with eigenvalue λ2 .
Since Av1 = λ1 v1 and Av2 = λ2 v2 we can write
 


λ1 0
A v1 v2 = v1 v2 .
0 λ2
 

λ1 0
Defining M = v1 v2 and Λ = we have
0 λ2

AM = M Λ.

When M is invertible, we have

A = M ΛM −1 .

M will always be invertible when the eigenvalues are distinct. For example, it can
be verified that
     −1
0 1 1 −1 −2 0 1 −1
= .
−2 −3 −2 1 0 11 −2 1

The formula A = M ΛM −1 is called the eigen-decomposition of A.

The trace and the determinant of A can be written in terms of the eigenvalues
of A as
n
X
tr(A) = λi (A),
i=1
det(A) = Πni=1 λi (A).

where λi (A) is the ith eigenvalue of A.

TOC
APPENDIX P.7. LINEAR ALGEBRA 485

Appendix P.7 .6 Linear Independence and Rank

The vectors x1 , x2 , . . . , xp ∈ Rn×1 are said to be linearly independent if
c1 x1 + c2 x2 + · · · + cp xp = 0,
implies that c1 = c2 = · · · = cp = 0. For example, the vectors (1, 0)> and
(0, 1)> are linearly independent since
       
1 0 c 0
c1 + c2 = 1 =
0 1 c2 0
implies that c1 = c2 = 0. Alternatively, the vectors (1, 0)> and (3, 0)> are not
linearly independent since
       
1 3 c + 3c0 0
c1 + c2 = 1 =
0 0 0 0
is true if c1 = −3c2 for any c2 .
The rank of a matrix is defined to be the number of linearly independent rows
or columns. For example
 
1 0
rank =2
0 1
 
1 3
rank = 1.
0 0

Appendix P.7 .7 Special Matrices

The matrix A ∈ Rn×n is said to be diagonal if
 
a11 0 . . . 0
 0 a22 . . . 0 
 
A= . .. .. .
 .. . . 
0 0 ... ann
For example,  
2 0 0 0
0 3 0 0
A=
0

0 4 0
0 0 0 5
is a diagonal matrix.
The matrix A ∈ Rn×n is said to be upper triangular if
 
a11 a12 . . . a1n
 0 a22 . . . a2n 
 
A= . .. ..  .
 .. . . 
0 0 ... ann

TOC
APPENDIX P.7. LINEAR ALGEBRA 486

For example,  
2 3 4 5
0 6 7 8
A=
0

0 9 10
0 0 0 11
is an upper triangular matrix.
The matrix A ∈ Rn×n is said to be in upper companion form if
 
−a1 −a2 . . . −an−1 −an
 1 0 ... 0 0 
 
A= . .. .. .
 .. . . 0 
0 0 ... 1 0

For example,  
−2 −3 −4 −5
1 0 0 0
A=
0

1 0 0
0 0 1 0
is in upper companion form.
The matrix A ∈ Rn×n is said to be a Vandermonde matrix if
 
1 a1 a21 . . . a1n−1
1 a2 a22 . . . an−1 
A= 2
1 a3 a23 . . . an−1  .

3
1 a4 a24 . . . an−1 4

For example,  
1 2 4 9
1 3 9 27 

A= 
1 4 16 64 
1 5 25 125
is a Vandermonde matrix.

TOC
 Bibliography

[1] H. Goldstein, Classical Mechanics. Addison-Wesley, 1951.

[2] V. I. Arnold, Mathematical Methods of Classical Mechanics. New York,

New York: Springer Verlag, 1989.

[3] J. Hanc, E. F. Taylor, and S. Tuleja, “Deriving Lagrange’s Equations using

Elementary Calculus,” American Journal of Physics, vol. 72, pp. 510–513,
apr 2014.

[4] G. F. Franklin, J. D. Powell, and A. Emami-Naeini, Feedback Control of

Dynamic Systems. Addison Wesley, 4rd ed., 2002.

[5] J. P. Hespanha, Linear Systems Theory. Princeton University Press, 2009.

[6] T. Kailath, Linear Systems. Englewood Cliffs, New Jersey: Prentice Hall,
1980.

[7] C.-T. Chen, Linear System Theory and Design. Oxford University Press,
1999.

[8] M. W. Spong, S. Hutchinson, and M. Vidyasagar, Robot Modeling and Con-

trol. Wiley, 2006.

[9] K. S. Fu, R. C. Gonzalez, and C. S. G. Lee, Robotics: Control, Sensing,

Vision, and Intelligence. New York, New York: McGraw Hill, 1987.

[10] J. J. Craig, Introduction to Robotics: Mechanics and Control. Pearson Pren-

tice Hall, third edit ed., 2005.

[11] H. K. Khalil, Nonlinear Control. Pearson, 2015.

TOC
BIBLIOGRAPHY 488

[12] A. Isidori, Nonlinear Control Systems. Communication and Control Engi-

neering, New York, New York: Springer Verlag, 2nd ed., 1989.
[13] H. Nijmeijer and A. J. van der Schaft, Nonlinear Dynamical Control Sys-
tems. New York, New York: Springer Verlag, 1990.

[14] S. Sastry, Nonlinear Systems: Analysis, Stability, and Control. Springer,

1999.
[15] Krstić, Kanellakopaulis, and Kokotovic, Nonlinear and Adaptive Control
Design. Wiley, 1995.
[16] A. V. Oppenheim and A. S. Willsky, Signals & Systems. Prentice Hall, sec-
ond ed., 1997.
[17] P. M. DeRusso, R. J. Roy, C. M. Close, and A. A. Desrochers, State Variables
for Engineers. Wiley, second ed., 1998.
[18] G. F. Franklin, J. D. Powell, and M. Workman, Digital Control of Dynamic
Systems. Addison Wesley, third edit ed., 1998.
[19] Dorato, Abdallah, and Cerone, Linear-Quadratic Control: An Introduction.
Prentice Hall, 1995.
[20] H. K. Khalil, Nonlinear Systems. Upper Saddle River, NJ: Prentice Hall,
3rd ed., 2002.

[21] D. G. Luenberger, “An Introduction to Observers,” IEEE Transactions on

Automatic Control, vol. 16, pp. 596–602, dec 1971.
[22] K. J. Astrom and R. M. Murray, Feedback Systems: An Introduction for
Scientists and Engineers. Princeton University Press, 2009.

[23] R. C. Dorf and R. H. Bishop, Modern Control Systems. Pearson Education,

13th editi ed., 2016.
[24] F. L. Lewis, Optimal Estimation: With an Introduction to Stochastic Control
Theory. New York, New York: John Wiley & Sons, 1986.

[25] S. Thrun, W. Burgard, and D. Fox, Probabilistic Robotics. MIT Press, 2006.
[26] B. D. O. Anderson and J. B. Moore, Linear Optimal Control. Englewood
Cliffs, New Jersey: Prentice Hall, 1971.
[27] Aided Navigation: GPS with High Rate Sensors. McGraw-Hill, 2008.

TOC
 Index

Ackermann’s formula, 166, 206 complex numbers - polar

adjugate of a matrix, 72, 457 coordinates, 249
analog computer, 201 complex numbers - rectangular
analog computer implementation, coordinates, 249
159 computed torque, 54
augmented state space equations, conjugate of a complex number, 250
228 control canonic form, 159, 160, 462
augmented system, 182 control canonical form, 316
control implementation, 315
ball and beam, 363 controllability matrix, 165, 182
block matrix, 458 controllable, 182
Bode canonical form, 254
controllable system, 165
Bode phase gain relationship, 296
critically damped, 99
Bode plot, 252, 254, 290
cross over frequency, 289
change of variables, 158, 163
closed-loop Bode plot, 293 damping forces, 33
closed-loop characteristic equation, damping ratio, 93, 96, 293
158 DC gain, 104, 183
closed-loop characteristic DC-gain, 94, 166
polynomial, 86 derivative control, 80
closed-loop performance, 271 derivative gain, 81
closed-loop poles, 86, 158 design models, 44
closed-loop transfer function, 86 design process, 1
co-factor, 457 desired characteristic polynomial,
companion form, 161 86
complex conjugate poles, 443 determinant of a matrix, 457
complex numbers, 249 diagonal matrix, 461

TOC
INDEX 490

digital differentiator, 155 integral gain, 80

digital implementation, 211 integrating factor, 437
dirty derivative, 141 integrator, 121, 140, 448
discrete differentiator, 141 integrator anti-windup, 141
discrete integrator, 141 interaction matrix, 182
distinct eigenvalues, 460 inverse Laplace transform, 94, 95,
disturbance observer, 227 439
disturbance rejection, 273, 276, inverse of a matrix, 458
277, 289, 309
Jacobian linearization, 45, 47, 69
eigen-decomposition, 460
eigenvalue of a matrix, 459 kinetic energy, 8, 11
eigenvalues, 72, 157
eigenvector, 459 Lagrangian, 33
equilibrium point, 46 Laplace transform, 55, 71, 228, 438
estimated state, 199 linear independence, 461
Euler’s relationship, 250 linearized system, 167, 183
Euler-Lagrange equation, 10 linearized systems, 208
Euler-Lagrange equations, 31, 33 loop gain, 290, 305
loopshape, 289
feedback linearization, 45, 48, 53 loopshaping, 247, 305
final value theorem, 121 loopshaping control, 3
first order response, 93 low-pass filter, 306, 307
forced response, 438
free integrator, 126 magnitude of a complex number,
frequency response, 247, 252, 271, 249
292 magnitude of a transfer function,
full state feedback, 157, 162 251
fundamental theorem of calculus, matrix, 455
122 matrix addition, 455
matrix adjugate, 457
gain margin, 292 matrix determinant, 457
generalized coordinates, 10, 32 matrix eigenvalue, 459
generalized forces, 10, 32 matrix inverse, 458
generalized velocity, 10 matrix minor, 457
matrix multiplication, 455
homogeneous response, 438 matrix rank, 461
matrix trace, 457
innovation, 200 matrix transpose, 456
input disturbance, 127, 227, 271 measured output, 167, 199
input disturbance rejection, 273, moments of inertia, 16
277
input saturation, 142, 294 natural frequency, 93, 96, 293
integral control, 80, 155, 156, 181, noise, 271
306 noise attenuation, 273, 274, 289,
integral feedback, 206 307

TOC
INDEX 491

non-singular matrix, 458 realization of a transfer function,

nonminimum phase zeros, 102 158
reference command, 80
observability matrix, 205 reference input, 127
observable, 229 reference output, 167, 181, 199
observable system, 206 residue, 441
observation error, 200 rise time, 94, 96, 99
observer, 199, 227 root locus, 181, 445
observer canonic form, 202
observer error, 200 s-function, 415
observer gain, 200, 203, 206 sample period, 141
observer-based control, 3 sampled data system, 139
observers, 155 second order response, 96
open loop characteristic second order systems, 80
polynomial, 85 sensor noise, 127
open loop poles, 85 separation principle, 206, 208
open-loop poles, 157 simulation Model, 1
ordinary differential equations, 437 stability margins, 305
output disturbance, 127, 271 state evolution equation, 67
output disturbance rejection, 273, state feedback, 155
276 state interaction matrix, 157, 162
output equation, 67 state space equations, 70, 157
over damped, 98 state space model, 67, 199
state space models, 3, 155
parallel axis theorem, 29 steady state error, 81, 121
partial fraction expansion, 101, 439 step response, 93, 94, 100, 293,
PD control, 85 437, 439
phase margin, 290, 310 successive loop closure, 102, 144,
phase of a complex number, 249 319
phase of a transfer function, 251 system type, 125, 128, 279, 307
phase-lag filter, 306, 307
phase-lead filter, 306, 310 Taylor series, 45
PI control, 80, 306 trace of a matrix, 457
PID control, 3, 80, 81, 139, 448 tracking performance, 272, 273,
point mass, 11, 31 289, 309
pole locations, 93 transfer function, 56, 58, 247, 251
pole placement, 181 transfer function model, 158
poles, 57, 72 transfer function models, 3, 70
potential energy, 8, 31 transpose of a matrix, 456
potential energy of a spring, 31 Tustin approximation, 141
prefilter, 317 type 0 system, 279
proportional control, 80 type 1 system, 280
proportional gain, 80, 306 type 2 system, 281

rank of a matrix, 461 under damped, 99

rational transfer function, 100 upper companion form, 462

TOC
INDEX 492

upper triangular matrix, 462 z-transform, 140

Vandermonde matrix, 462 zeros, 57, 72, 100

TOC