diff --git a/docs/src/internals/predictive_control.md b/docs/src/internals/predictive_control.md index 994cd6628..b57b4c0e9 100644 --- a/docs/src/internals/predictive_control.md +++ b/docs/src/internals/predictive_control.md @@ -30,7 +30,7 @@ ModelPredictiveControl.get_nonlinops(::NonLinMPC, ::ModelPredictiveControl.Gener ## Update Quadratic Optimization ```@docs -ModelPredictiveControl.initpred!(::PredictiveController, ::LinModel, ::Any, ::Any, ::Any, ::Any) +ModelPredictiveControl.initpred!(::PredictiveController, ::LinModel, ::Any, ::Any, ::Any, ::Any, ::Any) ModelPredictiveControl.linconstraint!(::PredictiveController, ::LinModel, ::TranscriptionMethod) ModelPredictiveControl.linconstrainteq! ``` diff --git a/src/controller/construct.jl b/src/controller/construct.jl index 63aed9b0e..3cc4880d8 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -553,14 +553,15 @@ get_Hc(nb::AbstractVector{Int}) = length(nb) """ - validate_args(mpc::PredictiveController, ry, d, D̂, R̂y, R̂u) + validate_args(mpc::PredictiveController, ry, d, lastu, D̂, R̂y, R̂u) Check the dimensions of the arguments of [`moveinput!`](@ref). """ -function validate_args(mpc::PredictiveController, ry, d, D̂, R̂y, R̂u) +function validate_args(mpc::PredictiveController, ry, d, lastu, D̂, R̂y, R̂u) ny, nd, nu, Hp = mpc.estim.model.ny, mpc.estim.model.nd, mpc.estim.model.nu, mpc.Hp size(ry) ≠ (ny,) && throw(DimensionMismatch("ry size $(size(ry)) ≠ output size ($ny,)")) size(d) ≠ (nd,) && throw(DimensionMismatch("d size $(size(d)) ≠ measured dist. size ($nd,)")) + size(lastu) ≠ (nu,) && throw(DimensionMismatch("lastu size $(size(lastu)) ≠ manip. input size ($nu,)")) size(D̂) ≠ (nd*Hp,) && throw(DimensionMismatch("D̂ size $(size(D̂)) ≠ measured dist. size × Hp ($(nd*Hp),)")) size(R̂y) ≠ (ny*Hp,) && throw(DimensionMismatch("R̂y size $(size(R̂y)) ≠ output size × Hp ($(ny*Hp),)")) size(R̂u) ≠ (nu*Hp,) && throw(DimensionMismatch("R̂u size $(size(R̂u)) ≠ manip. input size × Hp ($(nu*Hp),)")) diff --git a/src/controller/execute.jl b/src/controller/execute.jl index bf8fd110e..9a3c54eba 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -37,6 +37,7 @@ See also [`LinMPC`](@ref), [`ExplicitMPC`](@ref), [`NonLinMPC`](@ref). - `mpc::PredictiveController` : solve optimization problem of `mpc`. - `ry=mpc.estim.model.yop` : current output setpoints ``\mathbf{r_y}(k)``. - `d=[]` : current measured disturbances ``\mathbf{d}(k)``. +- `lastu=mpc.lastu0+mpc.estim.model.uop`: last manipulated input ``\mathbf{u}(k-1)``. - `D̂=repeat(d, mpc.Hp)` or *`Dhat`* : predicted measured disturbances ``\mathbf{D̂}``, constant in the future by default or ``\mathbf{d̂}(k+j)=\mathbf{d}(k)`` for ``j=1`` to ``H_p``. - `R̂y=repeat(ry, mpc.Hp)` or *`Rhaty`* : predicted output setpoints ``\mathbf{R̂_y}``, constant @@ -59,6 +60,7 @@ function moveinput!( mpc::PredictiveController, ry::Vector = mpc.estim.model.yop, d ::Vector = mpc.buffer.empty; + lastu::Vector = (mpc.buffer.u .= mpc.lastu0 .+ mpc.estim.model.uop), Dhat ::Vector = repeat!(mpc.buffer.D̂, d, mpc.Hp), Rhaty::Vector = repeat!(mpc.buffer.Ŷ, ry, mpc.Hp), Rhatu::Vector = mpc.Uop, @@ -69,8 +71,8 @@ function moveinput!( if mpc.estim.direct && !mpc.estim.corrected[] @warn "preparestate! should be called before moveinput! with current estimators" end - validate_args(mpc, ry, d, D̂, R̂y, R̂u) - initpred!(mpc, mpc.estim.model, d, D̂, R̂y, R̂u) + validate_args(mpc, ry, d, lastu, D̂, R̂y, R̂u) + initpred!(mpc, mpc.estim.model, d, lastu, D̂, R̂y, R̂u) linconstraint!(mpc, mpc.estim.model, mpc.transcription) linconstrainteq!(mpc, mpc.estim.model, mpc.transcription) Z̃ = optim_objective!(mpc) @@ -189,7 +191,7 @@ function addinfo!(info, mpc::PredictiveController) end @doc raw""" - initpred!(mpc::PredictiveController, model::LinModel, d, D̂, R̂y, R̂u) -> nothing + initpred!(mpc::PredictiveController, model::LinModel, d, lastu, D̂, R̂y, R̂u) -> nothing Init linear model prediction matrices `F, q̃, r` and current estimated output `ŷ`. @@ -208,8 +210,8 @@ They are computed with these equations using in-place operations: \end{aligned} ``` """ -function initpred!(mpc::PredictiveController, model::LinModel, d, D̂, R̂y, R̂u) - F = initpred_common!(mpc, model, d, D̂, R̂y, R̂u) +function initpred!(mpc::PredictiveController, model::LinModel, d, lastu, D̂, R̂y, R̂u) + F = initpred_common!(mpc, model, d, lastu, D̂, R̂y, R̂u) 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.lastu0, 1, 1) # F = F + V*lastu0 @@ -241,24 +243,25 @@ function initpred!(mpc::PredictiveController, model::LinModel, d, D̂, R̂y, R̂ end @doc raw""" - initpred!(mpc::PredictiveController, model::SimModel, d, D̂, R̂y, R̂u) + initpred!(mpc::PredictiveController, model::SimModel, d, lastu, D̂, R̂y, R̂u) -Init `ŷ, F, d0, D̂0, D̂e, R̂y, R̂u` vectors when model is not a [`LinModel`](@ref). +Init `lastu0, ŷ, F, d0, D̂0, D̂e, R̂y, R̂u` vectors when model is not a [`LinModel`](@ref). """ -function initpred!(mpc::PredictiveController, model::SimModel, d, D̂, R̂y, R̂u) - F = initpred_common!(mpc, model, d, D̂, R̂y, R̂u) +function initpred!(mpc::PredictiveController, model::SimModel, d, lastu, D̂, R̂y, R̂u) + F = initpred_common!(mpc, model, d, lastu, D̂, R̂y, R̂u) return nothing end """ - initpred_common!(mpc::PredictiveController, model::SimModel, d, D̂, R̂y, R̂u) -> mpc.F + initpred_common!(mpc::PredictiveController, model::SimModel, d, lastu, D̂, R̂y, R̂u) -> F Common computations of `initpred!` for all types of [`SimModel`](@ref). Will also init `mpc.F` with 0 values, or with the stochastic predictions `Ŷs` if `mpc.estim` is an [`InternalModel`](@ref). The function returns `mpc.F`. """ -function initpred_common!(mpc::PredictiveController, model::SimModel, d, D̂, R̂y, R̂u) +function initpred_common!(mpc::PredictiveController, model::SimModel, d, lastu, D̂, R̂y, R̂u) + mpc.lastu0 .= lastu .- model.uop mul!(mpc.Tu_lastu0, mpc.Tu, mpc.lastu0) mpc.ŷ .= evaloutput(mpc.estim, d) if model.nd > 0 diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index 15377c542..a7f9edb28 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -81,11 +81,12 @@ end preparestate!(mpc1, [10]) u = moveinput!(mpc1, r) @test u ≈ [1] atol=1e-2 - u = mpc1(r) + u = mpc1(r, lastu=[-1]) @test u ≈ [1] atol=1e-2 info = getinfo(mpc1) @test info[:u] ≈ u @test info[:Ŷ][end] ≈ r[1] atol=1e-2 + @test info[:ΔU] ≈ [2.0] atol=1e-2 mpc2 = LinMPC(linmodel, Nwt=[0], Cwt=Inf, Hp=1000, Hc=1) preparestate!(mpc2, [10]) u = moveinput!(mpc2, r) @@ -515,11 +516,12 @@ end preparestate!(mpc1, y) u = moveinput!(mpc1, r) @test u ≈ [1] atol=1e-2 - u = mpc1(r) + u = mpc1(r, lastu=[-1]) @test u ≈ [1] atol=1e-2 info = getinfo(mpc1) @test info[:u] ≈ u @test info[:Ŷ][end] ≈ r[1] atol=1e-2 + @test info[:ΔU] ≈ [2.0] atol=1e-2 mpc2 = ExplicitMPC(model, Nwt=[0], Hp=1000, Hc=1) preparestate!(mpc2, y) u = moveinput!(mpc2, [5]) @@ -756,11 +758,12 @@ end preparestate!(nmpc_lin, [10]) u = moveinput!(nmpc_lin, ry) @test u ≈ [1] atol=5e-2 - u = nmpc_lin(ry) + u = nmpc_lin(ry, lastu=[-1]) @test u ≈ [1] atol=5e-2 info = getinfo(nmpc_lin) @test info[:u] ≈ u @test info[:Ŷ][end] ≈ ry[1] atol=5e-2 + @test info[:ΔU] ≈ [2.0] atol=5e-2 setmodel!(nmpc_lin; Mwt=[0], Lwt=[1]) u = moveinput!(nmpc_lin; R̂u=fill(ru[1], Hp)) @test u ≈ [4] atol=5e-2