diff --git a/src/controller/construct.jl b/src/controller/construct.jl index e6c5e5939..0739a2b52 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -4,6 +4,10 @@ struct ControllerWeights{NT<:Real} Ñ_Hc::Hermitian{NT, Matrix{NT}} L_Hp::Hermitian{NT, Matrix{NT}} E ::NT + iszero_M_Hp::Vector{Bool} + iszero_Ñ_Hc::Vector{Bool} + iszero_L_Hp::Vector{Bool} + iszero_E::Bool function ControllerWeights{NT}( model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt=Inf, Ewt=0 ) where NT<:Real @@ -21,8 +25,12 @@ struct ControllerWeights{NT<:Real} # ΔŨ = ΔU (only hard constraints) Ñ_Hc = N_Hc end - E = Ewt - return new{NT}(M_Hp, Ñ_Hc, L_Hp, E) + E = Ewt + iszero_M_Hp = [iszero(M_Hp)] + iszero_Ñ_Hc = [iszero(Ñ_Hc)] + iszero_L_Hp = [iszero(L_Hp)] + iszero_E = iszero(E) + return new{NT}(M_Hp, Ñ_Hc, L_Hp, E, iszero_M_Hp, iszero_Ñ_Hc, iszero_L_Hp, iszero_E) end end diff --git a/src/controller/execute.jl b/src/controller/execute.jl index d113641ce..9e5f0b534 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -179,7 +179,7 @@ They are computed with these equations using in-place operations: \begin{aligned} \mathbf{F} &= \mathbf{G d_0}(k) + \mathbf{J D̂_0} + \mathbf{K x̂_0}(k) + \mathbf{V u_0}(k-1) + \mathbf{B} + \mathbf{Ŷ_s} \\ - \mathbf{C_y} &= \mathbf{F} - (\mathbf{R̂_y - Y_{op}}) \\ + \mathbf{C_y} &= \mathbf{F} - (\mathbf{R̂_y - Y_{op}}) \\ \mathbf{C_u} &= \mathbf{T} \mathbf{u_0}(k-1) - (\mathbf{R̂_u - U_{op}}) \\ \mathbf{q̃} &= 2[(\mathbf{M}_{H_p} \mathbf{Ẽ})' \mathbf{C_y} + (\mathbf{L}_{H_p} \mathbf{S̃})' \mathbf{C_u}] \\ @@ -193,32 +193,38 @@ function initpred!(mpc::PredictiveController, model::LinModel, d, D̂, R̂y, R̂ ŷ, F, q̃, r = mpc.ŷ, mpc.F, mpc.q̃, mpc.r Cy, Cu, M_Hp_Ẽ, L_Hp_S̃ = mpc.buffer.Ŷ, mpc.buffer.U, mpc.buffer.Ẽ, mpc.buffer.S̃ ŷ .= evaloutput(mpc.estim, d) - predictstoch!(mpc, mpc.estim) # init F with Ŷs for InternalModel - F .+= mpc.B # F = F + B - mul!(F, mpc.K, mpc.estim.x̂0, 1, 1) # F = F + K*x̂0 - mul!(F, mpc.V, mpc.estim.lastu0, 1, 1) # F = F + V*lastu0 + predictstoch!(mpc, mpc.estim) # init F with Ŷs for InternalModel + F .+= mpc.B # F = F + B + mul!(F, mpc.K, mpc.estim.x̂0, 1, 1) # F = F + K*x̂0 + mul!(F, mpc.V, mpc.estim.lastu0, 1, 1) # F = F + V*lastu0 if model.nd ≠ 0 mpc.d0 .= d .- model.dop mpc.D̂0 .= D̂ .- mpc.Dop mpc.D̂e[1:model.nd] .= d mpc.D̂e[model.nd+1:end] .= D̂ - mul!(F, mpc.G, mpc.d0, 1, 1) # F = F + G*d0 - mul!(F, mpc.J, mpc.D̂0, 1, 1) # F = F + J*D̂0 + mul!(F, mpc.G, mpc.d0, 1, 1) # F = F + G*d0 + mul!(F, mpc.J, mpc.D̂0, 1, 1) # F = F + J*D̂0 end + q̃ .= 0 + r .= 0 # --- output setpoint tracking term --- mpc.R̂y .= R̂y - Cy .= F .- (R̂y .- mpc.Yop) - mul!(M_Hp_Ẽ, mpc.weights.M_Hp, mpc.Ẽ) - mul!(q̃, M_Hp_Ẽ', Cy) # q̃ = M_Hp*Ẽ'*Cy - r .= dot(Cy, mpc.weights.M_Hp, Cy) + if !mpc.weights.iszero_M_Hp[] + Cy .= F .- (R̂y .- mpc.Yop) + mul!(M_Hp_Ẽ, mpc.weights.M_Hp, mpc.Ẽ) + mul!(q̃, M_Hp_Ẽ', Cy, 1, 1) # q̃ = q̃ + M_Hp*Ẽ'*Cy + r .+= dot(Cy, mpc.weights.M_Hp, Cy) # r = r + Cy'*M_Hp*Cy + end # --- input setpoint tracking term --- mpc.R̂u .= R̂u - Cu .= mpc.T_lastu0 .- (R̂u .- mpc.Uop) - mul!(L_Hp_S̃, mpc.weights.L_Hp, mpc.S̃) - mul!(q̃, L_Hp_S̃', Cu, 1, 1) # q̃ = q̃ + L_Hp*S̃'*Cu - r .+= dot(Cu, mpc.weights.L_Hp, Cu) + if !mpc.weights.iszero_L_Hp[] + Cu .= mpc.T_lastu0 .- (R̂u .- mpc.Uop) + mul!(L_Hp_S̃, mpc.weights.L_Hp, mpc.S̃) + mul!(q̃, L_Hp_S̃', Cu, 1, 1) # q̃ = q̃ + L_Hp*S̃'*Cu + r .+= dot(Cu, mpc.weights.L_Hp, Cu) # r = r + Cu'*L_Hp*Cu + end # --- finalize --- - lmul!(2, q̃) # q̃ = 2*q̃ + lmul!(2, q̃) # q̃ = 2*q̃ return nothing end @@ -230,7 +236,7 @@ Init `ŷ, F, d0, D̂0, D̂e, R̂y, R̂u` vectors when model is not a [`LinModel function initpred!(mpc::PredictiveController, model::SimModel, d, D̂, R̂y, R̂u) mul!(mpc.T_lastu0, mpc.T, mpc.estim.lastu0) mpc.ŷ .= evaloutput(mpc.estim, d) - predictstoch!(mpc, mpc.estim) # init F with Ŷs for InternalModel + predictstoch!(mpc, mpc.estim) # init F with Ŷs for InternalModel if model.nd ≠ 0 mpc.d0 .= d .- model.dop mpc.D̂0 .= D̂ .- mpc.Dop @@ -412,22 +418,36 @@ function obj_nonlinprog!( ) where NT<:Real nu, ny = model.nu, model.ny # --- output setpoint tracking term --- - Ȳ .= @views Ŷe[ny+1:end] - Ȳ .= mpc.R̂y .- Ȳ - JR̂y = dot(Ȳ, mpc.weights.M_Hp, Ȳ) + if mpc.weights.iszero_M_Hp[] + JR̂y = zero(NT) + else + Ȳ .= @views Ŷe[ny+1:end] + Ȳ .= mpc.R̂y .- Ȳ + JR̂y = dot(Ȳ, mpc.weights.M_Hp, Ȳ) + end # --- move suppression and slack variable term --- - JΔŨ = dot(ΔŨ, mpc.weights.Ñ_Hc, ΔŨ) + if mpc.weights.iszero_Ñ_Hc[] + JΔŨ = zero(NT) + else + JΔŨ = dot(ΔŨ, mpc.weights.Ñ_Hc, ΔŨ) + end # --- input setpoint tracking term --- - Ū .= @views Ue[1:end-nu] - Ū .= mpc.R̂u .- Ū - JR̂u = dot(Ū, mpc.weights.L_Hp, Ū) + if mpc.weights.iszero_L_Hp[] + JR̂u = zero(NT) + else + Ū .= @views Ue[1:end-nu] + Ū .= mpc.R̂u .- Ū + JR̂u = dot(Ū, mpc.weights.L_Hp, Ū) + end # --- economic term --- E_JE = obj_econ(mpc, model, Ue, Ŷe) return JR̂y + JΔŨ + JR̂u + E_JE end "By default, the economic term is zero." -obj_econ(::PredictiveController, ::SimModel, _ , _ ) = 0.0 +function obj_econ(::PredictiveController, ::SimModel, _ , ::AbstractVector{NT}) where NT + return zero(NT) +end @doc raw""" optim_objective!(mpc::PredictiveController) -> ΔŨ @@ -615,11 +635,13 @@ function setmodel!( for i=1:ny*Hp mpc.weights.M_Hp[i, i] = Mwt[(i-1) % ny + 1] end + mpc.weights.iszero_M_Hp[] = iszero(mpc.weights.M_Hp) 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.weights.M_Hp .= M_Hp + mpc.weights.iszero_M_Hp[] = iszero(mpc.weights.M_Hp) end if isnothing(Ñ_Hc) && !isnothing(Nwt) size(Nwt) == (nu,) || throw(ArgumentError("Nwt should be a vector of length $nu")) @@ -627,11 +649,13 @@ function setmodel!( for i=1:nu*Hc mpc.weights.Ñ_Hc[i, i] = Nwt[(i-1) % nu + 1] end + mpc.weights.iszero_Ñ_Hc[] = iszero(mpc.weights.Ñ_Hc) elseif !isnothing(Ñ_Hc) Ñ_Hc = to_hermitian(Ñ_Hc) nΔŨ = nu*Hc+nϵ size(Ñ_Hc) == (nΔŨ, nΔŨ) || throw(ArgumentError("Ñ_Hc size should be ($nΔŨ, $nΔŨ)")) mpc.weights.Ñ_Hc .= Ñ_Hc + mpc.weights.iszero_Ñ_Hc[] = iszero(mpc.weights.Ñ_Hc) end if isnothing(L_Hp) && !isnothing(Lwt) size(Lwt) == (nu,) || throw(ArgumentError("Lwt should be a vector of length $nu")) @@ -639,11 +663,13 @@ function setmodel!( for i=1:nu*Hp mpc.weights.L_Hp[i, i] = Lwt[(i-1) % nu + 1] end + mpc.weights.iszero_L_Hp[] = iszero(mpc.weights.L_Hp) 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.weights.L_Hp .= L_Hp + mpc.weights.iszero_L_Hp[] = iszero(mpc.weights.L_Hp) end setmodel_controller!(mpc, x̂op_old) return mpc diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index ec365ad0c..6023ac2d5 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -525,7 +525,7 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT gc = get_tmp(gc_cache, ΔŨ1) Ŷ0, x̂0end = predict!(Ȳ, x̂0, x̂0next, u0, û0, mpc, model, ΔŨ) Ue, Ŷe = extended_predictions!(Ue, Ŷe, Ū, mpc, model, Ŷ0, ΔŨ) - ϵ = (nϵ ≠ 0) ? ΔŨ[end] : 0 # ϵ = 0 if nϵ == 0 (meaning no relaxation) + ϵ = (nϵ ≠ 0) ? ΔŨ[end] : zero(T) # ϵ = 0 if nϵ == 0 (meaning no relaxation) mpc.con.gc!(gc, Ue, Ŷe, mpc.D̂e, mpc.p, ϵ) g = con_nonlinprog!(g, mpc, model, x̂0end, Ŷ0, gc, ϵ) return obj_nonlinprog!(Ȳ, Ū, mpc, model, Ue, Ŷe, ΔŨ)::T @@ -544,7 +544,7 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT gc = get_tmp(gc_cache, ΔŨ1) Ŷ0, x̂0end = predict!(Ȳ, x̂0, x̂0next, u0, û0, mpc, model, ΔŨ) Ue, Ŷe = extended_predictions!(Ue, Ŷe, Ū, mpc, model, Ŷ0, ΔŨ) - ϵ = (nϵ ≠ 0) ? ΔŨ[end] : 0 # ϵ = 0 if nϵ == 0 (meaning no relaxation) + ϵ = (nϵ ≠ 0) ? ΔŨ[end] : zero(T) # ϵ = 0 if nϵ == 0 (meaning no relaxation) mpc.con.gc!(gc, Ue, Ŷe, mpc.D̂e, mpc.p, ϵ) g = con_nonlinprog!(g, mpc, model, x̂0end, Ŷ0, gc, ϵ) end @@ -702,7 +702,9 @@ function con_nonlinprog!(g, mpc::NonLinMPC, ::SimModel, x̂0end, Ŷ0, gc, ϵ) 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.weights.E) ? 0.0 : mpc.weights.E*mpc.JE(Ue, Ŷe, mpc.D̂e, mpc.p) +function obj_econ( + mpc::NonLinMPC, model::SimModel, Ue, Ŷe::AbstractVector{NT} +) where NT<:Real + E_JE = mpc.weights.iszero_E ? zero(NT) : mpc.weights.E*mpc.JE(Ue, Ŷe, mpc.D̂e, mpc.p) return E_JE end \ No newline at end of file