added: xs_0 and as_0 args in NonLinModelDAE constructor for initial guesses

Project.toml

@@ -1,6 +1,6 @@
 name = "ModelPredictiveControl"
 uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c"
-version = "2.13.1"
+version = "2.13.2"
 authors = ["Francis Gagnon"]
 
 [deps]

src/controller/execute.jl

@@ -1,7 +1,7 @@
 @doc raw"""
     initstate!(mpc::PredictiveController, u, ym, d=[]) -> x̂
 
-Init the states of `mpc.estim` [`StateEstimator`](@ref) and warm start `mpc.Z̃` at zero.
+Init the states of `mpc.estim` [`StateEstimator`](@ref) and reset `mpc.Z̃` warm-start.
 
 It also stores `u - mpc.estim.model.uop` at `mpc.lastu0` for converting the input increments
 ``\mathbf{ΔU}`` to inputs ``\mathbf{U}``.

src/estimator/execute.jl

@@ -422,7 +422,7 @@ The covariance error estimate `P̂` can be set only if `estim` is a [`StateEstim
 that computes it.
 """
 function setstate!(estim::StateEstimator, x̂, P̂=nothing)
-    size(x̂) == (estim.nx̂,) || error("x̂ size must be $((estim.nx̂,))")
+    size(x̂) == (estim.nx̂,) || size(x̂) == () || error("x̂ size must be $((estim.nx̂,))")
     estim.x̂0 .= x̂ .- estim.x̂op
     setstate_cov!(estim, P̂)
     return estim

src/model/nonlinmodeldae.jl

@@ -51,6 +51,8 @@ struct NonLinModelDAE{
     yname::Vector{String}
     dname::Vector{String}
     xname::Vector{String}
+    xs_0::Vector{NT}
+    as_0::Vector{NT}
     x0_optim::Vector{NT}
     u0_optim::Vector{NT}
     d0_optim::Vector{NT}
@@ -59,6 +61,8 @@ struct NonLinModelDAE{
     function NonLinModelDAE{NT}(
         fq!::FQ, h!::H, Ts, nu, nx, na, ny, nd, 
         p::PT, 
+        xs_0,
+        as_0,
         transcription::TM, 
         optim_state::JMS,
         optim_output::JMO,
@@ -84,10 +88,12 @@ struct NonLinModelDAE{
         yname = ["\$y_{$i}\$" for i in 1:ny]
         dname = ["\$d_{$i}\$" for i in 1:nd]
         xname = ["\$x_{$i}\$" for i in 1:nx]
+        size(xs_0) ≠ (nx,) && throw(DimensionMismatch("xs_0 size $(size(xs_0)) ≠ state size ($nx,)"))
+        size(as_0) ≠ (na,) && throw(DimensionMismatch("as_0 size $(size(as_0)) ≠ alg. var. size ($na,)"))
         x0, a0 = zeros(NT, nx), zeros(NT, na)
         t  = zeros(NT, 1)
         # the updatestate!(model, u, d) API does not know the input `u` of the next time 
-        # step k+1, so only piecewise constant input `u` is supported here:
+        # step k+1, so only piecewise constant input `u` is supported here (h=0):
         transcription.h > 0 && error("Only zero-order hold (h=0) is supported for simulations of DAEs")
         iszero_Ha = validate_strictly_proper(NT, fq!, h!, nu, nx, na, ny, nd, p)
         Mo, Co, λo = init_orthocolloc(NT, transcription, nx, Ts)
@@ -117,10 +123,12 @@ struct NonLinModelDAE{
             nu, nx, na, ny, nd, 
             uop, yop, dop, xop, fop,
             uname, yname, dname, xname,
+            xs_0, as_0,
             x0_optim, u0_optim, d0_optim,
             iszero_Ha,
             buffer
         )
+        reset_warmstart!(model, transcription)
         init_optimization!(model, model.optim_state, model.optim_output)
         return model
     end
@@ -181,6 +189,8 @@ See also [`NonLinModel`](@ref) for ODEs.
 - `ny`: number of outputs.
 - `nd=0`: number of measured disturbances.
 - `p=[]`: parameters of the model (any type).
+- `xs_0=zeros(nx)`: initial guess (optimization warm-start) for the states.
+- `as_0=zeros(na)`: initial guess (optimization warm-start) for the algebraic variables.
 - `transcription=OrthogonalCollocation()` : a [`TrapezoidalCollocation`](@ref) or 
    [`OrthogonalCollocation`](@ref) instance for open-loop simulations.
 - `optim_state=JuMP.Model(Ipopt.Optimizer)` : nonlinear optimizer for [`updatestate!`](@ref),
@@ -233,7 +243,9 @@ NonLinModelDAE with a sample time Ts = 5.0 s:
 """
 function NonLinModelDAE{NT}(
     fq::Function, h::Function, Ts::Real, nu::Int, nx::Int, na::Int, ny::Int, nd::Int=0;
-    p=NT[], 
+    p = NT[], 
+    xs_0 = zeros(NT, nx),
+    as_0 = zeros(NT, na),
     transcription = OrthogonalCollocation(), 
     optim_state   = JuMP.Model(DEFAULT_NLP_OPTIMIZER, add_bridges=false),
     optim_output  = JuMP.Model(DEFAULT_NLP_OPTIMIZER, add_bridges=false),
@@ -243,15 +255,17 @@ function NonLinModelDAE{NT}(
     fq!, h! = get_mutating_functions_dae(NT, fq, h)
     hessian = validate_hessian(hessian, DEFAULT_NONLINDAE_HESSIAN)
     return NonLinModelDAE{NT}(
-        fq!, h!, Ts, nu, nx, na, ny, nd, p, 
+        fq!, h!, Ts, nu, nx, na, ny, nd, p, xs_0, as_0,
         transcription, optim_state, optim_output, jacobian, hessian
     )
 end
 
 function NonLinModelDAE(
     fq::Function, h::Function, Ts::Real, 
     nu::Int, nx::Int, na::Int, ny::Int, nd::Int=0;
-    p=Float64[], 
+    p = Float64[],
+    xs_0 = zeros(nx),
+    as_0 = zeros(na),
     transcription = OrthogonalCollocation(), 
     optim_state   = JuMP.Model(DEFAULT_NLP_OPTIMIZER, add_bridges=false),
     optim_output  = JuMP.Model(DEFAULT_NLP_OPTIMIZER, add_bridges=false),
@@ -260,7 +274,7 @@ function NonLinModelDAE(
 )
     return NonLinModelDAE{Float64}(
         fq, h, Ts, nu, nx, na, ny, nd; 
-        p, transcription, optim_state, optim_output, jacobian, hessian
+        p, xs_0, as_0, transcription, optim_state, optim_output, jacobian, hessian
     )
 end
 
@@ -392,6 +406,37 @@ function get_nZ_dae(transcription::OrthogonalCollocation, nx, na)
 end
 get_nZ_dae(::TrapezoidalCollocation, nx, na) = nx + 2na
 
+"""
+    reset_warmstart!(model::NonLinModelDAE, transcription::CollocationMethod)
+
+Reset warm-starting values `model.Z` and `model.a0` at construction values.
+"""
+function reset_warmstart!(model::NonLinModelDAE, transcription::OrthogonalCollocation)
+    nx, na, no = model.nx, model.na, transcription.no
+    x0s  = model.buffer.x
+    x0s .= model.xs_0 .- model.xop
+    a0s  = model.as_0
+    nk̄ = nx*no
+    model.a0                      .= a0s
+    model.Z[1:nx]                 .= x0s
+    model.Z[(nx+1):(nx+na)]       .= a0s
+    model.Z[(nx+na+1):(nx+na+nk̄)] .= 0 # state derivative ki warm-started at 0 
+    ā_Z = @views model.Z[(nx+na+nk̄+1):end]
+    repeat!(ā_Z, a0s, no)
+    return nothing
+end
+function reset_warmstart!(model::NonLinModelDAE, ::TrapezoidalCollocation)
+    nx, na = model.nx, model.na
+    x0s  = model.buffer.x
+    x0s .= model.xs_0 .- model.xop
+    a0s  = model.as_0
+    model.a0                 .= a0s
+    model.Z[1:nx]            .= x0s
+    model.Z[(nx+1):(nx+na)]  .= a0s
+    model.Z[(nx+na+1):end]   .= a0s
+    return nothing
+end
+
 @doc raw"""
     init_defectmat_dae(NT, ::OrthogonalCollocation, nx, na, Co, λo) -> Es, Ks, Aeq
 
@@ -652,7 +697,7 @@ end
 @doc raw"""
     initstate_core!(model::NonLinModelDAE, u0, d0)
 
-Warm-start `model.Z` and `model.a0` at zero if `model` is a [`NonLinModelDAE`](@ref).
+Reset warm-starting for `model.Z` and `model.a0` if `model` is a [`NonLinModelDAE`](@ref).
 
 The field `model.a0` and `model.Z` respectively warm-start [`evaloutput`](@ref) and
 [`updatestate!`](@ref) solving. The method also set `model.optim_u0` and `model.optim_d0` at
@@ -661,8 +706,7 @@ The field `model.a0` and `model.Z` respectively warm-start [`evaloutput`](@ref)
 since `model` is strictly proper w.r.t. `u0`.
 """
 function initstate_core!(model::NonLinModelDAE, u0, d0) 
-    model.Z  .= 0
-    model.a0 .= 0
+    reset_warmstart!(model, model.transcription)
     model.x0_optim .= model.x0
     model.u0_optim .= u0
     model.d0_optim .= d0

src/plot_sim.jl

@@ -142,6 +142,7 @@ function sim!(
     U_data  = Matrix{NT}(undef, plant.nu, N)
     D_data  = Matrix{NT}(undef, plant.nd, N)
     X_data  = Matrix{NT}(undef, plant.nx, N)
+    initstate!(plant, u, d)
     setstate!(plant, x_0)
     @progressif progress name="$(nameof(typeof(plant))) simulation" for i=1:N
         y = evaloutput(plant, d) 
@@ -275,7 +276,8 @@ function sim_closedloop!(
 ) where {NT<:Real}
     model = estim.model
     model.Ts ≈ plant.Ts || error("Sampling time of controller/estimator ≠ plant.Ts")
-    old_x0    = copy(plant.x0)
+    old_x0    = plant.buffer.x
+    old_x0   .= plant.x0
     T_data    = collect(plant.Ts*(0:(N-1)))
     Y_data    = Matrix{NT}(undef, plant.ny, N)
     Ŷ_data    = Matrix{NT}(undef, model.ny, N)
@@ -284,10 +286,11 @@ function sim_closedloop!(
     Ud_data   = Matrix{NT}(undef, plant.nu, N)
     Ru_data   = Matrix{NT}(undef, plant.nu, N)
     D_data    = Matrix{NT}(undef, plant.nd, N)
-    X_data    = Matrix{NT}(undef, plant.nx, N) 
+    X_data    = Matrix{NT}(undef, plant.nx, N)
     X̂_data    = Matrix{NT}(undef, estim.nx̂, N)
-    setstate!(plant, x_0)
     lastd, lasty = d, evaloutput(plant, d)
+    initstate!(plant, lastu, lastd)
+    setstate!(plant, x_0)
     initstate!(est_mpc, lastu, lasty[estim.i_ym], lastd)
     isnothing(x̂_0) || setstate!(est_mpc, x̂_0)
     @progressif progress name="$(nameof(typeof(est_mpc))) simulation" for i=1:N

src/sim_model.jl

@@ -193,7 +193,7 @@ end
 Set `model.x0` to `x - model.xop` from the argument `x`. 
 """
 function setstate!(model::SimModel, x)
-    size(x) == (model.nx,) || error("x size must be $((model.nx,))")
+    size(x) == (model.nx,) || size(x) == () || error("x size must be $((model.nx,))")
     model.x0 .= x .- model.xop
     return model
 end

test/1_test_sim_model.jl

@@ -30,6 +30,8 @@
     linmodel4 = LinModel(Gss)
     setstate!(linmodel4, [1;-1])
     @test linmodel4.x0 ≈ [1;-1]
+    setstate!(linmodel4, 8)
+    @test linmodel4.x0 ≈ [8; 8]
 
     linmodel5 = LinModel(sys,Ts,i_d=[3])
     setop!(linmodel5, uop=[10,50], yop=[50,30], dop=[20])
@@ -502,6 +504,16 @@ end
 
     dae7 = NonLinModelDAE{Float32}(fq!,h!, Ts, nu, nx, na, ny; p)
     @test isa(dae7, NonLinModelDAE{Float32})
+
+    dae8 = NonLinModelDAE(fq!, h!, Ts, nu, nx, na, ny; p, xs_0=[3])
+    @test dae8.xs_0   ≈ [3]
+    @test dae8.Z[1:1] ≈ [3]
+
+    transcription = OrthogonalCollocation(0, 1)
+    dae9 = NonLinModelDAE(fq!, h!, Ts, nu, nx, na, ny; p, as_0=[4], transcription)
+    @test dae9.as_0    ≈ [4]
+    @test dae9.Z[2:2]  ≈ [4]
+    @test dae9.a0      ≈ [4]
 
     @test_throws ErrorException NonLinModelDAE(
         (x,u,p)->(x+u+p, 0.0),
@@ -542,24 +554,27 @@ end
     p = [1.0]
 
     transcription = OrthogonalCollocation(0, 4, roots=:gausslegendre)
-    dae = NonLinModelDAE(fq!, h!, Ts, nu, nx, na, ny; p, transcription)
+    as_0 = [-1]
+    dae = NonLinModelDAE(fq!, h!, Ts, nu, nx, na, ny; p, transcription, as_0)
     u = [0.0]
     d = Float64[]
 
-    @test updatestate!(dae, u) ≈ zeros(1)
-    @test updatestate!(dae, u, d) ≈ zeros(1)
-    @test dae.x0 ≈ zeros(1)
-    @test evaloutput(dae) ≈ dae() ≈ zeros(1)
+    @test updatestate!(dae, u) ≈ zeros(1) atol=1e-6
+    @test updatestate!(dae, u, d) ≈ zeros(1) atol=1e-6
+    @test dae.x0 ≈ zeros(1) atol=1e-6
+    @test evaloutput(dae) ≈ zeros(1) atol=1e-6
 
     transcription = TrapezoidalCollocation()
     dae2 = NonLinModelDAE(fq!, h!, Ts, nu, nx, na, ny; p, transcription)
-    @test updatestate!(dae2, u) ≈ zeros(1)
-    @test updatestate!(dae2, u, d) ≈ zeros(1)
-    @test dae2.x0 ≈ zeros(1)
-    @test evaloutput(dae2) ≈ dae2() ≈ zeros(1)
-
-    x = initstate!(dae, [10]) # do nothing for NonLinModelDAE
-    @test evaloutput(dae) ≈ [0]
+    @test updatestate!(dae2, u) ≈ zeros(1) atol=1e-6
+    @test updatestate!(dae2, u, d) ≈ zeros(1) atol=1e-6
+    @test dae2.x0 ≈ zeros(1) atol=1e-6
+    @test dae2() ≈ zeros(1) atol=1e-6
+
+    x = initstate!(dae, [10])
+    @test dae.a0 ≈ as_0 atol=1e-6
+    @test dae.Z[2:2] ≈ as_0 atol=1e-6
+    @test evaloutput(dae) ≈ [0] atol=1e-6
 
     @test_throws DimensionMismatch updatestate!(dae, zeros(2))
     @test_throws DimensionMismatch updatestate!(dae, zeros(1), zeros(1))

test/2_test_state_estim.jl

@@ -83,6 +83,8 @@ end
     @test updatestate!(kalmanfilter2, [10, 3], [0.5, 6+0.1]) ≈ x
     setstate!(kalmanfilter1, [1,2,3,4])
     @test kalmanfilter1.x̂0 ≈ [1,2,3,4]
+    setstate!(kalmanfilter1, 8)
+    @test kalmanfilter1.x̂0 ≈ [8,8,8,8]
     for i in 1:40
         preparestate!(kalmanfilter1, [50, 30])
         updatestate!(kalmanfilter1, [11, 52], [50, 30])
@@ -490,6 +492,8 @@ end
     @test internalmodel1.x̂s ≈ zeros(2)
     setstate!(internalmodel1, [1,2])
     @test internalmodel1.x̂0 ≈ [1,2]
+    setstate!(internalmodel1, 8)
+    @test internalmodel1.x̂0 ≈ [8,8]
     linmodel2 = LinModel(append(tf(3, [5, 1]), tf(2, [10, 1])), 1.0)
     stoch_ym = append(tf([2.5, 1],[1.2, 1, 0]),tf([1.5, 1], [1.3, 1, 0]))
     internalmodel2 = InternalModel(linmodel2; stoch_ym)