diff --git a/docs/src/internals/state_estim.md b/docs/src/internals/state_estim.md index 2a8df67d0..6b92cc2d7 100644 --- a/docs/src/internals/state_estim.md +++ b/docs/src/internals/state_estim.md @@ -27,7 +27,6 @@ ModelPredictiveControl.get_nonlincon_oracle(::MovingHorizonEstimator, ::ModelPre ```@docs ModelPredictiveControl.f̂! ModelPredictiveControl.ĥ! -ModelPredictiveControl.f̂_input! ``` ## Update Quadratic Optimization diff --git a/src/controller/construct.jl b/src/controller/construct.jl index 588d93f9c..d9002cd3c 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -133,13 +133,13 @@ struct ControllerConstraint{NT<:Real, GCfunc<:Union{Nothing, Function}} vx̂ ::Matrix{NT} bx̂ ::Vector{NT} # matrices for the zero defect constraints (N/A for single shooting transcriptions): - Ẽŝ ::Matrix{NT} - Fŝ ::Vector{NT} - Gŝ ::Matrix{NT} - Jŝ ::Matrix{NT} - Kŝ ::Matrix{NT} - Vŝ ::Matrix{NT} - Bŝ ::Vector{NT} + ẼS ::Matrix{NT} + FS ::Vector{NT} + GS ::Matrix{NT} + JS ::Matrix{NT} + KS ::Matrix{NT} + VS ::Matrix{NT} + BS ::Vector{NT} # custom linear equality constraints: Ẽw ::Matrix{NT} Fw ::Vector{NT} @@ -176,7 +176,6 @@ struct ControllerConstraint{NT<:Real, GCfunc<:Union{Nothing, Function}} # indices of finite numbers in the b vector (linear inequality constraints): i_b ::BitVector # A matrices for the linear equality constraints: - A_ŝ ::Matrix{NT} Aeq ::Matrix{NT} # b vector for the linear equality constraints: beq ::Vector{NT} @@ -518,7 +517,7 @@ function setconstraint!( con.A_Umin, con.A_Umax, con.A_ΔŨmin, con.A_ΔŨmax, con.A_Ymin, con.A_Ymax, con.A_Wmin, con.A_Wmax, con.A_x̂min, con.A_x̂max, - con.A_ŝ + con.Aeq ) A = con.A[con.i_b, :] b = con.b[con.i_b] @@ -858,7 +857,7 @@ verify_cond(::TranscriptionMethod,_,_) = nothing Hp, Hc, PΔu, Pu, E, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂, - Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ, + ES, GS, JS, KS, VS, BS, Wy, Wu, Wd, Wr, gc!=nothing, nc=0 ) -> con, nϵ, P̃Δu, P̃u, Ẽ @@ -874,7 +873,7 @@ function init_defaultcon_mpc( Hp, Hc, PΔu, Pu, E, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂, - Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ, + ES, GS, JS, KS, VS, BS, Wy, Wu, Wd, Wr, gc!::GCfunc = nothing, nc = 0 ) where {NT<:Real, GCfunc<:Union{Nothing, Function}} @@ -882,6 +881,7 @@ function init_defaultcon_mpc( nu, ny, nx̂ = model.nu, model.ny, estim.nx̂ nw = size(Wy, 1) nW = nw*(Hp+1) + nS = size(ES, 1) nϵ = weights.isinf_C ? 0 : 1 u0min, u0max = fill(convert(NT,-Inf), nu), fill(convert(NT,+Inf), nu) Δumin, Δumax = fill(convert(NT,-Inf), nu), fill(convert(NT,+Inf), nu) @@ -910,7 +910,7 @@ function init_defaultcon_mpc( A_Ymin, A_Ymax, Ẽ = relaxŶ(E, C_ymin, C_ymax, nϵ) A_Wmin, A_Wmax, Ẽw = relaxW(E, Pu, Hp, W̄y, W̄u, C_wmin, C_wmax, nϵ) A_x̂min, A_x̂max, ẽx̂ = relaxterminal(ex̂, c_x̂min, c_x̂max, nϵ) - A_ŝ, Ẽŝ = augmentdefect(Eŝ, nϵ) + Aeq, ẼS = augmentdefect(ES, nϵ) i_Umin, i_Umax = .!isinf.(U0min), .!isinf.(U0max) i_ΔŨmin, i_ΔŨmax = .!isinf.(ΔŨmin), .!isinf.(ΔŨmax) i_Ymin, i_Ymax = .!isinf.(Y0min), .!isinf.(Y0max) @@ -920,22 +920,21 @@ function init_defaultcon_mpc( model, transcription, nc, i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_Wmin, i_Wmax, i_x̂min, i_x̂max, A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_Wmin, A_Wmax, A_x̂max, A_x̂min, - A_ŝ + Aeq ) - # dummy fx̂, Fw and Fŝ vectors (updated just before optimization) - fx̂, Fw, Fŝ = zeros(NT, nx̂), zeros(NT, nW), zeros(NT, nx̂*Hp) + # dummy fx̂, Fw and FS vectors (updated just before optimization) + fx̂, Fw, FS = zeros(NT, nx̂), zeros(NT, nW), zeros(NT, nS) # dummy b and beq vectors (updated just before optimization) b, beq = zeros(NT, size(A, 1)), zeros(NT, size(Aeq, 1)) con = ControllerConstraint{NT, GCfunc}( ẽx̂ , fx̂ , gx̂ , jx̂ , kx̂ , vx̂ , bx̂ , - Ẽŝ , Fŝ , Gŝ , Jŝ , Kŝ , Vŝ , Bŝ , + ẼS , FS , GS , JS , KS , VS , BS , Ẽw , Fw , W̄y , W̄u , W̄d , W̄r , nw , U0min , U0max , ΔŨmin , ΔŨmax , Y0min , Y0max , Wmin , Wmax , x̂0min , x̂0max , A_Umin , A_Umax , A_ΔŨmin , A_ΔŨmax , A_Ymin , A_Ymax , A_Wmin , A_Wmax , A_x̂min , A_x̂max , A , b , i_b , - A_ŝ , Aeq , beq , neq , C_ymin , C_ymax , C_wmin , C_wmax , c_x̂min , c_x̂max , @@ -1194,24 +1193,24 @@ function relaxterminal(ex̂::AbstractMatrix{NT}, c_x̂min, c_x̂max, nϵ) where end @doc raw""" - augmentdefect(Eŝ, nϵ) -> A_ŝ, Ẽŝ + augmentdefect(ES, nϵ) -> Aeq, ẼS Augment defect equality constraints with slack variable ϵ if `nϵ == 1`. -It returns the ``\mathbf{Ẽ_ŝ}`` matrix that appears in the defect equation -``\mathbf{Ŝ = Ẽ_ŝ Z̃ + F_ŝ}`` and the ``\mathbf{A}`` matrix for the equality constraints: +It returns the ``\mathbf{Ẽ_S}`` matrix that appears in the linear defect equation +``\mathbf{Ẽ_S Z̃ + F_S}`` and the ``\mathbf{A}`` matrix for the equality constraints: ```math -\mathbf{A_ŝ Z̃} = - \mathbf{F_ŝ} +\mathbf{A_{eq} Z̃} = \mathbf{b_{eq}} = - \mathbf{F_S} ``` """ -function augmentdefect(Eŝ::AbstractMatrix{NT}, nϵ) where NT<:Real +function augmentdefect(ES::AbstractMatrix{NT}, nϵ) where NT<:Real if nϵ == 1 # Z̃ = [Z; ϵ] - Ẽŝ = [Eŝ zeros(NT, size(Eŝ, 1), 1)] + ẼS = [ES zeros(NT, size(ES, 1), 1)] else # Z̃ = Z (only hard constraints) - Ẽŝ = Eŝ + ẼS = ES end - A_ŝ = Ẽŝ - return A_ŝ, Ẽŝ + Aeq = ẼS + return Aeq, ẼS end @doc raw""" diff --git a/src/controller/execute.jl b/src/controller/execute.jl index 2164255ad..cf49cfa58 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -74,7 +74,7 @@ function moveinput!( validate_args(mpc, ry, d, lastu, D̂, R̂y, R̂u) initpred!(mpc, mpc.estim.model, ry, d, lastu, D̂, R̂y, R̂u) linconstraint!(mpc, mpc.estim.model, mpc.transcription) - linconstrainteq!(mpc, mpc.estim.model, mpc.transcription) + linconstrainteq!(mpc, mpc.estim.model, mpc.estim, mpc.transcription) Z̃ = optim_objective!(mpc) return getinput!(mpc, Z̃) end @@ -312,7 +312,36 @@ function predictstoch!(Ŷs, mpc::PredictiveController, estim::InternalModel) return nothing end "Fill `Ŷs` vector with 0 values when `estim` is not an [`InternalModel`](@ref)." -predictstoch!(Ŷs, mpc::PredictiveController, ::StateEstimator) = (Ŷs .= 0; nothing) +predictstoch!(Ŷs, ::PredictiveController, ::StateEstimator) = (Ŷs .= 0; nothing) + +""" + disturbedinput!(Û0, mpc::PredictiveController, estim::StateEstimator, U0, X̂0) -> nothing + +Fill disturbed inputs of the augmented model `Û0` in-place with stochastic states in `X̂0` + +Both `Û0` and `U0` variables include deviation vectors from ``k+0`` to ``k+H_p-1``. The +predicted states `X̂0` include deviation vectors from ``k+1`` to ``k+H_p-1`` (the current one +is stored in `estim.x̂0`). + +This function is used for the collocation methods that directly call the state derivative +function `estim.model.f!` with the manipulated inputs augmented with the estimated +disturbances at model input (see [`init_estimstoch`](@ref)). It's also necessary to prefill +the `Û0` vector before anything since both `û0` and `û0next` are needed at each stage with +hold order `h>0`, thus potential race conditions with multi-threading. +""" +function disturbedinput!(Û0, mpc::PredictiveController, estim::StateEstimator, U0, X̂0) + nu, nx, nx̂ = estim.model.nu, estim.model.nx, estim.nx̂ + Cs_u = estim.Cs_u + Û0 .= U0 + for j=0:mpc.Hp-1 + xs = @views j < 1 ? estim.x̂0[(nx+1):(nx̂)] : X̂0[(nx+1+nx̂*(j-1)):(nx̂*j)] + û0 = @views Û0[(1+nu*j):(nu*(j+1))] + mul!(û0, Cs_u, xs, 1, 1) # û0 = u0 + Cs_u*xs + end + return nothing +end +"No input disturbances for [`InternalModel`](@ref), hence do `Û0 .= U0`." +disturbedinput!(Û0, ::PredictiveController, ::InternalModel, U0, _) = (Û0 .= U0; nothing) @doc raw""" linconstraint_custom!(mpc::PredictiveController, model::SimModel) @@ -690,14 +719,14 @@ function setmodel_controller!(mpc::PredictiveController, uop_old, x̂op_old) con.vx̂ .= vx̂ con.bx̂ .= bx̂ # --- defect matrices --- - Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ = init_defectmat(model, estim, transcription, Hp, Hc, nb) - A_ŝ, Ẽŝ = augmentdefect(Eŝ, mpc.nϵ) - con.Ẽŝ .= Ẽŝ - con.Gŝ .= Gŝ - con.Jŝ .= Jŝ - con.Kŝ .= Kŝ - con.Vŝ .= Vŝ - con.Bŝ .= Bŝ + ES, GS, JS, KS, VS, BS = init_defectmat(model, estim, transcription, Hp, Hc, nb) + Aeq, ẼS = augmentdefect(ES, mpc.nϵ) + con.ẼS .= ẼS + con.GS .= GS + con.JS .= JS + con.KS .= KS + con.VS .= VS + con.BS .= BS # --- custom linear constraints --- con.Ẽw .= Ẽw # --- linear inequality constraints --- @@ -718,8 +747,7 @@ function setmodel_controller!(mpc::PredictiveController, uop_old, x̂op_old) con.A_x̂max ] # --- linear equality constraints --- - con.A_ŝ .= A_ŝ - con.Aeq .= A_ŝ + con.Aeq .= Aeq # --- operating points --- con.U0min .+= mpc.Uop # convert U0 to U with the old operating point con.U0max .+= mpc.Uop # convert U0 to U with the old operating point diff --git a/src/controller/explicitmpc.jl b/src/controller/explicitmpc.jl index 414155724..eff16ef88 100644 --- a/src/controller/explicitmpc.jl +++ b/src/controller/explicitmpc.jl @@ -191,7 +191,9 @@ function Base.show(io::IO, mpc::ExplicitMPC) print_estim_dim(io, mpc.estim, n) end -linconstraint!(::ExplicitMPC, ::LinModel, ::TranscriptionMethod) = nothing +linconstraint!(::ExplicitMPC, ::LinModel, ::SingleShooting) = nothing +linconstrainteq!(::ExplicitMPC, ::LinModel, ::StateEstimator, ::SingleShooting) = nothing +linconstrainteq!(::ExplicitMPC, ::LinModel, ::InternalModel, ::SingleShooting) = nothing @doc raw""" optim_objective!(mpc::ExplicitMPC) -> Z̃ diff --git a/src/controller/linmpc.jl b/src/controller/linmpc.jl index 47424d799..a394ab56b 100644 --- a/src/controller/linmpc.jl +++ b/src/controller/linmpc.jl @@ -73,13 +73,13 @@ struct LinMPC{ model, estim, transcription, Hp, Hc, nb ) F = zeros(NT, ny*Hp) # dummy value (updated just before optimization) - Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ = init_defectmat(model, estim, transcription, Hp, Hc, nb) + ES, GS, JS, KS, VS, BS = init_defectmat(model, estim, transcription, Hp, Hc, nb) con, nϵ, P̃Δu, P̃u, Ẽ = init_defaultcon_mpc( estim, weights, transcription, Hp, Hc, PΔu, Pu, E, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂, - Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ, + ES, GS, JS, KS, VS, BS, Wy, Wu, Wd, Wr ) H̃ = init_quadprog(model, transcription, weights, Ẽ, P̃Δu, P̃u) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 41f481a81..519d31abc 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -103,13 +103,13 @@ struct NonLinMPC{ model, estim, transcription, Hp, Hc, nb ) F = zeros(NT, ny*Hp) # dummy value (updated just before optimization) - Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ = init_defectmat(model, estim, transcription, Hp, Hc, nb) + ES, GS, JS, KS, VS, BS = init_defectmat(model, estim, transcription, Hp, Hc, nb) con, nϵ, P̃Δu, P̃u, Ẽ = init_defaultcon_mpc( estim, weights, transcription, Hp, Hc, PΔu, Pu, E, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂, - Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ, + ES, GS, JS, KS, VS, BS, Wy, Wu, Wd, Wr, gc!, nc ) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 16eb74ac1..e53b07d38 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -600,24 +600,24 @@ end @doc raw""" init_defectmat( model::LinModel, estim, transcription::MultipleShooting, Hp, Hc, nb - ) -> Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ + ) -> ES, GS, JS, KS, VS, BS Init the matrices for computing the defects over the predicted states. Knowing that the decision vector ``\mathbf{Z}`` contains both ``\mathbf{ΔU}`` and ``\mathbf{X̂_0}`` vectors (with a [`MultipleShooting`](@ref) transcription), an equation -similar to the prediction matrices (see [`init_predmat`](@ref)) computes the defects over -the predicted states: +similar to the prediction matrices (see [`init_predmat`](@ref)) computes the defects of +the estimated states of ``H_p``: ```math \begin{aligned} - \mathbf{Ŝ} &= \mathbf{E_ŝ Z} + \mathbf{G_ŝ d_0}(k) + \mathbf{J_ŝ D̂_0} - + \mathbf{K_ŝ x̂_0}(k) + \mathbf{V_ŝ u_0}(k-1) - + \mathbf{B_ŝ} \\ - &= \mathbf{E_ŝ Z} + \mathbf{F_ŝ} + \mathbf{Ŝ} &= \mathbf{E_S Z} + \mathbf{G_S d_0}(k) + \mathbf{J_S D̂_0} + + \mathbf{K_S x̂_0}(k) + \mathbf{V_S u_0}(k-1) + + \mathbf{B_S} \\ + &= \mathbf{E_S Z} + \mathbf{F_S} \end{aligned} ``` They are forced to be ``\mathbf{Ŝ = 0}`` using the optimization equality constraints. The -matrices ``\mathbf{E_ŝ, G_ŝ, J_ŝ, K_ŝ, V_ŝ, B_ŝ}`` are defined in the Extended Help section. +matrices ``\mathbf{E_S, G_S, J_S, K_S, V_S, B_S}`` are defined in the Extended Help section. # Extended Help !!! details "Extended Help" @@ -634,34 +634,34 @@ matrices ``\mathbf{E_ŝ, G_ŝ, J_ŝ, K_ŝ, V_ŝ, B_ŝ}`` are defined in th The defect matrices are computed with: ```math \begin{aligned} - \mathbf{E_ŝ} &= \begin{bmatrix} - \mathbf{E_{ŝ}^{Δu}} & \mathbf{E_{ŝ}^{x̂}} \end{bmatrix} \\ - \mathbf{E_{ŝ}^{Δu}} &= \begin{bmatrix} + \mathbf{E_S} &= \begin{bmatrix} + \mathbf{E_{S}^{Δu}} & \mathbf{E_{S}^{x̂}} \end{bmatrix} \\ + \mathbf{E_{S}^{Δu}} &= \begin{bmatrix} \mathbf{Q}(n_1) & \mathbf{0} & \cdots & \mathbf{0} \\ \mathbf{Q}(n_2) & \mathbf{Q}(n_2) & \cdots & \mathbf{0} \\ \vdots & \vdots & \ddots & \vdots \\ \mathbf{Q}(n_{H_c}) & \mathbf{Q}(n_{H_c}) & \cdots & \mathbf{Q}(n_{H_c}) \end{bmatrix} \\ - \mathbf{E_{ŝ}^{x̂}} &= \begin{bmatrix} - -\mathbf{I} & \mathbf{0} & \cdots & \mathbf{0} & \mathbf{0} \\ - \mathbf{Â} & -\mathbf{I} & \cdots & \mathbf{0} & \mathbf{0} \\ - \vdots & \vdots & \ddots & \vdots & \vdots \\ - \mathbf{0} & \mathbf{0} & \cdots & \mathbf{Â} & -\mathbf{I} \end{bmatrix} \\ - \mathbf{G_ŝ} &= \begin{bmatrix} + \mathbf{E_{S}^{x̂}} &= \begin{bmatrix} + -\mathbf{I} & \mathbf{0} & \cdots & \mathbf{0} & \mathbf{0} \\ + \mathbf{Â} & -\mathbf{I} & \cdots & \mathbf{0} & \mathbf{0} \\ + \vdots & \vdots & \ddots & \vdots & \vdots \\ + \mathbf{0} & \mathbf{0} & \cdots & \mathbf{Â} & -\mathbf{I} \end{bmatrix} \\ + \mathbf{G_S} &= \begin{bmatrix} \mathbf{B̂_d} \\ \mathbf{0} \\ \vdots \\ \mathbf{0} \end{bmatrix} \\ - \mathbf{J_ŝ} &= \begin{bmatrix} + \mathbf{J_S} &= \begin{bmatrix} \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} & \mathbf{0} \\ \mathbf{B̂_d} & \mathbf{0} & \cdots & \mathbf{0} & \mathbf{0} \\ \vdots & \vdots & \ddots & \vdots & \vdots \\ \mathbf{0} & \mathbf{0} & \cdots & \mathbf{B̂_d} & \mathbf{0} \end{bmatrix} \\ - \mathbf{K_ŝ} &= \begin{bmatrix} + \mathbf{K_S} &= \begin{bmatrix} \mathbf{Â} \\ \mathbf{0} \\ \vdots \\ \mathbf{0} \end{bmatrix} \\ - \mathbf{V_ŝ} &= \begin{bmatrix} + \mathbf{V_S} &= \begin{bmatrix} \mathbf{B̂_u} \\ \mathbf{B̂_u} \\ \vdots \\ \mathbf{B̂_u} \end{bmatrix} \\ - \mathbf{B_ŝ} &= \begin{bmatrix} + \mathbf{B_S} &= \begin{bmatrix} \mathbf{f̂_{op} - x̂_{op}} \\ \mathbf{f̂_{op} - x̂_{op}} \\ \vdots \\ \mathbf{f̂_{op} - x̂_{op}} \end{bmatrix} \end{aligned} ``` - The ``\mathbf{E_ŝ^{Δu}}`` matrix structure is due to the move blocking implementation: + The ``\mathbf{E_S^{Δu}}`` matrix structure is due to the move blocking implementation: the ``\mathbf{ΔU}`` vector only contains the input increment of the free moves (see [`move_blocking`](@ref)). """ @@ -679,38 +679,130 @@ function init_defectmat( return Q end # --- current state estimates x̂0 --- - Kŝ = [Â; zeros(NT, nx̂*(Hp-1), nx̂)] + KS = [Â; zeros(NT, nx̂*(Hp-1), nx̂)] # --- previous manipulated inputs lastu0 --- - Vŝ = repeat(B̂u, Hp) + VS = repeat(B̂u, Hp) # --- decision variables Z --- nI_nx̂ = Matrix{NT}(-I, nx̂, nx̂) - Eŝ = [zeros(nx̂*Hp, nu*Hc) repeatdiag(nI_nx̂, Hp)] + ES = [zeros(NT, nx̂*Hp, nu*Hc) repeatdiag(nI_nx̂, Hp)] for j=1:Hc iCol = (1:nu) .+ nu*(j-1) for i=j:Hc ni = nb[i] iRow = (1:nx̂*ni) .+ nx̂*sum(nb[1:i-1]) - Q = @views Eŝ[iRow, iCol] + Q = @views ES[iRow, iCol] Q!(Q, ni) end end for j=1:Hp-1 iRow = (1:nx̂) .+ nx̂*j iCol = (1:nx̂) .+ nx̂*(j-1) .+ nu*Hc - Eŝ[iRow, iCol] = Â + ES[iRow, iCol] = Â end # --- current measured disturbances d0 and predictions D̂0 --- - Gŝ = [B̂d; zeros(NT, (Hp-1)*nx̂, nd)] - Jŝ = [zeros(nx̂, nd*Hp); repeatdiag(B̂d, Hp-1) zeros(NT, nx̂*(Hp-1), nd)] + GS = [B̂d; zeros(NT, nx̂*(Hp-1), nd)] + JS = [zeros(NT, nx̂, nd*Hp); repeatdiag(B̂d, Hp-1) zeros(NT, nx̂*(Hp-1), nd)] # --- state x̂op and state update f̂op operating points --- - Bŝ = repeat(estim.f̂op - estim.x̂op, Hp) - return Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ + BS = repeat(estim.f̂op - estim.x̂op, Hp) + return ES, GS, JS, KS, VS, BS +end + +@doc raw""" + init_defectmat( + model::SimModel, estim::StateEstimator, transcription::CollocationMethod, Hp, Hc, _ + ) + +Init the matrices for computing the defects of the stochastic states only. + +The documentation of [`init_estimstoch`](@ref) shows that the stochastic model of the +unmeasured disturbances is linear and discrete-time. The defect of the stochastic states +over ``H_p`` is therefore: +```math +\begin{aligned} + \mathbf{Ŝ_s} &= \mathbf{E_S Z} + \mathbf{K_S x̂_0}(k) \\ + &= \mathbf{E_S Z} + \mathbf{F_S} +\end{aligned} +``` +The matrices ``\mathbf{E_S}`` and ``\mathbf{K_S}`` are defined in the Extended Help section. + +# Extended Help +!!! details "Extended Help" + Using the stochastic matrix ``\mathbf{A_s}`` in `estim` (see [`init_estimstoch`](@ref)), + the defect matrices are computed with: + ```math + \begin{aligned} + \mathbf{E_{S}^{Δu}} &= \mathbf{0} \\ + \mathbf{E_{S}^{x̂}} &= \begin{bmatrix} + \mathbf{0} &-\mathbf{I} & \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ + \mathbf{0} & \mathbf{A_s} & \mathbf{0} &-\mathbf{I} & \cdots & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ + \vdots & \vdots & \vdots & \vdots & \ddots & \vdots & \vdots & \vdots \\ + \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \cdots & \mathbf{A_s} & \mathbf{0} &-\mathbf{I} \end{bmatrix} \\ + \mathbf{E_{S}^{k}} &= \mathbf{0} \\ + \mathbf{K_S} &= \begin{bmatrix} + \mathbf{0} & \mathbf{A_s} \\ + \mathbf{0} & \mathbf{0} \\ + \vdots & \vdots \\ + \mathbf{0} & \mathbf{0} \end{bmatrix} + \end{aligned} + ``` + and: + - if `transcription` is an [`OrthogonalCollocation`](@ref), ``\mathbf{E_S} = [\begin{smallmatrix} + \mathbf{E_{S}^{Δu}} & \mathbf{E_{S}^{x̂}} & \mathbf{E_{S}^{k}} \end{smallmatrix}]`` + - else ``\mathbf{E_S} = [\begin{smallmatrix} \mathbf{E_{S}^{Δu}} & \mathbf{E_{S}^{x̂}} \end{smallmatrix}]`` +""" +function init_defectmat( + model::SimModel, estim::StateEstimator{NT}, transcription::CollocationMethod, Hp, Hc, _ +) where {NT<:Real} + nu, nx, nd, nx̂, nxs = model.nu, model.nx, model.nd, estim.nx̂, estim.nxs + nZ = get_nZ(estim, transcription, Hp, Hc) + nK = nZ - nu*Hc - nx̂*Hp + As = estim.As + # --- current state estimates x̂0 --- + KS = zeros(NT, nxs*Hp, nx̂) + KS[1:nxs, nx+1:end] = As + # --- previous manipulated inputs lastu0 --- + VS = zeros(nxs*Hp, nu) + # --- decision variables Z --- + zeros_nI = [zeros(NT, nxs, nx) -I] + ES = [zeros(NT, nxs*Hp, nu*Hc) repeatdiag(zeros_nI, Hp) zeros(NT, nxs*Hp, nK)] + for j=1:Hp-1 + iRow = (1:nxs) .+ nxs*j + iCol = (nx+1:nx̂) .+ nx̂*(j-1) .+ nu*Hc + ES[iRow, iCol] = As + end + # --- current measured disturbances d0 and predictions D̂0 --- + GS = zeros(NT, nxs*Hp, nd) + JS = zeros(NT, nxs*Hp, nd*Hp) + # --- state x̂op and state update f̂op operating points --- + BS = zeros(NT, nxs*Hp) + return ES, GS, JS, KS, VS, BS +end + +""" + init_defectmat( + model::SimModel, estim::InternalModel{NT}, transcription::CollocationMethod, Hp, Hc, _ + ) -> ES, GS, JS, KS, VS, BS + +Return empty matrices for [`InternalModel`](@ref) (state vector is not augmented). +""" +function init_defectmat( + model::SimModel, estim::InternalModel{NT}, transcription::CollocationMethod, Hp, Hc, _ +) where {NT<:Real} + nx̂, nu, nd = estim.nx̂, model.nu, model.nd + nZ = get_nZ(estim, transcription, Hp, Hc) + ES = zeros(NT, 0, nZ) + GS = zeros(NT, 0, nd) + JS = zeros(NT, 0, nd*Hp) + KS = zeros(NT, 0, nx̂) + VS = zeros(NT, 0, nu) + BS = zeros(NT, 0) + return ES, GS, JS, KS, VS, BS end """ init_defectmat( model::SimModel, estim, transcription::TranscriptionMethod, Hp, Hc, nb - ) -> Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ + ) -> ES, GS, JS, KS, VS, BS Return empty matrices for all other cases (N/A). """ @@ -719,13 +811,13 @@ function init_defectmat( ) where {NT<:Real} nx̂, nu, nd = estim.nx̂, model.nu, model.nd nZ = get_nZ(estim, transcription, Hp, Hc) - Eŝ = zeros(NT, 0, nZ) - Gŝ = zeros(NT, 0, nd) - Jŝ = zeros(NT, 0, nd*Hp) - Kŝ = zeros(NT, 0, nx̂) - Vŝ = zeros(NT, 0, nu) - Bŝ = zeros(NT, 0) - return Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ + ES = zeros(NT, 0, nZ) + GS = zeros(NT, 0, nd) + JS = zeros(NT, 0, nd*Hp) + KS = zeros(NT, 0, nx̂) + VS = zeros(NT, 0, nu) + BS = zeros(NT, 0) + return ES, GS, JS, KS, VS, BS end @doc raw""" @@ -751,7 +843,7 @@ The argument `nc` is the number of custom nonlinear inequality constraints in finite numbers. `i_g` is a similar vector but for the indices of ``\mathbf{g}``. The method also returns the ``\mathbf{A, A_{eq}}`` matrices and `neq` if `args` is provided. In such a case, `args` needs to contain all the inequality and equality constraint matrices: -`A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_Wmin, A_Wmax, A_x̂min, A_x̂max, A_ŝ`. +`A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_Wmin, A_Wmax, A_x̂min, A_x̂max, Aeq`. The integer `neq` is the number of nonlinear equality constraints in ``\mathbf{g_{eq}}``. """ function init_matconstraint_mpc( @@ -768,7 +860,7 @@ function init_matconstraint_mpc( A_Ymin, A_Ymax, A_Wmin, A_Wmax, A_x̂min, A_x̂max, - A_ŝ + Aeq ) = args A = [ A_Umin; A_Umax; @@ -777,8 +869,7 @@ function init_matconstraint_mpc( A_Wmin; A_Wmax A_x̂min; A_x̂max; ] - Aeq = A_ŝ - neq = 0 + neq = 0 # number of nonlinear equality constraints end i_b = [i_Umin; i_Umax; i_ΔŨmin; i_ΔŨmax; i_Ymin; i_Ymax; i_Wmin; i_Wmax; i_x̂min; i_x̂max] i_g = trues(nc) @@ -794,10 +885,9 @@ function init_matconstraint_mpc( if isempty(args) A, Aeq, neq = nothing, nothing, nothing else - A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, _ , _ , A_Wmin, A_Wmax, _ , _ , A_ŝ = args + A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, _ , _ , A_Wmin, A_Wmax, _ , _ , Aeq = args A = [A_Umin; A_Umax; A_ΔŨmin; A_ΔŨmax; A_Wmin; A_Wmax] - Aeq = A_ŝ - neq = 0 + neq = 0 # number of nonlinear equality constraints end i_b = [i_Umin; i_Umax; i_ΔŨmin; i_ΔŨmax; i_Wmin; i_Wmax] i_g = [i_Ymin; i_Ymax; i_x̂min; i_x̂max; trues(nc)] @@ -813,11 +903,10 @@ function init_matconstraint_mpc( if isempty(args) A, Aeq, neq = nothing, nothing, nothing else - A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, _ , _ , A_Wmin, A_Wmax, A_x̂min, A_x̂max, A_ŝ = args + A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, _ , _ , A_Wmin, A_Wmax, A_x̂min, A_x̂max, Aeq = args A = [A_Umin; A_Umax; A_ΔŨmin; A_ΔŨmax; A_Wmin; A_Wmax; A_x̂min; A_x̂max] - Aeq = A_ŝ nΔŨ, nZ̃ = size(A_ΔŨmin) - neq = nZ̃ - nΔŨ + neq = nZ̃ - nΔŨ - size(Aeq, 1) # number of nonlinear equality constraints end i_b = [i_Umin; i_Umax; i_ΔŨmin; i_ΔŨmax; i_Wmin; i_Wmax; i_x̂min; i_x̂max] i_g = [i_Ymin; i_Ymax; trues(nc)] @@ -928,29 +1017,44 @@ end @doc raw""" linconstrainteq!( - mpc::PredictiveController, model::LinModel, transcription::MultipleShooting + mpc::PredictiveController, model::LinModel, ::StateEstimator, ::MultipleShooting ) Set `beq` vector for the linear model equality constraints (``\mathbf{A_{eq} Z̃ = b_{eq}}``). -Also init ``\mathbf{F_ŝ} = \mathbf{G_ŝ d_0}(k) + \mathbf{J_ŝ D̂_0} + \mathbf{K_ŝ x̂_0}(k) + -\mathbf{V_ŝ u_0}(k-1) + \mathbf{B_ŝ}``, see [`init_defectmat`](@ref). +Also init ``\mathbf{F_S} = \mathbf{G_S d_0}(k) + \mathbf{J_S D̂_0} + \mathbf{K_S x̂_0}(k) + +\mathbf{V_S u_0}(k-1) + \mathbf{B_S}``, see [`init_defectmat`](@ref). """ -function linconstrainteq!(mpc::PredictiveController, model::LinModel, ::MultipleShooting) - Fŝ = mpc.con.Fŝ - Fŝ .= mpc.con.Bŝ - mul!(Fŝ, mpc.con.Kŝ, mpc.estim.x̂0, 1, 1) - mul!(Fŝ, mpc.con.Vŝ, mpc.lastu0, 1, 1) +function linconstrainteq!( + mpc::PredictiveController, model::LinModel, ::StateEstimator, ::MultipleShooting +) + FS = mpc.con.FS + FS .= mpc.con.BS + mul!(FS, mpc.con.KS, mpc.estim.x̂0, 1, 1) + mul!(FS, mpc.con.VS, mpc.lastu0, 1, 1) if model.nd > 0 - mul!(Fŝ, mpc.con.Gŝ, mpc.d0, 1, 1) - mul!(Fŝ, mpc.con.Jŝ, mpc.D̂0, 1, 1) + mul!(FS, mpc.con.GS, mpc.d0, 1, 1) + mul!(FS, mpc.con.JS, mpc.D̂0, 1, 1) end - mpc.con.beq .= @. -Fŝ + mpc.con.beq .= @. -FS + linconeq = mpc.optim[:linconstrainteq] + JuMP.set_normalized_rhs(linconeq, mpc.con.beq) + return nothing +end + +function linconstrainteq!( + mpc::PredictiveController, ::SimModel, ::StateEstimator, ::CollocationMethod +) + FS = mpc.con.FS + # the only non-zeros matrices are ES and KS: + mul!(FS, mpc.con.KS, mpc.estim.x̂0) + mpc.con.beq .= @. -FS linconeq = mpc.optim[:linconstrainteq] JuMP.set_normalized_rhs(linconeq, mpc.con.beq) return nothing end -linconstrainteq!(::PredictiveController, ::SimModel, ::TranscriptionMethod) = nothing +linconstrainteq!(::PredictiveController, ::SimModel, ::InternalModel, ::CollocationMethod ) = nothing +linconstrainteq!(::PredictiveController, ::SimModel, ::StateEstimator, ::TranscriptionMethod) = nothing @doc raw""" set_warmstart!(mpc::PredictiveController, ::SingleShooting, Z̃var) -> Z̃s @@ -1341,20 +1445,16 @@ end Nonlinear equality constrains for [`NonLinModel`](@ref) and [`TrapezoidalCollocation`](@ref). The method mutates the `geq`, `X̂0`, `Û0` and `K̇` 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: +constraints `geq` includes the defects of the deterministic states only. The stochastic +states are handled seperatly as linear equality constraints, see [`init_defectmat`](@ref). +The deterministic state defects are computed with: ```math -\begin{aligned} -\mathbf{s_d}(k+j+1) &= \mathbf{x_0}(k+j) + 0.5 T_s [\mathbf{k̇}_1(k+j) + \mathbf{k̇}_2(k+j)] - - \mathbf{x_0}(k+j+1) \\ -\mathbf{s_s}(k+j+1) &= \mathbf{A_s x_s}(k+j) - \mathbf{x_s}(k+j+1) -\end{aligned} +\mathbf{s_d}(k+j+1) = \mathbf{x_0}(k+j) + 0.5 T_s [\mathbf{k̇}_1(k+j) + \mathbf{k̇}_2(k+j)] + - \mathbf{x_0}(k+j+1) ``` -for ``j = 0, 1, ... , H_p-1``, and 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: +for ``j = 0, 1, ... , H_p-1``, and in which ``\mathbf{x_0}`` is the deterministic state +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(k+j) &= \mathbf{f}\Big(\mathbf{x_0}(k+j), \mathbf{û_0}(k+j), \mathbf{d̂_0}(k+j), \mathbf{p}\Big) \\ @@ -1362,7 +1462,7 @@ deterministic and stochastic states extracted from the decision variables `Z̃`. \end{aligned} ``` in which ``h`` is the hold order `transcription.h` and the disturbed input ``\mathbf{û_0}`` -is defined in [`f̂_input!`](@ref). +is defined in [`f̂!`](@ref) documentation. """ function con_nonlinprogeq!( geq, X̂0, Û0, K̇, @@ -1377,11 +1477,7 @@ function con_nonlinprogeq!( nk = get_nk(model, transcription) D̂0 = mpc.D̂0 X̂0_Z̃ = @views Z̃[(nΔU+1):(nΔU+nX̂)] - for j=0:Hp-1 # prefilling Û0 to avoid race-condition (both û0 and û0next are needed): - x̂0_Z̃ = @views j < 1 ? mpc.estim.x̂0[1:nx̂] : X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*j)] - u0, û0 = @views U0[(1 + nu*j):(nu*(j+1))], Û0[(1 + nu*j):(nu*(j+1))] - f̂_input!(û0, mpc.estim, model, x̂0_Z̃, u0) - end + disturbedinput!(Û0, mpc, mpc.estim, U0, X̂0_Z̃) @threadsif f_threads for j=1:Hp if j < 2 x̂0_Z̃ = @views mpc.estim.x̂0[1:nx̂] @@ -1392,19 +1488,11 @@ function con_nonlinprogeq!( end k̇ = @views K̇[(1 + nk*(j-1)):(nk*j)] d̂0next = @views D̂0[(1 + nd*(j-1)):(nd*j)] - x̂0next = @views X̂0[(1 + nx̂*(j-1)):(nx̂*j)] x̂0next_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*j)] - sdnext = @views geq[(1 + nx̂*(j-1) ):(nx̂*(j-1) + nx)] - ssnext = @views geq[(1 + nx̂*(j-1) + nx):(nx̂*j )] + sdnext = @views geq[(1 + nx*(j-1) ):(nx*(j-1) + nx)] x0_Z̃ = @views x̂0_Z̃[1:nx] x0next_Z̃ = @views x̂0next_Z̃[1:nx] k̇1, k̇2 = @views k̇[1:nx], k̇[nx+1:2*nx] - # ----------------- stochastic defects ----------------------------------------- - xsnext = @views x̂0next[nx+1:end] - xsnext_Z̃ = @views x̂0next_Z̃[nx+1:end] - fs!(x̂0next, mpc.estim, model, x̂0_Z̃) - ssnext .= @. xsnext - xsnext_Z̃ - # ----------------- deterministic defects: trapezoidal collocation ------------- û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] if f_threads || h < 1 || j < 2 # we need to recompute k1 with multi-threading, even with h==1, since the @@ -1468,9 +1556,9 @@ and disturbances are piecewise constant or linear: \mathbf{d̂}_i(k+j) &= (1-τ_i)\mathbf{d̂_0}(k+j) + τ_i\mathbf{d̂_0}(k+j+1) \end{aligned} ``` -The disturbed input ``\mathbf{û_0}(k+j)`` is defined in [`f̂_input!`](@ref). The defects for -the stochastic states ``\mathbf{s_s}`` are computed as the [`TrapezoidalCollocation`](@ref) -method, and the ones for the continuity constraint of the deterministic states are: +The disturbed input ``\mathbf{û_0}`` is defined in [`f̂!`](@ref). The defects of the +stochastic states are linear equality constraints (see [`init_defectmat`](@ref)), and the +ones for the continuity constraint of the deterministic states are: ```math \mathbf{s_c}(k+j+1) = \mathbf{C_o} \begin{bmatrix} @@ -1496,15 +1584,11 @@ function con_nonlinprogeq!( no, τ = transcription.no, transcription.τ Mo, Co, λo = mpc.Mo, mpc.Co, mpc.λo nk = get_nk(model, transcription) - nx̂_nk = nx̂ + nk + nx_nk = nx + nk D̂0 = mpc.D̂0 X̂0_Z̃, K_Z̃ = @views Z̃[(nΔU+1):(nΔU+nX̂)], Z̃[(nΔU+nX̂+1):(nΔU+nX̂+nk*Hp)] D̂temp = mpc.buffer.D̂ - for j=0:Hp-1 # prefilling Û0 to avoid race-condition (both û0 and û0next are needed): - x̂0_Z̃ = @views j < 1 ? mpc.estim.x̂0[1:nx̂] : X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*j)] - u0, û0 = @views U0[(1 + nu*j):(nu*(j+1))], Û0[(1 + nu*j):(nu*(j+1))] - f̂_input!(û0, mpc.estim, model, x̂0_Z̃, u0) - end + disturbedinput!(Û0, mpc, mpc.estim, U0, X̂0_Z̃) @threadsif f_threads for j=1:Hp if j < 2 x̂0_Z̃ = @views mpc.estim.x̂0[1:nx̂] @@ -1516,18 +1600,11 @@ function con_nonlinprogeq!( k̇ = @views K̇[(1 + nk*(j-1)):(nk*j)] k_Z̃ = @views K_Z̃[(1 + nk*(j-1)):(nk*j)] d̂0next = @views D̂0[(1 + nd*(j-1)):(nd*j)] - x̂0next = @views X̂0[(1 + nx̂*(j-1)):(nx̂*j)] x̂0next_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*j)] - scnext = @views geq[(1 + nx̂_nk*(j-1) ):(nx̂_nk*(j-1) + nx)] - ssnext = @views geq[(1 + nx̂_nk*(j-1) + nx):(nx̂_nk*(j-1) + nx̂)] - sk = @views geq[(1 + nx̂_nk*(j-1) + nx̂):(nx̂_nk*j )] + scnext = @views geq[(1 + nx_nk*(j-1) ):(nx_nk*(j-1) + nx)] + sk = @views geq[(1 + nx_nk*(j-1) + nx):(nx_nk*j )] x0_Z̃ = @views x̂0_Z̃[1:nx] x0next_Z̃ = @views x̂0next_Z̃[1:nx] - # ----------------- stochastic defects ----------------------------------------- - xsnext = @views x̂0next[nx+1:end] - xsnext_Z̃ = @views x̂0next_Z̃[nx+1:end] - fs!(x̂0next, mpc.estim, model, x̂0_Z̃) - ssnext .= @. xsnext - xsnext_Z̃ # ----------------- collocation constraint defects ----------------------------- û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] Δk = k̇ diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index 717b66b15..d06cc06c2 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -121,25 +121,6 @@ function fs!(x̂0next, estim::StateEstimator, model::SimModel, x̂0) return nothing end -@doc raw""" - f̂_input!(û0, estim::StateEstimator, model::SimModel, x̂0, u0) -> nothing - -Compute the disturbed input ``\mathbf{û_0}`` of the augmented model from `x̂0` and `u0`. - -It mutates `û0` in place with the following equation: -```math -\mathbf{û_0}(k) = \mathbf{u_0}(k) + \mathbf{C_{s_u} x_s}(k) -``` -where ``\mathbf{C_{s_u}}`` is defined in [`init_estimstoch`](@ref), and ``\mathbf{x_s}`` is -extracted from `x̂0` as the last `estim.nxs` elements. -""" -function f̂_input!(û0, estim::StateEstimator, model::SimModel, x̂0, u0) - xs = @views x̂0[model.nx+1:end] - mul!(û0, estim.Cs_u, xs) # ys_u = Cs_u*xs - û0 .+= u0 # û0 = u0 + ys_u - return nothing -end - @doc raw""" ĥ!(ŷ0, estim::StateEstimator, model::SimModel, x̂0, d0) -> nothing diff --git a/src/estimator/internal_model.jl b/src/estimator/internal_model.jl index 24418af80..afc9285ac 100644 --- a/src/estimator/internal_model.jl +++ b/src/estimator/internal_model.jl @@ -189,13 +189,6 @@ Does nothing since [`InternalModel`](@ref) does not augment the state vector. """ fs!( _ , ::InternalModel, ::SimModel, _ ) = nothing -@doc raw""" - f̂_input!(û0, estim::InternalModel, model::SimModel, x̂0, u0) -> nothing - -Compute `û0 .= u0` since [`InternalModel`](@ref) does not augment the state vector. -""" -f̂_input!(û0, ::InternalModel, ::SimModel, _ , u0) = (û0 .= u0; nothing) - @doc raw""" ĥ!(ŷ0, estim::InternalModel, model::NonLinModel, x̂0, d0) diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index 4d7196f46..31b6a46bf 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -853,7 +853,7 @@ end nmpc16 = NonLinMPC(nonlinmodel_c, Hp=10, transcription=TrapezoidalCollocation(), nc=10, gc=gc!) @test nmpc16.transcription == TrapezoidalCollocation() @test length(nmpc16.Z̃) == nonlinmodel_c.nu*nmpc16.Hc + nmpc16.estim.nx̂*nmpc16.Hp + nmpc16.nϵ - @test nmpc16.con.neq == nmpc16.estim.nx̂*nmpc16.Hp + @test nmpc16.con.neq == nmpc16.estim.model.nx*nmpc16.Hp @test nmpc16.con.nc == 10 nmpc18 = NonLinMPC(nonlinmodel, Hp=10, gradient=AutoFiniteDiff(),