diff --git a/Project.toml b/Project.toml
index 52915e111..8be585cce 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 c93120ee4..63bb8bfc0 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 cd71da677..1be7cf0fe 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 37de69d4b..de69e08d8 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 230b4459e..4be5bb160 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 29f616bc8..605ed0af1 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 57bc2f28d..1c5032b3b 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 bf65b67c8..ead8faf96 100644
--- a/src/ModelPredictiveControl.jl
+++ b/src/ModelPredictiveControl.jl
@@ -1,12 +1,19 @@
module ModelPredictiveControl
-using PrecompileTools
+using PrecompileTools
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
import ControlSystemsBase
import ControlSystemsBase: ss, tf, delay
@@ -18,7 +25,7 @@ import JuMP
import JuMP: MOIU, MOI, GenericModel, Model, optimizer_with_attributes, register
import JuMP: @variable, @operator, @constraint, @objective
-import PreallocationTools: DiffCache, get_tmp
+import PreallocationTools: DiffCache, get_tmp # TODO: remove this dep if possible (with Cache of DI.jl)
import OSQP, Ipopt
diff --git a/src/controller/execute.jl b/src/controller/execute.jl
index d9176d3e3..d7cb78b81 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 224d0fdf7..d13cab407 100644
--- a/src/controller/linmpc.jl
+++ b/src/controller/linmpc.jl
@@ -135,7 +135,7 @@ arguments. This controller allocates memory at each time step for the optimizati
# Arguments
- `model::LinModel` : model used for controller predictions and state estimations.
-- `Hp=10+nk`: prediction horizon ``H_p``, `nk` is the number of delays in `model`.
+- `Hp=10+nk` : prediction horizon ``H_p``, `nk` is the number of delays in `model`.
- `Hc=2` : control horizon ``H_c``.
- `Mwt=fill(1.0,model.ny)` : main diagonal of ``\mathbf{M}`` weight matrix (vector).
- `Nwt=fill(0.1,model.nu)` : main diagonal of ``\mathbf{N}`` weight matrix (vector).
@@ -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 36b86a0cd..a60321367 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 65737bd72..9e3f5c0e7 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 62516b4d5..43e69e5f3 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 6000cf073..26f841e6e 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 953181384..b6ae2dbbb 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 0511eba81..de73b9411 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 700c3f1c1..90a411dbc 100644
--- a/src/general.jl
+++ b/src/general.jl
@@ -6,41 +6,6 @@ const DEFAULT_LWT = 0.0
const DEFAULT_CWT = 1e5
const DEFAULT_EWT = 0.0
-"Abstract type for all differentiation buffers."
-abstract type DifferentiationBuffer end
-
-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
- config::CT
-end
-
-"Create a JacobianBuffer with in-place function `f!`, output `y` and input `x`."
-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)
- return ForwardDiff.jacobian!(A, buffer.f!, y, x, buffer.config)
-end
-
"Termination status that means 'no solution available'."
const ERROR_STATUSES = (
JuMP.INFEASIBLE, JuMP.DUAL_INFEASIBLE, JuMP.LOCALLY_INFEASIBLE,
@@ -91,6 +56,10 @@ function limit_solve_time(optim::GenericModel, Ts)
end
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)
+
"Verify that x and y elements are different using `!==`."
isdifferent(x, y) = any(xi !== yi for (xi, yi) in zip(x, y))
diff --git a/src/model/linearization.jl b/src/model/linearization.jl
index 6fc47130c..c25156c61 100644
--- a/src/model/linearization.jl
+++ b/src/model/linearization.jl
@@ -1,72 +1,49 @@
-"A linearization buffer for the [`linearize`](@ref) function."
-struct LinearizationBuffer{
- NT <:Real,
- JB_FUD <:JacobianBuffer,
- JB_FXD <:JacobianBuffer,
- JB_FXU <:JacobianBuffer,
- JB_HD <:JacobianBuffer,
- JB_HX <:JacobianBuffer
-}
- x::Vector{NT}
- u::Vector{NT}
- d::Vector{NT}
- buffer_f_at_u_d::JB_FUD
- buffer_f_at_x_d::JB_FXD
- buffer_f_at_x_u::JB_FXU
- buffer_h_at_d ::JB_HD
- buffer_h_at_x ::JB_HX
- function LinearizationBuffer(
- x::Vector{NT},
- u::Vector{NT},
- d::Vector{NT},
- buffer_f_at_u_d::JB_FUD,
- buffer_f_at_x_d::JB_FXD,
- buffer_f_at_x_u::JB_FXU,
- buffer_h_at_d::JB_HD,
- buffer_h_at_x::JB_HX
- ) where {NT<:Real, JB_FUD, JB_FXD, JB_FXU, JB_HD, JB_HX}
- return new{NT, JB_FUD, JB_FXD, JB_FXU, JB_HD, JB_HX}(
- x, u, d,
- buffer_f_at_u_d,
- buffer_f_at_x_d,
- buffer_f_at_x_u,
- buffer_h_at_d,
- buffer_h_at_x
- )
- end
-
-end
+"""
+ get_linearization_func(NT, f!, h!, nu, nx, ny, nd, p, backend) -> linfunc!
-Base.show(io::IO, buffer::LinearizationBuffer) = print(io, "LinearizationBuffer object")
-
-function LinearizationBuffer(NT, f!, h!, nu, nx, ny, nd, p)
- x, u, d, f_at_u_d!, f_at_x_d!, f_at_x_u!, h_at_d!, h_at_x! = get_linearize_funcs(
- NT, f!, h!, nu, nx, ny, nd, p
- )
- xnext, y = Vector{NT}(undef, nx), Vector{NT}(undef, ny) # TODO: to replace ?
- return LinearizationBuffer(
- x, u, d,
- JacobianBuffer(f_at_u_d!, xnext, x),
- JacobianBuffer(f_at_x_d!, xnext, u),
- JacobianBuffer(f_at_x_u!, xnext, d),
- JacobianBuffer(h_at_d!, y, x),
- JacobianBuffer(h_at_x!, y, d)
- )
-end
+Return the `linfunc!` function that computes the Jacobians of `f!` and `h!` functions.
-"Get the linearization functions for [`NonLinModel`](@ref) `f!` and `h!` functions."
-function get_linearize_funcs(NT, f!, h!, nu, nx, ny, nd, p)
- x = Vector{NT}(undef, nx)
- u = Vector{NT}(undef, nu)
- d = Vector{NT}(undef, nd)
- f_at_u_d!(xnext, x) = f!(xnext, x, u, d, p)
- f_at_x_d!(xnext, u) = f!(xnext, x, u, d, p)
- f_at_x_u!(xnext, d) = f!(xnext, x, u, d, p)
- h_at_d!(y, x) = h!(y, x, d, p)
- h_at_x!(y, d) = h!(y, x, d, p)
- return x, u, d, f_at_u_d!, f_at_x_d!, f_at_x_u!, h_at_d!, h_at_x!
+The function has the following signature:
+```
+ linfunc!(xnext, y, A, Bu, C, Bd, Dd, backend, x, u, d, cst_x, cst_u, cst_d) -> nothing
+```
+and it should modifies in-place all the arguments before `backend`. The `backend` argument
+is an `AbstractADType` backend 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)
+ strict = Val(true)
+ xnext = zeros(NT, nx)
+ y = zeros(NT, ny)
+ x = zeros(NT, nx)
+ u = zeros(NT, nu)
+ d = zeros(NT, nd)
+ 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)
+ function linfunc!(xnext, y, A, Bu, C, Bd, Dd, backend, x, u, d, cst_x, cst_u, cst_d)
+ # all the arguments before `x` 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!(h_x!, y, C, C_prep, backend, x, cst_d)
+ jacobian!(h_d!, y, Dd, Dd_prep, backend, d, cst_x)
+ return nothing
+ end
+ return linfunc!
end
+
@doc raw"""
linearize(model::SimModel; x=model.x0+model.xop, u=model.uop, d=model.dop) -> linmodel
@@ -74,8 +51,9 @@ 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).
+necessarily an equilibrium, details in Extended Help). By default, [`ForwardDiff`](@extref ForwardDiff)
+automatically computes the Jacobians of ``\mathbf{f}`` and ``\mathbf{h}`` functions. Modify
+the `jacobian` keyword argument at the construction of `model` to swap the backend.
!!! warning
See Extended Help if you get an error like:
@@ -131,7 +109,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
@@ -181,8 +159,7 @@ function linearize!(
u0 .= u .- nonlinmodel.uop
d0 .= d .- nonlinmodel.dop
# --- compute the Jacobians at linearization points ---
- xnext0::Vector{NT}, y0::Vector{NT} = linmodel.buffer.x, linmodel.buffer.y
- get_jacobians!(linmodel, xnext0, y0, nonlinmodel, x0, u0, d0)
+ linearize_core!(linmodel, nonlinmodel, x0, u0, d0)
# --- compute the nonlinear model output at operating points ---
xnext0, y0 = linmodel.buffer.x, linmodel.buffer.y
h!(y0, nonlinmodel, x0, d0, model.p)
@@ -203,22 +180,20 @@ function linearize!(
return linmodel
end
-"Compute the 5 Jacobians of `model` at the linearization point and write them in `linmodel`."
-function get_jacobians!(linmodel::LinModel, xnext0, y0, model::SimModel, x0, u0, d0)
- linbuffer = model.linbuffer # a LinearizationBuffer object
- 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)
+"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
+ 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)
return nothing
end
"Copy the state-space matrices of `model` to `linmodel` if `model` is already linear."
-function get_jacobians!(linmodel::LinModel, _ , _ , model::LinModel, _ , _ , _)
+function linearize_core!(linmodel::LinModel, model::LinModel, _ , _ , _ )
linmodel.A .= model.A
linmodel.Bu .= model.Bu
linmodel.C .= model.C
diff --git a/src/model/linmodel.jl b/src/model/linmodel.jl
index 24f40ba05..041ebeea4 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 96dd64b6b..62e43ea62 100644
--- a/src/model/nonlinmodel.jl
+++ b/src/model/nonlinmodel.jl
@@ -1,5 +1,11 @@
struct NonLinModel{
- NT<:Real, F<:Function, H<:Function, PT<:Any, DS<:DiffSolver, LB<:LinearizationBuffer
+ NT<:Real,
+ F<:Function,
+ H<:Function,
+ PT<:Any,
+ DS<:DiffSolver,
+ JB<:AbstractADType,
+ LF<:Function
} <: SimModel{NT}
x0::Vector{NT}
f!::F
@@ -21,11 +27,20 @@ struct NonLinModel{
yname::Vector{String}
dname::Vector{String}
xname::Vector{String}
- linbuffer::LB
+ jacobian::JB
+ linfunc!::LF
buffer::SimModelBuffer{NT}
function NonLinModel{NT}(
- f!::F, h!::H, Ts, nu, nx, ny, nd, p::PT, solver::DS, linbuffer::LB
- ) where {NT<:Real, F<:Function, H<:Function, PT<:Any, DS<:DiffSolver, LB<:LinearizationBuffer}
+ f!::F, h!::H, Ts, nu, nx, ny, nd, p::PT, solver::DS, jacobian::JB, linfunc!::LF
+ ) where {
+ NT<:Real,
+ F<:Function,
+ H<:Function,
+ PT<:Any,
+ DS<:DiffSolver,
+ JB<:AbstractADType,
+ LF<:Function
+ }
Ts > 0 || error("Sampling time Ts must be positive")
uop = zeros(NT, nu)
yop = zeros(NT, ny)
@@ -39,7 +54,7 @@ struct NonLinModel{
x0 = zeros(NT, nx)
t = zeros(NT, 1)
buffer = SimModelBuffer{NT}(nu, nx, ny, nd)
- return new{NT, F, H, PT, DS, LB}(
+ return new{NT, F, H, PT, DS, JB, LF}(
x0,
f!, h!,
p,
@@ -48,15 +63,15 @@ struct NonLinModel{
nu, nx, ny, nd,
uop, yop, dop, xop, fop,
uname, yname, dname, xname,
- linbuffer,
+ jacobian, linfunc!,
buffer
)
end
end
@doc raw"""
- NonLinModel{NT}(f::Function, h::Function, Ts, nu, nx, ny, nd=0; p=[], solver=RungeKutta(4))
- NonLinModel{NT}(f!::Function, h!::Function, Ts, nu, nx, ny, nd=0; p=[], solver=RungeKutta(4))
+ NonLinModel{NT}(f::Function, h::Function, Ts, nu, nx, ny, nd=0; <keyword arguments>)
+ NonLinModel{NT}(f!::Function, h!::Function, Ts, nu, nx, ny, nd=0; <keyword arguments>)
Construct a nonlinear model from state-space functions `f`/`f!` and `h`/`h!`.
@@ -71,8 +86,10 @@ functions are defined as:
```
where ``\mathbf{x}``, ``\mathbf{y}``, ``\mathbf{u}``, ``\mathbf{d}`` and ``\mathbf{p}`` are
respectively the state, output, manipulated input, measured disturbance and parameter
-vectors. If the dynamics is a function of the time, simply add a measured disturbance
-defined as ``d(t) = t``. The functions can be implemented in two possible ways:
+vectors. As a mather of fact, the parameter argument `p` can be any Julia objects but use a
+mutable type if you want to change them later e.g.: a vector. If the dynamics is a function
+of the time, simply add a measured disturbance defined as ``d(t) = t``. The functions can be
+implemented in two possible ways:
1. **Non-mutating functions** (out-of-place): define them as `f(x, u, d, p) -> ẋ` and
`h(x, d, p) -> y`. This syntax is simple and intuitive but it allocates more memory.
@@ -80,25 +97,34 @@ defined as ``d(t) = t``. The functions can be implemented in two possible ways:
`h!(y, x, d, p) -> nothing`. This syntax reduces the allocations and potentially the
computational burden as well.
-`Ts` is the sampling time in second. `nu`, `nx`, `ny` and `nd` are the respective number of
-manipulated inputs, states, outputs and measured disturbances. The keyword argument `p`
-is the parameters of the model passed to the two functions. It can be any Julia objects but
-use a mutable type if you want to change them later e.g.: a vector.
-
!!! tip
Replace the `d` or `p` argument with `_` in your functions if not needed (see Examples below).
-A 4th order [`RungeKutta`](@ref) solver discretizes the differential equations by default.
The rest of the documentation assumes discrete dynamics since all models end up in this
form. The optional parameter `NT` explicitly set the number type of vectors (default to
`Float64`).
!!! 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).
+# Arguments
+- `f::Function` or `f!`: state function.
+- `h::Function` or `h!`: output function.
+- `Ts`: sampling time of in second.
+- `nu`: number of manipulated inputs.
+- `nx`: number of states.
+- `ny`: number of outputs.
+- `nd=0`: number of measured disturbances.
+- `p=[]`: parameters of the model (any type).
+- `solver=RungeKutta(4)`: a [`DiffSolver`](@ref) object for the discretization of continuous
+ dynamics, use `nothing` for discrete-time models (default to 4th order [`RungeKutta`](@ref)).
+- `jacobian=AutoForwardDiff()`: an `AbstractADType` backend when [`linearize`](@ref) is
+ called, see [`DifferentiationInterface` doc](@extref DifferentiationInterface List).
+
# Examples
```jldoctest
julia> f!(ẋ, x, u, _ , p) = (ẋ .= p*x .+ u; nothing);
@@ -142,20 +168,20 @@ NonLinModel with a sample time Ts = 2.0 s, empty solver and:
"""
function NonLinModel{NT}(
f::Function, h::Function, Ts::Real, nu::Int, nx::Int, ny::Int, nd::Int=0;
- p=NT[], solver=RungeKutta(4)
+ p=NT[], solver=RungeKutta(4), jacobian=AutoForwardDiff()
) 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)
- linbuffer = LinearizationBuffer(NT, f!, h!, nu, nx, ny, nd, p)
- return NonLinModel{NT}(f!, h!, Ts, nu, nx, ny, nd, p, solver, linbuffer)
+ 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!)
end
function NonLinModel(
f::Function, h::Function, Ts::Real, nu::Int, nx::Int, ny::Int, nd::Int=0;
- p=Float64[], solver=RungeKutta(4)
+ p=Float64[], solver=RungeKutta(4), jacobian=AutoForwardDiff()
)
- return NonLinModel{Float64}(f, h, Ts, nu, nx, ny, nd; p, solver)
+ return NonLinModel{Float64}(f, h, Ts, nu, nx, ny, nd; p, solver, jacobian)
end
"Get the mutating functions `f!` and `h!` from the provided functions in argument."
diff --git a/src/precompile.jl b/src/precompile.jl
index 6968678ef..9649c053c 100644
--- a/src/precompile.jl
+++ b/src/precompile.jl
@@ -11,42 +11,42 @@ initstate!(mpc_im, model.uop, y)
preparestate!(mpc_im, [55, 30])
mpc_im.estim()
u = mpc_im([55, 30])
-sim!(mpc_im, 3)
+sim!(mpc_im, 2)
mpc_kf = setconstraint!(LinMPC(KalmanFilter(model)), ymin=[45, -Inf])
initstate!(mpc_kf, model.uop, model())
preparestate!(mpc_kf, [55, 30])
mpc_kf.estim()
u = mpc_kf([55, 30])
-sim!(mpc_kf, 3, [55, 30])
+sim!(mpc_kf, 2, [55, 30])
mpc_lo = setconstraint!(LinMPC(Luenberger(model)), ymin=[45, -Inf])
initstate!(mpc_lo, model.uop, model())
preparestate!(mpc_lo, [55, 30])
mpc_lo.estim()
u = mpc_lo([55, 30])
-sim!(mpc_lo, 3, [55, 30])
+sim!(mpc_lo, 2, [55, 30])
mpc_ukf = setconstraint!(LinMPC(UnscentedKalmanFilter(model)), ymin=[45, -Inf])
initstate!(mpc_ukf, model.uop, model())
preparestate!(mpc_ukf, [55, 30])
mpc_ukf.estim()
u = mpc_ukf([55, 3])
-sim!(mpc_ukf, 3, [55, 30])
+sim!(mpc_ukf, 2, [55, 30])
mpc_ekf = setconstraint!(LinMPC(ExtendedKalmanFilter(model)), ymin=[45, -Inf])
initstate!(mpc_ekf, model.uop, model())
preparestate!(mpc_ekf, [55, 30])
mpc_ekf.estim()
u = mpc_ekf([55, 30])
-sim!(mpc_ekf, 3, [55, 30])
+sim!(mpc_ekf, 2, [55, 30])
mpc_skf = setconstraint!(LinMPC(SteadyKalmanFilter(model)), ymin=[45, -Inf])
initstate!(mpc_skf, model.uop, model())
preparestate!(mpc_skf, [55, 30])
mpc_skf.estim()
u = mpc_skf([55, 30])
-sim!(mpc_skf, 3, [55, 30])
+sim!(mpc_skf, 2, [55, 30])
mpc_mhe = setconstraint!(LinMPC(MovingHorizonEstimator(model, He=2)), ymin=[45, -Inf])
setconstraint!(mpc_mhe.estim, x̂min=[-50,-50,-50,-50], x̂max=[50,50,50,50])
@@ -54,16 +54,16 @@ initstate!(mpc_mhe, model.uop, model())
preparestate!(mpc_mhe, [55, 30])
mpc_mhe.estim()
u = mpc_mhe([55, 30])
-sim!(mpc_mhe, 3, [55, 30])
+sim!(mpc_mhe, 2, [55, 30])
nmpc_skf = setconstraint!(NonLinMPC(SteadyKalmanFilter(model), Cwt=Inf), ymin=[45, -Inf])
initstate!(nmpc_skf, model.uop, model())
preparestate!(nmpc_skf, [55, 30])
nmpc_skf.estim()
u = nmpc_skf([55, 30])
-sim!(nmpc_skf, 3, [55, 30])
+sim!(nmpc_skf, 2, [55, 30])
-res = sim!(model, 3)
+res = sim!(model, 2)
res_man = SimResult(model, res.U_data, res.Y_data; X_data=res.X_data)
@@ -72,7 +72,7 @@ initstate!(exmpc, model.uop, model())
preparestate!(exmpc, [55, 30])
exmpc.estim()
u = exmpc([55, 30])
-sim!(exmpc, 3, [55, 30])
+sim!(exmpc, 2, [55, 30])
f(x,u,_,model) = model.A*x + model.Bu*u
h(x,_,model) = model.C*x
@@ -87,7 +87,7 @@ initstate!(nmpc_im, nlmodel.uop, y)
preparestate!(nmpc_im, [55, 30])
nmpc_im.estim()
u = nmpc_im([55, 30])
-sim!(nmpc_im, 3, [55, 30])
+sim!(nmpc_im, 2, [55, 30])
nmpc_ukf = setconstraint!(
NonLinMPC(UnscentedKalmanFilter(nlmodel), Hp=10, Cwt=1e3), ymin=[45, -Inf]
@@ -95,13 +95,13 @@ nmpc_ukf = setconstraint!(
initstate!(nmpc_ukf, nlmodel.uop, y)
preparestate!(nmpc_ukf, [55, 30])
u = nmpc_ukf([55, 30])
-sim!(nmpc_ukf, 3, [55, 30])
+sim!(nmpc_ukf, 2, [55, 30])
nmpc_ekf = setconstraint!(NonLinMPC(ExtendedKalmanFilter(model), Cwt=Inf), ymin=[45, -Inf])
initstate!(nmpc_ekf, model.uop, model())
preparestate!(nmpc_ekf, [55, 30])
u = nmpc_ekf([55, 30])
-sim!(nmpc_ekf, 3, [55, 30])
+sim!(nmpc_ekf, 2, [55, 30])
nmpc_mhe = setconstraint!(
NonLinMPC(MovingHorizonEstimator(nlmodel, He=2), Hp=10, Cwt=Inf), ymin=[45, -Inf]
@@ -110,7 +110,7 @@ setconstraint!(nmpc_mhe.estim, x̂min=[-50,-50,-50,-50], x̂max=[50,50,50,50])
initstate!(nmpc_mhe, nlmodel.uop, y)
preparestate!(nmpc_mhe, [55, 30])
u = nmpc_mhe([55, 30])
-sim!(nmpc_mhe, 3, [55, 30])
+sim!(nmpc_mhe, 2, [55, 30])
function JE( _ , Ŷe, _ , R̂y)
Ŷ = Ŷe[3:end]
@@ -123,4 +123,7 @@ empc = setconstraint!(
)
preparestate!(empc, [55, 30])
u = empc()
-sim!(empc, 3)
\ No newline at end of file
+sim!(empc, 2)
+
+linearizemodel = linearize(nlmodel)
+setmodel!(mpc_kf, linearizemodel)
\ No newline at end of file
diff --git a/src/sim_model.jl b/src/sim_model.jl
index aaad33f8f..d169745cd 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/1_test_sim_model.jl b/test/1_test_sim_model.jl
index 5d5a77a56..9ffbd1794 100644
--- a/test/1_test_sim_model.jl
+++ b/test/1_test_sim_model.jl
@@ -152,6 +152,8 @@ end
@testitem "NonLinModel construction" setup=[SetupMPCtests] begin
using .SetupMPCtests, ControlSystemsBase, LinearAlgebra
+ using DifferentiationInterface
+ import FiniteDiff
linmodel1 = LinModel(sys,Ts,i_u=[1,2])
f1(x,u,_,model) = model.A*x + model.Bu*u
h1(x,_,model) = model.C*x
@@ -243,8 +245,9 @@ end
@test xnext ≈ zeros(2)
nonlinemodel7.h!(y, [0; 0], [0], nonlinemodel7.p)
@test y ≈ zeros(1)
+ nonlinemodel8 = NonLinModel(f2!, h2!, 1.0, 1, 2, 1, 1, p=p, jacobian=AutoFiniteDiff())
+ @test nonlinemodel8.jacobian == AutoFiniteDiff()
-
@test_throws ErrorException NonLinModel(
(x,u)->linmodel1.A*x + linmodel1.Bu*u,
(x,_,_)->linmodel1.C*x, Ts, 2, 4, 2, 1, solver=nothing)
@@ -280,7 +283,9 @@ end
end
@testitem "NonLinModel linearization" setup=[SetupMPCtests] begin
- using .SetupMPCtests, ControlSystemsBase, LinearAlgebra, ForwardDiff
+ using .SetupMPCtests, ControlSystemsBase, LinearAlgebra
+ using DifferentiationInterface
+ import ForwardDiff, FiniteDiff
Ts = 1.0
f1(x,u,d,_) = x.^5 .+ u.^4 .+ d.^3
h1(x,d,_) = x.^2 .+ d
@@ -292,14 +297,20 @@ end
@test linmodel1.Bd ≈ 3*d.^2
@test linmodel1.C ≈ 2*x.^1
@test linmodel1.Dd ≈ 1*d.^0
- linmodel2 = LinModel(nonlinmodel1; x, u, d)
- @test linmodel1.A ≈ linmodel2.A
- @test linmodel1.Bu ≈ linmodel2.Bu
- @test linmodel1.Bd ≈ linmodel2.Bd
- @test linmodel1.C ≈ linmodel2.C
- @test linmodel1.Dd ≈ linmodel2.Dd
- @test repr(nonlinmodel1.linbuffer) == "LinearizationBuffer object"
- @test repr(nonlinmodel1.linbuffer.buffer_f_at_u_d) == "DifferentiationBuffer with a JacobianConfig"
+ linmodel1b = LinModel(nonlinmodel1; x, u, d)
+ @test linmodel1.A ≈ linmodel1b.A
+ @test linmodel1.Bu ≈ linmodel1b.Bu
+ @test linmodel1.Bd ≈ linmodel1b.Bd
+ @test linmodel1.C ≈ linmodel1b.C
+ @test linmodel1.Dd ≈ linmodel1b.Dd
+
+ nonlinmodel2 = NonLinModel(f1,h1,Ts,1,1,1,1,solver=nothing, jacobian=AutoFiniteDiff())
+ linmodel2 = linearize(nonlinmodel2; x, u, d)
+ @test linmodel2.A ≈ 5*x.^4 atol=1e-3
+ @test linmodel2.Bu ≈ 4*u.^3 atol=1e-3
+ @test linmodel2.Bd ≈ 3*d.^2 atol=1e-3
+ @test linmodel2.C ≈ 2*x.^1 atol=1e-3
+ @test linmodel2.Dd ≈ 1*d.^0 atol=1e-3
f1!(ẋ, x, u, d, _) = (ẋ .= x.^5 .+ u.^4 .+ d.^3; nothing)
h1!(y, x, d, _) = (y .= x.^2 .+ d; nothing)
diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl
index fa2eb408c..5a807ad81 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 d099cb6ba..5dc8e7458 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