diff --git a/benchmark/1_bench_sim_model.jl b/benchmark/1_bench_sim_model.jl index 211542a41..2d3e08e31 100644 --- a/benchmark/1_bench_sim_model.jl +++ b/benchmark/1_bench_sim_model.jl @@ -28,4 +28,9 @@ UNIT_MODEL["NonLinModel"]["linearize!"] = ## ----------------- CASE STUDIES --------------------------------------------------------- ## ---------------------------------------------------------------------------------------- const CASE_MODEL = SUITE["SimModel"]["CASE STUDIES"] -# TODO: Add case study benchmarks for SimModel \ No newline at end of file + +u_pendulum = [0.5] +CASE_MODEL["NonLinModel"]["pendulum"]["updatestate!"] = + @benchmarkable( + updatestate!($pendulum_model, $u_pendulum); + ) \ No newline at end of file diff --git a/docs/src/manual/nonlinmpc2.md b/docs/src/manual/nonlinmpc2.md index 413e8a8b8..031eb7caf 100644 --- a/docs/src/manual/nonlinmpc2.md +++ b/docs/src/manual/nonlinmpc2.md @@ -215,7 +215,7 @@ function simDAE(plant, N; x_0) Y_data, U_data, D_data, X_data = zeros(ny, N), zeros(nu, N), zeros(nd, N), zeros(nx, N) c_Ain_0 = plant.p[1] setstate!(plant, x_0) - x = x_0 + x, d = x_0, [10.0] for i=1:N u = i ≤ (1N÷4) ? [10.0] : [11.0] d = i ≤ (2N÷4) ? [10.0] : [12.0] @@ -226,7 +226,9 @@ function simDAE(plant, N; x_0) U_data[:, i] = u D_data[:, i] = d X_data[:, i] = x - x = updatestate!(plant, u, d) + dnext = i+1 ≤ (2N÷4) ? [10.0] : [12.0] + x = updatestate!(plant, u, d; dnext) + d = dnext end plant.p[1] = c_Ain_0 return SimResult(plant, U_data, Y_data, D_data; X_data) @@ -236,7 +238,10 @@ N = 81 res = simDAE(plant, N; x_0) ``` -We can now plot the result: +Providing a `dnext` keyword argument to [`updatestate!`](@ref) allows discretizing the +measured disturbances with first-order hold (FOH), instead of zero-order hold, which is +consistent with the method applied by the [`MovingHorizonEstimator`](@ref). We can now plot +the result: ```@example 1 using Plots @@ -324,10 +329,9 @@ function simMHE(mhe, plant, N; x_0, x̂_0) c_Ain_0 = plant.p[1] setstate!(plant, x_0); setstate!(mhe, x̂_0) initstate!(mhe, [10], [7], [10]) - x = x_0 + x, d = x_0, [10.0] for i=1:N u = i ≤ (1N÷4) ? [10.0] : [11.0] - d = i ≤ (2N÷4) ? [10.0] : [12.0] c_Ain = i ≤ (3N÷4) ? c_Ain_0 : (c_Ain_0 - 0.01) plant.p[1] = c_Ain y = evaloutput(plant, d) @@ -342,8 +346,10 @@ function simMHE(mhe, plant, N; x_0, x̂_0) X_data[3, i] = c_Ain Ŷ_data[:, i] = ŷ X̂_data[:, i] = x̂ - x = updatestate!(plant, u, d) + dnext = i+1 ≤ (2N÷4) ? [10.0] : [12.0] + x = updatestate!(plant, u, d; dnext) x̂ = updatestate!(mhe, u, ym, dm) + d = dnext end plant.p[1] = c_Ain_0 return SimResult(mhe, U_data, Y_data, D_data; plant, X_data, X̂_data, Ŷ_data) @@ -399,9 +405,8 @@ function simNMPC(nmpc, plant, N, ry; x_0, x̂_0, noise=false) c_Ain_0 = plant.p[1] setstate!(plant, x_0); setstate!(nmpc, x̂_0) initstate!(nmpc, [10], [7], [10]) - x = x_0 + x, d = x_0, [10.0] for i=1:N - d = i ≤ (1N÷3) ? [10.0] : [12.0] c_Ain = i ≤ (2N÷3) ? c_Ain_0 : (c_Ain_0 + 0.01) plant.p[1] = c_Ain y = evaloutput(plant, d) @@ -418,8 +423,10 @@ function simNMPC(nmpc, plant, N, ry; x_0, x̂_0, noise=false) X_data[3, i] = c_Ain Ŷ_data[:, i] = ŷ X̂_data[:, i] = x̂ - x = updatestate!(plant, u, d) + dnext = i+1 ≤ (1N÷3) ? [10.0] : [12.0] + x = updatestate!(plant, u, d; dnext) x̂ = updatestate!(nmpc, u, ym, dm) + d = dnext end plant.p[1] = c_Ain_0 return SimResult(nmpc, U_data, Y_data, D_data; plant, X_data, X̂_data, Ŷ_data, Ry_data) diff --git a/src/controller/execute.jl b/src/controller/execute.jl index 7327de77d..727609386 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -156,11 +156,12 @@ function getinfo(mpc::PredictiveController{NT}) where NT<:Real Ue, Ŷe = Vector{NT}(undef, nUe), Vector{NT}(undef, nŶe) U0, Ŷ0 = similar(mpc.Uop), similar(mpc.Yop) Û0, X̂0 = Vector{NT}(undef, nÛ0), Vector{NT}(undef, nX̂0) + Ûi = Vector{NT}(undef, nÛ0) U, Ŷ = buffer.U, buffer.Ŷ D̂ = buffer.D̂ U0 = getU0!(U0, mpc, Z̃) ΔŨ = getΔŨ!(ΔŨ, mpc, transcription, Z̃) - Ŷ0, x̂0end = predict!(Ŷ0, x̂0end, X̂0, Û0, K, mpc, model, transcription, U0, Z̃) + Ŷ0, x̂0end = predict!(Ŷ0, x̂0end, X̂0, Û0, Ûi, K, mpc, model, transcription, U0, Z̃) Ue, Ŷe = extended_vectors!(Ue, Ŷe, mpc, U0, Ŷ0) U .= U0 .+ mpc.Uop Ŷ .= Ŷ0 .+ mpc.Yop @@ -464,7 +465,7 @@ If first warm-starts the solver with [`set_warmstart!`](@ref). It then calls `@error` log in the REPL and returns the warm-start value. A failed optimization also prints [`getinfo`](@ref) results in the debug log [if activated](https://docs.julialang.org/en/v1/stdlib/Logging/#Example:-Enable-debug-level-messages). """ -function optim_objective!(mpc::PredictiveController{NT}) where {NT<:Real} +function optim_objective!(mpc::PredictiveController) model, optim = mpc.estim.model, mpc.optim Z̃var::Vector{JuMP.VariableRef} = optim[:Z̃var] Z̃s = set_warmstart!(mpc, mpc.transcription, Z̃var) diff --git a/src/controller/explicitmpc.jl b/src/controller/explicitmpc.jl index 4f2c05365..476a5034a 100644 --- a/src/controller/explicitmpc.jl +++ b/src/controller/explicitmpc.jl @@ -210,7 +210,7 @@ optim_objective!(mpc::ExplicitMPC) = lmul!(-1, ldiv!(mpc.Z̃, mpc.H̃_chol, mpc. "Compute the predictions but not the terminal states if `mpc` is an [`ExplicitMPC`](@ref)." function predict!( - Ŷ0, x̂0end, _ , _ , _ , mpc::ExplicitMPC, ::LinModel, ::TranscriptionMethod, _ , Z̃ + Ŷ0, x̂0end, _ , _ , _ , _ , mpc::ExplicitMPC, ::LinModel, ::TranscriptionMethod, _ , Z̃ ) # in-place operations to reduce allocations : Ŷ0 .= mul!(Ŷ0, mpc.Ẽ, Z̃) .+ mpc.F diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 0b76a221a..20e67f528 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -606,15 +606,16 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real Ue, Ŷe = zeros(NT, nUe), zeros(NT, nŶe) U0, Ŷ0 = zeros(NT, nU), zeros(NT, nŶ) Û0, X̂0 = zeros(NT, nU), zeros(NT, nX̂) + Ûi = zeros(NT, nU) gc, g = zeros(NT, nc), zeros(NT, ng) gi, geq = zeros(NT, ngi), zeros(NT, neq) J_cache = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), - Cache(Û0), Cache(K), Cache(X̂0), + Cache(Û0), Cache(Ûi), Cache(K), Cache(X̂0), Cache(gc), Cache(g), Cache(geq), ) - function J!(Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, g, geq) - update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, g, geq, mpc, Z̃) + function J!(Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, Ûi, K̄, X̂0, gc, g, geq) + update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, Ûi, K̄, X̂0, gc, g, geq, mpc, Z̃) return obj_nonlinprog!(Ŷ0, U0, mpc, Ue, Ŷe, ΔŨ) end if !isnothing(hess) @@ -629,11 +630,11 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real # --- inequality constraint derivatives --- ∇g_cache = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), - Cache(Û0), Cache(K), Cache(X̂0), + Cache(Û0), Cache(Ûi), Cache(K), Cache(X̂0), Cache(gc), Cache(geq), Cache(g) ) - function gi!(gi, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, geq, g) - update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, g, geq, mpc, Z̃) + function gi!(gi, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, Ûi, K, X̂0, gc, geq, g) + update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, Ûi, K, X̂0, gc, g, geq, mpc, Z̃) gi .= @views g[i_g] return nothing end @@ -657,11 +658,13 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real end ∇²g_cache = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), - Cache(Û0), Cache(K), Cache(X̂0), + Cache(Û0), Cache(Ûi), Cache(K), Cache(X̂0), Cache(gc), Cache(geq), Cache(g), Cache(gi) ) - function ℓ_gi(Z̃, λi, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, geq, g, gi) - update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, g, geq, mpc, Z̃) + function ℓ_gi(Z̃, λi, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, Ûi, K̄, X̂0, gc, geq, g, gi) + update_predictions!( + ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, Ûi, K̄, X̂0, gc, g, geq, mpc, Z̃ + ) gi .= @views g[i_g] return dot(λi, gi) end @@ -674,11 +677,13 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real # --- equality constraint derivatives --- geq_cache = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), - Cache(Û0), Cache(K), Cache(X̂0), + Cache(Û0), Cache(Ûi), Cache(K), Cache(X̂0), Cache(gc), Cache(g) ) - function geq!(geq, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, g) - update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, g, geq, mpc, Z̃) + function geq!(geq, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, Ûi, K̄, X̂0, gc, g) + update_predictions!( + ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, Ûi, K̄, X̂0, gc, g, geq, mpc, Z̃ + ) return nothing end ∇geq_prep = prepare_jacobian(geq!, geq, mpc.jacobian, mpc.Z̃, geq_cache...) @@ -701,11 +706,13 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real end ∇²geq_cache = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), - Cache(Û0), Cache(K), Cache(X̂0), + Cache(Û0), Cache(Ûi), Cache(K), Cache(X̂0), Cache(gc), Cache(geq), Cache(g) ) - function ℓ_geq(Z̃, λeq, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, geq, g) - update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, g, geq, mpc, Z̃) + function ℓ_geq(Z̃, λeq, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, Ûi, K̄, X̂0, gc, geq, g) + update_predictions!( + ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, Ûi, K̄, X̂0, gc, g, geq, mpc, Z̃ + ) return dot(λeq, geq) end ∇²ℓgeq_prep = prepare_hessian(ℓ_geq, hess, mpc.Z̃, Constant(λeq), ∇²geq_cache...) @@ -846,16 +853,17 @@ function get_nonlinobj_op(mpc::NonLinMPC, optim::JuMP.GenericModel{JNT}) where J Ue::Vector{JNT}, Ŷe::Vector{JNT} = zeros(JNT, nUe), zeros(JNT, nŶe) U0::Vector{JNT}, Ŷ0::Vector{JNT} = zeros(JNT, nU), zeros(JNT, nŶ) Û0::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nU), zeros(JNT, nX̂) + Ûi::Vector{JNT} = zeros(JNT, nU) gc::Vector{JNT}, g::Vector{JNT} = zeros(JNT, nc), zeros(JNT, ng) geq::Vector{JNT} = zeros(JNT, neq) - function J!(Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K̄, X̂0, gc, g, geq) - update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K̄, X̂0, gc, g, geq, mpc, Z̃) + function J!(Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, Ûi, K̄, X̂0, gc, g, geq) + update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, Ûi, K̄, X̂0, gc, g, geq, mpc, Z̃) return obj_nonlinprog!(Ŷ0, U0, mpc, Ue, Ŷe, ΔŨ) end Z̃_J = zeros(JNT, nZ̃) J_cache = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), - Cache(Û0), Cache(K̄), Cache(X̂0), + Cache(Û0), Cache(Ûi), Cache(K̄), Cache(X̂0), Cache(gc), Cache(g), Cache(geq), ) ∇J_prep = prepare_gradient(J!, grad, Z̃_J, J_cache...; strict) @@ -974,24 +982,25 @@ function get_nonlincon_oracle(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JN Ue::Vector{JNT}, Ŷe::Vector{JNT} = zeros(JNT, nUe), zeros(JNT, nŶe) U0::Vector{JNT}, Ŷ0::Vector{JNT} = zeros(JNT, nU), zeros(JNT, nŶ) Û0::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nU), zeros(JNT, nX̂) + Ûi::Vector{JNT} = zeros(JNT, nU) gc::Vector{JNT}, g::Vector{JNT} = zeros(JNT, nc), zeros(JNT, ng) gi::Vector{JNT}, geq::Vector{JNT} = zeros(JNT, ngi), zeros(JNT, neq) λi::Vector{JNT}, λeq::Vector{JNT} = rand(JNT, ngi), rand(JNT, neq) # -------------- inequality constraint: nonlinear oracle ----------------------------- - function gi!(gi, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K̄, X̂0, gc, geq, g) - update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K̄, X̂0, gc, g, geq, mpc, Z̃) + function gi!(gi, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, Ûi, K̄, X̂0, gc, geq, g) + update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, Ûi, K̄, X̂0, gc, g, geq, mpc, Z̃) gi .= @views g[i_g] return nothing end - function ℓ_gi(Z̃, λi, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K̄, X̂0, gc, geq, g, gi) - update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K̄, X̂0, gc, g, geq, mpc, Z̃) + function ℓ_gi(Z̃, λi, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, Ûi, K̄, X̂0, gc, geq, g, gi) + update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, Ûi, K̄, X̂0, gc, g, geq, mpc, Z̃) gi .= @views g[i_g] return dot(λi, gi) end Z̃_∇gi = zeros(JNT, nZ̃) ∇gi_cache = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), - Cache(Û0), Cache(K̄), Cache(X̂0), + Cache(Û0), Cache(Ûi), Cache(K̄), Cache(X̂0), Cache(gc), Cache(geq), Cache(g) ) ∇gi_prep = prepare_jacobian(gi!, gi, jac, Z̃_∇gi, ∇gi_cache...; strict) @@ -1000,7 +1009,7 @@ function get_nonlincon_oracle(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JN if !isnothing(hess) ∇²gi_cache = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), - Cache(Û0), Cache(K̄), Cache(X̂0), + Cache(Û0), Cache(Ûi), Cache(K̄), Cache(X̂0), Cache(gc), Cache(geq), Cache(g), Cache(gi) ) ∇²gi_prep = prepare_hessian( @@ -1044,18 +1053,18 @@ function get_nonlincon_oracle(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JN eval_hessian_lagrangian = isnothing(hess) ? nothing : ∇²gi_func! ) # ------------- equality constraints : nonlinear oracle ------------------------------ - function geq!(geq, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K̄, X̂0, gc, g) - update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K̄, X̂0, gc, g, geq, mpc, Z̃) + function geq!(geq, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, Ûi, K̄, X̂0, gc, g) + update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, Ûi, K̄, X̂0, gc, g, geq, mpc, Z̃) return nothing end - function ℓ_geq(Z̃, λeq, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K̄, X̂0, gc, geq, g) - update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K̄, X̂0, gc, g, geq, mpc, Z̃) + function ℓ_geq(Z̃, λeq, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, Ûi, K̄, X̂0, gc, geq, g) + update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, Ûi, K̄, X̂0, gc, g, geq, mpc, Z̃) return dot(λeq, geq) end Z̃_∇geq = zeros(JNT, nZ̃) ∇geq_cache = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), - Cache(Û0), Cache(K̄), Cache(X̂0), + Cache(Û0), Cache(Ûi), Cache(K̄), Cache(X̂0), Cache(gc), Cache(g) ) ∇geq_prep = prepare_jacobian(geq!, geq, jac, Z̃_∇geq, ∇geq_cache...; strict) @@ -1064,7 +1073,7 @@ function get_nonlincon_oracle(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JN if !isnothing(hess) ∇²geq_cache = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), - Cache(Û0), Cache(K̄), Cache(X̂0), + Cache(Û0), Cache(Ûi), Cache(K̄), Cache(X̂0), Cache(gc), Cache(geq), Cache(g) ) ∇²geq_prep = prepare_hessian( @@ -1119,7 +1128,7 @@ end """ update_predictions!( - ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K̄, X̂0, gc, g, geq, + ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, Ûi, K̄, X̂0, gc, g, geq, mpc::PredictiveController, Z̃ ) -> nothing @@ -1128,18 +1137,18 @@ Update in-place all vectors for the predictions of `mpc` controller at decision The method mutates all the arguments before the `mpc` argument. """ function update_predictions!( - ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K̄, X̂0, gc, g, geq, + ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, Ûi, K̄, X̂0, gc, g, geq, mpc::PredictiveController, Z̃ ) model, transcription = mpc.estim.model, mpc.transcription U0 = getU0!(U0, mpc, Z̃) ΔŨ = getΔŨ!(ΔŨ, mpc, transcription, Z̃) - Ŷ0, x̂0end = predict!(Ŷ0, x̂0end, X̂0, Û0, K̄, mpc, model, transcription, U0, Z̃) + Ŷ0, x̂0end = predict!(Ŷ0, x̂0end, X̂0, Û0, Ûi, K̄, mpc, model, transcription, U0, Z̃) Ue, Ŷe = extended_vectors!(Ue, Ŷe, mpc, U0, Ŷ0) ϵ = getslack(mpc, Z̃) gc = con_custom!(gc, mpc, Ue, Ŷe, ϵ) g = con_nonlinprog!(g, mpc, model, transcription, x̂0end, Ŷ0, gc, ϵ) - geq = con_nonlinprogeq!(geq, X̂0, Û0, K̄, mpc, model, transcription, U0, Z̃) + geq = con_nonlinprogeq!(geq, X̂0, Û0, Ûi, K̄, mpc, model, transcription, U0, Z̃) return nothing end diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 31b979af8..5c8d016dd 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1276,7 +1276,7 @@ getU0!(U0, mpc::PredictiveController, Z̃) = (mul!(U0, mpc.P̃u, Z̃) .+ mpc.Tu_ @doc raw""" predict!( - Ŷ0, x̂0end, _ , _ , _ , + Ŷ0, x̂0end, _ , _ , _ , _ , mpc::PredictiveController, model::LinModel, transcription::TranscriptionMethod, _ , Z̃ ) -> Ŷ0, x̂0end @@ -1294,7 +1294,7 @@ identical for any [`TranscriptionMethod`](@ref) if the model is linear: ``` """ function predict!( - Ŷ0, x̂0end, _, _, _, + Ŷ0, x̂0end, _, _, _, _, mpc::PredictiveController, ::LinModel, ::TranscriptionMethod, _ , Z̃ ) @@ -1306,14 +1306,14 @@ end @doc raw""" predict!( - Ŷ0, x̂0end, X̂0, Û0, K̄, + Ŷ0, x̂0end, X̂0, Û0, Ûi, K̄, mpc::PredictiveController, model::NonLinModel, transcription::SingleShooting, U0, _ ) -> Ŷ0, x̂0end Compute vectors if `model` is a [`NonLinModel`](@ref) and for [`SingleShooting`](@ref). -The method mutates `Ŷ0`, `x̂0end`, `X̂0`, `Û0` and `K` arguments. The augmented model of +The method mutates `Ŷ0`, `x̂0end`, `X̂0`, `Û0`, `Ûi` and `K` arguments. The augmented model of [`f̂!`](@ref) and [`ĥ!`](@ref) functions is called recursively in a `for` loop: ```math \begin{aligned} @@ -1324,20 +1324,32 @@ The method mutates `Ŷ0`, `x̂0end`, `X̂0`, `Û0` and `K` arguments. The augm for ``j = 0, 1, ... , H_p``. """ function predict!( - Ŷ0, x̂0end, X̂0, Û0, K̄, - mpc::PredictiveController, model::NonLinModel, ::SingleShooting, + Ŷ0, x̂0end, X̂0, Û0, Ûi, K̄, + mpc::PredictiveController, model::NonLinModel, transcription::SingleShooting, U0, _ ) - nu, nx̂, ny, nd, nk̄, Hp = model.nu, mpc.estim.nx̂, model.ny, model.nd, model.nk̄, mpc.Hp + nu, ny, nd, nk̄ = model.nu, model.ny, model.nd, model.nk̄ + nx̂, Hp = mpc.estim.nx̂, mpc.Hp + h = transcription.h D̂0 = mpc.D̂0 + D̂i = mpc.buffer.D̂ x̂0 = @views mpc.estim.x̂0[1:nx̂] d̂0 = @views mpc.d0[1:nd] for j=1:Hp u0 = @views U0[(1 + nu*(j-1)):(nu*j)] û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] - k̄ = @views K̄[(1 + nk̄*(j-1)):(nk̄*j)] + k̄ = @views K̄[(1 + nk̄*(j-1)):(nk̄*j)] + ûi = @views Ûi[(1 + nu*(j-1)):(nu*j)] + d̂i = @views D̂i[(1 + nd*(j-1)):(nd*j)] + d̂0next = @views D̂0[(1 + nd*(j-1)):(nd*j)] x̂0next = @views X̂0[(1 + nx̂*(j-1)):(nx̂*j)] - f̂!(x̂0next, û0, k̄, mpc.estim, model, x̂0, u0, d̂0) + û0next = @views j < Hp ? Û0[(1 + nu*j):(nu*(j+1))] : Û0[1:nu] + if h > 0 && j < Hp + u0next = @views U0[(1 + nu*j):(nu*(j+1))] + else # j=Hp special case, û0(k+Hp-1) = û0(k+Hp), since Hc ≤ Hp implies Δu(k+Hp)=0: + u0next = u0 + end + f̂!(x̂0next, û0, û0next, k̄, ûi, d̂i, mpc.estim, model, x̂0, u0, d̂0, u0next, d̂0next) x̂0 = @views X̂0[(1 + nx̂*(j-1)):(nx̂*j)] d̂0 = @views D̂0[(1 + nd*(j-1)):(nd*j)] ŷ0 = @views Ŷ0[(1 + ny*(j-1)):(ny*j)] @@ -1350,7 +1362,7 @@ end @doc raw""" predict!( - Ŷ0, x̂0end, _ , _ , _ , + Ŷ0, x̂0end, _ ,_ , _ , _ , mpc::PredictiveController, model::SimModel, transcription::TranscriptionMethod, _ , Z̃ ) -> Ŷ0, x̂0end @@ -1366,7 +1378,7 @@ for ``j = 1, 2, ... , H_p``, and in which ``\mathbf{x̂_0}`` is the augmented st from the decision variable `Z̃`. """ function predict!( - Ŷ0, x̂0end, _, _, _, + Ŷ0, x̂0end, _, _, _, _, mpc::PredictiveController, model::SimModel, transcription::TranscriptionMethod, _ , Z̃ ) @@ -1475,7 +1487,7 @@ end @doc raw""" con_nonlinprogeq!( - geq, X̂0, Û0, K̄ + geq, X̂0, Û0, Ûi, K̄ mpc::PredictiveController, model::NonLinModel, transcription::MultipleShooting, U0, Z̃ ) -> geq @@ -1496,17 +1508,18 @@ state update function [`f!`](@ref). The disturbed input ``\mathbf{û_0}`` is def [`f̂!`](@ref) documentation. """ function con_nonlinprogeq!( - geq, X̂0, Û0, K̄, + geq, X̂0, Û0, Ûi, K̄, mpc::PredictiveController, model::NonLinModel, transcription::MultipleShooting, U0, Z̃ ) nx̂, nx, nu, nd, nk̄ = mpc.estim.nx̂, model.nx, model.nu, model.nd, model.nk̄ Hp, Hc = mpc.Hp, mpc.Hc nΔU, nX̂ = nu*Hc, nx̂*Hp - f_threads = transcription.f_threads + h, f_threads = transcription.h, transcription.f_threads D̂0 = mpc.D̂0 X̂0_Z̃ = @views Z̃[(nΔU+1):(nΔU+nX̂)] Û0 = disturbedinput!(Û0, mpc.estim, mpc.estim.x̂0, X̂0_Z̃, U0) + D̂i = mpc.buffer.D̂ @threadsif f_threads for j=1:Hp if j < 2 x̂d_Z̃ = @views mpc.estim.x̂0[1:nx] @@ -1515,12 +1528,17 @@ function con_nonlinprogeq!( x̂d_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-2)):(nx̂*(j-2) + nx)] d̂0 = @views D̂0[(1 + nd*(j-2)):(nd*(j-1))] end - û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] - k̄ = @views K̄[(1 + nk̄*(j-1)):(nk̄*j)] + û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] + ûi = @views Ûi[(1 + nu*(j-1)):(nu*j)] + d̂i = @views D̂i[(1 + nd*(j-1)):(nd*j)] + k̄ = @views K̄[(1 + nk̄*(j-1)):(nk̄*j)] + # j=Hp special case, û0(k+Hp-1) = û0(k+Hp), since Hc ≤ Hp implies Δu(k+Hp)=0: + û0next = @views (h > 0 && j < Hp) ? Û0[(1 + nu*j):(nu*(j+1))] : û0 + d̂0next = @views D̂0[(1 + nd*(j-1)):(nd*j)] x̂dnext = @views X̂0[(1 + nx̂*(j-1)):(nx̂*(j-1) + nx)] x̂dnext_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*(j-1) + nx)] - ŝdnext = @views geq[(1 + nx*(j-1)):(nx*j)] - f!(x̂dnext, k̄, model, x̂d_Z̃, û0, d̂0, model.p) + ŝdnext = @views geq[(1 + nx*(j-1)):(nx*j)] + f!(x̂dnext, k̄, ûi, d̂i, model, x̂d_Z̃, û0, d̂0, û0next, d̂0next) ŝdnext .= @. x̂dnext - x̂dnext_Z̃ end return geq @@ -1528,7 +1546,7 @@ end @doc raw""" con_nonlinprogeq!( - geq, _ , Û0, K̄dot, + geq, _ , Û0, _ , K̄dot, mpc::PredictiveController, model::SimModel, transcription::TrapezoidalCollocation, U0, Z̃ ) -> geq @@ -1560,7 +1578,7 @@ also computed from ``j = 0, 1, ... , H_p-1`` and: ``` """ function con_nonlinprogeq!( - geq, _ , Û0, K̄dot, + geq, _ , Û0, _ , K̄dot, mpc::PredictiveController, model::SimModel, transcription::TrapezoidalCollocation, U0, Z̃ ) @@ -1621,7 +1639,7 @@ end @doc raw""" con_nonlinprogeq!( - geq, _ , Û0, K̄dot, + geq, _ , Û0, Ûi, K̄dot, mpc::PredictiveController, model::SimModel, transcription::OrthogonalCollocation, U0, Z̃ ) -> geq @@ -1657,7 +1675,7 @@ for ``j = 0, 1, ... , H_p-1``. The defects for the continuity constraints and th states are linear equality constraints (see [`init_defectmat`](@ref)). """ function con_nonlinprogeq!( - geq, _ , Û0, K̄dot, + geq, _ , Û0, Ûi, K̄dot, mpc::PredictiveController, model::SimModel, transcription::OrthogonalCollocation, U0, Z̃ ) @@ -1675,8 +1693,7 @@ function con_nonlinprogeq!( Â0 = @views Z̃[(1 + nΔU + nX̂):(nΔU + nX̂ + nÂ)] K̄ = @views Z̃[(1 + nΔU + nX̂ + nÂ):(nΔU + nX̂ + n + nK̄)] Ā = @views Z̃[(1 + nΔU + nX̂ + n + nK̄):(nΔU + nX̂ + n + nK̄ + nĀ)] - Dtemp = mpc.buffer.D̂ - Utemp = similar(Û0) # TODO: remove this allocation + D̂i = mpc.buffer.D̂ Û0 = disturbedinput!(Û0, mpc.estim, mpc.estim.x̂0, X̂0, U0) Ŝk̄ = @views geq[1:nŜk̄] Q⁻ = @views geq[(nŜk̄+1):(nŜk̄+nÂ)] @@ -1710,7 +1727,7 @@ function con_nonlinprogeq!( Δk[(1 + (i-1)*nx):(i*nx)] = @views k̄[(1 + (i-1)*nx):(i*nx)] .- x̂d end mul!(ŝk̄, Mo, Δk) - d̂i, ûi = @views Dtemp[(1 + nd*(j-1)):(nd*j)], Utemp[(1 + nu*(j-1)):(nu*j)] + ûi, d̂i = @views Ûi[(1 + nu*(j-1)):(nu*j)], D̂i[(1 + nd*(j-1)):(nd*j)] for i=1:no k̇i, ki = @views k̄dot[(1 + (i-1)*nx):(i*nx)], k̄[(1 + (i-1)*nx):(i*nx)] qi, ai = @views q̄[(1 + (i-1)*na):(i*na)], ā[(1 + (i-1)*na):(i*na)] @@ -1731,4 +1748,4 @@ function con_nonlinprogeq!( end "No eq. constraints for other cases e.g. [`SingleShooting`](@ref), returns `geq` unchanged." -con_nonlinprogeq!(geq,_,_,_,::PredictiveController,::SimModel,::TranscriptionMethod,_,_)=geq +con_nonlinprogeq!(geq,_,_,_,_,::PredictiveController,::SimModel,::TranscriptionMethod,_,_)=geq diff --git a/src/estimator/construct.jl b/src/estimator/construct.jl index 4541155d0..c6a19aba9 100644 --- a/src/estimator/construct.jl +++ b/src/estimator/construct.jl @@ -1,6 +1,8 @@ struct StateEstimatorBuffer{NT<:Real} u ::Vector{NT} û ::Vector{NT} + ûi::Vector{NT} + di::Vector{NT} k̄ ::Vector{NT} x̂ ::Vector{NT} â ::Vector{NT} @@ -19,6 +21,7 @@ struct StateEstimatorBuffer{NT<:Real} ym::Vector{NT} ŷ ::Vector{NT} d ::Vector{NT} + ûnext::Vector{NT} empty::Vector{NT} end @@ -43,6 +46,8 @@ function StateEstimatorBuffer{NT}( u = Vector{NT}(undef, nu) û = Vector{NT}(undef, nu) k̄ = Vector{NT}(undef, nk̄) + ûi = Vector{NT}(undef, nu) + di = Vector{NT}(undef, nd) x̂ = Vector{NT}(undef, nx̂) â = Vector{NT}(undef, na) Z̃ = Vector{NT}(undef, nZ̃) @@ -60,9 +65,10 @@ function StateEstimatorBuffer{NT}( ym = Vector{NT}(undef, nym) ŷ = Vector{NT}(undef, ny) d = Vector{NT}(undef, nd) + ûnext = Vector{NT}(undef, nu) empty = Vector{NT}(undef, 0) return StateEstimatorBuffer{NT}( - u, û, k̄, x̂, â, Z̃, V̂, Ŵ, X̂, Â, Ŷ, U, D, P̂, Q̂, R̂, K̂, ym, ŷ, d, empty + u, û, ûi, di, k̄, x̂, â, Z̃, V̂, Ŵ, X̂, Â, Ŷ, U, D, P̂, Q̂, R̂, K̂, ym, ŷ, d, ûnext, empty ) end diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index 4b59ce8ea..08c123ef9 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -14,7 +14,10 @@ function remove_op!(estim::StateEstimator, ym, d, u=nothing) end @doc raw""" - f̂!(x̂0next, û0, k̄, estim::StateEstimator, model::SimModel, x̂0, u0, d0) -> nothing + f̂!( + x̂0next, û0, û0next, k̄, ûi, di, + estim::StateEstimator, model::SimModel, x̂0, u0, d0, u0next, d0next + ) -> nothing Mutating state update function ``\mathbf{f̂}`` of the augmented model. @@ -27,11 +30,13 @@ the function returns the next state of the augmented model, as deviation vectors \end{aligned} ``` where ``\mathbf{x̂_0}(k+1)`` is stored in `x̂0next` argument. The method mutates `x̂0next`, -`û0` and `k` in place. The argument `û0` stores the disturbed input of the augmented model -``\mathbf{û_0}``, and `k̄`, the intermediate stage values of `model.solver`, when applicable. -The model parameter `model.p` is not included in the function signature for conciseness. -The operating points are handled inside ``\mathbf{f̂}``. See Extended Help for details on -``\mathbf{û_0, f̂}`` and ``\mathbf{ĥ}`` implementations. +`û0`, `û0next`, `k`, `ûi` and `di` in-place. The argument `û0` and `û0next` respectively +store the disturbed input of the augmented model ``\mathbf{û_0}`` the current and next time +step, and `k̄`, the intermediate stage values of `model.solver`, when applicable. The +arguments store the interpolated values of the disturbed input and disturbance, +respectively. The model parameter `model.p` is not included in the function signature for +conciseness. The operating points are handled inside ``\mathbf{f̂}``. See Extended Help for +details on ``\mathbf{û_0, f̂}`` and ``\mathbf{ĥ}`` implementations. # Extended Help !!! details "Extended Help" @@ -61,12 +66,21 @@ The operating points are handled inside ``\mathbf{f̂}``. See Extended Help for are computed by [`augment_model`](@ref) (almost always zeros in practice for [`NonLinModel`](@ref)). """ -function f̂!(x̂0next, û0, k̄, estim::StateEstimator, model::SimModel, x̂0, u0, d0) - return f̂!(x̂0next, û0, k̄, model, estim.As, estim.Cs_u, estim.f̂op, estim.x̂op, x̂0, u0, d0) +function f̂!( + x̂0next, û0, û0next, k̄, ûi, di, + estim::StateEstimator, model::SimModel, x̂0, u0, d0, u0next, d0next +) + return f̂!( + x̂0next, û0, û0next, k̄, ûi, di, + model, estim.As, estim.Cs_u, estim.f̂op, estim.x̂op, x̂0, u0, d0, u0next, d0next + ) end @doc raw""" - f̂!(x̂0next, _ , _ , estim::StateEstimator, model::LinModel, x̂0, u0, d0) -> nothing + f̂!( + x̂0next, _ , _ , _ , _ , _ , + estim::StateEstimator, model::LinModel, x̂0, u0, d0, _ , _ + ) -> nothing Use the augmented model matrices and operating points if `model` is a [`LinModel`](@ref). @@ -83,7 +97,7 @@ Use the augmented model matrices and operating points if `model` is a [`LinModel ``` with the augmented matrices constructed by [`augment_model`](@ref). """ -function f̂!(x̂0next, _ , _ , estim::StateEstimator, ::LinModel, x̂0, u0, d0) +function f̂!(x̂0next, _, _, _, _, _, estim::StateEstimator, ::LinModel, x̂0, u0, d0, _, _) mul!(x̂0next, estim.Â, x̂0) mul!(x̂0next, estim.B̂u, u0, 1, 1) mul!(x̂0next, estim.B̂d, d0, 1, 1) @@ -92,18 +106,26 @@ function f̂!(x̂0next, _ , _ , estim::StateEstimator, ::LinModel, x̂0, u0, d0) end """ - f̂!(x̂0next, û0, k, model::SimModel, As, Cs_u, f̂op, x̂op, x̂0, u0, d0) + f̂!( + x̂0next, û0, û0next, k̄, ûi, di, + model::SimModel, As, Cs_u, f̂op, x̂op, x̂0, u0, d0, u0next, d0next + ) -> nothing Same than [`f̂!`](@ref) for [`SimModel`](@ref) but without the `estim` argument. """ -function f̂!(x̂0next, û0, k̄, model::SimModel, As, Cs_u, f̂op, x̂op, x̂0, u0, d0) +function f̂!( + x̂0next, û0, û0next, k̄, ûi, di, + model::SimModel, As, Cs_u, f̂op, x̂op, x̂0, u0, d0, u0next, d0next +) # `@views` macro avoid copies with matrix slice operator e.g. [a:b] @views x̂d, x̂s = x̂0[1:model.nx], x̂0[model.nx+1:end] @views x̂dnext, x̂snext = x̂0next[1:model.nx], x̂0next[model.nx+1:end] - mul!(û0, Cs_u, x̂s) # ys_u = Cs_u*xs - û0 .+= u0 # û0 = u0 + ys_u - f!(x̂dnext, k̄, model, x̂d, û0, d0, model.p) mul!(x̂snext, As, x̂s) + mul!(û0, Cs_u, x̂s) # ys_u(k) = Cs_u*xs(k) + û0 .+= u0 # û0(k) = u0(k) + ys_u(k) + mul!(û0next, Cs_u, x̂snext) # ys_u(k+1) = Cs_u*xs(k+1) + û0next .+= u0next # û0(k+1) = u0(k+1) + ys_u(k+1) + f!(x̂dnext, k̄, ûi, di, model, x̂d, û0, d0, û0next, d0next) x̂0next .+= f̂op .- x̂op return nothing end @@ -136,7 +158,7 @@ Same than [`ĥ!`](@ref) for [`SimModel`](@ref) but without the `estim` argument function ĥ!(ŷ0, model::SimModel, Cs_y::AbstractMatrix, x̂0, d0) # `@views` macro avoid copies with matrix slice operator e.g. [a:b] @views x̂d, x̂s = x̂0[1:model.nx], x̂0[model.nx+1:end] - h!(ŷ0, model, x̂d, d0, model.p) # y0 = h(xd, d0) + h!(ŷ0, model, x̂d, d0) # y0 = h(xd, d0) mul!(ŷ0, Cs_y, x̂s, 1, 1) # ŷ0 = y0 + Cs_y*xs return nothing end @@ -367,10 +389,9 @@ operating points with [`remove_op!`](@ref), calls [`update_estimate!`](@ref) and state estimate for the next time step ``\mathbf{x̂}_k(k+1)``. The method [`preparestate!`](@ref) should be called prior to this one to correct the estimate when applicable (if `estim.direct == true`). If `isnothing(ym)`, only the prediction step is performed in -[`update_estimate!`](@ref), for when the measured outputs are temporarily unavailable. Note -that the [`MovingHorizonEstimator`](@ref) with the default `direct=true` option is not able -to estimate ``\mathbf{x̂}_k(k+1)``, the returned value is therefore the current corrected -state ``\mathbf{x̂}_k(k)``. +[`update_estimate!`](@ref), for when the measured outputs are temporarily unavailable. The +`unext` and `dnext` arguments are not supported for [`StateEstimator`](@ref) since they +complexify the implementation and produce non-causal filters, so less useful in practice. # Examples ```jldoctest diff --git a/src/estimator/internal_model.jl b/src/estimator/internal_model.jl index cd8225286..3eb3d83f1 100644 --- a/src/estimator/internal_model.jl +++ b/src/estimator/internal_model.jl @@ -173,19 +173,28 @@ function matrices_internalmodel(model::SimModel{NT}) where NT<:Real end @doc raw""" - f̂!(x̂0next, _ , k̄, estim::InternalModel, model::SimModel, x̂0, u0, d0) + f̂!( + x̂0next, _ , _ , k̄, ûi, di, + estim::InternalModel, model::SimModel, x̂0, u0, d0, u0next, d0next + ) -> nothing State update function ``\mathbf{f̂}`` of [`InternalModel`](@ref). It calls [`f!`](@ref) directly since this estimator does not augment the states. """ -function f̂!(x̂0next, _ , k̄, estim::InternalModel, model::SimModel, x̂0, u0, d0) - f!(x̂0next, k̄, model, x̂0, u0, d0, model.p) +function f̂!( + x̂0next, _ , _ , k̄, ûi, di, + estim::InternalModel, model::SimModel, x̂0, u0, d0, u0next, d0next +) + f!(x̂0next, k̄, ûi, di, model, x̂0, u0, d0, u0next, d0next) x̂0next .+= estim.f̂op .- estim.x̂op return nothing end -function f̂!(x̂0next, _ , k̄, estim::InternalModel, model::LinModel, x̂0, u0, d0) - f!(x̂0next, k̄, model, x̂0, u0, d0, model.p) +function f̂!( + x̂0next, _ , _ , k̄, ûi, di, + estim::InternalModel, model::LinModel, x̂0, u0, d0, u0next, d0next +) + f!(x̂0next, k̄, ûi, di, model, x̂0, u0, d0, u0next, d0next) x̂0next .+= estim.f̂op .- estim.x̂op return nothing end @@ -195,8 +204,8 @@ end Output function ``\mathbf{ĥ}`` of [`InternalModel`](@ref), it calls [`h!`](@ref). """ -ĥ!(ŷ0, ::InternalModel, model::SimModel, x̂0, d0) = h!(ŷ0, model, x̂0, d0, model.p) -ĥ!(ŷ0, ::InternalModel, model::LinModel, x̂0, d0) = h!(ŷ0, model, x̂0, d0, model.p) +ĥ!(ŷ0, ::InternalModel, model::SimModel, x̂0, d0) = h!(ŷ0, model, x̂0, d0) +ĥ!(ŷ0, ::InternalModel, model::LinModel, x̂0, d0) = h!(ŷ0, model, x̂0, d0) @doc raw""" ĥ!(ŷ0, estim::InternalModel, model::SimModel, x̂0, â0, d0) @@ -313,11 +322,11 @@ This estimator does not augment the state vector, thus ``\mathbf{x̂ = x̂_d}``. [`init_internalmodel`](@ref) for details. """ function update_estimate!(estim::InternalModel, u0, _ , d0) - model = estim.model + model, buffer = estim.model, estim.buffer x̂d, x̂s, ŷs = estim.x̂d, estim.x̂s, estim.ŷs # -------------- deterministic model --------------------- - x̂dnext, û0, k̄ = estim.buffer.x̂, estim.buffer.û, estim.buffer.k̄ - f̂!(x̂dnext, û0, k̄, estim, estim.model, x̂d, u0, d0) + x̂dnext, k̄, ûi, di = buffer.x̂, buffer.k̄, buffer.ûi, buffer.di + f!(x̂dnext, k̄, ûi, di, model, x̂d, u0, d0, u0, d0) x̂d .= x̂dnext # this also updates estim.x̂0 (they are the same object) # --------------- stochastic model ----------------------- x̂snext = estim.x̂snext @@ -358,7 +367,7 @@ function init_estimate!(estim::InternalModel, model::LinModel{NT}, y0m, d0, u0) M = I - model.A x̂d .= M\x̂_tmp # also updates estim.x̂0 (they are the same object) ŷ0d = estim.buffer.ŷ - h!(ŷ0d, model, x̂d, d0, model.p) + h!(ŷ0d, model, x̂d, d0) ŷs = ŷ0d ŷs[estim.i_ym] .= @views y0m .- ŷ0d[estim.i_ym] # ŷs=0 for unmeasured outputs : diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index 7e4d69217..5bd5eaa08 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -875,12 +875,14 @@ function update_estimate!(estim::UnscentedKalmanFilter, u0, y0m, d0) if !estim.direct correct_estimate!(estim, y0m, d0) end + buffer = estim.buffer x̂0corr, X̂0corr, P̂corr = estim.x̂0, estim.X̂0, estim.cov.P̂ Q̂, nx̂ = estim.cov.Q̂, estim.nx̂ γ, m̂, Ŝ = estim.γ, estim.m̂, estim.Ŝ - x̂0next, û0, k̄ = estim.buffer.x̂, estim.buffer.û, estim.buffer.k̄ + x̂0next, û0, û0next, k̄ = buffer.x̂, buffer.û, buffer.ûnext, buffer.k̄ + ûi, di = buffer.ûi, buffer.di # in-place operations to reduce allocations: - P̂corr_temp = Hermitian(estim.buffer.P̂, :L) + P̂corr_temp = Hermitian(buffer.P̂, :L) P̂corr_temp .= P̂corr P̂corr_chol = cholesky!(P̂corr_temp) # also modifies P̂corr_temp sqrtP̂corr = P̂corr_chol.L @@ -891,7 +893,7 @@ function update_estimate!(estim::UnscentedKalmanFilter, u0, y0m, d0) X̂0next = X̂0corr # X̂0next is an alias of X̂0corr for j in axes(X̂0next, 2) x̂0corr_j, x̂0next_j = @views X̂0corr[:, j], X̂0next[:, j] - @views f̂!(x̂0next, û0, k̄, estim, estim.model, x̂0corr_j, u0, d0) + f̂!(x̂0next, û0, û0next, k̄, ûi, di, estim, estim.model, x̂0corr_j, u0, d0, u0, d0) x̂0next_j .= x̂0next end mul!(x̂0next, X̂0next, m̂) @@ -899,7 +901,7 @@ function update_estimate!(estim::UnscentedKalmanFilter, u0, y0m, d0) X̄0next .= X̂0next .- x̂0next Ŝ_X̄0nextᵀ = estim.X̂0' mul!(Ŝ_X̄0nextᵀ, Ŝ, X̄0next') - P̂next = estim.buffer.P̂ + P̂next = buffer.P̂ mul!(P̂next, X̄0next, Ŝ_X̄0nextᵀ) P̂next .+= Q̂ estim.x̂0 .= x̂0next @@ -1119,24 +1121,29 @@ function get_ekf_linfuncs(NT, model, i_ym, nint_u, nint_ym, jacobian) nu, ny, nd, nk̄ = model.nu, model.ny, model.nd, model.nk̄ nx̂ = model.nx + size(As, 1) x̂op = f̂op = zeros(nx̂) # not important for Jacobian computations - function f̂_ekf!(x̂0next, x̂0, û0, k, u0, d0) - return f̂!(x̂0next, û0, k, model, As, Cs_u, f̂op, x̂op, x̂0, u0, d0) + function f̂_ekf!(x̂0next, x̂0, u0, d0, û0, û0next, k̄, ûi, di) + return f̂!( + x̂0next, û0, û0next, k̄, ûi, di, + model, As, Cs_u, f̂op, x̂op, x̂0, u0, d0, u0, d0 + ) end ĥ_ekf!(ŷ0, x̂0, d0) = ĥ!(ŷ0, model, Cs_y, x̂0, d0) strict = Val(true) x̂0next = zeros(NT, nx̂) ŷ0 = zeros(NT, ny) x̂0 = zeros(NT, nx̂) - û0 = Cache(zeros(NT, nu)) - k̄ = Cache(zeros(NT, nk̄)) - cst_u0 = Constant(rand(NT, nu)) - cst_d0 = Constant(rand(NT, nd)) + û0, û0next = Cache(zeros(NT, nu)), Cache(zeros(NT, nu)) + k̄ = Cache(zeros(NT, nk̄)) + ûi, di = Cache(zeros(NT, nu)), Cache(zeros(NT, nd)) + cst_u0 = Constant(rand(NT, nu)) + cst_d0 = Constant(rand(NT, nd)) + F̂cache = (û0, û0next, k̄, ûi, di) F̂prep = prepare_jacobian( - f̂_ekf!, x̂0next, jacobian, x̂0, û0, k̄, cst_u0, cst_d0; strict + f̂_ekf!, x̂0next, jacobian, x̂0, cst_u0, cst_d0, F̂cache...; strict ) Ĥprep = prepare_jacobian(ĥ_ekf!, ŷ0, jacobian, x̂0, cst_d0; strict) function linfuncF̂!(F̂, x̂0next, backend, x̂0, cst_u0, cst_d0) - return jacobian!(f̂_ekf!, x̂0next, F̂, F̂prep, backend, x̂0, û0, k̄, cst_u0, cst_d0) + return jacobian!(f̂_ekf!, x̂0next, F̂, F̂prep, backend, x̂0, cst_u0, cst_d0, F̂cache...) end function linfuncĤ!(Ĥ, ŷ0, backend, x̂0, cst_d0) return jacobian!(ĥ_ekf!, ŷ0, Ĥ, Ĥprep, backend, x̂0, cst_d0) @@ -1275,16 +1282,18 @@ They predict the state `x̂` and covariance `P̂` with the same equations. See [`update_estimate`](@ref) methods for the equations. """ function predict_estimate_kf!(estim::Union{KalmanFilter, ExtendedKalmanFilter}, u0, d0, Â) + buffer = estim.buffer x̂0corr, P̂corr = estim.x̂0, estim.cov.P̂ Q̂ = estim.cov.Q̂ - x̂0next, û0, k̄ = estim.buffer.x̂, estim.buffer.û, estim.buffer.k̄ + x̂0next, û0, û0next, k̄ = buffer.x̂, buffer.û, buffer.ûnext, buffer.k̄ + ûi, di = buffer.ûi, buffer.di # in-place operations to reduce allocations: - f̂!(x̂0next, û0, k̄, estim, estim.model, x̂0corr, u0, d0) - P̂corr_Âᵀ = estim.buffer.P̂ + f̂!(x̂0next, û0, û0next, k̄, ûi, di, estim, estim.model, x̂0corr, u0, d0, u0, d0) + P̂corr_Âᵀ = buffer.P̂ mul!(P̂corr_Âᵀ, P̂corr, Â') - Â_P̂corr_Âᵀ = estim.buffer.Q̂ + Â_P̂corr_Âᵀ = buffer.Q̂ mul!(Â_P̂corr_Âᵀ, Â, P̂corr_Âᵀ) - P̂next = estim.buffer.P̂ + P̂next = buffer.P̂ P̂next .= Â_P̂corr_Âᵀ .+ Q̂ estim.x̂0 .= x̂0next estim.cov.P̂ .= Hermitian(P̂next, :L) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 619ad598f..f9d498d35 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -1471,11 +1471,12 @@ function get_nonlinobj_op( V̂e::Vector{JNT}, X̂e::Vector{JNT} = zeros(JNT, nV̂e), zeros(JNT, nX̂e) K̄::Vector{JNT} = zeros(JNT, nK̄) Û0::Vector{JNT}, Ŷ0::Vector{JNT} = zeros(JNT, nU), zeros(JNT, nŶ) + Ûi::Vector{JNT} = zeros(JNT, nU) gc::Vector{JNT}, g::Vector{JNT} = zeros(JNT, nc), zeros(JNT, ng) geq::Vector{JNT} = zeros(JNT, neq) - function J!(Z̃, x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq) + function J!(Z̃, x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, Ûi, K̄, Ŷ0, gc, g, geq) update_predictions!( - x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, Ûi, K̄, Ŷ0, gc, g, geq, estim, Z̃ ) return obj_nonlinprog(estim, model, x̄, V̂, Ŵ, Z̃) end @@ -1484,7 +1485,7 @@ function get_nonlinobj_op( Cache(x̂0arr), Cache(â0arr), Cache(x̄), Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(Â0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), - Cache(Û0), Cache(K̄), Cache(Ŷ0), + Cache(Û0), Cache(Ûi), Cache(K̄), Cache(Ŷ0), Cache(gc), Cache(g), Cache(geq) ) # temporarily "fill" the estimation window for the preparation of the gradient: @@ -1591,23 +1592,24 @@ function get_nonlincon_oracle( V̂e::Vector{JNT}, X̂e::Vector{JNT} = zeros(JNT, nV̂e), zeros(JNT, nX̂e) K̄::Vector{JNT} = zeros(JNT, nK̄) Û0::Vector{JNT}, Ŷ0::Vector{JNT} = zeros(JNT, nU), zeros(JNT, nŶ) + Ûi::Vector{JNT} = zeros(JNT, nU) gc::Vector{JNT}, g::Vector{JNT} = zeros(JNT, nc), zeros(JNT, ng) geq::Vector{JNT} = zeros(JNT, neq) gi::Vector{JNT} = zeros(JNT, ngi) λi::Vector{JNT}, λeq::Vector{JNT} = rand(JNT, ngi), rand(JNT, neq) # -------------- inequality constraint: nonlinear oracle ------------------------- - function gi!(gi, Z̃, x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq) + function gi!(gi, Z̃, x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, Ûi, K̄, Ŷ0, gc, g, geq) update_predictions!( - x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, Ûi, K̄, Ŷ0, gc, g, geq, estim, Z̃ ) gi .= @views g[i_g] return nothing end function ℓ_gi( - Z̃, λi, x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, gi + Z̃, λi, x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, Ûi, K̄, Ŷ0, gc, g, geq, gi ) update_predictions!( - x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, Ûi, K̄, Ŷ0, gc, g, geq, estim, Z̃ ) gi .= @views g[i_g] return dot(λi, gi) @@ -1617,7 +1619,7 @@ function get_nonlincon_oracle( Cache(x̂0arr), Cache(â0arr), Cache(x̄), Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(Â0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), - Cache(Û0), Cache(K̄), Cache(Ŷ0), + Cache(Û0), Cache(Ûi), Cache(K̄), Cache(Ŷ0), Cache(gc), Cache(g), Cache(geq) ) # temporarily "fill" the estimation windows for the preparation of the gradient: @@ -1631,7 +1633,7 @@ function get_nonlincon_oracle( Cache(x̂0arr), Cache(â0arr), Cache(x̄), Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(Â0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), - Cache(Û0), Cache(K̄), Cache(Ŷ0), + Cache(Û0), Cache(Ûi), Cache(K̄), Cache(Ŷ0), Cache(gc), Cache(g), Cache(geq), Cache(gi) ) @@ -1678,17 +1680,17 @@ function get_nonlincon_oracle( eval_hessian_lagrangian = isnothing(hess) ? nothing : ∇²gi_func! ) # ------------- equality constraints : nonlinear oracle ------------------------------ - function geq!(geq, Z̃, x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g) + function geq!(geq, Z̃, x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, Ûi, K̄, Ŷ0, gc, g) update_predictions!( - x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, Ûi, K̄, Ŷ0, gc, g, geq, estim, Z̃ ) return nothing end function ℓ_geq( - Z̃, λeq, x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq + Z̃, λeq, x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, Ûi, K̄, Ŷ0, gc, g, geq ) update_predictions!( - x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, Ûi, K̄, Ŷ0, gc, g, geq, estim, Z̃ ) return dot(λeq, geq) end @@ -1697,7 +1699,7 @@ function get_nonlincon_oracle( Cache(x̂0arr), Cache(â0arr), Cache(x̄), Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(Â0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), - Cache(Û0), Cache(K̄), Cache(Ŷ0), + Cache(Û0), Cache(Ûi), Cache(K̄), Cache(Ŷ0), Cache(gc), Cache(g) ) estim.Nk[] = He # see comment above @@ -1710,7 +1712,7 @@ function get_nonlincon_oracle( Cache(x̂0arr), Cache(â0arr), Cache(x̄), Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(Â0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), - Cache(Û0), Cache(K̄), Cache(Ŷ0), + Cache(Û0), Cache(Ûi), Cache(K̄), Cache(Ŷ0), Cache(gc), Cache(g), Cache(geq) ) estim.Nk[] = He # see comment above diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 1b9e31f91..92b067f02 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -152,14 +152,15 @@ function getinfo(estim::MovingHorizonEstimator{NT}) where NT<:Real Z̃ = estim.Z̃ info = Dict{Symbol, Any}() V̂, Ŵ, X̂0, Â0, Û0, Ŷ0 = buffer.V̂, buffer.Ŵ, buffer.X̂, buffer.Â, buffer.U, buffer.Ŷ - x̂0arr, â0arr = buffer.x̂, buffer.â - x̄, K = Vector{NT}(undef, nx̂), Vector{NT}(undef, nk̄*Nk) + â0arr = buffer.â + x̂0arr, x̄ = Vector{NT}(undef, nx̂), Vector{NT}(undef, nx̂) + Ûi, K̄ = Vector{NT}(undef, nu*Nk), Vector{NT}(undef, nk̄*Nk) x̂0arr = getx̂0arr!(x̂0arr, estim, Z̃) â0arr = getâ0arr!(â0arr, estim, estim.transcription, Z̃) Ŵ = getŴ!(Ŵ, estim, estim.transcription, Z̃) x̄ = getx̄!(x̄, estim, x̂0arr) V̂, X̂0, Â0 = predict_mhe!( - V̂, X̂0, Â0, Û0, K, Ŷ0, estim, model, estim.transcription, x̂0arr, â0arr, Ŵ, Z̃ + V̂, X̂0, Â0, Û0, Ûi, K̄, Ŷ0, estim, model, estim.transcription, x̂0arr, â0arr, Ŵ, Z̃ ) Ŷ0 = predict_outputs_mhe!(Ŷ0, estim, X̂0, Â0, x̂0arr, â0arr) J = obj_nonlinprog(estim, estim.model, x̄, V̂, Ŵ, Z̃) @@ -238,6 +239,7 @@ function addinfo!(info, estim::MovingHorizonEstimator{NT}, model::SimModel) wher V̂e, X̂e = zeros(NT, nV̂e), zeros(NT, nX̂e) K̄ = zeros(NT, nK̄) Û0, Ŷ0 = zeros(NT, nU), zeros(NT, nŶ) + Ûi = zeros(NT, nU) gc, g = zeros(NT, nc), zeros(NT, ng) geq = zeros(NT, neq) gi = zeros(NT, ngi) @@ -245,12 +247,12 @@ function addinfo!(info, estim::MovingHorizonEstimator{NT}, model::SimModel) wher Cache(x̂0arr), Cache(â0arr), Cache(x̄), Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(Â0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), - Cache(Û0), Cache(K̄), Cache(Ŷ0), + Cache(Û0), Cache(Ûi), Cache(K̄), Cache(Ŷ0), Cache(gc), Cache(g), Cache(geq) ) - function J!(Z̃, x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq) + function J!(Z̃, x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, Ûi, K̄, Ŷ0, gc, g, geq) update_predictions!( - x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, Ûi, K̄, Ŷ0, gc, g, geq, estim, Z̃ ) return obj_nonlinprog(estim, model, x̄, V̂, Ŵ, Z̃) end @@ -268,12 +270,12 @@ function addinfo!(info, estim::MovingHorizonEstimator{NT}, model::SimModel) wher Cache(x̂0arr), Cache(â0arr), Cache(x̄), Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(Â0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), - Cache(Û0), Cache(K̄), Cache(Ŷ0), + Cache(Û0), Cache(Ûi), Cache(K̄), Cache(Ŷ0), Cache(gc), Cache(g), Cache(geq) ) - function gi!(gi, Z̃, x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq) + function gi!(gi, Z̃, x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, Ûi, K̄, Ŷ0, gc, g, geq) update_predictions!( - x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, Ûi, K̄, Ŷ0, gc, g, geq, estim, Z̃ ) gi .= @views g[i_g] return nothing @@ -300,15 +302,15 @@ function addinfo!(info, estim::MovingHorizonEstimator{NT}, model::SimModel) wher Cache(x̂0arr), Cache(â0arr), Cache(x̄), Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(Â0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), - Cache(Û0), Cache(K̄), Cache(Ŷ0), + Cache(Û0), Cache(Ûi), Cache(K̄), Cache(Ŷ0), Cache(gc), Cache(g), Cache(geq), Cache(gi) ) function ℓ_gi( - Z̃, λi, x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, gi + Z̃, λi, x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, Ûi, K̄, Ŷ0, gc, g, geq, gi ) update_predictions!( - x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, Ûi, K̄, Ŷ0, gc, g, geq, estim, Z̃ ) gi .= @views g[i_g] return dot(λi, gi) @@ -324,12 +326,12 @@ function addinfo!(info, estim::MovingHorizonEstimator{NT}, model::SimModel) wher Cache(x̂0arr), Cache(â0arr), Cache(x̄), Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(Â0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), - Cache(Û0), Cache(K̄), Cache(Ŷ0), + Cache(Û0), Cache(Ûi), Cache(K̄), Cache(Ŷ0), Cache(gc), Cache(g) ) - function geq!(geq, Z̃, x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g) + function geq!(geq, Z̃, x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, Ûi, K̄, Ŷ0, gc, g) update_predictions!( - x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, Ûi, K̄, Ŷ0, gc, g, geq, estim, Z̃ ) return nothing end @@ -355,14 +357,14 @@ function addinfo!(info, estim::MovingHorizonEstimator{NT}, model::SimModel) wher Cache(x̂0arr), Cache(â0arr), Cache(x̄), Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(Â0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), - Cache(Û0), Cache(K̄), Cache(Ŷ0), + Cache(Û0), Cache(Ûi), Cache(K̄), Cache(Ŷ0), Cache(gc), Cache(g), Cache(geq) ) function ℓ_geq( - Z̃, λeq, x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq + Z̃, λeq, x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, Ûi, K̄, Ŷ0, gc, g, geq ) update_predictions!( - x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, Ûi, K̄, Ŷ0, gc, g, geq, estim, Z̃ ) return dot(λeq, geq) end @@ -603,7 +605,7 @@ If first warm-starts the solver with [`set_warmstart!`](@ref). It then calls `@error` log in the REPL and returns the warm-start value. A failed optimization also prints [`getinfo`](@ref) results in the debug log [if activated](@extref Julia Example:-Enable-debug-level-messages). """ -function optim_objective!(estim::MovingHorizonEstimator{NT}) where NT<:Real +function optim_objective!(estim::MovingHorizonEstimator) optim = estim.optim Z̃var::Vector{JuMP.VariableRef} = optim[:Z̃var] Z̃s = set_warmstart!(estim, estim.transcription, Z̃var) @@ -665,15 +667,15 @@ otherwise the state is for the next time step. """ function getstate!(estim::MovingHorizonEstimator{NT}, Z̃) where NT<:Real model, buffer = estim.model, estim.buffer - nx̂, Nk = estim.nx̂, estim.Nk[] + nu, nx̂, Nk = model.nu, estim.nx̂, estim.Nk[] nk̄ = get_nk̄(model, estim.transcription) x̂0arr, â0arr = buffer.x̂, buffer.â V̂, Ŵ, X̂0, Â0, Û0, Ŷ0 = buffer.V̂, buffer.Ŵ, buffer.X̂, buffer.Â, buffer.U, buffer.Ŷ - K̄ = Vector{NT}(undef, nk̄*Nk) # TODO: remove the allocation + Ûi, K̄ = Vector{NT}(undef, nu*Nk), Vector{NT}(undef, nk̄*Nk) # TODO: remove allocations getŴ!(Ŵ, estim, estim.transcription, estim.Z̃) getx̂0arr!(x̂0arr, estim, Z̃) predict_mhe!( - V̂, X̂0, Â0, Û0, K̄, Ŷ0, estim, model, estim.transcription, x̂0arr, â0arr, Ŵ, Z̃ + V̂, X̂0, Â0, Û0, Ûi, K̄, Ŷ0, estim, model, estim.transcription, x̂0arr, â0arr, Ŵ, Z̃ ) estim.x̂0 .= @views X̂0[((Nk-1)*nx̂+1):(Nk*nx̂)] return nothing @@ -905,7 +907,7 @@ end """ update_predictions!( - x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, + x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, Ûi, K̄, Ŷ0, gc, g, geq, estim::MovingHorizonEstimator, Z̃ ) -> nothing @@ -914,7 +916,7 @@ Update in-place the vectors for the predictions of `estim` estimator at decision The method mutates all the arguments before `estim` argument. """ function update_predictions!( - x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, + x̂0arr, â0arr, x̄, Ŵ, V̂, X̂0, Â0, Ŵe, V̂e, X̂e, Û0, Ûi, K̄, Ŷ0, gc, g, geq, estim::MovingHorizonEstimator, Z̃ ) model, transcription = estim.model, estim.transcription @@ -923,14 +925,14 @@ function update_predictions!( x̄ = getx̄!(x̄, estim, x̂0arr) Ŵ = getŴ!(Ŵ, estim, transcription, Z̃) V̂, X̂0, Â0 = predict_mhe!( - V̂, X̂0, Â0, Û0, K̄, Ŷ0, estim, model, transcription, x̂0arr, â0arr, Ŵ, Z̃ + V̂, X̂0, Â0, Û0, Ûi, K̄, Ŷ0, estim, model, transcription, x̂0arr, â0arr, Ŵ, Z̃ ) Ŵe, V̂e, X̂e = extended_vectors!(Ŵe, V̂e, X̂e, estim, Ŵ, V̂, X̂0, x̂0arr) ε = getslack(estim, Z̃) gc = con_custom_mhe!(gc, estim, X̂e, V̂e, Ŵe, x̄, ε) g = con_nonlinprog_mhe!(g, estim, model, transcription, X̂0, V̂, gc, ε) geq = con_nonlinprogeq_mhe!( - geq, X̂0, Û0, K̄, estim, model, transcription, x̂0arr, â0arr, Ŵ, Z̃ + geq, X̂0, Û0, Ûi, K̄, estim, model, transcription, x̂0arr, â0arr, Ŵ, Z̃ ) return nothing end diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 4102bff67..ee5f8b285 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1155,7 +1155,7 @@ function set_warmstart!( ) where NT<:Real model, buffer = estim.model, estim.buffer nk̄ = get_nk̄(estim.model, transcription) - nε, nx̂, nŵ, He, Nk = estim.nε, estim.nx̂, estim.nx̂, estim.He, estim.Nk[] + nu, nε, nx̂, nŵ, He, Nk = model.nu, estim.nε, estim.nx̂, estim.nx̂, estim.He, estim.Nk[] nx̃, nŴ = nε + nx̂, nŵ*He Z̃s = estim.buffer.Z̃ # --- slack variable ε --- @@ -1168,12 +1168,12 @@ function set_warmstart!( # --- verify definiteness of objective function --- x̄, â0arr = buffer.x̂, buffer.â V̂, Ŵ, X̂0, Â0, Û0, Ŷ0 = buffer.V̂, buffer.Ŵ, buffer.X̂, buffer.Â, buffer.U, buffer.Ŷ - K = Vector{NT}(undef, nk̄*Nk) # TODO: remove the allocation + Ûi, K̄ = Vector{NT}(undef, nu*Nk), Vector{NT}(undef, nk̄*Nk) # TODO: remove allocations x̂0arr = estim.x̂0arr_old x̄ .= 0 # x̂0arr == x̂arr_old implies the error at arrival x̄ is zero getŴ!(Ŵ, estim, transcription, Z̃s) predict_mhe!( - V̂, X̂0, Â0, Û0, K, Ŷ0, estim, model, estim.transcription, x̂0arr, â0arr, Ŵ, Z̃s + V̂, X̂0, Â0, Û0, Ûi, K̄, Ŷ0, estim, model, estim.transcription, x̂0arr, â0arr, Ŵ, Z̃s ) Js = obj_nonlinprog(estim, model, x̄, V̂, Ŵ, Z̃s) if !isfinite(Js) @@ -1257,7 +1257,7 @@ function set_warmstart!( model, buffer = estim.model, estim.buffer na = get_na(model) nk̄, nā = get_nk̄(estim.model, transcription), get_nā(estim.model, transcription) - nε, nx̂, nŵ, He, Nk = estim.nε, estim.nx̂, estim.nx̂, estim.He, estim.Nk[] + nu, nε, nx̂, nŵ, He, Nk = model.nu, estim.nε, estim.nx̂, estim.nx̂, estim.He, estim.Nk[] nx̃, nŴ, nX̂, nÂ, nK̄, nĀ = nε + nx̂, nŵ*He, nx̂*He, na*He, nk̄*He, nā*He Z̃s = estim.buffer.Z̃ # --- slack variable ε --- @@ -1288,12 +1288,14 @@ function set_warmstart!( # --- verify definiteness of objective function --- x̄, â0arr = buffer.x̂, buffer.â V̂, Ŵ, X̂0, Â0, Û0, Ŷ0 = buffer.V̂, buffer.Ŵ, buffer.X̂, buffer.Â, buffer.U, buffer.Ŷ - K̄ = Vector{NT}(undef, nk̄*Nk) # TODO: remove the allocation + Ûi, K̄ = Vector{NT}(undef, nu*Nk), Vector{NT}(undef, nk̄*Nk) # TODO: remove allocations x̂0arr = estim.x̂0arr_old â0arr = getâ0arr!(â0arr, estim, estim.transcription, Z̃s) x̄ .= 0 # x̂0arr == x̂arr_old implies the error at arrival x̄ is zero getŴ!(Ŵ, estim, transcription, Z̃s) - predict_mhe!(V̂, X̂0, Â0, Û0, K̄, Ŷ0, estim, model, estim.transcription, x̂0arr, â0arr, Ŵ, Z̃s) + predict_mhe!( + V̂, X̂0, Â0, Û0, Ûi, K̄, Ŷ0, estim, model, estim.transcription, x̂0arr, â0arr, Ŵ, Z̃s + ) Js = obj_nonlinprog(estim, model, x̄, V̂, Ŵ, Z̃s) if !isfinite(Js) Z̃s[nx̃+nX̂+nK̄+1:end] .= 0 # Ŵ = 0 @@ -1362,7 +1364,7 @@ function set_warmstart!( model, buffer = estim.model, estim.buffer na = get_na(model) nk̄, nā = get_nk̄(estim.model, transcription), get_nā(estim.model, transcription) - nε, nx̂, nŵ, He, Nk = estim.nε, estim.nx̂, estim.nx̂, estim.He, estim.Nk[] + nu, nε, nx̂, nŵ, He, Nk = model.nu, estim.nε, estim.nx̂, estim.nx̂, estim.He, estim.Nk[] nx̃, nŴ, nX̂, nÂ, nĀ = nε + nx̂, nŵ*He, nx̂*He, na*He, nā*He Z̃s = estim.buffer.Z̃ # --- slack variable ε --- @@ -1389,13 +1391,13 @@ function set_warmstart!( # --- verify definiteness of objective function --- x̄, â0arr = buffer.x̂, buffer.â V̂, Ŵ, X̂0, Â0, Û0, Ŷ0 = buffer.V̂, buffer.Ŵ, buffer.X̂, buffer.Â, buffer.U, buffer.Ŷ - K̄ = Vector{NT}(undef, nk̄*Nk) # TODO: remove the allocation + Ûi, K̄ = Vector{NT}(undef, nu*Nk), Vector{NT}(undef, nk̄*Nk) # TODO: remove allocations x̂0arr = estim.x̂0arr_old â0arr = getâ0arr!(â0arr, estim, transcription, Z̃s) x̄ .= 0 # x̂0arr == x̂arr_old implies the error at arrival x̄ is zero getŴ!(Ŵ, estim, transcription, Z̃s) predict_mhe!( - V̂, X̂0, Â0, Û0, K̄, Ŷ0, estim, model, estim.transcription, x̂0arr, â0arr, Ŵ, Z̃s + V̂, X̂0, Â0, Û0, Ûi, K̄, Ŷ0, estim, model, estim.transcription, x̂0arr, â0arr, Ŵ, Z̃s ) Js = obj_nonlinprog(estim, model, x̄, V̂, Ŵ, Z̃s) if !isfinite(Js) @@ -1447,7 +1449,7 @@ end @doc raw""" predict_mhe!( - V̂, X̂0, Â0, Û0, K̄, Ŷ0, + V̂, X̂0, Â0, Û0, Ûi, K̄, Ŷ0, estim::MovingHorizonEstimator, model::LinModel, transcription::TranscriptionMethod, x̂0arr, â0arr, Ŵ, Z̃ ) -> V̂, X̂0, Â0 @@ -1466,7 +1468,7 @@ noises from ``k-N_k+1`` to ``k``. The `X̂0` vector is estimated states from ``k The vector `Â0` is ignored and returned unchanged. """ function predict_mhe!( - V̂, X̂0, Â0, _ , _ , _ , + V̂, X̂0, Â0, _ , _ , _ , _ , estim::MovingHorizonEstimator, ::LinModel, ::TranscriptionMethod, _ , _ , _ , Z̃ ) @@ -1491,7 +1493,7 @@ end @doc raw""" predict_mhe!( - V̂, X̂0, Â0, Û0, K̄, Ŷ0, + V̂, X̂0, Â0, Û0, Ûi, K̄, Ŷ0, estim::MovingHorizonEstimator, model::NonLinModel, ::SingleShooting, x̂0arr, â0arr, Ŵ, Z̃ ) -> V̂, X̂0, Â0 @@ -1504,28 +1506,38 @@ called recursively in a `for` loop from ``j=1`` to ``N_k``, and by adding the es process noise ``\mathbf{ŵ}``. """ function predict_mhe!( - V̂, X̂0, Â0, Û0, K̄, Ŷ0, - estim::MovingHorizonEstimator, model::NonLinModel, ::SingleShooting, + V̂, X̂0, Â0, Û0, Ûi, K̄, Ŷ0, + estim::MovingHorizonEstimator, model::NonLinModel, transcription::SingleShooting, x̂0arr, _ , Ŵ, _ ) nu, nd, ny, nk̄ = model.nu, model.nd, model.ny, model.nk̄ nx̂, nŵ, nym, Nk = estim.nx̂, estim.nx̂, estim.nym, estim.Nk[] - p = estim.direct ? 0 : 1 + h = transcription.h + Di = estim.buffer.D + i_d0arr = estim.direct ? 0 : nd # the first nd elements in D0 are useless if p=1 x̂0 = @views x̂0arr[1:nx̂] for j=1:Nk u0 = @views estim.U0[(1+nu*(j-1)):(nu*j)] - d0 = @views estim.D0[(1+nd*(j+p-1)):(nd*(j+p))] + d0 = @views estim.D0[(1+nd*(j-1) + i_d0arr):(nd*j + i_d0arr)] ŵ = @views Ŵ[(1+nŵ*(j-1)):(nŵ*j)] k̄ = @views K̄[(1+nk̄*(j-1)):(nk̄*j)] - û0 = @views Û0[(1+nu*(j-1)):(nu*j)] - x̂0next = @views X̂0[(1+nx̂*(j-1)):(nx̂*j)] - f̂!(x̂0next, û0, k̄, estim, model, x̂0, u0, d0) + û0 = @views Û0[(1+nu*(j-1)):(nu*j)] + ûi = @views Ûi[(1 + nu*(j-1)):(nu*j)] + di = @views Di[(1 + nd*(j-1)):(nd*j)] + x̂0next = @views X̂0[(1+nx̂*(j-1)):(nx̂*j)] + d0next = @views (j < Nk) ? estim.D0[(1+nd*j + i_d0arr):(nd*(j+1) + i_d0arr)] : d0 + if h > 0 && j < Nk + u0next = estim.U0[(1+nu*j):(nu*(j+1))] + else + u0next = u0 + end + û0next = @views j < Nk ? Û0[(1+nu*j):(nu*(j+1))] : Û0[1:nu] + f̂!(x̂0next, û0, û0next, k̄, ûi, di, estim, model, x̂0, u0, d0, u0next, d0next) x̂0next .+= ŵ if estim.direct ŷ0next = @views Ŷ0[(1 + ny*(j-1)):(ny*j)] y0nextm = @views estim.Y0m[(1 + nym*(j-1)):(nym*j)] v̂next = @views V̂[(1 + nym*(j-1)):(nym*j)] - d0next = @views estim.D0[(1 + nd*j):(nd*(j+1))] ĥ!(ŷ0next, estim, model, x̂0next, d0next) ŷ0nextm = @views ŷ0next[estim.i_ym] if any(isnan, y0nextm) # nan in Y0m: y0m=ŷ0m => associated v̂ value = 0 @@ -1554,7 +1566,7 @@ end @doc raw""" predict_mhe!( - V̂, X̂0, Â0, _ , _ , Ŷ0, + V̂, X̂0, Â0, _ , _ , _ , Ŷ0, estim::MovingHorizonEstimator, model::SimModel, ::TranscriptionMethod, x̂0arr, â0arr, _ , Z̃ ) -> V̂, X̂0, Â0 @@ -1565,7 +1577,7 @@ The function mutates `V̂`, `X̂0`, `Â0`, and `Ŷ0` vector arguments. The aug function [`ĥ!`](@ref) is called multiple times in a `for` loop from ``j=1`` to ``N_k``. """ function predict_mhe!( - V̂, X̂0, Â0, _ , _ , Ŷ0, + V̂, X̂0, Â0, _ , _ , _ , Ŷ0, estim::MovingHorizonEstimator, model::SimModel, transcription::TranscriptionMethod, x̂0arr, â0arr, _ , Z̃ ) @@ -1696,15 +1708,15 @@ end @doc raw""" con_nonlinprogeq_mhe!( - geq, X̂0, Û0, K̄, + geq, X̂0, Û0, Ûi, K̄, estim::MovingHorizonEstimator, model::NonLinModel, ::MultipleShooting, x̂0arr, _ , Ŵ, Z̃ ) -> geq Nonlinear MHE equality constrains for [`NonLinModel`](@ref) and [`MultipleShooting`](@ref). -The method mutates the `geq`, `X̂0`, `Û0` and `K̄` vectors in argument. The defects of the -stochastic states are linear equality constraints (see [`init_defectmat_mhe`](@ref)). By +The method mutates the `geq`, `X̂0`, `Û0`, `Ûi` and `K̄` vectors in argument. The defects of +the stochastic states are linear equality constraints (see [`init_defectmat_mhe`](@ref)). By introducing the integer ``ℓ = k - N_k + p`` to shorten the notation, the defects of the estimated states are computed with: ```math @@ -1718,7 +1730,7 @@ state update function [`f!`](@ref). The disturbed input ``\mathbf{û_0}`` is def [`f̂!`](@ref) documentation. """ function con_nonlinprogeq_mhe!( - geq, X̂0, Û0, K̄, + geq, X̂0, Û0, Ûi, K̄, estim::MovingHorizonEstimator, model::NonLinModel, transcription::MultipleShooting, x̂0arr, _ , Ŵ, Z̃ ) @@ -1730,20 +1742,25 @@ function con_nonlinprogeq_mhe!( i_d0arr = estim.direct ? 0 : nd # the first nd elements in D0 are useless if p=1 X̂0_Z̃ = @views Z̃[(nx̃+1):(nx̃+nx̂*He)] Û0 = disturbedinput!(Û0, estim, x̂0arr, X̂0_Z̃, estim.U0) + Di = estim.buffer.D @threadsif f_threads for j=1:Nk if j < 2 x̂d_Z̃ = @views x̂0arr[1:nx] else x̂d_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-2)):(nx̂*(j-2) + nx)] end - d0 = @views estim.D0[(1 + nd*(j-1) + i_d0arr):(nd*j + i_d0arr)] - k̄ = @views K̄[(1 + nk̄*(j-1)):(nk̄*j)] - û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] - ŵd = @views Ŵ[(1 + nŵ*(j-1)):(nŵ*(j-1) + nw)] - x̂dnext = @views X̂0[(1 + nx̂*(j-1)):(nx̂*(j-1) + nx)] - x̂dnext_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*(j-1) + nx)] - ŝdnext = @views geq[(1 + nx*(j-1)):(nx*j)] - f!(x̂dnext, k̄, model, x̂d_Z̃, û0, d0, model.p) + d0 = @views estim.D0[(1 + nd*(j-1) + i_d0arr):(nd*j + i_d0arr)] + k̄ = @views K̄[(1 + nk̄*(j-1)):(nk̄*j)] + û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] + ûi = @views Ûi[(1 + nu*(j-1)):(nu*j)] + di = @views Di[(1 + nd*(j-1)):(nd*j)] + ŵd = @views Ŵ[(1 + nŵ*(j-1)):(nŵ*(j-1) + nw)] + x̂dnext = @views X̂0[(1 + nx̂*(j-1)):(nx̂*(j-1) + nx)] + x̂dnext_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*(j-1) + nx)] + û0next = @views j < Nk ? Û0[(1 + nu*j):(nu*(j+1))] : û0 + d0next = @views (j < Nk) ? estim.D0[(1+nd*j + i_d0arr):(nd*(j+1) + i_d0arr)] : d0 + ŝdnext = @views geq[(1 + nx*(j-1)):(nx*j)] + f!(x̂dnext, k̄, ûi, di, model, x̂d_Z̃, û0, d0, û0next, d0next) x̂dnext .+= ŵd ŝdnext .= @. x̂dnext - x̂dnext_Z̃ end @@ -1753,7 +1770,7 @@ end @doc raw""" con_nonlinprogeq_mhe!( - geq, _ , Û0, K̄dot, + geq, _ , Û0, _ , K̄dot, estim::MovingHorizonEstimator, model::SimModel, ::TrapezoidalCollocation, x̂0arr, â0arr, Ŵ, Z̃ ) -> geq @@ -1791,7 +1808,7 @@ plus one last residual for the arrival: ``` """ function con_nonlinprogeq_mhe!( - geq, _ , Û0, K̄dot, + geq, _ , Û0, _ , K̄dot, estim::MovingHorizonEstimator, model::SimModel, transcription::TrapezoidalCollocation, x̂0arr, â0arr, Ŵ, Z̃ ) @@ -1860,7 +1877,7 @@ end @doc raw""" con_nonlinprogeq_mhe!( - geq, _ , Û0, K̄dot, + geq, _ , Û0, Ûi, K̄dot, estim::MovingHorizonEstimator, model::SimModel, ::OrthogonalCollocation, x̂0arr, â0arr, _ , Z̃ ) -> geq @@ -1898,7 +1915,7 @@ stochastic states are linear equality constraints (see [`init_defectmat_mhe`](@r estimated process noise ``\mathbf{ŵ}(ℓ+j)`` are incorporated in the continuity constraint. """ function con_nonlinprogeq_mhe!( - geq, _ , Û0, K̄dot, + geq, _ , Û0, Ûi, K̄dot, estim::MovingHorizonEstimator, model::SimModel, transcription::OrthogonalCollocation, x̂0arr, â0arr, _ , Z̃ ) @@ -1917,8 +1934,7 @@ function con_nonlinprogeq_mhe!( Â0 = @views Z̃[(1 + nx̃_nX̂ + na):(nx̃_nX̂ + na + nÂ)] # skipping â0arr components K̄ = @views Z̃[(1 + nx̃_nX̂ + na + nÂ):(nx̃_nX̂ + na + n + nK̄)] Ā = @views Z̃[(1 + nx̃_nX̂ + na + n + nK̄):(nx̃_nX̂ + na + n + nK̄ + nĀ)] - Dtemp = estim.buffer.D - Utemp = similar(Û0) # TODO: remove this allocation + Di = estim.buffer.D Û0 = disturbedinput!(Û0, estim, x̂0arr, X̂0, estim.U0) (Nk < He) && (geq .= 0) Ŝk̄ = @views geq[1:nŜk̄] @@ -1958,7 +1974,7 @@ function con_nonlinprogeq_mhe!( Δk[(1 + (i-1)*nx):(i*nx)] = @views k̄[(1 + (i-1)*nx):(i*nx)] .- x̂d end mul!(ŝk̄, Mo, Δk) - di, ûi = @views Dtemp[(1 + nd*(j-1)):(nd*j)], Utemp[(1 + nu*(j-1)):(nu*j)] + di, ûi = @views Di[(1 + nd*(j-1)):(nd*j)], Ûi[(1 + nu*(j-1)):(nu*j)] for i=1:no k̇i, ki = @views k̄dot[(1 + (i-1)*nx):(i*nx)], k̄[(1 + (i-1)*nx):(i*nx)] qi, ai = @views q̄[(1 + (i-1)*na):(i*na)], ā[(1 + (i-1)*na):(i*na)] @@ -1979,4 +1995,4 @@ function con_nonlinprogeq_mhe!( end "No nonlinear eq. const. for other cases e.g. [`SingleShooting`](@ref), returns `geq` unchanged." -con_nonlinprogeq_mhe!(geq,_,_,_,::MovingHorizonEstimator, ::SimModel, ::TranscriptionMethod,_,_,_,_) = geq \ No newline at end of file +con_nonlinprogeq_mhe!(geq,_,_,_,_,::MovingHorizonEstimator, ::SimModel, ::TranscriptionMethod,_,_,_,_) = geq \ No newline at end of file diff --git a/src/model/linearization.jl b/src/model/linearization.jl index cbc7e4a1e..1b95b6580 100644 --- a/src/model/linearization.jl +++ b/src/model/linearization.jl @@ -16,9 +16,15 @@ is an `AbstractADType` object from `DifferentiationInterface`. The `cst_x`, `cst function get_linearization_func( NT, f!::F, h!::H, Ts, nu, nx, ny, nd, p, solver, backend ) where {F<:Function, H<:Function} - f_x!(xnext, x, k, u, d) = solver_f!(xnext, k, f!, Ts, solver, x, u, d, p) - f_u!(xnext, u, k, x, d) = solver_f!(xnext, k, f!, Ts, solver, x, u, d, p) - f_d!(xnext, d, k, x, u) = solver_f!(xnext, k, f!, Ts, solver, x, u, d, p) + function f_x!(xnext, x, u, d, k̄, ui, di) + return solver_f!(xnext, k̄, ui, di, f!, Ts, solver, x, u, d, u, d, p) # ZOH only + end + function f_u!(xnext, u, x, d, k̄, ui, di) + return solver_f!(xnext, k̄, ui, di, f!, Ts, solver, x, u, d, u, d, p) # ZOH only + end + function f_d!(xnext, d, x, u, k̄, ui, di) + return solver_f!(xnext, k̄, ui, di, f!, Ts, solver, x, u, d, u, d, p) # ZOH only + end h_x!(y, x, d) = h!(y, x, d, p) h_d!(y, d, x) = h!(y, x, d, p) strict = Val(true) @@ -27,23 +33,24 @@ function get_linearization_func( x = zeros(NT, nx) u = zeros(NT, nu) d = zeros(NT, nd) - k = zeros(NT, nx*(solver.ni+1)) - cache_k = Cache(k) + k̄ = zeros(NT, nx*(solver.ni+1)) + ui, di = zeros(NT, nu), zeros(NT, nd) + f_cache = (Cache(k̄), Cache(ui), Cache(di)) cst_x = Constant(rand(NT, nx)) cst_u = Constant(rand(NT, nu)) cst_d = Constant(rand(NT, nd)) - A_prep = prepare_jacobian(f_x!, xnext, backend, x, cache_k, cst_u, cst_d; strict) - Bu_prep = prepare_jacobian(f_u!, xnext, backend, u, cache_k, cst_x, cst_d; strict) - Bd_prep = prepare_jacobian(f_d!, xnext, backend, d, cache_k, cst_x, cst_u; strict) + A_prep = prepare_jacobian(f_x!, xnext, backend, x, cst_u, cst_d, f_cache...; strict) + Bu_prep = prepare_jacobian(f_u!, xnext, backend, u, cst_x, cst_d, f_cache...; strict) + Bd_prep = prepare_jacobian(f_d!, xnext, backend, d, cst_x, cst_u, f_cache...; strict) C_prep = prepare_jacobian(h_x!, y, backend, x, cst_d ; strict) Dd_prep = prepare_jacobian(h_d!, y, backend, d, cst_x ; strict) function linfunc!(xnext, y, A, Bu, C, Bd, Dd, backend, x, u, d, cst_x, cst_u, cst_d) # all the arguments before `backend` are mutated in this function - jacobian!(f_x!, xnext, A, A_prep, backend, x, cache_k, cst_u, cst_d) - jacobian!(f_u!, xnext, Bu, Bu_prep, backend, u, cache_k, cst_x, cst_d) + jacobian!(f_x!, xnext, A, A_prep, backend, x, cst_u, cst_d, f_cache...) + jacobian!(f_u!, xnext, Bu, Bu_prep, backend, u, cst_x, cst_d, f_cache...) jacobian!(h_x!, y, C, C_prep, backend, x, cst_d) if nd > 0 - jacobian!(f_d!, xnext, Bd, Bd_prep, backend, d, cache_k, cst_x, cst_u) + jacobian!(f_d!, xnext, Bd, Bd_prep, backend, d, cst_x, cst_u, f_cache...) jacobian!(h_d!, y, Dd, Dd_prep, backend, d, cst_x) end return nothing @@ -163,7 +170,7 @@ function linearize!( nonlinmodel = model buffer = nonlinmodel.buffer # --- remove the operating points of the nonlinear model (typically zeros) --- - x0, u0, d0, k̄ = buffer.x, buffer.u, buffer.d, buffer.k̄ + x0, u0, d0, k̄, ui, di = buffer.x, buffer.u, buffer.d, buffer.k̄, buffer.ui, buffer.di x0 .= x .- nonlinmodel.xop u0 .= u .- nonlinmodel.uop d0 .= d .- nonlinmodel.dop @@ -171,11 +178,11 @@ function linearize!( linearize_core!(linmodel, nonlinmodel, x0, u0, d0) # --- compute the nonlinear model output at operating points --- x0next, y0 = linmodel.buffer.x, linmodel.buffer.y - h!(y0, nonlinmodel, x0, d0, model.p) + h!(y0, nonlinmodel, x0, d0) y0 .+= nonlinmodel.yop y = y0 # --- compute the nonlinear model next state at operating points --- - f!(x0next, k̄, nonlinmodel, x0, u0, d0, model.p) + f!(x0next, k̄, ui, di, nonlinmodel, x0, u0, d0, u0, d0) # ZOH only x0next .+= nonlinmodel.fop xnext = x0next # xnext = f(x0,u0,d0) + fop - xop + xop = f(x0,u0,d0) + fop # --- recompute x since it was modified in buffer.x --- diff --git a/src/model/linmodel.jl b/src/model/linmodel.jl index 8fff8feb6..c1991c0db 100644 --- a/src/model/linmodel.jl +++ b/src/model/linmodel.jl @@ -290,11 +290,11 @@ function initstate_core!(model::LinModel, u0, d0) end """ - f!(x0next, _ , model::LinModel, x0, u0, d0, _ ) -> nothing + f!(x0next, _ , _ , _ , model::LinModel, x0, u0, d0 , _ , _ ) -> nothing Evaluate `x0next = A*x0 + Bu*u0 + Bd*d0` in-place when `model` is a [`LinModel`](@ref). """ -function f!(x0next, _ , model::LinModel, x0, u0, d0, _ ) +function f!(x0next, _ , _ , _ , model::LinModel, x0, u0, d0 , _ , _ ) mul!(x0next, model.A, x0) mul!(x0next, model.Bu, u0, 1, 1) mul!(x0next, model.Bd, d0, 1, 1) @@ -303,11 +303,11 @@ end """ - h!(y0, model::LinModel, x0, d0, _ ) -> nothing + h!(y0, model::LinModel, x0, d0) -> nothing Evaluate `y0 = C*x0 + Dd*d0` in-place when `model` is a [`LinModel`](@ref). """ -function h!(y0, model::LinModel, x0, d0, _ ) +function h!(y0, model::LinModel, x0, d0) mul!(y0, model.C, x0) mul!(y0, model.Dd, d0, 1, 1) return nothing diff --git a/src/model/nonlinmodel.jl b/src/model/nonlinmodel.jl index 035b2f2af..4a17d7b34 100644 --- a/src/model/nonlinmodel.jl +++ b/src/model/nonlinmodel.jl @@ -8,7 +8,9 @@ struct EmptySolver <: DiffSolver end "Call `f!` function directly for discrete-time models (no solver)." -function solver_f!(xnext, _ , f!::F, _ , ::EmptySolver, x, u, d, p) where F +function solver_f!( + xnext, _ , _ , _ , f!::F, _ , ::EmptySolver, x, u, d, _ , _ , p +) where F<:Function return f!(xnext, x, u, d, p) end @@ -315,23 +317,27 @@ LinModel(model::NonLinModel; kwargs...) = linearize(model; kwargs...) """ - f!(x0next, k̄, model::NonLinModel, x0, u0, d0, p) + f!(x0next, k̄, ui, di, model::NonLinModel, x0, u0, d0, u0next, d0next) Compute `x0next` using the [`DiffSolver`](@ref) in `model.solver` and `model.f!`. -The method mutates `x0next` and `k̄` arguments in-place. The latter is used to store the -intermediate stage values of the solver. +The method mutates `x0next`, `k̄`, `ui` and `di` arguments in-place. The `k̄` argument is used +to store the intermediate stage values of the solver. The `ui` and `di` arguments are used +to stored the interpolated inputs and measured disturbances, respectively (needed for FOH +discretization). The model parameter `model.p` is not included in the function signature for +conciseness. """ -function f!(x0next, k̄, model::NonLinModel, x0, u0, d0, p) - return solver_f!(x0next, k̄, model.f!, model.Ts, model.solver, x0, u0, d0, p) +function f!(x0next, k̄, ui, di, model::NonLinModel, x0, u0, d0, u0next, d0next) + Ts, solver, p = model.Ts, model.solver, model.p + return solver_f!(x0next, k̄, ui, di, model.f!, Ts, solver, x0, u0, d0, u0next, d0next, p) end """ - h!(y0, model::NonLinModel, x0, d0, p) + h!(y0, model::NonLinModel, x0, d0) Compute `y0` by calling `model.h!` directly for [`NonLinModel`](@ref). """ -h!(y0, model::NonLinModel, x0, d0, p) = model.h!(y0, x0, d0, p) +h!(y0, model::NonLinModel, x0, d0) = model.h!(y0, x0, d0, model.p) include("solver.jl") diff --git a/src/model/nonlinmodeldae.jl b/src/model/nonlinmodeldae.jl index b2aca834f..51f7dffdf 100644 --- a/src/model/nonlinmodeldae.jl +++ b/src/model/nonlinmodeldae.jl @@ -57,6 +57,8 @@ struct NonLinModelDAE{ x0_optim::Vector{NT} u0_optim::Vector{NT} d0_optim::Vector{NT} + u0next_optim::Vector{NT} + d0next_optim::Vector{NT} iszero_Ha::Bool buffer::SimModelBuffer{NT} function NonLinModelDAE{NT}( @@ -93,9 +95,6 @@ struct NonLinModelDAE{ 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 (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) nZ = get_nZ_dae(transcription, nx, na) @@ -105,6 +104,7 @@ struct NonLinModelDAE{ beq = zeros(NT, size(Aeq, 1)) neq = nZ - size(Aeq, 1) # number of nonlinear equality constraints x0_optim, u0_optim, d0_optim = zeros(NT, nx), zeros(NT, nu), zeros(NT, nd) + u0next_optim, d0next_optim = zeros(NT, nu), zeros(NT, nd) # force computation of derivatives for the first NLP iteration: force∇q, force∇geq = [true], [true] buffer = SimModelBuffer{NT}(nu, nx, ny, nd, 0, na) @@ -126,7 +126,7 @@ struct NonLinModelDAE{ uname, yname, dname, xname, unit, xs_0, as_0, - x0_optim, u0_optim, d0_optim, + x0_optim, u0_optim, d0_optim, u0next_optim, d0next_optim, iszero_Ha, buffer ) @@ -686,47 +686,61 @@ Update `k̄` and `geq` in place for the open loop root solving of [`NonLinModelD """ function update_predictions!(k̄, geq, model, Z) x0, u0, d0 = model.x0_optim, model.u0_optim, model.d0_optim - con_nonlinprogeq!(geq, k̄, model, model.transcription, x0, u0, d0, Z) + u0next, d0next = model.u0next_optim, model.d0next_optim + con_nonlinroots!(geq, k̄, model, model.transcription, x0, u0, d0, u0next, d0next, Z) return nothing end """ - con_nonlinprogeq!( - geq, k̄dot, model::NonLinModelDAE, ::TrapezoidalCollocation, x0, u0, d0, Z + con_nonlinroots!( + geq, k̄dot, + model::NonLinModelDAE, ::TrapezoidalCollocation, x0, u0, d0, u0next, d0next, Z ) -> geq Nonlinear equality constraint for open loop root solving of [`NonLinModelDAE`](@ref). """ -function con_nonlinprogeq!( - geq, k̄dot, model::NonLinModelDAE, ::TrapezoidalCollocation, x0, u0, d0, Z +function con_nonlinroots!( + geq, k̄dot, + model::NonLinModelDAE, ::TrapezoidalCollocation, x0, u0, d0, u0next, d0next, Z ) nx, na = model.nx, model.na - Ts = model.Ts + Ts = model.Ts + u⁺, d⁺ = u0, d0 + # h is ignored here, u0next & d0next values determine the hold order (like NonLinModel): + u⁻next, d⁻next = u0next, d0next x0next, a⁺, a0next = @views Z[1:nx], Z[(nx+1):(nx+na)], Z[(nx+na+1):(nx+2na)] sknext, q⁺, q⁻next = @views geq[1:nx], geq[(nx+1):(nx+na)], geq[(nx+na+1):(nx+2na)] k̇⁺, k̇⁻next = @views k̄dot[1:nx], k̄dot[(nx+1):(2nx)] - model.fq!(k̇⁺ , q⁺ , x0 , a⁺ , u0, d0, model.p) - model.fq!(k̇⁻next, q⁻next, x0next, a0next, u0, d0, model.p) + model.fq!(k̇⁺ , q⁺ , x0 , a⁺ , u⁺ , d⁺ , model.p) + model.fq!(k̇⁻next, q⁻next, x0next, a0next, u⁻next, d⁻next, model.p) sknext .= @. x0 + 0.5*Ts*(k̇⁺ + k̇⁻next) - x0next return geq end """ - con_nonlinprogeq!( - geq, k̄dot, model::NonLinModelDAE, transcription::OrthogonalCollocation, x0, u0, d0, Z + con_nonlinroots!( + geq, k̄dot + model::NonLinModelDAE, ::OrthogonalCollocation, x0, u0, d0, u0next, d0next, Z ) -> geq Same but using an [`OrthogonalCollocation`](@ref) transcription. """ -function con_nonlinprogeq!( - geq, k̄dot, model::NonLinModelDAE, transcription::OrthogonalCollocation, x0, u0, d0, Z +function con_nonlinroots!( + geq, k̄dot, + model::NonLinModelDAE, ::OrthogonalCollocation, x0, u0, d0, u0next, d0next, Z ) + transcription = model.transcription nx, na = model.nx, model.na - Mo, no = model.Mo, transcription.no + Mo = model.Mo + no, τ = transcription.no, transcription.τ + ui, di = model.buffer.ui, model.buffer.di + u⁺, d⁺ = u0, d0 + # h is ignored here, u0next & d0next values determine the hold order (like NonLinModel): + u⁻next, d⁻next = u0next, d0next nk̄, nā = get_nk̄(model, transcription), get_nā(model, transcription) a⁺, k̄, ā = @views Z[(nx+1):(nx+na)], Z[(nx+na+1):(nx+na+nk̄)], Z[(nx+na+nk̄+1):end] - q⁺, sk̄, q̄ = @views geq[1:na], geq[(na+1):(na+nk̄)], geq[(na+nk̄+1):(na+nk̄+nā)] - @views model.fq!(k̄dot[1:nx], q⁺, x0, a⁺, u0, d0, model.p) + q⁺, sk̄, q̄ = @views geq[1:na], geq[(na+1):(na+nk̄)], geq[(na+nk̄+1):(na+nk̄+nā)] + @views model.fq!(k̄dot[1:nx], q⁺, x0, a⁺, u⁺, d⁺, model.p) Δk = k̄dot for i=1:no Δk[(1 + (i-1)*nx):(i*nx)] = @views k̄[(1 + (i-1)*nx):(i*nx)] .- x0 @@ -735,7 +749,9 @@ function con_nonlinprogeq!( for i=1:no k̇i, ki = @views k̄dot[(1 + (i-1)*nx):(i*nx)], k̄[(1 + (i-1)*nx):(i*nx)] qi, ai = @views q̄[(1 + (i-1)*na):(i*na)], ā[(1 + (i-1)*na):(i*na)] - model.fq!(k̇i, qi, ki, ai, u0, d0, model.p) + ui .= (1-τ[i]).*u⁺ .+ τ[i].*u⁻next + di .= (1-τ[i]).*d⁺ .+ τ[i].*d⁻next + model.fq!(k̇i, qi, ki, ai, ui, di, model.p) end sk̄ .-= k̄dot return geq @@ -757,11 +773,13 @@ function initstate_core!(model::NonLinModelDAE, u0, d0) model.x0_optim .= model.x0 model.u0_optim .= u0 model.d0_optim .= d0 + model.u0next_optim .= u0 + model.d0next_optim .= d0 return nothing end @doc raw""" - f!(x0next, _ , model::NonLinModelDAE, x0, u0, d0, _ ) -> nothing + f!(x0next, _ , _ , _ , model::NonLinModelDAE, x0, u0, d0, u0next, d0next) -> nothing Solve the optimization `model.optim` problem for [`NonLinModelDAE`](@ref). @@ -769,11 +787,13 @@ After solving, the next state ``\mathbf{x_0}(k+1)`` will be stored in-place in t argument. The right limit of the current algebraic variable ``\mathbf{a^+}(k+0)`` will be also stored at `model.a0`. """ -function f!(x0next, _ , model::NonLinModelDAE, x0, u0, d0, _ ) +function f!(x0next, _ , _ , _ , model::NonLinModelDAE, x0, u0, d0, u0next, d0next) nx, na = model.nx, model.na model.x0_optim .= x0 model.u0_optim .= u0 model.d0_optim .= d0 + model.u0next_optim .= u0next + model.d0next_optim .= d0next linconstrainteq!(model, model.transcription) Zvar = model.optim_state[:Zvar] Z = solve!(model, model.optim_state, Zvar, model.Z) @@ -784,7 +804,7 @@ function f!(x0next, _ , model::NonLinModelDAE, x0, u0, d0, _ ) end """ - h!(y0, model::NonLinModelDAE, x0, d0, p) -> nothing + h!(y0, model::NonLinModelDAE, x0, d0) -> nothing Solve the algebraic equation to get `a0` and call `model.h!` for [`NonLinModelDAE`](@ref). @@ -792,7 +812,7 @@ If `model.iszero_Ha` is `true`, the algebraic variable is not used in `model.h!` to [`SparseConnectivityTracer.jl`](@extref SparseConnectivityTracer.jl), the algebraic equation solving is thus skipped and `model.h!` is called directly. """ -function h!(y0, model::NonLinModelDAE, x0, d0, p) +function h!(y0, model::NonLinModelDAE, x0, d0) if !model.iszero_Ha model.x0_optim .= x0 model.d0_optim .= d0 @@ -804,7 +824,7 @@ function h!(y0, model::NonLinModelDAE, x0, d0, p) else # model.h! is not a function of a0, this vector is not needed here: a0 = model.buffer.a end - model.h!(y0, x0, a0, d0, p) + model.h!(y0, x0, a0, d0, model.p) return nothing end @@ -827,7 +847,7 @@ Call `model.h!` with the `a0` argument if [`NonLinModelDAE`](@ref), else without The `a0` argument is ignored for [`SimModelODE`](@ref)s. See also [`fq_dae!`](@ref). """ h_dae!(y0, model::NonLinModelDAE, x0, a0, d0) = model.h!(y0, x0, a0, d0, model.p) -h_dae!(y0, model::SimModelODE, x0, _ , d0) = h!(y0, model, x0, d0, model.p) +h_dae!(y0, model::SimModelODE, x0, _ , d0) = h!(y0, model, x0, d0) function linconstrainteq!(model::NonLinModelDAE, ::OrthogonalCollocation) mul!(model.Fs, model.Ks, model.x0_optim) @@ -884,12 +904,14 @@ The function should be called after calling [`updatestate!`](@ref) on `model` ob returns the dictionary `info` with the following fields: - `:xnext` : next state, ``\mathbf{x}(k+1)`` +- `:unext` : left limit of the next manipulated input, ``\mathbf{u^-}(k+1)`` +- `:dnext` : left limit of the next measured disturbance, ``\mathbf{d^-}(k+1)`` +- `:x` : current state, ``\mathbf{x}(k)`` - `:a` : right limit of current algebraic variable, ``\mathbf{a^+}(k)`` -- `:q` : current algebraic equation residuals `res`, ``\mathbf{q(x, a^+, u, d, p)}`` +- `:u` : right limit of current manipulated input, ``\mathbf{u^+}(k)`` +- `:d` : right limit of current measured disturbances, ``\mathbf{d^+}(k)`` +- `:q` : current algebraic equation residuals `res`, ``\mathbf{q(x, a^+, u^+, d^+, p)}`` - `:y` : current output, ``\mathbf{y}(k)`` -- `:x` : current state, ``\mathbf{x}(k)`` -- `:u` : current manipulated input, ``\mathbf{u}(k)`` -- `:d` : current measured disturbances, ``\mathbf{u}(k)`` The following two fields are also available if the related method is called at least once: @@ -913,26 +935,34 @@ julia> a = round.(getinfo(model)[:a], digits=6) """ function getinfo(model::NonLinModelDAE{NT}) where NT<:Real x0, u0, d0 = model.x0_optim, model.u0_optim, model.d0_optim + u0next, d0next = model.u0next_optim, model.d0next_optim a0, p = model.a0, model.p buffer = model.buffer - ẋ, q, y0 = buffer.x, buffer.a, buffer.y + k̄, q, y0 = buffer.k̄, buffer.a, buffer.y + ẋ = @views k̄[1:model.nx] model.fq!(ẋ, q, x0, a0, u0, d0, p) model.h!(y0, x0, a0, d0, p) y = y0 y .+ model.yop - x, u, d = buffer.x, buffer.u, buffer.d + x = Vector{NT}(undef, model.nx) + u, d = buffer.u, buffer.d + unext, dnext = buffer.unext, buffer.dnext x .= x0 .+ model.xop u .= u0 .+ model.uop d .= d0 .+ model.dop a = a0 + unext .= u0next .+ model.uop + dnext .= d0next .+ model.dop info = Dict{Symbol, Any}() info[:xnext] = model.x0 + model.xop - info[:q] = q - info[:y] = y + info[:unext] = unext + info[:dnext] = dnext info[:x] = x info[:a] = a info[:u] = u info[:d] = d + info[:q] = q + info[:y] = y if JuMP.termination_status(model.optim_state) ≠ JuMP.OPTIMIZE_NOT_CALLED info[:sol_state] = JuMP.solution_summary(model.optim_state, verbose=true) end diff --git a/src/model/solver.jl b/src/model/solver.jl index c42577c2f..bb5b3b8f7 100644 --- a/src/model/solver.jl +++ b/src/model/solver.jl @@ -28,24 +28,37 @@ This solver is allocation-free if the `f!` and `h!` functions do not allocate. RungeKutta(order::Int=4; supersample::Int=1) = RungeKutta(order, supersample) "Solve the differential equation with the 4th order Runge-Kutta method." -function solver_f!(xnext, k̄, f!::F, Ts, solver::RungeKutta{4}, x, u, d, p) where F +function solver_f!( + xnext, k̄, ui, di, f!::F, Ts, solver::RungeKutta{4}, x, u, d, unext, dnext, p +) where F<:Function supersample = solver.supersample Ts_inner = Ts/supersample + isFOH_u = isdifferent(u, unext) + isFOH_d = isdifferent(d, dnext) nx = length(x) xcurr = @views k̄[1:nx] k1 = @views k̄[(1nx + 1):(2nx)] k2 = @views k̄[(2nx + 1):(3nx)] k3 = @views k̄[(3nx + 1):(4nx)] - k4 = @views k̄[(4nx + 1):(5nx)] + k4 = @views k̄[(4nx + 1):(5nx)] @. xcurr = x for i=1:supersample - f!(k1, xcurr, u, d, p) + τ = (i-1.0)/supersample + u1 = isFOH_u ? @.(ui = (1-τ)*u + (τ)*unext) : u + d1 = isFOH_d ? @.(di = (1-τ)*d + (τ)*dnext) : d + f!(k1, xcurr, u1, d1, p) @. xnext = xcurr + k1 * Ts_inner/2 - f!(k2, xnext, u, d, p) + τ = (i-0.5)/supersample + u2and3 = isFOH_u ? @.(ui = (1-τ)*u + (τ)*unext) : u + d2and3 = isFOH_d ? @.(di = (1-τ)*d + (τ)*dnext) : d + f!(k2, xnext, u2and3, d2and3, p) @. xnext = xcurr + k2 * Ts_inner/2 - f!(k3, xnext, u, d, p) + f!(k3, xnext, u2and3, d2and3, p) @. xnext = xcurr + k3 * Ts_inner - f!(k4, xnext, u, d, p) + τ = i/supersample + u4 = isFOH_u ? @.(ui = (1-τ)*u + (τ)*unext) : u + d4 = isFOH_d ? @.(di = (1-τ)*d + (τ)*dnext) : d + f!(k4, xnext, u4, d4, p) @. xcurr = xcurr + (k1 + 2k2 + 2k3 + k4)*Ts_inner/6 end @. xnext = xcurr @@ -63,15 +76,22 @@ const ForwardEuler(;supersample=1) = RungeKutta(1; supersample) "Solve the differential equation with the forward Euler method." -function solver_f!(xnext, k̄, f!::F, Ts, solver::RungeKutta{1}, x, u, d, p) where F +function solver_f!( + xnext, k̄, ui, di, f!::F, Ts, solver::RungeKutta{1}, x, u, d, unext, dnext, p +) where F<:Function supersample = solver.supersample Ts_inner = Ts/supersample + isFOH_u = isdifferent(u, unext) + isFOH_d = isdifferent(d, dnext) nx = length(x) xcurr = @views k̄[1:nx] k1 = @views k̄[(1nx + 1):(2nx)] @. xcurr = x for i=1:supersample - f!(k1, xcurr, u, d, p) + τ = (i-1)/supersample + u1 = isFOH_u ? @.(ui = (1-τ)*u + (τ)*unext) : u + d1 = isFOH_d ? @.(di = (1-τ)*d + (τ)*dnext) : d + f!(k1, xcurr, u1, d1, p) @. xcurr = xcurr + k1 * Ts_inner end @. xnext = xcurr diff --git a/src/precompile.jl b/src/precompile.jl index f2efad265..0479fca89 100644 --- a/src/precompile.jl +++ b/src/precompile.jl @@ -8,13 +8,13 @@ sys = [ ] Ts = 400.0 sys2 = minreal(ss(sys)) -function f!(xnext, x, u, _ , p) +function f1!(xnext, x, u, _ , p) A, B, _ = p mul!(xnext, A , x) mul!(xnext, B, u, 1, 1) return nothing end -function h!(y, x, _ , p) +function h1!(y, x, _ , p) _, _, C = p mul!(y, C, x) return nothing @@ -80,7 +80,7 @@ R̂y = repeat([55; 30], 3) exmpc.estim() exmpc([55, 30]) - nlmodel = NonLinModel(f!, h!, Ts, 2, 2, 2; solver=RungeKutta(4), p) + nlmodel = NonLinModel(f1!, h1!, Ts, 2, 2, 2; solver=RungeKutta(4), p) nlmodel = setop!(nlmodel, uop=[10, 10], yop=[50, 30]) y = nlmodel() diff --git a/src/sim_model.jl b/src/sim_model.jl index 8c755dc52..e1eb593c9 100644 --- a/src/sim_model.jl +++ b/src/sim_model.jl @@ -44,6 +44,10 @@ struct SimModelBuffer{NT<:Real} y::Vector{NT} d::Vector{NT} k̄::Vector{NT} + ui::Vector{NT} + di::Vector{NT} + unext::Vector{NT} + dnext::Vector{NT} empty::Vector{NT} end @@ -65,8 +69,12 @@ function SimModelBuffer{NT}( y = Vector{NT}(undef, ny) d = Vector{NT}(undef, nd) k̄ = Vector{NT}(undef, nx*(ni+1)) # the "+1" is necessary because of super-sampling + ui = Vector{NT}(undef, nu) + di = Vector{NT}(undef, nd) + unext = Vector{NT}(undef, nu) + dnext = Vector{NT}(undef, nd) empty = Vector{NT}(undef, 0) - return SimModelBuffer{NT}(u, x, a, y, d, k̄, empty) + return SimModelBuffer{NT}(u, x, a, y, d, k̄, ui, di, unext, dnext, empty) end @doc raw""" @@ -245,11 +253,16 @@ function preparestate!(model::SimModel) end @doc raw""" - updatestate!(model::SimModel, u, d=[]) -> xnext + updatestate!(model::SimModel, u, d=[]; unext=u, dnext=d) -> xnext Update `model.x0` states with current inputs `u` and meas. dist. `d` for the next time step. The method computes and returns the model state for the next time step ``\mathbf{x}(k+1)``. +The keyword arguments `unext` and `dnext` allow simulating with a first-order hold (FOH) on +manipulated inputs and measured disturbances, respectively. They are interpreted as the left +limit value at the next time step ``\mathbf{u^-}(k+1)`` and ``\mathbf{d^-}(k+1)``. The +default is zero-order hold (ZOH) on both. The two values are ignored for [`LinModel`](@ref) +since discretization is already performed at construction. # Examples ```jldoctest @@ -260,12 +273,18 @@ julia> x = updatestate!(model, [1]) 1.0 ``` """ -function updatestate!(model::SimModel{NT}, u, d=model.buffer.empty) where NT <: Real +function updatestate!( + model::SimModel{NT}, u, d=model.buffer.empty; unext=u, dnext=d +) where NT <: Real validate_args(model::SimModel, d, u) - u0, d0, x0next, k̄ = model.buffer.u, model.buffer.d, model.buffer.x, model.buffer.k̄ + u0, d0, k̄ = model.buffer.u, model.buffer.d, model.buffer.k̄ + x0next, u0next, d0next = model.buffer.x, model.buffer.unext, model.buffer.dnext + ui, di = model.buffer.ui, model.buffer.di u0 .= u .- model.uop d0 .= d .- model.dop - f!(x0next, k̄, model, model.x0, u0, d0, model.p) + u0next .= unext .- model.uop + d0next .= dnext .- model.dop + f!(x0next, k̄, ui, di, model, model.x0, u0, d0, u0next, d0next) x0next .+= model.fop .- model.xop model.x0 .= x0next xnext = x0next @@ -294,7 +313,7 @@ function evaloutput(model::SimModel{NT}, d=model.buffer.empty) where NT <: Real validate_args(model, d) d0, y0 = model.buffer.d, model.buffer.y d0 .= d .- model.dop - h!(y0, model, model.x0, d0, model.p) + h!(y0, model, model.x0, d0) y = y0 y .+= model.yop return y diff --git a/src/transcription.jl b/src/transcription.jl index deb8fbe7a..9afbab83c 100644 --- a/src/transcription.jl +++ b/src/transcription.jl @@ -33,9 +33,9 @@ Abstract subtype of [`TranscriptionMethod`](@ref) for direct collocation methods abstract type CollocationMethod <: TranscriptionMethod end @doc raw""" - SingleShooting() + SingleShooting(h::Int=0) -Construct a direct single shooting [`TranscriptionMethod`](@ref). +Construct a direct single shooting [`TranscriptionMethod`](@ref) with `h`th order hold. In the case of [`PredictiveController`](@ref) types, the decision variable in the optimization problem is (excluding the slack ``ϵ``, and without any custom move blocking): @@ -46,6 +46,11 @@ optimization problem is (excluding the slack ``ϵ``, and without any custom move \vdots \\ \mathbf{Δu}(k+H_c-1) \end{bmatrix} ``` +The `h` argument is `0` or `1`, for piecewise constant or linear manipulated inputs +``\mathbf{u}``. Measured disturbances ``\mathbf{d}`` are always piecewise linear. This +setting is ignored for [`LinModel`](@ref) since already discretized at construction, and it +only applies for the internal predictions of MPC and MHE. + This method computes the predictions by calling the augmented discrete-time model recursively over the prediction horizon ``H_p`` in the objective function, or by updating the linear coefficients of the quadratic optimization for [`LinModel`](@ref). It is @@ -71,12 +76,17 @@ plant model/constraints. The Extended Help details transcription of elements in ``\mathbf{Z}`` is therefore constant. The ``\mathbf{Ŵ}`` vector is defined in the Extended Help of [`MovingHorizonEstimator`](@ref). """ -struct SingleShooting <: ShootingMethod end +struct SingleShooting <: ShootingMethod + h::Int + function SingleShooting(h::Int=0) + return new(h) + end +end @doc raw""" - MultipleShooting(; f_threads=false, h_threads=false) + MultipleShooting(h::Int; f_threads=false, h_threads=false) -Construct a direct multiple shooting [`TranscriptionMethod`](@ref). +Construct a direct multiple shooting [`TranscriptionMethod`](@ref) with `h`th order hold. The decision variable of [`PredictiveController`](@ref) is (excluding ``ϵ``): ```math @@ -94,7 +104,7 @@ operating point ``\mathbf{x̂_{op}}`` (see [`augment_model`](@ref)): where ``\mathbf{x̂}_i(k+j)`` is the state prediction for time ``k+j``, estimated by the observer at time ``i=k`` or ``i=k-1`` depending on its `direct` flag. Note that ``\mathbf{X̂_0 = X̂}`` if the operating point is zero, which is typically the case in practice -for [`NonLinModel`](@ref). +for [`NonLinModel`](@ref). See [`SingleShooting`](@ref) for details on `h` arguments. This transcription computes the predictions by calling the augmented discrete-time model in the equality constraint function recursively over ``H_p``, or by updating the linear @@ -131,10 +141,11 @@ provided in the Extended Help. with zeros is for the unused decision variables at the beginning. """ struct MultipleShooting <: ShootingMethod + h::Int f_threads::Bool h_threads::Bool - function MultipleShooting(; f_threads=false, h_threads=false) - return new(f_threads, h_threads) + function MultipleShooting(h::Int=0; f_threads=false, h_threads=false) + return new(h, f_threads, h_threads) end end @@ -144,14 +155,10 @@ end Construct an implicit trapezoidal [`TranscriptionMethod`](@ref) with `h`th order hold. This is the simplest collocation method. It supports continuous-time [`NonLinModel`](@ref) -and [`NonLinModelDAE`](@ref). For [`NonLinModel`](@ref), the decision variables are the same -as for [`MultipleShooting`](@ref), hence similar computational costs. See the same docstring -for descriptions of `f_threads` and `h_threads` keywords. The Extended Help details -the decision variables for [`NonLinModelDAE`](@ref). The `h` argument is `0` or `1`, for -piecewise constant or linear manipulated inputs ``\mathbf{u}`` (`h=1` is slightly less -expensive). Note that the various [`DiffSolver`](@ref) here assume zero-order hold, so `h=1` -will induce a small plant-model mismatch if the plant is simulated with these solvers. -Measured disturbances ``\mathbf{d}`` are piecewise linear. +and [`NonLinModelDAE`](@ref). See [`SingleShooting`](@ref) for details on `h` arguments. For +[`NonLinModel`](@ref), the decision variables are the same as for [`MultipleShooting`](@ref), +hence similar computational costs. See the same docstring for descriptions of `f_threads` +and `h_threads` keywords. The Extended Help details the decision variables for [`NonLinModelDAE`](@ref). This transcription computes the predictions by calling the continuous-time model in the equality constraint function and by using the implicit trapezoidal rule. It can handle @@ -258,13 +265,13 @@ end h::Int=0, no::Int=3; f_threads=false, h_threads=false, roots=:gaussradau ) -Construct an orthogonal collocation [`TranscriptionMethod`](@ref). +Construct an orthogonal collocation [`TranscriptionMethod`](@ref) with `h`th order hold. Also known as pseudo-spectral method. It supports continuous-time [`NonLinModel`](@ref) -and [`NonLinModelDAE`](@ref). The `h` argument is the hold order for ``\mathbf{u}`` (`0` or -`1`), and the `no` argument, the number of collocation points ``n_o``. The decision variable -of [`PredictiveController`](@ref) with [`NonLinModel`](@ref) is similar to -[`MultipleShooting`](@ref), but it also includes the collocation points: +and [`NonLinModelDAE`](@ref). See [`SingleShooting`](@ref) for details on `h` arguments. The +`no` argument is the number of collocation points ``n_o``. The decision variable of +[`PredictiveController`](@ref) with [`NonLinModel`](@ref) is similar to [`MultipleShooting`](@ref), +but it also includes the collocation points: ```math \mathbf{Z} = \begin{bmatrix} \mathbf{ΔU} \\ \mathbf{X̂_0} \\ \mathbf{K̄} \end{bmatrix} ``` diff --git a/test/1_test_sim_model.jl b/test/1_test_sim_model.jl index c8d6a1fb8..2c4f65be9 100644 --- a/test/1_test_sim_model.jl +++ b/test/1_test_sim_model.jl @@ -227,9 +227,10 @@ end "4th order Runge-Kutta differential equation solver with 1 supersamples." nonlinmodel5 = NonLinModel(f3, h3, 1.0, 1, 2, 1, 1, solver=solver, p=p) ẋ, k̄, y = nonlinmodel5.buffer.x, nonlinmodel5.buffer.k̄, nonlinmodel5.buffer.y - ModelPredictiveControl.f!(ẋ, k̄, nonlinmodel5, [0; 0], [0], [0], nonlinmodel5.p) + ui, di = nonlinmodel5.buffer.ui, nonlinmodel5.buffer.di + ModelPredictiveControl.f!(ẋ, k̄, ui, di, nonlinmodel5, [0; 0], [0], [0], [0], [0]) @test ẋ ≈ zeros(2) - ModelPredictiveControl.h!(y, nonlinmodel5, [0; 0], [0], nonlinmodel5.p) + ModelPredictiveControl.h!(y, nonlinmodel5, [0; 0], [0]) @test y ≈ zeros(1) function f2!(ẋ, x, u , d, p) @@ -245,15 +246,17 @@ end end nonlinmodel6 = NonLinModel(f2!, h2!, 1.0, 1, 2, 1, 1, solver=RungeKutta(), p=p) ẋ, k̄, y = nonlinmodel6.buffer.x, nonlinmodel6.buffer.k̄, nonlinmodel6.buffer.y - ModelPredictiveControl.f!(ẋ, k̄, nonlinmodel6, [0; 0], [0], [0], nonlinmodel6.p) + ui, di = nonlinmodel6.buffer.ui, nonlinmodel6.buffer.di + ModelPredictiveControl.f!(ẋ, k̄, ui, di, nonlinmodel6, [0; 0], [0], [0], [0], [0]) @test ẋ ≈ zeros(2) - ModelPredictiveControl.h!(y, nonlinmodel6, [0; 0], [0], nonlinmodel6.p) + ModelPredictiveControl.h!(y, nonlinmodel6, [0; 0], [0]) @test y ≈ zeros(1) nonlinmodel7 = NonLinModel(f2!, h2!, 1.0, 1, 2, 1, 1, solver=ForwardEuler(), p=p) ẋ, k̄, y = nonlinmodel7.buffer.x, nonlinmodel7.buffer.k̄, nonlinmodel7.buffer.y - ModelPredictiveControl.f!(ẋ, k̄, nonlinmodel7, [0; 0], [0], [0], nonlinmodel7.p) + ui, di = nonlinmodel7.buffer.ui, nonlinmodel7.buffer.di + ModelPredictiveControl.f!(ẋ, k̄, ui, di, nonlinmodel7, [0; 0], [0], [0], [0], [0]) @test ẋ ≈ zeros(2) - ModelPredictiveControl.h!(y, nonlinmodel7, [0; 0], [0], nonlinmodel7.p) + ModelPredictiveControl.h!(y, nonlinmodel7, [0; 0], [0]) @test y ≈ zeros(1) nonlinmodel8 = NonLinModel(f2!, h2!, 1.0, 1, 2, 1, 1, p=p, jacobian=AutoFiniteDiff()) @test nonlinmodel8.jacobian == AutoFiniteDiff() @@ -336,15 +339,16 @@ end linmodel3 = linearize(nonlinmodel3; x, u, d) x0, u0, d0 = x - nonlinmodel3.xop, u - nonlinmodel3.uop, d - nonlinmodel3.dop x0next, k, y0 = nonlinmodel3.buffer.x, nonlinmodel3.buffer.k̄, nonlinmodel3.buffer.y + ui, di = nonlinmodel3.buffer.ui, nonlinmodel3.buffer.di backend = AutoForwardDiff() - f_A(x0next, x0, k) = ModelPredictiveControl.f!(x0next, k, nonlinmodel3, x0, u0, d0, nonlinmodel3.p) - f_Bu(x0next, u0, k) = ModelPredictiveControl.f!(x0next, k, nonlinmodel3, x0, u0, d0, nonlinmodel3.p) - f_Bd(x0next, d0, k) = ModelPredictiveControl.f!(x0next, k, nonlinmodel3, x0, u0, d0, nonlinmodel3.p) - h_C(y0, x0) = ModelPredictiveControl.h!(y0, nonlinmodel3, x0, d0, nonlinmodel3.p) - h_Dd(y0, d0) = ModelPredictiveControl.h!(y0, nonlinmodel3, x0, d0, nonlinmodel3.p) - A = jacobian(f_A, x0next, backend, x0, Cache(k)) - Bu = jacobian(f_Bu, x0next, backend, u0, Cache(k)) - Bd = jacobian(f_Bd, x0next, backend, d0, Cache(k)) + f_A(x0next, x0, k, ui, di) = ModelPredictiveControl.f!(x0next, k, ui, di, nonlinmodel3, x0, u0, d0, u0, d0) + f_Bu(x0next, u0, k, ui, di) = ModelPredictiveControl.f!(x0next, k, ui, di, nonlinmodel3, x0, u0, d0, u0, d0) + f_Bd(x0next, d0, k, ui, di) = ModelPredictiveControl.f!(x0next, k, ui, di, nonlinmodel3, x0, u0, d0, u0, d0) + h_C(y0, x0) = ModelPredictiveControl.h!(y0, nonlinmodel3, x0, d0) + h_Dd(y0, d0) = ModelPredictiveControl.h!(y0, nonlinmodel3, x0, d0) + A = jacobian(f_A, x0next, backend, x0, Cache(k), Cache(ui), Cache(di)) + Bu = jacobian(f_Bu, x0next, backend, u0, Cache(k), Cache(ui), Cache(di)) + Bd = jacobian(f_Bd, x0next, backend, d0, Cache(k), Cache(ui), Cache(di)) C = jacobian(h_C, y0, backend, x0) Dd = jacobian(h_Dd, y0, backend, d0) @test linmodel3.A ≈ A @@ -569,10 +573,11 @@ end @test dae.x0 ≈ zeros(1) atol=1e-6 @test evaloutput(dae) ≈ zeros(1) atol=1e-6 - transcription = TrapezoidalCollocation() + transcription = TrapezoidalCollocation(1) dae2 = NonLinModelDAE(fq!, h!, Ts, nu, nx, na, ny; p, transcription, xs_0, as_0) @test updatestate!(dae2, u) ≈ zeros(1) atol=1e-6 @test updatestate!(dae2, u, d) ≈ zeros(1) atol=1e-6 + @test updatestate!(dae2, u; unext=u) ≈ zeros(1) atol=1e-6 @test dae2.x0 ≈ zeros(1) atol=1e-6 @test dae2() ≈ zeros(1) atol=1e-6 diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index d99cf597d..74f17b290 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -548,6 +548,26 @@ end updatestate!(internalmodel5, [0], [6], [0]) @test internalmodel5.x̂d ≈ [0] @test internalmodel5.x̂s ≈ [6] + buffer = internalmodel5.buffer + x̂0next, k̄, ûi, di = buffer.x̂, buffer.k̄, buffer.ûi, buffer.di + û0, û0next = buffer.û, buffer.ûnext + @test_nowarn ModelPredictiveControl.f̂!( + x̂0next, û0, û0next, k̄, ûi, di, internalmodel5, dae, [0], [0], [0], [0], [0] + ) # for coverage + + internalmodel6 = InternalModel(linmodel, i_ym=[1]) + preparestate!(internalmodel6, [57]) + updatestate!(internalmodel6, [10, 50], [57]) + @test internalmodel6.x̂d ≈ [0, 0] + @test internalmodel6.x̂s ≈ [7] + buffer = internalmodel6.buffer + x̂0next, k̄, ûi, di = buffer.x̂, buffer.k̄, buffer.ûi, buffer.di + û0, û0next = buffer.û, buffer.ûnext + @test_nowarn ModelPredictiveControl.f̂!( + x̂0next, û0, û0next, k̄, ûi, di, + internalmodel6, linmodel, [0, 0], [10, 50], Float64[], [10, 50], Float64[] + ) # for coverage + end @testitem "IM set model" setup=[SetupMPCtests] begin @@ -1079,8 +1099,8 @@ end function h!(y, x, a, d, _ ) y[] = 2 * x[] + a[] + 0.1 * d[] end - Ts, p = 100.0, [0.01] - dae = NonLinModelDAE(fq!, h!, Ts, 1, 1, 1, 1, 1; p) + p = [0.01] + dae = NonLinModelDAE(fq!, h!, 100.0, 1, 1, 1, 1, 1; p) transcription = TrapezoidalCollocation() mhe = MovingHorizonEstimator(dae; He=3, transcription) @@ -1348,6 +1368,22 @@ end ) @test mhe8.x̂0 ≈ zeros(6) atol=1e-9 + function f!(ẋ, x, u, d, p) + ẋ[] = -p[] * (x[] - 0.2 * u[] - 0.2 * d[]) + return nothing + end + function h!(y, x, d, _ ) + y[] = x[] + 0.1 * d[] + end + p = [0.01] + model = NonLinModel(f!, h!, 100.0, 1, 1, 1, 1; p) + transcription = SingleShooting(1) + mhe_foh = MovingHorizonEstimator(model; He=3, transcription) + preparestate!(mhe_foh, [0.0], [0.0]) + x̂ = updatestate!(mhe_foh, [1.0], [0.0], [0.0]) + info = getinfo(mhe_foh) + @test mhe_foh.transcription.h == 1 + @test info[:x̂] ≈ x̂ end @testitem "MHE estim. & getinfo (NonLinModel, MS)" setup=[SetupMPCtests] begin @@ -1378,6 +1414,23 @@ end info = getinfo(mhe3) # test getinfo when Nk