diff --git a/src/controller/construct.jl b/src/controller/construct.jl index 17fe3da40..89103e84b 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -1,3 +1,31 @@ +"Include all the objective function weights of [`PredictiveController`](@ref)" +struct ControllerWeights{NT<:Real} + M_Hp::Hermitian{NT, Matrix{NT}} + Ñ_Hc::Hermitian{NT, Matrix{NT}} + L_Hp::Hermitian{NT, Matrix{NT}} + E ::NT + function ControllerWeights{NT}( + model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt=Inf, Ewt=0 + ) where NT<:Real + validate_weights(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt) + # convert `Diagonal` to normal `Matrix` if required: + M_Hp = Hermitian(convert(Matrix{NT}, M_Hp), :L) + N_Hc = Hermitian(convert(Matrix{NT}, N_Hc), :L) + L_Hp = Hermitian(convert(Matrix{NT}, L_Hp), :L) + nΔU = size(N_Hc, 1) + C = Cwt + if !isinf(Cwt) + # ΔŨ = [ΔU; ϵ] (ϵ is the slack variable) + Ñ_Hc = Hermitian([N_Hc zeros(NT, nΔU, 1); zeros(NT, 1, nΔU) C], :L) + else + # ΔŨ = ΔU (only hard constraints) + Ñ_Hc = N_Hc + end + E = Ewt + return new{NT}(M_Hp, Ñ_Hc, L_Hp, E) + end +end + "Include all the data for the constraints of [`PredictiveController`](@ref)" struct ControllerConstraint{NT<:Real, GCfunc<:Function} ẽx̂ ::Matrix{NT} @@ -609,7 +637,7 @@ function init_predmat(estim::StateEstimator{NT}, model::SimModel, Hp, Hc) where end @doc raw""" - init_quadprog(model::LinModel, Ẽ, S, M_Hp, N_Hc, L_Hp) -> H̃ + init_quadprog(model::LinModel, weights::ControllerWeights, Ẽ, S) -> H̃ Init the quadratic programming Hessian `H̃` for MPC. @@ -626,29 +654,30 @@ The vector ``\mathbf{q̃}`` and scalar ``r`` need recalculation each control per [`initpred!`](@ref). ``r`` does not impact the minima position. It is thus useless at optimization but required to evaluate the minimal ``J`` value. """ -function init_quadprog(::LinModel, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp) +function init_quadprog(::LinModel, weights::ControllerWeights, Ẽ, S̃) + M_Hp, Ñ_Hc, L_Hp = weights.M_Hp, weights.Ñ_Hc, weights.L_Hp H̃ = Hermitian(2*(Ẽ'*M_Hp*Ẽ + Ñ_Hc + S̃'*L_Hp*S̃), :L) return H̃ end -"Return empty matrices if `model` is not a [`LinModel`](@ref)." -function init_quadprog(::SimModel{NT}, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp) where {NT<:Real} +"Return empty matrix if `model` is not a [`LinModel`](@ref)." +function init_quadprog(::SimModel{NT}, weights::ControllerWeights, _, _) where {NT<:Real} H̃ = Hermitian(zeros(NT, 0, 0), :L) return H̃ end """ init_defaultcon_mpc( - estim, C, S, N_Hc, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, + estim, C, S, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, gc!=(_,_,_,_,_,_)->nothing, nc=0 - ) -> con, S̃, Ñ_Hc, Ẽ + ) -> con, S̃, Ẽ Init `ControllerConstraint` struct with default parameters based on estimator `estim`. -Also return `S̃`, `Ñ_Hc` and `Ẽ` matrices for the the augmented decision vector `ΔŨ`. +Also return `S̃` and `Ẽ` matrices for the the augmented decision vector `ΔŨ`. """ function init_defaultcon_mpc( estim::StateEstimator{NT}, - Hp, Hc, C, S, N_Hc, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂, + Hp, Hc, C, S, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂, gc!::GCfunc=(_,_,_,_,_,_)->nothing, nc=0 ) where {NT<:Real, GCfunc<:Function} model = estim.model @@ -667,9 +696,7 @@ function init_defaultcon_mpc( C_umin, C_umax, C_Δumin, C_Δumax, C_ymin, C_ymax = repeat_constraints(Hp, Hc, c_umin, c_umax, c_Δumin, c_Δumax, c_ymin, c_ymax) A_Umin, A_Umax, S̃ = relaxU(model, nϵ, C_umin, C_umax, S) - A_ΔŨmin, A_ΔŨmax, ΔŨmin, ΔŨmax, Ñ_Hc = relaxΔU( - model, nϵ, C, C_Δumin, C_Δumax, ΔUmin, ΔUmax, N_Hc - ) + A_ΔŨmin, A_ΔŨmax, ΔŨmin, ΔŨmax = relaxΔU(model, nϵ, C, C_Δumin, C_Δumax, ΔUmin, ΔUmax) A_Ymin, A_Ymax, Ẽ = relaxŶ(model, nϵ, C_ymin, C_ymax, E) A_x̂min, A_x̂max, ẽx̂ = relaxterminal(model, nϵ, c_x̂min, c_x̂max, ex̂) i_Umin, i_Umax = .!isinf.(U0min), .!isinf.(U0max) @@ -689,7 +716,7 @@ function init_defaultcon_mpc( A , b , i_b , C_ymin , C_ymax , c_x̂min , c_x̂max , i_g, gc! , nc ) - return con, nϵ, S̃, Ñ_Hc, Ẽ + return con, nϵ, S̃, Ẽ end "Repeat predictive controller constraints over prediction `Hp` and control `Hc` horizons." @@ -742,17 +769,14 @@ function relaxU(::SimModel{NT}, nϵ, C_umin, C_umax, S) where NT<:Real end @doc raw""" - relaxΔU( - model, nϵ, C, C_Δumin, C_Δumax, ΔUmin, ΔUmax, N_Hc - ) -> A_ΔŨmin, A_ΔŨmax, ΔŨmin, ΔŨmax, Ñ_Hc + relaxΔU(model, nϵ, C, C_Δumin, C_Δumax, ΔUmin, ΔUmax) -> A_ΔŨmin, A_ΔŨmax, ΔŨmin, ΔŨmax Augment input increments constraints with slack variable ϵ for softening. Denoting the input increments augmented with the slack variable ``\mathbf{ΔŨ} = [\begin{smallmatrix} \mathbf{ΔU} \\ ϵ \end{smallmatrix}]``, it returns the -augmented input increment weights ``\mathbf{Ñ}_{H_c}`` (that incorporate ``C``). It also -returns the augmented constraints ``\mathbf{ΔŨ_{min}}`` and ``\mathbf{ΔŨ_{max}}`` and the -``\mathbf{A}`` matrices for the inequality constraints: +augmented constraints ``\mathbf{ΔŨ_{min}}`` and ``\mathbf{ΔŨ_{max}}`` and the ``\mathbf{A}`` +matrices for the inequality constraints: ```math \begin{bmatrix} \mathbf{A_{ΔŨ_{min}}} \\ @@ -764,21 +788,19 @@ returns the augmented constraints ``\mathbf{ΔŨ_{min}}`` and ``\mathbf{ΔŨ_{ \end{bmatrix} ``` """ -function relaxΔU(::SimModel{NT}, nϵ, C, C_Δumin, C_Δumax, ΔUmin, ΔUmax, N_Hc) where NT<:Real - nΔU = size(N_Hc, 1) +function relaxΔU(::SimModel{NT}, nϵ, C, C_Δumin, C_Δumax, ΔUmin, ΔUmax) where NT<:Real + nΔU = length(ΔUmin) if nϵ == 1 # ΔŨ = [ΔU; ϵ] # 0 ≤ ϵ ≤ ∞ ΔŨmin, ΔŨmax = [ΔUmin; NT[0.0]], [ΔUmax; NT[Inf]] - A_ϵ = [zeros(NT, 1, length(ΔUmin)) NT[1.0]] + A_ϵ = [zeros(NT, 1, nΔU) NT[1.0]] A_ΔŨmin, A_ΔŨmax = -[I C_Δumin; A_ϵ], [I -C_Δumax; A_ϵ] - Ñ_Hc = Hermitian([N_Hc zeros(NT, nΔU, 1);zeros(NT, 1, nΔU) C], :L) else # ΔŨ = ΔU (only hard constraints) ΔŨmin, ΔŨmax = ΔUmin, ΔUmax I_Hc = Matrix{NT}(I, nΔU, nΔU) A_ΔŨmin, A_ΔŨmax = -I_Hc, I_Hc - Ñ_Hc = N_Hc end - return A_ΔŨmin, A_ΔŨmax, ΔŨmin, ΔŨmax, Ñ_Hc + return A_ΔŨmin, A_ΔŨmax, ΔŨmin, ΔŨmax end @doc raw""" diff --git a/src/controller/execute.jl b/src/controller/execute.jl index 040853da2..7044fc63b 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -206,15 +206,15 @@ function initpred!(mpc::PredictiveController, model::LinModel, d, D̂, R̂y, R̂ end mpc.R̂y .= R̂y Cy = F .- (R̂y .- mpc.Yop) - M_Hp_Ẽ = mpc.M_Hp*mpc.Ẽ + M_Hp_Ẽ = mpc.weights.M_Hp*mpc.Ẽ mul!(q̃, M_Hp_Ẽ', Cy) - r .= dot(Cy, mpc.M_Hp, Cy) + r .= dot(Cy, mpc.weights.M_Hp, Cy) if ~mpc.noR̂u mpc.R̂u .= R̂u Cu = mpc.T_lastu0 .- (R̂u .- mpc.Uop) - L_Hp_S̃ = mpc.L_Hp*mpc.S̃ + L_Hp_S̃ = mpc.weights.L_Hp*mpc.S̃ mul!(q̃, L_Hp_S̃', Cu, 1, 1) - r .+= dot(Cu, mpc.L_Hp, Cu) + r .+= dot(Cu, mpc.weights.L_Hp, Cu) end lmul!(2, q̃) return nothing @@ -414,14 +414,14 @@ function obj_nonlinprog!( # --- output setpoint tracking term --- Ȳ .= @views Ŷe[ny+1:end] Ȳ .= mpc.R̂y .- Ȳ - JR̂y = dot(Ȳ, mpc.M_Hp, Ȳ) + JR̂y = dot(Ȳ, mpc.weights.M_Hp, Ȳ) # --- move suppression and slack variable term --- - JΔŨ = dot(ΔŨ, mpc.Ñ_Hc, ΔŨ) + JΔŨ = dot(ΔŨ, mpc.weights.Ñ_Hc, ΔŨ) # --- input setpoint tracking term --- if !mpc.noR̂u Ū .= @views Ue[1:end-nu] Ū .= mpc.R̂u .- Ū - JR̂u = dot(Ū, mpc.L_Hp, Ū) + JR̂u = dot(Ū, mpc.weights.L_Hp, Ū) else JR̂u = 0.0 end @@ -589,12 +589,12 @@ prediction horizon ``H_p``. ```jldoctest julia> mpc = LinMPC(KalmanFilter(LinModel(ss(0.1, 0.5, 1, 0, 4.0)), σR=[√25]), Hp=1, Hc=1); -julia> mpc.estim.model.A[1], mpc.estim.R̂[1], mpc.M_Hp[1], mpc.Ñ_Hc[1] +julia> mpc.estim.model.A[1], mpc.estim.R̂[1], mpc.weights.M_Hp[1], mpc.weights.Ñ_Hc[1] (0.1, 25.0, 1.0, 0.1) julia> setmodel!(mpc, LinModel(ss(0.42, 0.5, 1, 0, 4.0)); R̂=[9], M_Hp=[10], Nwt=[0.666]); -julia> mpc.estim.model.A[1], mpc.estim.R̂[1], mpc.M_Hp[1], mpc.Ñ_Hc[1] +julia> mpc.estim.model.A[1], mpc.estim.R̂[1], mpc.weights.M_Hp[1], mpc.weights.Ñ_Hc[1] (0.42, 9.0, 10.0, 0.666) ``` """ @@ -617,44 +617,44 @@ function setmodel!( size(Mwt) == (ny,) || throw(ArgumentError("Mwt should be a vector of length $ny")) any(x -> x < 0, Mwt) && throw(ArgumentError("Mwt values should be nonnegative")) for i=1:ny*Hp - mpc.M_Hp[i, i] = Mwt[(i-1) % ny + 1] + mpc.weights.M_Hp[i, i] = Mwt[(i-1) % ny + 1] end elseif !isnothing(M_Hp) M_Hp = to_hermitian(M_Hp) nŶ = ny*Hp size(M_Hp) == (nŶ, nŶ) || throw(ArgumentError("M_Hp size should be ($nŶ, $nŶ)")) - mpc.M_Hp .= M_Hp + mpc.weights.M_Hp .= M_Hp end if isnothing(Ñ_Hc) && !isnothing(Nwt) size(Nwt) == (nu,) || throw(ArgumentError("Nwt should be a vector of length $nu")) any(x -> x < 0, Nwt) && throw(ArgumentError("Nwt values should be nonnegative")) for i=1:nu*Hc - mpc.Ñ_Hc[i, i] = Nwt[(i-1) % nu + 1] + mpc.weights.Ñ_Hc[i, i] = Nwt[(i-1) % nu + 1] end elseif !isnothing(Ñ_Hc) Ñ_Hc = to_hermitian(Ñ_Hc) nΔŨ = nu*Hc+nϵ size(Ñ_Hc) == (nΔŨ, nΔŨ) || throw(ArgumentError("Ñ_Hc size should be ($nΔŨ, $nΔŨ)")) - mpc.Ñ_Hc .= Ñ_Hc + mpc.weights.Ñ_Hc .= Ñ_Hc end if isnothing(L_Hp) && !isnothing(Lwt) size(Lwt) == (nu,) || throw(ArgumentError("Lwt should be a vector of length $nu")) any(x -> x < 0, Lwt) && throw(ArgumentError("Lwt values should be nonnegative")) for i=1:nu*Hp - mpc.L_Hp[i, i] = Lwt[(i-1) % nu + 1] + mpc.weights.L_Hp[i, i] = Lwt[(i-1) % nu + 1] end elseif !isnothing(L_Hp) L_Hp = to_hermitian(L_Hp) nU = nu*Hp size(L_Hp) == (nU, nU) || throw(ArgumentError("L_Hp size should be ($nU, $nU)")) - mpc.L_Hp .= L_Hp + mpc.weights.L_Hp .= L_Hp end - setmodel_controller!(mpc, x̂op_old, M_Hp, Ñ_Hc, L_Hp) + setmodel_controller!(mpc, x̂op_old) return mpc end "Update the prediction matrices, linear constraints and JuMP optimization." -function setmodel_controller!(mpc::PredictiveController, x̂op_old, M_Hp, Ñ_Hc, L_Hp) +function setmodel_controller!(mpc::PredictiveController, x̂op_old) estim, model = mpc.estim, mpc.estim.model nu, ny, nd, Hp, Hc = model.nu, model.ny, model.nd, mpc.Hp, mpc.Hc optim, con = mpc.optim, mpc.con @@ -715,7 +715,7 @@ function setmodel_controller!(mpc::PredictiveController, x̂op_old, M_Hp, Ñ_Hc JuMP.unregister(optim, :linconstraint) @constraint(optim, linconstraint, A*ΔŨvar .≤ b) # --- quadratic programming Hessian matrix --- - H̃ = init_quadprog(model, mpc.Ẽ, mpc.S̃, mpc.M_Hp, mpc.Ñ_Hc, mpc.L_Hp) + H̃ = init_quadprog(model, mpc.weights, mpc.Ẽ, mpc.S̃) mpc.H̃ .= H̃ set_objective_hessian!(mpc, ΔŨvar) return nothing diff --git a/src/controller/explicitmpc.jl b/src/controller/explicitmpc.jl index 00e6e5e6b..1d83d1db4 100644 --- a/src/controller/explicitmpc.jl +++ b/src/controller/explicitmpc.jl @@ -5,10 +5,7 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT} Hp::Int Hc::Int nϵ::Int - M_Hp::Hermitian{NT, Matrix{NT}} - Ñ_Hc::Hermitian{NT, Matrix{NT}} - L_Hp::Hermitian{NT, Matrix{NT}} - E::NT + weights::ControllerWeights{NT} R̂u::Vector{NT} R̂y::Vector{NT} noR̂u::Bool @@ -42,8 +39,7 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT} nu, ny, nd, nx̂ = model.nu, model.ny, model.nd, estim.nx̂ ŷ = copy(model.yop) # dummy vals (updated just before optimization) nϵ = 0 # no slack variable ϵ for ExplicitMPC - Ewt = 0 # economic costs not supported for ExplicitMPC - validate_weights(model, Hp, Hc, M_Hp, N_Hc, L_Hp) + weights = ControllerWeights{NT}(model, Hp, Hc, M_Hp, N_Hc, L_Hp) # convert `Diagonal` to normal `Matrix` if required: M_Hp = Hermitian(convert(Matrix{NT}, M_Hp), :L) N_Hc = Hermitian(convert(Matrix{NT}, N_Hc), :L) @@ -56,7 +52,7 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT} # dummy val (updated just before optimization): F, fx̂ = zeros(NT, ny*Hp), zeros(NT, nx̂) S̃, Ñ_Hc, Ẽ = S, N_Hc, E # no slack variable ϵ for ExplicitMPC - H̃ = init_quadprog(model, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp) + H̃ = init_quadprog(model, weights ,Ẽ, S̃) # dummy vals (updated just before optimization): q̃, r = zeros(NT, size(H̃, 1)), zeros(NT, 1) H̃_chol = cholesky(H̃) @@ -71,7 +67,7 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT} estim, ΔŨ, ŷ, Hp, Hc, nϵ, - M_Hp, Ñ_Hc, L_Hp, Ewt, + weights, R̂u, R̂y, noR̂u, S̃, T, T_lastu0, Ẽ, F, G, J, K, V, B, @@ -211,7 +207,7 @@ addinfo!(info, mpc::ExplicitMPC) = info "Update the prediction matrices and Cholesky factorization." -function setmodel_controller!(mpc::ExplicitMPC, _ , M_Hp, Ñ_Hc, L_Hp) +function setmodel_controller!(mpc::ExplicitMPC, _ ) estim, model = mpc.estim, mpc.estim.model nu, ny, nd, Hp, Hc = model.nu, model.ny, model.nd, mpc.Hp, mpc.Hc # --- predictions matrices --- @@ -224,7 +220,7 @@ function setmodel_controller!(mpc::ExplicitMPC, _ , M_Hp, Ñ_Hc, L_Hp) mpc.V .= V mpc.B .= B # --- quadratic programming Hessian matrix --- - H̃ = init_quadprog(model, mpc.Ẽ, mpc.S̃, mpc.M_Hp, mpc.Ñ_Hc, mpc.L_Hp) + H̃ = init_quadprog(model, mpc.weights, mpc.Ẽ, mpc.S̃) mpc.H̃ .= H̃ set_objective_hessian!(mpc) # --- operating points --- diff --git a/src/controller/linmpc.jl b/src/controller/linmpc.jl index b9500a915..66e428876 100644 --- a/src/controller/linmpc.jl +++ b/src/controller/linmpc.jl @@ -15,10 +15,7 @@ struct LinMPC{ Hp::Int Hc::Int nϵ::Int - M_Hp::Hermitian{NT, Matrix{NT}} - Ñ_Hc::Hermitian{NT, Matrix{NT}} - L_Hp::Hermitian{NT, Matrix{NT}} - E::NT + weights::ControllerWeights{NT} R̂u::Vector{NT} R̂y::Vector{NT} noR̂u::Bool @@ -50,12 +47,7 @@ struct LinMPC{ model = estim.model nu, ny, nd, nx̂ = model.nu, model.ny, model.nd, estim.nx̂ ŷ = copy(model.yop) # dummy vals (updated just before optimization) - Ewt = 0 # economic costs not supported for LinMPC - validate_weights(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt) - # convert `Diagonal` to normal `Matrix` if required: - M_Hp = Hermitian(convert(Matrix{NT}, M_Hp), :L) - N_Hc = Hermitian(convert(Matrix{NT}, N_Hc), :L) - L_Hp = Hermitian(convert(Matrix{NT}, L_Hp), :L) + weights = ControllerWeights{NT}(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt) # dummy vals (updated just before optimization): R̂y, R̂u, T_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp) noR̂u = iszero(L_Hp) @@ -63,10 +55,10 @@ struct LinMPC{ E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat(estim, model, Hp, Hc) # dummy vals (updated just before optimization): F, fx̂ = zeros(NT, ny*Hp), zeros(NT, nx̂) - con, nϵ, S̃, Ñ_Hc, Ẽ = init_defaultcon_mpc( - estim, Hp, Hc, Cwt, S, N_Hc, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂ + con, nϵ, S̃, Ẽ = init_defaultcon_mpc( + estim, Hp, Hc, Cwt, S, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂ ) - H̃ = init_quadprog(model, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp) + H̃ = init_quadprog(model, weights, Ẽ, S̃) # dummy vals (updated just before optimization): q̃, r = zeros(NT, size(H̃, 1)), zeros(NT, 1) Ks, Ps = init_stochpred(estim, Hp) @@ -80,7 +72,7 @@ struct LinMPC{ estim, optim, con, ΔŨ, ŷ, Hp, Hc, nϵ, - M_Hp, Ñ_Hc, L_Hp, Ewt, + weights, R̂u, R̂y, noR̂u, S̃, T, T_lastu0, Ẽ, F, G, J, K, V, B, diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 29671e88b..8ba2a3e97 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -17,10 +17,7 @@ struct NonLinMPC{ Hp::Int Hc::Int nϵ::Int - M_Hp::Hermitian{NT, Matrix{NT}} - Ñ_Hc::Hermitian{NT, Matrix{NT}} - L_Hp::Hermitian{NT, Matrix{NT}} - E::NT + weights::ControllerWeights{NT} JE::JEfunc p::P R̂u::Vector{NT} @@ -56,11 +53,7 @@ struct NonLinMPC{ ŷ = copy(model.yop) # dummy vals (updated just before optimization) validate_JE(NT, JE) gc! = get_mutating_gc(NT, gc) - validate_weights(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt) - # convert `Diagonal` to normal `Matrix` if required: - M_Hp = Hermitian(convert(Matrix{NT}, M_Hp), :L) - N_Hc = Hermitian(convert(Matrix{NT}, N_Hc), :L) - L_Hp = Hermitian(convert(Matrix{NT}, L_Hp), :L) + weights = ControllerWeights{NT}(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt) # dummy vals (updated just before optimization): R̂y, R̂u, T_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp) noR̂u = iszero(L_Hp) @@ -68,10 +61,10 @@ struct NonLinMPC{ E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat(estim, model, Hp, Hc) # dummy vals (updated just before optimization): F, fx̂ = zeros(NT, ny*Hp), zeros(NT, nx̂) - con, nϵ, S̃, Ñ_Hc, Ẽ = init_defaultcon_mpc( - estim, Hp, Hc, Cwt, S, N_Hc, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂, gc!, nc + con, nϵ, S̃, Ẽ = init_defaultcon_mpc( + estim, Hp, Hc, Cwt, S, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂, gc!, nc ) - H̃ = init_quadprog(model, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp) + H̃ = init_quadprog(model, weights, Ẽ, S̃) # dummy vals (updated just before optimization): q̃, r = zeros(NT, size(H̃, 1)), zeros(NT, 1) Ks, Ps = init_stochpred(estim, Hp) @@ -86,7 +79,8 @@ struct NonLinMPC{ estim, optim, con, ΔŨ, ŷ, Hp, Hc, nϵ, - M_Hp, Ñ_Hc, L_Hp, Ewt, JE, p, + weights, + JE, p, R̂u, R̂y, noR̂u, S̃, T, T_lastu0, Ẽ, F, G, J, K, V, B, @@ -467,7 +461,7 @@ function init_optimization!(mpc::NonLinMPC, model::SimModel, optim) @constraint(optim, linconstraint, A*ΔŨvar .≤ b) # --- nonlinear optimization init --- if mpc.nϵ == 1 && JuMP.solver_name(optim) == "Ipopt" - C = mpc.Ñ_Hc[end] + C = mpc.weights.Ñ_Hc[end] try JuMP.get_attribute(optim, "nlp_scaling_max_gradient") catch @@ -698,6 +692,6 @@ end "Evaluate the economic term `E*JE` of the objective function for [`NonLinMPC`](@ref)." function obj_econ(mpc::NonLinMPC, model::SimModel, Ue, Ŷe) - E_JE = iszero(mpc.E) ? 0.0 : mpc.E*mpc.JE(Ue, Ŷe, mpc.D̂e, mpc.p) + E_JE = iszero(mpc.weights.E) ? 0.0 : mpc.weights.E*mpc.JE(Ue, Ŷe, mpc.D̂e, mpc.p) return E_JE end \ No newline at end of file diff --git a/src/plot_sim.jl b/src/plot_sim.jl index d937f91ce..c524ba5a5 100644 --- a/src/plot_sim.jl +++ b/src/plot_sim.jl @@ -838,7 +838,7 @@ plot_recipe(::Nothing, ::SimResult{<:Real, <:PredictiveController}) = nothing t, res.Ŷ_data[i_y, :] end end - M_Hp_i = mpc.M_Hp[i_y, i_y] + M_Hp_i = mpc.weights.M_Hp[i_y, i_y] if i_y in indices_ry && !iszero(M_Hp_i) @series begin i == ny && (xguide --> "Time (s)") @@ -894,7 +894,7 @@ plot_recipe(::Nothing, ::SimResult{<:Real, <:PredictiveController}) = nothing legend --> false t, res.U_data[i_u, :] end - L_Hp_i = mpc.L_Hp[i_u, i_u] + L_Hp_i = mpc.weights.L_Hp[i_u, i_u] if i_u in indices_ru && !iszero(L_Hp_i) @series begin i == nu && (xguide --> "Time (s)") diff --git a/test/test_predictive_control.jl b/test/test_predictive_control.jl index eb6579cbf..5afbbfe07 100644 --- a/test/test_predictive_control.jl +++ b/test/test_predictive_control.jl @@ -11,13 +11,13 @@ sys = [ tf(1.90,[1800.0,1]) tf(1.90,[1800.0,1]) tf(1.90,[1800.0,1]); @test size(mpc2.Ẽ,2) == 4*mpc2.estim.model.nu mpc3 = LinMPC(model, Hc=4, Cwt=1e6) @test size(mpc3.Ẽ,2) == 4*mpc3.estim.model.nu + 1 - @test mpc3.Ñ_Hc[end] ≈ 1e6 + @test mpc3.weights.Ñ_Hc[end] ≈ 1e6 mpc4 = LinMPC(model, Mwt=[1,2], Hp=15) - @test mpc4.M_Hp ≈ Diagonal(diagm(repeat(Float64[1, 2], 15))) + @test mpc4.weights.M_Hp ≈ Diagonal(diagm(repeat(Float64[1, 2], 15))) mpc5 = LinMPC(model, Nwt=[3,4], Cwt=1e3, Hc=5) - @test mpc5.Ñ_Hc ≈ Diagonal(diagm([repeat(Float64[3, 4], 5); [1e3]])) + @test mpc5.weights.Ñ_Hc ≈ Diagonal(diagm([repeat(Float64[3, 4], 5); [1e3]])) mpc6 = LinMPC(model, Lwt=[0,1], Hp=15) - @test mpc6.L_Hp ≈ Diagonal(diagm(repeat(Float64[0, 1], 15))) + @test mpc6.weights.L_Hp ≈ Diagonal(diagm(repeat(Float64[0, 1], 15))) mpc7 = @test_logs( (:warn, "Solving time limit is not supported by the optimizer."), LinMPC(model, optim=JuMP.Model(DAQP.Optimizer)) @@ -30,11 +30,11 @@ sys = [ tf(1.90,[1800.0,1]) tf(1.90,[1800.0,1]) tf(1.90,[1800.0,1]); @test mpc9.estim.nint_u == [1, 1] @test mpc9.estim.nint_ym == [0, 0] mpc10 = LinMPC(model, M_Hp=Diagonal(collect(1.01:0.01:1.2))) - @test mpc10.M_Hp ≈ Diagonal(collect(1.01:0.01:1.2)) + @test mpc10.weights.M_Hp ≈ Diagonal(collect(1.01:0.01:1.2)) mpc11 = LinMPC(model, N_Hc=Diagonal([0.1,0.11,0.12,0.13]), Cwt=Inf) - @test mpc11.Ñ_Hc ≈ Diagonal([0.1,0.11,0.12,0.13]) + @test mpc11.weights.Ñ_Hc ≈ Diagonal([0.1,0.11,0.12,0.13]) mcp12 = LinMPC(model, L_Hp=Diagonal(collect(0.001:0.001:0.02))) - @test mcp12.L_Hp ≈ Diagonal(collect(0.001:0.001:0.02)) + @test mcp12.weights.L_Hp ≈ Diagonal(collect(0.001:0.001:0.02)) model2 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), zeros(1,0), zeros(1,0), 1.0) mpc13 = LinMPC(model2) @test isa(mpc13, LinMPC{Float32}) @@ -326,13 +326,13 @@ end u = moveinput!(mpc, r) @test u ≈ [13] atol=1e-2 setmodel!(mpc, Mwt=[100], Nwt=[200], Lwt=[300]) - @test mpc.M_Hp ≈ diagm(fill(100, 1000)) - @test mpc.Ñ_Hc ≈ diagm([200, 1e4]) - @test mpc.L_Hp ≈ diagm(fill(300, 1000)) + @test mpc.weights.M_Hp ≈ diagm(fill(100, 1000)) + @test mpc.weights.Ñ_Hc ≈ diagm([200, 1e4]) + @test mpc.weights.L_Hp ≈ diagm(fill(300, 1000)) setmodel!(mpc, M_Hp=diagm(1:1000), Ñ_Hc=diagm([0.1;1e6]), L_Hp=diagm(1.1:1000.1)) - @test mpc.M_Hp ≈ diagm(1:1000) - @test mpc.Ñ_Hc ≈ diagm([0.1;1e6]) - @test mpc.L_Hp ≈ diagm(1.1:1000.1) + @test mpc.weights.M_Hp ≈ diagm(1:1000) + @test mpc.weights.Ñ_Hc ≈ diagm([0.1;1e6]) + @test mpc.weights.L_Hp ≈ diagm(1.1:1000.1) end @testset "LinMPC real-time simulations" begin @@ -354,11 +354,11 @@ end @test isa(mpc1.estim, SteadyKalmanFilter) @test size(mpc1.Ẽ,1) == 15*mpc1.estim.model.ny mpc4 = ExplicitMPC(model, Mwt=[1,2], Hp=15) - @test mpc4.M_Hp ≈ Diagonal(diagm(repeat(Float64[1, 2], 15))) + @test mpc4.weights.M_Hp ≈ Diagonal(diagm(repeat(Float64[1, 2], 15))) mpc5 = ExplicitMPC(model, Nwt=[3,4], Hc=5) - @test mpc5.Ñ_Hc ≈ Diagonal(diagm(repeat(Float64[3, 4], 5))) + @test mpc5.weights.Ñ_Hc ≈ Diagonal(diagm(repeat(Float64[3, 4], 5))) mpc6 = ExplicitMPC(model, Lwt=[0,1], Hp=15) - @test mpc6.L_Hp ≈ Diagonal(diagm(repeat(Float64[0, 1], 15))) + @test mpc6.weights.L_Hp ≈ Diagonal(diagm(repeat(Float64[0, 1], 15))) kf = KalmanFilter(model) mpc8 = ExplicitMPC(kf) @test isa(mpc8.estim, KalmanFilter) @@ -366,11 +366,11 @@ end @test mpc9.estim.nint_u == [1, 1] @test mpc9.estim.nint_ym == [0, 0] mpc10 = ExplicitMPC(model, M_Hp=Diagonal(collect(1.01:0.01:1.2))) - @test mpc10.M_Hp ≈ Diagonal(collect(1.01:0.01:1.2)) + @test mpc10.weights.M_Hp ≈ Diagonal(collect(1.01:0.01:1.2)) mpc11 = ExplicitMPC(model, N_Hc=Diagonal([0.1,0.11,0.12,0.13])) - @test mpc11.Ñ_Hc ≈ Diagonal([0.1,0.11,0.12,0.13]) + @test mpc11.weights.Ñ_Hc ≈ Diagonal([0.1,0.11,0.12,0.13]) mcp12 = ExplicitMPC(model, L_Hp=Diagonal(collect(0.001:0.001:0.02))) - @test mcp12.L_Hp ≈ Diagonal(collect(0.001:0.001:0.02)) + @test mcp12.weights.L_Hp ≈ Diagonal(collect(0.001:0.001:0.02)) model2 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), zeros(1,0), zeros(1,0), 1.0) mpc13 = ExplicitMPC(model2) @test isa(mpc13, ExplicitMPC{Float32}) @@ -486,13 +486,13 @@ end u = moveinput!(mpc, r) @test u ≈ [13] atol=1e-2 setmodel!(mpc, Mwt=[100], Nwt=[200], Lwt=[300]) - @test mpc.M_Hp ≈ diagm(fill(100, 1000)) - @test mpc.Ñ_Hc ≈ diagm([200]) - @test mpc.L_Hp ≈ diagm(fill(300, 1000)) + @test mpc.weights.M_Hp ≈ diagm(fill(100, 1000)) + @test mpc.weights.Ñ_Hc ≈ diagm([200]) + @test mpc.weights.L_Hp ≈ diagm(fill(300, 1000)) setmodel!(mpc, M_Hp=diagm(1:1000), Ñ_Hc=[0.1], L_Hp=diagm(1.1:1000.1)) - @test mpc.M_Hp ≈ diagm(1:1000) - @test mpc.Ñ_Hc ≈ [0.1] - @test mpc.L_Hp ≈ diagm(1.1:1000.1) + @test mpc.weights.M_Hp ≈ diagm(1:1000) + @test mpc.weights.Ñ_Hc ≈ [0.1] + @test mpc.weights.L_Hp ≈ diagm(1.1:1000.1) end @testset "NonLinMPC construction" begin @@ -509,15 +509,15 @@ end @test size(nmpc2.Ẽ, 2) == 4*nonlinmodel.nu nmpc3 = NonLinMPC(nonlinmodel, Hp=15, Hc=4, Cwt=1e6) @test size(nmpc3.Ẽ, 2) == 4*nonlinmodel.nu + 1 - @test nmpc3.Ñ_Hc[end] == 1e6 + @test nmpc3.weights.Ñ_Hc[end] == 1e6 nmpc4 = NonLinMPC(nonlinmodel, Hp=15, Mwt=[1,2]) - @test nmpc4.M_Hp ≈ Diagonal(diagm(repeat(Float64[1, 2], 15))) + @test nmpc4.weights.M_Hp ≈ Diagonal(diagm(repeat(Float64[1, 2], 15))) nmpc5 = NonLinMPC(nonlinmodel, Hp=15 ,Nwt=[3,4], Cwt=1e3, Hc=5) - @test nmpc5.Ñ_Hc ≈ Diagonal(diagm([repeat(Float64[3, 4], 5); [1e3]])) + @test nmpc5.weights.Ñ_Hc ≈ Diagonal(diagm([repeat(Float64[3, 4], 5); [1e3]])) nmpc6 = NonLinMPC(nonlinmodel, Hp=15, Lwt=[0,1]) - @test nmpc6.L_Hp ≈ Diagonal(diagm(repeat(Float64[0, 1], 15))) + @test nmpc6.weights.L_Hp ≈ Diagonal(diagm(repeat(Float64[0, 1], 15))) nmpc7 = NonLinMPC(nonlinmodel, Hp=15, Ewt=1e-3, JE=(Ue,Ŷe,D̂e,p) -> p*dot(Ue,Ŷe)+sum(D̂e), p=10) - @test nmpc7.E == 1e-3 + @test nmpc7.weights.E == 1e-3 @test nmpc7.JE([1,2],[3,4],[4,6],10) == 10*dot([1,2],[3,4])+sum([4,6]) optim = JuMP.Model(optimizer_with_attributes(Ipopt.Optimizer, "nlp_scaling_max_gradient"=>1.0)) nmpc8 = NonLinMPC(nonlinmodel, Hp=15, optim=optim) @@ -533,11 +533,11 @@ end @test nmpc11.estim.nint_u == [1, 1] @test nmpc11.estim.nint_ym == [0, 0] nmpc12 = NonLinMPC(nonlinmodel, Hp=10, M_Hp=Diagonal(collect(1.01:0.01:1.2))) - @test nmpc12.M_Hp ≈ Diagonal(collect(1.01:0.01:1.2)) + @test nmpc12.weights.M_Hp ≈ Diagonal(collect(1.01:0.01:1.2)) nmpc13 = NonLinMPC(nonlinmodel, Hp=10, N_Hc=Diagonal([0.1,0.11,0.12,0.13]), Cwt=Inf) - @test nmpc13.Ñ_Hc ≈ Diagonal([0.1,0.11,0.12,0.13]) + @test nmpc13.weights.Ñ_Hc ≈ Diagonal([0.1,0.11,0.12,0.13]) nmcp14 = NonLinMPC(nonlinmodel, Hp=10, L_Hp=Diagonal(collect(0.001:0.001:0.02))) - @test nmcp14.L_Hp ≈ Diagonal(collect(0.001:0.001:0.02)) + @test nmcp14.weights.L_Hp ≈ Diagonal(collect(0.001:0.001:0.02)) nmpc15 = NonLinMPC(nonlinmodel, Hp=10, gc=(Ue,Ŷe,D̂e,p,ϵ)-> [p*dot(Ue,Ŷe)+sum(D̂e)+ϵ], nc=1, p=10) LHS = zeros(1) nmpc15.con.gc!(LHS,[1,2],[3,4],[4,6],10,0.1) @@ -869,25 +869,25 @@ end u = moveinput!(mpc, r) @test u ≈ [13] atol=1e-2 setmodel!(mpc, Mwt=[100], Nwt=[200], Lwt=[300]) - @test mpc.M_Hp ≈ diagm(fill(100, 1000)) - @test mpc.Ñ_Hc ≈ diagm([200, 1e4]) - @test mpc.L_Hp ≈ diagm(fill(300, 1000)) + @test mpc.weights.M_Hp ≈ diagm(fill(100, 1000)) + @test mpc.weights.Ñ_Hc ≈ diagm([200, 1e4]) + @test mpc.weights.L_Hp ≈ diagm(fill(300, 1000)) setmodel!(mpc, M_Hp=diagm(1:1000), Ñ_Hc=diagm([0.1;1e6]), L_Hp=diagm(1.1:1000.1)) - @test mpc.M_Hp ≈ diagm(1:1000) - @test mpc.Ñ_Hc ≈ diagm([0.1;1e6]) - @test mpc.L_Hp ≈ diagm(1.1:1000.1) + @test mpc.weights.M_Hp ≈ diagm(1:1000) + @test mpc.weights.Ñ_Hc ≈ diagm([0.1;1e6]) + @test mpc.weights.L_Hp ≈ diagm(1.1:1000.1) f = (x,u,d,_) -> estim.model.A*x + estim.model.Bu*u + estim.model.Bd*d h = (x,d,_) -> estim.model.C*x + estim.model.Du*d nonlinmodel = NonLinModel(f, h, 10.0, 1, 1, 1) nmpc = NonLinMPC(nonlinmodel, Nwt=[0], Cwt=1e4, Hp=1000, Hc=10) setmodel!(nmpc, Mwt=[100], Nwt=[200], Lwt=[300]) - @test nmpc.M_Hp ≈ diagm(fill(100, 1000)) - @test nmpc.Ñ_Hc ≈ diagm([fill(200, 10); 1e4]) - @test nmpc.L_Hp ≈ diagm(fill(300, 1000)) + @test nmpc.weights.M_Hp ≈ diagm(fill(100, 1000)) + @test nmpc.weights.Ñ_Hc ≈ diagm([fill(200, 10); 1e4]) + @test nmpc.weights.L_Hp ≈ diagm(fill(300, 1000)) setmodel!(nmpc, M_Hp=diagm(1:1000), Ñ_Hc=diagm([fill(0.1, 10);1e6]), L_Hp=diagm(1.1:1000.1)) - @test nmpc.M_Hp ≈ diagm(1:1000) - @test nmpc.Ñ_Hc ≈ diagm([fill(0.1, 10);1e6]) - @test nmpc.L_Hp ≈ diagm(1.1:1000.1) + @test nmpc.weights.M_Hp ≈ diagm(1:1000) + @test nmpc.weights.Ñ_Hc ≈ diagm([fill(0.1, 10);1e6]) + @test nmpc.weights.L_Hp ≈ diagm(1.1:1000.1) @test_throws ErrorException setmodel!(nmpc, deepcopy(nonlinmodel)) end