diff --git a/Project.toml b/Project.toml index 52915e11..8be585cc 100644 --- a/Project.toml +++ b/Project.toml @@ -1,51 +1,51 @@ name = "ModelPredictiveControl" uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c" authors = ["Francis Gagnon"] -version = "1.4.4" +version = "1.5.0" [deps] -LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" -Logging = "56ddb016-857b-54e1-b83d-db4d58db5568" -Random = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c" ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e" +DifferentiationInterface = "a0c0ee7d-e4b9-4e03-894e-1c5f64a51d63" ForwardDiff = "f6369f11-7733-5829-9624-2563aa707210" Ipopt = "b6b21f68-93f8-5de0-b562-5493be1d77c9" 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" RecipesBase = "3cdcf5f2-1ef4-517c-9805-6587b60abb01" +SparseConnectivityTracer = "9f842d2f-2579-4b1d-911e-f412cf18a3f5" +SparseMatrixColorings = "0a514795-09f3-496d-8182-132a7b665d35" [compat] -julia = "1.10" -LinearAlgebra = "1.10" -Logging = "1.10" -Random = "1.10" ControlSystemsBase = "1.9" +DifferentiationInterface = "0.6.45" ForwardDiff = "0.10" Ipopt = "1" JuMP = "1.21" +LinearAlgebra = "1.10" +Logging = "1.10" OSQP = "0.8" PreallocationTools = "0.4.14" PrecompileTools = "1" ProgressLogging = "0.1" +Random = "1.10" RecipesBase = "1" +SparseConnectivityTracer = "0.6.13" +SparseMatrixColorings = "0.4.14" +julia = "1.10" [extras] DAQP = "c47d62df-3981-49c8-9651-128b1cd08617" Documenter = "e30172f5-a6a5-5a46-863b-614d45cd2de4" +FiniteDiff = "6a86dc24-6348-571c-b903-95158fe2bd41" Plots = "91a5bcdd-55d7-5caf-9e0b-520d859cae80" Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40" -TestItems = "1c621080-faea-4a02-84b6-bbd5e436b8fe" TestItemRunner = "f8b46487-2199-4994-9208-9a1283c18c0a" +TestItems = "1c621080-faea-4a02-84b6-bbd5e436b8fe" [targets] -test = [ - "Test", - "TestItems", - "TestItemRunner", - "Documenter", - "Plots", - "DAQP" - ] +test = ["Test", "TestItems", "TestItemRunner", "Documenter", "Plots", "DAQP", "FiniteDiff"] diff --git a/README.md b/README.md index c93120ee..63bb8bfc 100644 --- a/README.md +++ b/README.md @@ -9,7 +9,9 @@ An open source [model predictive control](https://en.wikipedia.org/wiki/Model_pr package for Julia. The package depends on [`ControlSystemsBase.jl`](https://github.com/JuliaControl/ControlSystems.jl) -for the linear systems and [`JuMP.jl`](https://github.com/jump-dev/JuMP.jl) for the solving. +for the linear systems, [`JuMP.jl`](https://github.com/jump-dev/JuMP.jl) for the +optimization and [`DifferentiationInterface.jl`](https://github.com/JuliaDiff/DifferentiationInterface.jl) +for the differentiation. ## Installation @@ -102,9 +104,13 @@ for more detailed examples. - measured disturbances - input setpoints - easy integration with `Plots.jl` -- optimization based on `JuMP.jl`: - - quickly compare multiple optimizers - - nonlinear solvers relying on automatic differentiation (exact derivative) +- optimization based on `JuMP.jl` to quickly compare multiple optimizers: + - many quadratic solvers for linear control + - many nonlinear solvers for nonlinear control (local or global) +- derivatives based on `DifferentiationInterface.jl` to compare different approaches: + - automatic differentiation (exact solution) + - symbolic differentiation (exact solution) + - finite difference (approximate solution) - supported transcription methods of the optimization problem: - direct single shooting - direct multiple shooting diff --git a/docs/Project.toml b/docs/Project.toml index cd71da67..1be7cf0f 100644 --- a/docs/Project.toml +++ b/docs/Project.toml @@ -1,19 +1,20 @@ [deps] -LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" -Logging = "56ddb016-857b-54e1-b83d-db4d58db5568" ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e" DAQP = "c47d62df-3981-49c8-9651-128b1cd08617" Documenter = "e30172f5-a6a5-5a46-863b-614d45cd2de4" +DocumenterInterLinks = "d12716ef-a0f6-4df4-a9f1-a5a34e75c656" JuMP = "4076af6c-e467-56ae-b986-b466b2749572" +LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" +Logging = "56ddb016-857b-54e1-b83d-db4d58db5568" ModelingToolkit = "961ee093-0014-501f-94e3-6117800e7a78" Plots = "91a5bcdd-55d7-5caf-9e0b-520d859cae80" [compat] -LinearAlgebra = "1.10" -Logging = "1.10" ControlSystemsBase = "1" +DAQP = "0.6" Documenter = "1" JuMP = "1" -DAQP = "0.6" -Plots = "1" +LinearAlgebra = "1.10" +Logging = "1.10" ModelingToolkit = "9.50" +Plots = "1" diff --git a/docs/make.jl b/docs/make.jl index 37de69d4..de69e08d 100644 --- a/docs/make.jl +++ b/docs/make.jl @@ -3,9 +3,17 @@ ENV["PLOTS_TEST"] = "true" ENV["GKSwstype"] = "nul" push!(LOAD_PATH,"../src/") -using Documenter +using Documenter, DocumenterInterLinks using ModelPredictiveControl +links = InterLinks( + "Julia" => "https://docs.julialang.org/en/v1/objects.inv", + "ControlSystemsBase" => "https://juliacontrol.github.io/ControlSystems.jl/stable/objects.inv", + "JuMP" => "https://jump.dev/JuMP.jl/stable/objects.inv", + "DifferentiationInterface" => "https://juliadiff.org/DifferentiationInterface.jl/DifferentiationInterface/stable/objects.inv", + "ForwardDiff" => "https://juliadiff.org/ForwardDiff.jl/stable/objects.inv", +) + DocMeta.setdocmeta!( ModelPredictiveControl, :DocTestSetup, @@ -16,6 +24,7 @@ makedocs( sitename = "ModelPredictiveControl.jl", #format = Documenter.LaTeX(platform = "none"), doctest = true, + plugins = [links], format = Documenter.HTML( prettyurls = get(ENV, "CI", nothing) == "true", edit_link = "main" diff --git a/docs/src/index.md b/docs/src/index.md index 230b4459..4be5bb16 100644 --- a/docs/src/index.md +++ b/docs/src/index.md @@ -4,14 +4,17 @@ An open source [model predictive control](https://en.wikipedia.org/wiki/Model_pr package for Julia. The package depends on [`ControlSystemsBase.jl`](https://github.com/JuliaControl/ControlSystems.jl) -for the linear systems and [`JuMP.jl`](https://github.com/jump-dev/JuMP.jl) for the solving. +for the linear systems, [`JuMP.jl`](https://github.com/jump-dev/JuMP.jl) for the +optimization and [`DifferentiationInterface.jl`](https://github.com/JuliaDiff/DifferentiationInterface.jl) +for the differentiation. The objective is to provide a simple, clear and modular framework to quickly design model predictive controllers (MPCs) in Julia, while preserving the flexibility for advanced real-time optimization. Modern MPCs based on closed-loop state estimators are the main focus of the package, but classical approaches that rely on internal models are also possible. The -`JuMP.jl` interface allows the user to test different solvers easily if the performance of -the default settings is not satisfactory. +`JuMP` and `DifferentiationInterface` dependencies allows the user to test different +optimizers and automatic differentiation (AD) backends easily if the performances of the +default settings are not satisfactory. The documentation is divided in two parts: diff --git a/docs/src/manual/mtk.md b/docs/src/manual/mtk.md index 29f616bc..605ed0af 100644 --- a/docs/src/manual/mtk.md +++ b/docs/src/manual/mtk.md @@ -11,8 +11,8 @@ old_logger = global_logger(); global_logger(errlogger); ## Pendulum Model -This example integrates the simple pendulum model of the [last section](@ref man_nonlin) in the -[`ModelingToolkit.jl`](https://docs.sciml.ai/ModelingToolkit/stable/) (MTK) framework and +This example integrates the simple pendulum model of the [last section](@ref man_nonlin) in +[`ModelingToolkit`](https://docs.sciml.ai/ModelingToolkit/stable/) (MTK) framework and extracts appropriate `f!` and `h!` functions to construct a [`NonLinModel`](@ref). An [`NonLinMPC`](@ref) is designed from this model and simulated to reproduce the results of the last section. diff --git a/docs/src/manual/nonlinmpc.md b/docs/src/manual/nonlinmpc.md index 57bc2f28..1c5032b3 100644 --- a/docs/src/manual/nonlinmpc.md +++ b/docs/src/manual/nonlinmpc.md @@ -329,7 +329,7 @@ Nonlinear MPC is more computationally expensive than [`LinMPC`](@ref). Solving t should always be faster than the sampling time ``T_s = 0.1`` s for real-time operation. This requirement is sometimes hard to meet on electronics or mechanical systems because of the fast dynamics. To ease the design and comparison with [`LinMPC`](@ref), the [`linearize`](@ref) -function allows automatic linearization of [`NonLinModel`](@ref) based on [`ForwardDiff.jl`](https://juliadiff.org/ForwardDiff.jl/stable/). +function allows automatic linearization of [`NonLinModel`](@ref) based on [`ForwardDiff`](@extref ForwardDiff). We first linearize `model` at the point ``θ = π`` rad and ``ω = τ = 0`` (inverted position): ```@example man_nonlin diff --git a/src/ModelPredictiveControl.jl b/src/ModelPredictiveControl.jl index bf65b67c..9d7d699d 100644 --- a/src/ModelPredictiveControl.jl +++ b/src/ModelPredictiveControl.jl @@ -1,12 +1,19 @@ module ModelPredictiveControl -using PrecompileTools +using PrecompileTools # TODO: remove this dep if possible (with Cache of DI.jl) using LinearAlgebra using Random: randn using RecipesBase using ProgressLogging -using ForwardDiff + +using DifferentiationInterface: ADTypes.AbstractADType, AutoForwardDiff, AutoSparse +using DifferentiationInterface: gradient!, jacobian!, prepare_gradient, prepare_jacobian +using DifferentiationInterface: Constant, Cache +using SparseConnectivityTracer: TracerSparsityDetector +using SparseMatrixColorings: GreedyColoringAlgorithm, sparsity_pattern + +import ForwardDiff #TODO: delete this after `linearize!` and `ExtendedKalmanFilter` are updated import ControlSystemsBase import ControlSystemsBase: ss, tf, delay diff --git a/src/controller/execute.jl b/src/controller/execute.jl index d9176d3e..d7cb78b8 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -123,19 +123,19 @@ function getinfo(mpc::PredictiveController{NT}) where NT<:Real x̂0end = similar(mpc.estim.x̂0) Ue, Ŷe = Vector{NT}(undef, nUe), Vector{NT}(undef, nŶe) U0, Ŷ0 = similar(mpc.Uop), similar(mpc.Yop) - X̂0, Û0 = Vector{NT}(undef, nX̂0), Vector{NT}(undef, nÛ0) + Û0, X̂0 = Vector{NT}(undef, nÛ0), Vector{NT}(undef, nX̂0) U, Ŷ = similar(mpc.Uop), similar(mpc.Yop) - Ŷs = similar(mpc.Yop) U0 = getU0!(U0, mpc, Z̃) - ΔŨ = getΔŨ!(ΔŨ, mpc, mpc.transcription, Z̃) + ΔŨ = getΔŨ!(ΔŨ, mpc, transcription, Z̃) Ŷ0, x̂0end = predict!(Ŷ0, x̂0end, X̂0, Û0, mpc, model, transcription, U0, Z̃) Ue, Ŷe = extended_vectors!(Ue, Ŷe, mpc, U0, Ŷ0) U .= U0 .+ mpc.Uop Ŷ .= Ŷ0 .+ mpc.Yop J = obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ) + Ŷs = similar(mpc.Yop) predictstoch!(Ŷs, mpc, mpc.estim) info[:ΔU] = Z̃[1:mpc.Hc*model.nu] - info[:ϵ] = mpc.nϵ == 1 ? mpc.Z̃[end] : zero(NT) + info[:ϵ] = getϵ(mpc, Z̃) info[:J] = J info[:U] = U info[:u] = info[:U][1:model.nu] @@ -161,6 +161,15 @@ function getinfo(mpc::PredictiveController{NT}) where NT<:Real return info end +""" + getϵ(mpc::PredictiveController, Z̃) -> ϵ + +Get the slack `ϵ` from the decision vector `Z̃` if present, otherwise return 0. +""" +function getϵ(mpc::PredictiveController, Z̃::AbstractVector{NT}) where NT<:Real + return mpc.nϵ ≠ 0 ? Z̃[end] : zero(NT) +end + """ addinfo!(info, mpc::PredictiveController) -> info @@ -361,6 +370,9 @@ function obj_nonlinprog!( return JR̂y + JΔŨ + JR̂u + E_JE end +"No custom nonlinear constraints `gc` by default, return `gc` unchanged." +con_custom!(gc, ::PredictiveController, _ , _, _ ) = gc + "By default, the economic term is zero." function obj_econ(::PredictiveController, ::SimModel, _ , ::AbstractVector{NT}) where NT return zero(NT) diff --git a/src/controller/linmpc.jl b/src/controller/linmpc.jl index 224d0fdf..8ebeca15 100644 --- a/src/controller/linmpc.jl +++ b/src/controller/linmpc.jl @@ -146,8 +146,8 @@ arguments. This controller allocates memory at each time step for the optimizati - `Cwt=1e5` : slack variable weight ``C`` (scalar), use `Cwt=Inf` for hard constraints only. - `transcription=SingleShooting()` : a [`TranscriptionMethod`](@ref) for the optimization. - `optim=JuMP.Model(OSQP.MathOptInterfaceOSQP.Optimizer)` : quadratic optimizer used in - the predictive controller, provided as a [`JuMP.Model`](https://jump.dev/JuMP.jl/stable/api/JuMP/#JuMP.Model) - (default to [`OSQP`](https://osqp.org/docs/parsers/jump.html) optimizer). + the predictive controller, provided as a [`JuMP.Model`](@extref) object (default to + [`OSQP`](https://osqp.org/docs/parsers/jump.html) optimizer). - additional keyword arguments are passed to [`SteadyKalmanFilter`](@ref) constructor. # Examples @@ -259,11 +259,11 @@ function LinMPC( end """ - init_optimization!(mpc::LinMPC, model::LinModel, optim) + init_optimization!(mpc::LinMPC, model::LinModel, optim::JuMP.GenericModel) -> nothing Init the quadratic optimization for [`LinMPC`](@ref) controllers. """ -function init_optimization!(mpc::LinMPC, model::LinModel, optim) +function init_optimization!(mpc::LinMPC, model::LinModel, optim::JuMP.GenericModel) # --- variables and linear constraints --- con = mpc.con nZ̃ = length(mpc.Z̃) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 36b86a0c..a6032136 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -1,11 +1,20 @@ -const DEFAULT_NONLINMPC_OPTIMIZER = optimizer_with_attributes(Ipopt.Optimizer,"sb"=>"yes") const DEFAULT_NONLINMPC_TRANSCRIPTION = SingleShooting() +const DEFAULT_NONLINMPC_OPTIMIZER = optimizer_with_attributes(Ipopt.Optimizer,"sb"=>"yes") +const DEFAULT_NONLINMPC_GRADIENT = AutoForwardDiff() +const DEFAULT_NONLINMPC_JACDENSE = AutoForwardDiff() +const DEFAULT_NONLINMPC_JACSPARSE = AutoSparse( + AutoForwardDiff(); + sparsity_detector=TracerSparsityDetector(), + coloring_algorithm=GreedyColoringAlgorithm(), +) struct NonLinMPC{ NT<:Real, SE<:StateEstimator, TM<:TranscriptionMethod, - JM<:JuMP.GenericModel, + JM<:JuMP.GenericModel, + GB<:AbstractADType, + JB<:AbstractADType, PT<:Any, JEfunc<:Function, GCfunc<:Function @@ -16,6 +25,8 @@ struct NonLinMPC{ # different since solvers that support non-Float64 are scarce. optim::JM con::ControllerConstraint{NT, GCfunc} + gradient::GB + jacobian::JB Z̃::Vector{NT} ŷ::Vector{NT} Hp::Int @@ -52,12 +63,14 @@ struct NonLinMPC{ function NonLinMPC{NT}( estim::SE, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt, JE::JEfunc, gc!::GCfunc, nc, p::PT, - transcription::TM, optim::JM + transcription::TM, optim::JM, gradient::GB, jacobian::JB ) where { NT<:Real, SE<:StateEstimator, TM<:TranscriptionMethod, JM<:JuMP.GenericModel, + GB<:AbstractADType, + JB<:AbstractADType, PT<:Any, JEfunc<:Function, GCfunc<:Function, @@ -94,8 +107,9 @@ struct NonLinMPC{ nZ̃ = get_nZ(estim, transcription, Hp, Hc) + nϵ Z̃ = zeros(NT, nZ̃) buffer = PredictiveControllerBuffer(estim, transcription, Hp, Hc, nϵ) - mpc = new{NT, SE, TM, JM, PT, JEfunc, GCfunc}( + mpc = new{NT, SE, TM, JM, GB, JB, PT, JEfunc, GCfunc}( estim, transcription, optim, con, + gradient, jacobian, Z̃, ŷ, Hp, Hc, nϵ, weights, @@ -187,8 +201,11 @@ This controller allocates memory at each time step for the optimization. - `p=model.p` : ``J_E`` and ``\mathbf{g_c}`` functions parameter ``\mathbf{p}`` (any type). - `transcription=SingleShooting()` : a [`TranscriptionMethod`](@ref) for the optimization. - `optim=JuMP.Model(Ipopt.Optimizer)` : nonlinear optimizer used in the predictive - controller, provided as a [`JuMP.Model`](https://jump.dev/JuMP.jl/stable/api/JuMP/#JuMP.Model) - (default to [`Ipopt`](https://github.com/jump-dev/Ipopt.jl) optimizer). + controller, provided as a [`JuMP.Model`](@extref) object (default to [`Ipopt`](https://github.com/jump-dev/Ipopt.jl) optimizer). +- `gradient=AutoForwardDiff()` : an `AbstractADType` backend for the gradient of the objective + function, see [`DifferentiationInterface` doc](@extref DifferentiationInterface List). +- `jacobian=default_jacobian(transcription)` : an `AbstractADType` backend for the Jacobian + of the nonlinear constraints, see `gradient` above for the options (default in Extended Help). - additional keyword arguments are passed to [`UnscentedKalmanFilter`](@ref) constructor (or [`SteadyKalmanFilter`](@ref), for [`LinModel`](@ref)). @@ -237,10 +254,19 @@ NonLinMPC controller with a sample time Ts = 10.0 s, Ipopt optimizer, UnscentedK The keyword argument `nc` is the number of elements in `LHS`, and `gc!`, an alias for the `gc` argument (both `gc` and `gc!` accepts non-mutating and mutating functions). - The optimization relies on [`JuMP`](https://github.com/jump-dev/JuMP.jl) automatic - differentiation (AD) to compute the objective and constraint derivatives. Optimizers - generally benefit from exact derivatives like AD. However, the [`NonLinModel`](@ref) - state-space functions must be compatible with this feature. See [Automatic differentiation](https://jump.dev/JuMP.jl/stable/manual/nlp/#Automatic-differentiation) + By default, the optimization relies on dense [`ForwardDiff`](@extref ForwardDiff) + automatic differentiation (AD) to compute the objective and constraint derivatives. One + exception: if `transcription` is not a [`SingleShooting`](@ref), the `jacobian` argument + defaults to this [sparse backend](@extref DifferentiationInterface AutoSparse-object): + ```julia + AutoSparse( + AutoForwardDiff(); + sparsity_detector = TracerSparsityDetector(), + coloring_algorithm = GreedyColoringAlgorithm() + ) + ``` + Optimizers generally benefit from exact derivatives like AD. However, the [`NonLinModel`](@ref) + state-space functions must be compatible with this feature. See [`JuMP` documentation](@extref JuMP Common-mistakes-when-writing-a-user-defined-operator) for common mistakes when writing these functions. Note that if `Cwt≠Inf`, the attribute `nlp_scaling_max_gradient` of `Ipopt` is set to @@ -265,13 +291,15 @@ function NonLinMPC( p = model.p, transcription::TranscriptionMethod = DEFAULT_NONLINMPC_TRANSCRIPTION, optim::JuMP.GenericModel = JuMP.Model(DEFAULT_NONLINMPC_OPTIMIZER, add_bridges=false), + gradient::AbstractADType = DEFAULT_NONLINMPC_GRADIENT, + jacobian::AbstractADType = default_jacobian(transcription), kwargs... ) estim = UnscentedKalmanFilter(model; kwargs...) return NonLinMPC( estim; Hp, Hc, Mwt, Nwt, Lwt, Cwt, Ewt, JE, gc, nc, p, M_Hp, N_Hc, L_Hp, - transcription, optim + transcription, optim, gradient, jacobian ) end @@ -294,13 +322,15 @@ function NonLinMPC( p = model.p, transcription::TranscriptionMethod = DEFAULT_NONLINMPC_TRANSCRIPTION, optim::JuMP.GenericModel = JuMP.Model(DEFAULT_NONLINMPC_OPTIMIZER, add_bridges=false), + gradient::AbstractADType = DEFAULT_NONLINMPC_GRADIENT, + jacobian::AbstractADType = default_jacobian(transcription), kwargs... ) estim = SteadyKalmanFilter(model; kwargs...) return NonLinMPC( estim; Hp, Hc, Mwt, Nwt, Lwt, Cwt, Ewt, JE, gc, nc, p, M_Hp, N_Hc, L_Hp, - transcription, optim + transcription, optim, gradient, jacobian ) end @@ -347,6 +377,8 @@ function NonLinMPC( p = estim.model.p, transcription::TranscriptionMethod = DEFAULT_NONLINMPC_TRANSCRIPTION, optim::JuMP.GenericModel = JuMP.Model(DEFAULT_NONLINMPC_OPTIMIZER, add_bridges=false), + gradient::AbstractADType = DEFAULT_NONLINMPC_GRADIENT, + jacobian::AbstractADType = default_jacobian(transcription), ) where { NT<:Real, SE<:StateEstimator{NT} @@ -360,10 +392,13 @@ function NonLinMPC( gc! = get_mutating_gc(NT, gc) return NonLinMPC{NT}( estim, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt, JE, gc!, nc, p, - transcription, optim + transcription, optim, gradient, jacobian ) end +default_jacobian(::SingleShooting) = DEFAULT_NONLINMPC_JACDENSE +default_jacobian(::TranscriptionMethod) = DEFAULT_NONLINMPC_JACSPARSE + """ validate_JE(NT, JE) -> nothing @@ -477,11 +512,11 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real end """ - init_optimization!(mpc::NonLinMPC, model::SimModel, optim) + init_optimization!(mpc::NonLinMPC, model::SimModel, optim::JuMP.GenericModel) -> nothing Init the nonlinear optimization for [`NonLinMPC`](@ref) controllers. """ -function init_optimization!(mpc::NonLinMPC, model::SimModel, optim) +function init_optimization!(mpc::NonLinMPC, model::SimModel, optim::JuMP.GenericModel) # --- variables and linear constraints --- con, transcription = mpc.con, mpc.transcription nZ̃ = length(mpc.Z̃) @@ -505,7 +540,9 @@ function init_optimization!(mpc::NonLinMPC, model::SimModel, optim) JuMP.set_attribute(optim, "nlp_scaling_max_gradient", 10.0/C) end end - Jfunc, ∇Jfunc!, gfuncs, ∇gfuncs!, geqfuncs, ∇geqfuncs! = get_optim_functions(mpc, optim) + Jfunc, ∇Jfunc!, gfuncs, ∇gfuncs!, geqfuncs, ∇geqfuncs! = get_optim_functions( + mpc, optim + ) @operator(optim, J, nZ̃, Jfunc, ∇Jfunc!) @objective(optim, Min, J(Z̃var...)) init_nonlincon!(mpc, model, transcription, gfuncs, ∇gfuncs!, geqfuncs, ∇geqfuncs!) @@ -530,120 +567,97 @@ This method is really intricate and I'm not proud of it. That's because of 3 ele - These functions are used inside the nonlinear optimization, so they must be type-stable and as efficient as possible. - The `JuMP` NLP syntax forces splatting for the decision variable, which implies use - of `Vararg{T,N}` (see the [performance tip](https://docs.julialang.org/en/v1/manual/performance-tips/#Be-aware-of-when-Julia-avoids-specializing)) - and memoization to avoid redundant computations. This is already complex, but it's even + of `Vararg{T,N}` (see the [performance tip][@extref Julia Be-aware-of-when-Julia-avoids-specializing] + ) and memoization to avoid redundant computations. This is already complex, but it's even worse knowing that most automatic differentiation tools do not support splatting. - The signature of gradient and hessian functions is not the same for univariate (`nZ̃ == 1`) and multivariate (`nZ̃ > 1`) operators in `JuMP`. Both must be defined. -Inspired from: [User-defined operators with vector outputs](https://jump.dev/JuMP.jl/stable/tutorials/nonlinear/tips_and_tricks/#User-defined-operators-with-vector-outputs) +Inspired from: [User-defined operators with vector outputs](@extref JuMP User-defined-operators-with-vector-outputs) """ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT<:Real - model, transcription = mpc.estim.model, mpc.transcription + # ----- 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 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̂ - nΔŨ, nUe, nŶe = nu*Hc + nϵ, nU + nu, nŶ + ny - Ncache = nZ̃ + 3 - myNaN = convert(JNT, NaN) # fill Z̃ with NaNs to force update_simulations! at 1st call: - # ---------------------- differentiation cache --------------------------------------- - Z̃_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(fill(myNaN, nZ̃), Ncache) - ΔŨ_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nΔŨ), Ncache) - x̂0end_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nx̂), Ncache) - Ŷe_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nŶe), Ncache) - Ue_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nUe), Ncache) - Ŷ0_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nŶ), Ncache) - U0_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nU), Ncache) - Û0_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nU), Ncache) - X̂0_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nX̂), Ncache) - gc_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nc), Ncache) - g_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, ng), Ncache) - geq_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, neq), Ncache) - # --------------------- update simulation function ------------------------------------ - function update_simulations!( - Z̃arg::Union{NTuple{N, T}, AbstractVector{T}}, Z̃cache - ) where {N, T<:Real} - if isdifferent(Z̃cache, Z̃arg) - for i in eachindex(Z̃cache) - # Z̃cache .= Z̃arg is type unstable with Z̃arg::NTuple{N, FowardDiff.Dual} - Z̃cache[i] = Z̃arg[i] - end - Z̃ = Z̃cache - ϵ = (nϵ ≠ 0) ? Z̃[end] : zero(T) # ϵ = 0 if nϵ == 0 (meaning no relaxation) - ΔŨ = get_tmp(ΔŨ_cache, T) - x̂0end = get_tmp(x̂0end_cache, T) - Ue, Ŷe = get_tmp(Ue_cache, T), get_tmp(Ŷe_cache, T) - U0, Ŷ0 = get_tmp(U0_cache, T), get_tmp(Ŷ0_cache, T) - X̂0, Û0 = get_tmp(X̂0_cache, T), get_tmp(Û0_cache, T) - gc, g = get_tmp(gc_cache, T), get_tmp(g_cache, T) - geq = get_tmp(geq_cache, T) - U0 = getU0!(U0, mpc, Z̃) - ΔŨ = getΔŨ!(ΔŨ, mpc, transcription, Z̃) - Ŷ0, x̂0end = predict!(Ŷ0, x̂0end, X̂0, Û0, mpc, model, transcription, U0, Z̃) - Ue, Ŷe = extended_vectors!(Ue, Ŷe, mpc, U0, Ŷ0) - 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̃) - end - return nothing - end - # --------------------- objective functions ------------------------------------------- + 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̂) + 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̂) + gc::Vector{JNT}, g::Vector{JNT} = zeros(JNT, nc), zeros(JNT, ng) + geq::Vector{JNT} = zeros(JNT, neq) + # ---------------------- objective function ------------------------------------------- function Jfunc(Z̃arg::Vararg{T, N}) where {N, T<:Real} - update_simulations!(Z̃arg, get_tmp(Z̃_cache, T)) - ΔŨ = get_tmp(ΔŨ_cache, T) - Ue, Ŷe = get_tmp(Ue_cache, T), get_tmp(Ŷe_cache, T) - U0, Ŷ0 = get_tmp(U0_cache, T), get_tmp(Ŷ0_cache, T) + if isdifferent(Z̃arg, Z̃) + Z̃ .= Z̃arg + update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq, mpc, Z̃) + end return obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ)::T end - function Jfunc_vec(Z̃arg::AbstractVector{T}) where T<:Real - update_simulations!(Z̃arg, get_tmp(Z̃_cache, T)) - ΔŨ = get_tmp(ΔŨ_cache, T) - Ue, Ŷe = get_tmp(Ue_cache, T), get_tmp(Ŷe_cache, T) - U0, Ŷ0 = get_tmp(U0_cache, T), get_tmp(Ŷ0_cache, T) - return obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ)::T + 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̃) + return obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ) end - Z̃_∇J = fill(myNaN, nZ̃) - ∇J = Vector{JNT}(undef, nZ̃) # gradient of objective J - ∇J_buffer = GradientBuffer(Jfunc_vec, Z̃_∇J) + 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(gc), Cache(g), Cache(geq), + ) + ∇J_prep = prepare_gradient(Jfunc!, mpc.gradient, Z̃_∇J, ∇J_context...; strict) + ∇J = Vector{JNT}(undef, nZ̃) ∇Jfunc! = if nZ̃ == 1 - function (Z̃arg::T) where T<:Real + function (Z̃arg) Z̃_∇J .= Z̃arg - gradient!(∇J, ∇J_buffer, Z̃_∇J) + gradient!(Jfunc!, ∇J, ∇J_prep, mpc.gradient, Z̃_∇J, ∇J_context...) return ∇J[begin] # univariate syntax, see JuMP.@operator doc end else function (∇J::AbstractVector{T}, Z̃arg::Vararg{T, N}) where {N, T<:Real} Z̃_∇J .= Z̃arg - gradient!(∇J, ∇J_buffer, Z̃_∇J) + gradient!(Jfunc!, ∇J, ∇J_prep, mpc.gradient, Z̃_∇J, ∇J_context...) return ∇J # multivariate syntax, see JuMP.@operator doc end end # --------------------- inequality constraint functions ------------------------------- gfuncs = Vector{Function}(undef, ng) for i in eachindex(gfuncs) - func_i = function (Z̃arg::Vararg{T, N}) where {N, T<:Real} - update_simulations!(Z̃arg, get_tmp(Z̃_cache, T)) - g = get_tmp(g_cache, T) + 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̃) + end return g[i]::T end - gfuncs[i] = func_i + gfuncs[i] = gfunc_i end - function gfunc_vec!(g, Z̃vec::AbstractVector{T}) where T<:Real - update_simulations!(Z̃vec, get_tmp(Z̃_cache, T)) - g .= get_tmp(g_cache, T) - return g + 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̃) end - Z̃_∇g = fill(myNaN, nZ̃) - g_vec = Vector{JNT}(undef, ng) - ∇g = Matrix{JNT}(undef, ng, nZ̃) # Jacobian of inequality constraints g - ∇g_buffer = JacobianBuffer(gfunc_vec!, g_vec, Z̃_∇g) - ∇gfuncs! = Vector{Function}(undef, ng) + 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(gc), Cache(geq), + ) + # temporarily enable all the inequality constraints for sparsity detection: + mpc.con.i_g[1:end-nc] .= true + ∇g_prep = prepare_jacobian(gfunc!, g, mpc.jacobian, Z̃_∇g, ∇g_context...; strict) + mpc.con.i_g[1:end-nc] .= false + ∇g = init_diffmat(JNT, mpc.jacobian, ∇g_prep, nZ̃, ng) + ∇gfuncs! = Vector{Function}(undef, ng) for i in eachindex(∇gfuncs!) - ∇gfuncs![i] = if nZ̃ == 1 + ∇gfuncs_i! = if nZ̃ == 1 function (Z̃arg::T) where T<:Real if isdifferent(Z̃arg, Z̃_∇g) Z̃_∇g .= Z̃arg - jacobian!(∇g, ∇g_buffer, g_vec, Z̃_∇g) + jacobian!(gfunc!, g, ∇g, ∇g_prep, mpc.jacobian, Z̃_∇g, ∇g_context...) end return ∇g[i, begin] # univariate syntax, see JuMP.@operator doc end @@ -651,47 +665,80 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT function (∇g_i, Z̃arg::Vararg{T, N}) where {N, T<:Real} if isdifferent(Z̃arg, Z̃_∇g) Z̃_∇g .= Z̃arg - jacobian!(∇g, ∇g_buffer, g_vec, Z̃_∇g) + jacobian!(gfunc!, g, ∇g, ∇g_prep, mpc.jacobian, Z̃_∇g, ∇g_context...) end return ∇g_i .= @views ∇g[i, :] # multivariate syntax, see JuMP.@operator doc end end + ∇gfuncs![i] = ∇gfuncs_i! end # --------------------- equality constraint functions --------------------------------- geqfuncs = Vector{Function}(undef, neq) for i in eachindex(geqfuncs) - func_i = function (Z̃arg::Vararg{T, N}) where {N, T<:Real} - update_simulations!(Z̃arg, get_tmp(Z̃_cache, T)) - geq = get_tmp(geq_cache, T) + 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̃) + end return geq[i]::T end - geqfuncs[i] = func_i + geqfuncs[i] = geqfunc_i end - function geqfunc_vec!(geq, Z̃vec::AbstractVector{T}) where T<:Real - update_simulations!(Z̃vec, get_tmp(Z̃_cache, T)) - geq .= get_tmp(geq_cache, T) - return geq + 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̃) end - Z̃_∇geq = fill(myNaN, nZ̃) # NaN to force update at 1st call - geq_vec = Vector{JNT}(undef, neq) - ∇geq = Matrix{JNT}(undef, neq, nZ̃) # Jacobian of equality constraints geq - ∇geq_buffer = JacobianBuffer(geqfunc_vec!, geq_vec, Z̃_∇geq) - ∇geqfuncs! = Vector{Function}(undef, neq) + 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(gc), Cache(g) + ) + ∇geq_prep = prepare_jacobian(geqfunc!, geq, mpc.jacobian, Z̃_∇geq, ∇geq_context...; strict) + ∇geq = init_diffmat(JNT, mpc.jacobian, ∇geq_prep, nZ̃, neq) + ∇geqfuncs! = Vector{Function}(undef, neq) for i in eachindex(∇geqfuncs!) # only multivariate syntax, univariate is impossible since nonlinear equality # constraints imply MultipleShooting, thus input increment ΔU and state X̂0 in Z̃: - ∇geqfuncs![i] = + ∇geqfuncs_i! = function (∇geq_i, Z̃arg::Vararg{T, N}) where {N, T<:Real} if isdifferent(Z̃arg, Z̃_∇geq) Z̃_∇geq .= Z̃arg - jacobian!(∇geq, ∇geq_buffer, geq_vec, Z̃_∇geq) + jacobian!( + geqfunc!, geq, ∇geq, ∇geq_prep, mpc.jacobian, Z̃_∇geq, ∇geq_context... + ) end return ∇geq_i .= @views ∇geq[i, :] end + ∇geqfuncs![i] = ∇geqfuncs_i! end return Jfunc, ∇Jfunc!, gfuncs, ∇gfuncs!, geqfuncs, ∇geqfuncs! end +""" + update_predictions!( + ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq, + mpc::PredictiveController, Z̃ + ) -> nothing + +Update in-place all vectors for the predictions of `mpc` controller at decision vector `Z̃`. + +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̃ +) + 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̃) + 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̃) + return nothing +end + @doc raw""" con_custom!(gc, mpc::NonLinMPC, Ue, Ŷe, ϵ) -> gc diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 65737bd7..9e3f5c0e 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -624,7 +624,7 @@ function init_nonlincon!( for i in 1:con.nc name = Symbol("g_c_$i") optim[name] = JuMP.add_nonlinear_operator( - optim, nZ̃, gfuncs[i_base+i]; name + optim, nZ̃, gfuncs[i_base+i], ∇gfuncs![i_base+i]; name ) end end diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index 62516b4d..43e69e5f 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -81,7 +81,7 @@ end Construct a steady-state Kalman Filter with the [`LinModel`](@ref) `model`. The steady-state (or [asymptotic](https://en.wikipedia.org/wiki/Kalman_filter#Asymptotic_form)) -Kalman filter is based on the process model : +Kalman filter is based on the process model: ```math \begin{aligned} \mathbf{x}(k+1) &= @@ -160,7 +160,7 @@ SteadyKalmanFilter estimator with a sample time Ts = 0.5 s, LinModel and: !!! tip Increasing `σQint_u` and `σQint_ym` values increases the integral action "gain". - The constructor pre-compute the steady-state Kalman gain `K̂` with the [`kalman`](https://juliacontrol.github.io/ControlSystems.jl/stable/lib/synthesis/#ControlSystemsBase.kalman-Tuple{Any,%20Any,%20Any,%20Any,%20Any,%20Vararg{Any}}) + The constructor pre-compute the steady-state Kalman gain `K̂` with the [`kalman`](@extref ControlSystemsBase.kalman) function. It can sometimes fail, for example when `model` matrices are ill-conditioned. In such a case, you can try the alternative time-varying [`KalmanFilter`](@ref). """ @@ -802,7 +802,7 @@ See [`init_ukf`](@ref) for the definition of the constants ``\mathbf{m̂, Ŝ}`` superscript in e.g. ``\mathbf{X̂}_{k-1}^j(k)`` refers the vector at the ``j``th column of ``\mathbf{X̂}_{k-1}(k)``. The symbol ``\mathbf{0}`` is a vector with zeros. The number of sigma points is ``n_σ = 2 n_\mathbf{x̂} + 1``. The matrices ``\sqrt{\mathbf{P̂}_{k-1}(k)}`` -and ``\sqrt{\mathbf{P̂}_{k}(k)}`` are the the lower triangular factors of [`cholesky`](https://docs.julialang.org/en/v1/stdlib/LinearAlgebra/#LinearAlgebra.cholesky) +and ``\sqrt{\mathbf{P̂}_{k}(k)}`` are the the lower triangular factors of [`cholesky`](@extref Julia LinearAlgebra.cholesky) results. The correction and prediction step equations are provided below. The correction step is skipped if `estim.direct == true` since it's already done by the user. @@ -951,8 +951,8 @@ Construct an extended Kalman Filter with the [`SimModel`](@ref) `model`. Both [`LinModel`](@ref) and [`NonLinModel`](@ref) are supported. The process model and the keyword arguments are identical to [`UnscentedKalmanFilter`](@ref), except for `α`, `β` and `κ` which do not apply to the extended Kalman Filter. The Jacobians of the augmented model -``\mathbf{f̂, ĥ}`` are computed with [`ForwardDiff.jl`](https://github.com/JuliaDiff/ForwardDiff.jl) -automatic differentiation. This estimator allocates memory for the Jacobians. +``\mathbf{f̂, ĥ}`` are computed with [`ForwardDiff`](@extref ForwardDiff) automatic +differentiation. This estimator allocates memory for the Jacobians. !!! warning See the Extended Help of [`linearize`](@ref) function if you get an error like: @@ -1043,9 +1043,9 @@ augmented process model: \end{aligned} ``` The matrix ``\mathbf{Ĥ^m}`` is the rows of ``\mathbf{Ĥ}`` that are measured outputs. The -function [`ForwardDiff.jacobian`](https://juliadiff.org/ForwardDiff.jl/stable/user/api/#ForwardDiff.jacobian) -automatically computes them. The correction and prediction step equations are provided below. -The correction step is skipped if `estim.direct == true` since it's already done by the user. +Jacobians are computed with [`ForwardDiff`](@extref ForwardDiff). The correction and +prediction step equations are provided below. The correction step is skipped if +`estim.direct == true` since it's already done by the user. # Correction Step ```math diff --git a/src/estimator/luenberger.jl b/src/estimator/luenberger.jl index 6000cf07..26f841e6 100644 --- a/src/estimator/luenberger.jl +++ b/src/estimator/luenberger.jl @@ -77,8 +77,8 @@ Extended Help). The argument `poles` is a vector of `model.nx + sum(nint_u) + su elements specifying the observer poles/eigenvalues (near ``z=0.5`` by default). The observer is constructed with a direct transmission from ``\mathbf{y^m}`` if `direct=true` (a.k.a. current observers, in opposition to the delayed/prediction form). The method computes the -observer gain `K̂` with [`place`](https://juliacontrol.github.io/ControlSystems.jl/stable/lib/synthesis/#ControlSystemsBase.place). -This estimator is allocation-free. +observer gain `K̂` with [`place`](@extref ControlSystemsBase.place) function. This estimator +is allocation-free. # Examples ```jldoctest diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 95318138..b6ae2dbb 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -1,5 +1,7 @@ const DEFAULT_LINMHE_OPTIMIZER = OSQP.MathOptInterfaceOSQP.Optimizer const DEFAULT_NONLINMHE_OPTIMIZER = optimizer_with_attributes(Ipopt.Optimizer,"sb"=>"yes") +const DEFAULT_NONLINMHE_GRADIENT = AutoForwardDiff() +const DEFAULT_NONLINMHE_JACOBIAN = AutoForwardDiff() @doc raw""" Include all the data for the constraints of [`MovingHorizonEstimator`](@ref). @@ -46,6 +48,8 @@ struct MovingHorizonEstimator{ NT<:Real, SM<:SimModel, JM<:JuMP.GenericModel, + GB<:AbstractADType, + JB<:AbstractADType, CE<:StateEstimator, } <: StateEstimator{NT} model::SM @@ -53,6 +57,8 @@ struct MovingHorizonEstimator{ # different since solvers that support non-Float64 are scarce. optim::JM con::EstimatorConstraint{NT} + gradient::GB + jacobian::JB covestim::CE Z̃::Vector{NT} lastu0::Vector{NT} @@ -108,9 +114,18 @@ struct MovingHorizonEstimator{ corrected::Vector{Bool} buffer::StateEstimatorBuffer{NT} function MovingHorizonEstimator{NT}( - model::SM, He, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, Cwt, optim::JM, covestim::CE; + model::SM, + He, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, Cwt, + optim::JM, gradient::GB, jacobian::JB, covestim::CE; direct=true - ) where {NT<:Real, SM<:SimModel{NT}, JM<:JuMP.GenericModel, CE<:StateEstimator{NT}} + ) where { + NT<:Real, + SM<:SimModel{NT}, + JM<:JuMP.GenericModel, + GB<:AbstractADType, + JB<:AbstractADType, + CE<:StateEstimator{NT} + } nu, ny, nd = model.nu, model.ny, model.nd He < 1 && throw(ArgumentError("Estimation horizon He should be ≥ 1")) Cwt < 0 && throw(ArgumentError("Cwt weight should be ≥ 0")) @@ -154,8 +169,10 @@ struct MovingHorizonEstimator{ P̂arr_old = copy(P̂_0) Nk = [0] corrected = [false] - estim = new{NT, SM, JM, CE}( - model, optim, con, covestim, + estim = new{NT, SM, JM, GB, JB, CE}( + model, optim, con, + gradient, jacobian, + covestim, Z̃, lastu0, x̂op, f̂op, x̂0, He, nϵ, i_ym, nx̂, nym, nyu, nxs, @@ -253,9 +270,13 @@ transcription for now. - `σPint_ym_0=fill(1,sum(nint_ym))` or *`sigmaPint_ym_0`* : same than `σP_0` but for the unmeasured disturbances at measured outputs ``\mathbf{P_{int_{ym}}}(0)`` (composed of integrators). - `Cwt=Inf` : slack variable weight ``C``, default to `Inf` meaning hard constraints only. -- `optim=default_optim_mhe(model)` : a [`JuMP.Model`](https://jump.dev/JuMP.jl/stable/api/JuMP/#JuMP.Model) - with a quadratic/nonlinear optimizer for solving (default to [`Ipopt`](https://github.com/jump-dev/Ipopt.jl), +- `optim=default_optim_mhe(model)` : a [`JuMP.Model`](@extref) object with a quadratic or + nonlinear optimizer for solving (default to [`Ipopt`](https://github.com/jump-dev/Ipopt.jl), or [`OSQP`](https://osqp.org/docs/parsers/jump.html) if `model` is a [`LinModel`](@ref)). +- `gradient=AutoForwardDiff()` : an `AbstractADType` backend for the gradient of the objective + function when `model` is not a [`LinModel`](@ref), see [`DifferentiationInterface` doc](@extref DifferentiationInterface List). +- `jacobian=AutoForwardDiff()` : an `AbstractADType` backend for the Jacobian of the + constraints when `model` is not a [`LinModel`](@ref), see `gradient` above for the options. - `direct=true`: construct with a direct transmission from ``\mathbf{y^m}`` (a.k.a. current estimator, in opposition to the delayed/predictor form). @@ -336,10 +357,12 @@ MovingHorizonEstimator estimator with a sample time Ts = 10.0 s, Ipopt optimizer - If `model` is a [`LinModel`](@ref), the optimization is treated as a quadratic program with a time-varying Hessian, which is generally cheaper than nonlinear programming. By default, a [`KalmanFilter`](@ref) estimates the arrival covariance (customizable). - - Else, a nonlinear program with automatic differentiation (AD) solves the optimization. - Optimizers generally benefit from exact derivatives like AD. However, the `f` and `h` - functions must be compatible with this feature. See [Automatic differentiation](https://jump.dev/JuMP.jl/stable/manual/nlp/#Automatic-differentiation) - for common mistakes when writing these functions. An [`UnscentedKalmanFilter`](@ref) + - Else, a nonlinear program with dense [`ForwardDiff`](@extref ForwardDiff) automatic + differentiation (AD) compute the objective and constraint derivatives by default + (customizable). Optimizers generally benefit from exact derivatives like AD. However, + the `f` and `h` functions must be compatible with this feature. See the + [`JuMP` documentation](@extref JuMP Common-mistakes-when-writing-a-user-defined-operator) + for common mistakes when writing these functions. Also, an [`UnscentedKalmanFilter`](@ref) estimates the arrival covariance by default. The slack variable ``ϵ`` relaxes the constraints if enabled, see [`setconstraint!`](@ref). @@ -365,6 +388,8 @@ function MovingHorizonEstimator( sigmaQint_ym = fill(1, max(sum(nint_ym), 0)), Cwt::Real = Inf, optim::JM = default_optim_mhe(model), + gradient::AbstractADType = DEFAULT_NONLINMHE_GRADIENT, + jacobian::AbstractADType = DEFAULT_NONLINMHE_JACOBIAN, direct = true, σP_0 = sigmaP_0, σQ = sigmaQ, @@ -380,7 +405,7 @@ function MovingHorizonEstimator( R̂ = Hermitian(diagm(NT[σR;].^2), :L) isnothing(He) && throw(ArgumentError("Estimation horizon He must be explicitly specified")) return MovingHorizonEstimator( - model, He, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, Cwt; direct, optim + model, He, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, Cwt; direct, optim, gradient, jacobian ) end @@ -391,6 +416,8 @@ default_optim_mhe(::SimModel) = JuMP.Model(DEFAULT_NONLINMHE_OPTIMIZER, add_brid MovingHorizonEstimator( model, He, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, Cwt=Inf; optim=default_optim_mhe(model), + gradient=AutoForwardDiff(), + jacobian=AutoForwardDiff(), direct=true, covestim=default_covestim_mhe(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct) ) @@ -407,12 +434,17 @@ supported types are [`KalmanFilter`](@ref), [`UnscentedKalmanFilter`](@ref) and function MovingHorizonEstimator( model::SM, He, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, Cwt=Inf; optim::JM = default_optim_mhe(model), + gradient::AbstractADType = DEFAULT_NONLINMHE_GRADIENT, + jacobian::AbstractADType = DEFAULT_NONLINMHE_JACOBIAN, direct = true, covestim::CE = default_covestim_mhe(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct) ) where {NT<:Real, SM<:SimModel{NT}, JM<:JuMP.GenericModel, CE<:StateEstimator{NT}} P̂_0, Q̂, R̂ = to_mat(P̂_0), to_mat(Q̂), to_mat(R̂) return MovingHorizonEstimator{NT}( - model, He, i_ym, nint_u, nint_ym, P̂_0, Q̂ , R̂, Cwt, optim, covestim; direct + model, + He, i_ym, nint_u, nint_ym, P̂_0, Q̂ , R̂, Cwt, + optim, gradient, jacobian, covestim; + direct ) end @@ -1179,17 +1211,19 @@ function init_predmat_mhe( end """ - init_optimization!(estim::MovingHorizonEstimator, model::SimModel, optim) + init_optimization!( + estim::MovingHorizonEstimator, model::LinModel, optim::JuMP.GenericModel + ) Init the quadratic optimization of [`MovingHorizonEstimator`](@ref). """ function init_optimization!( - estim::MovingHorizonEstimator, ::LinModel, optim::JuMP.GenericModel + estim::MovingHorizonEstimator, model::LinModel, optim::JuMP.GenericModel, ) nZ̃ = length(estim.Z̃) JuMP.num_variables(optim) == 0 || JuMP.empty!(optim) JuMP.set_silent(optim) - limit_solve_time(estim.optim, estim.model.Ts) + limit_solve_time(optim, model.Ts) @variable(optim, Z̃var[1:nZ̃]) A = estim.con.A[estim.con.i_b, :] b = estim.con.b[estim.con.i_b] @@ -1199,19 +1233,21 @@ function init_optimization!( end """ - init_optimization!(estim::MovingHorizonEstimator, model::SimModel, optim) + init_optimization!( + estim::MovingHorizonEstimator, model::SimModel, optim::JuMP.GenericModel, + ) -> nothing Init the nonlinear optimization of [`MovingHorizonEstimator`](@ref). """ function init_optimization!( - estim::MovingHorizonEstimator, model::SimModel, optim::JuMP.GenericModel{JNT}, + estim::MovingHorizonEstimator, model::SimModel, optim::JuMP.GenericModel{JNT} ) where JNT<:Real C, con = estim.C, estim.con nZ̃ = length(estim.Z̃) # --- variables and linear constraints --- JuMP.num_variables(optim) == 0 || JuMP.empty!(optim) JuMP.set_silent(optim) - limit_solve_time(estim.optim, estim.model.Ts) + limit_solve_time(optim, model.Ts) @variable(optim, Z̃var[1:nZ̃]) A = estim.con.A[con.i_b, :] b = estim.con.b[con.i_b] @@ -1266,7 +1302,7 @@ end """ get_optim_functions( - estim::MovingHorizonEstimator, optim::JuMP.GenericModel + estim::MovingHorizonEstimator, optim::JuMP.GenericModel, ) -> Jfunc, ∇Jfunc!, gfuncs, ∇gfuncs! Return the functions for the nonlinear optimization of [`MovingHorizonEstimator`](@ref). @@ -1280,115 +1316,96 @@ This method is really intricate and I'm not proud of it. That's because of 3 ele - These functions are used inside the nonlinear optimization, so they must be type-stable and as efficient as possible. - The `JuMP` NLP syntax forces splatting for the decision variable, which implies use - of `Vararg{T,N}` (see the [performance tip](https://docs.julialang.org/en/v1/manual/performance-tips/#Be-aware-of-when-Julia-avoids-specializing)) + of `Vararg{T,N}` (see the [performance tip](@extref Julia Be-aware-of-when-Julia-avoids-specializing)) and memoization to avoid redundant computations. This is already complex, but it's even worse knowing that most automatic differentiation tools do not support splatting. - The signature of gradient and hessian functions is not the same for univariate (`nZ̃ == 1`) and multivariate (`nZ̃ > 1`) operators in `JuMP`. Both must be defined. -Inspired from: [User-defined operators with vector outputs](https://jump.dev/JuMP.jl/stable/tutorials/nonlinear/tips_and_tricks/#User-defined-operators-with-vector-outputs) +Inspired from: [User-defined operators with vector outputs](@extref JuMP User-defined-operators-with-vector-outputs) """ function get_optim_functions( - estim::MovingHorizonEstimator, ::JuMP.GenericModel{JNT} + estim::MovingHorizonEstimator, ::JuMP.GenericModel{JNT}, ) 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 nV̂, nX̂, ng, nZ̃ = He*nym, He*nx̂, length(con.i_g), length(estim.Z̃) - Ncache = nZ̃ + 3 - myNaN = convert(JNT, NaN) # fill Z̃ with NaNs to force update_simulations! at 1st call: - # ---------------------- differentiation cache --------------------------------------- - Z̃_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(fill(myNaN, nZ̃), Ncache) - V̂_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nV̂), Ncache) - g_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, ng), Ncache) - X̂0_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nX̂), Ncache) - x̄_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nx̂), Ncache) - û0_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nu), Ncache) - ŷ0_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nŷ), Ncache) - # --------------------- update simulation function ------------------------------------ - function update_simulations!( - Z̃arg::Union{NTuple{N, T}, AbstractVector{T}}, Z̃cache - ) where {N, T <:Real} - if isdifferent(Z̃cache, Z̃arg) - for i in eachindex(Z̃cache) - # Z̃cache .= Z̃arg is type unstable with Z̃arg::NTuple{N, FowardDiff.Dual} - Z̃cache[i] = Z̃arg[i] - end - Z̃ = Z̃cache - ϵ = (nϵ ≠ 0) ? Z̃[begin] : zero(T) # ϵ = 0 if Cwt=Inf (meaning: no relaxation) - V̂, X̂0 = get_tmp(V̂_cache, T), get_tmp(X̂0_cache, T) - û0, ŷ0 = get_tmp(û0_cache, T), get_tmp(ŷ0_cache, T) - g = get_tmp(g_cache, T) - V̂, X̂0 = predict!(V̂, X̂0, û0, ŷ0, estim, model, Z̃) - g = con_nonlinprog!(g, estim, model, X̂0, V̂, ϵ) - end - return nothing - end + 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̂) + û0::Vector{JNT}, ŷ0::Vector{JNT} = zeros(JNT, nu), zeros(JNT, nŷ) + g::Vector{JNT} = zeros(JNT, ng) + x̄::Vector{JNT} = zeros(JNT, nx̂) # --------------------- objective functions ------------------------------------------- function Jfunc(Z̃arg::Vararg{T, N}) where {N, T<:Real} - Z̃ = get_tmp(Z̃_cache, T) - update_simulations!(Z̃arg, Z̃) - x̄, V̂ = get_tmp(x̄_cache, T), get_tmp(V̂_cache, T) + if isdifferent(Z̃arg, Z̃) + Z̃ .= Z̃arg + update_prediction!(V̂, X̂0, û0, ŷ0, g, estim, Z̃) + end return obj_nonlinprog!(x̄, estim, model, V̂, Z̃)::T end - function Jfunc_vec(Z̃arg::AbstractVector{T}) where T<:Real - Z̃ = get_tmp(Z̃_cache, T) - update_simulations!(Z̃arg, Z̃) - x̄, V̂ = get_tmp(x̄_cache, T), get_tmp(V̂_cache, T) - return obj_nonlinprog!(x̄, estim, model, V̂, Z̃)::T + function Jfunc!(Z̃, V̂, X̂0, û0, ŷ0, g, x̄) + update_prediction!(V̂, X̂0, û0, ŷ0, g, estim, Z̃) + return obj_nonlinprog!(x̄, estim, model, V̂, Z̃) end - Z̃_∇J = fill(myNaN, nZ̃) - ∇J = Vector{JNT}(undef, nZ̃) # gradient of objective J - ∇J_buffer = GradientBuffer(Jfunc_vec, Z̃_∇J) - ∇Jfunc! = if nZ̃ == 1 - function (Z̃arg::T) where T<:Real - Z̃_∇J .= Z̃arg - gradient!(∇J, ∇J_buffer, Z̃_∇J) - return ∇J[begin] # univariate syntax, see JuMP.@operator doc - end - else - function (∇J::AbstractVector{T}, Z̃arg::Vararg{T, N}) where {N, T<:Real} - Z̃_∇J .= Z̃arg - gradient!(∇J, ∇J_buffer, Z̃_∇J) - return ∇J # multivariate syntax, see JuMP.@operator doc - end + Z̃_∇J = fill(myNaN, nZ̃) + ∇J_context = ( + Cache(V̂), Cache(X̂0), + Cache(û0), Cache(ŷ0), + Cache(g), + Cache(x̄), + ) + # temporarily "fill" the estimation window for the preperation of the gradient: + estim.Nk[] = He + ∇J_prep = prepare_gradient(Jfunc!, estim.gradient, Z̃_∇J, ∇J_context...; strict) + estim.Nk[] = 0 + ∇J = Vector{JNT}(undef, nZ̃) + ∇Jfunc! = function (∇J::AbstractVector{T}, Z̃arg::Vararg{T, N}) where {N, T<:Real} + # only the multivariate syntax of JuMP.@operator, univariate is impossible for MHE + # since Z̃ comprises the arrival state estimate AND the estimated process noise + Z̃_∇J .= Z̃arg + gradient!(Jfunc!, ∇J, ∇J_prep, estim.gradient, Z̃_∇J, ∇J_context...) + return ∇J end + # --------------------- inequality constraint functions ------------------------------- gfuncs = Vector{Function}(undef, ng) for i in eachindex(gfuncs) - func_i = function (Z̃arg::Vararg{T, N}) where {N, T<:Real} - update_simulations!(Z̃arg, get_tmp(Z̃_cache, T)) - g = get_tmp(g_cache, T) + 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̃) + end return g[i]::T end - gfuncs[i] = func_i + gfuncs[i] = gfunc_i end - function gfunc_vec!(g, Z̃vec::AbstractVector{T}) where T<:Real - update_simulations!(Z̃vec, get_tmp(Z̃_cache, T)) - g .= get_tmp(g_cache, T) - return g + function gfunc!(g, Z̃, V̂, X̂0, û0, ŷ0) + return update_prediction!(V̂, X̂0, û0, ŷ0, g, estim, Z̃) end - Z̃_∇g = fill(myNaN, nZ̃) - g_vec = Vector{JNT}(undef, ng) - ∇g = Matrix{JNT}(undef, ng, nZ̃) # Jacobian of inequality constraints g - ∇g_buffer = JacobianBuffer(gfunc_vec!, g_vec, Z̃_∇g) - ∇gfuncs! = Vector{Function}(undef, ng) + Z̃_∇g = fill(myNaN, nZ̃) + ∇g_context = ( + Cache(V̂), Cache(X̂0), + Cache(û0), Cache(ŷ0), + ) + # temporarily enable all the inequality constraints for sparsity detection: + estim.con.i_g .= true + estim.Nk[] = He + ∇g_prep = prepare_jacobian(gfunc!, g, estim.jacobian, Z̃_∇g, ∇g_context...; strict) + estim.con.i_g .= false + estim.Nk[] = 0 + ∇g = init_diffmat(JNT, estim.jacobian, ∇g_prep, nZ̃, ng) + ∇gfuncs! = Vector{Function}(undef, ng) for i in eachindex(∇gfuncs!) - ∇gfuncs![i] = if nZ̃ == 1 - function (Z̃arg::T) where T<:Real - if isdifferent(Z̃arg, Z̃_∇g) - Z̃_∇g .= Z̃arg - jacobian!(∇g, ∇g_buffer, g_vec, Z̃_∇g) - end - return ∇g[i, begin] # univariate syntax, see JuMP.@operator doc - end - else - function (∇g_i, Z̃arg::Vararg{T, N}) where {N, T<:Real} - if isdifferent(Z̃arg, Z̃_∇g) - Z̃_∇g .= Z̃arg - jacobian!(∇g, ∇g_buffer, g_vec, Z̃_∇g) - end - return ∇g_i .= @views ∇g[i, :] # multivariate syntax, see JuMP.@operator doc + ∇gfuncs![i] = function (∇g_i, Z̃arg::Vararg{T, N}) where {N, T<:Real} + # only the multivariate syntax of JuMP.@operator, see above for the explanation + if isdifferent(Z̃arg, Z̃_∇g) + Z̃_∇g .= Z̃arg + jacobian!(gfunc!, g, ∇g, ∇g_prep, estim.jacobian, Z̃_∇g, ∇g_context...) end + return ∇g_i .= @views ∇g[i, :] end end return Jfunc, ∇Jfunc!, gfuncs, ∇gfuncs! diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 0511eba8..de73b941 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -160,6 +160,15 @@ function getinfo(estim::MovingHorizonEstimator{NT}) where NT<:Real return info end +""" + getϵ(estim::MovingHorizonEstimator, Z̃) -> ϵ + +Get the slack `ϵ` from the decision vector `Z̃` if present, otherwise return 0. +""" +function getϵ(estim::MovingHorizonEstimator, Z̃::AbstractVector{NT}) where NT<:Real + return estim.nϵ ≠ 0 ? Z̃[begin] : zero(NT) +end + """ add_data_windows!(estim::MovingHorizonEstimator, y0m, d0, u0=estim.lastu0) -> ismoving @@ -370,7 +379,7 @@ where ``\mathbf{ŵ}_{k-1}(k-j)`` is the input increment for time ``k-j`` comput last time step ``k-1``. It then calls `JuMP.optimize!(estim.optim)` and extract the solution. A failed optimization prints an `@error` log in the REPL and returns the warm-start value. A failed optimization also prints [`getinfo`](@ref) results in -the debug log [if activated](https://docs.julialang.org/en/v1/stdlib/Logging/#Example:-Enable-debug-level-messages). +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 @@ -501,7 +510,9 @@ Objective function of [`MovingHorizonEstimator`](@ref) when `model` is a [`LinMo It can be called on a [`MovingHorizonEstimator`](@ref) object to evaluate the objective function at specific `Z̃` and `V̂` values. """ -function obj_nonlinprog!( _ , estim::MovingHorizonEstimator, ::LinModel, _ , Z̃) +function obj_nonlinprog!( + _ , estim::MovingHorizonEstimator, ::LinModel, _ , Z̃::AbstractVector{NT} +) where NT<:Real return obj_quadprog(Z̃, estim.H̃, estim.q̃) + estim.r[] end @@ -513,14 +524,16 @@ Objective function of the MHE when `model` is not a [`LinModel`](@ref). The function `dot(x, A, x)` is a performant way of calculating `x'*A*x`. This method mutates `x̄` vector arguments. """ -function obj_nonlinprog!(x̄, estim::MovingHorizonEstimator, ::SimModel, V̂, Z̃) +function obj_nonlinprog!( + x̄, estim::MovingHorizonEstimator, ::SimModel, V̂, Z̃::AbstractVector{NT} +) where NT<:Real nϵ, Nk = estim.nϵ, estim.Nk[] nYm, nŴ, nx̂, invP̄ = Nk*estim.nym, Nk*estim.nx̂, estim.nx̂, estim.invP̄ nx̃ = nϵ + nx̂ invQ̂_Nk, invR̂_Nk = @views estim.invQ̂_He[1:nŴ, 1:nŴ], estim.invR̂_He[1:nYm, 1:nYm] x̂0arr, Ŵ, V̂ = @views Z̃[nx̃-nx̂+1:nx̃], Z̃[nx̃+1:nx̃+nŴ], V̂[1:nYm] x̄ .= estim.x̂0arr_old .- x̂0arr - Jϵ = nϵ ≠ 0 ? estim.C*Z̃[begin]^2 : 0 + Jϵ = nϵ ≠ 0 ? estim.C*Z̃[begin]^2 : zero(NT) return dot(x̄, invP̄, x̄) + dot(Ŵ, invQ̂_Nk, Ŵ) + dot(V̂, invR̂_Nk, V̂) + Jϵ end @@ -582,6 +595,22 @@ function predict!(V̂, X̂0, û0, ŷ0, estim::MovingHorizonEstimator, model::S return V̂, X̂0 end + +""" + update_predictions!(V̂, X̂0, û0, ŷ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̃) + model = estim.model + V̂, X̂0 = predict!(V̂, X̂0, û0, ŷ0, estim, model, Z̃) + ϵ = getϵ(estim, Z̃) + g = con_nonlinprog!(g, estim, model, X̂0, V̂, ϵ) + return nothing +end + """ con_nonlinprog!(g, estim::MovingHorizonEstimator, model::SimModel, X̂0, V̂, ϵ) diff --git a/src/general.jl b/src/general.jl index 700c3f1c..76330c08 100644 --- a/src/general.jl +++ b/src/general.jl @@ -13,20 +13,6 @@ function Base.show(io::IO, buffer::DifferentiationBuffer) return print(io, "DifferentiationBuffer with a $(typeof(buffer.config).name.name)") end -"Struct with both function and configuration for ForwardDiff gradient." -struct GradientBuffer{FT<:Function, CT<:ForwardDiff.GradientConfig} <: DifferentiationBuffer - f::FT - config::CT -end - -"Create a GradientBuffer with function `f` and input `x`." -GradientBuffer(f, x) = GradientBuffer(f, ForwardDiff.GradientConfig(f, x)) - -"Compute in-place and return the gradient of `buffer.f` at `x`." -function gradient!(g, buffer::GradientBuffer, x) - return ForwardDiff.gradient!(g, buffer.f, x, buffer.config) -end - "Struct with both function and configuration for ForwardDiff Jacobian." struct JacobianBuffer{FT<:Function, CT<:ForwardDiff.JacobianConfig} <: DifferentiationBuffer f!::FT @@ -37,10 +23,14 @@ end JacobianBuffer(f!, y, x) = JacobianBuffer(f!, ForwardDiff.JacobianConfig(f!, y, x)) "Compute in-place and return the Jacobian matrix of `buffer.f!` at `x`." -function jacobian!(A, buffer::JacobianBuffer, y, x) +function get_jacobian!(A, buffer::JacobianBuffer, y, x) return ForwardDiff.jacobian!(A, buffer.f!, y, x, buffer.config) end +"Init a differentiation result matrix as dense or sparse matrix, as required by `backend`." +init_diffmat(T, backend::AbstractADType, _ , nx , ny) = Matrix{T}(undef, ny, nx) +init_diffmat(T, backend::AutoSparse ,prep , _ , _ ) = similar(sparsity_pattern(prep), T) + "Termination status that means 'no solution available'." const ERROR_STATUSES = ( JuMP.INFEASIBLE, JuMP.DUAL_INFEASIBLE, JuMP.LOCALLY_INFEASIBLE, diff --git a/src/model/linearization.jl b/src/model/linearization.jl index 6fc47130..65626caf 100644 --- a/src/model/linearization.jl +++ b/src/model/linearization.jl @@ -75,7 +75,7 @@ Linearize `model` at the operating points `x`, `u`, `d` and return the [`LinMode The arguments `x`, `u` and `d` are the linearization points for the state ``\mathbf{x}``, manipulated input ``\mathbf{u}`` and measured disturbance ``\mathbf{d}``, respectively (not necessarily an equilibrium, details in Extended Help). The Jacobians of ``\mathbf{f}`` and -``\mathbf{h}`` functions are automatically computed with [`ForwardDiff.jl`](https://github.com/JuliaDiff/ForwardDiff.jl). +``\mathbf{h}`` functions are automatically computed with [`ForwardDiff`](@extref ForwardDiff). !!! warning See Extended Help if you get an error like: @@ -131,7 +131,7 @@ julia> linmodel.A equations are similar if the nonlinear model has nonzero operating points. Automatic differentiation (AD) allows exact Jacobians. The [`NonLinModel`](@ref) `f` and - `h` functions must be compatible with this feature though. See [Automatic differentiation](https://jump.dev/JuMP.jl/stable/manual/nlp/#Automatic-differentiation) + `h` functions must be compatible with this feature though. See [`JuMP` documentation](@extref JuMP Common-mistakes-when-writing-a-user-defined-operator) for common mistakes when writing these functions. """ function linearize(model::SimModel{NT}; kwargs...) where NT<:Real @@ -209,11 +209,11 @@ function get_jacobians!(linmodel::LinModel, xnext0, y0, model::SimModel, x0, u0, linbuffer.x .= x0 linbuffer.u .= u0 linbuffer.d .= d0 - jacobian!(linmodel.A, linbuffer.buffer_f_at_u_d, xnext0, x0) - jacobian!(linmodel.Bu, linbuffer.buffer_f_at_x_d, xnext0, u0) - jacobian!(linmodel.Bd, linbuffer.buffer_f_at_x_u, xnext0, d0) - jacobian!(linmodel.C, linbuffer.buffer_h_at_d, y0, x0) - jacobian!(linmodel.Dd, linbuffer.buffer_h_at_x, y0, d0) + get_jacobian!(linmodel.A, linbuffer.buffer_f_at_u_d, xnext0, x0) + get_jacobian!(linmodel.Bu, linbuffer.buffer_f_at_x_d, xnext0, u0) + get_jacobian!(linmodel.Bd, linbuffer.buffer_f_at_x_u, xnext0, d0) + get_jacobian!(linmodel.C, linbuffer.buffer_h_at_d, y0, x0) + get_jacobian!(linmodel.Dd, linbuffer.buffer_h_at_x, y0, d0) return nothing end diff --git a/src/model/linmodel.jl b/src/model/linmodel.jl index 24f40ba0..041ebeea 100644 --- a/src/model/linmodel.jl +++ b/src/model/linmodel.jl @@ -84,7 +84,7 @@ resamples discrete ones if `Ts ≠ sys.Ts`, computes a new realization if not mi separates the ``\mathbf{z}`` terms in two parts (details in Extended Help). The rest of the documentation assumes discrete dynamics since all systems end up in this form. -See also [`ss`](https://juliacontrol.github.io/ControlSystems.jl/stable/lib/constructors/#ControlSystemsBase.ss) +See also [`ss`](@extref ControlSystemsBase.ss) # Examples ```jldoctest @@ -112,12 +112,12 @@ LinModel with a sample time Ts = 0.1 s and: \mathbf{y}(k) &= \mathbf{C x}(k) + \mathbf{D z}(k) \end{aligned} ``` - Continuous dynamics are internally discretized using [`c2d`](https://juliacontrol.github.io/ControlSystems.jl/stable/lib/constructors/#ControlSystemsBase.c2d) - and `:zoh` for manipulated inputs, and `:tustin`, for measured disturbances. Lastly, if + Continuous dynamics are internally discretized using [`c2d`](@extref ControlSystemsBase.c2d) + and `:zoh` for manipulated inputs, and `:tustin`, for measured disturbances. Lastly, if `sys` is discrete and the provided argument `Ts ≠ sys.Ts`, the system is resampled by using the aforementioned discretization methods. - Note that the constructor transforms the system to its minimal realization using [`minreal`](https://juliacontrol.github.io/ControlSystems.jl/stable/lib/constructors/#ControlSystemsBase.minreal) + Note that the constructor transforms the system to its minimal realization using [`minreal`](@extref ControlSystemsBase.minreal) for controllability/observability. As a consequence, the final state-space representation may be different from the one provided in `sys`. It is also converted into a more practical form (``\mathbf{D_u=0}`` because of the zero-order hold): @@ -192,7 +192,7 @@ Convert to minimal realization state-space when `sys` is a transfer function. `sys` is equal to ``\frac{\mathbf{y}(s)}{\mathbf{z}(s)}`` for continuous-time, and ``\frac{\mathbf{y}(z)}{\mathbf{z}(z)}``, for discrete-time. -See also [`tf`](https://juliacontrol.github.io/ControlSystems.jl/stable/lib/constructors/#ControlSystemsBase.tf) +See also [`tf`](@extref ControlSystemsBase.tf) # Examples ```jldoctest diff --git a/src/model/nonlinmodel.jl b/src/model/nonlinmodel.jl index 96dd64b6..b5676e82 100644 --- a/src/model/nonlinmodel.jl +++ b/src/model/nonlinmodel.jl @@ -95,7 +95,8 @@ form. The optional parameter `NT` explicitly set the number type of vectors (def !!! warning The two functions must be in pure Julia to use the model in [`NonLinMPC`](@ref), - [`ExtendedKalmanFilter`](@ref), [`MovingHorizonEstimator`](@ref) and [`linearize`](@ref). + [`ExtendedKalmanFilter`](@ref), [`MovingHorizonEstimator`](@ref) and [`linearize`](@ref), + except if a finite difference backend is used (e.g. [`AutoFiniteDiff`](@extref DifferentiationInterface List). See also [`LinModel`](@ref). diff --git a/src/sim_model.jl b/src/sim_model.jl index aaad33f8..d169745c 100644 --- a/src/sim_model.jl +++ b/src/sim_model.jl @@ -301,10 +301,10 @@ end Sleep for `model.Ts` s minus the time elapsed since the last call to [`savetime!`](@ref). -It calls [`sleep`](https://docs.julialang.org/en/v1/base/parallel/#Base.sleep) if `busywait` -is `false`. Else, a simple `while` loop implements busy-waiting. As a rule-of-thumb, -busy-waiting should be used if `model.Ts < 0.1` s, since the accuracy of `sleep` is around 1 -ms. Can be used to implement simple soft real-time simulations, see the example below. +It calls [`sleep`](@extref Julia Base.sleep) if `busywait` is `false`. Else, a simple +`while` loop implements busy-waiting. As a rule-of-thumb, busy-waiting should be used if +`model.Ts < 0.1` s, since the accuracy of `sleep` is around 1 ms. Can be used to implement +simple soft real-time simulations, see the example below. !!! warning The allocations in Julia are garbage-collected (GC) automatically. This can affect the diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index fa2eb408..5a807ad8 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -781,11 +781,13 @@ end end @testitem "MovingHorizonEstimator construction" setup=[SetupMPCtests] begin - using .SetupMPCtests, ControlSystemsBase, LinearAlgebra, JuMP, Ipopt + using .SetupMPCtests, ControlSystemsBase, LinearAlgebra + using JuMP, Ipopt, DifferentiationInterface + import FiniteDiff linmodel1 = LinModel(sys,Ts,i_d=[3]) - f(x,u,d,_) = linmodel1.A*x + linmodel1.Bu*u + linmodel1.Bd*d - h(x,d,_) = linmodel1.C*x + linmodel1.Du*d - nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing) + f(x,u,d,model) = model.A*x + model.Bu*u + model.Bd*d + h(x,d,model) = model.C*x + model.Dd*d + nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing, p=linmodel1) mhe1 = MovingHorizonEstimator(linmodel1, He=5) @test mhe1.nym == 2 @@ -857,6 +859,12 @@ end mhe13 = MovingHorizonEstimator(linmodel2, He=5) @test isa(mhe13, MovingHorizonEstimator{Float32}) + mhe14 = MovingHorizonEstimator( + nonlinmodel, He=5, gradient=AutoFiniteDiff(), jacobian=AutoFiniteDiff() + ) + @test mhe14.gradient == AutoFiniteDiff() + @test mhe14.jacobian == AutoFiniteDiff() + @test_throws ArgumentError MovingHorizonEstimator(linmodel1) @test_throws ArgumentError MovingHorizonEstimator(linmodel1, He=0) @test_throws ArgumentError MovingHorizonEstimator(linmodel1, Cwt=-1) @@ -864,10 +872,12 @@ end @testitem "MovingHorizonEstimator estimation and getinfo" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra, JuMP, Ipopt, ForwardDiff - linmodel1 = setop!(LinModel(sys,Ts,i_u=[1,2], i_d=[3]), uop=[10,50], yop=[50,30], dop=[5]) - f(x,u,d,_) = linmodel1.A*x + linmodel1.Bu*u + linmodel1.Bd*d - h(x,d,_) = linmodel1.C*x + linmodel1.Dd*d - nonlinmodel = setop!(NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing), uop=[10,50], yop=[50,30], dop=[5]) + linmodel1 = LinModel(sys,Ts,i_u=[1,2], i_d=[3]) + linmodel1 = setop!(linmodel1, uop=[10,50], yop=[50,30], dop=[5]) + f(x,u,d,model) = model.A*x + model.Bu*u + model.Bd*d + h(x,d,model) = model.C*x + model.Dd*d + nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing, p=linmodel1) + nonlinmodel = setop!(nonlinmodel, uop=[10,50], yop=[50,30], dop=[5]) mhe1 = MovingHorizonEstimator(nonlinmodel, He=2) preparestate!(mhe1, [50, 30], [5]) @@ -968,13 +978,10 @@ end x̂ = updatestate!(mhe3, [0], [0]) @test x̂ ≈ [0, 0] atol=1e-3 @test isa(x̂, Vector{Float32}) - mhe4 = setconstraint!(MovingHorizonEstimator(nonlinmodel, He=1, nint_ym=0), v̂max=[50,50]) g_V̂max_end = mhe4.optim[:g_V̂max_2].func - # test gfunc_i(i,::NTuple{N, Float64}) - @test g_V̂max_end(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) ≤ 0.0 - # test gfunc_i(i,::NTuple{N, ForwardDiff.Dual}) : - @test ForwardDiff.gradient(vec->g_V̂max_end(vec...), zeros(8)) ≈ zeros(8) + # execute update_predictions! branch in `gfunc_i` for coverage: + @test_nowarn g_V̂max_end(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) ≤ 0.0 Q̂ = diagm([1/4, 1/4, 1/4, 1/4].^2) R̂ = diagm([1, 1].^2) diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index d099cb6b..5dc8e745 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -555,7 +555,9 @@ end end @testitem "NonLinMPC construction" setup=[SetupMPCtests] begin - using .SetupMPCtests, ControlSystemsBase, LinearAlgebra, JuMP, Ipopt + using .SetupMPCtests, ControlSystemsBase, LinearAlgebra + using JuMP, Ipopt, DifferentiationInterface + import FiniteDiff linmodel1 = LinModel(sys,Ts,i_d=[3]) nmpc0 = NonLinMPC(linmodel1, Hp=15) @test isa(nmpc0.estim, SteadyKalmanFilter) @@ -612,6 +614,12 @@ end @test nmpc17.transcription == MultipleShooting() @test length(nmpc17.Z̃) == linmodel1.nu*nmpc17.Hc + nmpc17.estim.nx̂*nmpc17.Hp + nmpc17.nϵ @test size(nmpc17.con.Aeq, 1) == nmpc17.estim.nx̂*nmpc17.Hp + nmpc18 = NonLinMPC(nonlinmodel, Hp=10, + gradient=AutoFiniteDiff(), + jacobian=AutoFiniteDiff() + ) + @test nmpc18.gradient == AutoFiniteDiff() + @test nmpc18.jacobian == AutoFiniteDiff() nonlinmodel2 = NonLinModel{Float32}(f, h, Ts, 2, 4, 2, 1, solver=nothing) nmpc15 = NonLinMPC(nonlinmodel2, Hp=15) @@ -629,7 +637,9 @@ end end @testitem "NonLinMPC moves and getinfo" setup=[SetupMPCtests] begin - using .SetupMPCtests, ControlSystemsBase, LinearAlgebra, ForwardDiff + using .SetupMPCtests, ControlSystemsBase, LinearAlgebra + using DifferentiationInterface + import FiniteDiff linmodel = setop!(LinModel(tf(5, [2000, 1]), 3000.0), yop=[10]) Hp = 100 nmpc_lin = NonLinMPC(linmodel, Nwt=[0], Hp=Hp, Hc=1) @@ -661,9 +671,9 @@ end u = moveinput!(nmpc) @test u ≈ [4] atol=5e-2 linmodel2 = LinModel([tf(5, [2000, 1]) tf(7, [8000,1])], 3000.0, i_d=[2]) - f = (x,u,d,_) -> linmodel2.A*x + linmodel2.Bu*u + linmodel2.Bd*d - h = (x,d,_) -> linmodel2.C*x + linmodel2.Dd*d - nonlinmodel = NonLinModel(f, h, 3000.0, 1, 2, 1, 1, solver=nothing) + f = (x,u,d,model) -> model.A*x + model.Bu*u + model.Bd*d + h = (x,d,model) -> model.C*x + model.Dd*d + nonlinmodel = NonLinModel(f, h, 3000.0, 1, 2, 1, 1, solver=nothing, p=linmodel2) nmpc2 = NonLinMPC(nonlinmodel, Nwt=[0], Hp=100, Hc=1) preparestate!(nmpc2, [0], [0]) # if d=[0.1], the output will eventually reach 7*0.1=0.7, no action needed (u=0): @@ -683,20 +693,23 @@ end preparestate!(nmpc4, [0], [0]) u = moveinput!(nmpc4, [0], d, R̂u=fill(12, nmpc4.Hp)) @test u ≈ [12] atol=5e-2 - nmpc5 = setconstraint!(NonLinMPC(nonlinmodel, Hp=15, Cwt=Inf), ymin=[1]) - g_Y0min_end = nmpc5.optim[:g_Y0min_15].func - # test gfunc_i(i,::NTuple{N, Float64}): - @test g_Y0min_end(20.0, 10.0) ≤ 0.0 - # test gfunc_i(i,::NTuple{N, ForwardDiff.Dual}) : - @test ForwardDiff.gradient(vec->g_Y0min_end(vec...), [20.0, 10.0]) ≈ [-5, -5] atol=1e-3 linmodel3 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), 0, 0, 3000.0) + nmpc5 = NonLinMPC(nonlinmodel, Hp=1, Hc=1, Cwt=Inf, transcription=MultipleShooting()) + nmpc5 = setconstraint!(nmpc5, ymin=[1]) + # execute update_predictions! branch in `gfunc_i` for coverage: + g_Y0min_end = nmpc5.optim[:g_Y0min_1].func + println(nmpc5.Z̃) + @test_nowarn g_Y0min_end(10.0, 9.0, 8.0, 7.0) + # execute update_predictions! branch in `geqfunc_i` for coverage: + geq_end = nmpc5.optim[:geq_2].func + @test_nowarn geq_end(5.0, 4.0, 3.0, 2.0) nmpc6 = NonLinMPC(linmodel3, Hp=10) preparestate!(nmpc6, [0]) @test moveinput!(nmpc6, [0]) ≈ [0.0] - nonlinmodel2 = NonLinModel{Float32}(f, h, 3000.0, 1, 2, 1, 1, solver=nothing) + 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], Float32[]) + nonlinmodel2.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()) @@ -713,6 +726,19 @@ end info = getinfo(nmpc9) @test info[:u] ≈ u @test info[:Ŷ][end] ≈ 20 atol=5e-2 + nmpc10 = setconstraint!(NonLinMPC( + nonlinmodel, Nwt=[0], Hp=100, Hc=1, + gradient=AutoFiniteDiff(), + jacobian=AutoFiniteDiff()), + ymax=[100], ymin=[-100] + ) + preparestate!(nmpc10, [0], [0]) + u = moveinput!(nmpc10, [10], [0]) + @test u ≈ [2] atol=5e-2 + info = getinfo(nmpc10) + @test info[:u] ≈ u + @test info[:Ŷ][end] ≈ 10 atol=5e-2 + @test_nowarn ModelPredictiveControl.info2debugstr(info) end