diff --git a/Project.toml b/Project.toml index 4caf686c7..e6914e024 100644 --- a/Project.toml +++ b/Project.toml @@ -1,7 +1,7 @@ name = "ModelPredictiveControl" uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c" authors = ["Francis Gagnon"] -version = "1.9.0" +version = "1.9.1" [deps] ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e" diff --git a/README.md b/README.md index 680d47754..6797cc193 100644 --- a/README.md +++ b/README.md @@ -115,6 +115,7 @@ for more detailed examples. - supported transcription methods of the optimization problem: - direct single shooting - direct multiple shooting + - trapezoidal collocation - additional information about the optimum to ease troubleshooting - real-time control loop features: - implementations that carefully limits the allocations diff --git a/docs/src/internals/predictive_control.md b/docs/src/internals/predictive_control.md index 18e960663..2a6480a86 100644 --- a/docs/src/internals/predictive_control.md +++ b/docs/src/internals/predictive_control.md @@ -41,5 +41,8 @@ ModelPredictiveControl.linconstrainteq! ```@docs ModelPredictiveControl.optim_objective!(::PredictiveController) ModelPredictiveControl.set_warmstart! +ModelPredictiveControl.predict! +ModelPredictiveControl.con_nonlinprog! +ModelPredictiveControl.con_nonlinprogeq! ModelPredictiveControl.getinput! ``` diff --git a/docs/src/internals/state_estim.md b/docs/src/internals/state_estim.md index 8b04a57bd..fe0ebd6ad 100644 --- a/docs/src/internals/state_estim.md +++ b/docs/src/internals/state_estim.md @@ -40,6 +40,8 @@ ModelPredictiveControl.linconstraint!(::MovingHorizonEstimator, ::LinModel) ```@docs ModelPredictiveControl.optim_objective!(::MovingHorizonEstimator) ModelPredictiveControl.set_warmstart_mhe! +ModelPredictiveControl.predict_mhe! +ModelPredictiveControl.con_nonlinprog_mhe! ``` ## Remove Operating Points diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 4c3fbadf9..8c512140b 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -75,8 +75,7 @@ order hold) between the samples, but linear interpolation will be added soon. This transcription computes the predictions by calling the continuous-time model in the equality constraint function and by using the implicit trapezoidal rule. It can handle -moderately stiff systems and is A-stable. However, it may not be as efficient as more -advanced collocation methods for highly stiff systems. Note that the built-in [`StateEstimator`](@ref) +moderately stiff systems and is A-stable. Note that the built-in [`StateEstimator`](@ref) will still use the `solver` provided at the construction of the [`NonLinModel`](@ref) to estimate the plant states, not the trapezoidal rule (see `supersample` option of [`RungeKutta`](@ref) for stiff systems). See Extended Help for more details. @@ -89,7 +88,8 @@ transcription method. Note that the stochastic model of the unmeasured disturbances is strictly discrete-time, as described in [`ModelPredictiveControl.init_estimstoch`](@ref). Collocation methods require continuous-time dynamics. Because of this, the stochastic states are transcribed - separately using a [`MultipleShooting`](@ref) method. + separately using a [`MultipleShooting`](@ref) method. See [`con_nonlinprogeq!`](@ref) + for more details. """ struct TrapezoidalCollocation <: CollocationMethod nc::Int @@ -1119,8 +1119,14 @@ getU0!(U0, mpc::PredictiveController, Z̃) = (mul!(U0, mpc.P̃u, Z̃) .+ mpc.Tu_ Compute the predictions `Ŷ0`, terminal states `x̂0end` if model is a [`LinModel`](@ref). The method mutates `Ŷ0` and `x̂0end` vector arguments. The `x̂end` vector is used for -the terminal constraints applied on ``\mathbf{x̂}_{k-1}(k+H_p)``. The computations are -identical for any [`TranscriptionMethod`](@ref) if the model is linear. +the terminal constraints applied on ``\mathbf{x̂_0}(k+H_p)``. The computations are +identical for any [`TranscriptionMethod`](@ref) if the model is linear: +```math +\begin{aligned} +\mathbf{Ŷ_0} &= \mathbf{Ẽ Z̃} + \mathbf{F} \\ +\mathbf{x̂_0}(k+H_p) &= \mathbf{ẽ_x̂ Z̃} + \mathbf{f_x̂} +\end{aligned} +``` """ function predict!( Ŷ0, x̂0end, _, _, _, @@ -1142,7 +1148,14 @@ end Compute vectors if `model` is a [`NonLinModel`](@ref) and for [`SingleShooting`](@ref). -The method mutates `Ŷ0`, `x̂0end`, `X̂0`, `Û0` and `K0` arguments. +The method mutates `Ŷ0`, `x̂0end`, `X̂0`, `Û0` and `K0` arguments. The augmented model of +[`f̂!`](@ref) and [`ĥ!`](@ref) functions is called recursively in a `for` loop: +```math +\begin{aligned} +\mathbf{x̂_0}(k+1) &= \mathbf{f̂}\Big(\mathbf{x̂_0}(k), \mathbf{u_0}(k), \mathbf{d_0}(k) \Big) \\ +\mathbf{ŷ_0}(k) &= \mathbf{ĥ}\Big(\mathbf{x̂_0}(k), \mathbf{d_0}(k) \Big) +\end{aligned} +``` """ function predict!( Ŷ0, x̂0end, X̂0, Û0, K0, @@ -1173,17 +1186,22 @@ end predict!( Ŷ0, x̂0end, _ , _ , _ , mpc::PredictiveController, model::NonLinModel, transcription::TranscriptionMethod, - U0, Z̃ + _ , Z̃ ) -> Ŷ0, x̂0end Compute vectors if `model` is a [`NonLinModel`](@ref) and other [`TranscriptionMethod`](@ref). -The method mutates `Ŷ0` and `x̂0end` arguments. +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) +``` +in which ``\mathbf{x̂_0^†}`` is the augmented state extracted from the decision variable `Z̃`. """ function predict!( Ŷ0, x̂0end, _, _, _, mpc::PredictiveController, model::NonLinModel, ::TranscriptionMethod, - U0, Z̃ + _ , Z̃ ) nu, ny, nd, nx̂, Hp, Hc = model.nu, model.ny, model.nd, mpc.estim.nx̂, mpc.Hp, mpc.Hc X̂0 = @views Z̃[(nu*Hc+1):(nu*Hc+nx̂*Hp)] # Z̃ = [ΔU; X̂0; ϵ] @@ -1208,7 +1226,7 @@ end Nonlinear constrains when `model` is a [`LinModel`](@ref). The method mutates the `g` vectors in argument and returns it. Only the custom constraints -are include in the `g` vector. +`gc` are include in the `g` vector. """ function con_nonlinprog!( g, ::PredictiveController, ::LinModel, ::TranscriptionMethod, _ , _ , gc, ϵ @@ -1285,7 +1303,7 @@ function con_nonlinprog!( return g end -""" +@doc raw""" con_nonlinprogeq!( geq, X̂0, Û0, K0 mpc::PredictiveController, model::NonLinModel, transcription::MultipleShooting, @@ -1294,7 +1312,14 @@ end Nonlinear equality constrains for [`NonLinModel`](@ref) and [`MultipleShooting`](@ref). -The method mutates the `geq`, `X̂0`, `Û0` and `K0` vectors in argument. +The method mutates the `geq`, `X̂0`, `Û0` and `K0` vectors in argument. The nonlinear +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) +``` +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). """ function con_nonlinprogeq!( geq, X̂0, Û0, K0, @@ -1333,21 +1358,33 @@ end Nonlinear equality constrains for [`NonLinModel`](@ref) and [`TrapezoidalCollocation`](@ref). -The method mutates the `geq`, `X̂0`, `Û0` and `K0` vectors in argument. The deterministic -and stochastic states are handled separately since collocation methods require -continuous-time state-space models, and the stochastic model of the unmeasured disturbances -is discrete-time. Also note that operating points in `model` are typically zeros for -[`NonLinModel`](@ref), but they are handled rigorously here if it's not the case. It should -be noted that linearization of continuous-time dynamics at non-equilibrium points leads to: +The method mutates the `geq`, `X̂0`, `Û0` and `K0` vectors in argument. + +The nonlinear equality constraints `geq` only includes the state defects. The deterministic +and stochastic states are handled separately since collocation methods require continuous- +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 - \mathbf{ẋ_0}(t) ≈ \mathbf{A x_0}(t) + \mathbf{B_u u_0}(t) + \mathbf{B_d d_0}(t) +\begin{aligned} +\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) +\end{aligned} ``` -as opposed to, for discrete-time models: +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 - \mathbf{x_0}(k+1) ≈ \mathbf{A x_0}(k) + \mathbf{B_u u_0}(k) + \mathbf{B_d d_0}(k) - + \mathbf{f_{op}} - \mathbf{x_{op}} +\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) +\end{aligned} ``` -hence no need to add `model.fop` and subtract `model.xop` in the collocation equations. +and the input of the augmented model is: +```math +\mathbf{û_0}(k) = \mathbf{u_0}(k) + \mathbf{C_{s_u} x_s}(k) +``` +the ``\mathbf{A_s, C_{s_u}}`` matrices are defined in [`init_estimstoch`](@ref) doc. """ function con_nonlinprogeq!( geq, X̂0, Û0, K0, @@ -1378,17 +1415,18 @@ function con_nonlinprogeq!( sdnext, ssnext = @views ŝnext[1:nx], ŝnext[nx+1:end] mul!(û0, Cs_u, xs) # ys_u = Cs_u*xs û0 .+= u0 # û0 = u0 + ys_u - ẋ0, ẋ0next = @views k0[1:nx], k0[nx+1:2*nx] - # no need to handle model.fop and model.xop, see docstring: - model.f!(ẋ0, xd, û0, d0, p) - model.f!(ẋ0next, xdnext_Z̃, û0, d0next, p) # assuming ZOH on manipulated inputs u + k1, k2 = @views k0[1:nx], k0[nx+1:2*nx] + model.f!(k1, xd, û0, d0, p) + model.f!(k2, xdnext_Z̃, û0, d0next, p) # assuming ZOH on manipulated inputs u xsnext = @views x̂0next[nx+1:end] mul!(xsnext, As, xs) - sdnext .= @. xd - xdnext_Z̃ + (Ts/2)*(ẋ0 + ẋ0next) + sdnext .= @. xd - xdnext_Z̃ + (Ts/2)*(k1 + k2) ssnext .= @. xsnext - xsnext_Z̃ x̂0 = x̂0next_Z̃ # using states in Z̃ for next iteration (allow parallel for) d0 = d0next end return geq end + +"No eq. constraints for other cases e.g. [`SingleShooting`](@ref), returns `geq` unchanged." con_nonlinprogeq!(geq,_,_,_,::PredictiveController,::SimModel,::TranscriptionMethod,_,_)=geq \ No newline at end of file diff --git a/src/estimator/construct.jl b/src/estimator/construct.jl index 1097cb297..8ae2558b4 100644 --- a/src/estimator/construct.jl +++ b/src/estimator/construct.jl @@ -356,11 +356,14 @@ end """ default_nint(model::SimModel, i_ym=1:model.ny, nint_u=0) -One integrator on each measured output by default if `model` is not a [`LinModel`](@ref). +One integrator on each measured output by default for other cases e.g. [`NonLinModel`](@ref). -Theres is no verification the augmented model remains observable. If the integrator quantity -per manipulated input `nint_u ≠ 0`, the method returns zero integrator on each measured -output. +If the integrator quantity per manipulated input `nint_u ≠ 0`, the method returns zero +integrator on each measured output. + +!!! warning + Theres is no verification the augmented model remains observable. The resulting + [`StateEstimator`](@ref) object should be assessed separately with e.g.: [`sim!`](@ref). """ function default_nint(model::SimModel, i_ym=1:model.ny, nint_u=0) validate_ym(model, i_ym) diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index 0bcf08c9a..45cc50558 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -22,7 +22,7 @@ By introducing an augmented state vector ``\mathbf{x̂_0}`` like in [`augment_mo the function returns the next state of the augmented model, as deviation vectors: ```math \begin{aligned} - \mathbf{x̂_0}(k+1) &= \mathbf{f̂}\Big(\mathbf{x̂_0}(k), \mathbf{u_0}(k), \mathbf{d_0}(k)\Big) + \mathbf{x̂_0}(k+1) &= \mathbf{f̂}\Big(\mathbf{x̂_0}(k), \mathbf{u_0}(k), \mathbf{d_0}(k)\Big) \\ \mathbf{ŷ_0}(k) &= \mathbf{ĥ}\Big(\mathbf{x̂_0}(k), \mathbf{d_0}(k)\Big) \end{aligned} ``` @@ -65,10 +65,23 @@ function f̂!(x̂0next, û0, k0, estim::StateEstimator, model::SimModel, x̂0, return f̂!(x̂0next, û0, k0, model, estim.As, estim.Cs_u, estim.f̂op, estim.x̂op, x̂0, u0, d0) end -""" +@doc raw""" f̂!(x̂0next, _ , _ , estim::StateEstimator, model::LinModel, x̂0, u0, d0) -> nothing Use the augmented model matrices and operating points if `model` is a [`LinModel`](@ref). + +# Extended Help +!!! details "Extended Help" + + This method computes: + ```math + \begin{aligned} + \mathbf{x̂_0}(k+1) &= \mathbf{Â x̂_0}(k) + \mathbf{B̂_u u_0}(k) + \mathbf{B̂_d d_0}(k) + + \mathbf{f̂_{op}} - \mathbf{x̂_{op}} \\ + \mathbf{ŷ_0}(k) &= \mathbf{Ĉ x̂_0}(k) + \mathbf{D̂_d d_0}(k) + \end{aligned} + ``` + with the augmented matrices constructed by [`augment_model`](@ref). """ function f̂!(x̂0next, _ , _ , estim::StateEstimator, ::LinModel, x̂0, u0, d0) mul!(x̂0next, estim.Â, x̂0) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index c54a056ff..414b1c86a 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -115,7 +115,7 @@ function getinfo(estim::MovingHorizonEstimator{NT}) where NT<:Real info = Dict{Symbol, Any}() V̂, X̂0 = similar(estim.Y0m[1:nym*Nk]), similar(estim.X̂0[1:nx̂*Nk]) û0, k0, ŷ0 = buffer.û, buffer.k, buffer.ŷ - V̂, X̂0 = predict!(V̂, X̂0, û0, k0, ŷ0, estim, model, estim.Z̃) + V̂, X̂0 = predict_mhe!(V̂, X̂0, û0, k0, ŷ0, estim, model, estim.Z̃) x̂0arr = @views estim.Z̃[nx̃-nx̂+1:nx̃] x̄ = estim.x̂0arr_old - x̂0arr X̂0 = [x̂0arr; X̂0] @@ -418,7 +418,7 @@ function optim_objective!(estim::MovingHorizonEstimator{NT}) where NT<:Real # --------- update estimate ----------------------- û0, ŷ0, k0 = buffer.û, buffer.ŷ, buffer.k estim.Ŵ[1:nŵ*Nk] .= @views estim.Z̃[nx̃+1:nx̃+nŵ*Nk] # update Ŵ with optimum for warm-start - V̂, X̂0 = predict!(V̂, X̂0, û0, k0, ŷ0, estim, model, estim.Z̃) + V̂, X̂0 = predict_mhe!(V̂, X̂0, û0, k0, ŷ0, estim, model, estim.Z̃) x̂0next = @views X̂0[end-nx̂+1:end] estim.x̂0 .= x̂0next return estim.Z̃ @@ -461,7 +461,7 @@ function set_warmstart_mhe!(V̂, X̂0, estim::MovingHorizonEstimator{NT}, Z̃var # --- process noise estimates Ŵ --- Z̃s[nx̃+1:end] = estim.Ŵ # verify definiteness of objective function: - V̂, X̂0 = predict!(V̂, X̂0, û0, k0, ŷ0, estim, model, Z̃s) + V̂, X̂0 = predict_mhe!(V̂, X̂0, û0, k0, ŷ0, estim, model, Z̃s) Js = obj_nonlinprog!(x̄, estim, model, V̂, Z̃s) if !isfinite(Js) Z̃s[nx̃+1:end] = 0 @@ -578,16 +578,22 @@ function obj_nonlinprog!( return dot(x̄, invP̄, x̄) + dot(Ŵ, invQ̂_Nk, Ŵ) + dot(V̂, invR̂_Nk, V̂) + Jϵ end -""" - predict!(V̂, X̂0, û0, k0, ŷ0, estim::MovingHorizonEstimator, model::LinModel, Z̃) -> V̂, X̂0 +@doc raw""" + predict_mhe!(V̂, X̂0, _, _, _, estim::MovingHorizonEstimator, model::LinModel, Z̃) -> V̂, X̂0 Compute the `V̂` vector and `X̂0` vectors for the `MovingHorizonEstimator` and `LinModel`. -The function mutates `V̂`, `X̂0`, `û0` and `ŷ0` vector arguments. The vector `V̂` is the -estimated sensor noises from ``k-N_k+1`` to ``k``. The `X̂0` vector is estimated states from -``k-N_k+2`` to ``k+1``. +The function mutates `V̂` and `X̂0` vector arguments. The vector `V̂` is the estimated sensor +noises from ``k-N_k+1`` to ``k``. The `X̂0` vector is estimated states from ``k-N_k+2`` to +``k+1``. The computations are (by truncating the matrices when `N_k < H_e`): +```math +\begin{aligned} +\mathbf{V̂} &= \mathbf{Ẽ Z̃} + \mathbf{F} \\ +\mathbf{X̂_0} &= \mathbf{Ẽ_x̂ Z̃} + \mathbf{F_x̂} +\end{aligned} +``` """ -function predict!(V̂, X̂0, _ , _ , _ , estim::MovingHorizonEstimator, ::LinModel, Z̃) +function predict_mhe!(V̂, X̂0, _ , _ , _ , estim::MovingHorizonEstimator, ::LinModel, Z̃) nϵ, Nk = estim.nϵ, estim.Nk[] nX̂, nŴ, nYm = estim.nx̂*Nk, estim.nx̂*Nk, estim.nym*Nk nZ̃ = nϵ + estim.nx̂ + nŴ @@ -596,8 +602,16 @@ function predict!(V̂, X̂0, _ , _ , _ , estim::MovingHorizonEstimator, ::LinMod return V̂, X̂0 end -"Compute the two vectors when `model` is not a `LinModel`." -function predict!(V̂, X̂0, û0, k0, ŷ0, estim::MovingHorizonEstimator, model::SimModel, Z̃) +@doc raw""" + predict_mhe!(V̂, X̂0, û0, k0, ŷ0, estim::MovingHorizonEstimator, model::SimModel, Z̃) -> V̂, X̂0 + +Compute the vectors when `model` is *not* a [`LinModel`](@ref). + +The function mutates `V̂`, `X̂0`, `û0` and `ŷ0` vector arguments. The augmented model of +[`f̂!`](@ref) and [`ĥ!`](@ref) is called recursively in a `for` loop from ``j=1`` to ``N_k``, +and by adding the estimated process noise ``\mathbf{ŵ}``. +""" +function predict_mhe!(V̂, X̂0, û0, k0, ŷ0, estim::MovingHorizonEstimator, model::SimModel, Z̃) nϵ, Nk = estim.nϵ, estim.Nk[] nu, nd, nx̂, nŵ, nym = model.nu, model.nd, estim.nx̂, estim.nx̂, estim.nym nx̃ = nϵ + nx̂ @@ -646,18 +660,18 @@ The method mutates all the arguments before `estim` argument. """ function update_prediction!(V̂, X̂0, û0, k0, ŷ0, g, estim::MovingHorizonEstimator, Z̃) model = estim.model - V̂, X̂0 = predict!(V̂, X̂0, û0, k0, ŷ0, estim, model, Z̃) + V̂, X̂0 = predict_mhe!(V̂, X̂0, û0, k0, ŷ0, estim, model, Z̃) ϵ = getϵ(estim, Z̃) - g = con_nonlinprog!(g, estim, model, X̂0, V̂, ϵ) + g = con_nonlinprog_mhe!(g, estim, model, X̂0, V̂, ϵ) return nothing end """ - con_nonlinprog!(g, estim::MovingHorizonEstimator, model::SimModel, X̂0, V̂, ϵ) + con_nonlinprog_mhe!(g, estim::MovingHorizonEstimator, model::SimModel, X̂0, V̂, ϵ) -> g -Nonlinear constrains for [`MovingHorizonEstimator`](@ref). +Compute nonlinear constrains `g` in-place for [`MovingHorizonEstimator`](@ref). """ -function con_nonlinprog!(g, estim::MovingHorizonEstimator, ::SimModel, X̂0, V̂, ϵ) +function con_nonlinprog_mhe!(g, estim::MovingHorizonEstimator, ::SimModel, X̂0, V̂, ϵ) nX̂con, nX̂ = length(estim.con.X̂0min), estim.nx̂ *estim.Nk[] nV̂con, nV̂ = length(estim.con.V̂min), estim.nym*estim.Nk[] for i in eachindex(g) @@ -684,7 +698,7 @@ function con_nonlinprog!(g, estim::MovingHorizonEstimator, ::SimModel, X̂0, V̂ end "No nonlinear constraints if `model` is a [`LinModel`](@ref), return `g` unchanged." -con_nonlinprog!(g, ::MovingHorizonEstimator, ::LinModel, _ , _ , _ ) = g +con_nonlinprog_mhe!(g, ::MovingHorizonEstimator, ::LinModel, _ , _ , _ ) = g "Throw an error if P̂ != nothing." function setstate_cov!(::MovingHorizonEstimator, P̂)