Problem

A double integrator is a second-order model, with a second derivative, where the derivative is independent of the state. This model appears in many engineering disciplines.

Solution

You will write the equations for the model dynamics and implement these in a function.

How It Works

One-dimensional linear motion can be modeled with the following differential equations: (6.1)
(6.2)
r and υ are states; m, the mass, is a parameter; and F, the force, is an input. The state vector is
(6.3)

The variable x is represented by a MATLAB two rows × one column array. The first element of the array is r and the second is υ.

To write a function for the derivatives, divide by m to isolate the derivatives on the left-hand side of the equations. The terms on the right-hand side are what you will calculate in this so-called RHS function.
(6.4)
(6.5)

Writing these equations in vector notation, you have (6.6)

Now you can write the function, RHSDoubleIntegrator, for the derivative of the state vector x. Note the prefix of RHS in the name, which you use to identify all functions that can be integrated. The velocity term is the second element of state x, and is passed as the derivative of the position state r. The derivative of the velocity state is the acceleration a. The RHS function has a placeholder ∼ for the first argument, where the integrator will pass the time t, which this function doesn’t require.

function xDot = RHSDoubleIntegrator(∼, x, a)

xDot = [x(2);a];

Problem

You need to use numerical integration in the simulations to evolve the state of the systems.

Solution

You will review the equations for a fourth-order Runge-Kutta integrator and develop a function to perform fixed-step integration.

How It Works

Let’s look at a simple model for linear motion. You need to put the equations in state equation form, which is a set of first-order differential equations with the derivatives on the left-hand side. Take, for example, the one-dimensional motion model from Recipe 6-1, with the derivative terms for r and υ on the left-hand side of the equations.
(6.7)
(6.8)

Multiply both sides by dt.
(6.9)
(6.10)

Replace dt with the fixed-time interval h. You can now write the simplest type of numerical integrator for computing the state at step k + 1 from the state at step k.
(6.11)
(6.12)

Step k is at time t _k and step k + 1 is at time t _k+1, where t _k+1 = t _k + h.

This simple integrator is called Euler integration. It assumes that the force F _k is constant over the time interval h. Euler integration works fairly well for simple systems, like the preceding one. For example, for a spring system
(6.13)
the natural frequency of oscillation is ω. If the time step is greater than half the period of the oscillation 2π/ω, the numerical integration cannot capture the dynamics. In practice, the time step, h, must be much lower than half.

Euler integration is rarely used for engineering due to its limited accuracy. One of the most popular methods used for control system simulation is the fourth-order Runge-Kutta method. The equations are as follows: (6.14)
f  (x, u, t) is the right-hand side of the differential equations. O(h ⁴) means the truncation error due to the order of the integration goes as the fourth power of the time step. This means that if you halve this time step, the error drops to 0.0625 of the error with the bigger time step. In the preceding system, the right-hand sides are computed at four different points, twice at h/2 and once at t and t + h.
(6.15)

MATLAB has many numerical integrators. They are designed to integrate over any interval. Some, such as ode113, adjust the step size and the order of the integration (Euler is first-order, the preceding Runge-Kutta is fourth-order), between the desired interval. For digital control, you need to integrate between the step size of the digital controller. You could use ode113 for this, but usually the fourth-order Runge-Kutta is sufficient. This method is used for all of the examples in this book.

You next want to look at the RungeKutta function that implements equation 6.14. Note the use of feval to evaluate the right-hand-side function in the following code.

function x = RungeKutta( Fun, t, x, h, varargin )

hO2 = 0.5*h;

tPHO2 = t + hO2;

k1 = feval ( Fun, t, x, varargin{:} );

k2 = feval ( Fun, tPHO2, x + hO2*k1, varargin{:} );

k3 = feval ( Fun, tPHO2, x + hO2*k2, varargin{:} );

k4 = feval ( Fun, t+h, x + h*k3, varargin{:} );

x = x + h*(k1 + 2*(k2+k3) + k4)/6;

Fun is a pointer to the right-hand-side function. varargin is passed to that function, which enables the dynamics model to have any number of parameters. This RungeKutta function is used in all of the examples in this book. You precompute all values that are used multiple times, such as . This is particularly important in functions that are called repeatedly.

You pass this function a handle to the right-hand-side function, RHSDoubleIntegrator, which implements equation 6.1.

In order to integrate the model one step, you call

xNew = RungeKutta( @RHSDoubleIntegrator, ∼, x, h, a )

Our RungeKutta function, and all MATLAB integrators, have the dependent variable first, which is t in this case. Since it isn’t used here, you replace it with the tilde. MATLAB’s code analyzer will suggest this for efficiency for all unused function inputs and outputs in your code.

Problem

You want digital control software that can control a double integrator system and other dynamical systems.

Solution

You will derive the equations for a damped second-order system in the time domain and then create a sampled data version and implement it a MATLAB function.

How It Works

If a constant force F is applied to the system, the mass m will accelerate and its position will change with the square of time. The analytical solution for the two states, r and υ, is
(6.16)
(6.17)

If you wish to have the position of mass stay close to zero, you can use a control system known as a regulator. You will use a proportional-derivative regulator. This regulator measures position and applies a control force proportional to the position error and to the derivative of the position error. Let’s look at a particularly simple form of this control:
(6.18)

F _c is the control force. You don’t have to be able to measure the disturbance force, F, for this to work. Picking the gains, k _r and k _υ , is easy if you write the dynamical system as a second-order system:
(6.19)
(6.20)
(6.21)

The controlled system is a damped second-order oscillator. You can write the desired differential equation as (6.22)
where the gains are (6.23)
(6.24)
σ is the undamped natural frequency and ζ is the damping ratio. This system will always be stable as long as the gains are positive. If F if constant the position will settle to an offset. This is
(6.25)

This method of control design is called “pole-placement”.

Virtually all control systems are implemented on digital computers, so you must transform this controller into a digital form. Assume that you only measure position. The first step is to assemble the control in a continuous time state space form. For this implementation, you add a rate filter to the PD controller. (6.26)
(6.27)
(6.28)
(6.29)
(6.30)
(6.31)
(6.32)
where ω _r is the cutoff frequency for the first-order filter on the rate term; ω _n is the undamped natural frequency for the controller; and ζ is the controller damping ratio. m is the “mass” or inertia. You can always set this to 1 and scale the control output. The undamped natural frequency gives the bandwidth of the controller. The higher the bandwidth, the faster it responds to errors. The higher the bandwidth, the smaller the offset error will be due to a constant input.
(6.33)
(6.34)
x is the controller state and u is the position measurement. The state space form is convenient for computation but still assumes that you are sampling continuously. There are many ways to convert this to digital form. You will use a zero-order-hold, meaning you will compute the control each sample and hold the value over that sample period. You convert this using the matrix exponential function in MATLAB, expm. If T is the sample period, you assemble the matrix
(6.35)

The sampled time versions of a and b are
(6.36)

The digital controller is now (6.37)
(6.38)

Let’s now look at PDControl. This function designs and implements the control system derived in equation 6.26. The name stands for Proportional-Derivative Control. It has several child functions. The header is shown below. It has a link to the help for one of the subfunctions, which is active when the header is displayed at the command line. You list each data structure field for the input and output.

%% PDCONTROL Design and implement a PD Controller in sampled time .

%% Forms

% d = PDControl ( ' struct ' )

% d = PDControl ( ' initialize ', d )

% [ y , d ] = PDControl ( ' update ', u , d )

%

%% Description

% Designs a PD controller and implements it in discrete form .

%

% y = -c*x - d*u

% x = a*x + b*u

%

% where u is the input and y is the output. This controller has a first

% order rate filter on the derivative term.

%

% Set the mode to initialize to create the state space matrices for the

% controller. Set the mode to update to update the controller and get a

% new output.

%

% Utilizes the subfunction C2DZOH to discretize, see <a href="matlab: help PDControl>

CToDZOH">CToDZOH help</a>

%

%% Inputs

% mode (1,1) ' initialize ' or ' update '

% u (1,1) Measurement

% d (.) Data structure

% .m (1,1) Mass

% .zeta (1,1) Damping ratio

% .wN (1,1) Undamped natural frequency

% .wD (1,1) Derivative term filter cutoff

% .tSamp (1,1) Sampling period*

% .x (1,1) Controller state

%

%% Outputs

% y (1,1) Control

% d (.) Data structure additions

% .a (1,1) State transition matrix

% .b (1,1) Input matrix

% .c (1,1) State output matrix

% .d (1,1) Feedthrough matrix

% .x (1,1) Updated controller state

Next, the body of the function. Note the switch statement and the two child functions, CToDZOH and DefaultStruct, at the bottom.

function [y, d] = PDControl( mode, u, d )

% Demo

if ( nargin < 1 )

disp ('Demo⊔of⊔PDControl⊔using⊔the⊔default⊔struct')

d = PDControl('struct');

d = PDControl('initialize',d);

disp (d)

return

end

switch lower (mode)

case 'initialize'

d = u;

w = d.wD + 2*d.zeta*d.wN;

k = d.wD*d.wNˆ2*d.m/w;

tau = ((d.m/k)*d.wN*(d.wN + 2*d.zeta*d.wD) - 1 )/w;

d.a = -w;

d.b = w;

d.c = -k*w*tau;

d.d = k*(tau*w + 1);

[d.a, d.b] = CToDZOH(d.a,d.b,d.tSamp);

y = d;

case 'update'

y = -d.c*d.x - d.d*u;

d.x = d.a*d.x + d.b*u;

case 'struct'

y = DefaultStruct;

otherwise

error ('%s⊔is⊔not⊔a⊔valid⊔mode',mode);

end

function [f, g] = CToDZOH( a, b, T )

%% CToDZOH

% Continuous to discrete transformation using a zero order hold. Discretize

% using a matrix exponential , < matlab:doc (' expm ') expm >.

%

% [f, g] = C2DZOH( a, b, T )

%

% * Inputs *

%

% a (1,1) Continuous plant matrix

% b (1,1) Input matrix

% T (1,1) Time step

%

% * Outputs *

%

% f (1,1) Discrete plant

% g (1,1) Discrete input

%

% See also

% expm

q = expm ([a*T b*T; zeros (1,2)]);

f = q(1,1);

g = q(1,2);

function d = DefaultStruct

d.m = 1.0;

d.zeta = 0.7;

d.wN = 0.1;

d.wD = 0.5;

d.tSamp = 1.0;

d.x = 0.0;

d.a = [];

d.b = [];

d.c = [];

d.d = [];

This is our standard format for an engineering function. The following are its important features:

• It combines design and implementation in one function.

• It returns the default data structure that it uses.

• It has modes.

• It has a built-in demo.

• It has nested functions.

The built-in demo uses the default values. The default values give the user an idea of reasonable parameters for the function. This built-in demo generates the state space matrices, which are scalars in this case.

Demo of PDControl using the default struct

m: 1

zeta: 0.7000

wN: 0.1000

wD: 0.5000

tSamp: 1

x: 0

a: 0.5273

b: 0.4727

c: -0.0722

d: 0.0800

A more elaborate demo, with a simulation, could have been added. The built-in demos are very useful because they show the user a simple example of how to use the function. It also is helpful in developing the function, because you can test the function by just typing the function name in the command line.

The first argument is the mode variable that indicates which case in the switch statement the function should execute. The “initialize” mode must always be run first. The initialization modifies the data structure, which is used as the function’s memory. You could also have used persistent variables for the function memory. Using an output makes it easier to programmatically inspect the contents of memory. The “update” is used to update the controller as new inputs arrive. The switch statement has an “otherwise” case to warn the user of mode errors. This throws an error, stopping execution of the script. You may not always want to do this and may just use a warning to warn the user.

The nested function CToDZOHconverts the continuous control system to a sampled-data control system using equation 6.35. The name stands for Continuous to Discrete Zero-Order Hold. A non-controls expert wouldn’t immediately understand the acronym, but the expanded name is too long to be a useful function name.

If you were building a toolbox, the CToDZOH function would likely be a separate file. For this book, it is only used by PDControl, so you put it into that function file.

Problem

You want to simulate digital control of the double integrator model.

Solution

You will write a script that calls the control function and integrator sequentially in a loop and plots the results.

How It Works

Table 6-1 presents the nominal values for the control parameters that you will use for the double integrator simulation.

The simulation script is implemented in DoubleIntegratorSim.m. Note the use of cell breaks to divide the script into sections that can be run independently. The “See also” section lists the functions used, which are links when the header is displayed via the command-line help.

%% Double Integrator Demo

% Demonstrate control of a double integrator .

%% See also

% PDControl, RungeKutta, RHSDoubleIntegrator, TimeLabel

%% Initialize

tEnd = 100; % Simulation end time (sec)

dT = 0.1; % Time step (sec)

aD = 1.0; % Disturbance acceleration (m/sˆ2)

controlIsOn = false; % True if the controller is to be used

x = [0;0]; % [position;velocity]

% Controller parameters

d = PDControl( 'struct' );

d.zeta = 1.0;

d.wN = 0.1;

d.wD = 1.0;

d.tSamp = dT;

d = PDControl( 'initialize', d );

%% Simulation

nSim = tEnd/dT+1;

xPlot = zeros (3,nSim);

for k = 1:nSim

if ( controlIsOn )

[u, d] = PDControl('update',x(1),d);

else

u = 0;

end

xPlot(:,k) = [x;u];

x = RungeKutta( 'RHSDoubleIntegrator', 0, x, dT, aD+u );

end

%% Plot the results

yL = {'r⊔(m)' 'v⊔(m/s)' 'u⊔(m/sˆ2)'};

[t,tL] = TimeLabel(dT*(0:(nSim-1)));

PlotSet( t, xPlot, 'x⊔label', tL, 'y⊔label', yL );

The first code block sets up simulation parameters that the user can change. The controlIsOn variable is set to true if the controller is to be used. This makes it easy to test the script without the controller. When the controller is disabled, you get the “open loop” response. It is a good idea to make sure that the open-loop response makes sense before testing the controller.

The second block sets up the controller. Recall that PDControl has three arguments: 'struct' 'initialize', 'update'. The first returns the data structure required by the function. You fill the structure fields with the values selected for the problem in the lines that follow that statement. At the end of this block, you initialize the controller. This sets the controller state to zero and creates the sampled time state space matrices, which in this case are four scalars.

The third block is the simulation with the check to see if the controller is on. Note the sequential use of the control function followed by the integrator. This is discrete control because the control, u, is constant over the integration time step. The final block plots the results.

Figure 6-1 shows the open-loop response obtained by setting the controlIsOn flag to false and executing DoubleIntegratorSim. The velocity increases linearly and the position increases with the square of time, as it should. The output agrees with the analytical solution in equation 6.16. Figure 6-2 shows the closed-loop response, with controlIsOn set to true. The velocity goes to zero and the position reaches a constant, though not zero. The control acceleration u exactly matches the disturbance acceleration a. You could have eliminated the position offset by using a proportional integral differential (PID) controller.

Figure 6-1. The open-loop response with a constant disturbance acceleration of 1 m/s²

Figure 6-2. The closed-loop response with a constant disturbance acceleration of 1 m/s²

The two examples show that the simulation works without the control and that the control performs as expected. This script is an integrated test of all of the functions listed in the header. It does a good job of testing their functionality. However, one test isn’t sufficient to understand the controller. Let’s make the controller underdamped by setting ζ in the field d.zeta, to 0.2. Now the response oscillates (see Figure 6-3). You set tEnd to 300 to show that it damps.

Figure 6-3. The closed-loop response with a constant disturbance acceleration of 1 m/s² and ζ equal to 0.2

Another thing to try is setting the bandwidth really high. Set ω _n , in field d.wN, to 8, and the rate filter bandwidth d.wD to 50. The result is shown in Figure 6-4. The controller is unstable because the bandwidth is much higher than that allowed by the sampling rate.

Figure 6-4. The closed-loop response using a bandwidth too high for the sampling

Your bandwidth has to be less than half the sampling bandwidth, which is (6.39)

All the results are expected behavior. The last case is what is known as an edge or corner case that shows that the expected instability does happen. These four cases are a minimalist set of tests for this admittedly simple control system example.

Problem

You want your time axes to have easy-to-read units, not just seconds.

Solution

You will create a function that checks the duration and converts from seconds to minutes, hours, days, or years.

How It Works

You check the maximum time in the array of times and scale it to larger time units. The time units implemented are seconds, minutes, hours, days, and years. You return the scaled time vector, the label string, and the units string, as it might be useful.

function [t, c, s] = TimeLabel( t )

secInYear = 365.25*86400;

secInDay = 86400;

secInHour = 3600;

secInMinute = 60;

tMax = max (t);

if ( tMax > secInYear )

c = 'Time⊔(years)';

s = 'years';

t = t/secInYear;

elseif ( tMax > 3*secInDay )

c = 'Time⊔(days)';

t = t/secInDay;

s = 'days';

elseif ( tMax > 3*secInHour )

c = 'Time⊔(hours)';

t = t/secInHour;

s = 'hours';

elseif ( tMax > 3*secInMinute )

c = 'Time⊔(min)';

t = t/secInMinute;

s = 'min';

else

c = 'Time⊔(sec)';

s = 'sec';

end

Problem

You frequently generate figures with multiple subplots during control analysis, and this results in large blocks of repetitive code at the bottom of every script.

Solution

You make a function that can easily generate subplots with a single line.

How It Works

You will write a function that uses parameter pairs to flexibly create subplot figures in a single function call. The y input can have multiple rows, and the x input can have one row or the same number of rows as y. You supply default labels so that the function can be called with just two inputs.

The only parameters supported in this version are various labels and the plot type—standard plot, semilogx, semilogy, and loglog—but you can easily add functionality for line thickness, plot markers, shading, and so forth. You use a for loop to check every other component of varargin in a switch statement. Remember that varargin provides a cell array of arguments.

Note the use of a built-in demo showing both main branches of the function, for one x series and for two.

The plotting code creates subplots in the figure for each plot based on the number of rows in x and y. This function assumes the subplots are in a single column, but you could extend the logic to create multiple columns or any arrangement of subplots that suits your application. The grid is turned on.

function PlotSet( x, y, varargin )

% Demo

%-----

if ( nargin < 1 )

x = linspace (1,1000);

y = [ sin (0.01*x); cos (0.01*x)];

disp ('PlotSet:⊔One⊔x⊔and⊔two⊔y⊔rows')

PlotSet( x, y )

disp ('PlotSet:⊔Two⊔x⊔and⊔two⊔y⊔rows')

PlotSet( [x;y(1,:)], y )

return ;

end

% Defaults

nCol = 1;

n = size (x,1);

m = size (y,1);

yLabel = cell (1,m);

xLabel = cell (1,n);

plotTitle = cell (1,n);

for k = 1:m

yLabel{k} = 'y';

end

for k = 1:n

xLabel{k} = 'x';

plotTitle{k} = 'y⊔vs.⊔x';

end

figTitle = 'PlotSet';

plotType = 'plot';

% Handle input parameters

for k = 1:2: length (varargin)

switch lower (varargin{k} )

case 'x⊔label'

for j = 1:n

xLabel{j} = varargin{k+1};

end

case 'y⊔label'

temp = varargin{k+1};

if ( ischar(temp) )

yLabel{1} = temp;

else

yLabel = temp;

end

case 'plot⊔title'

plotTitle{1} = varargin{k+1};

case 'figure⊔title'

figTitle = varargin{k+1};

case 'plot⊔type'

plotType = varargin{k+1};

otherwise

fprintf (1,'%s⊔is⊔not⊔an⊔allowable⊔parameter ',varargin{k});

end

end

h = figure ;

set (h,'Name',figTitle);

% First path is for just one row in x

if ( n == 1 )

for k = 1:m

subplot (m,nCol,k);

plotXY(x,y(k,:),plotType);

xlabel (xLabel{1});

ylabel (yLabel{k});

if ( k == 1 )

title (plotTitle{1})

end

grid on

end

else

for k = 1:n

subplot (n,nCol,k);

plotXY(x(k,:),y(k,:),plotType);

xlabel (xLabel{k});

ylabel (yLabel{k});

title (plotTitle{k})

grid on

end

end

%% Implement different plot types

function plotXY(x,y, type )

switch type

case 'plot'

plot (x,y);

case {'log' 'loglog' 'log⊔log'}

loglog (x,y);

case {'xlog' 'semilogx' 'x⊔log'}

semilogx (x,y);

case {'ylog' 'semilogy' 'y⊔log'}

semilogy (x,y);

otherwise

error ('%s⊔is⊔not⊔an⊔available⊔plot⊔type', type );

end

This function allows DoubleIntegratorSim.m to generate its plots with one line of code.