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 =
-1.0 -14.5 ± 5.5
1.0 0.0
B =
10.0
0.0
C =
0.0 1.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(structural_simplify(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(structural_simplify(closed_loop), [F.xd => 0], (0.0, 8.0))
sol = solve(prob, Rodas5P(), abstol=1e-8, reltol=1e-8, initializealg=SciMLBase.NoInit(), 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.48680999430341;
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. This requires that the solution contains the states of the simplified system, accessible through the idxs argument like sol(t, idxs=x). By linearizing the same system as we simulated, we ensure that this condition holds, doing so requires that we specify the inputs and outputs as analysis points rather than as variables.
We can replicate the figure above by linearizing the plant and the controller individually, by providing the loop_openings argument. When linearizing the plant, we disconnect the controller input by passing loop_openings=[closed_loop.u], and when linearizing the controller, we have various options for disconnecting the 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...)
When we open the loop at u, we get a slightly different result:
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...)
In this case, all static gains are 1. A similar result is obtained by explicitly breaking the scheduling feedback v in addition to an opening of y:
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...)
We have thus far treated the controller as a SISO system, but we could also view it as a system with two inputs, the measurement feedback and the scheduling feedback. For completeness, we show below how to derive the corresponding MIMO systems
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 = feedback.(controllers_mimo .* plants_mimo) # Look at complementary sensitivity function in the input, since this is a SISO system
bodeplot(closed_loops_mimo, w; title="Loop open at MIMO", kwargs...)
Why 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"),
)
if we open at both y and v or we open at u, we get controllers for the different values of the scheduling variable, and the corresponding measurement feedback (which is the same as the scheduling variable in this case).
using Test
@test all(sminreal.(controllersv) .== sminreal.(controllersu))Test PassedHowever, if we only open at y we get controller linearizations that 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₊x(t))[3]")
Symbol("(Cgs₊x(t))[2]")
Symbol("(Cgs₊x(t))[1]")
Symbol("duffing₊v(t)")
Symbol("duffing₊x(t)")notice how the state of the plant is included in the controller, this is an indication that we didn't fully isolate the controller during the linearizaiton. 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]")The 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 easiest way to ensure that the controller is properly disconnected from the plant while taking into account the different scheduling along the trajectory is thus to break at u.
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