Batch Linearization and gain scheduling
This example will demonstrate how to linearize a nonlinear ModelingToolkit model in multiple different operating points, and some tools to work with groups of linear models representing the same system in different operating points. We'll end with designing and simulating a gain-scheduled controller, i.e., a nonlinear controller created as an interpolation between linear controllers.
An operating point is typically understood as a tuple of the form $(x, u)$, where $x$ is the state vector and $u$ is the input vector. However, we may choose to include parameters $p$ in the operating point as well. This may be useful when some parameters are uncertain or time varying, and we want to perform analysis over multiple possible parameter values.
System model
The model will be a simple Duffing oscillator:
using ControlSystemsMTK, ModelingToolkit, MonteCarloMeasurements, ModelingToolkitStandardLibrary.Blocks
unsafe_comparisons(true)
# Create a model
@parameters t k=10 k3=2 c=1
@variables x(t)=0 [bounds = (-0.5, 1.5)]
@variables v(t)=0
D = Differential(t)
@named y = Blocks.RealOutput()
@named u = Blocks.RealInput()
eqs = [D(x) ~ v
D(v) ~ -k * x - k3 * x^3 - c * v + 10u.u
y.u ~ x]
@named duffing = System(eqs, t, systems=[y, u])\[ \begin{align} \frac{\mathrm{d}x\left( t \right)}{\mathrm{d}t} &= v\left( t \right) \\ \frac{\mathrm{d}v\left( t \right)}{\mathrm{d}t} &= 10 ~ \mathtt{u.u}\left( t \right) - c ~ v\left( t \right) - k ~ x\left( t \right) - \left( x\left( t \right) \right)^{3} ~ \mathtt{k3} \\ \mathtt{y.u}\left( t \right) &= x\left( t \right) \end{align} \]
Batch linearization
To perform batch linearization, we create a vector of operating points, and then linearize the model around each of these points. The function batch_ss does this for us, and returns a vector of StateSpace models, one for each operating point. An operating point is a Dict that maps variables in the MTK model to numerical values. In the example below, we simply sample the variables uniformly within their bounds specified when we created the variables (normally, we might want to linearize on stationary points)
N = 5 # Number of samples
xs = range(getbounds(x)[1], getbounds(x)[2], length=N)
ops = Dict.(u.u => 0, x .=> xs)5-element Vector{Dict{Num, Real}}:
Dict(x(t) => -0.5, u₊u(t) => 0)
Dict(x(t) => 0.0, u₊u(t) => 0)
Dict(x(t) => 0.5, u₊u(t) => 0)
Dict(x(t) => 1.0, u₊u(t) => 0)
Dict(x(t) => 1.5, u₊u(t) => 0)Just like ModelingToolkit.linearize, batch_ss takes the set of inputs and the set of outputs to linearize between.
Ps, ssys = batch_ss(duffing, [u.u], [y.u], ops)Plotting functions like bodeplot accept vectors of systems, so this works
using ControlSystemsBase, Plots
w = exp10.(LinRange(-2, 3, 200))
bodeplot(Ps, w, legend=false)
We can also convert the vector of system models to a single model with RobustAndOptimalControl.ss2particles, which will convert the coefficients of the state space models to MonteCarloMeasurements.Particles objects.
using RobustAndOptimalControl
P = RobustAndOptimalControl.ss2particles(Ps) # convert to a single StateSpace system with `Particles` as coefficients.StateSpace{Continuous, Particles{Float64, 5}}
A =
0.0 1.0
-14.5 ± 5.5 -1.0
B =
0.0
10.0
C =
1.0 0.0
D =
0.0
Continuous-time state-space modelnotice how some coefficients are plotted like uncertain numbers -13.8 ± 4.3. We can plot such models as well:
bodeplot(P, w, legend=:bottomright, adaptive=false) # Should look similar to the one above
Controller tuning
Let's also do some controller tuning for the linearized models above. The function batch_tune is not really required here, but it shows how we might go about building more sophisticated tools for batch tuning. In this example, we will tune a PID controller using the function loopshapingPID. Note, this procedure is not limited to tuning a gain-scheduled PID controller, it should work for gain-scheduling of any LTI controller.
!!! "note" Interpolating between controllers There are multiple ways in which one could interpolate between different controllers. The two most common approaches are to interpolate their outputs, and interpolating their coefficients. When interpolating the coefficients, it is important to ensure that all controllers have the same structure for the interpolation to be meaningful. One may for example interpolate between PID coefficients, or between the coefficients of a state-space model. When interpolating state-space matrices, the systems must all share the same basis, i.e., the state variables must all have the same meaning among the interpolated systems. When converting a transfer function to state-space form, a numerical balancing is performed, this alters the meaning of the state variables and may introduce artificial dynamics to the interpolated system. We thus pass balance=false to the function ss to avoid this, or pick a form explicitly, e.g., modal_form.
function batch_tune(f, Ps)
f.(Ps)
end
Cs = batch_tune(Ps) do P
C, kp, ki, kd, fig, CF = loopshapingPID(P, 7; Mt=1.2, Tf = 1/100)
ss(CF, balance=false)
modal_form(ss(CF, balance=true))[1]
end
P = RobustAndOptimalControl.ss2particles(Ps)
C = RobustAndOptimalControl.ss2particles(Cs)
nyquistplot(P * C,
w,
ylims = (-10, 2),
xlims = (-8, 5),
points = true,
Ms_circles = [1.5, 2],
Mt_circles = [1.2])
Above, we plotted the Nyquist curve of the loop-transfer function for all system realizations. RobustAndOptimalControl.jl has some facilities for fitting circles around the Nyquist curve for uncertain systems, which we could use here:
centers, radii = fit_complex_perturbations(P * C, w; relative = false, nominal = :center)
nyquistcircles!(w, centers, radii, ylims = (-5, 1), xlims = (-3, 4))
some methods for robust control operate on such circles. Notice how the circles are conservative in many cases, this is typically due to the gain varying between the models for the same phase.
If you plot the Nyquist curve using the plotly() backend rather than the default gr() backend used here, you can hover the mouse over the curves and see which frequency they correspond to etc.
Gain scheduling
Above, we tuned one controller for each operating point, wouldn't it be nice if we had some features to simulate a gain-scheduled controller that interpolates between the different controllers depending on the operating pont? GainScheduledStateSpace is such a thing, we show how to use it below. For fun, we simulate some reference step responses for each individual controller in the array Cs and end with simulating the gain-scheduled controller.
using OrdinaryDiffEqNonlinearSolve, OrdinaryDiffEqRosenbrock
using DataInterpolations # Required to interpolate between the controllers
@named fb = Blocks.Add(k2=-1)
@named ref = Blocks.Square(frequency=1/6, amplitude=0.5, offset=0.5, start_time=1)
@named F = Blocks.SecondOrder(w=15, d=1) # A reference pre-filter
connect = ModelingToolkit.connect
closed_loop_eqs = [
connect(ref.output, :r0, F.input)
connect(F.output, :r, fb.input1) # Add an analysis point :r
connect(duffing.y, :y, fb.input2) # Add an analysis point :y
]
plot(layout=2)
# Simulate each individual controller
for C in Cs
@named Ci = System(C)
eqs = [
closed_loop_eqs
connect(fb.output, Ci.input)
connect(Ci.output, duffing.u)
]
@named closed_loop = System(eqs, t, systems=[duffing, Ci, fb, ref, F])
prob = ODEProblem(mtkcompile(closed_loop), [F.x => 0, F.xd => 0], (0.0, 8.0))
sol = solve(prob, Rodas5P(), abstol=1e-8, reltol=1e-8)
plot!(sol, idxs=[duffing.y.u, duffing.u.u], layout=2, lab="")
end
# Simulate gain-scheduled controller
@named Cgs = GainScheduledStateSpace(Cs, xs, interpolator=LinearInterpolation)
eqs = [
closed_loop_eqs
connect(fb.output, Cgs.input)
connect(Cgs.output, :u, duffing.u)
connect(duffing.y, :v, Cgs.scheduling_input) # Don't forget to connect the scheduling variable!
]
@named closed_loop = System(eqs, t, systems=[duffing, Cgs, fb, ref, F])
prob = ODEProblem(mtkcompile(closed_loop), [F.x => 0, F.xd => 0], (0.0, 8.0))
sol = solve(prob, Rodas5P(), abstol=1e-8, reltol=1e-8, dtmax=0.01)
plot!(sol, idxs=[duffing.y.u, duffing.u.u], l=(2, :red), lab="Gain scheduled")
plot!(sol, idxs=F.output.u, l=(1, :black, :dash, 0.5), lab="Ref")
If everything worked as expected, the gain-scheduled controller should perform reasonably well across the entire scheduling range.
C-Code generation
We can generate C-code to interpolate our controller using the function SymbolicControlSystems.print_c_array from SymbolicControlSystems.jl. If the controller is a standard ControlSystemsBase.StateSpace object, a function that filters the input through the controller can be generated by calling SymbolicControlSystems.ccode. But if the controller is a vector of controllers representing a gain-scheduled controller, a function that creates the interpolated dynamics is written. In the code below, we shorten the vector of controllers to make the generated C-code easier to read by passing Cs[1:3:end] and xs[1:3:end]
using SymbolicControlSystems, ControlSystemsBase
Cs_disc = c2d.(Cs, 0.05, :tustin) # Discretize the controller before generating code
code = SymbolicControlSystems.print_c_array(stdout, Cs_disc[1:3:end], xs[1:3:end], "Cs")double Cs_interp_vect[2];
Cs_interp_vect[0] = -0.5;
Cs_interp_vect[1] = 1.0;
double Cs_A[3][3];
Cs_A[0][0] = 1.0;
Cs_A[0][1] = 0.0;
Cs_A[0][2] = 0.0;
Cs_A[1][0] = 0.0;
Cs_A[1][1] = -0.4867631074908737;
Cs_A[1][2] = 0.32780332775069837;
Cs_A[2][0] = 0.0;
Cs_A[2][1] = -0.3278033277506983;
Cs_A[2][2] = -0.48676310749087365;
double Cs_B_interp[3][1][2];
Cs_B_interp[0][0][0] = 0.3531155943158151;
Cs_B_interp[0][0][1] = 0.37434049971517114;
Cs_B_interp[1][0][0] = 3.774581494484414;
Cs_B_interp[1][0][1] = 3.780197210209189;
Cs_B_interp[2][0][0] = -2.4108172908403955;
Cs_B_interp[2][0][1] = -2.414404036706001;
void interpolate_Cs_B(double *Cs_B, double *Cs_B_interp, double *Cs_interp_vect, double t) {
int k = 0;
for (k = 0; k < 0; ++k) { // Loop to find correct interval
if (t < Cs_interp_vect[k+1]) {
break;
}
} // t is now between indices k and k+1 modolu edge cases below
double t0 = Cs_interp_vect[k];
double t1 = Cs_interp_vect[k+1];
double l = t1 - t0; // Length of interval between points
double alpha = (t-t0)/l; // Between 0 and 1, proportion of k+1 point
if (t < Cs_interp_vect[0]) { // edge cases
alpha = 0;
}else if (t > Cs_interp_vect[1]) {
alpha = 1;
}
for (int i = 0; i < 3; ++i) { // Cs_B has dimensions (3, 1)
for (int j = 0; j < 1; ++j) { // Cs_B_interp has dimensions (3, 1, 2)
Cs_B[i][j] = (1-alpha)*Cs_B_interp[i][j][k] + alpha*Cs_B_interp[i][j][k+1];
}
}
}
double Cs_C_interp[1][3][2];
Cs_C_interp[0][0][0] = 7.062311886316298;
Cs_C_interp[0][0][1] = 7.486809994303409;
Cs_C_interp[0][1][0] = 8.10532803892276;
Cs_C_interp[0][1][1] = 7.818183138865548;
Cs_C_interp[0][2][0] = 31.99882836354859;
Cs_C_interp[0][2][1] = 31.240889205601384;
void interpolate_Cs_C(double *Cs_C, double *Cs_C_interp, double *Cs_interp_vect, double t) {
int k = 0;
for (k = 0; k < 0; ++k) { // Loop to find correct interval
if (t < Cs_interp_vect[k+1]) {
break;
}
} // t is now between indices k and k+1 modolu edge cases below
double t0 = Cs_interp_vect[k];
double t1 = Cs_interp_vect[k+1];
double l = t1 - t0; // Length of interval between points
double alpha = (t-t0)/l; // Between 0 and 1, proportion of k+1 point
if (t < Cs_interp_vect[0]) { // edge cases
alpha = 0;
}else if (t > Cs_interp_vect[1]) {
alpha = 1;
}
for (int i = 0; i < 1; ++i) { // Cs_C has dimensions (1, 3)
for (int j = 0; j < 3; ++j) { // Cs_C_interp has dimensions (1, 3, 2)
Cs_C[i][j] = (1-alpha)*Cs_C_interp[i][j][k] + alpha*Cs_C_interp[i][j][k+1];
}
}
}
double Cs_D_interp[1][1][2];
Cs_D_interp[0][0][0] = 60.857237577405584;
Cs_D_interp[0][0][1] = 58.98538773339739;
void interpolate_Cs_D(double *Cs_D, double *Cs_D_interp, double *Cs_interp_vect, double t) {
int k = 0;
for (k = 0; k < 0; ++k) { // Loop to find correct interval
if (t < Cs_interp_vect[k+1]) {
break;
}
} // t is now between indices k and k+1 modolu edge cases below
double t0 = Cs_interp_vect[k];
double t1 = Cs_interp_vect[k+1];
double l = t1 - t0; // Length of interval between points
double alpha = (t-t0)/l; // Between 0 and 1, proportion of k+1 point
if (t < Cs_interp_vect[0]) { // edge cases
alpha = 0;
}else if (t > Cs_interp_vect[1]) {
alpha = 1;
}
for (int i = 0; i < 1; ++i) { // Cs_D has dimensions (1, 1)
for (int j = 0; j < 1; ++j) { // Cs_D_interp has dimensions (1, 1, 2)
Cs_D[i][j] = (1-alpha)*Cs_D_interp[i][j][k] + alpha*Cs_D_interp[i][j][k+1];
}
}
}The generated code starts by defining the interpolation vector xs, this variable is called Cs_interp_vect in the generated code. The code then defines all the $A$ matrices as a 3-dimensional array, followed by a function that performs the interpolation interpolate_Cs_A. This function takes the output array as the first argument, a pointer to the 3D array with interpolation matrices, the interpolation vector as well as the interpolation variable t, in this document called $v$. The same code is then repeated for the matrices $B,C,D$ as well if they require interpolation (if they are all the same, no interpolation code is written).
Linearize around a trajectory
We can linearize around a trajectory obtained from solve using the function trajectory_ss. We provide it with a vector of time points along the trajectory at which to linearize, and in this case we specify the inputs and outputs to linearize between as analysis points r and y.
timepoints = 0:0.01:8
Ps2, ssys, ops2, resolved_ops = trajectory_ss(closed_loop, closed_loop.r, closed_loop.y, sol; t=timepoints, verbose=true);
bodeplot(Ps2, w, legend=false)
Not how the closed-loop system changes very little along the trajectory, this is a good indication that the gain-scheduled controller is able to make the system appear linear.
Internally, trajectory_ss works very much the same as batch_ss, but constructs operating points automatically along the trajectory using ModelingToolkit.LinearizationOpPoint. The operating points are extracted from the differential states and parameters of the solution. We specify the inputs and outputs as analysis points to properly define the linearization interface.
We can replicate the figure above by linearizing the plant and the controller individually, by providing the loop_openings argument. Opening a loop breaks the corresponding connection, and the disconnected signal becomes a parameter of the linearized system. A loop opening changes the input-output structure of the linearization, not the operating point: ModelingToolkit holds a disconnected signal at the value it has in the loop-closed solution at each time point, i.e., at the value of the signal that drove it. All parts are thus linearized at the same operating point as the closed loop, which is required for their linearizations to combine into the linearization of the closed loop. To linearize around some other value of a disconnected signal, pass it through the op argument, e.g., op = Dict(opened_signal => 0). When linearizing the plant, we disconnect the controller output by passing loop_openings=[closed_loop.u], and when linearizing the controller, we have various options for disconnecting the plant:
- Break the connection from plant output to controller input by passing
loop_openings=[closed_loop.y] - Break the connection between the controller and the plant input by passing
loop_openings=[closed_loop.u] - Break the connection
yas well as the scheduling variablev(which is another form of feedback) by passingloop_openings=[closed_loop.y, closed_loop.v]
We will explore these options below, starting with the first option, breaking the connection y:
kwargs = (; adaptive=false, legend=false)
plants, _ = trajectory_ss(closed_loop, closed_loop.u, closed_loop.y, sol; t=timepoints, verbose=true, loop_openings=[closed_loop.u]);
controllersy, ssy, ops3, resolved_ops3 = trajectory_ss(closed_loop, closed_loop.r, closed_loop.u, sol; t=timepoints, verbose=true, loop_openings=[closed_loop.y]);
closed_loopsy = feedback.(plants .* controllersy)
bodeplot(closed_loopsy, w; title="Loop open at y", kwargs...)
With the loop opened only at y, the scheduling feedback through v remains in place, so the controller linearizations contain the path from the controller output through the plant to the scheduling input. Closing the loop at y thus reproduces the linearization of the closed loop exactly:
using Test
@test all(isapprox(freqresp(cy, w), freqresp(P2, w), rtol=1e-6) for (cy, P2) in zip(closed_loopsy, Ps2))Test PassedWhen we open the loop at u instead, the scheduling variable v remains connected, so the controller is linearized at the scheduling value along the trajectory and is fully isolated from the plant:
controllersu, ssu = trajectory_ss(closed_loop, closed_loop.r, closed_loop.u, sol; t=timepoints, verbose=true, loop_openings=[closed_loop.u]);
closed_loopsu = feedback.(plants .* controllersu)
bodeplot(closed_loopsu, w; title="Loop open at u", kwargs...)
These controllers do not contain the dependence of the controller output on the scheduling variable, i.e., the derivative of the gain-scheduled controller with respect to v, which is nonzero whenever the controller is not at an equilibrium of the scheduled family. The loop feedback(plant * controller) omits this path, so the result differs from the linearization of the closed loop along the trajectory.
We may also break the scheduling feedback v in addition to y. Since the disconnected signals keep their values in the loop-closed solution, the controller is scheduled along the trajectory also in this case, and the result coincides with opening at u:
controllersv, ssv = trajectory_ss(closed_loop, closed_loop.r, closed_loop.u, sol; t=timepoints, verbose=true, loop_openings=[closed_loop.y, closed_loop.v]);
closed_loopsv = feedback.(plants .* controllersv)
bodeplot(closed_loopsv, w; title="Loop open at v and y", kwargs...)
@test all(isapprox(freqresp(cv, w), freqresp(cu, w), rtol=1e-6) for (cv, cu) in zip(controllersv, controllersu))Test PassedWe have thus far treated the controller as a SISO system, but we could also view it as a system with two inputs, the reference and the scheduling feedback, and the plant as a system with the two outputs y and v. The measurement y enters the controller through the error r - y, i.e., with a negative sign, whereas the scheduling variable v enters with a positive sign. With $C_r$ and $C_v$ denoting the transfer functions of the controller from r and v to u, the controller output is $u = C_r r - (C_r - C_v) y$, and the transfer function from r to y is $P C_r / (1 + P (C_r - C_v))$:
plants_mimo, _ = trajectory_ss(closed_loop, closed_loop.u, [closed_loop.y, closed_loop.v], sol; t=timepoints, verbose=true, loop_openings=[closed_loop.u]);
controllers_mimo, ssm = trajectory_ss(closed_loop, [closed_loop.r, closed_loop.v], closed_loop.u, sol; t=timepoints, verbose=true, loop_openings=[closed_loop.u]);
closed_loops_mimo = map(controllers_mimo, plants_mimo) do C, P
C, P = ss(C), ss(P)
feedback(P[1, 1], C[1, 1] - C[1, 2]) * C[1, 1]
end
bodeplot(closed_loops_mimo, w; title="Loop open at u, MIMO controller", kwargs...)
Since the two-input controller contains the scheduling path, this also reproduces the linearization of the closed loop:
@test all(isapprox(freqresp(cm, w), freqresp(P2, w), rtol=1e-6) for (cm, P2) in zip(closed_loops_mimo, Ps2))Test PassedWhy are the results different depending on where we open the loop? We can understand the difference by comparing the Bode plots of the controllers.
plot(
bodeplot(Cs, w, legend=false, plotphase=false, title="Designed controllers"),
bodeplot(controllersy, w, legend=false, plotphase=false, title="Loop open at y"),
bodeplot(controllersu, w, legend=false, plotphase=false, title="Loop open at u"),
bodeplot(controllersv, w, legend=false, plotphase=false, title="Loop open at v and y"),
)
Opening at u, or at v and y, yields the gain-scheduled controller evaluated at the scheduling value along the trajectory. If we only open at y, the controller linearizations still contain the closed loop through the scheduling connection v. We can verify this by looking at what variables are present in the input-output map
sminreal(controllersy[end]).x5-element Vector{Symbol}:
Symbol("Cgs₊scheduling_input₊u(t)")
Symbol("duffing₊v(t)")
Symbol("(Cgs₊x(t))[3]")
Symbol("(Cgs₊x(t))[2]")
Symbol("(Cgs₊x(t))[1]")notice how the state of the plant is included in the controller, this is an indication that the controller is not isolated from the plant. If we do the same thing for the controller with the loop opened at u, we see that the state of the plant is not included in the controller:
sminreal(controllersu[end]).x3-element Vector{Symbol}:
Symbol("(Cgs₊x(t))[3]")
Symbol("(Cgs₊x(t))[2]")
Symbol("(Cgs₊x(t))[1]")Opening at u thus yields a controller with fewer states than opening only at y, since the latter remains coupled to the plant through the scheduling feedback:
@test sminreal(controllersu[end]).nx < sminreal(controllersy[end]).nxTest PassedThe call to sminreal is important here, it removes the states that are not needed to represent the input-output map of the system. The state of the full model, including the plant state, is present in the linearization before this call.
The choice of loop opening thus depends on the purpose of the analysis. To obtain the controller itself, e.g., for comparison with the designed controllers, open the loop at u. To reconstruct the closed loop from the plant and the controller, the scheduling feedback must be retained, either by opening the loop only at y, or by treating v as an additional input of the controller.
Summary
We have seen how to
- Perform linearization of a nonlinear ModelingToolkit model in multiple different operating points
- Handle arrays of models or models with
Particlesas coefficients - Simulate a gain-scheduled controller that interpolates between linear controllers
- Write C-code to perform the interpolation of the controllers
Batch linearization in multiple different operating points is an intuitive way to perform analysis of a nonlinear control system. Gain-scheduling is an equally intuitive way of realizing a nonlinear controller. Care should be taken to ensure that the scheduling variable does not change too fast such that the linear assumption at each instance of time is not violated.
@test sol(6.99, idxs=closed_loop.duffing.y.u) ≈ 0.0 atol=0.01Test Passed