From 2ea58875bc8502fcd2f47d36fd0ed807e71551a6 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 24 Mar 2025 12:29:57 -0400 Subject: [PATCH 1/8] wip: simplifying allocations in `DiffSolver` --- src/model/solver.jl | 29 +++++++++++++++++++---------- src/sim_model.jl | 11 +++++++---- 2 files changed, 26 insertions(+), 14 deletions(-) diff --git a/src/model/solver.jl b/src/model/solver.jl index 4051abf41..ec8fed4d6 100644 --- a/src/model/solver.jl +++ b/src/model/solver.jl @@ -2,14 +2,22 @@ abstract type DiffSolver end "Empty solver for nonlinear discrete-time models." -struct EmptySolver <: DiffSolver end -get_solver_functions(::DataType, ::EmptySolver, f!, h!, _ ... ) = f!, h! +struct EmptySolver <: DiffSolver + ns::Int + EmptySolver() = new(0) +end + +function get_solver_functions(NT::DataType, ::EmptySolver, f!, h!, _ ... ) + f_solver!(xnext, _ , x, u, d, p) = f!(xnext, x, u, d, p) + return f!, h! +end function Base.show(io::IO, solver::EmptySolver) print(io, "Empty differential equation solver.") end struct RungeKutta <: DiffSolver + ns::Int order::Int supersample::Int function RungeKutta(order::Int, supersample::Int) @@ -22,7 +30,8 @@ struct RungeKutta <: DiffSolver if supersample < 1 throw(ArgumentError("supersample must be greater than 0")) end - return new(order, supersample) + ns = order # only true for order ≤ 4 + return new(ns, order, supersample) end end @@ -54,18 +63,18 @@ end "Get the f! function for the 4th order explicit Runge-Kutta solver." function get_rk4_function(NT, solver, fc!, Ts, nx, Nc) Ts_inner = Ts/solver.supersample - xcur_cache::DiffCache{Vector{NT}, Vector{NT}} = DiffCache(zeros(NT, nx), Nc) - k1_cache::DiffCache{Vector{NT}, Vector{NT}} = DiffCache(zeros(NT, nx), Nc) - k2_cache::DiffCache{Vector{NT}, Vector{NT}} = DiffCache(zeros(NT, nx), Nc) - k3_cache::DiffCache{Vector{NT}, Vector{NT}} = DiffCache(zeros(NT, nx), Nc) - k4_cache::DiffCache{Vector{NT}, Vector{NT}} = DiffCache(zeros(NT, nx), Nc) + xcur = zeros(NT, nx) + k1 = zeros(NT, nx) + k2 = zeros(NT, nx) + k3 = zeros(NT, nx) + k4 = zeros(NT, nx) f! = function rk4_solver!(xnext, x, u, d, p) CT = promote_type(eltype(x), eltype(u), eltype(d)) - xcur = get_tmp(xcur_cache, CT) + #=xcur = get_tmp(xcur_cache, CT) k1 = get_tmp(k1_cache, CT) k2 = get_tmp(k2_cache, CT) k3 = get_tmp(k3_cache, CT) - k4 = get_tmp(k4_cache, CT) + k4 = get_tmp(k4_cache, CT)=# xterm = xnext @. xcur = x for i=1:solver.supersample diff --git a/src/sim_model.jl b/src/sim_model.jl index d169745cd..93a4d00b0 100644 --- a/src/sim_model.jl +++ b/src/sim_model.jl @@ -25,23 +25,26 @@ struct SimModelBuffer{NT<:Real} x::Vector{NT} y::Vector{NT} d::Vector{NT} + K::Matrix{NT} empty::Vector{NT} end @doc raw""" - SimModelBuffer{NT}(nu::Int, nx::Int, ny::Int, nd::Int, linearization=nothing) + SimModelBuffer{NT}(nu::Int, nx::Int, ny::Int, nd::Int, ns::Int=0) Create a buffer for `SimModel` objects for inputs, states, outputs, and disturbances. -The buffer is used to store intermediate results during simulation without allocating. +The buffer is used to store intermediate results during simulation without allocating. The +argument `ns` is the number of stage of the [`DiffSolver`](@ref), when applicable. """ -function SimModelBuffer{NT}(nu::Int, nx::Int, ny::Int, nd::Int, ) where {NT<:Real} +function SimModelBuffer{NT}(nu::Int, nx::Int, ny::Int, nd::Int, ns::Int=0) where {NT<:Real} u = Vector{NT}(undef, nu) x = Vector{NT}(undef, nx) y = Vector{NT}(undef, ny) d = Vector{NT}(undef, nd) + K = Matrix{NT}(undef, nx, ns) empty = Vector{NT}(undef, 0) - return SimModelBuffer{NT}(u, x, y, d, empty) + return SimModelBuffer{NT}(u, x, y, d, K, empty) end From 8048dbe0e78ef7956abbe52ad3efb73a2a7da4c3 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 26 Mar 2025 12:28:05 -0400 Subject: [PATCH 2/8] wip: `RungeKutta(4)` now works without `DiffCache` --- src/model/nonlinmodel.jl | 4 +- src/model/solver.jl | 94 +++++++++++++++++++++------------------- src/sim_model.jl | 4 +- 3 files changed, 54 insertions(+), 48 deletions(-) diff --git a/src/model/nonlinmodel.jl b/src/model/nonlinmodel.jl index 62e43ea62..45dcbafa2 100644 --- a/src/model/nonlinmodel.jl +++ b/src/model/nonlinmodel.jl @@ -53,7 +53,7 @@ struct NonLinModel{ xname = ["\$x_{$i}\$" for i in 1:nx] x0 = zeros(NT, nx) t = zeros(NT, 1) - buffer = SimModelBuffer{NT}(nu, nx, ny, nd) + buffer = SimModelBuffer{NT}(nu, nx, ny, nd, solver.ns) return new{NT, F, H, PT, DS, JB, LF}( x0, f!, h!, @@ -263,7 +263,7 @@ Call [`linearize(model; x, u, d)`](@ref) and return the resulting linear model. LinModel(model::NonLinModel; kwargs...) = linearize(model; kwargs...) "Call `model.f!(xnext0, x0, u0, d0, p)` for [`NonLinModel`](@ref)." -f!(xnext0, model::NonLinModel, x0, u0, d0, p) = model.f!(xnext0, x0, u0, d0, p) +f!(xnext0, model::NonLinModel, x0, u0, d0, p) = model.f!(xnext0, model.buffer.K, x0, u0, d0, p) "Call `model.h!(y0, x0, d0, p)` for [`NonLinModel`](@ref)." h!(y0, model::NonLinModel, x0, d0, p) = model.h!(y0, x0, d0, p) diff --git a/src/model/solver.jl b/src/model/solver.jl index ec8fed4d6..27e1b8425 100644 --- a/src/model/solver.jl +++ b/src/model/solver.jl @@ -3,13 +3,28 @@ abstract type DiffSolver end "Empty solver for nonlinear discrete-time models." struct EmptySolver <: DiffSolver - ns::Int + ns::Int # number of stages EmptySolver() = new(0) end -function get_solver_functions(NT::DataType, ::EmptySolver, f!, h!, _ ... ) - f_solver!(xnext, _ , x, u, d, p) = f!(xnext, x, u, d, p) - return f!, h! +""" + get_solver_functions(NT::DataType, solver::EmptySolver, f!, h!, Ts, nu, nx, ny, nd) + +Get `solver_f!` and `solver_h!` functions for the `EmptySolver` (discrete models). + +The functions should have the following signature: +``` + solver_f!(xnext, K, x, u, d, p) -> nothing + solver_h!(y, x, d, p) -> nothing +``` +in which `xnext`, `K` and `y` arguments should be mutated in-place. The `K` argument is +a vector of `nx*(solver.ns+1)` elements to store the solver intermediary stage values, +and also the current state value when `supersample ≠ 1`. +""" +function get_solver_functions(::DataType, ::EmptySolver, f!, h!, _ , _ , _ , _ ) + solver_f!(xnext, _ , x, u, d, p) = f!(xnext, x, u, d, p) + solver_h! = h! + return solver_f!, solver_h! end function Base.show(io::IO, solver::EmptySolver) @@ -17,9 +32,9 @@ function Base.show(io::IO, solver::EmptySolver) end struct RungeKutta <: DiffSolver - ns::Int - order::Int - supersample::Int + ns::Int # number of stages + order::Int # order of the method + supersample::Int # number of internal steps function RungeKutta(order::Int, supersample::Int) if order ≠ 4 && order ≠ 1 throw(ArgumentError("only 1st and 4th order Runge-Kutta is supported.")) @@ -30,7 +45,7 @@ struct RungeKutta <: DiffSolver if supersample < 1 throw(ArgumentError("supersample must be greater than 0")) end - ns = order # only true for order ≤ 4 + ns = order # only true for order ≤ 4 with RungeKutta return new(ns, order, supersample) end end @@ -46,59 +61,50 @@ This solver is allocation-free if the `f!` and `h!` functions do not allocate. """ RungeKutta(order::Int=4; supersample::Int=1) = RungeKutta(order, supersample) -"Get the `f!` and `h!` functions for the explicit Runge-Kutta solvers." -function get_solver_functions(NT::DataType, solver::RungeKutta, fc!, hc!, Ts, _ , nx, _ , _ ) +"Get `solve_f!` and `solver_h!` functions for the explicit Runge-Kutta solvers." +function get_solver_functions(NT::DataType, solver::RungeKutta, f!, h!, Ts, _ , nx, _ , _ ) Nc = nx + 2 - f! = if solver.order==4 - get_rk4_function(NT, solver, fc!, Ts, nx, Nc) + solver_f! = if solver.order==4 + get_rk4_function(NT, solver, f!, Ts, nx, Nc) elseif solver.order==1 - get_euler_function(NT, solver, fc!, Ts, nx, Nc) + get_euler_function(NT, solver, f!, Ts, nx, Nc) else throw(ArgumentError("only 1st and 4th order Runge-Kutta is supported.")) end - h! = hc! - return f!, h! + solver_h! = h! + return solver_f!, solver_h! end "Get the f! function for the 4th order explicit Runge-Kutta solver." -function get_rk4_function(NT, solver, fc!, Ts, nx, Nc) +function get_rk4_function(NT, solver, f!, Ts, nx, Nc) Ts_inner = Ts/solver.supersample - xcur = zeros(NT, nx) - k1 = zeros(NT, nx) - k2 = zeros(NT, nx) - k3 = zeros(NT, nx) - k4 = zeros(NT, nx) - f! = function rk4_solver!(xnext, x, u, d, p) - CT = promote_type(eltype(x), eltype(u), eltype(d)) - #=xcur = get_tmp(xcur_cache, CT) - k1 = get_tmp(k1_cache, CT) - k2 = get_tmp(k2_cache, CT) - k3 = get_tmp(k3_cache, CT) - k4 = get_tmp(k4_cache, CT)=# - xterm = xnext - @. xcur = x + function rk4_solver_f!(xnext, K, x, u, d, p) + xcurr = @views K[1:nx] + k1 = @views K[(1nx + 1):(2nx)] + k2 = @views K[(2nx + 1):(3nx)] + k3 = @views K[(3nx + 1):(4nx)] + k4 = @views K[(4nx + 1):(5nx)] + @. xcurr = x for i=1:solver.supersample - fc!(k1, xcur, u, d, p) - @. xterm = xcur + k1 * Ts_inner/2 - fc!(k2, xterm, u, d, p) - @. xterm = xcur + k2 * Ts_inner/2 - fc!(k3, xterm, u, d, p) - @. xterm = xcur + k3 * Ts_inner - fc!(k4, xterm, u, d, p) - @. xcur = xcur + (k1 + 2k2 + 2k3 + k4)*Ts_inner/6 + f!(k1, xcurr, u, d, p) + @. xnext = xcurr + k1 * Ts_inner/2 + f!(k2, xnext, u, d, p) + @. xnext = xcurr + k2 * Ts_inner/2 + f!(k3, xnext, u, d, p) + @. xnext = xcurr + k3 * Ts_inner + f!(k4, xnext, u, d, p) + @. xcurr = xcurr + (k1 + 2k2 + 2k3 + k4)*Ts_inner/6 end - @. xnext = xcur + @. xnext = xcurr return nothing end - return f! + return rk4_solver_f! end "Get the f! function for the explicit Euler solver." function get_euler_function(NT, solver, fc!, Ts, nx, Nc) Ts_inner = Ts/solver.supersample - xcur_cache::DiffCache{Vector{NT}, Vector{NT}} = DiffCache(zeros(NT, nx), Nc) - k_cache::DiffCache{Vector{NT}, Vector{NT}} = DiffCache(zeros(NT, nx), Nc) - f! = function euler_solver!(xnext, x, u, d, p) + function euler_solver_f!(xnext, x, u, d, p) CT = promote_type(eltype(x), eltype(u), eltype(d)) xcur = get_tmp(xcur_cache, CT) k = get_tmp(k_cache, CT) @@ -111,7 +117,7 @@ function get_euler_function(NT, solver, fc!, Ts, nx, Nc) @. xnext = xcur return nothing end - return f! + return euler_solver_f! end """ diff --git a/src/sim_model.jl b/src/sim_model.jl index 93a4d00b0..931c7005b 100644 --- a/src/sim_model.jl +++ b/src/sim_model.jl @@ -25,7 +25,7 @@ struct SimModelBuffer{NT<:Real} x::Vector{NT} y::Vector{NT} d::Vector{NT} - K::Matrix{NT} + K::Vector{NT} empty::Vector{NT} end @@ -42,7 +42,7 @@ function SimModelBuffer{NT}(nu::Int, nx::Int, ny::Int, nd::Int, ns::Int=0) where x = Vector{NT}(undef, nx) y = Vector{NT}(undef, ny) d = Vector{NT}(undef, nd) - K = Matrix{NT}(undef, nx, ns) + K = Vector{NT}(undef, nx*(ns+1)) # the "+1" is necessary because of super-sampling empty = Vector{NT}(undef, 0) return SimModelBuffer{NT}(u, x, y, d, K, empty) end From 2bbb4168420d430d51e86f3814c1a96f4549158e Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 27 Mar 2025 16:00:45 -0400 Subject: [PATCH 3/8] wip: simplifying allocations in `DiffSolvers` --- src/estimator/execute.jl | 37 ++++++++++----------- src/estimator/internal_model.jl | 15 ++++++--- src/model/linearization.jl | 50 ++++++++++++++++------------- src/model/linmodel.jl | 14 ++++---- src/model/nonlinmodel.jl | 29 ++++++++++------- src/model/solver.jl | 57 ++++++++++++++++----------------- src/sim_model.jl | 25 ++++++++------- 7 files changed, 120 insertions(+), 107 deletions(-) diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index 29b968806..d9fb030f2 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -18,7 +18,7 @@ function remove_op!(estim::StateEstimator, ym, d, u=nothing) end @doc raw""" - f̂!(x̂next0, û0, estim::StateEstimator, model::SimModel, x̂0, u0, d0) -> nothing + f̂!(x̂0next, û0, x0i, estim::StateEstimator, model::SimModel, x̂0, u0, d0) -> nothing Mutating state function ``\mathbf{f̂}`` of the augmented model. @@ -30,39 +30,40 @@ function returns the next state of the augmented model, defined as: \mathbf{ŷ_0}(k) &= \mathbf{ĥ}\Big(\mathbf{x̂_0}(k), \mathbf{d_0}(k)\Big) \end{aligned} ``` -where ``\mathbf{x̂_0}(k+1)`` is stored in `x̂next0` argument. The method mutates `x̂next0` and -`û0` in place, the latter stores the input vector of the augmented model -``\mathbf{u_0 + ŷ_{s_u}}``. The model parameter vector `model.p` is not included in the -function signature for conciseness. +where ``\mathbf{x̂_0}(k+1)`` is stored in `x̂0next` argument. The method mutates `x̂0next`, +`û0` and `x0i` in place. The argument `û0` is the input vector of the augmented model, +computed by ``\mathbf{û_0 = u_0 + ŷ_{s_u}}``. The argument `x0i` is used to store the +intermediate stage values of `model.solver` (when applicable). The model parameter vector +`model.p` is not included in the function signature for conciseness. """ -function f̂!(x̂next0, û0, estim::StateEstimator, model::SimModel, x̂0, u0, d0) - return f̂!(x̂next0, û0, model, estim.As, estim.Cs_u, x̂0, u0, d0) +function f̂!(x̂0next, û0, x0i, estim::StateEstimator, model::SimModel, x̂0, u0, d0) + return f̂!(x̂0next, û0, x0i, model, estim.As, estim.Cs_u, x̂0, u0, d0) end """ - f̂!(x̂next0, _ , 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 if `model` is a [`LinModel`](@ref). """ -function f̂!(x̂next0, _ , estim::StateEstimator, ::LinModel, x̂0, u0, d0) - mul!(x̂next0, estim.Â, x̂0) - mul!(x̂next0, estim.B̂u, u0, 1, 1) - mul!(x̂next0, estim.B̂d, d0, 1, 1) +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) return nothing end """ - f̂!(x̂next0, û0, model::SimModel, As, Cs_u, x̂0, u0, d0) + f̂!(x̂0next, û0, x0i, model::SimModel, As, Cs_u, x̂0, u0, d0) Same than [`f̂!`](@ref) for [`SimModel`](@ref) but without the `estim` argument. """ -function f̂!(x̂next0, û0, model::SimModel, As, Cs_u, x̂0, u0, d0) +function f̂!(x̂0next, û0, x0i, model::SimModel, As, Cs_u, 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̂d_next, x̂s_next = x̂next0[1:model.nx], x̂next0[model.nx+1:end] - mul!(û0, Cs_u, x̂s) + @views x̂d_next, x̂s_next = x̂0next[1:model.nx], x̂0next[model.nx+1:end] + mul!(û0, Cs_u, x̂s) # ŷs_u = Cs_u * x̂s û0 .+= u0 - f!(x̂d_next, model, x̂d, û0, d0, model.p) + f!(x̂d_next, x0i, model, x̂d, û0, d0, model.p) mul!(x̂s_next, As, x̂s) return nothing end @@ -96,7 +97,7 @@ function ĥ!(ŷ0, model::SimModel, Cs_y, 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) - mul!(ŷ0, Cs_y, x̂s, 1, 1) + mul!(ŷ0, Cs_y, x̂s, 1, 1) # ŷ0 = ŷ0 + Cs_y*x̂s return nothing end diff --git a/src/estimator/internal_model.jl b/src/estimator/internal_model.jl index 6b72c8673..b7c3d4545 100644 --- a/src/estimator/internal_model.jl +++ b/src/estimator/internal_model.jl @@ -168,20 +168,25 @@ function matrices_internalmodel(model::SimModel{NT}) where NT<:Real end @doc raw""" - f̂!(x̂next0, _ , estim::InternalModel, model::NonLinModel, x̂0, u0, d0) + f̂!(x̂0next, _ , x̂0i, estim::InternalModel, model::NonLinModel, x̂0, u0, d0) State function ``\mathbf{f̂}`` of [`InternalModel`](@ref) for [`NonLinModel`](@ref). -It calls `model.f!(x̂next0, x̂0, u0 ,d0, model.p)` since this estimator does not augment the states. +It calls `model.solver_f!(x̂0next, x̂0i, x̂0, u0 ,d0, model.p)` directly since this estimator +does not augment the states. """ -f̂!(x̂next0, _, ::InternalModel, model::NonLinModel, x̂0, u0, d0) = model.f!(x̂next0, x̂0, u0, d0, model.p) +function f̂!(x̂0next, _ , x̂0i, ::InternalModel, model::NonLinModel, x̂0, u0, d0) + return model.solver_f!(x̂0next, x̂0i, x̂0, u0, d0, model.p) +end @doc raw""" ĥ!(ŷ0, estim::InternalModel, model::NonLinModel, x̂0, d0) -Output function ``\mathbf{ĥ}`` of [`InternalModel`](@ref), it calls `model.h!`. +Output function ``\mathbf{ĥ}`` of [`InternalModel`](@ref), it calls `model.solver_h!`. """ -ĥ!(x̂next0, ::InternalModel, model::NonLinModel, x̂0, d0) = model.h!(x̂next0, x̂0, d0, model.p) +function ĥ!(ŷ0, ::InternalModel, model::NonLinModel, x̂0, d0) + return model.solver_h!(ŷ0, x̂0, d0, model.p) +end @doc raw""" diff --git a/src/model/linearization.jl b/src/model/linearization.jl index a18d58888..077d39626 100644 --- a/src/model/linearization.jl +++ b/src/model/linearization.jl @@ -1,7 +1,9 @@ """ - get_linearization_func(NT, f!, h!, nu, nx, ny, nd, p, backend) -> linfunc! + get_linearization_func( + NT, solver_f!, solver_h!, nu, nx, ny, nd, ns, p, solver, backend + ) -> linfunc! -Return the `linfunc!` function that computes the Jacobians of `f!` and `h!` functions. +Return `linfunc!` function that computes Jacobians of `solver_f!` and `solver_h!` functions. The function has the following signature: ``` @@ -11,31 +13,33 @@ and it should modifies in-place all the arguments before `backend`. The `backend is an `AbstractADType` object from `DifferentiationInterface`. The `cst_x`, `cst_u` and `cst_d` are `DifferentiationInterface.Constant` objects with the linearization points. """ -function get_linearization_func(NT, f!, h!, nu, nx, ny, nd, p, backend) - f_x!(xnext, x, u, d) = f!(xnext, x, u, d, p) - f_u!(xnext, u, x, d) = f!(xnext, x, u, d, p) - f_d!(xnext, d, x, u) = f!(xnext, x, u, d, p) - h_x!(y, x, d) = h!(y, x, d, p) - h_d!(y, d, x) = h!(y, x, d, p) +function get_linearization_func(NT, solver_f!, solver_h!, nu, nx, ny, nd, p, solver, backend) + f_x!(xnext, x, xi, u, d) = solver_f!(xnext, xi, x, u, d, p) + f_u!(xnext, u, xi, x, d) = solver_f!(xnext, xi, x, u, d, p) + f_d!(xnext, d, xi, x, u) = solver_f!(xnext, xi, x, u, d, p) + h_x!(y, x, d) = solver_h!(y, x, d, p) + h_d!(y, d, x) = solver_h!(y, x, d, p) strict = Val(true) xnext = zeros(NT, nx) y = zeros(NT, ny) x = zeros(NT, nx) u = zeros(NT, nu) d = zeros(NT, nd) + xi = zeros(NT, nx*(solver.ni+1)) + cache_xi = Cache(xi) cst_x = Constant(x) cst_u = Constant(u) cst_d = Constant(d) - A_prep = prepare_jacobian(f_x!, xnext, backend, x, cst_u, cst_d; strict) - Bu_prep = prepare_jacobian(f_u!, xnext, backend, u, cst_x, cst_d; strict) - Bd_prep = prepare_jacobian(f_d!, xnext, backend, d, cst_x, cst_u; 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) + A_prep = prepare_jacobian(f_x!, xnext, backend, x, cache_xi, cst_u, cst_d; strict) + Bu_prep = prepare_jacobian(f_u!, xnext, backend, u, cache_xi, cst_x, cst_d; strict) + Bd_prep = prepare_jacobian(f_d!, xnext, backend, d, cache_xi, cst_x, cst_u; 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, cst_u, cst_d) - jacobian!(f_u!, xnext, Bu, Bu_prep, backend, u, cst_x, cst_d) - jacobian!(f_d!, xnext, Bd, Bd_prep, backend, d, cst_x, cst_u) + jacobian!(f_x!, xnext, A, A_prep, backend, x, cache_xi, cst_u, cst_d) + jacobian!(f_u!, xnext, Bu, Bu_prep, backend, u, cache_xi, cst_x, cst_d) + jacobian!(f_d!, xnext, Bd, Bd_prep, backend, d, cache_xi, cst_x, cst_u) jacobian!(h_x!, y, C, C_prep, backend, x, cst_d) jacobian!(h_d!, y, Dd, Dd_prep, backend, d, cst_x) return nothing @@ -154,21 +158,21 @@ function linearize!( nonlinmodel = model buffer = nonlinmodel.buffer # --- remove the operating points of the nonlinear model (typically zeros) --- - x0, u0, d0 = buffer.x, buffer.u, buffer.d + x0, u0, d0, x0i = buffer.x, buffer.u, buffer.d, buffer.xi x0 .= x .- nonlinmodel.xop u0 .= u .- nonlinmodel.uop d0 .= d .- nonlinmodel.dop # --- compute the Jacobians at linearization points --- linearize_core!(linmodel, nonlinmodel, x0, u0, d0) # --- compute the nonlinear model output at operating points --- - xnext0, y0 = linmodel.buffer.x, linmodel.buffer.y + x0next, y0 = linmodel.buffer.x, linmodel.buffer.y h!(y0, nonlinmodel, x0, d0, model.p) y = y0 y .= y0 .+ nonlinmodel.yop # --- compute the nonlinear model next state at operating points --- - f!(xnext0, nonlinmodel, x0, u0, d0, model.p) - xnext = xnext0 - xnext .= xnext0 .+ nonlinmodel.fop .- nonlinmodel.xop + f!(x0next, x0i, nonlinmodel, x0, u0, d0, model.p) + xnext = x0next + xnext .= x0next .+ nonlinmodel.fop .- nonlinmodel.xop # --- modify the linear model operating points --- linmodel.uop .= u linmodel.yop .= y @@ -182,13 +186,13 @@ end "Call `linfunc!` function to compute the Jacobians of `model` at the linearization point." function linearize_core!(linmodel::LinModel, model::SimModel, x0, u0, d0) - xnext0, y0 = linmodel.buffer.x, linmodel.buffer.y + x0next, y0 = linmodel.buffer.x, linmodel.buffer.y A, Bu, C, Bd, Dd = linmodel.A, linmodel.Bu, linmodel.C, linmodel.Bd, linmodel.Dd cst_x = Constant(x0) cst_u = Constant(u0) cst_d = Constant(d0) backend = model.jacobian - model.linfunc!(xnext0, y0, A, Bu, C, Bd, Dd, backend, x0, u0, d0, cst_x, cst_u, cst_d) + model.linfunc!(x0next, y0, A, Bu, C, Bd, Dd, backend, x0, u0, d0, cst_x, cst_u, cst_d) return nothing end diff --git a/src/model/linmodel.jl b/src/model/linmodel.jl index 041ebeea4..f54c03c4c 100644 --- a/src/model/linmodel.jl +++ b/src/model/linmodel.jl @@ -258,20 +258,20 @@ function steadystate!(model::LinModel, u0, d0) end """ - f!(xnext0, model::LinModel, x0, u0, d0, p) -> nothing + f!(x0next, _ , model::LinModel, x0, u0, d0, _ ) -> nothing -Evaluate `xnext0 = A*x0 + Bu*u0 + Bd*d0` in-place when `model` is a [`LinModel`](@ref). +Evaluate `x0next = A*x0 + Bu*u0 + Bd*d0` in-place when `model` is a [`LinModel`](@ref). """ -function f!(xnext0, model::LinModel, x0, u0, d0, _ ) - mul!(xnext0, model.A, x0) - mul!(xnext0, model.Bu, u0, 1, 1) - mul!(xnext0, model.Bd, d0, 1, 1) +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) return nothing end """ - h!(y0, model::LinModel, x0, d0, p) -> nothing + h!(y0, model::LinModel, x0, d0, _ ) -> nothing Evaluate `y0 = C*x0 + Dd*d0` in-place when `model` is a [`LinModel`](@ref). """ diff --git a/src/model/nonlinmodel.jl b/src/model/nonlinmodel.jl index 45dcbafa2..e8b4f2748 100644 --- a/src/model/nonlinmodel.jl +++ b/src/model/nonlinmodel.jl @@ -8,8 +8,8 @@ struct NonLinModel{ LF<:Function } <: SimModel{NT} x0::Vector{NT} - f!::F - h!::H + solver_f!::F + solver_h!::H p::PT solver::DS Ts::NT @@ -31,7 +31,8 @@ struct NonLinModel{ linfunc!::LF buffer::SimModelBuffer{NT} function NonLinModel{NT}( - f!::F, h!::H, Ts, nu, nx, ny, nd, p::PT, solver::DS, jacobian::JB, linfunc!::LF + solver_f!::F, solver_h!::H, Ts, nu, nx, ny, nd, + p::PT, solver::DS, jacobian::JB, linfunc!::LF ) where { NT<:Real, F<:Function, @@ -53,10 +54,10 @@ struct NonLinModel{ xname = ["\$x_{$i}\$" for i in 1:nx] x0 = zeros(NT, nx) t = zeros(NT, 1) - buffer = SimModelBuffer{NT}(nu, nx, ny, nd, solver.ns) + buffer = SimModelBuffer{NT}(nu, nx, ny, nd, solver.ni) return new{NT, F, H, PT, DS, JB, LF}( x0, - f!, h!, + solver_f!, solver_h!, p, solver, Ts, t, @@ -172,9 +173,13 @@ function NonLinModel{NT}( ) where {NT<:Real} isnothing(solver) && (solver=EmptySolver()) f!, h! = get_mutating_functions(NT, f, h) - f!, h! = get_solver_functions(NT, solver, f!, h!, Ts, nu, nx, ny, nd) - linfunc! = get_linearization_func(NT, f!, h!, nu, nx, ny, nd, p, jacobian) - return NonLinModel{NT}(f!, h!, Ts, nu, nx, ny, nd, p, solver, jacobian, linfunc!) + solver_f!, solver_h! = get_solver_functions(NT, solver, f!, h!, Ts, nu, nx, ny, nd) + linfunc! = get_linearization_func( + NT, solver_f!, solver_h!, nu, nx, ny, nd, p, solver, jacobian + ) + return NonLinModel{NT}( + solver_f!, solver_h!, Ts, nu, nx, ny, nd, p, solver, jacobian, linfunc! + ) end function NonLinModel( @@ -262,11 +267,11 @@ Call [`linearize(model; x, u, d)`](@ref) and return the resulting linear model. """ LinModel(model::NonLinModel; kwargs...) = linearize(model; kwargs...) -"Call `model.f!(xnext0, x0, u0, d0, p)` for [`NonLinModel`](@ref)." -f!(xnext0, model::NonLinModel, x0, u0, d0, p) = model.f!(xnext0, model.buffer.K, x0, u0, d0, p) +"Call `model.solver_f!(x0next, x0i, x0, u0, d0, p)` for [`NonLinModel`](@ref)." +f!(x0next, x0i, model::NonLinModel, x0, u0, d0, p) = model.solver_f!(x0next, x0i, x0, u0, d0, p) -"Call `model.h!(y0, x0, d0, p)` for [`NonLinModel`](@ref)." -h!(y0, model::NonLinModel, x0, d0, p) = model.h!(y0, x0, d0, p) +"Call `model.solver_h!(y0, x0, d0, p)` for [`NonLinModel`](@ref)." +h!(y0, model::NonLinModel, x0, d0, p) = model.solver_h!(y0, x0, d0, p) detailstr(model::NonLinModel) = ", $(typeof(model.solver).name.name)($(model.solver.order)) solver" detailstr(::NonLinModel{<:Real, <:Function, <:Function, <:Any, <:EmptySolver}) = ", empty solver" \ No newline at end of file diff --git a/src/model/solver.jl b/src/model/solver.jl index 27e1b8425..51b25343e 100644 --- a/src/model/solver.jl +++ b/src/model/solver.jl @@ -3,8 +3,8 @@ abstract type DiffSolver end "Empty solver for nonlinear discrete-time models." struct EmptySolver <: DiffSolver - ns::Int # number of stages - EmptySolver() = new(0) + ni::Int # number of intermediate stages + EmptySolver() = new(-1) end """ @@ -14,12 +14,12 @@ Get `solver_f!` and `solver_h!` functions for the `EmptySolver` (discrete models The functions should have the following signature: ``` - solver_f!(xnext, K, x, u, d, p) -> nothing + solver_f!(xnext, xi, x, u, d, p) -> nothing solver_h!(y, x, d, p) -> nothing ``` -in which `xnext`, `K` and `y` arguments should be mutated in-place. The `K` argument is -a vector of `nx*(solver.ns+1)` elements to store the solver intermediary stage values, -and also the current state value when `supersample ≠ 1`. +in which `xnext`, `xi` and `y` arguments are mutated in-place. The `xi` argument is +a vector of `nx*(solver.ni+1)` elements to store the solver intermediate stage values (and +also the current state value for when `supersample ≠ 1`). """ function get_solver_functions(::DataType, ::EmptySolver, f!, h!, _ , _ , _ , _ ) solver_f!(xnext, _ , x, u, d, p) = f!(xnext, x, u, d, p) @@ -32,7 +32,7 @@ function Base.show(io::IO, solver::EmptySolver) end struct RungeKutta <: DiffSolver - ns::Int # number of stages + ni::Int # number of intermediate stages order::Int # order of the method supersample::Int # number of internal steps function RungeKutta(order::Int, supersample::Int) @@ -45,8 +45,8 @@ struct RungeKutta <: DiffSolver if supersample < 1 throw(ArgumentError("supersample must be greater than 0")) end - ns = order # only true for order ≤ 4 with RungeKutta - return new(ns, order, supersample) + ni = order # only true for order ≤ 4 with RungeKutta + return new(ni, order, supersample) end end @@ -63,11 +63,10 @@ RungeKutta(order::Int=4; supersample::Int=1) = RungeKutta(order, supersample) "Get `solve_f!` and `solver_h!` functions for the explicit Runge-Kutta solvers." function get_solver_functions(NT::DataType, solver::RungeKutta, f!, h!, Ts, _ , nx, _ , _ ) - Nc = nx + 2 - solver_f! = if solver.order==4 - get_rk4_function(NT, solver, f!, Ts, nx, Nc) - elseif solver.order==1 - get_euler_function(NT, solver, f!, Ts, nx, Nc) + solver_f! = if solver.order == 4 + get_rk4_function(NT, solver, f!, Ts, nx) + elseif solver.order == 1 + get_euler_function(NT, solver, f!, Ts, nx) else throw(ArgumentError("only 1st and 4th order Runge-Kutta is supported.")) end @@ -76,14 +75,14 @@ function get_solver_functions(NT::DataType, solver::RungeKutta, f!, h!, Ts, _ , end "Get the f! function for the 4th order explicit Runge-Kutta solver." -function get_rk4_function(NT, solver, f!, Ts, nx, Nc) +function get_rk4_function(NT, solver, f!, Ts, nx) Ts_inner = Ts/solver.supersample - function rk4_solver_f!(xnext, K, x, u, d, p) - xcurr = @views K[1:nx] - k1 = @views K[(1nx + 1):(2nx)] - k2 = @views K[(2nx + 1):(3nx)] - k3 = @views K[(3nx + 1):(4nx)] - k4 = @views K[(4nx + 1):(5nx)] + function rk4_solver_f!(xnext, xi, x, u, d, p) + xcurr = @views xi[1:nx] + k1 = @views xi[(1nx + 1):(2nx)] + k2 = @views xi[(2nx + 1):(3nx)] + k3 = @views xi[(3nx + 1):(4nx)] + k4 = @views xi[(4nx + 1):(5nx)] @. xcurr = x for i=1:solver.supersample f!(k1, xcurr, u, d, p) @@ -104,17 +103,15 @@ end "Get the f! function for the explicit Euler solver." function get_euler_function(NT, solver, fc!, Ts, nx, Nc) Ts_inner = Ts/solver.supersample - function euler_solver_f!(xnext, x, u, d, p) - CT = promote_type(eltype(x), eltype(u), eltype(d)) - xcur = get_tmp(xcur_cache, CT) - k = get_tmp(k_cache, CT) - xterm = xnext - @. xcur = x + function euler_solver_f!(xnext, xi, x, u, d, p) + xcurr = @views xi[1:nx] + k1 = @views xi[(1nx + 1):(2nx)] + @. xcurr = x for i=1:solver.supersample - fc!(k, xcur, u, d, p) - @. xcur = xcur + k * Ts_inner + fc!(k1, xcurr, u, d, p) + @. xcurr = xcurr + k1 * Ts_inner end - @. xnext = xcur + @. xnext = xcurr return nothing end return euler_solver_f! diff --git a/src/sim_model.jl b/src/sim_model.jl index 931c7005b..377dfe944 100644 --- a/src/sim_model.jl +++ b/src/sim_model.jl @@ -25,26 +25,27 @@ struct SimModelBuffer{NT<:Real} x::Vector{NT} y::Vector{NT} d::Vector{NT} - K::Vector{NT} + xi::Vector{NT} empty::Vector{NT} end @doc raw""" - SimModelBuffer{NT}(nu::Int, nx::Int, ny::Int, nd::Int, ns::Int=0) + SimModelBuffer{NT}(nu::Int, nx::Int, ny::Int, nd::Int, ni::Int=0) Create a buffer for `SimModel` objects for inputs, states, outputs, and disturbances. -The buffer is used to store intermediate results during simulation without allocating. The -argument `ns` is the number of stage of the [`DiffSolver`](@ref), when applicable. +The buffer is used to store temporary results during simulation without allocating. The +argument `ni` is the number of intermediate stage of the [`DiffSolver`](@ref), when +applicable. """ -function SimModelBuffer{NT}(nu::Int, nx::Int, ny::Int, nd::Int, ns::Int=0) where {NT<:Real} +function SimModelBuffer{NT}(nu::Int, nx::Int, ny::Int, nd::Int, ni::Int=0) where {NT<:Real} u = Vector{NT}(undef, nu) x = Vector{NT}(undef, nx) y = Vector{NT}(undef, ny) d = Vector{NT}(undef, nd) - K = Vector{NT}(undef, nx*(ns+1)) # the "+1" is necessary because of super-sampling + xi = Vector{NT}(undef, nx*(ni+1)) # the "+1" is necessary because of super-sampling empty = Vector{NT}(undef, 0) - return SimModelBuffer{NT}(u, x, y, d, K, empty) + return SimModelBuffer{NT}(u, x, y, d, xi, empty) end @@ -248,13 +249,13 @@ julia> x = updatestate!(model, [1]) """ function updatestate!(model::SimModel{NT}, u, d=model.buffer.empty) where NT <: Real validate_args(model::SimModel, d, u) - u0, d0, xnext0 = model.buffer.u, model.buffer.d, model.buffer.x + u0, d0, x0next, x0i = model.buffer.u, model.buffer.d, model.buffer.x, model.buffer.xi u0 .= u .- model.uop d0 .= d .- model.dop - f!(xnext0, model, model.x0, u0, d0, model.p) - xnext0 .+= model.fop .- model.xop - model.x0 .= xnext0 - xnext = xnext0 + f!(x0next, x0i, model, model.x0, u0, d0, model.p) + x0next .+= model.fop .- model.xop + model.x0 .= x0next + xnext = x0next xnext .+= model.xop return xnext end From eedcade8a438145b8f321d14057a9702966d7ff4 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 29 Mar 2025 11:21:27 -0400 Subject: [PATCH 4/8] removed: `DiffCache`s in `RungeKutta` also renamed `nonlinmodel.f!` and `nonlinmodel.h!` to `nonlinmodel.solver_f!` and `nonlinmodel.solver_h!`, to be more explicit that it's the function for a specific `DiffSolver` instance --- src/controller/execute.jl | 17 ++++---- src/controller/nonlinmpc.jl | 46 +++++++++++++--------- src/controller/transcription.jl | 44 ++++++++++++--------- src/estimator/construct.jl | 8 ++-- src/estimator/internal_model.jl | 8 ++-- src/estimator/kalman.jl | 39 +++++++++++-------- src/estimator/luenberger.jl | 4 +- src/estimator/mhe/construct.jl | 28 +++++++------- src/estimator/mhe/execute.jl | 38 +++++++++--------- src/model/linmodel.jl | 4 +- src/model/nonlinmodel.jl | 22 +++++++++-- src/model/solver.jl | 4 +- test/1_test_sim_model.jl | 64 +++++++++++++++++-------------- test/3_test_predictive_control.jl | 2 +- 14 files changed, 187 insertions(+), 141 deletions(-) diff --git a/src/controller/execute.jl b/src/controller/execute.jl index d7cb78b81..19e43cb02 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -113,24 +113,27 @@ julia> round.(getinfo(mpc)[:Ŷ], digits=3) ``` """ function getinfo(mpc::PredictiveController{NT}) where NT<:Real - model, transcription = mpc.estim.model, mpc.transcription - nΔŨ = mpc.Hc*model.nu + mpc.nϵ + model, buffer, transcription = mpc.estim.model, mpc.buffer, mpc.transcription + nΔŨ, nXi = mpc.Hc*model.nu + mpc.nϵ, mpc.Hp*model.nxi nŶe, nUe = (mpc.Hp+1)*model.ny, (mpc.Hp+1)*model.nu nX̂0, nÛ0 = mpc.estim.nx̂*mpc.Hp, model.nu*mpc.Hp Z̃ = mpc.Z̃ info = Dict{Symbol, Any}() - ΔŨ = Vector{NT}(undef, nΔŨ) - x̂0end = similar(mpc.estim.x̂0) + ΔŨ = Vector{NT}(undef, nΔŨ) + x̂0end = similar(mpc.estim.x̂0) + X0i = Vector{NT}(undef, nXi) 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) - U, Ŷ = similar(mpc.Uop), similar(mpc.Yop) + 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, mpc, model, transcription, U0, Z̃) + Ŷ0, x̂0end = predict!(Ŷ0, x̂0end, X̂0, Û0, X0i, mpc, model, transcription, U0, Z̃) Ue, Ŷe = extended_vectors!(Ue, Ŷe, mpc, U0, Ŷ0) U .= U0 .+ mpc.Uop Ŷ .= Ŷ0 .+ mpc.Yop + D̂ .= mpc.D̂0 + mpc.Dop J = obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ) Ŷs = similar(mpc.Yop) predictstoch!(Ŷs, mpc, mpc.estim) @@ -140,7 +143,7 @@ function getinfo(mpc::PredictiveController{NT}) where NT<:Real info[:U] = U info[:u] = info[:U][1:model.nu] info[:d] = mpc.d0 + model.dop - info[:D̂] = mpc.D̂0 + mpc.Dop + info[:D̂] = D̂ info[:ŷ] = mpc.ŷ info[:Ŷ] = Ŷ info[:x̂end] = x̂0end + mpc.estim.x̂op diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index a60321367..f9be4ec4c 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -578,15 +578,17 @@ Inspired from: [User-defined operators with vector outputs](@extref JuMP User-de function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT<:Real # ----- common cache for Jfunc, gfuncs, geqfuncs called with floats ------------------- model = mpc.estim.model - nu, ny, nx̂, nϵ, Hp, Hc = model.nu, model.ny, mpc.estim.nx̂, mpc.nϵ, mpc.Hp, mpc.Hc + nu, ny, nx̂, nϵ, nxi = model.nu, model.ny, mpc.estim.nx̂, mpc.nϵ, model.nxi + Hp, Hc = mpc.Hp, mpc.Hc ng, nc, neq = length(mpc.con.i_g), mpc.con.nc, mpc.con.neq - nZ̃, nU, nŶ, nX̂ = length(mpc.Z̃), Hp*nu, Hp*ny, Hp*nx̂ + nZ̃, nU, nŶ, nX̂, nXi = length(mpc.Z̃), Hp*nu, Hp*ny, Hp*nx̂, Hp*nxi nΔŨ, nUe, nŶe = nu*Hc + nϵ, nU + nu, nŶ + ny strict = Val(true) myNaN = convert(JNT, NaN) # NaN to force update_simulations! at first call: Z̃ ::Vector{JNT} = fill(myNaN, nZ̃) ΔŨ::Vector{JNT} = zeros(JNT, nΔŨ) x̂0end::Vector{JNT} = zeros(JNT, nx̂) + X0i::Vector{JNT} = zeros(JNT, nXi) 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̂) @@ -596,18 +598,18 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT function Jfunc(Z̃arg::Vararg{T, N}) where {N, T<:Real} if isdifferent(Z̃arg, Z̃) Z̃ .= Z̃arg - update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq, mpc, Z̃) + update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X0i, X̂0, gc, g, geq, mpc, Z̃) end return obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ)::T end - function Jfunc!(Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq) - update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq, mpc, Z̃) + function Jfunc!(Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X0i, X̂0, gc, g, geq) + update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X0i, X̂0, gc, g, geq, mpc, Z̃) return obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ) end Z̃_∇J = fill(myNaN, nZ̃) ∇J_context = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), - Cache(Û0), Cache(X̂0), + Cache(Û0), Cache(X0i), Cache(X̂0), Cache(gc), Cache(g), Cache(geq), ) ∇J_prep = prepare_gradient(Jfunc!, mpc.gradient, Z̃_∇J, ∇J_context...; strict) @@ -631,19 +633,23 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT gfunc_i = function (Z̃arg::Vararg{T, N}) where {N, T<:Real} if isdifferent(Z̃arg, Z̃) Z̃ .= Z̃arg - update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq, mpc, Z̃) + update_predictions!( + ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X0i, X̂0, gc, g, geq, mpc, Z̃ + ) end return g[i]::T end gfuncs[i] = gfunc_i end - function gfunc!(g, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, geq) - return update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq, mpc, Z̃) + function gfunc!(g, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X0i, X̂0, gc, geq) + return update_predictions!( + ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X0i, X̂0, gc, g, geq, mpc, Z̃ + ) end Z̃_∇g = fill(myNaN, nZ̃) ∇g_context = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), - Cache(Û0), Cache(X̂0), + Cache(Û0), Cache(X0i), Cache(X̂0), Cache(gc), Cache(geq), ) # temporarily enable all the inequality constraints for sparsity detection: @@ -678,19 +684,23 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT geqfunc_i = function (Z̃arg::Vararg{T, N}) where {N, T<:Real} if isdifferent(Z̃arg, Z̃) Z̃ .= Z̃arg - update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq, mpc, Z̃) + update_predictions!( + ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X0i, X̂0, gc, g, geq, mpc, Z̃ + ) end return geq[i]::T end geqfuncs[i] = geqfunc_i end - function geqfunc!(geq, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g) - return update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq, mpc, Z̃) + function geqfunc!(geq, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X0i, X̂0, gc, g) + return update_predictions!( + ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X0i, X̂0, gc, g, geq, mpc, Z̃ + ) end Z̃_∇geq = fill(myNaN, nZ̃) ∇geq_context = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), - Cache(Û0), Cache(X̂0), + Cache(Û0), Cache(X0i), Cache(X̂0), Cache(gc), Cache(g) ) ∇geq_prep = prepare_jacobian(geqfunc!, geq, mpc.jacobian, Z̃_∇geq, ∇geq_context...; strict) @@ -716,7 +726,7 @@ end """ update_predictions!( - ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq, + ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X0i, X̂0, gc, g, geq, mpc::PredictiveController, Z̃ ) -> nothing @@ -725,17 +735,17 @@ 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, X̂0, gc, g, geq, mpc::PredictiveController, Z̃ + ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X0i, 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, mpc, model, transcription, U0, Z̃) + Ŷ0, x̂0end = predict!(Ŷ0, x̂0end, X̂0, Û0, X0i, mpc, model, transcription, U0, Z̃) Ue, Ŷe = extended_vectors!(Ue, Ŷe, mpc, U0, Ŷ0) ϵ = getϵ(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, mpc, model, transcription, U0, Z̃) + geq = con_nonlinprogeq!(geq, X̂0, Û0, X0i, mpc, model, transcription, U0, Z̃) return nothing end diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 1f5b45473..8258b714e 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1028,7 +1028,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 @@ -1040,7 +1040,7 @@ the terminal constraints applied on ``\mathbf{x̂}_{k-1}(k+H_p)``. The computati identical for any [`TranscriptionMethod`](@ref) if the model is linear. """ function predict!( - Ŷ0, x̂0end, _ , _ , + Ŷ0, x̂0end, _, _, _, mpc::PredictiveController, ::LinModel, ::TranscriptionMethod, _ , Z̃ ) @@ -1052,29 +1052,31 @@ end @doc raw""" predict!( - Ŷ0, x̂0end, X̂0, Û0, + Ŷ0, x̂0end, X̂0, Û0, X0i, 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` and `Û0` arguments. +The method mutates `Ŷ0`, `x̂0end`, `X̂0`, `Û0` and `X0i` arguments. """ function predict!( - Ŷ0, x̂0end, X̂0, Û0, + Ŷ0, x̂0end, X̂0, Û0, X0i, mpc::PredictiveController, model::NonLinModel, ::SingleShooting, U0, _ ) - nu, nx̂, ny, nd, Hp, Hc = model.nu, mpc.estim.nx̂, model.ny, model.nd, mpc.Hp, mpc.Hc + nu, nx̂, ny, nd, nxi = model.nu, mpc.estim.nx̂, model.ny, model.nd, model.nxi + Hp, Hc = mpc.Hp, mpc.Hc D̂0 = mpc.D̂0 x̂0 = @views mpc.estim.x̂0[1:nx̂] d0 = @views mpc.d0[1:nd] for j=1:Hp - u0 = @views U0[(1 + nu*(j-1)):(nu*j)] - û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] - x̂0next = @views X̂0[(1 + nx̂*(j-1)):(nx̂*j)] - f̂!(x̂0next, û0, mpc.estim, model, x̂0, u0, d0) + u0 = @views U0[(1 + nu*(j-1)):(nu*j)] + û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] + x0i = @views X0i[(1 + nxi*(j-1)):(nxi*j)] + x̂0next = @views X̂0[(1 + nx̂*(j-1)):(nx̂*j)] + f̂!(x̂0next, û0, x0i, mpc.estim, model, x̂0, u0, d0) x̂0next .+= mpc.estim.f̂op .- mpc.estim.x̂op x̂0 = @views X̂0[(1 + nx̂*(j-1)):(nx̂*j)] d0 = @views D̂0[(1 + nd*(j-1)):(nd*j)] @@ -1088,7 +1090,7 @@ end @doc raw""" predict!( - Ŷ0, x̂0end, _ , _ , + Ŷ0, x̂0end, _ , _ , _ , mpc::PredictiveController, model::NonLinModel, transcription::MultipleShooting, U0, Z̃ ) -> Ŷ0, x̂0end @@ -1098,7 +1100,7 @@ Compute vectors if `model` is a [`NonLinModel`](@ref) and for [`MultipleShooting The method mutates `Ŷ0` and `x̂0end` arguments. """ function predict!( - Ŷ0, x̂0end, _, _, + Ŷ0, x̂0end, _, _, _, mpc::PredictiveController, model::NonLinModel, ::MultipleShooting, U0, Z̃ ) @@ -1204,17 +1206,20 @@ end """ con_nonlinprogeq!( - geq, X̂0, Û0, mpc::PredictiveController, model::NonLinModel, ::MultipleShooting, U0, Z̃ + geq, X̂0, Û0, X0i + mpc::PredictiveController, model::NonLinModel, ::MultipleShooting, U0, Z̃ ) Nonlinear equality constrains for [`NonLinModel`](@ref) and [`MultipleShooting`](@ref). -The method mutates the `geq`, `X̂0` and `Û0` vectors in argument. +The method mutates the `geq`, `X̂0`, `Û0` and `X0i` vectors in argument. """ function con_nonlinprogeq!( - geq, X̂0, Û0, mpc::PredictiveController, model::NonLinModel, ::MultipleShooting, U0, Z̃ + geq, X̂0, Û0, X0i, + mpc::PredictiveController, model::NonLinModel, ::MultipleShooting, U0, Z̃ ) - nx̂, nu, nd, Hp, Hc = mpc.estim.nx̂, model.nu, model.nd, mpc.Hp, mpc.Hc + nu, nx̂, ny, nd, nxi = model.nu, mpc.estim.nx̂, model.ny, model.nd, model.nxi + Hp, Hc = mpc.Hp, mpc.Hc nΔU, nX̂ = nu*Hc, nx̂*Hp D̂0 = mpc.D̂0 X̂0_Z̃ = @views Z̃[(nΔU+1):(nΔU+nX̂)] @@ -1225,7 +1230,8 @@ function con_nonlinprogeq!( u0 = @views U0[(1 + nu*(j-1)):(nu*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, mpc.estim, model, x̂0, u0, d0) + x0i = @views X0i[(1 + nxi*(j-1)):(nxi*j)] + f̂!(x̂0next, û0, x0i, mpc.estim, model, x̂0, u0, d0) x̂0next .+= mpc.estim.f̂op .- mpc.estim.x̂op x̂0next_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*j)] ŝnext = @views geq[(1 + nx̂*(j-1)):(nx̂*j)] @@ -1235,5 +1241,5 @@ function con_nonlinprogeq!( end return geq end -con_nonlinprogeq!(geq,_,_,::PredictiveController,::NonLinModel,::SingleShooting, _,_) = geq -con_nonlinprogeq!(geq,_,_,::PredictiveController,::LinModel,::TranscriptionMethod,_,_) = geq \ No newline at end of file +con_nonlinprogeq!(geq,_,_,_,::PredictiveController,::NonLinModel,::SingleShooting, _,_)=geq +con_nonlinprogeq!(geq,_,_,_,::PredictiveController,::LinModel,::TranscriptionMethod,_,_)=geq \ No newline at end of file diff --git a/src/estimator/construct.jl b/src/estimator/construct.jl index d46402f26..7d3f1d907 100644 --- a/src/estimator/construct.jl +++ b/src/estimator/construct.jl @@ -1,6 +1,7 @@ struct StateEstimatorBuffer{NT<:Real} u ::Vector{NT} û ::Vector{NT} + xi::Vector{NT} x̂ ::Vector{NT} P̂ ::Matrix{NT} Q̂ ::Matrix{NT} @@ -13,17 +14,18 @@ struct StateEstimatorBuffer{NT<:Real} end @doc raw""" - StateEstimatorBuffer{NT}(nu::Int, nx̂::Int, nym::Int, ny::Int, nd::Int) + StateEstimatorBuffer{NT}(nu::Int, nx̂::Int, nym::Int, ny::Int, nd::Int, nxi::Int=0) Create a buffer for `StateEstimator` objects for estimated states and measured outputs. The buffer is used to store intermediate results during estimation without allocating. """ function StateEstimatorBuffer{NT}( - nu::Int, nx̂::Int, nym::Int, ny::Int, nd::Int + nu::Int, nx̂::Int, nym::Int, ny::Int, nd::Int, nxi::Int=0 ) where NT <: Real u = Vector{NT}(undef, nu) û = Vector{NT}(undef, nu) + xi = Vector{NT}(undef, nxi) x̂ = Vector{NT}(undef, nx̂) P̂ = Matrix{NT}(undef, nx̂, nx̂) Q̂ = Matrix{NT}(undef, nx̂, nx̂) @@ -33,7 +35,7 @@ function StateEstimatorBuffer{NT}( ŷ = Vector{NT}(undef, ny) d = Vector{NT}(undef, nd) empty = Vector{NT}(undef, 0) - return StateEstimatorBuffer{NT}(u, û, x̂, P̂, Q̂, R̂, K̂, ym, ŷ, d, empty) + return StateEstimatorBuffer{NT}(u, û, xi, x̂, P̂, Q̂, R̂, K̂, ym, ŷ, d, empty) end @doc raw""" diff --git a/src/estimator/internal_model.jl b/src/estimator/internal_model.jl index b7c3d4545..21399b0fe 100644 --- a/src/estimator/internal_model.jl +++ b/src/estimator/internal_model.jl @@ -32,7 +32,7 @@ struct InternalModel{NT<:Real, SM<:SimModel} <: StateEstimator{NT} function InternalModel{NT}( model::SM, i_ym, Asm, Bsm, Csm, Dsm ) where {NT<:Real, SM<:SimModel} - nu, ny, nd = model.nu, model.ny, model.nd + nu, ny, nd, nxi = model.nu, model.ny, model.nd, model.nxi nym, nyu = validate_ym(model, i_ym) validate_internalmodel(model, nym, Csm, Dsm) As, Bs, Cs, Ds = stoch_ym2y(model, i_ym, Asm, Bsm, Csm, Dsm) @@ -48,7 +48,7 @@ struct InternalModel{NT<:Real, SM<:SimModel} <: StateEstimator{NT} ŷs = zeros(NT, ny) direct = true # InternalModel always uses direct transmission from ym corrected = [false] - buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd) + buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nxi) return new{NT, SM}( model, lastu0, x̂op, f̂op, x̂0, x̂d, x̂s, ŷs, x̂snext, @@ -284,8 +284,8 @@ function update_estimate!(estim::InternalModel, _ , d0, u0) model = estim.model x̂d, x̂s, ŷs = estim.x̂d, estim.x̂s, estim.ŷs # -------------- deterministic model --------------------- - x̂dnext = estim.buffer.x̂ - f!(x̂dnext, model, x̂d, u0, d0, model.p) + x̂dnext, x0i = estim.buffer.x̂, estim.buffer.xi + f!(x̂dnext, x0i, model, x̂d, u0, d0, model.p) x̂d .= x̂dnext # this also updates estim.x̂0 (they are the same object) # --------------- stochastic model ----------------------- x̂snext = estim.x̂snext diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index a41235f01..2610f26f6 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -30,7 +30,7 @@ struct SteadyKalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT} function SteadyKalmanFilter{NT}( model::SM, i_ym, nint_u, nint_ym, Q̂, R̂; direct=true ) where {NT<:Real, SM<:LinModel} - nu, ny, nd = model.nu, model.ny, model.nd + nu, ny, nd, nxi = model.nu, model.ny, model.nd, model.nxi nym, nyu = validate_ym(model, i_ym) As, Cs_u, Cs_y, nint_u, nint_ym = init_estimstoch(model, i_ym, nint_u, nint_ym) nxs = size(As, 1) @@ -60,7 +60,7 @@ struct SteadyKalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT} x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)] Q̂, R̂ = Hermitian(Q̂, :L), Hermitian(R̂, :L) corrected = [false] - buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd) + buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nxi) return new{NT, SM}( model, lastu0, x̂op, f̂op, x̂0, @@ -311,7 +311,7 @@ struct KalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT} function KalmanFilter{NT}( model::SM, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct=true ) where {NT<:Real, SM<:LinModel} - nu, ny, nd = model.nu, model.ny, model.nd + nu, ny, nd, nxi = model.nu, model.ny, model.nd, model.nxi nym, nyu = validate_ym(model, i_ym) As, Cs_u, Cs_y, nint_u, nint_ym = init_estimstoch(model, i_ym, nint_u, nint_ym) nxs = size(As, 1) @@ -326,7 +326,7 @@ struct KalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT} P̂ = Hermitian(copy(P̂_0.data), :L) # copy on P̂_0.data necessary for Julia Nightly K̂ = zeros(NT, nx̂, nym) corrected = [false] - buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd) + buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nxi) return new{NT, SM}( model, lastu0, x̂op, f̂op, x̂0, P̂, @@ -534,7 +534,7 @@ struct UnscentedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT} function UnscentedKalmanFilter{NT}( model::SM, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, α, β, κ; direct=true ) where {NT<:Real, SM<:SimModel{NT}} - nu, ny, nd = model.nu, model.ny, model.nd + nu, ny, nd, nxi = model.nu, model.ny, model.nd, model.nxi nym, nyu = validate_ym(model, i_ym) As, Cs_u, Cs_y, nint_u, nint_ym = init_estimstoch(model, i_ym, nint_u, nint_ym) nxs = size(As, 1) @@ -553,7 +553,7 @@ struct UnscentedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT} X̂0, X̄0 = zeros(NT, nx̂, nσ), zeros(NT, nx̂, nσ) Ŷ0m, Ȳ0m = zeros(NT, nym, nσ), zeros(NT, nym, nσ) corrected = [false] - buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd) + buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nxi) return new{NT, SM}( model, lastu0, x̂op, f̂op, x̂0, P̂, @@ -843,7 +843,7 @@ function update_estimate!(estim::UnscentedKalmanFilter, y0m, d0, u0) x̂0corr, X̂0corr, P̂corr = estim.x̂0, estim.X̂0, estim.P̂ Q̂, nx̂ = estim.Q̂, estim.nx̂ γ, m̂, Ŝ = estim.γ, estim.m̂, estim.Ŝ - x̂0next, û0 = estim.buffer.x̂, estim.buffer.û + x̂0next, û0, x0i = estim.buffer.x̂, estim.buffer.û, estim.buffer.xi # in-place operations to reduce allocations: P̂corr_temp = Hermitian(estim.buffer.P̂, :L) P̂corr_temp .= P̂corr @@ -856,7 +856,7 @@ function update_estimate!(estim::UnscentedKalmanFilter, y0m, d0, u0) X̂0next = X̂0corr for j in axes(X̂0next, 2) @views x̂0corr .= X̂0corr[:, j] - @views f̂!(X̂0next[:, j], û0, estim, estim.model, x̂0corr, u0, d0) + @views f̂!(X̂0next[:, j], û0, x0i, estim, estim.model, x̂0corr, u0, d0) end x̂0next .= mul!(x̂0corr, X̂0next, m̂) X̄0next = estim.X̄0 @@ -917,7 +917,7 @@ struct ExtendedKalmanFilter{ function ExtendedKalmanFilter{NT}( model::SM, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; jacobian::JB, linfunc!::LF, direct=true ) where {NT<:Real, SM<:SimModel, JB<:AbstractADType, LF<:Function} - nu, ny, nd = model.nu, model.ny, model.nd + nu, ny, nd, nxi = model.nu, model.ny, model.nd, model.nxi nym, nyu = validate_ym(model, i_ym) As, Cs_u, Cs_y, nint_u, nint_ym = init_estimstoch(model, i_ym, nint_u, nint_ym) nxs = size(As, 1) @@ -934,7 +934,7 @@ struct ExtendedKalmanFilter{ F̂_û, F̂ = zeros(NT, nx̂+nu, nx̂), zeros(NT, nx̂, nx̂) Ĥ, Ĥm = zeros(NT, ny, nx̂), zeros(NT, nym, nx̂) corrected = [false] - buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd) + buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nxi) return new{NT, SM, JB, LF}( model, lastu0, x̂op, f̂op, x̂0, P̂, @@ -1075,21 +1075,26 @@ objects with the linearization points. """ function get_ekf_linfunc(NT, model, i_ym, nint_u, nint_ym, jacobian) As, Cs_u, Cs_y = init_estimstoch(model, i_ym, nint_u, nint_ym) - f̂_ekf!(x̂0next, x̂0, û0, u0, d0) = f̂!(x̂0next, û0, model, As, Cs_u, x̂0, u0, d0) + f̂_ekf!(x̂0next, x̂0, û0, x0i, u0, d0) = f̂!(x̂0next, û0, x0i, model, As, Cs_u, x̂0, u0, d0) ĥ_ekf!(ŷ0, x̂0, d0) = ĥ!(ŷ0, model, Cs_y, x̂0, d0) strict = Val(true) - nu, ny, nd = model.nu, model.ny, model.nd + nu, ny, nd, nxi = model.nu, model.ny, model.nd, model.nxi nx̂ = model.nx + size(As, 1) x̂0next = zeros(NT, nx̂) ŷ0 = zeros(NT, ny) x̂0 = zeros(NT, nx̂) - tmp_û0 = Cache(zeros(NT, nu)) + tmp_û0 = Cache(zeros(NT, nu)) + tmp_x0i = Cache(zeros(NT, nxi)) cst_u0 = Constant(zeros(NT, nu)) cst_d0 = Constant(zeros(NT, nd)) - F̂_prep = prepare_jacobian(f̂_ekf!, x̂0next, jacobian, x̂0, tmp_û0, cst_u0, cst_d0; strict) + F̂_prep = prepare_jacobian( + f̂_ekf!, x̂0next, jacobian, x̂0, tmp_û0, tmp_x0i, cst_u0, cst_d0; strict + ) Ĥ_prep = prepare_jacobian(ĥ_ekf!, ŷ0, jacobian, x̂0, cst_d0; strict) function linfunc!(x̂0next, ŷ0::Nothing, F̂, Ĥ::Nothing, backend, x̂0, cst_u0, cst_d0) - return jacobian!(f̂_ekf!, x̂0next, F̂, F̂_prep, backend, x̂0, tmp_û0, cst_u0, cst_d0) + return jacobian!( + f̂_ekf!, x̂0next, F̂, F̂_prep, backend, x̂0, tmp_û0, tmp_x0i, cst_u0, cst_d0 + ) end function linfunc!(x̂0next::Nothing, ŷ0, F̂::Nothing, Ĥ, backend, x̂0, _ , cst_d0) return jacobian!(ĥ_ekf!, ŷ0, Ĥ, Ĥ_prep, backend, x̂0, cst_d0) @@ -1243,9 +1248,9 @@ They predict the state `x̂` and covariance `P̂` with the same equations. See function predict_estimate_kf!(estim::Union{KalmanFilter, ExtendedKalmanFilter}, u0, d0, Â) x̂0corr, P̂corr = estim.x̂0, estim.P̂ Q̂ = estim.Q̂ - x̂0next, û0 = estim.buffer.x̂, estim.buffer.û + x̂0next, û0, x0i = estim.buffer.x̂, estim.buffer.û, estim.buffer.xi # in-place operations to reduce allocations: - f̂!(x̂0next, û0, estim, estim.model, x̂0corr, u0, d0) + f̂!(x̂0next, û0, x0i, estim, estim.model, x̂0corr, u0, d0) P̂corr_Âᵀ = estim.buffer.P̂ mul!(P̂corr_Âᵀ, P̂corr.data, Â') # the ".data" weirdly removes a type instability in mul! Â_P̂corr_Âᵀ = estim.buffer.Q̂ diff --git a/src/estimator/luenberger.jl b/src/estimator/luenberger.jl index 26f841e6e..024da7906 100644 --- a/src/estimator/luenberger.jl +++ b/src/estimator/luenberger.jl @@ -28,7 +28,7 @@ struct Luenberger{NT<:Real, SM<:LinModel} <: StateEstimator{NT} function Luenberger{NT, SM}( model, i_ym, nint_u, nint_ym, poles; direct=true ) where {NT<:Real, SM<:LinModel} - nu, ny, nd = model.nu, model.ny, model.nd + nu, ny, nd, nxi = model.nu, model.ny, model.nd, model.nxi nym, nyu = validate_ym(model, i_ym) validate_luenberger(model, nint_u, nint_ym, poles) As, Cs_u, Cs_y, nint_u, nint_ym = init_estimstoch(model, i_ym, nint_u, nint_ym) @@ -44,7 +44,7 @@ struct Luenberger{NT<:Real, SM<:LinModel} <: StateEstimator{NT} lastu0 = zeros(NT, nu) x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)] corrected = [false] - buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd) + buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nxi) return new{NT, SM}( model, lastu0, x̂op, f̂op, x̂0, diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index b6ae2dbbb..afeccc353 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -126,7 +126,7 @@ struct MovingHorizonEstimator{ JB<:AbstractADType, CE<:StateEstimator{NT} } - nu, ny, nd = model.nu, model.ny, model.nd + nu, ny, nd, nxi = model.nu, model.ny, model.nd, model.nxi He < 1 && throw(ArgumentError("Estimation horizon He should be ≥ 1")) Cwt < 0 && throw(ArgumentError("Cwt weight should be ≥ 0")) nym, nyu = validate_ym(model, i_ym) @@ -156,7 +156,7 @@ struct MovingHorizonEstimator{ nD0 = direct ? nd*(He+1) : nd*He U0, D0 = zeros(NT, nu*He), zeros(NT, nD0) Ŵ = zeros(NT, nx̂*He) - buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd) + buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nxi) P̂_0 = Hermitian(P̂_0, :L) Q̂, R̂ = Hermitian(Q̂, :L), Hermitian(R̂, :L) P̂_0 = Hermitian(P̂_0, :L) @@ -1329,12 +1329,14 @@ function get_optim_functions( ) where {JNT <: Real} # ---------- common cache for Jfunc, gfuncs called with floats ------------------------ model, con = estim.model, estim.con - nx̂, nym, nŷ, nu, nϵ, He = estim.nx̂, estim.nym, model.ny, model.nu, estim.nϵ, estim.He + nx̂, nym, nŷ, nu, nϵ, nxi = estim.nx̂, estim.nym, model.ny, model.nu, estim.nϵ, model.nxi + He = estim.He nV̂, nX̂, ng, nZ̃ = He*nym, He*nx̂, length(con.i_g), length(estim.Z̃) myNaN = convert(JNT, NaN) # NaN to force update_simulations! at first call strict = Val(true) Z̃::Vector{JNT} = fill(myNaN, nZ̃) V̂::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nV̂), zeros(JNT, nX̂) + x0i::Vector{JNT} = zeros(JNT, nxi) û0::Vector{JNT}, ŷ0::Vector{JNT} = zeros(JNT, nu), zeros(JNT, nŷ) g::Vector{JNT} = zeros(JNT, ng) x̄::Vector{JNT} = zeros(JNT, nx̂) @@ -1342,22 +1344,21 @@ function get_optim_functions( function Jfunc(Z̃arg::Vararg{T, N}) where {N, T<:Real} if isdifferent(Z̃arg, Z̃) Z̃ .= Z̃arg - update_prediction!(V̂, X̂0, û0, ŷ0, g, estim, Z̃) + update_prediction!(V̂, X̂0, û0, x0i, ŷ0, g, estim, Z̃) end return obj_nonlinprog!(x̄, estim, model, V̂, Z̃)::T end - function Jfunc!(Z̃, V̂, X̂0, û0, ŷ0, g, x̄) - update_prediction!(V̂, X̂0, û0, ŷ0, g, estim, Z̃) + function Jfunc!(Z̃, V̂, X̂0, û0, x0i, ŷ0, g, x̄) + update_prediction!(V̂, X̂0, û0, x0i, ŷ0, g, estim, Z̃) return obj_nonlinprog!(x̄, estim, model, V̂, Z̃) end Z̃_∇J = fill(myNaN, nZ̃) ∇J_context = ( - Cache(V̂), Cache(X̂0), - Cache(û0), Cache(ŷ0), + Cache(V̂), Cache(X̂0), Cache(û0), Cache(x0i), Cache(ŷ0), Cache(g), Cache(x̄), ) - # temporarily "fill" the estimation window for the preperation of the gradient: + # temporarily "fill" the estimation window for the preparation of the gradient: estim.Nk[] = He ∇J_prep = prepare_gradient(Jfunc!, estim.gradient, Z̃_∇J, ∇J_context...; strict) estim.Nk[] = 0 @@ -1376,19 +1377,18 @@ function get_optim_functions( gfunc_i = function (Z̃arg::Vararg{T, N}) where {N, T<:Real} if isdifferent(Z̃arg, Z̃) Z̃ .= Z̃arg - update_prediction!(V̂, X̂0, û0, ŷ0, g, estim, Z̃) + update_prediction!(V̂, X̂0, û0, x0i, ŷ0, g, estim, Z̃) end return g[i]::T end gfuncs[i] = gfunc_i end - function gfunc!(g, Z̃, V̂, X̂0, û0, ŷ0) - return update_prediction!(V̂, X̂0, û0, ŷ0, g, estim, Z̃) + function gfunc!(g, Z̃, V̂, X̂0, û0, x0i, ŷ0) + return update_prediction!(V̂, X̂0, û0, x0i, ŷ0, g, estim, Z̃) end Z̃_∇g = fill(myNaN, nZ̃) ∇g_context = ( - Cache(V̂), Cache(X̂0), - Cache(û0), Cache(ŷ0), + Cache(V̂), Cache(X̂0), Cache(û0), Cache(x0i), Cache(ŷ0), ) # temporarily enable all the inequality constraints for sparsity detection: estim.con.i_g .= true diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index d26db959b..8f352c7b8 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -104,14 +104,14 @@ julia> round.(getinfo(estim)[:Ŷ], digits=3) ``` """ function getinfo(estim::MovingHorizonEstimator{NT}) where NT<:Real - model, Nk = estim.model, estim.Nk[] + model, buffer, Nk = estim.model, estim.buffer, estim.Nk[] nu, ny, nd = model.nu, model.ny, model.nd nx̂, nym, nŵ, nϵ = estim.nx̂, estim.nym, estim.nx̂, estim.nϵ nx̃ = nϵ + nx̂ info = Dict{Symbol, Any}() V̂, X̂0 = similar(estim.Y0m[1:nym*Nk]), similar(estim.X̂0[1:nx̂*Nk]) - û0, ŷ0 = similar(model.uop), similar(model.yop) - V̂, X̂0 = predict!(V̂, X̂0, û0, ŷ0, estim, model, estim.Z̃) + û0, x0i, ŷ0 = buffer.û, buffer.xi, buffer.ŷ + V̂, X̂0 = predict!(V̂, X̂0, û0, x0i, ŷ0, estim, model, estim.Z̃) x̂0arr = @views estim.Z̃[nx̃-nx̂+1:nx̃] x̄ = estim.x̂0arr_old - x̂0arr X̂0 = [x̂0arr; X̂0] @@ -382,19 +382,17 @@ 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 - model, optim = estim.model, estim.optim - nu, ny = model.nu, model.ny + model, optim, buffer = estim.model, estim.optim, estim.buffer + nu, ny, nxi = model.nu, model.ny, model.nxi nx̂, nym, nŵ, nϵ, Nk = estim.nx̂, estim.nym, estim.nx̂, estim.nϵ, estim.Nk[] nx̃ = nϵ + nx̂ Z̃var::Vector{JuMP.VariableRef} = optim[:Z̃var] - V̂ = Vector{NT}(undef, nym*Nk) - X̂0 = Vector{NT}(undef, nx̂*Nk) - û0 = Vector{NT}(undef, nu) - ŷ0 = Vector{NT}(undef, ny) - x̄ = Vector{NT}(undef, nx̂) + V̂ = Vector{NT}(undef, nym*Nk) + X̂0 = Vector{NT}(undef, nx̂*Nk) + û0, ŷ0, x̄, x0i = buffer.û, buffer.ŷ, buffer.x̂, buffer.xi ϵ_0 = estim.nϵ ≠ 0 ? estim.Z̃[begin] : empty(estim.Z̃) Z̃_0 = [ϵ_0; estim.x̂0arr_old; estim.Ŵ] - V̂, X̂0 = predict!(V̂, X̂0, û0, ŷ0, estim, model, Z̃_0) + V̂, X̂0 = predict!(V̂, X̂0, û0, x0i, ŷ0, estim, model, Z̃_0) J_0 = obj_nonlinprog!(x̄, estim, model, V̂, Z̃_0) # initial Z̃0 with Ŵ=0 if objective or constraint function not finite : isfinite(J_0) || (Z̃_0 = [ϵ_0; estim.x̂0arr_old; zeros(NT, nŵ*estim.He)]) @@ -436,7 +434,7 @@ function optim_objective!(estim::MovingHorizonEstimator{NT}) where NT<:Real end # --------- update estimate ----------------------- estim.Ŵ[1:nŵ*Nk] .= @views estim.Z̃[nx̃+1:nx̃+nŵ*Nk] # update Ŵ with optimum for warm-start - V̂, X̂0 = predict!(V̂, X̂0, û0, ŷ0, estim, model, estim.Z̃) + V̂, X̂0 = predict!(V̂, X̂0, û0, x0i, ŷ0, estim, model, estim.Z̃) x̂0next = @views X̂0[end-nx̂+1:end] estim.x̂0 .= x̂0next return estim.Z̃ @@ -545,7 +543,7 @@ function obj_nonlinprog!( end """ - predict!(V̂, X̂0, û0, ŷ0, estim::MovingHorizonEstimator, model::LinModel, Z̃) -> V̂, X̂0 + predict!(V̂, X̂0, û0, x0i, ŷ0, estim::MovingHorizonEstimator, model::LinModel, Z̃) -> V̂, X̂0 Compute the `V̂` vector and `X̂0` vectors for the `MovingHorizonEstimator` and `LinModel`. @@ -553,7 +551,7 @@ The function mutates `V̂`, `X̂0`, `û0` and `ŷ0` vector arguments. The vect estimated sensor noises from ``k-N_k+1`` to ``k``. The `X̂0` vector is estimated states from ``k-N_k+2`` to ``k+1``. """ -function predict!(V̂, X̂0, _ , _ , estim::MovingHorizonEstimator, ::LinModel, Z̃) +function predict!(V̂, X̂0, _ , _ , _ , estim::MovingHorizonEstimator, ::LinModel, Z̃) nϵ, Nk = estim.nϵ, estim.Nk[] nX̂, nŴ, nYm = estim.nx̂*Nk, estim.nx̂*Nk, estim.nym*Nk nZ̃ = nϵ + estim.nx̂ + nŴ @@ -563,7 +561,7 @@ function predict!(V̂, X̂0, _ , _ , estim::MovingHorizonEstimator, ::LinModel, end "Compute the two vectors when `model` is not a `LinModel`." -function predict!(V̂, X̂0, û0, ŷ0, estim::MovingHorizonEstimator, model::SimModel, Z̃) +function predict!(V̂, X̂0, û0, x0i, ŷ0, estim::MovingHorizonEstimator, model::SimModel, Z̃) nϵ, Nk = estim.nϵ, estim.Nk[] nu, nd, nx̂, nŵ, nym = model.nu, model.nd, estim.nx̂, estim.nx̂, estim.nym nx̃ = nϵ + nx̂ @@ -575,7 +573,7 @@ function predict!(V̂, X̂0, û0, ŷ0, estim::MovingHorizonEstimator, model::S u0 = @views estim.U0[ (1 + nu * (j-1)):(nu*j)] ŵ = @views Z̃[(1 + nx̃ + nŵ*(j-1)):(nx̃ + nŵ*j)] x̂0next = @views X̂0[(1 + nx̂ *(j-1)):(nx̂ *j)] - f̂!(x̂0next, û0, estim, model, x̂0, u0, d0) + f̂!(x̂0next, û0, x0i, estim, model, x̂0, u0, d0) x̂0next .+= ŵ .+ estim.f̂op .- estim.x̂op y0nextm = @views estim.Y0m[(1 + nym * (j-1)):(nym*j)] d0next = @views estim.D0[(1 + nd*j):(nd*(j+1))] @@ -594,7 +592,7 @@ function predict!(V̂, X̂0, û0, ŷ0, estim::MovingHorizonEstimator, model::S ŷ0m = @views ŷ0[estim.i_ym] V̂[(1 + nym*(j-1)):(nym*j)] .= y0m .- ŷ0m x̂0next = @views X̂0[(1 + nx̂ *(j-1)):(nx̂ *j)] - f̂!(x̂0next, û0, estim, model, x̂0, u0, d0) + f̂!(x̂0next, û0, x0i, estim, model, x̂0, u0, d0) x̂0next .+= ŵ .+ estim.f̂op .- estim.x̂op x̂0 = x̂0next end @@ -604,15 +602,15 @@ end """ - update_predictions!(V̂, X̂0, û0, ŷ0, g, estim::MovingHorizonEstimator, Z̃) + update_predictions!(V̂, X̂0, û0, x0i, ŷ0, g, estim::MovingHorizonEstimator, Z̃) Update in-place the vectors for the predictions of `estim` estimator at decision vector `Z̃`. The method mutates all the arguments before `estim` argument. """ -function update_prediction!(V̂, X̂0, û0, ŷ0, g, estim::MovingHorizonEstimator, Z̃) +function update_prediction!(V̂, X̂0, û0, x0i, ŷ0, g, estim::MovingHorizonEstimator, Z̃) model = estim.model - V̂, X̂0 = predict!(V̂, X̂0, û0, ŷ0, estim, model, Z̃) + V̂, X̂0 = predict!(V̂, X̂0, û0, x0i, ŷ0, estim, model, Z̃) ϵ = getϵ(estim, Z̃) g = con_nonlinprog!(g, estim, model, X̂0, V̂, ϵ) return nothing diff --git a/src/model/linmodel.jl b/src/model/linmodel.jl index f54c03c4c..fc7a453ca 100644 --- a/src/model/linmodel.jl +++ b/src/model/linmodel.jl @@ -12,6 +12,7 @@ struct LinModel{NT<:Real} <: SimModel{NT} nx::Int ny::Int nd::Int + nxi::Int uop::Vector{NT} yop::Vector{NT} dop::Vector{NT} @@ -49,13 +50,14 @@ struct LinModel{NT<:Real} <: SimModel{NT} xname = ["\$x_{$i}\$" for i in 1:nx] x0 = zeros(NT, nx) t = zeros(NT, 1) + nxi = 0 # not used for LinModel buffer = SimModelBuffer{NT}(nu, nx, ny, nd) return new{NT}( A, Bu, C, Bd, Dd, x0, p, Ts, t, - nu, nx, ny, nd, + nu, nx, ny, nd, nxi, uop, yop, dop, xop, fop, uname, yname, dname, xname, buffer diff --git a/src/model/nonlinmodel.jl b/src/model/nonlinmodel.jl index e8b4f2748..62233c87e 100644 --- a/src/model/nonlinmodel.jl +++ b/src/model/nonlinmodel.jl @@ -18,6 +18,7 @@ struct NonLinModel{ nx::Int ny::Int nd::Int + nxi::Int uop::Vector{NT} yop::Vector{NT} dop::Vector{NT} @@ -54,14 +55,16 @@ struct NonLinModel{ xname = ["\$x_{$i}\$" for i in 1:nx] x0 = zeros(NT, nx) t = zeros(NT, 1) - buffer = SimModelBuffer{NT}(nu, nx, ny, nd, solver.ni) + ni = solver.ni + nxi = nx*(ni+1) + buffer = SimModelBuffer{NT}(nu, nx, ny, nd, ni) return new{NT, F, H, PT, DS, JB, LF}( x0, solver_f!, solver_h!, p, solver, Ts, t, - nu, nx, ny, nd, + nu, nx, ny, nd, nxi, uop, yop, dop, xop, fop, uname, yname, dname, xname, jacobian, linfunc!, @@ -267,10 +270,21 @@ Call [`linearize(model; x, u, d)`](@ref) and return the resulting linear model. """ LinModel(model::NonLinModel; kwargs...) = linearize(model; kwargs...) -"Call `model.solver_f!(x0next, x0i, x0, u0, d0, p)` for [`NonLinModel`](@ref)." +""" + f!(x0next, x0i, model::NonLinModel, x0, u0, d0, p) + +Call `model.solver_f!(x0next, x0i, x0, u0, d0, p)` for [`NonLinModel`](@ref). + +The method mutate `x0next` and `x0i` arguments in-place. The latter is used to store the +intermediate stage values of `model.solver` [`DiffSolver`](@ref). +""" f!(x0next, x0i, model::NonLinModel, x0, u0, d0, p) = model.solver_f!(x0next, x0i, x0, u0, d0, p) -"Call `model.solver_h!(y0, x0, d0, p)` for [`NonLinModel`](@ref)." +""" + h!(y0, model::NonLinModel, x0, d0, p) + +Call `model.solver_h!(y0, x0, d0, p)` for [`NonLinModel`](@ref). +""" h!(y0, model::NonLinModel, x0, d0, p) = model.solver_h!(y0, x0, d0, p) detailstr(model::NonLinModel) = ", $(typeof(model.solver).name.name)($(model.solver.order)) solver" diff --git a/src/model/solver.jl b/src/model/solver.jl index 51b25343e..5c1a9baaa 100644 --- a/src/model/solver.jl +++ b/src/model/solver.jl @@ -21,7 +21,7 @@ in which `xnext`, `xi` and `y` arguments are mutated in-place. The `xi` argument a vector of `nx*(solver.ni+1)` elements to store the solver intermediate stage values (and also the current state value for when `supersample ≠ 1`). """ -function get_solver_functions(::DataType, ::EmptySolver, f!, h!, _ , _ , _ , _ ) +function get_solver_functions(::DataType, ::EmptySolver, f!, h!, _ , _ , _ , _ , _ ) solver_f!(xnext, _ , x, u, d, p) = f!(xnext, x, u, d, p) solver_h! = h! return solver_f!, solver_h! @@ -101,7 +101,7 @@ function get_rk4_function(NT, solver, f!, Ts, nx) end "Get the f! function for the explicit Euler solver." -function get_euler_function(NT, solver, fc!, Ts, nx, Nc) +function get_euler_function(NT, solver, fc!, Ts, nx) Ts_inner = Ts/solver.supersample function euler_solver_f!(xnext, xi, x, u, d, p) xcurr = @views xi[1:nx] diff --git a/test/1_test_sim_model.jl b/test/1_test_sim_model.jl index 9ffbd1794..2ec105f10 100644 --- a/test/1_test_sim_model.jl +++ b/test/1_test_sim_model.jl @@ -162,10 +162,10 @@ end @test nonlinmodel1.nu == 2 @test nonlinmodel1.nd == 0 @test nonlinmodel1.ny == 2 - xnext, y = similar(nonlinmodel1.x0), similar(nonlinmodel1.yop) - nonlinmodel1.f!(xnext,[0,0],[0,0],[1],nonlinmodel1.p) + xnext, xi, y = nonlinmodel1.buffer.x, nonlinmodel1.buffer.xi, nonlinmodel1.buffer.y + nonlinmodel1.solver_f!(xnext, xi, [0,0],[0,0],[1],nonlinmodel1.p) @test xnext ≈ zeros(2,) - nonlinmodel1.h!(y,[0,0],[1],nonlinmodel1.p) + nonlinmodel1.solver_h!(y,[0,0],[1],nonlinmodel1.p) @test y ≈ zeros(2,) linmodel2 = LinModel(sys,Ts,i_d=[3]) @@ -177,10 +177,10 @@ end @test nonlinmodel2.nu == 2 @test nonlinmodel2.nd == 1 @test nonlinmodel2.ny == 2 - xnext, y = similar(nonlinmodel2.x0), similar(nonlinmodel2.yop) - nonlinmodel2.f!(xnext,[0,0,0,0],[0,0],[0],nonlinmodel2.p) + xnext, xi, y = nonlinmodel2.buffer.x, nonlinmodel2.buffer.xi, nonlinmodel2.buffer.y + nonlinmodel2.solver_f!(xnext, xi,[0,0,0,0],[0,0],[0],nonlinmodel2.p) @test xnext ≈ zeros(4,) - nonlinmodel2.h!(y,[0,0,0,0],[0],nonlinmodel2.p) + nonlinmodel2.solver_h!(y,[0,0,0,0],[0],nonlinmodel2.p) @test y ≈ zeros(2,) nonlinmodel3 = NonLinModel{Float32}(f2,h2,Ts,2,4,2,1,solver=nothing) @@ -198,10 +198,10 @@ end return nothing end nonlinmodel4 = NonLinModel(f1!, h1!, Ts, 2, 4, 2, 1, solver=nothing, p=linmodel2) - xnext, y = similar(nonlinmodel4.x0), similar(nonlinmodel4.yop) - nonlinmodel4.f!(xnext,[0,0,0,0],[0,0],[0],nonlinmodel4.p) + xnext, xi, y = nonlinmodel4.buffer.x, nonlinmodel4.buffer.xi, nonlinmodel4.buffer.y + nonlinmodel4.solver_f!(xnext,xi,[0,0,0,0],[0,0],[0],nonlinmodel4.p) @test xnext ≈ zeros(4) - nonlinmodel4.h!(y,[0,0,0,0],[0],nonlinmodel4.p) + nonlinmodel4.solver_h!(y,[0,0,0,0],[0],nonlinmodel4.p) @test y ≈ zeros(2) A = [0 0.5; -0.2 -0.1] @@ -216,10 +216,10 @@ end @test string(solver) == "4th order Runge-Kutta differential equation solver with 1 supersamples." nonlinmodel5 = NonLinModel(f3, h3, 1.0, 1, 2, 1, 1, solver=solver, p=p) - xnext, y = similar(nonlinmodel5.x0), similar(nonlinmodel5.yop) - nonlinmodel5.f!(xnext, [0; 0], [0], [0], nonlinmodel5.p) + xnext, xi, y = nonlinmodel5.buffer.x, nonlinmodel5.buffer.xi, nonlinmodel5.buffer.y + nonlinmodel5.solver_f!(xnext,xi, [0; 0], [0], [0], nonlinmodel5.p) @test xnext ≈ zeros(2) - nonlinmodel5.h!(y, [0; 0], [0], nonlinmodel5.p) + nonlinmodel5.solver_h!(y, [0; 0], [0], nonlinmodel5.p) @test y ≈ zeros(1) function f2!(ẋ, x, u , d, p) @@ -234,19 +234,19 @@ end return nothing end nonlinmodel6 = NonLinModel(f2!, h2!, 1.0, 1, 2, 1, 1, solver=RungeKutta(), p=p) - xnext, y = similar(nonlinmodel6.x0), similar(nonlinmodel6.yop) - nonlinmodel6.f!(xnext, [0; 0], [0], [0], nonlinmodel6.p) + xnext, xi, y = nonlinmodel6.buffer.x, nonlinmodel6.buffer.xi, nonlinmodel6.buffer.y + nonlinmodel6.solver_f!(xnext,xi, [0; 0], [0], [0], nonlinmodel6.p) @test xnext ≈ zeros(2) - nonlinmodel6.h!(y, [0; 0], [0], nonlinmodel6.p) + nonlinmodel6.solver_h!(y, [0; 0], [0], nonlinmodel6.p) @test y ≈ zeros(1) - nonlinemodel7 = NonLinModel(f2!, h2!, 1.0, 1, 2, 1, 1, solver=ForwardEuler(), p=p) - xnext, y = similar(nonlinemodel7.x0), similar(nonlinemodel7.yop) - nonlinemodel7.f!(xnext, [0; 0], [0], [0], nonlinemodel7.p) + nonlinmodel7 = NonLinModel(f2!, h2!, 1.0, 1, 2, 1, 1, solver=ForwardEuler(), p=p) + xnext, xi, y = nonlinmodel7.buffer.x, nonlinmodel7.buffer.xi, nonlinmodel7.buffer.y + nonlinmodel7.solver_f!(xnext, xi, [0; 0], [0], [0], nonlinmodel7.p) @test xnext ≈ zeros(2) - nonlinemodel7.h!(y, [0; 0], [0], nonlinemodel7.p) + nonlinmodel7.solver_h!(y, [0; 0], [0], nonlinmodel7.p) @test y ≈ zeros(1) - nonlinemodel8 = NonLinModel(f2!, h2!, 1.0, 1, 2, 1, 1, p=p, jacobian=AutoFiniteDiff()) - @test nonlinemodel8.jacobian == AutoFiniteDiff() + nonlinmodel8 = NonLinModel(f2!, h2!, 1.0, 1, 2, 1, 1, p=p, jacobian=AutoFiniteDiff()) + @test nonlinmodel8.jacobian == AutoFiniteDiff() @test_throws ErrorException NonLinModel( (x,u)->linmodel1.A*x + linmodel1.Bu*u, @@ -316,19 +316,25 @@ end h1!(y, x, d, _) = (y .= x.^2 .+ d; nothing) nonlinmodel3 = NonLinModel(f1!,h1!,Ts,1,1,1,1,solver=RungeKutta()) linmodel3 = linearize(nonlinmodel3; x, u, d) - u0, d0 = u - nonlinmodel3.uop, d - nonlinmodel3.dop - xnext, y = similar(nonlinmodel3.x0), similar(nonlinmodel3.yop) - A = ForwardDiff.jacobian((xnext, x) -> nonlinmodel3.f!(xnext, x, u0, d0, nonlinmodel3.p), xnext, x) - Bu = ForwardDiff.jacobian((xnext, u0) -> nonlinmodel3.f!(xnext, x, u0, d0, nonlinmodel3.p), xnext, u0) - Bd = ForwardDiff.jacobian((xnext, d0) -> nonlinmodel3.f!(xnext, x, u0, d0, nonlinmodel3.p), xnext, d0) - C = ForwardDiff.jacobian((y, x) -> nonlinmodel3.h!(y, x, d0, nonlinmodel3.p), y, x) - Dd = ForwardDiff.jacobian((y, d0) -> nonlinmodel3.h!(y, x, d0, nonlinmodel3.p), y, d0) + x0, u0, d0 = x - nonlinmodel3.xop, u - nonlinmodel3.uop, d - nonlinmodel3.dop + xnext, xi, y = nonlinmodel3.buffer.x, nonlinmodel3.buffer.xi, nonlinmodel3.buffer.y + backend = AutoForwardDiff() + f_A(xnext, x0, xi) = nonlinmodel3.solver_f!(xnext, xi, x0, u0, d0, nonlinmodel3.p) + f_Bu(xnext, u0, xi) = nonlinmodel3.solver_f!(xnext, xi, x0, u0, d0, nonlinmodel3.p) + f_Bd(xnext, d0, xi) = nonlinmodel3.solver_f!(xnext, xi, x0, u0, d0, nonlinmodel3.p) + h_C(y, x0) = nonlinmodel3.solver_h!(y, x0, d0, nonlinmodel3.p) + h_Dd(y, d0) = nonlinmodel3.solver_h!(y, x0, d0, nonlinmodel3.p) + A = jacobian(f_A, xnext, backend, x0, Cache(xi)) + Bu = jacobian(f_Bu, xnext, backend, u0, Cache(xi)) + Bd = jacobian(f_Bd, xnext, backend, d0, Cache(xi)) + C = jacobian(h_C, y, backend, x0) + Dd = jacobian(h_Dd, y, backend, d0) @test linmodel3.A ≈ A @test linmodel3.Bu ≈ Bu @test linmodel3.Bd ≈ Bd @test linmodel3.C ≈ C @test linmodel3.Dd ≈ Dd - + # test `linearize` at a non-equilibrium point: Ynl, Yl = let nonlinmodel3=nonlinmodel3 N = 5 diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index e18dcc3d8..ba7729aa2 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -708,7 +708,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) - nonlinmodel2.h!(y, Float32[0,0], Float32[0], nonlinmodel2.p) + nonlinmodel2.solver_h!(y, Float32[0,0], Float32[0], nonlinmodel2.p) preparestate!(nmpc7, [0], [0]) @test moveinput!(nmpc7, [0], [0]) ≈ [0.0] nmpc8 = NonLinMPC(nonlinmodel, Nwt=[0], Hp=100, Hc=1, transcription=MultipleShooting()) From e15b12c7e65817f168758abd0a559a9a913ab909 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 29 Mar 2025 11:46:16 -0400 Subject: [PATCH 5/8] changed: renamed `xi` to `k`and `Xi` to `K` --- src/controller/execute.jl | 6 +++--- src/controller/nonlinmpc.jl | 38 ++++++++++++++++----------------- src/controller/transcription.jl | 30 +++++++++++++------------- src/estimator/construct.jl | 10 ++++----- src/estimator/execute.jl | 16 +++++++------- src/estimator/internal_model.jl | 8 +++---- src/estimator/kalman.jl | 30 +++++++++++++------------- src/estimator/luenberger.jl | 4 ++-- src/estimator/mhe/construct.jl | 24 ++++++++++----------- src/estimator/mhe/execute.jl | 26 +++++++++++----------- src/model/linearization.jl | 26 +++++++++++----------- src/model/linmodel.jl | 6 +++--- src/model/nonlinmodel.jl | 14 ++++++------ src/model/solver.jl | 22 +++++++++---------- src/sim_model.jl | 10 ++++----- test/1_test_sim_model.jl | 38 ++++++++++++++++----------------- 16 files changed, 154 insertions(+), 154 deletions(-) diff --git a/src/controller/execute.jl b/src/controller/execute.jl index 19e43cb02..52ab2b6ee 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -114,14 +114,14 @@ julia> round.(getinfo(mpc)[:Ŷ], digits=3) """ function getinfo(mpc::PredictiveController{NT}) where NT<:Real model, buffer, transcription = mpc.estim.model, mpc.buffer, mpc.transcription - nΔŨ, nXi = mpc.Hc*model.nu + mpc.nϵ, mpc.Hp*model.nxi + nΔŨ, nK = mpc.Hc*model.nu + mpc.nϵ, mpc.Hp*model.nk nŶe, nUe = (mpc.Hp+1)*model.ny, (mpc.Hp+1)*model.nu nX̂0, nÛ0 = mpc.estim.nx̂*mpc.Hp, model.nu*mpc.Hp Z̃ = mpc.Z̃ info = Dict{Symbol, Any}() ΔŨ = Vector{NT}(undef, nΔŨ) x̂0end = similar(mpc.estim.x̂0) - X0i = Vector{NT}(undef, nXi) + K0 = Vector{NT}(undef, nK) 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) @@ -129,7 +129,7 @@ function getinfo(mpc::PredictiveController{NT}) where NT<:Real D̂ = buffer.D̂ U0 = getU0!(U0, mpc, Z̃) ΔŨ = getΔŨ!(ΔŨ, mpc, transcription, Z̃) - Ŷ0, x̂0end = predict!(Ŷ0, x̂0end, X̂0, Û0, X0i, mpc, model, transcription, U0, Z̃) + Ŷ0, x̂0end = predict!(Ŷ0, x̂0end, X̂0, Û0, K0, 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/nonlinmpc.jl b/src/controller/nonlinmpc.jl index f9be4ec4c..0794088da 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -578,17 +578,17 @@ Inspired from: [User-defined operators with vector outputs](@extref JuMP User-de function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT<:Real # ----- common cache for Jfunc, gfuncs, geqfuncs called with floats ------------------- model = mpc.estim.model - nu, ny, nx̂, nϵ, nxi = model.nu, model.ny, mpc.estim.nx̂, mpc.nϵ, model.nxi + nu, ny, nx̂, nϵ, nk = model.nu, model.ny, mpc.estim.nx̂, mpc.nϵ, model.nk Hp, Hc = mpc.Hp, mpc.Hc ng, nc, neq = length(mpc.con.i_g), mpc.con.nc, mpc.con.neq - nZ̃, nU, nŶ, nX̂, nXi = length(mpc.Z̃), Hp*nu, Hp*ny, Hp*nx̂, Hp*nxi + nZ̃, nU, nŶ, nX̂, nK = length(mpc.Z̃), Hp*nu, Hp*ny, Hp*nx̂, Hp*nk nΔŨ, nUe, nŶe = nu*Hc + nϵ, nU + nu, nŶ + ny strict = Val(true) myNaN = convert(JNT, NaN) # NaN to force update_simulations! at first call: Z̃ ::Vector{JNT} = fill(myNaN, nZ̃) ΔŨ::Vector{JNT} = zeros(JNT, nΔŨ) x̂0end::Vector{JNT} = zeros(JNT, nx̂) - X0i::Vector{JNT} = zeros(JNT, nXi) + K0::Vector{JNT} = zeros(JNT, nK) 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̂) @@ -598,18 +598,18 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT function Jfunc(Z̃arg::Vararg{T, N}) where {N, T<:Real} if isdifferent(Z̃arg, Z̃) Z̃ .= Z̃arg - update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X0i, X̂0, gc, g, geq, mpc, Z̃) + update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃) end return obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ)::T end - function Jfunc!(Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X0i, X̂0, gc, g, geq) - update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X0i, X̂0, gc, g, geq, mpc, Z̃) + function Jfunc!(Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq) + update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃) return obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ) end Z̃_∇J = fill(myNaN, nZ̃) ∇J_context = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), - Cache(Û0), Cache(X0i), Cache(X̂0), + Cache(Û0), Cache(K0), Cache(X̂0), Cache(gc), Cache(g), Cache(geq), ) ∇J_prep = prepare_gradient(Jfunc!, mpc.gradient, Z̃_∇J, ∇J_context...; strict) @@ -634,22 +634,22 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT if isdifferent(Z̃arg, Z̃) Z̃ .= Z̃arg update_predictions!( - ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X0i, X̂0, gc, g, geq, mpc, Z̃ + ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃ ) end return g[i]::T end gfuncs[i] = gfunc_i end - function gfunc!(g, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X0i, X̂0, gc, geq) + function gfunc!(g, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, geq) return update_predictions!( - ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X0i, X̂0, gc, g, geq, mpc, Z̃ + ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃ ) end Z̃_∇g = fill(myNaN, nZ̃) ∇g_context = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), - Cache(Û0), Cache(X0i), Cache(X̂0), + Cache(Û0), Cache(K0), Cache(X̂0), Cache(gc), Cache(geq), ) # temporarily enable all the inequality constraints for sparsity detection: @@ -685,22 +685,22 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT if isdifferent(Z̃arg, Z̃) Z̃ .= Z̃arg update_predictions!( - ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X0i, X̂0, gc, g, geq, mpc, Z̃ + ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃ ) end return geq[i]::T end geqfuncs[i] = geqfunc_i end - function geqfunc!(geq, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X0i, X̂0, gc, g) + function geqfunc!(geq, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g) return update_predictions!( - ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X0i, X̂0, gc, g, geq, mpc, Z̃ + ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃ ) end Z̃_∇geq = fill(myNaN, nZ̃) ∇geq_context = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), - Cache(Û0), Cache(X0i), Cache(X̂0), + Cache(Û0), Cache(K0), Cache(X̂0), Cache(gc), Cache(g) ) ∇geq_prep = prepare_jacobian(geqfunc!, geq, mpc.jacobian, Z̃_∇geq, ∇geq_context...; strict) @@ -726,7 +726,7 @@ end """ update_predictions!( - ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X0i, X̂0, gc, g, geq, + ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc::PredictiveController, Z̃ ) -> nothing @@ -735,17 +735,17 @@ 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, X0i, X̂0, gc, g, geq, mpc::PredictiveController, Z̃ + ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, 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, X0i, mpc, model, transcription, U0, Z̃) + Ŷ0, x̂0end = predict!(Ŷ0, x̂0end, X̂0, Û0, K0, mpc, model, transcription, U0, Z̃) Ue, Ŷe = extended_vectors!(Ue, Ŷe, mpc, U0, Ŷ0) ϵ = getϵ(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, X0i, mpc, model, transcription, U0, Z̃) + geq = con_nonlinprogeq!(geq, X̂0, Û0, K0, mpc, model, transcription, U0, Z̃) return nothing end diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 8258b714e..d449769fe 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1052,31 +1052,31 @@ end @doc raw""" predict!( - Ŷ0, x̂0end, X̂0, Û0, X0i, + Ŷ0, x̂0end, X̂0, Û0, K0, 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 `X0i` arguments. +The method mutates `Ŷ0`, `x̂0end`, `X̂0`, `Û0` and `K0` arguments. """ function predict!( - Ŷ0, x̂0end, X̂0, Û0, X0i, + Ŷ0, x̂0end, X̂0, Û0, K0, mpc::PredictiveController, model::NonLinModel, ::SingleShooting, U0, _ ) - nu, nx̂, ny, nd, nxi = model.nu, mpc.estim.nx̂, model.ny, model.nd, model.nxi + nu, nx̂, ny, nd, nk = model.nu, mpc.estim.nx̂, model.ny, model.nd, model.nk Hp, Hc = mpc.Hp, mpc.Hc D̂0 = mpc.D̂0 x̂0 = @views mpc.estim.x̂0[1:nx̂] d0 = @views mpc.d0[1:nd] for j=1:Hp - u0 = @views U0[(1 + nu*(j-1)):(nu*j)] - û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] - x0i = @views X0i[(1 + nxi*(j-1)):(nxi*j)] - x̂0next = @views X̂0[(1 + nx̂*(j-1)):(nx̂*j)] - f̂!(x̂0next, û0, x0i, mpc.estim, model, x̂0, u0, d0) + u0 = @views U0[(1 + nu*(j-1)):(nu*j)] + û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] + k0 = @views K0[(1 + nk*(j-1)):(nk*j)] + x̂0next = @views X̂0[(1 + nx̂*(j-1)):(nx̂*j)] + f̂!(x̂0next, û0, k0, mpc.estim, model, x̂0, u0, d0) x̂0next .+= mpc.estim.f̂op .- mpc.estim.x̂op x̂0 = @views X̂0[(1 + nx̂*(j-1)):(nx̂*j)] d0 = @views D̂0[(1 + nd*(j-1)):(nd*j)] @@ -1206,19 +1206,19 @@ end """ con_nonlinprogeq!( - geq, X̂0, Û0, X0i + geq, X̂0, Û0, K0 mpc::PredictiveController, model::NonLinModel, ::MultipleShooting, U0, Z̃ ) Nonlinear equality constrains for [`NonLinModel`](@ref) and [`MultipleShooting`](@ref). -The method mutates the `geq`, `X̂0`, `Û0` and `X0i` vectors in argument. +The method mutates the `geq`, `X̂0`, `Û0` and `K0` vectors in argument. """ function con_nonlinprogeq!( - geq, X̂0, Û0, X0i, + geq, X̂0, Û0, K0, mpc::PredictiveController, model::NonLinModel, ::MultipleShooting, U0, Z̃ ) - nu, nx̂, ny, nd, nxi = model.nu, mpc.estim.nx̂, model.ny, model.nd, model.nxi + nu, nx̂, ny, nd, nk = model.nu, mpc.estim.nx̂, model.ny, model.nd, model.nk Hp, Hc = mpc.Hp, mpc.Hc nΔU, nX̂ = nu*Hc, nx̂*Hp D̂0 = mpc.D̂0 @@ -1230,8 +1230,8 @@ function con_nonlinprogeq!( u0 = @views U0[(1 + nu*(j-1)):(nu*j)] û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] x̂0next = @views X̂0[(1 + nx̂*(j-1)):(nx̂*j)] - x0i = @views X0i[(1 + nxi*(j-1)):(nxi*j)] - f̂!(x̂0next, û0, x0i, mpc.estim, model, x̂0, u0, d0) + k0 = @views K0[(1 + nk*(j-1)):(nk*j)] + f̂!(x̂0next, û0, k0, mpc.estim, model, x̂0, u0, d0) x̂0next .+= mpc.estim.f̂op .- mpc.estim.x̂op x̂0next_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*j)] ŝnext = @views geq[(1 + nx̂*(j-1)):(nx̂*j)] diff --git a/src/estimator/construct.jl b/src/estimator/construct.jl index 7d3f1d907..247f53a6b 100644 --- a/src/estimator/construct.jl +++ b/src/estimator/construct.jl @@ -1,7 +1,7 @@ struct StateEstimatorBuffer{NT<:Real} u ::Vector{NT} û ::Vector{NT} - xi::Vector{NT} + k::Vector{NT} x̂ ::Vector{NT} P̂ ::Matrix{NT} Q̂ ::Matrix{NT} @@ -14,18 +14,18 @@ struct StateEstimatorBuffer{NT<:Real} end @doc raw""" - StateEstimatorBuffer{NT}(nu::Int, nx̂::Int, nym::Int, ny::Int, nd::Int, nxi::Int=0) + StateEstimatorBuffer{NT}(nu::Int, nx̂::Int, nym::Int, ny::Int, nd::Int, nk::Int=0) Create a buffer for `StateEstimator` objects for estimated states and measured outputs. The buffer is used to store intermediate results during estimation without allocating. """ function StateEstimatorBuffer{NT}( - nu::Int, nx̂::Int, nym::Int, ny::Int, nd::Int, nxi::Int=0 + nu::Int, nx̂::Int, nym::Int, ny::Int, nd::Int, nk::Int=0 ) where NT <: Real u = Vector{NT}(undef, nu) û = Vector{NT}(undef, nu) - xi = Vector{NT}(undef, nxi) + k = Vector{NT}(undef, nk) x̂ = Vector{NT}(undef, nx̂) P̂ = Matrix{NT}(undef, nx̂, nx̂) Q̂ = Matrix{NT}(undef, nx̂, nx̂) @@ -35,7 +35,7 @@ function StateEstimatorBuffer{NT}( ŷ = Vector{NT}(undef, ny) d = Vector{NT}(undef, nd) empty = Vector{NT}(undef, 0) - return StateEstimatorBuffer{NT}(u, û, xi, x̂, P̂, Q̂, R̂, K̂, ym, ŷ, d, empty) + return StateEstimatorBuffer{NT}(u, û, k, x̂, P̂, Q̂, R̂, K̂, ym, ŷ, d, empty) end @doc raw""" diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index d9fb030f2..867a237bb 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -18,7 +18,7 @@ function remove_op!(estim::StateEstimator, ym, d, u=nothing) end @doc raw""" - f̂!(x̂0next, û0, x0i, estim::StateEstimator, model::SimModel, x̂0, u0, d0) -> nothing + f̂!(x̂0next, û0, k0, estim::StateEstimator, model::SimModel, x̂0, u0, d0) -> nothing Mutating state function ``\mathbf{f̂}`` of the augmented model. @@ -31,13 +31,13 @@ function returns the next state of the augmented model, defined as: \end{aligned} ``` where ``\mathbf{x̂_0}(k+1)`` is stored in `x̂0next` argument. The method mutates `x̂0next`, -`û0` and `x0i` in place. The argument `û0` is the input vector of the augmented model, -computed by ``\mathbf{û_0 = u_0 + ŷ_{s_u}}``. The argument `x0i` is used to store the +`û0` and `k0` in place. The argument `û0` is the input vector of the augmented model, +computed by ``\mathbf{û_0 = u_0 + ŷ_{s_u}}``. The argument `k0` is used to store the intermediate stage values of `model.solver` (when applicable). The model parameter vector `model.p` is not included in the function signature for conciseness. """ -function f̂!(x̂0next, û0, x0i, estim::StateEstimator, model::SimModel, x̂0, u0, d0) - return f̂!(x̂0next, û0, x0i, model, estim.As, estim.Cs_u, x̂0, u0, d0) +function f̂!(x̂0next, û0, k0, estim::StateEstimator, model::SimModel, x̂0, u0, d0) + return f̂!(x̂0next, û0, k0, model, estim.As, estim.Cs_u, x̂0, u0, d0) end """ @@ -53,17 +53,17 @@ function f̂!(x̂0next, _ , _ , estim::StateEstimator, ::LinModel, x̂0, u0, d0) end """ - f̂!(x̂0next, û0, x0i, model::SimModel, As, Cs_u, x̂0, u0, d0) + f̂!(x̂0next, û0, k0, model::SimModel, As, Cs_u, x̂0, u0, d0) Same than [`f̂!`](@ref) for [`SimModel`](@ref) but without the `estim` argument. """ -function f̂!(x̂0next, û0, x0i, model::SimModel, As, Cs_u, x̂0, u0, d0) +function f̂!(x̂0next, û0, k0, model::SimModel, As, Cs_u, 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̂d_next, x̂s_next = x̂0next[1:model.nx], x̂0next[model.nx+1:end] mul!(û0, Cs_u, x̂s) # ŷs_u = Cs_u * x̂s û0 .+= u0 - f!(x̂d_next, x0i, model, x̂d, û0, d0, model.p) + f!(x̂d_next, k0, model, x̂d, û0, d0, model.p) mul!(x̂s_next, As, x̂s) return nothing end diff --git a/src/estimator/internal_model.jl b/src/estimator/internal_model.jl index 21399b0fe..184756523 100644 --- a/src/estimator/internal_model.jl +++ b/src/estimator/internal_model.jl @@ -32,7 +32,7 @@ struct InternalModel{NT<:Real, SM<:SimModel} <: StateEstimator{NT} function InternalModel{NT}( model::SM, i_ym, Asm, Bsm, Csm, Dsm ) where {NT<:Real, SM<:SimModel} - nu, ny, nd, nxi = model.nu, model.ny, model.nd, model.nxi + nu, ny, nd, nk = model.nu, model.ny, model.nd, model.nk nym, nyu = validate_ym(model, i_ym) validate_internalmodel(model, nym, Csm, Dsm) As, Bs, Cs, Ds = stoch_ym2y(model, i_ym, Asm, Bsm, Csm, Dsm) @@ -48,7 +48,7 @@ struct InternalModel{NT<:Real, SM<:SimModel} <: StateEstimator{NT} ŷs = zeros(NT, ny) direct = true # InternalModel always uses direct transmission from ym corrected = [false] - buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nxi) + buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk) return new{NT, SM}( model, lastu0, x̂op, f̂op, x̂0, x̂d, x̂s, ŷs, x̂snext, @@ -284,8 +284,8 @@ function update_estimate!(estim::InternalModel, _ , d0, u0) model = estim.model x̂d, x̂s, ŷs = estim.x̂d, estim.x̂s, estim.ŷs # -------------- deterministic model --------------------- - x̂dnext, x0i = estim.buffer.x̂, estim.buffer.xi - f!(x̂dnext, x0i, model, x̂d, u0, d0, model.p) + x̂dnext, k0 = estim.buffer.x̂, estim.buffer.k + f!(x̂dnext, k0, model, x̂d, u0, d0, model.p) x̂d .= x̂dnext # this also updates estim.x̂0 (they are the same object) # --------------- stochastic model ----------------------- x̂snext = estim.x̂snext diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index 2610f26f6..465d029a2 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -30,7 +30,7 @@ struct SteadyKalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT} function SteadyKalmanFilter{NT}( model::SM, i_ym, nint_u, nint_ym, Q̂, R̂; direct=true ) where {NT<:Real, SM<:LinModel} - nu, ny, nd, nxi = model.nu, model.ny, model.nd, model.nxi + nu, ny, nd, nk = model.nu, model.ny, model.nd, model.nk nym, nyu = validate_ym(model, i_ym) As, Cs_u, Cs_y, nint_u, nint_ym = init_estimstoch(model, i_ym, nint_u, nint_ym) nxs = size(As, 1) @@ -60,7 +60,7 @@ struct SteadyKalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT} x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)] Q̂, R̂ = Hermitian(Q̂, :L), Hermitian(R̂, :L) corrected = [false] - buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nxi) + buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk) return new{NT, SM}( model, lastu0, x̂op, f̂op, x̂0, @@ -311,7 +311,7 @@ struct KalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT} function KalmanFilter{NT}( model::SM, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct=true ) where {NT<:Real, SM<:LinModel} - nu, ny, nd, nxi = model.nu, model.ny, model.nd, model.nxi + nu, ny, nd, nk = model.nu, model.ny, model.nd, model.nk nym, nyu = validate_ym(model, i_ym) As, Cs_u, Cs_y, nint_u, nint_ym = init_estimstoch(model, i_ym, nint_u, nint_ym) nxs = size(As, 1) @@ -326,7 +326,7 @@ struct KalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT} P̂ = Hermitian(copy(P̂_0.data), :L) # copy on P̂_0.data necessary for Julia Nightly K̂ = zeros(NT, nx̂, nym) corrected = [false] - buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nxi) + buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk) return new{NT, SM}( model, lastu0, x̂op, f̂op, x̂0, P̂, @@ -534,7 +534,7 @@ struct UnscentedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT} function UnscentedKalmanFilter{NT}( model::SM, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, α, β, κ; direct=true ) where {NT<:Real, SM<:SimModel{NT}} - nu, ny, nd, nxi = model.nu, model.ny, model.nd, model.nxi + nu, ny, nd, nk = model.nu, model.ny, model.nd, model.nk nym, nyu = validate_ym(model, i_ym) As, Cs_u, Cs_y, nint_u, nint_ym = init_estimstoch(model, i_ym, nint_u, nint_ym) nxs = size(As, 1) @@ -553,7 +553,7 @@ struct UnscentedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT} X̂0, X̄0 = zeros(NT, nx̂, nσ), zeros(NT, nx̂, nσ) Ŷ0m, Ȳ0m = zeros(NT, nym, nσ), zeros(NT, nym, nσ) corrected = [false] - buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nxi) + buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk) return new{NT, SM}( model, lastu0, x̂op, f̂op, x̂0, P̂, @@ -843,7 +843,7 @@ function update_estimate!(estim::UnscentedKalmanFilter, y0m, d0, u0) x̂0corr, X̂0corr, P̂corr = estim.x̂0, estim.X̂0, estim.P̂ Q̂, nx̂ = estim.Q̂, estim.nx̂ γ, m̂, Ŝ = estim.γ, estim.m̂, estim.Ŝ - x̂0next, û0, x0i = estim.buffer.x̂, estim.buffer.û, estim.buffer.xi + x̂0next, û0, k0 = estim.buffer.x̂, estim.buffer.û, estim.buffer.k # in-place operations to reduce allocations: P̂corr_temp = Hermitian(estim.buffer.P̂, :L) P̂corr_temp .= P̂corr @@ -856,7 +856,7 @@ function update_estimate!(estim::UnscentedKalmanFilter, y0m, d0, u0) X̂0next = X̂0corr for j in axes(X̂0next, 2) @views x̂0corr .= X̂0corr[:, j] - @views f̂!(X̂0next[:, j], û0, x0i, estim, estim.model, x̂0corr, u0, d0) + @views f̂!(X̂0next[:, j], û0, k0, estim, estim.model, x̂0corr, u0, d0) end x̂0next .= mul!(x̂0corr, X̂0next, m̂) X̄0next = estim.X̄0 @@ -917,7 +917,7 @@ struct ExtendedKalmanFilter{ function ExtendedKalmanFilter{NT}( model::SM, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; jacobian::JB, linfunc!::LF, direct=true ) where {NT<:Real, SM<:SimModel, JB<:AbstractADType, LF<:Function} - nu, ny, nd, nxi = model.nu, model.ny, model.nd, model.nxi + nu, ny, nd, nk = model.nu, model.ny, model.nd, model.nk nym, nyu = validate_ym(model, i_ym) As, Cs_u, Cs_y, nint_u, nint_ym = init_estimstoch(model, i_ym, nint_u, nint_ym) nxs = size(As, 1) @@ -934,7 +934,7 @@ struct ExtendedKalmanFilter{ F̂_û, F̂ = zeros(NT, nx̂+nu, nx̂), zeros(NT, nx̂, nx̂) Ĥ, Ĥm = zeros(NT, ny, nx̂), zeros(NT, nym, nx̂) corrected = [false] - buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nxi) + buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk) return new{NT, SM, JB, LF}( model, lastu0, x̂op, f̂op, x̂0, P̂, @@ -1075,16 +1075,16 @@ objects with the linearization points. """ function get_ekf_linfunc(NT, model, i_ym, nint_u, nint_ym, jacobian) As, Cs_u, Cs_y = init_estimstoch(model, i_ym, nint_u, nint_ym) - f̂_ekf!(x̂0next, x̂0, û0, x0i, u0, d0) = f̂!(x̂0next, û0, x0i, model, As, Cs_u, x̂0, u0, d0) + f̂_ekf!(x̂0next, x̂0, û0, k0, u0, d0) = f̂!(x̂0next, û0, k0, model, As, Cs_u, x̂0, u0, d0) ĥ_ekf!(ŷ0, x̂0, d0) = ĥ!(ŷ0, model, Cs_y, x̂0, d0) strict = Val(true) - nu, ny, nd, nxi = model.nu, model.ny, model.nd, model.nxi + nu, ny, nd, nk = model.nu, model.ny, model.nd, model.nk nx̂ = model.nx + size(As, 1) x̂0next = zeros(NT, nx̂) ŷ0 = zeros(NT, ny) x̂0 = zeros(NT, nx̂) tmp_û0 = Cache(zeros(NT, nu)) - tmp_x0i = Cache(zeros(NT, nxi)) + tmp_x0i = Cache(zeros(NT, nk)) cst_u0 = Constant(zeros(NT, nu)) cst_d0 = Constant(zeros(NT, nd)) F̂_prep = prepare_jacobian( @@ -1248,9 +1248,9 @@ They predict the state `x̂` and covariance `P̂` with the same equations. See function predict_estimate_kf!(estim::Union{KalmanFilter, ExtendedKalmanFilter}, u0, d0, Â) x̂0corr, P̂corr = estim.x̂0, estim.P̂ Q̂ = estim.Q̂ - x̂0next, û0, x0i = estim.buffer.x̂, estim.buffer.û, estim.buffer.xi + x̂0next, û0, k0 = estim.buffer.x̂, estim.buffer.û, estim.buffer.k # in-place operations to reduce allocations: - f̂!(x̂0next, û0, x0i, estim, estim.model, x̂0corr, u0, d0) + f̂!(x̂0next, û0, k0, estim, estim.model, x̂0corr, u0, d0) P̂corr_Âᵀ = estim.buffer.P̂ mul!(P̂corr_Âᵀ, P̂corr.data, Â') # the ".data" weirdly removes a type instability in mul! Â_P̂corr_Âᵀ = estim.buffer.Q̂ diff --git a/src/estimator/luenberger.jl b/src/estimator/luenberger.jl index 024da7906..430f9b2ac 100644 --- a/src/estimator/luenberger.jl +++ b/src/estimator/luenberger.jl @@ -28,7 +28,7 @@ struct Luenberger{NT<:Real, SM<:LinModel} <: StateEstimator{NT} function Luenberger{NT, SM}( model, i_ym, nint_u, nint_ym, poles; direct=true ) where {NT<:Real, SM<:LinModel} - nu, ny, nd, nxi = model.nu, model.ny, model.nd, model.nxi + nu, ny, nd, nk = model.nu, model.ny, model.nd, model.nk nym, nyu = validate_ym(model, i_ym) validate_luenberger(model, nint_u, nint_ym, poles) As, Cs_u, Cs_y, nint_u, nint_ym = init_estimstoch(model, i_ym, nint_u, nint_ym) @@ -44,7 +44,7 @@ struct Luenberger{NT<:Real, SM<:LinModel} <: StateEstimator{NT} lastu0 = zeros(NT, nu) x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)] corrected = [false] - buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nxi) + buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk) return new{NT, SM}( model, lastu0, x̂op, f̂op, x̂0, diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index afeccc353..b85c05fbc 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -126,7 +126,7 @@ struct MovingHorizonEstimator{ JB<:AbstractADType, CE<:StateEstimator{NT} } - nu, ny, nd, nxi = model.nu, model.ny, model.nd, model.nxi + nu, ny, nd, nk = model.nu, model.ny, model.nd, model.nk He < 1 && throw(ArgumentError("Estimation horizon He should be ≥ 1")) Cwt < 0 && throw(ArgumentError("Cwt weight should be ≥ 0")) nym, nyu = validate_ym(model, i_ym) @@ -156,7 +156,7 @@ struct MovingHorizonEstimator{ nD0 = direct ? nd*(He+1) : nd*He U0, D0 = zeros(NT, nu*He), zeros(NT, nD0) Ŵ = zeros(NT, nx̂*He) - buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nxi) + buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk) P̂_0 = Hermitian(P̂_0, :L) Q̂, R̂ = Hermitian(Q̂, :L), Hermitian(R̂, :L) P̂_0 = Hermitian(P̂_0, :L) @@ -1329,14 +1329,14 @@ function get_optim_functions( ) where {JNT <: Real} # ---------- common cache for Jfunc, gfuncs called with floats ------------------------ model, con = estim.model, estim.con - nx̂, nym, nŷ, nu, nϵ, nxi = estim.nx̂, estim.nym, model.ny, model.nu, estim.nϵ, model.nxi + nx̂, nym, nŷ, nu, nϵ, nk = estim.nx̂, estim.nym, model.ny, model.nu, estim.nϵ, model.nk He = estim.He nV̂, nX̂, ng, nZ̃ = He*nym, He*nx̂, length(con.i_g), length(estim.Z̃) myNaN = convert(JNT, NaN) # NaN to force update_simulations! at first call strict = Val(true) Z̃::Vector{JNT} = fill(myNaN, nZ̃) V̂::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nV̂), zeros(JNT, nX̂) - x0i::Vector{JNT} = zeros(JNT, nxi) + k0::Vector{JNT} = zeros(JNT, nk) û0::Vector{JNT}, ŷ0::Vector{JNT} = zeros(JNT, nu), zeros(JNT, nŷ) g::Vector{JNT} = zeros(JNT, ng) x̄::Vector{JNT} = zeros(JNT, nx̂) @@ -1344,17 +1344,17 @@ function get_optim_functions( function Jfunc(Z̃arg::Vararg{T, N}) where {N, T<:Real} if isdifferent(Z̃arg, Z̃) Z̃ .= Z̃arg - update_prediction!(V̂, X̂0, û0, x0i, ŷ0, g, estim, Z̃) + update_prediction!(V̂, X̂0, û0, k0, ŷ0, g, estim, Z̃) end return obj_nonlinprog!(x̄, estim, model, V̂, Z̃)::T end - function Jfunc!(Z̃, V̂, X̂0, û0, x0i, ŷ0, g, x̄) - update_prediction!(V̂, X̂0, û0, x0i, ŷ0, g, estim, Z̃) + function Jfunc!(Z̃, V̂, X̂0, û0, k0, ŷ0, g, x̄) + update_prediction!(V̂, X̂0, û0, k0, ŷ0, g, estim, Z̃) return obj_nonlinprog!(x̄, estim, model, V̂, Z̃) end Z̃_∇J = fill(myNaN, nZ̃) ∇J_context = ( - Cache(V̂), Cache(X̂0), Cache(û0), Cache(x0i), Cache(ŷ0), + Cache(V̂), Cache(X̂0), Cache(û0), Cache(k0), Cache(ŷ0), Cache(g), Cache(x̄), ) @@ -1377,18 +1377,18 @@ function get_optim_functions( gfunc_i = function (Z̃arg::Vararg{T, N}) where {N, T<:Real} if isdifferent(Z̃arg, Z̃) Z̃ .= Z̃arg - update_prediction!(V̂, X̂0, û0, x0i, ŷ0, g, estim, Z̃) + update_prediction!(V̂, X̂0, û0, k0, ŷ0, g, estim, Z̃) end return g[i]::T end gfuncs[i] = gfunc_i end - function gfunc!(g, Z̃, V̂, X̂0, û0, x0i, ŷ0) - return update_prediction!(V̂, X̂0, û0, x0i, ŷ0, g, estim, Z̃) + function gfunc!(g, Z̃, V̂, X̂0, û0, k0, ŷ0) + return update_prediction!(V̂, X̂0, û0, k0, ŷ0, g, estim, Z̃) end Z̃_∇g = fill(myNaN, nZ̃) ∇g_context = ( - Cache(V̂), Cache(X̂0), Cache(û0), Cache(x0i), Cache(ŷ0), + Cache(V̂), Cache(X̂0), Cache(û0), Cache(k0), Cache(ŷ0), ) # temporarily enable all the inequality constraints for sparsity detection: estim.con.i_g .= true diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 8f352c7b8..f5f2c9a53 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -110,8 +110,8 @@ function getinfo(estim::MovingHorizonEstimator{NT}) where NT<:Real nx̃ = nϵ + nx̂ info = Dict{Symbol, Any}() V̂, X̂0 = similar(estim.Y0m[1:nym*Nk]), similar(estim.X̂0[1:nx̂*Nk]) - û0, x0i, ŷ0 = buffer.û, buffer.xi, buffer.ŷ - V̂, X̂0 = predict!(V̂, X̂0, û0, x0i, ŷ0, estim, model, estim.Z̃) + û0, k0, ŷ0 = buffer.û, buffer.k, buffer.ŷ + V̂, X̂0 = predict!(V̂, X̂0, û0, k0, ŷ0, estim, model, estim.Z̃) x̂0arr = @views estim.Z̃[nx̃-nx̂+1:nx̃] x̄ = estim.x̂0arr_old - x̂0arr X̂0 = [x̂0arr; X̂0] @@ -383,16 +383,16 @@ the debug log [if activated](@extref Julia Example:-Enable-debug-level-messages) """ function optim_objective!(estim::MovingHorizonEstimator{NT}) where NT<:Real model, optim, buffer = estim.model, estim.optim, estim.buffer - nu, ny, nxi = model.nu, model.ny, model.nxi + nu, ny, nk = model.nu, model.ny, model.nk nx̂, nym, nŵ, nϵ, Nk = estim.nx̂, estim.nym, estim.nx̂, estim.nϵ, estim.Nk[] nx̃ = nϵ + nx̂ Z̃var::Vector{JuMP.VariableRef} = optim[:Z̃var] V̂ = Vector{NT}(undef, nym*Nk) X̂0 = Vector{NT}(undef, nx̂*Nk) - û0, ŷ0, x̄, x0i = buffer.û, buffer.ŷ, buffer.x̂, buffer.xi + û0, ŷ0, x̄, k0 = buffer.û, buffer.ŷ, buffer.x̂, buffer.k ϵ_0 = estim.nϵ ≠ 0 ? estim.Z̃[begin] : empty(estim.Z̃) Z̃_0 = [ϵ_0; estim.x̂0arr_old; estim.Ŵ] - V̂, X̂0 = predict!(V̂, X̂0, û0, x0i, ŷ0, estim, model, Z̃_0) + V̂, X̂0 = predict!(V̂, X̂0, û0, k0, ŷ0, estim, model, Z̃_0) J_0 = obj_nonlinprog!(x̄, estim, model, V̂, Z̃_0) # initial Z̃0 with Ŵ=0 if objective or constraint function not finite : isfinite(J_0) || (Z̃_0 = [ϵ_0; estim.x̂0arr_old; zeros(NT, nŵ*estim.He)]) @@ -434,7 +434,7 @@ function optim_objective!(estim::MovingHorizonEstimator{NT}) where NT<:Real end # --------- update estimate ----------------------- estim.Ŵ[1:nŵ*Nk] .= @views estim.Z̃[nx̃+1:nx̃+nŵ*Nk] # update Ŵ with optimum for warm-start - V̂, X̂0 = predict!(V̂, X̂0, û0, x0i, ŷ0, estim, model, estim.Z̃) + V̂, X̂0 = predict!(V̂, X̂0, û0, k0, ŷ0, estim, model, estim.Z̃) x̂0next = @views X̂0[end-nx̂+1:end] estim.x̂0 .= x̂0next return estim.Z̃ @@ -543,7 +543,7 @@ function obj_nonlinprog!( end """ - predict!(V̂, X̂0, û0, x0i, ŷ0, estim::MovingHorizonEstimator, model::LinModel, Z̃) -> V̂, X̂0 + predict!(V̂, X̂0, û0, k0, ŷ0, estim::MovingHorizonEstimator, model::LinModel, Z̃) -> V̂, X̂0 Compute the `V̂` vector and `X̂0` vectors for the `MovingHorizonEstimator` and `LinModel`. @@ -561,7 +561,7 @@ function predict!(V̂, X̂0, _ , _ , _ , estim::MovingHorizonEstimator, ::LinMod end "Compute the two vectors when `model` is not a `LinModel`." -function predict!(V̂, X̂0, û0, x0i, ŷ0, estim::MovingHorizonEstimator, model::SimModel, Z̃) +function predict!(V̂, X̂0, û0, k0, ŷ0, estim::MovingHorizonEstimator, model::SimModel, Z̃) nϵ, Nk = estim.nϵ, estim.Nk[] nu, nd, nx̂, nŵ, nym = model.nu, model.nd, estim.nx̂, estim.nx̂, estim.nym nx̃ = nϵ + nx̂ @@ -573,7 +573,7 @@ function predict!(V̂, X̂0, û0, x0i, ŷ0, estim::MovingHorizonEstimator, mod u0 = @views estim.U0[ (1 + nu * (j-1)):(nu*j)] ŵ = @views Z̃[(1 + nx̃ + nŵ*(j-1)):(nx̃ + nŵ*j)] x̂0next = @views X̂0[(1 + nx̂ *(j-1)):(nx̂ *j)] - f̂!(x̂0next, û0, x0i, estim, model, x̂0, u0, d0) + f̂!(x̂0next, û0, k0, estim, model, x̂0, u0, d0) x̂0next .+= ŵ .+ estim.f̂op .- estim.x̂op y0nextm = @views estim.Y0m[(1 + nym * (j-1)):(nym*j)] d0next = @views estim.D0[(1 + nd*j):(nd*(j+1))] @@ -592,7 +592,7 @@ function predict!(V̂, X̂0, û0, x0i, ŷ0, estim::MovingHorizonEstimator, mod ŷ0m = @views ŷ0[estim.i_ym] V̂[(1 + nym*(j-1)):(nym*j)] .= y0m .- ŷ0m x̂0next = @views X̂0[(1 + nx̂ *(j-1)):(nx̂ *j)] - f̂!(x̂0next, û0, x0i, estim, model, x̂0, u0, d0) + f̂!(x̂0next, û0, k0, estim, model, x̂0, u0, d0) x̂0next .+= ŵ .+ estim.f̂op .- estim.x̂op x̂0 = x̂0next end @@ -602,15 +602,15 @@ end """ - update_predictions!(V̂, X̂0, û0, x0i, ŷ0, g, estim::MovingHorizonEstimator, Z̃) + update_predictions!(V̂, X̂0, û0, k0, ŷ0, g, estim::MovingHorizonEstimator, Z̃) Update in-place the vectors for the predictions of `estim` estimator at decision vector `Z̃`. The method mutates all the arguments before `estim` argument. """ -function update_prediction!(V̂, X̂0, û0, x0i, ŷ0, g, estim::MovingHorizonEstimator, Z̃) +function update_prediction!(V̂, X̂0, û0, k0, ŷ0, g, estim::MovingHorizonEstimator, Z̃) model = estim.model - V̂, X̂0 = predict!(V̂, X̂0, û0, x0i, ŷ0, estim, model, Z̃) + V̂, X̂0 = predict!(V̂, X̂0, û0, k0, ŷ0, estim, model, Z̃) ϵ = getϵ(estim, Z̃) g = con_nonlinprog!(g, estim, model, X̂0, V̂, ϵ) return nothing diff --git a/src/model/linearization.jl b/src/model/linearization.jl index 077d39626..bb17c9e54 100644 --- a/src/model/linearization.jl +++ b/src/model/linearization.jl @@ -14,9 +14,9 @@ is an `AbstractADType` object from `DifferentiationInterface`. The `cst_x`, `cst `cst_d` are `DifferentiationInterface.Constant` objects with the linearization points. """ function get_linearization_func(NT, solver_f!, solver_h!, nu, nx, ny, nd, p, solver, backend) - f_x!(xnext, x, xi, u, d) = solver_f!(xnext, xi, x, u, d, p) - f_u!(xnext, u, xi, x, d) = solver_f!(xnext, xi, x, u, d, p) - f_d!(xnext, d, xi, x, u) = solver_f!(xnext, xi, x, u, d, p) + f_x!(xnext, x, k, u, d) = solver_f!(xnext, k, x, u, d, p) + f_u!(xnext, u, k, x, d) = solver_f!(xnext, k, x, u, d, p) + f_d!(xnext, d, k, x, u) = solver_f!(xnext, k, x, u, d, p) h_x!(y, x, d) = solver_h!(y, x, d, p) h_d!(y, d, x) = solver_h!(y, x, d, p) strict = Val(true) @@ -25,21 +25,21 @@ function get_linearization_func(NT, solver_f!, solver_h!, nu, nx, ny, nd, p, sol x = zeros(NT, nx) u = zeros(NT, nu) d = zeros(NT, nd) - xi = zeros(NT, nx*(solver.ni+1)) - cache_xi = Cache(xi) + k = zeros(NT, nx*(solver.ni+1)) + cache_k = Cache(k) cst_x = Constant(x) cst_u = Constant(u) cst_d = Constant(d) - A_prep = prepare_jacobian(f_x!, xnext, backend, x, cache_xi, cst_u, cst_d; strict) - Bu_prep = prepare_jacobian(f_u!, xnext, backend, u, cache_xi, cst_x, cst_d; strict) - Bd_prep = prepare_jacobian(f_d!, xnext, backend, d, cache_xi, cst_x, cst_u; strict) + 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) 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_xi, cst_u, cst_d) - jacobian!(f_u!, xnext, Bu, Bu_prep, backend, u, cache_xi, cst_x, cst_d) - jacobian!(f_d!, xnext, Bd, Bd_prep, backend, d, cache_xi, cst_x, cst_u) + 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_d!, xnext, Bd, Bd_prep, backend, d, cache_k, cst_x, cst_u) jacobian!(h_x!, y, C, C_prep, backend, x, cst_d) jacobian!(h_d!, y, Dd, Dd_prep, backend, d, cst_x) return nothing @@ -158,7 +158,7 @@ function linearize!( nonlinmodel = model buffer = nonlinmodel.buffer # --- remove the operating points of the nonlinear model (typically zeros) --- - x0, u0, d0, x0i = buffer.x, buffer.u, buffer.d, buffer.xi + x0, u0, d0, k0 = buffer.x, buffer.u, buffer.d, buffer.k x0 .= x .- nonlinmodel.xop u0 .= u .- nonlinmodel.uop d0 .= d .- nonlinmodel.dop @@ -170,7 +170,7 @@ function linearize!( y = y0 y .= y0 .+ nonlinmodel.yop # --- compute the nonlinear model next state at operating points --- - f!(x0next, x0i, nonlinmodel, x0, u0, d0, model.p) + f!(x0next, k0, nonlinmodel, x0, u0, d0, model.p) xnext = x0next xnext .= x0next .+ nonlinmodel.fop .- nonlinmodel.xop # --- modify the linear model operating points --- diff --git a/src/model/linmodel.jl b/src/model/linmodel.jl index fc7a453ca..aadb50ddb 100644 --- a/src/model/linmodel.jl +++ b/src/model/linmodel.jl @@ -12,7 +12,7 @@ struct LinModel{NT<:Real} <: SimModel{NT} nx::Int ny::Int nd::Int - nxi::Int + nk::Int uop::Vector{NT} yop::Vector{NT} dop::Vector{NT} @@ -50,14 +50,14 @@ struct LinModel{NT<:Real} <: SimModel{NT} xname = ["\$x_{$i}\$" for i in 1:nx] x0 = zeros(NT, nx) t = zeros(NT, 1) - nxi = 0 # not used for LinModel + nk = 0 # not used for LinModel buffer = SimModelBuffer{NT}(nu, nx, ny, nd) return new{NT}( A, Bu, C, Bd, Dd, x0, p, Ts, t, - nu, nx, ny, nd, nxi, + nu, nx, ny, nd, nk, uop, yop, dop, xop, fop, uname, yname, dname, xname, buffer diff --git a/src/model/nonlinmodel.jl b/src/model/nonlinmodel.jl index 62233c87e..d6c0c5d98 100644 --- a/src/model/nonlinmodel.jl +++ b/src/model/nonlinmodel.jl @@ -18,7 +18,7 @@ struct NonLinModel{ nx::Int ny::Int nd::Int - nxi::Int + nk::Int uop::Vector{NT} yop::Vector{NT} dop::Vector{NT} @@ -56,7 +56,7 @@ struct NonLinModel{ x0 = zeros(NT, nx) t = zeros(NT, 1) ni = solver.ni - nxi = nx*(ni+1) + nk = nx*(ni+1) buffer = SimModelBuffer{NT}(nu, nx, ny, nd, ni) return new{NT, F, H, PT, DS, JB, LF}( x0, @@ -64,7 +64,7 @@ struct NonLinModel{ p, solver, Ts, t, - nu, nx, ny, nd, nxi, + nu, nx, ny, nd, nk, uop, yop, dop, xop, fop, uname, yname, dname, xname, jacobian, linfunc!, @@ -271,14 +271,14 @@ Call [`linearize(model; x, u, d)`](@ref) and return the resulting linear model. LinModel(model::NonLinModel; kwargs...) = linearize(model; kwargs...) """ - f!(x0next, x0i, model::NonLinModel, x0, u0, d0, p) + f!(x0next, k0, model::NonLinModel, x0, u0, d0, p) -Call `model.solver_f!(x0next, x0i, x0, u0, d0, p)` for [`NonLinModel`](@ref). +Call `model.solver_f!(x0next, k0, x0, u0, d0, p)` for [`NonLinModel`](@ref). -The method mutate `x0next` and `x0i` arguments in-place. The latter is used to store the +The method mutate `x0next` and `k0` arguments in-place. The latter is used to store the intermediate stage values of `model.solver` [`DiffSolver`](@ref). """ -f!(x0next, x0i, model::NonLinModel, x0, u0, d0, p) = model.solver_f!(x0next, x0i, x0, u0, d0, p) +f!(x0next, k0, model::NonLinModel, x0, u0, d0, p) = model.solver_f!(x0next, k0, x0, u0, d0, p) """ h!(y0, model::NonLinModel, x0, d0, p) diff --git a/src/model/solver.jl b/src/model/solver.jl index 5c1a9baaa..41a9c582c 100644 --- a/src/model/solver.jl +++ b/src/model/solver.jl @@ -14,10 +14,10 @@ Get `solver_f!` and `solver_h!` functions for the `EmptySolver` (discrete models The functions should have the following signature: ``` - solver_f!(xnext, xi, x, u, d, p) -> nothing + solver_f!(xnext, k, x, u, d, p) -> nothing solver_h!(y, x, d, p) -> nothing ``` -in which `xnext`, `xi` and `y` arguments are mutated in-place. The `xi` argument is +in which `xnext`, `k` and `y` arguments are mutated in-place. The `k` argument is a vector of `nx*(solver.ni+1)` elements to store the solver intermediate stage values (and also the current state value for when `supersample ≠ 1`). """ @@ -77,12 +77,12 @@ end "Get the f! function for the 4th order explicit Runge-Kutta solver." function get_rk4_function(NT, solver, f!, Ts, nx) Ts_inner = Ts/solver.supersample - function rk4_solver_f!(xnext, xi, x, u, d, p) - xcurr = @views xi[1:nx] - k1 = @views xi[(1nx + 1):(2nx)] - k2 = @views xi[(2nx + 1):(3nx)] - k3 = @views xi[(3nx + 1):(4nx)] - k4 = @views xi[(4nx + 1):(5nx)] + function rk4_solver_f!(xnext, k, x, u, d, p) + xcurr = @views k[1:nx] + k1 = @views k[(1nx + 1):(2nx)] + k2 = @views k[(2nx + 1):(3nx)] + k3 = @views k[(3nx + 1):(4nx)] + k4 = @views k[(4nx + 1):(5nx)] @. xcurr = x for i=1:solver.supersample f!(k1, xcurr, u, d, p) @@ -103,9 +103,9 @@ end "Get the f! function for the explicit Euler solver." function get_euler_function(NT, solver, fc!, Ts, nx) Ts_inner = Ts/solver.supersample - function euler_solver_f!(xnext, xi, x, u, d, p) - xcurr = @views xi[1:nx] - k1 = @views xi[(1nx + 1):(2nx)] + function euler_solver_f!(xnext, k, x, u, d, p) + xcurr = @views k[1:nx] + k1 = @views k[(1nx + 1):(2nx)] @. xcurr = x for i=1:solver.supersample fc!(k1, xcurr, u, d, p) diff --git a/src/sim_model.jl b/src/sim_model.jl index 377dfe944..c9ee82bb2 100644 --- a/src/sim_model.jl +++ b/src/sim_model.jl @@ -25,7 +25,7 @@ struct SimModelBuffer{NT<:Real} x::Vector{NT} y::Vector{NT} d::Vector{NT} - xi::Vector{NT} + k::Vector{NT} empty::Vector{NT} end @@ -43,9 +43,9 @@ function SimModelBuffer{NT}(nu::Int, nx::Int, ny::Int, nd::Int, ni::Int=0) where x = Vector{NT}(undef, nx) y = Vector{NT}(undef, ny) d = Vector{NT}(undef, nd) - xi = Vector{NT}(undef, nx*(ni+1)) # the "+1" is necessary because of super-sampling + k = Vector{NT}(undef, nx*(ni+1)) # the "+1" is necessary because of super-sampling empty = Vector{NT}(undef, 0) - return SimModelBuffer{NT}(u, x, y, d, xi, empty) + return SimModelBuffer{NT}(u, x, y, d, k, empty) end @@ -249,10 +249,10 @@ julia> x = updatestate!(model, [1]) """ function updatestate!(model::SimModel{NT}, u, d=model.buffer.empty) where NT <: Real validate_args(model::SimModel, d, u) - u0, d0, x0next, x0i = model.buffer.u, model.buffer.d, model.buffer.x, model.buffer.xi + u0, d0, x0next, k0 = model.buffer.u, model.buffer.d, model.buffer.x, model.buffer.k u0 .= u .- model.uop d0 .= d .- model.dop - f!(x0next, x0i, model, model.x0, u0, d0, model.p) + f!(x0next, k0, model, model.x0, u0, d0, model.p) x0next .+= model.fop .- model.xop model.x0 .= x0next xnext = x0next diff --git a/test/1_test_sim_model.jl b/test/1_test_sim_model.jl index 2ec105f10..058ffabf2 100644 --- a/test/1_test_sim_model.jl +++ b/test/1_test_sim_model.jl @@ -162,8 +162,8 @@ end @test nonlinmodel1.nu == 2 @test nonlinmodel1.nd == 0 @test nonlinmodel1.ny == 2 - xnext, xi, y = nonlinmodel1.buffer.x, nonlinmodel1.buffer.xi, nonlinmodel1.buffer.y - nonlinmodel1.solver_f!(xnext, xi, [0,0],[0,0],[1],nonlinmodel1.p) + xnext, k, y = nonlinmodel1.buffer.x, nonlinmodel1.buffer.k, nonlinmodel1.buffer.y + nonlinmodel1.solver_f!(xnext, k, [0,0],[0,0],[1],nonlinmodel1.p) @test xnext ≈ zeros(2,) nonlinmodel1.solver_h!(y,[0,0],[1],nonlinmodel1.p) @test y ≈ zeros(2,) @@ -177,8 +177,8 @@ end @test nonlinmodel2.nu == 2 @test nonlinmodel2.nd == 1 @test nonlinmodel2.ny == 2 - xnext, xi, y = nonlinmodel2.buffer.x, nonlinmodel2.buffer.xi, nonlinmodel2.buffer.y - nonlinmodel2.solver_f!(xnext, xi,[0,0,0,0],[0,0],[0],nonlinmodel2.p) + xnext, k, y = nonlinmodel2.buffer.x, nonlinmodel2.buffer.k, nonlinmodel2.buffer.y + nonlinmodel2.solver_f!(xnext, k,[0,0,0,0],[0,0],[0],nonlinmodel2.p) @test xnext ≈ zeros(4,) nonlinmodel2.solver_h!(y,[0,0,0,0],[0],nonlinmodel2.p) @test y ≈ zeros(2,) @@ -198,8 +198,8 @@ end return nothing end nonlinmodel4 = NonLinModel(f1!, h1!, Ts, 2, 4, 2, 1, solver=nothing, p=linmodel2) - xnext, xi, y = nonlinmodel4.buffer.x, nonlinmodel4.buffer.xi, nonlinmodel4.buffer.y - nonlinmodel4.solver_f!(xnext,xi,[0,0,0,0],[0,0],[0],nonlinmodel4.p) + xnext, k, y = nonlinmodel4.buffer.x, nonlinmodel4.buffer.k, nonlinmodel4.buffer.y + nonlinmodel4.solver_f!(xnext,k,[0,0,0,0],[0,0],[0],nonlinmodel4.p) @test xnext ≈ zeros(4) nonlinmodel4.solver_h!(y,[0,0,0,0],[0],nonlinmodel4.p) @test y ≈ zeros(2) @@ -216,8 +216,8 @@ end @test string(solver) == "4th order Runge-Kutta differential equation solver with 1 supersamples." nonlinmodel5 = NonLinModel(f3, h3, 1.0, 1, 2, 1, 1, solver=solver, p=p) - xnext, xi, y = nonlinmodel5.buffer.x, nonlinmodel5.buffer.xi, nonlinmodel5.buffer.y - nonlinmodel5.solver_f!(xnext,xi, [0; 0], [0], [0], nonlinmodel5.p) + xnext, k, y = nonlinmodel5.buffer.x, nonlinmodel5.buffer.k, nonlinmodel5.buffer.y + nonlinmodel5.solver_f!(xnext,k, [0; 0], [0], [0], nonlinmodel5.p) @test xnext ≈ zeros(2) nonlinmodel5.solver_h!(y, [0; 0], [0], nonlinmodel5.p) @test y ≈ zeros(1) @@ -234,14 +234,14 @@ end return nothing end nonlinmodel6 = NonLinModel(f2!, h2!, 1.0, 1, 2, 1, 1, solver=RungeKutta(), p=p) - xnext, xi, y = nonlinmodel6.buffer.x, nonlinmodel6.buffer.xi, nonlinmodel6.buffer.y - nonlinmodel6.solver_f!(xnext,xi, [0; 0], [0], [0], nonlinmodel6.p) + xnext, k, y = nonlinmodel6.buffer.x, nonlinmodel6.buffer.k, nonlinmodel6.buffer.y + nonlinmodel6.solver_f!(xnext,k, [0; 0], [0], [0], nonlinmodel6.p) @test xnext ≈ zeros(2) nonlinmodel6.solver_h!(y, [0; 0], [0], nonlinmodel6.p) @test y ≈ zeros(1) nonlinmodel7 = NonLinModel(f2!, h2!, 1.0, 1, 2, 1, 1, solver=ForwardEuler(), p=p) - xnext, xi, y = nonlinmodel7.buffer.x, nonlinmodel7.buffer.xi, nonlinmodel7.buffer.y - nonlinmodel7.solver_f!(xnext, xi, [0; 0], [0], [0], nonlinmodel7.p) + xnext, k, y = nonlinmodel7.buffer.x, nonlinmodel7.buffer.k, nonlinmodel7.buffer.y + nonlinmodel7.solver_f!(xnext, k, [0; 0], [0], [0], nonlinmodel7.p) @test xnext ≈ zeros(2) nonlinmodel7.solver_h!(y, [0; 0], [0], nonlinmodel7.p) @test y ≈ zeros(1) @@ -317,16 +317,16 @@ end nonlinmodel3 = NonLinModel(f1!,h1!,Ts,1,1,1,1,solver=RungeKutta()) linmodel3 = linearize(nonlinmodel3; x, u, d) x0, u0, d0 = x - nonlinmodel3.xop, u - nonlinmodel3.uop, d - nonlinmodel3.dop - xnext, xi, y = nonlinmodel3.buffer.x, nonlinmodel3.buffer.xi, nonlinmodel3.buffer.y + xnext, k, y = nonlinmodel3.buffer.x, nonlinmodel3.buffer.k, nonlinmodel3.buffer.y backend = AutoForwardDiff() - f_A(xnext, x0, xi) = nonlinmodel3.solver_f!(xnext, xi, x0, u0, d0, nonlinmodel3.p) - f_Bu(xnext, u0, xi) = nonlinmodel3.solver_f!(xnext, xi, x0, u0, d0, nonlinmodel3.p) - f_Bd(xnext, d0, xi) = nonlinmodel3.solver_f!(xnext, xi, x0, u0, d0, nonlinmodel3.p) + f_A(xnext, x0, k) = nonlinmodel3.solver_f!(xnext, k, x0, u0, d0, nonlinmodel3.p) + f_Bu(xnext, u0, k) = nonlinmodel3.solver_f!(xnext, k, x0, u0, d0, nonlinmodel3.p) + f_Bd(xnext, d0, k) = nonlinmodel3.solver_f!(xnext, k, x0, u0, d0, nonlinmodel3.p) h_C(y, x0) = nonlinmodel3.solver_h!(y, x0, d0, nonlinmodel3.p) h_Dd(y, d0) = nonlinmodel3.solver_h!(y, x0, d0, nonlinmodel3.p) - A = jacobian(f_A, xnext, backend, x0, Cache(xi)) - Bu = jacobian(f_Bu, xnext, backend, u0, Cache(xi)) - Bd = jacobian(f_Bd, xnext, backend, d0, Cache(xi)) + A = jacobian(f_A, xnext, backend, x0, Cache(k)) + Bu = jacobian(f_Bu, xnext, backend, u0, Cache(k)) + Bd = jacobian(f_Bd, xnext, backend, d0, Cache(k)) C = jacobian(h_C, y, backend, x0) Dd = jacobian(h_Dd, y, backend, d0) @test linmodel3.A ≈ A From 5c0436bd46d764af67fdbfbc3fca44d821cf62c4 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 29 Mar 2025 11:47:26 -0400 Subject: [PATCH 6/8] bump --- Project.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Project.toml b/Project.toml index 8be585cce..a79d67a63 100644 --- a/Project.toml +++ b/Project.toml @@ -1,7 +1,7 @@ name = "ModelPredictiveControl" uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c" authors = ["Francis Gagnon"] -version = "1.5.0" +version = "1.5.1" [deps] ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e" From ec93aa501111b2bf4b04db8e5168a3b98c397237 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 29 Mar 2025 11:58:31 -0400 Subject: [PATCH 7/8] debug: `predict!` method of `ExplicitMPC` --- src/controller/explicitmpc.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/controller/explicitmpc.jl b/src/controller/explicitmpc.jl index 60ab4d207..d8729d3d3 100644 --- a/src/controller/explicitmpc.jl +++ b/src/controller/explicitmpc.jl @@ -195,7 +195,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 From 3af2012e30a07f309009591c02715cb33542abd6 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 29 Mar 2025 13:03:35 -0400 Subject: [PATCH 8/8] removed: `PreallocationTools` dependency --- Project.toml | 2 -- src/ModelPredictiveControl.jl | 2 -- 2 files changed, 4 deletions(-) diff --git a/Project.toml b/Project.toml index a79d67a63..682a5d423 100644 --- a/Project.toml +++ b/Project.toml @@ -12,7 +12,6 @@ JuMP = "4076af6c-e467-56ae-b986-b466b2749572" LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" Logging = "56ddb016-857b-54e1-b83d-db4d58db5568" OSQP = "ab2f91bb-94b4-55e3-9ba0-7f65df51de79" -PreallocationTools = "d236fae5-4411-538c-8e31-a6e3d9e00b46" PrecompileTools = "aea7be01-6a6a-4083-8856-8a6e6704d82a" ProgressLogging = "33c8b6b6-d38a-422a-b730-caa89a2f386c" Random = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c" @@ -29,7 +28,6 @@ JuMP = "1.21" LinearAlgebra = "1.10" Logging = "1.10" OSQP = "0.8" -PreallocationTools = "0.4.14" PrecompileTools = "1" ProgressLogging = "0.1" Random = "1.10" diff --git a/src/ModelPredictiveControl.jl b/src/ModelPredictiveControl.jl index ead8faf96..4bc201cd7 100644 --- a/src/ModelPredictiveControl.jl +++ b/src/ModelPredictiveControl.jl @@ -25,8 +25,6 @@ import JuMP import JuMP: MOIU, MOI, GenericModel, Model, optimizer_with_attributes, register import JuMP: @variable, @operator, @constraint, @objective -import PreallocationTools: DiffCache, get_tmp # TODO: remove this dep if possible (with Cache of DI.jl) - import OSQP, Ipopt export SimModel, LinModel, NonLinModel