From a4795adebb7602f9c852c6363a413656a0c0bf56 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 5 Apr 2024 17:43:09 -0400 Subject: [PATCH 1/5] doc: `Cwt=Inf` in nonlinear MPC manual --- docs/src/manual/nonlinmpc.md | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/docs/src/manual/nonlinmpc.md b/docs/src/manual/nonlinmpc.md index 99f7d268b..3dcdfeb03 100644 --- a/docs/src/manual/nonlinmpc.md +++ b/docs/src/manual/nonlinmpc.md @@ -87,7 +87,7 @@ The vectors `σQ` and σR `σR` are the standard deviations of the process and s respectively. The value for the velocity ``ω`` is higher here (`σQ` second value) since ``\dot{ω}(t)`` equation includes an uncertain parameter: the friction coefficient ``K``. Also, the argument `nint_u` explicitly adds one integrating state at the model input, the -motor torque ``τ`` , with an associated standard deviation `σQint_u` of 0.1 N m. The +motor torque ``τ``, with an associated standard deviation `σQint_u` of 0.1 N m. The estimator tuning is tested on a plant with a 25 % larger friction coefficient ``K``: ```@example 1 @@ -108,12 +108,12 @@ As the motor torque is limited to -1.5 to 1.5 N m, we incorporate the input cons a [`NonLinMPC`](@ref): ```@example 1 -nmpc = NonLinMPC(estim, Hp=20, Hc=2, Mwt=[0.5], Nwt=[2.5]) +nmpc = NonLinMPC(estim, Hp=20, Hc=2, Mwt=[0.5], Nwt=[2.5], Cwt=Inf) nmpc = setconstraint!(nmpc, umin=[-1.5], umax=[+1.5]) ``` -We test `mpc` performance on `plant` by imposing an angular setpoint of 180° (inverted -position): +The option `Cwt=Inf` disables constraint softening. We test `mpc` performance on `plant` by +imposing an angular setpoint of 180° (inverted position): ```@example 1 using Logging; disable_logging(Warn) # hide @@ -185,7 +185,7 @@ function JE(UE, ŶE, _ ) τ, ω = UE[1:end-1], ŶE[2:2:end-1] return Ts*sum(τ.*ω) end -empc = NonLinMPC(estim2, Hp=20, Hc=2, Mwt=[0.5, 0], Nwt=[2.5], Ewt=3.5e3, JE=JE) +empc = NonLinMPC(estim2, Hp=20, Hc=2, Mwt=[0.5, 0], Nwt=[2.5], Cwt=Inf, Ewt=3.5e3, JE=JE) empc = setconstraint!(empc, umin=[-1.5], umax=[+1.5]) ``` @@ -250,7 +250,7 @@ A [`SteadyKalmanFilter`](@ref) and a [`LinMPC`](@ref) are designed from `linmode ```@example 1 kf = SteadyKalmanFilter(linmodel; σQ, σR, nint_u, σQint_u) -mpc = LinMPC(kf, Hp=20, Hc=2, Mwt=[0.5], Nwt=[2.5]) +mpc = LinMPC(kf, Hp=20, Hc=2, Mwt=[0.5], Nwt=[2.5], Cwt=Inf) mpc = setconstraint!(mpc, umin=[-1.5], umax=[+1.5]) ``` @@ -288,7 +288,7 @@ Constructing a [`LinMPC`](@ref) with `DAQP`: ```@example 1 using JuMP, DAQP daqp = Model(DAQP.Optimizer, add_bridges=false) -mpc2 = LinMPC(kf, Hp=20, Hc=2, Mwt=[0.5], Nwt=[2.5], optim=daqp) +mpc2 = LinMPC(kf, Hp=20, Hc=2, Mwt=[0.5], Nwt=[2.5], Cwt=Inf, optim=daqp) mpc2 = setconstraint!(mpc2, umin=[-1.5], umax=[+1.5]) ``` From bb07f7f716476e8fdc78b7c7b6fbdfff03a387d8 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 7 Apr 2024 21:28:45 -0400 Subject: [PATCH 2/5] changed: `NonLinMPC` without collect on decision vars. --- src/controller/nonlinmpc.jl | 62 ++++++++++++++----------------------- 1 file changed, 23 insertions(+), 39 deletions(-) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 6e29438e8..bcaf9d413 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -304,6 +304,7 @@ function init_optimization!(mpc::NonLinMPC, optim::JuMP.GenericModel{JNT}) where Jfunc, gfunc = let mpc=mpc, model=model, ng=ng, nΔŨ=nΔŨ, nŶ=Hp*ny, nx̂=nx̂, nu=nu, nU=Hp*nu Nc = nΔŨ + 3 last_ΔŨtup_float, last_ΔŨtup_dual = nothing, nothing + ΔŨ_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nΔŨ), Nc) Ŷ_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nŶ), Nc) U_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nU), Nc) g_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, ng), Nc) @@ -313,63 +314,46 @@ function init_optimization!(mpc::NonLinMPC, optim::JuMP.GenericModel{JNT}) where û_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nu), Nc) Ȳ_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nŶ), Nc) Ū_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nU), Nc) - function Jfunc(ΔŨtup::JNT...) + function Jfunc(ΔŨtup::T...)::T where T <: Real ΔŨ1 = ΔŨtup[begin] - Ŷ = get_tmp(Ŷ_cache, ΔŨ1) - ΔŨ = collect(ΔŨtup) - if ΔŨtup !== last_ΔŨtup_float - x̂, x̂next = get_tmp(x̂_cache, ΔŨ1), get_tmp(x̂next_cache, ΔŨ1) - u, û = get_tmp(u_cache, ΔŨ1), get_tmp(û_cache, ΔŨ1) - Ŷ, x̂end = predict!(Ŷ, x̂, x̂next, u, û, mpc, model, ΔŨ) - g = get_tmp(g_cache, ΔŨ1) - g = con_nonlinprog!(g, mpc, model, x̂end, Ŷ, ΔŨ) - last_ΔŨtup_float = ΔŨtup + ΔŨ, Ŷ = get_tmp(ΔŨ_cache, ΔŨ1), get_tmp(Ŷ_cache, ΔŨ1) + if T == JNT + isnewvalue = (ΔŨtup !== last_ΔŨtup_float) + isnewvalue && (last_ΔŨtup_float = ΔŨtup) + else + isnewvalue = (ΔŨtup !== last_ΔŨtup_dual) + isnewvalue && (last_ΔŨtup_dual = ΔŨtup) end - U, Ȳ, Ū = get_tmp(U_cache, ΔŨ1), get_tmp(Ȳ_cache, ΔŨ1), get_tmp(Ū_cache, ΔŨ1) - return obj_nonlinprog!(U, Ȳ, Ū, mpc, model, Ŷ, ΔŨ) - end - function Jfunc(ΔŨtup::ForwardDiff.Dual...) - ΔŨ1 = ΔŨtup[begin] - Ŷ = get_tmp(Ŷ_cache, ΔŨ1) - ΔŨ = collect(ΔŨtup) - if ΔŨtup !== last_ΔŨtup_dual + if isnewvalue x̂, x̂next = get_tmp(x̂_cache, ΔŨ1), get_tmp(x̂next_cache, ΔŨ1) u, û = get_tmp(u_cache, ΔŨ1), get_tmp(û_cache, ΔŨ1) + ΔŨ .= ΔŨtup Ŷ, x̂end = predict!(Ŷ, x̂, x̂next, u, û, mpc, model, ΔŨ) g = get_tmp(g_cache, ΔŨ1) g = con_nonlinprog!(g, mpc, model, x̂end, Ŷ, ΔŨ) - last_ΔŨtup_dual = ΔŨtup end U, Ȳ, Ū = get_tmp(U_cache, ΔŨ1), get_tmp(Ȳ_cache, ΔŨ1), get_tmp(Ū_cache, ΔŨ1) - return obj_nonlinprog!(U, Ȳ, Ū, mpc, model, Ŷ, ΔŨ) + return obj_nonlinprog!(U, Ȳ, Ū, mpc, model, Ŷ, ΔŨ)::T end - function gfunc_i(i, ΔŨtup::NTuple{N, JNT}) where N + function gfunc_i(i, ΔŨtup::NTuple{N, T})::T where {N, T <:Real} ΔŨ1 = ΔŨtup[begin] g = get_tmp(g_cache, ΔŨ1) - if ΔŨtup !== last_ΔŨtup_float - Ŷ = get_tmp(Ŷ_cache, ΔŨ1) - ΔŨ = collect(ΔŨtup) - x̂, x̂next = get_tmp(x̂_cache, ΔŨ1), get_tmp(x̂next_cache, ΔŨ1) - u, û = get_tmp(u_cache, ΔŨ1), get_tmp(û_cache, ΔŨ1) - Ŷ, x̂end = predict!(Ŷ, x̂, x̂next, u, û, mpc, model, ΔŨ) - g = con_nonlinprog!(g, mpc, model, x̂end, Ŷ, ΔŨ) - last_ΔŨtup_float = ΔŨtup + if T == JNT + isnewvalue = (ΔŨtup !== last_ΔŨtup_float) + isnewvalue && (last_ΔŨtup_float = ΔŨtup) + else + isnewvalue = (ΔŨtup !== last_ΔŨtup_dual) + isnewvalue && (last_ΔŨtup_dual = ΔŨtup) end - return g[i] - end - function gfunc_i(i, ΔŨtup::NTuple{N, ForwardDiff.Dual}) where N - ΔŨ1 = ΔŨtup[begin] - g = get_tmp(g_cache, ΔŨ1) - if ΔŨtup !== last_ΔŨtup_dual - Ŷ = get_tmp(Ŷ_cache, ΔŨ1) - ΔŨ = collect(ΔŨtup) + if isnewvalue + ΔŨ, Ŷ = get_tmp(ΔŨ_cache, ΔŨ1), get_tmp(Ŷ_cache, ΔŨ1) x̂, x̂next = get_tmp(x̂_cache, ΔŨ1), get_tmp(x̂next_cache, ΔŨ1) u, û = get_tmp(u_cache, ΔŨ1), get_tmp(û_cache, ΔŨ1) + ΔŨ .= ΔŨtup Ŷ, x̂end = predict!(Ŷ, x̂, x̂next, u, û, mpc, model, ΔŨ) g = con_nonlinprog!(g, mpc, model, x̂end, Ŷ, ΔŨ) - last_ΔŨtup_dual = ΔŨtup end - return g[i] + return g[i]::T end gfunc = [(ΔŨ...) -> gfunc_i(i, ΔŨ) for i in 1:ng] (Jfunc, gfunc) From 5ff0fe4bd2cc6a1bad6a892b6715bd02aed9e068 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 8 Apr 2024 12:33:50 -0400 Subject: [PATCH 3/5] changed: `MHE` without collect on decision var. --- src/controller/nonlinmpc.jl | 24 ++++++------- src/estimator/mhe/construct.jl | 66 ++++++++++++---------------------- src/general.jl | 6 ++-- src/plot_sim.jl | 2 +- test/test_state_estim.jl | 10 +++--- 5 files changed, 43 insertions(+), 65 deletions(-) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index bcaf9d413..607dd9f05 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -314,24 +314,20 @@ function init_optimization!(mpc::NonLinMPC, optim::JuMP.GenericModel{JNT}) where û_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nu), Nc) Ȳ_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nŶ), Nc) Ū_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nU), Nc) - function Jfunc(ΔŨtup::T...)::T where T <: Real + function Jfunc(ΔŨtup::T...)::T where {T <: Real} ΔŨ1 = ΔŨtup[begin] - ΔŨ, Ŷ = get_tmp(ΔŨ_cache, ΔŨ1), get_tmp(Ŷ_cache, ΔŨ1) if T == JNT - isnewvalue = (ΔŨtup !== last_ΔŨtup_float) - isnewvalue && (last_ΔŨtup_float = ΔŨtup) + last_ΔŨtup_float = ΔŨtup else - isnewvalue = (ΔŨtup !== last_ΔŨtup_dual) - isnewvalue && (last_ΔŨtup_dual = ΔŨtup) - end - if isnewvalue - x̂, x̂next = get_tmp(x̂_cache, ΔŨ1), get_tmp(x̂next_cache, ΔŨ1) - u, û = get_tmp(u_cache, ΔŨ1), get_tmp(û_cache, ΔŨ1) - ΔŨ .= ΔŨtup - Ŷ, x̂end = predict!(Ŷ, x̂, x̂next, u, û, mpc, model, ΔŨ) - g = get_tmp(g_cache, ΔŨ1) - g = con_nonlinprog!(g, mpc, model, x̂end, Ŷ, ΔŨ) + last_ΔŨtup_dual = ΔŨtup end + ΔŨ, Ŷ = get_tmp(ΔŨ_cache, ΔŨ1), get_tmp(Ŷ_cache, ΔŨ1) + x̂, x̂next = get_tmp(x̂_cache, ΔŨ1), get_tmp(x̂next_cache, ΔŨ1) + u, û = get_tmp(u_cache, ΔŨ1), get_tmp(û_cache, ΔŨ1) + ΔŨ .= ΔŨtup + Ŷ, x̂end = predict!(Ŷ, x̂, x̂next, u, û, mpc, model, ΔŨ) + g = get_tmp(g_cache, ΔŨ1) + g = con_nonlinprog!(g, mpc, model, x̂end, Ŷ, ΔŨ) U, Ȳ, Ū = get_tmp(U_cache, ΔŨ1), get_tmp(Ȳ_cache, ΔŨ1), get_tmp(Ū_cache, ΔŨ1) return obj_nonlinprog!(U, Ȳ, Ū, mpc, model, Ŷ, ΔŨ)::T end diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 51c9bf466..66f5d5415 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -1008,72 +1008,52 @@ function init_optimization!( Jfunc, gfunc = let estim=estim, model=model, nZ̃=nZ̃, nV̂=nV̂, nX̂=nX̂, ng=ng, nx̂=nx̂, nu=nu, nŷ=nŷ Nc = nZ̃ + 3 last_Z̃tup_float, last_Z̃tup_dual = nothing, nothing + Z̃_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nZ̃), Nc) V̂_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nV̂), Nc) g_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, ng), Nc) X̂_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nX̂), Nc) x̄_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nx̂), Nc) û_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nu), Nc) ŷ_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nŷ), Nc) - function Jfunc(Z̃tup::JNT...) + function Jfunc(Z̃tup::T...)::T where {T <: Real} Z̃1 = Z̃tup[begin] - V̂ = get_tmp(V̂_cache, Z̃1) - Z̃ = collect(Z̃tup) - if Z̃tup !== last_Z̃tup_float - g = get_tmp(g_cache, Z̃1) - X̂ = get_tmp(X̂_cache, Z̃1) - û, ŷ = get_tmp(û_cache, Z̃1), get_tmp(ŷ_cache, Z̃1) - V̂, X̂ = predict!(V̂, X̂, û, ŷ, estim, model, Z̃) - g = con_nonlinprog!(g, estim, model, X̂, V̂, Z̃) + if T == JNT last_Z̃tup_float = Z̃tup - end - x̄ = get_tmp(x̄_cache, Z̃1) - return obj_nonlinprog!(x̄, estim, model, V̂, Z̃) - end - function Jfunc(Z̃tup::ForwardDiff.Dual...) - Z̃1 = Z̃tup[begin] - V̂ = get_tmp(V̂_cache, Z̃1) - Z̃ = collect(Z̃tup) - if Z̃tup !== last_Z̃tup_dual - g = get_tmp(g_cache, Z̃1) - X̂ = get_tmp(X̂_cache, Z̃1) - û, ŷ = get_tmp(û_cache, Z̃1), get_tmp(ŷ_cache, Z̃1) - V̂, X̂ = predict!(V̂, X̂, û, ŷ, estim, model, Z̃) - g = con_nonlinprog!(g, estim, model, X̂, V̂, Z̃) + else last_Z̃tup_dual = Z̃tup end + Z̃, V̂ = get_tmp(Z̃_cache, Z̃1), get_tmp(V̂_cache, Z̃1) + X̂ = get_tmp(X̂_cache, Z̃1) + û, ŷ = get_tmp(û_cache, Z̃1), get_tmp(ŷ_cache, Z̃1) + Z̃ .= Z̃tup + V̂, X̂ = predict!(V̂, X̂, û, ŷ, estim, model, Z̃) + g = get_tmp(g_cache, Z̃1) + g = con_nonlinprog!(g, estim, model, X̂, V̂, Z̃) x̄ = get_tmp(x̄_cache, Z̃1) - return obj_nonlinprog!(x̄, estim, model, V̂, Z̃) + return obj_nonlinprog!(x̄, estim, model, V̂, Z̃)::T end - function gfunc_i(i, Z̃tup::NTuple{N, JNT}) where N + function gfunc_i(i, Z̃tup::NTuple{N, T})::T where {N, T <:Real} Z̃1 = Z̃tup[begin] g = get_tmp(g_cache, Z̃1) - if Z̃tup !== last_Z̃tup_float - Z̃ = collect(Z̃tup) - V̂ = get_tmp(V̂_cache, Z̃1) - X̂ = get_tmp(X̂_cache, Z̃1) - û, ŷ = get_tmp(û_cache, Z̃1), get_tmp(ŷ_cache, Z̃1) - V̂, X̂ = predict!(V̂, X̂, û, ŷ, estim, model, Z̃) - g = con_nonlinprog!(g, estim, model, X̂, V̂, Z̃) - last_Z̃tup_float = Z̃tup + if T == JNT + isnewvalue = (Z̃tup !== last_Z̃tup_float) + isnewvalue && (last_Z̃tup_float = Z̃tup) + else + isnewvalue = (Z̃tup !== last_Z̃tup_dual) + isnewvalue && (last_Z̃tup_dual = Z̃tup) end - return g[i] - end - function gfunc_i(i, Z̃tup::NTuple{N, ForwardDiff.Dual}) where N - Z̃1 = Z̃tup[begin] - g = get_tmp(g_cache, Z̃1) - if Z̃tup !== last_Z̃tup_dual - Z̃ = collect(Z̃tup) - V̂ = get_tmp(V̂_cache, Z̃1) + if isnewvalue + Z̃, V̂ = get_tmp(Z̃_cache, Z̃1), get_tmp(V̂_cache, Z̃1) X̂ = get_tmp(X̂_cache, Z̃1) û, ŷ = get_tmp(û_cache, Z̃1), get_tmp(ŷ_cache, Z̃1) + Z̃ .= Z̃tup V̂, X̂ = predict!(V̂, X̂, û, ŷ, estim, model, Z̃) g = con_nonlinprog!(g, estim, model, X̂, V̂, Z̃) - last_Z̃tup_dual = Z̃tup end return g[i] end gfunc = [(Z̃...) -> gfunc_i(i, Z̃) for i in 1:ng] - Jfunc, gfunc + (Jfunc, gfunc) end register(optim, :Jfunc, nZ̃, Jfunc, autodiff=true) @NLobjective(optim, Min, Jfunc(Z̃var...)) diff --git a/src/general.jl b/src/general.jl index 5394a9b18..f18eeb9ee 100644 --- a/src/general.jl +++ b/src/general.jl @@ -7,11 +7,11 @@ const DEFAULT_CWT = 1e5 const DEFAULT_EWT = 0.0 "Termination status that means 'no solution available'." -const ERROR_STATUSES = [ +const ERROR_STATUSES = ( INFEASIBLE, DUAL_INFEASIBLE, LOCALLY_INFEASIBLE, INFEASIBLE_OR_UNBOUNDED, NUMERICAL_ERROR, INVALID_MODEL, INVALID_OPTION, INTERRUPTED, OTHER_ERROR -] +) "Verify that `optim` termination status is `OPTIMAL` or `LOCALLY_SOLVED`." function issolved(optim::JuMP.GenericModel) @@ -22,7 +22,7 @@ end "Verify that `optim` termination status means 'no solution available'." function iserror(optim::JuMP.GenericModel) status = termination_status(optim) - return any(status .== ERROR_STATUSES) + return any(errstatus->isequal(status, errstatus), ERROR_STATUSES) end "Evaluate the quadratic programming objective function `0.5x'*H*x + q'*x` at `x`." diff --git a/src/plot_sim.jl b/src/plot_sim.jl index e5a120fe4..38b4bfa92 100644 --- a/src/plot_sim.jl +++ b/src/plot_sim.jl @@ -129,7 +129,7 @@ function sim!( U_data[:, i] = u D_data[:, i] = d X_data[:, i] = plant.x - x = updatestate!(plant, u, d) + updatestate!(plant, u, d) end return SimResult(plant, T_data, Y_data, U_data, Y_data, U_data, U_data, U_data, D_data, X_data, X_data) diff --git a/test/test_state_estim.jl b/test/test_state_estim.jl index 92ea3fd0c..9a5bd10a2 100644 --- a/test/test_state_estim.jl +++ b/test/test_state_estim.jl @@ -682,10 +682,12 @@ end mhe4 = setconstraint!(MovingHorizonEstimator(nonlinmodel, He=1, nint_ym=0), x̂max=[50,50,50,50]) g_X̂max_end = mhe4.optim.nlp_model.operators.registered_multivariate_operators[end].f - @test g_X̂max_end((1.0, 1.0, 1.0, 1.0)) ≤ 0.0 # test gfunc_i(i,::NTuple{N, Float64}) - # test gfunc_i(i,::NTuple{N, ForwardDiff.Dual}) : - @test ForwardDiff.gradient(g_X̂max_end, [1.0, 1.0, 1.0, 1.0]) ≈ [0.0, 0.0, 0.0, 0.0] - + # test gfunc_i(i,::NTuple{N, Float64}): + @test g_X̂max_end( + (1.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0)) ≤ 0.0 + # test gfunc_i(i,::NTuple{N, ForwardDiff.Dual}): + @test ForwardDiff.gradient( + g_X̂max_end, [1.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0]) ≈ [0, 0, 0, 0, 0, 0, 0, 0] Q̂ = diagm([1/4, 1/4, 1/4, 1/4].^2) R̂ = diagm([1, 1].^2) optim = Model(Ipopt.Optimizer) From cbe18110022d046945536f230b976019e12894e8 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 8 Apr 2024 12:47:40 -0400 Subject: [PATCH 4/5] changed: error handling one less alloc --- src/controller/execute.jl | 7 +++++-- src/estimator/mhe/execute.jl | 7 +++++-- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/src/controller/execute.jl b/src/controller/execute.jl index f8673213f..c75947e5d 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -445,7 +445,6 @@ function optim_objective!(mpc::PredictiveController{NT}) where {NT<:Real} rethrow(err) end end - ΔŨcurr, ΔŨlast = value.(ΔŨvar), ΔŨ0 if !issolved(optim) status = termination_status(optim) if iserror(optim) @@ -457,7 +456,11 @@ function optim_objective!(mpc::PredictiveController{NT}) where {NT<:Real} end @debug solution_summary(optim, verbose=true) end - mpc.ΔŨ .= iserror(optim) ? ΔŨlast : ΔŨcurr + if iserror(optim) + mpc.ΔŨ .= ΔŨ0 + else + mpc.ΔŨ .= value.(ΔŨvar) + end return mpc.ΔŨ end diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index c4d34cf98..3e1330d2e 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -62,7 +62,6 @@ function update_estimate!(estim::MovingHorizonEstimator{NT}, u, ym, d) where NT< end end # -------- error handling ------------------------- - Z̃curr, Z̃last = value.(Z̃var), Z̃0 if !issolved(optim) status = termination_status(optim) if iserror(optim) @@ -74,7 +73,11 @@ function update_estimate!(estim::MovingHorizonEstimator{NT}, u, ym, d) where NT< end @debug solution_summary(optim, verbose=true) end - estim.Z̃ .= iserror(optim) ? Z̃last : Z̃curr + if iserror(optim) + mpc.Z̃ .= Z̃0 + else + mpc.Z̃ .= value.(Z̃var) + end # --------- update estimate ----------------------- estim.Ŵ[1:nŵ*Nk] .= @views estim.Z̃[nx̃+1:nx̃+nŵ*Nk] # update Ŵ with optimum for warm-start V̂, X̂ = predict!(V̂, X̂, û, ŷ, estim, model, estim.Z̃) From 579b2ecf803782ee88059cff458142e1b667ab37 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 8 Apr 2024 13:22:55 -0400 Subject: [PATCH 5/5] debug MHE --- src/estimator/mhe/execute.jl | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 3e1330d2e..4cd5d284c 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -74,9 +74,9 @@ function update_estimate!(estim::MovingHorizonEstimator{NT}, u, ym, d) where NT< @debug solution_summary(optim, verbose=true) end if iserror(optim) - mpc.Z̃ .= Z̃0 + estim.Z̃ .= Z̃0 else - mpc.Z̃ .= value.(Z̃var) + estim.Z̃ .= value.(Z̃var) end # --------- update estimate ----------------------- estim.Ŵ[1:nŵ*Nk] .= @views estim.Z̃[nx̃+1:nx̃+nŵ*Nk] # update Ŵ with optimum for warm-start