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
using ModelingToolkit: getdefault
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 = ODESystem(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 u_{+}u\left( t \right) - c v\left( t \right) - k x\left( t \right) - \left( x\left( t \right) \right)^{3} k3 \\ 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 = 16 # Number of samples
xs = range(getbounds(x)[1], getbounds(x)[2], length=N)
ops = Dict.(x .=> xs)16-element Vector{Dict{Num, Float64}}:
Dict(x(t) => -0.5)
Dict(x(t) => -0.36666666666666664)
Dict(x(t) => -0.23333333333333334)
Dict(x(t) => -0.1)
Dict(x(t) => 0.03333333333333333)
Dict(x(t) => 0.16666666666666666)
Dict(x(t) => 0.3)
Dict(x(t) => 0.43333333333333335)
Dict(x(t) => 0.5666666666666667)
Dict(x(t) => 0.7)
Dict(x(t) => 0.8333333333333334)
Dict(x(t) => 0.9666666666666667)
Dict(x(t) => 1.1)
Dict(x(t) => 1.2333333333333334)
Dict(x(t) => 1.3666666666666667)
Dict(x(t) => 1.5)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, 16}}
A =
0.0 1.0
-13.8 ± 4.3 -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) # 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.
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)
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 OrdinaryDiffEq
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, 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 = ODESystem(C)
eqs = [
closed_loop_eqs
connect(fb.output, Ci.input)
connect(Ci.output, duffing.u)
]
@named closed_loop = ODESystem(eqs, t, systems=[duffing, Ci, fb, ref, F])
prob = ODEProblem(structural_simplify(closed_loop), [], (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, duffing.u)
connect(duffing.y, Cgs.scheduling_input) # Don't forget to connect the scheduling variable!
]
@named closed_loop = ODESystem(eqs, t, systems=[duffing, Cgs, fb, ref, F])
prob = ODEProblem(structural_simplify(closed_loop), [], (0.0, 8.0))
sol = solve(prob, Rodas5P(), abstol=1e-8, reltol=1e-8)
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 better than each of the included controllers individually.
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:7:end] and xs[1:7: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:7:end], xs[1:7:end], "Cs")double Cs_interp_vect[3];
Cs_interp_vect[0] = -0.5;
Cs_interp_vect[1] = 0.43333333333333335;
Cs_interp_vect[2] = 1.3666666666666667;
double Cs_A_interp[3][3][3];
Cs_A_interp[0][0][0] = 1.0;
Cs_A_interp[0][0][1] = 1.0;
Cs_A_interp[0][0][2] = 1.0;
Cs_A_interp[0][1][0] = 0.08410402202598245;
Cs_A_interp[0][1][1] = 0.08410402202598245;
Cs_A_interp[0][1][2] = 0.1682080440519649;
Cs_A_interp[0][2][0] = 0.05933874072269696;
Cs_A_interp[0][2][1] = 0.05933874072269696;
Cs_A_interp[0][2][2] = 0.11867748144539392;
Cs_A_interp[1][0][0] = 0.0;
Cs_A_interp[1][0][1] = 0.0;
Cs_A_interp[1][0][2] = 0.0;
Cs_A_interp[1][1][0] = -0.15895977974017558;
Cs_A_interp[1][1][1] = -0.15895977974017558;
Cs_A_interp[1][1][2] = -0.15895977974017558;
Cs_A_interp[1][2][0] = 0.5933874072269696;
Cs_A_interp[1][2][1] = 0.5933874072269696;
Cs_A_interp[1][2][2] = 0.5933874072269696;
Cs_A_interp[2][0][0] = -0.0;
Cs_A_interp[2][0][1] = -0.0;
Cs_A_interp[2][0][2] = -0.0;
Cs_A_interp[2][1][0] = -0.3621749311688048;
Cs_A_interp[2][1][1] = -0.3621749311688048;
Cs_A_interp[2][1][2] = -0.3621749311688048;
Cs_A_interp[2][2][0] = -0.8145664352415717;
Cs_A_interp[2][2][1] = -0.8145664352415717;
Cs_A_interp[2][2][2] = -0.8145664352415717;
void interpolate_Cs_A(double *Cs_A, double *Cs_A_interp, double *Cs_interp_vect, double t) {
int k = 0;
for (k = 0; k < 1; ++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[2]) {
alpha = 1;
}
for (int i = 0; i < 3; ++i) { // Cs_A has dimensions (3, 3)
for (int j = 0; j < 3; ++j) { // Cs_A_interp has dimensions (3, 3, 3)
Cs_A[i][j] = (1-alpha)*Cs_A_interp[i][j][k] + alpha*Cs_A_interp[i][j][k+1];
}
}
}
double Cs_B_interp[3][1][3];
Cs_B_interp[0][0][0] = 0.18988397031263038;
Cs_B_interp[0][0][1] = 0.18988397031263038;
Cs_B_interp[0][0][2] = 0.37976794062526076;
Cs_B_interp[1][0][0] = 1.8988397031263036;
Cs_B_interp[1][0][1] = 1.8988397031263036;
Cs_B_interp[1][0][2] = 1.8988397031263036;
Cs_B_interp[2][0][0] = 0.5933874072269698;
Cs_B_interp[2][0][1] = 0.5933874072269698;
Cs_B_interp[2][0][2] = 0.5933874072269698;
void interpolate_Cs_B(double *Cs_B, double *Cs_B_interp, double *Cs_interp_vect, double t) {
int k = 0;
for (k = 0; k < 1; ++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[2]) {
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, 3)
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][3];
Cs_C_interp[0][0][0] = 7.610511654602729;
Cs_C_interp[0][0][1] = 7.535642178914856;
Cs_C_interp[0][0][2] = 4.882924724484842;
Cs_C_interp[0][1][0] = -29.905306701232092;
Cs_C_interp[0][1][1] = -29.961780517651928;
Cs_C_interp[0][1][2] = -28.823167168374564;
Cs_C_interp[0][2][0] = 19.017886742939247;
Cs_C_interp[0][2][1] = 19.06972543674406;
Cs_C_interp[0][2][2] = 17.87849175640801;
void interpolate_Cs_C(double *Cs_C, double *Cs_C_interp, double *Cs_interp_vect, double t) {
int k = 0;
for (k = 0; k < 1; ++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[2]) {
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, 3)
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][3];
Cs_D_interp[0][0][0] = 60.85723757740559;
Cs_D_interp[0][0][1] = 61.023121397581;
Cs_D_interp[0][0][2] = 57.21117362050563;
void interpolate_Cs_D(double *Cs_D, double *Cs_D_interp, double *Cs_interp_vect, double t) {
int k = 0;
for (k = 0; k < 1; ++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[2]) {
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, 3)
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 = trajectory_ss(closed_loop, :r, :y, sol; t=timepoints)
bodeplot(Ps2, w, legend=false)
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.
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.
using Test
@test sol(6.99, idxs=closed_loop.duffing.y.u) ≈ 0.0 atol=0.01Test Passed