diff --git a/docs/src/public/sim_model.md b/docs/src/public/sim_model.md index a6f096585..8c8f5be2a 100644 --- a/docs/src/public/sim_model.md +++ b/docs/src/public/sim_model.md @@ -67,3 +67,9 @@ ModelPredictiveControl.DiffSolver ```@docs RungeKutta ``` + +### ForwardEuler + +```@docs +ForwardEuler +``` diff --git a/src/ModelPredictiveControl.jl b/src/ModelPredictiveControl.jl index cd001f873..da4ab2ebe 100644 --- a/src/ModelPredictiveControl.jl +++ b/src/ModelPredictiveControl.jl @@ -23,7 +23,7 @@ import PreallocationTools: DiffCache, get_tmp import OSQP, Ipopt export SimModel, LinModel, NonLinModel -export DiffSolver, RungeKutta +export DiffSolver, RungeKutta, ForwardEuler export setop!, setname! export setstate!, setmodel!, preparestate!, updatestate!, evaloutput, linearize, linearize! export savetime!, periodsleep diff --git a/src/controller/explicitmpc.jl b/src/controller/explicitmpc.jl index 95df04982..14909b0c4 100644 --- a/src/controller/explicitmpc.jl +++ b/src/controller/explicitmpc.jl @@ -100,7 +100,7 @@ suitable for applications that require small sample times. The keyword arguments identical to [`LinMPC`](@ref), except for `Cwt` and `optim` which are not supported. This method uses the default state estimator, a [`SteadyKalmanFilter`](@ref) with default -arguments. +arguments. This controller is allocation-free. # Examples ```jldoctest diff --git a/src/controller/linmpc.jl b/src/controller/linmpc.jl index dbaeb3803..0112d3e26 100644 --- a/src/controller/linmpc.jl +++ b/src/controller/linmpc.jl @@ -117,7 +117,7 @@ from ``j=0`` to ``H_p-1``. The slack variable ``ϵ`` relaxes the constraints, as in [`setconstraint!`](@ref) documentation. See Extended Help for a detailed nomenclature. This method uses the default state estimator, a [`SteadyKalmanFilter`](@ref) with default -arguments. +arguments. This controller allocates memory at each time step for the optimization. # Arguments - `model::LinModel` : model used for controller predictions and state estimations. diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index f72be7771..8e0c96655 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -144,6 +144,8 @@ This method uses the default state estimator: - if `model` is a [`LinModel`](@ref), a [`SteadyKalmanFilter`](@ref) with default arguments; - else, an [`UnscentedKalmanFilter`](@ref) with default arguments. +This controller allocates memory at each time step for the optimization. + !!! warning See Extended Help if you get an error like: `MethodError: no method matching Float64(::ForwardDiff.Dual)`. diff --git a/src/estimator/internal_model.jl b/src/estimator/internal_model.jl index a32d4349d..6b72c8673 100644 --- a/src/estimator/internal_model.jl +++ b/src/estimator/internal_model.jl @@ -71,7 +71,8 @@ Construct an internal model estimator based on `model` ([`LinModel`](@ref) or [` unmeasured ``\mathbf{y^u}``. `model` evaluates the deterministic predictions ``\mathbf{ŷ_d}``, and `stoch_ym`, the stochastic predictions of the measured outputs ``\mathbf{ŷ_s^m}`` (the unmeasured ones being ``\mathbf{ŷ_s^u=0}``). The predicted outputs -sum both values : ``\mathbf{ŷ = ŷ_d + ŷ_s}``. See the Extended Help for more details. +sum both values : ``\mathbf{ŷ = ŷ_d + ŷ_s}``. See the Extended Help for more details. This +estimator is allocation-free is `model` simulations do not allocate. !!! warning `InternalModel` estimator does not work if `model` is integrating or unstable. The diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index 9a0aec83b..62516b4d5 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -101,7 +101,7 @@ on the covariance tuning. The matrices ``\mathbf{Ĉ^m, D̂_d^m}`` are the rows ``\mathbf{Ĉ, D̂_d}`` that correspond to measured outputs ``\mathbf{y^m}`` (and unmeasured ones, for ``\mathbf{Ĉ^u, D̂_d^u}``). The Kalman filter will estimate the current state with the newest measurements ``\mathbf{x̂}_k(k)`` if `direct` is `true`, else it will predict the -state of the next time step ``\mathbf{x̂}_k(k+1)``. +state of the next time step ``\mathbf{x̂}_k(k+1)``. This estimator is allocation-free. # Arguments !!! info @@ -352,6 +352,7 @@ the estimation error covariance of `model` states augmented with the stochastic ``\mathbf{P̂}_{-1}(0) = \mathrm{diag}\{ \mathbf{P}(0), \mathbf{P_{int_{u}}}(0), \mathbf{P_{int_{ym}}}(0) \}``. The initial state estimate ``\mathbf{x̂}_{-1}(0)`` can be manually specified with [`setstate!`](@ref), or automatically with [`initstate!`](@ref). +This estimator is allocation-free. # Arguments !!! info @@ -596,6 +597,7 @@ matrix ``\mathbf{P̂}`` is the estimation error covariance of `model` state augm stochastic ones. Three keyword arguments specify its initial value with ``\mathbf{P̂}_{-1}(0) = \mathrm{diag}\{ \mathbf{P}(0), \mathbf{P_{int_{u}}}(0), \mathbf{P_{int_{ym}}}(0) \}``. The initial state estimate ``\mathbf{x̂}_{-1}(0)`` can be manually specified with [`setstate!`](@ref). +This estimator is allocation-free if `model` simulations do not allocate. # Arguments !!! info @@ -950,7 +952,7 @@ Both [`LinModel`](@ref) and [`NonLinModel`](@ref) are supported. The process mod 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. +automatic differentiation. This estimator allocates memory for the Jacobians. !!! warning See the Extended Help of [`linearize`](@ref) function if you get an error like: diff --git a/src/estimator/luenberger.jl b/src/estimator/luenberger.jl index 714e365e5..8924af2aa 100644 --- a/src/estimator/luenberger.jl +++ b/src/estimator/luenberger.jl @@ -78,6 +78,7 @@ elements specifying the observer poles/eigenvalues (near ``z=0.5`` by default). 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. # Examples ```jldoctest diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index e7cb5c6b8..8129bae4c 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -215,7 +215,8 @@ the keyword argument `direct=true` (default value), the constant ``p=0`` in the above, and the MHE is in the current form. Else ``p=1``, leading to the prediction form. See [`UnscentedKalmanFilter`](@ref) for details on the augmented process model and -``\mathbf{R̂}, \mathbf{Q̂}`` covariances. +``\mathbf{R̂}, \mathbf{Q̂}`` covariances. This estimator allocates a fair amount of memory +at each time step. !!! warning See the Extended Help if you get an error like: diff --git a/src/model/linearization.jl b/src/model/linearization.jl index 6e6831678..71494cc7e 100644 --- a/src/model/linearization.jl +++ b/src/model/linearization.jl @@ -120,7 +120,8 @@ end Linearize `model` and store the result in `linmodel` (in-place). -The keyword arguments are identical to [`linearize`](@ref). +The keyword arguments are identical to [`linearize`](@ref). The code is allocation-free if +`model` simulations does not allocate. # Examples ```jldoctest diff --git a/src/model/nonlinmodel.jl b/src/model/nonlinmodel.jl index 402e22302..50b5293b2 100644 --- a/src/model/nonlinmodel.jl +++ b/src/model/nonlinmodel.jl @@ -103,7 +103,7 @@ julia> f!(ẋ, x, u, _ , p) = (ẋ .= p*x .+ u; nothing); julia> h!(y, x, _ , _ ) = (y .= 0.1x; nothing); julia> model1 = NonLinModel(f!, h!, 5.0, 1, 1, 1, p=-0.2) # continuous dynamics -NonLinModel with a sample time Ts = 5.0 s, RungeKutta solver and: +NonLinModel with a sample time Ts = 5.0 s, RungeKutta(4) solver and: 1 manipulated inputs u 1 states x 1 outputs y @@ -233,5 +233,5 @@ f!(xnext0, model::NonLinModel, x0, u0, d0, p) = model.f!(xnext0, x0, u0, d0, p) "Call `model.h!(y0, x0, d0, p)` for [`NonLinModel`](@ref)." h!(y0, model::NonLinModel, x0, d0, p) = model.h!(y0, x0, d0, p) -detailstr(model::NonLinModel) = ", $(typeof(model.solver).name.name) solver" +detailstr(model::NonLinModel) = ", $(typeof(model.solver).name.name)($(model.solver.order)) solver" detailstr(::NonLinModel{<:Real, <:Function, <:Function, <:Any, <:EmptySolver}) = ", empty solver" \ No newline at end of file diff --git a/src/model/solver.jl b/src/model/solver.jl index 9ab54a3c7..fb5b353b2 100644 --- a/src/model/solver.jl +++ b/src/model/solver.jl @@ -13,8 +13,8 @@ struct RungeKutta <: DiffSolver order::Int supersample::Int function RungeKutta(order::Int, supersample::Int) - if order ≠ 4 - throw(ArgumentError("only 4th order Runge-Kutta is supported.")) + if order ≠ 4 && order ≠ 1 + throw(ArgumentError("only 1st and 4th order Runge-Kutta is supported.")) end if order < 1 throw(ArgumentError("order must be greater than 0")) @@ -29,15 +29,17 @@ end """ RungeKutta(order=4; supersample=1) -Create a Runge-Kutta solver with optional super-sampling. +Create an explicit Runge-Kutta solver with optional super-sampling. -Only the 4th order Runge-Kutta is supported for now. The keyword argument `supersample` -provides the number of internal steps (default to 1 step). +The argument `order` specifies the order of the Runge-Kutta solver, which must be 1 or 4. +The keyword argument `supersample` provides the number of internal steps (default to 1 step). +This solver is allocation-free if the `f!` and `h!` functions do not allocate. """ RungeKutta(order::Int=4; supersample::Int=1) = RungeKutta(order, supersample) -"Get the `f!` and `h!` functions for Runge-Kutta solver." +"Get the `f!` and `h!` functions for the explicit Runge-Kutta solvers." function get_solver_functions(NT::DataType, solver::RungeKutta, fc!, hc!, Ts, _ , nx, _ , _ ) + order = solver.order Ts_inner = Ts/solver.supersample Nc = nx + 2 xcur_cache::DiffCache{Vector{NT}, Vector{NT}} = DiffCache(zeros(NT, nx), Nc) @@ -45,33 +47,57 @@ function get_solver_functions(NT::DataType, solver::RungeKutta, fc!, hc!, Ts, _ k2_cache::DiffCache{Vector{NT}, Vector{NT}} = DiffCache(zeros(NT, nx), Nc) k3_cache::DiffCache{Vector{NT}, Vector{NT}} = DiffCache(zeros(NT, nx), Nc) k4_cache::DiffCache{Vector{NT}, Vector{NT}} = DiffCache(zeros(NT, nx), Nc) - f! = function inner_solver_f!(xnext, x, u, d, p) - CT = promote_type(eltype(x), eltype(u), eltype(d)) - xcur = get_tmp(xcur_cache, CT) - k1 = get_tmp(k1_cache, CT) - k2 = get_tmp(k2_cache, CT) - k3 = get_tmp(k3_cache, CT) - k4 = get_tmp(k4_cache, CT) - xterm = xnext - @. xcur = x - for i=1:solver.supersample - @. xterm = xcur - fc!(k1, xterm, u, d, p) - @. xterm = xcur + k1 * Ts_inner/2 - fc!(k2, xterm, u, d, p) - @. xterm = xcur + k2 * Ts_inner/2 - fc!(k3, xterm, u, d, p) - @. xterm = xcur + k3 * Ts_inner - fc!(k4, xterm, u, d, p) - @. xcur = xcur + (k1 + 2k2 + 2k3 + k4)*Ts_inner/6 + if order==1 + f! = function euler_solver!(xnext, x, u, d, p) + CT = promote_type(eltype(x), eltype(u), eltype(d)) + xcur = get_tmp(xcur_cache, CT) + k1 = get_tmp(k1_cache, CT) + xterm = xnext + @. xcur = x + for i=1:solver.supersample + fc!(k1, xcur, u, d, p) + @. xcur = xcur + k1 * Ts_inner + end + @. xnext = xcur + return nothing + end + elseif order==4 + f! = function rk4_solver!(xnext, x, u, d, p) + CT = promote_type(eltype(x), eltype(u), eltype(d)) + xcur = get_tmp(xcur_cache, CT) + k1 = get_tmp(k1_cache, CT) + k2 = get_tmp(k2_cache, CT) + k3 = get_tmp(k3_cache, CT) + k4 = get_tmp(k4_cache, CT) + xterm = xnext + @. xcur = x + for i=1:solver.supersample + fc!(k1, xcur, u, d, p) + @. xterm = xcur + k1 * Ts_inner/2 + fc!(k2, xterm, u, d, p) + @. xterm = xcur + k2 * Ts_inner/2 + fc!(k3, xterm, u, d, p) + @. xterm = xcur + k3 * Ts_inner + fc!(k4, xterm, u, d, p) + @. xcur = xcur + (k1 + 2k2 + 2k3 + k4)*Ts_inner/6 + end + @. xnext = xcur + return nothing end - @. xnext = xcur - return nothing end h! = hc! return f!, h! end +""" + ForwardEuler(; supersample=1) + +Create a Forward Euler solver with optional super-sampling. + +This is an alias for `RungeKutta(1; supersample)`, see [`RungeKutta`](@ref). +""" +const ForwardEuler(;supersample=1) = RungeKutta(1; supersample) + function Base.show(io::IO, solver::RungeKutta) N, n = solver.order, solver.supersample print(io, "$(N)th order Runge-Kutta differential equation solver with $n supersamples.") diff --git a/test/test_sim_model.jl b/test/test_sim_model.jl index 52b7cf1be..87a47888c 100644 --- a/test/test_sim_model.jl +++ b/test/test_sim_model.jl @@ -200,7 +200,7 @@ end Dd = reshape([0], 1, 1) f3(x, u, d, _) = A*x + Bu*u+ Bd*d h3(x, d, _) = C*x + Dd*d - solver=RungeKutta() + solver=RungeKutta(4) @test string(solver) == "4th order Runge-Kutta differential equation solver with 1 supersamples." nonlinmodel5 = NonLinModel(f3, h3, 1.0, 1, 2, 1, 1, solver=solver) @@ -227,6 +227,13 @@ end @test xnext ≈ zeros(2) nonlinmodel6.h!(y, [0; 0], [0], nonlinmodel6.p) @test y ≈ zeros(1) + nonlinemodel7 = NonLinModel(f2!, h2!, 1.0, 1, 2, 1, 1, solver=ForwardEuler()) + xnext, y = similar(nonlinemodel7.x0), similar(nonlinemodel7.yop) + nonlinemodel7.f!(xnext, [0; 0], [0], [0], nonlinemodel7.p) + @test xnext ≈ zeros(2) + nonlinemodel7.h!(y, [0; 0], [0], nonlinemodel7.p) + @test y ≈ zeros(1) + @test_throws ErrorException NonLinModel( (x,u)->linmodel1.A*x + linmodel1.Bu*u,