Practical Guide to State-space Control

Graduate-level control theory for high schoolers

Tyler Veness
Practical Guide to State-space Control
Graduate-level control theory for high schoolers

Tyler Veness
Copyright © 2017 Tyler Veness
https://github.com/calcmogul/state-space-guide
Licensed under the Creative Commons Attribution-ShareAlike 4.0 International License (the “Li-
cense”). You may not use this file except in compliance with the License. You may obtain a copy
of the License at http://creativecommons.org/licenses/by-sa/4.0. Unless required by
applicable law or agreed to in writing, software distributed under the License is distributed on an
“ ” , , either express or implied. See the
License for the specific language governing permissions and limitations under the License.
Generated on December 8, 2018 from commit 7806744. The latest version can be downloaded from
https://file.tavsys.net/control/state-space-guide.pdf.
 Tree next to Oakes Circle at UCSC

Contents

Preface . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xi

0 Notes to the reader . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1

0.1 Prerequisites 1
0.2 The structure of this book 1
0.3 The ethos of this book 2
0.3.1 The role of classical control theory . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
0.3.2 An integrated approach to nonlinear control theory . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
0.4 The mindset of an egoless engineer 3
0.5 Request for feedback 3

1 Introduction to control theory . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

1.1 Nomenclature 5

I Kinematics and dynamics

2 Calculus . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
2.1 Derivatives 9
2.1.1 Power rule . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
2.1.2 Product rule . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
2.1.3 Chain rule . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
2.2 Integrals 10
2.2.1 Change of variables . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
2.3 Tables 11
2.3.1 Common derivatives and integrals . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

3 Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
3.1 Linear motion 13
3.2 Angular motion 13
3.3 Curvilinear motion 14
3.3.1 Two-wheeled vehicle kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14

4 Model examples . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
4.1 Pendulum 15
4.1.1 Force derivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
4.1.2 Torque derivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
4.1.3 Energy derivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18
4.2 DC brushed motor 19
4.2.1 Equations of motion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
4.2.2 Calculating constants . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
4.3 Elevator 21
4.3.1 Equations of motion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
4.4 Flywheel 23
4.4.1 Equations of motion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
4.5 Drivetrain 25
4.5.1 Equations of motion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25
4.6 Single-jointed arm 27
4.6.1 Equations of motion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27
4.7 Rotating claw 29
4.7.1 Equations of motion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29

II Classical control theory

5 Control system basics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33

5.1 What is gain? 33
5.2 Block diagrams 33
5.3 Why feedback control? 34

6 PID controllers . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35
6.1 The definition 35
6.1.1 Proportional gain . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35
6.1.2 Integral gain . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36
6.1.3 Derivative gain . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36
6.2 Response types 36
6.3 Types of PID controllers 37
6.4 General control theory perspective 38
6.5 Manual tuning 39
6.6 Limitations 39

7 Laplace domain analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41

7.1 The Fourier transform 41
7.2 The Laplace transform 43
7.2.1 The definition . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43
7.3 Transfer functions 45
7.3.1 Poles and zeroes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45
7.3.2 Nonminimum phase zeroes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46
7.3.3 Pole-zero cancellation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47
7.3.4 Transfer functions in feedback . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47
7.4 Root locus 48
7.5 Actuator saturation 50
7.6 Case studies of Laplace domain analysis 51
7.6.1 Steady-state error . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51
7.6.2 Flywheel PID control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53
7.7 Gain margin and phase margin 56

III Modern control theory

8 Linear algebra . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59
8.1 Vectors 59
8.1.1 What is a vector? . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59
8.1.2 Geometric interpretation of vectors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60
8.1.3 Vector addition . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61
8.1.4 Scalar-vector multiplication . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61
8.2 Linear combinations, span, and basis vectors 62
8.2.1 Basis vectors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62
8.2.2 Linear combination . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62
8.2.3 Span . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63
8.2.4 Linear dependence and independence . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64
8.3 Linear transformations and matrices 65
8.3.1 What is a linear transformation? . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65
8.3.2 Describing transformations numerically . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65
8.3.3 Examples of linear transformations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67
8.4 Matrix multiplication as composition 69
8.4.1 General matrix multiplication . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70
8.4.2 Matrix multiplication associativity . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71
8.5 The determinant 71
8.5.1 Scaling areas . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72
8.5.2 Exploring the determinant . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72
8.5.3 The determinant in 3D . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73
8.5.4 Computing the determinant . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 74
8.6 Inverse matrices, column space, and null space 74
8.6.1 Linear systems of equations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 74
8.6.2 Inverse . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 76
8.6.3 Rank and column space . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 77
8.6.4 Null space . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 78
8.6.5 Closing remarks . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 78
8.7 Nonsquare matrices as transformations between dimensions 78
8.8 Eigenvectors and eigenvalues 79
8.8.1 What is an eigenvector? . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 79
8.8.2 Eigenvectors in 3D rotation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 80
8.8.3 Finding eigenvalues . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 80
8.8.4 Transformations with no eigenvectors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 83
8.8.5 Repeated eigenvalues . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 83
8.8.6 Transformations with larger eigenvector spans . . . . . . . . . . . . . . . . . . . . . . . . . . . . 83
8.9 Miscellaneous notation 84

9 State-space controllers . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 85
9.1 From PID control to model-based control 85
9.2 What is a dynamical system? 86
9.3 State-space notation 86
9.3.1 What is state-space? . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 86
9.3.2 Benefits over classical control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 86
9.3.3 The equations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 87
9.4 Controllability 87
9.5 Observability 88
9.6 Closed-loop controller 89
9.7 Pole placement 90
9.8 LQR 90
9.8.1 Bryson’s rule . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 92
9.9 Case studies of controller design methods 92
9.10 Model augmentation 93
9.10.1 Plant augmentation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 93
9.10.2 Controller augmentation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 93
9.10.3 Observer augmentation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 93
9.10.4 Output augmentation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 94
9.10.5 Examples . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 94
9.11 Feedforwards 95
9.11.1 Steady-state feedforward . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 95
9.11.2 Two-state feedforward . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 98
9.11.3 Case studies of feedforwards . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 100
9.11.4 Feedforwards for unmodeled dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 100
9.12 Integral control 101
9.12.1 Plant augmentation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 102
9.12.2 U error estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 102

10 Digital control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 105

10.1 Phase loss 105
10.2 s-plane to z-plane 106
10.2.1 z-plane stability . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 106
10.2.2 z-plane behavior . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 106
10.2.3 Nyquist frequency . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 107
10.3 Discretization methods 108
10.4 Effects of discretization on controller performance 110
10.5 The matrix exponential 112
10.6 The Taylor series 113
10.7 Zero-order hold for state-space 114

11 State-space model examples . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 117

11.1 Pendulum 117
11.1.1 Write model in state-space representation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 117
11.1.2 Add estimator for unmeasured states . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 118
11.1.3 Implement controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 118
11.1.4 Simulate model/controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 118
11.1.5 Verify pole locations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 119
11.1.6 Unit test . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 119
11.1.7 Test on real system . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 119
11.2 Elevator 119
11.2.1 Continuous state-space model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 119
11.2.2 Model augmentation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 120
11.2.3 Simulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 120
11.2.4 Implementation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 120
11.3 Flywheel 122
11.3.1 Continuous state-space model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 122
11.3.2 Model augmentation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 122
11.3.3 Simulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 122
11.3.4 Implementation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 124
11.4 Drivetrain 124
11.4.1 Continuous state-space model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 124
11.4.2 Model augmentation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 125
11.4.3 Simulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 126
11.4.4 Implementation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 126
11.5 Single-jointed arm 127
11.5.1 Continuous state-space model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 127
11.5.2 Model augmentation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 127
11.5.3 Simulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 128
11.5.4 Implementation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 128
11.6 Rotating claw 130
11.6.1 Continuous state-space model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 130
11.6.2 Simulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 130

12 Nonlinear control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 131

12.1 Introduction 131
12.2 Linearization 132
12.3 Lyapunov stability 132
12.4 Control law for nonholonomic wheeled vehicle 132
12.4.1 Ramsete controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 133
12.4.2 Linear reference tracker . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 135
12.5 Further reading 138

IV Estimation and localization

13 State-space observers . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 141
13.1 Luenberger observer 141
13.1.1 Eigenvalues of closed-loop observer . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 143

14 Stochastic control theory . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 145

14.1 Terminology 145
14.2 Introduction to probability 145
14.2.1 Random variables . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 145
14.2.2 Expected value . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 146
14.2.3 Variance . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 147
14.2.4 Joint probability density functions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 147
14.2.5 Covariance . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 148
14.2.6 Correlation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 148
14.2.7 Independence . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 148
14.2.8 Marginal probability density functions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 149
14.2.9 Conditional probability density functions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 149
14.2.10 Bayes’s rule . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 149
14.2.11 Conditional expectation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 150
14.2.12 Conditional variances . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 150
14.2.13 Random vectors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 150
14.2.14 Covariance matrix . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 150
14.2.15 Relations for independent random vectors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 151
14.2.16 Gaussian random variables . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 152
14.3 Linear stochastic systems 152
14.3.1 State vector expectation evolution . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 153
14.3.2 State covariance matrix evolution . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 153
14.3.3 Measurement vector expectation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 154
14.3.4 Measurement covariance matrix . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 154
14.4 Two-sensor problem 154
14.5 Kalman filter 155
14.5.1 Derivations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 155
14.5.2 Predict and update equations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 158
14.5.3 Setup . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 159
14.5.4 Simulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 161
14.5.5 Kalman filter as Luenberger observer . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 161
14.6 Kalman smoother 163
14.7 MMAE 166
14.8 Nonlinear observers 166
14.8.1 Extended Kalman filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 166
14.8.2 Unscented Kalman filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 167
15 Pose estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 169
15.1 Nonlinear pose estimation 169

V Motion planning

16 Motion profiles . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 177

16.1 Jerk 178
16.2 Profile selection 178
16.3 Profile equations 179
16.4 Other profile types 179
16.5 Further reading 180

VI Appendices

A Simplifying block diagrams . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 183

A.1 Cascaded blocks 183
A.2 Combining blocks in parallel 183
A.3 Removing a block from a feedforward loop 184
A.4 Eliminating a feedback loop 184
A.5 Removing a block from a feedback loop 185

B Installing Python packages . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 187

B.1 Windows instructions 187
B.2 Linux instructions 187

C State-space canonical forms . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 189

C.1 Controllable canonical form 189
C.2 Observable canonical form 189

D Derivations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 191
D.1 Transfer function in feedback 191
D.2 Optimal control law 192
D.3 Zero-order hold for state-space 193
D.4 Kalman filter as Luenberger observer 195
D.4.1 Luenberger observer with separate prediction and update . . . . . . . . . . . . . . . . . . . 196
D.5 Trapezoidal motion profile 196
D.6 S-curve motion profile 196

Glossary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 198

Bibliography . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 199
Online 199
Miscellaneous 200

Index . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 201
 Flower bush near Spencer’s Market in Santa Maria, CA

Preface

Motivation
I am the software mentor for a FIRST Robotics Competition (FRC) team. My responsibilities for
that include teaching programming, software engineering practices, and applications of control theory.
The curriculum I developed as of the spring of 2017 (located at https://csweb.frc3512.com/
ci/) taught rookies enough to be minimally competitive, but it provided no formal avenues of growth
for veteran students.

Also, out of a six week build season, the software team usually only got a few days with the completed
robot due to poor build schedule management. This led to two problems. First, two days is only
enough time to verify basic software functionality, not test and tune feedback controllers. Second,
this was also the first time the robot’s electromechanical systems have been tested after integration, so
any issues that arose consumed valuable software integration time while the team traced the problem
to a mechanical, electrical, or software cause.

This book expands my curriculum to cover control theory topics I have learned in my graduate-level
engineering classes at University of California, Santa Cruz. It introduces state-space controllers and
serves as a practical guide for formulating and implementing them. Since state-space control utilizes a
system model, both problems mentioned earlier can be addressed. Basic software functionality can be
tested against it and feedback controllers can be tuned automatically based on system constraints. This
allows software teams to test their work much earlier in the build season in a controlled environment
as well as save time during feedback controller design, implementation, and testing.

I originally started writing this book because I felt that the topic wasn’t difficult, but the information for
it wasn’t easily accessible. When I was a high school robotics team member, I had to learn everything
from scratch through various internet sources and eventually from college courses as part of my
bachelor’s degree. Control theory has a certain beauty to it, and I want more people to appreciate
it the way I do. Therefore, my goal is to make the learning process quicker and easier for the team
members who come after me by collating all the relevant information.
xii

Intended audience
This guide is intended to make an advanced engineering topic approachable so it can be applied by
those who aren’t experts in control theory. My intended audience is high school students who are
members of a FIRST Robotics Competition team. As such, they will likely have passing knowledge
of PID control and have basic proficiency in programming. This guide will expand their incomplete
understanding of control theory to include the fundamentals of classical control theory, enough linear
algebra to understand the notation and mechanics of modern control, and finally how to apply modern
control to design challenges they regularly see in their FRC robots from year to year.

Acknowledgements
I would like to thank my controls engineering instructors Dejan Milutinović and Gabriel Elkaim of
University of California, Santa Cruz. They taught their classes from a pragmatic perspective focused
on application and intuition that I appreciated. I would also like to thank Dejan Milutinović for
introducing me to the field of control theory and both of my instructors for teaching me what it
means to be a controls engineer.
Thanks to Austin Schuh from FRC team 971 for providing the final continuous state-space models
used in the examples section.
 Trees by Baskin Engineering building at UCSC

0. Notes to the reader

0.1 Prerequisites
Knowledge of basic algebra and complex numbers is assumed. Some introductory physics and cal-
culus will be taught as necessary.

0.2 The structure of this book

This book consists of five parts and a collection of appendices that address the four tasks a controls
engineer carries out: derive a model of the system (kinematics), design a controller for the model
(control theory), design an observer to estimate the current state of the model (localization), and plan
how the controller is going to drive the model to a desired state (motion planning).
Part I, “Kinematics and dynamics,” introduces the basic calculus and physics concepts required to
derive the models used in the later chapters. It walks through the derivations for several common
FRC subsystems.
Part II, “Classical control theory,” introduces the basics of control theory, teaches the fundamentals
of PID controller design, describes what a transfer function is, and shows how they can be used to
analyze dynamical systems. Emphasis is placed on the geometric intuition of this analysis rather than
the frequency domain math.
Part III, “Modern control theory,” first provides a crash course in the geometric intuition behind
linear algebra and covers enough of the mechanics of evaluating matrix algebra for the reader to
follow along in later chapters. It covers state-space representation, controllability, and observability.
The intuition gained in part II and the notation of linear algebra are used to model and control linear
multiple-input, multiple-output (MIMO) systems and covers discretization, LQR controller design,
LQE observer design, and feedforwards. Then, these concepts are applied to design and implement
controllers for real systems. The examples from part I are converted to state-space representation,
implemented, and tested with a digital controller.
Part III also introduces the basics of nonlinear control system analysis with Lyapunov functions. It
 2 Chapter 0. Notes to the reader

presents an example of a nonlinear controller for a unicycle-like vehicle as well as how to apply it to
a two-wheeled vehicle. Since nonlinear control isn’t the focus of this book, we mention other books
and resources for further reading.

Part IV, “Estimation and localization,” introduces the field of stochastic control theory. The Luen-
berger observer and the probability theory behind the Kalman filter is taught with several examples
of creative applications of Kalman filter theory.

Part V, “Motion planning,” covers planning how the robot will get from its current state to some
desired state in a manner achievable by its dynamics. Motion profiles are introduced.

The appendices provide further enrichment that isn’t required for a passing understanding of the
material. This includes derivations for many of the results presented and used in the mainmatter of
the book.

The Python scripts used to generate the plots in the case studies double as reference implementa-
tions of the techniques discussed in their respective chapters. They are available in this book’s Git
repository. Its location is listed on the copyright page.

0.3 The ethos of this book

This book is intended as both a tutorial for new students and as a reference manual for more ex-
perienced readers who need to review a thing or two. While it isn’t comprehensive, the reader will
hopefully learn enough to either implement the concepts presented themselves or know where to look
for more information.

Some parts are mathematically rigorous, but I believe in giving students a solid theoretical foundation
with emphasis on intuition so they can apply it to new problems. To achieve deep understanding of
the topics in this book, math is unavoidable. With that said, I try to provide practical and intuitive
explanations whenever possible.

0.3.1 The role of classical control theory

The sections on classical control theory are only included to develop geometric intuition for the math-
ematical machinery of modern control theory. Many tools exclusive to classical control theory (root
locus, Bode plots, Nyquist plots, etc.) aren’t useful for or relevant to the examples presented, so they
serve only to complicate the learning process.

Classical control theory is interesting in that one can perform stability and robustness analyses and
design reasonable controllers for systems on the back of a napkin. It’s also useful for controlling
systems which don’t have a model. One can generate a Bode plot of a system by feeding in sine
waves of increasing frequency and recording the amplitude of the output oscillations. This data can
be used to create a transfer function or lead and lag compensators can be applied directly based on the
Bode plot. However, computing power is much more plentiful nowadays; we should take advantage
of this in the design phase and use the more modern tools that enables when it makes sense.

This book uses LQR and modern control over, say, loop shaping with Bode and Nyquist plots be-
cause we have accurate dynamical models to leverage, and LQR allows directly expressing what the
author is concerned with optimizing: state excursion relative to control effort. Applying lead and
lag compensators, while effective for robust controller design, doesn’t provide the same expressive
power.
 0.4 The mindset of an egoless engineer 3

0.3.2 An integrated approach to nonlinear control theory

Most teaching resources separate linear and nonlinear control with the latter being reserved for a
different course. Here, they are introduced together because the concepts of nonlinear control apply
often, and it isn’t that much of a leap (if Lyapunov stability isn’t included). The control and estimation
chapters cover relevant tools for dealing with nonlinearities like linearization when appropriate.

0.4 The mindset of an egoless engineer

The following maxim summarizes what I hope to teach my robotics students (with examples drawn
from controls engineering).
“Engineer based on requirements, not an ideology.”
Engineering is filled with trade-offs. The tools should fit the job, and not every problem is a nail
waiting to be struck by a hammer. Instead, assess the minimum requirements (min specs) for a
solution to the task at hand and do only enough work to satisfy them; exceeding your specifications is
a waste of time and money. If you require performance or maintainability above the min specs, your
min specs were chosen incorrectly by definition.
Controls engineering is pragmatic in a similar respect: solve. the. problem. For control of nonlinear
systems, plant inversion is elegant on paper but doesn’t work with an inaccurate model, yet using a
theoretically incorrect solution like linear approximations of the nonlinear system works well enough
to be used industry-wide. There are more sophisticated controllers than PID, but we use PID anyway
for its versatility and simplicity. Sometimes the inferior solutions are more effective or have a more
desirable cost-benefit ratio than what the control system designer considers ideal or clean. Choose
the tool that is most effective.
Solutions need to be good enough, but do not need to be perfect. We want to avoid integrators
as they introduce instability, but we use them anyway because they work well for meeting tracking
specifications. One should not blindly defend a design or follow an ideology, because there is always a
case where its antithesis is a better option. The engineer should be able to determine when this is the
case, set aside their ego, and do what will meet the specifications of their client (e.g., system response
characteristics, maintainability, usability). Preferring one solution over another for pragmatic or
technical reasons is fine, but the engineer should not care on a personal level which sufficient solution
is chosen.

0.5 Request for feedback

While we have tried to write a book that makes the topics of control theory approachable, it still may
be dense or fast-paced for some readers (it covers three classes of feedback control, two of which
are for graduate students, in one short book). Please send us feedback, corrections, or suggestions
through the GitHub link listed on the copyright page. New examples that demonstrate key concepts
and make them more accessible are also appreciated.
This page intentionally left blank
 Road near walking trail off of Rice Ranch Road in Santa Maria, CA

1. Introduction to control theory

Control systems are all around us and we interact with them daily. A small list of ones you may have
seen includes heaters and air conditioners with thermostats, cruise control and the anti-lock braking
system (ABS) on automobiles, and fan speed modulation on modern laptops. Control systems monitor
or control the behavior of systems like these and may consist of humans controlling them directly
(manual control), or of only machines (automatic control).
How can we prove closed-loop controllers on an autonomous car, for example, will behave safely
and meet the desired performance specifications in the presence of uncertainty? Control theory is an
application of algebra and geometry used to analyze and predict the behavior of systems, make them
respond how we want them to, and make them robust to disturbances and uncertainty.
Controls engineering is, put simply, the engineering process applied to control theory. As such,
it’s more than just applied math. While control theory has some beautiful math behind it, controls
engineering is an engineering discipline like any other that is filled with trade-offs. The solutions
control theory gives should always be sanity checked and informed by our performance specifications.
We don’t need to be perfect; we just need to be good enough to meet our specifications (see section
0.4 for more on engineering).

1.1 Nomenclature
Most resources for advanced engineering topics assume a level of knowledge well above that which is
necessary. Part of the problem is the use of jargon. While it efficiently communicates ideas to those
within the field, new people who aren’t familiar with it are lost. See the glossary for a list of words
and phrases commonly used in control theory, their origins, and their meaning. Links to the glossary
are provided for certain words throughout the book and will use this color.
The system or collection of actuators being controlled by a control system is called the plant. Con-
trollers which don’t include information measured from the plant’s output are called open-loop con-
trollers. Controllers which incorporate information fed back from the plant’s output are called closed-
loop controllers or feedback controllers.
6 Chapter 1. Introduction to control theory

Table 1.1 describes how the terms input and output apply to plants versus controllers and what letters
are commonly associated with each when working with them. Namely, that the terms input and
output are defined with respect to the plant, not the controller.

Plant Controller
Inputs u(t) r(t), y(t)
Outputs y(t) u(t)

Table 1.1: Plant versus controller nomenclature

I
Kinematics and dynamics

2 Calculus . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
2.1 Derivatives
2.2 Integrals
2.3 Tables

3 Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
3.1 Linear motion
3.2 Angular motion
3.3 Curvilinear motion

4 Model examples . . . . . . . . . . . . . . . . . . . . 15
4.1 Pendulum
4.2 DC brushed motor
4.3 Elevator
4.4 Flywheel
4.5 Drivetrain
4.6 Single-jointed arm
4.7 Rotating claw
This page intentionally left blank
 Hills by northbound freeway between Santa Maria and Ventura

2. Calculus

This book uses derivatives and integrals occasionally to represent small changes in values over small
changes in time and the infintessimal sum of values over time respectively. If you are interested in
more information after reading this chapter, 3Blue1Brown does a fantastic job of introducing them in
his Essence of calculus video series [12]. We recommend watching videos 1 through 3 and 7 through
11 from that playlist for a solid foundation. The Taylor series (presented in video 11) will be used in
chapter 10.

2.1 Derivatives
Derivatives are expressions for the slope of a curve at arbitrary points. Common notations for this
operation on a function like f (x) include

Leibniz notation Lagrange notation

d
dx f (x) f ′ (x)
d2
dx2
f (x) f ′′ (x)
d3
dx3
f (x) f ′′′ (x)
d4
dx4
f (x) f (4) (x)
dn
dxn f (x) f (n) (x)

Table 2.1: Notation for derivatives of f (x)

Lagrange notation is usually voiced as “f prime of x”, “f double-prime of x”, etc.

2.1.1 Power rule

f (x) = xn
 10 Chapter 2. Calculus

f ′ (x) = nxn−1

2.1.2 Product rule

This is for taking the derivative of the product of two expressions.

h(x) = f (x)g(x)
h′ (x) = f ′ (x)g(x) + f (x)g ′ (x)

2.1.3 Chain rule

This is for taking the derivative of nested expressions.

h(x) = f (g(x))
h′ (x) = f ′ (g(x)) · g ′ (x)

For example

h(x) = (3x + 2)5

h′ (x) = 5 (3x + 2)4 · (3)
h′ (x) = 15 (3x + 2)4

2.2 Integrals
The integral is the inverse operation of the derivative and calculates the area under a curve. Here is
an example of one based on table 2.2.

∫
eat dt
1 at
e +C
a

The arbitrary constant C is needed because when you take a derivative, constants are discarded be-
cause vertical offsets don’t affect the slope. When performing the inverse operation, we don’t have
enough information to determine the constant.
However, we can provide bounds for the integration.

∫ t
eat dt
0
( )t
1 at 
e + C 
a
( ) ( )0
1 at 1 a·0
e +C − e +C
a a
 2.3 Tables 11
( ) ( )
1 at 1
e +C − +C
a a
1 at 1
e +C − −C
a a
1 at 1
e −
a a

When we do this, the constant cancels out.

2.2.1 Change of variables

Change of variables substitutes an expression with a single variable to make the calculation more
straightforward. Here’s an example of integration which utilizes it.
∫
cos ωt dt

Let u = ωt.

du = ω dt
1
dt = du
ω

Now substitute the expressions for u and dt in.

∫
1
cos u du
ω
∫
1
cos u du
ω
1
sin u + C
ω
1
sin ωt + C
ω

2.3 Tables
2.3.1 Common derivatives and integrals
12 Chapter 2. Calculus

∫
f (x) dx f (x) f ′ (x)
ax a 0
1 2
2 ax ax a
1 a+1
a+1 x xa axa−1
1 ax
ae eax aeax
− cos(x) sin(x) cos(x)
sin(x) cos(x) − sin(x)
cos(x) − sin(x) − cos(x)
− sin(x) − cos(x) sin(x)

Table 2.2: Common derivatives and integrals

 Hills by northbound freeway between Santa Maria and Ventura

3. Dynamics

3.1 Linear motion

∑
F = ma

∑
where F is the sum of all forces applied to an object in Newtons, m is the mass of the object in
kg, and a is the net acceleration of the object in sm2 .

1
x(t) = x0 + v0 t + at2
2

where x(t) is an object’s position at time t, x0 is the initial position, v0 is the initial velocity, and a is
the acceleration.

3.2 Angular motion

∑
τ = Iα

∑
where τ is the sum of all torques applied to an object in Newton-meters, I is the moment of inertia
of the object in kg-m2 (also called the rotational mass), and α is the net angular acceleration of the
object in rad
s2
.

1
θ(t) = θ0 + ω0 t + αt2
2

where θ(t) is an object’s angle at time t, θ0 is the initial angle, ω0 is the initial angular velocity, and
α is the angular acceleration.
 14 Chapter 3. Dynamics

3.3 Curvilinear motion

3.3.1 Two-wheeled vehicle kinematics
The mapping from v and ω to the left and right wheel velocities vL and vR are derived as follows.
Let ⃗vC be the velocity vector of the center of rotation, ⃗vL be the velocity vector of the left wheel, ⃗vR
be the velocity vector of the right wheel, rb is the distance from the center of rotation to each wheel,
and ω is the counterclockwise turning rate around the center of rotation. For this derivation, we’ll
assume the robot is facing in the ĵ direction.
The main equation we’ll need is the following.

⃗vB = ⃗vA + ωA × ⃗rB|A

where ⃗vB is the velocity vector at point B, ⃗vA is the velocity vector at point A, ωA is the angular
velocity vector at point A, and ⃗rB|A is the distance vector from point A to point B (also described as
the “distance to B relative to A”).
First, we’ll derive vl .

⃗vl = vc ĵ + ω k̂ × −rb î
⃗vl = vc ĵ − ωrb ĵ
⃗vl = (vc − ωrb )ĵ
|⃗vl | = vc − ωrb
vl = vc − ωrb (3.1)

Next, we’ll derive vr .

⃗vr = vc ĵ + ω k̂ × rb î
⃗vr = vc ĵ + ωrb ĵ
⃗vr = (vc + ωrb )ĵ
|⃗vr | = vc + ωrb
vr = vc + ωrb (3.2)
 Hills by northbound freeway between Santa Maria and Ventura

4. Model examples

A model is a set of differential equations describing how the system behaves over time. There are
two common approaches for developing them.
1. Collecting data on the physical system’s behavior and performing system identification with it.
2. Using physics to derive the system’s model from first principles.
We’ll use the second approach in this book.
The models derived here should cover most types of motion seen on an FRC robot. Furthermore,
they can be easily tweaked to describe many types of mechanisms just by pattern-matching. There’s
only so many ways to hook up a mass to a motor in FRC. The flywheel model can be used for spinning
mechanisms, the elevator model can be used for spinning mechanisms transformed to linear motion,
and the single-jointed arm model can be used for rotating servo mechanisms (it’s just the flywheel
model augmented with a position state).
These models assume all motor controllers driving DC brushed motors are set to brake mode instead
of coast mode. Brake mode behaves the same as coast mode except where the applied voltage is zero.
In brake mode, the motor leads are shorted together to prevent movement. In coast mode, the motor
leads are an open circuit.

4.1 Pendulum
Kinematics and dynamics are a rather large topics, so for now, we’ll just focus on the basics required
for working with the models in this book. We’ll derive the same model, a pendulum, using three
approaches: sum of forces, sum of torques, and conservation of energy.

4.1.1 Force derivation

Consider figure 4.1a, which shows the forces acting on a pendulum.
Note that the path of the pendulum sweeps out an arc of a circle. The angle θ is measured in radians.
The blue arrow is the gravitational force acting on the bob, and the violet arrows are that same force
16 Chapter 4. Model examples

θ
θ
y0
y1 θ0
mg sin θ
mg cos θ h
θ
mg
(b) Trigonometry of a pendulum
(a) Force diagram of a pendulum

Figure 4.1: Pendulum force diagrams

resolved into components parallel and perpendicular to the bob’s instantaneous motion. The direction
of the bob’s instantaneous velocity always points along the red axis, which is considered the tangential
axis because its direction is always tangent to the circle. Consider Newton’s second law

F = ma

where F is the sum of forces on the object, m is mass, and a is the acceleration. Because we are only
concerned with changes in speed, and because the bob is forced to stay in a circular path, we apply
Newton’s equation to the tangential axis only. The short violet arrow represents the component of the
gravitational force in the tangential axis, and trigonometry can be used to determine its magnitude.
Therefore

−mg sin θ = ma
a = −g sin θ

where g is the acceleration due to gravity near the surface of the earth. The negative sign on the right
hand side implies that θ and a always point in opposite directions. This makes sense because when a
pendulum swings further to the left, we would expect it to accelerate back toward the right.
This linear acceleration a along the red axis can be related to the change in angle θ by the arc length
formulas; s is arc length and l is the length of the pendulum.

s = lθ (4.1)
ds dθ
v= =l
dt dt
d2 s d2 θ
a= 2 =l 2
dt dt

Therefore

d2 θ
l = −g sin θ
dt2
 4.1 Pendulum 17

d2 θ g
= − sin θ
dt2 l
g
θ̈ = − sin θ
l

4.1.2 Torque derivation

The equation can be obtained using two definitions for torque.

τ =r×F

First start by defining the torque on the pendulum bob using the force due to gravity.

τ = l × Fg

where l is the length vector of the pendulum and Fg is the force due to gravity.
For now just consider the magnitude of the torque on the pendulum.

|τ | = −mgl sin θ

where m is the mass of the pendulum, g is the acceleration due to gravity, l is the length of the
pendulum and θ is the angle between the length vector and the force due to gravity.
Next rewrite the angular momentum.

L = r × p = mr × (ω × r)

Again just consider the magnitude of the angular momentum.

|L| = mr2 ω
dθ
|L| = ml2
dt
d d2θ
|L| = ml2 2
dt dt

dL
According to τ = dt , we can just compare the magnitudes.

d2 θ
−mgl sin θ = ml2
dt2
g d2 θ
− sin θ = 2
l dt
g
θ̈ = − sin θ
l

which is the same result from force analysis.

 18 Chapter 4. Model examples

4.1.3 Energy derivation

The equation can also be obtained via the conservation of mechanical energy principle: any object
falling a vertical distance h would acquire kinetic energy equal to that which it lost to the fall. In other
words, gravitational potential energy is converted into kinetic energy. Change in potential energy is
given by

∆U = mgh

The change in kinetic energy (body started from rest) is given by

1
∆K = mv 2
2
Since no energy is lost, the gain in one must be equal to the loss in the other

1
mv 2 = mgh
2
The change in velocity for a given change in height can be expressed as

√
v= 2gh

dθ
Using equation (4.1), this equation can be rewritten in terms of dt .

dθ √
v=l = 2gh
dt
dθ 2gh
= (4.2)
dt l

where h is the vertical distance the pendulum fell. Look at figure 4.1b, which presents the trigonom-
etry of a pendulum. If the pendulum starts its swing from some initial angle θ0 , then y0 , the vertical
distance from the pivot point, is given by

y0 = l cos θ0

Similarly for y1 , we have

y1 = l cos θ

Then h is the difference of the two

h = l(cos θ − cos θ0 )

Substituting this into equation (4.2) gives

√
dθ 2g
= (cos θ − cos θ0 )
dt l
 4.2 DC brushed motor 19

This equation is known as the first integral of motion. It gives the velocity in terms of the location
and includes an integration constant related to the initial displacement (θ0 ). We can differentiate by
applying the chain rule with respect to time. Doing so gives the acceleration.

√
d dθ d 2g
= (cos θ − cos θ0 )
dt dt dt l
d2 θ 1 − 2gl sin θ dθ
2
= √
dt 2 2g (cos θ − cos θ ) dt
l 0
2g
√
2
d θ 1 − l sin θ 2g
2
= √ (cos θ − cos θ0 )
dt 2 2g (cos θ − cos θ ) l
l 0

d2 θ g
= − sin θ
dt2 l
g
θ̈ = − sin θ
l

which is the same result from force analysis.

4.2 DC brushed motor

We will be deriving a first-order model for a DC brushed motor. A second-order model would include
the inductance of the motor windings as well, but we’re assuming the time constant of the inductor
is small enough that its affect on the model behavior is negligible for FRC use cases (see section 7.4
for a demonstration of this for a real DC brushed motor).
The first-order model will only require numbers from the motor’s datasheet. The second-order model
would require measuring the motor inductance as well, which generally isn’t in the datasheet. It can
be difficult to measure accurately without the right equipment.

4.2.1 Equations of motion

The circuit for a DC brushed motor is shown in figure 4.2.

V
ωm
Vemf = Kv

Figure 4.2: DC brushed motor circuit

V is the voltage applied to the motor, I is the current through the motor in Amps, R is the resistance
across the motor in Ohms, ωm is the angular velocity of the motor in radians per second, and Kv
is the angular velocity constant in radians per second per Volt. This circuit reflects the following
relation.
 20 Chapter 4. Model examples

Figure 4.3: Example motor datasheet for 775pro

ωm
V = IR + (4.3)
Kv

The mechanical relation for a DC brushed motor is

τm = Kt I (4.4)

where τm is the torque produced by the motor in Newton-meters and Kt is the torque constant in
Newton-meters per Amp. Therefore

τm
I=
Kt

Substitute this into equation (4.3).

τm ωm
V = R+ (4.5)
Kt Kv

4.2.2 Calculating constants

A typical motor’s datasheet should include graphs of the motor’s measured torque and current for
different angular velocities for a given voltage applied to the motor. Figure 4.3 is an example. Data
for the most common motors in FRC can be found at https://motors.vex.com.
To find Kt

τm = Kt I
τm
Kt =
I
 4.3 Elevator 21
τm,stall
Kt = (4.6)
Istall

where τm,stall is the stall torque and Istall is the stall current of the motor from its datasheet.
To find R, recall equation (4.3).

ωm
V = IR +
Kv

When the motor is stalled, ωm = 0.

V = Istall R
V
R= (4.7)
Istall

where Istall is the stall current of the motor and V is the voltage applied to the motor at stall.
To find Kv , again recall equation (4.3).

ωm
V = IR +
Kv
ωm
V − IR =
Kv
ωm
Kv =
V − IR

When the motor is spinning under no load

ωm,f ree
Kv = (4.8)
V − If ree R

where ωm,f ree is the angular velocity of the motor under no load (also known as the free speed), and
V is the voltage applied to the motor when it’s spinning at ωm,f ree , and If ree is the current drawn by
the motor under no load.
If several identical motors are being used in one gearbox for a mechanism, multiply the stall torque
by the number of motors.

4.3 Elevator
4.3.1 Equations of motion
This elevator consists of a DC brushed motor attached to a pulley that drives a mass up or down.
Gear ratios are written as output over input, so G is greater than one in figure 4.4.
Based on figure 4.4

τm G = τp (4.9)
22 Chapter 4. Model examples

R ωp

I ωm

Vemf
m vm
V G
r

circuit mechanics

Figure 4.4: Elevator system diagram

where G is the gear ratio between the motor and the pulley and τp is the torque produced by the
pulley.

rFm = τp (4.10)

where r is the radius of the pulley. Substitute equation (4.9) into equation (4.5).

τp
G ωm
V = R+
Kt Kv
τp ωm
V = R+
GKt Kv

Substitute in equation (4.10).

rFm ωm
V = R+ (4.11)
GKt Kv

The angular velocity of the motor armature ωm is

ωm = Gωp (4.12)

where ωp is the angular velocity of the pulley. The velocity of the mass (the elevator carriage) is

vm = rωp

vm
ωp = (4.13)
r
Substitute equation (4.13) into equation (4.12).

vm
ωm = G (4.14)
r
Substitute equation (4.14) into equation (4.11).
 4.4 Flywheel 23

rFm G vrm
V = R+
GKt Kv
RrFm G
V = + vm
GKt rKv

Solve for Fm .

RrFm G
=V − vm
GKt rKv
( )
G GKt
Fm = V − vm
rKv Rr
GKt 2
G Kt
Fm = V − vm (4.15)
Rr Rr2 Kv

∑
F = mam (4.16)

∑
where F is the sum of forces applied to the elevator carriage, m is the mass of the elevator carriage
in kilograms, and am is the acceleration of the elevator carriage.

mam = Fm

R Gravity is not part of the modeled dynamics because it complicates the state-space model and
the controller will behave well enough without it.

( )
GKt G2 K t
mam = V − vm
Rr Rr2 Kv
GKt G2 Kt
am = V − vm (4.17)
Rrm Rr2 mKv

4.4 Flywheel
4.4.1 Equations of motion
This flywheel consists of a DC brushed motor attached to a spinning mass of non-negligible moment
of inertia.
Gear ratios are written as output over input, so G is greater than one in figure 4.5.

We will start with the equation derived earlier for a DC brushed motor, equation (4.5).

τm ωm
V = R+
Kt Kv
24 Chapter 4. Model examples

R ωf

I ωm J

Vemf
V G

circuit mechanics

Figure 4.5: Flywheel system diagram

Solve for the angular acceleration. First, we’ll rearrange the terms because from inspection, V is the
model input, ωm is the state, and τm contains the angular acceleration.

R 1
V = τm + ωm
Kt Kv

Solve for τm .

R 1
V = τm + ωm
Kt Kv
R 1
τm =V − ωm
Kt Kv
Kt Kt
τm = V − ωm
R Kv R

Since τm G = τf and ωm = Gωf

(τ ) Kt Kt
f
= V − (Gωf )
G R Kv R
τf Kt GKt
= V − ωf
G R Kv R
GKt G2 Kt
τf = V − ωf (4.18)
R Kv R

The torque applied to the flywheel is defined as

τf = J ω̇f (4.19)

where J is the moment of inertia of the flywheel and ω̇f is the angular acceleration. Substitute
equation (4.19) into equation (4.18).

GKt G2 Kt
(J ω̇f ) = V − ωf
R Kv R
 4.5 Drivetrain 25

GKt G2 Kt
ω̇f = V − ωf (4.20)
RJ Kv RJ

4.5 Drivetrain
4.5.1 Equations of motion
This drivetrain consists of two DC brushed motors per side which are chained together on their
respective sides and drive wheels which are assumed to be massless.
ωl ωr

J
r
rb

Figure 4.6: Drivetrain system diagram

From equation (4.18) of the flywheel model derivations

GKt G2 Kt
τ= V − ω (4.21)
R Kv R

where τ is the torque applied by one wheel of the drivetrain, G is the gear ratio of the drivetrain, Kt
is the torque constant of the motor, R is the resistance of the motor, and Kv is the angular velocity
constant. Since τ = rF and ω = vr where v is the velocity of a given drivetrain side along the ground
and r is the drivetrain wheel radius

GKt G2 Kt ( v )
(rF ) = V −
R Kv R r
GKt G2 Kt
rF = V − v
R Kv Rr
GKt G2 Kt
F = V − v
Rr Kv Rr2
G2 Kt GKt
F =− v+ V
Kv Rr2 Rr

Therefore, for each side of the robot

G2l Kt Gl Kt
Fl = − vl + Vl
Kv Rr2 Rr
G2 Kt Gr Kt
Fr = − r 2 vr + Vr
Kv Rr Rr

where the l and r subscripts denote the side of the robot to which each variable corresponds.
G2 K 2
Let C1 = − KvlRrt2 , C2 = Gl Kt
Rr , C3 = − KGvrRr
Kt
2 , and C4 =
Gr Kt
Rr .

Fl = C1 vl + C2 Vl (4.22)
26 Chapter 4. Model examples

Fr = C3 vr + C4 Vr (4.23)

First, find the sum of forces.

∑
F = ma
Fl + Fr = mv̇
v̇l + v̇r
Fl + Fr = m
2
2
(Fl + Fr ) = v̇l + v̇r
m
2
v̇l = (Fl + Fr ) − v̇r (4.24)
m

Next, find the sum of torques.

∑
τ = J ω̇
( )
v̇r − v̇l
τl + τr = J
2rb

where rb is the radius of the drivetrain.

v̇r − v̇l
(−rb Fl ) + (rb Fr ) = J
2rb
J
−rb Fl + rb Fr = (v̇r − v̇l )
2rb
J
−Fl + Fr = 2 (v̇r − v̇l )
2rb
2rb2
(−Fl + Fr ) = v̇r − v̇l
J
2r2
v̇r = v̇l + b (−Fl + Fr )
J

Substitute in equation (4.24) to obtain an expression for v̇r .

( )
2 2r2
v̇r = (Fl + Fr ) − v̇r + b (−Fl + Fr )
m J
2r 2
2
2v̇r = (Fl + Fr ) + b (−Fl + Fr )
m J
1 rb2
v̇r = (Fl + Fr ) + (−Fl + Fr ) (4.25)
m J
1 1 r2 r2
v̇r = Fl + Fr − b Fl + b Fr
m
( m) J( J )
1 rb2 1 r2
v̇r = − Fl + + b Fr (4.26)
m J m J
 4.6 Single-jointed arm 27

Substitute equation (4.25) back into equation (4.24) to obtain an expression for v̇l .

( )
2 1 rb2
v̇l = (Fl + Fr ) − (Fl + Fr ) + (−Fl + Fr )
m m J
r 2
1
v̇l = (Fl + Fr ) − b (−Fl + Fr )
m J
1 rb2
v̇l = (Fl + Fr ) + (Fl − Fr )
m J
1 1 r2 r2
v̇l = Fl + Fr + b Fl − b Fr
m
( m) J( J )
1 rb2 1 r2
v̇l = + Fl + − b Fr (4.27)
m J m J

Now, plug the expressions for Fl and Fr into equation (4.26).

( ) ( )
1 r2 1 r2
v̇r = − b Fl + + b Fr
m J m J
( 2 ) ( )
1 rb 1 rb2
v̇r = − (C1 vl + C2 Vl ) + + (C3 vr + C4 Vr ) (4.28)
m J m J

Now, plug the expressions for Fl and Fr into equation (4.27).

( ) ( )
1 rb2 1 rb2
v̇l = + Fl + − Fr
m J m J
( ) ( )
1 r2 1 r2
v̇l = + b (C1 vl + C2 Vl ) + − b (C3 vr + C4 Vr ) (4.29)
m J m J

4.6 Single-jointed arm

4.6.1 Equations of motion
This single-jointed arm consists of a DC brushed motor attached to a pulley that spins a straight bar
in pitch.
Gear ratios are written as output over input, so G is greater than one in figure 4.7.
We will start with the equation derived earlier for a DC brushed motor, equation (4.5).

τm ωm
V = R+
Kt Kv

Solve for the angular acceleration. First, we’ll rearrange the terms because from inspection, V is the
model input, ωm is the state, and τm contains the angular acceleration.

R 1
V = τm + ωm
Kt Kv

Solve for τm .
28 Chapter 4. Model examples

m
R ωarm
l
I ωm

Vemf
V G

circuit mechanics

Figure 4.7: Single-jointed arm system diagram

R 1
V = τm + ωm
Kt Kv
R 1
τm =V − ωm
Kt Kv
Kt Kt
τm = V − ωm
R Kv R

Since τm G = τarm and ωm = Gωarm

(τ ) Kt Kt
arm
= V − (Gωf )
G R Kv R
τarm Kt GKt
= V − ωarm
G R Kv R
GKt G2 Kt
τarm = V − ωarm (4.30)
R Kv R

The angular velocity of the arm is defined as

τarm = J ω̇arm (4.31)

where J is the moment of inertia of the arm and ω̇arm is the angular acceleration. Substitute equation
(4.31) into equation (4.30).

GKt G2 Kt
(J ω̇arm ) = V − ωarm
R Kv R
GKt G2 Kt
ω̇arm = V − ωarm (4.32)
RJ Kv RJ

J can be approximated as the moment of inertia of a thin rod rotating around one end. Therefore
 4.7 Rotating claw 29

1
J = ml2 (4.33)
3

where m is the mass of the arm and l is the length of the arm.

4.7 Rotating claw

4.7.1 Equations of motion
This claw consists of independent upper and lower jaw pieces each driven by its own DC brushed
motor.
This page intentionally left blank
II
Classical control theory

5 Control system basics . . . . . . . . . . . . . . . 33

5.1 What is gain?
5.2 Block diagrams
5.3 Why feedback control?

6 PID controllers . . . . . . . . . . . . . . . . . . . . . . 35
6.1 The definition
6.2 Response types
6.3 Types of PID controllers
6.4 General control theory perspective
6.5 Manual tuning
6.6 Limitations

7 Laplace domain analysis . . . . . . . . . . . . 41

7.1 The Fourier transform
7.2 The Laplace transform
7.3 Transfer functions
7.4 Root locus
7.5 Actuator saturation
7.6 Case studies of Laplace domain analysis
7.7 Gain margin and phase margin
This page intentionally left blank
 On trail between McHenry Library and Media Theater at UCSC

5. Control system basics

5.1 What is gain?

Gain is a proportional value that shows the relationship between the magnitude of an input signal to
the magnitude of an output signal at steady-state. Many systems contain a method by which the gain
can be altered, providing more or less “power” to the system.

Figure 5.1 shows a system with a hypothetical input and output. Since the output is twice the ampli-
tude of the input, the system has a gain of 2.

Input System Output

t K t

Figure 5.1: Demonstration of system with a gain of K = 2

5.2 Block diagrams

When designing or analyzing a control system, it is useful to model it graphically. Block diagrams
are used for this purpose. They can be manipulated and simplified systematically (see appendix A).
Figure 5.2 is an example of one.

The open-loop gain is the total gain from the sum node at the input to the output branch. The feedback
gain is the total gain from the output back to the input sum node. The circle’s output is the sum of
its inputs.

Figure 5.3 is a block diagram with more formal notation in a feedback configuration.
 34 Chapter 5. Control system basics

+
input open-loop output
∓

feedback

Figure 5.2: Block diagram with nomenclature

+
X(s) P1 Y (s)
∓

P2

Figure 5.3: Feedback block diagram

Theorem 5.2.1 — Closed-loop gain for a feedback loop.

Y (s) P1
= (5.1)
X(s) 1 ∓ P1 P2

See appendix D.1 for a derivation.

5.3 Why feedback control?

Let’s say we are controlling a DC brushed motor. With just a mathematical model and knowledge
of all current states of the system (i.e., angular velocity), we can predict all future states given the
future voltage inputs. Why then do we need feedback control? If the system is disturbed in any way
that isn’t modeled by our equations, like a load was applied to the armature, or voltage sag in the
rest of the circuit caused the commanded voltage to not match the actual applied voltage, the angular
velocity of the motor will deviate from the model over time.
To combat this, we can take measurements of the system and the environment to detect this deviation
and account for it. For example, we could measure the current position and estimate an angular
velocity from it. We can then give the motor corrective commands as well as steer our model back
to reality. This feedback allows us to account for uncertainty and be robust to it.
 Treeline by Crown/Merril bus stop at UCSC

6. PID controllers

6.1 The definition

Negative feedback loops drive the difference between the reference and output to zero. Figure 6.1
shows a block diagram for a system controlled by a PID controller.

Kp e(t)

+ e(t) ∫t + u(t) y(t)

r(t) Ki e(τ ) dτ Plant
− 0 + +

Kd de(t)
dt

Figure 6.1: PID controller diagram

r(t) reference u(t) control input
e(t) error y(t) output

PID control has three gains acting on the error.

6.1.1 Proportional gain

The Proportional gain compensates for the current error. This gain acts like a “software-defined
spring”. Recall from physics that we model springs as F = −kx where F is the force applied, k is
a proportional constant, and x is the displacement from the equilibrium point (0 in this case). For
P control of a DC motor toward zero, F is proportional to the voltage applied, k is the proportional
constant from the P term, and x is the measured position. In other words, the “force” with which the
proportional gain pulls the system’s output toward the reference is proportional to the error.
 36 Chapter 6. PID controllers

6.1.2 Integral gain

The Integral gain compensates for past error (i.e., steady-state error). It integrates the error over time
and adds the current total times Ki to the control input. When the system is close to the reference in
steady-state, the proportional term is too small to pull the output to the reference and the derivative
term is zero. The integral gain is commonly used to address this problem.
There are better approaches to fix steady-state error like using feedforwards or constraining when the
integral control acts using other knowledge of the system. We will discuss these in more detail when
we get to modern control theory.

6.1.3 Derivative gain

The Derivative gain compensates for future error by slowing the controller down if the error is chang-
ing over time. This gain acts like a “software-defined damper”. These are commonly seen on door
closers, and their damping force increases linearly with velocity.

6.2 Response types

A system driven by a PID controller generally has three types of responses: underdamped, over-
damped, and critically damped. These are shown in figure 6.2.
For the step responses in figure 6.2, rise time is the time the system takes to initially reach the reference
after applying the step input. Settling time is the time the system takes to settle at the reference after
the step input is applied. An underdamped response oscillates around the reference before settling.
An overdamped response is slow to rise and does not overshoot the reference. A critically damped
response has the fastest rise time without overshooting the reference.
 6.3 Types of PID controllers 37

Figure 6.2: PID controller response types

6.3 Types of PID controllers

PID controller inputs that are different orders of derivatives, such as position and velocity, affect the
system response differently. Below is the standard form for a position PID controller.
Definition 6.3.1 — Position PID controller.
∫ t
de
u(t) = Kp e(t) + Ki e(τ ) dτ + Kd (6.1)
0 dt

where e(t) is the position error at the current time t, τ is the integration variable, Kp is the
proportional gain, Ki is the integral gain, and Kd is the derivative gain.

The integral integrates from time 0 to the current time t. We use τ for the integration because we
need a variable to take on multiple values throughout the integral, but we can’t use t because we
already defined that as the current time.
If the position controller formulation is measuring and controlling velocity instead, the error e(t)
becomes dedt . Substituting this into equation (6.1) yields

∫ t
de de d2 e
u(t) = Kp + Ki dτ + Kd 2
dt 0 dτ dt
de 2
d e
u(t) = Kp + Ki e(t) + Kd 2 (6.2)
dt dt

So the velocity de
dt is integrated by the integral term. By the fundamental theorem of calculus, the
derivative and integral cancel because they are inverse operations. This produces just the error e(t),
so the Ki term has the effect of proportional control. Furthermore, the Kp term in equation (6.2)
behaves like a derivative term for velocity PID control due to the de de
dt . Letting ev = dt , this means
the velocity controller is analogous to theorem 6.3.1.
 38 Chapter 6. PID controllers

Note this isn’t strictly true because the fundamental theorem of calculus requires that e(t) is continu-
ous, but when we actually implement the controller, e(t) is discrete. Therefore, the result here is only
correct up to the accuracy of the iterative integration method used. We’ll discuss approximations like
these in section 10.3 and how they affect controller behavior.
Theorem 6.3.1 — Velocity PID controller.
∫ t
u(t) = Kp ev (t) + Ki ev (τ ) dτ (6.3)
0

where ev (t) is the velocity error at the current time t, τ is the integration variable, Kp is now the
derivative gain, and Ki is now the proportional gain.

Integral control for the velocity is analogous to the throttle pedal on a car. One must hold the throttle
pedal (the control input) at a nonzero value to keep the car traveling at the reference velocity.

R Note: For a real velocity control implementation, a steady-state feedforward and proportional
control are preferred to integral control.

Read https://en.wikipedia.org/wiki/PID_controller for more information on PID con-

trol theory.

6.4 General control theory perspective

PID control defines setpoint as the desired position and process variable as the measured position.
Control theory has more general terms for these: reference and output respectively.
The derivative term is commonly used to “slow down” the system if it’s already heading toward the
reference. We will explore what Kp and Kd are really doing for a two-state system (position and
velocity) and why Kd acts that way.
First, we will rearrange the equation for a PD controller.

ek − ek−1
uk = Kp ek + Kd
dt

where uk is the control input at timestep k and ek is the error at timestep k. ek is defined as ek =
rk − xk where rk is the reference and xk is the current state at timestep k.

(rk − xk ) − (rk−1 − xk−1 )

uk = Kp (rk − xk ) + Kd
dt
rk − xk − rk−1 + xk−1
uk = Kp (rk − xk ) + Kd
dt
rk − rk−1 − xk + xk−1
uk = Kp (rk − xk ) + Kd
dt
(rk − rk−1 ) − (xk − xk−1 )
uk = Kp (rk − xk ) + Kd
( dt )
rk − rk−1 xk − xk−1
uk = Kp (rk − xk ) + Kd −
dt dt
 6.5 Manual tuning 39
r −r x −x
Notice how k dtk−1 is the velocity of the reference. By the same reasoning, k dtk−1 is the system’s
velocity at a given timestep. That means the Kd term of the PD controller is driving the estimated
velocity to the reference velocity. If the reference is constant, that means the Kd term is trying to
drive the velocity of the system to zero.
However, Kp and Kd are controlling the same actuator, and their effects conflict with each other;
Kp is trying to make the system move while Kd is trying to stop it. If Kp is larger than Kd , one is
in effect slowing down the response of the controller during transients with the hope of decreasing
overshoot and settling time. If one makes Kd much larger than Kp , Kd overpowers Kp to bring the
system to a stop. However, when the velocity is low enough, Kp is stronger and starts accelerating
the system again. This oscillatory behavior in the velocity repeats as the system moves toward the
reference.

6.5 Manual tuning

These steps apply for both position and velocity PID controllers.
1. Set Kp , Ki , and Kd to zero.
2. Increase Kp until the output starts to oscillate around the reference.
3. Increase Kd as much as possible without introducing jittering in the system response.
If the controller settles at an output above or below the reference, increase Ki such that the con-
troller reaches the reference in a reasonable amount of time. However, a steady-state feedforward is
preferred to integral control (especially for velocity PID control).

R Note: Adding an integral gain to the controller is an incorrect way to eliminate steady-state
error. A better approach would be to tune it with an integrator added to the plant, but this
requires a model. Since we are doing output-based rather than model-based control, our only
option is to add an integrator to the controller.

Beware that if Ki is too large, integral windup can occur. Following a large change in reference, the
integral term can accumulate an error larger than the maximal control input. As a result, the system
overshoots and continues to increase until this accumulated error is unwound.

6.6 Limitations
PID’s heuristic method of tuning is a reasonable choice when there is no a priori knowledge of the
system dynamics. However, controllers with much better response can be developed if a dynamical
model of the system is known. Furthermore, PID only applies to single-input, single-output (SISO)
systems and can drive up to two states to references by using both the proportional and integral terms.
We’ll revisit this in subsection 7.6.2.
This page intentionally left blank
 Grass clearing by Interdisciplinary Sciences building and Thimann Labs at UCSC

7. Laplace domain analysis

This chapter briefly discusses what transfer functions are, how the locations of poles and zeroes affects
system response and stability, and how controllers affect pole locations. The case studies cover various
aspects of PID control using the algebraic approach of transfer functions.

7.1 The Fourier transform

The Fourier transform decomposes a function of time into its component frequencies. Each of these
frequencies is part of what’s called a basis. These basis waveforms can be multiplied by their respec-
tive contribution amount and summed to produce the original signal (this weighted sum is called a
linear combination). In other words, the Fourier transform provides a way for us to determine, given
some signal, what frequencies can we add together and in what amounts to produce the original signal.
Think of an Fmajor4 chord which has the notes F4 (349.23 Hz), A4 (440 Hz), and C4 (261.63 Hz).
The waveform over time looks like figure 7.1.
Notice how this complex waveform can be represented just by three frequencies. They show up as
Dirac delta functions1 in the frequency domain with the area underneath them equal to their contri-
bution (see figure 7.2).
Since Euler’s identity states that eiθ = cos θ + i sin θ, we can represent frequencies as complex num-
bers on a number line (we will use ejω = cos ω + j sin ω for notational consistency going forward).
For example, 400 Hz would be ej400 . The frequency domain just uses the complex exponent 400j.

1
The Dirac delta function is zero everywhere except at the origin. The nonzero region has an infinitesimal width and
has a height such that the area within that region is 1.
42 Chapter 7. Laplace domain analysis

Figure 7.1: Frequency decomposition of Fmajor4 chord

Figure 7.2: Fourier transform of Fmajor4 chord

 7.2 The Laplace transform 43

7.2 The Laplace transform

The Laplace domain is a generalization of the frequency domain that has the frequency (jω) on the
imaginary y-axis and a real number on the x-axis, yielding a two-dimensional coordinate system. We
represent coordinates in this space as a complex number s = σ + jω. The real part σ cooresponds
to the x-axis and the imaginary part jω cooresponds to the y-axis (see figure 7.3).

Im(jω)

Re(σ)

Figure 7.3: Laplace domain

To extend our analogy of each coordinate being represented by some basis, we now have the y coor-
dinate representing the oscillation frequency of the system response (the frequency domain) and also
the x coordinate representing the speed at which that oscillation decays and the system converges to
zero (i.e., a decaying exponential). Figure 7.7 shows this for various points.
If we move the component frequencies in the Fmajor4 chord example parallel to the real axis to
σ = −25, the resulting time domain response attenuates according to the decaying exponential
e−25t (see figure 7.4).
Note that this explanation as a basis isn’t exact because the Laplace basis isn’t orthogonal (that is,
the x and y coordinates affect each other and have cross-talk). In the frequency domain, we had a
basis of sine waves that we represented as delta functions in the frequency domain. Each frequency
contribution was independent of the others. In the Laplace domain, this is not the case; a pure
1
exponential is s−a (a rational function where a is a real number) instead of a delta function. This
function is nonzero at points that aren’t actually frequencies present in the time domain. Figure 7.5
demonstrates this, which shows the Laplace transform of the Fmajor4 chord plotted in 3D.
Notice how the values of the function around each component frequency decrease according to
√ 1 in the x and y directions (in just the x direction, it would be x1 ).
2 2
x +y

7.2.1 The definition

The Laplace transform of a function f (t) is defined as

∫ ∞
L{f (t)} = F (s) = f (t)e−st dt
0

We won’t be computing any Laplace transforms by hand using this formula (everyone in the real world
looks these up in a table anyway). Common Laplace transforms (assuming zero initial conditions)
44 Chapter 7. Laplace domain analysis

Figure 7.4: Fmajor4 chord at σ = 0 and σ = −25

Figure 7.5: Laplace transform of Fmajor4 chord plotted in 3D

 7.3 Transfer functions 45

are shown in table 7.1. Of particular note are the Laplace transforms for the derivative, unit step2 ,
and exponential decay. We can see that a derivative is equivalent to multiplying by s, and an integral
is equivalent to multiplying by 1s . We’ll discuss the decaying exponential in subsection 7.3.1.

Time domain Laplace domain

Linearity a f (t) + b g(t) a F (s) + b G(s)
Convolution (f ∗ g)(t) F (s) G(s)
Derivative f ′ (t) s F (s)
nth derivative f (n) (t) sn F (s)
1
Unit step u(t) s
1
Ramp t u(t) s2

Exponential decay e−αt u(t) 1

s+α

Table 7.1: Common Laplace transforms and Laplace transform properties with zero initial conditions

7.3 Transfer functions

A transfer function maps an input coordinate to an output coordinate in the Laplace domain. These
can be obtained by applying the Laplace transform to a differential equation and rearranging the
terms to obtain a ratio of the output variable to the input variable. Equation (7.1) is an example of a
transfer function.

zeroes
z }| {
(s − 9 + 9i)(s − 9 − 9i)
H(s) = (7.1)
s(s + 10)
| {z }
poles

7.3.1 Poles and zeroes

The roots of factors in the numerator of a transfer function are called zeroes because they make
the transfer function approach zero. Likewise, the roots of factors in the denominator of a transfer
function are called poles because they make the transfer function approach infinity; on a 3D graph,
these look like the poles of a circus tent (see figure 7.6).
When the factors of the denominator are broken apart using partial fraction expansion into something
A B
like s+a + s+b , the constants A and B are called residues, which determine how much each pole
contributes to the system response.
You may notice that the factors representing poles look a lot like the Laplace transform for a decaying
exponential. This is why the time domain responses of systems are typically decaying exponentials.
One differential equation which can produce this kind of response is ẋ = ax where ẋ is the derivative
of x and a < 0.

R Imaginary poles and zeroes always come in complex conjugate pairs (e.g., −2 + 3i, −2 − 3i).

2
The unit step u(t) is defined as 0 for t < 0 and 1 for t ≥ 0.
 46 Chapter 7. Laplace domain analysis

Figure 7.6: Equation 7.1 plotted in 3D

The locations of the closed-loop poles in the complex plane determine the stability of the system.
Each pole represents a frequency mode of the system, and their location determines how much of
each response is induced for a given input frequency. Figure 7.7 shows the impulse responses in the
time domain for transfer functions with various pole locations. They all have an initial condition of
1.
Im(jω)

Stable Unstable

t
t
t

LHP RHP
t

t t

Re(σ)

t t t

Figure 7.7: Impulse response vs pole location

When a system is stable, its output may oscillate but it converges to steady-state. When a system is
marginally stable, its output oscillates at a constant amplitude forever. When a system is unstable, its
output grows without bound.

7.3.2 Nonminimum phase zeroes

While poles in the RHP are unstable, the same is not true for zeroes. They can be characterized by
the system initially moving in the wrong direction before heading toward the reference. Since the
 7.3 Transfer functions 47

Location Stability
Left Half-plane (LHP) Stable
Imaginary axis Marginally stable
Right Half-plane (RHP) Unstable

Table 7.2: Pole location and stability

poles always move toward the zeroes, zeroes impose a “speed limit” on the system response because
it takes a finite amount of time to move the wrong direction, then change directions.
One example of this type of system is bicycle steering. Try riding a bicycle without holding the
handle bars, then poke the right handle; the bicycle turns right. Furthermore, if one is holding the
handlebars and wants to turn left, rotating the handlebars counterclockwise will make the bicycle fall
toward the right. The rider has to lean into the turn and overpower the nonminimum phase dymamics
to go the desired direction.
Another example is a segway. To move forward by some distance, the segway must first roll backward
to rotate the segway forward. Once the segway starts falling in that direction, it begins rolling forward
to avoid falling over until it reaches the target distance. At that point, the segway increases its forward
speed to pitch backward and slow itself down. To come to a stop, the segway rolls backward again
to level itself out.

7.3.3 Pole-zero cancellation

Pole-zero cancellation occurs when a pole and zero are located at the same place in the s-plane. This
effectively eliminates the contribution of each to the system dynamics. By placing poles and zeroes
at various locations (this is done by placing transfer functions in series), we can eliminate undesired
system dynamics. While this may appear to be a useful design tool at first, there are major caveats.
Most of these are due to model uncertainty resulting in poles which aren’t in the locations the controls
designer expected.
Notch filters are typically used to dampen a specific range of frequencies in the system response. If
its band is made too narrow, it can still leave the undesirable dynamics, but now you can no longer
measure them in the response. They are still happening, but they are what’s called unobservable.
Never pole-zero cancel unstable or nonminimum phase dynamics. If the model doesn’t quite reflect
reality, an attempted pole cancellation by placing a nonminimum phase zero results in the pole still
moving to the zero placed next to it. You have the same dynamics as before, but the pole is also stuck
where it is no matter how much feedback gain is applied. For an attempted nonminimum phase zero
cancellation, you have effectively placed an unstable pole that’s unobservable. This means the system
will be going unstable and blowing up, but you won’t be able to detect this and react to it.
Keep in mind when making design decisions that the model likely isn’t perfect. The whole point of
feedback control is to be robust to this kind of uncertainty.

7.3.4 Transfer functions in feedback

For controllers to regulate a system or track a reference, they must be placed in positive or negative
feedback with the plant (whether to use positive or negative depends on the plant in question). Stable
feedback loops attempt to make the output equal the reference.
The transfer function of figure 7.8, a control system diagram with feedback, from input to output is
 48 Chapter 7. Laplace domain analysis

+
X(s) K G Y (s)
−

Figure 7.8: Feedback controller block diagram

X(s) input H measurement transfer function
K controller gain Y (s) output
G plant transfer function

Y (s) KG
Gcl (s) = = (7.2)
X(s) 1 + KGH

The numerator is the open-loop gain and the denominator is one plus the gain around the feedback
loop, which may include parts of the open-loop gain (see appendix D.1 for a derivation). As another
example, the transfer function from the input to the error is

E(s) 1
Gcl (s) = = (7.3)
X(s) 1 + KGH

The roots of the denominator of Gcl (s) are different from those of the open-loop transfer function
KG(s). These are called the closed-loop poles.

7.4 Root locus

In closed-loop, the poles can be moved around by adjusting the controller gain, but the zeroes stay
put. The root locus shows where the poles will go as the gain for a P controller is increased and tells
us for what range of gains the controller will be stable. As the controller gain is increased, poles
can move toward negative infinity (figure 7.9), move toward each other then split toward asymptotes
(figure 7.10), or move toward zeroes (figure 7.11). The system in figure 7.11 becomes unstable as
the gain is increased.
We won’t be using root locus plots for any of our control systems analysis later, but it does help
provide an intuition for what controllers actually do to a system.
If poles are much farther left in the LHP than the typical system dynamics exhibit, they can be con-
sidered negligible. Every system has some form of unmodeled high frequency, nonlinear dynamics,
but they can be safely ignored depending on the operating regime.
To demonstrate this, consider the transfer function for a second-order DC brushed motor from voltage
to position

K
G(s) =
s((Js + b)(Ls + R) + K 2 )

where J = 3.2284 × 10−6 kg-m2 , b = 3.5077 × 10−6 N -m-s, Ke = Kt = 0.0274 V /rad/s,

R = 4 Ω, and L = 2.75 × 10−6 H.
7.4 Root locus 49

Figure 7.9: Root locus showing pole moving to- Figure 7.10: Root locus showing poles moving
ward negative infinity toward asymptotes

Figure 7.11: Root locus of equation (7.1) show-

ing poles moving toward zeroes.

This plant has the root locus shown in figure 7.12. In proportional feedback, the plant is unstable for
large values of K. However, if we remove the unstable pole by setting L in the transfer function to
zero, we get the root locus in figure 7.13. For small values of K, both systems are stable and have
nearly indistinguishable step responses due to the exceedingly small contribution from the fast pole
(see figures 7.14 and 7.15). The high frequency dynamics only cause instability for large values of K
that induce fast system responses. In other words, the system responses of the second-order model
and its first-order approximation are similar for low frequency operating regimes.
Why can’t unstable poles close to the origin be ignored in the same way? The response of high
frequency stable poles decays rapidly. Unstable poles, on the other hand, represent unstable dynamics
which cause the system output to grow to infinity. Regardless of how slow these unstable dynamics
are, they will eventually dominate the response.
 50 Chapter 7. Laplace domain analysis

Figure 7.12: Root locus of second-order DC Figure 7.13: Root locus of first-order DC
brushed motor plant brushed motor plant

Figure 7.14: Step response of second-order DC Figure 7.15: Step response of first-order DC
brushed motor plant brushed motor plant

7.5 Actuator saturation

Recall that a controller calculates its output based on the error between the reference and the current
state. Plant in the real world don’t have unlimited control authority available for the controller to
apply. When the actuator limits are reached, the controller acts as if the gain has been temporarily
reduced.
We’ll try to explain this through a bit of math. Let’s say we have a controller u = k(r − x) where
u is the control effort, k is the gain, r is the reference, and x is the current state. Let umax be the
limit of the actuator’s output which is less than the uncapped value of u and kmax be the associated
maximum gain. We will now compare the capped and uncapped controllers for the same reference
and current state.

umax < u
kmax (r − x) < k(r − x)
kmax < k

For the inequality to hold, kmax must be less than the original value for k. This reduced gain is
evident in a system response when there is a linear change in state instead of an exponential one as it
 7.6 Case studies of Laplace domain analysis 51

approaches the reference. This is due to the control effort no longer following a decaying exponential
plot. Once the system is closer to the reference, the controller will stop saturating and produce realistic
controller values again.

7.6 Case studies of Laplace domain analysis

We’ll be using equation (7.4), the transfer function for a PID controller, in the case studies.

Ki
K(s) = Kp + + Kd s (7.4)
s

Remember, multiplication by 1s corresponds to an integral in the Laplace domain and multiplication

by s corresponds to a derivative.

7.6.1 Steady-state error

To demonstrate the problem of steady-state error, we will use a DC brushed motor controlled by a
velocity PID controller. A DC brushed motor has a transfer function from voltage (V ) to angular
velocity (θ̇) of

Θ̇(s) K
G(s) = = (7.5)
V (s) (Js + b)(Ls + R) + K 2

First, we’ll try controlling it with a P controller defined as

K(s) = Kp

When these are in unity feedback, the transfer function from the input voltage to the error is

E(s) 1
=
V (s) 1 + K(s)G(s)
1
E(s) = V (s)
1 + K(s)G(s)
1
E(s) = ( ) V (s)
K
1 + (Kp ) (Js+b)(Ls+R)+K 2

1
E(s) = Kp K
V (s)
1+ (Js+b)(Ls+R)+K 2

The steady-state of a transfer function can be found via

lim sH(s) (7.6)

s→0

since steady-state has an input freuency of zero.

ess = lim sE(s)

s→0
52 Chapter 7. Laplace domain analysis
1
ess = lim s Kp K
V (s)
s→0 1+ (Js+b)(Ls+R)+K 2
1 1
ess = lim s Kp K
s→0 1+ s
(Js+b)(Ls+R)+K 2
1
ess = lim Kp K
s→0 1+ (Js+b)(Ls+R)+K 2
1
ess = Kp K
1+ (J(0)+b)(L(0)+R)+K 2
1
ess = Kp K
(7.7)
1+ bR+K 2

Notice that the steady-state error is nonzero. To fix this, an integrator must be included in the con-
troller.

Ki
K(s) = Kp +
s

The same steady-state calculations are performed as before with the new controller.

E(s) 1
=
V (s) 1 + K(s)G(s)
1
E(s) = V (s)
1 + K(s)G(s)
( )
1 1
E(s) = ( )( )
1 + Kp + Ksi K s
(Js+b)(Ls+R)+K 2
( )
1 1
ess = lim s ( ) ( )
s→0
1 + Kp + sKi K s
(Js+b)(Ls+R)+K 2
1
ess = lim ( )( )
s→0 Ki K
1 + Kp + s (Js+b)(Ls+R)+K 2
1 s
ess = lim ( )( )
s→0
1 + Kp + Ki K s
s (Js+b)(Ls+R)+K 2
s
ess = lim ( )
s→0 K
s + (Kp s + Ki ) (Js+b)(Ls+R)+K 2
0
ess = ( )
K
0 + (Kp (0) + Ki ) (J(0)+b)(L(0)+R)+K 2
0
ess = K
Ki bR+K 2

The denominator is nonzero, so ess = 0. Therefore, an integrator is required to eliminate steady-state

error in all cases for this model.
 7.6 Case studies of Laplace domain analysis 53

It should be noted that ess in equation (7.7) approaches zero for Kp = ∞. This is known as a bang-
bang controller. In practice, an infinite switching frequency cannot be achieved, but it may be close
enough for some performance specifications.

7.6.2 Flywheel PID control

PID controllers typically control voltage to a motor in FRC independent of the equations of motion of
that motor. For position PID control, large values of Kp can lead to overshoot and Kd is commonly
used to reduce overshoots. Let’s consider a flywheel controlled with a standard PID controller. Why
wouldn’t Kd provide damping for velocity overshoots in this case?
PID control is designed to control second-order and first-order systems well. It can be used to control
a lot of things, but struggles when given higher order systems. It has three degrees of freedom. Two
are used to place the two poles of the system, and the third is used to remove steady-state error. With
higher order systems like a one input, seven state system, there aren’t enough degrees of freedom to
place the system’s poles in desired locations. This will result in poor control.
The math for PID doesn’t assume voltage, a motor, etc. It defines an output based on derivatives
and integrals of its input. We happen to use it for motors because it actually works pretty well for it
because motors are second-order systems.
The following math will be in continuous time, but the same ideas apply to discrete time. This is all
assuming a velocity controller.
Our simple motor model hooked up to a mass is

ω
V = IR + (7.8)
Kv
τ = IKt (7.9)
dω
τ =J (7.10)
dt

For an explanation of where these equations come from, read section 4.2.
dω
First, we’ll solve for dt in terms of V .
Substitute equation (7.9) into equation (7.8).

ω
V = IR +
Kv
( )
τ ω
V = R+
Kt Kv

Substitute in equation (7.10).

( )
J dω
dt ω
V = R+
Kt Kv

dω
Solve for dt .
54 Chapter 7. Laplace domain analysis

J dω
dt ω
V = R+
Kt Kv
ω J dω
V − = dt R
Kv Kt
( )
dω Kt ω
= V −
dt JR Kv
dω Kt Kt
=− ω+ V
dt JRKv JR

Now take the Laplace transform.

Kt Kt
sω = − ω+ V (7.11)
JRKv JR
ω
Solve for the transfer function H(s) = V.

Kt Kt
sω = − ω+ V
JRKv JR
( )
Kt Kt
s+ ω= V
JRKv JR
Kt
ω JR
= Kt
V s + JRK v

That gives us a pole at − JRK

Kt
v
, which is actually stable. Notice that there is only one pole.
First, we’ll use a simple P loop.

V = Kp (ωgoal − ω)

Substitute this controller into equation (7.11).

Kt Kt
sω = − ω+ Kp (ωgoal − ω)
JRKv JR
ω
Solve for the transfer function H(s) = ωgoal .

Kt Kt Kp
sω = −
ω+ (ωgoal − ω)
JRKv JR
Kt Kt Kp Kt Kp
sω = − ω+ ωgoal − ω
JRKv JR JR
( )
Kt Kt Kp Kt Kp
s+ + ω= ωgoal
JRKv JR JR
7.6 Case studies of Laplace domain analysis 55
Kt Kp
ω
=( JR )
ωgoal s+ Kt
+
Kt Kp
JRKv JR

( )
Kt Kp
This has a pole at − Kt
JRKv + JR . Assuming that that quantity is negative (i.e., we are stable),
1
that pole corresponds to a time constant of Kt Kt Kp .
JRKv
+ JR

As can be seen above, a flywheel has a single pole. It therefore only needs a single pole controller to
place all of its poles anywhere.

R This analysis assumes that the motor is well coupled to the mass and that the time constant of
the inductor is small enough that it doesn’t factor into the motor equations. In Austin Schuh’s
experience with 971’s robots, these are pretty good assumptions.

Next, we’ll try a PD loop. (This will use a perfect derivative, but anyone following along closely
already knows that we can’t really take a derivative here, so the math will need to be updated at some
point. We could switch to discrete time and pick a differentiation method, or pick some other way
of modeling the derivative.)

V = Kp (ωgoal − ω) + Kd s(ωgoal − ω)

Substitute this controller into equation (7.11).

Kt Kt
sω = − ω+ (Kp (ωgoal − ω) + Kd s(ωgoal − ω))
JRKv JR
Kt Kt Kp Kt Kd s
sω = − ω+ (ωgoal − ω) + (ωgoal − ω)
JRKv JR JR
Kt Kt Kp Kt Kp Kt Kd s Kt Kd s
sω = − ω+ ωgoal − ω+ ωgoal − ω
JRKv JR JR JR JR

Collect the common terms on separate sides and refactor.

Kt Kd s Kt Kt Kp Kt Kp Kt Kd s
sω + ω+ ω+ ω= ωgoal + ωgoal
JR JRKv JR JR JR
( ( ) )
Kt Kd Kt Kt Kp Kt
s 1+ + + ω= (Kp + Kd s) ωgoal
JR JRKv JR JR
Kt
ω JR (K p + Kd s)
=( ( ) )
ωgoal s 1+ Kt Kd
+ Kt
+
Kt Kp
JR JRKv JR

Kt Kt Kp
K +
So, we added a zero at − Kdp and moved our pole to − JRKvKt KJR
d
. This isn’t progress. We’ve added
1+ JR
more complexity to our system and, practically speaking, gotten nothing good out of it. Zeroes should
 56 Chapter 7. Laplace domain analysis

be avoided if at all possible because they amplify unwanted high frequency modes of the system. At
least this is a stable zero, but it’s still undesirable.
In summary, derivative doesn’t help on a flywheel. Kd may help if the real system isn’t ideal, but we
don’t suggest relying on that.

7.7 Gain margin and phase margin

One generally needs to learn about Bode plots and Nyquist plots to truly understand gain and phase
margin and their origins, but those plots are large topics unto themselves. Since we won’t be using
either of these plots for controller design, we’ll just cover what gain and phase margin are in a general
sense and how they are used. We’ll be discussing how discretization affects phase margin in section
10.1.
Gain margin and phase margin are two metrics for measuring a system’s relative stability. Gain and
phase margin are the amounts by which the closed-loop gain and phase can be varied respectively be-
fore the system becomes unstable. In a sense, they are safety margins for when unmodeled dynamics
affect the system response.
For a more thorough explanation of gain and phase margin, watch Brian Douglas’s video on them
[14]. He has other videos too on classical control methods like Bode and Nyquist plots that we
recommend.
III
Modern control theory

8 Linear algebra . . . . . . . . . . . . . . . . . . . . . . 59
8.1 Vectors
8.2 Linear combinations, span, and basis vectors
8.3 Linear transformations and matrices
8.4 Matrix multiplication as composition
8.5 The determinant
8.6 Inverse matrices, column space, and null space
8.7 Nonsquare matrices as transformations between dimen-
sions
8.8 Eigenvectors and eigenvalues
8.9 Miscellaneous notation

9 State-space controllers . . . . . . . . . . . . . . 85
9.1 From PID control to model-based control
9.2 What is a dynamical system?
9.3 State-space notation
9.4 Controllability
9.5 Observability
9.6 Closed-loop controller
9.7 Pole placement
9.8 LQR
9.9 Case studies of controller design methods
9.10 Model augmentation
9.11 Feedforwards
9.12 Integral control

10 Digital control . . . . . . . . . . . . . . . . . . . . . 105

10.1 Phase loss
10.2 s-plane to z-plane
10.3 Discretization methods
10.4 Effects of discretization on controller performance
10.5 The matrix exponential
10.6 The Taylor series
10.7 Zero-order hold for state-space

11 State-space model examples . . . . . . . 117

11.1 Pendulum
11.2 Elevator
11.3 Flywheel
11.4 Drivetrain
11.5 Single-jointed arm
11.6 Rotating claw

12 Nonlinear control . . . . . . . . . . . . . . . . . . 131

12.1 Introduction
12.2 Linearization
12.3 Lyapunov stability
12.4 Control law for nonholonomic wheeled vehicle
12.5 Further reading
This page intentionally left blank
 Hills by northbound freeway between Santa Maria and Ventura

8. Linear algebra

Modern control theory borrows concepts from linear algebra. At first, linear algebra may appear very
abstract, but there are simple geometric intuitions underlying it. First, watch 3Blue1Brown’s preview
video for the Essence of linear algebra video series (5 minutes) [4]. The goal here is to provide an
intuitive, geometric understanding of linear algebra as a method of linear transformations.

While only a subset of the material from the videos will be presented here that we think is relevant
to this book, we highly suggest watching the whole series [3].

R The following sections are essentially transcripts of the video content for those who don’t want
to watch an hour of YouTube videos. However, we suggest watching the videos instead because
animations are better at conveying the geometric intuition involved than text.

8.1 Vectors
8.1.1 What is a vector?
The fundamental building block for linear algebra is the vector. Broadly speaking, there are three
distinct but related ideas about vectors: the physics student perspective, the computer science student
perspective, and the mathmetician’s perspective.

The physics student perspective is that vectors are arrows pointing in space. A given vector is defined
by its length and direction, but as long as those two facts are the same, you can move it around and
it’s still the same vector. Vectors in the flat plane are two-dimensional, and those sitting in broader
space that we live in are three-dimensional.

The computer science perspective is that vectors are ordered lists of numbers. For example, let’s
say you were doing some analytics about house prices and the only features you cared about where
square footage and price. You might model each house with a pair of numbers where the first indicates
square footage and the second indicates price. Notice the order matters here. In the lingo, you’d be
 60 Chapter 8. Linear algebra

modeling houses as two-dimensional vectors where, in this context, vector is a synonym for list, and
what makes it two-dimensional is that the length of that list is two.
The mathematician, on the other hand, seeks to generalize both these views by saying that a vector
can be anything where there’s a sensible notion of adding two vectors and multiplying a vector by a
number (operations that we’ll talk about later on). The details of this view are rather abstract and
won’t be needed for this book as we’ll favor a more concrete setting. We bring it up here because it
hints at the fact that the ideas of vector addition and multiplication by numbers will play an important
role throughout linear algebra.

8.1.2 Geometric interpretation of vectors

Before we talk about vector addition and multiplication by numbers, let’s settle on a specific thought
to have in mind when we say the word “vector”. Given the geometric focus that we’re intending here,
whenever we introduce a new topic involving vectors, we want you to first think about an arrow, and
specifically, think about that arrow inside a coordinate system, like the x-y plane with its tail sitting
at the origin. This is slightly different from the physics student perspective where vectors can freely
sit anywhere they want in space. In linear algebra, it’s almost always the case that your vector will
be rooted at the origin. Then, once you understand a new concept in the concept of arrows in space,
we’ll translate it over to the “list of numbers” point of view, which we can do by considering the
coordinates of the vector.
While you may already be familiar with this coordinate system, it’s worth walking through explicitly
since this is where all of the important back-and-forth happens between the two perspectives of
linear algebra. Focusing your attention on two dimensions for the moment, you have a horizontal line
called the x-axis and a vertical line called the y-axis. The point at which they intersect is called the
origin, which you should think of as the center of space and the root of all vectors. After choosing
an arbitrary length to represent one, you make tick marks on each axis to represent this distance.
The coordinates of a vector is a pair of numbers that essentially gives instructions for getting from
the tail of that vector at the origin to its tip. The first number is how far to move along the x-axis
(positive numbers indicating rightward motion and negative numbers indicating leftward motion), and
the second number is how far to move parallel to the y-axis after that (positive numbers indicating
upward motion and negative numbers indicating downward motion). To distinguish vectors from
points, the convention is to write this pair of numbers vertically with square brackets around them.
For example:

[ ]
3
−1

Every pair of numbers represents one and only one vector, and every vector is associated with one
and only one pair of numbers. In three dimensions, there is a third axis called the z-axis which is
perpendicular to both the x and y axes. In this case, each vector is associated with an ordered triplet
of numbers. The first is how far to move along the x-axis, the second is how far to move parallel to
the y-axis, and the third is how far to then move parallel to this new z-axis. For example:

 
2
1
3

Every triplet of numbers represents one unique vector in space, and every vector in space represents
exactly one triplet of numbers.
 8.1 Vectors 61

8.1.3 Vector addition

Back to vector addition and multiplication by numbers. Afterall, every topic in linear algebra is going
to center around these two operations. Luckily, each one is straightforward to define. Let’s say we
have two vectors, one pointing up and a little to the right, and the other one pointing right and down
a bit. To add these two vectors, move the second one so that its tail sits at the tip of the first one.
Then, if you draw a new vector from the tail of the first one to where the tip of the second one now
sits, that new vector is their sum.
This definition of addition, by the way, is one of the only times in linear algebra where we let vectors
stray away from the origin, but why is this a reasonable thing to do? Why this definition of addition
and not some other one? Each vector represents a sort of movement, a step with a certain distance
and direction in space. If you take a step along the first vector, then take a step in the direction and
distance described by the second vector, the overall effect is just the same as if you moved along the
sum of those two vectors to start with.
You could think about this as an extension of how we think about adding numbers on a number line.
One way that we teach students to think about this, say with 2 + 5, is to think of moving two steps to
the right, followed by another 5 steps to the right. The overall effect is the same as if you just took 7
steps to the right. In fact, let’s see how vector addition looks numerically. The first vector here has
coordinates (1, 2) and the second has coordinates (3, −1).

[ ] [ ]
1 3
+
2 −1

When you take the vector sum using this tip-to-tail method, you can think of a four-step path from
the origin to the tip of the second vector: “walk 1 to the right, then 2 up, then 3 to the right, then 1
down.” Reorganizing these steps so that you first do all of the rightward motion, then do all of the
vertical motion, you can read it as saying, “first move 1 + 3 to the right, then move 2 + (−1) up,” so
the new vector has coordinates 1 + 3 and 2 + (−1).

[ ] [ ] [ ]
1 3 1+3
+ =
2 −1 2 + (−1)

In general, vector addition in this list-of-numbers conception looks like matching up their terms, and
adding each one together.

[ ] [ ] [ ]
x1 x2 x1 + x2
+ =
y1 y2 y1 + y2

8.1.4 Scalar-vector multiplication

The other fundamental vector operation is multiplication by a number. Now this is best understood
just by looking at a few examples. If you take the number 2, and multiply it by a given vector, you
stretch out that vector so that it’s two times as long as when you started. If you multiply that vector by,
say, 31 , you compress it down so that it’s 13 of the original length. When you multiply it by a negative
number, like −1.8, then the vector is first flipped around, then stretched out by that factor of 1.8.
This process of stretching, compressing, or reversing the direction of a vector is called “scaling”, and
whenver a number like 2 or 13 or −1.8 acting like this–scaling some vector–we call it a “scalar”. In
fact, throughout linear algebra, one of the main things numbers do is scale vectors, so it’s common to
 62 Chapter 8. Linear algebra

use the word “scalar” interchangeably with the word “number”. Numerically, stretching out a vector
by a factor of, say, 2, corresponds to multiplying each of its components by that factor, 2.

[ ] [ ]
3 6
2· =
1 2

So in the conception of vectors as lists of numbers, multiplying a given vector by a scalar means
multiplying each one of those components by that scalar.

[ ] [ ]
x 2x
2· =
y 2y

R See the corresponding Essence of linear algebra video for a more visual presentation (5 minutes)
[11].

8.2 Linear combinations, span, and basis vectors

Vector coordinates were probably already familiar to you, but there’s another interesting way to think
about these coordinates which is central to linear algebra. When given a pair of numbers that’s meant
to describe a vector, like (3, 2), we want you to think about each coordinate as a scalar, meaning,
think about how each one stretches or compresses vectors.

8.2.1 Basis vectors

In the xy-coordinate system, there are two special vectors: the one pointing to the right with length
1, commonly called “i-hat”, or the unit vector in the x-direction (î), and the one pointing straight up,
with length 1, commonly called “j-hat”, or the unit vector in the y-direction (ĵ).
Now think of the x-coordinate of our vector as a scalar that scales î, stretching it by a factor of 3, and
the y-coordinate as a scalar that scales ĵ, flipping it and stretching it by a factor of 2. In this sense,
the vectors that these coordinates describe is the sum of two scaled vectors (3)î + (−2)ĵ. This idea
of adding together two scaled vectors is a surprisingly important concept. Those two vectors, î and
ĵ, have a special name, by the way. Together they’re called the basis of a coordinate system (î and
ĵ are the “basis vectors” of the xy-coordinate system). When you think about coordinates as scalars,
the basis vectors are what those scalars actually scale.
By framing our coordinate system in terms of these two special basis vectors, it raises an interesting
and subtle point: we could have chosen different basis vectors and had a completely reasonable, new
coordinate system system. For example, take some vector pointing up and to the right, along with
some other vector pointing down and to the right, in some way. Take a moment to think about all the
different vectors that you can get by choosing two scalars, using each one to scale one of the vectors,
then adding together what you get. Which two-dimensional vectors can you reach by altering the
choices of scalars? The answer is that you can reach every possible two-dimensional vector. A new
pair of basis vectors like this still gives us a valid way to go back and forth between pairs of numbers
and two-dimensional vectors, but the association is definitely different from the one that you get using
the more standard basis of î and ĵ.

8.2.2 Linear combination

Any time we describe vectors numerically, it depends on an implicit choice of what basis vectors
we’re using. So any time that you’re scaling two vectors and adding them like this, it’s called a linear
 8.2 Linear combinations, span, and basis vectors 63

combination of those two vectors. Below is a linear combination of vectors ⃗v and w

⃗ with scalars a
and b.

a⃗v + bw
⃗

Where does this word “linear” come from? Why does this have anything to do with lines? This isn’t
the etymology, but if you fix one of those scalars and let the other one change its value freely, the tip
of the resulting vector draws a straight line.

8.2.3 Span
Now, if you let both scalars range freely and consider every possible resultant vector, there are three
things that can happen. For most pairs of vectors, you’ll be able to reach every possible point in the
plane; every two-dimensional vector is within your grasp. However, in the unlucky case where your
two original vectors happen to line up, the tip of the resulting vector is limited to just a single line
passing through the origin. The vectors could also both be zero, in which case the resultant vector is
just at the origin.
The set of all possible vectors that you can reach with a linear combination of a given pair of vectors
is called the span of those two vectors. So, restating what we just discussed in this lingo, the span of
most pairs of 2D vectors is all vectors in 2D space, but when they line up, their span is all vectors
whose tip sits on a certain line.
Remember how we said that linear algebra revolves around vector addition and scalar multiplication?
The span of two vectors is a way of asking, “What are all the possible vectors one can reach using
only these two fundamental operations, vector addition and scalar multiplication?”
Thinking about a whole collection of vectors sitting on a line gets crowded, and even more so to
think about all two-dimensional vectors at once filling up the plane. When dealing with collections
of vectors like this, it’s common to represent each one with just a point in space where the tip of the
vector was. This way, if you want to think about every possible vector whose tip sits on a certain line,
just think about the line itself.
Likewise, to think about all possible two-dimensional vectors at once, conceptualize each one as the
point where its tip sits. In effect, you’re thinking about the infinite, flat sheet of two-dimensional
space itself, leaving the arrows out of it.
In general, if you’re thinking about a vector on its own, think of it as an arrow, and if you’re dealing
with a collection of vectors, it’s convenient to think of them all as points. Therefore, for our span
example, the span of most pairs of vectors ends up being the entire infinite sheet of two-dimensional
space, but if they line up, their span is just a line.
The idea of span gets more interesting if we start thinking about vectors in three-dimensional space.
For example, given two vectors in 3D space that are not pointing in the same direction, what does it
mean to take their span? Their span is the collection of all possible linear combinations of those two
vectors, meaning all possible vectors you get by scaling each vector in some way, then adding them
together.
You can imagine turning two different knobs to change the two scalars defining the linear combination,
adding the scaled vectors and following the tip of the resulting vector. That tip will trace out a flat
sheet cutting through the origin of three-dimensional space. This flat sheet is the span of the two
vectors. More precisely, the span of the two vectors is the set of all possible vectors whose tips sit on
that flat sheet.
 64 Chapter 8. Linear algebra

So what happens if we add a third vector and consider the span of all three? A linear combination of
three vectors is defined similarly as it is for two; you’ll choose three different scalars, scale each of
those vectors, then add them all together. The linear combination of ⃗v , w,
⃗ and ⃗u looks like

a⃗v + bw
⃗ + c⃗u

where a, b, and c are allowed to vary. Again, the span of these vectors is the set of all possible linear
combinations.

Two different things could happen here. If your third vector happens to be sitting on the span of
the first two, then the span doesn’t change; you’re trapped on that same flat sheet. In other words,
adding a scaled version of that third vector to the linear combination doesn’t give you access to any
new vectors. However, if you just randomly choose a third vector, it’s almost certainly not sitting
on the span of those first two. Then, since it’s pointing in a separate direction, it unlocks access to
every possible three-dimensional vector. As you scale that new third vector, it moves around that
span sheet of the first two, sweeping it through all of space.

Another way to think about it is that you’re making full use of the three, freely-changing scalars that
you have at your disposal to acces the full three dimensions of space.

8.2.4 Linear dependence and independence

In the case where the third vector was already sitting on the span of the first two, or the case where
two vectors happen to line up, we want some terminology to describe the fact that at least one of
these vectors is redundant–not adding anything to our span. When there are multiple vectors and one
could be removed without reducing the span, the relevant terminology is to say that they are linearly
dependent.

In other words, one of the vectors can be expressed as a linear combination of the others since it’s
already in the span of the others.

⃗ for some values of a and b

⃗u = a⃗v + bw

On the other hand, if each vector really does add another dimension to the span, they’re said to be
linearly independent.

⃗ ̸= a⃗v for all values of a

w

Now with all that terminology, and hopefully some good mental images to go with it, the technical
definition of a basis of a space is as follows.
Definition 8.2.1 — Basis of a vector space. The basis of a vector space is a set of linearly
independent vectors that span the full space.

R See the corresponding Essence of linear algebra video for a more visual presentation (10 min-
utes) [6].
 8.3 Linear transformations and matrices 65

8.3 Linear transformations and matrices

This section focuses on what linear transformations look like in the case of two dimensions and how
they relate to the idea of a matrix-vector multiplication.

[ ][ ] [ ]
1 −3 5 (1)(5) + (−3)(7)
=
2 4 7 (2)(5) + (4)(7)

In particular, we want to show you a way to think about matrix-vector multiplication that doesn’t rely
on memorization of the procedure shown above.

8.3.1 What is a linear transformation?

To start, let’s just parse this term “linear transformation”. “Transformation” is essentially another
name for “function”. It’s something that takes in inputs and returns an output for each one. Specifi-
cally in the context of linear algebra, we consider transformations that take in some vector and spit
out another vector.
[ ] [ ]
5 2
L(⃗v )
7 −3

Vector input Vector output

So why use the word “transformation” instead of “function” if they mean the same thing? It’s to be
suggestive of a certain way to visualize this input-output relation. You see, a great way to understand
functions of vectors is to use movement. If a transformation takes some input vector to some out-
put vector, we imagine that input vector moving over to the output vector. Then to understand the
transformation as a whole, we might imagine watching every possible input vector move over to its
corresponding output vector. it gets really crowded to think about all the vectors all at once, where
each one is an arrow. Therefore, as we mentioned in the previous section, it’s useful to conceptualize
each vector as a single point where its tip sits rather than an arrow. To think about a transformation
taking every possible input vector to some output vector, we watch every point in space moving to
some other point.

The effect of various transformations moving around all of the points in space gives the feeling of
compressing and morphing space itself. As you can imagine though, arbitrary transformations can
look complicated. Luckily, linear algebra limits itself to a special type of transformation, ones that
are easier to understand, called “linear” transformations. Visually speaking, a transformation is linear
if it has two properties: all straight lines must remain as such, and the origin must remain fixed in
place. In general, you should think of linear transformations as keeping grid lines parallel and evenly
spaced.

8.3.2 Describing transformations numerically

Some transformations are simple to think about, like rotations about the origin. Others are more
difficult to describe with words. So how could one describe these transformations numerically? If
you were, say, programming some animations to make a video teaching the topic, what formula could
you give the computer so that if you give it the coordinates of a vector, it would return the coordinates
of where that vectors lands?
66 Chapter 8. Linear algebra
[ ] [ ]
xin xout
????
yin yout

You only need to record where the two basis vectors, î and ĵ, each land, and everything else will follow
from that. For example, consider the vector v with coordinates (−1, 2), meaning ⃗v = −1î + 2ĵ. If
we play some transformation and follow where all three of these vectors go, the property that grid
lines remain parallel and evenly spaced has a really important consequence: the place where ⃗v lands
will be −1 times the vector where î landed plus 2 times the vector where ĵ landed. In other words, it
started off as a certain linear combination of î and ĵ and it ends up as that same linear combination of
where those two vectors landed. This means you can deduce where ⃗v must go based only on where î
and ĵ each land. For this transformation, î lands on the coordinates (1, −2) and ĵ lands on the x-axis
at the coordinates (3, 0).

Transformed ⃗v = −1(Transformed î) + 2(Transformed ĵ)

[ ] [ ]
1 3
Transformed ⃗v = −1 +2
−2 0

Adding that all together, you can deduce that ⃗v has to land on the vector (5, 2).

[ ]
−1(1) + 2(3)
Transformed ⃗v =
−1(−2) + 2(0)
[ ]
5
Transformed ⃗v =
2

This is a good point to pause and ponder, because it’s pretty important. This gives us a technique
to deduce where any vectors land, so long as we have a record of where î and ĵ each land, without
needing to watch the transformation itself.
Given a vector with more general coordinates x and y, it will land on x times the vector where î lands
(1, −2), plus y times the vector where ĵ lands (3, 0). Carrying out that sum, you see that it lands at
(1x + 3y, −2x + 0y).

[ ] [ ]
1 3
î → ĵ →
−2 0

[ ] [ ] [ ] [ ]
x 1 3 1x + 3y
→x +y =
y −2 0 −2x + 0y

Given any vector, this formula will describe where that vector lands.
What all of this is saying is that a two dimensional linear transformation is completely described by
just four numbers: the two coordinates for where î lands and the two coordinates for where ĵ lands.
It’s common to package these coordinates into a two-by-two grid of numbers, called a two-by-two
matrix, where you can interpret the columns as the two special vectors where î and ĵ each land. If î
lands on the vector (3, −2) and ĵ lands on the vector (2, 1), this two-by-two matrix would be
 8.3 Linear transformations and matrices 67

[ ]
3 2
−2 1

If you’re given a two-by-two matrix describing a linear transformation and some specific vector, say
(5, 7), and you want to know where that linear transformation takes that vector, you can multiply the
coordinates of the vector by the corresponding columns of the matrix, then add together the result.

[ ][ ] [ ] [ ]
3 2 5 3 2
=5 +7
−2 1 7 −2 1

This corresponds with the idea of adding the scaled versions of our new basis vectors.
Let’s see what this looks like in the most general case where your matrix has entries a, b, c, d.

[ ]
a b
c d

Remember, this matrix is just a way of packaging the information needed to describe a linear trans-
formation. Always remember to interpret that first column, (a, c), as the place where the first basis
vector lands and that second column, (b, d), as the place where the second basis vector lands.
When we apply this transformation to some vector (x, y), the result will be x times (a, c) plus y times
(b, d). Together, this gives a vector (ax + by, cx + dy).

[ ][ ] [ ] [ ] [ ]
a b x a b ax + by
=x +y =
c d y c d cx + dy

You could even define this as matrix-vector multiplication when you put the matrix on the left of
the vector like it’s a function. Then, you could make high schoolers memorize this, without showing
them the crucial part that makes it feel intuitive (yes, that was sarcasm). Isn’t it more fun to think
about these columns as the transformed versions of your basis vectors and to think about the result
as the appropriate linear combination of those vectors?

8.3.3 Examples of linear transformations

Let’s practice describing a few linear transformations with matrices. For example, if we rotate all
of space 90° counterclockwise then î lands on the coordinates (0, 1) and ĵ lands on the coordinates
(−1, 0). So the matrix we end up with has the columns (0, 1), (−1, 0).

[ ]
0 −1
1 0

To ascertain what happens to any vector after a 90° rotation, you could just multiply its coordinates
by this matrix.

[ ][ ]
0 −1 x
1 0 y
68 Chapter 8. Linear algebra

Here’s a fun transformation with a special name, called a “shear”. In it, î remains fixed so the first
column of the matrix is (1, 0), but ĵ moves over to the coordinates (1, 1) which become the second
column of the matrix.

[ ]
1 1
0 1

And, at the risk of being redundant here, figuring out how shear transforms a given vector comes
down to multiplying this matrix by that vector.

[ ][ ]
1 1 x
0 1 y

Let’s say we want to go the other way around, starting with a matrix, say with columns (1, 2) and
(3, 1), and we want to deduce what its transformation looks like. Pause and take a moment to see if
you can imagine it.

[ ]
1 3
2 1

One way to do this is to first move î to (1, 2). Then, move ĵ to (3, 1), always moving the rest of space
in such a way that that keeps grid lines parallel and evenly spaced.

Suppose that the vectors that î and ĵ land on are linearly dependent as in the following matrix (that
is, it has linearly dependent columns).

[ ]
2 −2
1 −1

If you recall from last section, this means that one vector is a scaled version of the other, so that
linear transformation compresses all of 2D space onto the line where those two vectors sit. This is
also known as the one-dimensional span of those two linearly dependent vectors.

To sum up, linear transformations are a way to move around space such that the grid lines remain
parallel and evenly spaced and such that the origin remains fixed. Delightfully, these transformations
can be described using only a handful of numbers: the coordinates of where each basis vector lands.
Matrices give us a language to describe these transformations where the columns represent those
coordinates and matrix-vector multiplication is just a way to compute what that transformation does
to a given vector. The important take-away here is that every time you see a matrix, you can interpret
it as a certain transformation of space. Once you really digest this idea, you’re in a great position
to understand linear algebra deeply. Almost all of the topics coming up, from matrix multiplication
to determinants, eigenvalues, etc. will become easier to understand once you start thinking about
matrices as transformations of space.

R See the corresponding Essence of linear algebra video for a more visual presentation (11 min-
utes) [7].
 8.4 Matrix multiplication as composition 69

8.4 Matrix multiplication as composition

Often, you find yourself wanting to describe the effect of applying one transformation and then an-
other. For example, you may want to describe what happens when you first rotate the plane 90°
counterclockwise then apply a shear. The overall effect here, from start to finish, is another linear
transformation distinct from the rotation and the shear. This new linear transformation is commonly
called the “composition” of the two separate transformations we applied, and like any linear trans-
formation, it can be described with a matrix all its own by following î and ĵ. In this example, the
ultimate landing spot for î after both transformations is (1, 1), so that’s the first column of the matrix.
Likewise, ĵ ultimately ends up at the location (−1, 0), so we make that the second column of the
matrix.

[ ]
1 −1
1 −0

This new matrix captures the overall effect of applying a rotation then a shear but as one single action
rather than two successive ones.
Here’s one way to think about that new matrix: if you were to feed some vector through the rotation
then the shear, the long way to compute where it ends up is to, first, multiply it on the left by the
rotation matrix; then, take whatever you get and multiply that on the left by the shear matrix.

[ ] ([ ] [ ])
1 1 0 −1 x
0 1 1 0 y

This is, numerically speaking, what it means to apply a rotation then a shear to a given vector, but the
result should be the same as just applying this new composition matrix we found to that same vector.
This applies to any vector because this new matrix is supposed to capture the same overall effect as
the rotation-then-shear action.

[ ] ([ ] [ ]) [ ][ ]
1 1 0 −1 x 1 −1 x
=
0 1 1 0 y 1 0 y

Based on how things are written down here, it’s reasonable to call this new matrix the “product” of
the original two matrices.

[ ][ ] [ ]
1 1 0 −1 1 −1
=
0 1 1 0 1 0

We can think about how to compute that product more generally in just a moment, but it’s way too
easy to get lost in the forest of numbers. Always remember that multiplying two matrices like this
has the geometric meaning of applying one transformation then another.
One oddity here is that we are reading the transformations from right to left; you first apply the
transformation represented by the matrix on the right, then you apply the transformation represented
by the matrix on the left. This stems from function notation, since we write functions on the left of
variables, so every time you compose two functions, you always have to read it right to left.
Let’s look at another example. Take the matrix with columns (1, 1) and (−2, 0).
 70 Chapter 8. Linear algebra

[ ]
1 −2
M1 =
1 0

Next, take the matrix with columns (0, 1) and (2, 0).

[ ]
0 2
M2 =
1 0

The total effect of applying M1 then M2 gives us a new transformation, so let’s find its matrix. First,
we need to determine where î goes. After applying M1 , the new coordinates of î, by definition, are
given by that first column of M1 , namely, (1, 1). To see what happens after applying M2 , multiply
the matrix for M2 by that vector (1, 1). Working it out the way described in the last section, you’ll
get the vector (2, 1).

[ ][ ] [ ] [ ] [ ]
0 2 1 0 2 2
=1 +1 =
1 0 1 1 0 1

This will be the first column of the composition matrix. Likewise, to follow ĵ, the second column of
M1 tells us that it first lands on (−2, 0). Then, when we apply M2 to that vector, you can work out
the matrix-vector product to get (0, −2).

[ ][ ] [ ] [ ] [ ]
0 2 −2 0 2 0
= −2 +0 =
1 0 0 1 0 −2

This will be the second column of the composition matrix.

[ ][ ] [ ]
0 2 1 −2 2 0
=
1 0 1 0 1 −2

8.4.1 General matrix multiplication

Let’s go through that same process again, but this time, we’ll use variable entries in each matrix, just
to show that the same line of reasoning works for any matrices. This is more symbol-heavy, but it
should be pretty satisfying for anyone who has previously been taught matrix multiplication the more
rote way.

[ ][ ] [ ]
a b e f ? ?
=
c d g h ? ?

To follow where î goes, start by looking at the first column of the matrix on the right, since this is
where î initially lands. Multiplying that column by the matrix on the left is how you can tell where
the intermediate version of î ends up after applying the second transformation.

[ ][ ] [ ] [ ] [ ]
a b e a b ae + bg
=e +g =
c d g c d ce + dg

So the first column of the composition matrix will always equal the left matrix times the first column
of the right matrix. Likewise, ĵ will always initially land on the second column of the right matrix,
 8.5 The determinant 71

so multiplying by this second column will give its final location, and hence, that’s the second column
of the composition matrix.

[ ][ ] [ ] [ ] [ ]
a b f a b af + bh
=f +h =
c d h c d cf + dh

So the complete composition matrix is

[ ][ ] [ ]
a b e f ae + bg af + bh
=
c d g h ce + dg cf + dh

Notice there’s a lot of symbols here, and it’s common to be taught this formula as something to
memorize along with a certain algorithmic process to help remember it. Before memorizing that
process, you should get in the habit of thinking about what matrix multiplication really represents:
applying one transformation after another. This will give you a much better conceptual framework
that makes the properties of matrix multiplication much easier to understand.

8.4.2 Matrix multiplication associativity

For example, here’s a question: does it matter what order we put the two matrices in when we multiply
them? Let’s think through a simple example. Take a shear which fixes î and moves ĵ over to the right,
and a 90° rotation. If you first do the shear then rotate, we can see that î ends up at (0, 1) and ĵ ends
up at (−1, 1). Both are generally pointing close together. If you first rotate then do the shear, î ends
up over at (1, 1) and ĵ is off on a different direction at (−1, 0) and they’re pointing farther apart. The
overall effect here is clearly different, so evidently, order totally does matter. Notice by thinking in
terms of transformations, that’s the kind of thing that you can do in your head by visualizing. No
matrix multiplication necessary.
Let’s consider trying to prove that matrix multiplication is associative. This means that if you have
three matrices A, B, and C, and you multiply them all together, it shouldn’t matter if you first compute
A times B then multiply the result by C, or if you first multiply B times C then multiply that result
by A on the left. In other words, it doesn’t matter where you put the parenthesis.

?
(AB)C = A(BC)

If you try to work through this numerically, it’s horrible, and unenligthening for that matter. However,
when you think about matrix multiplication as applying one transformation after another, this property
is just trivial. Can you see why? What it’s saying is that if you first apply C then B, then A, it’s the
same as applying C, then B then A. There’s nothing to prove, you’re just applying the same three
things one after the other all in the same order. This might feel like cheating, but it’s not. This is a
valid proof that matrix multiplication is associative, and even better than that, it’s a good explanation
for why that property should be true.

R See the corresponding Essence of linear algebra video for a more visual presentation (10 min-
utes) [8].

8.5 The determinant

So, moving forward, we will be assuming you have a visual understanding of linear transformations
and how they’re represented with matrices.
 72 Chapter 8. Linear algebra

8.5.1 Scaling areas

If you think about a couple linear transformations, you might notice how some of them seem to stretch
space out while others compress it. It’s useful for understanding these transformations to measure
exactly how much it stretches or compresses things (more specifically, to measure the factor by which
areas are scaled). For example, look at the matrix with columns (3, 0), and (0, 2).

[ ]
3 0
0 2

It scales î by a factor of 3 and scales ĵ by a factor of 2. Now, if we focus our attention on the one-by-
one square whose bottom sits on î and whose left side sits on ĵ, after the transformation, this turns
into a 2 by 3 rectangle. Since this region started out with area 1 and ended up with area 6, we can say
the linear transformation has scaled its area by a factor of 6. Compare that to a shear whose matrix
has columns (1, 0) and (1, 1) meaning î stays in place and ĵ moves over to (1, 1).

[ ]
1 1
0 1

That same unit square determined by î and ĵ gets slanted and turned into a parallelogram, but the
area of that parallelogram is still 1 since its base and height each continue to each have length 1. Even
though this transformation pushes things about, it seems to leave areas unchanged (at least in the case
of that one uint square).
Actually though, if you know how much the area of that one single unit square changes, you can know
how the area of any possible region in space changes. First off, notice that whatever happens to one
square in the grid has to happen to any other square in the grid no matter the size. This follows from
the fact that grid lines remain parallel and evenly spaced. Then, any shape that’s not a grid square can
be approximated by grid squares pretty well with arbitrarily good approximations if you use small
enough grid squares. So, since the areas of all those tiny grid squares are being scaled by some single
amount, the area of the shape as a whole will also be scaled by that same single amount.

8.5.2 Exploring the determinant

This special scaling factor, the factor by which a linear transformation changes any area, is called
the determinant of that transformation. We’ll show how to compute the determinant of a transforma-
tion using its matrix later on, but understanding what it represents is much more important than the
computation. For example, the determinant of a transformation would be 3 if that transformation
increases the area of the region by a factor of 3.

([ ])
0 2
det =3
−1.5 1

The determinant of a matrix is commonly denoted by vertical bars instead of square brackets.

 
 0 2

−1.5 1 = 3

1
The determinant of a transformation would be 2 if it compresses all areas by a factor of 12 .
 8.5 The determinant 73

 
 0.5 0.5
 
−0.5 0.5 = 0.5

The determinant of a 2D transformation is zero if it compresses all of space onto a line or even onto
a single point since then, the area of any region would become zero.

 
4 2
 
2 1 = 0

That last example proved to be pretty important. It means checking if the determinant of a given
matrix is zero will give a way of computing whether the transformation associated with that matrix
compresses everything into a smaller dimension.
This analogy so far isn’t quite right. The full concept of a determinant allows for negative values.

 
1 2 
 
3 4 = −2

What would scaling an area by a negative amount even mean? This has to do with the idea of ori-
entation. A 2D transformation with a negative determinant essentially flips space over. Any trans-
formations that do this are said to “invert the orientation of space”. Another way to think about it
is in terms of î and ĵ. In their starting positions, ĵ is to the left of î. If, after a transformation, ĵ is
now on the right side of î, the orientation of space has been inverted. Whenever this happens, the
determinant will be negative. The absolute value of the determinant still tells you the factor by which
areas have been scaled.
For example, the matrix with columns (1, 1) and (2, −1) encodes a transformation that has determi-
nant −3.

 
1 2 
 
1 −1 = −3

This means that space gets flipped over and areas are scaled by a factor of 3.
Why would this idea of a negative area scaling factor be a natural way to describe orientation-flipping?
Think about the series of transformations you get by slowly letting î rotate closer and closer to ĵ. As
î gets closer, all the areas in space are getting compressed more and more meaning the determinant
approaches zero. Once î lines up perfectly with ĵ, the determinant is zero. Then, if î continues,
doesn’t it feel natural for the determinant to keep decreasing into negative numbers?

8.5.3 The determinant in 3D

That’s the understanding of determinants in two dimensions. What should it mean for three dimen-
sions? The determinant of a 3 × 3 matrix tells you how much volumes get scaled. A determinant of
zero would mean that all of space is compressed onto something with zero volume meaning either
a flat plane, a line, or in the most extreme case, a single point. This means that the columns of the
matrix are linearly dependent.
What should negative determinants mean for three dimensions? One way to describe orientation in
3D is with the right-hand rule. Point the forefinger of your right hand in the direction if î, stick out
your middle finger in the direction of ĵ, and notice how when you point your thumb up, it is in the
 74 Chapter 8. Linear algebra

direction of k̂. If you can still do that after the transformation, orientation has not changed and the
determinant is positive. Otherwise, if after the transformation it only makes sense to do that with
your left hand, orientation has been flipped and the determinant is negative.

8.5.4 Computing the determinant

How do you actually compute the determinant? For a 2 × 2 matrix with entries a, b, c, d, the formula
is as follows.
 
a b 
 
 c d = ad − bc

Here’s part of an intution for where this formula comes from. Let’s say that the terms b and c were
both zero. Then, the term a tells you how much î is stretched in the x-direction and the term d tells
you how much ĵ is stretched in the y-direction. Since those other terms are zero, it should make
sense that ad gives the area of the rectangle that the unit square turns into. Even if only one of b
or c are zero, you’ll have a parallelogram with a base of a and a height d, so the area should still
be ad. Loosely speaking, if both b and c are nonzero, then that bc term tells you how much this
parallelogram is stretched or compressed in the diagonal direction.
If you feel like computing determinants by hand is something that you need to know (you won’t for
this book), the only way to get it down is to just practice it with a few. This is all triply true for 3D
determinants. There is a formula, and if you feel like that’s something you need to know, you should
practice with a few matrices.

 
a b c       
  e f  d f  d e 
d e f  = a      
  h i  − b g i  + c g h
g h i 

We don’t think those computations fall within the essence of linear algebra, but understanding what
the determinant represents falls within that essence.

R See the corresponding Essence of linear algebra video for a more visual presentation (10 min-
utes) [10].

8.6 Inverse matrices, column space, and null space

As you can probably tell by now, the bulk of this chapter is on understanding matrix and vector
operations through that more visual lens of linear transformations. This section is no exception,
describing the concepts of inverse matrices, columns space, rank, and null space through that lens. A
fair warning though: we’re not going to talk about the methods for actually computing these things,
and some would argue that that’s pretty important. There are a lot of very good resources for learning
those methods outside of this chapter. Keywords: “Gaussian elimination” and “row echelon form”.
Most of the value that we actually have to add here is on the intuition half. Plus, in practice, we
usually use software to compute these things for us anyway.

8.6.1 Linear systems of equations

First, a few words on the usefulness of linear algebra. By now, you already have a hint for how it’s
used in describing the manipulation of space, which is useful for computer graphics and robotics.
However, one of the main reasons that linear algebra is more broadly applicable, and required for
8.6 Inverse matrices, column space, and null space 75

just about any technical discipline, is that it lets us solve certain systems of equations. When we
say “system of equations”, we mean there is a list of variables, things you don’t know, and a list of
equations relating them. For example,

6x − 3y + 2z = 7
x + 2y + 5z = 0
2x − 8y − z = −2

is a system of equations with the unknowns x, y, and z.

In a lot of situations, those equations can get very complicated, but, if you’re lucky, they might take
on a certain special form. Within each equation, the only thing happening to each variable is that it’s
scaled by some constant, and the only thing happening to each of those scaled variables is that they’re
added to each other, so no exponents or fancy functions, or multiplying two variables together.
The typical way to organize this sort of special system of equations is to throw all the variables on
the left and put any lingering constants on the right. It’s also nice to vertically line up the common
variables, and to do that, you might need to throw in some zero coefficients whenever the variable
doesn’t show up in one of the equations.

2x + 5y + 3z = −3
4x + 0y + 8z = 0
1x + 3y + 0z = 2

This is called a “linear system of equations”. You might notice that this looks a lot like matrix-
vector multiplication. In fact, you can package all of the equations together into a single vector
equation, where you have the matrix containing all the constant coefficients, a vector containing all
the constant coefficients, and a vector containing all the variables. Their matrix-vector product equals
some different constant vector.

    
2 5 3 x −3
4 0 8 y  =  0 
1 3 0 z 2

Let’s name that constant matrix A, denote the vector holding the variables with x, and call the con-
stant vector on the right-hand side v. This is more than just a notational trick to get our system of
equations written on one line. It sheds light on a pretty cool geometric interpretation for the problem.

Ax = v

The matrix A corresponds with some linear transformation, so solving Ax = v means we’re looking
for a vector x which, after applying the transformation A, lands on v.
Think about what’s happening here for a moment. You can hold in your head this really complicated
idea of multiple variables all intermingling with each other just by thinking about compressing or
morphing space and trying to determine which vector lands on another.
 76 Chapter 8. Linear algebra

To start simple, let’s say you have a system with two equations and two unknowns. This means the
matrix A is a 2 × 2 matrix, and v and x are each two-dimensional vectors.

2x + 2y = −4
1x + 3y = −1
[ ][ ] [ ]
2 2 x −4
=
1 3 y −1

8.6.2 Inverse
How we think about the solutions to this equation depends on whether the transformation associated
with A compresses all of space into a lower dimension, like a line or a point, or if it leaves everything
spanning the full two dimensions where it started. In the language of the last section, we subdivide
into the case where A has zero determinant and the case where A has nonzero determinant.
Let’s start with the most likely case where the determinant is nonzero, meaning space does not get
compressed into a zero area region. In this case, there will always be one and only one vector that
lands on v, and you can find it by playing the transformation in reverse. Following where v goes as
we undo the transformation, you’ll find the vector x such that A times x equals v.
When you play the transformation in reverse, it actually corresponds to a separate linear transforma-
tion, commonly called the “inverse of A” denoted A−1 .

[ ]−1
−1 3 1
A =
0 2

For example, if A was a counterclockwise rotation by 90°, then the inverse of A would be a clockwise
rotation by 90°.

[ ] [ ]
0 −1 0 1
A= A−1 =
1 0 −1 0

If A was a rightward shear that pushes ĵ one unit to the right, the inverse of A would be a leftward
shear that pushes ĵ one unit to the left.
In general, A−1 is the unique transformation with the property that if you first apply A, then follow
it with the transformation A−1 , you end up back where you started. Applying one transformation
after another is captured algebraically with matrix multiplication, so the core property of this trans-
formation A−1 is that A−1 A equals the matrix that corresponds to doing nothing.
The transformation that does nothing is called the “identity transformation”. It leaves î and ĵ each
where they are, unmoved, so its columns are (1, 0) and (0, 1).

[ ]
−1 1 0
A A=
0 1

Once you find this inverse, which in practice, you do with a computer, you can solve your equation
by multipling this inverse matrix by v.
 8.6 Inverse matrices, column space, and null space 77

Ax = v
−1
A Ax = A−1 v
x = A−1 v

Again, what this means geometrically is that you’re playing the transformation in reverse and following
v. This nonzero determinant case, which for a random choice of matrix is by far the most likely one,
corresponds with the idea that if you have two unknowns and two equations, it’s almost certainly the
case that there’s a single, unique solution.
This idea also makes sense in higher dimensions when the number of equations equals the number
of unknowns. Again, the system of equations can be translated to the geometric interpretation where
you have some transformation, A, some vector v, and you’re looking for the vector x that lands on v.
As long as the transformation A doesn’t compress all of space into a lower dimension, meaning, its
determinant is nonzero, there will be an inverse transformation, A−1 , with the property that if you
first do A, then you do A−1 , it’s the same as doing nothing. To solve your equation, you just have to
multiply that reverse transformation matrix by the vector v.
When the determinant is zero and the transformation associated with this system of equations com-
presses space into a smaller dimension, there is no inverse. You cannot uncompress a line to turn it
into a plane. At least, that’s not something that a function can do. That would require transforming
each individual vector into a whole line full of vectors, but functions can only take a single input to
a single output.
Similarly, for three equations and three unknowns, there will be no inverse if the corresponding
transformation compresses 3D space onto the plane, or even if it compresses it onto a line, or a point.
Those all correspond to a determinant of zero since any region is compressed into something with
zero volume.
It’s still possible that a solution exists even when there is no inverse. It’s just that when your trans-
formation compresses space onto, say, a line, you have to be lucky enough that the vector v exists
somewhere on that line.

8.6.3 Rank and column space

You might notice that some of these zero determinant cases feel a lot more restrictive than others.
Given a 3×3 matrix, for example, it seems a lot harder for a solution to exist when it compresses space
onto a line compared to when it compresses space onto a plane even though both of those have zero
determinant. We have some language that’s more specific than just saying “zero determinant”. When
the output of a transformation is a line, meaning it’s one-dimensional, we say the transformation has
a rank of one. If all the vectors land on some two-dimensional plane, we say the transformation has
a rank of two. The word “rank” means the number of dimensions in the output of a transformation.
For instance, in the case of 2 × 2 matrices, the highest possible rank is 2. It means the basis vectors
continue to span the full two dimensions of space, and the determinant is nonzero. For 3×3 matrices,
rank 2 means that we’ve collapsed, but not as much as we would have collapsed for a rank 1 situation.
If a 3D transformation has a nonzero determinant and its output fills all of 3D space, it has a rank of
3.
This set of all possible outputs for your matrix, whether it’s a line, a plane, 3D space, whatever, is
called the column space of your matrix. You can probably guess where that name comes from. The
columns of your matrix tell you where the basis vectors land, and the span of those transformed basis
 78 Chapter 8. Linear algebra

vectors gives you all possible outputs. In other words, the column space is the span of the columns
of your matrix, so a more precise definition of rank would be that it’s the number of dimensions in
the column space. When this rank is as high as it can be, meaning it equals the number of columns,
we call the matrix “full rank”.

8.6.4 Null space

Notice, the zero vector will always be included in the column space since linear transformations must
keep the origin fixed in place. For a full rank transformation, the only vector that lands at the origin is
the zero vector itself, but for matrices that aren’t full rank, which compress to a smaller dimension, you
can have a whole bunch of vectors that land on zero. If a 2D transformation compresses space onto
a line, for example, there is a separate line in a different direction full of vectors that get compressed
onto the origin. If a 3D transformation compresses space onto a plane, there’s also a full line of
vectors that land on the origin. If a 3D transformation compresses all the space onto a line, then
there’s a whole plane full of vectors that land on the origin.

This set of vectors that lands on the origin is called the null space or the kernel of your matrix. It’s
the space of all vectors that become null in the sense that they land on the zero vector. In terms of
the linear system of equations Ax = v, when v happens to be the zero vector, the null space gives
you all the possible solutions to the equation.

8.6.5 Closing remarks

That’s a high-level overview of how to think about linear systems of equations geometrically. Each
system has some kind of linear transformation associated with it, and when that transformation has
an inverse, you can use that inverse to solve your system. Otherwise, the idea of column space lets
us understand when a solution even exists, and the idea of a null space helps us understand what the
set of all possible solutions can look like.

Again, there’s a lot not covered here, most notably how to compute these things. We also had to limit
the scope to examples where the number of equations equals the number of unknowns. The goal here
is not to try to teach everything: it’s that you come away with a strong intuition for inverse matrices,
column space, and null space, and that those intuitions make any future learning that you do more
fruitful.

R See the corresponding Essence of linear algebra video for a more visual presentation (12 min-
utes) [5].

8.7 Nonsquare matrices as transformations between dimensions

When we’ve talked about linear transformations so far, we’ve only really talked about transformations
from 2D vectors to other 2D vectors, represented with 2 × 2 matrices; or from 3D vectors to other
3D vectors, represented with 3 × 3 matrices. What about nonsquare matrices? We’ll take a moment
to discuss what those mean geometrically.

By now, you have most of the background you need to start pondering a question like this on your
own, but we’ll start talking through it, just to give a little mental momentum.

It’s perfectly reasonable to talk about transformations between dimensions, such as one that takes 2D
vectors to 3D vectors. Again, what makes one of these linear is that grid lines remain parallel and
evenly spaced, and that the origin maps to the origin.
 8.8 Eigenvectors and eigenvalues 79

Encoding one of these transformations with a matrix the same as what we’ve done before. You look
at where each basis vector lands and write the coordinates of the landing spots as the coordinates
of the landing spots as the columns of a matrix. For example, the following is a transformation that
takes î to the coordinates (2, −1, −2) and ĵ to the coordinates (0, 1, 1).

 
2 0
−1 1
−2 1

Notice, this means the matrix encoding our transformation has three rows and two columns, which,
to use standard terminology, makes it a 3 × 2 matrix. In the language of last section, the column
space of this matrix, the place where all the vectors land, is a 2D plane slicing through the origin of
3D space. The matrix is still full rank since the number of dimensions in this column space is the
same as the number of dimensions of the input space.
If you see a 3 × 2 matrix out in the wild, you can know that it has the geometric interpretation of
mapping two dimensions to three dimensions since the two columns indicate that the input space has
two basis vectors, and the three rows indicate that the landing spots for each of those basis vectors is
described with three separate coordinates.
For a 2 × 3 matrix, the three columns indicate a starting space that has three basis vectors, so it
starts in three dimensions; and the two rows indicate that the landing spot for each of those three
basis vectors is described with only two coordinates, so they must be landing in two dimensions. It’s
a transformation from 3D space onto the 2D plane.
You could also have a transformation from two dimensions to one dimension. One-dimensional space
is really just the number line, so a transformation like this takes in 2D vectors and returns numbers.
Thinking about gridlines remaining parallel and evenly spaced is messy due to all the compression
happening here, so in this case, the visual understanding for what linearity means is that if you have a
line of evenly spaced dots, it would remain evenly spaced once they’re mapped onto the number line.
One of these transformations is encoded with a 1 × 2 matrix, each of whose two columns has just a
single entry. The two columns represent where the basis vectors land, and each one of those columns
requires just one number, the number that that basis vector landed on.

R See the corresponding Essence of linear algebra video for a more visual presentation (4 minutes)
[9].

8.8 Eigenvectors and eigenvalues

8.8.1 What is an eigenvector?
To start, consider some linear transformation in two dimensions that moves the basis vector î to the
coordinates (3, 0) and ĵ to (1, 2), so it’s represented with a matrix whose columns are (3, 0) and
(1, 2).

[ ]
3 1
0 2

Focus in on what it does to one particular vector and think about the span of that vector, the line
passing through its origin and its tip. Most vectors are going to get knocked off their span during
 80 Chapter 8. Linear algebra

the transformation, but some special vectors do remain on their own span meaning the effect that the
matrix has on such a vector is just to stretch it or compress it like a scalar.

For this specific example, the basis vector î is one such special vector. The span of î is the x-axis,
and from the first column of the matrix, we can see that î moves over to three times itself still on that
x-axis. What’s more, due to the way linear transformations work, any other vector on the x-axis is
also just stretched by a factor of 3, and hence, remains on its own span.

A slightly sneakier vector that remains on its own span during this transformation is (−1, 1). It ends
up getting stretched by a factor of 2. Again, linearity is going to imply that any other vector on the
diagonal line spanned by this vector is just going to get stretched out by a factor of 2.

For this transformation, those are all the vectors with this special property of staying on their span.
Those on the x-axis get stretched out by a factor of 3 and those on the diagonal line get stretched
out by a factor of 2. Any other vector is going to get rotated somewhat during the transformation
and knocked off the line that it spans. As you might have guessed by now, these special vectors are
called the eigenvectors of the transformation, and each eigenvector has associated with it an eigenvalue,
which is just the factor by which it’s stretched or compressed during the transformation.

Of course, there’s nothing special about stretching vs compressing or the fact that these eigenval-
ues happen to be positive. In another example, you could have an eigenvector with eigenvalue − 12 ,
meaning that the vector gets flipped and compressed by a factor of 12 .

[ ]
0.5 −1
−1 0.5

The important part here is that it stays on the line that it spans out without getting rotated off of it.

8.8.2 Eigenvectors in 3D rotation

For a glimpse of why this might be a useful thing to think about, consider some three-dimensional
rotation. If you can find an eigenvector for that rotation, a vector that remains on its own span, you
have found the axis of rotation. It’s much easier to think about a 3D rotation in terms of some axis of
rotation and an angle by which it’s rotating rather than thinking about the full 3 × 3 matrix associated
with that transformation. In this case, by the way, the corresponding eigenvalue would have to be
1 since rotations never stretch or compress anything, so the length of the vector would remain the
same.

8.8.3 Finding eigenvalues

The following pattern shows up a lot in linear algebra. With any linear transformation described by a
matrix, you could understand what it’s doing by reading off the columns of this matrix as the landing
spots for basis vectors, but often a better way to get at the heart of what the linear transformation
actually does, less dependent on your particular coordinate system, is to find the eigenvectors and
eigenvalues.

I won’t cover the full details on methods for computing eigenvectors and eigenvalues here, but I’ll try
to give an overview of the computational ideas that are most important for a conceptual understanding.
Symbolically, an eigenvector look like the following

Av = λv
8.8 Eigenvectors and eigenvalues 81

A is the matrix representing some transformation, v is the eigenvector, and λ is a number, namely
the corresponding eigenvalue. This expression is saying that the matrix-vector product Av gives the
same result as just scaling the eigenvector v by some value λ. Finding the eigenvectors and their
eigenvalues of the matrix A involves finding the values of v and λ that make this expression true.
It’s awkward to work with at first because that left-hand side represents matrix-vector multiplication,
but the right-hand side is scalar-vector multiplication. Let’s rewrite the right-hand side as some kind
of matrix-vector multiplication using a matrix which has the effect of scaling any vector by a factor
of λ. The columns of such a matrix will represent what happens to each basis vector, and each basis
vector is simply multiplied by λ, so this matrix will have the number λ down the diagonal and zeroes
everywhere else.

 
λ 0 0
0 λ 0
0 0 λ

The common way to write this is to factor out λ and write it as λI where I is the identity matrix with
ones down the diagonal.

Av = (λI)v

With both sides looking like matrix-vector multiplication, we can subtract off that right-hand side
and factor out v.

Av − (λI)v = 0
(A − λI)v = 0

We now have a new matrix A − λI, and we’re looking for a vector v such that this new matrix times
v gives the zero vector. This will always be true if v itself is the zero vector, but that’s boring. We
want a nonzero eigenvector. The only way it’s possible for the product of a matrix with a nonzero
vector to become zero is if the transformation associated with that matrix compresses space into a
lower dimension. That compression corresponds to a zero determinant for the matrix.

det(A − λI) = 0

To be concrete, let’s say your matrix A has columns (2, 1) and (2, 3), and think about subtracting
off a variable amount λ.

([ ])
2−λ 2
det =0
1 3−λ

The goal is to find a value of λ that will make this determinant zero meaning the tweaked transfor-
mation compresses space into a lower dimension. In this case, that value is λ = 1. Of course, if we
had chosen some other matrix, the eigenvalue might not necessarily be 1.
This is kind of a lot, but let’s unravel what this is saying. When λ = 1, the matrix A − λI compresses
space onto a line. That means there’s a nonzero vector v such that (A − λI)v equals the zero vector.
82 Chapter 8. Linear algebra

(A − λI)v = 0

Remember, we care about that because it means Av = λv, which you can read off as saying that
the vector v is an eigenvector of A staying on its own span during the transformation A. For the
following example

[ ]
2 2
v = 1v
1 3

the corresponding eigenvalue is 1, so v would actually just stay fixed in place.

To summarize our line of reasoning:

Av = λv
Av − λIv = 0
(A − λI)v = 0
det(A − λI) = 0

To see this in action, let’s visit the example from the start with a matrix whose columns are (3, 0) and
(1, 2). To determine if a value λ is an eigenvalue, subtract it from the diagonals of this matrix and
compute the determinant.

[ ]
3 1
0 2
[ ]
3−λ 1
0 2−λ
([ ])
3−λ 1
det = (3 − λ)(2 − λ) − 1 · 0
0 2−λ
= (3 − λ)(2 − λ)

We get a certain quadratic polynomial in λ. Since λ can only be an eigenvalue if this determinant
happens to be zero, you can conclude that the only possible eigenvalues are λ = 2 and λ = 3.
To determine what the eigenvectors are that actually have one of these eigenvalues, say λ = 2, plug
in that value of λ to the matrix and then solve for which vectors this diagonally altered matrix sends
to zero.

[ ][ ] [ ]
3−2 1 x 0
=
0 2−2 y 0

If you computed this the way you would any other linear system, you’d see that the solutions are all
the vectors on the diagonal line spanned by (−1, 1). This corresponds to the fact that the unaltered
matrix has the effect of stretching all those vectors by a factor of 2.
 8.8 Eigenvectors and eigenvalues 83

8.8.4 Transformations with no eigenvectors

A 2D transformation doesn’t have to have eigenvectors. For example, consider a rotation by 90°.

[ ]
0 −1
1 0

This doesn’t have any eigenvectors since it rotates every vector off its own span. If you actually tried
computing the eigenvalues of a rotation like this, notice what happens.

[ ]
0 −1
1 0
[ ]
−λ −1
1 −λ
([ ])
−λ −1
det = (−λ)(−λ) − (−1)(1)
1 −λ
= λ2 + 1 = 0

The only roots of that polynomial are the imaginary numbers i and −i. The fact that there are no
real number solutions indicates that there are no eigenvectors.

R Interestingly though, the fact that multiplication by i in the complex plane looks like a 90°
rotation is related to the fact that i is an eigenvalue of this transformation of 2D real vectors.
The specifics of this are out of scope, but note that eigenvalues which are complex numbers
generally correspond to some kind of rotation in the transformation.

8.8.5 Repeated eigenvalues

Another interesting example is a shear which fixes î in place and moves ĵ over by 1.

[ ]
1 1
0 1

All the vectors on the x-axis are eigenvectors with eigenvalue 1. In fact, these are the only eigenvec-
tors.

([ ])
1−λ 1
det = (1 − λ)(1 − λ) = 0
0 1−λ
(1 − λ)2 = 0

The only root of this expression is λ = 1.

8.8.6 Transformations with larger eigenvector spans

Keep in mind it’s also possible to have just one eigenvalue but with more than just a line full of
eigenvectors. A simple example is a matrix that scales everything by 2.
 84 Chapter 8. Linear algebra

[ ]
2 0
0 2

The only eigenvalue is 2, but every vector in the plane gets to be an eigenvector with that eigenvalue.

R See the corresponding Essence of linear algebra video for a more visual presentation (17 min-
utes) [2].

8.9 Miscellaneous notation

This book works with two-dimensional matrices in the sense that they only have rows and columns.
The dimensionality of these matrices is specified by row first, then column. For example, a matrix
with two rows and three columns would be a two-by-three matrix. A square matrix has the same
number of rows as columns. Matrices commonly use capital letters while vectors use lowercase
letters.
The matrix I is known as the identity matrix, which is a square matrix with ones along its diagonal
and zeroes elsewhere. For example

 
1 0 0
0 1 0
0 0 1

The matrix denoted by 0m×n is a matrix filled with zeroes with m rows and n columns.
The T in AT denotes transpose, which flips the matrix across its diagonal such that the rows become
columns and vice versa.
The † in B† denotes the Moore-Penrose pseudoinverse given by B† = (BT B)−1 BT . The pseudoin-
verse is used when the matrix is nonsquare and thus not invertible to produce a close approximation
of an inverse in the least squares sense.
 Night sky above Dufour Street in Santa Cruz, CA

9. State-space controllers

R Chapters from here on use the frccontrol Python package to demonstrate the concepts dis-
cussed and perform the complex math required. See appendix B for how to install it.

When we want to command a system to a set of states, we design a controller with certain control laws
to do it. PID controllers use the system outputs with proportional, integral, and derivative control
laws. In state-space, we also have knowledge of the system states so we can do better.

Modern control theory uses state-space representation to model and control systems. State-space
representation models systems as a set of state, input, and output variables related by first-order
differential equations that describe how the system’s state changes over time given the current states
and inputs.

9.1 From PID control to model-based control

As mentioned before, controls engineers have a more general framework to describe control theory
than just PID control. PID controller designers are focused on fiddling with controller parameters
relating to the current, past, and future error rather than the underlying system states. Integral control
is a commonly used tool, and some people use integral action as the majority of the control action.
While this approach works in a lot of situations, it is an incomplete view of the world.

Model-based control has a completely different mindset. Controls designers using model-based con-
trol care about developing an accurate model of the system, then driving the states they care about
to zero (or to a reference). Integral control is added with uerror estimation if needed to handle
model uncertainty, but we prefer not to use it because its response is hard to tune and some of its
destabilizing dynamics aren’t visible during simulation.
 86 Chapter 9. State-space controllers

9.2 What is a dynamical system?

A dynamical system is a system whose motion varies according to a set of differential equations. A
dynamical system is considered linear if the differential equations describing its dynamics consist
only of linear operators. Linear operators are things like constant gain multiplications, derivatives,
and integrals. You can define reasonably accurate linear models for pretty much everything you’ll see
in FRC with just those relations.
But let’s say you have a DC brushed motor hooked up to a power supply and you applied a constant
voltage to it from rest. The motor approaches a steady-state angular velocity, but the shape of the
angular velocity curve over time isn’t a line. In fact, it’s a decaying exponential curve akin to

( )
ω = ωmax 1 − e−t

where ω is the angular velocity and ωmax is the maximum angular velocity. If DC brushed motors
are said to behave linearly, then why is this?
Linearity refers to a system’s equations of motion, not its time domain response. The equation defin-
ing the motor’s change in angular velocity over time looks like

ω̇ = −aω + bV

where ω̇ is the derivative of ω with respect to time, V is the input voltage, and a and b are constants
specific to the motor. This equation, unlike the one shown before, is actually linear because it only
consists of multiplications and additions relating the input V and current state ω.
Also of note is that the relation between the input voltage and the angular velocity of the output
shaft is a linear regression. You’ll see why if you model a DC brushed motor as a voltage source
and generator producing back-EMF (in the equation above, bV corresponds to the voltage source
and −aω corresponds to the back-EMF). As you increase the input voltage, the back-EMF increases
linearly with the motor’s angular velocity. If there was a friction term that varied with the angular
velocity squared (air resistance is one example), the relation from input to output would be a curve.
Friction that scales with just the angular velocity would result in a lower maximum angular velocity,
but because that term can be lumped into the back-EMF term, the response is still linear.

9.3 State-space notation

9.3.1 What is state-space?
Recall from last chapter that 2D space has two axes: x and y. We represent locations within this
space as a pair of numbers packaged in a vector, and each coordinate is a measure of how far to
move along the corresponding axis. State-space is a Cartesian coordinate system with an axis for
each state variable, and we represent locations within it the same way we do for 2D space: with a list
of numbers in a vector. Each element in the vector corresponds to a state of the system.
In addition to the state, inputs and outputs are represented as vectors. Since the mapping from the
current states and inputs to the change in state is a system of equations, it’s natural to write it in matrix
form.

9.3.2 Benefits over classical control

State-space notation provides a more convenient and compact way to model and analyze systems
with multiple inputs and outputs. For a system with p inputs and q outputs, we would have to write
 9.4 Controllability 87

q × p Laplace transforms to represent it. Not only is the resulting algebra unwieldy, but it only works
for linear systems. Including nonzero initial conditions complicates the algebra even more. State-
space representation uses the time domain instead of the Laplace domain, so it can model nonlinear
systems1 and trivially supports nonzero initial conditions.
Students are still taught classical control first because it provides a framework within which to under-
stand the results we get from the fancy mathematical machinery of modern control.

9.3.3 The equations

Below are the continuous and discrete versions of state-space notation.
Definition 9.3.1 — State-space notation.

ẋ = Ax + Bu (9.1)
y = Cx + Du (9.2)

xk+1 = Axk + Buk (9.3)

yk+1 = Cxk + Duk (9.4)

A system matrix x state vector

B input matrix u input vector
C output matrix y output vector
D feedthrough matrix

Matrix Rows × Columns Matrix Rows × Columns

A states × states x states × 1
B states × inputs u inputs × 1
C outputs × states y outputs × 1
D outputs × inputs

Table 9.1: State-space matrix dimensions

In the continuous case, the change in state and the output are linear combinations of the state vector
and the input vector. The A and B matrices are used to map the state vector x and the input vector
u to a change in the state vector ẋ. The C and D matrices are used to map the state vector x and
the input vector u to an output vector y.

9.4 Controllability
State controllability implies that it is possible – by admissible inputs – to steer the states from any
initial value to any final value within some finite time window.

1
This book focuses on analysis and control of linear systems. See chapter 12 for more on nonlinear control.
 88 Chapter 9. State-space controllers

Theorem 9.4.1 — Controllability. A continuous time-invariant linear state-space model is

controllable if and only if

([ ])
rank B AB A2 B · · · An−1 B = n (9.5)

where rank is the number of linearly independent rows in a matrix and n is the number of state
variables.

The matrix in equation (9.5) being rank-deficient means the inputs cannot apply transforms along all
axes in the state-space; the transformation the matrix represents is collapsed into a lower dimension.
The condition number of the controllability matrix C is defined as σσmax (C)
min (C)
where σmax is the max-
2
imum singular value and σmin is the minimum singular value. As this number approaches infinity,
one or more of the states becomes uncontrollable. This number can also be used to tell us which actu-
ators are better than others for the given system; a lower condition number means that the actuators
have more control authority.

9.5 Observability
Observability is a measure for how well internal states of a system can be inferred by knowledge of
its external outputs. The observability and controllability of a system are mathematical duals (i.e., as
controllability proves that an input is available that brings any initial state to any desired final state,
observability proves that knowing enough output values provides enough information to predict the
initial state of the system).
Theorem 9.5.1 — Observability. A continuous time-invariant linear state-space model is ob-
servable if and only if

 
C
 
CA
 
rank   = n
.. (9.6)
 .
CA n−1

where rank is the number of linearly independent rows in a matrix and n is the number of state
variables.

The matrix in equation (9.6) being rank-deficient means the outputs do not contain contributions from
every state. That is, not all states are mapped to a linear commbination in the output. Therefore, the
outputs alone are insufficient to estimate all the states.
The condition number of the observability matrix O is defined as σσmax (O)
min (O)
where σmax is the maxi-
2
mum singular value and σmin is the minimum singular value. As this number approaches infinity,
one or more of the states becomes unobservable. This number can also be used to tell us which
sensors are better than others for the given system; a lower condition number means the outputs
produced by the sensors are better indicators of the system state.

2
Singular values are a generalization of eigenvalues for nonsquare matrices.
 9.6 Closed-loop controller 89

9.6 Closed-loop controller

With the control law u = K(r − x), we can derive the closed-loop state-space equations. We’ll
discuss where this control law comes from in subsection 9.8.
First is the state update equation. Substitute the control law into equation (9.1).

ẋ = Ax + BK(r − x)
ẋ = Ax + BKr − BKx
ẋ = (A − BK)x + BKr (9.7)

Now for the output equation. Substitute the control law into equation (9.2).

y = Cx + D(K(r − x))
y = Cx + DKr − DKx
y = (C − DK)x + DKr (9.8)

Now, we’ll do the same for the discrete system. We’d like to know whether the system defined by
equation (9.3) operating with the control law uk = K(rk − xk ) converges to the reference rk .

xk+1 = Axk + Buk

xk+1 = Axk + B(K(rk − xk ))
xk+1 = Axk + BKrk − BKxk
xk+1 = Axk − BKxk + BKrk
xk+1 = (A − BK)xk + BKrk

Theorem 9.6.1 — Closed-loop state-space controller.

ẋ = (A − BK)x + BKr (9.9)

y = (C − DK)x + DKr (9.10)

xk+1 = (A − BK)xk + BKrk (9.11)

yk = (C − DK)xk + DKrk (9.12)

A system matrix K controller gain matrix

B input matrix x state vector
C output matrix r reference vector
D feedthrough matrix y output vector

Instead of commanding the system to a state using the vector u directly, we can now specify a vector of
desired states through r and the controller will choose values of u for us over time to make the system
 90 Chapter 9. State-space controllers

Matrix Rows × Columns Matrix Rows × Columns

A states × states x states × 1
B states × inputs u inputs × 1
C outputs × states y outputs × 1
D outputs × inputs r states × 1
K inputs × states

Table 9.2: Controller matrix dimensions

converge to the reference. For equation (9.9) to reach steady-state, the eigenvalues of A − BK must
be in the left-half plane. For equation (9.11) to have a bounded output, the eigenvalues of A − BK
must be within the unit circle.

The eigenvalues of A − BK are the poles of the closed-loop system. Therefore, the rate of conver-
gence and stability of the closed-loop system can be changed by moving the poles via the eigenvalues
of A − BK. A and B are inherent to the system, but K can be chosen arbitrarily by the controller
designer.

9.7 Pole placement

This is the practice of placing the poles of a closed-loop system directly to produce a desired response.
This can be done manually for state feedback controllers with controllable canonical form (see section
C.1). This can also be done manually for state observers with observable canonical form (see section
C.2).

In general, pole placement should only be used if you know what you’re doing. It’s much easier to let
LQR place the poles for you, then use those as a starting point for pole placement.

9.8 LQR
Instead of placing the poles of a closed-loop system manually, LQR design places the poles for us
based on acceptable error and control effort constraints. “LQR” stands for “Linear-Quadratic Reg-
ulator”. This method of controller design uses a quadratic function for the cost-to-go defined as the
sum of the error and control effort over time for the linear system ẋ = Ax + Bu.

∫∞
( )
J= xT Qx + uT Ru dt
0

where J represents a tradeoff between state excursion and control effort with the weighting factors
Q and R. LQR finds a control law u that minimizes the cost function. Q and R slide the cost along
a Pareto boundary between state tracking and control effort (see figure 9.1). Pareto optimality for
this problem means that an improvement in state tracking cannot be obtained without using more
control effort to do so. Also, a reduction in control effort cannot be obtained without sacrificing state
tracking performance. Pole placement, on the other hand, will have a cost anywhere on, above, or to
the right of the Pareto boundary (no cost can be inside the boundary).
9.8 LQR 91

Figure 9.1: Pareto boundary for LQR

The minimum of LQR’s cost function is found by setting the derivative of the cost function to zero
and solving for the control law u. However, matrix calculus is used instead of normal calculus to take
the derivative.

The feedback control law that minimizes J, which we’ll call the “optimal control law”, is shown in
theorem 9.8.1.
Theorem 9.8.1 — Optimal control law.

u = −Kx (9.13)

This means that optimal control can be achieved with simply a set of proportional gains on all the
states. This control law will make all states converge to zero assuming the system is controllable. To
converge to nonzero states, a reference vector r can be added to the state x.
Theorem 9.8.2 — Optimal control law with nonzero reference.

u = K(r − x) (9.14)

To use the control law, we need knowledge of the full state of the system. That means we either have
to measure all our states directly or estimate those we do not measure.

See appendix D.2 for how K is calculated in Python. If the result is finite, the controller is guaranteed
to be stable and robust with a phase margin of 60 degrees [20].

R LQR design’s Q and R matrices don’t need discretization, but the K calculated for continuous
time and discrete time systems will be different.
 92 Chapter 9. State-space controllers

9.8.1 Bryson’s rule

The next obvious question is what values to choose for Q and R. With Bryson’s rule, the diagonals
of the Q and R matrices are chosen based on the maximum acceptable value for each state and
actuator. The nondiagonal elements are zero. The balance between Q and R can be slid along the
Pareto boundary using a weighting factor ρ.

∫∞ ( [( )2 ( )2 ] [( )2 ( )2 ])
x1 xn u1 un
J= ρ + ... + + + ... + dt
x1,max xn,max u1,max un,max
0

 ρ   1 
x21,max
0 ... 0 u21,max
0 ... 0
 ..   .. 
 ρ
.   1
. 
 0 x22,max   0 u22,max 
Q=  R= 
 .. ..   .. .. 
 . . 0   . . 0 
ρ 1
0 ... 0 x2n,max
0 ... 0 u2n,max

Small values of ρ penalize control effort while large values of ρ penalize state excursions. Large
values would be chosen in applications like fighter jets where performance is necessary. Spacecrafts
would use small values to conserve their limited fuel supply.

9.9 Case studies of controller design methods

This example uses the following second-order model for a CIM motor (a DC brushed motor).

[ ] [ ]
− Jb Kt
0 [ ] [ ]
A= J B= C= 1 0 D= 0
− KLe −RL
1
L

Figure 9.2 shows the response using poles placed at (0.1, 0) and (0.9, 0) and LQR with the following
cost matrices.

[ ]
1
0 [ 1
]
Q= 202 R=
1 122
0 402

LQR selected poles at (0.593, 0) and (0.955, 0). Notice with pole placement that as the current pole
moves left, the control effort becomes more aggressive.
 9.10 Model augmentation 93

Figure 9.2: Second-order CIM motor response with pole placement and LQR

9.10 Model augmentation

This section will teach various tricks for manipulating state-space models with the goal of demystify-
ing the matrix algebra at play. We will use the augmentation techniques discussed here in the section
on integral control.

Matrix augmentation is the process of appending rows or columns to a matrix. In state-space, there
are several common types of augmentation used: plant augmentation, controller augmentation, and
observer augmentation.

9.10.1 Plant augmentation

Plant augmentation is the process of adding a state to a model’s state vector and adding a correspond-
ing row to the A and B matrices.

9.10.2 Controller augmentation

Controller augmentation is the process of adding a column to a controller’s K matrix. This is often
done in combination with plant augmentation to add controller dynamics relating to a newly added
state.

9.10.3 Observer augmentation

Observer augmentation is closely related to plant augmentation. In addition to adding entries to the
observer matrix L, the observer is using this augmented plant for estimation purposes. This is better
explained with an example.

By augmenting the plant with a bias term with no dynamics (represented by zeroes in its rows in A
and B, the observer will attempt to estimate a value for this bias term that makes the model best
reflect the measurements taken of the real system. Note that we’re not collecting any data on this bias
term directly; it’s what’s known as a hidden state. Rather than our inputs and other states affecting
it directly, the observer determines a value for it based on what is most likely given the model and
 94 Chapter 9. State-space controllers

current measurements. We just tell the plant what kind of dynamics the term has and the observer
will estimate it for us.

9.10.4 Output augmentation

Output augmentation is the process of adding rows to the C matrix. This is done to help the controls
designer visualize the behavior of internal states or other aspects of the system in MATLAB or Python
Control. C matrix augmentation doesn’t affect state feedback, so the designer has a lot of freedom
here. Noting that the output is defined as y = Cx + Du, The following row augmentations of C
may prove useful. Of course, D needs to be augmented with zeroes as well in these cases to maintain
the correct matrix dimensionality.
Since u = −Kx, augmenting C with −K makes the observer estimate the control input u applied.

y = Cx + Du
[ ] [ ] [ ]
y C D
= x+ u
u −K 0

This works because K has the same number of columns as states.

Various states can also be produced in the output with I matrix augmentation.

9.10.5 Examples
Snippet 9.1 shows how one packs together the following augmented matrix in Python.

[ ]
A−I B
C D

#!/ usr/bin/env python3

import numpy as np

def main ():

J = 7.7500e -05
b = 8.9100e -05
Kt = 0.0184
Ke = 0.0211
R = 0.0916
L = 5.9000e -05

# fmt: off
A = np. matrix ([[-b / J, Kt / J],
[-Ke / L, -R / L]])
B = np. matrix ([[0] ,
[1 / L]])
C = np. matrix ([[1 , 0]])
D = np. matrix ([[0]])
# fmt: on

print ("A =")

print (A)
print ("B =")
print (B)
 9.11 Feedforwards 95

print ("C =")

print (C)
print ("D =")
print (D)

tmp = np. concatenate (

(
np. concatenate ((A - np.eye(A.shape [0]) , B), axis =1) ,
np. concatenate ((C, D), axis =1) ,
),
axis =0,
)

print ("[A, B; C, D] =")

print (tmp)

if __name__ == " __main__ ":

main ()

Snippet 9.1. Matrix augmentation example

Section 9.12 demonstrates model augmentation for different types of integral control.

9.11 Feedforwards
Feedforwards are used to inject information about either the system’s dynamics (like a model does)
or the intended movement into a controller. Feedforward is generally used to handle parts of the
control actions we already know must be applied to make a system track a reference, then let the
feedback controller correct for at runtime what we do not or cannot know about the system. We will
present two ways of implementing feedforward for state feedback.

9.11.1 Steady-state feedforward

Steady-state feedforwards apply the control effort required to keep a system at the reference if it is no
longer moving (i.e., the system is at steady-state). The first steady-state feedforward converts desired
outputs to desired states.

xc = Nx yc

Nx converts desired outputs yc to desired states xc (also known as r). For steady-state, that is

xss = Nx yss (9.15)

The second steady-state feedforward converts the desired outputs y to the control input required at
steady-state.

uc = Nu yc

Nu converts the desired outputs y to the control input u required at steady-state. For steady-state,
that is
96 Chapter 9. State-space controllers

uss = Nu yss (9.16)

Continuous case
To find the control input required at steady-state, set equation (9.1) to zero.

ẋ = Ax + Bu
y = Cx + Du

0 = Axss + Buss
yss = Cxss + Duss

0 = ANx yss + BNu yss

yss = CNx yss + DNu yss

[ ] [ ]
0 ANx + BNu
= y
yss CNx + DNu ss
[ ] [ ]
0 ANx + BNu
=
1 CNx + DNu
[ ] [ ][ ]
0 A B Nx
=
1 C D Nu
[ ] [ ]† [ ]
Nx A B 0
=
Nu C D 1

where † is the Moore-Penrose pseudoinverse.

Discrete case
Now, we’ll do the same thing for the discrete system. To find the control input required at steady-state,
set equation (9.3) to zero.

xk+1 = Axk + Buk

yk = Cxk + Duk

xss = Axss + Buss

yss = Cxss + Duss
9.11 Feedforwards 97

0 = (A − I)xss + Buss
yss = Cxss + Duss

0 = (A − I)Nx yss + BNu yss

yss = CNx yss + DNu yss

[ ] [ ]
0 (A − I)Nx + BNu
= yss
yss CNx + DNu
[ ] [ ]
0 (A − I)Nx + BNu
=
1 CNx + DNu
[ ] [ ][ ]
0 A − I B Nx
=
1 C D Nu
[ ] [ ]† [ ]
Nx A−I B 0
=
Nu C D 1

where † is the Moore-Penrose pseudoinverse.

Deriving steady-state input

Now, we’ll find an expression that uses Nx and Nu to convert the reference r to a control input
feedforward uf f . Let’s start with equation (9.15).

xss = Nx yss
N†x xss = yss

Now substitute this into equation (9.16).

uss = Nu yss
uss = Nu (N†x xss )
uss = Nu N†x xss

uss and xss are also known as uf f and r respectively.

uf f = Nu N†x r

So all together, we get theorem 9.11.1.

 98 Chapter 9. State-space controllers

Theorem 9.11.1 — Steady-state feedforward.

Continuous:
[ ] [ ]† [ ]
Nx A B 0
= (9.17)
Nu C D 1
Discrete:
[ ] [ ]† [ ]
Nx A−I B 0
= (9.18)
Nu C D 1

uf f = Nu N†x r (9.19)

In the augmented matrix, B should contain one column corresponding to an actuator and C should
contain one row whose output will be driven by that actuator. More than one actuator or output can
be included in the computation at once, but the result won’t be the same as if they were computed
independently and summed afterward.
After computing the feedforward for each actuator-output pair, the respective collections of Nx
and Nu matrices can summed to produce the combined feedforward.

If the augmented matrix in theorem 9.11.1 is square (number of inputs = number of outputs), the
normal matrix inverse can be used instead.

9.11.2 Two-state feedforward

Let’s start with the equation for the reference dynamics

rk+1 = Ark + Buf f

where uf f is the feedforward input. Note that this feedforward equation does not and should not
take into account any feedback terms. We want to find the optimal uf f such that we minimize the
tracking error between rk+1 and rk .

rk+1 − Ark = Buf f

To solve for uf f , we need to take the inverse of the nonsquare matrix B. This isn’t possible, but we
can find the pseudoinverse given some constraints on the state tracking error and control effort. To
find the optimal solution for these sorts of trade-offs, one can define a cost function and attempt to
minimize it. To do this, we’ll first solve the expression for 0.

0 = Buf f − (rk+1 − Ark )

This expression will be the state tracking cost we use in our cost function.
Our cost function will use an H2 norm with Q as the state cost matrix with dimensionality states ×
states and R as the control input cost matrix with dimensionality inputs × inputs.

J = (Buf f − (rk+1 − Ark ))T Q(Buf f − (rk+1 − Ark )) + uTff Ruf f

9.11 Feedforwards 99

R rk+1 − Ark will only return a nonzero vector if the reference isn’t following the system dynam-
ics. If it is, the feedback controller already compensates for it. This feedforward compensates
for any unmodeled dynamics reflected in how the reference is changing (or not changing). In
the case of a constant reference, the feedforward opposes any system dynamics that would
change the state over time.

The following theorems will be needed to find the minimum of J.

∂xT Ax
Theorem 9.11.2 ∂x = 2Ax where A is symmetric.

∂(Ax+b)T C(Dx+e)
Theorem 9.11.3 ∂x = AT C(Dx + e) + DT CT (Ax + b)

∂(Ax+b)T C(Ax+b)
Corollary 9.11.4 ∂x = 2AT C(Ax + b) where C is symmetric.
Proof:
∂(Ax + b)T C(Ax + b)
= AT C(Ax + b) + AT CT (Ax + b)
∂x
∂(Ax + b)T C(Ax + b)
= (AT C + AT CT )(Ax + b)
∂x

C is symmetric, so

∂(Ax + b)T C(Ax + b)

= (AT C + AT C)(Ax + b)
∂x
∂(Ax + b)T C(Ax + b)
= 2AT C(Ax + b)
∂x

Given theorem 9.11.2 and corollary 9.11.4, find the minimum of J by taking the partial derivative
with respect to uf f and setting the result to 0.

∂J
= 2BT Q(Buf f − (rk+1 − Ark )) + 2Ruf f
∂uf f
0 = 2BT Q(Buf f − (rk+1 − Ark )) + 2Ruf f
0 = BT Q(Buf f − (rk+1 − Ark )) + Ruf f
0 = BT QBuf f − BT Q(rk+1 − Ark ) + Ruf f
BT QBuf f + Ruf f = BT Q(rk+1 − Ark )
(BT QB + R)uf f = BT Q(rk+1 − Ark )
uf f = (BT QB + R)−1 BT Q(rk+1 − Ark )

Theorem 9.11.5 — Two-state feedforward.

uf f = Kf f (rk+1 − Ark ) (9.20)

T −1 T
where Kf f = (B QB + R) B Q (9.21)
 100 Chapter 9. State-space controllers

Figure 9.3: Second-order CIM motor response with various feedforwards

If control effort is considered inexpensive, R ≪ Q and uf f approaches corollary 9.11.6.

Corollary 9.11.6 — Two-state feedforward with inexpensive control effort.

uf f = Kf f (rk+1 − Ark ) (9.22)

where Kf f = (BT QB)−1 BT Q (9.23)

R If the cost matrix Q isn’t included in the cost function (that is, Q is set to the identity matrix),
Kf f becomes the Moore-Penrose pseudoinverse of B given by B† = (BT B)−1 BT .

9.11.3 Case studies of feedforwards

Second-order CIM motor model
Each feedforward implementation has advantages. The steady-state feedforward allows using specific
actuators to maintain the reference at steady-state. The two-state feedforward doesn’t support this,
but can be used for reference tracking as well with the same tuning parameters as LQR design. Figure
9.3 shows both types of feedforwards applied to a second-order CIM motor model.
The two-state feedforward isn’t as effective in figure 9.3 because the R matrix penalized control effort.
If the R cost matrix is removed from the two-state feedforward calculation, the reference tracking is
much better (see figure 9.4).

9.11.4 Feedforwards for unmodeled dynamics

While the feedforwards previously mentioned only handle modeled dynamics, one can also include
feedforwards for unmodeled dynamics separately. Consider an elevator model which doesn’t include
gravity. A constant voltage offset can be used compensate for this. The feedforward takes the form
of a voltage constant because voltage is proportional to force applied, and the force is acting in only
one direction at all times.
 9.12 Integral control 101

Figure 9.4: Second-order CIM motor response with two-state feedforwards

uf f = Vapp (9.24)

where Vapp is a constant. Another feedforward holds a single-jointed arm steady in the presence of
gravity. It has the following form.

uf f = Vapp cos θ (9.25)

where Vapp is the voltage required to keep the single-jointed arm level with the ground, and θ is
the angle of the arm relative to the ground. Therefore, the force applied is greatest when the arm is
parallel with the ground and zero when the arm is perpendicular to the ground (at that point, the joint
supports all the weight).
Note that the elevator model could be augmented easily enough to include gravity and still be linear,
but this wouldn’t work for the single-jointed arm since a trigonometric function is required to model
the gravitational force in the arm’s rotating reference frame3 .

9.12 Integral control

A common way of implementing integral control is to add an additional state that is the integral of
the error of the variable intended to have zero steady-state error.
There are two drawbacks to this method. First, there is integral windup on a unit step input. That
is, the integrator accumulates even if the system is tracking the model correctly. The second is
demonstrated by an example from Jared Russell of FRC team 254. Say there is a position/velocity
trajectory for some plant to follow. Without integral control, one can calculate a desired Kx to use as
the control input. As a result of using both desired position and velocity, reference tracking is good.
3
While the applied torque of the motor is constant throughout the arm’s range of motion, the torque caused by gravity
in the opposite direction varies according to the arm’s angle.
 102 Chapter 9. State-space controllers

With integral control added, the reference is always the desired position, but there is no way to tell
the controller the desired velocity.
Consider carefully whether integral control is necessary. One can get relatively close without integral
control, and integral adds all the issues listed above. Below, it is assumed that the controls designer
has determined that integral control will be worth the inconvenience.
There are three methods FRC team 971 has used over the years:
1. Augment the plant as described earlier. For an arm, one would add an “integral of position”
state.
2. Add an integrator to the output of the controller, then estimate the control effort being applied.
971 has called this Delta U control. The upside is that it doesn’t have the windup issue described
above; the integrator only acts if the system isn’t behaving like the model, which was the original
intent. The downside is working with it is very confusing.
3. Estimate an “error” in the observer and compensate for it. This quantity is the difference
between what was applied and what was observed to happen. To use it, you simply add it to
your control input and it will converge.
We’ll present the first and third methods since the third is strictly better than the second.

9.12.1 Plant augmentation

We want to augment the system with an integral term that integrates the error e = r − y = r − Cx.

∫
xI = e dt

ẋI = e = r − Cx

The plant is augmented as

[˙] [ ][] [ ] [ ]
x A 0 x B 0
= + u+ r
xI −C 0 xI 0 I
[˙] [ ][ ] [ ][ ]
x A 0 x B 0 u
= +
xI −C 0 xI 0 I r

The controller is augmented as

u = K(r − x) − KI xI
([ ] [ ])
[ ] r x
u = K KI −
0 xI

9.12.2 U error estimation

Let uerror be the difference between the input actually applied to a system and the desired input. The
uerror term is then added to the system as follows.

ẋ = Ax + B (u + uerror )
9.12 Integral control 103

ẋ = Ax + Bu + Buerror

For a multiple-output system, this would be

ẋ = Ax + Bu + Berror uerror

where Berror is the column vector that maps uerror to changes in the rest of the state the same way
B does for u. Berror is only a column of B if uerror corresponds to an existing input within u.
The plant is augmented as

[ ] [ ][ ] [ ]
x˙ A Berror x B
= + u
uerror 0 0 uerror 0
[ ]
[ ] x
y= C 0 + Du
uerror

With this model, the observer will estimate both the state and the uerror term. The controller is
augmented similarly. r is augmented with a zero for the goal uerror term.

u = K (r − x) − kerror uerror
([ ] [ ])
[ ] r x
u = K kerror −
0 uerror

where kerror is a column vector with a 1 in a given row if uerror should be applied to that input or a
0 otherwise.
This process can be repeated for an arbitrary error which can be corrected via some linear combina-
tion of the inputs.
This page intentionally left blank
 Chaparral by Merril Apartments at UCSC

10. Digital control

The complex plane discussed so far deals with continuous systems. In decades past, plants and con-
trollers were implemented using analog electronics, which are continuous in nature. Nowadays, mi-
croprocessors can be used to achieve cheaper, less complex controller designs. Discretization con-
verts the continuous model we’ve worked with so far from a set of differential equations like

ẋ = x − 3 (10.1)

to a set of difference equations like

xk+1 = xk + (xk − 3)∆T (10.2)

where the difference equation is run with some update period denoted by T , by ∆T , or sometimes
sloppily by dt.
While higher order terms of a differential equation are derivatives of the state variable (e.g., ẍ in
relation to equation (10.1)), higher order terms of a difference equation are delayed copies of the
state variable (e.g., xk−1 with respect to xk in equation (10.2)).

10.1 Phase loss

However, discretization has drawbacks. Since a microcontroller performs discrete steps, there is a
sample delay that introduces phase loss in the controller. Phase loss is the reduction of phase margin
that occurs in digital implementations of feedback controllers from sampling the continuous system
at discrete time intervals. As the sample rate of the controller decreases, the phase margin decreases
according to − T2 ω where T is the sample period and ω is the frequency of the system dynamics.
Instability occurs if the phase margin of the system reaches zero. Large amounts of phase loss can
make a stable controller in the continuous domain become unstable in the discrete domain. Here are
a few ways to combat this.
 106 Chapter 10. Digital control

• Run the controller with a high sample rate.

• Designing the controller in the analog domain with enough phase margin to compensate for
any phase loss that occurs as part of discretization.
• Convert the plant to the digital domain and design the controller completely in the digital
domain.

10.2 s-plane to z-plane

Transfer functions are converted to impulse responses using the Z-transform. The s-plane’s LHP
maps to the inside of a unit circle in the z-plane. Table 10.1 contains a few common points and figure
10.1 shows the mapping visually.

s-plane z-plane
(0, 0) (1, 0)
imaginary axis edge of unit circle
(−∞, 0) (0, 0)

Table 10.1: Mapping from s-plane to z-plane

Figure 10.1: Mapping of axes from s-plane (left) to z-plane (right)

10.2.1 z-plane stability

Eigenvalues of a system that are within the unit circle are stable, but why is that? Let’s consider a
scalar equation xk+1 = axk . a < 1 makes xk+1 converge to zero. The same applies to a complex
number like z = x + yi for xk+1 = zxk . If the magnitude of the complex number z is less than
one, xk+1 will converge to zero. Values with a magnitude of 1 oscillate forever because xk+1 never
decays.

10.2.2 z-plane behavior

As ω increases in s = jω, a pole in the z-plane moves around the perimeter of the unit circle. Once
it hits ω2s (half the sampling frequency) at (−1, 0), the pole wraps around. This is due to poles faster
than the sample frequency folding down to below the sample frequency (that is, higher frequency
signals alias to lower frequency ones).
You may notice that poles can be placed at (0, 0) in the z-plane. This is known as a deadbeat controller.
An Nth -order deadbeat controller decays to the reference in N timesteps. While this sounds great,
 10.2 s-plane to z-plane 107

there are other considerations like control effort, robustness, and noise immunity. These will be
discussed in more detail with LQR and LQE.
If poles from (1, 0) to (0, 0) on the x-axis approach infinity, then what do poles from (−1, 0) to
(0, 0) represent? Them being faster than infinity doesn’t make sense. Poles in this location exhibit
oscillatory behavior similar to complex conjugate pairs. See figures 10.2 and 10.3. The jaggedness
of these signals is due to the frequency of the system dynamics being above the Nyquist frequency.
The discretized signal doesn’t have enough samples to reconstruct the continuous system’s dynamics.

Figure 10.2: Single poles in various locations in Figure 10.3: Complex conjugate poles in vari-
z-plane ous locations in z-plane

10.2.3 Nyquist frequency

To completely reconstruct a signal, the Nyquist-Shannon sampling theorem states that it must be
sampled at a frequency at least twice the maximum frequency it contains. The highest frequency a
given sample rate can capture is called the Nyquist frequency, which is half the sample frequency.
This is why recorded audio is sampled at 44.1k Hz. The maximum frequency a typical human can
hear is about 20 kHz, so the Nyquist frequency is 40 kHz. (44.1kHz in particular was chosen for
unrelated historical reasons.)
Frequencies above the Nyquist frequency are folded down across it. The higher frequency and the
folded down lower frequency are said to alias each other1 . Figure 10.4 provides a demonstration of
aliasing.
The effect of these high-frequency aliases can be reduced with a low-pass filter (called an anti-aliasing
filter in this application).

1 def
The aliases of a frequency f can be expressed as falias (N ) = |f − N fs |. For example, if a 200 Hz sine wave is
sampled at 150 Hz, the observer will see a 50 Hz signal instead of a 200 Hz one.
 108 Chapter 10. Digital control

Figure 10.4: The samples of two sine waves can be identical when at least one of them is at a frequency
above half the sample rate. In this case, the 2 Hz sine wave is above the Nyquist frequency 1.5 Hz.

10.3 Discretization methods

Discretization is done using a zero-order hold. That is, the system state is only updated at discrete
intervals and it’s held constant between samples (see figure 10.5). The exact method of applying this
uses the matrix exponential, but this can be computationaly expensive. Instead, approximations such
as the following are used.
1. Forward Euler method. This is defined as yn+1 = yn + f (tn , yn )∆t.
2. Backward Euler method. This is defined as yn+1 = yn + f (tn+1 , yn+1 )∆t.
−1
3. Bilinear transform. The first-order bilinear approximation is s = T2 1−z
1+z −1
.
where the function f (tn , yn ) is the slope of y at n and T is the sample period for the discrete system.
Each of these methods is essentially finding the area underneath a curve. The forward and backward
Euler methods use rectangles to approximate that area while the bilinear transform uses trapezoids
(see figures 10.6 and 10.7). Since these are approximations, there is distortion between the real
discrete system’s poles and the approximate poles. This is in addition to the phase loss introduced
by discretizing at a given sample rate in the first place. For fast-changing systems, this distortion can
quickly lead to instability.
10.3 Discretization methods 109

Figure 10.5: Zero-order hold of a system response

Figure 10.6: Discretization methods applied to velocity data

 110 Chapter 10. Digital control

Figure 10.7: Position plot of discretization methods applied to velocity data

10.4 Effects of discretization on controller performance

Running a feedback controller at a faster update rate doesn’t always mean better control. In fact,
you may be using more computational resources than you need. However, here are some reasons for
running at a faster update rate.
Firstly, if you have a discrete model of the system, that model can more accurately approximate the
underlying continuous system. Not all controllers use a model though.
Secondly, the controller can better handle fast system dynamics. If the system can move from its
initial state to the desired one in under 250ms, you obviously want to run the controller with a period
less than 250ms. When you reduce the sample period, you’re making the discrete controller more
accurately reflect what the equivalent continuous controller would do (controllers built from analog
circuit components like op-amps are continuous).
Running at a lower sample rate only causes problems if you don’t take into account the response time
of your system. Some systems like heaters have outputs that change on the order of minutes. Running
a control loop at 1kHz doesn’t make sense for this because the plant input the controller computes
won’t change much, if at all, in 1ms.
Figures 10.8, 10.9, and 10.10 show simulations of the same controller for different sampling methods
and sample rates, which have varying levels of fidelity to the real system.
Forward Euler is numerically unstable for low sample rates. The bilinear transform is a significant
improvement due to it being a second-order approximation, but zero-order hold performs best due to
the matrix exponential including much higher orders (we’ll cover the matrix exponential in the next
section).
Table 10.2 compares the Taylor series expansions of the discretization methods presented so far
(these are found using polynomial division). The bilinear transform does best with accuracy trailing
off after the third-order term. Forward Euler has no second-order or higher terms, so it undershoots.
Backward Euler has twice the second-order term and overshoots the remaining higher order terms as
10.4 Effects of discretization on controller performance 111

Figure 10.8: Sampling methods for system simulation with T = 0.1s

Figure 10.9: Sampling methods for system simulation with T = 0.05s

 112 Chapter 10. Digital control

Figure 10.10: Sampling methods for system simulation with T = 0.01s

well.

Method Conversion Taylor series expansion

Zero-order hold z = eT s z = 1 + T s + 12 T 2 s2 + 16 T 3 s3 + . . .
1+ 12 T s
Bilinear z= 1− 21 T s
z = 1 + T s + 12 T 2 s2 + 14 T 3 s3 + . . .

Forward Euler z = 1 + Ts z = 1 + Ts
1
Reverse Euler z= 1−T s z = 1 + T s + T 2 s2 + T 3 s3 + . . .

Table 10.2: Taylor series expansions of discretization methods (scalar case). The zero-order hold
discretization method is exact.

10.5 The matrix exponential

The matrix exponential (and system discretization in general) is typically solved with a computer.
Python Control’s StateSpace.sample() with the “zoh” method (the default) does this.
Definition 10.5.1 — Matrix exponential. Let X be an n × n matrix. The exponential of X
denoted by eX is the n × n matrix given by the following power series.

∞
∑ 1 k
eX = X (10.3)
k!
k=0

where X0 is defined to be the identity matrix I with the same dimensions as X.

To understand why the matrix exponential is used in the discretization process, consider the set of
differential equations ẋ = Ax we use to describe systems (systems also have a Bu term, but we’ll
 10.6 The Taylor series 113

ignore it for clarity). The solution to this type of differential equation uses an exponential. Since we
are using matrices and vectors here, we use the matrix exponential.

x(t) = eAt x0

where x0 contains the initial conditions. If the initial state is the current system state, then we can
describe the system’s state over time as

xk+1 = eAT xk

where T is the time between samples xk and xk+1 .

10.6 The Taylor series

The definition for the matrix exponential and the approximations below all use the Taylor series
expansion. The Taylor series is a method of approximating a function like et via the summation
of weighted polynomial terms like tk . et has the following Taylor series around t = 0.

∞ n
∑
t t
e =
n!
n=0

where a finite upper bound on the number of terms produces an approximation of et . As n increases,
the polynomial terms increase in power and the weights by which they are multiplied decrease. For
et and some other functions, the Taylor series expansion equals the original function for all values of
t as the number of terms approaches infinity2 . Figure 10.11 shows the Taylor series expansion of et
around t = 0 for a varying number of terms.
We’ll expand the first few terms of the Taylor series expansion in equation (10.3) for X = AT so
we can compare it with other methods.

∑
3
1 1 1
(AT )k = I + AT + A2 T 2 + A3 T 3
k! 2 6
k=0

Table 10.3 compares the Taylor series expansions of the discretization methods for the matrix case.
These use a more complex formula which we won’t present here.

Method Conversion Taylor series expansion

Zero-order hold Φ = eAT Φ = I + AT + 12 A2 T 2 + 16 A3 T 3 + . . .
( )( )−1
Bilinear Φ = I + 12 AT I − 21 AT Φ = I + AT + 12 A2 T 2 + 14 A3 T 3 + . . .
Forward Euler Φ = I + AT Φ = I + AT
Reverse Euler Φ = (I − AT )−1 Φ = I + AT + A2 T 2 + A3 T 3 + . . .

Table 10.3: Taylor series expansions of discretization methods (matrix case). The zero-order hold
discretization method is exact.

2
Functions for which their Taylor series expansion converges to and also equals it are called analytic functions.
 114 Chapter 10. Digital control

Figure 10.11: Taylor series expansions of et around t = 0 for n terms

Each of them has different stability properties. The bilinear transform preserves the (in)stability of
the continuous time system.

10.7 Zero-order hold for state-space

Given the following continuous time state space model

ẋ = Ac x + Bc u + w
y = Cc x + Dc u + v

where w is the process noise, v is the measurement noise, and both are zero-mean white noise sources
with covariances of Qc and Rc respectively. w and v are defined as normally distributed random
variables.

w ∼ N (0, Qc )
v ∼ N (0, Rc )

The model can be discretized as follows

xk+1 = Ad xk + Bd uk + wk
yk = Cd xk + Dd uk + vk

with covariances
10.7 Zero-order hold for state-space 115

wk ∼ N (0, Qd )
vk ∼ N (0, Rd )

Theorem 10.7.1 — Zero-order hold for state-space.

Ad = eAc T (10.4)
∫ T
Bd = eAc τ dτ Bc = A−1
c (Ad − I)Bc (10.5)
0
Cd = Cc (10.6)
Dd = Dc (10.7)
∫ T
T
Qd = eAc τ Qc eAc τ dτ (10.8)
τ =0
1
Rd = Rc (10.9)
T

where a subscript of d denotes discrete, a subscript of c denotes the continuous version of the
corresponding matrix, T is the sample period for the discrete system, and eAc T is the matrix
exponential of Ac .

See appendix D.3 for derivations.

Qd is computed as
 

−AT Qc  [ ]
T
0 A −ATd A−1
d Qd
e =
0 Ad

and Qd is the lower-right quadrant multiplied by the upper-right quadrant [18]. To compute Ad and
Bd in one step, one can utilize the following property.
 

A B [ ]
T
0 0 Ad Bd
e =
0 I
This page intentionally left blank
 Fake plant in hotel lobby in Ventura, CA

11. State-space model examples

Up to now, we’ve just been teaching what tools are available. Now, we’ll go into specifics on how to
apply them and provide advice on certain applications.
The code shown in each example can be obtained from frccontrol’s Git repository at https://
github.com/calcmogul/frccontrol/tree/master/examples. See appendix B for setup in-
structions.

11.1 Pendulum
11.1.1 Write model in state-space representation
Below is the model for a pendulum

g
θ̈ = − sin θ
l

where θ is the angle of the pendulum and l is the length of the pendulum.
Since state-space representation requires that only single derivatives be used, they should be broken up
as separate states. We’ll reassign θ̇ to be ω so the derivatives are easier to keep straight for state-space
representation.

g
ω̇ = − sin θ
l

Now separate the states.

θ̇ = ω
g
ω̇ = − sin θ
l
 118 Chapter 11. State-space model examples

Since this model is nonlinear, we should linearize it. We will use the small angle approximation
(sin θ = θ for small values of θ).

θ̇ = ω
g
ω̇ = − θ
l

Now write the model in state-space representation.

[˙] [ ][ ]
θ 0 1 θ
= (11.1)
ω − gl 0 ω

11.1.2 Add estimator for unmeasured states

For full state feedback, knowledge of all states is required. If not all states are measured directly, an
estimator can be used to supplement them.
For example, we may only be measuring θ in the pendulum example, not θ̇, so we’ll need to estimate
the latter. The C matrix the observer would use in this case is

[ ]
C= 1 0

11.1.3 Implement controller

Use Bryson’s rule when making the performance vs control effort trade-off. Optimizing for perfor-
mance will get you to the reference as fast as possible while optimizing control effort will get you
to the reference in the most “fuel-efficient” way possible. The latter, for example, would potentially
avoid voltage drops from motor usage on robots with a limited power supply, but the result would be
slower to reach the reference.

11.1.4 Simulate model/controller

This can be done in any platform supporting numerical computation. Common choices are MATLAB,
v-REP, or Python. Tweak the LQR gains as necessary.
If you’re comfortable enough with it, you can use the controller designed by LQR as a starting point
and tweak the pole locations after that with pole placement to produce the desired response.
Simulating a closed-loop system
Recall equation (9.9) where a closed-loop system is written as ẋ = (A − BK)x + BKr. In the
open-loop system, our control input u was a vector of system inputs. In the closed-loop system, our
control input u is now a vector of reference states r. To use this form in simulation, the corresponding
state-space matrices, which we’ll denote with apostrophes, would be

A′ = A − BK
B′ = BK
C′ = C − DK
D′ = DK
 11.2 Elevator 119

11.1.5 Verify pole locations

Check the pole locations as a sanity check and to potentially gain an intuition for the chosen pole
locations.

11.1.6 Unit test

Write unit tests to test the model performance and robustness with different initial conditions and
references. For C++, we recommend Google Test.

11.1.7 Test on real system

Try the controller on a real system with low maximum control inputs for safety. The control inputs
can be increased after verifying the sensors function and mechanisms move the correct direction.

11.2 Elevator
11.2.1 Continuous state-space model
The position and velocity of the elevator can be written as

ẋm = vm (11.2)
v̇m = am (11.3)

where by equation (4.17),

GKt G2 Kt
am = V − vm
Rrm Rr2 mKv

Substitute this into equation (11.3).

GKt G2 Kt
v̇m = V − vm
Rrm Rr2 mKv
G2 Kt GKt
v̇m =− 2 vm + V (11.4)
Rr mKv Rrm

Theorem 11.2.1 — Elevator state-space model.

ẋ = Ax + Bu
y = Cx + Du
[ ]
x
x= y=x u=V
vm
[ ] [ ]
0 1 0 [ ]
A= G2 Kt B = GKt C= 1 0 D=0 (11.5)
0 − Rr2 mKv Rrm
 120 Chapter 11. State-space model examples

11.2.2 Model augmentation

As per subsection 9.12.2, we will now augment the model so a uerror term is added to the control
input.
The plant and observer augmentations should be performed before the model is discretized. After the
controller gain is computed with the unaugmented discrete model, the controller may be augmented.
Therefore, the plant and observer augmentations assume a continuous model and the controller aug-
mentation assumes a discrete controller.

 
x
xaug =  vm  y = x u = V
uerror
[ ] [ ]
A B B [ ]
Aaug = Baug = Caug = C 0 Daug = D (11.6)
01×2 0 0

[ ]
[] r
Kaug = K 1 raug = (11.7)
0

This will compensate for unmodeled dynamics such as gravity. However, using a constant feedfor-
ward to counteract gravity is preferred over this method.

11.2.3 Simulation
Python Control will be used to discretize the model and simulate it. One of the frccontrol examples1
creates and tests a controller for it.

R Python Control currently doesn’t support finding the transmission zeroes of MIMO systems with
a different number of inputs than outputs, so control.pzmap() and frccontrol.System.plot_pzmaps()
fail with an error if Slycot isn’t installed.

Figure 11.1 shows the pole-zero maps for the open-loop system, closed-loop system, and observer.
Figure 11.2 shows the system response with them.
Figure 11.2 shows the system response.

11.2.4 Implementation
The script linked above also generates two files: ElevatorCoeffs.h and ElevatorCoeffs.cpp. These can
be used with the WPILib StateSpacePlant, StateSpaceController, and StateSpaceObserver classes in
C++ and Java. A C++ implementation of this elevator controller is available online2 .

R Instead of implementing uerror estimation to compensate for gravity, one can apply a constant
voltage feedforward since input voltage is proportional to force and gravity is a constant force.

1
https://github.com/calcmogul/frccontrol/blob/master/examples/elevator.py
2
https://github.com/calcmogul/allwpilib/tree/state-space/wpilibcExamples/src/main/cpp/
examples/StateSpaceElevator
11.2 Elevator 121

Figure 11.1: Elevator pole-zero maps

Figure 11.2: Elevator response

 122 Chapter 11. State-space model examples

11.3 Flywheel
11.3.1 Continuous state-space model
By equation (4.20)

G2 Kt GKt
ω̇f = − ωf + V
Kv RJ RJ

Theorem 11.3.1 — Flywheel state-space model.

ẋ = Ax + Bu
y = Cx + Du

x = ωf y = ωf u=V
2
A = −K
G Kt
v RJ
B= GKt
RJ C=1 D=0 (11.8)

11.3.2 Model augmentation

As per subsection 9.12.2, we will now augment the model so a uerror term is added to the control
input.
The plant and observer augmentations should be performed before the model is discretized. After the
controller gain is computed with the unaugmented discrete model, the controller may be augmented.
Therefore, the plant and observer augmentations assume a continuous model and the controller aug-
mentation assumes a discrete controller.

[ ]
ωf
x= y = ωf u=V
uerror

[ ] [ ]
A B B [ ]
Aaug = Baug = Caug = C 0 Daug = D (11.9)
0 0 0

[ ]
[ ] r
Kaug = K 1 raug = (11.10)
0

This will compensate for unmodeled dynamics such as projectiles slowing down the flywheel.

11.3.3 Simulation
Python Control will be used to discretize the model and simulate it. One of the frccontrol examples3
creates and tests a controller for it.
Figure 11.3 shows the pole-zero maps for the open-loop system, closed-loop system, and observer.
Figure 11.4 shows the system response with them.
Notice how the control effort when the reference is reached is nonzero. This is the two-state feedfor-
ward compensating for the system dynamics attempting to slow the flywheel down when no voltage
is applied.

3
https://github.com/calcmogul/frccontrol/blob/master/examples/flywheel.py
11.3 Flywheel 123

Figure 11.3: Flywheel pole-zero maps

Figure 11.4: Flywheel response

 124 Chapter 11. State-space model examples

11.3.4 Implementation
The script linked above also generates two files: FlywheelCoeffs.h and FlywheelCoeffs.cpp. These
can be used with the WPILib StateSpacePlant, StateSpaceController, and StateSpaceObserver classes
in C++ and Java. A C++ implementation of this flywheel controller is available online4 .

11.4 Drivetrain
11.4.1 Continuous state-space model
The position and velocity of each drivetrain side can be written as

ẋl = vl (11.11)
v̇l = v̇l (11.12)
ẋr = vr (11.13)
v̇r = v̇r (11.14)

By equations (4.28) and (4.29)

( ) ( )
1 rb2 1 rb2
v̇l = + (C1 vl + C2 Vl ) + − (C3 vr + C4 Vr )
m J m J
( ) ( )
1 r2 1 r2
v̇r = − b (C1 vl + C2 Vl ) + + b (C3 vr + C4 Vr )
m J m J

Theorem 11.4.1 — Drivetrain state-space model.

ẋ = Ax + Bu
y = Cx + Du
 
xl [ ] [ ]
 vl  xl V
 
x=  y= u= l
xr xr Vr
vr
   
0 ( 1 ) 0 ( 0 ) ( 0 ) ( 0 )
 rb2 rb2   1 r2 r2 
0 m − J C3  − Jb C4 
1 1
m + J C1 0  m + Jb C2 1
A=  B= 
m
0 0 0 1   0 0 
 ( 2
) ( 2
)  ( 2
) ( 2
) 
r r r r
m − J C1 0 m − J C2
1 1 1 1
0 b
m + J C3
b b
m + J C4
b
[ ]
1 0 0 0
C= D = 02×2
0 0 1 0
(11.15)
G2 K 2
where C1 = − KvlRrt2 , C2 = Gl K t
Rr , C3 = − KGvrRr
Kt
2 , and C4 =
Gr Kt
Rr .

4
https://github.com/calcmogul/allwpilib/tree/state-space/wpilibcExamples/src/main/cpp/
examples/StateSpaceFlywheel
 11.4 Drivetrain 125

11.4.2 Model augmentation

As per subsection 9.12.2, we will now augment the model so uerror terms are added to the control
inputs.
The plant and observer augmentations should be performed before the model is discretized. After the
controller gain is computed with the unaugmented discrete model, the controller may be augmented.
Therefore, the plant and observer augmentations assume a continuous model and the controller aug-
mentation assumes a discrete controller.
For this augmented model, the left and right wheel positions are filtered encoder positions and are
not adjusted for heading error. The turning velocity computed from the left and right velocities is
adjusted by the gyroscope angular velocity. The angular velocity uerror term is the angular velocity
error between the wheel speeds and the gyroscope measurement.

 
x [ ] [ ]
 uerror,l  y V
xaug 
=  yaug = u= l
uerror,r  ω Vr
uerror,angle

where ω is the angular rate of the robot’s center of mass measured by a gyroscope.


1
0 [ ]
Bω = 
−1
 Cω = 0 − 1
2rb 0 1
2rb

[ ] [ ]
A B Bω B
Aaug = Baug =
[ 03×4 0 ]
3×2 0 3×1 [03×2 ] (11.16)
C 02×3 D
Caug = Daug =
Cω 01×3 01×2

The augmentation of A with B maps the left and right wheel velocity uerror terms to their respective
states. The augmentation of A with Bω maps the angular velocity uerror term to corresponding
changes in the left and right wheel positions.
Cω maps the left and right wheel velocities to an angular velocity estimate that the observer can
compare against the gyroscope measurement.

 
[ ] r
1 0 0 [ ] 0
Kerror = Kaug = K Kerror raug  
= (11.17)
0 1 0 0
0

This will compensate for unmodeled dynamics such as understeer due to the wheel friction inherent
in skid-steer robots.

R The process noise for the angular velocity uerror term should be the encoder model uncertainty.
The augmented measurement noise term is obviously for the gyroscope measurement.
 126 Chapter 11. State-space model examples

Figure 11.5: Drivetrain pole-zero maps

11.4.3 Simulation
Python Control will be used to discretize the model and simulate it. One of the frccontrol examples5
creates and tests a controller for it.

R Python Control currently doesn’t support finding the transmission zeroes of MIMO systems with
a different number of inputs than outputs, so control.pzmap() and frccontrol.System.plot_pzmaps()
fail with an error if Slycot isn’t installed.

Figure 11.5 shows the pole-zero maps for the open-loop system, closed-loop system, and observer.
Figure 11.6 shows the system response with them.
Figure 11.6 shows the system response.
Given the high inertia in drivetrains, it’s better to drive the reference with a motion profile instead of
a step input for reproducibility.

11.4.4 Implementation
The script linked above also generates two files: DrivetrainCoeffs.h and DrivetrainCoeffs.cpp. These
can be used with the WPILib StateSpacePlant, StateSpaceController, and StateSpaceObserver classes
in C++ and Java. A C++ implementation of this drivetrain controller is available online6 .

5
https://github.com/calcmogul/frccontrol/blob/master/examples/drivetrain.py
6
https://github.com/calcmogul/allwpilib/tree/state-space/wpilibcExamples/src/main/cpp/
examples/StateSpaceDrivetrain
 11.5 Single-jointed arm 127

Figure 11.6: Drivetrain response

11.5 Single-jointed arm

11.5.1 Continuous state-space model
The position and velocity of the elevator can be written as

θ̇arm = ωarm (11.18)

ω̇arm = ω̇arm (11.19)

By equation (4.32)

G2 Kt GKt
ω̇arm = − ωarm + V
Kv RJ RJ

Theorem 11.5.1 — Single-jointed arm state-space model.

ẋ = Ax + Bu
y = Cx + Du
[ ]
θarm
x= y = θarm u = V
ωarm
[ ] [ ]
0 1 0 [ ]
A= G2 Kt B = GKt C= 1 0 D=0 (11.20)
0 − Kv RJ RJ

11.5.2 Model augmentation

As per subsection 9.12.2, we will now augment the model so a uerror term is added to the control
input.
 128 Chapter 11. State-space model examples

The plant and observer augmentations should be performed before the model is discretized. After the
controller gain is computed with the unaugmented discrete model, the controller may be augmented.
Therefore, the plant and observer augmentations assume a continuous model and the controller aug-
mentation assumes a discrete controller.

[ ]
x
xaug = y = θarm u = V
uerror

[ ] [ ]
A B B [ ]
Aaug = Baug = Caug = C 0 Daug = D (11.21)
01×2 0 0

[ ]
[ ] r
Kaug = K 1 raug = (11.22)
0

This will compensate for unmodeled dynamics such as gravity or other external loading from lifted ob-
jects. However, if only gravity compensation is desired, a feedforward of the form uf f = Vgravity cos θ
is preferred where Vgravity is the voltage required to hold the arm level with the ground and θ is the
angle of the arm with the ground.

11.5.3 Simulation
Python Control will be used to discretize the model and simulate it. One of the frccontrol examples7
creates and tests a controller for it.

R Python Control currently doesn’t support finding the transmission zeroes of MIMO systems with
a different number of inputs than outputs, so control.pzmap() and frccontrol.System.plot_pzmaps()
fail with an error if Slycot isn’t installed.

Figure 11.7 shows the pole-zero maps for the open-loop system, closed-loop system, and observer.
Figure 11.8 shows the system response with them.
Figure 11.8 shows the system response.

11.5.4 Implementation
The script linked above also generates two files: SingleJointedArmCoeffs.h and SingleJointedArm-
Coeffs.cpp. These can be used with the WPILib StateSpacePlant, StateSpaceController, and StateS-
paceObserver classes in C++ and Java. A C++ implementation of this single-jointed arm controller
is available online8 .

7
https://github.com/calcmogul/frccontrol/blob/master/examples/single_jointed_arm.py
8
https://github.com/calcmogul/allwpilib/tree/state-space/wpilibcExamples/src/main/cpp/
examples/StateSpaceSingleJointedArm
11.5 Single-jointed arm 129

Figure 11.7: Single-jointed arm pole-zero maps

Figure 11.8: Single-jointed arm response

 130 Chapter 11. State-space model examples

11.6 Rotating claw

11.6.1 Continuous state-space model
11.6.2 Simulation
 Trees by Interdisciplinary Sciences building at UCSC

12. Nonlinear control

While many tools exist for designing controllers for linear systems, all systems in reality are inherently
nonlinear. We’ll briefly mention some considerations for nonlinear systems.

12.1 Introduction
Recall from linear system theory that we defined systems as having the following form:

ẋ = Ax + Bu + Γw
y = Cx + Du + v

In this equation, A and B are constant matrices, which means they are both time-invariant and linear
(all transformations on the system state are linear ones, and those transformations remain the same
for all time). In nonlinear and time-variant systems, the state evolution and output are defined by
arbitrary functions of the current states and inputs.

ẋ = f (x, u, w)
y = h(x, u, v)

Nonlinear functions come up regularly when attempting to control the pose of a vehicle in the global
coordinate frame instead of the vehicle’s rotating local coordinate frame. Converting from one to
the other requires applying a rotation matrix, which consists of sine and cosine operations. These
functions are nonlinear.
 132 Chapter 12. Nonlinear control

12.2 Linearization
One way to control nonlinear systems is to linearize the model around a reference point. Then, all
the powerful tools that exist for linear controls can be applied. This is done by taking the partial
derivative of the functions.

∂f (x,0,0) ∂f (0,u,0) ∂h(x,0,0) ∂h(0,u,0)

A= ∂x B= ∂u C= ∂x D= ∂u

Linearization of a nonlinear equation is a Taylor series expansion to only the first-order terms (that
is, terms whose variables have exponents on the order of x1 ). This is where the small angle approxi-
mations for sin θ and cos θ (θ and 1 respectively) come from.
Higher order partial derivatives can be added to better approximate the nonlinear dynamics. We typ-
ically only linearize around equilibrium points1 because we are interested in how the system behaves
when perturbed from equilibrium. An FAQ on this goes into more detail [15]. To be clear though,
linearizing the system around the current state as the system evolves does give a closer approximation
over time.
Note that linearization with static matrices (that is, with a time-invariant linear system) only works
if the original system in question is feedback linearizable.

12.3 Lyapunov stability

Lyapunov stability is a fundamental concept in nonlinear control, so we’re going to give a brief
overview of what it is so students can research it further.
Since the state evolution in nonlinear systems is defined by a function rather than a constant matrix,
the system’s poles as determined by linearization move around. Nonlinear control uses Lyapunov
stability to determine if nonlinear systems are stable. From a linear control theory point of view,
Lyapunov stability says the system is stable if, for a given initial condition, all possible eigenvalues
of A from that point on remain in the left-half plane. However, nonlinear control uses a different
definition.
Essentially, Lyapunov stability means that the system trajectory can be kept arbitrarily close to the
origin by starting sufficiently close to it. Lyapunov’s direct method is concerned with finding a func-
tion representing the energy in a system to prove that the system is stable around an equilibrium point.
This can be used to prove a system’s open-loop stability as well as its closed-loop stability in the pres-
ence of a controller. Typically, these functions include the energy of the system or the derivatives of
the system state, which are then used to show the system decays to some ground state.
Lyapunov functions are merely sufficient to prove stability (as opposed to a specific Lyapunov function
being necessary). If one function doesn’t prove it, another candidate should be tried. For this reason,
we refer to these functions as Lyapunov candidate functions.

12.4 Control law for nonholonomic wheeled vehicle

Why would we need a nonlinear control law in addition to the linear ones we have used so far? If
we use the original approach with an LQR controller for left and right position and velocity states,
the controller only deals with the local pose. If the robot deviates from the path, there is no way for

1
Equilibrium points are points where ẋ = 0. At these points, the system is in steady-state.
 12.4 Control law for nonholonomic wheeled vehicle 133

the controller to correct and the robot may not reach the desired global pose. This is due to multiple
endpoints existing for the robot which have the same encoder path arc lengths.
Instead of using wheel path arc lengths (which are in the robot’s local coordinate frame), nonlinear
controllers like pure pursuit and Ramsete use global pose. The controller uses this extra information
to guide a linear reference tracker like an LQR controller back in by adjusting the references of the
LQR controller.

12.4.1 Ramsete controller

The paper Control of Wheeled Mobile Robots: An Experimental Overview describes a nonlinear con-
troller for a wheeled vehicle with unicycle-like kinematics; a global pose consisting of x, y, and θ;
and a desired pose consisting of xd , yd , and θd [19]. We’ll call it the Ramsete nonlinear control law
because that’s the acronym for the title of the book it came from in Italian (“Robotica Articolata e
Mobile per i SErvizi e le TEcnologie”).
Velocity and turning rate command derivation
The state tracking error e in the vehicle’s coordinate frame is defined as

    
e1 cos θ sin θ 0 xd − x
e2  = − sin θ cos θ 0  yd − y 
e3 0 0 1 θd − θ

where e1 is the tracking error in x, e2 is the tracking error in y, and e3 is the tracking error in θ. The
3 × 3 matrix is a rotation matrix that transforms the error in the pose (represented by xd − x, yd − y,
and θd − θ) from the global coordinate frame into the vehicle’s coordinate frame.
Multiplying the matrices out gives

e1 = (xd − x) cos θ + (yd − y) sin θ

e2 = −(xd − x) sin θ + (yd − y) cos θ
e 3 = θd − θ

e1 = (xd − x) cos θ + (yd − y) sin θ (12.1)

e2 = (yd − y) cos θ − (xd − x) sin θ (12.2)
e 3 = θd − θ (12.3)

We will use the following control laws u1 and u2 for velocity and turning rate respectively.

u1 = −k1 e1
(12.4)
u2 = −k2 vd sinc(e3 )e2 − k3 e3

sin e3
where k1 , k2 , and k3 are time-varying gains and sinc(e3 ) is defined as e3 .

Substitute equations (12.1), (12.2), and (12.3) into the control laws.

u1 = −k1 ((xd − x) cos θ + (yd − y) sin θ

134 Chapter 12. Nonlinear control

u2 = −k2 vd sinc(θd − θ)((yd − y) cos θ − (xd − x) sin θ) − k3 (θd − θ)

Our velocity and turning rate commands for the vehicle will use the following nonlinear transforma-
tion of the inputs.

v = vd cos e3 − u1
ω = ωd − u2

Substituting the control laws u1 and u2 into these equations gives

v = vd cos(θd − θ) − (−k1 ((xd − x) cos θ + (yd − y) sin θ)

v = vd cos(θd − θ) + k1 ((xd − x) cos θ + (yd − y) sin θ)

ω = ωd − (−k2 vd sinc(θd − θ)((yd − y) cos θ − (xd − x) sin θ) − k3 (θd − θ))

ω = ωd + k2 vd sinc(θd − θ)((yd − y) cos θ − (xd − x) sin θ) + k3 (θd − θ)

Theorem 12.4.1 Assuming that vd and ωd are bounded with bounded derivatives, and that
vd (t) → 0 or ωd (t) → 0 when t → ∞, the control laws in equation (12.4) globally asymptotically
stabilize the origin e = 0.
Proof:
To prove convergence, the paper previously mentioned uses the following Lyapunov function.

k2 2 e2
V = (e1 + e22 ) + 3
2 2

where k2 is a tuning constant, e1 is the tracking error in x, e2 is the tracking error in y, and e3 is
the tracking error in θ.
The time derivative along the solutions of the closed-loop system is nonincreasing since

V̇ = −k1 k2 e21 − k3 e23 ≤ 0

Thus, ∥e(t)∥ is bounded, V̇ (t) is uniformly continuous, and V (t) tends to some limit value. Using
the Barbalat lemma, V̇ (t) tends to zero [19].

Nonlinear controller equations

Let k2 = b.
 12.4 Control law for nonholonomic wheeled vehicle 135

Theorem 12.4.2 — Ramsete nonlinear control law.

v = vd cos(θd − θ) + k1 (vd , ωd )((xd − x) cos θ + (yd − y) sin θ) (12.5)

ω = ωd + bvd sinc(θd − θ)((yd − y) cos θ − (xd − x) sin θ) + k3 (vd , ωd )(θd − θ) (12.6)
√
k1 (vd (t), ωd (t)) = k3 (vd (t), ωd (t)) = 2ζ ωd2 (t) + bvd2 (t) (12.7)
sin(θd − θ)
sinc(θd − θ) = (12.8)
θd − θ

v velocity command vd desired velocity

ω turning rate command ωd desired turning rate
x actual x position in global coordinate frame xd desired x position
y actual y position in global coordinate frame yd desired y position
θ actual angle in global coordinate frame θd desired angle
b and ζ are tuning parameters where b > 0 and ζ ∈ (0, 1). Larger values of b make convergence
more aggressive (like a proportional term), and larger values of ζ provide more damping.

v and ω should be the references for a reference tracker for the drivetrain. This can be a linear
controller. x, y, and θ are obtained via a nonlinear pose estimator (see section 15.1 for how to
implement one).

12.4.2 Linear reference tracker

We need a velocity reference tracker for the nonlinear controller’s commands. Starting from equation
(11.15), we’ll derive two models for this purpose and compare their responses with a straight profile
and as part of a nonlinear trajectory follower.

Left/right velocity reference tracker

 
xl [ ] [ ]
 vl  xl V
 
x=  y= u= l
xr xr Vr
vr

   
0 ( 1 ) 0 ( 0 ) ( 0 ) ( 0 )
 2
rb rb2   1 r2 r2 
0 m − J C3  − Jb C4 
1 1
+ C1 0  m + Jb C2 1
A=  B= 
m J m
0 0 ) 0 ( 1 )   0 0 
 ( 2 2
 ( 2
) ( 2
) 
rb r r r
0 1
− C1 0 1
m + J C3
b
m − J C2
1 b 1
m + J C4
b
[ m J
]
1 0 0 0
C= D = 02×2
0 0 1 0

G2 K 2
where C1 = − KvlRrt2 , C2 = Gl Kt
Rr , C3 = − KGvrRr
Kt
2 , and C4 =
Gr Kt
Rr .

To obtain a left/right velocity reference tracker, we can just remove the position states from the model.
136 Chapter 12. Nonlinear control

Theorem 12.4.3 — Left/right velocity reference tracker.

[ ] [ ] [ ]
vl vl V
x= y= u= l
vr vr Vr

( ) ( )  ( ) ( ) 
rb2 rb2 rb2 rb2
1
+ J C1
1
− J C3 
1
+ J C2
1
− J C4 
A= ( m ) (m ) B= ( m ) (m )
rb2 rb2 rb2 rb2
−1
J C1
1
+ J C3
1
− J C2
1
+ J C4 (12.9)
[ ]
m m m m
1 0
C= D = 02×2
0 1

G2 K 2
where C1 = − KvlRrt2 , C2 = Gl K t
Rr , C3 = − KGvrRr
Kt
2 , and C4 =
Gr Kt
Rr .

See https://github.com/calcmogul/state-space-guide/blob/master/code/ramsete_
decoupled.py for an implementation.
Linear/angular velocity reference tracker
Since the Ramsete controller produces velocity and turning rate commands, it would be more conve-
nient if the states are velocity and turning rate instead of left and right wheel velocity. We’ll create a
second model that has velocity and angular velocity states and see how well it performs.

( ) ( )
1 rb2 1 rb2
v̇l = + (C1 vl + C2 Vl ) + − (C3 vr + C4 Vr )
m J m J
( ) ( )
1 rb2 1 rb2
v̇r = − (C1 vl + C2 Vl ) + + (C3 vr + C4 Vr )
m J m J

Substitute in equations (3.1) and (3.2).

( ) ( )
1 r2 1 r2
v̇c + ω̇rb = + b (C1 (vc − ωrb ) + C2 Vl ) + − b (C3 (vc + ωrb ) + C4 Vr )
m J m J
(12.10)
( ) ( )
1 rb2 1 rb2
v̇c − ω̇rb = − (C1 (vc − ωrb ) + C2 Vl ) + + (C3 (vc + ωrb ) + C4 Vr )
m J m J
(12.11)

Now, we need to solve for v̇c and ω̇. First, we’ll add equation (12.10) to equation (12.11).

2 2
2v̇c = (C1 (vc − ωrb ) + C2 Vl ) + (C3 (vc + ωrb ) + C4 Vr )
m m
1 1
v̇c = (C1 (vc − ωrb ) + C2 Vl ) + (C3 (vc + ωrb ) + C4 Vr )
m m
1 1 1 1
v̇c = (C1 + C3 )vc + (−C1 + C3 )ωrb + C2 Vl + C4 Vr
m m m m
1 rb 1 1
v̇c = (C1 + C3 )vc + (−C1 + C3 )ω + C2 Vl + C4 Vr
m m m m
12.4 Control law for nonholonomic wheeled vehicle 137

Next, we’ll subtract equation (12.11) from equation (12.10).

2rb2 2r2
2ω̇rb = (C1 (vc − ωrb ) + C2 Vl ) − b (C3 (vc + ωrb ) + C4 Vr )
J J
rb rb
ω̇ = (C1 (vc − ωrb ) + C2 Vl ) − (C3 (vc + ωrb ) + C4 Vr )
J J
rb rb rb rb
ω̇ = (C1 − C3 )vc + (−C1 − C3 )ωrb + C2 Vl − C4 Vr
J J J J
r 2
rb rb rb
ω̇ = (C1 − C3 )vc + b (−C1 − C3 )ω + C2 Vl − C4 Vr
J J J J

Now, just convert the two equations to state-space notation.

Theorem 12.4.4 — Linear/angular velocity reference tracker.
[ ] [ ] [ ]
v vl V
x= y= u= l
ω vr Vr

[ rb
] [1 ]
1 1
m (C1 + C3 ) m (−C1 + C3 ) m C2 m C4
A= rb2 B=
rb
(C − C3 ) J (−C1 − C3 )
rb
J C2 − rJb C4
[J 1 ] (12.12)
1 −rb
C= D = 02×2
1 rb

G2 K 2
where C1 = − KvlRrt2 , C2 = Gl K t
Rr , C3 = − KGvrRr
Kt
2 , and C4 =
Gr Kt
Rr .

See https://github.com/calcmogul/state-space-guide/blob/master/code/ramsete_
coupled.py for an implementation.
Reference tracker comparison
Figures 12.1 and 12.2 shows how well each reference tracker performs.

Figure 12.1: Left/right velocity reference Figure 12.2: Linear/angular velocity reference
tracker response to a motion profile tracker response to a motion profile

For a simple s-curve motion profile, they behave similarly.

Figure 12.3 demonstrates the Ramsete controller with the left/right velocity reference tracker for
a typical FRC drivetrain with the reference tracking behavior shown in figure 12.4 and figure 12.5
 138 Chapter 12. Nonlinear control

demonstrates the Ramsete controller with the velocity / angular velocity reference tracker for a typical
FRC drivetrain with the reference tracking behavior shown in figure 12.6.

Figure 12.3: Ramsete controller response with

Figure 12.4: Ramsete controller’s left/right ve-
left/right velocity reference tracker (b = 2, ζ =
locity reference tracker response
0.7)

Figure 12.5: Ramsete controller response with

Figure 12.6: Ramsete controller’s velocity / an-
velocity / angular velocity reference tracker (b =
gular velocity reference tracker response
2, ζ = 0.7)

The Ramsete controller behaves relatively the same in each case, but the left/right velocity reference
tracker tracks the references produced by the Ramsete controller better and with smaller control
efforts overall. This is likely due to the second controller having coupled dynamics. That is, the
control inputs don’t act independently on the system states. In the coupled controller, v and ω require
opposing control actions to reach their respective references, which results in them fighting each other.
This hypothesis is supported by the condition number of the coupled controller’s controllability matrix
being larger (2.692 for coupled and 1.314 for decoupled).
Therefore, theorem 12.4.3 is a superior reference tracker to theorem 12.4.4 due to its decoupled
dynamics, even though conversions are required between it and the Ramsete controller’s commands.

12.5 Further reading

For learning more about nonlinear control, we recommend reading the book Applied Nonlinear Con-
trol by Jean-Jacques Slotine. For a more complex type of nonlinear control, read A guiding vector
field algorithm for path following control of nonholonomic mobile robots [16].
IV
Estimation and localization

13 State-space observers . . . . . . . . . . . . . 141

13.1 Luenberger observer

14 Stochastic control theory . . . . . . . . . . . 145

14.1 Terminology
14.2 Introduction to probability
14.3 Linear stochastic systems
14.4 Two-sensor problem
14.5 Kalman filter
14.6 Kalman smoother
14.7 MMAE
14.8 Nonlinear observers

15 Pose estimation . . . . . . . . . . . . . . . . . . . 169

15.1 Nonlinear pose estimation
This page intentionally left blank
 Road next to Stevenson Academic building at UCSC

13. State-space observers

State-space observers are used to estimate states which cannot be measured directly. This can be due
to noisy measurements or the state not being measurable (a hidden state). This information can be
used for localization, which is the process of using external measurements to determine an agent’s
pose1 , or orientation in the world.
One type of state estimator is LQE. “LQE” stands for “Linear-Quadratic Estimator”. Similar to LQR,
it places the estimator poles such that it minimizes the sum of squares of the error. The Luenberger
observer and Kalman filter are examples of these.
Computer vision can also be used for localization. By extracting features from an image taken by
the agent’s camera, like a retroreflective target in FRC, and comparing them to known dimensions,
one can determine where the agent’s camera would have to be to see that image. This can be used to
correct our state estimate in the same way we do with an encoder or gyroscope.

13.1 Luenberger observer

1
An agent is a system-agnostic term for independent controlled actors like robots or aircraft.
142 Chapter 13. State-space observers

Theorem 13.1.1 — Luenberger observer.

x̂˙ = Ax̂ + Bu + L(y − ŷ) (13.1)

ŷ = Cx̂ + Du (13.2)

x̂k+1 = Ax̂k + Buk + L(yk − ŷk ) (13.3)

ŷk = Cx̂k + Duk (13.4)

A system matrix x̂ state estimate vector

B input matrix u input vector
C output matrix y output vector
D feedthrough matrix ŷ output estimate vector
L estimator gain matrix

Matrix Rows × Columns Matrix Rows × Columns

A states × states x̂ states × 1
B states × inputs u inputs × 1
C outputs × states y outputs × 1
D outputs × inputs ŷ outputs × 1
L states × outputs

Table 13.1: Luenberger observer matrix dimensions

Variables denoted with a hat are estimates of the corresponding variable. For example, x̂ is the
estimate of the true state x.

Notice that a Luenberger observer has an extra term in the state evolution equation. This term uses the
difference between the estimated outputs and measured outputs to steer the estimated state toward
the true state. Large values of L trust the measurements more while small values trust the model
more.

R Using an estimator forfeits the performance guarantees from earlier, but the responses are still
generally very good if the process and measurement noises are small enough. See John Doyle’s
paper Guaranteed Margins for LQG Regulators for a proof.

A Luenberger observer combines the prediction and update steps of an estimator. To run them
separately, use the equations in theorem 13.1.2 instead.
 13.1 Luenberger observer 143

Theorem 13.1.2 — Luenberger observer with separate predict/update.

Predict step
x̂− −
k+1 = Ax̂k + Buk (13.5)
Update step
− −1
k+1 = x̂k+1 + A L(yk − ŷk )
x̂+ (13.6)
ŷk = Cx̂−
k (13.7)

See appendix D.4.1 for a derivation.

13.1.1 Eigenvalues of closed-loop observer

The eigenvalues of the system matrix can be used to determine whether a state observer’s estimate
will converge to the true state.
Plugging equation (13.4) into equation (13.3) gives

x̂k+1 = Ax̂k + Buk + L(yk − ŷk )

x̂k+1 = Ax̂k + Buk + L(yk − (Cx̂k + Duk ))
x̂k+1 = Ax̂k + Buk + L(yk − Cx̂k − Duk )

Plugging in equation (9.4) gives

x̂k+1 = Ax̂k + Buk + L((Cxk + Duk ) − Cx̂k − Duk )

x̂k+1 = Ax̂k + Buk + L(Cxk + Duk − Cx̂k − Duk )
x̂k+1 = Ax̂k + Buk + L(Cxk − Cx̂k )
x̂k+1 = Ax̂k + Buk + LC(xk − x̂k )

Let Ek = xk − x̂k be the error in the estimate x̂k .

x̂k+1 = Ax̂k + Buk + LCEk

Subtracting this from equation (9.3) gives

xk+1 − x̂k+1 = Axk + Buk − (Ax̂k + Buk + LCEk )

Ek+1 = Axk + Buk − (Ax̂k + Buk + LCEk )
Ek+1 = Axk + Buk − Ax̂k − Buk − LCEk
Ek+1 = Axk − Ax̂k − LCEk
Ek+1 = A(xk − x̂k ) − LCEk
Ek+1 = AEk − LCEk
Ek+1 = (A − LC)Ek (13.8)
144 Chapter 13. State-space observers

For equation (13.8) to have a bounded output, the eigenvalues of A − LC must be within the unit
circle. These eigenvalues represent how fast the estimator converges to the true state of the given
model. A fast estimator converges quickly while a slow estimator avoids amplifying noise in the
measurements used to produce a state estimate.
As stated before, the controller and estimator are dual problems. Controller gains can be found assum-
ing perfect estimator (i.e., perfect knowledge of all states). Estimator gains can be found assuming
an accurate model and a controller with perfect tracking.
The effect of noise can be seen if it is modeled stochastically as

x̂k+1 = Ax̂k + Buk + L((yk + νk ) − ŷk )

where νk is the measurement noise. Rearranging this equation yields

x̂k+1 = Ax̂k + Buk + L(yk − ŷk + νk )

x̂k+1 = Ax̂k + Buk + L(yk − ŷk ) + Lνk

As L increases, the measurement noise is amplified.

 Road next to Stevenson Academic building at UCSC

14. Stochastic control theory

Stochastic control theory is a subfield of control theory that deals with the existence of uncertainty
either in observations or in the noise that drives the evolution of a system. We assign probability
distributions to this random noise and aim to achieve a desired control task despite the presence of
this noise.
Stochastic optimal control is concerned with doing this with minimum cost defined by some cost
functional, like we did with LQR earlier. First, we’ll cover the basics of probability and how we rep-
resent linear stochastic systems in state-space representation. Then, we’ll derive an optimal estimator
using this knowledge, the Kalman filter, and demonstrate creative applications of the Kalman filter
theory.

14.1 Terminology
First, we should provide definitions for terms that have specific meanings in this field.
A causal system is one that uses only past information. A noncausal system also uses information
from the future. A filter is a causal system that filters information through a probabilistic model to
produce an estimate of a desired quantity that can’t be measured directly. A smoother is a noncausal
system, so it uses information from before and after the current state to produce a better estimate.

14.2 Introduction to probability

14.2.1 Random variables
A random variable is a variable whose values are the outcomes of a random phenomenon. As such,
a random variable is defined as a function that maps the outcomes of an unpredictable process to
numerical quantities. A particular output of this function is called a sample. The sample space is the
set of possible values taken by the random variable.
A probability density function (PDF) is a function whose value at any given sample in the sample
space is the probability of the value of the random variable equaling that sample. The area under the
 146 Chapter 14. Stochastic control theory

Figure 14.1: Probability density function

function over a range gives the probability that the sample falls within that range. Let x be a random
variable, and let p(x) denote the probability density function of x. The probability that the value of
x will be in the interval x ∈ [x1 , x1 + dx] is p(x1 ) dx1 (see figure 14.1).
A probability of zero means that the sample will not occur and a probability of one means that the
sample will always occur. Probability density functions require that no probabilities are negative and
that the sum of all probabilities is 1. If the probabilities sum to 1, that means one of those outcomes
must happen.

∫ ∞
p(x) ≥ 0, p(x) dx = 1
−∞

14.2.2 Expected value

Expected value or expectation is a weighted average of the values the PDF can produce where the
weight for each is the corresponding probability of that value occurring. This can be written mathe-
matically as

∫ ∞
x = E[x] = x p(x) dx
−∞

The expectation can be applied to random functions as well as random variables.

∫ ∞
E[f (x)] = f (x) p(x) dx
−∞

The mean of a random variable is denoted by an overbar (e.g., x). The expectation of the difference
between a random variable and its mean converges to zero. In other words, the expectation of a
random variable is its mean.
 14.2 Introduction to probability 147

∫ ∞
E[x − x] = (x − x) p(x) dx
∫−∞
∞ ∫ ∞
E[x − x] = x p(x) dx − x p(x) dx
−∞ −∞
∫ ∞ ∫ ∞
E[x − x] = x p(x) dx − x p(x) dx
−∞ −∞
E[x − x] = x − x · 1
E[x − x] = 0

14.2.3 Variance
Informally, variance is a measure of how far the outcome of a random variable deviates from its mean.
Later, we will use variance to quantify how confident we are in the estimate of a random variable.
The standard deviation is the square root of the variance.

∫ ∞
var(x) = σ = E[(x − x) ] =
2 2
(x − x)2 p(x) dx
−∞
√
std[x] = σ = var(x)

14.2.4 Joint probability density functions

Probability density functions can also include more than one variable. Let x and y are random vari-
ables. The joint probability density function p(x, y) defines the probability p(x, y) dx dy, so that x
and y are in the intervals x ∈ [x, x + dx], y ∈ [y, y + dy] (see figure 14.2 for an example of a joint
PDF).
Joint probability density functions also require that no probabilities are negative and that the sum of
all probabilities is 1.
∫ ∞ ∫ ∞
p(x, y) ≥ 0, p(x, y) dx dy = 1
−∞ −∞

The expected values for joint PDFs are as follows.

∫ ∞ ∫ ∞
E[x] = x dx dy
∫−∞ −∞
∞ ∫ ∞
E[y] = y dx dy
∫−∞ −∞
∞ ∫ ∞
E[f (x, y)] = f (x, y) dx dy
−∞ −∞

The variance of a joint PDF measures how a variable correlates with itself.

∫ ∫ ∞
∞
var(x) = Σxx = E[(x − x) ] = 2
∞ (x − y)2 p(x, y) dx dy
− −∞
 148 Chapter 14. Stochastic control theory

Figure 14.2: Joint probability density function

∫ ∞ ∫ ∞
var(y) = Σyy = E[(y − y)2 ] = (y − y)2 p(x, y) dx dy
−∞ −∞

14.2.5 Covariance
A covariance is a measurement of how a variable correlates with another. If they vary in the same
direction, the covariance increases. If they vary in opposite directions, the covariance decreases.
∫ ∞ ∫ ∞
cov(x, y) = Σxy = E[(x − x)(y − y)] = (x − y)(y − y) p(x, y) dx dy
−∞ −∞

14.2.6 Correlation
Correlation is defined as

Σxy
ρ(x, y) = √ , |ρ(x, y)| ≤ 1
Σxx Σyy

14.2.7 Independence
Two random variables are independent if the following relation is true.

p(x, y) = p(x) p(y)

This means that the values of x do not correlate with the values of y. That is, the outcome of one
random variable does not affect another’s outcome. If we assume independence,

∫ ∞ ∫ ∞
E[xy] = xy p(x, y) dx dy
−∞ −∞
 14.2 Introduction to probability 149
∫ ∞ ∫ ∞
E[xy] = xy p(x) p(y) dx dy
∫−∞
∞
−∞
∫ ∞
E[xy] = x p(x) dx y p(y) dy
−∞ −∞
E[xy] = E[x]E[y]
E[xy] = x y

cov(x, y) = E[(x − x)(y − y)]

cov(x, y) = E[(x − x)]E[(y − y)]
cov(x, y) = 0 · 0

Therefore, the covariance Σxy is zero. Furthermore, ρ(x, y) = 0.

14.2.8 Marginal probability density functions

Given two random variables x and y whose joint distribution is known, the marginal PDF p(x)
expresses the probability of x averaged over information about y. In other words, it’s the PDF of x
when y is unknown. This is calculated by integrating the joint PDF over y.

∫ ∞
p(x) = p(x, y) dy
−∞

14.2.9 Conditional probability density functions

Let us assume that we know the joint PDF p(x, y) and the exact value for y. The conditional PDF
gives the probability of x in the interval [x, x + dx] for the given value y.
If p(x, y) is known, then we also know p(x, y = y ∗ ). However, note that the latter is not the
conditional density p(x|y ∗ ), instead

∫ ∞
∗
C(y ) = p(x, y = y ∗ ) dx
−∞
1
p(x|y ∗ ) = p(x, y = y ∗ )
C(y ∗ )

1
The scale factor C(y ∗ ) is used to scale the area under the PDF to 1.

14.2.10 Bayes’s rule

Bayes’s rule is used to determine the probability of an event based on prior knowledge of conditions
related to the event.

p(x, y) = p(x|y) p(y) = p(y|x) p(x)

If x and y are independent, then p(x|y) = p(x), p(y|x) = p(y), and p(x, y) = p(x) p(y).
 150 Chapter 14. Stochastic control theory

14.2.11 Conditional expectation

The concept of expectation can also be applied to conditional PDFs. This allows us to determine
what the mean of a variable is given prior knowledge of other variables.

∫ ∞
E[x|y] = x p(x|y) dx = f (y), E[x|y] ̸= E[x]
∫−∞
∞
E[y|x] = y p(y|x) dy = f (x), E[y|x] ̸= E[y]
−∞

14.2.12 Conditional variances

var(x|y) = E[(x − E[x|y])2 |y]

∫ ∞
var(x|y) = (x − E[x|y])2 p(x|y) dx
−∞

14.2.13 Random vectors

Now we will extend the probability concepts discussed so far to vectors where each element has a
PDF.



x1
 x2 
x= 
. . .
xn

The elements of x are scalar variables jointly distributed with a joint density p(x1 , x2 , . . . , xn ). The
expectation is

∫ ∞
E[x] = x = x p(x) dx
−∞
 
E[x1 ]
 E[x2 ] 
 
E[x] =  . 
.
 . 
E[xn ]
∫ ∞ ∫ ∞
E[xi ] = ... xi p(x1 , x2 , . . . , xn ) dx1 . . . dxn
∫−∞
∞
−∞

E[f (x)] = f (x) p(x) dx

−∞

14.2.14 Covariance matrix

The covariance matrix for a random vector x ∈ Rn is

Σ = cov(x, x) = E[(x − x)(x − x)T ]

 14.2 Introduction to probability 151
 
cov(x1 , x1 ) cov(x1 , x2 ) . . . cov(x1 , xn )
 cov(x2 , x1 ) cov(x1 , x2 ) . . . cov(x1 , xn ) 
 
Σ= .. .. .. .. 
 . . . . 
cov(xn , x1 ) cov(xn , x2 ) . . . cov(xn , xn )

This n × n matrix is symmetric and positive semidefinite. A positive semidefinite matrix satisfies the
relation that for any v ∈ Rn for which v ̸= 0, vT Σv ≥ 0. In other words, the eigenvalues of Σ are
all greater than or equal to zero.

14.2.15 Relations for independent random vectors

First, independent vectors imply linearity from p(x, y) = p(x) p(y).

E[Ax + By] = AE[x] + BE[y]

E[Ax + By] = Ax + By

Second, independent vectors being uncorrelated means their covariance is zero.

Σxy = cov(x, y)
Σxy = E[(x − x)(y − y)T ]
Σxy = E[xyT ] − E[xyT ] − E[xyT ] + E[xyT ]
Σxy = E[xyT ] − E[x]yT − xE[yT ] + xyT
Σxy = E[xyT ] − xyT − xyT + xyT
Σxy = E[xyT ] − xyT (14.1)

Now, compute E[xyT ].

∫ ∫
E[xyT ] = xyT p(x) p(y) dx dyT
∫X Y ∫
T
E[xy ] = p(x) x dx p(y) yT dyT
X Y
E[xyT ] = xyT (14.2)

Substitute equation (14.2) into equation (14.1).

Σxy = (xyT ) − xyT

Σxy = 0

Using these results, we can compute the covariance of z = Ax + By where Σxy = 0, Σx =

cov(x, x), and Σy = cov(y, y).
 152 Chapter 14. Stochastic control theory

Σz = cov(z, z)
Σz = E[(z − z)(z − z)T ]
Σz = E[(Ax + By − Ax − By)(Ax + By − Ax − By)T ]
Σz = E[(A(x − x) + B(y − y))(A(x − x) + B(y − y))T ]
Σz = E[(A(x − x) + B(y − y))((x − x)T AT + (y − y)T BT )]
Σz = E[A(x − x)(x − x)T AT + A(x − x)(y − y)T BT +
B(y − y)(x − x)T AT + B(y − y)(y − y)T BT ]

Since x and y are independent,

Σz = E[A(x − x)(x − x)T AT + 0 + 0 + B(y − y)(y − y)T BT ]

Σz = E[A(x − x)(x − x)T AT ] + E[B(y − y)(y − y)T BT ]
Σz = AE[(x − x)(x − x)T ]AT + BE[(y − y)(y − y)T ]BT
Σz = AΣx AT + BΣy BT

14.2.16 Gaussian random variables

A Gaussian random variable has the following properties:

E[x] = x
var(x) = σ 2
1 (x−x)2
p(x) = √ e− 2σ 2
2πσ 2

While we could use any random variable to represent a random process, we use the Gaussian random
variable a lot in probability theory due to the central limit theorem.
Definition 14.2.1 — Central limit theorem. When independent random variables are added,
their properly normalized sum tends toward a normal distribution (a Gaussian distribution or “bell
curve”).

This is the case even if the original variables themselves are not normally distributed. The theorem is a
key concept in probability theory because it implies that probabilistic and statistical methods that work
for normal distributions can be applicable to many problems involving other types of distributions.
For example, suppose that a sample is obtained containing a large number of independent observa-
tions, and that the arithmetic mean of the observed values is computed. The central limit theorem
says that the computed values of the mean will tend toward being distributed according to a normal
distribution.

14.3 Linear stochastic systems

Given the following stochastic system
 14.3 Linear stochastic systems 153

xk+1 = Φxk + Buk + Γwk

yk = Hxk + Duk + vk

where wk is the process noise and vk is the measurement noise,

E[wk ] = 0
E[wk wkT ] = Qk
E[vk ] = 0
E[vk vkT ] = Rk

where Qk is the process noise covariance matrix and Rk is the measurement noise covariance matrix.
We assume the noise samples are independent, so E[wk wjT ] = 0 and E[vk vkT ] = 0 where k ̸= j.
Furthermore, process noise samples are independent from measurement noise samples.
We’ll compute the expectation of these equations and their covariance matrices, which we’ll use later
for deriving the Kalman filter.

14.3.1 State vector expectation evolution

First, we will compute how the expectation of the system state evolves.

E[xk+1 ] = E[Φxk + Buk + Γwk ]

E[xk+1 ] = E[Φxk ] + E[Buk ] + E[Γwk ]
E[xk+1 ] = ΦE[xk ] + BE[uk ] + ΓE[wk ]
E[xk+1 ] = ΦE[xk ] + Buk + 0
xk+1 = Φxk + Buk

14.3.2 State covariance matrix evolution

Now, we will use this to compute how the state covariance matrix P evolves.

xk+1 − xk+1 = Φxk + Buk + Γwk − (Φxk − Buk )

xk+1 − xk+1 = Φ(xk − xk ) + Γwk

E[(xk+1 − xk+1 )(xk+1 − xk+1 )T ] = E[(Φ(xk − xk ) + Γwk )(Φ(xk − xk ) + Γwk )T ]

Pk+1 = E[(Φ(xk − xk ) + Γwk )(Φ(xk − xk ) + Γwk )T ]

Pk+1 = E[(Φ(xk − xk )(xk − xk )T ΦT ] + E[Φ(xk − xk )wkT ΓT ]+
 154 Chapter 14. Stochastic control theory

E[Γwk (xk − xk )T ΦT ] + E[Γwk wkT ΓT ]

Pk+1 = ΦE[(xk − xk )(xk − xk )T ]ΦT + ΦE[(xk − xk )wkT ]ΓT +
ΓE[wk (xk − xk )T ]ΦT + ΓE[wk wkT ]ΓT
Pk+1 = ΦPk ΦT + ΦE[(xk − xk )wkT ]ΓT +
ΓE[wk (xk − xk )T ]ΦT + ΓQk ΓTk
Pk+1 = ΦPk ΦT + 0 + 0 + ΓQk ΓT
Pk+1 = ΦPk ΦT + ΓQk ΓT

14.3.3 Measurement vector expectation

Next, we will compute the expectation of the output y.

E[yk ] = E[Hxk + Duk + vk ]

E[yk ] = HE[xk ] + Duk + 0
yk = Hxk + Duk

14.3.4 Measurement covariance matrix

Now, we will use this to compute how the measurement covariance matrix S evolves.

yk − yk = Hxk + Duk + vk − (Hxk + Duk )

yk − yk = H(xk − xk ) + vk

E[(yk − yk )(yk − yk )T ] = E[(H(xk − xk ) + vk )(H(xk − xk ) + vk )T ]

Sk = E[(H(xk − xk ) + vk )(H(xk − xk ) + vk )T ]
Sk = E[(H(xk − xk )(xk − xk )T HT ] + E[vk vkT ]
Sk = HE[((xk − xk )(xk − xk )T ]HT + Rk
Sk = HPk HT + Rk

14.4 Two-sensor problem

We’ll skip the probability derivations here, but given two data points with associated variances rep-
resented by Gaussian distribution, the information can be optimally combined into a third Gaussian
distribution with a mean value and variance. The expected value of x given a measurement z1 is

σ02 σ2
E[x|z1 ] = µ = z 1 + x0 (14.3)
σ02 + σ2 2
σ0 + σ 2

The variance of x given z1 is

σ 2 σ02
E[(x − µ)2 |z1 ] = (14.4)
σ02 + σ 2
 14.5 Kalman filter 155

The expected value, which is also the maximum likelihood value, is the linear combination of the
prior expected (maximum likelihood) value and the measurement. The expected value is a reasonable
estimator of x.

σ02 σ2
x̂ = E[x|z1 ] = z 1 + x0 (14.5)
σ02 + σ 2 σ02 + σ 2
x̂ = w1 z1 + w2 x0

Note that the weights w1 and w2 sum to 1. When the prior (i.e., prior knowledge of state) is uninfor-
mative (a large variance)

σ02
w1 = lim 2 =0 (14.6)
σ02 →0 σ0 + σ 2

σ2
w2 = lim 2 =1 (14.7)
σ02 →0 σ0 + σ 2

and x̂ = z1 . That is, the weight is on the observations and the estimate is equal to the measurement.
Let us assume we have a model providing an almost exact prior for x. In that case, σ02 approaches 0
and

σ02
w1 = lim 2 =1 (14.8)
σ02 →0 σ0 + σ 2

σ2
w2 = lim 2 =0 (14.9)
σ02 →0 σ0 + σ 2

The Kalman filter uses this optimal fusion as the basis for its operation.

14.5 Kalman filter

So far, we’ve derived equations for updating the expected value and state covariance without mea-
surements and how to incorporate measurements into an initial state optimally. Now, we’ll combine
these concepts to produce an estimator which minimizes the error covariance for linear systems.

14.5.1 Derivations
− −
k+1 = x̂k+1 +Kk+1 (yk+1 −Hk+1 x̂k+1 ), we want to find the
Given the posteriori update equation x̂+
value of K that minimizes the error covariance, because doing this minimizes the estimation error.

a posteriori estimate covariance matrix

k+1 = cov(xk+1 − x̂k+1 )

P+ +

− −
k+1 = cov(xk+1 − (x̂k+1 + Kk+1 (yk+1 − Hx̂k+1 )))
P+
− −
k+1 = cov(xk+1 − (x̂k+1 + Kk+1 (Hk+1 xk+1 + vk+1 − Hx̂k+1 )))
P+
− −
k+1 = cov(xk+1 − (x̂k+1 + Kk+1 Hk+1 xk+1 + Kk+1 vk+1 − Kk+1 Hx̂k+1 ))
P+
156 Chapter 14. Stochastic control theory
− −
k+1 = cov(xk+1 − x̂k+1 − Kk+1 Hk+1 xk+1 − Kk+1 vk+1 + Kk+1 Hx̂k+1 )
P+
− −
k+1 = cov(xk+1 − x̂k+1 − Kk+1 Hk+1 (xk+1 − x̂k+1 ) − Kk+1 vk+1 )
P+
−
k+1 = cov((I − Kk+1 Hk+1 )(xk+1 − x̂k+1 ) − Kk+1 vk+1 )
P+
−
k+1 = cov((I − Kk+1 Hk+1 )(xk+1 − x̂k+1 )) + cov(Kk+1 vk+1 )
P+
−
k+1 = (I − Kk+1 Hk+1 )cov(xk+1 − x̂k+1 )(I − Kk+1 Hk+1 ) + Kk+1 cov(vk+1 )Kk+1
P+ T T

−
k+1 = (I − Kk+1 Hk+1 )Pk+1 (I − Kk+1 Hk+1 ) + Kk+1 Rk+1 Kk+1
P+ T T

Kalman gain
The error in the a posteriori state estimation is xk+1 − x̂−k+1 . We want to minimize the expected
value of the square of the magnitude of this vector. This is equivalent to minimizing the trace of the
a posteriori estimate covariance matrix P− k+1 .

First, we’ll expand the equation for P−

k+1 and collect terms.

−
k+1 = (I − Kk+1 Hk+1 )Pk+1 (I − Kk+1 Hk+1 ) + Kk+1 Rk+1 Kk+1
P+ T T

− −
k+1 = (Pk+1 − Kk+1 Hk+1 Pk+1 )(I − Kk+1 Hk+1 ) + Kk+1 Rk+1 Kk+1
P+ T T

− −
k+1 = (Pk+1 − Kk+1 Hk+1 Pk+1 )(I − Hk+1 Kk+1 ) + Kk+1 Rk+1 Kk+1
P+ T T T T

− −
k+1 = Pk+1 (I − Hk+1 Kk+1 ) − Kk+1 Hk+1 Pk+1 (I − Hk+1 Kk+1 ) + Kk+1 Rk+1 Kk+1
P+ T T T T T T T

− − − −
k+1 = Pk+1 − Pk+1 Hk+1 Kk+1 − Kk+1 Hk+1 Pk+1 + Kk+1 Hk+1 Pk+1 Hk+1 Kk+1 +
P+ T T T T

Kk+1 Rk+1 KTk+1

− − −
k+1 = Pk+1 − Pk+1 Hk+1 Kk+1 − Kk+1 Hk+1 Pk+1 +
P+ T T

Kk+1 (Hk+1 P− T T
k+1 Hk+1 + Rk+1 )Kk+1
− − −
k+1 = Pk+1 − Pk+1 Hk+1 Kk+1 − Kk+1 Hk+1 Pk+1 + Kk+1 Sk+1 Kk+1
P+ T T T
(14.10)

Now we’ll take the trace.

− − −
k+1 ) = tr(Pk+1 ) − tr(Pk+1 Hk+1 Kk+1 ) − tr(Kk+1 Hk+1 Pk+1 ) + tr(Kk+1 Sk+1 Kk+1 )
tr(P+ T T T

Transpose one of the terms twice.

− −T T −
k+1 ) = tr(Pk+1 ) − tr((Kk+1 Hk+1 Pk+1 ) ) − tr(Kk+1 Hk+1 Pk+1 ) + tr(Kk+1 Sk+1 Kk+1 )
tr(P+ T

P−
k+1 is symmetric, so we can drop the transpose.

− − −
k+1 ) = tr(Pk+1 ) − tr((Kk+1 Hk+1 Pk+1 ) ) − tr(Kk+1 Hk+1 Pk+1 ) + tr(Kk+1 Sk+1 Kk+1 )
tr(P+ T T

The trace of a matrix is equal to the trace of its transpose since the elements used in the trace are on
the diagonal.
14.5 Kalman filter 157

− − −
k+1 ) = tr(Pk+1 ) − tr(Kk+1 Hk+1 Pk+1 ) − tr(Kk+1 Hk+1 Pk+1 ) + tr(Kk+1 Sk+1 Kk+1 )
tr(P+ T

− −
k+1 ) = tr(Pk+1 ) − 2 tr(Kk+1 Hk+1 Pk+1 ) + tr(Kk+1 Sk+1 Kk+1 )
tr(P+ T

Given theorems 14.5.1 and 14.5.2

∂
Theorem 14.5.1 ∂A tr(ABAT ) = 2AB where B is symmetric.

∂
Theorem 14.5.2 ∂A tr(AC) = CT

find the minimum of the trace of P+

k+1 by taking the partial derivative with respect to K and setting
the result to 0.

∂ tr(P+
k+1 )
= 0 − 2(Hk+1 P− T
k+1 ) + 2Kk+1 Sk+1
∂K
∂ tr(P+
k+1 )
= −2P−T T
k+1 Hk+1 + 2Kk+1 Sk+1
∂K
∂ tr(P+
k+1 )
= −2P− T
k+1 Hk+1 + 2Kk+1 Sk+1
∂K
0 = −2P− T
k+1 Hk+1 + 2Kk+1 Sk+1
2Kk+1 Sk+1 = 2P− T
k+1 Hk+1
Kk+1 Sk+1 = P− T
k+1 Hk+1
Kk+1 = P− T −1
k+1 Hk+1 Sk+1

This is the optimal Kalman gain.

Simplified a priori estimate covariance matrix

If the optimal Kalman gain is used, the a posteriori estimate covariance matrix update equation can
be simplified. First, we’ll manipulate the equation for the optimal Kalman gain.

Kk+1 = P− T −1
k+1 Hk+1 Sk+1
Kk+1 Sk+1 KTk+1 = P− T T
k+1 Hk+1 Kk+1

Now we’ll substitute it into equation (14.10).

− − −
k+1 = Pk+1 − Pk+1 Hk+1 Kk+1 − Kk+1 Hk+1 Pk+1 + Kk+1 Sk+1 Kk+1
P+ T T T

− − − −
k+1 = Pk+1 − Pk+1 Hk+1 Kk+1 − Kk+1 Hk+1 Pk+1 + Pk+1 Hk+1 Kk+1
P+ T T T T

− −
k+1 = Pk+1 − Kk+1 Hk+1 Pk+1
P+
−
k+1 = (I − Kk+1 Hk+1 )Pk+1
P+
 158 Chapter 14. Stochastic control theory

14.5.2 Predict and update equations

Now that we’ve derived all the pieces we need, we can finally write all the equations for a Kalman
filter. Theorem 14.5.3 shows the predict and update steps for a Kalman filter at the k th timestep.
Theorem 14.5.3 — Kalman filter.

Predict step
x̂−
k+1 = Φx̂k + Buk (14.11)
P− − T
k+1 = ΦPk Φ + ΓQΓ
T
(14.12)
Update step
Kk+1 = P− T − T
k+1 H (HPk+1 H + R)
−1
(14.13)
x̂+
k+1 = x̂−
k+1 + Kk+1 (yk+1 − Hx̂−
k+1 ) (14.14)
+
Pk+1 = (I − Kk+1 H)P−
k+1 (14.15)

Φ system matrix x̂ state estimate vector

B input matrix u input vector
H measurement matrix y output vector
P error covariance matrix Q process noise covariance matrix
K Kalman gain matrix R measurement noise covariance matrix
Γ process noise intensity vector
where a superscript of minus denotes a priori and plus denotes a posteriori estimate (before and
after update respectively).

H, Q, and R from the equations derived earlier are made constants here. Φ is replaced with A for
continuous systems.

R To implement a discrete time Kalman filter from a continuous model, the model and continuous
time Q and R matrices can be discretized using theorem 10.7.1.

Matrix Rows × Columns Matrix Rows × Columns

Φ states × states x̂ states × 1
B states × inputs u inputs × 1
H outputs × states y outputs × 1
P states × states Q states × states
K states × outputs R outputs × outputs
Γ states × 1

Table 14.1: Kalman filter matrix dimensions

Unknown states in a Kalman filter are generally represented by a Wiener (pronounced VEE-ner)
 14.5 Kalman filter 159

process1 . This process has the property that its variance increases linearly with time t.

14.5.3 Setup
Equations to model
The following example system will be used to describe how to define and initialize the matrices for a
Kalman filter.
A robot is between two parallel walls. It starts driving from one wall to the other at a velocity of
0.8cm/s and uses ultrasonic sensors to provide noisy measurements of the distances to the walls in
front of and behind it. To estimate the distance between the walls, we will define three states: robot
position, robot velocity, and distance between the walls.

xk+1 = xk + vk ∆T (14.16)
vk+1 = vk (14.17)
xw
k+1 = xw
k (14.18)

This can be converted to the following state-space model.

 
xk
xk =  v k  (14.19)
xwk
     
1 1 0 0 0
xk+1 = 0 0 0 xk + 0.8 + 0.1 wk (14.20)
0 0 1 0 0

where the Gaussian random variable wk has a mean of 0 and a variance of 1. The observation model
is

[ ]
1 0 0
yk = x + θk (14.21)
−1 0 1 k

where the covariance matrix of Gaussian measurement noise θ is a 2 × 2 matrix with both diagonals
10cm2 .
The state vector is usually initialized using the first measurement or two. The covariance matrix
entries are assigned by calculating the covariance of the expressions used when assigning the state
vector. Let k = 2.

[ ]
Q= 1 (14.22)
[ ]
10 0
R= (14.23)
0 10
 
yk,1
x̂ = (yk,1 − yk−1,1 )/dt (14.24)
yk,1 + yk,2
1
Explaining why we use the Wiener process would require going much more in depth into stochastic processes and
Itô calculus, which is outside the scope of this book.
160 Chapter 14. Stochastic control theory
 
10 10/dt 10
P = 10/dt 20/dt2 10/dt (14.25)
10 10/dt 20

Initial conditions
To fill in the P matrix, we calculate the covariance of each combination of state variables. The
resulting value is a measure of how much those variables are correlated. Due to how the covariance
calculation works out, the covariance between two variables is the sum of the variance of matching
terms which aren’t constants multiplied by any constants the two have. If no terms match, the variables
are uncorrelated and the covariance is zero.
In P11 , the terms in x1 correlate with itself. Therefore, P11 is x1 ’s variance, or P11 = 10. For
P21 , One term correlates between x1 and x2 , so P21 = 10 dt . The constants from each are sim-
ply multiplied together. For P22 , both measurements are correlated, so the variances add together.
20
Therefore, P22 = dt 2 . It continues in this fashion until the matrix is filled up. Order doesn’t matter
for correlation, so the matrix is symmetric.
Selection of priors
Choosing good priors is important for a well performing filter, even if little information is known.
This applies to both the measurement noise and the noise model. The act of giving a state variable a
large variance means you know something about the system. Namely, you aren’t sure whether your
initial guess is close to the true state. If you make a guess and specify a small variance, you are telling
the filter that you are very confident in your guess. If that guess is incorrect, it will take the filter a
long time to move away from your guess to the true value.
Covariance selection
While one could assume no correlation between the state variables and set the covariance matrix
entries to zero, this may not reflect reality. The Kalman filter is still guarenteed to converge to the
steady-state covariance after an infinite time, but it will take longer than otherwise.
Noise model selection
We typically use a Gaussian distribution for the noise model because the sum of many independent
random variables produces a normal distribution by the central limit theorem. Kalman filters only
require that the noise is zero-mean. If the true value has an equal probability of being anywhere
within a certain range, use a uniform distribution instead. Each of these communicates information
regarding what you know about a system in addition to what you do not.
Process noise and measurement noise covariance selection
Recall that the process noise covariance is Q and the measurement noise covariance is R. To tune
the elements of these, it can be helpful to take a collection of measurements, then run the Kalman
filter on them offline to evaluate its performance.
The diagonal elements of R are the variances of each measurement, which can be easily determined
from the offline measurements. The diagonal elements of Q are the variances of each state. They
represent how much each state is expected to deviate from the model.
Selecting Q is more difficult. If the data is trusted too much over the model, one risks overfitting
the data. One should balance estimating any hidden states sufficiently with actually filtering out the
noise.
Modeling other noise colors
The Kalman filter assumes a model with zero-mean white noise. If the model is incomplete in some
e = y − Hx̂
way, whether it’s missing dynamics or assumes an incorrect noise model, the residual y
 14.5 Kalman filter 161

Figure 14.3: State estimates and measurements with Kalman filter

over time will be something other than white noise.

To handle other colors of noise in a Kalman filter, define that color of noise in terms of white noise
and augment the model with it.

14.5.4 Simulation
Figure 14.3 shows the state estimates and measurements of the Kalman filter over time. Figure 14.4
shows the position estimate and variance over time. Figure 14.5 shows the wall position estimate
and variance over time. Notice how the variances decrease over time as the filter gathers more
measurements. This means that the filter becomes more confident in its state estimates.

The final precisions in estimating the position of the robot and the wall are the square roots of the
corresponding elements in the covariance matrix. That is,√
0.5188 m and 0.4491 m respectively. They
are smaller than the precision of the raw measurements, 10 = 3.1623 m. As expected, combining
the information from several measurements produces a better estimate than any one measurement
alone.

14.5.5 Kalman filter as Luenberger observer

A Kalman filter can be represented as a Luenberger observer by letting C = H and L = AKk (see
appendix D.4). The Luenberger observer has a constant observer gain matrix L, so the steady-state
Kalman gain is used to calculate it. We will demonstrate how to find this shortly.

Kalman filter theory provides a way to place the poles of the Luenberger observer optimally in the
same way we placed the poles of the controller optimally with LQR. The eigenvalues of the Kalman
filter are

eig(A(I − Kk H)) (14.26)

162 Chapter 14. Stochastic control theory

Figure 14.4: Robot position estimate and variance with Kalman filter

Figure 14.5: Wall position estimate and variance with Kalman filter
 14.6 Kalman smoother 163

Steady-state Kalman gain

One may have noticed that the error covariance matrix can be updated independently of the rest of
the model. The error covariance matrix tends toward a steady-state value, and this matrix can be
obtained via the discrete algebraic Ricatti equation. This can then be used to compute a steady-state
Kalman gain.

Snippet 14.1 computes the steady-state matrices for a Kalman filter.

import control as cnt
import numpy as np
import scipy as sp

def kalmd(sys , Q, R):

""" Solves for the steady state kalman gain and error covariance matrices .

Keyword arguments :
sys -- discrete state -space model
Q -- process noise covariance matrix
R -- measurement noise covariance matrix

Returns :
Kalman gain , error covariance matrix .
"""
m = sys.A. shape [0]

observability_rank = np. linalg . matrix_rank (cnt.obsv(sys.A, sys.C))

if observability_rank != m:
print(
" Warning : Observability of %d != %d, unobservable state "
% ( observability_rank , m)
)

# Compute the steady state covariance matrix

P_prior = sp. linalg . solve_discrete_are (a=sys.A.T, b=sys.C.T, q=Q, r=R)
S = sys.C * P_prior * sys.C.T + R
K = P_prior * sys.C.T * np. linalg .inv(S)
P = (np.eye(m) - K * sys.C) * P_prior

return K, P

Snippet 14.1. Steady-state Kalman gain and error covariance matrices calculation in Python

14.6 Kalman smoother

The Kalman filter uses the data up to the current time to produce an optimal estimate of the system
state. If data beyond the current time is available, it can be ran through a Kalman smoother to produce
a better estimate. This is done by recording measurements, then applying the smoother to it offline.

The Kalman smoother does a forward pass on the available data, then a backward pass through the
system dynamics so it takes into account the data before and after the current time. This produces
state variances that are lower than that of a Kalman filter.

We will modify the robot model so that instead of a velocity of 0.8cm/s with random noise, the
velocity is modeled as a random walk from the current velocity.
164 Chapter 14. Stochastic control theory

Figure 14.6: Robot position with Kalman smoother

 
xk
xk =  v k  (14.27)
xwk

   
1 1 0 0
xk+1 = 0 1 0 xk + 0.1 wk
   (14.28)
0 0 1 0

We will use the same observation model as before.

Using the same data from subsection 14.5.4, figures 14.6, 14.7, and 14.8 show the improved state
estimates and figure 14.9 shows the improved robot position covariance with a Kalman smoother.
Notice how the wall position produced by the smoother is a constant. This is because that state has
no dynamics, so the final estimate from the Kalman filter is already the best estimate.
14.6 Kalman smoother 165

Figure 14.7: Robot velocity with Kalman smoother

Figure 14.8: Wall position with Kalman smoother

 166 Chapter 14. Stochastic control theory

Figure 14.9: Robot position variance with Kalman smoother

14.7 MMAE
MMAE stands for Multiple Model Adaptive Estimation. MMAE runs multiple Kalman filters with
different models on the same data. The Kalman filter with the lowest residual has the highest likeli-
hood of accurately reflecting reality. This can be used to detect certain system states like an aircraft
engine failing without needing to invest in costly sensors to determine this directly.

For example, say you have three Kalman filters: one for turning left, one for turning right, and one
for going straight. If the control input is attempting to fly the plane straight and the Kalman filter for
going left has the lowest residual, the aircraft’s left engine probably failed.

14.8 Nonlinear observers

In this book, we have covered the Kalman filter, which is the optimal unbiased estimator for linear
systems. It isn’t optimal for nonlinear systems, but several extensions to it have been developed to
make it more accurate.

14.8.1 Extended Kalman filter

This method linearizes the matrices used during the prediction step. In addition to the A, B, C, and
D matrices above, the process noise intensity vector Γ is linearized as follows:

∂f (x, 0, 0)
Γ=
∂w

where w is the process noise included in the stochastic model.

From there, the continuous Kalman filter equations are used like normal to compute the error covari-
ance matrix P and Kalman gain matrix. The state estimate update can still use the function h(x) for
accuracy.
 14.8 Nonlinear observers 167

− −
k+1 = x̂k+1 + Kk+1 (yk+1 − h(x̂k+1 ))
x̂+

14.8.2 Unscented Kalman filter

This method linearizes around carefully chosen points to minimize the modeling error. There’s a lot
of detail to cover, so we recommend just reading a paper on it [22].
Here’s a paper on a quaternion-based Unscented Kalman filter for orientation tracking [17].
This page intentionally left blank
 Road next to Stevenson Academic building at UCSC

15. Pose estimation

15.1 Nonlinear pose estimation

The simplest way to perform pose estimation is to integrate the velocity in each orthogonal direction
over time. In two dimensions, one could use

xk+1 = xk + vk cos θk T
yk+1 = yk + vk sin θk T

where T is the sample period. xd , yd , and θd are obtained from a desired time-based trajectory.
This odometry approach assumes that the robot follows a straight path between samples (that is,
ω = 0). We can obtain a more accurate approximation by including first order dynamics for the
heading θ.

 
x
x = y 
θ

vx , vy , and ω are the x and y velocities of the robot within its local coordinate frame, which will be
treated as constants. These values in a column vector are called a ”twist”.

R There are two coordinate frames used here: robot and global. A superscript on the left side of a
matrix denotes the coordinate frame in which that matrix is represented. The robot’s coordinate
frame is denoted by R and the global coordinate frame is denoted by G.

In the robot frame

170 Chapter 15. Pose estimation

R
dx = R vx dt
R
dy = R vy dt
R
dθ = R ω dt

To transform this into the global frame, we apply a counterclockwise rotation matrix where θ changes
over time.

G    R 
dx cos θ(t) − sin θ(t) 0 vx
dy  =  sin θ(t) cos θ(t) 0 vy dt
dθ 0 0 1 ω
G    R 
dx cos ωt − sin ωt 0 vx
dy  =  sin ωt cos ωt 0 vy dt
dθ 0 0 1 ω

Now, integrate the matrix equation (matrices are integrated element-wise). This derivation heavily
utilizes the integration method described in section 2.2.1.

G   sin ωt cos ωt
 R  t
∆x 0 vx 
ω
∆y  = − cos ωt
ω
  
sin ωt
0 vy 
ω ω 
∆θ 0 0 t ω 
0
G   sin ωt cos ωt−1
 R 
∆x ω ω 0 vx
∆y  =  1−cos ωt sin ωt
0   vy 
ω ω
∆θ 0 0 t ω

This equation assumes a starting orientation of θ = 0. For nonzero starting orientations, we can apply
a counterclockwise rotation by θ.

G     sin ωt  R 
∆x cos θ − sin θ 0 ω
cos ωt−1
ω 0 vx
∆y  =  sin θ cos θ 0  1−cos ωt sin ωt
0   vy 
ω ω
∆θ 0 0 1 0 0 t ω

If we factor out a t, we can use change in pose between updates instead of velocities.

G     sin ωt  R 
∆x cos θ − sin θ 0 ω
cos ωt−1
ω 0 vx
∆y  =  sin θ cos θ 0  1−cos ωt sin ωt
0 vy 
ω ω
∆θ 0 0 1 0 0 t ω
G     sin ωt  R 
∆x cos θ − sin θ 0 ωt
cos ωt−1
ωt 0 vx
∆y  =  sin θ cos θ 0  1−cos ωt sin ωt
0 vy t
ωt ωt
∆θ 0 0 1 0 0 1 ω
15.1 Nonlinear pose estimation 171
G     sin ωt  R 
∆x cos θ − sin θ 0 ωt
cos ωt−1
ωt 0 vx t
∆y  =  sin θ cos θ 0  1−cos ωt sin ωt
0   vy t 
ωt ωt
∆θ 0 0 1 0 0 1 ωt
G     R   R 
∆x cos θ − sin θ 0 sin ∆θ
∆θ
cos ∆θ−1
∆θ 0 ∆x
∆y  =  sin θ cos θ 0  1−cos ∆θ sin ∆θ
0 ∆y 
∆θ ∆θ
∆θ 0 0 1 0 0 1 ∆θ

The previous version used the current velocity and projected the model forward to the next timestep
(into the future). As such, the prediction must be done after any controller calculations are performed.
With the second version, the locally measured pose deltas can only be measured using past samples,
so the model update must be performed before any controller calculations to advance the model to
the current timestep.
When the robot is traveling on a straight trajectory (ω = 0), some expressions within the equation
above are indeterminate. We can approximate these with Taylor series expansions.

sin ωt t3 ω 2
=t− + ...
ω 6
sin ωt t3 ω 2
∼t−
ω 6
cos ωt − 1 t2 ω t4 ω 3
=− + − ...
ω 2 4
cos ωt − 1 t2 ω
∼−
ω 2
1 − cos ωt t2 ω t4 ω 3
= − + ...
ω 2 4
1 − cos ωt t2 ω
∼
ω 2

If we let ω = 0, we should get the standard kinematic equations like x = vt with a rotation applied
to them.

sin ωt t3 · 02
∼t− =t
ω 6
cos ωt − 1 t2 · 0
∼− =0
ω 2
1 − cos ωt t2 · 0
∼ =0
ω 2

Now substitute these into equation (15.1).

G     R 
∆x cos θ − sin θ 0 t 0 0 vx
∆y  =  sin θ cos θ 0 0 t 0 vy 
∆θ 0 0 1 0 0 t ω
172 Chapter 15. Pose estimation
G    
∆x cos θ − sin θ 0 vx t
∆y  =  sin θ cos θ 0 vy t
∆θ 0 0 1 ωt

As expected, the equations simplify to the first order case with a rotation matrix applied to the veloc-
ities in the robot’s local coordinate frame.
Differential drive robots have vy = 0 since they only move in the direction of the current heading,
which is along the x-axis. Therefore,

G    
∆x cos θ − sin θ 0 vx t
∆y  =  sin θ cos θ 0  0 
∆θ 0 0 1 ωt
G   
∆x vx t cos θ
∆y  =  vy t sin θ 
∆θ ωt

Theorem 15.1.1 — Nonlinear pose estimator with constant curvature.

G     sin ωt  R 
∆x cos θ − sin θ 0 ω
cos ωt−1
ω 0 vx
∆y  =  sin θ cos θ 0  1−cos ωt sin ωt
0   vy  (15.1)
ω ω
∆θ 0 0 1 0 0 t ω

or

G    R  sin ∆θ  R 
∆x cos θ − sin θ 0 ∆θ
cos ∆θ−1
∆θ 0 ∆x
∆y  =  sin θ cos θ 0  1−cos ∆θ sin ∆θ
0 ∆y  (15.2)
∆θ ∆θ
∆θ 0 0 1 0 0 1 ∆θ

where G denotes global coordinate frame and R denotes robot’s coordinate frame.
For sufficiently small ω:

sin ωt t3 ω 2 cos ωt − 1 t2 ω 1 − cos ωt t2 ω

=t− =− = (15.3)
ω 6 ω 2 ω 2
sin ωt t2 ω 2 cos ωt − 1 tω 1 − cos ωt tω
=1− =− = (15.4)
ωt 6 ωt 2 ωt 2

∆x change in pose’s x vx velocity along x-axis

∆y change in pose’s y vy velocity along y-axis
∆θ change in pose’s θ ω angular velocity
t Time since last pose update θ starting angle in global coordinate frame
This change in pose can be added directly to the previous pose estimate to update it.

Figure 15.1 shows the error in the global pose coordinates over time for the simpler odometry method
compared to the method using twists (uses the Ramsete controller from subsection 12.4.1).
15.1 Nonlinear pose estimation 173

Figure 15.1: Odometry error compared to method using twists

The highest error is 0.0035m, or roughly an eighth inch, in x over a 10m path starting with a 2m
displacement. This is negligible when considering other possible sources of error like turning scrub
on skid steer robots. Perhaps the difference is more noticeable for paths with higher curvatures and
longer duration.
This page intentionally left blank
V
Motion planning

16 Motion profiles . . . . . . . . . . . . . . . . . . . . . 177

16.1 Jerk
16.2 Profile selection
16.3 Profile equations
16.4 Other profile types
16.5 Further reading
This page intentionally left blank
 OPERS field at UCSC

16. Motion profiles

If smooth, predictable motion of a system over time is desired, it’s best to only change a system’s
reference as fast as the system is able to physically move. Motion profiles, also known as trajectories,
are used for this purpose. For multi-state systems, each state is given its own trajectory. Since these
states are usually position and velocity, they share different derivatives of the same profile.
For one degree of freedom (1 DOF) point-to-point movements in FRC, the most commonly used
profiles are the trapezoidal profile (figure 16.1) and the S-curve profile (figure 16.2). These profiles
accelerate the system to a maximum velocity from rest, then decelerate it later such that the final
acceleration velocity, are zero at the moment the system arrives at the desired location.

Figure 16.1: Trapezoidal profile Figure 16.2: S-curve profile

These profiles are given their names based on the shape of their velocity trajectory. The trapezoidal
profile has a velocity trajectory shaped like a trapezoid and the S-curve profile has a velocity trajectory
shaped like an S-curve.
In the context of a point-to-point move, a full S-curve consists of seven distinct phases of motion.
Phase I starts moving the system from rest at a linearly increasing acceleration until it reaches the
 178 Chapter 16. Motion profiles

maximum acceleration. In phase II, the profile accelerates at this maximum acceleration rate until
it must start decreasing as it approaches the maximum velocity. This occurs in phase III when the
acceleration linearly decreases until it reaches zero. In phase IV, the velocity is constant until de-
celeration begins, at which point the profiles decelerates in a manner symmetric to phases I, II and
III.

A trapezoidal profile, on the other hand, has three phases. It is a subset of an S-curve profile, hav-
ing only the phases corresponding to phase II of the S-curve profile (constant acceleration), phase
IV (constant velocity), and phase VI (constant deceleration). This reduced number of phases under-
scores the difference between these two profiles: the S-curve profile has extra motion phases which
transition between periods of acceleration, and periods of nonacceleration; the trapezoidal profile
has instantaneous transitions between these phases. This can be seen in the acceleration graphs of
the corresponding velocity profiles for these two profile types.

16.1 Jerk
The motion characteristic that defines the change in acceleration, or transitional period, is known as
”jerk”. Jerk is defined as the rate of change of acceleration with time. In a trapezoidal profile, the
jerk (change in acceleration) is infinite at the phase transitions, while in the S-curve profile the jerk
is a constant value, spreading the change in acceleration over a period of time.

From figures 16.1 and 16.2, we can see S-curve profiles are smoother than trapezoidal profiles. Why,
however, do the S-curve profile result in less load oscillation? For a given load, the higher the jerk,
the greater the amount of unwanted vibration energy will be generated, and the broader the frequency
spectrum of the vibration’s energy will be.

This means that more rapid changes in acceleration induce more powerful vibrations, and more vibra-
tional modes will be excited. Because vibrational energy is absorbed in the system mechanics, it may
cause an increase in settling time or reduced accuracy if the vibration frequency matches resonances
in the mechanical system.

16.2 Profile selection

Since trapezoidal profiles spend their time at full acceleration or full deceleration, they are, from
the standpoint of profile execution, faster than S-curve profiles. However, if this “all on”/“all off”
approach causes an increase in settling time, the advantage is lost. Often, only a small amount of
“S” (transition between acceleration and no acceleration) can substantially reduce induced vibration.
Therefore to optimize throughput, the S-curve profile must be tuned for each a given load and given
desired transfer speed.

What S-curve form is right for a given system? On an application by application basis, the specific
choice of the form of the S-curve will depend on the mechanical nature of the system and the desired
performance specifications. For example, in medical applications which involve liquid transfers that
should not be jostled, it would be appropriate to choose a profile with no phase II and VI segment at
all. Instead the acceleration transitions would be spread out as far as possible, thereby maximizing
smoothness.

In other applications involving high speed pick and place, overall transfer speed is most important,
so a good choice might be an S-curve with transition phases (phases I, III, V, and VII) that are five to
fifteen percent of phase II and VI. In this case, the S-curve profile will add a small amount of time to
the overall transfer time. However, the reduced load oscillation at the end of the move considerably
 16.3 Profile equations 179

decreases the total effective transfer time. Trial and error using a motion measurement system is gen-
erally the best way to determine the right amount of “S” because modelling high frequency dynamics
is difficult to do accurately.
Another consideration is whether that “S” segment will actually lead to smoother control of the system.
If the high frequency dynamics at play are negligible, one can use the simpler trapezoidal profile.

16.3 Profile equations

The trapezoidal profile uses the following equations.

1
x(t) = x0 + v0 t + at2
2
v(t) = v0 + at

where x(t) is the position at time t, x0 is the initial position, v0 is the initial velocity, and a is the
acceleration at time t. The S-curve profile equations also include jerk.

1 1
x(t) = x0 + v0 t + at2 + jt3
2 6
1 2
v(t) = v0 + at + jt
2
a(t) = a0 + jt

where j is the jerk at time t, a(t) is the acceleration at time t, and a0 is the initial acceleration.
More derivations are required to determine when to start and stop the different profile phases. The
derivations for a trapezoid profile are in appendix D.5 and the derivations for an S-curve profile are
in appendix D.6.

16.4 Other profile types

The ultimate goal of any profile is to match the profile’s motion characteristics to the desired appli-
cation. Trapezoidal and S-curve profiles work well when the system’s torque response curve is fairly
flat. In other words, when the output torque does not vary that much over the range of velocities the
system will be experiencing. This is true for most servo motor systems, whether DC brushed or DC
brushless.
Step motors, however, do not have flat torque/speed curves. Torque output is nonlinear, sometimes
has a large drop at a location called the “mid-range instability”, and generally drops off at higher
velocities.
Mid-range instability occurs at the step frequency when the motor’s natural resonance frequency
matches the current step rate. To address mid-range instability, the most common technique is to use
a nonzero starting velocity. This means that the profile instantly “jumps” to a programmed velocity
upon initial acceleration, and while decelerating. While crude, this technique sometimes provides
better results than a smooth ramp for zero, particularly for systems that do not use a microstepping
drive technique.
To address torque drop-off at higher velocities, a parabolic profile can be used. The corresponding
acceleration curve has the smallest acceleration when the velocity is highest. This is a good match for
 180 Chapter 16. Motion profiles

stepper motors because there is less torque available at higher speeds. However, notice that starting
and ending accelerations are very high, and there is no “S” phase where the acceleration smoothly
transitions to zero. If load oscillation is a problem, parabolic profiles may not work as well as an
S-curve despite the fact that a standard S-curve profile is not optimized for a stepper motor from the
standpoint of the torque/speed curve.

16.5 Further reading

FRC teams 254 and 971 gave a talk at FIRST World Championships in 2015 about motion profiles
[1].
VI
Appendices

A Simplifying block diagrams . . . . . . . . . 183

A.1 Cascaded blocks
A.2 Combining blocks in parallel
A.3 Removing a block from a feedforward loop
A.4 Eliminating a feedback loop
A.5 Removing a block from a feedback loop

B Installing Python packages . . . . . . . . . 187

B.1 Windows instructions
B.2 Linux instructions

C State-space canonical forms . . . . . . . . 189

C.1 Controllable canonical form
C.2 Observable canonical form

D Derivations . . . . . . . . . . . . . . . . . . . . . . . . 191
D.1 Transfer function in feedback
D.2 Optimal control law
D.3 Zero-order hold for state-space
D.4 Kalman filter as Luenberger observer
D.5 Trapezoidal motion profile
D.6 S-curve motion profile

Glossary . . . . . . . . . . . . . . . . . . . . . . . . . . . 198

Bibliography . . . . . . . . . . . . . . . . . . . . . . . 199
Online
Miscellaneous

Index . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 201
This page intentionally left blank
 Sunset in an airplane over New Mexico

A. Simplifying block diagrams

A.1 Cascaded blocks

Y = (P1 P2 )X (A.1)

X(s) P1 P2 Y (s)

Figure A.1: Cascaded blocks

X(s) P1 P2 Y (s)

Figure A.2: Simplified cascaded blocks

A.2 Combining blocks in parallel

Y = P1 X ± P2 X (A.2)

+
X(s) P1 Y (s)
±

P2

Figure A.3: Parallel blocks

 184 Appendix A. Simplifying block diagrams

X(s) P1 ± P2 Y (s)

Figure A.4: Simplified parallel blocks

A.3 Removing a block from a feedforward loop

Y = P1 X ± P2 X (A.3)

+
X(s) P1 Y (s)
±

P2

Figure A.5: Feedforward loop

P1 +
X(s) P2 Y (s)
P2
±

Figure A.6: Transformed feedforward loop

A.4 Eliminating a feedback loop

Y = P1 (X ∓ P2 Y ) (A.4)

+
X(s) P1 Y (s)
∓

P2

Figure A.7: Feedback loop

P1
X(s) 1±P1 P 2 Y (s)

Figure A.8: Simplified feedback loop

 A.5 Removing a block from a feedback loop 185

A.5 Removing a block from a feedback loop

Y = P1 (X ∓ P2 Y ) (A.5)

+
X(s) P1 Y (s)
∓

P2

Figure A.9: Feedback loop

1 −
X(s) P1 P2 Y (s)
P2
∓

Figure A.10: Transformed feedback loop

This page intentionally left blank
 Sunset in an airplane over New Mexico

B. Installing Python packages

B.1 Windows instructions

To install Python, download the installer for Python 3.5 or higher from https://www.python.org/
downloads/ and run it.
To install Python packages, run py -3 -m pip install pkg via cmd.exe or Powershell where
pkg should be the name of the package. Packages can be upgraded with py -3 -m pip install
--user --upgrade pkg.

B.2 Linux instructions

To install Python, install the appropriate packages from table B.1 using your system’s package man-
ager.

Debian/Ubuntu Arch Linux

python3 python
python3-pip python-pip

Table B.1: Required system packages

To install Python packages, run pip3 install --user pkg where pkg should be the name of the
package. Using --user makes installation not require root privileges. Packages can be upgraded
with pip3 install --user --upgrade pkg.
This page intentionally left blank
 Sunset in an airplane over New Mexico

C. State-space canonical forms

There are two canonical forms used to represent state-space models: controllable canonical form and
observable canonical form. They are used to guarantee controllability and observability of a system
respectively, which are mathematical duals of each other. That is, the controller and estimator (state
observer) are complementary problems.

C.1 Controllable canonical form

Given a system of the form

n1 s3 + n2 s2 + n3 s + n4
G(s) = (C.1)
s4 + d1 s3 + d2 s2 + d3 s + d4

the canonical realization of it that satisfies equation (9.5) is

   
0 1 0 0 0
 0 0 1 0  0
ẋ(t) = 
 0
 x(t) +   u(t) (C.2)
0 0 1  0
−d4 −d3 −d2 −d1 1
[ ]
y(t) = n4 n3 n2 n1 x(t) (C.3)

C.2 Observable canonical form

The canonical realization of the system in equation (C.1) that satisfies equation (9.6) is
190 Appendix C. State-space canonical forms

   
0 0 0−d4 n4
1 0 0 
−d3   
ẋ(t) =  x(t) + n3  u(t) (C.4)
0 1 0−d2  n2 
0 0 1−d1 n1
[ ]
y(t) = 0 0 0 1 x(t) (C.5)
 Sunset in an airplane over New Mexico

D. Derivations

D.1 Transfer function in feedback

Given the feedback network in figure D.1, find an expression for Y (s).

+ Z(s)
X(s) G(s) Y (s)
−

H(s)

Figure D.1: Closed-loop block diagram

Y (s) = Z(s)G(s)
Z(s) = X(s) − Y (s)H(s)
X(s) = Z(s) + Y (s)H(s)
X(s) = Z(s) + Z(s)G(s)H(s)
Y (s) Z(s)G(s)
=
X(s) Z(s) + Z(s)G(s)H(s)
Y (s) G(s)
= (D.1)
X(s) 1 + G(s)H(s)

A more general form is

Y (s) G(s)
= (D.2)
X(s) 1 ∓ G(s)H(s)
 192 Appendix D. Derivations

where positive feedback uses the top sign and negative feedback uses the bottom sign.

D.2 Optimal control law

For a continuous time linear system described by

ẋ = Ax + Bu (D.3)

with the cost function

∫∞
( )
J= xT Qx + uT Ru dt
0

where J represents a tradeoff between state excursion and control effort with the weighting factors
Q and R, the feedback control law which minimizes J is

u = −Kx

where K is given by

( )
K = R−1 BT P + NT

and P is found by solving the continuous time algebraic Riccati equation defined as

( )
AT P + PA − (PB + N) R−1 BT P + NT + Q = 0

or alternatively

AT P + PA − PBR−1 BT P + Q = 0

with

A = A − BR−1 NT
Q = Q − NR−1 NT

Snippet D.1 computes the optimal infinite horizon, discrete time LQR controller.
import control as cnt
import numpy as np
import scipy as sp

def dlqr(sys , Q, R):

""" Solves for the optimal discrete -time LQR controller .

x(n+1) = A * x(n) + B * u(n)

J = sum (0, inf , x.T * Q * x + u.T * R * u)
 D.3 Zero-order hold for state-space 193

Keyword arguments :
A -- numpy . matrix ( states x states ), The A matrix .
B -- numpy . matrix ( inputs x states ), The B matrix .
Q -- numpy . matrix ( states x states ), The state cost matrix .
R -- numpy . matrix ( inputs x inputs ), The control effort cost matrix .

Returns :
numpy. matrix ( states x inputs ), K
"""
m = sys.A. shape [0]

controllability_rank = np. linalg . matrix_rank (cnt.ctrb(sys.A, sys.B))

if controllability_rank != m:
print(
" Warning : Controllability of %d != %d, uncontrollable state"
% ( controllability_rank , m)
)

# P = A.T * P * A - (A.T * P * B) * np. linalg .inv(R + B.T * P * B) *

# (B.T * P.T * A) + Q
P = sp. linalg . solve_discrete_are (a=sys.A, b=sys.B, q=Q, r=R)

F = np. linalg .inv(R + sys.B.T * P * sys.B) * sys.B.T * P * sys.A

return F

Snippet D.1. Infinite horizon, discrete time LQR computation in Python

Other formulations of LQR for finite horizon and discrete time can be seen on Wikipedia [13].
MIT OpenCourseWare has a rigorous proof of the results shown above [21].

D.3 Zero-order hold for state-space

Starting with the continuous model

ẋ(t) = Ax(t) + Bu(t)

we know that the matrix exponential is

d At
e = AeAt = eAt A
dt

and by premultiplying the model we get

e−At ẋ(t) = e−At Ax(t) + e−At Bu(t)

which we recognize as

d ( −At )
e x(t) = e−At Bu(t)
dt

By integrating this equation, we get

194 Appendix D. Derivations

∫ t
−At
e x(t) − e x(0) = 0
e−Aτ Bu(τ ) dτ
0
∫ t
x(t) = eAt x(0) + eA(t−τ ) Bu(τ ) dτ
0

which is an analytical solution to the continuous model. Now we want to discretize it.

def
xk = x(kT )
∫ kT
xk = e AkT
x(0) + eA(kT −τ ) Bu(τ ) dτ
0
∫ (k+1)T
xk+1 = e A(k+1)T
x(0) + eA((k+1)T −τ ) Bu(τ ) dτ
0
∫ kT ∫ (k+1)T
A((k+1)T −τ )
xk+1 = e A(k+1)T
x(0) + e Bu(τ ) dτ + eA((k+1)T −τ ) Bu(τ ) dτ
0 kT
∫ kT ∫ (k+1)T
xk+1 = e A(k+1)T
x(0) + eA((k+1)T −τ ) Bu(τ ) dτ + eA(kT +T −τ ) Bu(τ ) dτ
0 kT
( ∫ kT ) ∫ (k+1)T
xk+1 = eAT eAkT x(0) + eA(kT −τ ) Bu(τ ) dτ + eA(kT +T −τ ) Bu(τ ) dτ
0 kT
| {z }
xk

We assume that u is constant during each timestep, so it can be pulled out of the integral.

(∫ )
(k+1)T
xk+1 = eAT xk + eA(kT +T −τ ) dτ Buk
kT

The second term can be simplified by substituting it with the function v(τ ) = kT + T − τ . Note
that dτ = −dv.

(∫ )
v((k+1)T )
xk+1 = e AT
xk − e Av
dv Buk
v(kT )
(∫ 0 )
xk+1 = e AT
xk − eAv
dv Buk
T
(∫ T )
xk+1 = eAT xk + Av
e dv Buk
0
−1 Av T
xk+1 = e AT
xk + A e |0 Buk
−1 AT
xk+1 = e AT
xk + A A0
(e −e )Buk
xk+1 = eAT xk + A−1 (eAT − I)Buk

which is an exact solution to the discretization problem.

 D.4 Kalman filter as Luenberger observer 195

D.4 Kalman filter as Luenberger observer

A Luenberger observer is defined as

−
k+1 = Ax̂k + Buk + L(yk − ŷk )
x̂+ (D.4)
ŷk = Cx̂−
k (D.5)

where a superscript of minus denotes a priori and plus denotes a posteriori estimate. Combining
equation (D.4) and equation (D.5) gives

− −
k+1 = Ax̂k + Buk + L(yk − Cx̂k )
x̂+ (D.6)

The following is a Kalman filter that considers the current update step and the next predict step
together rather than the current predict step and current update step.

Update step
Kk = P− T − T
k H (HPk H + R)
−1
(D.7)
x̂+
k = x̂−
k + Kk (yk − Hx̂−
k) (D.8)
P+k = (I − Kk H)P−
k (D.9)
Predict step
x̂+ +
k+1 = Ax̂k + Buk (D.10)
P−
k+1 = AP+
kA
T
+ ΓQΓ T
(D.11)

Substitute equation (D.8) into equation (D.10).

− −
k+1 = A(x̂k + Kk (yk − Hx̂k )) + Buk
x̂+
− −
k+1 = Axk + AKk (yk − Hx̂k ) + Buk
x̂+
− −
k+1 = Ax̂k + Buk + AKk (yk − Hx̂k )
x̂+

Let C = H and L = AKk .

− −
k+1 = Ax̂k + Buk + L(yk − Cx̂k )
x̂+ (D.12)

which matches equation (D.6). Therefore, the eigenvalues of the Kalman filter observer can be ob-
tained by

eig(A − LC)
eig(A − (AKk )(H))
eig(A(I − Kk H)) (D.13)
 196 Appendix D. Derivations

D.4.1 Luenberger observer with separate prediction and update

To run a Luenberger observer with separate prediction and update steps, substitute the relationship
between the Luenberger observer and Kalman filter matrices derived above into the Kalman filter
equations.
Appendix D.4 shows that C = H and L = AKk . Since L and A are constant, one must assume
Kk has reached steady-state. Then, K = A−1 L. Substitute this and C = H into the Kalman filter
update equation.

− −
k+1 = x̂k+1 + K(yk+1 − Hx̂k+1 )
x̂+
− −1 −
k+1 = x̂k+1 + A L(yk+1 − Cx̂k+1 )
x̂+

Substitute in equation (13.4).

− −1
k+1 = x̂k+1 + A L(yk+1 − ŷk+1 )
x̂+

The predict step is the same as the Kalman filter’s. Therefore, a Luenberger observer run with pre-
diction and update steps is written as follows.

Predict step
x̂− −
k+1 = Ax̂k + Buk (D.14)
Update step
− −1
k+1 = x̂k+1 + A L(yk+1 − ŷk+1 )
x̂+ (D.15)
ŷk+1 = Cx̂−
k+1 (D.16)

D.5 Trapezoidal motion profile

D.6 S-curve motion profile
 Sunset near Porter Academic building at UCSC

Glossary

agent An independent actor being controlled through autonomy or human-in-the-loop (e.g., a robot,
aircraft, etc.).

control effort A term describing how much force, pressure, etc. an actuator is exerting.
control input The input of a plant used for the purpose of controlling it.
control law Also known as control policy, is a mathematical formula used by the controller to deter-
mine the input u that is sent to the plant. This control law is designed to drive the system
from its current state to some other desired state.
control system Monitors and controls the behavior of a system.
controller Used in positive or negative feedback with a plant to bring about a desired system state
by driving the difference between a reference signal and the output to zero.

discretization The process by which a continuous (e.g., analog) system or controller design is con-
verted to discrete (e.g., digital).
disturbance An external force acting on a system that isn’t included in the system’s model.
disturbance rejection The quality of a feedback control system to compensate for external forces
to reach a desired reference.

error Reference minus an output or state.

feedback gain The gain from the output to an earlier point in a control system diagram.

gain A proportional value that shows the relationship between the magnitude of an input signal to
the magnitude of an output signal at steady-state.
gain margin See section 7.7 on gain and phase margin.

impulse response The response of a system to the Dirac delta function.

input An input to the plant (hence the name) that can be used to change the plant’s state.

linearization A method by which a nonlinear system’s dynamics are approximated by a linear system.
localization The process of using measurements of the environment to determine an agent’s pose.
198 Glossary

model A set of mathematical equations that reflects some aspect of a physical system’s behavior.

noise immunity The quality of a system to have its performance or stability unaffected by noise in
the outputs (see also: robustness).

observer In control theory, a system that provides an estimate of the internal state of a given real
system from measurements of the input and output of the real system.
open-loop gain The gain directly from the input to the output, ignoring loops.
output Measurements from sensors.
output-based control Controls the system’s state via the outputs.
overshoot The amount by which a system’s state surpasses the reference after rising toward it.

phase margin See section 7.7 on gain and phase margin.

plant The system or collection of actuators being controlled.
pose The orientation of an agent in the world, which is represented by all or part of the agent’s state.
process variable The term used to describe the output of a PID controller.

realization In control theory, this is an implementation of a given input-output behavior as a state-

space model.
reference The desired state.
regulator A controller that attempts to minimize the error from a constant reference in the presence
of disturbances.
rise time The time a system takes to initially reach the reference after applying a step input.
robustness The quality of a feedback control system to remain stable in response to disturbances
and uncertainty.

setpoint The term used to describe the reference of a PID controller.

settling time The time a system takes to settle at the reference after a step input is applied.
state A characteristic of a system (e.g., velocity) that can be used to determine the system’s future
behavior.
state feedback Uses state instead of output in feedback.
steady-state error Error after system reaches equilibrium.
step input A system input that is 0 for t < 0 and 1 for t ≥ 0.
step response The response of a system to a step input.
stochastic process A process whose model is partially or completely defined by random variables.
system A term encompassing a plant and its interaction with a controller and observer, which is
treated as a single entity. Mathematically speaking, a system maps inputs to outputs through
a linear combination of states.
system response The behavior of a system over time for a given input.

time-invariant The system’s fundamental response does not change over time.
tracking In control theory, the process of making the output of a control system follow the reference.

unity feedback A feedback network in a control system diagram with a feedback gain of 1.
 Sunset near Porter Academic building at UCSC

Bibliography

Online
[13] Wikipedia Commons. Linear-quadratic regulator. : https : / / en . wikipedia . org /
wiki / Linear % E2 % 80 % 93quadratic _ regulator (visited on 03/24/2018) (cited on
page 193).
[15] Sean Humbert. Why do we have to linearize around an equilibrium point? : https : / /
www . cds . caltech . edu / %7Emurray / courses / cds101 / fa02 / faq / 02 - 10 - 09 _
linearization.html (visited on 07/12/2018) (cited on page 132).
[16] Kapitanyuk, Proskurnikov, and Cao. A guiding vector field algorithm for path following control
of nonholonomic mobile robots. : https://arxiv.org/pdf/1610.04391.pdf (visited
on 08/09/2018) (cited on page 138).
[17] Edgar Kraft. A Quaternion-based Unscented Kalman Filter for Orientation Tracking. : https:
//kodlab.seas.upenn.edu/uploads/Arun/UKFpaper.pdf (visited on 07/11/2018)
(cited on page 167).
[18] Charles F. Van Loan. Computing Integrals Involving the Matrix Exponential. : https :
/ / www . cs . cornell . edu / cv / ResearchPDF / computing . integrals . involving .
Matrix.Exp.pdf (visited on 06/21/2018) (cited on page 115).
[19] Luca, Oriolo, and Vendittelli. Control of Wheeled Mobile Robots: An Experimental Overview.
: https : / / www . dis . uniroma1 . it / ~labrob / pub / papers / Ramsete01 . pdf
(visited on 08/09/2018) (cited on pages 133, 134).
[20] MIT OpenCourseWare. Linear Quadratic Regulator. : https://ocw.mit.edu/courses/
mechanical-engineering/2-154-maneuvering-and-control-of-surface-and-
underwater-vehicles-13-49-fall-2004/lecture-notes/lec19.pdf (visited on
06/26/2018) (cited on page 91).
[21] Russ Tedrake. Chapter 9. Linear Quadratic Regulators. : http : / / underactuated .
csail.mit.edu/underactuated.html?chapter=lqr (visited on 07/08/2018) (cited on
page 193).
200 Bibliography

[22] Eric A. Wan and Rudolph van der Merwe. The Unscented Kalman Filter for Nonlinear Esti-
mation. : https://www.seas.harvard.edu/courses/cs281/papers/unscented.
pdf (visited on 06/26/2018) (cited on page 167).

Misc
[1] FRC team 254. Motion Planning and Control in FRC. 2015. : https://www.youtube.
com/watch?v=8319J1BEHwM (cited on page 180).
[2] 3Blue1Brown. Eigenvectors and eigenvalues. 2016. : https://youtu.be/PFDu9oVAE-g
(cited on page 84).
[3] 3Blue1Brown. Essence of linear algebra. 2016. : https://www.youtube.com/playlist?
list=PLZHQObOWTQDPD3MizzM2xVFitgF8hE_ab (cited on page 59).
[4] 3Blue1Brown. Essence of linear algebra preview. 2016. : https://youtu.be/kjBOesZCoqc
(cited on page 59).
[5] 3Blue1Brown. Inverse matrices, column space, and null space. 2016. : https://youtu.
be/uQhTuRlWMxw (cited on page 78).
[6] 3Blue1Brown. Linear combinations, span, and basis vectors. 2016. : https://youtu.
be/k7RM-ot2NWY (cited on page 64).
[7] 3Blue1Brown. Linear transformations and matrices. 2016. : https : / / youtu . be /
kYB8IZa5AuE (cited on page 68).
[8] 3Blue1Brown. Linear transformations and matrices. 2016. : https : / / youtu . be /
XkY2DOUCWMU (cited on page 71).
[9] 3Blue1Brown. Nonsquare matrices as transformations between dimensions. 2016. : https:
//youtu.be/v8VSDg_WQlA (cited on page 79).
[10] 3Blue1Brown. The determinant. 2016. : https://youtu.be/Ip3X9LOh2dk (cited on
page 74).
[11] 3Blue1Brown. Vectors, what even are they? 2016. : https://youtu.be/fNk_zzaMoSs
(cited on page 62).
[12] 3Blue1Brown. Essence of calculus. 2017. : https://www.youtube.com/playlist?
list=PLZHQObOWTQDMsr9K-rj53DwVRMYO3t5Yr (cited on page 9).
[14] Brian Douglas. Gain and Phase Margins Explained! 2015. : https : / / youtu . be /
ThoA4amCAX4 (cited on page 56).
 Sunset near Porter Academic building at UCSC

Index

Block diagrams, 33 DC brushed motor equations, 20

simplification, 183 drivetrain equations, 124
elevator equations, 119
Controller design flywheel equations, 122
actuator saturation, 50 rotating claw equations, 130
controllability, 87 single-jointed arm equations, 127
controllable canonical form, 189
LQR, 90 Gain, 33
Bryson’s rule, 92
optimal control law, 91 Integral control
observability, 88 plant augmentation, 102
observable canonical form, 189 U error estimation, 102
pole placement, 90
Linear algebra
Digital signal processing basis vectors, 62
aliasing, 107 linear combination, 62
Nyquist frequency, 107 vectors, 59
Discretization, 108
backward Euler method, 108 Matrices
bilinear transform, 108 determinant, 71
forward Euler method, 108 eigenvalues, 80
matrix exponential, 112 inverse, 76
Taylor series, 113 linear systems, 74
zero-order hold, 114 linear transformation, 65
multiplication, 69
Feedforward rank, 77
steady-state feedforward, 97 Model augmentation
two-state feedforward, 100 of controller, 93
FRC models of observer, 93
202 Index

of output, 94 marginal probability density functions,

of plant, 93 149
Motion profiles, 177 probability density function, 145
S-curve, 177 probability density functions, 147
trapezoidal, 178 random variables, 145
variance, 147
Nonlinear control
extended Kalman filter, 166 Stability
linearization, 118, 132 eigenvalues, 89, 143
Lyapunov stability, 132 gain margin, 56
unscented Kalman filter, 167 phase margin, 56
poles and zeroes, 46
Optimal control root locus, 48
LQR, 90 State-space controllers
Bryson’s rule, 92 closed-loop, 89
optimal control law, 91 open-loop, 87
two-state feedforward, 100 State-space observers
Kalman filter
Physics as Luenberger observer, 161
conservation of energy, 18 derivations, 152, 155
sum of forces, 15 equations, 158
sum of torques, 17 extended Kalman filter, 166
PID control, 35, 53, 85 MMAE, 166
Probability, 145 setup, 159
Bayes’s rule, 149 smoother, 163
central limit theorem, 152 unscented Kalman filter, 167
conditional expectation, 150 Luenberger observer, 141
conditional probability density functions, Steady-state error, 51
149 Stochastic
conditional variances, 150 linear systems, 152
covariance, 148 measurement noise, 153
covariance matrix, 150 process noise, 153
expected value, 146 two-sensor problem, 154