From bf5c0ec6cc96ac1576c6c806006cfe690c00caa2 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 1 May 2024 16:33:09 -0400 Subject: [PATCH 1/3] added: `setname!` function for variable names in plot labels doc: use `setname!` on the pendulum --- docs/src/manual/nonlinmpc.md | 13 +++++--- docs/src/public/sim_model.md | 26 +++++++++------ src/ModelPredictiveControl.jl | 3 +- src/model/linearization.jl | 6 +++- src/model/linmodel.jl | 17 +++++++++- src/model/nonlinmodel.jl | 15 ++++++++- src/plot_sim.jl | 63 ++++++++++++++++++++++------------- src/sim_model.jl | 48 +++++++++++++++++++++++--- 8 files changed, 143 insertions(+), 48 deletions(-) diff --git a/docs/src/manual/nonlinmpc.md b/docs/src/manual/nonlinmpc.md index f1b9c3e92..c93a5b7b7 100644 --- a/docs/src/manual/nonlinmpc.md +++ b/docs/src/manual/nonlinmpc.md @@ -53,8 +53,9 @@ end const par = (9.8, 0.4, 1.2, 0.3) f(x, u, _ ) = pendulum(par, x, u) h(x, _ ) = [180/π*x[1]] # [°] -Ts, nu, nx, ny = 0.1, 1, 2, 1 -model = NonLinModel(f, h, Ts, nu, nx, ny) +nu, nx, ny, Ts = 1, 2, 1, 0.1 +vu, vx, vy = ["\$τ\$ (N m)"], ["\$θ\$ (rad)", "\$ω\$ (rad/s)"], ["\$θ\$ (°)"] +model = setname!(NonLinModel(f, h, Ts, nu, nx, ny); u=vu, x=vx, y=vy) ``` The output function ``\mathbf{h}`` converts the ``θ`` angle to degrees. Note that special @@ -72,6 +73,8 @@ plot(res, plotu=false) savefig(ans, "plot1_NonLinMPC.svg"); nothing # hide ``` +The [`setname!`](@ref) function allows customizing the Y-axis labels. + ![plot1_NonLinMPC](plot1_NonLinMPC.svg) ## Nonlinear Model Predictive Controller @@ -93,7 +96,7 @@ estimator tuning is tested on a plant with a 25 % larger friction coefficient `` ```@example 1 const par_plant = (par[1], par[2], 1.25*par[3], par[4]) f_plant(x, u, _ ) = pendulum(par_plant, x, u) -plant = NonLinModel(f_plant, h, Ts, nu, nx, ny) +plant = setname!(NonLinModel(f_plant, h, Ts, nu, nx, ny); u=vu, x=vx, y=vy) res = sim!(estim, N, [0.5], plant=plant, y_noise=[0.5]) plot(res, plotu=false, plotxwithx̂=true) savefig(ans, "plot2_NonLinMPC.svg"); nothing # hide @@ -173,8 +176,8 @@ Kalman Filter similar to the previous one (``\mathbf{y^m} = θ`` and ``\mathbf{y ```@example 1 h2(x, _ ) = [180/π*x[1], x[2]] nu, nx, ny = 1, 2, 2 -model2 = NonLinModel(f , h2, Ts, nu, nx, ny) -plant2 = NonLinModel(f_plant, h2, Ts, nu, nx, ny) +model2 = setname!(NonLinModel(f , h2, Ts, nu, nx, ny), u=vu, x=vx, y=[vy; vx[2]]) +plant2 = setname!(NonLinModel(f_plant, h2, Ts, nu, nx, ny), u=vu, x=vx, y=[vy; vx[2]]) estim2 = UnscentedKalmanFilter(model2; σQ, σR, nint_u, σQint_u, i_ym=[1]) ``` diff --git a/docs/src/public/sim_model.md b/docs/src/public/sim_model.md index a09f61312..6c13b0ebf 100644 --- a/docs/src/public/sim_model.md +++ b/docs/src/public/sim_model.md @@ -29,18 +29,10 @@ LinModel NonLinModel ``` -## Differential Equation Solvers - -### DiffSolver - -```@docs -ModelPredictiveControl.DiffSolver -``` - -### RungeKutta +## Set Variable Names ```@docs -RungeKutta +setname! ``` ## Set Operating Points @@ -55,3 +47,17 @@ setop! linearize linearize! ``` + +## Differential Equation Solvers + +### DiffSolver + +```@docs +ModelPredictiveControl.DiffSolver +``` + +### RungeKutta + +```@docs +RungeKutta +``` diff --git a/src/ModelPredictiveControl.jl b/src/ModelPredictiveControl.jl index 4d8db0522..369d8a8e7 100644 --- a/src/ModelPredictiveControl.jl +++ b/src/ModelPredictiveControl.jl @@ -23,7 +23,8 @@ import OSQP, Ipopt export SimModel, LinModel, NonLinModel export DiffSolver, RungeKutta -export setop!, setstate!, setmodel!, updatestate!, evaloutput, linearize, linearize! +export setop!, setname! +export setstate!, setmodel!, updatestate!, evaloutput, linearize, linearize! export StateEstimator, InternalModel, Luenberger export SteadyKalmanFilter, KalmanFilter, UnscentedKalmanFilter, ExtendedKalmanFilter export MovingHorizonEstimator diff --git a/src/model/linearization.jl b/src/model/linearization.jl index e7ad4fbfa..8f3b0e17e 100644 --- a/src/model/linearization.jl +++ b/src/model/linearization.jl @@ -80,13 +80,17 @@ function linearize(model::SimModel{NT}; kwargs...) where NT<:Real Bd = Matrix{NT}(undef, nx, nd) Dd = Matrix{NT}(undef, ny, nd) linmodel = LinModel{NT}(A, Bu, C, Bd, Dd, model.Ts) + linmodel.uname .= model.uname + linmodel.xname .= model.xname + linmodel.yname .= model.yname + linmodel.dname .= model.dname return linearize!(linmodel, model; kwargs...) end """ linearize!(linmodel::LinModel, model::SimModel; ) -> linmodel -Linearize `model` and store the result in `linmodel`. +Linearize `model` and store the result in `linmodel` (in-place). The keyword arguments are identical to [`linearize`](@ref). diff --git a/src/model/linmodel.jl b/src/model/linmodel.jl index 1ba517837..a119ef1aa 100644 --- a/src/model/linmodel.jl +++ b/src/model/linmodel.jl @@ -15,6 +15,10 @@ struct LinModel{NT<:Real} <: SimModel{NT} dop::Vector{NT} xop::Vector{NT} fop::Vector{NT} + uname::Vector{String} + yname::Vector{String} + dname::Vector{String} + xname::Vector{String} function LinModel{NT}(A, Bu, C, Bd, Dd, Ts) where {NT<:Real} A, Bu = to_mat(A, 1, 1), to_mat(Bu, 1, 1) nu, nx = size(Bu, 2), size(A, 2) @@ -35,8 +39,19 @@ struct LinModel{NT<:Real} <: SimModel{NT} dop = zeros(NT, nd) xop = zeros(NT, nx) fop = zeros(NT, nx) + uname = ["\$u_{$i}\$" for i in 1:nu] + yname = ["\$y_{$i}\$" for i in 1:ny] + dname = ["\$d_{$i}\$" for i in 1:nd] + xname = ["\$x_{$i}\$" for i in 1:nx] x0 = zeros(NT, nx) - return new(A, Bu, C, Bd, Dd, x0, Ts, nu, nx, ny, nd, uop, yop, dop, xop, fop) + return new{NT}( + A, Bu, C, Bd, Dd, + x0, + Ts, + nu, nx, ny, nd, + uop, yop, dop, xop, fop, + uname, yname, dname, xname + ) end end diff --git a/src/model/nonlinmodel.jl b/src/model/nonlinmodel.jl index 0b4dbda65..610d77935 100644 --- a/src/model/nonlinmodel.jl +++ b/src/model/nonlinmodel.jl @@ -13,6 +13,10 @@ struct NonLinModel{NT<:Real, F<:Function, H<:Function, DS<:DiffSolver} <: SimMod dop::Vector{NT} xop::Vector{NT} fop::Vector{NT} + uname::Vector{String} + yname::Vector{String} + dname::Vector{String} + xname::Vector{String} function NonLinModel{NT, F, H, DS}( f!::F, h!::H, solver::DS, Ts, nu, nx, ny, nd ) where {NT<:Real, F<:Function, H<:Function, DS<:DiffSolver} @@ -22,9 +26,18 @@ struct NonLinModel{NT<:Real, F<:Function, H<:Function, DS<:DiffSolver} <: SimMod dop = zeros(NT, nd) xop = zeros(NT, nx) fop = zeros(NT, nx) + uname = ["\$u_{$i}\$" for i in 1:nu] + yname = ["\$y_{$i}\$" for i in 1:ny] + dname = ["\$d_{$i}\$" for i in 1:nd] + xname = ["\$x_{$i}\$" for i in 1:nx] x0 = zeros(NT, nx) return new{NT, F, H, DS}( - x0, f!, h!, solver, Ts, nu, nx, ny, nd, uop, yop, dop, xop, fop + x0, + f!, h!, + solver, Ts, + nu, nx, ny, nd, + uop, yop, dop, xop, fop, + uname, yname, dname, xname ) end end diff --git a/src/plot_sim.jl b/src/plot_sim.jl index ee0cc39f6..42c4aa04e 100644 --- a/src/plot_sim.jl +++ b/src/plot_sim.jl @@ -313,6 +313,11 @@ sim_getu!(::StateEstimator, u, _ , _ , _ ) = u nu = size(res.U_data, 1) nd = size(res.D_data, 1) nx = size(res.X_data, 1) + model = res.obj + uname = model.uname + yname = model.yname + dname = model.dname + xname = model.xname layout_mat = [(ny, 1)] plotu && (layout_mat = [layout_mat (nu, 1)]) (plotd && nd ≠ 0) && (layout_mat = [layout_mat (nd, 1)]) @@ -324,7 +329,7 @@ sim_getu!(::StateEstimator, u, _ , _ , _ ) = u for i in 1:ny @series begin i == ny && (xguide --> "Time (s)") - yguide --> "\$y_$i\$" + yguide --> yname[i] color --> 1 subplot --> subplot_base + i label --> "\$\\mathbf{y}\$" @@ -338,7 +343,7 @@ sim_getu!(::StateEstimator, u, _ , _ , _ ) = u for i in 1:nu @series begin i == nu && (xguide --> "Time (s)") - yguide --> "\$u_$i\$" + yguide --> uname[i] color --> 1 subplot --> subplot_base + i seriestype --> :steppost @@ -354,7 +359,7 @@ sim_getu!(::StateEstimator, u, _ , _ , _ ) = u for i in 1:nd @series begin i == nd && (xguide --> "Time (s)") - yguide --> "\$d_$i\$" + yguide --> dname[i] color --> 1 subplot --> subplot_base + i label --> "\$\\mathbf{d}\$" @@ -369,7 +374,7 @@ sim_getu!(::StateEstimator, u, _ , _ , _ ) = u for i in 1:nx @series begin i == nx && (xguide --> "Time (s)") - yguide --> "\$x_$i\$" + yguide --> xname[i] color --> 1 subplot --> subplot_base + i label --> "\$\\mathbf{x}\$" @@ -396,6 +401,11 @@ end nd = size(res.D_data, 1) nx = size(res.X_data, 1) nx̂ = size(res.X̂_data, 1) + model = res.obj.model + uname = model.uname + yname = model.yname + dname = model.dname + xname = model.xname layout_mat = [(ny, 1)] plotu && (layout_mat = [layout_mat (nu, 1)]) (plotd && nd ≠ 0) && (layout_mat = [layout_mat (nd, 1)]) @@ -407,7 +417,7 @@ end for i in 1:ny @series begin i == ny && (xguide --> "Time (s)") - yguide --> "\$y_$i\$" + yguide --> yname[i] color --> 1 subplot --> subplot_base + i label --> "\$\\mathbf{y}\$" @@ -417,7 +427,7 @@ end if plotŷ @series begin i == ny && (xguide --> "Time (s)") - yguide --> "\$y_$i\$" + yguide --> yname[i] color --> 2 subplot --> subplot_base + i linestyle --> :dashdot @@ -434,7 +444,7 @@ end for i in 1:nu @series begin i == nu && (xguide --> "Time (s)") - yguide --> "\$u_$i\$" + yguide --> uname[i] color --> 1 subplot --> subplot_base + i seriestype --> :steppost @@ -450,7 +460,7 @@ end for i in 1:nd @series begin i == nd && (xguide --> "Time (s)") - yguide --> "\$d_$i\$" + yguide --> dname[i] color --> 1 subplot --> subplot_base + i label --> "\$\\mathbf{d}\$" @@ -465,7 +475,7 @@ end for i in 1:nx @series begin i == nx && !plotxwithx̂ && (xguide --> "Time (s)") - yguide --> "\$x_$i\$" + yguide --> xname[i] color --> 1 subplot --> subplot_base + i label --> "\$\\mathbf{x}\$" @@ -481,11 +491,11 @@ end @series begin i == nx̂ && (xguide --> "Time (s)") withPlantState = plotxwithx̂ && i ≤ nx - yguide --> (withPlantState ? "\$x_$i\$" : "\$\\hat{x}_$i\$") + yguide --> (withPlantState ? xname[i] : "\$\\hat{x}_{$i}\$") color --> 2 subplot --> subplot_base + i - linestyle --> :dashdot - linewidth --> 0.75 + linestyle --> :dashdot + linewidth --> 0.75 label --> "\$\\mathbf{\\hat{x}}\$" legend --> (withPlantState ? true : false) t, res.X̂_data[i, :] @@ -517,6 +527,11 @@ end nd = size(res.D_data, 1) nx = size(res.X_data, 1) nx̂ = size(res.X̂_data, 1) + model = res.obj.estim.model + uname = model.uname + yname = model.yname + dname = model.dname + xname = model.xname layout_mat = [(ny, 1)] plotu && (layout_mat = [layout_mat (nu, 1)]) (plotd && nd ≠ 0) && (layout_mat = [layout_mat (nd, 1)]) @@ -531,7 +546,7 @@ end for i in 1:ny @series begin i == ny && (xguide --> "Time (s)") - yguide --> "\$y_$i\$" + yguide --> yname[i] color --> 1 subplot --> subplot_base + i label --> "\$\\mathbf{y}\$" @@ -541,7 +556,7 @@ end if plotŷ @series begin i == ny && (xguide --> "Time (s)") - yguide --> "\$y_$i\$" + yguide --> yname[i] color --> 2 subplot --> subplot_base + i linestyle --> :dashdot @@ -554,7 +569,7 @@ end if plotry && !iszero(mpc.M_Hp[i, i]) @series begin i == ny && (xguide --> "Time (s)") - yguide --> "\$y_$i\$" + yguide --> yname[i] color --> 3 subplot --> subplot_base + i linestyle --> :dash @@ -567,7 +582,7 @@ end if plotymin && !isinf(Ymin[i]) @series begin i == ny && (xguide --> "Time (s)") - yguide --> "\$y_$i\$" + yguide --> yname[i] color --> 4 subplot --> subplot_base + i linestyle --> :dot @@ -580,7 +595,7 @@ end if plotymax && !isinf(Ymax[i]) @series begin i == ny && (xguide --> "Time (s)") - yguide --> "\$y_$i\$" + yguide --> yname[i] color --> 5 subplot --> subplot_base + i linestyle --> :dot @@ -597,7 +612,7 @@ end for i in 1:nu @series begin i == nu && (xguide --> "Time (s)") - yguide --> "\$u_$i\$" + yguide --> uname[i] color --> 1 subplot --> subplot_base + i seriestype --> :steppost @@ -608,7 +623,7 @@ end if plotru && !iszero(mpc.L_Hp[i, i]) @series begin i == nu && (xguide --> "Time (s)") - yguide --> "\$u_$i\$" + yguide --> uname[i] color --> 3 subplot --> subplot_base + i seriestype --> :steppost @@ -621,7 +636,7 @@ end if plotumin && !isinf(Umin[i]) @series begin i == nu && (xguide --> "Time (s)") - yguide --> "\$u_$i\$" + yguide --> uname[i] color --> 4 subplot --> subplot_base + i linestyle --> :dot @@ -634,7 +649,7 @@ end if plotumax && !isinf(Umax[i]) @series begin i == nu && (xguide --> "Time (s)") - yguide --> "\$u_$i\$" + yguide --> uname[i] color --> 5 subplot --> subplot_base + i linestyle --> :dot @@ -652,7 +667,7 @@ end for i in 1:nd @series begin i == nd && (xguide --> "Time (s)") - yguide --> "\$d_$i\$" + yguide --> dname[i] color --> 1 subplot --> subplot_base + i label --> "\$\\mathbf{d}\$" @@ -667,7 +682,7 @@ end for i in 1:nx @series begin i == nx && !plotxwithx̂ && (xguide --> "Time (s)") - yguide --> "\$x_$i\$" + yguide --> xname[i] color --> 1 subplot --> subplot_base + i label --> "\$\\mathbf{x}\$" @@ -683,7 +698,7 @@ end @series begin i == nx̂ && (xguide --> "Time (s)") withPlantState = plotxwithx̂ && i ≤ nx - yguide --> (withPlantState ? "\$x_$i\$" : "\$\\hat{x}_$i\$") + yguide --> (withPlantState ? xname[i] : "\$\\hat{x}_{$i}\$") color --> 2 subplot --> subplot_base + i linestyle --> :dashdot diff --git a/src/sim_model.jl b/src/sim_model.jl index 34eb6aec8..abed4f854 100644 --- a/src/sim_model.jl +++ b/src/sim_model.jl @@ -75,23 +75,61 @@ function setop!( ) if !isnothing(uop) size(uop) == (model.nu,) || error("uop size must be $((model.nu,))") - model.uop[:] = uop + model.uop .= uop end if !isnothing(yop) size(yop) == (model.ny,) || error("yop size must be $((model.ny,))") - model.yop[:] = yop + model.yop .= yop end if !isnothing(dop) size(dop) == (model.nd,) || error("dop size must be $((model.nd,))") - model.dop[:] = dop + model.dop .= dop end if !isnothing(xop) size(xop) == (model.nx,) || error("xop size must be $((model.nx,))") - model.xop[:] = xop + model.xop .= xop end if !isnothing(fop) size(fop) == (model.nx,) || error("fop size must be $((model.nx,))") - model.fop[:] = fop + model.fop .= fop + end + return model +end + +@doc raw""" + setname!(model::SimModel; u=nothing, y=nothing, d=nothing, x=nothing) -> model + +Set the names of `model` inputs `u`, outputs `y`, disturbances `d`, and states `x`. + +The keyword arguments `u`, `y`, `d`, and `x` must be vectors of strings. The strings are +used in the plotting functions. + +# Examples +```jldoctest +model = setname!(LinModel(tf(3, [10, 1]), 2.0), u=[raw"$A$ (%)"], y=[raw"$T$ (∘C)"]) +LinModel with a sample time Ts = 2.0 s and: + 1 manipulated inputs u + 1 states x + 1 outputs y + 0 measured disturbances d +``` +""" +function setname!(model::SimModel; u=nothing, y=nothing, d=nothing, x=nothing) + if !isnothing(u) + size(u) == (model.nu,) || error("u size must be $((model.nu,))") + model.uname .= u + end + if !isnothing(y) + size(y) == (model.ny,) || error("y size must be $((model.ny,))") + model.yname .= y + end + if !isnothing(d) + size(d) == (model.nd,) || error("d size must be $((model.nd,))") + model.dname .= d + end + if !isnothing(x) + size(x) == (model.nx,) || error("x size must be $((model.nx,))") + model.xname .= x end return model end From bb16dbfee5985ab9a49af1212d0a78a90470996f Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 1 May 2024 16:36:38 -0400 Subject: [PATCH 2/3] minor doc correction --- src/sim_model.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/sim_model.jl b/src/sim_model.jl index abed4f854..910fc63dd 100644 --- a/src/sim_model.jl +++ b/src/sim_model.jl @@ -106,7 +106,7 @@ used in the plotting functions. # Examples ```jldoctest -model = setname!(LinModel(tf(3, [10, 1]), 2.0), u=[raw"$A$ (%)"], y=[raw"$T$ (∘C)"]) +model = setname!(LinModel(tf(3, [10, 1]), 2.0), u=["\$A\$ (%)"], y=["\$T\$ (∘C)"]) LinModel with a sample time Ts = 2.0 s and: 1 manipulated inputs u 1 states x From 0c84034ad0e6f4d75a3db3d6b69ca7c2fadb5a2f Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 1 May 2024 17:09:51 -0400 Subject: [PATCH 3/3] debug tests --- src/sim_model.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/sim_model.jl b/src/sim_model.jl index 910fc63dd..1896db06f 100644 --- a/src/sim_model.jl +++ b/src/sim_model.jl @@ -106,7 +106,7 @@ used in the plotting functions. # Examples ```jldoctest -model = setname!(LinModel(tf(3, [10, 1]), 2.0), u=["\$A\$ (%)"], y=["\$T\$ (∘C)"]) +julia> model = setname!(LinModel(tf(3, [10, 1]), 2.0), u=["\$A\$ (%)"], y=["\$T\$ (∘C)"]) LinModel with a sample time Ts = 2.0 s and: 1 manipulated inputs u 1 states x