diff --git a/.gitignore b/.gitignore index 23b09cb2d..c0255b4c5 100644 --- a/.gitignore +++ b/.gitignore @@ -5,3 +5,4 @@ LocalPreferences.toml .vscode/ *.cov docs/Manifest.toml +lcov.info diff --git a/Project.toml b/Project.toml index f44c14812..28106dbd8 100644 --- a/Project.toml +++ b/Project.toml @@ -1,7 +1,7 @@ name = "ModelPredictiveControl" uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c" authors = ["Francis Gagnon"] -version = "0.24.1" +version = "1.0.0" [deps] ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e" diff --git a/README.md b/README.md index 8ad573e0a..883bb2789 100644 --- a/README.md +++ b/README.md @@ -24,7 +24,7 @@ using Pkg; Pkg.add("ModelPredictiveControl") To construct model predictive controllers (MPCs), we must first specify a plant model that is typically extracted from input-output data using [system identification](https://github.com/baggepinnen/ControlSystemIdentification.jl). The model here is linear with one input, two outputs and a large time delay in the first -channel: +channel (a transfer function matrix, with $s$ as the Laplace variable): ```math \mathbf{G}(s) = \frac{\mathbf{y}(s)}{\mathbf{u}(s)} = diff --git a/docs/Project.toml b/docs/Project.toml index ef63dc804..7d5632a39 100644 --- a/docs/Project.toml +++ b/docs/Project.toml @@ -5,10 +5,14 @@ Documenter = "e30172f5-a6a5-5a46-863b-614d45cd2de4" 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] ControlSystemsBase = "1" +DAQP = "0.5" Documenter = "1" +JuMP = "1" LinearAlgebra = "1.6" Logging = "1.6" +Plots = "1" diff --git a/docs/make.jl b/docs/make.jl index cf099540c..37de69d4b 100644 --- a/docs/make.jl +++ b/docs/make.jl @@ -27,6 +27,7 @@ makedocs( "Examples" => [ "Linear Design" => "manual/linmpc.md", "Nonlinear Design" => "manual/nonlinmpc.md", + "ModelingToolkit" => "manual/mtk.md", ], ], "Functions" => [ diff --git a/docs/src/index.md b/docs/src/index.md index 80c9e49dc..230b4459e 100644 --- a/docs/src/index.md +++ b/docs/src/index.md @@ -29,6 +29,7 @@ Pages = [ "manual/installation.md", "manual/linmpc.md", "manual/nonlinmpc.md", + "manual/mtk.md" ] ``` diff --git a/docs/src/manual/mtk.md b/docs/src/manual/mtk.md new file mode 100644 index 000000000..2766555a6 --- /dev/null +++ b/docs/src/manual/mtk.md @@ -0,0 +1,140 @@ +# [Manual: ModelingToolkit Integration](@id man_mtk) + +```@contents +Pages = ["mtk.md"] +``` + +```@setup 1 +using Logging; errlogger = ConsoleLogger(stderr, Error); +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 +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. + +!!! danger "Disclaimer" + This simple example is not an official interface to `ModelingToolkit.jl`. It is provided + as a basic starting template to combine both packages. There is no guarantee that it + will work for all corner cases. + +We first construct and instantiate the pendulum model: + +```@example 1 +using ModelPredictiveControl, ModelingToolkit +using ModelingToolkit: D_nounits as D, t_nounits as t, varmap_to_vars +@mtkmodel Pendulum begin + @parameters begin + g = 9.8 + L = 0.4 + K = 1.2 + m = 0.3 + end + @variables begin + θ(t) # state + ω(t) # state + τ(t) # input + y(t) # output + end + @equations begin + D(θ) ~ ω + D(ω) ~ -g/L*sin(θ) - K/m*ω + τ/m/L^2 + y ~ θ * 180 / π + end +end +@named mtk_model = Pendulum() +mtk_model = complete(mtk_model) +``` + +We than convert the MTK model to an [input-output system](https://docs.sciml.ai/ModelingToolkit/stable/basics/InputOutput/): + +```@example 1 +function generate_f_h(model, inputs, outputs) + (_, f_ip), dvs, psym, io_sys = ModelingToolkit.generate_control_function( + model, inputs, split=false; outputs + ) + if any(ModelingToolkit.is_alg_equation, equations(io_sys)) + error("Systems with algebraic equations are not supported") + end + h_ = ModelingToolkit.build_explicit_observed_function(io_sys, outputs; inputs = inputs) + nx = length(dvs) + vx = string.(dvs) + par = varmap_to_vars(defaults(io_sys), psym) + function f!(ẋ, x, u, _ , _ ) + f_ip(ẋ, x, u, par, 1) + nothing + end + function h!(y, x, _ , _ ) + y .= h_(x, 1, par, 1) + nothing + end + return f!, h!, nx, vx +end +inputs, outputs = [mtk_model.τ], [mtk_model.y] +f!, h!, nx, vx = generate_f_h(mtk_model, inputs, outputs) +nu, ny, Ts = length(inputs), length(outputs), 0.1 +vu, vy = ["\$τ\$ (Nm)"], ["\$θ\$ (°)"] +nothing # hide +``` + +A [`NonLinModel`](@ref) can now be constructed: + +```@example 1 +model = setname!(NonLinModel(f!, h!, Ts, nu, nx, ny); u=vu, x=vx, y=vy) +``` + +We also instantiate a plant model with a 25 % larger friction coefficient ``K``: + +```@example 1 +mtk_model.K = defaults(mtk_model)[mtk_model.K] * 1.25 +f_plant, h_plant, _, _ = generate_f_h(mtk_model, inputs, outputs) +plant = setname!(NonLinModel(f_plant, h_plant, Ts, nu, nx, ny); u=vu, x=vx, y=vy) +``` + +## Controller Design + +We can than reproduce the Kalman filter and the controller design of the [last section](@ref man_nonlin): + +```@example 1 +α=0.01; σQ=[0.1, 1.0]; σR=[5.0]; nint_u=[1]; σQint_u=[0.1] +estim = UnscentedKalmanFilter(model; α, σQ, σR, nint_u, σQint_u) +Hp, Hc, Mwt, Nwt = 20, 2, [0.5], [2.5] +nmpc = NonLinMPC(estim; Hp, Hc, Mwt, Nwt, Cwt=Inf) +umin, umax = [-1.5], [+1.5] +nmpc = setconstraint!(nmpc; umin, umax) +``` + +The 180° setpoint response is identical: + +```@example 1 +using Plots +N = 35 +using JuMP; unset_time_limit_sec(nmpc.optim) # hide +res_ry = sim!(nmpc, N, [180.0], plant=plant, x_0=[0, 0], x̂_0=[0, 0, 0]) +plot(res_ry) +savefig("plot1_MTK.svg"); nothing # hide +``` + +![plot1_MTK](plot1_MTK.svg) + +and also the output disturbance rejection: + +```@example 1 +res_yd = sim!(nmpc, N, [180.0], plant=plant, x_0=[π, 0], x̂_0=[π, 0, 0], y_step=[10]) +plot(res_yd) +savefig("plot2_MTK.svg"); nothing # hide +``` + +![plot2_MTK](plot2_MTK.svg) + +## Acknowledgement + +Authored by `1-Bart-1` and `baggepinnen`, thanks for the contribution. + +```@setup 1 +global_logger(old_logger); +``` diff --git a/docs/src/manual/nonlinmpc.md b/docs/src/manual/nonlinmpc.md index 4e93a1ff4..8fcf5bc32 100644 --- a/docs/src/manual/nonlinmpc.md +++ b/docs/src/manual/nonlinmpc.md @@ -40,34 +40,34 @@ The plant model is nonlinear: in which ``g`` is the gravitational acceleration in m/s², ``L``, the pendulum length in m, ``K``, the friction coefficient at the pivot point in kg/s, and ``m``, the mass attached at -the end of the pendulum in kg. The [`NonLinModel`](@ref) constructor assumes by default -that the state function `f` is continuous in time, that is, an ordinary differential -equation system (like here): +the end of the pendulum in kg, all bundled in the parameter vector ``\mathbf{p} = +[\begin{smallmatrix} g & L & K & m \end{smallmatrix}]'``. The [`NonLinModel`](@ref) +constructor assumes by default that the state function `f` is continuous in time, that is, +an ordinary differential equation system (like here): ```@example 1 using ModelPredictiveControl -function pendulum(par, x, u) - g, L, K, m = par # [m/s²], [m], [kg/s], [kg] +function f(x, u, _ , p) + g, L, K, m = p # [m/s²], [m], [kg/s], [kg] θ, ω = x[1], x[2] # [rad], [rad/s] τ = u[1] # [Nm] dθ = ω dω = -g/L*sin(θ) - K/m*ω + τ/m/L^2 return [dθ, dω] end -# declared constants, to avoid type-instability in the f function, for speed: -const par = (9.8, 0.4, 1.2, 0.3) -f(x, u, _ ) = pendulum(par, x, u) -h(x, _ ) = [180/π*x[1]] # [°] +h(x, _ , _ ) = [180/π*x[1]] # [°] +p_model = [9.8, 0.4, 1.2, 0.3] nu, nx, ny, Ts = 1, 2, 1, 0.1 vu, vx, vy = ["\$τ\$ (Nm)"], ["\$θ\$ (rad)", "\$ω\$ (rad/s)"], ["\$θ\$ (°)"] -model = setname!(NonLinModel(f, h, Ts, nu, nx, ny); u=vu, x=vx, y=vy) +model = setname!(NonLinModel(f, h, Ts, nu, nx, ny; p=p_model); u=vu, x=vx, y=vy) ``` The output function ``\mathbf{h}`` converts the ``θ`` angle to degrees. Note that special characters like ``θ`` can be typed in the Julia REPL or VS Code by typing `\theta` and -pressing the `` key. The tuple `par` is constant here to improve the [performance](https://docs.julialang.org/en/v1/manual/performance-tips/#Avoid-untyped-global-variables). -A 4th order [`RungeKutta`](@ref) method solves the differential equations by default. It is -good practice to first simulate `model` using [`sim!`](@ref) as a quick sanity check: +pressing the `` key. Note that the parameter `p` can be of any type but use a mutable +type like a vector of you want to modify it later. A 4th order [`RungeKutta`](@ref) method +solves the differential equations by default. It is good practice to first simulate `model` +using [`sim!`](@ref) as a quick sanity check: ```@example 1 using Plots @@ -101,9 +101,9 @@ motor torque ``τ``, with an associated standard deviation `σQint_u` of 0.1 N m estimator tuning is tested on a plant with a 25 % larger friction coefficient ``K``: ```@example 1 -const par_plant = (par[1], par[2], 1.25*par[3], par[4]) -f_plant(x, u, _ ) = pendulum(par_plant, x, u) -plant = setname!(NonLinModel(f_plant, h, Ts, nu, nx, ny); u=vu, x=vx, y=vy) +p_plant = copy(p_model) +p_plant[3] = 1.25*p_model[3] +plant = setname!(NonLinModel(f, h, Ts, nu, nx, ny; p=p_plant); u=vu, x=vx, y=vy) res = sim!(estim, N, [0.5], plant=plant, y_noise=[0.5]) plot(res, plotu=false, plotxwithx̂=true) savefig("plot2_NonLinMPC.svg"); nothing # hide @@ -182,10 +182,10 @@ angle ``θ`` is measured here). As the arguments of [`NonLinMPC`](@ref) economic Kalman Filter similar to the previous one (``\mathbf{y^m} = θ`` and ``\mathbf{y^u} = ω``): ```@example 1 -h2(x, _ ) = [180/π*x[1], x[2]] +h2(x, _ , _ ) = [180/π*x[1], x[2]] nu, nx, ny = 1, 2, 2 -model2 = setname!(NonLinModel(f , h2, Ts, nu, nx, ny), u=vu, x=vx, y=[vy; vx[2]]) -plant2 = setname!(NonLinModel(f_plant, h2, Ts, nu, nx, ny), u=vu, x=vx, y=[vy; vx[2]]) +model2 = setname!(NonLinModel(f, h2, Ts, nu, nx, ny; p=p_model), u=vu, x=vx, y=[vy; vx[2]]) +plant2 = setname!(NonLinModel(f, h2, Ts, nu, nx, ny; p=p_plant), u=vu, x=vx, y=[vy; vx[2]]) estim2 = UnscentedKalmanFilter(model2; σQ, σR, nint_u, σQint_u, i_ym=[1]) ``` @@ -194,11 +194,12 @@ output vector of `plant` argument corresponds to the model output vector in `mpc We can now define the ``J_E`` function and the `empc` controller: ```@example 1 -function JE(UE, ŶE, _ ) +function JE(UE, ŶE, _ , p) + Ts = p τ, ω = UE[1:end-1], ŶE[2:2:end-1] return Ts*sum(τ.*ω) end -empc = NonLinMPC(estim2; Hp, Hc, Nwt, Mwt=[0.5, 0], Cwt=Inf, Ewt=3.5e3, JE=JE) +empc = NonLinMPC(estim2; Hp, Hc, Nwt, Mwt=[0.5, 0], Cwt=Inf, Ewt=3.5e3, JE=JE, p=Ts) empc = setconstraint!(empc; umin, umax) ``` diff --git a/src/controller/execute.jl b/src/controller/execute.jl index 04989c2c7..843ff10b1 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -89,7 +89,7 @@ The function should be called after calling [`moveinput!`](@ref). It returns the - `:Ŷs` or *`:Yhats`* : predicted stochastic output over ``H_p`` of [`InternalModel`](@ref), ``\mathbf{Ŷ_s}`` - `:R̂y` or *`:Rhaty`* : predicted output setpoint over ``H_p``, ``\mathbf{R̂_y}`` - `:R̂u` or *`:Rhatu`* : predicted manipulated input setpoint over ``H_p``, ``\mathbf{R̂_u}`` -- `:x̂end` or *`:xhatend`* : optimal terminal states, ``\mathbf{x̂}_{k-1}(k+H_p)`` +- `:x̂end` or *`:xhatend`* : optimal terminal states, ``\mathbf{x̂}_i(k+H_p)`` - `:J` : objective value optimum, ``J`` - `:U` : optimal manipulated inputs over ``H_p``, ``\mathbf{U}`` - `:u` : current optimal manipulated input, ``\mathbf{u}(k)`` @@ -369,19 +369,7 @@ function obj_nonlinprog!( U0, Ȳ, _ , mpc::PredictiveController, model::LinModel, Ŷ0, ΔŨ::AbstractVector{NT} ) where NT <: Real J = obj_quadprog(ΔŨ, mpc.H̃, mpc.q̃) + mpc.r[] - if !iszero(mpc.E) - ŷ, D̂E = mpc.ŷ, mpc.D̂E - U = U0 - U .+= mpc.Uop - uend = @views U[(end-model.nu+1):end] - Ŷ = Ȳ - Ŷ .= Ŷ0 .+ mpc.Yop - UE = [U; uend] - ŶE = [ŷ; Ŷ] - E_JE = mpc.E*mpc.JE(UE, ŶE, D̂E) - else - E_JE = 0.0 - end + E_JE = obj_econ!(U0, Ȳ, mpc, model, Ŷ0, ΔŨ) return J + E_JE end @@ -413,22 +401,13 @@ function obj_nonlinprog!( JR̂u = 0.0 end # --- economic term --- - if !iszero(mpc.E) - ny, Hp, ŷ, D̂E = model.ny, mpc.Hp, mpc.ŷ, mpc.D̂E - U = U0 - U .+= mpc.Uop - uend = @views U[(end-model.nu+1):end] - Ŷ = Ȳ - Ŷ .= Ŷ0 .+ mpc.Yop - UE = [U; uend] - ŶE = [ŷ; Ŷ] - E_JE = mpc.E*mpc.JE(UE, ŶE, D̂E) - else - E_JE = 0.0 - end + E_JE = obj_econ!(U0, Ȳ, mpc, model, Ŷ0, ΔŨ) return JR̂y + JΔŨ + JR̂u + E_JE end +"By default, the economic term is zero." +obj_econ!( _ , _ , ::PredictiveController, ::SimModel, _ , _ ) = 0.0 + @doc raw""" optim_objective!(mpc::PredictiveController) -> ΔŨ diff --git a/src/controller/explicitmpc.jl b/src/controller/explicitmpc.jl index 9ba406d8c..71e829bee 100644 --- a/src/controller/explicitmpc.jl +++ b/src/controller/explicitmpc.jl @@ -167,7 +167,7 @@ function ExplicitMPC( return ExplicitMPC{NT, SE}(estim, Hp, Hc, M_Hp, N_Hc, L_Hp) end -setconstraint!(::ExplicitMPC,kwargs...) = error("ExplicitMPC does not support constraints.") +setconstraint!(::ExplicitMPC; kwargs...) = error("ExplicitMPC does not support constraints.") function Base.show(io::IO, mpc::ExplicitMPC) Hp, Hc = mpc.Hp, mpc.Hc diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 1f9c8a011..7e113d9ab 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -4,7 +4,8 @@ struct NonLinMPC{ NT<:Real, SE<:StateEstimator, JM<:JuMP.GenericModel, - JEfunc<:Function + JEfunc<:Function, + P<:Any } <: PredictiveController{NT} estim::SE # note: `NT` and the number type `JNT` in `JuMP.GenericModel{JNT}` can be @@ -21,6 +22,7 @@ struct NonLinMPC{ L_Hp::Hermitian{NT, Matrix{NT}} E::NT JE::JEfunc + p::P R̂u0::Vector{NT} R̂y0::Vector{NT} noR̂u::Bool @@ -46,12 +48,13 @@ struct NonLinMPC{ Yop::Vector{NT} Dop::Vector{NT} buffer::PredictiveControllerBuffer{NT} - function NonLinMPC{NT, SE, JM, JEFunc}( - estim::SE, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt, JE::JEFunc, optim::JM - ) where {NT<:Real, SE<:StateEstimator, JM<:JuMP.GenericModel, JEFunc<:Function} + function NonLinMPC{NT, SE, JM, JEFunc, P}( + estim::SE, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt, JE::JEFunc, p::P, optim::JM + ) where {NT<:Real, SE<:StateEstimator, JM<:JuMP.GenericModel, JEFunc<:Function, P<:Any} model = estim.model nu, ny, nd, nx̂ = model.nu, model.ny, model.nd, estim.nx̂ ŷ = copy(model.yop) # dummy vals (updated just before optimization) + validate_JE(NT, JE) validate_weights(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt) # convert `Diagonal` to normal `Matrix` if required: M_Hp = Hermitian(convert(Matrix{NT}, M_Hp), :L) @@ -77,11 +80,11 @@ struct NonLinMPC{ nΔŨ = size(Ẽ, 2) ΔŨ = zeros(NT, nΔŨ) buffer = PredictiveControllerBuffer{NT}(nu, ny, nd, Hp) - mpc = new{NT, SE, JM, JEFunc}( + mpc = new{NT, SE, JM, JEFunc, P}( estim, optim, con, ΔŨ, ŷ, Hp, Hc, nϵ, - M_Hp, Ñ_Hc, L_Hp, Ewt, JE, + M_Hp, Ñ_Hc, L_Hp, Ewt, JE, p, R̂u0, R̂y0, noR̂u, S̃, T, T_lastu0, Ẽ, F, G, J, K, V, B, @@ -109,12 +112,12 @@ controller minimizes the following objective function at each discrete time ``k` + \mathbf{(ΔU)}' \mathbf{N}_{H_c} \mathbf{(ΔU)} \\& + \mathbf{(R̂_u - U)}' \mathbf{L}_{H_p} \mathbf{(R̂_u - U)} + C ϵ^2 - + E J_E(\mathbf{U}_E, \mathbf{Ŷ}_E, \mathbf{D̂}_E) + + E J_E(\mathbf{U}_E, \mathbf{Ŷ}_E, \mathbf{D̂}_E, \mathbf{p}) \end{aligned} ``` See [`LinMPC`](@ref) for the variable definitions. The custom economic function ``J_E`` can penalizes solutions with high economic costs. Setting all the weights to 0 except ``E`` -creates a pure economic model predictive controller (EMPC). The arguments of ``J_E`` are +creates a pure economic model predictive controller (EMPC). The arguments of ``J_E`` include the manipulated inputs, the predicted outputs and measured disturbances from ``k`` to ``k+H_p`` inclusively: ```math @@ -124,10 +127,11 @@ the manipulated inputs, the predicted outputs and measured disturbances from ``k ``` since ``H_c ≤ H_p`` implies that ``\mathbf{Δu}(k+H_p) = \mathbf{0}`` or ``\mathbf{u}(k+H_p)= \mathbf{u}(k+H_p-1)``. The vector ``\mathbf{D̂}`` includes the predicted measured disturbance -over ``H_p``. +over ``H_p``. The argument ``\mathbf{p}`` is a custom parameter object of any type but use a +mutable one if you want to modify it later e.g.: a vector. !!! tip - Replace any of the 3 arguments with `_` if not needed (see `JE` default value below). + Replace any of the 4 arguments with `_` if not needed (see `JE` default value below). This method uses the default state estimator : @@ -150,7 +154,8 @@ This method uses the default state estimator : - `L_Hp=diagm(repeat(Lwt,Hp))` : positive semidefinite symmetric matrix ``\mathbf{L}_{H_p}``. - `Cwt=1e5` : slack variable weight ``C`` (scalar), use `Cwt=Inf` for hard constraints only. - `Ewt=0.0` : economic costs weight ``E`` (scalar). -- `JE=(_,_,_)->0.0` : economic function ``J_E(\mathbf{U}_E, \mathbf{Ŷ}_E, \mathbf{D̂}_E)``. +- `JE=(_,_,_,_)->0.0` : economic function ``J_E(\mathbf{U}_E, \mathbf{Ŷ}_E, \mathbf{D̂}_E, \mathbf{p})``. +- `p=model.p` : ``J_E`` function parameter ``\mathbf{p}`` (any type). - `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). @@ -159,7 +164,7 @@ This method uses the default state estimator : # Examples ```jldoctest -julia> model = NonLinModel((x,u,_)->0.5x+u, (x,_)->2x, 10.0, 1, 1, 1, solver=nothing); +julia> model = NonLinModel((x,u,_,_)->0.5x+u, (x,_,_)->2x, 10.0, 1, 1, 1, solver=nothing); julia> mpc = NonLinMPC(model, Hp=20, Hc=1, Cwt=1e6) NonLinMPC controller with a sample time Ts = 10.0 s, Ipopt optimizer, UnscentedKalmanFilter estimator and: @@ -200,12 +205,13 @@ function NonLinMPC( L_Hp = diagm(repeat(Lwt, Hp)), Cwt = DEFAULT_CWT, Ewt = DEFAULT_EWT, - JE::Function = (_,_,_) -> 0.0, + JE::Function = (args...) -> 0.0, + p = model.p, optim::JuMP.GenericModel = JuMP.Model(DEFAULT_NONLINMPC_OPTIMIZER, add_bridges=false), kwargs... ) estim = UnscentedKalmanFilter(model; kwargs...) - NonLinMPC(estim; Hp, Hc, Mwt, Nwt, Lwt, Cwt, Ewt, JE, M_Hp, N_Hc, L_Hp, optim) + NonLinMPC(estim; Hp, Hc, Mwt, Nwt, Lwt, Cwt, Ewt, JE, p, M_Hp, N_Hc, L_Hp, optim) end function NonLinMPC( @@ -220,12 +226,13 @@ function NonLinMPC( L_Hp = diagm(repeat(Lwt, Hp)), Cwt = DEFAULT_CWT, Ewt = DEFAULT_EWT, - JE::Function = (_,_,_) -> 0.0, + JE::Function = (args...) -> 0.0, + p = model.p, optim::JuMP.GenericModel = JuMP.Model(DEFAULT_NONLINMPC_OPTIMIZER, add_bridges=false), kwargs... ) estim = SteadyKalmanFilter(model; kwargs...) - NonLinMPC(estim; Hp, Hc, Mwt, Nwt, Lwt, Cwt, Ewt, JE, M_Hp, N_Hc, L_Hp, optim) + NonLinMPC(estim; Hp, Hc, Mwt, Nwt, Lwt, Cwt, Ewt, JE, p, M_Hp, N_Hc, L_Hp, optim) end @@ -236,7 +243,7 @@ Use custom state estimator `estim` to construct `NonLinMPC`. # Examples ```jldoctest -julia> model = NonLinModel((x,u,_)->0.5x+u, (x,_)->2x, 10.0, 1, 1, 1, solver=nothing); +julia> model = NonLinModel((x,u,_,_)->0.5x+u, (x,_,_)->2x, 10.0, 1, 1, 1, solver=nothing); julia> estim = UnscentedKalmanFilter(model, σQint_ym=[0.05]); @@ -264,15 +271,33 @@ function NonLinMPC( L_Hp = diagm(repeat(Lwt, Hp)), Cwt = DEFAULT_CWT, Ewt = DEFAULT_EWT, - JE::JEFunc = (_,_,_) -> 0.0, + JE::JEFunc = (args...) -> 0.0, + p::P = estim.model.p, optim::JM = JuMP.Model(DEFAULT_NONLINMPC_OPTIMIZER, add_bridges=false), -) where {NT<:Real, SE<:StateEstimator{NT}, JM<:JuMP.GenericModel, JEFunc<:Function} +) where {NT<:Real, SE<:StateEstimator{NT}, JM<:JuMP.GenericModel, JEFunc<:Function, P<:Any} nk = estimate_delays(estim.model) if Hp ≤ nk @warn("prediction horizon Hp ($Hp) ≤ estimated number of delays in model "* "($nk), the closed-loop system may be unstable or zero-gain (unresponsive)") end - return NonLinMPC{NT, SE, JM, JEFunc}(estim, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt, JE, optim) + return NonLinMPC{NT, SE, JM, JEFunc, P}( + estim, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt, JE, p, optim + ) +end + +""" + validate_JE(NT, JE) -> nothing + +Validate `JE` function argument signature. +""" +function validate_JE(NT, JE) + if !hasmethod(JE, Tuple{Vector{NT}, Vector{NT}, Vector{NT}, Any}) + error( + "the economic function has no method with type signature "* + "JE(UE::Vector{$(NT)}, ŶE::Vector{$(NT)}, D̂E::Vector{$(NT)}, p::Any)" + ) + end + return nothing end """ @@ -285,7 +310,7 @@ function addinfo!(info, mpc::NonLinMPC) UE = [U; U[(end - mpc.estim.model.nu + 1):end]] ŶE = [ŷ; Ŷ] D̂E = [d; D̂] - info[:JE] = mpc.JE(UE, ŶE, D̂E) + info[:JE] = mpc.JE(UE, ŶE, D̂E, mpc.p) info[:sol] = JuMP.solution_summary(mpc.optim, verbose=true) return info end @@ -457,4 +482,22 @@ function con_nonlinprog!(g, mpc::NonLinMPC, ::SimModel, x̂0end, Ŷ0, ΔŨ) end "No nonlinear constraints if `model` is a [`LinModel`](@ref), return `g` unchanged." -con_nonlinprog!(g, ::NonLinMPC, ::LinModel, _ , _ , _ ) = g \ No newline at end of file +con_nonlinprog!(g, ::NonLinMPC, ::LinModel, _ , _ , _ ) = g + +"Evaluate the economic term of the objective function for [`NonLinMPC`](@ref)." +function obj_econ!(U0, Ȳ, mpc::NonLinMPC, model::SimModel, Ŷ0, ΔŨ) + if !iszero(mpc.E) + ny, Hp, ŷ, D̂E = model.ny, mpc.Hp, mpc.ŷ, mpc.D̂E + U = U0 + U .+= mpc.Uop + uend = @views U[(end-model.nu+1):end] + Ŷ = Ȳ + Ŷ .= Ŷ0 .+ mpc.Yop + UE = [U; uend] + ŶE = [ŷ; Ŷ] + E_JE = mpc.E*mpc.JE(UE, ŶE, D̂E, mpc.p) + else + E_JE = 0.0 + end + return E_JE +end \ No newline at end of file diff --git a/src/estimator/construct.jl b/src/estimator/construct.jl index 4c43d3d2c..326c8c63b 100644 --- a/src/estimator/construct.jl +++ b/src/estimator/construct.jl @@ -11,12 +11,12 @@ manipulated input and measured outputs. The function returns the state-space mat ```math \begin{aligned} \mathbf{x_{s}}(k+1) &= \mathbf{A_s x_s}(k) + \mathbf{B_s e}(k) \\ -\mathbf{y_{s_{u}}}(k) &= \mathbf{C_{s_{u}} x_s}(k) \\ -\mathbf{y_{s_{ym}}}(k) &= \mathbf{C_{s_{ym}} x_s}(k) +\mathbf{y_{s_{u}}}(k) &= \mathbf{C_{s_{u}} x_s}(k) \\ +\mathbf{y_{s_{y}}}(k) &= \mathbf{C_{s_{y}} x_s}(k) \end{aligned} ``` where ``\mathbf{e}(k)`` is an unknown zero mean white noise and ``\mathbf{A_s} = -\mathrm{diag}(\mathbf{A_{s_{u}}, A_{s_{ym}}})``. The estimations does not use ``\mathbf{B_s}``, +\mathrm{diag}(\mathbf{A_{s_{u}}, A_{s_{y}}})``. The estimations does not use ``\mathbf{B_s}``, it is thus ignored. The function [`init_integrators`](@ref) builds the state-space matrices. """ function init_estimstoch( @@ -119,7 +119,30 @@ returns the augmented matrices `Â`, `B̂u`, `Ĉ`, `B̂d` and `D̂d`: ``` An error is thrown if the augmented model is not observable and `verify_obsv == true`. The augmented operating points `x̂op` and `f̂op` are simply ``\mathbf{x_{op}}`` and -``\mathbf{f_{op}}`` vectors appended with zeros (see [`setop!`](@ref)). +``\mathbf{f_{op}}`` vectors appended with zeros (see [`setop!`](@ref)). See Extended Help +for a detailed definition of the augmented matrices. + +# Extended Help +!!! details "Extended Help" + Using the `As`, `Cs_u` and `Cs_y` matrices of the stochastic model provided in argument + and the `model.A`, `model.Bu`, `model.Bd`, `model.C`, `model.Dd` matrices, the + state-space matrices of the augmented model are defined as follows: + ```math + \begin{aligned} + \mathbf{Â} &= \begin{bmatrix} + \mathbf{A} & \mathbf{B_u C_{s_u}} \\ + \mathbf{0} & \mathbf{A_s} \end{bmatrix} \\ + \mathbf{B̂_u} &= \begin{bmatrix} + \mathbf{B_u} \\ + \mathbf{0} \end{bmatrix} \\ + \mathbf{Ĉ} &= \begin{bmatrix} + \mathbf{C} & \mathbf{C_{s_y}} \end{bmatrix} \\ + \mathbf{B̂_d} &= \begin{bmatrix} + \mathbf{B_d} \\ + \mathbf{0} \end{bmatrix} \\ + \mathbf{D̂_d} &= \mathbf{D_d} + \end{aligned} + ``` """ function augment_model(model::LinModel{NT}, As, Cs_u, Cs_y; verify_obsv=true) where NT<:Real nu, nx, nd = model.nu, model.nx, model.nd diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index 2714fd2b0..25eff2eff 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -32,7 +32,8 @@ function returns the next state of the augmented model, defined as: ``` where ``\mathbf{x̂_0}(k+1)`` is stored in `x̂next0` argument. The method mutates `x̂next0` and `û0` in place, the latter stores the input vector of the augmented model -``\mathbf{u_0 + ŷ_{s_u}}``. +``\mathbf{u_0 + ŷ_{s_u}}``. The model parameter vector `model.p` is not included in the +function signature for conciseness. """ function f̂!(x̂next0, û0, estim::StateEstimator, model::SimModel, x̂0, u0, d0) # `@views` macro avoid copies with matrix slice operator e.g. [a:b] @@ -40,7 +41,7 @@ function f̂!(x̂next0, û0, estim::StateEstimator, model::SimModel, x̂0, u0, @views x̂d_next, x̂s_next = x̂next0[1:model.nx], x̂next0[model.nx+1:end] mul!(û0, estim.Cs_u, x̂s) û0 .+= u0 - f!(x̂d_next, model, x̂d, û0, d0) + f!(x̂d_next, model, x̂d, û0, d0, model.p) mul!(x̂s_next, estim.As, x̂s) return nothing end @@ -65,7 +66,7 @@ Mutating output function ``\mathbf{ĥ}`` of the augmented model, see [`f̂!`](@ function ĥ!(ŷ0, estim::StateEstimator, model::SimModel, x̂0, d0) # `@views` macro avoid copies with matrix slice operator e.g. [a:b] @views x̂d, x̂s = x̂0[1:model.nx], x̂0[model.nx+1:end] - h!(ŷ0, model, x̂d, d0) + h!(ŷ0, model, x̂d, d0, model.p) mul!(ŷ0, estim.Cs_y, x̂s, 1, 1) return nothing end diff --git a/src/estimator/internal_model.jl b/src/estimator/internal_model.jl index 7d2f04c38..f1b8bbfe1 100644 --- a/src/estimator/internal_model.jl +++ b/src/estimator/internal_model.jl @@ -171,16 +171,16 @@ end State function ``\mathbf{f̂}`` of [`InternalModel`](@ref) for [`NonLinModel`](@ref). -It calls `model.f!(x̂next0, x̂0, u0 ,d0)` since this estimator does not augment the states. +It calls `model.f!(x̂next0, x̂0, u0 ,d0, model.p)` since this estimator does not augment the states. """ -f̂!(x̂next0, _, ::InternalModel, model::NonLinModel, x̂0, u0, d0) = model.f!(x̂next0, x̂0, u0, d0) +f̂!(x̂next0, _, ::InternalModel, model::NonLinModel, x̂0, u0, d0) = model.f!(x̂next0, x̂0, u0, d0, model.p) @doc raw""" ĥ!(ŷ0, estim::InternalModel, model::NonLinModel, x̂0, d0) Output function ``\mathbf{ĥ}`` of [`InternalModel`](@ref), it calls `model.h!`. """ -ĥ!(x̂next0, ::InternalModel, model::NonLinModel, x̂0, d0) = model.h!(x̂next0, x̂0, d0) +ĥ!(x̂next0, ::InternalModel, model::NonLinModel, x̂0, d0) = model.h!(x̂next0, x̂0, d0, model.p) @doc raw""" @@ -246,7 +246,7 @@ Compute the current stochastic output estimation `ŷs` for [`InternalModel`](@r """ function correct_estimate!(estim::InternalModel, y0m, d0) ŷ0d = estim.buffer.ŷ - h!(ŷ0d, estim.model, estim.x̂d, d0) + h!(ŷ0d, estim.model, estim.x̂d, d0, estim.model.p) ŷs = estim.ŷs for j in eachindex(ŷs) # broadcasting was allocating unexpectedly, so we use a loop if j in estim.i_ym @@ -279,7 +279,7 @@ function update_estimate!(estim::InternalModel, _ , d0, u0) x̂d, x̂s, ŷs = estim.x̂d, estim.x̂s, estim.ŷs # -------------- deterministic model --------------------- x̂dnext = estim.buffer.x̂ - f!(x̂dnext, model, x̂d, u0, d0) + f!(x̂dnext, model, x̂d, u0, d0, model.p) x̂d .= x̂dnext # this also updates estim.x̂0 (they are the same object) # --------------- stochastic model ----------------------- x̂snext = estim.x̂snext @@ -317,7 +317,7 @@ function init_estimate!(estim::InternalModel, model::LinModel{NT}, y0m, d0, u0) # TODO: use estim.buffer.x̂ to reduce the allocation: x̂d .= (I - model.A)\(model.Bu*u0 + model.Bd*d0 + model.fop - model.xop) ŷ0d = estim.buffer.ŷ - h!(ŷ0d, model, x̂d, d0) + h!(ŷ0d, model, x̂d, d0, model.p) ŷs = ŷ0d ŷs[estim.i_ym] .= @views y0m .- ŷ0d[estim.i_ym] # ŷs=0 for unmeasured outputs : diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index 786bb582c..1c40a280e 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -582,9 +582,10 @@ See [`SteadyKalmanFilter`](@ref) for details on ``\mathbf{v}(k), \mathbf{w}(k)`` \text{diag}(Q, Q_{int_u}, Q_{int_{ym}})}`` and ``\mathbf{R̂ = R}``. The functions ``\mathbf{f̂, ĥ}`` are `model` state-space functions augmented with the stochastic model of the unmeasured disturbances, which is specified by the numbers of integrator `nint_u` and -`nint_ym` (see Extended Help). The ``\mathbf{ĥ^m}`` function represents the measured outputs -of ``\mathbf{ĥ}`` function (and unmeasured ones, for ``\mathbf{ĥ^u}``). The matrix -``\mathbf{P̂}`` is the estimation error covariance of `model` state augmented with the +`nint_ym` (see Extended Help). Model parameters ``\mathbf{p}`` are not argument of +``\mathbf{f̂, ĥ}`` functions for conciseness. The ``\mathbf{ĥ^m}`` function represents the +measured outputs of ``\mathbf{ĥ}`` function (and unmeasured ones, for ``\mathbf{ĥ^u}``). The +matrix ``\mathbf{P̂}`` is the estimation error covariance of `model` state augmented with the 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). @@ -622,7 +623,7 @@ initial state estimate ``\mathbf{x̂}_{-1}(0)`` can be manually specified with [ # Examples ```jldoctest -julia> model = NonLinModel((x,u,_)->0.1x+u, (x,_)->2x, 10.0, 1, 1, 1, solver=nothing); +julia> model = NonLinModel((x,u,_,_)->0.1x+u, (x,_,_)->2x, 10.0, 1, 1, 1, solver=nothing); julia> estim = UnscentedKalmanFilter(model, σR=[1], nint_ym=[2], σPint_ym_0=[1, 1]) UnscentedKalmanFilter estimator with a sample time Ts = 10.0 s, NonLinModel and: @@ -950,7 +951,7 @@ automatic differentiation. # Examples ```jldoctest -julia> model = NonLinModel((x,u,_)->0.2x+u, (x,_)->-3x, 5.0, 1, 1, 1, solver=nothing); +julia> model = NonLinModel((x,u,_,_)->0.2x+u, (x,_,_)->-3x, 5.0, 1, 1, 1, solver=nothing); julia> estim = ExtendedKalmanFilter(model, σQ=[2], σQint_ym=[2], σP_0=[0.1], σPint_ym_0=[0.1]) ExtendedKalmanFilter estimator with a sample time Ts = 5.0 s, NonLinModel and: diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 2cbb1e711..023af2d94 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -256,7 +256,7 @@ See [`UnscentedKalmanFilter`](@ref) for details on the augmented process model a # Examples ```jldoctest -julia> model = NonLinModel((x,u,_)->0.1x+u, (x,_)->2x, 10.0, 1, 1, 1, solver=nothing); +julia> model = NonLinModel((x,u,_,_)->0.1x+u, (x,_,_)->2x, 10.0, 1, 1, 1, solver=nothing); julia> estim = MovingHorizonEstimator(model, He=5, σR=[1], σP_0=[0.01]) MovingHorizonEstimator estimator with a sample time Ts = 10.0 s, Ipopt optimizer, NonLinModel and: diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 6ee80cc68..3e29abc14 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -67,7 +67,7 @@ end Get additional info on `estim` [`MovingHorizonEstimator`](@ref) optimum for troubleshooting. -If `estim.direct==true`, the function should be called after calling [`preparestate!`](@ref) +If `estim.direct==true`, the function should be called after calling [`preparestate!`](@ref). Otherwise, call it after [`updatestate!`](@ref). It returns the dictionary `info` with the following fields: diff --git a/src/model/linearization.jl b/src/model/linearization.jl index 671314655..9239919dc 100644 --- a/src/model/linearization.jl +++ b/src/model/linearization.jl @@ -21,7 +21,7 @@ necessarily an equilibrium, details in Extended Help). The Jacobians of ``\mathb # Examples ```jldoctest -julia> model = NonLinModel((x,u,_)->x.^3 + u, (x,_)->x, 0.1, 1, 1, 1, solver=nothing); +julia> model = NonLinModel((x,u,_,_)->x.^3 + u, (x,_,_)->x, 0.1, 1, 1, 1, solver=nothing); julia> linmodel = linearize(model, x=[10.0], u=[0.0]); @@ -96,7 +96,7 @@ The keyword arguments are identical to [`linearize`](@ref). # Examples ```jldoctest -julia> model = NonLinModel((x,u,_)->x.^3 + u, (x,_)->x, 0.1, 1, 1, 1, solver=nothing); +julia> model = NonLinModel((x,u,_,_)->x.^3 + u, (x,_,_)->x, 0.1, 1, 1, 1, solver=nothing); julia> linmodel = linearize(model, x=[10.0], u=[0.0]); linmodel.A 1×1 Matrix{Float64}: @@ -121,22 +121,22 @@ function linearize!( A::Matrix{NT}, Bu::Matrix{NT}, Bd::Matrix{NT} = linmodel.A, linmodel.Bu, linmodel.Bd C::Matrix{NT}, Dd::Matrix{NT} = linmodel.C, linmodel.Dd xnext0::Vector{NT}, y0::Vector{NT} = linmodel.buffer.x, linmodel.buffer.y - myf_x0!(xnext0, x0) = f!(xnext0, nonlinmodel, x0, u0, d0) - myf_u0!(xnext0, u0) = f!(xnext0, nonlinmodel, x0, u0, d0) - myf_d0!(xnext0, d0) = f!(xnext0, nonlinmodel, x0, u0, d0) - myh_x0!(y0, x0) = h!(y0, nonlinmodel, x0, d0) - myh_d0!(y0, d0) = h!(y0, nonlinmodel, x0, d0) + myf_x0!(xnext0, x0) = f!(xnext0, nonlinmodel, x0, u0, d0, model.p) + myf_u0!(xnext0, u0) = f!(xnext0, nonlinmodel, x0, u0, d0, model.p) + myf_d0!(xnext0, d0) = f!(xnext0, nonlinmodel, x0, u0, d0, model.p) + myh_x0!(y0, x0) = h!(y0, nonlinmodel, x0, d0, model.p) + myh_d0!(y0, d0) = h!(y0, nonlinmodel, x0, d0, model.p) ForwardDiff.jacobian!(A, myf_x0!, xnext0, x0) ForwardDiff.jacobian!(Bu, myf_u0!, xnext0, u0) ForwardDiff.jacobian!(Bd, myf_d0!, xnext0, d0) ForwardDiff.jacobian!(C, myh_x0!, y0, x0) ForwardDiff.jacobian!(Dd, myh_d0!, y0, d0) # --- compute the nonlinear model output at operating points --- - h!(y0, nonlinmodel, x0, d0) + h!(y0, nonlinmodel, x0, d0, model.p) y = y0 y .= y0 .+ nonlinmodel.yop # --- compute the nonlinear model next state at operating points --- - f!(xnext0, nonlinmodel, x0, u0, d0) + f!(xnext0, nonlinmodel, x0, u0, d0, model.p) xnext = xnext0 xnext .= xnext0 .+ nonlinmodel.fop .- nonlinmodel.xop # --- modify the linear model operating points --- diff --git a/src/model/linmodel.jl b/src/model/linmodel.jl index 4ccb7ffe0..206e4e4a4 100644 --- a/src/model/linmodel.jl +++ b/src/model/linmodel.jl @@ -5,6 +5,7 @@ struct LinModel{NT<:Real} <: SimModel{NT} Bd ::Matrix{NT} Dd ::Matrix{NT} x0::Vector{NT} + p ::Nothing Ts::NT t::Vector{NT} nu::Int @@ -35,6 +36,7 @@ struct LinModel{NT<:Real} <: SimModel{NT} size(C) == (ny,nx) || error("C size must be $((ny,nx))") size(Bd) == (nx,nd) || error("Bd size must be $((nx,nd))") size(Dd) == (ny,nd) || error("Dd size must be $((ny,nd))") + p = nothing # the parameter field p is not used for LinModel Ts > 0 || error("Sampling time Ts must be positive") uop = zeros(NT, nu) yop = zeros(NT, ny) @@ -50,7 +52,8 @@ struct LinModel{NT<:Real} <: SimModel{NT} buffer = SimModelBuffer{NT}(nu, nx, ny, nd) return new{NT}( A, Bu, C, Bd, Dd, - x0, + x0, + p, Ts, t, nu, nx, ny, nd, uop, yop, dop, xop, fop, @@ -254,11 +257,11 @@ function steadystate!(model::LinModel, u0, d0) end """ - f!(xnext0, model::LinModel, x0, u0, d0) -> nothing + f!(xnext0, model::LinModel, x0, u0, d0, p) -> nothing Evaluate `xnext0 = A*x0 + Bu*u0 + Bd*d0` in-place when `model` is a [`LinModel`](@ref). """ -function f!(xnext0, model::LinModel, x0, u0, d0) +function f!(xnext0, model::LinModel, x0, u0, d0, _ ) mul!(xnext0, model.A, x0) mul!(xnext0, model.Bu, u0, 1, 1) mul!(xnext0, model.Bd, d0, 1, 1) @@ -267,11 +270,11 @@ end """ - h!(y0, model::LinModel, x0, d0) -> nothing + h!(y0, model::LinModel, x0, d0, p) -> nothing Evaluate `y0 = C*x0 + Dd*d0` in-place when `model` is a [`LinModel`](@ref). """ -function h!(y0, model::LinModel, x0, d0) +function h!(y0, model::LinModel, x0, d0, _ ) mul!(y0, model.C, x0) mul!(y0, model.Dd, d0, 1, 1) return nothing diff --git a/src/model/nonlinmodel.jl b/src/model/nonlinmodel.jl index 0952def94..0032bc2ae 100644 --- a/src/model/nonlinmodel.jl +++ b/src/model/nonlinmodel.jl @@ -1,7 +1,10 @@ -struct NonLinModel{NT<:Real, F<:Function, H<:Function, DS<:DiffSolver} <: SimModel{NT} +struct NonLinModel{ + NT<:Real, F<:Function, H<:Function, P<:Any, DS<:DiffSolver +} <: SimModel{NT} x0::Vector{NT} f!::F h!::H + p::P solver::DS Ts::NT t::Vector{NT} @@ -19,9 +22,9 @@ struct NonLinModel{NT<:Real, F<:Function, H<:Function, DS<:DiffSolver} <: SimMod dname::Vector{String} xname::Vector{String} buffer::SimModelBuffer{NT} - function NonLinModel{NT, F, H, DS}( - f!::F, h!::H, solver::DS, Ts, nu, nx, ny, nd - ) where {NT<:Real, F<:Function, H<:Function, DS<:DiffSolver} + function NonLinModel{NT, F, H, P, DS}( + f!::F, h!::H, Ts, nu, nx, ny, nd, p::P, solver::DS + ) where {NT<:Real, F<:Function, H<:Function, P<:Any, DS<:DiffSolver} Ts > 0 || error("Sampling time Ts must be positive") uop = zeros(NT, nu) yop = zeros(NT, ny) @@ -35,9 +38,10 @@ struct NonLinModel{NT<:Real, F<:Function, H<:Function, DS<:DiffSolver} <: SimMod x0 = zeros(NT, nx) t = zeros(NT, 1) buffer = SimModelBuffer{NT}(nu, nx, ny, nd) - return new{NT, F, H, DS}( + return new{NT, F, H, P, DS}( x0, - f!, h!, + f!, h!, + p, solver, Ts, t, nu, nx, ny, nd, @@ -49,8 +53,8 @@ struct NonLinModel{NT<:Real, F<:Function, H<:Function, DS<:DiffSolver} <: SimMod end @doc raw""" - NonLinModel{NT}(f::Function, h::Function, Ts, nu, nx, ny, nd=0; solver=RungeKutta(4)) - NonLinModel{NT}(f!::Function, h!::Function, Ts, nu, nx, ny, nd=0; 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; p=[], solver=RungeKutta(4)) Construct a nonlinear model from state-space functions `f`/`f!` and `h`/`h!`. @@ -59,23 +63,28 @@ continuous dynamics. Use `solver=nothing` for the discrete case (see Extended He functions are defined as: ```math \begin{aligned} - \mathbf{ẋ}(t) &= \mathbf{f}\Big( \mathbf{x}(t), \mathbf{u}(t), \mathbf{d}(t) \Big) \\ - \mathbf{y}(t) &= \mathbf{h}\Big( \mathbf{x}(t), \mathbf{d}(t) \Big) + \mathbf{ẋ}(t) &= \mathbf{f}\Big( \mathbf{x}(t), \mathbf{u}(t), \mathbf{d}(t), \mathbf{p} \Big) \\ + \mathbf{y}(t) &= \mathbf{h}\Big( \mathbf{x}(t), \mathbf{d}(t), \mathbf{p} \Big) \end{aligned} ``` -They can be implemented in two possible ways: - -1. **Non-mutating functions** (out-of-place): define them as `f(x, u, d) -> ẋ` and - `h(x, d) -> y`. This syntax is simple and intuitive but it allocates more memory. -2. **Mutating functions** (in-place): define them as `f!(ẋ, x, u, d) -> nothing` and - `h!(y, x, d) -> nothing`. This syntax reduces the allocations and potentially the +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: + +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. +2. **Mutating functions** (in-place): define them as `f!(ẋ, x, u, d, p) -> nothing` and + `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. +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 of any Julia object +but use a mutable type if you want to change them later e.g.: a vector. !!! tip - Replace the `d` argument with `_` if `nd = 0` (see Examples below). + 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 @@ -90,20 +99,20 @@ See also [`LinModel`](@ref). # Examples ```jldoctest -julia> f!(ẋ, x, u, _ ) = (ẋ .= -0.2x .+ u; nothing); +julia> f!(ẋ, x, u, _ , p) = (ẋ .= p*x .+ u; nothing); -julia> h!(y, x, _ ) = (y .= 0.1x; nothing); +julia> h!(y, x, _ , _ ) = (y .= 0.1x; nothing); -julia> model1 = NonLinModel(f!, h!, 5.0, 1, 1, 1) # continuous dynamics +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: 1 manipulated inputs u 1 states x 1 outputs y 0 measured disturbances d -julia> f(x, u, _ ) = 0.1x + u; +julia> f(x, u, _ , _ ) = 0.1x + u; -julia> h(x, _ ) = 2x; +julia> h(x, _ , _ ) = 2x; julia> model2 = NonLinModel(f, h, 2.0, 1, 1, 1, solver=nothing) # discrete dynamics NonLinModel with a sample time Ts = 2.0 s, empty solver and: @@ -118,39 +127,44 @@ NonLinModel with a sample time Ts = 2.0 s, empty solver and: State-space functions are similar for discrete dynamics: ```math \begin{aligned} - \mathbf{x}(k+1) &= \mathbf{f}\Big( \mathbf{x}(k), \mathbf{u}(k), \mathbf{d}(k) \Big) \\ - \mathbf{y}(k) &= \mathbf{h}\Big( \mathbf{x}(k), \mathbf{d}(k) \Big) + \mathbf{x}(k+1) &= \mathbf{f}\Big( \mathbf{x}(k), \mathbf{u}(k), \mathbf{d}(k), \mathbf{p} \Big) \\ + \mathbf{y}(k) &= \mathbf{h}\Big( \mathbf{x}(k), \mathbf{d}(k), \mathbf{p} \Big) \end{aligned} ``` with two possible implementations as well: - 1. **Non-mutating functions**: define them as `f(x, u, d) -> xnext` and `h(x, d) -> y`. - 2. **Mutating functions**: define them as `f!(xnext, x, u, d) -> nothing` and - `h!(y, x, d) -> nothing`. + 1. **Non-mutating functions**: define them as `f(x, u, d, p) -> xnext` and + `h(x, d, p) -> y`. + 2. **Mutating functions**: define them as `f!(xnext, x, u, d, p) -> nothing` and + `h!(y, x, d, p) -> nothing`. """ function NonLinModel{NT}( - f::Function, h::Function, Ts::Real, nu::Int, nx::Int, ny::Int, nd::Int=0; - solver=RungeKutta(4) + f::Function, h::Function, Ts::Real, nu::Int, nx::Int, ny::Int, nd::Int=0; + p=NT[], solver=RungeKutta(4) ) where {NT<:Real} isnothing(solver) && (solver=EmptySolver()) ismutating_f = validate_f(NT, f) ismutating_h = validate_h(NT, h) - f! = ismutating_f ? f : (xnext, x, u, d) -> xnext .= f(x, u, d) - h! = ismutating_h ? h : (y, x, d) -> y .= h(x, d) + f! = ismutating_f ? f : f!(xnext, x, u, d, p) = (xnext .= f(x, u, d, p); nothing) + h! = ismutating_h ? h : h!(y, x, d, p) = (y .= h(x, d, p); nothing) f!, h! = get_solver_functions(NT, solver, f!, h!, Ts, nu, nx, ny, nd) - F, H, DS = get_types(f!, h!, solver) - return NonLinModel{NT, F, H, DS}(f!, h!, solver, Ts, nu, nx, ny, nd) + F, H, P, DS = get_types(f!, h!, p, solver) + return NonLinModel{NT, F, H, P, DS}(f!, h!, Ts, nu, nx, ny, nd, p, solver) end function NonLinModel( - f::Function, h::Function, Ts::Real, nu::Int, nx::Int, ny::Int, nd::Int=0; - solver=RungeKutta(4) + f::Function, h::Function, Ts::Real, nu::Int, nx::Int, ny::Int, nd::Int=0; + p=Float64[], solver=RungeKutta(4) ) - return NonLinModel{Float64}(f, h, Ts, nu, nx, ny, nd; solver) + return NonLinModel{Float64}(f, h, Ts, nu, nx, ny, nd; p, solver) end "Get the types of `f!`, `h!` and `solver` to construct a `NonLinModel`." -get_types(::F, ::H, ::DS) where {F<:Function, H<:Function, DS<:DiffSolver} = F, H, DS +function get_types( + ::F, ::H, ::P, ::DS +) where {F<:Function, H<:Function, P<:Any, DS<:DiffSolver} + return F, H, P, DS +end """ validate_f(NT, f) -> ismutating @@ -158,12 +172,13 @@ get_types(::F, ::H, ::DS) where {F<:Function, H<:Function, DS<:DiffSolver} = F, Validate `f` function argument signature and return `true` if it is mutating. """ function validate_f(NT, f) - ismutating = hasmethod(f, Tuple{Vector{NT}, Vector{NT}, Vector{NT}, Vector{NT}}) - if !(ismutating || hasmethod(f, Tuple{Vector{NT}, Vector{NT}, Vector{NT}})) + ismutating = hasmethod(f, Tuple{Vector{NT}, Vector{NT}, Vector{NT}, Vector{NT}, Any}) + if !(ismutating || hasmethod(f, Tuple{Vector{NT}, Vector{NT}, Vector{NT}, Any})) error( "the state function has no method with type signature "* - "f(x::Vector{$(NT)}, u::Vector{$(NT)}, d::Vector{$(NT)}) or mutating form "* - "f!(xnext::Vector{$(NT)}, x::Vector{$(NT)}, u::Vector{$(NT)}, d::Vector{$(NT)})" + "f(x::Vector{$(NT)}, u::Vector{$(NT)}, d::Vector{$(NT)}, p::Any) or "* + "mutating form f!(xnext::Vector{$(NT)}, x::Vector{$(NT)}, u::Vector{$(NT)}, "* + "d::Vector{$(NT)}, p::Any)" ) end return ismutating @@ -175,12 +190,12 @@ end Validate `h` function argument signature and return `true` if it is mutating. """ function validate_h(NT, h) - ismutating = hasmethod(h, Tuple{Vector{NT}, Vector{NT}, Vector{NT}}) - if !(ismutating || hasmethod(h, Tuple{Vector{NT}, Vector{NT}})) + ismutating = hasmethod(h, Tuple{Vector{NT}, Vector{NT}, Vector{NT}, Any}) + if !(ismutating || hasmethod(h, Tuple{Vector{NT}, Vector{NT}, Any})) error( "the output function has no method with type signature "* - "h(x::Vector{$(NT)}, d::Vector{$(NT)}) or mutating form "* - "h!(y::Vector{$(NT)}, x::Vector{$(NT)}, d::Vector{$(NT)})" + "h(x::Vector{$(NT)}, d::Vector{$(NT)}, p::Any) or mutating form "* + "h!(y::Vector{$(NT)}, x::Vector{$(NT)}, d::Vector{$(NT)}, p::Any)" ) end return ismutating @@ -189,11 +204,11 @@ end "Do nothing if `model` is a [`NonLinModel`](@ref)." steadystate!(::SimModel, _ , _ ) = nothing -"Call `f!(xnext0, x0, u0, d0)` with `model.f!` method for [`NonLinModel`](@ref)." -f!(xnext0, model::NonLinModel, x0, u0, d0) = model.f!(xnext0, x0, u0, d0) +"Call `model.f!(xnext0, x0, u0, d0, p)` for [`NonLinModel`](@ref)." +f!(xnext0, model::NonLinModel, x0, u0, d0, p) = model.f!(xnext0, x0, u0, d0, p) -"Call `h!(y0, x0, d0)` with `model.h` method for [`NonLinModel`](@ref)." -h!(y0, model::NonLinModel, x0, d0) = model.h!(y0, x0, d0) +"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(::NonLinModel{<:Real, <:Function, <:Function, <:EmptySolver}) = ", empty solver" \ No newline at end of file +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 e157a3e5a..97f1ed7db 100644 --- a/src/model/solver.jl +++ b/src/model/solver.jl @@ -16,6 +16,9 @@ struct RungeKutta <: DiffSolver if order ≠ 4 throw(ArgumentError("only 4th order Runge-Kutta is supported.")) end + if order < 1 + throw(ArgumentError("order must be greater than 0")) + end if supersample < 1 throw(ArgumentError("supersample must be greater than 0")) end @@ -31,15 +34,7 @@ Create a 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). """ -function RungeKutta(order::Int=4; supersample::Int=1) - if order < 1 - throw(ArgumentError("order must be greater than 0")) - end - if supersample < 1 - throw(ArgumentError("supersample must be greater than 0")) - end - return RungeKutta(order, supersample) -end +RungeKutta(order::Int=4; supersample::Int=1) = RungeKutta(order, supersample) "Get the `f!` and `h!` functions for Runge-Kutta solver." function get_solver_functions(NT::DataType, solver::RungeKutta, fc!, hc!, Ts, _ , nx, _ , _ ) @@ -50,7 +45,7 @@ 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(xnext, x, u, d) + f! = function inner_solver_f!(xnext, x, u, d, p) CT = promote_type(eltype(x), eltype(u), eltype(d)) # dummy variable for get_tmp, necessary for PreallocationTools + Julia 1.6 : var::CT = 0 @@ -61,15 +56,15 @@ function get_solver_functions(NT::DataType, solver::RungeKutta, fc!, hc!, Ts, _ k4 = get_tmp(k4_cache, var) @. xcur = x for i=1:solver.supersample - xterm = xnext + xterm = xnext # TODO: move this out of the loop, just above (to test) ? @. xterm = xcur - fc!(k1, xterm, u, d) + fc!(k1, xterm, u, d, p) @. xterm = xcur + k1 * Ts_inner/2 - fc!(k2, xterm, u, d) + fc!(k2, xterm, u, d, p) @. xterm = xcur + k2 * Ts_inner/2 - fc!(k3, xterm, u, d) + fc!(k3, xterm, u, d, p) @. xterm = xcur + k3 * Ts_inner - fc!(k4, xterm, u, d) + fc!(k4, xterm, u, d, p) @. xcur = xcur + (k1 + 2k2 + 2k3 + k4)*Ts_inner/6 end @. xnext = xcur diff --git a/src/plot_sim.jl b/src/plot_sim.jl index 1b92736b3..15c1839d9 100644 --- a/src/plot_sim.jl +++ b/src/plot_sim.jl @@ -118,7 +118,7 @@ internal states. # Examples ```jldoctest -julia> plant = NonLinModel((x,u,d)->0.1x+u+d, (x,_)->2x, 10.0, 1, 1, 1, 1, solver=nothing); +julia> plant = NonLinModel((x,u,d,_)->0.1x+u+d, (x,_,_)->2x, 5, 1, 1, 1, 1, solver=nothing); julia> res = sim!(plant, 15, [0], [0], x_0=[1]) Simulation results of NonLinModel with 15 time steps. diff --git a/src/precompile.jl b/src/precompile.jl index 1e5ebb72e..53e753d2c 100644 --- a/src/precompile.jl +++ b/src/precompile.jl @@ -74,10 +74,13 @@ exmpc.estim() u = exmpc([55, 30]) sim!(exmpc, 3, [55, 30]) -f(x,u,_) = model.A*x + model.Bu*u -h(x,_) = model.C*x +f(x,u,_,model) = model.A*x + model.Bu*u +h(x,_,model) = model.C*x -nlmodel = setop!(NonLinModel(f, h, Ts, 2, 2, 2, solver=nothing), uop=[10, 10], yop=[50, 30]) +nlmodel = setop!( + NonLinModel(f, h, Ts, 2, 2, 2, solver=nothing, p=model), + uop=[10, 10], yop=[50, 30] +) y = nlmodel() nmpc_im = setconstraint!(NonLinMPC(InternalModel(nlmodel), Hp=10, Cwt=Inf), ymin=[45, -Inf]) initstate!(nmpc_im, nlmodel.uop, y) @@ -86,7 +89,9 @@ nmpc_im.estim() u = nmpc_im([55, 30]) sim!(nmpc_im, 3, [55, 30]) -nmpc_ukf = setconstraint!(NonLinMPC(UnscentedKalmanFilter(nlmodel), Hp=10, Cwt=1e3), ymin=[45, -Inf]) +nmpc_ukf = setconstraint!( + NonLinMPC(UnscentedKalmanFilter(nlmodel), Hp=10, Cwt=1e3), ymin=[45, -Inf] +) initstate!(nmpc_ukf, nlmodel.uop, y) preparestate!(nmpc_ukf, [55, 30]) u = nmpc_ukf([55, 30]) @@ -98,19 +103,24 @@ preparestate!(nmpc_ekf, [55, 30]) u = nmpc_ekf([55, 30]) sim!(nmpc_ekf, 3, [55, 30]) -nmpc_mhe = setconstraint!(NonLinMPC(MovingHorizonEstimator(nlmodel, He=2), Hp=10, Cwt=Inf), ymin=[45, -Inf]) +nmpc_mhe = setconstraint!( + NonLinMPC(MovingHorizonEstimator(nlmodel, He=2), Hp=10, Cwt=Inf), ymin=[45, -Inf] +) 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]) -function JE( _ , ŶE, _ ) - Ŷ = ŶE[3:end] - Eŷ = repeat([55; 30], 10) - Ŷ - return dot(Eŷ, I, Eŷ) +function JE( _ , ŶE, _ , R̂y) + Ŷ = ŶE[3:end] + Ȳ = R̂y - Ŷ + return dot(Ȳ, Ȳ) end -empc = setconstraint!(NonLinMPC(nlmodel, Mwt=[0, 0], Hp=10, Cwt=Inf, Ewt=1, JE=JE), ymin=[45, -Inf]) +R̂y = repeat([55; 30], 10) +empc = setconstraint!( + NonLinMPC(nlmodel, Mwt=[0, 0], Hp=10, Cwt=Inf, Ewt=1, JE=JE, p=R̂y), ymin=[45, -Inf] +) preparestate!(empc, [55, 30]) u = empc() sim!(empc, 3) \ No newline at end of file diff --git a/src/sim_model.jl b/src/sim_model.jl index 37927b142..4a49529ca 100644 --- a/src/sim_model.jl +++ b/src/sim_model.jl @@ -11,7 +11,7 @@ Functor allowing callable `SimModel` object as an alias for [`evaloutput`](@ref) # Examples ```jldoctest -julia> model = NonLinModel((x,u,_)->-x + u, (x,_)->x .+ 20, 10.0, 1, 1, 1, solver=nothing); +julia> model = NonLinModel((x,u,_,_)->-x + u, (x,_,_)->x .+ 20, 4, 1, 1, 1, solver=nothing); julia> y = model() 1-element Vector{Float64}: @@ -141,19 +141,19 @@ LinModel with a sample time Ts = 2.0 s and: """ function setname!(model::SimModel; u=nothing, y=nothing, d=nothing, x=nothing) if !isnothing(u) - size(u) == (model.nu,) || error("u size must be $((model.nu,))") + size(u) == (model.nu,) || throw(DimensionMismatch("u size must be $((model.nu,))")) model.uname .= u end if !isnothing(y) - size(y) == (model.ny,) || error("y size must be $((model.ny,))") + size(y) == (model.ny,) || throw(DimensionMismatch("y size must be $((model.ny,))")) model.yname .= y end if !isnothing(d) - size(d) == (model.nd,) || error("d size must be $((model.nd,))") + size(d) == (model.nd,) || throw(DimensionMismatch("d size must be $((model.nd,))")) model.dname .= d end if !isnothing(x) - size(x) == (model.nx,) || error("x size must be $((model.nx,))") + size(x) == (model.nx,) || throw(DimensionMismatch("x size must be $((model.nx,))")) model.xname .= x end return model @@ -187,10 +187,11 @@ detailstr(model::SimModel) = "" @doc raw""" initstate!(model::SimModel, u, d=[]) -> x -Init `model.x0` with manipulated inputs `u` and measured disturbances `d` steady-state. +Init `model.x0` with manipulated inputs `u` and meas. dist. `d` steady-state and reset time. -The method tries to initialize the model state ``\mathbf{x}`` at steady-state. It removes -the operating points on `u` and `d` and calls [`steadystate!`](@ref): +The method resets the discrete time step `model.k` at `0` and tries to initialize the model +state ``\mathbf{x}`` at steady-state. It removes the operating points on `u` and `d` and +calls [`steadystate!`](@ref): - If `model` is a [`LinModel`](@ref), the method computes the steady-state of current inputs `u` and measured disturbances `d`. @@ -233,9 +234,10 @@ end @doc raw""" updatestate!(model::SimModel, u, d=[]) -> xnext -Update `model.x0` states with current inputs `u` and measured disturbances `d`. +Update `model.x0` states with current inputs `u` and meas. dist. `d` for the next time step. The method computes and returns the model state for the next time step ``\mathbf{x}(k+1)``. +It also increment the discrete time step `model.k` by `1`. # Examples ```jldoctest @@ -251,7 +253,7 @@ function updatestate!(model::SimModel{NT}, u, d=model.buffer.empty) where NT <: u0, d0, xnext0 = model.buffer.u, model.buffer.d, model.buffer.x u0 .= u .- model.uop d0 .= d .- model.dop - f!(xnext0, model, model.x0, u0, d0) + f!(xnext0, model, model.x0, u0, d0, model.p) xnext0 .+= model.fop .- model.xop model.x0 .= xnext0 xnext = xnext0 @@ -280,7 +282,7 @@ function evaloutput(model::SimModel{NT}, d=model.buffer.empty) where NT <: Real validate_args(model, d) d0, y0 = model.buffer.d, model.buffer.y d0 .= d .- model.dop - h!(y0, model, model.x0, d0) + h!(y0, model, model.x0, d0, model.p) y = y0 y .+= model.yop return y diff --git a/test/test_plot_sim.jl b/test/test_plot_sim.jl index 3662b1abf..3450b7bba 100644 --- a/test/test_plot_sim.jl +++ b/test/test_plot_sim.jl @@ -110,6 +110,18 @@ end @test all(p5[end-4][2][:y] .≈ +101) @test all(p5[end-3][2][:y] .≈ +102) @test all(p5[end-2][2][:y] .≈ +103) + p6 = plot(res, plotxwithx̂=true, plotx̂min=true, plotx̂max=false) + @test p6[1][1][:x] ≈ res.T_data + @test all(p6[end-5][3][:y] .≈ -100) + @test all(p6[end-4][3][:y] .≈ -101) + @test all(p6[end-3][3][:y] .≈ -102) + @test all(p6[end-2][3][:y] .≈ -103) + p7 = plot(res, plotxwithx̂=true, plotx̂min=false, plotx̂max=true) + @test p7[1][1][:x] ≈ res.T_data + @test all(p7[end-5][3][:y] .≈ +100) + @test all(p7[end-4][3][:y] .≈ +101) + @test all(p7[end-3][3][:y] .≈ +102) + @test all(p7[end-2][3][:y] .≈ +103) end @testset "PredictiveController quick simulation" begin @@ -147,7 +159,10 @@ end end @testset "PredictiveController Plots" begin - mpc = LinMPC(LinModel(sys, Ts, i_d=[3]), Lwt=[0.01, 0.01]) + estim = MovingHorizonEstimator(LinModel(sys, Ts, i_d=[3]), He=5) + estim = setconstraint!(estim, x̂min=[-100,-101,-102,-103,-104,-105]) + estim = setconstraint!(estim, x̂max=[+100,+101,+102,+103,+104,+105]) + mpc = LinMPC(estim, Lwt=[0.01, 0.01]) mpc = setconstraint!(mpc, umin=[-50, -51], umax=[52, 53], ymin=[-54,-55], ymax=[56,57]) res = sim!(mpc, 15) p1 = plot(res, plotŷ=true) @@ -198,4 +213,36 @@ end @test p8[1][1][:x] ≈ res.T_data @test all(p8[end-4][3][:y] .≈ 56.0) @test all(p8[end-3][3][:y] .≈ 57.0) + p9 = plot(res, plotx̂=true, plotx̂min=true, plotx̂max=false) + @test p9[1][1][:x] ≈ res.T_data + @test all(p9[end-5][2][:y] .≈ -100.0) + @test all(p9[end-4][2][:y] .≈ -101.0) + @test all(p9[end-3][2][:y] .≈ -102.0) + @test all(p9[end-2][2][:y] .≈ -103.0) + @test all(p9[end-1][2][:y] .≈ -104.0) + @test all(p9[end-0][2][:y] .≈ -105.0) + p10 = plot(res, plotx̂=true, plotx̂min=false, plotx̂max=true) + @test p10[1][1][:x] ≈ res.T_data + @test all(p10[end-5][2][:y] .≈ +100.0) + @test all(p10[end-4][2][:y] .≈ +101.0) + @test all(p10[end-3][2][:y] .≈ +102.0) + @test all(p10[end-2][2][:y] .≈ +103.0) + @test all(p10[end-1][2][:y] .≈ +104.0) + @test all(p10[end-0][2][:y] .≈ +105.0) + p11 = plot(res, plotxwithx̂=true, plotx̂min=true, plotx̂max=false) + @test p11[1][1][:x] ≈ res.T_data + @test all(p11[end-5][3][:y] .≈ -100.0) + @test all(p11[end-4][3][:y] .≈ -101.0) + @test all(p11[end-3][3][:y] .≈ -102.0) + @test all(p11[end-2][3][:y] .≈ -103.0) + @test all(p11[end-1][2][:y] .≈ -104.0) + @test all(p11[end-0][2][:y] .≈ -105.0) + p12 = plot(res, plotxwithx̂=true, plotx̂min=false, plotx̂max=true) + @test p12[1][1][:x] ≈ res.T_data + @test all(p12[end-5][3][:y] .≈ +100.0) + @test all(p12[end-4][3][:y] .≈ +101.0) + @test all(p12[end-3][3][:y] .≈ +102.0) + @test all(p12[end-2][3][:y] .≈ +103.0) + @test all(p12[end-1][2][:y] .≈ +104.0) + @test all(p12[end-0][2][:y] .≈ +105.0) end \ No newline at end of file diff --git a/test/test_predictive_control.jl b/test/test_predictive_control.jl index edfb46897..5213db7fd 100644 --- a/test/test_predictive_control.jl +++ b/test/test_predictive_control.jl @@ -134,7 +134,8 @@ end preparestate!(mpc1, [50, 30]) updatestate!(mpc1, mpc1.estim.model.uop, [50, 30]) @test mpc1.estim.x̂0 ≈ [0,0,0,0] - preparestate!(mpc1, [50, 30]) + # do not call preparestate! before moveinput! for the warning: + moveinput!(mpc1, [10, 50]) @test_throws ArgumentError updatestate!(mpc1, [0,0]) end @@ -348,6 +349,8 @@ end model2 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), zeros(1,0), zeros(1,0), 1.0) mpc13 = ExplicitMPC(model2) @test isa(mpc13, ExplicitMPC{Float32}) + + @test_throws ArgumentError LinMPC(model, Hp=0) end @testset "ExplicitMPC moves and getinfo" begin @@ -471,9 +474,9 @@ end linmodel1 = LinModel(sys,Ts,i_d=[3]) nmpc0 = NonLinMPC(linmodel1, Hp=15) @test isa(nmpc0.estim, SteadyKalmanFilter) - f = (x,u,d) -> linmodel1.A*x + linmodel1.Bu*u + linmodel1.Bd*d - h = (x,d) -> linmodel1.C*x + linmodel1.Dd*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, p=linmodel1, solver=nothing) nmpc1 = NonLinMPC(nonlinmodel, Hp=15) @test isa(nmpc1.estim, UnscentedKalmanFilter) @test size(nmpc1.R̂y0, 1) == 15*nmpc1.estim.model.ny @@ -488,9 +491,9 @@ end @test nmpc5.Ñ_Hc ≈ Diagonal(diagm([repeat(Float64[3, 4], 5); [1e3]])) nmpc6 = NonLinMPC(nonlinmodel, Hp=15, Lwt=[0,1]) @test nmpc6.L_Hp ≈ Diagonal(diagm(repeat(Float64[0, 1], 15))) - nmpc7 = NonLinMPC(nonlinmodel, Hp=15, Ewt=1e-3, JE=(UE,ŶE,D̂E) -> UE.*ŶE.*D̂E) + nmpc7 = NonLinMPC(nonlinmodel, Hp=15, Ewt=1e-3, JE=(UE,ŶE,D̂E,p) -> p*UE.*ŶE.*D̂E, p=2) @test nmpc7.E == 1e-3 - @test nmpc7.JE([1,2],[3,4],[4,6]) == [12, 48] + @test nmpc7.JE([1,2],[3,4],[4,6],2) == 2*[1,2].*[3,4].*[4,6] optim = JuMP.Model(optimizer_with_attributes(Ipopt.Optimizer, "nlp_scaling_max_gradient"=>1.0)) nmpc8 = NonLinMPC(nonlinmodel, Hp=15, optim=optim) @test solver_name(nmpc8.optim) == "Ipopt" @@ -518,6 +521,7 @@ end @test_throws ArgumentError NonLinMPC(nonlinmodel, Hp=15, Ewt=[1, 1]) @test_throws ArgumentError NonLinMPC(nonlinmodel) + @test_throws ErrorException NonLinMPC(nonlinmodel, Hp=15, JE=(_,_,_)->0.0) end @testset "NonLinMPC moves and getinfo" begin @@ -534,16 +538,16 @@ end @test info[:Ŷ][end] ≈ r[1] atol=5e-2 Hp = 1000 R̂y = fill(r[1], Hp) - JE = (_ , ŶE, _ ) -> sum((ŶE[2:end] - R̂y).^2) - nmpc = NonLinMPC(linmodel, Mwt=[0], Nwt=[0], Cwt=Inf, Ewt=1, JE=JE, Hp=Hp, Hc=1) + JE = (_ , ŶE, _ , R̂y) -> sum((ŶE[2:end] - R̂y).^2) + nmpc = NonLinMPC(linmodel, Mwt=[0], Nwt=[0], Cwt=Inf, Ewt=1, JE=JE, p=R̂y, Hp=Hp, Hc=1) preparestate!(nmpc, [10]) u = moveinput!(nmpc) @test u ≈ [1] atol=5e-2 # ensure that the current estimated output is updated for correct JE values: @test nmpc.ŷ ≈ evaloutput(nmpc.estim, Float64[]) 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 + 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) nmpc2 = NonLinMPC(nonlinmodel, Nwt=[0], Hp=1000, Hc=1) d = [0.1] @@ -576,7 +580,7 @@ end nonlinmodel2 = NonLinModel{Float32}(f, h, 3000.0, 1, 2, 1, 1, solver=nothing) nmpc7 = NonLinMPC(nonlinmodel2, Hp=10) y = similar(nonlinmodel2.yop) - nonlinmodel2.h!(y, Float32[0,0], Float32[0]) + nonlinmodel2.h!(y, Float32[0,0], Float32[0], Float32[]) preparestate!(nmpc7, [0], [0]) @test moveinput!(nmpc7, [0], [0]) ≈ [0.0] end @@ -625,8 +629,8 @@ end @testset "NonLinMPC other methods" begin linmodel = setop!(LinModel(sys,Ts,i_u=[1,2]), uop=[10,50], yop=[50,30]) - f = (x,u,_) -> linmodel.A*x + linmodel.Bu*u - h = (x,_) -> linmodel.C*x + f = (x,u,_,_) -> linmodel.A*x + linmodel.Bu*u + h = (x,_,_) -> linmodel.C*x nonlinmodel = setop!( NonLinModel(f, h, Ts, 2, 2, 2, solver=nothing), uop=[10,50], yop=[50,30] ) @@ -649,8 +653,8 @@ end setconstraint!(nmpc_lin, c_ymin=[1.0,1.1], c_ymax=[1.2,1.3]) @test all((-nmpc_lin.con.A_Ymin[:, end], -nmpc_lin.con.A_Ymax[:, end]) .≈ ([1.0,1.1], [1.2,1.3])) - f = (x,u,d) -> linmodel1.A*x + linmodel1.Bu*u + linmodel1.Bd*d - h = (x,d) -> linmodel1.C*x + linmodel1.Dd*d + f = (x,u,d,_) -> linmodel1.A*x + linmodel1.Bu*u + linmodel1.Bd*d + h = (x,d,_) -> linmodel1.C*x + linmodel1.Dd*d nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing) nmpc = NonLinMPC(nonlinmodel, Hp=1, Hc=1) @@ -712,8 +716,8 @@ end info = getinfo(nmpc_lin) @test info[:x̂end][1] ≈ 0 atol=1e-1 - f = (x,u,_) -> linmodel.A*x + linmodel.Bu*u - h = (x,_) -> linmodel.C*x + f = (x,u,_,_) -> linmodel.A*x + linmodel.Bu*u + h = (x,_,_) -> linmodel.C*x nonlinmodel = NonLinModel(f, h, linmodel.Ts, 1, 1, 1, solver=nothing) nmpc = NonLinMPC(nonlinmodel, Hp=50, Hc=5) @@ -788,8 +792,8 @@ end @test mpc.M_Hp ≈ diagm(1:1000) @test mpc.Ñ_Hc ≈ diagm([0.1;1e6]) @test mpc.L_Hp ≈ diagm(1.1:1000.1) - f = (x,u,d) -> estim.model.A*x + estim.model.Bu*u + estim.model.Bd*d - h = (x,d) -> estim.model.C*x + estim.model.Du*d + f = (x,u,d,_) -> estim.model.A*x + estim.model.Bu*u + estim.model.Bd*d + h = (x,d,_) -> estim.model.C*x + estim.model.Du*d nonlinmodel = NonLinModel(f, h, 10.0, 1, 1, 1) nmpc = NonLinMPC(nonlinmodel, Nwt=[0], Cwt=1e4, Hp=1000, Hc=10) setmodel!(nmpc, Mwt=[100], Nwt=[200], Lwt=[300]) @@ -805,8 +809,8 @@ end @testset "LinMPC v.s. NonLinMPC" begin linmodel = setop!(LinModel(sys,Ts,i_d=[3]), uop=[10,50], yop=[50,30], dop=[20]) - f = (x,u,d) -> linmodel.A*x + linmodel.Bu*u + linmodel.Bd*d - h = (x,d) -> linmodel.C*x + linmodel.Dd*d + f = (x,u,d,_) -> linmodel.A*x + linmodel.Bu*u + linmodel.Bd*d + h = (x,d,_) -> linmodel.C*x + linmodel.Dd*d nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing) nonlinmodel = setop!(nonlinmodel, uop=[10,50], yop=[50,30], dop=[20]) optim = JuMP.Model(optimizer_with_attributes(Ipopt.Optimizer, "sb"=>"yes")) diff --git a/test/test_sim_model.jl b/test/test_sim_model.jl index e88142172..52b7cf1be 100644 --- a/test/test_sim_model.jl +++ b/test/test_sim_model.jl @@ -79,6 +79,17 @@ Gss2 = c2d(sys_ss[:,1:2], 0.5Ts, :zoh) linmodel12 = LinModel{Float32}(Gss.A, Gss.B, Gss.C, zeros(2, 0), zeros(2, 0), Ts) @test isa(linmodel12, LinModel{Float32}) + linmodel13 = LinModel(sys,Ts,i_d=[3]) + linmodel13 = setname!(linmodel13, + u=["u_c", "u_h"], + y=["y_L", "y_T"], + d=["u_l"], + x=["X_1", "X_2", "X_3", "X_4"] + ) + @test all(linmodel13.uname .== ["u_c", "u_h"]) + @test all(linmodel13.yname .== ["y_L", "y_T"]) + @test all(linmodel13.dname .== ["u_l"]) + @test all(linmodel13.xname .== ["X_1", "X_2", "X_3", "X_4"]) @test_throws ErrorException LinModel(sys) @test_throws ErrorException LinModel(sys,-Ts) @@ -101,7 +112,8 @@ end @test evaloutput(linmodel1, Float64[]) ≈ linmodel1(Float64[]) ≈ [50,30] x = initstate!(linmodel1, [10, 60]) @test evaloutput(linmodel1) ≈ [50 + 19.0, 30 + 7.4] - @test updatestate!(linmodel1, [10, 60]) ≈ x + @test preparestate!(linmodel1, [10, 60]) ≈ x + @test updatestate!(linmodel1, [10, 60]) ≈ x linmodel2 = LinModel(append(tf(1, [1, 0]), tf(2, [10, 1])), 1.0) x = initstate!(linmodel2, [10, 3]) @test evaloutput(linmodel2) ≈ [0, 6] @@ -132,22 +144,22 @@ end @testset "NonLinModel construction" begin linmodel1 = LinModel(sys,Ts,i_u=[1,2]) - f1(x,u,_) = linmodel1.A*x + linmodel1.Bu*u - h1(x,_) = linmodel1.C*x - nonlinmodel1 = NonLinModel(f1,h1,Ts,2,2,2,solver=nothing) + f1(x,u,_,model) = model.A*x + model.Bu*u + h1(x,_,model) = model.C*x + nonlinmodel1 = NonLinModel(f1,h1,Ts,2,2,2,solver=nothing,p=linmodel1) @test nonlinmodel1.nx == 2 @test nonlinmodel1.nu == 2 @test nonlinmodel1.nd == 0 @test nonlinmodel1.ny == 2 xnext, y = similar(nonlinmodel1.x0), similar(nonlinmodel1.yop) - nonlinmodel1.f!(xnext,[0,0],[0,0],[1]) + nonlinmodel1.f!(xnext,[0,0],[0,0],[1],nonlinmodel1.p) @test xnext ≈ zeros(2,) - nonlinmodel1.h!(y,[0,0],[1]) + nonlinmodel1.h!(y,[0,0],[1],nonlinmodel1.p) @test y ≈ zeros(2,) linmodel2 = LinModel(sys,Ts,i_d=[3]) - f2(x,u,d) = linmodel2.A*x + linmodel2.Bu*u + linmodel2.Bd*d - h2(x,d) = linmodel2.C*x + linmodel2.Dd*d + f2(x,u,d,_) = linmodel2.A*x + linmodel2.Bu*u + linmodel2.Bd*d + h2(x,d,_) = linmodel2.C*x + linmodel2.Dd*d nonlinmodel2 = NonLinModel(f2,h2,Ts,2,4,2,1,solver=nothing) @test nonlinmodel2.nx == 4 @@ -155,30 +167,30 @@ end @test nonlinmodel2.nd == 1 @test nonlinmodel2.ny == 2 xnext, y = similar(nonlinmodel2.x0), similar(nonlinmodel2.yop) - nonlinmodel2.f!(xnext,[0,0,0,0],[0,0],[0]) + nonlinmodel2.f!(xnext,[0,0,0,0],[0,0],[0],nonlinmodel2.p) @test xnext ≈ zeros(4,) - nonlinmodel2.h!(y,[0,0,0,0],[0]) + nonlinmodel2.h!(y,[0,0,0,0],[0],nonlinmodel2.p) @test y ≈ zeros(2,) nonlinmodel3 = NonLinModel{Float32}(f2,h2,Ts,2,4,2,1,solver=nothing) @test isa(nonlinmodel3, NonLinModel{Float32}) - function f1!(xnext, x, u, d) + function f1!(xnext, x, u, d,_) mul!(xnext, linmodel2.A, x) mul!(xnext, linmodel2.Bu, u, 1, 1) mul!(xnext, linmodel2.Bd, d, 1, 1) return nothing end - function h1!(y, x, d) + function h1!(y, x, d,_) mul!(y, linmodel2.C, x) mul!(y, linmodel2.Dd, d, 1, 1) return nothing end nonlinmodel4 = NonLinModel(f1!, h1!, Ts, 2, 4, 2, 1, solver=nothing) xnext, y = similar(nonlinmodel4.x0), similar(nonlinmodel4.yop) - nonlinmodel4.f!(xnext,[0,0,0,0],[0,0],[0]) + nonlinmodel4.f!(xnext,[0,0,0,0],[0,0],[0],nonlinmodel4.p) @test xnext ≈ zeros(4) - nonlinmodel4.h!(y,[0,0,0,0],[0]) + nonlinmodel4.h!(y,[0,0,0,0],[0],nonlinmodel4.p) @test y ≈ zeros(2) A = [0 0.5; -0.2 -0.1] @@ -186,46 +198,55 @@ end Bd = reshape([0; 0.5], 2, 1) C = [0.4 0] Dd = reshape([0], 1, 1) - f3(x, u, d) = A*x + Bu*u+ Bd*d - h3(x, d) = C*x + Dd*d - nonlinmodel5 = NonLinModel(f3, h3, 1.0, 1, 2, 1, 1, solver=RungeKutta()) + f3(x, u, d, _) = A*x + Bu*u+ Bd*d + h3(x, d, _) = C*x + Dd*d + solver=RungeKutta() + @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) xnext, y = similar(nonlinmodel5.x0), similar(nonlinmodel5.yop) - nonlinmodel5.f!(xnext, [0; 0], [0], [0]) + nonlinmodel5.f!(xnext, [0; 0], [0], [0], nonlinmodel5.p) @test xnext ≈ zeros(2) - nonlinmodel5.h!(y, [0; 0], [0]) + nonlinmodel5.h!(y, [0; 0], [0], nonlinmodel5.p) @test y ≈ zeros(1) - function f2!(ẋ, x, u , d) + function f2!(ẋ, x, u , d, _) mul!(ẋ, A, x) mul!(ẋ, Bu, u, 1, 1) mul!(ẋ, Bd, d, 1, 1) return nothing end - function h2!(y, x, d) + function h2!(y, x, d, _) mul!(y, C, x) mul!(y, Dd, d, 1, 1) return nothing end nonlinmodel6 = NonLinModel(f2!, h2!, 1.0, 1, 2, 1, 1, solver=RungeKutta()) xnext, y = similar(nonlinmodel6.x0), similar(nonlinmodel6.yop) - nonlinmodel6.f!(xnext, [0; 0], [0], [0]) + nonlinmodel6.f!(xnext, [0; 0], [0], [0], nonlinmodel6.p) @test xnext ≈ zeros(2) - nonlinmodel6.h!(y, [0; 0], [0]) + nonlinmodel6.h!(y, [0; 0], [0], nonlinmodel6.p) @test y ≈ zeros(1) @test_throws ErrorException NonLinModel( (x,u)->linmodel1.A*x + linmodel1.Bu*u, - (x,_)->linmodel1.C*x, Ts, 2, 4, 2, 1, solver=nothing) + (x,_,_)->linmodel1.C*x, Ts, 2, 4, 2, 1, solver=nothing) @test_throws ErrorException NonLinModel( (x,u,_)->linmodel1.A*x + linmodel1.Bu*u, + (x,_,_)->linmodel1.C*x, Ts, 2, 4, 2, 1, solver=nothing) + @test_throws ErrorException NonLinModel( + (x,u,_,_)->linmodel1.A*x + linmodel1.Bu*u, (x)->linmodel1.C*x, Ts, 2, 4, 2, 1, solver=nothing) + @test_throws ErrorException NonLinModel( + (x,u,_,_)->linmodel1.A*x + linmodel1.Bu*u, + (x,_)->linmodel1.C*x, Ts, 2, 4, 2, 1, solver=nothing) end @testset "NonLinModel sim methods" begin linmodel1 = LinModel(sys,Ts,i_u=[1,2]) - f1(x,u,_) = linmodel1.A*x + linmodel1.Bu*u - h1(x,_) = linmodel1.C*x - nonlinmodel = NonLinModel(f1,h1,Ts,2,2,2,solver=nothing) + f1(x,u,_,model) = model.A*x + model.Bu*u + h1(x,_,model) = model.C*x + nonlinmodel = NonLinModel(f1,h1,Ts,2,2,2,p=linmodel1,solver=nothing) @test updatestate!(nonlinmodel, zeros(2,)) ≈ zeros(2) @test updatestate!(nonlinmodel, zeros(2,), Float64[]) ≈ zeros(2) @@ -242,8 +263,8 @@ end @testset "NonLinModel linearization" begin Ts = 1.0 - f1(x,u,d) = x.^5 + u.^4 + d.^3 - h1(x,d) = x.^2 + d + f1(x,u,d,_) = x.^5 + u.^4 + d.^3 + h1(x,d,_) = x.^2 + d nonlinmodel1 = NonLinModel(f1,h1,Ts,1,1,1,1,solver=nothing) x, u, d = [2.0], [3.0], [4.0] linmodel1 = linearize(nonlinmodel1; x, u, d) @@ -259,17 +280,17 @@ end @test linmodel1.C ≈ linmodel2.C @test linmodel1.Dd ≈ linmodel2.Dd - f1!(ẋ, x, u, d) = (ẋ .= x.^5 + u.^4 + d.^3; nothing) - h1!(y, x, d) = (y .= x.^2 + d; nothing) + f1!(ẋ, x, u, d, _) = (ẋ .= x.^5 + u.^4 + d.^3; nothing) + h1!(y, x, d, _) = (y .= x.^2 + d; nothing) nonlinmodel3 = NonLinModel(f1!,h1!,Ts,1,1,1,1,solver=RungeKutta()) linmodel3 = linearize(nonlinmodel3; x, u, d) u0, d0 = u - nonlinmodel3.uop, d - nonlinmodel3.dop xnext, y = similar(nonlinmodel3.x0), similar(nonlinmodel3.yop) - A = ForwardDiff.jacobian((xnext, x) -> nonlinmodel3.f!(xnext, x, u0, d0), xnext, x) - Bu = ForwardDiff.jacobian((xnext, u0) -> nonlinmodel3.f!(xnext, x, u0, d0), xnext, u0) - Bd = ForwardDiff.jacobian((xnext, d0) -> nonlinmodel3.f!(xnext, x, u0, d0), xnext, d0) - C = ForwardDiff.jacobian((y, x) -> nonlinmodel3.h!(y, x, d0), y, x) - Dd = ForwardDiff.jacobian((y, d0) -> nonlinmodel3.h!(y, x, d0), y, d0) + A = ForwardDiff.jacobian((xnext, x) -> nonlinmodel3.f!(xnext, x, u0, d0, nonlinmodel3.p), xnext, x) + Bu = ForwardDiff.jacobian((xnext, u0) -> nonlinmodel3.f!(xnext, x, u0, d0, nonlinmodel3.p), xnext, u0) + Bd = ForwardDiff.jacobian((xnext, d0) -> nonlinmodel3.f!(xnext, x, u0, d0, nonlinmodel3.p), xnext, d0) + C = ForwardDiff.jacobian((y, x) -> nonlinmodel3.h!(y, x, d0, nonlinmodel3.p), y, x) + Dd = ForwardDiff.jacobian((y, d0) -> nonlinmodel3.h!(y, x, d0, nonlinmodel3.p), y, d0) @test linmodel3.A ≈ A @test linmodel3.Bu ≈ Bu @test linmodel3.Bd ≈ Bd @@ -298,8 +319,8 @@ end @testset "NonLinModel real time simulations" begin linmodel1 = LinModel(tf(2, [10, 1]), 0.1) nonlinmodel1 = NonLinModel( - (x,u,_)->linmodel1.A*x + linmodel1.Bu*u, - (x,_)->linmodel1.C*x, + (x,u,_,_)->linmodel1.A*x + linmodel1.Bu*u, + (x,_,_)->linmodel1.C*x, linmodel1.Ts, 1, 1, 1, 0, solver=nothing ) times1 = zeros(5) @@ -311,8 +332,8 @@ end @test all(isapprox.(diff(times1[2:end]), 0.1, atol=0.01)) linmodel2 = LinModel(tf(2, [0.1, 1]), 0.001) nonlinmodel2 = NonLinModel( - (x,u,_)->linmodel2.A*x + linmodel2.Bu*u, - (x,_)->linmodel2.C*x, + (x,u,_,_)->linmodel2.A*x + linmodel2.Bu*u, + (x,_,_)->linmodel2.C*x, linmodel2.Ts, 1, 1, 1, 0, solver=nothing ) times2 = zeros(5) diff --git a/test/test_state_estim.jl b/test/test_state_estim.jl index 906ef3d77..cdf4fce01 100644 --- a/test/test_state_estim.jl +++ b/test/test_state_estim.jl @@ -367,8 +367,8 @@ end @test internalmodel2.nxs == 1 @test internalmodel2.nx̂ == 4 - f(x,u,d) = linmodel2.A*x + linmodel2.Bu*u + linmodel2.Bd*d - h(x,d) = linmodel2.C*x + linmodel2.Dd*d + 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, Ts, 2, 4, 2, 2, solver=nothing) internalmodel3 = InternalModel(nonlinmodel) @test internalmodel3.nym == 2 @@ -485,8 +485,8 @@ end @testset "UnscentedKalmanFilter construction" begin 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 + 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) ukf1 = UnscentedKalmanFilter(linmodel1) @@ -542,8 +542,8 @@ end @testset "UnscentedKalmanFilter estimator methods" begin linmodel1 = LinModel(sys,Ts,i_u=[1,2]) - f(x,u,_) = linmodel1.A*x + linmodel1.Bu*u - h(x,_) = linmodel1.C*x + f(x,u,_,_) = linmodel1.A*x + linmodel1.Bu*u + h(x,_,_) = linmodel1.C*x nonlinmodel = setop!(NonLinModel(f, h, Ts, 2, 2, 2, solver=nothing), uop=[10,50], yop=[50,30]) ukf1 = UnscentedKalmanFilter(nonlinmodel) preparestate!(ukf1, [50, 30]) @@ -614,8 +614,8 @@ end setmodel!(ukf1, Q̂=[1e-3], R̂=[1e-6]) @test ukf1.Q̂ ≈ [1e-3] @test ukf1.R̂ ≈ [1e-6] - f(x,u,d) = linmodel.A*x + linmodel.Bu*u + linmodel.Bd*d - h(x,d) = linmodel.C*x + linmodel.Du*d + f(x,u,d,_) = linmodel.A*x + linmodel.Bu*u + linmodel.Bd*d + h(x,d,_) = linmodel.C*x + linmodel.Du*d nonlinmodel = NonLinModel(f, h, 10.0, 1, 1, 1) ukf2 = UnscentedKalmanFilter(nonlinmodel, nint_ym=0) setmodel!(ukf2, Q̂=[1e-3], R̂=[1e-6]) @@ -626,8 +626,8 @@ end @testset "ExtendedKalmanFilter construction" begin 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 + 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) ekf1 = ExtendedKalmanFilter(linmodel1) @@ -679,8 +679,8 @@ end @testset "ExtendedKalmanFilter estimator methods" begin linmodel1 = LinModel(sys,Ts,i_u=[1,2]) - f(x,u,_) = linmodel1.A*x + linmodel1.Bu*u - h(x,_) = linmodel1.C*x + f(x,u,_,_) = linmodel1.A*x + linmodel1.Bu*u + h(x,_,_) = linmodel1.C*x nonlinmodel = setop!(NonLinModel(f, h, Ts, 2, 2, 2, solver=nothing), uop=[10,50], yop=[50,30]) ekf1 = ExtendedKalmanFilter(nonlinmodel) preparestate!(ekf1, [50, 30]) @@ -751,8 +751,8 @@ end setmodel!(ekf1, Q̂=[1e-3], R̂=[1e-6]) @test ekf1.Q̂ ≈ [1e-3] @test ekf1.R̂ ≈ [1e-6] - f(x,u,d) = linmodel.A*x + linmodel.Bu*u + linmodel.Bd*d - h(x,d) = linmodel.C*x + linmodel.Du*d + f(x,u,d,_) = linmodel.A*x + linmodel.Bu*u + linmodel.Bd*d + h(x,d,_) = linmodel.C*x + linmodel.Du*d nonlinmodel = NonLinModel(f, h, 10.0, 1, 1, 1) ekf2 = ExtendedKalmanFilter(nonlinmodel, nint_ym=0) setmodel!(ekf2, Q̂=[1e-3], R̂=[1e-6]) @@ -763,8 +763,8 @@ end @testset "MovingHorizonEstimator construction" begin 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 + 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) mhe1 = MovingHorizonEstimator(linmodel1, He=5) @@ -844,8 +844,8 @@ end @testset "MovingHorizonEstimator estimation and getinfo" begin 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 + 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]) mhe1 = MovingHorizonEstimator(nonlinmodel, He=2) @@ -1005,8 +1005,8 @@ end setconstraint!(mhe2, C_v̂min=0.05(31:38), C_v̂max=0.06(31:38)) @test all((-mhe2.con.A_V̂min[:, end], -mhe2.con.A_V̂max[:, end]) .≈ (0.05(31:38), 0.06(31:38))) - f(x,u,d) = linmodel1.A*x + linmodel1.Bu*u - h(x,d) = linmodel1.C*x + f(x,u,d,_) = linmodel1.A*x + linmodel1.Bu*u + h(x,d,_) = linmodel1.C*x nonlinmodel = setop!(NonLinModel(f, h, Ts, 2, 2, 2, solver=nothing), uop=[10,50], yop=[50,30]) mhe3 = MovingHorizonEstimator(nonlinmodel, He=4, nint_ym=0, Cwt=1e3) @@ -1100,8 +1100,8 @@ end info = getinfo(mhe) @test info[:V̂] ≈ [-1,-1] atol=5e-2 - f(x,u,_) = linmodel1.A*x + linmodel1.Bu*u - h(x,_) = linmodel1.C*x + f(x,u,_,_) = linmodel1.A*x + linmodel1.Bu*u + h(x,_,_) = linmodel1.C*x nonlinmodel = setop!(NonLinModel(f, h, Ts, 2, 2, 2, solver=nothing), uop=[10,50], yop=[50,30]) mhe2 = MovingHorizonEstimator(nonlinmodel, He=1, nint_ym=0) @@ -1184,8 +1184,8 @@ end setmodel!(mhe, Q̂=[1e-3], R̂=[1e-6]) @test mhe.Q̂ ≈ [1e-3] @test mhe.R̂ ≈ [1e-6] - f(x,u,d) = linmodel.A*x + linmodel.Bu*u + linmodel.Bd*d - h(x,d) = linmodel.C*x + linmodel.Du*d + f(x,u,d,_) = linmodel.A*x + linmodel.Bu*u + linmodel.Bd*d + h(x,d,_) = linmodel.C*x + linmodel.Du*d nonlinmodel = NonLinModel(f, h, 10.0, 1, 1, 1) mhe2 = MovingHorizonEstimator(nonlinmodel, He=1, nint_ym=0) setmodel!(mhe2, Q̂=[1e-3], R̂=[1e-6]) @@ -1224,8 +1224,8 @@ end updatestate!(kf, [11, 50], y, [25]) end @test X̂_mhe ≈ X̂_kf atol=1e-3 rtol=1e-3 - f = (x,u,d) -> linmodel1.A*x + linmodel1.Bu*u + linmodel1.Bd*d - h = (x,d) -> linmodel1.C*x + linmodel1.Dd*d + f = (x,u,d,_) -> linmodel1.A*x + linmodel1.Bu*u + linmodel1.Bd*d + h = (x,d,_) -> linmodel1.C*x + linmodel1.Dd*d nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing) nonlinmodel = setop!(nonlinmodel, uop=[10,50], yop=[50,30], dop=[20]) mhe = MovingHorizonEstimator(nonlinmodel, He=5, nint_ym=0, direct=false) @@ -1272,8 +1272,8 @@ end @testset "MovingHorizonEstimator LinModel v.s. NonLinModel" begin linmodel = setop!(LinModel(sys,Ts,i_d=[3]), uop=[10,50], yop=[50,30], dop=[20]) - f = (x,u,d) -> linmodel.A*x + linmodel.Bu*u + linmodel.Bd*d - h = (x,d) -> linmodel.C*x + linmodel.Dd*d + f = (x,u,d,_) -> linmodel.A*x + linmodel.Bu*u + linmodel.Bd*d + h = (x,d,_) -> linmodel.C*x + linmodel.Dd*d nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing) nonlinmodel = setop!(nonlinmodel, uop=[10,50], yop=[50,30], dop=[20]) optim = JuMP.Model(optimizer_with_attributes(Ipopt.Optimizer, "sb" => "yes"))