Controls Engineering in the

FIRST Robotics Competition

Graduate-level control theory for high schoolers

Tyler Veness
Controls Engineering in the
FIRST Robotics Competition
Graduate-level control theory for high schoolers

Tyler Veness
Copyright © 2017-2020 Tyler Veness
HTTPS://GITHUB.COM/CALCMOGUL/CONTROLS-ENGINEERING-IN-FRC
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 https://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 “AS IS” BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the
specific language governing permissions and limitations under the License.
Generated from commit 5132b8b made on April 23, 2020. The latest version can be downloaded
from https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
Tree next to Oakes Circle at UCSC

Contents

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

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

0.1 Prerequisites 1
0.2 Structure of this book 1
0.3 Ethos of this book 2
0.3.1 Role of classical control theory . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
0.3.2 An integrated approach to nonlinear control theory . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
0.4 Mindset of an egoless engineer 3
0.5 Request for feedback 3

I Fundamentals of control theory

1 Control system basics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

1.1 Nomenclature 7
1.2 What is gain? 8
1.3 Block diagrams 8
1.4 Why feedback control? 9

2 PID controllers . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
2.1 Proportional term 11
2.2 Derivative term 12
2.3 Integral term 14
2.4 PID controller definition 16
2.5 Response types 16
2.6 Manual tuning 17
2.7 Actuator saturation 17
2.8 Limitations 18

3 Transfer functions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
3.1 Laplace transform 19
3.2 Parts of a transfer function 19
3.2.1 Poles and zeroes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
3.2.2 Nonminimum phase zeroes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
3.2.3 Pole-zero cancellation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
3.3 Transfer functions in feedback 22
3.3.1 Root locus . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22

II Modern control theory

4 Linear algebra . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27
4.1 Vectors 27
4.2 Linear combinations, span, and basis vectors 27
4.3 Linear transformations and matrices 27
4.4 Matrix multiplication as composition 27
4.5 The determinant 28
4.6 Inverse matrices, column space, and null space 28
4.7 Nonsquare matrices as transformations between dimensions 28
4.8 Eigenvectors and eigenvalues 28
4.9 Miscellaneous notation 28

5 State-space controllers . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29
5.1 From PID control to model-based control 29
5.2 What is a dynamical system? 30
5.3 State-space notation 30
5.3.1 What is state-space? . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30
5.3.2 Benefits over classical control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30
5.3.3 Definition . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31
5.4 Controllability 32
5.5 Observability 32
5.6 Closed-loop controller 33
5.7 Pole placement 34
5.8 Linear-quadratic regulator 34
5.8.1 Bryson’s rule . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35
5.8.2 Pole placement vs LQR . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36
5.9 Model augmentation 37
5.9.1 Plant augmentation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37
5.9.2 Controller augmentation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37
5.9.3 Observer augmentation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37
5.9.4 Output augmentation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38
5.9.5 Examples . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38
5.10 Feedforward 39
5.10.1 Plant inversion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40
5.10.2 Unmodeled dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41
5.11 Integral control 42
5.11.1 Plant augmentation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43
5.11.2 Input error estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43

6 Digital control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45
6.1 Phase loss 45
6.2 s-plane to z-plane 46
6.2.1 z-plane stability . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46
6.2.2 z-plane behavior . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47
6.2.3 Nyquist frequency . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47
6.3 Discretization methods 48
6.4 Effects of discretization on controller performance 50
6.5 Matrix exponential 52
6.6 Taylor series 53
6.7 Zero-order hold for state-space 54

7 Nonlinear control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57
7.1 Introduction 57
7.2 Linearization 57
7.3 Lyapunov stability 58
7.4 Affine systems 59
7.4.1 Feedback linearization for reference tracking . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59
7.5 Further reading 60

8 State-space applications . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61
8.1 Elevator 61
8.1.1 Continuous state-space model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61
8.1.2 Model augmentation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62
8.1.3 Gravity feedforward . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62
8.1.4 Simulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63
8.1.5 Implementation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63
8.2 Flywheel 63
8.2.1 Continuous state-space model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63
8.2.2 Model augmentation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64
8.2.3 Simulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64
8.2.4 Implementation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64
8.2.5 Flywheel model without encoder . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64
8.2.6 Voltage compensation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 66
8.3 Single-jointed arm 66
8.3.1 Continuous state-space model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 66
8.3.2 Model augmentation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67
8.3.3 Gravity feedforward . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67
8.3.4 Simulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 68
8.3.5 Implementation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 68
8.4 Pendulum 69
8.4.1 State-space model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 69
8.5 Differential drive 70
8.5.1 Velocity subspace state-space model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70
8.5.2 Linear time-varying model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72
8.5.3 Improving model accuracy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 74
8.5.4 Cross track error controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 75
8.5.5 Explicit time-varying control law . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 76
8.5.6 Nonlinear observer design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 78
8.6 Ramsete unicycle controller 82
8.6.1 Velocity and turning rate command derivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 82
8.6.2 Nonlinear controller equations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 83
8.6.3 Linear reference tracker . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 84
8.7 Linear time-varying unicycle controller (cascaded) 87
8.7.1 Explicit time-varying control law . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 88

III Estimation and localization

9 Stochastic control theory . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 93
9.1 Terminology 93
9.2 State observers 93
9.2.1 Luenberger observer . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 94
9.3 Introduction to probability 96
9.3.1 Random variables . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 96
9.3.2 Expected value . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 97
9.3.3 Variance . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 97
9.3.4 Joint probability density functions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 98
9.3.5 Covariance . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 99
9.3.6 Correlation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 99
9.3.7 Independence . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 99
9.3.8 Marginal probability density functions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 100
9.3.9 Conditional probability density functions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 100
9.3.10 Bayes’s rule . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 100
9.3.11 Conditional expectation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 100
9.3.12 Conditional variances . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 100
9.3.13 Random vectors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 100
9.3.14 Covariance matrix . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 101
9.3.15 Relations for independent random vectors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 101
9.3.16 Gaussian random variables . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 102
9.4 Linear stochastic systems 103
9.4.1 State vector expectation evolution . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 103
9.4.2 Error covariance matrix evolution . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 103
9.4.3 Measurement vector expectation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 104
9.4.4 Measurement covariance matrix . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 104
9.5 Two-sensor problem 104
9.6 Kalman filter 105
9.6.1 Derivations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 105
9.6.2 Predict and update equations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 108
9.6.3 Setup . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 110
9.6.4 Simulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 111
9.6.5 Kalman filter as Luenberger observer . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 112
9.7 Kalman smoother 114
9.7.1 Derivations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 114
9.7.2 State update equation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 114
9.7.3 Error covariance equation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 115
9.7.4 Optimal estimate . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 116
9.7.5 Predict and update equations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 117
9.7.6 Example . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 118
9.8 Extended Kalman filter 120
9.9 Unscented Kalman filter 120
9.10 Multiple model adaptive estimation 121

10 Pose estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 123

10.1 Euler integration 123
10.2 Pose exponential 123
10.2.1 What is a group? . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 123
10.2.2 What is a pose? . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 124
10.2.3 What is a twist? . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 124
10.2.4 Derivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 124
10.2.5 Lie groups . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 127
10.3 Pose correction 128

IV System modeling

11 Calculus . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 131
11.1 Derivatives 131
11.1.1 Power rule . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 132
11.1.2 Product rule . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 132
11.1.3 Chain rule . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 132
11.2 Integrals 132
11.2.1 Change of variables . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 133
11.3 Tables 133
11.3.1 Common derivatives and integrals . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 133
12 Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 135
12.1 Linear motion 135
12.2 Angular motion 135
12.3 Vectors 136
12.3.1 Basic vector operations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 136
12.3.2 Cross product . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 136
12.4 Curvilinear motion 137
12.4.1 Differential drive . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 137
12.4.2 Mecanum drive . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 139
12.4.3 Swerve drive . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 142

13 Model examples . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 145

13.1 DC brushed motor 145
13.1.1 Equations of motion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 146
13.1.2 Calculating constants . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 146
13.1.3 Current limiting . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 148
13.2 Elevator 148
13.2.1 Equations of motion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 148
13.3 Flywheel 150
13.3.1 Equations of motion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 150
13.3.2 Calculating constants . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 151
13.4 Single-jointed arm 152
13.4.1 Equations of motion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 152
13.4.2 Calculating constants . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 153
13.5 Pendulum 154
13.5.1 Force derivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 154
13.5.2 Torque derivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 155
13.5.3 Energy derivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 156
13.6 Differential drive 157
13.6.1 Equations of motion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 157
13.6.2 Calculating constants . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 160

14 System identification . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 161

14.1 1 DOF mechanism model 162
14.2 Drivetrain velocity model 163

V Motion planning

15 Motion profiles . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 169

15.1 1 DOF motion profiles 169
15.1.1 Jerk . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 170
15.1.2 Profile selection . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 170
15.1.3 Profile equations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 171
15.1.4 Other profile types . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 171
15.1.5 Further reading . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 172
15.2 2 DOF motion profiles 172

16 Trajectory optimization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 173

VI Appendices

A Simplifying block diagrams . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 177

A.1 Cascaded blocks 177
A.2 Combining blocks in parallel 177
A.3 Removing a block from a feedforward loop 178
A.4 Eliminating a feedback loop 178
A.5 Removing a block from a feedback loop 179

B Laplace domain analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 181

B.1 Projections 181
B.2 Fourier transform 184
B.3 Laplace transform 186
B.4 Laplace transform definition 188
B.5 Case study: steady-state error 188
B.6 Case study: flywheel PID control 190

C Robust control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 195

C.1 Gain margin and phase margin 195

D Installing Python packages . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 197

D.1 Windows instructions 197
D.2 Linux instructions 197

E Linear-quadratic regulator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 199

E.1 Derivation 199
E.2 Output feedback 202
E.3 Implicit model following 203

F QR-weighted linear plant inversion . . . . . . . . . . . . . . . . . . . . . . . . . . . 207

F.1 Necessary theorems 207
F.2 Setup 208
F.3 Minimization 208

G Steady-state feedforward . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 211

G.1 Continuous case 211
G.2 Discrete case 212
G.3 Deriving steady-state input 213
G.3.1 Case study: second-order CIM motor model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 213

H Derivations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 215
H.1 Transfer function in feedback 215
H.2 Zero-order hold for state-space 216
H.3 Kalman filter as Luenberger observer 217
H.3.1 Luenberger observer with separate prediction and update . . . . . . . . . . . . . . . . . . . 218
H.4 Trapezoidal motion profile 218

Glossary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 220

Bibliography . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 221
Online 221
Miscellaneous 222

Index . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 223
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 learned in my graduate-level en-
gineering 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 con-
straints. 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 future team
members 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 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, “Fundamentals of control theory,” introduces the basics of control theory, teaches the funda-
mentals 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 II, “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 I 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 IV are converted to state-space representation, implemented,
and tested with a discrete controller.
Part II also introduces the basics of nonlinear control system analysis with Lyapunov functions. It
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.
2 Chapter 0. Notes to the reader

Part III, “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 IV, “System modeling,” introduces the basic calculus and physics concepts required to derive
the models used in the previous chapters. It walks through the derivations for several common FRC
subsystems. Then, methods for system identification are discussed for empirically measuring model
parameters.
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. It introduces motion profiles with one degree
of freedom for simple maneuvers. Trajectory optimization methods are presented for generating
profiles with higher degrees of freedom.
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 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 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 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 Mindset of an egoless engineer

Engineering has a mindset, not just a skillset. Engineers have a unique way of approaching problems,
and 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
I
Fundamentals of control
theory

1 Control system basics . . . . . . . . . . . . . . . . 7

1.1 Nomenclature
1.2 What is gain?
1.3 Block diagrams
1.4 Why feedback control?

2 PID controllers . . . . . . . . . . . . . . . . . . . . . . 11
2.1 Proportional term
2.2 Derivative term
2.3 Integral term
2.4 PID controller definition
2.5 Response types
2.6 Manual tuning
2.7 Actuator saturation
2.8 Limitations

3 Transfer functions . . . . . . . . . . . . . . . . . . 19
3.1 Laplace transform
3.2 Parts of a transfer function
3.3 Transfer functions in feedback
This page intentionally left blank
Road near walking trail off of Rice Ranch Road in Santa Maria, CA

1. Control system basics

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. A con-
troller is used to drive the plant from its current state to some desired state (the reference). Controllers
which don’t include information measured from the plant’s output are called open-loop controllers.
8 Chapter 1. Control system basics

Controllers which incorporate information fed back from the plant’s output are called closed-loop
controllers or feedback controllers. Figure 1.1 shows a plant in feedback with a controller.

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

r(t)
− Controller Plant

Figure 1.1: Control system nomenclature for a closed-loop system

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

Note that the input and output of a system are defined from the plant’s point of view. The negative
feedback controller shown is driving the difference between the reference and output, also known as
the error, to zero.

1.2 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 1.2 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 1.2: Demonstration of system with a gain of K = 2

1.3 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 1.3 is an example of one.

+
input open-loop output
∓

feedback

Figure 1.3: Block diagram with nomenclature

The open-loop gain is the total gain from the sum node at the input (the circle) to the output branch.
1.4 Why feedback control? 9

This would be the system’s gain if the feedback loop was disconnected. The feedback gain is the
total gain from the output back to the input sum node. A sum node’s output is the sum of its inputs.
Figure 1.4 is a block diagram with more formal notation in a feedback configuration.

+
X(s) P1 Y (s)
∓

P2

Figure 1.4: Feedback block diagram

∓ means “minus or plus” where a minus represents negative feedback.

1.4 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.
This page intentionally left blank
On trail between McHenry Library and Media Theater at UCSC

2. PID controllers

The PID controller is a commonly used feedback controller consisting of proportional, integral, and
derivative terms, hence the name. This chapter will build up the definition of a PID controller term
by term while trying to provide intuition for how each of them behaves.
First, we’ll get some nomenclature for PID controllers out of the way. The reference is called the
setpoint (the desired position) and the output is called the process variable (the measured position).
Below are some common variable naming conventions for relevant quantities.

r(t) setpoint u(t) control input

e(t) error y(t) output
The error e(t) is r(t) − y(t).
For those already familiar with PID control, this book’s interpretation won’t be consistent with the
classical intuition of ”past”, ”present”, and ”future” error. We will be approaching it from the view-
point of modern control theory with proportional controllers applied to different physical quantities
we care about. This will provide a more complete explanation of the derivative term’s behavior
for constant and moving setpoints, and this intuition will carry over to the modern control methods
covered later in this book.
The proportional term drives the position error to zero, the derivative term drives the velocity error to
zero, and the integral term accumulates the area between the setpoint and output plots over time (the
integral of position error) and adds the current total to the control input. We’ll go into more detail on
each of these.

2.1 Proportional term

The Proportional term drives the position error to zero.
12 Chapter 2. PID controllers

Definition 2.1.1 — Proportional controller.

u(t) = Kp e(t) (2.1)

where Kp is the proportional gain and e(t) is the error at the current time t.

Figure 2.1 shows a block diagram for a system controlled by a P controller.

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

r(t)
−
Kp e(t) Plant

Figure 2.1: P controller block diagram

Proportional gains act like “software-defined springs” that pull the system toward the desired posi-
tion. 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. This can be written
another way as F = k(0 − x) where 0 is the equilibrium point. If we let the equilibrium point be
our feedback controller’s setpoint, the equations have a one-to-one correspondence.

F = k(r − x)
u(t) = Kp e(t) = Kp (r(t) − y(t))

so the “force” with which the proportional controller pulls the system’s output toward the setpoint is
proportional to the error, just like a spring.

2.2 Derivative term

The Derivative term drives the velocity error to zero.

Definition 2.2.1 — PD controller.

de
u(t) = Kp e(t) + Kd (2.2)
dt

where Kp is the proportional gain, Kd is the derivative gain, and e(t) is the error at the current
time t.

Figure 2.2 shows a block diagram for a system controlled by a PD controller.

Kp e(t)
+ e(t) + u(t) y(t)
r(t) Plant
− +
Kd de(t)
dt

Figure 2.2: PD controller block diagram

2.2 Derivative term 13

Figure 2.3: PD controller on an elevator

A PD controller has a proportional controller for position (Kp ) and a proportional controller for
velocity (Kd ). The velocity setpoint is implicitly provided by how the position setpoint changes over
time. Figure 2.3 shows an example for an elevator.
To prove a PD controller is just two proportional controllers, we will rearrange the equation for a PD
controller.
ek − ek−1
u k = Kp e k + 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 setpoint 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

r −r x −x
Notice how k dtk−1 is the velocity of the setpoint. 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 setpoint velocity.
If the setpoint is constant, the implicit velocity setpoint is zero, so the Kd term slows the system
down if it’s moving. This acts like a “software-defined damper”. These are commonly seen on door
closers, and their damping force increases linearly with velocity.
14 Chapter 2. PID controllers

2.3 Integral term

The Integral term accumulates the area between the setpoint and output plots over time (i.e., the
integral of position error) and adds the current total to the control input. Accumulating the area
between two curves is called integration.

Definition 2.3.1 — PI controller.

Z t
u(t) = Kp e(t) + Ki e(τ ) dτ (2.3)
0

where Kp is the proportional gain, Ki is the integral gain, e(t) is the error at the current time t,
and τ is the integration variable.
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.

Figure 2.4 shows a block diagram for a system controlled by a PI controller.

Kp e(t)
+ e(t) + u(t) y(t)
r(t) Plant
− +
∫t
Ki 0
e(τ ) dτ

Figure 2.4: PI controller block diagram

When the system is close to the setpoint in steady-state, the proportional term may be too small to pull
the output all the way to the setpoint, and the derivative term is zero. This can result in steady-state
error, as shown in figure 2.5.
A common way of eliminating steady-state error is to integrate the error and add it to the control
input. This increases the control effort until the system converges. Figure 2.5 shows an example
of steady-state error for a flywheel, and figure 2.6 shows how an integrator added to the flywheel
controller eliminates it. However, too high of an integral gain can lead to overshoot, as shown in
figure 2.7.

R 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.
2.3 Integral term 15

Figure 2.5: P controller on a flywheel with steady-state error

Figure 2.6: PI controller on a flywheel without Figure 2.7: PI controller on a flywheel with over-
steady-state error shoot from large Ki gain
16 Chapter 2. PID controllers

2.4 PID controller definition

When these three terms are combined, one gets the typical definition for a PID controller.

Definition 2.4.1 — PID controller.

Z t
de
u(t) = Kp e(t) + Ki e(τ ) dτ + Kd (2.4)
0 dt

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

Figure 2.8 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 2.8: PID controller block diagram

2.5 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 2.9.
For the step responses in figure 2.9, 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.
2.6 Manual tuning 17

Figure 2.9: PID controller response types

2.6 Manual tuning

These steps apply to position PID controllers. Velocity PID controllers typically don’t need Kd .
1. Set Kp , Ki , and Kd to zero.
2. Increase Kp until the output starts to oscillate around the setpoint.
3. Increase Kd as much as possible without introducing jittering in the system response.
If the setpoint follows a trapezoidal motion profile (see section 15.1), tuning becomes a lot easier.
Plot the position setpoint, velocity setpoint, measured position, and measured velocity. The velocity
setpoint can be obtained via numerical differentiation of the position setpoint (i.e., vdesired,k =
rk −rk−1
∆t ). Increase Kp until the position tracks well, then increase Kd until the velocity tracks well.
If the controller settles at an output above or below the setpoint, one can increase Ki such that the
controller reaches the setpoint in a reasonable amount of time. However, a steady-state feedforward
is strongly preferred over 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 setpoint, 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.

2.7 Actuator saturation

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.
18 Chapter 2. PID controllers

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
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.

2.8 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; we’ll cover methods for multiple-input, multiple-output (MIMO) control in part II of this
book.
Treeline by Crown/Merril bus stop at UCSC

3. Transfer functions

This chapter briefly discusses what transfer functions are, how the locations of poles and zeroes affect
system response and stability, and how controllers affect pole locations. As long as you gain under-
standing of those concepts, don’t worry too much about not being able to follow the math presented
here; we won’t use transfer functions in modern control theory (it’s mainly provided for complete-
ness). This chapter is intended to provide a framework within which to understand results from the
mathematical machinery of modern control as well as vocabulary to communicate that understand-
ing.

3.1 Laplace transform

For an introduction to Laplace transforms and the geometric intuition behind transfer functions, we
recommend watching Zach Star’s video “What does the Laplace Transform really tell us? A visual
explanation (plus applications)” (21 minutes) [27]. An optional, more mathematical introduction is
presented in appendix B for completeness.

3.2 Parts of a transfer function

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 (3.1) is an example of a
transfer function.
zeroes
z }| {
(s − 9 + 9i)(s − 9 − 9i)
H(s) = (3.1)
s(s + 10)
| {z }
poles

3.2.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
20 Chapter 3. Transfer functions

Figure 3.1: Equation (3.1) plotted in 3D

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 3.1).
When the factors of the denominator are broken apart using partial fraction expansion into something
like s+a
A B
+ s+b , the constants A and B are called residues, which determine how much each pole
contributes to the system response.
The factors representing poles are each the Laplace transform of a decaying exponential1 . That means
the time domain responses of systems comprise decaying exponentials (e.g., y = e−t ).

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

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 3.2 shows the impulse responses in the
time domain for transfer functions with various pole locations. They all have an initial condition of
1.

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

Table 3.1: Pole location and stability

When a system is stable, its output may oscillate but it converges to steady-state. When a system is
1
We are handwaving Laplace transform derivations because they are complicated and neither relevant nor useful.
3.2 Parts of a transfer function 21

Im(jω)
Stable Unstable

t
t
t

LHP RHP
t

t t

Re(σ)

t t t

Figure 3.2: Impulse response vs pole location

marginally stable, its output oscillates at a constant amplitude forever. When a system is unstable, its
output grows without bound.

3.2.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
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 dynamics
to go the desired direction.
Another example is a Segway. To move forward by some distance, the Segway must first roll back-
ward 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.

3.2.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
22 Chapter 3. Transfer functions

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.

3.3 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.

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

Figure 3.3: Feedback controller block diagram

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

The transfer function of figure 3.3, a control system diagram with feedback, from input to output is
Y (s) KG
Gcl (s) = = (3.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 H.1 for a derivation). As another
example, the transfer function from the input to the error is
E(s) 1
Gcl (s) = = (3.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.

3.3.1 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 3.4), move toward each other then split toward asymptotes
(figure 3.5), or move toward zeroes (figure 3.6). The system in figure 3.6 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.
3.3 Transfer functions in feedback 23

Figure 3.4: Root locus showing pole moving to- Figure 3.5: Root locus showing poles moving to-
ward negative infinity ward asymptotes

Figure 3.6: Root locus of equation (3.1) showing

poles moving toward zeroes.

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 (a CIM
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.0181 V /rad/s,

R = 0.0902 Ω, and L = 230 × 10−6 H.
This plant has the root locus shown in figure 3.7. 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 3.8. 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 3.9 and 3.10). 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
24 Chapter 3. Transfer functions

Figure 3.7: Root locus of second-order DC Figure 3.8: Root locus of first-order DC brushed
brushed motor plant motor plant

Figure 3.9: Step response of second-order DC Figure 3.10: Step response of first-order DC
brushed motor plant brushed motor plant

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.
II
Modern control theory

4 Linear algebra . . . . . . . . . . . . . . . . . . . . . . 27
4.1 Vectors
4.2 Linear combinations, span, and basis vectors
4.3 Linear transformations and matrices
4.4 Matrix multiplication as composition
4.5 The determinant
4.6 Inverse matrices, column space, and null space
4.7 Nonsquare matrices as transformations between dimen-
sions
4.8 Eigenvectors and eigenvalues
4.9 Miscellaneous notation

5 State-space controllers . . . . . . . . . . . . . . 29
5.1 From PID control to model-based control
5.2 What is a dynamical system?
5.3 State-space notation
5.4 Controllability
5.5 Observability
5.6 Closed-loop controller
5.7 Pole placement
5.8 Linear-quadratic regulator
5.9 Model augmentation
5.10 Feedforward
5.11 Integral control

6 Digital control . . . . . . . . . . . . . . . . . . . . . . 45
6.1 Phase loss
6.2 s-plane to z-plane
6.3 Discretization methods
6.4 Effects of discretization on controller performance
6.5 Matrix exponential
6.6 Taylor series
6.7 Zero-order hold for state-space

7 Nonlinear control . . . . . . . . . . . . . . . . . . . 57
7.1 Introduction
7.2 Linearization
7.3 Lyapunov stability
7.4 Affine systems
7.5 Further reading

8 State-space applications . . . . . . . . . . . . 61
8.1 Elevator
8.2 Flywheel
8.3 Single-jointed arm
8.4 Pendulum
8.5 Differential drive
8.6 Ramsete unicycle controller
8.7 Linear time-varying unicycle controller (cascaded)
This page intentionally left blank
Grass clearing by Interdisciplinary Sciences building and Thimann Labs at UCSC

4. 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.
We would normally include written material here for learning linear algebra, but 3Blue1Brown’s
animated videos are better at conveying the geometric intuition involved than anything we could
write here with static text. Instead of making an inferior copy of his work, we’ll provide bibliography
entries for a selection of his videos.

4.1 Vectors
Watch the “Vectors, what even are they?” video from 3Blue1Brown’s Essence of linear algebra series
(5 minutes) [11].

4.2 Linear combinations, span, and basis vectors

Watch the “Linear combination, span, and basis vectors” video from 3Blue1Brown’s Essence of linear
algebra series (10 minutes) [6].

4.3 Linear transformations and matrices

Watch the “Linear transformations and matrices” video from 3Blue1Brown’s Essence of linear algebra
series (11 minutes) [7].

4.4 Matrix multiplication as composition

Watch the “Matrix multiplication as composition” video from 3Blue1Brown’s Essence of linear alge-
bra series (10 minutes) [8].
28 Chapter 4. Linear algebra

4.5 The determinant

Watch the “The determinant” video from 3Blue1Brown’s Essence of linear algebra series (10 minutes)
[10].

4.6 Inverse matrices, column space, and null space

Watch the “Inverse matrices, column space, and null space” video from 3Blue1Brown’s Essence of
linear algebra series (12 minutes) [5].

4.7 Nonsquare matrices as transformations between dimensions

Watch the “Nonsquare matrices as transformations between dimensions” video from 3Blue1Brown’s
Essence of linear algebra series (4 minutes) [9].

4.8 Eigenvectors and eigenvalues

Watch the “Eigenvectors and eigenvalues” video from 3Blue1Brown’s Essence of linear algebra series
(17 minutes) [3].

4.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

5. 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 D 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.

5.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.
30 Chapter 5. State-space controllers

5.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.

5.3 State-space notation

5.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.

5.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
q × p transfer functions to represent it. Not only is the resulting algebra unwieldy, but it only works
5.3 State-space notation 31

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.
If modern control theory is so great and classical control theory isn’t needed to use it, why learn
classical control theory at all? We teach classical control theory because it provides a framework
within which to understand results from the mathematical machinery of modern control as well as
vocabulary with which to communicate that understanding. For example, faster poles (poles moved
to the left in the s-plane) mean faster decay, and oscillation means there is at least one pair of complex
conjugate poles. Not only can you describe what happened succinctly, but you know why it happened
from a theoretical perspective.

5.3.3 Definition
Below are the continuous and discrete versions of state-space notation.

Definition 5.3.1 — State-space notation.

ẋ = Ax + Bu (5.1)
y = Cx + Du (5.2)

xk+1 = Axk + Buk (5.3)

yk = Cxk + Duk (5.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 5.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.

1
This book focuses on analysis and control of linear systems. See chapter 7 for more on nonlinear control.
32 Chapter 5. State-space controllers

5.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.

Theorem 5.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 (5.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 (5.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.
(C)
The condition number of the controllability matrix C is defined as σσmax
min (C)
where σmax is the max-
imum singular value2 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.

5.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 5.5.1 — Observability. A continuous time-invariant linear state-space model is ob-

servable if and only if

 
C
 CA 
 
rank  ..  = n (5.6)
 . 
CAn−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 (5.6) being rank-deficient means the outputs do not contain contributions
from every state. That is, not all states are mapped to a linear combination in the output. Therefore,
the outputs alone are insufficient to estimate all the states.
(O)
The condition number of the observability matrix O is defined as σσmax
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
2
Singular values are a generalization of eigenvalues for nonsquare matrices.
5.6 Closed-loop controller 33

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.

5.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 5.8.
First is the state update equation. Substitute the control law into equation (5.1).

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

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

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

Now, we’ll do the same for the discrete system. We’d like to know whether the system defined by
equation (5.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 5.6.1 — Closed-loop state-space controller.

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

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

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

yk = (C − DK)xk + DKrk (5.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
34 Chapter 5. 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 5.2: Controller matrix dimensions

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
converge to the reference. For equation (5.9) to reach steady-state, the eigenvalues of A − BK must
be in the left-half plane. For equation (5.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.

5.7 Pole placement

This is the practice of placing the poles of a closed-loop system directly to produce a desired response.
Python Control offers several pole placement algorithms for generating controller or observer gains
from a set of poles. 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, which we’ll discuss next.

5.8 Linear-quadratic regulator

Instead of placing the poles of a closed-loop system manually, the linear-quadratic regulator (LQR)
places the poles for us based on acceptable relative error and control effort costs. 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.
Z∞


J= xT Qx + uT Ru dt
0

where J represents a trade-off 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 5.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).
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.
5.8 Linear-quadratic regulator 35

Figure 5.1: Pareto boundary for LQR

The feedback control law that minimizes J is shown in theorem 5.8.1.

Theorem 5.8.1 — Linear-quadratic regulator.

Z∞


min xT Qx + uT Ru dt
u
0
subject to ẋ = Ax + Bu (5.13)

If the system is controllable, the optimal control policy u∗ that drives all the states to zero is −Kx.
To converge to nonzero states, a reference vector r can be added to the state x.

u = K(r − x) (5.14)

This means that optimal control can be achieved with simply a set of proportional gains on all the
states. 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 E 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 [25].

R LQR design’s Q and R matrices don’t need discretization, but the K calculated for contin-
uous time and discrete time systems will be different. The discrete time gains approach the
continuous time gains as the sample period tends to zero.

5.8.1 Bryson’s rule

The next obvious question is what values to choose for Q and R. While this can be more of an art
than a science, Bryson’s rule provides a good starting point. With Bryson’s rule, the diagonals of the
36 Chapter 5. State-space controllers

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 ρ.
Z∞ " 2  2 # " 2  2 #!
x1 xm u1 un
J= ρ + ... + + + ... + dt
x1,max xm,max u1,max un,max
0
 ρ   1 
x21,max
0 ... 0 u21,max
0 ... 0
 ..   .. 
 0 ρ
.   0 1
. 
 x22,max   u22,max 
Q=  R =  
 .. ..   .. .. 
 . . 0   . . 0 
ρ 1
0 ... 0 x2m,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.

5.8.2 Pole placement vs LQR

This example uses the following second-order model for a CIM motor (a DC brushed motor).
 b Kt
  
−J 0    
A= J B = C= 1 0 D= 0
Ke R 1
− L −L L

Figure 5.2 shows the response using discrete poles3 placed at (0.1, 0) and (0.9, 0) and LQR with the
following cost matrices.
 1 
0  
Q = 20 2
1 R = 1212
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.

3
See section 6.2 for pole mappings of discrete systems (inside unit circle is stable). The pole mappings mentioned so
far (LHP is stable) only apply to continuous systems.
5.9 Model augmentation 37

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

5.9 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.

5.9.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.

5.9.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.

5.9.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
38 Chapter 5. 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.

5.9.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.

5.9.5 Examples
Snippet 5.1 shows how one packs together the following augmented matrix in Python using concate-
nation.  
A B
C D
#!/ usr/bin/env python3

import numpy as np

def main ():

A = np.array ([[1 , 2], [3, 4]])
B = np.array ([[5] , [6]])
C = np.array ([[7 , 8]])
D = np.array ([[9]])

tmp = np. concatenate (

(np. concatenate ((A, B), axis =1) , np. concatenate ((C, D), axis =1)),
axis =0
)

print ("A =")

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

if __name__ == " __main__ ":

main ()
5.10 Feedforward 39

Snippet 5.1. Matrix augmentation example: concatenation

Snippet 5.2 shows how one packs together the same augmented matrix in Python using array slices.
#!/ usr/bin/env python3

import numpy as np

def main ():

A = np.array ([[1 , 2], [3, 4]])
B = np.array ([[5] , [6]])
C = np.array ([[7 , 8]])
D = np.array ([[9]])

tmp = np.zeros ((3, 3))

tmp [:2, :2] = A # tmp [0:2 , 0:2] = A
tmp [:2, 2:] = B # tmp [0:2 , 2:3] = B
tmp [2:, :2] = C # tmp [2:3 , 0:2] = C
tmp [2:, 2:] = D # tmp [2:3 , 2:3] = D

print ("A =")

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

if __name__ == " __main__ ":

main ()

Snippet 5.2. Matrix augmentation example: array slices

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

5.10 Feedforward
So far, we’ve used feedback control for reference tracking (making a system’s output follow a desired
reference signal). While this is effective, it’s a reactionary measure; the system won’t start applying
control effort until the system is already behind. If we could tell the controller about the desired
movement and required input beforehand, the system could react quicker and the feedback controller
could do less work. A controller that feeds information forward into the plant like this is called a
feedforward controller.
A feedforward controller injects information about the system’s dynamics (like a model does) or the
desired movement. The feedforward handles parts of the control actions we already know must be
applied to make a system track a reference, then feedback compensates for what we do not or cannot
know about the system’s behavior at runtime.
There are two types of feedforwards: model-based feedforward and feedforward for unmodeled
40 Chapter 5. State-space controllers

dynamics. The first solves a mathematical model of the system for the inputs required to meet desired
velocities and accelerations. The second compensates for unmodeled forces or behaviors directly so
the feedback controller doesn’t have to. Both types can facilitate simpler feedback controllers; we’ll
cover examples of each.

5.10.1 Plant inversion

Plant inversion is a method of model-based feedforward for state feedback. It solves the plant for
the input that will make the plant track a desired output. This is called inversion because in a block
diagram, the inverted plant feedforward and plant cancel out to produce a unity system from input to
output.
While it can be an effective tool, the following should be kept in mind.
1. Don’t invert an unstable plant. If the expected plant doesn’t match the real plant exactly, the
plant inversion will still result in an unstable system. Stabilize the plant first with feedback,
then inject an inversion.
2. Don’t invert a nonminimum phase system. The advice for pole-zero cancellation in subsection
3.2.3 applies here.
Necessary theorems
The following theorem will be needed to derive the linear plant inversion equation.

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

Setup
Let’s start with the equation for the reference dynamics

rk+1 = Ark + Buk

where uk 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 uk such that we minimize the tracking
error between rk+1 and rk .
rk+1 − Ark = Buk

To solve for uk , 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 = Buk − (rk+1 − Ark )

This expression will be the state tracking cost we use in the following cost function as an H2 norm.

J = (Buk − (rk+1 − Ark ))T (Buk − (rk+1 − Ark ))

Minimization
Given theorem 5.10.1, find the minimum of J by taking the partial derivative with respect to uk and
setting the result to 0.
∂J
= 2BT (Buk − (rk+1 − Ark ))
∂uk
5.10 Feedforward 41

0 = 2BT (Buk − (rk+1 − Ark ))

0 = 2BT Buk − 2BT (rk+1 − Ark )
2BT Buk = 2BT (rk+1 − Ark )
BT Buk = BT (rk+1 − Ark )
uk = (BT B)−1 BT (rk+1 − Ark )

(BT B)−1 BT is the Moore-Penrose pseudoinverse of B denoted by B† .

Theorem 5.10.2 — Linear plant inversion. Given the discrete model xk+1 = Axk + Buk ,
the plant inversion feedforward is

uk = B† (rk+1 − Ark ) (5.15)

where B† is the Moore-Penrose pseudoinverse of B, rk+1 is the reference at the next timestep,
and rk is the reference at the current timestep.

Discussion
Linear plant inversion in theorem 5.10.2 compensates for reference dynamics that don’t follow how
the model inherently behaves. If they do follow the model, the feedforward has nothing to do as
the model already behaves in the desired manner. When this occurs, rk+1 − Ark will return a zero
vector.
For example, a constant reference requires a feedforward that opposes system dynamics that would
change the state over time. If the system has no dynamics, then A = I and thus

uk = B† (rk+1 − Irk )
uk = B† (rk+1 − rk )

For a constant reference, rk+1 = rk .

uk = B† (rk − rk )
uk = B† (0)
uk = 0

so no feedforward is required to hold a system with no dynamics at a constant reference, as expected.

Figure 5.3 shows plant inversion applied to a second-order CIM motor model. Plant inversion ac-
counts for the motor back-EMF and eliminates steady-state error.

5.10.2 Unmodeled dynamics

In addition to plant inversion, one can include feedforwards for unmodeled dynamics. 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.

uk = Vapp (5.16)
42 Chapter 5. State-space controllers

Figure 5.3: Second-order CIM motor response with plant inversion

where Vapp is a constant. Another feedforward holds a single-jointed arm steady in the presence of
gravity. It has the following form.
uk = Vapp cos θ (5.17)

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 frame4 .

5.11 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.
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
4
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.
5.11 Integral control 43

has determined that integral control will be worth the inconvenience.

We’ll present two methods:
1. Augment the plant as described earlier. For an arm, one would add an “integral of position”
state.
2. Estimate the “error” in the control input (the difference between what was applied versus what
was observed to happen) via the observer and compensate for it. We’ll call this “u error esti-
mation”.

5.11.1 Plant augmentation

We want to augment the system with an integral term that integrates the error e = r − y = r − Cx.
Z
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

5.11.2 Input error estimation

Given the desired input produced by a controller, unmodeled disturbances may cause the observed
behavior of a system to deviate from its model. Input error estimation estimates the difference be-
tween the desired input and a hypothetical input that makes the model match the observed behavior.
This value can be added to the control input to make the controller compensate for unmodeled dis-
turbances and make the model better predict the system’s future behavior.
First, we’ll consider the one-dimensional case. 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 )

u + uerror is the hypothetical input actually applied to the system.

ẋ = Ax + Bu + Buerror

The following equation generalizes this to a multiple-input system.

ẋ = Ax + Bu + Berror uerror
44 Chapter 5. State-space controllers

where Berror is a 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.
Given the above equation, we’ll augment the plant as
      
x˙ A Berror x B
= + u
uerror 0 0 uerror 0
 
  x
y= C 0 + Du
uerror

Notice how the state is augmented with 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.
Chaparral by Merril Apartments at UCSC

6. 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 differential equation like

ẋ = x − 3 (6.1)

to a difference equation like

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

where xk refers to the value of x at the k th timestep. The difference equation is run with some update
period denoted by T , by ∆T , or sometimes sloppily by dt1 .
While higher order terms of a differential equation are derivatives of the state variable (e.g., ẍ in
relation to equation (6.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 (6.2)).

6.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 margin2
that occurs in digital implementations of feedback controllers from sampling the continuous system

1
The discretization of equation (6.1) to equation (6.2) uses the forward Euler discretization method.
2
See section C.1 for an explanation of phase margin.
46 Chapter 6. Digital control

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.
• 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.

6.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 6.1 contains a few common points and figure
6.1 shows the mapping visually.

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

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

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

6.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.
6.2 s-plane to z-plane 47

6.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,
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 6.2 and 6.3. The jaggedness
of these signals is due to the frequency of the system dynamics being above the Nyquist frequency
(twice the sample frequency). The discretized signal doesn’t have enough samples to reconstruct the
continuous system’s dynamics.

Figure 6.2: Single poles in various locations in Figure 6.3: Complex conjugate poles in various
z-plane locations in z-plane

6.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 other3 . Figure 6.4 demonstrates aliasing.
The effect of these high-frequency aliases can be reduced with a low-pass filter (called an anti-aliasing
filter in this application).

3 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.
48 Chapter 6. Digital control

Figure 6.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.

6.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 6.5). The exact method of applying this
uses the matrix exponential, but this can be computationally 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.
3. Bilinear transform. The first-order bilinear approximation is s = T2 1−z .
−1
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 6.6 and 6.7). Since these are approximations, there is distortion between the real dis-
crete 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.
6.3 Discretization methods 49

Figure 6.5: Zero-order hold of a system response

Figure 6.6: Discretization methods applied to velocity data

50 Chapter 6. Digital control

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

6.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 6.8, 6.9, and 6.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 6.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
6.4 Effects of discretization on controller performance 51

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

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

52 Chapter 6. Digital control

Figure 6.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+ 21 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 6.2: Taylor series expansions of discretization methods (scalar case). The zero-order hold
discretization method is exact.

6.5 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 6.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.

∞
X 1 k
eX = X (6.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
6.6 Taylor series 53

differential equations ẋ = Ax we use to describe systems (systems also have a Bu term, but we’ll
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 .

6.6 Taylor series

R Watch the “Taylor series” video from 3Blue1Brown’s Essence of calculus series (22 minutes)
[13] for an explanation of how the Taylor series expansion works.

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
X t
et =
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 infinity4 . Figure 6.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 (6.3) for X = AT so we
can compare it with other methods.
3
X 1 1 1
(AT )k = I + AT + A2 T 2 + A3 T 3
k! 2 6
k=0

Table 6.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.
Each of them has different stability properties. The bilinear transform preserves the (in)stability of
the continuous time system.

4
Functions for which their Taylor series expansion converges to and also equals it are called analytic functions.
54 Chapter 6. Digital control

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

Method Conversion Taylor series expansion

Zero-order hold A d = e Ac T Ad = I + Ac T + 21 A2c T 2 + 61 A3c T 3 + . . .


−1
Bilinear Ad = I + 12 Ac T I − 12 Ac T Ad = I + Ac T + 21 A2c T 2 + 41 A3c T 3 + . . .
Forward Euler Ad = I + Ac T Ad = I + Ac T
Reverse Euler Ad = (I − Ac T )−1 Ad = I + Ac T + A2c T 2 + A3c T 3 + . . .

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

6.7 Zero-order hold for state-space

Given the following continuous time state space model

ẋ = Ac x + Bc u + w
y = Cc x + D c 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
y k = Cd x k + D d u k + v k
6.7 Zero-order hold for state-space 55

with covariances

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

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

A d = e Ac T (6.4)
Z T
Bd = eAc τ dτ Bc = A−1
c (Ad − I)Bc (6.5)
0
Cd = Cc (6.6)
Dd = Dc (6.7)
Z T
T
Qd = eAc τ Qc eAc τ dτ (6.8)
τ =0
1
Rd = Rc (6.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 H.2 for derivations.

To compute Ad and Bd in one step, one can utilize the following property.
 
A c Bc   
 T
0 0 A d Bd
e =
0 I

Qd can be computed as
 
−Ac Qc   
T
ATc −Ad A−1

Φ=e
0
= d Qd
0 ATd

where Qd = ΦT2,2 Φ1,2 [22].

To see why Rc is being divided by T , consider the discrete white noise sequence vk and the (non-
physically realizable) continuous white noise process v. Whereas Rd,k = E[vk vkT ] is a covariance
matrix, Rc (t) defined by E[v(t)vT (τ )] = Rc (t)δ(t − τ ) is a spectral density matrix (the Dirac
function δ(t − τ ) has units of 1/sec). The covariance matrix Rc (t)δ(t − τ ) has infinite-valued
elements. The discrete white noise sequence can be made to approximate the continuous white noise
process by shrinking the pulse lengths (T ) and increasing their amplitude, such that Rd → T1 Rc .
That is, in the limit as T → 0, the discrete noise sequence tends to one of infinite-valued pulses of
zero duration such that the area under the ”impulse” autocorrelation function is Rd T . This is equal
to the area Rc under the continuous white noise impulse autocorrelation function.
This page intentionally left blank
Trees by Interdisciplinary Sciences building at UCSC

7. 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.

7.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.

7.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 Jacobians
of f and h.
∂f (x,u,w) ∂f (x,u,w) ∂h(x,u,v) ∂h(x,u,v)
A= ∂x B= ∂u C= ∂x D= ∂u
58 Chapter 7. Nonlinear control

Let f (x, u) be defined as  

f1 (x, u)
 f2 (x, u) 
 
ẋ = f (x, u) =  .. 
 . 
fm (x, u)

The subscript denotes a row of f , where each row represents the dynamics of a state.
The Jacobian is the partial derivative of a vector-valued function with respect to one of the vector
arguments. The Jacobian of f has as many rows as f , and the columns are filled with partial deriv-
iatives of f ’s rows with respect to each of the argument’s elements. For example, the Jacobian of f
with respect to x is  ∂f 
1 ∂f1 ∂f1
∂x1 ∂x2 . . . ∂x m
 ∂f2 ∂f2 ∂f2 
∂f (x, u)  ∂x ∂x . . . ∂x 
A= =  .
1
.
2
. .
m

∂x  .. .. .. .. 
∂fm ∂fm ∂fm
∂x1 ∂x2 ... ∂xm

∂f1
∂x1 is the partial derivative of the first row of f with respect to the first state, and so on for all rows
of f and states. This has n2 permutations and thus produces a square matrix.
The Jacobian of f with respect to u is
 ∂f ∂f1 ∂f1

1
...
 ∂u
∂f2
1 ∂u2
∂f2
∂un
∂f2 
∂f (x, u)  ∂u ∂u2 ... ∂un 
B= = .  1 .. 
.. 
..
∂u  .. . . . 
∂fm ∂fm ∂fm
∂u1 ∂u2 ... ∂un

∂f1
∂u1 is the partial derivative of the first row of f with respect to the first input, and so on for all rows
of f and inputs. This has m × n permutations and can produce a nonsquare matrix if m ̸= n.
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 [17]. 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.

7.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.

1
Equilibrium points are points where ẋ = 0. At these points, the system is in steady-state.
7.4 Affine systems 59

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.
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 uses a function consisting of the energy in
a system or derivatives of the system’s state to prove stability around an equilibrium point. This is
done by showing that the function, and thus its inputs, decay to some ground state.
More than one Lyapunov function can prove stability, and if one function doesn’t prove it, another
candidate should be tried. For this reason, we refer to these functions as Lyapunov candidate func-
tions.

7.4 Affine systems

Let x = x0 + δx and u = u0 + δu where δx and δu are perturbations from (x0 , u0 ). A first-order
linearization of ẋ = f (x, u) around (x0 , u0 ) gives

∂f (x, u) ∂f (x, u)
ẋ ≈ f (x0 , u0 ) + δx + δu
∂x x0 ,u0 ∂u x0 ,u0
∂f (x, u) ∂f (x, u)
ẋ = f (x0 , u0 ) + δx + δu
∂x x0 ,u0 ∂u x0 ,u0

An affine system is a linear system with a constant offset in the dynamics. If (x0 , u0 ) is an equilibrium
point, f (x0 , u0 ) = 0, the resulting model is linear, and LQR works as usual. If (x0 , u0 ) is, say, the
current operating point rather than an equilibrium point, the easiest way to correctly apply LQR is
1. Find a control input u0 that makes (x0 , u0 ) an equilibrium point.
2. Obtain an LQR for the linearized system.
3. Add u0 to the LQR’s control input.
For a control-affine system (a nonlinear system with linear control inputs) ẋ = f (x) + Bu, u0 can
be derived via plant inversion as follows.

ẋ = f (x0 ) + Bu0
0 = f (x0 ) + Bu0
Bu0 = −f (x0 )
u0 = −B† f (x0 ) (7.1)

7.4.1 Feedback linearization for reference tracking

Feedback linearization lets us erase the nonlinear dynamics of a system so we can apply our own
(usually linear) dynamics for reference tracking. To do this, we will perform a similar procedure as
in subsection 5.10.1 and solve for u given the reference dynamics in ṙ.

ṙ = f (x) + Bu
Bu = ṙ − f (x)
60 Chapter 7. Nonlinear control

u = B† (ṙ − f (x)) (7.2)

R To use equation (7.2) in a discrete controller, one can approximate ṙ with rk+1 −rk
T where T is
the time period between the two references.

7.5 Further reading

For a more complex type of trajectory tracking, read A guiding vector field algorithm for path fol-
lowing control of nonholonomic mobile robots [19].
To learn more about nonlinear control, we recommend watching the underactuated robotics lectures
by MIT [28]. Links to it and the associated lecture note [29] are in the bibliography. The books
Nonlinear Dynamics and Chaos by Steven Strogatz and Applied Nonlinear Control by Jean-Jacques
Slotine are also good references.
Fake plant in hotel lobby in Ventura, CA

8. State-space applications

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 D for setup instructions.

8.1 Elevator
8.1.1 Continuous state-space model
The position and velocity derivatives of the elevator can be written as

ẋ = v (8.1)
v̇ = a (8.2)

where by equation (13.15),

GKt G2 K t
a= V − v
Rrm Rr2 mKv

Substitute this into equation (8.2).

GKt G2 K t
v̇ = V − v
Rrm Rr2 mKv
G2 K t GKt
v̇ = − 2 v+ V (8.3)
Rr mKv Rrm

Factor out v and V into column vectors.

 ˙  h G2 K i    GK   
v = − Rr2 mKt v v + Rrmt V
62 Chapter 8. State-space applications

Augment the matrix equation with the position state x, which has the model equation ẋ = v. The
matrix elements corresponding to v will be 1, and the others will be 0 since they don’t appear, so
ẋ = 0x + 1v + 0V . The existing rows will have zeroes inserted where x is multiplied in.
˙ " #   
x 0 1 x 0  
= 2K + GKt V
v 0 − RrG2 mK t
v
v Rrm

Theorem 8.1.1 — Elevator state-space model.

ẋ = Ax + Bu
y = Cx + Du
 
x
x= y=x u=V
v
" #  
0 1 0  
A= G2 Kt B = GKt C= 1 0 D=0 (8.4)
0 − Rr2 mKv Rrm

8.1.2 Model augmentation

As per subsection 5.11.2, we will now augment the model so a uerror state 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 =  v  y = x u = V
uerror
   
A B B  
Aaug = Baug = Caug = C 0 Daug = D (8.5)
01×2 0 0
 
  r
Kaug = K 1 raug = (8.6)
0

This will compensate for unmodeled dynamics such as gravity. However, using a constant voltage
feedforward to counteract gravity is preferred over uerror estimation in this case because it results in
a simpler controller with similar performance.

8.1.3 Gravity feedforward

Input voltage is proportional to force and gravity is a constant force, so a constant voltage feedforward
can compensate for gravity. We’ll model gravity as a disturbance described by −mg. To compensate
for it, we want to find a voltage that is equal and opposite to it. The bottom row of the continuous
elevator model contains the acceleration terms.
Buf f = −(unmodeled dynamics)

where B is the motor acceleration term from B and uf f is the voltage feedforward.
Buf f = −(−mg)
8.2 Flywheel 63

Figure 8.1: Elevator response

Buf f = mg
GKt
uf f = mg
Rrm
Rrm2 g
uf f =
GKt

8.1.4 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. Figure 8.1 shows the closed-loop system response.

8.1.5 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 .

8.2 Flywheel
8.2.1 Continuous state-space model
By equation (13.18)
G2 Kt GKt
ω̇ = − ω+ V
Kv RJ RJ
Factor out ω and V into column vectors.
 ˙  h G2 K i    GK   
ω = − Kv RJt ω + RJt V

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
64 Chapter 8. State-space applications

Theorem 8.2.1 — Flywheel state-space model.

ẋ = Ax + Bu
y = Cx + Du

x=ω y=ω u=V

2
G Kt
A = −K v RJ
B= GKt
RJ C=1 D=0 (8.7)

8.2.2 Model augmentation

As per subsection 5.11.2, we will now augment the model so a uerror state 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= y=ω u=V
uerror
   
A B B  
Aaug = Baug = Caug = C 0 Daug = D (8.8)
0 0 0
 
  r
Kaug = K 1 raug = (8.9)
0

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

8.2.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 8.2 shows the closed-loop system response.
Notice how the control effort when the reference is reached is nonzero. This is a plant inversion
feedforward compensating for the system dynamics attempting to slow the flywheel down when no
voltage is applied.

8.2.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 .

8.2.5 Flywheel model without encoder

In the FIRST Robotics Competition, we can get the current drawn for specific channels on the power
distribution panel. We can theoretically use this to estimate the angular velocity of a DC motor
without an encoder. We’ll start with the flywheel model derived earlier as equation (13.18).
GKt G2 K t
ω̇ = V − ω
RJ Kv RJ
3
https://github.com/calcmogul/frccontrol/blob/master/examples/flywheel.py
4
https://github.com/calcmogul/allwpilib/tree/state-space/wpilibcExamples/src/main/cpp/examples/
StateSpaceFlywheel
8.2 Flywheel 65

Figure 8.2: Flywheel response

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

Next, we’ll derive the current I as an output.

ω
V = IR +
Kv
ω
IR = V −
Kv
1 1
I=− ω+ V
Kv R R

Therefore,

Theorem 8.2.2 — Flywheel state-space model without encoder.

ẋ = Ax + Bu
y = Cx + Du

x=ω y=I u=V

2
G Kt
A = −Kv RJ
B= GKt
RJ C = − K1v R D= 1
R
(8.10)

Notice that in this model, the output doesn’t provide any direct measurements of the state. To esti-
mate the full state (alsa known as full observability), we only need the outputs to collectively include
linear combinations of every state5 . We’ll revisit this in chapter 9 with an example that uses range
measurements to estimate an object’s orientation.
5
While the flywheel model’s outputs are a linear combination of both the states and inputs, inputs don’t provide new
information about the states. Therefore, they don’t affect whether the system is observable.
66 Chapter 8. State-space applications

The effectiveness of this model’s observer is heavily dependent on the quality of the current sensor
used. If the sensor’s noise isn’t zero-mean, the observer won’t converge to the true state.

8.2.6 Voltage compensation

To improve controller tracking, one may want to use the voltage renormalized to the power rail voltage
to compensate for voltage drop when current spikes occur. This can be done as follows.
Vnominal
V = Vcmd (8.11)
Vrail

where V is the controller’s new input voltage, Vcmd is the old input voltage, Vnominal is the rail voltage
when effects like voltage drop due to current draw are ignored, and Vrail is the real rail voltage.
To drive the model with a more accurate voltage that includes voltage drop, the reciprocal can be
used.
Vrail
V = Vcmd (8.12)
Vnominal

where V is the model’s new input voltage. Note that if both the controller compensation and model
compensation equations are applied, the original voltage is obtained. The model input only drops
from ideal if the compensated controller voltage saturates.

8.3 Single-jointed arm

8.3.1 Continuous state-space model
The angle and angular rate derivatives of the arm can be written as

θ̇arm = ωarm (8.13)

ω̇arm = ω̇arm (8.14)

By equation (13.22)
G2 Kt GKt
ω̇arm = − ωarm + V
Kv RJ RJ

Factor out ωarm and V into column vectors.

 ˙  h G2 K i     
ωarm = − Kv RJt ωarm + GK
RJ
t
V

Augment the matrix equation with the angle state θarm , which has the model equation θ̇arm = ωarm .
The matrix elements corresponding to ωarm will be 1, and the others will be 0 since they don’t appear,
so θ̇arm = 0θarm +1ωarm +0V . The existing rows will have zeroes inserted where θarm is multiplied
in.
 ˙  " #   
θarm 0 1 θarm 0  
= G2 Kt + GKt V
ωarm 0 −K v RJ
ωarm RJ
8.3 Single-jointed arm 67

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

ẋ = Ax + Bu
y = Cx + Du

θarm
x= y = θarm u = V
ωarm
" #  
0 1 0  
A= 2
G Kt B = GKt C= 1 0 D=0 (8.15)
0 − Kv RJ RJ

8.3.2 Model augmentation

As per subsection 5.11.2, we will now augment the model so a uerror state 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 = y = θarm u = V
uerror
   
A B B  
Aaug = Baug = Caug = C 0 Daug = D (8.16)
01×2 0 0
 
  r
Kaug = K 1 raug = (8.17)
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.

8.3.3 Gravity feedforward

Input voltage is proportional to torque and gravity is a constant force, but the torque applied against
the motor varies according to the arm’s angle. We’ll use sum of torques to find a compensating torque.
We’ll model gravity as a disturbance described by −mg where m is the arm’s mass. To compensate
for it, we want to find a torque that is equal and opposite to the torque applied to the arm by gravity.
The bottom row of the continuous elevator model contains the angular acceleration terms, so Buf f
is angular acceleration caused by the motor; JBuf f is the torque.

JBuf f = −(r × F)
JBuf f = −(rF cos θ)

Torque is usually written as rF sin θ where θ is the angle between the r and F vectors, but θ in this
case is being measured from the horizontal axis rather than the vertical one, so the force vector is π4
radians out of phase. Thus, an angle of 0 results in the maximum torque from gravity being applied
rather than the minimum.
68 Chapter 8. State-space applications

Figure 8.3: Single-jointed arm response

The force of gravity mg is applied at the center of the arm’s mass. For a uniform beam, this is
halfway down its length, or L2 where L is the length of the arm.
  
L
JBuf f = − (−mg) cos θ
2
L
JBuf f = mg cos θ
2

RJ , so
GKt
B=

GKt L
J uf f = mg cos θ
RJ 2
RJ L
uf f = mg cos θ
JGKt 2
L Rmg
uf f = cos θ
2 GKt

L
2 can be adjusted according to the location of the arm’s center of mass.

8.3.4 Simulation
Python Control will be used to discretize the model and simulate it. One of the frccontrol examples6
creates and tests a controller for it. Figure 8.3 shows the closed-loop system response.

8.3.5 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-

6
https://github.com/calcmogul/frccontrol/blob/master/examples/single_jointed_arm.py
8.4 Pendulum 69

paceObserver classes in C++ and Java. A C++ implementation of this single-jointed arm controller
is available online7 .

8.4 Pendulum
8.4.1 State-space model
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
 T
This makes our state vector θ ω and our nonlinear model
 
ω
f (x, u) =
− gl sin θ

Linearization around θ = 0
To apply our tools for linear control theory, the model must be a linear combination of the states and
inputs (addition and multiplication by constants). Since this model is nonlinear on account of the sine
function, we should linearize it.
Linearization finds a tangent line to the nonlinear dynamics at a desired point in the state-space.
The Taylor series is a way to approximate arbitrary functions with polynomials, so we can use it for
linearization.
The taylor series expansion for sin θ around θ = 0 is θ − 61 θ3 + 1 5
120 θ − . . .. We’ll take just the
first-order term θ to obtain a linear function.

θ̇ = ω
g
ω̇ = − θ
l

Now write the model in state-space representation. We’ll write out the system of equations with the
zeroed variables included to assist with this.

θ̇ = 0θ + 1ω
g
ω̇ = − θ + 0ω
l
7
https://github.com/calcmogul/allwpilib/tree/state-space/wpilibcExamples/src/main/cpp/examples/
StateSpaceSingleJointedArm
70 Chapter 8. State-space applications

Factor out θ and ω into a column vector.

˙   
θ 0 1 θ
= (8.18)
ω − gl 0 ω

Linearization with the Jacobian

Here’s the original nonlinear model in state-space representation.
 
ω
f (x, u) =
− gl sin θ

If we want to linearize around an arbitrary point, we can take the Jacobian with respect to x.
 
∂f (x, u) 0 1
=
∂x − gl cos θ 0

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.
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

Therefore, the measurement vector is

y = Cx
 
  θ
y= 1 0
ω
y = 1θ + 0ω
y=θ

Theorem 8.4.1 — Linear time-varying pendulum state-space model with no input.

ẋ = Ax
y = Cx
 
θ
x= y=θ
ω
 
0 1  
A= g C= 1 0 (8.19)
− l cos θ 0

8.5 Differential drive

8.5.1 Velocity subspace state-space model
By equations (13.34) and (13.35)
   
1 rb2 1 rb2
v̇l = + (C1 vl + C2 Vl ) + − (C3 vr + C4 Vr )
m J m J
8.5 Differential drive 71
   
1 r2 1 r2
v̇r = − b (C1 vl + C2 Vl ) + + b (C3 vr + C4 Vr )
m J m J

Regroup the terms into states vl and vr and inputs Vl and Vr .

       
1 r2 1 r2 1 rb2 1 r2
v̇l = + b C1 vl + + b C 2 Vl + − C3 vr + − b C 4 Vr
m J m J m J m J
       
1 rb2 1 rb2 1 rb2
1 rb2
v̇r = − C1 vl + − C 2 Vl + + C3 vr + + C 4 Vr
m J m J m J m J

       
1 rb2 1 rb2 1 rb2 1 rb2
v̇l = + C1 vl + − C3 vr + + C 2 Vl + − C 4 Vr
m J m J m J m J
       
1 r2 1 r2 1 r2 1 r2
v̇r = − b C1 vl + + b C3 vr + − b C 2 Vl + + b C 4 Vr
m J m J m J m J

Factor out vl and vr into a column vector and Vl and Vr into a column vector.
         
˙ 1 rb2 1 rb2   1 rb2 1 rb2  
vl m + J C 1 m − J C 3 vl m + J C 2 m − J C 4  Vl
=       +     
vr r2 r2 r2 rb2
1
− b C1
m J
1
+ b C3 vr
m J
1
− b C2 m
1
+J m J C4
Vr

Theorem 8.5.1 — Differential drive velocity state-space model.

ẋ = Ax + Bu
y = Cx + Du
     
vl v V
x= y= l u= l
vr vr Vr
         
1 rb2 1 r2 1 rb2 1 rb2
+ J C1 − Jb C3 + C 2 m − J C4 
A=  m  m   B=   m J
  
1 rb2 1 r2 1 rb2 1 rb2
− m J C1 m + Jb C3 m − J C2 m + J C4 (8.20)

1 0
C= D = 02×2
0 1

G2 K 2
where C1 = − KvlRrt2 , C2 = G l Kt
Rr , C3 = − KGvrRr
Kt
2 , and C4 = Rr .
Gr Kt

Simulation
Python Control will be used to discretize the model and simulate it. One of the frccontrol examples8
creates and tests a controller for it. Figure 8.4 shows the closed-loop 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.

8
https://github.com/calcmogul/frccontrol/blob/master/examples/differential_drive.py
72 Chapter 8. State-space applications

Figure 8.4: Drivetrain response

8.5.2 Linear time-varying model

The model in theorem 8.5.1 is linear, but only includes the velocity dynamics, not the dynamics of
the drivetrain’s global pose. The change in global pose is defined by these three equations.
vl + vr vr vl
ẋ = cos θ = cos θ + cos θ
2 2 2
vl + vr vr vl
ẏ = sin θ = sin θ + sin θ
2 2 2
vr − vl vr vl
θ̇ = = −
2rb 2rb 2rb

Next, we’ll augment the linear subspace’s state with the global pose x, y, and θ. Here’s the model as
 T  T
a vector function where x = x y θ vl vr and u = Vl Vr .
 
vr
2 cos θ + v2l cos θ


vr
2 sin θ + v2l sin θ 

 vr vl 
f (x, u) =  2rb − 
2rb  (8.21)
 1 
rb2

1 r2

1 r2
 
1 rb2
 
 m+ J C1 vl + − Jb C3 vr + m + Jb C2 Vl + m − 
J C 4 Vr 
  m     
1 rb2 1 rb2 1 r 2
1 rb2
m − J C1 vl + m + J C3 vr + m − Jb C2 Vl + m + J C 4 Vr

As mentioned in chapter 7, one can approximate a nonlinear system via linearizations around points
of interest in the state-space and design controllers for those linearized subspaces. If we sample
linearization points progressively closer together, we converge on a control policy for the original
nonlinear system. Since the linear plant being controlled varies with time, its controller is called a
linear time-varying (LTV) controller.
If we use LQRs for the linearized subspaces, the nonlinear control policy will also be locally optimal.
We’ll be taking this approach with a differential drive. To create an LQR, we need to linearize
8.5 Differential drive 73

equation (8.21).
 
0 0 − vl +v
2 sin θ
r 1
2cos θ 1
2 cos θ
0 vl +vr 1 1 
 0 2 cos θ 2sin θ 2 sin θ 
∂f (x, u)  0 0 0 1
− 2rb 1 
=
  2
  2rb 
2


∂x 0 0 0 1 r
+ Jb C1 1
−
rb
C 3
 m   m J
 
1 r2 1 rb2
0 0 0 m − Jb C1 m + J C3
 
0 0
 0 0 
 

∂f (x, u)  0  0  
=   
∂u 1 r2 1 rb2 
 m + Jb C2 m − J C 4
    
1 rb2 1 rb2
m − J C 2 m + J C 4

Therefore,

Theorem 8.5.2 — Linear time-varying differential drive state-space model.

ẋ = Ax + Bu
y = Cx + Du
 T  T  T
x = x y θ v l vr y = θ vl vr u = Vl V r

 1 1   
0 0 −vs 2c 2c 0 0
0 1 1   
 0 vc 2s 2s   0 0 
0 0 0 − 2r1b 1   0  0  
A=
 
r2
  2rb 
rb2

 B=
 1 2
rb
 2
rb


1 1 1
0 0 0 + Jb C1 − J C3   m+ J C2 − J C4 
 m  m     m  
1 r2 1 r2 1 rb2 1 r2
0 0 0 m − Jb C1 m + Jb C3 m − J C2 m + Jb C4
 
0 0 1 0 0

C= 0 0 0 1 0 D = 03×2
0 0 0 0 1
(8.22)
G2l Kt 2
where v = vl +v
2 , c = cos θ, s = sin θ, C1 = −
r
, C2 = GRr l Kt
Kv Rr 2
, C3 = − KGvrRr
Kt
2 , and

C4 = Rr . The constants C1 through C4 are from the derivation in section 13.6.

Gr Kt

The LQR gain should be recomputed around the current operating point regularly due to the high
nonlinearity of this system. A less computationally expensive controller will be developed in later
sections.

We can also use this in an extended Kalman filter as is since the measurement model (y = Cx+Du)
is linear.
With this controller, θ becomes uncontrollable at v = 0 due to the x and y dynamics being equivalent
to a unicycle; it can’t change its heading unless it’s rolling (just like a bicycle). However, a differential
drive can rotate in place. To address this controller’s limitation at v = 0, one can temporarily switch
to an LQR of just θ, vl , and vr ; linearize the controller around a slightly nonzero state; or plan a new
trajectory after the previous one completes that provides nonzero wheel velocities to rotate the robot.
74 Chapter 8. State-space applications

8.5.3 Improving model accuracy

Figures 8.5 and 8.6 demonstrate the tracking behavior of the linearized differential drive controller.

Figure 8.6: Linear time-varying differential

Figure 8.5: Linear time-varying differential
drive controller response (first order)
drive controller x-y plot (first order)

The linearized differential drive model doesn’t track well because the first-order linearization of A
doesn’t capture the full heading dynamics, making the model update inaccurate. This linearization
inaccuracy is evident in the Hessian matrix (second partial derivative with respect to the state vector)
being nonzero.  
0 0 − vl +v
2 cos θ 0 0
r

0 0 − vl +vr sin θ 0 0
∂ 2 f (x, u) 0 0
2 
2
=  0 0 0 
∂x 0 0 0 0 0
0 0 0 0 0

The second-order Taylor series expansion of the model around x0 would be

∂f (x, u) 1 ∂ 2 f (x, u)
f (x, u0 ) ≈ f (x0 , u0 ) + (x − x0 ) + (x − x0 )2
∂x 2 ∂x2

To include higher-order dynamics in the linearized differential drive model integration, we recom-
mend using the fourth-order Runge-Kutta (RK4) integration method on equation (8.21). See snippet
8.1 for an implementation of RK4.
def runge_kutta (f, x, u, dt):
""" Fourth order Runge -Kutta integration .

Keyword arguments :
f -- vector function to integrate
x -- vector of states
u -- vector of inputs ( constant for dt)
dt -- time for which to integrate
"""
half_dt = dt * 0.5
k1 = f(x, u)
k2 = f(x + half_dt * k1 , u)
k3 = f(x + half_dt * k2 , u)
k4 = f(x + dt * k3 , u)
return x + dt / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4)
8.5 Differential drive 75

Snippet 8.1. Fourth-order Runge-Kutta integration in Python

Figures 8.7 and 8.8 show a simulation using RK4 instead of the first-order model.

Figure 8.8: Linear time-varying differential

Figure 8.7: Linear time-varying differential
drive controller (global reference frame formu-
drive controller (global reference frame formu-
lation) response
lation) x-y plot

8.5.4 Cross track error controller

Figure 8.10: Ramsete nonlinear controller re-

Figure 8.9: Ramsete nonlinear controller x-y
sponse
plot

The tracking performance of the linearized differential drive controller (figures 8.7 and 8.8) and
Ramsete (figures 8.9 and 8.10) for a given trajectory are similar, but the former’s performance-effort
trade-off can be tuned more intuitively via the Q and R gains. However, if the x and y error cost are
too high, the x and y components of the controller will fight each other, and it will take longer to
converge to the path. This can be fixed by applying a counterclockwise rotation matrix to the global
tracking error to transform it into the robot’s coordinate frame.
R    G 
ex cos θ sin θ 0 ex
ey  = − sin θ cos θ 0 ey 
eθ 0 0 1 eθ
76 Chapter 8. State-space applications

where the the superscript R represents the robot’s coordinate frame and the superscript G represents
the global coordinate frame.
With this transformation, the x and y error cost in LQR penalize the error ahead of the robot and cross-
track error respectively instead of global pose error. Since the cross-track error is always measured
from the robot’s coordinate frame, the model used to compute the LQR should be linearized around
θ = 0 at all times.
 
0 0 − vl +v 2 sin 0
r 1
2 cos 0
1
2 cos 0
0 0 vl +vr cos 0 1 1 
 2 2 sin 0 2 sin 0 
0 0 0 1
− 2rb 1 
A=     2r b  
1 rb2
1 rb2 
0 0 0 + C 1 − C 3
 m J
  m J
 
1 rb2 1 rb2
0 0 0 m − J C 1 m + J C 3
 1 1 
0 0 0 2 2
0 0 v +v 

l
2
r
0 0 
0 0 0 − 1 1 
A=   2r b  2r b  
1 r 2
1 r 2 
0 0 0 m + J C1
b
m − J C3 
b 
  2
  2

1 r 1 r
0 0 0 m − J C1
b
m + J C3
b

This model results in figures 8.11 and 8.12, which show slightly better tracking performance than the
previous formulation.

Figure 8.12: Linear time-varying differential

Figure 8.11: Linear time-varying differential
drive controller response
drive controller x-y plot

8.5.5 Explicit time-varying control law

Another downside of this controller over Ramsete is that the user must generate controller gains for
every state they visit in the state-space (an implicit control law) whereas Ramsete has two closed-form
functions for its explicit control law.
A possible solution to this is fitting a vector function of the states to the linearized differential drive
controller gains. Fortunately, the formulation of the controller dealing with cross-track error only
requires linearization around different values of v rather than v and θ; only a two-dimensional func-
tion is needed rather than a three-dimensional one. See figures 8.13 through 8.17 for plots of each
controller gain for a range of velocities.
8.5 Differential drive 77

With the exception of the x gain plot, all functions are a variation of a square root. v = 0 and v = 1
were used to scale each function to produce a close approximation. The sign function9 is used for
symmetry around the origin in the regression for y.

Theorem 8.5.3 — Linear time-varying differential drive controller.

 T  T  T
x = x y θ v l vr y = θ vl vr u = Vl V r

The following A and B matrices of a continuous system are used to compute the LQR.

 1 1   
0 0 0 2 2 0 0
0 0 v 0 0   0 0 
   
0 0 0 − 2r1b 1   0  0  
A=
 
r2
  2rb 
r 2

 B=
 1 rb2

r 2


1 1 1
0 0 0 + Jb C1 − Jb C3   m+ J C2 − Jb C4 
 m  m     m  
1 r2 1 r2 1 rb2 1 r2
0 0 0 m − Jb C1 m + Jb C3 m − J C2 m + Jb C4
(8.23)
G2 K G r Kt 2
where v = vl +v r l t Gl Kt
2 , C1 = − Kv Rr 2 , C2 = Rr , C3 = − Kv Rr 2 , and C4 = Rr .
G r Kt
The constants
C1 through C4 are from the derivation in section 13.6.
The locally optimal controller for this model is u = K(v)(r − x) where

 p 
kx k1,2 (v) sgn(v) kθ,1 p|v| k1,4 (v) k2,4 (v)
K(v) = (8.24)
kx −k1,2 (v) sgn(v) −kθ,1 |v| k2,4 (v) k1,4 (v)
p
k1,2 (v) = ky,0 + (ky,1 − ky,0 ) |v| (8.25)
p
k1,4 (v) = kv+ ,0 + (kv+ ,1 − kv+ ,0 ) |v| (8.26)
p
k2,4 (v) = kv− ,0 − (kv+ ,1 − kv+ ,0 ) |v| (8.27)

Using K computed via LQR at v = 0

kx = K1,1 ky,0 = K1,2 kv+ ,0 = K1,4 kv− ,0 = K2,4

Using K computed via LQR at v = 1

ky,1 = K1,2 kθ,1 = K1,3 kv+ ,1 = K1,4

Figures 8.18 through 8.21 show the responses of the exact and approximate solutions are the same.

9
The sign function is defined as follows:

−1
 x<0
sgn(x) = 0 x=0

1 x>0

78 Chapter 8. State-space applications

8.5.6 Nonlinear observer design

Encoder position augmentation
Estimation of the global pose can be significantly improved if encoder position measurements are
used instead of velocity measurements. By augmenting the plant with the line integral of each wheel’s
velocity over time, we can provide a mapping from model states to position measurements. We can
augment the linear subspace of the model as follows.
Augment the matrix equation with position states xl and xr , which have the model equations ẋl = vl
and ẋr = vr . The matrix elements corresponding to vl in the first equation and vr in the second
equation will be 1, and the others will be 0 since they don’t appear, so ẋl = 1vl + 0vr + 0xl + 0xr +
0Vl + 0Vr and ẋr = 0vl + 1vr + 0xl + 0xr + 0Vl + 0Vr . The existing rows will have zeroes inserted
where xl and xr are multiplied in.
˙      
xl 1 0 vl 0 0 Vl
= +
xr 0 1 vr 0 0 Vr

 T
This produces the following linear subspace over x = vl vr xl xr .
         
1 rb2 1 rb2 1 rb2 1 rb2
+ J C1 − J C3 0 0 + J C2 − J C4 
 m  m    m  m 
 1 rb2 1 rb2   1 rb2 1 rb2 
A=  m − J C1 m + J C3 0 0 B =  m − J C2 m + J C4 
   
 1 0 0 0  0 0 
0 1 0 0 0 0
(8.28)
 T
The measurement model for the complete nonlinear model is now y = θ xl xr instead of
 T
y = θ vl vr .
U error estimation
As per subsection 5.11.2, we will now augment the model so uerror states 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.
The three uerror states we’ll be adding are uerror,l , uerror,r , and uerror,heading for left voltage error,
right voltage error, and heading error respectively. The left and right wheel positions are filtered
encoder positions and are not adjusted for heading error. The turning angle computed from the
left and right wheel positions is adjusted by the gyroscope heading. The heading uerror state is the
heading error between what the wheel positions imply and the gyroscope measurement.
 T
The full state is thus x = x y θ vl vr xl xr uerror,l uerror,r uerror,heading .
The complete nonlinear model is as follows. Let v = vl +v 2 . The three uerror states augment the
r

linear subspace, so the nonlinear pose dynamics are the same.

˙  
x v cos θ
y  =  v sin θ  (8.29)
vr vl
θ 2rb − 2rb
8.5 Differential drive 79

The left and right voltage error states should be mapped to the corresponding velocity states, so the
system matrix should be augmented with B.
The heading uerror is measuring counterclockwise encoder understeer relative to the gyroscope head-
ing, so it should add to the left position and subtract from the right position for clockwise correction
of encoder positions. That corresponds to the following input mapping vector.
 
0
0
Bθ =  1


−1

Now we’ll augment the linear system matrix horizontally to accomodate the uerror states.
 
vl
˙  vr 
vl  
 xl 
 vr    
  = A B Bθ  xr  + Bu (8.30)
 xl   
 uerror,l 
xr  
 uerror,r 
uerror,heading

A and B are the linear subspace from equation (8.28).

The uerror states have no dynamics. The observer selects them to minimize the difference between
the expected and actual measurements.
 ˙ 
uerror,l
 uerror,r  = 03×1 (8.31)
uerror,heading

The controller is augmented as follows.

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

This controller augmentation compensates for unmodeled dynamics like:

1. Understeer caused by wheel friction inherent in skid-steer robots
2. Battery voltage drop under load, which reduces the available control authority

R The process noise for the voltage error states should be how much the voltage can be expected
to drop. The heading error state should be the encoder model uncertainty.
80 Chapter 8. State-space applications

Figure 8.13: Linear time-varying differential Figure 8.14: Linear time-varying differential
drive controller LQR gain regression fit (x) drive controller LQR gain fit regression fit (y)

Figure 8.15: Linear time-varying differential Figure 8.16: Linear time-varying differential
drive controller LQR gain regression fit (θ) drive controller LQR gain regression fit (vl )

Figure 8.17: Linear time-varying differential

drive controller LQR gain regression fit (vr )
8.5 Differential drive 81

Figure 8.18: Linear time-varying differential Figure 8.19: Linear time-varying differential
drive controller x-y plot (approximate) drive controller x-y plot (exact)

Figure 8.20: Linear time-varying differential Figure 8.21: Linear time-varying differential
drive controller response (approximate) drive controller response (exact)
82 Chapter 8. State-space applications

8.6 Ramsete unicycle controller

Ramsete is a nonlinear time-varying feedback controller for unicycle models that drives the model
to a desired pose along a two-dimensional trajectory. 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 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.
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 [24]. We’ll call it Ramsete 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”).

8.6.1 Velocity and turning rate command derivation

The state tracking error e in the vehicle’s coordinate frame is defined as
    
ex cos θ sin θ 0 xd − x
ey  = − sin θ cos θ 0  yd − y 
eθ 0 0 1 θd − θ

where ex is the tracking error in x, ey is the tracking error in y, and eθ 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.
We will use the following control laws u1 and u2 for velocity and turning rate respectively.
u1 = −k1 ex
(8.33)
u2 = −k3 eθ − k2 vd sinc(eθ )ey

where k1 , k2 , and k3 are time-varying gains and sinc(eθ ) is defined as sineθeθ . This choice of control
law may seem arbitrary, and that’s because it is. The only requirement on our choice is that there
exist an associated Lyapunov candidate function that proves the control law is globally asymptotically
stable. We’ll provide a sketch of a proof in theorem 8.6.1.
Our velocity and turning rate commands for the vehicle will use the following nonlinear transforma-
tion of the inputs.
v = vd cos eθ − u1
ω = ωd − u2

Substituting the control laws u1 and u2 into these equations gives

v = vd cos eθ + k1 ex
ω = k3 eθ + ωd + k2 vd sinc(eθ )ey
8.6 Ramsete unicycle controller 83

Theorem 8.6.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 (8.33) globally asymptotically
stabilize the origin e = 0.
Proof:
To prove convergence, the paper previously mentioned uses the following Lyapunov function.

k2 2 e2
V = (ex + e2y ) + θ
2 2

where k2 is a tuning constant, ex is the tracking error in x, ey is the tracking error in y, and eθ is
the tracking error in θ.
The time derivative along the solutions of the closed-loop system is nonincreasing since

V̇ = −k1 k2 e2x − k3 e2θ ≤ 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 [24].

8.6.2 Nonlinear controller equations

q
Let k2 = b and k = k1 (vd , ωd ) = k3 (vd , ωd ) = 2ζ ωd2 + bvd2 .

Theorem 8.6.2 — Ramsete unicycle controller.

    
ex cos θ sin θ 0 xd − x
ey  = − sin θ cos θ 0  yd − y  (8.34)
eθ 0 0 1 θd − θ
v = vd cos eθ + kex (8.35)
ω = ωd + keθ + bvd sinc(eθ )ey (8.36)
q
k = 2ζ ωd2 + bvd2 (8.37)
sin eθ
sinc(eθ ) = (8.38)
eθ

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 (see subsection 8.6.3). x, y, and θ are obtained via a pose estimator (see chapter 10 for
84 Chapter 8. State-space applications

how to implement one). The desired velocity, turning rate, and pose can be varied over time according
to a desired trajectory.
With this controller, θ becomes uncontrollable at v = 0. This is fine for controlling unicycles because
that’s already an inherent structural limitation; unicycles can’t change their heading unless they are
rolling (just like bicycles). However, differential drives can rotate in place. To address this controller’s
limitation at v = 0, one can temporarily switch to an LQR of just θ, vl , and vr ; or plan a new trajectory
that provides nonzero desired turning rates to rotate the robot.

8.6.3 Linear reference tracker

We need a velocity reference tracker for the nonlinear controller’s commands. Starting from equation
(8.20), 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 
 1 rb2 1 rb2   1 r2 1 r2 
0 m + J C1 0 m − J C3   m + Jb C2 m − Jb C4 
A=0
 B=
 


  0  0  1    0   0  
r 2 r 2 r 2 r 2
1 1 1 1
0 m − J C1 0
b
m + J C3
b
m − J C2
b
m + J C4
b
 
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 = Rr .
G r Kt

To obtain a left/right velocity reference tracker, we can just remove the position states from the
model.

Theorem 8.6.3 — Left/right velocity reference tracker.

     
vl v V
x= y= l u= l
vr vr Vr

         
1 rb2 1 rb2 1 rb2 1 rb2
+ J C1 − J C3  + J C2 − J C4 
A=  m  m  B=  m  m 
1 rb2 1 rb2 1 rb2 1 rb2
−m J C1 m + J C3 m − J C2 m + J C4 (8.39)
 
1 0
C= D = 02×2
0 1

G2 K 2
where C1 = − KvlRrt2 , C2 = G l Kt
Rr , C3 = − KGvrRr
Kt
2 , and C4 = Rr .
Gr Kt

See https://github.com/calcmogul/controls-engineering-in-frc/blob/master/modern-control-theory/
ss-applications/ramsete_decoupled.py for an implementation.
8.6 Ramsete unicycle controller 85

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 (12.1) and (12.2).

   
1 rb2 1 rb2
v̇c + ω̇rb = + (C1 (vc − ωrb ) + C2 Vl ) + − (C3 (vc + ωrb ) + C4 Vr ) (8.40)
m J m J
   
1 rb2 1 rb2
v̇c − ω̇rb = − (C1 (vc − ωrb ) + C2 Vl ) + + (C3 (vc + ωrb ) + C4 Vr ) (8.41)
m J m J

Now, we need to solve for v̇c and ω̇. First, we’ll add equation (8.40) to equation (8.41).
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
Next, we’ll subtract equation (8.41) from equation (8.40).
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
rb rb2 rb rb
ω̇ = (C1 − C3 )vc + (−C1 − C3 )ω + C2 Vl − C4 Vr
J J J J
Now, just convert the two equations to state-space notation.

Theorem 8.6.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
rb
J (C1 − C3 ) J (−C1 − C3 ) J C2 − rJb C4
  (8.42)
1 −rb
C= D = 02×2
1 rb

G2 K 2
where C1 = − KvlRrt2 , C2 = G l Kt
Rr , C3 = − KGvrRr
Kt
2 , and C4 = Rr .
Gr Kt
86 Chapter 8. State-space applications

See https://github.com/calcmogul/controls-engineering-in-frc/blob/master/modern-control-theory/
ss-applications/ramsete_coupled.py for an implementation.
Reference tracker comparison
Figures 8.22 and 8.23 shows how well each reference tracker performs.

Figure 8.22: Left/right velocity reference tracker Figure 8.23: Linear/angular velocity reference
response to a motion profile tracker response to a motion profile

For a simple s-curve motion profile, they behave similarly.

Figure 8.24 demonstrates the Ramsete controller with the left/right velocity reference tracker for
a typical FRC drivetrain with the reference tracking behavior shown in figure 8.25 and figure 8.26
demonstrates the Ramsete controller with the velocity / angular velocity reference tracker for a typical
FRC drivetrain with the reference tracking behavior shown in figure 8.27.
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 8.6.3 is a superior reference tracker to theorem 8.6.4 due to its decoupled dy-
namics, even though conversions are required between it and the Ramsete controller’s commands.
8.7 Linear time-varying unicycle controller (cascaded) 87

Figure 8.24: Ramsete controller response with Figure 8.25: Ramsete controller’s left/right ve-
left/right velocity reference tracker (b = 2, ζ = locity reference tracker response
0.7)

Figure 8.26: Ramsete controller response with Figure 8.27: Ramsete controller’s velocity / an-
velocity / angular velocity reference tracker (b = gular velocity reference tracker response
2, ζ = 0.7)

8.7 Linear time-varying unicycle controller (cascaded)

One can also create a linear time-varying controller with a cascaded control architecture like Ramsete.
This section will derive a locally optimal replacement for Ramsete.
The change in global pose for a unicycle is defined by the following three equations.

ẋ = v cos θ
ẏ = v sin θ
θ̇ = ω

 T  T
Here’s the model as a vector function where x = x y θ and u = v ω .
 
v cos θ
f (x, u) =  v sin θ  (8.43)
ω
88 Chapter 8. State-space applications

To create an LQR, we need to linearize this.

 
0 0 −v sin θ
∂f (x, u) 
= 0 0 v cos θ 
∂x
0 0 0
 
cos θ 0
∂f (x, u) 
= sin θ 0
∂u
0 1

Therefore,

Theorem 8.7.1 — Linear time-varying unicycle state-space model.

ẋ = Ax + Bu
y = Cx + Du
 
x  
  v

x= y  y= θ u=
ω
θ
   
0 0 −v sin θ cos θ 0
A = 0 0 v cos θ  B =  sin θ 0
(8.44)
0 0  0 0 1
C= 0 0 1 D = 01×2

The LQR gain should be recomputed around the current operating point regularly due to the high
nonlinearity of this system. A less computationally expensive controller will be developed next.

8.7.1 Explicit time-varying control law

As with the LTV differential drive, we will fit a vector function of the states to the LTV unicycle
controller gains. Fortunately, the formulation of the controller dealing with cross-track error only
requires linearization around different values of v rather than v and θ; only a two-dimensional func-
tion is needed rather than a three-dimensional one. See figures 8.28 through 8.30 for plots of each
controller gain for a range of velocities.
With the exception of the x gain plot, all functions are a variation of a square root. v = 0 and v = 1
were used to scale each function to produce a close approximation. The sign function10 is used for
symmetry around the origin in the regression for y.

10
The sign function is defined as follows:

−1
 x<0
sgn(x) = 0 x=0

1 x>0

8.7 Linear time-varying unicycle controller (cascaded) 89

Figure 8.28: Linear time-varying unicycle con- Figure 8.29: Linear time-varying unicycle con-
troller cascaded LQR gain regression (x) troller cascaded LQR gain regression (y)

Figure 8.30: Linear time-varying unicycle con-

troller cascaded LQR gain regression (θ)

Theorem 8.7.2 — Linear time-varying unicycle controller. The following A and B matrices
of a continuous system are used to compute the LQR.

     
0 0 0 1 0 x  
v

A= 0 0 v  
B= 0 0  
x= y  u= (8.45)
ω
0 0 0 0 1 θ

The locally optimal controller for this model is u = K(v)(r − x) where

 
kx 0 0
K(v) = p (8.46)
0 ky (v) sgn(v) kθ |v|
p
ky (v) = ky,0 + (ky,1 − ky,0 ) |v| (8.47)

Using K computed via LQR at v = 0

kx = K1,1 ky,0 = K2,2

Using K computed via LQR at v = 1

ky,1 = K2,2 kθ = K2,3

This page intentionally left blank
III
Estimation and localization

9 Stochastic control theory . . . . . . . . . . . . 93

9.1 Terminology
9.2 State observers
9.3 Introduction to probability
9.4 Linear stochastic systems
9.5 Two-sensor problem
9.6 Kalman filter
9.7 Kalman smoother
9.8 Extended Kalman filter
9.9 Unscented Kalman filter
9.10 Multiple model adaptive estimation

10 Pose estimation . . . . . . . . . . . . . . . . . . . 123

10.1 Euler integration
10.2 Pose exponential
10.3 Pose correction
This page intentionally left blank
A different kind of banana distribution [23]

9. Stochastic control theory

Stochastic control theory is a subfield of control theory that deals with the existence of uncertainty
either in observations or in noise that drives the evolution of a system. We assign probability distri-
butions to this random noise and aim to achieve a desired control task despite the presence of this
uncertainty.
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.

9.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.

9.2 State observers

State 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 estimation error.
1
An agent is a system-agnostic term for independent controlled actors like robots or aircraft.
94 Chapter 9. Stochastic control theory

The Luenberger observer and Kalman filter are examples of these, where the latter chooses the pole
locations optimally based on the model and measurement uncertainties.
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.

9.2.1 Luenberger observer

We’ll introduce the Luenberger observer first to demonstrate the general form of a state estimator
and some of their properties.

Theorem 9.2.1 — Luenberger observer.

x̂˙ = Ax̂ + Bu + L(y − ŷ) (9.1)

ŷ = Cx̂ + Du (9.2)

x̂k+1 = Ax̂k + Buk + L(yk − ŷk ) (9.3)

ŷk = Cx̂k + Duk (9.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 9.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.
9.2 State observers 95

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 9.2.2 instead.

Theorem 9.2.2 — Luenberger observer with separate predict/update.

Predict step
x̂− −
k+1 = Ax̂k + Buk (9.5)
Update step
x̂+ − −1
k+1 = x̂k+1 + A L(yk − ŷk ) (9.6)
ŷk = Cx̂−
k (9.7)

See appendix H.3.1 for a derivation.

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 (9.4) into equation (9.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 (5.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 (5.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 (9.8)
96 Chapter 9. Stochastic control theory

For equation (9.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.

9.3 Introduction to probability

Now we’ll begin establishing probability concepts we need to describe and manipulate stochastic
systems.

9.3.1 Random variables

A random variable is a variable whose values are the outcomes of a random phenomenon, like dice
rolls or noisy process measurements. 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 of the random variable whose value at a given
sample (measured value) in the sample space (the range of possible measured values) is the probability
of that sample occurrring. The area under the 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 ) dx. In other words, the probability is the area under the PDF within the region [x1 , x1 + dx]
(see figure 9.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. In other words, Z∞
p(x) ≥ 0, p(x) dx = 1
−∞

or given that the probability of a given sample is greater than or equal to zero, the sum of probabilities
for all possible input values is equal to one.
9.3 Introduction to probability 97

Figure 9.1: Probability density function

9.3.2 Expected value

Expected value or expectation is a weighted average of the values the PDF can produce where the
weight for each value is the probability of that value occurring. This can be written mathematically
as Z ∞
x = E[x] = x p(x) dx
−∞

The expectation can be applied to random functions as well as random variables.

Z ∞
E[f (x)] = f (x) p(x) dx
−∞

The mean of a random variable is denoted by an overbar (e.g., x) pronounced x-bar. The expectation
of the difference between a random variable and its mean x − x converges to zero. In other words,
the expectation of a random variable is its mean. Here’s a proof.
Z ∞
E[x − x] = (x − x) p(x) dx
Z−∞∞ Z ∞
E[x − x] = x p(x) dx − x p(x) dx
−∞ −∞
Z ∞ Z ∞
E[x − x] = x p(x) dx − x p(x) dx
−∞ −∞
E[x − x] = x − x · 1
E[x − x] = 0

9.3.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;
98 Chapter 9. Stochastic control theory

Figure 9.2: Joint probability density function

we can’t know the true value of that variable without randomness, but we can give a bound on its
randomness. Z ∞
var(x) = σ 2 = E[(x − x)2 ] = (x − x)2 p(x) dx
−∞

The standard deviation is the square root of the variance.

p
std[x] = σ = var(x)

9.3.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]. In other words, the probability is the
volume under a region of the PDF manifold (see figure 9.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. Z Z
∞ ∞
p(x, y) ≥ 0, p(x, y) dx dy = 1
−∞ −∞

The expected values for joint PDFs are as follows.

Z ∞Z ∞
E[x] = x dx dy
−∞ −∞
Z ∞Z ∞
E[y] = y dx dy
−∞ −∞
Z ∞Z ∞
E[f (x, y)] = f (x, y) dx dy
−∞ −∞
9.3 Introduction to probability 99

The variance of a joint PDF measures how a variable correlates with itself (we’ll cover variances with
respect to other variables shortly).
Z ∞Z ∞
2
var(x) = Σxx = E[(x − x) ] = (x − x)2 p(x, y) dx dy
Z −∞
∞ Z ∞
−∞

var(y) = Σyy = E[(y − y)2 ] = (y − y)2 p(x, y) dx dy

−∞ −∞

9.3.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.
Z ∞Z ∞
cov(x, y) = Σxy = E[(x − x)(y − y)] = (x − x)(y − y) p(x, y) dx dy
−∞ −∞

9.3.6 Correlation
Two random variables are correlated if the result of one random variable affects the result of another.
Correlation is defined as
Σxy
ρ(x, y) = p , |ρ(x, y)| ≤ 1
Σxx Σyy

So two variable’s correlation is defined as their covariance over the geometric mean of their variances.
Uncorrelated sources have a covariance of zero.

9.3.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,
Z ∞Z ∞
E[xy] = xy p(x, y) dx dy
−∞ −∞
Z ∞Z ∞
E[xy] = xy p(x) p(y) dx dy
−∞ −∞
Z ∞ Z ∞
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, as expected. Furthermore, ρ(x, y) = 0, which means they are
uncorrelated.
100 Chapter 9. Stochastic control theory

9.3.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.
Z ∞
p(x) = p(x, y) dy
−∞

9.3.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
Z ∞
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.

9.3.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).

9.3.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.
Z ∞
E[x|y] = x p(x|y) dx = f (y), E[x|y] ̸= E[x]
−∞
Z ∞
E[y|x] = y p(y|x) dy = f (x), E[y|x] ̸= E[y]
−∞

9.3.12 Conditional variances

var(x|y) = E[(x − E[x|y])2 |y]

Z ∞
var(x|y) = (x − E[x|y])2 p(x|y) dx
−∞

9.3.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
9.3 Introduction to probability 101

The elements of x are scalar variables jointly distributed with a joint density p(x1 , x2 , . . . , xn ). The
expectation is
Z ∞
E[x] = x = x p(x) dx
−∞
 
E[x1 ]
 E[x2 ] 
 
E[x] =  . 
 . .
E[xn ]
Z ∞ Z ∞
E[xi ] = ... xi p(x1 , x2 , . . . , xn ) dx1 . . . dxn
−∞ −∞
Z ∞
E[f (x)] = f (x) p(x) dx
−∞

9.3.14 Covariance matrix

The covariance matrix for a random vector x ∈ Rn is

Σ = cov(x, x) = E[(x − x)(x − x)T ]

 
cov(x1 , x1 ) cov(x1 , x2 ) . . . cov(x1 , xn )
 cov(x2 , x1 ) cov(x2 , 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.

9.3.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 (9.9)

Now, compute E[xyT ].

Z Z
E[xyT ] = xyT p(x) p(y) dx dyT
X Y
102 Chapter 9. Stochastic control theory

Factor out constants from the inner integral. This includes variables which are held constant for each
inner integral evaluation.
Z Z
T
E[xy ] = p(x) x dx p(y) yT dyT
X Y

Each of these integrals is just the expected value of their respective integration variable.

E[xyT ] = xyT (9.10)

Substitute equation (9.10) into equation (9.9).

Σxy = (xyT ) − xyT

Σxy = 0

Using these results, we can compute the covariance of z = Ax + By.

Σ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, Σxy = 0 and the cross terms cancel out.

Σ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

Recall that Σx = cov(x, x) and Σy = cov(y, y).

Σz = AΣx AT + BΣy BT

9.3.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.
9.4 Linear stochastic systems 103

Definition 9.3.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.

9.4 Linear stochastic systems

Given the following stochastic system

xk+1 = Axk + Buk + Γwk

yk = Cxk + 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.

9.4.1 State vector expectation evolution

First, we will compute how the expectation of the system state evolves.

E[xk+1 ] = E[Axk + Buk + Γwk ]

E[xk+1 ] = E[Axk ] + E[Buk ] + E[Γwk ]
E[xk+1 ] = AE[xk ] + BE[uk ] + ΓE[wk ]
E[xk+1 ] = AE[xk ] + Buk + 0
xk+1 = Axk + Buk

9.4.2 Error covariance matrix evolution

Now, we will use this to compute how the error covariance matrix P evolves.

xk+1 − xk+1 = Axk + Buk + Γwk − (Axk − Buk )

104 Chapter 9. Stochastic control theory

xk+1 − xk+1 = A(xk − xk ) + Γwk

E[(xk+1 − xk+1 )(xk+1 − xk+1 )T ] = E[(A(xk − xk ) + Γwk )(A(xk − xk ) + Γwk )T ]

Pk+1 = E[(A(xk − xk ) + Γwk )(A(xk − xk ) + Γwk )T ]

Pk+1 = E[(A(xk − xk )(xk − xk )T AT ] + E[A(xk − xk )wkT ΓT ]+
E[Γwk (xk − xk )T AT ] + E[Γwk wkT ΓT ]
Pk+1 = AE[(xk − xk )(xk − xk )T ]AT + AE[(xk − xk )wkT ]ΓT +
ΓE[wk (xk − xk )T ]AT + ΓE[wk wkT ]ΓT
Pk+1 = APk AT + AE[(xk − xk )wkT ]ΓT + ΓE[wk (xk − xk )T ]AT + ΓQk ΓTk

Since the error and noise are independent, the cross terms are zero.

Pk+1 = APk AT + A E[(xk − xk )wkT ] ΓT + Γ E[wk (xk − xk )T ] AT + ΓQk ΓTk

| {z } | {z }
0 0
T T
Pk+1 = APk A + ΓQk Γ

9.4.3 Measurement vector expectation

Next, we will compute the expectation of the output y.

E[yk ] = E[Cxk + Duk + vk ]

E[yk ] = CE[xk ] + Duk + 0
yk = Cxk + Duk

9.4.4 Measurement covariance matrix

Now, we will use this to compute how the measurement covariance matrix S evolves.

yk − yk = Cxk + Duk + vk − (Cxk + Duk )

yk − yk = C(xk − xk ) + vk

E[(yk − yk )(yk − yk )T ] = E[(C(xk − xk ) + vk )(C(xk − xk ) + vk )T ]

Sk = E[(C(xk − xk ) + vk )(C(xk − xk ) + vk )T ]
Sk = E[(C(xk − xk )(xk − xk )T CT ] + E[vk vkT ]
Sk = CE[((xk − xk )(xk − xk )T ]CT + Rk
Sk = CPk CT + Rk

9.5 Two-sensor problem

We’ll skip the probability derivations here, but given two data points with associated variances rep-
resented by Gaussian distributions, the information can be optimally combined into a third Gaussian
distribution with its own mean value and variance. The expected value of x given a measurement z1
is
σ2 σ2
E[x|z1 ] = µ = 2 0 2 z1 + 2 x0 (9.11)
σ0 + σ σ0 + σ 2
9.6 Kalman filter 105

The variance of x given z1 is

σ 2 σ02
E[(x − µ)2 |z1 ] = (9.12)
σ02 + σ 2

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 (9.13)
σ02 + σ2 2
σ0 + σ 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 (9.14)
σ02 →0 σ0 + σ 2

σ2
w2 = lim 2 =1 (9.15)
σ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 (9.16)
σ02 →0 σ0 + σ 2

σ2
w2 = lim 2 =0 (9.17)
σ02 →0 σ0 + σ 2

The Kalman filter uses this optimal fusion as the basis for its operation.

9.6 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.

9.6.1 Derivations
Given the a posteriori update equation x̂+ −
k+1 = x̂k+1 + Kk+1 (yk+1 − ŷk+1 ), we want to find the
value of Kk+1 that minimizes the a posteriori estimate covariance (the error covariance) because this
minimizes the estimation error.
a posteriori estimate covariance update equation
The following is the definition of the a posteriori estimate covariance matrix.

P+ +
k+1 = cov(xk+1 − x̂k+1 )

Substitute in the a posteriori update equation and expand the measurement equations.

P+ −
k+1 = cov(xk+1 − (x̂k+1 + Kk+1 (yk+1 − ŷk+1 )))
106 Chapter 9. Stochastic control theory

P+ −
k+1 = cov(xk+1 − x̂k+1 − Kk+1 (yk+1 − ŷk+1 ))
P+ −
k+1 = cov(xk+1 − x̂k+1 −
Kk+1 ((Ck+1 xk+1 + Dk+1 uk+1 + vk+1 ) − (Ck+1 x̂−
k+1 + Dk+1 uk+1 )))
P+ −
k+1 = cov(xk+1 − x̂k+1 −
Kk+1 (Ck+1 xk+1 + Dk+1 uk+1 + vk+1 − Ck+1 x̂−
k+1 − Dk+1 uk+1 ))

Reorder terms.

P+ −
k+1 = cov(xk+1 − x̂k+1 −
Kk+1 (Ck+1 xk+1 − Ck+1 x̂−
k+1 + Dk+1 uk+1 − Dk+1 uk+1 + vk+1 ))

The Dk+1 uk+1 terms cancel.

P+ − −
k+1 = cov(xk+1 − x̂k+1 − Kk+1 (Ck+1 xk+1 − Ck+1 x̂k+1 + vk+1 ))

Distribute Kk+1 to vk+1 .

P+ − −
k+1 = cov(xk+1 − x̂k+1 − Kk+1 (Ck+1 xk+1 − Ck+1 x̂k+1 ) − Kk+1 vk+1 )

Factor out Ck+1 .

P+ − −
k+1 = cov(xk+1 − x̂k+1 − Kk+1 Ck+1 (xk+1 − x̂k+1 ) − Kk+1 vk+1 )

Factor out xk+1 − x̂−

k+1 to the right.

P+ −
k+1 = cov((I − Kk+1 Ck+1 )(xk+1 − x̂k+1 ) − Kk+1 vk+1 )

Covariance is a linear operator, so it can be applied to each term separately. Covariance squares
terms internally, so the negative sign on Kk+1 vk+1 is removed.

P+ −
k+1 = cov((I − Kk+1 Ck+1 )(xk+1 − x̂k+1 )) + cov(Kk+1 vk+1 )

Now just evaluate the covariances.

P+ − T T
k+1 = (I − Kk+1 Ck+1 ) cov(xk+1 − x̂k+1 )(I − Kk+1 Ck+1 ) + Kk+1 cov(vk+1 )Kk+1
P+ − T T
k+1 = (I − Kk+1 Ck+1 )Pk+1 (I − Kk+1 Ck+1 ) + Kk+1 Rk+1 Kk+1

Finding the optimal 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 .

We’ll start with the equation for P+

k+1 .

P+ − T T
k+1 = (I − Kk+1 Ck+1 )Pk+1 (I − Kk+1 Ck+1 ) + Kk+1 Rk+1 Kk+1

We’re going to expand the equation for P+ −

k+1 and collect terms. First, multiply in Pk+1 .

P+ − − T T
k+1 = (Pk+1 − Kk+1 Ck+1 Pk+1 )(I − Kk+1 Ck+1 ) + Kk+1 Rk+1 Kk+1
9.6 Kalman filter 107

Tranpose each term in I − Kk+1 Ck+1 . I is symmetric, so its transpose is dropped.

P+ − − T T T
k+1 = (Pk+1 − Kk+1 Ck+1 Pk+1 )(I − Ck+1 Kk+1 ) + Kk+1 Rk+1 Kk+1

Multiply in I − CTk+1 KTk+1 .

P+ − T T − T T T
k+1 = Pk+1 (I − Ck+1 Kk+1 ) − Kk+1 Ck+1 Pk+1 (I − Ck+1 Kk+1 ) + Kk+1 Rk+1 Kk+1

Expand terms.

P+ − − T T − − T T
k+1 = Pk+1 − Pk+1 Ck+1 Kk+1 − Kk+1 Ck+1 Pk+1 + Kk+1 Ck+1 Pk+1 Ck+1 Kk+1
Kk+1 Rk+1 KTk+1

Factor out Kk+1 and KTk+1 .

P+ − − T T −
k+1 = Pk+1 − Pk+1 Ck+1 Kk+1 − Kk+1 Ck+1 Pk+1 +
Kk+1 (Ck+1 P− T T
k+1 Ck+1 + Rk+1 )Kk+1
P+ − − T T − T
k+1 = Pk+1 − Pk+1 Ck+1 Kk+1 − Kk+1 Ck+1 Pk+1 + Kk+1 Sk+1 Kk+1 (9.18)

Now take the trace.

tr(P+ − − T T − T
k+1 ) = tr(Pk+1 ) − tr(Pk+1 Ck+1 Kk+1 ) − tr(Kk+1 Ck+1 Pk+1 ) + tr(Kk+1 Sk+1 Kk+1 )

Transpose one of the terms twice.

tr(P+ − −T T − T
k+1 ) = tr(Pk+1 ) − tr((Kk+1 Ck+1 Pk+1 ) ) − tr(Kk+1 Ck+1 Pk+1 ) + tr(Kk+1 Sk+1 Kk+1 )

P−
k+1 is symmetric, so we can drop the transpose.

tr(P+ − − T − T
k+1 ) = tr(Pk+1 ) − tr((Kk+1 Ck+1 Pk+1 ) ) − tr(Kk+1 Ck+1 Pk+1 ) + tr(Kk+1 Sk+1 Kk+1 )

The trace of a matrix is equal to the trace of its transpose since the elements used in the trace are on
the diagonal.

tr(P+ − − − T
k+1 ) = tr(Pk+1 ) − tr(Kk+1 Ck+1 Pk+1 ) − tr(Kk+1 Ck+1 Pk+1 ) + tr(Kk+1 Sk+1 Kk+1 )
tr(P+ − − T
k+1 ) = tr(Pk+1 ) − 2 tr(Kk+1 Ck+1 Pk+1 ) + tr(Kk+1 Sk+1 Kk+1 )

Given theorems 9.6.1 and 9.6.2

Theorem 9.6.1 ∂
∂A tr(ABAT ) = 2AB where B is symmetric.

Theorem 9.6.2 ∂
∂A tr(AC) = CT
108 Chapter 9. Stochastic control theory

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(Ck+1 P− T
k+1 ) + 2Kk+1 Sk+1
∂K
∂ tr(P+
k+1 )
= −2P−T T
k+1 Ck+1 + 2Kk+1 Sk+1
∂K
∂ tr(P+
k+1 )
= −2P− T
k+1 Ck+1 + 2Kk+1 Sk+1
∂K
0 = −2P− T
k+1 Ck+1 + 2Kk+1 Sk+1
2Kk+1 Sk+1 = 2P− T
k+1 Ck+1
Kk+1 Sk+1 = P− T
k+1 Ck+1
Kk+1 = P− T −1
k+1 Ck+1 Sk+1

This is the optimal Kalman gain.

Simplified a priori estimate covariance update equation
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 Ck+1 Sk+1
Kk+1 Sk+1 = P− T
k+1 Ck+1
Kk+1 Sk+1 KTk+1 = P− T T
k+1 Ck+1 Kk+1

Now we’ll substitute it into equation (9.18).

P+ − − T T − T
k+1 = Pk+1 − Pk+1 Ck+1 Kk+1 − Kk+1 Ck+1 Pk+1 + Kk+1 Sk+1 Kk+1
P+ − − T T − − T T
k+1 = Pk+1 − Pk+1 Ck+1 Kk+1 − Kk+1 Ck+1 Pk+1 + (Pk+1 Ck+1 Kk+1 )
P+ − −
k+1 = Pk+1 − Kk+1 Ck+1 Pk+1

Factor out P−
k+1 to the right.

P+ −
k+1 = (I − Kk+1 Ck+1 )Pk+1

9.6.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 9.6.3 shows the predict and update steps for a Kalman filter at the k th timestep.
9.6 Kalman filter 109

Theorem 9.6.3 — Kalman filter.

Predict step
x̂−
k+1 = Ax̂k + Buk (9.19)
P−
k+1 = AP−
kA
T
+ ΓQΓ T
(9.20)
Update step
Kk+1 = P− T − T
k+1 C (CPk+1 C + R)
−1
(9.21)
x̂+
k+1 = x̂−
k+1 + Kk+1 (yk+1 − Cx̂−
k+1 − Duk+1 ) (9.22)
P+k+1 = (I − Kk+1 C)P−
k+1 (9.23)

A system matrix x̂ state estimate vector

B input matrix u input vector
C output matrix y output vector
D feedthrough matrix Γ process noise intensity vector
P error covariance matrix Q process noise covariance matrix
K Kalman gain matrix R measurement noise covariance matrix
where a superscript of minus denotes a priori and plus denotes a posteriori estimate (before and
after update respectively).

C, D, Q, and R from the equations derived earlier are made constants here.

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 6.7.1.

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 Γ states × 1
P states × states Q states × states
K states × outputs R outputs × outputs

Table 9.2: Kalman filter matrix dimensions

Unknown states in a Kalman filter are generally represented by a Wiener (pronounced VEE-ner)
process2 . This process has the property that its variance increases linearly with time t.

2
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.
110 Chapter 9. Stochastic control theory

9.6.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 (9.24)
vk+1 = vk (9.25)
xw
k+1 = xw
k (9.26)

This can be converted to the following state-space model.

 
xk
xk =  vk  (9.27)
xwk
     
1 1 0 0 0
xk+1 = 0 0 0 xk + 0.8 + 0.1 wk (9.28)
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 (9.29)
−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 (9.30)
 
10 0
R= (9.31)
0 10
 
yk,1
x̂ = (yk,1 − yk−1,1 )/dt (9.32)
yk,1 + yk,2
 
10 10/dt 10
P = 10/dt 20/dt2 10/dt (9.33)
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
9.6 Kalman filter 111

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
way, whether it’s missing dynamics or assumes an incorrect noise model, the residual y e = y − Cx̂
over time will have probability characteristics not indicative of white noise (e.g., it isn’t zero-mean).
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.

9.6.4 Simulation
Figure 9.3 shows the state estimates and measurements of the Kalman filter over time. Figure 9.4
shows the position estimate and variance over time. Figure 9.5 shows the wall position estimate
112 Chapter 9. Stochastic control theory

Figure 9.3: State estimates and measurements with Kalman filter

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.

9.6.5 Kalman filter as Luenberger observer

A Kalman filter can be represented as a Luenberger observer by letting L = AKk (see appendix
H.3). 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 C)) (9.34)

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 Riccati equation. This can then be used to compute a steady-state
Kalman gain.
Snippet 9.1 computes the steady-state matrices for a Kalman filter.
9.6 Kalman filter 113

Figure 9.4: Robot position estimate and variance with Kalman filter

Figure 9.5: Wall position estimate and variance with Kalman filter
114 Chapter 9. Stochastic control theory

import control as ct
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 (ct.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 9.1. Steady-state Kalman gain and error covariance matrices calculation in Python

9.7 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.

9.7.1 Derivations
9.7.2 State update equation
Let x̂(t) be the state estimate from the forward pass based on samples 0 to t and x̂b (t) be the state
estimate from the backward pass based on samples T to t.

x̂(t|T ) = Ax̂(t) + A′ x̂b (t) (9.35)

where A and A′ are weighting factors.

x(t) + A′ x(t) + A′ x
x̂(t|T ) = Ax(t) + Ae eb (t)
9.7 Kalman smoother 115

where x
e(t) represents the error in the forward state estimate and x
eb (t) represents the error in the
backward state estimate.

x(t) + x x(t) + A′ x(t) + A′ x

e(t|T ) = Ax(t) + Ae eb (t)
x x(t) + A′ x(t) + A′ x
e(t|T ) = Ax(t) − x(t) + Ae eb (t)

Factor out x(t).

e(t|T ) = (A + A′ − I)x(t) + Ae
x x(t) + A′ x
eb (t)

For unbiased filtering errors such as x

e(t) and x
eb (t), we want to have an unbiased smoothing error.
Therefore, we set A + A′ − I to zero. This yields A′ = I − A, so

e(t|T ) = Ae
x x(t) + (I − A)e
xb (t)

9.7.3 Error covariance equation

Next, find the error covariance.

P(t|T ) = AP(t)AT + (I − A)Pb (t)(I − A)T (9.36)

Find the minimum of the trace of P(t|T ) by taking the partial derivaitve with respect to A and
setting the result to 0 ((t) has been dropped from covariance matrices for clarity).

0 = 2AP + 2(I − A)Pb (−I)

0 = 2AP − 2(I − A)Pb
0 = AP − (I − A)Pb
0 = AP − (Pb − APb )
0 = AP − Pb + APb
0 = A(P + Pb ) − Pb
A(P + Pb ) = Pb
A = Pb (P + Pb )−1 (9.37)

I − A = I − Pb (P + Pb )−1
I − A = (P + Pb )(P + Pb )−1 − Pb (P + Pb )−1
I − A = (P + Pb − Pb )(P + Pb )−1
I − A = P(P + Pb )−1 (9.38)

AT = (Pb (P + Pb )−1 )T
AT = ((P + Pb )−1 )T PTb
AT = ((P + Pb )T )−1 PTb
AT = (PT + PTb )−1 PTb

Covariance matrices are symmetric, so

AT = (P + Pb )−1 Pb (9.39)
116 Chapter 9. Stochastic control theory

(I − A)T = (P(P + Pb )−1 )T

(I − A)T = ((P + Pb )−1 )T PT
(I − A)T = ((P + Pb )T )−1 PT
(I − A)T = (PT + PTb )−1 PT

Covariance matrices are symmetric, so

(I − A)T = (P + Pb )−1 P (9.40)

Now starting from equation (9.36), substitute in equations (9.37) through (9.40).

P(t|T ) = APAT + (I − A)Pb (I − A)T

P(t|T ) = (Pb (P + Pb )−1 )P((P + Pb )−1 Pb ) + (P(P + Pb )−1 )Pb ((P + Pb )−1 P)
P(t|T ) = Pb (P + Pb )−1 P(P + Pb )−1 Pb + P(P + Pb )−1 Pb (P + Pb )−1 P

Apply theorem 9.7.1 to the right sides of each term to combine them.

Theorem 9.7.1 AB = (B−1 A−1 )−1

P(t|T ) = Pb (P + Pb )−1 P(P−1

b (P + Pb ))
−1
+ P(P + Pb )−1 Pb (P−1 (P + Pb ))−1
P(t|T ) = Pb (P + Pb )−1 P(P−1
b P + I)
−1
+ P(P + Pb )−1 Pb (I + P−1 Pb )−1

Apply theorem 9.7.1 to the right sides of each term again.

P(t|T ) = Pb (P + Pb )−1 ((P−1 −1 −1

b P + I)P ) + P(P + Pb )−1 ((I + P−1 Pb )P−1
b )
−1

P(t|T ) = Pb (P + Pb )−1 (P−1 −1 −1

b +P ) + P(P + Pb )−1 (P−1 −1 −1
b +P )
P(t|T ) = Pb (P + Pb )−1 (P−1 + P−1
b )
−1
+ P(P + Pb )−1 (P−1 + P−1
b )
−1

Factor out (P−1 + P−1

b )
−1 to the right.

P(t|T ) = (Pb (P + Pb )−1 + P(P + Pb )−1 )(P−1 + P−1

b )
−1

Factor out (P + Pb )−1 to the right.

P(t|T ) = (Pb + P)(P + Pb )−1 (P−1 + P−1

b )
−1

P(t|T ) = (P + Pb )(P + Pb )−1 (P−1 + P−1

b )
−1

P(t|T ) = I(P−1 + P−1

b )
−1

P(t|T ) = (P(t)−1 + Pb (t)−1 )−1 (9.41)

9.7.4 Optimal estimate

Now find the optimal estimate x̂(t|T ) starting from equation (9.35).

x̂(t|T ) = Ax̂(t) + A′ x̂b (t)

x̂(t|T ) = Ax̂(t) + (I − A)x̂b (t)
9.7 Kalman smoother 117

x̂(t|T ) = Pb (P + Pb )−1 x̂(t) + P(P + Pb )−1 x̂b (t)

x̂(t|T ) = Pb (P + Pb )−1 PP−1 x̂(t) + P(P + Pb )−1 Pb P−1
b x̂b (t)

Apply theorem 9.7.1.

x̂(t|T ) = Pb (P−1 (P + Pb ))−1 P−1 x̂(t) + P(P−1 −1 −1

b (P + Pb )) Pb x̂b (t)
x̂(t|T ) = Pb (I + P−1 Pb )−1 P−1 x̂(t) + P(P−1 −1 −1
b P + I) Pb x̂b (t)

Apply theorem 9.7.1 again.

x̂(t|T ) = ((I + P−1 Pb )P−1 −1 −1 −1 −1 −1 −1

b ) P x̂(t) + ((Pb P + I)P ) Pb x̂b (t)
x̂(t|T ) = (P−1 −1 −1 −1 −1 −1 −1 −1
b + P ) P x̂(t) + (Pb + P ) Pb x̂b (t)

Factor out (P−1 −1 −1 to the left.

b +P )

x̂(t|T ) = (P−1 −1 −1 −1 −1
b + P ) (P x̂(t) + Pb x̂b (t))

Substitute in equation (9.41).

x̂(t|T ) = P(t|T )(P−1 x̂(t) + P−1

b x̂b (t))
x̂(t|T ) = P(t|T )(P−1 (t)x̂(t) + P−1
b (t)x̂b (t)) (9.42)

9.7.5 Predict and update equations

One first does a forward pass with the typical Kalman filter equations and stores the results. Then
one can use the Rauch-Tung-Striebel (RTS) algorithm to do the backward pass (see theorem 9.7.2).
Theorem 9.7.2 shows the predict and and update steps for the forward and backward passes for a
Kalman smoother at the k th timestep.

Theorem 9.7.2 — Kalman smoother.

Forward predict step

x̂−
k+1 = Ax̂k + Buk (9.43)
P−
k+1 = AP−
kA
T
+ ΓQΓ T
(9.44)
Forward update step
Kk+1 = P− T − T
k+1 C (CPk+1 C + R)
−1
(9.45)
x̂+ − −
k+1 = x̂k+1 + Kk+1 (yk+1 − Cx̂k+1 − Duk+1 ) (9.46)
P+
k+1 = (I − Kk+1 C)P−
k+1 (9.47)
Backward update step
K k = P+ T −
k Ak (Pk+1 )
−1
(9.48)
x̂k|N = x̂+ −
k + Kk (x̂k+1|N − x̂k+1 ) (9.49)
Pk|N = P+
k + Kk (Pk+1|N − P− T
k+1 )Kk (9.50)
Backward initial conditions
x̂N |N = x̂+
N (9.51)
PN |N = P+
N (9.52)
118 Chapter 9. Stochastic control theory

Figure 9.6: Robot position with Kalman smoother

9.7.6 Example
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.
 
xk
xk = vk 
 (9.53)
xw
k
   
1 1 0 0
xk+1 = 0 1 0 xk + 0.1 wk
   (9.54)
0 0 1 0

We will use the same observation model as before.

Using the same data from subsection 9.6.4, figures 9.6, 9.7, and 9.8 show the improved state estimates
and figure 9.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.
See Roger Labbe’s book Kalman and Bayesian Filters in Python for more on smoothing3 .

3
https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/13-Smoothing.ipynb
9.7 Kalman smoother 119

Figure 9.7: Robot velocity with Kalman smoother

Figure 9.8: Wall position with Kalman smoother

120 Chapter 9. Stochastic control theory

Figure 9.9: Robot position variance with Kalman smoother

9.8 Extended Kalman filter

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.
The extended Kalman filter linearizes the matrices used during the prediction step. A, B, C, and D
are linearized as follows:
∂f (x, u) ∂f (x, u) ∂h(x, u) ∂h(x, u)
A≈ B≈ C≈ D≈
∂x ∂u ∂x ∂u

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.
x̂+ − −
k+1 = x̂k+1 + Kk+1 (yk+1 − h(x̂k+1 ))

9.9 Unscented Kalman filter

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.
The unscented Kalman filter propagates carefully chosen points called sigma points through the non-
linear model to obtain an estimate of the true covariance (as opposed to a linearized version of it).
We recommend reading Roger Labbe’s book Kalman and Bayesian Filters in Python for more on
unscented Kalman filters4 .

4
https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/10-Unscented-Kalman-Filter.
ipynb
9.10 Multiple model adaptive estimation 121

The original paper on the unscented Kalman filter is also an option [18]. The equations for van der
Merwe’s sigma point algorithm are in [31]. Here’s a paper on a quaternion-based Unscented Kalman
filter for orientation tracking [21].

9.10 Multiple model adaptive estimation

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 likelihood 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.
See Roger Labbe’s book Kalman and Bayesian Filters in Python for more on MMAE5 .

5
MMAE section of https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/
14-Adaptive-Filtering.ipynb
This page intentionally left blank
Road next to Stevenson Academic building at UCSC

10. Pose estimation

Pose is a term for the orientation of an agent (a system with a controller). The plant usually includes
the pose in its state vector. We’ll cover several methods for estimating an agent’s pose from local
measurements such as encoders and gyroscope heading.

10.1 Euler integration

The simplest way to perform pose estimation via dead reckoning (that is, no direct measurements
of the pose are used) 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
θk+1 = θgyro,k+1

where T is the sample period. This odometry approach assumes that the robot follows a straight path
between samples (that is, ω = 0 at all but the sample times).

10.2 Pose exponential

We can obtain a more accurate approximation of the pose by including first order dynamics for the
heading θ. To provide a rationale for the math we’re about to do, we need to cover some aspects of
group theory.

10.2.1 What is a group?

In mathematics, a group is a set equipped with a binary operation (an operation with two arguments)
that combines any two elements (of the set) to form a third element in such a way that four conditions
called group axioms are satisfied: closure, associativity, identity, and invertibility.
Closure means that the result is in the same set as the arguments.
124 Chapter 10. Pose estimation

Associativity means that within an expression containing two or more occurrences in a row of the
same associative operator, the order in which the operations are performed does not matter as long
as the sequence of the operands is not changed. In other words, different groupings of the operations
produces the same result.
Identity, or an identity element, is a special type of element of a set with respect to a binary operation
on that set, which leaves any element of the set unchanged when combined with it. For example,
the additive identity of the set of integers is zero, which means that any integer summed with zero
produces that integer.
Invertibility means there is an element that can “undo” the effect of combination with another given
element. For integers and the addition operator, the inverse element would be the negation.

10.2.2 What is a pose?

To develop what a pose is in group theory, we need to define a few key groups. SO(2) is the special
orthogonal group in dimension 2. They represent a 2D rotation.
SE(2) is the special euclidean group in dimension 2. They represent a 2D rotation and a 2D translation,
which we call a 2D pose. In other words, pose is an element of SE(2).

10.2.3 What is a twist?

A 2D twist is an element of the tangent space of SE(2) (like the tangential distance traveled by the
robot along an arc in SE(2)). We use the “pose exponential” to map a twist (an element of the tangent
space) to an element of SE(2). In other words, we map a twist to a pose.
We call it a pose exponential because it’s an exponential map onto a pose. The term exponential is
used because the solution for integrating a change in something (like a slope or tangent) over time is
usually an exponential. For example, dxdt = ax has the solution x = x0 e .
at

We use the pose exponential to take encoder measurement deltas and gyro angle deltas (which are in
the tangent space and are thus a twist) and turn them into a change in pose. This gets added to the
pose from the last update.

10.2.4 Derivation
We can obtain a more accurate approximation of the pose than Euler integration 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.

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 (the tangent space)

R
dx = R vx dt
R
dy = R vy dt
10.2 Pose exponential 125
R
dθ = R ω dt

To transform this into the global frame SE(2), 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 11.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 cos ωt−1
 R 
∆x cos θ − sin θ 0 ω ω 0 vx
∆y  =  sin θ cos θ 0  1−cos ωt
ω
sin ωt
ω 0   vy  (10.1)
∆θ 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 cos ωt−1
 R 
∆x cos θ − sin θ 0 ω ω 0 vx
∆y  =  sin θ cos θ 0  1−cos ωt sin ωt
0   vy 
ω ω
∆θ 0 0 1 0 0 t ω
G     sin ωt cos ωt−1
 R  
∆x cos θ − sin θ 0 ωt ωt 0 vx
∆y  =  sin θ cos θ 0  1−cos ωt sin ωt
0 vy t
ωt ωt
∆θ 0 0 1 0 0 1 ω
G      sin ωt cos ωt−1
 R  
∆x cos θ − sin θ 0 ωt ωt 0 v x t
∆y  =  sin θ cos θ 0  1−cos ωt sin ωt
0 vy t
ωt ωt
∆θ 0 0 1 0 0 1 ωt
G     R  sin ∆θ cos ∆θ−1
 R 
∆x cos θ − sin θ 0 ∆θ ∆θ 0 ∆x
∆y  =  sin θ cos θ 0  1−cos ∆θ
∆θ sin ∆θ
∆θ 0 ∆y  (10.2)
∆θ 0 0 1 0 0 1 ∆θ

R 
The vector ∆x ∆y ∆θ T is a twist because it’s an element of the tangent space (the robot’s
local coordinate frame).
126 Chapter 10. Pose estimation

Equation (10.1) 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 equation (10.2), 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 t 2 ω 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 t 3 · 02
∼t− =t
ω 6
cos ωt − 1 t2 · 0
∼− =0
ω 2
1 − cos ωt t2 · 0
∼ =0
ω 2

Now substitute these into equation (10.3).

G     R 
∆x cos θ − sin θ 0 t 0 0 vx
∆y  =  sin θ cos θ 0 0 t 0 vy 
∆θ 0 0 1 0 0 t ω
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
10.2 Pose exponential 127
G   
∆x vx t cos θ
∆y  =  vy t sin θ 
∆θ ωt

Theorem 10.2.1 — Pose exponential.

G     sin ωt cos ωt−1
 R 
∆x cos θ − sin θ 0 ω ω 0 vx
∆y  =  sin θ cos θ 0  1−cos ωt
ω
sin ωt
ω 0  v y  (10.3)
∆θ 0 0 1 0 0 t ω

or

G    R  sin ∆θ cos ∆θ−1

 R 
∆x cos θ − sin θ 0 ∆θ ∆θ 0 ∆x
∆y  =  sin θ cos θ 0  1−cos ∆θ
∆θ
sin ∆θ
∆θ 0   ∆y  (10.4)
∆θ 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− =− = (10.5)
ω 6 ω 2 ω 2
sin ωt t2 ω 2 cos ωt − 1 tω 1 − cos ωt tω
=1− =− = (10.6)
ω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 10.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 8.6).
The highest error is 4cm in x over a 10m path starting with a 2m displacement. The difference
would be even more noticeable for paths with higher curvatures and longer durations. One mitigation
is using a smaller update period. However, there are bigger sources of error like turning scrub on
skid steer robots that should be dealt with before odometry numerical accuracy.

10.2.5 Lie groups

While we avoided the topic in our explanation, pose is what’s known as a Lie group (a group that is
also a differentiable manifold). There’s a lot of mathematical and controls results developed around
Lie groups, so we’re mentioning the connection here in case you want to search the Internet for more
information.
128 Chapter 10. Pose estimation

Figure 10.1: Odometry error compared to method using twists (dt = 0.05s)

10.3 Pose correction

The previous methods for pose estimation have assumed no direct pose measurements are available.
At best, only heading is available. To augment any of them with corrections from full pose mea-
surements, an Extended Kalman filter (section 9.8) or Unscented Kalman filter (section 9.9) can be
used.
IV
System modeling

11 Calculus . . . . . . . . . . . . . . . . . . . . . . . . . . . 131
11.1 Derivatives
11.2 Integrals
11.3 Tables

12 Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . 135
12.1 Linear motion
12.2 Angular motion
12.3 Vectors
12.4 Curvilinear motion

13 Model examples . . . . . . . . . . . . . . . . . . . 145

13.1 DC brushed motor
13.2 Elevator
13.3 Flywheel
13.4 Single-jointed arm
13.5 Pendulum
13.6 Differential drive

14 System identification . . . . . . . . . . . . . . . 161

14.1 1 DOF mechanism model
14.2 Drivetrain velocity model
This page intentionally left blank
Hills by northbound freeway between Santa Maria and Ventura

11. Calculus

This book uses derivatives and integrals occasionally to represent small changes in values over small
changes in time and the infinitesimal sum of values over time respectively. The formulas and tables
presented here are all you’ll need to carry through with the math in later chapters.
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 6.

11.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 11.1: Notation for derivatives of f (x)

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

132 Chapter 11. Calculus

11.1.1 Power rule

f (x) = xn
f ′ (x) = nxn−1

11.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)

11.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

11.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 11.2.
Z
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.
Z t
eat dt
0
 t
1 at
e +C
a
   0
1 at 1 a·0
e +C − e +C
a a
   
1 at 1
e +C − +C
a a
1 at 1
e +C − −C
a a
11.3 Tables 133
1 at 1
e −
a a

When we do this, the constant cancels out.

11.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.
Z
cos ωt dt

Let u = ωt.

du = ω dt
1
dt = du
ω

Now substitute the expressions for u and dt in.

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

11.3 Tables
11.3.1 Common derivatives and integrals
R
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 11.2: Common derivatives and integrals

This page intentionally left blank
Hills by northbound freeway between Santa Maria and Ventura

12. Dynamics

12.1 Linear motion

X
F = ma
P
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.

12.2 Angular motion

X
τ = Iα
P
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.
136 Chapter 12. Dynamics

12.3 Vectors
Vectors are quantities with a magnitude and a direction. Vectors in three-dimensional space have a
coordinate for each spatial direction x, y, and z. Let’s take the vector ⃗a = ⟨1, 2, 3⟩. ⃗a is a three-
dimensional vector that describes a movement 1 unit in the x direction, 2 units in the y direction, and
3 units in the z direction.
We define î, ĵ, and k̂ as vectors that represent the fundamental movements one can make the three-
dimensional space: 1 unit of movement in the x direction, 1 unit of movement y direction, and 1 unit
of movement in the z direction respectively. These three vectors form a basis of three-dimensional
space because copies of them can be added together to reach any point in three-dimensional space.

î = ⟨1, 0, 0⟩
ĵ = ⟨0, 1, 0⟩
k̂ = ⟨0, 0, 1⟩

We can also write the vector ⃗a in terms of these basis vectors.

⃗a = 1î + 2ĵ + 3k̂

12.3.1 Basic vector operations

We will now show this is equivalent to the original notation through some vector mathematics. First,
we’ll substitute in the values for the basis vectors.

⃗a = 1⟨1, 0, 0⟩ + 2⟨0, 1, 0⟩ + 3⟨0, 0, 1⟩

Scalars are multiplied component-wise with vectors.

⃗a = ⟨1, 0, 0⟩ + ⟨0, 2, 0⟩ + ⟨0, 0, 3⟩

Vectors are added by summing each of their components.

⃗a = ⟨1, 2, 3⟩

12.3.2 Cross product

The cross product is denoted by ×. The cross product of the basis vectors are computed as follows.

î × ĵ = k̂
ĵ × k̂ = î
k̂ × î = ĵ

They proceed in a cyclic fashion through i, j, and k. If a vector is crossed with itself, it produces the
zero vector (a scalar zero for each coordinate). The cross product of the basis vectors in the opposite
order progress backwards and include a negative sign.

î × k̂ = −ĵ
k̂ × ĵ = −î
ĵ × î = −k̂
12.4 Curvilinear motion 137

Given vectors ⃗u = aî + bĵ + ck̂ and ⃗v = dî + eĵ + f k̂, ⃗u × ⃗v is computed using the distributive
property.

⃗u × ⃗v = (aî + bĵ + ck̂) × (dî + eĵ + f k̂)

⃗u × ⃗v = ad(î × î) + ae(î × ĵ) + af (î × k̂)+
bd(ĵ × î) + be(ĵ × ĵ) + bf (ĵ × k̂)+
cd(k̂ × î) + ce(k̂ × ĵ) + cf (k̂ × k̂)
⃗u × ⃗v = aek̂ + af (−ĵ) + bd(−k̂) + bf î + cdĵ + ce(−î)
⃗u × ⃗v = aek̂ − af ĵ − bdk̂ + bf î + cdĵ − ceî
⃗u × ⃗v = (bf − ce)î + (cd − af )ĵ + (ae − bd)k̂

12.4 Curvilinear motion

For these derivations, we’ll assume positive x (î) is forward, positive y (ĵ) is to the left, positive z (k̂)
is up, and the robot is facing in the x direction. This axes convention is known as North-West-Up
(NWU), and is shown in figure 12.1.

+x

+y

Figure 12.1: 2D projection of North-West-Up (NWU) axes convention. The positive z axis is pointed
out of the page toward the reader.

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”).

12.4.1 Differential drive

A differential drive has two wheels, one on each side, separated by some distance 2rb . The forces
they generate when moving forward are shown in figure 12.2.
138 Chapter 12. Dynamics

vl vr

+x

+y

Figure 12.2: Differential drive free body diagram

Inverse kinematics
The mapping from v and ω to the left and right wheel velocities vl and vr is 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.
Once we have the vector equation representing the wheel’s velocity, we’ll project it onto the wheel
direction vector using the dot product.
First, we’ll derive vl .

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

Now, project this vector onto the left wheel, which is pointed in the î direction.

î
vl = (vc − ωrb )î ·
∥î∥

The magnitude of î is 1, so the denominator cancels.

vl = (vc − ωrb )î · î

vl = vc − ωrb (12.1)

Next, we’ll derive vr .

⃗vr = vc î + ω k̂ × rb ĵ
⃗vr = vc î + ωrb î
⃗vr = (vc + ωrb )î

Now, project this vector onto the right wheel, which is pointed in the î direction.

î
vr = (vc + ωrb )î ·
∥î∥

The magnitude of î is 1, so the denominator cancels.

vr = (vc + ωrb )î · î

12.4 Curvilinear motion 139

vr = vc + ωrb (12.2)

So the two inverse kinematic equations are as follows.

vl = vc − ωrb (12.3)
vr = vc + ωrb (12.4)

Forward kinematics
The mapping from the left and right wheel velocities vl and vr to v and ω is derived as follows.

vr = vc + ωrb
vc = vr − ωrb (12.5)

Substitute equation (12.5) equation for vl .

vl = vc − ωrb
vl = (vr − ωrb ) − ωrb
vl = vr − 2ωrb
2ωrb = vr − vl
vr − vl
ω=
2rb

Substitute this back into equation (12.5).

vc = vr − ωrb
 
vr − vl
vc = vr − rb
2rb
vr − vl
vc = vr −
2
vr vl
vc = vr − +
2 2
vr + vl
vc =
2

So the two forward kinematic equations are as follows.

vr + vl
vc = (12.6)
2
vr − vl
ω= (12.7)
2rb

12.4.2 Mecanum drive

A mecanum drive has four wheels, one on each corner of a rectangular chassis. The wheels have
rollers offset at 45 degrees (whether it’s clockwise or not varies per wheel). The forces they generate
when moving forward are shown in figure 12.3.
140 Chapter 12. Dynamics

vf l vf r

vrl vrr

+x

+y

Figure 12.3: Mecanum drive free body diagram

Inverse kinematics
First, we’ll derive the front-left wheel kinematics.

⃗vf l = vx î + vy ĵ + ω k̂ × (rf lx î + rf ly ĵ)

⃗vf l = vx î + vy ĵ + ωrf lx ĵ − ωrf ly î
⃗vf l = (vx − ωrf ly )î + (vy + ωrf lx )ĵ

Project the front-left wheel onto its wheel vector.

î − ĵ
vf l = ((vx − ωrf ly )î + (vy + ωrf lx )ĵ) · √
2
1
vf l = ((vx − ωrf ly ) − (vy + ωrf lx )) √
2
1
vf l = (vx − ωrf ly − vy − ωrf lx ) √
2
1
vf l = (vx − vy − ωrf ly − ωrf lx ) √
2
1
vf l = (vx − vy − ω(rf lx + rf ly )) √
2
1 1 1
vf l = √ vx − √ vy − √ ω(rf lx + rf ly ) (12.8)
2 2 2

Next, we’ll derive the front-right wheel kinematics.

⃗vf r = vx î + vy ĵ + ω k̂ × (rf rx î + rf ry ĵ)

⃗vf r = vx î + vy ĵ + ωrf rx ĵ − ωrf ry î
⃗vf r = (vx − ωrf ry )î + (vy + ωrf rx )ĵ

Project the front-right wheel onto its wheel vector.

1
vf r = ((vx − ωrf ry )î + (vy + ωrf rx )ĵ) · (î + ĵ) √
2
1
vf r = ((vx − ωrf ry ) + (vy + ωrf rx )) √
2
1
vf r = (vx − ωrf ry + vy + ωrf rx ) √
2
12.4 Curvilinear motion 141
1
vf r = (vx + vy − ωrf ry + ωrf rx ) √
2
1
vf r = (vx + vy + ω(rf rx − rf ry )) √
2
1 1 1
vf r = √ vx + √ vy + √ ω(rf rx − rf ry ) (12.9)
2 2 2

Next, we’ll derive the rear-left wheel kinematics.

⃗vrl = vx î + vy ĵ + ω k̂ × (rrlx î + rrly ĵ)

⃗vrl = vx î + vy ĵ + ωrrlx ĵ − ωrrly î
⃗vrl = (vx − ωrrly )î + (vy + ωrrlx )ĵ

Project the rear-left wheel onto its wheel vector.

1
vrl = ((vx − ωrrly )î + (vy + ωrrlx )ĵ) · (î + ĵ) √
2
1
vrl = ((vx − ωrrly ) + (vy + ωrrlx )) √
2
1
vrl = (vx − ωrrly + vy + ωrrlx ) √
2
1
vrl = (vx + vy − ωrrly + ωrrlx ) √
2
1
vrl = (vx + vy + ω(rrlx − rrly )) √
2
1 1 1
vrl = √ vx + √ vy + √ ω(rrlx − rrly ) (12.10)
2 2 2

Next, we’ll derive the rear-right wheel kinematics.

⃗vrr = vx î + vy ĵ + ω k̂ × (rrrx î + rrry ĵ)

⃗vrr = vx î + vy ĵ + ωrrrx ĵ − ωrrry î
⃗vrr = (vx − ωrrry )î + (vy + ωrrrx )ĵ

Project the rear-right wheel onto its wheel vector.

î − ĵ
vrr = ((vx − ωrrry )î + (vy + ωrrrx )ĵ) · √
2
1
vrr = ((vx − ωrrry ) − (vy + ωrrrx )) √
2
1
vrr = (vx − ωrrry − vy − ωrrrx ) √
2
1
vrr = (vx − vy − ωrrry − ωrrrx ) √
2
1
vrr = (vx − vy − ω(rrrx + rrry )) √
2
1 1 1
vrr = √ vx − √ vy − √ ω(rrrx + rrry ) (12.11)
2 2 2
142 Chapter 12. Dynamics

This gives the following inverse kinematic equations.

1 1 1
vf l = √ vx − √ vy − √ ω(rf lx + rf ly )
2 2 2
1 1 1
vf r = √ vx + √ vy + √ ω(rf rx − rf ry )
2 2 2
1 1 1
vrl = √ vx + √ vy + √ ω(rrlx − rrly )
2 2 2
1 1 1
vrr = √ vx − √ vy − √ ω(rrrx + rrry )
2 2 2

Now we’ll factor them out into matrices.

   √1 − √1 
− √12 (rf lx + rf ly )  
vf l 2 2
vf r   √1 √1 √1 (rf r − rf r )  vx
 = 1  x y 
 vrl   √
2
√1
2 2
√1 (rrl − rrl ) 
  vy 
x y
2 2 2 ω
vrr √1 − √1 − √12 (rrrx + rrry )
2 2
   
vf l 1 −1 −(rf lx + rf ly )  
vf r  1 1 vx
 = √  1 (rf rx − rf ry ) 
  vy 
 vrl  (12.12)
2 1 1 (rrlx − rrly ) 
ω
vrr 1 −1 −(rrrx + rrry )

Forward kinematics
Let M be the 4×3 inverse kinematics matrix above including the √1 factor. The forward kinematics
2
are  
  vf l
vx  
 vy  = M† vf r  (12.13)
 vrl 
ω
vrr

where M† is the pseudoinverse of M.

12.4.3 Swerve drive

A swerve drive has an arbitrary number of wheels which can rotate in place independent of the chassis.
The forces they generate are shown in figure 12.4.

v1

v3 v2

+x
v4

+y

Figure 12.4: Swerve drive free body diagram

12.4 Curvilinear motion 143

Inverse kinematics

⃗vwheel1 = ⃗vrobot + ω
⃗ robot × ⃗rrobot2wheel1
⃗vwheel2 = ⃗vrobot + ω
⃗ robot × ⃗rrobot2wheel2
⃗vwheel3 = ⃗vrobot + ω
⃗ robot × ⃗rrobot2wheel3
⃗vwheel4 = ⃗vrobot + ω
⃗ robot × ⃗rrobot2wheel4

where ⃗v is linear velocity vector, ω

⃗ is angular velocity vector, ⃗r is distance vector from the center of
rotation to the wheel, ⃗vrobot = vx î + vy ĵ, and ⃗rrobot2wheel = rx î + ry ĵ.

⃗v1 = vx î + vy ĵ + ω k̂ × (r1x î + r1y ĵ)

⃗v2 = vx î + vy ĵ + ω k̂ × (r2x î + r2y ĵ)
⃗v3 = vx î + vy ĵ + ω k̂ × (r3x î + r3y ĵ)
⃗v4 = vx î + vy ĵ + ω k̂ × (r4x î + r4y ĵ)

⃗v1 = vx î + vy ĵ + (ωr1x ĵ − ωr1y î)

⃗v2 = vx î + vy ĵ + (ωr2x ĵ − ωr2y î)
⃗v3 = vx î + vy ĵ + (ωr3x ĵ − ωr3y î)
⃗v4 = vx î + vy ĵ + (ωr4x ĵ − ωr4y î)

⃗v1 = vx î + vy ĵ + ωr1x ĵ − ωr1y î

⃗v2 = vx î + vy ĵ + ωr2x ĵ − ωr2y î
⃗v3 = vx î + vy ĵ + ωr3x ĵ − ωr3y î
⃗v4 = vx î + vy ĵ + ωr4x ĵ − ωr4y î

⃗v1 = vx î − ωr1y î + vy ĵ + ωr1x ĵ

⃗v2 = vx î − ωr2y î + vy ĵ + ωr2x ĵ
⃗v3 = vx î − ωr3y î + vy ĵ + ωr3x ĵ
⃗v4 = vx î − ωr4y î + vy ĵ + ωr4x ĵ

⃗v1 = (vx − ωr1y )î + (vy + ωr1x )ĵ

⃗v2 = (vx − ωr2y )î + (vy + ωr2x )ĵ
⃗v3 = (vx − ωr3y )î + (vy + ωr3x )ĵ
⃗v4 = (vx − ωr4y )î + (vy + ωr4x )ĵ

Separate the i-hat components into independent equations.

v1x = vx − ωr1y
v2x = vx − ωr2y
v3x = vx − ωr3y
v4x = vx − ωr4y
144 Chapter 12. Dynamics

Separate the j-hat components into independent equations.

v1y = vy + ωr1x
v2y = vy + ωr2x
v3y = vy + ωr3x
v4y = vy + ωr4x

Now we’ll factor them out into matrices.

   
v1x 1 0 −r1y
v2x  1 0 −r2y 
   
v3x  1 0 −r3y   
    vx
v4x  1 0 −r4y 
 =   vy 
v1y  0 1 r1x 
    ω
v2y  0 1 r2x 
   
v3y  0 1 r3x 
v4y 0 1 r4x

Rearrange the rows so the x and y components are in adjacent rows.

   
v1x 1 0 −r1y
v1y  0 1 r1x 
   
v2x  1 0 −r2y   
   
v2y  0 1 r2x  vx
 =   (12.14)
v3x  1 0 −r3y  vy
   
v3y  0 1 r3x  ω
   
v4x  1 0 −r4y 
v4y 0 1 r4x

To convert from swerve module x and y velocity components to a velocity and heading, use the
Pythagorean theorem and arctangent respectively, as shown below.
q
v = vx2 + vy2 (12.15)
 
vy
θ = tan−1 (12.16)
vx

Forward kinematics
Let M be the 8 × 3 inverse kinematics matrix above. The forward kinematics are
 
v1x
v1y 
 
  v2x 
vx  
 
vy  = M† v2y  (12.17)
v3x 
ω  
v3y 
 
v4x 
v4y

where M† is the pseudoinverse of M.

Hills by northbound freeway between Santa Maria and Ventura

13. 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.

13.1 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 subsection
3.3.1 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.
146 Chapter 13. Model examples

13.1.1 Equations of motion

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

V
ω
Vemf = Kv

Figure 13.1: 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, ω 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.
ω
V = IR + (13.1)
Kv

The mechanical relation for a DC brushed motor is

τ = Kt I (13.2)

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

Substitute this into equation (13.1).

τ ω
V = R+ (13.3)
Kt Kv

13.1.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 13.2 is an example. Data
for the most common motors in FRC can be found at https://motors.vex.com.
Torque constant Kt

τ = Kt I
τ
Kt =
I
τstall
Kt = (13.4)
Istall

where τstall is the stall torque and Istall is the stall current of the motor from its datasheet.
13.1 DC brushed motor 147

Figure 13.2: Example motor datasheet for 775pro

Resistance R
Recall equation (13.1).
ω
V = IR +
Kv
When the motor is stalled, ω = 0.
V = Istall R
V
R= (13.5)
Istall

where Istall is the stall current of the motor and V is the voltage applied to the motor at stall.
Angular velocity constant Kv
Recall equation (13.1).
ω
V = IR +
Kv
ω
V − IR =
Kv
ω
Kv =
V − IR

When the motor is spinning under no load

ωf ree
Kv = (13.6)
V − If ree R

where ω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 ω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.
148 Chapter 13. Model examples

13.1.3 Current limiting

Current limiting of a DC brushed motor reduces the maximum input voltage to avoid exceeding a
current threshold. Predictive current limiting uses a projected estimate of the current, so the voltage
is reduced before the current threshold is exceeded. Reactive current limiting uses an actual current
measurement, so the voltage is reduced after the current threshold is exceeded.
The following pseudocode demonstrates each type of current limiting.
# Normal feedback control
V = K @ (r - x)

# Calculations for predictive current limiting

omega = angular_velocity_measurement
I = V / R - omega / (Kv * R)

# Calculations for reactive current limiting

I = current_measurement
omega = Kv * V - I * R * Kv # or can be angular velocity measurement

# If predicted / actual current above max , limit current by reducing voltage

if I > I_max:
V = I_max * R + omega / Kv

Snippet 13.1. Limits current of DC motor to Imax

13.2 Elevator
13.2.1 Equations of motion
This elevator consists of a DC brushed motor attached to a pulley that drives a mass up or down.

R ωp

I ωm

Vemf
m v
V G
r

circuit mechanics

Figure 13.3: Elevator system diagram

Gear ratios are written as output over input, so G is greater than one in figure 13.3.
We want to derive an equation for the carriage acceleration a (derivative of v) given an input voltage
V , which we can integrate to get carriage velocity and position.
First, we’ll find a torque to substitute into the equation for a DC brushed motor. Based on figure 13.3

τm G = τp (13.7)
13.2 Elevator 149

where G is the gear ratio between the motor and the pulley and τp is the torque produced by the
pulley.
rFm = τp (13.8)

where r is the radius of the pulley. Substitute equation (13.7) into τm in the DC brushed motor
equation (13.3).
τp
G ωm
V = R+
Kt Kv
τp ωm
V = R+
GKt Kv

Substitute in equation (13.8) for τp .

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

The angular velocity of the motor armature ωm is

ωm = Gωp (13.10)

where ωp is the angular velocity of the pulley. The velocity of the mass (the elevator carriage) is
v = rωp
v
ωp = (13.11)
r
Substitute equation (13.11) into equation (13.10).
v
ωm = G (13.12)
r

Substitute equation (13.12) into equation (13.9) for ωm .

rFm Gv
V = R+ r
GKt Kv
RrFm G
V = + v
GKt rKv

Solve for Fm .
RrFm G
=V − v
GKt rKv
 
G GKt
Fm = V − v
rKv Rr
GKt 2
G Kt
Fm = V − v (13.13)
Rr Rr2 Kv
X
F = ma (13.14)
P
where F is the sum of forces applied to the elevator carriage, m is the mass of the elevator carriage
in kilograms, and a is the acceleration of the elevator carriage.
ma = Fm
150 Chapter 13. Model examples

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
ma = V − v
Rr Rr2 Kv
GKt G2 K t
a= V − v (13.15)
Rrm Rr2 mKv

This model will be converted to state-space notation in section 8.1.

13.3 Flywheel
13.3.1 Equations of motion
This flywheel consists of a DC brushed motor attached to a spinning mass of non-negligible moment
of inertia.

R ωf

I ωm J

Vemf
V G

circuit mechanics

Figure 13.4: Flywheel system diagram

Gear ratios are written as output over input, so G is greater than one in figure 13.4.
We want to derive an equation for the flywheel angular acceleration ω̇f given an input voltage V ,
which we can integrate to get flywheel angular velocity.
We will start with the equation derived earlier for a DC brushed motor, equation (13.3).
τ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 .
R 1
V = τm + ωm
Kt Kv
R 1
τm =V − ωm
Kt Kv
13.3 Flywheel 151
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 (13.16)
R Kv R

The torque applied to the flywheel is defined as

τf = J ω̇f (13.17)

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

GKt G2 K t
(J ω̇f ) = V − ωf
R Kv R
GKt G2 K t
ω̇f = V − ωf
RJ Kv RJ
G2 K t GKt
ω̇f = − ωf + V
Kv RJ RJ

We’ll relabel ωf as ω for convenience.

G2 Kt GKt
ω̇ = − ω+ V (13.18)
Kv RJ RJ

This model will be converted to state-space notation in section 8.2.

13.3.2 Calculating constants

Moment of inertia J
Given the simplicity of this mechanism, it may be easier to compute this value theoretically using
material properties in CAD. A procedure for measuring it experimentally is presented below.
First, rearrange equation (13.18) into the form y = mx + b such that J is in the numerator of m.

G2 Kt GKt
ω̇ = − ω+ V
Kv RJ RJ
G2 Kt GKt
J ω̇ = − ω+ V
Kv R R

Multiply by Kv R
G2 Kt
on both sides.

JKv R GKt Kv R
ω̇ = −ω + · 2 V
G2 K t R G Kt
JKv R Kv
ω̇ = −ω + V
G2 K t G
152 Chapter 13. Model examples
JKv R Kv
ω=− 2
ω̇ + V (13.19)
G Kt G

The test procedure is as follows.

1. Run the flywheel forward at a constant voltage. Record the angular velocity over time.
2. Compute the angular acceleration from the angular velocity data as the difference between
each sample divided by the time between them.
3. Perform a linear regression of angular velocity versus angular acceleration. The slope of this
line has the form − JK vR
G2 Kt
as per equation (13.19).
2
4. Multiply the slope by − GKvKRt to obtain a least squares estimate of J.

13.4 Single-jointed arm

13.4.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.

m
R ωarm
l
I ωm

Vemf
V G

circuit mechanics

Figure 13.5: Single-jointed arm system diagram

Gear ratios are written as output over input, so G is greater than one in figure 13.5.
We want to derive an equation for the arm angular acceleration ω̇arm given an input voltage V , which
we can integrate to get arm angular velocity and angle.
We will start with the equation derived earlier for a DC brushed motor, equation (13.3).
τ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 .
R 1
V = τm + ωm
Kt Kv
13.4 Single-jointed arm 153
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ωarm )
G R Kv R
τarm Kt GKt
= V − ωarm
G R Kv R
GKt G2 Kt
τarm = V − ωarm (13.20)
R Kv R

The torque applied to the arm is defined as

τarm = J ω̇arm (13.21)

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

GKt G2 K t
(J ω̇arm ) = V − ωarm
R Kv R
G2 K t GKt
ω̇arm =− ωarm + V
Kv RJ RJ

We’ll relabel ωarm as ω for convenience.

G2 Kt GKt
ω̇ = − ω+ V (13.22)
Kv RJ RJ

This model will be converted to state-space notation in section 8.3.

13.4.2 Calculating constants

Moment of inertia J
Given the simplicity of this mechanism, it may be easier to compute this value theoretically using
material properties in CAD. J can also be approximated as the moment of inertia of a thin rod
rotating around one end. Therefore
1
J = ml2 (13.23)
3

where m is the mass of the arm and l is the length of the arm. Otherwise, a procedure for measuring
it experimentally is presented below.
First, rearrange equation (13.22) into the form y = mx + b such that J is in the numerator of m.

G2 Kt GKt
ω̇ = − ω+ V
Kv RJ RJ
G2 Kt GKt
J ω̇ = − ω+ V
Kv R R
154 Chapter 13. Model examples

Multiply by Kv R
G2 Kt
on both sides.

JKv R GKt Kv R
2
ω̇ = −ω + · 2 V
G Kt R G Kt
JKv R Kv
ω̇ = −ω + V
G2 K t G
JKv R Kv
ω = − 2 ω̇ + V (13.24)
G Kt G

The test procedure is as follows.

1. Orient the arm such that its axis of rotation is aligned with gravity (i.e., the arm is on its side).
This avoids gravity affecting the measurements.
2. Run the arm forward at a constant voltage. Record the angular velocity over time.
3. Compute the angular acceleration from the angular velocity data as the difference between
each sample divided by the time between them.
4. Perform a linear regression of angular velocity versus angular acceleration. The slope of this
line has the form − JK vR
G2 Kt
as per equation (13.24).
2
5. Multiply the slope by − GKvKRt to obtain a least squares estimate of J.

13.5 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.

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

Figure 13.6: Pendulum force diagrams

13.5.1 Force derivation

Consider figure 13.6a, 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
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
13.5 Pendulum 155

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θ (13.25)
ds dθ
v= =l
dt dt
d2 s d2 θ
a= 2 =l 2
dt dt

Therefore
d2 θ
l = −g sin θ
dt2
d2 θ g
2
= − sin θ
dt l
g
θ̈ = − sin θ
l

13.5.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)
156 Chapter 13. Model examples

Again just consider the magnitude of the angular momentum.

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

According to τ = dt ,
dL
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.

13.5.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

p
v = 2gh

Using equation (13.25), this equation can be rewritten in terms of dt .

dθ

dθ p
v=l = 2gh
dt
dθ 2gh
= (13.26)
dt l

where h is the vertical distance the pendulum fell. Look at figure 13.6b, 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
13.6 Differential drive 157

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 (13.26) gives

r
dθ 2g
= (cos θ − cos θ0 )
dt l

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.
r
d dθ d 2g
= (cos θ − cos θ0 )
dt dt dt l
d2 θ 1 − 2gl sin θ dθ
2
= q
dt 2 2g (cos θ − cos θ ) dt
l 0
2g
r
d2 θ 1 − l sin θ 2g
= q (cos θ − cos θ0 )
dt2 2 2g (cos θ − cos θ ) l
l 0

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

which is the same result from force analysis.

13.6 Differential drive

13.6.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 13.7: Differential drive system diagram

We want to derive equations for the accelerations of the left and right sides of the robot v̇l and v̇r
given left and right input voltages Vl and Vr .
From equation (13.16) of the flywheel model derivations

GKt G2 Kt
τ= V − ω (13.27)
R Kv R
158 Chapter 13. Model examples

where τ is the torque applied by one wheel of the differential drive, G is the gear ratio of the differ-
ential drive, 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 drive side
along the ground and r is the drive wheel radius

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

Therefore, for each side of the robot

G2l Kt Gl K t
Fl = − vl + Vl
Kv Rr2 Rr
G2 K t Gr Kt
Fr = − r 2 v r + 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 = Rr .
G r Kt

Fl = C 1 v l + C 2 V l (13.28)
Fr = C 3 v r + C 4 V r (13.29)

First, find the sum of forces.

X
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 (13.30)
m

Next, find the sum of torques.

X
τ = J ω̇
 
v̇r − v̇l
τl + τr = J
2rb

where rb is the radius of the differential drive.

v̇r − v̇l
(−rb Fl ) + (rb Fr ) = J
2rb
13.6 Differential drive 159
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 (13.30) for v̇l 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 r2
v̇r = (Fl + Fr ) + b (−Fl + Fr ) (13.31)
m J
1 1 r2 r2
v̇r = Fl + Fr − b Fl + b Fr
m
 m J J 
r 2 r2
1 1
v̇r = − b Fl + + b Fr (13.32)
m J m J

Substitute equation (13.31) back into equation (13.30) 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 (13.33)
m J m J

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

   
1 rb2 1 rb2
v̇r = − Fl + + Fr
m J m J
   
1 rb2 1 rb2
v̇r = − (C1 vl + C2 Vl ) + + (C3 vr + C4 Vr ) (13.34)
m J m J

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

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

This model will be converted to state-space notation in section 8.5.

160 Chapter 13. Model examples

13.6.2 Calculating constants

Moment of inertia J
We’ll use empirical measurements of linear and angular velocity to determine J. First, we’ll derive
the equation required to perform a linear regression using velocity test data.

τ1 = r × F
τ1 = rma

where τ1 is the torque applied by a drive motor during only linear acceleration, r is the wheel radius,
m is the robot mass, and a is the linear acceleration.

τ2 = Iα

where τ2 is the torque applied by a drive motor during only angular acceleration, I is the moment of
inertia (same as J), and α is the angular acceleration. If a constant voltage is applied during both the
linear and angular acceleration tests, τ1 = τ2 . Therefore,

rma = Iα

Integrate with respect to time.

rmv + C1 = Iω + C2
rmv = Iω + C3
I
v= ω + C3 (13.36)
rm

where v is linear velocity and ω is angular velocity. C1 , C2 , and C3 are arbitrary constants of inte-
gration that won’t be needed. The test procedure is as follows.
1. Run the drivetrain forward at a constant voltage. Record the linear velocity over time using
encoders.
2. Rotate the drivetrain around its center by applying the same voltage as the linear acceleration
test with the motors driving in opposite directions. Record the angular velocity over time using
a gyroscope.
3. Perform a linear regression of linear velocity versus angular velocity. The slope of this line has
the form rm I
as per equation (13.36).
4. Multiply the slope by rm to obtain a least squares estimate of I.
Hills by northbound freeway between Santa Maria and Ventura

14. System identification

For now, this chapter just covers how to obtain state-space models for common mechanisms from a
feedforward model derived from performance data.
162 Chapter 14. System identification

14.1 1 DOF mechanism model

ẋ = Ax + Bu
x = velocity ẋ = acceleration u = voltage

If u = Kv v, then x = v and ẋ = 0. Therefore

0 = Av + B(Kv v) (14.1)

If u = Kv v + Ka a, then x = v and ẋ = a. Therefore

a = Av + B(Kv v + Ka a) (14.2)

Subtract equation (14.1) from equation (14.2).

a = B(Ka a)
1 = BKa
1
B=
Ka

Substitute B back into (14.1) to obtain A.

 
1
0 = Av + (Kv v)
Ka
Kv
0 = Av + v
Ka
Kv
− v = Av
Ka
Kv
A=−
Ka

A model with position and velocity states would be

Theorem 14.1.1 — 1 DOF mechanism position model.

ẋ = Ax + Bu
 
position  
x= u = voltage
velocity
   
0 1 0
ẋ = Kv x + 1 u (14.3)
0 −K a Ka
14.2 Drivetrain velocity model 163

14.2 Drivetrain velocity model

ẋ = Ax + Bu
     
Kv1 v v 0
If u = , then x = and ẋ = .
Kv1 v v 0
       
0 A1 A2 v B1 B2 Kv1 v
= +
0 A2 A1 v B2 B1 Kv1 v

Since the column vectors contain the same element, the elements in the second row can be rearranged.
       
0 A1 A2 v B1 B2 Kv1 v
= +
0 A1 A2 v B1 B2 Kv1 v

Since the rows are linearly dependent, we can use just one of them.
   
0 = A1 A2 v + B1 B2 Kv1 v
 
A1
   A2 
0 = v v Kv1 v Kv1 v  B1 


B2
 
A1
   A2 
0 = 1 1 Kv1 Kv1  B1 


B2

   
  v a
If u = Kv1 v + Ka1 a , then x = and ẋ = .
v a
       
a A1 A2 v B1 B2 Kv1 v + Ka1 a
= +
a A2 A1 v B2 B1 Kv1 v + Ka1 a

Since the column vectors contain the same element, the elements in the second row can be rearranged.
       
a A1 A2 v B1 B2 Kv1 v + Ka1 a
= +
a A1 A2 v B1 B2 Kv1 v + Ka1 a

Since the rows are linearly dependent, we can use just one of them.
    
a = A1 A2 v + B1 B2 Kv1 v + Ka1 a
 
A1
   A2 
a = v v Kv1 v + Ka1 a Kv1 + Ka1 a  B1 


B2

     
−Kv2 v −v 0
If u = , then x = and ẋ = .
Kv2 v v 0
       
0 A1 A2 −v B1 B2 −Kv2 v
= +
0 A2 A1 v B2 B1 Kv2 v
164 Chapter 14. System identification
       
0 −A1 A2 v −B1 B2 Kv2 v
= +
0 −A2 A1 v −B2 B1 Kv2 v
       
0 −A1 A2 v −B1 B2 Kv2 v
= +
0 A1 −A2 v B1 −B2 Kv2 v

Since the column vectors contain the same element, the elements in the second row can be rearranged.
       
0 −A1 A2 v −B1 B2 Kv2 v
= +
0 −A1 A2 v −B1 B2 Kv2 v

Since the rows are linearly dependent, we can use just one of them.
   
0 = −A1 A2 v + −B1 B2 Kv2 v
0 = −vA1 + vA2 − Kv2 vB1 + Kv2 vB2
 
A1
   A2 
0 = −v v −Kv2 v Kv2 v  
 B1 
B2
 
A1
   A2 
0 = −1 1 −Kv2 Kv2  B1 


B2

     
−Kv2 v − Ka2 a −v −a
If u = , then x = and ẋ = .
Kv2 v + Ka2 a v a
       
−a A1 A2 −v B1 B2 −Kv2 v − Ka2 a
= +
a A2 A1 v B2 B1 Kv2 v + Ka2 a
       
−a −A1 A2 v −B1 B2 Kv2 v + Ka2 a
= +
a −A2 A1 v −B2 B1 Kv2 v + Ka2 a
       
−a −A1 A2 v −B1 B2 Kv2 v + Ka2 a
= +
a A1 −A2 v B1 −B2 Kv2 v + Ka2 a

Since the column vectors contain the same element, the elements in the second row can be rearranged.
       
−a −A1 A2 v −B1 B2 Kv2 v + Ka2 a
= +
−a −A1 A2 v −B1 B2 Kv2 v + Ka2 a

Since the rows are linearly dependent, we can use just one of them.
    
−a = −A1 A2 v + −B1 B2 Kv2 v + Ka2 a
−a = −vA1 + vA2 − (Kv2 v + Ka2 a)B1 + Kv2 v + Ka2 a)B2
 
A1
   A2 
−a = −v v −(Kv2 v + Ka2 a) Kv2 v + Ka2 a  
 B1 
B2
14.2 Drivetrain velocity model 165
 
A1
  A2 
a = v −v Kv2 v + Ka2 a −(Kv2 v + Ka2 a)  
B1 
B2

Now stack the rows.

    
0 1 1 Kv1 Kv1 A1
a  v v K v + K a K v + K a   
 = v1 a1 v1 a1  A2 
0 −1 1 −Kv2 Kv2   B1 
a v −v Kv2 v + Ka2 a −(Kv2 v + Ka2 a) B2

Solve for matrix elements with Wolfram Alpha. Let b = Kv1 , c = Ka1 , d = Kv2 , and f = Ka2 .
inverse of {{1, 1, b, b}, {v, v, b v + c a, b v + c a},
{-1, 1, -d, d}, {v, -v, d v + f a, -(d v + f a)}} * {{0}, {a}, {0}, {a}}
   
A1 −cd − bf
A2   
  = 1  cd − bf 
B1  2cf  c + f 
B2 f −c
   
A1 −Ka1 Kv2 − Kv1 Ka2
 A2  1  Ka1 Kv2 − Kv1 Ka2 
 =  
B1  2Ka1 Ka2  Ka1 + Ka2 
B2 Ka2 − Ka1

To summarize,

Theorem 14.2.1 — Drivetrain velocity model.

ẋ = Ax + Bu
   
left velocity left voltage
x= u=
right velocity right voltage
   
A1 A2 B1 B2
ẋ = x+ u
A2 A1 B2 B1
   
A1 −Ka,linear Kv,angular − Kv,linear Ka,angular
 A2  1  Ka,linear Kv,angular − Kv,linear Ka,angular 
 =   (14.4)
B1  2Ka,linear Ka,angular  Ka,linear + Ka,angular 
B2 Ka,angular − Ka,linear

If Kv and Ka are the same for both the linear and angular cases, it devolves to the one-dimensional
case. This means the left and right sides are decoupled.
   
A1 −Ka Kv − Kv Ka
 A2  1  Ka K v − K v Ka 
 =  
B1  2Ka Ka  Ka + Ka 
B2 Ka − Ka
166 Chapter 14. System identification
   
A1 −2Kv Ka
 A2  1  0 
 =  
B1  2Ka Ka  2Ka 
B2 0
   Kv 
A1 − Ka
 A2   0 
 = 1 
 B1   
Ka
B2 0
V
Motion planning

15 Motion profiles . . . . . . . . . . . . . . . . . . . . . 169

15.1 1 DOF motion profiles
15.2 2 DOF motion profiles

16 Trajectory optimization . . . . . . . . . . . . . 173

This page intentionally left blank
OPERS field at UCSC

15. 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.

15.1 1 DOF motion profiles

For one degree of freedom (1 DOF) point-to-point movements in FRC, the most commonly used
profiles are the trapezoidal profile (figure 15.1) and the S-curve profile (figure 15.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 15.1: Trapezoidal profile Figure 15.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
170 Chapter 15. Motion profiles

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
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.

15.1.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 15.1 and 15.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.

15.1.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
15.1 1 DOF motion profiles 171

the overall transfer time. However, the reduced load oscillation at the end of the move considerably
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 modeling 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.

15.1.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 H.4. We’re omitting an S-curve profile derivation
because the continuous acceleration over time that S-curves provide isn’t required for FRC mecha-
nisms.

15.1.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
stepper motors because there is less torque available at higher speeds. However, notice that starting
172 Chapter 15. Motion profiles

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.

15.1.5 Further reading

FRC teams 254 and 971 gave a talk at FIRST World Championships in 2015 about motion profiles
[1].

15.2 2 DOF motion profiles

In FRC, two degree of freedom (2 DOF) point-to-point movements are almost always within the
context of drivetrains where the two degrees of freedom are the x and y axes.
A path is a set of (x, y) points for the drivetrain to follow. A trajectory is a path that includes both
the states (e.g., position and velocity) and control inputs (e.g., voltage) of the drivetrain as functions
of time.
Currently, the most common form of multidimensional trajectory planning in FRC is based on poly-
nomial splines. The splines determine the path of the trajectory, and the path is parameterized by a
trapezoidal motion profile to create a trajectory. A Dive into WPILib Trajectories by Declan Freeman-
Gleason introduces 2 DOF trajectory planning in FRC using polynomial splines [16].
Planning Motion Trajectories for Mobile Robots Using Splines by Christoph Sprunk provides a more
general treatment of spline-based trajectory generation [26].
OPERS field at UCSC

16. Trajectory optimization

To contextualize some definitions related to trajectory optimization, let’s use the example of a driv-
etrain in FRC. A path is a set of (x, y) points for the system to follow. A trajectory is a path that
includes both the states (e.g., position and velocity) and control inputs (e.g., voltage) of the system
as functions of time. Trajectory optimization refers to a set of methods for finding the best choice of
trajectory (for some mathematical definition of “best”) by selecting states and inputs of the system
as functions of time.
Matthew Kelly has an approachable introduction to trajectory optimization on his website [20].
This page intentionally left blank
VI
Appendices
A Simplifying block diagrams . . . . . . . . . 177
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 Laplace domain analysis . . . . . . . . . . . 181

B.1 Projections
B.2 Fourier transform
B.3 Laplace transform
B.4 Laplace transform definition
B.5 Case study: steady-state error
B.6 Case study: flywheel PID control

C Robust control . . . . . . . . . . . . . . . . . . . . . 195

C.1 Gain margin and phase margin

D Installing Python packages . . . . . . . . . 197

D.1 Windows instructions
D.2 Linux instructions

E Linear-quadratic regulator . . . . . . . . . . 199

E.1 Derivation
E.2 Output feedback
E.3 Implicit model following

F QR-weighted linear plant inversion . . 207

F.1 Necessary theorems
F.2 Setup
F.3 Minimization

G Steady-state feedforward . . . . . . . . . . . 211

G.1 Continuous case
G.2 Discrete case
G.3 Deriving steady-state input

H Derivations . . . . . . . . . . . . . . . . . . . . . . . . 215
H.1 Transfer function in feedback
H.2 Zero-order hold for state-space
H.3 Kalman filter as Luenberger observer
H.4 Trapezoidal motion profile

Glossary . . . . . . . . . . . . . . . . . . . . . . . . . . . 220

Bibliography . . . . . . . . . . . . . . . . . . . . . . . 221
Online
Miscellaneous

Index . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 223
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

178 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 P2 Y (s)
±

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 179

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) P2 P1 P 2 Y (s)
∓

Figure A.10: Transformed feedback loop

This page intentionally left blank
Sunset in an airplane over New Mexico

B. Laplace domain analysis

This appendix uses Laplace transforms and transfer functions to analyze properties of control systems
like steady-state error.
These case studies cover various aspects of PID control using the algebraic approach of transfer
functions. For this, we’ll be using equation (B.1), the transfer function for a PID controller.
Ki
K(s) = Kp + + Kd s (B.1)
s

First, we need to define what Laplace transforms and transfer functions are, which is rooted in the
concept of orthogonal projections.

B.1 Projections
Consider a two-dimensional Euclidean space R2 shown in figure B.1 (each R is a dimension whose
domain is the set of real numbers, so R2 is the standard x-y plane).
Ordinarily, we notate points in this plane by their components in the set of basis vectors {î, ĵ}, where
î (pronounced i-hat) is the unit vector in the positive x direction and ĵ is the unit vector in the positive
y direction. Figure B.2 shows an example vector v in this basis.
How do we find the coordinates of v in this basis mathematically? As long as the basis is orthogonal
(i.e., the basis vectors are at right angles to each other), we simply take the orthogonal projection of
v onto î and ĵ. Intuitively, this means finding “the amount of v that points in the direction of î or ĵ”.
Note that a set of orthogonal vectors have a dot product of zero with respect to each other.
More formally, we can calculate projections with the dot product - the projection of v onto any other
vector w is as follows.
v·w
projw v =
|w|

Since î and ĵ are unit vectors, their magnitudes are 1 so the coordinates of v are v · î and v · ĵ.
182 Appendix B. Laplace domain analysis

Figure B.1: Euclidean space R2

2
v = 2î + 1.5ĵ
1.5
ĵ
x
î

Figure B.2: v with basis set {î, ĵ}

We can use this same process to find the coordinates of v in any orthogonal basis. For example,
imagine the basis {î + ĵ, î − ĵ} - the coordinates in this basis are given by v·(√î+2 ĵ) and v·(√î−2 ĵ) . Let’s
“unwrap” the formula for dot product and look a bit more closely.
n
v · (î + ĵ) 1 X
√ =√ vi (î + ĵ)i
2 2 i=0

where the subscript i denotes which component of each vector and n is the total number of compo-
nents. To change coordinates, we expanded both v and î + ĵ in a basis, multiplied their components,
and added them up. Here’s a more concrete example. Let v = 2î + 1.5ĵ from figure B.2. First, we’ll
project v onto the î + ĵ basis vector.
v · (î + ĵ) 1
√ = √ (2î · î + 1.5ĵ · ĵ)
2 2
v · (î + ĵ) 1
√ = √ (2 + 1.5)
2 2
v · (î + ĵ) 3.5
√ =√
2 2
B.1 Projections 183
√
v · (î + ĵ) 3.5 2
√ =
2 2
v · (î + ĵ) √
√ = 1.75 2
2

Next, we’ll project v onto the î − ĵ basis vector.

v · (î − ĵ) 1
√ = √ (2î · î − 1.5ĵ · ĵ)
2 2
v · (î − ĵ) 1
√ = √ (2 − 1.5)
2 2
v · (î − ĵ) 0.5
√ =√
2 2
√
v · (î − ĵ) 0.5 2
√ =
2 2
v · (î − ĵ) √
√ = 0.25 2
2

Figure B.3 shows this result geometrically with respect to the basis {î + ĵ, î − ĵ}.

√
0.25 2
v
î + ĵ √
1.75 2
x
î − ĵ

Figure B.3: v with basis {î + ĵ, î − ĵ}

The previous example was only a change of coordinates in a finite-dimensional vector space. However,
as we will see, the core idea does not change much when we move to more complicated structures.
Observe the formula for the Fourier transform.
Z ∞
ˆ
f (ξ) = f (x)e−2πixξ dx where ξ ∈ R
−∞

This is fundamentally the same formula we had before. f (x) has taken the place of vn , e−2πixξ has
taken the place of (î + ĵ)i , and the sum over i has turned into an integral over dx, but the underlying
concept is the same. To change coordinates in a function space, we simply take the orthogonal pro-
jection onto our new basis functions. In the case of the Fourier transform, the function basis is the
184 Appendix B. Laplace domain analysis

family of functions of the form f (x) = e−2πixξ for ξ ∈ R. Since these functions are oscillatory at a
frequency determined by ξ, we can think of this as a “frequency basis”.

R Watch the “Abstract vector spaces” video from 3Blue1Brown’s Essence of linear algebra series
(17 minutes) [2] for a more geometric introduction to using functions as a basis.

Now, the Laplace transform is somewhat more complicated - as it turns out, the Fourier basis is
orthogonal, so the analogy to the simpler vector space holds almost-precisely. The Laplace transform
is not orthogonal, so we can’t interpret it strictly as a change of coordinates in the traditional sense.
However, the intuition is the same: we are taking the orthogonal projection of our original function
onto the functions of our new basis set.
Z ∞
F (s) = f (t)e−st dt, where s ∈ C
0

Here, it becomes obvious that the Laplace transform is a generalization of the Fourier transform in
that the basis family is strictly larger (we have allowed the “frequency” parameter to take complex
values, as opposed to merely real values). As a result, the Laplace basis contains functions that grow
and decay, while the Fourier basis does not.

B.2 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 B.4.
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 B.5).

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.
B.2 Fourier transform 185

Figure B.4: Frequency decomposition of Fmajor4 chord

Figure B.5: Fourier transform of Fmajor4 chord

186 Appendix B. Laplace domain analysis

B.3 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 σ corresponds
to the x-axis and the imaginary part jω corresponds to the y-axis (see figure B.6).

Im(jω)

Re(σ)

Figure B.6: 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 3.2 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 B.7).
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 B.8
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
x +y2
B.3 Laplace transform 187

Figure B.7: Fmajor4 chord at σ = 0 and σ = −25

Figure B.8: Laplace transform of Fmajor4 chord plotted in 3D

188 Appendix B. Laplace domain analysis

B.4 Laplace transform definition

The Laplace transform of a function f (t) is defined as
Z ∞
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)
are shown in table B.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 .

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
1
Exponential decay e−αt u(t) s+α

Table B.1: Common Laplace transforms and Laplace transform properties with zero initial conditions

B.5 Case study: 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) = = (B.2)
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

2
The unit step u(t) is defined as 0 for t < 0 and 1 for t ≥ 0.
B.5 Case study: steady-state error 189
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) (B.3)

s→0

since steady-state has an input frequency of zero.

ess = lim sE(s)

s→0
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
(B.4)
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 + Ksi 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
190 Appendix B. Laplace domain analysis
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.
It should be noted that ess in equation (B.4) 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.

B.6 Case study: 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 + (B.5)
Kv
τ = IKt (B.6)
dω
τ =J (B.7)
dt

For an explanation of where these equations come from, read section 13.1.
First, we’ll solve for dω
dt in terms of V .
Substitute equation (B.6) into equation (B.5).
ω
V = IR +
K
  v
τ ω
V = R+
Kt Kv

Substitute in equation (B.7).

J dω
dt ω
V = R+
Kt Kv
B.6 Case study: flywheel PID control 191

Solve for dt .
dω

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. Because the Laplace transform is a linear operator, we can take the
Laplace transform of each term individually. Based on table B.1, dω
dt becomes sω and ω(t) and V (t)
become ω(s) and V (s) respectively (the parenthetical notation has been dropped for clarity).
Kt Kt
sω = − ω+ V (B.8)
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 (B.8).

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

Solve for the transfer function H(s) = ωgoal .

ω

Kt K t Kp
sω = − ω+ (ωgoal − ω)
JRKv JR
Kt K t Kp Kt Kp
sω = − ω+ ωgoal − ω
JRKv JR JR
 
Kt Kt Kp Kt Kp
s+ + ω= ωgoal
JRKv JR JR
Kt Kp
ω JR
= 
ωgoal s+ Kt
+
Kt K p
JRKv JR
192 Appendix B. Laplace domain analysis

 
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 (B.8).

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 K p Kt Kd s
sω + ω+ ω+ ω= ωgoal + ωgoal
JR JRKv JR JR JR
   
Kt Kd Kt Kt K p 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
+ JRKv + JR
JR

Kt Kt Kp
K +
So, we added a zero at − Kpd 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 in return. Zeroes
should be avoided if at all possible because they amplify unwanted high frequency modes of the
system and are noisier the faster the system is sampled. At least this is a stable zero, but it’s still
undesirable.
In summary, derivative doesn’t help on an ideal flywheel. Kd may compensate for unmodeled dy-
namics such as accelerating projectiles slowing the flywheel down, but that effect may also increase
B.6 Case study: flywheel PID control 193

recovery time; Kd drives the acceleration to zero in the undesired case of negative acceleration as
well as well as the actually desired case of positive acceleration.
We’ll cover a superior compensation method much later in subsection 5.11.2 that avoids zeroes in the
controller, doesn’t act against the desired control action, and facilitates better tracking.
This page intentionally left blank
Sunset in an airplane over New Mexico

C. Robust control

C.1 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 them for controller design, we’ll just cover what gain and phase margin are in a general sense
and how they are used.
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
[15]. He has other videos too on classical control methods like Bode and Nyquist plots that we
recommend.
This page intentionally left blank
Sunset in an airplane over New Mexico

D. Installing Python packages

D.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.

D.2 Linux instructions

To install Python, install the appropriate packages from table D.1 using your system’s package man-
ager.

Debian/Ubuntu Arch Linux

python3 python
python3-pip python-pip

Table D.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

E. Linear-quadratic regulator

This appendix will go into more detail on the linear-quadratic regulator’s derivation and interesting
applications.

E.1 Derivation
Let a continuous time linear system be defined as

ẋ = Ax + Bu (E.1)

with the cost function

Z∞  T   
x Q N x
J= dt
u NT R u
0

where J represents a trade-off between state excursion and control effort with the weighting factors
Q, R, and N. Q is the weight matrix for error, R is the weight matrix for control effort, and N is
a cross weight matrix between error and control effort. N is commonly utilized when penalizing the
output in addition to the state and input.
Z∞  T  
x Qx + Nu
J= dt
u NT x + Ru
0
Z∞  
  Qx + Nu
J= xT uT dt
NT x + Ru
0
Z∞



J= xT (Qx + Nu) + uT NT x + Ru dt
0
200 Appendix E. Linear-quadratic regulator
Z∞


J= xT Qx + xT Nu + uT NT x + uT Ru dt
0
Z∞  
T T T

T T
J= x Qx + x Nu + x Nu + u Ru dt
0
Z∞


J= xT Qx + 2xT Nu + uT Ru dt
0
Z∞


J= xT Qx + uT Ru + 2xT Nu dt
0

The feedback control law which minimizes J subject to the constraint ẋ = Ax + Bu is

u = −Kx

where K is given by

K = R−1 BT S + NT

and S is found by solving the continuous time algebraic Riccati equation defined as


AT S + SA − (SB + N) R−1 BT S + NT + Q = 0

or alternatively
AT S + SA − SBR−1 BT S + Q = 0

with
A = A − BR−1 NT
Q = Q − NR−1 NT

If there is no cross-correlation between error and control effort, N is a zero matrix and the cost
function simplifies to
Z∞


J= xT Qx + uT Ru dt
0

The feedback control law which minimizes this J subject to ẋ = Ax + Bu is

u = −Kx

where K is given by
K = R−1 BT S

and S is found by solving the continuous time algebraic Riccati equation defined as
AT S + SA − SBR−1 BT S + Q = 0

The discrete time LQR controller is computed via a slightly different cost function, constraint, and
resulting algebraic Riccati equation. Snippet E.1 computes the optimal infinite horizon, discrete time
LQR controller.
E.1 Derivation 201

import control as ct
import numpy as np
import scipy as sp

def lqr (*args , ** kwargs ):

""" Solves for the optimal linear - quadratic regulator (LQR).

For a continuous system :

.. math :: xdot = A * x + B * u
.. math :: J = \\ int_0 ^\\ infty (x^T Q x + u^T R u + 2 x^T N u) dt

For a discrete system :

.. math :: x(n + 1) = A x(n) + B u(n)

.. math :: J = \\ sum_0 ^\\ infty (x^T Q x + u^T R u + 2 x^T N u) \\ Delta T

Keyword arguments :
sys -- StateSpace object representing a linear system .
Q -- numpy.array( states x states ), state cost matrix .
R -- numpy.array( inputs x inputs ), control effort cost matrix .
N -- numpy.array( states x inputs ), cross weight matrix .

Returns :
K -- numpy.array( states x inputs ), controller gain matrix .
"""
sys = args [0]
Q = args [1]
R = args [2]
if len(args) == 4:
N = args [3]
else:
N = np.zeros (( sys.A.shape [0], sys.B.shape [1]))

m = sys.A.shape [0]

controllability_rank = np. linalg . matrix_rank (ct.ctrb(sys.A, sys.B))

if controllability_rank != m:
print(
" Warning : Controllability of %d != %d, uncontrollable state"
% ( controllability_rank , m)
)

if sys.dt == None:
P = sp. linalg . solve_continuous_are (a=sys.A, b=sys.B, q=Q, r=R, s=N)
return np. linalg .solve(R, sys.B.T @ P + N.T)
else:
P = sp. linalg . solve_discrete_are (a=sys.A, b=sys.B, q=Q, r=R, s=N)
return np. linalg .solve(R + sys.B.T @ P @ sys.B, sys.B.T @ P @ sys.A +
N.T)

Snippet E.1. Infinite horizon, discrete time LQR computation in Python

Other formulations of LQR for finite horizon and discrete time can be seen on Wikipedia [14].
MIT OpenCourseWare has a rigorous proof of the results shown above [30].
202 Appendix E. Linear-quadratic regulator

E.2 Output feedback

LQR is normally used for state feedback on

ẋ = Ax + Bu
y = Cx + Du

with the cost functional Z ∞

J= xT Qx + uT Ru dt
0

However, we may not know how to select costs for some of the states, or we don’t care what certain
internal states are doing. Output feedback can accomodate this. Not only can we make our out-
put contain a subset of states, but we can use any other cost metric we can think of as long as it’s
representable as a linear combination of the states and inputs1 .
For output feedback, we want to minimize the following cost functional.
Z ∞


J= yT Qy + uT Ru dt
0

Substitute in the expression for y.

Z ∞


J= (Cx + Du)T Q(Cx + Du) + uT Ru dt
0

Apply the transpose to the left-hand side of the Q term.

Z ∞


J= (xT CT + uT DT )Q(Cx + Du) + uT Ru dt
0

 T  
x x
Factor out from the left side and from the right side of each term.
u u
Z ∞  T      T    !
x CT   x x 0 0 x
J= Q C D + dt
0 u DT u u 0 R u
Z ∞  T  T     !
x C   0 0 x
J= T Q C D + dt
0 u D 0 R u

Multiply in Q.
Z ∞  T  T     !
x C Q   0 0 x
J= C D + dt
0 u DT Q 0 R u

Multiply matrices in the left term together.

Z ∞  T  T     !
x C QC CT QD 0 0 x
J= + dt
0 u DT QC DT QD 0 R u
1
We’ll see this later on in section E.3 when we define the cost metric as deviation from the behavior of another model.
E.3 Implicit model following 203

Add the terms together.

 
CT QC CT QD
Z ∞  T | {z }   
x | {z }
J=  Q N  x (E.2)
u DT QC DT QD + R u dt
0
| {z } | {z }
NT R

Thus, output feedback can be defined as the following optimization problem.

Theorem E.2.1 — Linear-quadratic regulator with output feedback.

 
CT QC CT QD
Z ∞  T | {z }   
x | {z }
min  Q N  x
u u DT QC DT QD + R u dt
0
| {z } | {z }
NT R
subject to ẋ = Ax + Bu (E.3)

The optimal control policy u∗ is K(r − y) where r is the desired output and y is the measured
output defined as y = Cx + Du. K can be computed via the typical LQR equations based on
the algebraic Ricatti equation.

If the output is only the whole state vector, then C = I, D = 0, and the cost functional simplifies to
that of state feedback LQR.
Z ∞  T   
x Q 0 x
J= dt
0 u 0 R u

E.3 Implicit model following

If we want to design a feedback controller that erases the dynamics of our system and makes it behave
like some other system, we can use implicit model following. This is used on the Blackhawk helicopter
at NASA Ames research center when they want to make it fly like experimental aircraft (within the
limits of the helicopter’s actuators, of course).
Let the original system dynamics be

ẋ = Ax + Bu
y = Cx

and the desired system dynamics be

ż = Aref z

ẏ = Cẋ
ẏ = C(Ax + Bu)
ẏ = CAx + CBu

We want to minimize the following cost functional.

Z ∞


J= (ẏ − ż)T Q(ẏ − ż) + uT Ru dt
0
204 Appendix E. Linear-quadratic regulator

We’ll be measuring the desired system’s state, so let y = z.

ż = Aref y
ż = Aref Cx

Therefore,

ẏ − ż = CAx + CBu − (Aref Cx)

ẏ − ż = (CA − Aref C)x + CBu

Substitute this into the cost functional.

Z ∞


J= (ẏ − ż)T Q(ẏ − ż) + uT Ru dt
Z0 ∞


J= ((CA − Aref C)x + CBu)T Q((CA − Aref C)x + CBu) + uT Ru dt
0

Apply the transpose to the left-hand side of the Q term.

Z ∞


J= (xT (CA − Aref C)T + uT (CB)T )Q((CA − Aref C)x + CBu) + uT Ru dt
0

 T  
x x
Factor out from the left side and from the right side of each term.
u u
Z ∞  T      T    !
x (CA − Aref C)T   x x 0 0 x
J= Q CA − Aref C CB + dt
0 u (CB)T u u 0 R u
Z ∞  T      !
x (CA − Aref C)T   0 0 x
J= Q CA − Aref C CB + dt
0 u (CB)T 0 R u

Multiply in Q.
Z ∞  T      !
x (CA − Aref C)T Q   0 0 x
J= CA − Aref C CB + dt
0 u (CB)T Q 0 R u

Multiply matrices in the left term together.

Z ∞  T      !
x (CA − Aref C)T Q(CA − Aref C) (CA − Aref C)T Q(CB) 0 0 x
J= T T + dt
0 u (CB) Q(CA − Aref C) (CB) Q(CB) 0 R u

Add the terms together.

 
(CA − Aref C)T Q(CA − Aref C) (CA − Aref C)T Q(CB)
Z ∞  T | {z } | {z }  
x  Q N  x
J=  T T  dt
0 u  (CB) Q(CA − Aref C) (CB) Q(CB) + R  u
| {z } | {z }
NT R
(E.4)
Thus, implicit model following can be defined as the following optimization problem.
E.3 Implicit model following 205

Theorem E.3.1 — Implicit model following.

 
(CA − Aref C)T Q(CA − Aref C) (CA − Aref C)T Q(CB)
Z ∞  T | {z } | {z }  
x  Q N  x
min  T T  dt
u 0 u  (CB) Q(CA − A ref C) (CB) Q(CB) + R  u
| {z } | {z }
NT R

subject to ẋ = Ax + Bu (E.5)

The optimal control policy u∗ is −Kx. K can be computed via the typical LQR equations based
on the algebraic Ricatti equation.

The control law uimf = −Kx makes ẋ = Ax + Buimf match the open-loop response of ż =
Aref z.
If the original and desired system have the same states, then C = I and the cost functional simplifies
to  
(A − Aref )T Q(A − Aref ) (A − Aref )T QB
Z ∞  T | {z } | {z }  
x  Q N  x
J=  T T  dt (E.6)
0 u  B Q(A − A ref ) B QB + R  u
| {z } | {z }
NT R
This page intentionally left blank
Sunset in an airplane over New Mexico

F. QR-weighted linear plant inversion

F.1 Necessary theorems

The following theorem and corollary will be needed to derive the QR-weighted linear plant inversion
equation.

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

∂(Ax+b)T C(Ax+b)
Corollary F.1.2 ∂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
208 Appendix F. QR-weighted linear plant inversion

F.2 Setup
Let’s start with the equation for the reference dynamics
rk+1 = Ark + Buk

where uk 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 uk such that we minimize the tracking
error between rk+1 and rk .
rk+1 − Ark = Buk

To solve for uk , 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 = Buk − (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 = (Buk − (rk+1 − Ark ))T Q(Buk − (rk+1 − Ark )) + uTk Ruk

F.3 Minimization
Given theorem 5.10.1 and corollary F.1.2, find the minimum of J by taking the partial derivative
with respect to uk and setting the result to 0.
∂J
= 2BT Q(Buk − (rk+1 − Ark )) + 2Ruk
∂uk
0 = 2BT Q(Buk − (rk+1 − Ark )) + 2Ruk
0 = BT Q(Buk − (rk+1 − Ark )) + Ruk
0 = BT QBuk − BT Q(rk+1 − Ark ) + Ruk
BT QBuk + Ruk = BT Q(rk+1 − Ark )
(BT QB + R)uk = BT Q(rk+1 − Ark )
uk = (BT QB + R)−1 BT Q(rk+1 − Ark )

Theorem F.3.1 — QR-weighted linear plant inversion. Given the discrete model xk+1 =
Axk + Buk , the plant inversion feedforward is

uk = Kf f (rk+1 − Ark )

where Kf f = (BT QB + R)−1 BT Q, rk+1 is the reference at the next timestep, and rk is the
reference at the current timestep.

Figure F.1 shows plant inversion applied to a second-order CIM motor model.
Plant inversion isn’t as effective with both Q and R cost because the R matrix penalized control
effort. The reference tracking with no cost matrices is much better.
F.3 Minimization 209

Figure F.1: Second-order CIM motor response with plant inversion

This page intentionally left blank
Sunset in an airplane over New Mexico

G. 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 (G.1)

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
uss = Nu yss (G.2)

G.1 Continuous case

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

ẋ = Ax + Bu
y = Cx + Du

0 = Axss + Buss
yss = Cxss + Duss

0 = ANx yss + BNu yss

212 Appendix G. Steady-state feedforward

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.

G.2 Discrete case

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

xk+1 = Axk + Buk

yk = Cxk + Duk

xss = Axss + Buss

yss = Cxss + Duss

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.

G.3 Deriving steady-state input 213

G.3 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 (G.1).

xss = Nx yss
N†x xss = yss

Now substitute this into equation (G.2).

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 G.3.1.

Theorem G.3.1 — Steady-state feedforward.

Continuous:
   †  
Nx A B 0
= (G.3)
Nu C D 1
Discrete:
   †  
Nx A−I B 0
= (G.4)
Nu C D 1

uf f = Nu N†x r (G.5)

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 G.3.1 is square (number of inputs = number of outputs), the
normal matrix inverse can be used instead.

G.3.1 Case study: 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. Plant inversion doesn’t support this, but can be
used for reference tracking as well with the same tuning parameters as LQR design. Figure G.1
shows both types of feedforwards applied to a second-order CIM motor model.
214 Appendix G. Steady-state feedforward

Figure G.1: Second-order CIM motor response Figure G.2: Second-order CIM motor response
with various feedforwards with plant inversions

Plant inversion isn’t as effective in figure G.1 because the R matrix penalized control effort. If the
R cost matrix is removed from the plant inversion calculation, the reference tracking is much better
(see figure G.2).
Sunset in an airplane over New Mexico

H. Derivations

H.1 Transfer function in feedback

Given the feedback network in figure H.1, find an expression for Y (s).

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

H(s)

Figure H.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)
= (H.1)
X(s) 1 + G(s)H(s)

A more general form is

Y (s) G(s)
= (H.2)
X(s) 1 ∓ G(s)H(s)

where positive feedback uses the top sign and negative feedback uses the bottom sign.
216 Appendix H. Derivations

H.2 Zero-order hold for state-space

Starting with the continuous model

ẋ(t) = Ax(t) + Bu(t)

by premultiplying the model by e−At , we get

e−At ẋ(t) = e−At Ax(t) + e−At Bu(t)

e−At ẋ(t) − e−At Ax(t) = e−At Bu(t)

The derivative of the matrix exponential is

d At
e = AeAt = eAt A
dt

so we recognize the previous equation as

d −At

e x(t) = e−At Bu(t)
dt

By integrating this equation, we get

Z t
e−At x(t) − e0 x(0) = e−Aτ Bu(τ ) dτ
0
Z t
At
x(t) = e 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 )
Z kT
xk = eAkT x(0) + eA(kT −τ ) Bu(τ ) dτ
0
Z (k+1)T
xk+1 = e A(k+1)T
x(0) + eA((k+1)T −τ ) Bu(τ ) dτ
0
Z kT Z (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
Z kT Z (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
 Z kT  Z (k+1)T
A(kT −τ )
xk+1 = eAT e AkT
x(0) + e 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.
Z !
(k+1)T
xk+1 = eAT xk + eA(kT +T −τ ) dτ Buk
kT
H.3 Kalman filter as Luenberger observer 217

The second term can be simplified by substituting it with the function v(τ ) = kT + T − τ . Note
that dτ = −dv.
Z v((k+1)T ) !
xk+1 = eAT xk − eAv dv Buk
v(kT )
Z 0 
AT Av
xk+1 = e xk − e dv Buk
T
Z T 
xk+1 = eAT xk + Av
e dv Buk
0
AT −1 Av T
xk+1 = e xk + A e |0 Buk
AT −1 AT A0
xk+1 = e xk + A (e −e )Buk
xk+1 = eAT xk + A−1 (eAT − I)Buk

which is an exact solution to the discretization problem.

H.3 Kalman filter as Luenberger observer

A Luenberger observer is defined as

x̂+ −
k+1 = Ax̂k + Buk + L(yk − ŷk ) (H.3)
ŷk = Cx̂−
k (H.4)

where a superscript of minus denotes a priori and plus denotes a posteriori estimate. Combining
equation (H.3) and equation (H.4) gives

x̂+ − −
k+1 = Ax̂k + Buk + L(yk − Cx̂k ) (H.5)

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
K k = P− T − T
k C (CPk C + R)
−1
(H.6)
x̂+
k = x̂−
k + Kk (yk − Cx̂−
k) (H.7)
P+k = (I − Kk C)P−
k (H.8)
Predict step
x̂+ +
k+1 = Ax̂k + Buk (H.9)
P−
k+1 = AP+
kA
T
+ ΓQΓ T
(H.10)

Substitute equation (H.7) into equation (H.9).

x̂+ − −
k+1 = A(x̂k + Kk (yk − Cx̂k )) + Buk
x̂+ − −
k+1 = Axk + AKk (yk − Cx̂k ) + Buk
x̂+ − −
k+1 = Ax̂k + Buk + AKk (yk − Cx̂k )

Let L = AKk .
x̂+ − −
k+1 = Ax̂k + Buk + L(yk − Cx̂k ) (H.11)
218 Appendix H. Derivations

which matches equation (H.5). Therefore, the eigenvalues of the Kalman filter observer can be ob-
tained by

eig(A − LC)
eig(A − (AKk )(C))
eig(A(I − Kk C)) (H.12)

H.3.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 H.3 shows that L = AKk . Since L and A are constant, one must assume Kk has reached
steady-state. Then, K = A−1 L. Substitute this into the Kalman filter update equation.

x̂+ − −
k+1 = x̂k+1 + K(yk+1 − Cx̂k+1 )
x̂+ − −1 −
k+1 = x̂k+1 + A L(yk+1 − Cx̂k+1 )

Substitute in equation (9.4).

x̂+ − −1
k+1 = x̂k+1 + A L(yk+1 − ŷk+1 )

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 (H.13)
Update step
x̂+ − −1
k+1 = x̂k+1 + A L(yk+1 − ŷk+1 ) (H.14)
ŷk+1 = Cx̂−
k+1 (H.15)

H.4 Trapezoidal 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 Applies an input to 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 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.
feedback gain The gain from the output to an earlier point in a control system diagram.
feedforward controller A controller that injects information about the system’s dynamics (like a
model does) or the desired movement. The feedforward handles parts of the control actions
we already know must be applied to make a system track a reference, then the feedback
controller compensates for what we do not or cannot know about the system’s behavior at
runtime.
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.
220 Glossary

gain margin See section C.1 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.
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 estimates 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 C.1 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 plant in the context of PID control.
realization In control theory, this is an implementation of a given input-output behavior as a state-
space model.
reference The desired state. This value is used as the reference point for a controller’s error calcula-
tion.
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 a constant greater than 0 for t ≥ 0. A step input
that is 1 for t ≥ 0 is called a unit step input.
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
[14] Wikipedia Commons. Linear-quadratic regulator. URL: https : / / en . wikipedia . org / wiki /
Linear%E2%80%93quadratic_regulator (visited on 03/24/2018) (cited on page 201).
[16] Declan Freeman-Gleason. A Dive into WPILib Trajectories. 2020. URL: https://pietroglyph.
github.io/trajectory-presentation/ (cited on page 172).
[17] Sean Humbert. Why do we have to linearize around an equilibrium point? URL: https://www.
cds.caltech.edu/%7Emurray/courses/cds101/fa02/faq/02-10-09_linearization.html (visited
on 07/12/2018) (cited on page 58).
[18] Simon J. Julier and Jeffrey K. Uhlmann. A New Extension of the Kalman Filter to Nonlinear
Systems. URL: https://www.cs.unc.edu/~welch/kalman/media/pdf/Julier1997_SPIE_KF.pdf
(visited on 09/29/2019) (cited on page 121).
[19] Kapitanyuk, Proskurnikov, and Cao. A guiding vector field algorithm for path following control
of nonholonomic mobile robots. URL: https : / / arxiv . org / pdf / 1610 . 04391 . pdf (visited on
08/09/2018) (cited on page 60).
[20] Matthew Kelly. An Introduction to Trajectory Optimization: How to Do Your Own Direct Collo-
cation. URL: https://www.matthewpeterkelly.com/research/MatthewKelly_IntroTrajectoryOptimization_
SIAM_Review_2017.pdf (visited on 08/27/2019) (cited on page 173).
[21] Edgar Kraft. A Quaternion-based Unscented Kalman Filter for Orientation Tracking. URL: https:
//kodlab.seas.upenn.edu/uploads/Arun/UKFpaper.pdf (visited on 07/11/2018) (cited on
page 121).
[22] Charles F. Van Loan. Computing Integrals Involving the Matrix Exponential. URL: https://www.
cs.cornell.edu/cv/ResearchPDF/computing.integrals.involving.Matrix.Exp.pdf (visited on
06/21/2018) (cited on page 55).
[23] Andrew W. Long et al. The Banana Distribution is Gaussian: A Localization Study with Expo-
nential Coordinates. 2008. URL: https://rpk.lcsr.jhu.edu/wp-content/uploads/2014/08/p34_
Long12_The-Banana-Distribution.pdf (cited on page 93).
222 Bibliography

[24] Luca, Oriolo, and Vendittelli. Control of Wheeled Mobile Robots: An Experimental Overview.
URL: https : / / www . dis . uniroma1 . it / ~labrob / pub / papers / Ramsete01 . pdf (visited on
08/09/2018) (cited on pages 82, 83).
[25] MIT OpenCourseWare. Linear Quadratic Regulator. URL: 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 35).
[26] Christoph Sprunk. Planning Motion Trajectories for Mobile Robots Using Splines. 2008. URL:
http://www2.informatik.uni-freiburg.de/~lau/students/Sprunk2008.pdf (cited on page 172).
[29] Russ Tedrake. Underactuated Robotics. 2019. URL: http://underactuated.mit.edu/underactuated.
html (cited on page 60).
[30] Russ Tedrake. Chapter 9. Linear Quadratic Regulators. URL: http://underactuated.mit.edu/lqr.
html (visited on 03/22/2020) (cited on page 201).
[31] Eric A. Wan and Rudolph van der Merwe. The Unscented Kalman Filter for Nonlinear Esti-
mation. URL: https://www.seas.harvard.edu/courses/cs281/papers/unscented.pdf (visited on
06/26/2018) (cited on page 121).

Misc
[1] FRC team 254. Motion Planning and Control in FRC. 2015. URL: https://www.youtube.com/
watch?v=8319J1BEHwM (cited on page 172).
[2] 3Blue1Brown. Abstract vector spaces. 2016. URL: https : / / www . youtube . com / watch ? v =
TgKwz5Ikpc8 (cited on page 184).
[3] 3Blue1Brown. Eigenvectors and eigenvalues. 2016. URL: https://www.youtube.com/watch?v=
PFDu9oVAE-g (cited on page 28).
[4] 3Blue1Brown. Essence of linear algebra preview. 2016. URL: https : / / www . youtube . com /
watch?v=kjBOesZCoqc (cited on page 27).
[5] 3Blue1Brown. Inverse matrices, column space, and null space. 2016. URL: https : / / www .
youtube.com/watch?v=uQhTuRlWMxw (cited on page 28).
[6] 3Blue1Brown. Linear combinations, span, and basis vectors. 2016. URL: https://www.youtube.
com/watch?v=k7RM-ot2NWY (cited on page 27).
[7] 3Blue1Brown. Linear transformations and matrices. 2016. URL: https://www.youtube.com/
watch?v=kYB8IZa5AuE (cited on page 27).
[8] 3Blue1Brown. Matrix multiplication as composition. 2016. URL: https://www.youtube.com/
watch?v=XkY2DOUCWMU (cited on page 27).
[9] 3Blue1Brown. Nonsquare matrices as transformations between dimensions. 2016. URL: https:
//www.youtube.com/watch?v=v8VSDg_WQlA (cited on page 28).
[10] 3Blue1Brown. The determinant. 2016. URL: https://www.youtube.com/watch?v=Ip3X9LOh2dk
(cited on page 28).
[11] 3Blue1Brown. Vectors, what even are they? 2016. URL: https://www.youtube.com/watch?v=
fNk_zzaMoSs (cited on page 27).
[12] 3Blue1Brown. Essence of calculus. 2017. URL: https : / / www . youtube . com / playlist ? list =
PLZHQObOWTQDMsr9K-rj53DwVRMYO3t5Yr (cited on page 131).
[13] 3Blue1Brown. Taylor series. 2017. URL: https://www.youtube.com/watch?v=3d6DsjIBzJ4
(cited on page 53).
[15] Brian Douglas. Gain and Phase Margins Explained! 2015. URL: https://www.youtube.com/
watch?v=ThoA4amCAX4 (cited on page 195).
[27] Zach Star. What does the Laplace Transform really tell us? A visual explanation (plus applica-
tions). 2019. URL: https://www.youtube.com/watch?v=n2y7n6jw5d0 (cited on page 19).
[28] Russ Tedrake. 6.832 Underactuated Robotics. 2019. URL: https://www.youtube.com/channel/
UChfUOAhz7ynELF-s_1LPpWg/videos (cited on page 60).
Sunset near Porter Academic building at UCSC

Index

block diagrams, 8 FRC models

simplification, 177 DC brushed motor equations, 146
differential drive equations, 70
controller design elevator equations, 61
actuator saturation, 17 flywheel equations, 63
controllability, 32 single-jointed arm equations, 66
linear time-varying control, 72
linear-quadratic regulator, 34 gain, 8
Bryson’s rule, 35
definition, 35 integral control
implicit model following, 203 input error estimation, 43
output feedback, 202 plant augmentation, 43
observability, 32
linear algebra
pole placement, 34
basis vectors, 27
digital signal processing linear combination, 27
aliasing, 47 vectors, 27
Nyquist frequency, 47
discretization, 48 matrices
backward Euler method, 48 determinant, 28
bilinear transform, 48 eigenvalues, 28
forward Euler method, 48 inverse, 28
matrix exponential, 52 linear systems, 28
Taylor series, 53 linear transformation, 27
zero-order hold, 54 multiplication, 27
pseudoinverse, 28
feedforward rank, 28
linear plant inversion, 41 transpose, 28
steady-state feedforward, 213 model augmentation
224 Index

of controller, 37 covariance matrix, 101

of observer, 37 expected value, 97
of output, 38 marginal probability density functions,
of plant, 37 100
motion profiles probability density function, 96
1 DOF, 169 probability density functions, 98
2 DOF, 172 random variables, 96
S-curve, 170 variance, 97
trajectory optimization, 173
trapezoidal, 170 stability
eigenvalues, 34, 95
nonlinear control gain margin, 195
extended Kalman filter, 120 phase margin, 195
linear time-varying control, 72 poles and zeroes, 20
linearization, 57, 69 root locus, 22
Lyapunov stability, 58 state-space controllers
Unscented Kalman filter, 120 closed-loop, 33
open-loop, 31
optimal control
state-space observers
linear plant inversion, 41
Extended Kalman filter, 120
linear time-varying control, 72
Kalman filter
linear-quadratic regulator, 34
as Luenberger observer, 112
Bryson’s rule, 35
derivations, 103, 105
definition, 35
equations, 108
physics setup, 110
conservation of energy, 156 Kalman smoother, 114
sum of forces, 154 derivations, 114
sum of torques, 155 equations, 117
PID control, 11, 29, 190 Luenberger observer, 94
probability, 96 multiple model adaptive estimation, 121
Bayes’s rule, 100 Unscented Kalman filter, 120
central limit theorem, 103 steady-state error, 188
conditional expectation, 100 stochastic
conditional probability density functions, linear systems, 103
100 measurement noise, 103
conditional variances, 100 process noise, 103
covariance, 99 two-sensor problem, 104