From 83e646b8d579345431e2bcc84fedf77b2c0a21f3 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 22 Oct 2026 16:47:55 -0400 Subject: [PATCH 01/56] wip: support FOH in `RungeKutta(4)` --- src/controller/execute.jl | 8 +++++--- src/estimator/execute.jl | 2 +- src/model/linmodel.jl | 8 ++++---- src/model/nonlinmodel.jl | 19 ++++++++++++------- src/model/solver.jl | 32 ++++++++++++++++++++++++-------- src/sim_model.jl | 28 ++++++++++++++++++++++------ 6 files changed, 68 insertions(+), 29 deletions(-) diff --git a/src/controller/execute.jl b/src/controller/execute.jl index 6f4be31ac..68752e0ed 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -549,12 +549,14 @@ function getinput!(mpc, Z̃) end """ - updatestate!(mpc::PredictiveController, u, ym, d=[]) -> x̂next + updatestate!(mpc::PredictiveController, u, ym, d=[]; unext=u, dnext=d) -> x̂next Call [`updatestate!`](@ref) on `mpc.estim` [`StateEstimator`](@ref). """ -function updatestate!(mpc::PredictiveController, u, ym, d=mpc.estim.buffer.empty) - return updatestate!(mpc.estim, u, ym, d) +function updatestate!( + mpc::PredictiveController, u, ym, d=mpc.estim.buffer.empty; unext=u, dnext=d +) + return updatestate!(mpc.estim, u, ym, d; unext, dnext) end updatestate!(::PredictiveController, _ ) = throw(ArgumentError("missing measured outputs ym")) diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index 4b59ce8ea..e63fb73fc 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -384,7 +384,7 @@ julia> x̂ = updatestate!(kf, u, ym) # x̂[2] is the integrator state (nint_ym a 0.0 ``` """ -function updatestate!(estim::StateEstimator, u, ym, d=estim.buffer.empty) +function updatestate!(estim::StateEstimator, u, ym, d=estim.buffer.empty;) isnothing(ym) && (ym = estim.buffer.ym; ym .= NaN) # nothing: skip correction step if estim.direct && !estim.prepared[] error("preparestate! must be called before updatestate! with direct=true option") diff --git a/src/model/linmodel.jl b/src/model/linmodel.jl index 8fff8feb6..949469a75 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..80adfe689 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,26 @@ LinModel(model::NonLinModel; kwargs...) = linearize(model; kwargs...) """ - f!(x0next, k̄, model::NonLinModel, x0, u0, d0, p) + f!(x0next, k̄, 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. +intermediate stage values of the solver. 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̄, model::NonLinModel, x0, u0, d0, u0next, d0next) + return solver_f!( + x0next, k̄, model.f!, model.Ts, model.solver, x0, u0, d0, u0next, d0next, model.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/solver.jl b/src/model/solver.jl index c42577c2f..716a3c70f 100644 --- a/src/model/solver.jl +++ b/src/model/solver.jl @@ -28,7 +28,9 @@ 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 nx = length(x) @@ -36,16 +38,25 @@ function solver_f!(xnext, k̄, f!::F, Ts, solver::RungeKutta{4}, x, u, d, p) whe 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 + @. ui = (1-τ)*u + (τ)*unext + @. di = (1-τ)*d + (τ)*dnext + f!(k1, xcurr, ui, di, p) @. xnext = xcurr + k1 * Ts_inner/2 - f!(k2, xnext, u, d, p) + τ = (i-0.5)/supersample + @. ui = (1-τ)*u + (τ)*unext + @. di = (1-τ)*d + (τ)*dnext + f!(k2, xnext, ui, di, p) @. xnext = xcurr + k2 * Ts_inner/2 - f!(k3, xnext, u, d, p) + f!(k3, xnext, ui, di, p) @. xnext = xcurr + k3 * Ts_inner - f!(k4, xnext, u, d, p) + τ = i/supersample + @. ui = (1-τ)*u + (τ)*unext + @. di = (1-τ)*d + (τ)*dnext + f!(k4, xnext, ui, di, p) @. xcurr = xcurr + (k1 + 2k2 + 2k3 + k4)*Ts_inner/6 end @. xnext = xcurr @@ -63,7 +74,9 @@ 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 nx = length(x) @@ -71,7 +84,10 @@ function solver_f!(xnext, k̄, f!::F, Ts, solver::RungeKutta{1}, x, u, d, p) whe k1 = @views k̄[(1nx + 1):(2nx)] @. xcurr = x for i=1:supersample - f!(k1, xcurr, u, d, p) + τ = (i-1)/supersample + @. ui = (1-τ)*u + (τ)*unext + @. di = (1-τ)*d + (τ)*dnext + f!(k1, xcurr, ui, di, p) @. xcurr = xcurr + k1 * Ts_inner end @. xnext = xcurr diff --git a/src/sim_model.jl b/src/sim_model.jl index 8c755dc52..b30980b52 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,13 @@ 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 first-order hold (FOH) +assumption on manipulated inputs and measure disturbances, respectively. # Examples ```jldoctest @@ -260,12 +270,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 +310,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 From b7342251226b6204957235fad8c44c90cdffb382 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 24 Oct 2026 11:01:16 -0400 Subject: [PATCH 02/56] wip: idem --- src/controller/transcription.jl | 2 +- src/model/linmodel.jl | 2 +- src/model/nonlinmodel.jl | 9 ++++----- src/model/nonlinmodeldae.jl | 2 +- 4 files changed, 7 insertions(+), 8 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 31b979af8..e741d633f 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1520,7 +1520,7 @@ function con_nonlinprogeq!( 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) + f!(x̂dnext, k̄, model, x̂d_Z̃, û0, d̂0) ŝdnext .= @. x̂dnext - x̂dnext_Z̃ end return geq diff --git a/src/model/linmodel.jl b/src/model/linmodel.jl index 949469a75..f57b433ea 100644 --- a/src/model/linmodel.jl +++ b/src/model/linmodel.jl @@ -294,7 +294,7 @@ end 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) diff --git a/src/model/nonlinmodel.jl b/src/model/nonlinmodel.jl index 80adfe689..d46d4834d 100644 --- a/src/model/nonlinmodel.jl +++ b/src/model/nonlinmodel.jl @@ -317,7 +317,7 @@ LinModel(model::NonLinModel; kwargs...) = linearize(model; kwargs...) """ - f!(x0next, k̄, model::NonLinModel, x0, u0, d0, u0next, d0next) + f!(x0next, k̄, ui, di, model::NonLinModel, x0, u0, d0, u0next, d0next) Compute `x0next` using the [`DiffSolver`](@ref) in `model.solver` and `model.f!`. @@ -325,10 +325,9 @@ The method mutates `x0next` and `k̄` arguments in-place. The latter is used to intermediate stage values of the solver. The model parameter `model.p` is not included in the function signature for conciseness. """ -function f!(x0next, k̄, model::NonLinModel, x0, u0, d0, u0next, d0next) - return solver_f!( - x0next, k̄, model.f!, model.Ts, model.solver, x0, u0, d0, u0next, d0next, model.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 """ diff --git a/src/model/nonlinmodeldae.jl b/src/model/nonlinmodeldae.jl index f8f2a7db4..c5f848d17 100644 --- a/src/model/nonlinmodeldae.jl +++ b/src/model/nonlinmodeldae.jl @@ -827,7 +827,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) From 89855fc51511bfb01ac6f40907abacde388cdcfd Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 25 Oct 2026 11:57:25 -0400 Subject: [PATCH 03/56] wip: idem --- src/model/nonlinmodel.jl | 8 ++-- src/model/nonlinmodeldae.jl | 90 +++++++++++++++++++++++++------------ src/sim_model.jl | 5 ++- 3 files changed, 70 insertions(+), 33 deletions(-) diff --git a/src/model/nonlinmodel.jl b/src/model/nonlinmodel.jl index d46d4834d..4a17d7b34 100644 --- a/src/model/nonlinmodel.jl +++ b/src/model/nonlinmodel.jl @@ -321,9 +321,11 @@ LinModel(model::NonLinModel; kwargs...) = linearize(model; kwargs...) 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 model parameter `model.p` is not included in -the function signature for conciseness. +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̄, ui, di, model::NonLinModel, x0, u0, d0, u0next, d0next) Ts, solver, p = model.Ts, model.solver, model.p diff --git a/src/model/nonlinmodeldae.jl b/src/model/nonlinmodeldae.jl index c5f848d17..66d3ec07b 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,66 @@ 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_nonlinprogeq!( + 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 + 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 + geq, k̄dot, + model::NonLinModelDAE, ::TrapezoidalCollocation, x0, u0, d0, u0next, d0next, Z ) + transcription = model.transcription nx, na = model.nx, model.na - Ts = model.Ts + h = transcription.h + Ts = model.Ts + u⁺, d⁺ = u0, d0 + u⁻next = h > 0 ? u0next : u0 + d⁻next = d0next # always first-order hold for measured disturbances d 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 + 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 + 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 + h = transcription.h + Mo = model.Mo + no, τ = transcription.no, transcription.τ + ui, di = model.buffer.ui, model.buffer.di + u⁺, d⁺ = u0, d0 + u⁻next = h > 0 ? u0next : u0 + d⁻next = d0next # always first-order hold for measured disturbances d 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 +754,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 +778,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 +792,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 +809,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 +817,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 +829,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 @@ -884,12 +909,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` : right limit of the next manipulated input, ``\mathbf{u^+}(k+1)`` +- `:dnext` : right 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 +940,33 @@ 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 + 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/sim_model.jl b/src/sim_model.jl index b30980b52..4d841f413 100644 --- a/src/sim_model.jl +++ b/src/sim_model.jl @@ -258,8 +258,9 @@ end 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 first-order hold (FOH) -assumption on manipulated inputs and measure disturbances, respectively. +The keyword arguments `unext` and `dnext` allow simulating with a first-order hold (FOH) on +manipulated inputs and measured disturbances, respectively (interpreted as the right limit +value of the next time step ``k+1``). # Examples ```jldoctest From 2739d0249d76b5e7d0b8a86aa288633489f87f8d Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 25 Oct 2026 13:50:48 -0400 Subject: [PATCH 04/56] added: `h` argument to `ShootingMethod`s --- src/controller/transcription.jl | 4 +-- src/model/linmodel.jl | 2 +- src/precompile.jl | 6 ++-- src/transcription.jl | 50 +++++++++++++++++++-------------- 4 files changed, 35 insertions(+), 27 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index e741d633f..3db088c0c 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1519,8 +1519,8 @@ function con_nonlinprogeq!( k̄ = @views K̄[(1 + nk̄*(j-1)):(nk̄*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) + ŝ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 diff --git a/src/model/linmodel.jl b/src/model/linmodel.jl index f57b433ea..c1991c0db 100644 --- a/src/model/linmodel.jl +++ b/src/model/linmodel.jl @@ -290,7 +290,7 @@ 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). """ 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/transcription.jl b/src/transcription.jl index deb8fbe7a..bfb250c05 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,12 @@ 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. Note that `unext` and +`dnext` keyword arguments must be explicitly provided to the [`updatestate!`](@ref) method +to simulate a plant with first-order hold (FOH) inputs. + 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 +77,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 +105,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 +142,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 +156,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 +266,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} ``` From 9ec2cd4d196dd772669d4155d61733ebc24ba2c2 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 25 Oct 2026 13:58:19 -0400 Subject: [PATCH 05/56] doc: `LinModel` detail in `updatestate!` --- src/sim_model.jl | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/sim_model.jl b/src/sim_model.jl index 4d841f413..3d81fe92f 100644 --- a/src/sim_model.jl +++ b/src/sim_model.jl @@ -260,7 +260,8 @@ Update `model.x0` states with current inputs `u` and meas. dist. `d` for the nex 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 (interpreted as the right limit -value of the next time step ``k+1``). +value of the next time step ``k+1``). The default is zero-order hold (ZOH) on both. The two +values are ignored for [`LinModel`](@ref) since discretization is done at construction. # Examples ```jldoctest From 43f948f339d1f12e884387ee2cb4ec3fca9dfd23 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 26 Oct 2026 17:44:59 -0400 Subject: [PATCH 06/56] changed: left limit in `updatestate!` doc This is truly the left limit at the next time step. If current and next values are identical, it's ZOH discretization, if they are different, it's FOH discretization. --- src/estimator/execute.jl | 11 +++++++---- src/model/nonlinmodeldae.jl | 13 ++++++------- src/sim_model.jl | 6 +++--- 3 files changed, 16 insertions(+), 14 deletions(-) diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index e63fb73fc..bbcb7c1e3 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -367,10 +367,13 @@ 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. + +As mentioned at the previous method, the `unext` and `dnext` keyword arguments allow FOH +discretization during estimation, but note that these two values are typically unknown on +real physical plant. Also 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)``. # Examples ```jldoctest diff --git a/src/model/nonlinmodeldae.jl b/src/model/nonlinmodeldae.jl index 90a1d9247..2fbebb6b4 100644 --- a/src/model/nonlinmodeldae.jl +++ b/src/model/nonlinmodeldae.jl @@ -710,8 +710,8 @@ function con_nonlinprogeq!( h = transcription.h Ts = model.Ts u⁺, d⁺ = u0, d0 - u⁻next = h > 0 ? u0next : u0 - d⁻next = d0next # always first-order hold for measured disturbances d + # 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)] @@ -735,13 +735,12 @@ function con_nonlinprogeq!( ) transcription = model.transcription nx, na = model.nx, model.na - h = transcription.h Mo = model.Mo no, τ = transcription.no, transcription.τ ui, di = model.buffer.ui, model.buffer.di u⁺, d⁺ = u0, d0 - u⁻next = h > 0 ? u0next : u0 - d⁻next = d0next # always first-order hold for measured disturbances d + # 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ā)] @@ -909,8 +908,8 @@ 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` : right limit of the next manipulated input, ``\mathbf{u^+}(k+1)`` -- `:dnext` : right limit of the next measured disturbance, ``\mathbf{d^+}(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)`` - `:u` : right limit of current manipulated input, ``\mathbf{u^+}(k)`` diff --git a/src/sim_model.jl b/src/sim_model.jl index 3d81fe92f..ab4ca16cb 100644 --- a/src/sim_model.jl +++ b/src/sim_model.jl @@ -259,9 +259,9 @@ Update `model.x0` states with current inputs `u` and meas. dist. `d` for the nex 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 (interpreted as the right limit -value of the next time step ``k+1``). The default is zero-order hold (ZOH) on both. The two -values are ignored for [`LinModel`](@ref) since discretization is done at construction. +manipulated inputs and measured disturbances, respectively (interpreted as the left limit +value at the next time step ``k+1``). The default is zero-order hold (ZOH) on both. The two +values are ignored for [`LinModel`](@ref) since discretization is performed at construction. # Examples ```jldoctest From 300dfaebb48f1053547f115b4a60da4ea086118f Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 27 Oct 2026 14:28:00 -0400 Subject: [PATCH 07/56] removed: useless parameter in methods --- src/controller/execute.jl | 2 +- src/estimator/mhe/execute.jl | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/controller/execute.jl b/src/controller/execute.jl index b62eed2a2..a4b7b72f9 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -464,7 +464,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/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 1b9e31f91..88c5e9431 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -603,7 +603,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) From 075dfdbeeeb2f9c6a365b6b5d31275bb90c02b0e Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 28 Oct 2026 09:45:16 -0400 Subject: [PATCH 08/56] doc: minor change --- src/sim_model.jl | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/sim_model.jl b/src/sim_model.jl index ab4ca16cb..9bfdbb940 100644 --- a/src/sim_model.jl +++ b/src/sim_model.jl @@ -261,7 +261,8 @@ The method computes and returns the model state for the next time step ``\mathbf The keyword arguments `unext` and `dnext` allow simulating with a first-order hold (FOH) on manipulated inputs and measured disturbances, respectively (interpreted as the left limit value at the next time step ``k+1``). The default is zero-order hold (ZOH) on both. The two -values are ignored for [`LinModel`](@ref) since discretization is performed at construction. +values are ignored for [`LinModel`](@ref) since discretization is already performed at +construction. # Examples ```jldoctest From 465cce1ad76b78500483935314b618fdc3313737 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 28 Oct 2026 13:50:49 -0400 Subject: [PATCH 09/56] doc: `StateEstimator` does not support `unext` and `dnext` --- src/estimator/execute.jl | 12 ++++-------- src/sim_model.jl | 2 +- 2 files changed, 5 insertions(+), 9 deletions(-) diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index bbcb7c1e3..6c58696f8 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -367,13 +367,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. - -As mentioned at the previous method, the `unext` and `dnext` keyword arguments allow FOH -discretization during estimation, but note that these two values are typically unknown on -real physical plant. Also 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 produce non-causal filters, so less useful in practice. # Examples ```jldoctest @@ -387,7 +383,7 @@ julia> x̂ = updatestate!(kf, u, ym) # x̂[2] is the integrator state (nint_ym a 0.0 ``` """ -function updatestate!(estim::StateEstimator, u, ym, d=estim.buffer.empty;) +function updatestate!(estim::StateEstimator, u, ym, d=estim.buffer.empty) isnothing(ym) && (ym = estim.buffer.ym; ym .= NaN) # nothing: skip correction step if estim.direct && !estim.prepared[] error("preparestate! must be called before updatestate! with direct=true option") diff --git a/src/sim_model.jl b/src/sim_model.jl index 9bfdbb940..857f8816b 100644 --- a/src/sim_model.jl +++ b/src/sim_model.jl @@ -261,7 +261,7 @@ The method computes and returns the model state for the next time step ``\mathbf The keyword arguments `unext` and `dnext` allow simulating with a first-order hold (FOH) on manipulated inputs and measured disturbances, respectively (interpreted as the left limit value at the next time step ``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 +values are ignored for [`LinModel`](@ref) since discretization is already performed at construction. # Examples From e4ee6f4af4de9c36047dbae1bec443f084ca5ecb Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 28 Oct 2026 13:51:32 -0400 Subject: [PATCH 10/56] doc: minor correction --- src/estimator/execute.jl | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index 6c58696f8..713badb93 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -368,8 +368,8 @@ state estimate for the next time step ``\mathbf{x̂}_k(k+1)``. The method [`prep 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. The -`unext` and `dnext` arguments are not supported for [`StateEstimator`](@ref) since -they complexify the implementation produce non-causal filters, so less useful in practice. +`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 From eca7e541487df49bf23036c8a1cd6662b906ab9e Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 28 Oct 2026 14:16:21 -0400 Subject: [PATCH 11/56] added: temp fix --- src/controller/transcription.jl | 1 + 1 file changed, 1 insertion(+) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 3db088c0c..dca6a6a5f 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1507,6 +1507,7 @@ function con_nonlinprogeq!( 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) + Ûi, D̂i = similar(Û0), similar(D̂0) # TODO: DELETE THIS AND ADD NEW ARGUMENTS @threadsif f_threads for j=1:Hp if j < 2 x̂d_Z̃ = @views mpc.estim.x̂0[1:nx] From 870c601316cc657604d2fd97e313056bfcbc58b7 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 28 Oct 2026 14:39:17 -0400 Subject: [PATCH 12/56] removed: `unext` and `dnext` arg to `updatestate!(::PredictiveController)` --- src/controller/execute.jl | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/controller/execute.jl b/src/controller/execute.jl index a4b7b72f9..befea6de5 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -549,14 +549,14 @@ function getinput!(mpc, Z̃) end """ - updatestate!(mpc::PredictiveController, u, ym, d=[]; unext=u, dnext=d) -> x̂next + updatestate!(mpc::PredictiveController, u, ym, d=[]) -> x̂next Call [`updatestate!`](@ref) on `mpc.estim` [`StateEstimator`](@ref). """ function updatestate!( - mpc::PredictiveController, u, ym, d=mpc.estim.buffer.empty; unext=u, dnext=d + mpc::PredictiveController, u, ym, d=mpc.estim.buffer.empty ) - return updatestate!(mpc.estim, u, ym, d; unext, dnext) + return updatestate!(mpc.estim, u, ym, d) end updatestate!(::PredictiveController, _ ) = throw(ArgumentError("missing measured outputs ym")) From df83f24d7ea2d696017d0a90285231fc7e741e69 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 28 Oct 2026 14:51:46 -0400 Subject: [PATCH 13/56] =?UTF-8?q?wip:=20new=20`f=CC=82!`=20supporting=20on?= =?UTF-8?q?ly=20ZOH?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/controller/execute.jl | 4 +--- src/controller/transcription.jl | 17 +++++++++++------ src/estimator/execute.jl | 7 ++++--- 3 files changed, 16 insertions(+), 12 deletions(-) diff --git a/src/controller/execute.jl b/src/controller/execute.jl index befea6de5..4d39c46e9 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -553,9 +553,7 @@ end Call [`updatestate!`](@ref) on `mpc.estim` [`StateEstimator`](@ref). """ -function updatestate!( - mpc::PredictiveController, u, ym, d=mpc.estim.buffer.empty -) +function updatestate!(mpc::PredictiveController, u, ym, d=mpc.estim.buffer.empty) return updatestate!(mpc.estim, u, ym, d) end updatestate!(::PredictiveController, _ ) = throw(ArgumentError("missing measured outputs ym")) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index dca6a6a5f..9f83ea0ef 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1503,7 +1503,7 @@ function con_nonlinprogeq!( 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) @@ -1516,8 +1516,13 @@ 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)] @@ -1577,6 +1582,7 @@ function con_nonlinprogeq!( X̂0 = @views Z̃[(nΔU+1):(nΔU+nX̂)] Â0 = @views Z̃[(nΔU+nX̂+1):(nΔU+nX̂+nÂ)] A⁺ = @views Z̃[(nΔU+nX̂+nÂ+1):(nΔU+nX̂+nÂ+nĀ)] + Ûi, D̂i = similar(Û0), similar(D̂0) # TODO: DELETE THIS AND ADD NEW ARGUMENTS Û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Â)] @@ -1676,8 +1682,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 + Ûi, D̂i = similar(Û0), similar(D̂0) # TODO: DELETE THIS AND ADD NEW ARGUMENTS Û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Â)] @@ -1711,7 +1716,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)] diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index 713badb93..ace0081da 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -59,7 +59,8 @@ The operating points are handled inside ``\mathbf{f̂}``. See Extended Help for The ``\mathbf{f}`` and ``\mathbf{h}`` functions above are in fact the [`f!`](@ref) and [`h!`](@ref) methods, respectively. The operating points ``\mathbf{x̂_{op}, f̂_{op}}`` are computed by [`augment_model`](@ref) (almost always zeros in practice for - [`NonLinModel`](@ref)). + [`NonLinModel`](@ref)). Note that this function only supports ZOH discretization, call + [`f!`](@ref) directly for FOH inputs. """ 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) @@ -102,7 +103,7 @@ function f̂!(x̂0next, û0, k̄, model::SimModel, As, Cs_u, f̂op, x̂op, x̂0 @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) + f!(x̂dnext, k̄, ûi, di, model, x̂d, û0, d0, û0, d0) mul!(x̂snext, As, x̂s) x̂0next .+= f̂op .- x̂op return nothing @@ -136,7 +137,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 From b4f0722c9c96df906dbda46c1b677a4e3489863b Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 28 Oct 2026 15:11:12 -0400 Subject: [PATCH 14/56] =?UTF-8?q?debug:=20UKF=20with=20new=20`f=CC=82!`=20?= =?UTF-8?q?signature?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/estimator/construct.jl | 6 +++++- src/estimator/execute.jl | 24 ++++++++++++------------ src/estimator/internal_model.jl | 10 +++++----- src/estimator/kalman.jl | 3 ++- 4 files changed, 24 insertions(+), 19 deletions(-) diff --git a/src/estimator/construct.jl b/src/estimator/construct.jl index 4541155d0..3aecfa85a 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} @@ -43,6 +45,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̃) @@ -62,7 +66,7 @@ function StateEstimatorBuffer{NT}( d = Vector{NT}(undef, nd) 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, empty ) end diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index ace0081da..51c92abc5 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -14,7 +14,7 @@ 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, k̄, ûi, di, estim::StateEstimator, model::SimModel, x̂0, u0, d0) -> nothing Mutating state update function ``\mathbf{f̂}`` of the augmented model. @@ -27,11 +27,11 @@ 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`, `k`, `ûi` and `di` 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. # Extended Help !!! details "Extended Help" @@ -62,12 +62,12 @@ The operating points are handled inside ``\mathbf{f̂}``. See Extended Help for [`NonLinModel`](@ref)). Note that this function only supports ZOH discretization, call [`f!`](@ref) directly for FOH inputs. """ -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, k̄, ûi, di, estim::StateEstimator, model::SimModel, x̂0, u0, d0) + return f̂!(x̂0next, û0, k̄, ûi, di, model, estim.As, estim.Cs_u, estim.f̂op, estim.x̂op, x̂0, u0, d0) 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). @@ -84,7 +84,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) @@ -93,11 +93,11 @@ 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, k, ûi, di, model::SimModel, As, Cs_u, f̂op, x̂op, x̂0, u0, d0) 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, k̄, ûi, di, model::SimModel, As, Cs_u, f̂op, x̂op, x̂0, u0, 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] @views x̂dnext, x̂snext = x̂0next[1:model.nx], x̂0next[model.nx+1:end] diff --git a/src/estimator/internal_model.jl b/src/estimator/internal_model.jl index cd8225286..3637ae5bd 100644 --- a/src/estimator/internal_model.jl +++ b/src/estimator/internal_model.jl @@ -173,19 +173,19 @@ 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) 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) + f!(x̂0next, k̄, ûi, di, model, x̂0, u0, d0, u0, d0) 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) + f!(x̂0next, k̄, ûi, di, model, x̂0, u0, d0, u0, d0) x̂0next .+= estim.f̂op .- estim.x̂op return nothing end diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index 7e4d69217..8be35341e 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -879,6 +879,7 @@ function update_estimate!(estim::UnscentedKalmanFilter, u0, y0m, d0) Q̂, nx̂ = estim.cov.Q̂, estim.nx̂ γ, m̂, Ŝ = estim.γ, estim.m̂, estim.Ŝ x̂0next, û0, k̄ = estim.buffer.x̂, estim.buffer.û, estim.buffer.k̄ + ûi, di = estim.buffer.ûi, estim.buffer.di # in-place operations to reduce allocations: P̂corr_temp = Hermitian(estim.buffer.P̂, :L) P̂corr_temp .= P̂corr @@ -891,7 +892,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) + @views f̂!(x̂0next, û0, k̄, ûi, di, estim, estim.model, x̂0corr_j, u0, d0) x̂0next_j .= x̂0next end mul!(x̂0next, X̂0next, m̂) From 0e787f827e60f50f489af3d3f420e8bf3b1d89d6 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 28 Oct 2026 15:15:15 -0400 Subject: [PATCH 15/56] doc: math notation in `updatestate!` --- src/sim_model.jl | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/sim_model.jl b/src/sim_model.jl index 857f8816b..bcb9ebaf1 100644 --- a/src/sim_model.jl +++ b/src/sim_model.jl @@ -260,9 +260,9 @@ Update `model.x0` states with current inputs `u` and meas. dist. `d` for the nex 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 (interpreted as the left limit -value at the next time step ``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. +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 From 06b7e8340d72c28404baf70769327a13dc6d1dc1 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 28 Oct 2026 16:31:05 -0400 Subject: [PATCH 16/56] added: `NonLinMPC` and `SingleShooting` support `h=1` --- src/controller/transcription.jl | 39 ++++++++++++++++++++++++++++----- src/estimator/execute.jl | 4 ++-- 2 files changed, 35 insertions(+), 8 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 9f83ea0ef..bf15928a9 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1325,19 +1325,47 @@ for ``j = 0, 1, ... , H_p``. """ function predict!( Ŷ0, x̂0end, X̂0, Û0, K̄, - mpc::PredictiveController, model::NonLinModel, ::SingleShooting, + 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 + nx, nu, ny, nd, nk̄ = model.nx, model.nu, model.ny, model.nd, model.nk̄ + nx̂, Hp = mpc.estim.nx̂, mpc.Hp + h = transcription.h + As, Cs_u = mpc.estim.As, mpc.estim.Cs_u D̂0 = mpc.D̂0 + Ûi, D̂i = similar(Û0), similar(D̂0) # TODO: DELETE THIS AND ADD NEW ARGUMENTS x̂0 = @views mpc.estim.x̂0[1:nx̂] d̂0 = @views mpc.d0[1:nd] for j=1:Hp + # ------ extract the samples for k+j ------------------------ + x̂d, x̂s = @views x̂0[1:nx], x̂0[(1 + nx):end] 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) + x̂dnext, x̂snext = @views x̂0next[1:nx], x̂0next[(1 + nx):end] + # ----------------- update state of stochastic model (linear) --------------------- + mul!(x̂snext, As, x̂s) + # ----------------- compute current and next disturbed inputs --------------------- + if h < 1 || j < 2 # else, û0 was already computed and stored in Û0 at j-1 + mul!(û0, Cs_u, x̂s) # ys_u = Cs_u*xs + û0 .+= u0 # û0 = u0 + ys_u + end + if h > 0 && j < Hp + u0next = @views U0[(1 + nu*j):(nu*(j+1))] + û0next = @views Û0[(1 + nu*j):(nu*(j+1))] + mul!(û0next, Cs_u, x̂snext) + û0next .+= u0next + else # j=Hp special case, û0(k+Hp-1) = û0(k+Hp), since Hc ≤ Hp implies Δu(k+Hp)=0: + û0next = û0 + end + # ---------------- update state of deterministic model ---------------------------- + # f̂! avoided to support FOH + f!(x̂dnext, k̄, ûi, d̂i, model, x̂d, û0, d̂0, û0next, d̂0next) + # --------0000---- compute predicted output for k+j ------------------------------- 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)] @@ -1520,7 +1548,7 @@ function con_nonlinprogeq!( û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: + # 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)] @@ -1582,7 +1610,6 @@ function con_nonlinprogeq!( X̂0 = @views Z̃[(nΔU+1):(nΔU+nX̂)] Â0 = @views Z̃[(nΔU+nX̂+1):(nΔU+nX̂+nÂ)] A⁺ = @views Z̃[(nΔU+nX̂+nÂ+1):(nΔU+nX̂+nÂ+nĀ)] - Ûi, D̂i = similar(Û0), similar(D̂0) # TODO: DELETE THIS AND ADD NEW ARGUMENTS Û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Â)] diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index 51c92abc5..cdc707d41 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -67,7 +67,7 @@ function f̂!(x̂0next, û0, k̄, ûi, di, estim::StateEstimator, model::SimMo 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). @@ -93,7 +93,7 @@ function f̂!(x̂0next, _ , _ , _ , _ , estim::StateEstimator, ::LinModel, x̂0, end """ - f̂!(x̂0next, û0, k, ûi, di, model::SimModel, As, Cs_u, f̂op, x̂op, x̂0, u0, d0) + f̂!(x̂0next, û0, k, ûi, di, model::SimModel, As, Cs_u, f̂op, x̂op, x̂0, u0, d0) -> nothing Same than [`f̂!`](@ref) for [`SimModel`](@ref) but without the `estim` argument. """ From b259c87624e2b06566693b47377daaae22b3d1a4 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 28 Oct 2026 16:55:45 -0400 Subject: [PATCH 17/56] =?UTF-8?q?changed:=20re-implementing=20`f=CC=82!`?= =?UTF-8?q?=20with=20FOH=20support=20It=20has=20the=20advantage=20that=20i?= =?UTF-8?q?t=20creates=20an=20abstraction=20for=20`InternalModel`,=20in=20?= =?UTF-8?q?which=20the=20state=20vector=20is=20never=20augmented.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/controller/transcription.jl | 10 ++++++-- src/estimator/execute.jl | 45 +++++++++++++++++++++++---------- 2 files changed, 40 insertions(+), 15 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index bf15928a9..d1fbdda11 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1321,7 +1321,14 @@ The method mutates `Ŷ0`, `x̂0end`, `X̂0`, `Û0` and `K` arguments. The augm \mathbf{ŷ_0}(k+j) &= \mathbf{ĥ}\Big(\mathbf{x̂_0}(k+j), \mathbf{d̂_0}(k+j) \Big) \end{aligned} ``` -for ``j = 0, 1, ... , H_p``. +for ``j = 0, 1, ... , H_p``. + + + + + The [`f̂!`](@ref) is not actually called here since it does not +support FOH discretization, and it is needed here. The internals of [`f̂`](@ref) are +reproduced here for this reason. """ function predict!( Ŷ0, x̂0end, X̂0, Û0, K̄, @@ -1363,7 +1370,6 @@ function predict!( û0next = û0 end # ---------------- update state of deterministic model ---------------------------- - # f̂! avoided to support FOH f!(x̂dnext, k̄, ûi, d̂i, model, x̂d, û0, d̂0, û0next, d̂0next) # --------0000---- compute predicted output for k+j ------------------------------- x̂0 = @views X̂0[(1 + nx̂*(j-1)):(nx̂*j)] diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index cdc707d41..0ec463899 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̄, ûi, di, 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,9 +30,11 @@ 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`, `k`, `ûi` and `di` 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 +`û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. @@ -59,15 +64,23 @@ details on ``\mathbf{û_0, f̂}`` and ``\mathbf{ĥ}`` implementations. The ``\mathbf{f}`` and ``\mathbf{h}`` functions above are in fact the [`f!`](@ref) and [`h!`](@ref) methods, respectively. The operating points ``\mathbf{x̂_{op}, f̂_{op}}`` are computed by [`augment_model`](@ref) (almost always zeros in practice for - [`NonLinModel`](@ref)). Note that this function only supports ZOH discretization, call - [`f!`](@ref) directly for FOH inputs. + [`NonLinModel`](@ref)). """ -function f̂!(x̂0next, û0, k̄, ûi, di, estim::StateEstimator, model::SimModel, x̂0, u0, d0) - return f̂!(x̂0next, û0, k̄, ûi, di, model, estim.As, estim.Cs_u, estim.f̂op, estim.x̂op, x̂0, u0, d0) +function f̂!( + x̂0next, û0, 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). @@ -84,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) @@ -93,17 +106,23 @@ function f̂!(x̂0next, _ , _ , _ , _ , estim::StateEstimator, ::LinModel, x̂0, end """ - f̂!(x̂0next, û0, k, ûi, di, model::SimModel, As, Cs_u, f̂op, x̂op, x̂0, u0, d0) -> nothing + 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̄, ûi, di, 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̄, ûi, di, model, x̂d, û0, d0, û0, d0) + f!(x̂dnext, k̄, ûi, di, model, x̂d, û0, d0, û0next, d0next) mul!(x̂snext, As, x̂s) x̂0next .+= f̂op .- x̂op return nothing From 93337be97eb407d0dc2d51ea038bc1c2c8fe116b Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 28 Oct 2026 16:56:58 -0400 Subject: [PATCH 18/56] changed: idem --- src/estimator/execute.jl | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index 0ec463899..e78ab9697 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -120,10 +120,12 @@ function f̂!( # `@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̄, ûi, di, model, x̂d, û0, d0, û0next, d0next) 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 From cc130a10b3df31029c569a2c862120be2ca60790 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 28 Oct 2026 17:45:46 -0400 Subject: [PATCH 19/56] changed: idem --- src/controller/transcription.jl | 37 ++++++++------------------------- src/estimator/construct.jl | 4 +++- src/estimator/execute.jl | 2 +- src/estimator/internal_model.jl | 19 ++++++++++++----- src/estimator/kalman.jl | 11 +++++----- 5 files changed, 33 insertions(+), 40 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index d1fbdda11..f6615b4b6 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1321,31 +1321,21 @@ The method mutates `Ŷ0`, `x̂0end`, `X̂0`, `Û0` and `K` arguments. The augm \mathbf{ŷ_0}(k+j) &= \mathbf{ĥ}\Big(\mathbf{x̂_0}(k+j), \mathbf{d̂_0}(k+j) \Big) \end{aligned} ``` -for ``j = 0, 1, ... , H_p``. - - - - - The [`f̂!`](@ref) is not actually called here since it does not -support FOH discretization, and it is needed here. The internals of [`f̂`](@ref) are -reproduced here for this reason. +for ``j = 0, 1, ... , H_p``. """ function predict!( Ŷ0, x̂0end, X̂0, Û0, K̄, mpc::PredictiveController, model::NonLinModel, transcription::SingleShooting, U0, _ ) - nx, nu, ny, nd, nk̄ = model.nx, model.nu, model.ny, model.nd, model.nk̄ + nu, ny, nd, nk̄ = model.nu, model.ny, model.nd, model.nk̄ nx̂, Hp = mpc.estim.nx̂, mpc.Hp h = transcription.h - As, Cs_u = mpc.estim.As, mpc.estim.Cs_u D̂0 = mpc.D̂0 Ûi, D̂i = similar(Û0), similar(D̂0) # TODO: DELETE THIS AND ADD NEW ARGUMENTS x̂0 = @views mpc.estim.x̂0[1:nx̂] d̂0 = @views mpc.d0[1:nd] for j=1:Hp - # ------ extract the samples for k+j ------------------------ - x̂d, x̂s = @views x̂0[1:nx], x̂0[(1 + nx):end] 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)] @@ -1353,25 +1343,16 @@ function predict!( 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)] - x̂dnext, x̂snext = @views x̂0next[1:nx], x̂0next[(1 + nx):end] - # ----------------- update state of stochastic model (linear) --------------------- - mul!(x̂snext, As, x̂s) - # ----------------- compute current and next disturbed inputs --------------------- - if h < 1 || j < 2 # else, û0 was already computed and stored in Û0 at j-1 - mul!(û0, Cs_u, x̂s) # ys_u = Cs_u*xs - û0 .+= u0 # û0 = u0 + ys_u - end if h > 0 && j < Hp - u0next = @views U0[(1 + nu*j):(nu*(j+1))] - û0next = @views Û0[(1 + nu*j):(nu*(j+1))] - mul!(û0next, Cs_u, x̂snext) - û0next .+= u0next - else # j=Hp special case, û0(k+Hp-1) = û0(k+Hp), since Hc ≤ Hp implies Δu(k+Hp)=0: + u0next = U0[(1 + nu*j):(nu*(j+1))] + û0next = Û0[(1 + nu*j):(nu*(j+1))] + else + u0next = u0 û0next = û0 end - # ---------------- update state of deterministic model ---------------------------- - f!(x̂dnext, k̄, ûi, d̂i, model, x̂d, û0, d̂0, û0next, d̂0next) - # --------0000---- compute predicted output for k+j ------------------------------- + u0next = @views (h > 0 && j < Hp) ? U0[(1 + nu*j):(nu*(j+1))] : u0 + û0next = @views (h > 0 && j < Hp) ? Û0[(1 + nu*j):(nu*(j+1))] : û0 + 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)] diff --git a/src/estimator/construct.jl b/src/estimator/construct.jl index 3aecfa85a..c6a19aba9 100644 --- a/src/estimator/construct.jl +++ b/src/estimator/construct.jl @@ -21,6 +21,7 @@ struct StateEstimatorBuffer{NT<:Real} ym::Vector{NT} ŷ ::Vector{NT} d ::Vector{NT} + ûnext::Vector{NT} empty::Vector{NT} end @@ -64,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, û, ûi, di, 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 e78ab9697..08c123ef9 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -67,7 +67,7 @@ details on ``\mathbf{û_0, f̂}`` and ``\mathbf{ĥ}`` implementations. [`NonLinModel`](@ref)). """ function f̂!( - x̂0next, û0, k̄, ûi, di, + x̂0next, û0, û0next, k̄, ûi, di, estim::StateEstimator, model::SimModel, x̂0, u0, d0, u0next, d0next ) return f̂!( diff --git a/src/estimator/internal_model.jl b/src/estimator/internal_model.jl index 3637ae5bd..2b5ede2fc 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̄, ûi, di, 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̄, ûi, di, estim::InternalModel, model::SimModel, x̂0, u0, d0) - f!(x̂0next, k̄, ûi, di, model, x̂0, u0, d0, u0, d0) +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, u0, d0, u0next, d0next) x̂0next .+= estim.f̂op .- estim.x̂op return nothing end -function f̂!(x̂0next, _ , k̄, ûi, di, estim::InternalModel, model::LinModel, x̂0, u0, d0) - f!(x̂0next, k̄, ûi, di, model, x̂0, u0, d0, u0, d0) +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, u0, d0, u0next, d0next) x̂0next .+= estim.f̂op .- estim.x̂op return nothing end diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index 8be35341e..0aa7ab1c9 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -875,13 +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̄ - ûi, di = estim.buffer.ûi, estim.buffer.di + 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 @@ -892,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̄, ûi, di, 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̂) @@ -900,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 From daff46a815adf92774e30f389b192ad288a105f8 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 28 Oct 2026 17:48:39 -0400 Subject: [PATCH 20/56] =?UTF-8?q?added:=20comment=20about=20`u=CC=820next`?= =?UTF-8?q?=20and=20`u=CC=820`=20aliasing?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/controller/transcription.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index f6615b4b6..21e4cf991 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1348,7 +1348,7 @@ function predict!( û0next = Û0[(1 + nu*j):(nu*(j+1))] else u0next = u0 - û0next = û0 + û0next = û0 # û0next and û0 are aliased in f̂!, but it's okay since u0next=u0 end u0next = @views (h > 0 && j < Hp) ? U0[(1 + nu*j):(nu*(j+1))] : u0 û0next = @views (h > 0 && j < Hp) ? Û0[(1 + nu*j):(nu*(j+1))] : û0 From cbd103f58562a728ac3ca2c3953c03bfb859d9b2 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 28 Oct 2026 17:56:14 -0400 Subject: [PATCH 21/56] changed: avoid aliasing and apply `transcription.h` --- src/controller/transcription.jl | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 21e4cf991..7ff6db0b0 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1343,15 +1343,12 @@ function predict!( 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)] + û0next = @views j < Hp ? Û0[(1 + nu*j):(nu*(j+1))] : Û0[1:nu] if h > 0 && j < Hp - u0next = U0[(1 + nu*j):(nu*(j+1))] - û0next = Û0[(1 + nu*j):(nu*(j+1))] - else + 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 - û0next = û0 # û0next and û0 are aliased in f̂!, but it's okay since u0next=u0 end - u0next = @views (h > 0 && j < Hp) ? U0[(1 + nu*j):(nu*(j+1))] : u0 - û0next = @views (h > 0 && j < Hp) ? Û0[(1 + nu*j):(nu*(j+1))] : û0 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)] From de15ac3c21c1503fca3e1282de7d85edbcfcf1fc Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 28 Oct 2026 18:02:20 -0400 Subject: [PATCH 22/56] doc: `h` only applies to internal predictions in `SingleShooting` --- src/transcription.jl | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/transcription.jl b/src/transcription.jl index bfb250c05..f428a33e5 100644 --- a/src/transcription.jl +++ b/src/transcription.jl @@ -48,9 +48,8 @@ optimization problem is (excluding the slack ``ϵ``, and without any custom move ``` 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. Note that `unext` and -`dnext` keyword arguments must be explicitly provided to the [`updatestate!`](@ref) method -to simulate a plant with first-order hold (FOH) inputs. +setting is ignored for [`LinModel`](@ref) since already discretized. Note this setting +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 From 506782c5dff149dcb1e2f18a2bee57ef76c8e472 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 28 Oct 2026 18:04:44 -0400 Subject: [PATCH 23/56] doc: idem --- src/transcription.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/transcription.jl b/src/transcription.jl index f428a33e5..9afbab83c 100644 --- a/src/transcription.jl +++ b/src/transcription.jl @@ -48,7 +48,7 @@ optimization problem is (excluding the slack ``ϵ``, and without any custom move ``` 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. Note this setting +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 From b60ee5c175a152004279acddbda4f958bf1af15d Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 28 Oct 2026 18:15:52 -0400 Subject: [PATCH 24/56] =?UTF-8?q?changed:=20new=20`f=CC=82!`=20signature?= =?UTF-8?q?=20in=20`ExtendedKalmanFilter`?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/estimator/kalman.jl | 21 +++++++++++++-------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index 0aa7ab1c9..2ce010ec0 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -1121,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, û0, û0next, k̄, ûi, di, u0, d0) + 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) From a030b07b47ba6184feeb68679e40a615b2f36d70 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 28 Oct 2026 18:46:36 -0400 Subject: [PATCH 25/56] debug: correct signature for `h!` --- src/estimator/internal_model.jl | 6 +++--- src/estimator/kalman.jl | 12 +++++++----- 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/src/estimator/internal_model.jl b/src/estimator/internal_model.jl index 2b5ede2fc..a66b005f4 100644 --- a/src/estimator/internal_model.jl +++ b/src/estimator/internal_model.jl @@ -204,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) @@ -367,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 2ce010ec0..c8725aaba 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -1282,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) From 10d9f5ebb95bcba0e45ac3bc3787871ca144adf3 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 28 Oct 2026 19:59:31 -0400 Subject: [PATCH 26/56] changed: new `f!` signature for `InternalModel` --- src/estimator/internal_model.jl | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/estimator/internal_model.jl b/src/estimator/internal_model.jl index a66b005f4..acf268c68 100644 --- a/src/estimator/internal_model.jl +++ b/src/estimator/internal_model.jl @@ -322,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 From 93d265153195d82260b57c27ceb05d6ad7f50c2d Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 28 Oct 2026 19:59:48 -0400 Subject: [PATCH 27/56] changed: new `f!` signature for linearization --- src/model/linearization.jl | 29 ++++++++++++++++++----------- 1 file changed, 18 insertions(+), 11 deletions(-) diff --git a/src/model/linearization.jl b/src/model/linearization.jl index cbc7e4a1e..7e1bf8c4f 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, k̄, ui, di, u, d) + return solver_f!(xnext, k̄, ui, di, f!, Ts, solver, x, u, d, u, d, p) # ZOH only + end + function f_u!(xnext, u, k̄, ui, di, x, d) + return solver_f!(xnext, k̄, ui, di, f!, Ts, solver, x, u, d, u, d, p) # ZOH only + end + function f_d!(xnext, d, k̄, ui, di, x, u) + 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 From 4886eb04df309a121822d3f521ec671b871b3d3d Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 28 Oct 2026 20:06:42 -0400 Subject: [PATCH 28/56] changed: idem --- src/model/linearization.jl | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/model/linearization.jl b/src/model/linearization.jl index 7e1bf8c4f..1b95b6580 100644 --- a/src/model/linearization.jl +++ b/src/model/linearization.jl @@ -16,13 +16,13 @@ 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} - function f_x!(xnext, x, k̄, ui, di, u, d) + 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, k̄, ui, di, x, d) + 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, k̄, ui, di, x, u) + 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) @@ -170,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 @@ -178,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 --- From 8775e210a8be86212fe385a7bddbe8a68c71d240 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 30 Oct 2026 20:10:32 -0400 Subject: [PATCH 29/56] added: `MovingHorizonEstimator` with `SingleShooting` support `h=1` --- src/estimator/mhe/transcription.jl | 25 ++++++++++++++++++------- 1 file changed, 18 insertions(+), 7 deletions(-) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 4102bff67..90c7dbd82 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1505,27 +1505,38 @@ process noise ``\mathbf{ŵ}``. """ function predict_mhe!( V̂, X̂0, Â0, Û0, K̄, Ŷ0, - estim::MovingHorizonEstimator, model::NonLinModel, ::SingleShooting, + 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 + Ui, Di = similar(Û0), similar(estim.D0) # TODO: DELETE THIS AND ADD NEW ARGUMENTS + 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 Ui[(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] + #d0next = @views estim.D0[(1 + nd*j):(nd*(j+1))] + 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 From dfb1e145fc15c4373d0cb9eda3035bc95c5fbc5d Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 30 Oct 2026 20:12:04 -0400 Subject: [PATCH 30/56] removed: useless comment --- src/estimator/mhe/transcription.jl | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 90c7dbd82..9612fcf54 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1520,8 +1520,8 @@ function predict_mhe!( ŵ = @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)] - ûi = @views Ui[(1 + nu*(j-1)):(nu*j)] - di = @views Di[(1 + nd*(j-1)):(nd*j)] + ûi = @views Ui[(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 @@ -1530,7 +1530,6 @@ function predict_mhe!( u0next = u0 end û0next = @views j < Nk ? Û0[(1+nu*j):(nu*(j+1))] : Û0[1:nu] - #d0next = @views estim.D0[(1 + nd*j):(nd*(j+1))] f̂!(x̂0next, û0, û0next, k̄, ûi, di, estim, model, x̂0, u0, d0, u0next, d0next) x̂0next .+= ŵ if estim.direct From 3ee8533982fefd3301b3307d16e8ce1af62482c2 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 31 Oct 2026 12:18:22 -0400 Subject: [PATCH 31/56] test: update `SimModel` tests --- test/1_test_sim_model.jl | 32 ++++++++++++++++++-------------- 1 file changed, 18 insertions(+), 14 deletions(-) diff --git a/test/1_test_sim_model.jl b/test/1_test_sim_model.jl index c8d6a1fb8..17382c4f8 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 From 9ee3510752b7c4cbb071cce110a1887204560b57 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 31 Oct 2026 12:38:30 -0400 Subject: [PATCH 32/56] debug: `ExtendedKalmanFilter` argument order in Jacobian --- src/estimator/kalman.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index c8725aaba..5bd5eaa08 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -1121,7 +1121,7 @@ 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, û0next, k̄, ûi, di, 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 From a746642ed0163883407133588ff1d9273ebfc06a Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 31 Oct 2026 12:59:13 -0400 Subject: [PATCH 33/56] changed: using buffers for D horizons --- src/controller/transcription.jl | 6 +++--- src/estimator/mhe/transcription.jl | 23 ++++++++++++++--------- 2 files changed, 17 insertions(+), 12 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 7ff6db0b0..73feef3c5 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1332,7 +1332,7 @@ function predict!( nx̂, Hp = mpc.estim.nx̂, mpc.Hp h = transcription.h D̂0 = mpc.D̂0 - Ûi, D̂i = similar(Û0), similar(D̂0) # TODO: DELETE THIS AND ADD NEW ARGUMENTS + Ûi, D̂i = similar(Û0), mpc.buffer.D̂ # TODO: DELETE THIS AND ADD NEW ARGUMENTS x̂0 = @views mpc.estim.x̂0[1:nx̂] d̂0 = @views mpc.d0[1:nd] for j=1:Hp @@ -1519,7 +1519,7 @@ function con_nonlinprogeq!( 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) - Ûi, D̂i = similar(Û0), similar(D̂0) # TODO: DELETE THIS AND ADD NEW ARGUMENTS + Ûi, D̂i = similar(Û0), mpc.buffer.D̂ # TODO: DELETE THIS AND ADD NEW ARGUMENTS @threadsif f_threads for j=1:Hp if j < 2 x̂d_Z̃ = @views mpc.estim.x̂0[1:nx] @@ -1693,7 +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Ā)] - Ûi, D̂i = similar(Û0), similar(D̂0) # TODO: DELETE THIS AND ADD NEW ARGUMENTS + Ûi, D̂i = similar(Û0), mpc.buffer.D̂ # TODO: DELETE THIS AND ADD NEW ARGUMENTS Û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Â)] diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 9612fcf54..e5a2399df 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1511,7 +1511,7 @@ function predict_mhe!( nu, nd, ny, nk̄ = model.nu, model.nd, model.ny, model.nk̄ nx̂, nŵ, nym, Nk = estim.nx̂, estim.nx̂, estim.nym, estim.Nk[] h = transcription.h - Ui, Di = similar(Û0), similar(estim.D0) # TODO: DELETE THIS AND ADD NEW ARGUMENTS + Ui, Di = similar(Û0), estim.buffer.D # TODO: DELETE THIS AND ADD NEW ARGUMENTS 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 @@ -1740,20 +1740,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) + Ûi, Di = similar(Û0), estim.buffer.D # TODO: DELETE THIS AND ADD NEW ARGUMENTS @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 estim.D0[(1+nd*j + i_d0arr):(nd*(j+1) + i_d0arr)] # (j < Nk) ? : 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 From 6a4c0cf89f19c8766b50f1cc0c8dbe11d22a24f7 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 31 Oct 2026 15:18:10 -0400 Subject: [PATCH 34/56] debug: MHE measured dist. exception for `k+1` --- src/estimator/mhe/transcription.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index e5a2399df..396c2b691 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1756,7 +1756,7 @@ function con_nonlinprogeq_mhe!( 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 estim.D0[(1+nd*j + i_d0arr):(nd*(j+1) + i_d0arr)] # (j < Nk) ? : d0 + 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 From 7792ec9db92d4104dbaf3f6d87054e8f4a61cd18 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 31 Oct 2026 15:33:28 -0400 Subject: [PATCH 35/56] test: looser tolerance for MHE vs KF This is expected since now `d` are FOH discretized in MHE but not in EKF, so a small differences in the results. I may add the support for FOH in EKF later, but not super useful in practice since non-causal. --- test/2_test_state_estim.jl | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index d99cf597d..11e002395 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -2062,8 +2062,8 @@ end updatestate!(ukf, [11, 50], y, [25]) updatestate!(ekf, [11, 50], y, [25]) end - @test X̂_mhe ≈ X̂_ukf atol=1e-3 rtol=1e-3 - @test X̂_mhe ≈ X̂_ekf atol=1e-3 rtol=1e-3 + @test X̂_mhe ≈ X̂_ukf atol=1e-2 rtol=1e-2 + @test X̂_mhe ≈ X̂_ekf atol=1e-2 rtol=1e-2 ukf = UnscentedKalmanFilter(nonlinmodel, nint_ym=0, direct=true) ekf = ExtendedKalmanFilter(nonlinmodel, nint_ym=0, direct=true) @@ -2089,8 +2089,8 @@ end updatestate!(ukf, [11, 50], y, [25]) updatestate!(ekf, [11, 50], y, [25]) end - @test X̂_mhe ≈ X̂_ukf atol=1e-3 rtol=1e-3 - @test X̂_mhe ≈ X̂_ekf atol=1e-3 rtol=1e-3 + @test X̂_mhe ≈ X̂_ukf atol=1e-2 rtol=1e-2 + @test X̂_mhe ≈ X̂_ekf atol=1e-2 rtol=1e-2 end @testitem "MHE LinModel v.s. NonLinModel" setup=[SetupMPCtests] begin From 2a2f2754b912ecc4fdbc4b7138b16e88958be104 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 31 Oct 2026 16:38:10 -0400 Subject: [PATCH 36/56] test: revert tolerances and remove the effect of `d` --- test/2_test_state_estim.jl | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index 11e002395..38dfae827 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -2062,8 +2062,8 @@ end updatestate!(ukf, [11, 50], y, [25]) updatestate!(ekf, [11, 50], y, [25]) end - @test X̂_mhe ≈ X̂_ukf atol=1e-2 rtol=1e-2 - @test X̂_mhe ≈ X̂_ekf atol=1e-2 rtol=1e-2 + @test X̂_mhe ≈ X̂_ukf atol=1e-3 rtol=1e-3 + @test X̂_mhe ≈ X̂_ekf atol=1e-3 rtol=1e-3 ukf = UnscentedKalmanFilter(nonlinmodel, nint_ym=0, direct=true) ekf = ExtendedKalmanFilter(nonlinmodel, nint_ym=0, direct=true) @@ -2079,18 +2079,18 @@ end X̂_ekf = zeros(4, 6) for i in 1:6 y = [50,31] + randn(2) - x̂_mhe = preparestate!(mhe, y, [25]) - x̂_ukf = preparestate!(ukf, y, [25]) - x̂_ekf = preparestate!(ekf, y, [25]) + x̂_mhe = preparestate!(mhe, y, [20]) + x̂_ukf = preparestate!(ukf, y, [20]) + x̂_ekf = preparestate!(ekf, y, [20]) X̂_mhe[:,i] = x̂_mhe X̂_ukf[:,i] = x̂_ukf X̂_ekf[:,i] = x̂_ekf - updatestate!(mhe, [11, 50], y, [25]) - updatestate!(ukf, [11, 50], y, [25]) - updatestate!(ekf, [11, 50], y, [25]) + updatestate!(mhe, [11, 50], y, [20]) + updatestate!(ukf, [11, 50], y, [20]) + updatestate!(ekf, [11, 50], y, [20]) end - @test X̂_mhe ≈ X̂_ukf atol=1e-2 rtol=1e-2 - @test X̂_mhe ≈ X̂_ekf atol=1e-2 rtol=1e-2 + @test X̂_mhe ≈ X̂_ukf atol=1e-3 rtol=1e-3 + @test X̂_mhe ≈ X̂_ekf atol=1e-3 rtol=1e-3 end @testitem "MHE LinModel v.s. NonLinModel" setup=[SetupMPCtests] begin From 98ad8e083802520365aff95397a17a21a4902b80 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 31 Oct 2026 16:53:10 -0400 Subject: [PATCH 37/56] doc: `FOH` meas. disturbance in pH neutralization example --- docs/src/manual/nonlinmpc2.md | 25 ++++++++++++++++--------- 1 file changed, 16 insertions(+), 9 deletions(-) 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) From d66fd0ac08b4792f82bad381c48fe7df41ed0d1c Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 1 Nov 2026 16:25:48 -0500 Subject: [PATCH 38/56] test: debug test --- test/3_test_predictive_control.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index ad00956dd..680600c8a 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -1038,7 +1038,7 @@ end nonlinmodel2 = NonLinModel{Float32}(f, h, 3000.0, 1, 2, 1, 1, solver=nothing, p=linmodel2) nmpc7 = NonLinMPC(nonlinmodel2, Hp=10) y = similar(nonlinmodel2.yop) - ModelPredictiveControl.h!(y, nonlinmodel2, Float32[0,0], Float32[0], nonlinmodel2.p) + ModelPredictiveControl.h!(y, nonlinmodel2, Float32[0,0], Float32[0]) preparestate!(nmpc7, [0], [0]) @test moveinput!(nmpc7, [0], [0]) ≈ [0.0] atol=5e-2 From 179ce6aca8a9e538412fe263dd0412b28ada2c12 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 1 Nov 2026 16:37:05 -0500 Subject: [PATCH 39/56] doc: minor correction --- src/sim_model.jl | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/sim_model.jl b/src/sim_model.jl index bcb9ebaf1..e1eb593c9 100644 --- a/src/sim_model.jl +++ b/src/sim_model.jl @@ -259,10 +259,10 @@ Update `model.x0` states with current inputs `u` and meas. dist. `d` for the nex 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 (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. +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 From abacf34631e682c85ad1f206f3b4e418bb8f1411 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 1 Nov 2026 16:44:56 -0500 Subject: [PATCH 40/56] test: call `updatestate!` with `unext` kwarg --- test/1_test_sim_model.jl | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/test/1_test_sim_model.jl b/test/1_test_sim_model.jl index 17382c4f8..2c4f65be9 100644 --- a/test/1_test_sim_model.jl +++ b/test/1_test_sim_model.jl @@ -573,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 From cc0835a2f41752edee445b20ec8461c1fa97130c Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 1 Nov 2026 17:23:18 -0500 Subject: [PATCH 41/56] test: new `SingleShooting(1)` and `MultipleShooting(1)` tests --- test/2_test_state_estim.jl | 33 ++++++++++++++++++++++++++++++ test/3_test_predictive_control.jl | 34 +++++++++++++++++++++++++++++++ 2 files changed, 67 insertions(+) diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index 38dfae827..75e12dea7 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -1348,6 +1348,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 + Ts, p = 100.0, [0.01] + model = NonLinModel(f!, h!, Ts, 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 +1394,23 @@ end info = getinfo(mhe3) # test getinfo when Nk Date: Sun, 1 Nov 2026 19:47:23 -0500 Subject: [PATCH 42/56] =?UTF-8?q?debug:=20do=20not=20use=20`buffer.x=CC=82?= =?UTF-8?q?`=20in=20`getinfo(mhe)`?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- 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 88c5e9431..7415fd52b 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -152,8 +152,8 @@ 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̄, K = Vector{NT}(undef, nx̂), Vector{NT}(undef, nx̂), 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̃) From db678f4d52b9e931b05e06cdae416d49fcf51ce4 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 2 Nov 2026 09:34:17 -0500 Subject: [PATCH 43/56] test: debug variable name clash --- test/2_test_state_estim.jl | 4 ++-- test/3_test_predictive_control.jl | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index 75e12dea7..417505563 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -1355,8 +1355,8 @@ end function h!(y, x, d, _ ) y[] = x[] + 0.1 * d[] end - Ts, p = 100.0, [0.01] - model = NonLinModel(f!, h!, Ts, 1, 1, 1, 1; p) + 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]) diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index d3ff32ee9..10dd7e52e 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -1069,8 +1069,8 @@ end function h!(y, x, d, _ ) y[] = x[] + 0.1 * d[] end - Ts, p = 100.0, [0.01] - model = NonLinModel(f!, h!, Ts, 1, 1, 1, 1; p) + p = [0.01] + model = NonLinModel(f!, h!, 100.0, 1, 1, 1, 1; p) transcription = SingleShooting(1) nmpc_foh = NonLinMPC(model; Hp=5, Hc=1, Nwt=[0], transcription) preparestate!(nmpc_foh, [0.0], [0.0]) From 6cf8ab26c62b559dbc7a4823e86f18912d917130 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 2 Nov 2026 10:33:56 -0500 Subject: [PATCH 44/56] debug: do not use `buffer.x` in `getinfo(model)` The buffer is used in the returned value of `updatestate!`. Using it in `getinfo` would mutate the output argument of `updatestate!` unexpectedly! --- src/model/nonlinmodeldae.jl | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/model/nonlinmodeldae.jl b/src/model/nonlinmodeldae.jl index 2fbebb6b4..d006432b6 100644 --- a/src/model/nonlinmodeldae.jl +++ b/src/model/nonlinmodeldae.jl @@ -948,7 +948,8 @@ function getinfo(model::NonLinModelDAE{NT}) where NT<:Real 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 From 49794786179e839c818ad1fbb1e7c707b343f036 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 2 Nov 2026 11:56:30 -0500 Subject: [PATCH 45/56] test: avoid clashes on `Ts` variable name --- test/2_test_state_estim.jl | 16 ++++++++-------- test/3_test_predictive_control.jl | 16 ++++++++-------- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index 417505563..c96cce1d9 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -1079,8 +1079,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) @@ -1402,8 +1402,8 @@ end function h!(y, x, d, _ ) y[] = x[] + 0.1 * d[] end - Ts, p = 100.0, [0.01] - model = NonLinModel(f!, h!, Ts, 1, 1, 1, 1; p) + p = [0.01] + model = NonLinModel(f!, h!, 100.0, 1, 1, 1, 1; p) transcription = MultipleShooting(1) mhe_foh = MovingHorizonEstimator(model; He=3, direct=false, transcription) preparestate!(mhe_foh, [0.0], [0.0]) @@ -1497,9 +1497,9 @@ end function h!(y, x, a, d, _ ) y[] = 2 * x[] + a[] + 0.1 * d[] end - Ts, p = 100.0, [0.01] + p = [0.01] as_0, xs_0 = [-1.0], [1.0] - dae = NonLinModelDAE(fq!, h!, Ts, 1, 1, 1, 1, 1; p, xs_0, as_0) + dae = NonLinModelDAE(fq!, h!, 100.0, 1, 1, 1, 1, 1; p, xs_0, as_0) transcription = TrapezoidalCollocation(f_threads=true, h_threads=false) mhe = MovingHorizonEstimator(dae; He=2, transcription, hessian=true) @@ -1551,9 +1551,9 @@ end function h!(y, x, a, d, _ ) y[] = 2 * x[] + a[] + 0.1 * d[] end - Ts, p = 100.0, [0.01] + p = [0.01] as_0, xs_0 = [-0.01], [0.01] - dae = NonLinModelDAE(fq!, h!, Ts, 1, 1, 1, 1, 1; p, xs_0, as_0) + dae = NonLinModelDAE(fq!, h!, 100.0, 1, 1, 1, 1, 1; p, xs_0, as_0) no, roots = 3, :gausslegendre transcription = OrthogonalCollocation(0, no; roots, f_threads=true, h_threads=false) diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index 10dd7e52e..883c1c768 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -918,8 +918,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() nmpc = NonLinMPC(dae; He=3, Hp=3, transcription) @@ -1115,8 +1115,8 @@ end function h!(y, x, d, _ ) y[] = x[] + 0.1 * d[] end - Ts, p = 100.0, [0.01] - model = NonLinModel(f!, h!, Ts, 1, 1, 1, 1; p) + p = [0.01] + model = NonLinModel(f!, h!, 100.0, 1, 1, 1, 1; p) transcription = MultipleShooting(1) nmpc_foh = NonLinMPC(model; Hp=5, Hc=1, Nwt=[0], transcription) preparestate!(nmpc_foh, [0.0], [0.0]) @@ -1195,9 +1195,9 @@ end y[] = x[] + a[] + 0.1 * d[] end Kp = 2*0.2 # static gain for input, factor 2 since the output is 2*x (a==x) - Ts, p = 100.0, [0.01] + p = [0.01] as_0, xs_0 = [-1.0], [1.0] - dae = NonLinModelDAE(fq!, h!, Ts, 1, 1, 1, 1, 1; p, xs_0, as_0) + dae = NonLinModelDAE(fq!, h!, 100.0, 1, 1, 1, 1, 1; p, xs_0, as_0) transcription = TrapezoidalCollocation(f_threads=true, h_threads=false) nmpc = NonLinMPC(dae; Hp=100, Hc=1, He=2, Nwt=[0.0], transcription) @@ -1238,9 +1238,9 @@ end y[] = x[] + a[] + 0.1 * d[] end Kp = 2*0.2 # static gain for input, factor 2 since the output is 2*x (a==x) - Ts, p = 100.0, [0.01] + p = [0.01] as_0, xs_0 = [-1.0], [1.0] - dae = NonLinModelDAE(fq!, h!, Ts, 1, 1, 1, 1, 1; p, xs_0, as_0) + dae = NonLinModelDAE(fq!, h!, 100.0, 1, 1, 1, 1, 1; p, xs_0, as_0) no, roots = 3, :gausslegendre transcription = OrthogonalCollocation(0, no; roots, f_threads=true, h_threads=false) From 917db63963e271baa29b97232827e4d36d7876c8 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 2 Nov 2026 12:13:30 -0500 Subject: [PATCH 46/56] bench: `updatestate!` on the pendulum It allows tracking the allocations of `RungeKutta` on the benchmark suite. It should be 0 allocation. --- benchmark/1_bench_sim_model.jl | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/benchmark/1_bench_sim_model.jl b/benchmark/1_bench_sim_model.jl index 211542a41..283da9b1f 100644 --- a/benchmark/1_bench_sim_model.jl +++ b/benchmark/1_bench_sim_model.jl @@ -28,4 +28,13 @@ 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 +# TODO: Add case study benchmarks for SimModel + +u_pendulum = [0.5] +CASE_STUDY["NonLinModel"]["pendulum"]["updatestate!"] = + @benchmarkable( + updatestate!($pendulum_model, $u_pendulum); + ) + + +pendulum_model \ No newline at end of file From 698e31df24f562ca9e5e7ed71c89fff8ad4cb499 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 2 Nov 2026 12:17:28 -0500 Subject: [PATCH 47/56] bench: cleanup --- benchmark/1_bench_sim_model.jl | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/benchmark/1_bench_sim_model.jl b/benchmark/1_bench_sim_model.jl index 283da9b1f..e4dc60370 100644 --- a/benchmark/1_bench_sim_model.jl +++ b/benchmark/1_bench_sim_model.jl @@ -31,10 +31,7 @@ const CASE_MODEL = SUITE["SimModel"]["CASE STUDIES"] # TODO: Add case study benchmarks for SimModel u_pendulum = [0.5] -CASE_STUDY["NonLinModel"]["pendulum"]["updatestate!"] = +CASE_MODEL["NonLinModel"]["pendulum"]["updatestate!"] = @benchmarkable( updatestate!($pendulum_model, $u_pendulum); - ) - - -pendulum_model \ No newline at end of file + ) \ No newline at end of file From e0d56aa9e4fdff8682ef00097346b79747740bcd Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 2 Nov 2026 12:17:56 -0500 Subject: [PATCH 48/56] bench: cleanup 2 --- benchmark/1_bench_sim_model.jl | 1 - 1 file changed, 1 deletion(-) diff --git a/benchmark/1_bench_sim_model.jl b/benchmark/1_bench_sim_model.jl index e4dc60370..2d3e08e31 100644 --- a/benchmark/1_bench_sim_model.jl +++ b/benchmark/1_bench_sim_model.jl @@ -28,7 +28,6 @@ UNIT_MODEL["NonLinModel"]["linearize!"] = ## ----------------- CASE STUDIES --------------------------------------------------------- ## ---------------------------------------------------------------------------------------- const CASE_MODEL = SUITE["SimModel"]["CASE STUDIES"] -# TODO: Add case study benchmarks for SimModel u_pendulum = [0.5] CASE_MODEL["NonLinModel"]["pendulum"]["updatestate!"] = From 287433f91a032be910baced113b67c1a113b72f9 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 2 Nov 2026 12:37:56 -0500 Subject: [PATCH 49/56] added: special path for ZOH in `RungeKutta` --- src/model/solver.jl | 30 +++++++++++++++++------------- 1 file changed, 17 insertions(+), 13 deletions(-) diff --git a/src/model/solver.jl b/src/model/solver.jl index 716a3c70f..bb5b3b8f7 100644 --- a/src/model/solver.jl +++ b/src/model/solver.jl @@ -33,6 +33,8 @@ function solver_f!( ) 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)] @@ -42,21 +44,21 @@ function solver_f!( @. xcurr = x for i=1:supersample τ = (i-1.0)/supersample - @. ui = (1-τ)*u + (τ)*unext - @. di = (1-τ)*d + (τ)*dnext - f!(k1, xcurr, ui, di, p) + 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 τ = (i-0.5)/supersample - @. ui = (1-τ)*u + (τ)*unext - @. di = (1-τ)*d + (τ)*dnext - f!(k2, xnext, ui, di, p) + 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, ui, di, p) + f!(k3, xnext, u2and3, d2and3, p) @. xnext = xcurr + k3 * Ts_inner τ = i/supersample - @. ui = (1-τ)*u + (τ)*unext - @. di = (1-τ)*d + (τ)*dnext - f!(k4, xnext, ui, di, p) + 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 @@ -79,15 +81,17 @@ function solver_f!( ) 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 τ = (i-1)/supersample - @. ui = (1-τ)*u + (τ)*unext - @. di = (1-τ)*d + (τ)*dnext - f!(k1, xcurr, ui, di, p) + 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 From 112ceffcc2bfac4f65814cf4460ece065fcfc929 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 3 Nov 2026 08:11:54 -0500 Subject: [PATCH 50/56] =?UTF-8?q?added:=20remove=20`U=CC=82i`=20allocation?= =?UTF-8?q?=20in=20`NonLinMPC`?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/controller/execute.jl | 3 ++- src/controller/explicitmpc.jl | 2 +- src/controller/nonlinmpc.jl | 40 +++++++++++++++++---------------- src/controller/transcription.jl | 32 +++++++++++++------------- 4 files changed, 40 insertions(+), 37 deletions(-) diff --git a/src/controller/execute.jl b/src/controller/execute.jl index 4d39c46e9..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 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..f7a1ab66b 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -846,16 +846,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 +975,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 +1002,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 +1046,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 +1066,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 +1121,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 +1130,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 73feef3c5..a83b9ed13 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,7 +1324,7 @@ 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̄, + Ŷ0, x̂0end, X̂0, Û0, Ûi, K̄, mpc::PredictiveController, model::NonLinModel, transcription::SingleShooting, U0, _ ) @@ -1332,7 +1332,7 @@ function predict!( nx̂, Hp = mpc.estim.nx̂, mpc.Hp h = transcription.h D̂0 = mpc.D̂0 - Ûi, D̂i = similar(Û0), mpc.buffer.D̂ # TODO: DELETE THIS AND ADD NEW ARGUMENTS + 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 @@ -1362,7 +1362,7 @@ end @doc raw""" predict!( - Ŷ0, x̂0end, _ , _ , _ , + Ŷ0, x̂0end, _ ,_ , _ , _ , mpc::PredictiveController, model::SimModel, transcription::TranscriptionMethod, _ , Z̃ ) -> Ŷ0, x̂0end @@ -1378,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̃ ) @@ -1487,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 @@ -1508,7 +1508,7 @@ 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̃ ) @@ -1519,7 +1519,7 @@ function con_nonlinprogeq!( 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) - Ûi, D̂i = similar(Û0), mpc.buffer.D̂ # TODO: DELETE THIS AND ADD NEW ARGUMENTS + 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] @@ -1546,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 @@ -1578,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̃ ) @@ -1639,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 @@ -1675,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̃ ) @@ -1693,7 +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Ā)] - Ûi, D̂i = similar(Û0), mpc.buffer.D̂ # TODO: DELETE THIS AND ADD NEW ARGUMENTS + 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Â)] From 546a984bcef99f2d0b3b0b7348cff1987c40d5b1 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 3 Nov 2026 08:46:43 -0500 Subject: [PATCH 51/56] =?UTF-8?q?added:=20remove=20`U=CC=82i`=20allocation?= =?UTF-8?q?=20in=20`MovingHorizonEstimator`?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/controller/nonlinmpc.jl | 25 ++++++------ src/controller/transcription.jl | 2 +- src/estimator/mhe/construct.jl | 32 ++++++++-------- src/estimator/mhe/execute.jl | 21 +++++----- src/estimator/mhe/transcription.jl | 61 +++++++++++++++--------------- 5 files changed, 73 insertions(+), 68 deletions(-) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index f7a1ab66b..93fbeae9a 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,10 +658,10 @@ 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) + 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, K, X̂0, gc, g, geq, mpc, Z̃) gi .= @views g[i_g] return dot(λi, gi) @@ -677,8 +678,8 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real Cache(Û0), 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...) @@ -704,8 +705,8 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real Cache(Û0), 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...) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index a83b9ed13..5c8d016dd 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1748,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/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 7415fd52b..6755d3c5b 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.Ŷ - â0arr = buffer.â - x̂0arr, x̄, K = Vector{NT}(undef, nx̂), 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̃) @@ -665,15 +666,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 +906,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 +915,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 +924,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 396c2b691..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,14 +1506,14 @@ 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, + 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[] h = transcription.h - Ui, Di = similar(Û0), estim.buffer.D # TODO: DELETE THIS AND ADD NEW ARGUMENTS + 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 @@ -1520,7 +1522,7 @@ function predict_mhe!( ŵ = @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)] - ûi = @views Ui[(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 @@ -1564,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 @@ -1575,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̃ ) @@ -1706,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 @@ -1728,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̃ ) @@ -1740,7 +1742,7 @@ 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) - Ûi, Di = similar(Û0), estim.buffer.D # TODO: DELETE THIS AND ADD NEW ARGUMENTS + Di = estim.buffer.D @threadsif f_threads for j=1:Nk if j < 2 x̂d_Z̃ = @views x̂0arr[1:nx] @@ -1768,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 @@ -1806,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̃ ) @@ -1875,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 @@ -1913,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̃ ) @@ -1932,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̄] @@ -1973,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)] @@ -1994,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 From 3594ae874b3f4d15857e87223b996a3c98f8c74c Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 3 Nov 2026 08:48:20 -0500 Subject: [PATCH 52/56] =?UTF-8?q?debug:=20forgotten=20`U=CC=82i`=20cache?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/controller/nonlinmpc.jl | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 93fbeae9a..6604d3d8e 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -675,7 +675,7 @@ 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, Ûi, K, X̂0, gc, g) @@ -702,7 +702,7 @@ 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, Ûi, K, X̂0, gc, geq, g) From a926be14c8ba3d2407061df7a01293f93d219ada Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 3 Nov 2026 08:53:49 -0500 Subject: [PATCH 53/56] debug: idem --- src/estimator/mhe/execute.jl | 31 ++++++++++++++++--------------- 1 file changed, 16 insertions(+), 15 deletions(-) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 6755d3c5b..92b067f02 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -239,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) @@ -246,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 @@ -269,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 @@ -301,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) @@ -325,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 @@ -356,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 From 6966f88d5f3b81b234a92692201273ce35815d2a Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 3 Nov 2026 09:44:27 -0500 Subject: [PATCH 54/56] debug: idem --- src/controller/nonlinmpc.jl | 22 ++++++++++++++-------- 1 file changed, 14 insertions(+), 8 deletions(-) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 6604d3d8e..20e67f528 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -614,8 +614,8 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real 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, Û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̃) + 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) @@ -661,8 +661,10 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real 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, Ûi, 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 @@ -678,8 +680,10 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real Cache(Û0), Cache(Ûi), Cache(K), Cache(X̂0), Cache(gc), Cache(g) ) - 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̃) + 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...) @@ -705,8 +709,10 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real 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, Û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̃) + 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...) From 710a6e3e5dc1c3de9c4a363364f993dff2b7229b Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 3 Nov 2026 10:29:24 -0500 Subject: [PATCH 55/56] debug: `InternalModel` with `SingleShooting` now work ALso improve coverage of `InternalModel` --- src/estimator/internal_model.jl | 4 ++-- test/2_test_state_estim.jl | 20 ++++++++++++++++++++ 2 files changed, 22 insertions(+), 2 deletions(-) diff --git a/src/estimator/internal_model.jl b/src/estimator/internal_model.jl index acf268c68..3eb3d83f1 100644 --- a/src/estimator/internal_model.jl +++ b/src/estimator/internal_model.jl @@ -186,7 +186,7 @@ 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, 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 @@ -194,7 +194,7 @@ 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, 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 diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index c96cce1d9..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 From 2ce9c890865144e9733fbdec201636c652b6d880 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 3 Nov 2026 11:57:24 -0500 Subject: [PATCH 56/56] debug: rename eq. const. to `con_nonlinroots!` in `NonLinModelDAE` To avoid a clash creating method ambiguities --- src/model/nonlinmodeldae.jl | 14 +++++--------- 1 file changed, 5 insertions(+), 9 deletions(-) diff --git a/src/model/nonlinmodeldae.jl b/src/model/nonlinmodeldae.jl index d006432b6..51f7dffdf 100644 --- a/src/model/nonlinmodeldae.jl +++ b/src/model/nonlinmodeldae.jl @@ -687,27 +687,23 @@ 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 u0next, d0next = model.u0next_optim, model.d0next_optim - con_nonlinprogeq!( - geq, k̄, model, model.transcription, x0, u0, d0, u0next, d0next, Z - ) + con_nonlinroots!(geq, k̄, model, model.transcription, x0, u0, d0, u0next, d0next, Z) return nothing end """ - con_nonlinprogeq!( + 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!( +function con_nonlinroots!( geq, k̄dot, model::NonLinModelDAE, ::TrapezoidalCollocation, x0, u0, d0, u0next, d0next, Z ) - transcription = model.transcription nx, na = model.nx, model.na - h = transcription.h Ts = model.Ts u⁺, d⁺ = u0, d0 # h is ignored here, u0next & d0next values determine the hold order (like NonLinModel): @@ -722,14 +718,14 @@ function con_nonlinprogeq!( end """ - con_nonlinprogeq!( + 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!( +function con_nonlinroots!( geq, k̄dot, model::NonLinModelDAE, ::OrthogonalCollocation, x0, u0, d0, u0next, d0next, Z )