diff --git a/Project.toml b/Project.toml index 99766f349..7b4a641d2 100644 --- a/Project.toml +++ b/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] diff --git a/src/controller/execute.jl b/src/controller/execute.jl index cd268c439..561bb946f 100644 --- a/src/controller/execute.jl +++ b/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}``. diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index 7de5cda42..c2c4e5a53 100644 --- a/src/estimator/execute.jl +++ b/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 diff --git a/src/model/nonlinmodeldae.jl b/src/model/nonlinmodeldae.jl index 6f4b50eba..5036b8b8a 100644 --- a/src/model/nonlinmodeldae.jl +++ b/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,7 +255,7 @@ 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 @@ -251,7 +263,9 @@ 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 diff --git a/src/plot_sim.jl b/src/plot_sim.jl index 71025bd56..467e19856 100644 --- a/src/plot_sim.jl +++ b/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 diff --git a/src/sim_model.jl b/src/sim_model.jl index d8af3044b..b0f4a6f32 100644 --- a/src/sim_model.jl +++ b/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 diff --git a/test/1_test_sim_model.jl b/test/1_test_sim_model.jl index 03aa6295c..c31b827d0 100644 --- a/test/1_test_sim_model.jl +++ b/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)) diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index b920e0d06..b46cc981a 100644 --- a/test/2_test_state_estim.jl +++ b/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)