Merge pull request #274 from JuliaControl/lastu_arg

added: `lastu` keyword argument to `moveinput!`

Author: franckgaga

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!
 ```

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),)"))

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

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