44Pages = ["nonlinmpc.md"]
55```
66
7- ``` @setup 1
8- using Logging; errlogger = ConsoleLogger(stderr, Error);
9- old_logger = global_logger(); global_logger(errlogger);
7+ ``` @setup man_nonlin
8+ using Logging;
9+ with_logger(ConsoleLogger(stderr, Error)) do
1010```
1111
1212## Nonlinear Model
@@ -45,7 +45,7 @@ the end of the pendulum in kg, all bundled in the parameter vector ``\mathbf{p}
4545constructor assumes by default that the state function ` f ` is continuous in time, that is,
4646an ordinary differential equation system (like here):
4747
48- ``` @example 1
48+ ``` @example man_nonlin
4949using ModelPredictiveControl
5050function f(x, u, _ , p)
5151 g, L, K, m = p # [m/s²], [m], [kg/s], [kg]
@@ -69,7 +69,7 @@ vector of you want to modify it later. A 4th order [`RungeKutta`](@ref) method s
6969differential equations by default. It is good practice to first simulate ` model ` using
7070[ ` sim! ` ] ( @ref ) as a quick sanity check:
7171
72- ``` @example 1
72+ ``` @example man_nonlin
7373using Plots
7474u = [0.5]
7575N = 35
@@ -88,7 +88,7 @@ method.
8888
8989An [ ` UnscentedKalmanFilter ` ] ( @ref ) estimates the plant state :
9090
91- ``` @example 1
91+ ``` @example man_nonlin
9292α=0.01; σQ=[0.1, 1.0]; σR=[5.0]; nint_u=[1]; σQint_u=[0.1]
9393estim = UnscentedKalmanFilter(model; α, σQ, σR, nint_u, σQint_u)
9494```
@@ -100,7 +100,7 @@ Also, the argument `nint_u` explicitly adds one integrating state at the model i
100100motor torque `` τ `` , with an associated standard deviation ` σQint_u ` of 0.1 N m. The
101101estimator tuning is tested on a plant with a 25 % larger friction coefficient `` K `` :
102102
103- ``` @example 1
103+ ``` @example man_nonlin
104104p_plant = copy(p_model)
105105p_plant[3] = 1.25*p_model[3]
106106plant = setname!(NonLinModel(f, h, Ts, nu, nx, ny; p=p_plant); u=vu, x=vx, y=vy)
@@ -117,7 +117,7 @@ static errors. The Kalman filter performance seems sufficient for control.
117117As the motor torque is limited to -1.5 to 1.5 N m, we incorporate the input constraints in
118118a [ ` NonLinMPC ` ] ( @ref ) :
119119
120- ``` @example 1
120+ ``` @example man_nonlin
121121Hp, Hc, Mwt, Nwt = 20, 2, [0.5], [2.5]
122122nmpc = NonLinMPC(estim; Hp, Hc, Mwt, Nwt, Cwt=Inf)
123123umin, umax = [-1.5], [+1.5]
@@ -127,7 +127,7 @@ nmpc = setconstraint!(nmpc; umin, umax)
127127The option ` Cwt=Inf ` disables the slack variable ` ϵ ` for constraint softening. We test ` mpc `
128128performance on ` plant ` by imposing an angular setpoint of 180° (inverted position):
129129
130- ``` @example 1
130+ ``` @example man_nonlin
131131using JuMP; unset_time_limit_sec(nmpc.optim) # hide
132132res_ry = sim!(nmpc, N, [180.0], plant=plant, x_0=[0, 0], x̂_0=[0, 0, 0])
133133plot(res_ry)
@@ -140,7 +140,7 @@ The controller seems robust enough to variations on ``K`` coefficient. Starting
140140inverted position, the closed-loop response to a step disturbances of 10° is also
141141satisfactory:
142142
143- ``` @example 1
143+ ``` @example man_nonlin
144144res_yd = sim!(nmpc, N, [180.0], plant=plant, x_0=[π, 0], x̂_0=[π, 0, 0], y_step=[10])
145145plot(res_yd)
146146savefig("plot4_NonLinMPC.svg"); nothing # hide
@@ -181,7 +181,7 @@ angle ``θ`` is measured here). As the arguments of [`NonLinMPC`](@ref) economic
181181` JE ` do not include the states, the speed is now defined as an unmeasured output to design a
182182Kalman Filter similar to the previous one (`` \mathbf{y^m} = θ `` and `` \mathbf{y^u} = ω `` ):
183183
184- ``` @example 1
184+ ``` @example man_nonlin
185185h2(x, _ , _ ) = [180/π*x[1], x[2]]
186186nu, nx, ny = 1, 2, 2
187187model2 = setname!(NonLinModel(f, h2, Ts, nu, nx, ny; p=p_model), u=vu, x=vx, y=[vy; vx[2]])
@@ -193,7 +193,7 @@ The `plant2` object based on `h2` is also required since [`sim!`](@ref) expects
193193output vector of ` plant ` argument corresponds to the model output vector in ` mpc ` argument.
194194We can now define the `` J_E `` function and the ` empc ` controller:
195195
196- ``` @example 1
196+ ``` @example man_nonlin
197197function JE(Ue, Ŷe, _ , p)
198198 Ts = p
199199 τ, ω = Ue[1:end-1], Ŷe[2:2:end-1]
@@ -209,7 +209,7 @@ lead to a static error on the angle setpoint. The second element of `Mwt` is zer
209209speed `` ω `` is not requested to track a setpoint. The closed-loop response to a 180°
210210setpoint is similar:
211211
212- ``` @example 1
212+ ``` @example man_nonlin
213213unset_time_limit_sec(empc.optim) # hide
214214res2_ry = sim!(empc, N, [180, 0], plant=plant2, x_0=[0, 0], x̂_0=[0, 0, 0])
215215plot(res2_ry, ploty=[1])
@@ -220,7 +220,7 @@ savefig("plot5_NonLinMPC.svg"); nothing # hide
220220
221221and the energy consumption is slightly lower:
222222
223- ``` @example 1
223+ ``` @example man_nonlin
224224function calcW(res)
225225 τ, ω = res.U_data[1, 1:end-1], res.X_data[2, 1:end-1]
226226 return Ts*sum(τ.*ω)
@@ -230,7 +230,7 @@ Dict(:W_nmpc => calcW(res_ry), :W_empc => calcW(res2_ry))
230230
231231Also, for a 10° step disturbance:
232232
233- ``` @example 1
233+ ``` @example man_nonlin
234234res2_yd = sim!(empc, N, [180; 0]; plant=plant2, x_0=[π, 0], x̂_0=[π, 0, 0], y_step=[10, 0])
235235plot(res2_yd, ploty=[1])
236236savefig("plot6_NonLinMPC.svg"); nothing # hide
@@ -241,7 +241,7 @@ savefig("plot6_NonLinMPC.svg"); nothing # hide
241241the new controller is able to recuperate a little more energy from the pendulum (i.e.
242242negative work):
243243
244- ``` @example 1
244+ ``` @example man_nonlin
245245Dict(:W_nmpc => calcW(res_yd), :W_empc => calcW(res2_yd))
246246```
247247
@@ -268,7 +268,7 @@ in which ``ϵ`` is the slack variable (scalar), an necessary feature to ensure f
268268when the nonlinear inequality constraints are active. There is also an additional ` LHS `
269269argument ("left-hand side" of the inequality above) for the in-place version:
270270
271- ``` @example 1
271+ ``` @example man_nonlin
272272function gc!(LHS, Ue, Ŷe, _, p, ϵ)
273273 Pmax, Hp = p
274274 i_τ, i_ω = 1, 2
@@ -290,7 +290,7 @@ similar mutating forms are also possible for the `f` and `h` functions of [`NonL
290290We construct the controller by enabling relaxation with the ` Cwt ` argument, and also by
291291specifying the number of custom inequality constraints ` nc ` :
292292
293- ``` @example 1
293+ ``` @example man_nonlin
294294Cwt, Pmax, nc = 1e5, 3, Hp+1
295295p_nmpc2 = [Pmax, Hp]
296296nmpc2 = NonLinMPC(estim2; Hp, Hc, Nwt=Nwt, Mwt=[0.5, 0], Cwt, gc!, nc, p=p_nmpc2)
@@ -300,14 +300,14 @@ nmpc2 # hide
300300
301301In addition to the 180° setpoint response:
302302
303- ``` @example 1
304- res3_ry = sim!(nmpc2, N, [180; 0]; plant=plant2, x_0=[0, 0], x̂_0=[0, 0, 0])
303+ ``` @example man_nonlin
304+ res3_ry = sim!(nmpc2, N, [180; 0]; plant=plant2, x_0=[0, 0], x̂_0=[0, 0, 0]);
305305nothing # hide
306306```
307307
308308a plot for the power `` P(t) `` is included below:
309309
310- ``` @example 1
310+ ``` @example man_nonlin
311311function plotWithPower(res, Pmax)
312312 t, τ, ω = res.T_data, res.U_data[1, :], res.X_data[2, :]
313313 P, Pmax = τ.*ω, fill(Pmax, size(t))
@@ -334,13 +334,13 @@ fast dynamics. To ease the design and comparison with [`LinMPC`](@ref), the [`li
334334function allows automatic linearization of [ ` NonLinModel ` ] ( @ref ) based on [ ` ForwardDiff.jl ` ] ( https://juliadiff.org/ForwardDiff.jl/stable/ ) .
335335We first linearize ` model ` at the point `` θ = π `` rad and `` ω = τ = 0 `` (inverted position):
336336
337- ``` @example 1
337+ ``` @example man_nonlin
338338linmodel = linearize(model, x=[π, 0], u=[0])
339339```
340340
341341A [ ` SteadyKalmanFilter ` ] ( @ref ) and a [ ` LinMPC ` ] ( @ref ) are designed from ` linmodel ` :
342342
343- ``` @example 1
343+ ``` @example man_nonlin
344344
345345skf = SteadyKalmanFilter(linmodel; σQ, σR, nint_u, σQint_u)
346346mpc = LinMPC(skf; Hp, Hc, Mwt, Nwt, Cwt=Inf)
@@ -349,7 +349,7 @@ mpc = setconstraint!(mpc, umin=[-1.5], umax=[+1.5])
349349
350350The linear controller satisfactorily rejects the 10° step disturbance:
351351
352- ``` @example 1
352+ ``` @example man_nonlin
353353res_lin = sim!(mpc, N, [180.0]; plant, x_0=[π, 0], y_step=[10])
354354plot(res_lin)
355355savefig("plot9_NonLinMPC.svg"); nothing # hide
@@ -367,7 +367,7 @@ than one):
367367 embedded quadratic programming using recursive LDLᵀ updates. IEEE Trans. Autom. Contr.,
368368 67(8). < https://doi.org/doi:10.1109/TAC.2022.3176430 > .
369369
370- ``` @example 1
370+ ``` @example man_nonlin
371371using LinearAlgebra; poles = eigvals(linmodel.A)
372372```
373373
@@ -379,7 +379,7 @@ using Pkg; Pkg.add("DAQP")
379379
380380Constructing a [ ` LinMPC ` ] ( @ref ) with ` DAQP ` :
381381
382- ``` @example 1
382+ ``` @example man_nonlin
383383using JuMP, DAQP
384384daqp = Model(DAQP.Optimizer, add_bridges=false)
385385mpc2 = LinMPC(skf; Hp, Hc, Mwt, Nwt, Cwt=Inf, optim=daqp)
@@ -388,7 +388,7 @@ mpc2 = setconstraint!(mpc2; umin, umax)
388388
389389does slightly improve the rejection of the step disturbance:
390390
391- ``` @example 1
391+ ``` @example man_nonlin
392392res_lin2 = sim!(mpc2, N, [180.0]; plant, x_0=[π, 0], y_step=[10])
393393plot(res_lin2)
394394savefig("plot10_NonLinMPC.svg"); nothing # hide
@@ -402,7 +402,7 @@ average). However, remember that `linmodel` is only valid for angular positions
402402For example, the 180° setpoint response from 0° is unsatisfactory since the predictions are
403403poor in the first quadrant:
404404
405- ``` @example 1
405+ ``` @example man_nonlin
406406res_lin3 = sim!(mpc2, N, [180.0]; plant, x_0=[0, 0])
407407plot(res_lin3)
408408savefig("plot11_NonLinMPC.svg"); nothing # hide
@@ -422,7 +422,7 @@ be designed with minimal efforts. The [`SteadyKalmanFilter`](@ref) does not supp
422422[ ` setmodel! ` ] ( @ref ) so we need to use the time-varying [ ` KalmanFilter ` ] ( @ref ) , and we
423423initialize it with a linearization at `` θ = ω = τ = 0 `` :
424424
425- ``` @example 1
425+ ``` @example man_nonlin
426426linmodel = linearize(model, x=[0, 0], u=[0])
427427kf = KalmanFilter(linmodel; σQ, σR, nint_u, σQint_u)
428428mpc3 = LinMPC(kf; Hp, Hc, Mwt, Nwt, Cwt=Inf, optim=daqp)
@@ -431,7 +431,7 @@ mpc3 = setconstraint!(mpc3; umin, umax)
431431
432432We create a function that simulates the plant and the adaptive controller:
433433
434- ``` @example 1
434+ ``` @example man_nonlin
435435function sim_adapt!(mpc, nonlinmodel, N, ry, plant, x_0, x̂_0, y_step=[0])
436436 U_data, Y_data, Ry_data = zeros(plant.nu, N), zeros(plant.ny, N), zeros(plant.ny, N)
437437 setstate!(plant, x_0)
@@ -459,7 +459,7 @@ that is, when both ``\mathbf{u}(k)`` and ``\mathbf{x̂}_{k}(k)`` are available a
459459operating point. The [ ` SimResult ` ] ( @ref ) object is for plotting purposes only. The adaptive
460460[ ` LinMPC ` ] ( @ref ) performances are similar to the nonlinear MPC, both for the 180° setpoint:
461461
462- ``` @example 1
462+ ``` @example man_nonlin
463463x_0 = [0, 0]; x̂_0 = [0, 0, 0]; ry = [180]
464464res_slin = sim_adapt!(mpc3, model, N, ry, plant, x_0, x̂_0)
465465plot(res_slin)
@@ -470,7 +470,7 @@ savefig("plot12_NonLinMPC.svg"); nothing # hide
470470
471471and the 10° step disturbance:
472472
473- ``` @example 1
473+ ``` @example man_nonlin
474474x_0 = [π, 0]; x̂_0 = [π, 0, 0]; y_step = [10]
475475res_slin = sim_adapt!(mpc3, model, N, ry, plant, x_0, x̂_0, y_step)
476476plot(res_slin)
@@ -482,6 +482,6 @@ savefig("plot13_NonLinMPC.svg"); nothing # hide
482482The computations of the successive linearization MPC are about 75 times faster than the
483483nonlinear MPC on average, an impressive gain for similar closed-loop performances!
484484
485- ``` @setup 1
486- global_logger(old_logger);
485+ ``` @setup man_nonlin
486+ end
487487```
0 commit comments