diff --git a/src/controller/explicitmpc.jl b/src/controller/explicitmpc.jl index 49cc6b950..bf07765d1 100644 --- a/src/controller/explicitmpc.jl +++ b/src/controller/explicitmpc.jl @@ -181,9 +181,9 @@ function Base.show(io::IO, mpc::ExplicitMPC) nu, nd = mpc.estim.model.nu, mpc.estim.model.nd nx̂, nym, nyu = mpc.estim.nx̂, mpc.estim.nym, mpc.estim.nyu n = maximum(ndigits.((Hp, Hc, nu, nx̂, nym, nyu, nd))) + 1 - println(io, "$(typeof(mpc).name.name) controller with a sample time Ts = "* + println(io, "$(nameof(typeof(mpc))) controller with a sample time Ts = "* "$(mpc.estim.model.Ts) s, "* - "$(typeof(mpc.estim).name.name) estimator and:") + "$(nameof(typeof(mpc.estim))) estimator and:") println(io, "$(lpad(Hp, n)) prediction steps Hp") println(io, "$(lpad(Hc, n)) control steps Hc") print_estim_dim(io, mpc.estim, n) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 8c512140b..09e6e4de2 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1194,9 +1194,9 @@ Compute vectors if `model` is a [`NonLinModel`](@ref) and other [`TranscriptionM The method mutates `Ŷ0` and `x̂0end` arguments. The augmented output function [`ĥ!`](@ref) is called multiple times in a `for` loop: ```math -\mathbf{ŷ_0}(k) = \mathbf{ĥ}\Big(\mathbf{x̂_0^†}(k+j), \mathbf{d_0}(k) \Big) +\mathbf{ŷ_0}(k) = \mathbf{ĥ}\Big(\mathbf{x̂_0}(k), \mathbf{d_0}(k) \Big) ``` -in which ``\mathbf{x̂_0^†}`` is the augmented state extracted from the decision variable `Z̃`. +in which ``\mathbf{x̂_0}`` is the augmented state extracted from the decision variable `Z̃`. """ function predict!( Ŷ0, x̂0end, _, _, _, @@ -1316,10 +1316,10 @@ The method mutates the `geq`, `X̂0`, `Û0` and `K0` vectors in argument. The n equality constraints `geq` only includes the augmented state defects, computed with: ```math \mathbf{ŝ}(k+1) = \mathbf{f̂}\Big(\mathbf{x̂_0}(k), \mathbf{u_0}(k), \mathbf{d_0}(k)\Big) - - \mathbf{x̂_0^†}(k+1) + - \mathbf{x̂_0}(k+1) ``` -in which ``\mathbf{x̂_0^†}`` is the augmented state extracted from the decision variables -`Z̃`, and ``\mathbf{f̂}``, the augmented state function defined in [`f̂!`](@ref). +in which the augmented state ``\mathbf{x̂_0}`` are extracted from the decision variables +`Z̃`, and ``\mathbf{f̂}`` is the augmented state function defined in [`f̂!`](@ref). """ function con_nonlinprogeq!( geq, X̂0, Û0, K0, @@ -1366,18 +1366,18 @@ time state-space models, and the stochastic model of the unmeasured disturbances is discrete-time. The deterministic and stochastic defects are respectively computed with: ```math \begin{aligned} -\mathbf{s_d}(k+1) &= \mathbf{x_0}(k) - \mathbf{x_0^†}(k+1) +\mathbf{s_d}(k+1) &= \mathbf{x_0}(k) - \mathbf{x_0}(k+1) + 0.5 T_s (\mathbf{k}_1 + \mathbf{k}_2) \\ -\mathbf{s_s}(k+1) &= \mathbf{A_s x_s}(k) - \mathbf{x_s^†}(k+1) +\mathbf{s_s}(k+1) &= \mathbf{A_s x_s}(k) - \mathbf{x_s}(k+1) \end{aligned} ``` -in which ``\mathbf{x_0^†}`` and ``\mathbf{x_s^†}`` are the deterministic and stochastic -states extracted from the decision variables `Z̃`. The ``\mathbf{k}`` coefficients are +in which ``\mathbf{x_0}`` and ``\mathbf{x_s}`` are the deterministic and stochastic states +extracted from the decision variables `Z̃`. The ``\mathbf{k}`` coefficients are evaluated from the continuous-time function `model.f!` and: ```math \begin{aligned} -\mathbf{k}_1 &= \mathbf{f}\Big(\mathbf{x_0}(k), \mathbf{û_0}(k), \mathbf{d_0}(k) \Big) \\ -\mathbf{k}_2 &= \mathbf{f}\Big(\mathbf{x_0^†}(k+1), \mathbf{û_0}(k), \mathbf{d_0}(k+1)\Big) +\mathbf{k}_1 &= \mathbf{f}\Big(\mathbf{x_0}(k), \mathbf{û_0}(k), \mathbf{d_0}(k) \Big) \\ +\mathbf{k}_2 &= \mathbf{f}\Big(\mathbf{x_0}(k+1), \mathbf{û_0}(k), \mathbf{d_0}(k+1)\Big) \end{aligned} ``` and the input of the augmented model is: diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index 91ca91013..cb8e18af3 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -877,7 +877,8 @@ struct ExtendedKalmanFilter{ SM<:SimModel, KC<:KalmanCovariances, JB<:AbstractADType, - LF<:Function + FF<:Function, + HF<:Function } <: StateEstimator{NT} model::SM cov ::KC @@ -907,13 +908,23 @@ struct ExtendedKalmanFilter{ Ĥ ::Matrix{NT} Ĥm ::Matrix{NT} jacobian::JB - linfunc!::LF + linfuncF̂!::FF + linfuncĤ!::HF direct::Bool corrected::Vector{Bool} buffer::StateEstimatorBuffer{NT} function ExtendedKalmanFilter{NT}( - model::SM, i_ym, nint_u, nint_ym, cov::KC; jacobian::JB, linfunc!::LF, direct=true - ) where {NT<:Real, SM<:SimModel, KC<:KalmanCovariances, JB<:AbstractADType, LF<:Function} + model::SM, + i_ym, nint_u, nint_ym, cov::KC; + jacobian::JB, linfuncF̂!::FF, linfuncĤ!::HF, direct=true + ) where { + NT<:Real, + SM<:SimModel, + KC<:KalmanCovariances, + JB<:AbstractADType, + FF<:Function, + HF<:Function + } nu, ny, nd, nk = model.nu, model.ny, model.nd, model.nk nym, nyu = validate_ym(model, i_ym) As, Cs_u, Cs_y, nint_u, nint_ym = init_estimstoch(model, i_ym, nint_u, nint_ym) @@ -927,7 +938,7 @@ struct ExtendedKalmanFilter{ Ĥ, Ĥm = zeros(NT, ny, nx̂), zeros(NT, nym, nx̂) corrected = [false] buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk) - return new{NT, SM, KC, JB, LF}( + return new{NT, SM, KC, JB, FF, HF}( model, cov, x̂op, f̂op, x̂0, @@ -936,7 +947,7 @@ struct ExtendedKalmanFilter{ Â, B̂u, Ĉ, B̂d, D̂d, Ĉm, D̂dm, K̂, F̂_û, F̂, Ĥ, Ĥm, - jacobian, linfunc!, + jacobian, linfuncF̂!, linfuncĤ!, direct, corrected, buffer ) @@ -1044,60 +1055,54 @@ function ExtendedKalmanFilter( ) where {NT<:Real, SM<:SimModel{NT}} P̂_0, Q̂, R̂ = to_mat(P̂_0), to_mat(Q̂), to_mat(R̂) cov = KalmanCovariances(model, i_ym, nint_u, nint_ym, Q̂, R̂, P̂_0) - linfunc! = get_ekf_linfunc(NT, model, i_ym, nint_u, nint_ym, jacobian) + linfuncF̂!, linfuncĤ! = get_ekf_linfuncs(NT, model, i_ym, nint_u, nint_ym, jacobian) return ExtendedKalmanFilter{NT}( - model, i_ym, nint_u, nint_ym, cov; jacobian, direct, linfunc! + model, i_ym, nint_u, nint_ym, cov; jacobian, linfuncF̂!, linfuncĤ!, direct ) end """ - get_ekf_linfunc(NT, model, i_ym, nint_u, nint_ym, jacobian) -> linfunc! + get_ekf_linfuncs(NT, model, i_ym, nint_u, nint_ym, jacobian) -> linfuncF̂!, linfuncĤ! -Return the `linfunc!` function that computes the Jacobians of the augmented model. +Return the functions that computes the `F̂` and `Ĥ` Jacobians of the augmented model. -The function has the two following methods: +The functions has the following signatures: ``` -linfunc!(x̂0next , ::Nothing, F̂ , ::Nothing, backend, x̂0, cst_u0, cst_d0) -> nothing -linfunc!(::Nothing, ŷ0 , ::Nothing, Ĥ , backend, x̂0, _ , cst_d0) -> nothing +linfuncF̂!(F̂, x̂0next , backend, x̂0, cst_u0, cst_d0) -> nothing +linfuncĤ!(Ĥ, ŷ0 , backend, x̂0, cst_d0) -> nothing ``` -To respectively compute only `F̂` or `Ĥ` Jacobian. The methods mutate all the arguments -before `backend` argument. The `backend` argument is an `AbstractADType` object from +They mutates all the arguments before `backend`, which is an `AbstractADType` object from `DifferentiationInterface`. The `cst_u0` and `cst_d0` are `DifferentiationInterface.Constant` objects with the linearization points. """ -function get_ekf_linfunc(NT, model, i_ym, nint_u, nint_ym, jacobian) +function get_ekf_linfuncs(NT, model, i_ym, nint_u, nint_ym, jacobian) As, Cs_u, Cs_y = init_estimstoch(model, i_ym, nint_u, nint_ym) - nxs = size(As, 1) - x̂op, f̂op = [model.xop; zeros(nxs)], [model.fop; zeros(nxs)] - f̂_ekf!(x̂0next, x̂0, û0, k0, u0, d0) = f̂!( - x̂0next, û0, k0, model, As, Cs_u, f̂op, x̂op, x̂0, u0, d0 - ) - ĥ_ekf!(ŷ0, x̂0, d0) = ĥ!( - ŷ0, model, Cs_y, x̂0, d0 - ) - strict = Val(true) nu, ny, nd, nk = model.nu, model.ny, model.nd, model.nk - nx̂ = model.nx + nxs + nx̂ = model.nx + size(As, 1) + x̂op = f̂op = zeros(nx̂) # not important for Jacobian computations + function f̂_ekf!(x̂0next, x̂0, û0, k0, u0, d0) + return f̂!(x̂0next, û0, k0, model, As, Cs_u, f̂op, x̂op, x̂0, u0, d0) + end + ĥ_ekf!(ŷ0, x̂0, d0) = ĥ!(ŷ0, model, Cs_y, x̂0, d0) + strict = Val(true) x̂0next = zeros(NT, nx̂) ŷ0 = zeros(NT, ny) x̂0 = zeros(NT, nx̂) - tmp_û0 = Cache(zeros(NT, nu)) - tmp_x0i = Cache(zeros(NT, nk)) + û0 = Cache(zeros(NT, nu)) + k0 = Cache(zeros(NT, nk)) cst_u0 = Constant(zeros(NT, nu)) cst_d0 = Constant(zeros(NT, nd)) - F̂_prep = prepare_jacobian( - f̂_ekf!, x̂0next, jacobian, x̂0, tmp_û0, tmp_x0i, cst_u0, cst_d0; strict + F̂prep = prepare_jacobian( + f̂_ekf!, x̂0next, jacobian, x̂0, û0, k0, cst_u0, cst_d0; strict ) - Ĥ_prep = prepare_jacobian(ĥ_ekf!, ŷ0, jacobian, x̂0, cst_d0; strict) - function linfunc!(x̂0next, ŷ0::Nothing, F̂, Ĥ::Nothing, backend, x̂0, cst_u0, cst_d0) - return jacobian!( - f̂_ekf!, x̂0next, F̂, F̂_prep, backend, x̂0, tmp_û0, tmp_x0i, cst_u0, cst_d0 - ) + Ĥprep = prepare_jacobian(ĥ_ekf!, ŷ0, jacobian, x̂0, cst_d0; strict) + function linfuncF̂!(F̂, x̂0next, backend, x̂0, cst_u0, cst_d0) + return jacobian!(f̂_ekf!, x̂0next, F̂, F̂prep, backend, x̂0, û0, k0, cst_u0, cst_d0) end - function linfunc!(x̂0next::Nothing, ŷ0, F̂::Nothing, Ĥ, backend, x̂0, _ , cst_d0) - return jacobian!(ĥ_ekf!, ŷ0, Ĥ, Ĥ_prep, backend, x̂0, cst_d0) + function linfuncĤ!(Ĥ, ŷ0, backend, x̂0, cst_d0) + return jacobian!(ĥ_ekf!, ŷ0, Ĥ, Ĥprep, backend, x̂0, cst_d0) end - return linfunc! + return linfuncF̂!, linfuncĤ! end """ @@ -1106,12 +1111,12 @@ end Do the same but for the [`ExtendedKalmanFilter`](@ref). """ function correct_estimate!(estim::ExtendedKalmanFilter, y0m, d0) - model, x̂0 = estim.model, estim.x̂0 + x̂0 = estim.x̂0 cst_d0 = Constant(d0) - ŷ0, Ĥ = estim.buffer.ŷ, estim.Ĥ - estim.linfunc!(nothing, ŷ0, nothing, Ĥ, estim.jacobian, x̂0, nothing, cst_d0) - estim.Ĥm .= @views estim.Ĥ[estim.i_ym, :] - return correct_estimate_kf!(estim, y0m, d0, estim.Ĥm) + ŷ0, Ĥ, Ĥm = estim.buffer.ŷ, estim.Ĥ, estim.Ĥm + estim.linfuncĤ!(Ĥ, ŷ0, estim.jacobian, x̂0, cst_d0) + Ĥm .= @views Ĥ[estim.i_ym, :] + return correct_estimate_kf!(estim, y0m, d0, Ĥm) end @@ -1154,19 +1159,14 @@ and prediction step equations are provided below. The correction step is skipped ``` """ function update_estimate!(estim::ExtendedKalmanFilter{NT}, y0m, d0, u0) where NT<:Real - model, x̂0 = estim.model, estim.x̂0 - nx̂, nu = estim.nx̂, model.nu - cst_u0, cst_d0 = Constant(u0), Constant(d0) if !estim.direct - ŷ0, Ĥ = estim.buffer.ŷ, estim.Ĥ - estim.linfunc!(nothing, ŷ0, nothing, Ĥ, estim.jacobian, x̂0, nothing, cst_d0) - estim.Ĥm .= @views estim.Ĥ[estim.i_ym, :] - correct_estimate_kf!(estim, y0m, d0, estim.Ĥm) + correct_estimate!(estim, y0m, d0) end + cst_u0, cst_d0 = Constant(u0), Constant(d0) x̂0corr = estim.x̂0 x̂0next, F̂ = estim.buffer.x̂, estim.F̂ - estim.linfunc!(x̂0next, nothing, F̂, nothing, estim.jacobian, x̂0corr, cst_u0, cst_d0) - return predict_estimate_kf!(estim, u0, d0, estim.F̂) + estim.linfuncF̂!(F̂, x̂0next, estim.jacobian, x̂0corr, cst_u0, cst_d0) + return predict_estimate_kf!(estim, u0, d0, F̂) end "Set `estim.cov.P̂` to `estim.cov.P̂_0` for the time-varying Kalman Filters." diff --git a/src/estimator/mhe.jl b/src/estimator/mhe.jl index f5cc239cc..6123ce81a 100644 --- a/src/estimator/mhe.jl +++ b/src/estimator/mhe.jl @@ -5,9 +5,9 @@ function Base.show(io::IO, estim::MovingHorizonEstimator) nu, nd = estim.model.nu, estim.model.nd nx̂, nym, nyu = estim.nx̂, estim.nym, estim.nyu n = maximum(ndigits.((nu, nx̂, nym, nyu, nd))) + 1 - println(io, "$(typeof(estim).name.name) estimator with a sample time "* + println(io, "$(nameof(typeof(estim))) estimator with a sample time "* "Ts = $(estim.model.Ts) s, $(JuMP.solver_name(estim.optim)) optimizer, "* - "$(typeof(estim.model).name.name) and:") + "$(nameof(typeof(estim.model))) and:") print_estim_dim(io, estim, n) end diff --git a/src/plot_sim.jl b/src/plot_sim.jl index 7261e636d..31ab098a7 100644 --- a/src/plot_sim.jl +++ b/src/plot_sim.jl @@ -101,7 +101,7 @@ get_nx̂(mpc::PredictiveController) = mpc.estim.nx̂ function Base.show(io::IO, res::SimResult) N = length(res.T_data) - print(io, "Simulation results of $(typeof(res.obj).name.name) with $N time steps.") + print(io, "Simulation results of $(nameof(typeof(res.obj))) with $N time steps.") end @@ -137,7 +137,7 @@ function sim!( D_data = Matrix{NT}(undef, plant.nd, N) X_data = Matrix{NT}(undef, plant.nx, N) setstate!(plant, x_0) - @progress name="$(typeof(plant).name.name) simulation" for i=1:N + @progress name="$(nameof(typeof(plant))) simulation" for i=1:N y = evaloutput(plant, d) Y_data[:, i] .= y U_data[:, i] .= u @@ -282,7 +282,7 @@ function sim_closedloop!( lastd, lasty = d, evaloutput(plant, d) initstate!(est_mpc, lastu, lasty[estim.i_ym], lastd) isnothing(x̂_0) || setstate!(est_mpc, x̂_0) - @progress name="$(typeof(est_mpc).name.name) simulation" for i=1:N + @progress name="$(nameof(typeof(est_mpc))) simulation" for i=1:N d = lastd + d_step + d_noise.*randn(plant.nd) y = evaloutput(plant, d) + y_step + y_noise.*randn(plant.ny) ym = y[estim.i_ym] diff --git a/src/predictive_control.jl b/src/predictive_control.jl index 6c9c35f7b..2563d0261 100644 --- a/src/predictive_control.jl +++ b/src/predictive_control.jl @@ -31,10 +31,10 @@ function Base.show(io::IO, mpc::PredictiveController) nu, nd = mpc.estim.model.nu, mpc.estim.model.nd nx̂, nym, nyu = mpc.estim.nx̂, mpc.estim.nym, mpc.estim.nyu n = maximum(ndigits.((Hp, Hc, nu, nx̂, nym, nyu, nd))) + 1 - println(io, "$(typeof(mpc).name.name) controller with a sample time Ts = "* + println(io, "$(nameof(typeof(mpc))) controller with a sample time Ts = "* "$(mpc.estim.model.Ts) s, $(JuMP.solver_name(mpc.optim)) optimizer, "* - "$(typeof(mpc.transcription).name.name) transcription, "* - "$(typeof(mpc.estim).name.name) estimator and:") + "$(nameof(typeof(mpc.transcription))) transcription, "* + "$(nameof(typeof(mpc.estim))) estimator and:") println(io, "$(lpad(Hp, n)) prediction steps Hp") println(io, "$(lpad(Hc, n)) control steps Hc") println(io, "$(lpad(nϵ, n)) slack variable ϵ (control constraints)") diff --git a/src/sim_model.jl b/src/sim_model.jl index 696068558..ebb006f2a 100644 --- a/src/sim_model.jl +++ b/src/sim_model.jl @@ -372,7 +372,7 @@ function Base.show(io::IO, model::SimModel) nu, nd = model.nu, model.nd nx, ny = model.nx, model.ny n = maximum(ndigits.((nu, nx, ny, nd))) + 1 - println(io, "$(typeof(model).name.name) with a sample time Ts = $(model.Ts) s"* + println(io, "$(nameof(typeof(model))) with a sample time Ts = $(model.Ts) s"* "$(detailstr(model)) and:") println(io, "$(lpad(nu, n)) manipulated inputs u") println(io, "$(lpad(nx, n)) states x") diff --git a/src/state_estim.jl b/src/state_estim.jl index b681ef2d1..691393d42 100644 --- a/src/state_estim.jl +++ b/src/state_estim.jl @@ -32,8 +32,8 @@ function Base.show(io::IO, estim::StateEstimator) nu, nd = estim.model.nu, estim.model.nd nx̂, nym, nyu = estim.nx̂, estim.nym, estim.nyu n = maximum(ndigits.((nu, nx̂, nym, nyu, nd))) + 1 - println(io, "$(typeof(estim).name.name) estimator with a sample time "* - "Ts = $(estim.model.Ts) s, $(typeof(estim.model).name.name) and:") + println(io, "$(nameof(typeof(estim))) estimator with a sample time "* + "Ts = $(estim.model.Ts) s, $(nameof(typeof(estim.model))) and:") print_estim_dim(io, estim, n) end