diff --git a/docs/src/internals/predictive_control.md b/docs/src/internals/predictive_control.md index 8bf16e5db..0700a5232 100644 --- a/docs/src/internals/predictive_control.md +++ b/docs/src/internals/predictive_control.md @@ -43,7 +43,7 @@ ModelPredictiveControl.linconstrainteq! ```@docs ModelPredictiveControl.optim_objective!(::PredictiveController) -ModelPredictiveControl.set_warmstart! +ModelPredictiveControl.set_warmstart_mpc! ModelPredictiveControl.predict! ModelPredictiveControl.disturbedinput! ModelPredictiveControl.con_nonlinprog! diff --git a/docs/src/internals/state_estim.md b/docs/src/internals/state_estim.md index 6b92cc2d7..9b7cf41cb 100644 --- a/docs/src/internals/state_estim.md +++ b/docs/src/internals/state_estim.md @@ -12,6 +12,7 @@ ModelPredictiveControl.init_integrators ModelPredictiveControl.augment_model ModelPredictiveControl.init_ukf ModelPredictiveControl.init_internalmodel +ModelPredictiveControl.init_ZtoŴ ModelPredictiveControl.init_predmat_mhe ModelPredictiveControl.relaxarrival ModelPredictiveControl.relaxX̂ diff --git a/docs/src/public/generic_func.md b/docs/src/public/generic_func.md index f29abaa86..15ac57289 100644 --- a/docs/src/public/generic_func.md +++ b/docs/src/public/generic_func.md @@ -7,6 +7,38 @@ Pages = ["generic_func.md"] This page contains the documentation of functions that are generic to [`SimModel`](@ref), [`StateEstimator`](@ref) and [`PredictiveController`](@ref) types. +## Direct Transcription Methods + +### TranscriptionMethod + +```@docs +ModelPredictiveControl.TranscriptionMethod +``` + +### SingleShooting + +```@docs +SingleShooting +``` + +### MultipleShooting + +```@docs +MultipleShooting +``` + +### TrapezoidalCollocation + +```@docs +TrapezoidalCollocation +``` + +### OrthogonalCollocation + +```@docs +OrthogonalCollocation +``` + ## Set Constraint ```@docs diff --git a/docs/src/public/predictive_control.md b/docs/src/public/predictive_control.md index dd8d0df3b..297f8b8b2 100644 --- a/docs/src/public/predictive_control.md +++ b/docs/src/public/predictive_control.md @@ -103,35 +103,3 @@ NonLinMPC ```@docs moveinput! ``` - -## Direct Transcription Methods - -### TranscriptionMethod - -```@docs -ModelPredictiveControl.TranscriptionMethod -``` - -### SingleShooting - -```@docs -SingleShooting -``` - -### MultipleShooting - -```@docs -MultipleShooting -``` - -### TrapezoidalCollocation - -```@docs -TrapezoidalCollocation -``` - -### OrthogonalCollocation - -```@docs -OrthogonalCollocation -``` diff --git a/src/ModelPredictiveControl.jl b/src/ModelPredictiveControl.jl index 0129a16da..c786b558c 100644 --- a/src/ModelPredictiveControl.jl +++ b/src/ModelPredictiveControl.jl @@ -58,6 +58,7 @@ export SimResult, getinfo, sim! include("general.jl") include("sim_model.jl") +include("transcription.jl") include("state_estim.jl") include("predictive_control.jl") include("plot_sim.jl") diff --git a/src/controller/construct.jl b/src/controller/construct.jl index 406cdce61..af263ba32 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -1,16 +1,16 @@ -const MSG_LINMODEL_ERR = "estim.model type must be a LinModel, see ManualEstimator docstring "* +const MSG_LINMODEL_ERR = "estim.model type must be a LinModel, see ManualEstimator docstring " * "to use a nonlinear state estimator with a linear controller" struct PredictiveControllerBuffer{NT<:Real} - u ::Vector{NT} - Z̃ ::Vector{NT} - D̂ ::Vector{NT} - Ŷ ::Vector{NT} - U ::Vector{NT} + u::Vector{NT} + Z̃::Vector{NT} + D̂::Vector{NT} + Ŷ::Vector{NT} + U::Vector{NT} D̂e::Vector{NT} Ŷe::Vector{NT} Ue::Vector{NT} - Ẽ ::Matrix{NT} + Ẽ::Matrix{NT} P̃u::Matrix{NT} empty::Vector{NT} end @@ -24,18 +24,18 @@ The buffer is used to store intermediate results during computation without allo """ function PredictiveControllerBuffer( estim::StateEstimator{NT}, transcription::TranscriptionMethod, Hp::Int, Hc::Int, nϵ::Int -) where NT <: Real +) where NT<:Real nu, ny, nd = estim.model.nu, estim.model.ny, estim.model.nd - nZ̃ = get_nZ(estim, transcription, Hp, Hc) + nϵ - u = Vector{NT}(undef, nu) - Z̃ = Vector{NT}(undef, nZ̃) - D̂ = Vector{NT}(undef, nd*Hp) - Ŷ = Vector{NT}(undef, ny*Hp) - U = Vector{NT}(undef, nu*Hp) + nZ̃ = get_nZ_mpc(estim, transcription, Hp, Hc) + nϵ + u = Vector{NT}(undef, nu) + Z̃ = Vector{NT}(undef, nZ̃) + D̂ = Vector{NT}(undef, nd*Hp) + Ŷ = Vector{NT}(undef, ny*Hp) + U = Vector{NT}(undef, nu*Hp) D̂e = Vector{NT}(undef, nd*(Hp+1)) Ŷe = Vector{NT}(undef, ny*(Hp+1)) Ue = Vector{NT}(undef, nu*(Hp+1)) - Ẽ = Matrix{NT}(undef, ny*Hp, nZ̃) + Ẽ = Matrix{NT}(undef, ny*Hp, nZ̃) P̃u = Matrix{NT}(undef, nu*Hp, nZ̃) empty = Vector{NT}(undef, 0) return PredictiveControllerBuffer{NT}(u, Z̃, D̂, Ŷ, U, D̂e, Ŷe, Ue, Ẽ, P̃u, empty) @@ -45,31 +45,31 @@ end struct ControllerWeights{ NT<:Real, # parameters to support both dense and Diagonal matrices (with specialization): - MW<:AbstractMatrix{NT}, - NW<:AbstractMatrix{NT}, - LW<:AbstractMatrix{NT}, + MW<:AbstractMatrix{NT}, + NW<:AbstractMatrix{NT}, + LW<:AbstractMatrix{NT}, } - M_Hp::Hermitian{NT, MW} - Ñ_Hc::Hermitian{NT, NW} - L_Hp::Hermitian{NT, LW} - E ::NT + M_Hp::Hermitian{NT,MW} + Ñ_Hc::Hermitian{NT,NW} + L_Hp::Hermitian{NT,LW} + E::NT iszero_M_Hp::Vector{Bool} iszero_Ñ_Hc::Vector{Bool} iszero_L_Hp::Vector{Bool} iszero_E::Bool - isinf_C ::Bool + isinf_C::Bool function ControllerWeights{NT}( M_Hp::MW, N_Hc::NW, L_Hp::LW, Cwt, Ewt ) where { - NT<:Real, - MW<:AbstractMatrix{NT}, - NW<:AbstractMatrix{NT}, + NT<:Real, + MW<:AbstractMatrix{NT}, + NW<:AbstractMatrix{NT}, LW<:AbstractMatrix{NT} } nΔU = size(N_Hc, 1) C = Cwt isinf_C = isinf(C) - if !isinf_C + if !isinf_C # ΔŨ = [ΔU; ϵ] (ϵ is the slack variable) Ñ_Hc = [N_Hc zeros(NT, nΔU, 1); zeros(NT, 1, nΔU) C] isdiag(N_Hc) && (Ñ_Hc = Diagonal(Ñ_Hc)) # NW(Ñ_Hc) does not work on Julia 1.10 @@ -85,8 +85,8 @@ struct ControllerWeights{ iszero_Ñ_Hc = [iszero(Ñ_Hc)] iszero_L_Hp = [iszero(L_Hp)] iszero_E = iszero(E) - return new{NT, MW, NW, LW}( - M_Hp, Ñ_Hc, L_Hp, E, + return new{NT,MW,NW,LW}( + M_Hp, Ñ_Hc, L_Hp, E, iszero_M_Hp, iszero_Ñ_Hc, iszero_L_Hp, iszero_E, isinf_C ) end @@ -94,8 +94,8 @@ end "Outer constructor to validate and convert weight matrices if necessary." function ControllerWeights( - model::SimModel{NT}, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt=Inf, Ewt=0 - ) where {NT<:Real} + model::SimModel{NT}, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt=Inf, Ewt=0 +) where {NT<:Real} validate_weights(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt) M_Hp, N_Hc, L_Hp = NT.(M_Hp), NT.(N_Hc), NT.(L_Hp) return ControllerWeights{NT}(M_Hp, N_Hc, L_Hp, Cwt, Ewt) @@ -105,97 +105,97 @@ end function validate_weights(model, Hp, Hc, M_Hp, N_Hc, L_Hp, C=Inf, E=nothing) nu, ny = model.nu, model.ny nM, nN, nL = ny*Hp, nu*Hc, nu*Hp - Hp < 1 && throw(ArgumentError("Prediction horizon Hp should be ≥ 1")) - Hc < 1 && throw(ArgumentError("Control horizon Hc should be ≥ 1")) + Hp < 1 && throw(ArgumentError("Prediction horizon Hp should be ≥ 1")) + Hc < 1 && throw(ArgumentError("Control horizon Hc should be ≥ 1")) Hc > Hp && throw(ArgumentError("Control horizon Hc should be ≤ prediction horizon Hp")) - size(M_Hp) ≠ (nM,nM) && throw(DimensionMismatch("M_Hp size $(size(M_Hp)) ≠ (ny*Hp, ny*Hp) ($nM,$nM)")) - size(N_Hc) ≠ (nN,nN) && throw(DimensionMismatch("N_Hc size $(size(N_Hc)) ≠ (nu*Hc, nu*Hc) ($nN,$nN)")) - size(L_Hp) ≠ (nL,nL) && throw(DimensionMismatch("L_Hp size $(size(L_Hp)) ≠ (nu*Hp, nu*Hp) ($nL,$nL)")) + size(M_Hp) ≠ (nM, nM) && throw(DimensionMismatch("M_Hp size $(size(M_Hp)) ≠ (ny*Hp, ny*Hp) ($nM,$nM)")) + size(N_Hc) ≠ (nN, nN) && throw(DimensionMismatch("N_Hc size $(size(N_Hc)) ≠ (nu*Hc, nu*Hc) ($nN,$nN)")) + size(L_Hp) ≠ (nL, nL) && throw(DimensionMismatch("L_Hp size $(size(L_Hp)) ≠ (nu*Hp, nu*Hp) ($nL,$nL)")) (isdiag(M_Hp) && any(diag(M_Hp) .< 0)) && throw(ArgumentError("Mwt values should be nonnegative")) (isdiag(N_Hc) && any(diag(N_Hc) .< 0)) && throw(ArgumentError("Nwt values should be nonnegative")) (isdiag(L_Hp) && any(diag(L_Hp) .< 0)) && throw(ArgumentError("Lwt values should be nonnegative")) !ishermitian(M_Hp) && throw(ArgumentError("M_Hp should be hermitian")) !ishermitian(N_Hc) && throw(ArgumentError("N_Hc should be hermitian")) !ishermitian(L_Hp) && throw(ArgumentError("L_Hp should be hermitian")) - size(C) ≠ () && throw(DimensionMismatch("Cwt should be a real scalar")) - C < 0 && throw(ArgumentError("Cwt weight should be ≥ 0")) + size(C) ≠ () && throw(DimensionMismatch("Cwt should be a real scalar")) + C < 0 && throw(ArgumentError("Cwt weight should be ≥ 0")) !isnothing(E) && size(E) ≠ () && throw(DimensionMismatch("Ewt should be a real scalar")) end "Include all the data for the constraints of [`PredictiveController`](@ref)" -struct ControllerConstraint{NT<:Real, GCfunc<:Union{Nothing, Function}} +struct ControllerConstraint{NT<:Real,GCfunc<:Union{Nothing,Function}} # matrices for the terminal constraints: - ẽx̂ ::Matrix{NT} - fx̂ ::Vector{NT} - gx̂ ::Matrix{NT} - jx̂ ::Matrix{NT} - kx̂ ::Matrix{NT} - vx̂ ::Matrix{NT} - bx̂ ::Vector{NT} + ẽx̂::Matrix{NT} + fx̂::Vector{NT} + gx̂::Matrix{NT} + jx̂::Matrix{NT} + kx̂::Matrix{NT} + vx̂::Matrix{NT} + bx̂::Vector{NT} # matrices for the zero defect constraints (N/A for single shooting transcriptions): - ẼS ::Matrix{NT} - FS ::Vector{NT} - GS ::Matrix{NT} - JS ::Matrix{NT} - KS ::Matrix{NT} - VS ::Matrix{NT} - BS ::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} - W̄y ::SparseMatrixCSC{NT, Int} - W̄u ::SparseMatrixCSC{NT, Int} - W̄d ::SparseMatrixCSC{NT, Int} - W̄r ::SparseMatrixCSC{NT, Int} - nw ::Int + Ẽw::Matrix{NT} + Fw::Vector{NT} + W̄y::SparseMatrixCSC{NT,Int} + W̄u::SparseMatrixCSC{NT,Int} + W̄d::SparseMatrixCSC{NT,Int} + W̄r::SparseMatrixCSC{NT,Int} + nw::Int # bounds over the prediction horizon (deviation vectors from operating points): - U0min ::Vector{NT} - U0max ::Vector{NT} - ΔUmin ::Vector{NT} - ΔUmax ::Vector{NT} - Y0min ::Vector{NT} - Y0max ::Vector{NT} - Wmin ::Vector{NT} - Wmax ::Vector{NT} - x̂0min ::Vector{NT} - x̂0max ::Vector{NT} + U0min::Vector{NT} + U0max::Vector{NT} + ΔUmin::Vector{NT} + ΔUmax::Vector{NT} + Y0min::Vector{NT} + Y0max::Vector{NT} + Wmin::Vector{NT} + Wmax::Vector{NT} + x̂0min::Vector{NT} + x̂0max::Vector{NT} # vectors for the box constraints: - Z̃min ::Vector{NT} - Z̃max ::Vector{NT} + Z̃min::Vector{NT} + Z̃max::Vector{NT} # A matrices for the linear inequality constraints: - A_Umin ::SparseMatrixCSC{NT, Int} - A_Umax ::SparseMatrixCSC{NT, Int} - A_ΔUmin ::SparseMatrixCSC{NT, Int} - A_ΔUmax ::SparseMatrixCSC{NT, Int} - A_Ymin ::Matrix{NT} - A_Ymax ::Matrix{NT} - A_Wmin ::Matrix{NT} - A_Wmax ::Matrix{NT} - A_x̂min ::Matrix{NT} - A_x̂max ::Matrix{NT} - A ::Matrix{NT} + A_Umin::SparseMatrixCSC{NT,Int} + A_Umax::SparseMatrixCSC{NT,Int} + A_ΔUmin::SparseMatrixCSC{NT,Int} + A_ΔUmax::SparseMatrixCSC{NT,Int} + A_Ymin::Matrix{NT} + A_Ymax::Matrix{NT} + A_Wmin::Matrix{NT} + A_Wmax::Matrix{NT} + A_x̂min::Matrix{NT} + A_x̂max::Matrix{NT} + A::Matrix{NT} # b vector for the linear inequality constraints: - b ::Vector{NT} + b::Vector{NT} # indices of finite numbers in the b vector (linear inequality constraints): - i_b ::BitVector - # A matrices for the linear equality constraints: - Aeq ::Matrix{NT} - # b vector for the linear equality constraints: - beq ::Vector{NT} - # nonlinear equality constraints: - neq ::Int + i_b::BitVector + # Aeq matrix for the linear equality constraints: + Aeq::Matrix{NT} + # beq vector for the linear equality constraints: + beq::Vector{NT} + # number of nonlinear equality constraints: + neq::Int # constraint softness parameter vectors needing separate storage: - C_ymin ::Vector{NT} - C_ymax ::Vector{NT} - C_wmin ::Vector{NT} - C_wmax ::Vector{NT} - c_x̂min ::Vector{NT} - c_x̂max ::Vector{NT} + C_ymin::Vector{NT} + C_ymax::Vector{NT} + C_wmin::Vector{NT} + C_wmax::Vector{NT} + c_x̂min::Vector{NT} + c_x̂max::Vector{NT} # indices of finite numbers in the g vector (nonlinear inequality constraints): - i_g ::BitVector + i_g::BitVector # custom nonlinear inequality constraints: - gc! ::GCfunc - nc ::Int + gc!::GCfunc + nc::Int end @doc raw""" @@ -322,41 +322,41 @@ LinMPC controller with a sample time Ts = 4.0 s: value for these matrices is treated as a zero matrix. """ function setconstraint!( - mpc::PredictiveController; - umin = nothing, umax = nothing, - Deltaumin = nothing, Deltaumax = nothing, - ymin = nothing, ymax = nothing, - wmin = nothing, wmax = nothing, - xhatmin = nothing, xhatmax = nothing, - c_umin = nothing, c_umax = nothing, - c_Deltaumin = nothing, c_Deltaumax = nothing, - c_ymin = nothing, c_ymax = nothing, - c_wmin = nothing, c_wmax = nothing, - c_xhatmin = nothing, c_xhatmax = nothing, - Umin = nothing, Umax = nothing, - DeltaUmin = nothing, DeltaUmax = nothing, - Ymin = nothing, Ymax = nothing, - Wmin = nothing, Wmax = nothing, - C_umax = nothing, C_umin = nothing, - C_Deltaumax = nothing, C_Deltaumin = nothing, - C_ymax = nothing, C_ymin = nothing, - C_wmax = nothing, C_wmin = nothing, - Δumin = Deltaumin, Δumax = Deltaumax, - x̂min = xhatmin, x̂max = xhatmax, - c_Δumin = c_Deltaumin, c_Δumax = c_Deltaumax, - c_x̂min = c_xhatmin, c_x̂max = c_xhatmax, - ΔUmin = DeltaUmin, ΔUmax = DeltaUmax, - C_Δumin = C_Deltaumin, C_Δumax = C_Deltaumax, + mpc::PredictiveController; + umin=nothing, umax=nothing, + Deltaumin=nothing, Deltaumax=nothing, + ymin=nothing, ymax=nothing, + wmin=nothing, wmax=nothing, + xhatmin=nothing, xhatmax=nothing, + c_umin=nothing, c_umax=nothing, + c_Deltaumin=nothing, c_Deltaumax=nothing, + c_ymin=nothing, c_ymax=nothing, + c_wmin=nothing, c_wmax=nothing, + c_xhatmin=nothing, c_xhatmax=nothing, + Umin=nothing, Umax=nothing, + DeltaUmin=nothing, DeltaUmax=nothing, + Ymin=nothing, Ymax=nothing, + Wmin=nothing, Wmax=nothing, + C_umax=nothing, C_umin=nothing, + C_Deltaumax=nothing, C_Deltaumin=nothing, + C_ymax=nothing, C_ymin=nothing, + C_wmax=nothing, C_wmin=nothing, + Δumin=Deltaumin, Δumax=Deltaumax, + x̂min=xhatmin, x̂max=xhatmax, + c_Δumin=c_Deltaumin, c_Δumax=c_Deltaumax, + c_x̂min=c_xhatmin, c_x̂max=c_xhatmax, + ΔUmin=DeltaUmin, ΔUmax=DeltaUmax, + C_Δumin=C_Deltaumin, C_Δumax=C_Deltaumax, ) - model, estim, con = mpc.estim.model, mpc.estim, mpc.con + model, estim, con = mpc.estim.model, mpc.estim, mpc.con transcription, optim = mpc.transcription, mpc.optim nu, ny, nx̂, Hp, Hc = model.nu, model.ny, estim.nx̂, mpc.Hp, mpc.Hc nϵ, nw, nc = mpc.nϵ, con.nw, con.nc notSolvedYet = (JuMP.termination_status(optim) == JuMP.OPTIMIZE_NOT_CALLED) if isnothing(Umin) && !isnothing(umin) size(umin) == (nu,) || throw(DimensionMismatch("umin size must be $((nu,))")) - for i = 1:nu*Hp - con.U0min[i] = umin[(i-1) % nu + 1] - mpc.Uop[i] + for i = 1:(nu*Hp) + con.U0min[i] = umin[(i-1)%nu+1] - mpc.Uop[i] end elseif !isnothing(Umin) size(Umin) == (nu*Hp,) || throw(DimensionMismatch("Umin size must be $((nu*Hp,))")) @@ -364,35 +364,35 @@ function setconstraint!( end if isnothing(Umax) && !isnothing(umax) size(umax) == (nu,) || throw(DimensionMismatch("umax size must be $((nu,))")) - for i = 1:nu*Hp - con.U0max[i] = umax[(i-1) % nu + 1] - mpc.Uop[i] + for i = 1:(nu*Hp) + con.U0max[i] = umax[(i-1)%nu+1] - mpc.Uop[i] end elseif !isnothing(Umax) - size(Umax) == (nu*Hp,) || throw(DimensionMismatch("Umax size must be $((nu*Hp,))")) + size(Umax) == (nu*Hp,) || throw(DimensionMismatch("Umax size must be $((nu*Hp,))")) con.U0max .= Umax .- mpc.Uop end if isnothing(ΔUmin) && !isnothing(Δumin) size(Δumin) == (nu,) || throw(DimensionMismatch("Δumin size must be $((nu,))")) - for i = 1:nu*Hc - con.ΔUmin[i] = Δumin[(i-1) % nu + 1] + for i = 1:(nu*Hc) + con.ΔUmin[i] = Δumin[(i-1)%nu+1] end elseif !isnothing(ΔUmin) - size(ΔUmin) == (nu*Hc,) || throw(DimensionMismatch("ΔUmin size must be $((nu*Hc,))")) + size(ΔUmin) == (nu*Hc,) || throw(DimensionMismatch("ΔUmin size must be $((nu*Hc,))")) con.ΔUmin .= ΔUmin end if isnothing(ΔUmax) && !isnothing(Δumax) size(Δumax) == (nu,) || throw(DimensionMismatch("Δumax size must be $((nu,))")) - for i = 1:nu*Hc - con.ΔUmax[i] = Δumax[(i-1) % nu + 1] + for i = 1:(nu*Hc) + con.ΔUmax[i] = Δumax[(i-1)%nu+1] end elseif !isnothing(ΔUmax) - size(ΔUmax) == (nu*Hc,) || throw(DimensionMismatch("ΔUmax size must be $((nu*Hc,))")) + size(ΔUmax) == (nu*Hc,) || throw(DimensionMismatch("ΔUmax size must be $((nu*Hc,))")) con.ΔUmax .= ΔUmax end if isnothing(Ymin) && !isnothing(ymin) size(ymin) == (ny,) || throw(DimensionMismatch("ymin size must be $((ny,))")) - for i = 1:ny*Hp - con.Y0min[i] = ymin[(i-1) % ny + 1] - mpc.Yop[i] + for i = 1:(ny*Hp) + con.Y0min[i] = ymin[(i-1)%ny+1] - mpc.Yop[i] end elseif !isnothing(Ymin) size(Ymin) == (ny*Hp,) || throw(DimensionMismatch("Ymin size must be $((ny*Hp,))")) @@ -400,8 +400,8 @@ function setconstraint!( end if isnothing(Ymax) && !isnothing(ymax) size(ymax) == (ny,) || throw(DimensionMismatch("ymax size must be $((ny,))")) - for i = 1:ny*Hp - con.Y0max[i] = ymax[(i-1) % ny + 1] - mpc.Yop[i] + for i = 1:(ny*Hp) + con.Y0max[i] = ymax[(i-1)%ny+1] - mpc.Yop[i] end elseif !isnothing(Ymax) size(Ymax) == (ny*Hp,) || throw(DimensionMismatch("Ymax size must be $((ny*Hp,))")) @@ -409,8 +409,8 @@ function setconstraint!( end if isnothing(Wmin) && !isnothing(wmin) size(wmin) == (nw,) || throw(DimensionMismatch("wmin size must be $((nw,))")) - for i = 1:nw*(Hp+1) - con.Wmin[i] = wmin[(i-1) % nw + 1] + for i = 1:(nw*(Hp+1)) + con.Wmin[i] = wmin[(i-1)%nw+1] end elseif !isnothing(Wmin) size(Wmin) == (nw*(Hp+1),) || throw(DimensionMismatch("Wmin size must be $((nw*(Hp+1),))")) @@ -418,8 +418,8 @@ function setconstraint!( end if isnothing(Wmax) && !isnothing(wmax) size(wmax) == (nw,) || throw(DimensionMismatch("wmax size must be $((nw,))")) - for i = 1:nw*(Hp+1) - con.Wmax[i] = wmax[(i-1) % nw + 1] + for i = 1:(nw*(Hp+1)) + con.Wmax[i] = wmax[(i-1)%nw+1] end elseif !isnothing(Wmax) size(Wmax) == (nw*(Hp+1),) || throw(DimensionMismatch("Wmax size must be $((nw*(Hp+1),))")) @@ -443,14 +443,14 @@ function setconstraint!( notSolvedYet || error("Cannot set softness parameters after calling moveinput!") end if notSolvedYet - isnothing(C_umin) && !isnothing(c_umin) && (C_umin = repeat(c_umin, Hp)) - isnothing(C_umax) && !isnothing(c_umax) && (C_umax = repeat(c_umax, Hp)) - isnothing(C_Δumin) && !isnothing(c_Δumin) && (C_Δumin = repeat(c_Δumin, Hc)) - isnothing(C_Δumax) && !isnothing(c_Δumax) && (C_Δumax = repeat(c_Δumax, Hc)) - isnothing(C_ymin) && !isnothing(c_ymin) && (C_ymin = repeat(c_ymin, Hp)) - isnothing(C_ymax) && !isnothing(c_ymax) && (C_ymax = repeat(c_ymax, Hp)) - isnothing(C_wmin) && !isnothing(c_wmin) && (C_wmin = repeat(c_wmin, Hp+1)) - isnothing(C_wmax) && !isnothing(c_wmax) && (C_wmax = repeat(c_wmax, Hp+1)) + isnothing(C_umin) && !isnothing(c_umin) && (C_umin = repeat(c_umin, Hp)) + isnothing(C_umax) && !isnothing(c_umax) && (C_umax = repeat(c_umax, Hp)) + isnothing(C_Δumin) && !isnothing(c_Δumin) && (C_Δumin = repeat(c_Δumin, Hc)) + isnothing(C_Δumax) && !isnothing(c_Δumax) && (C_Δumax = repeat(c_Δumax, Hc)) + isnothing(C_ymin) && !isnothing(c_ymin) && (C_ymin = repeat(c_ymin, Hp)) + isnothing(C_ymax) && !isnothing(c_ymax) && (C_ymax = repeat(c_ymax, Hp)) + isnothing(C_wmin) && !isnothing(c_wmin) && (C_wmin = repeat(c_wmin, Hp+1)) + isnothing(C_wmax) && !isnothing(c_wmax) && (C_wmax = repeat(c_wmax, Hp+1)) if !isnothing(C_umin) size(C_umin) == (nu*Hp,) || throw(DimensionMismatch("C_umin size must be $((nu*Hp,))")) any(<(0), C_umin) && error("C_umin weights should be non-negative") @@ -464,7 +464,7 @@ function setconstraint!( if !isnothing(C_Δumin) size(C_Δumin) == (nu*Hc,) || throw(DimensionMismatch("C_Δumin size must be $((nu*Hc,))")) any(<(0), C_Δumin) && error("C_Δumin weights should be non-negative") - con.A_ΔUmin[:, end] .= @. -C_Δumin + con.A_ΔUmin[:, end] .= @. -C_Δumin end if !isnothing(C_Δumax) size(C_Δumax) == (nu*Hc,) || throw(DimensionMismatch("C_Δumax size must be $((nu*Hc,))")) @@ -510,18 +510,18 @@ function setconstraint!( end Z̃min, Z̃max = init_boxconstraint_mpc( estim, transcription, Hp, Hc, nϵ, - con.ΔUmin, con.ΔUmax, con.x̂0min, con.x̂0max, - con.A_ΔUmin, con.A_ΔUmax, con.A_x̂min, con.A_x̂max + con.ΔUmin, con.ΔUmax, con.x̂0min, con.x̂0max, + con.A_ΔUmin, con.A_ΔUmax, con.A_x̂min, con.A_x̂max ) Z̃var::Vector{JuMP.VariableRef} = optim[:Z̃var] if notSolvedYet con.i_b[:], con.i_g[:], con.A[:] = init_matconstraint_mpc( model, transcription, Z̃min, Z̃max, nc, nϵ, - con.U0min, con.U0max, con.ΔUmin, con.ΔUmax, - con.Y0min, con.Y0max, con.Wmin, con.Wmax, - con.x̂0min, con.x̂0max, - con.A_Umin, con.A_Umax, con.A_ΔUmin, con.A_ΔUmax, - con.A_Ymin, con.A_Ymax, con.A_Wmin, con.A_Wmax, + con.U0min, con.U0max, con.ΔUmin, con.ΔUmax, + con.Y0min, con.Y0max, con.Wmin, con.Wmax, + con.x̂0min, con.x̂0max, + con.A_Umin, con.A_Umax, con.A_ΔUmin, con.A_ΔUmax, + con.A_Ymin, con.A_Ymax, con.A_Wmin, con.A_Wmax, con.A_x̂min, con.A_x̂max, con.Aeq ) @@ -541,9 +541,9 @@ function setconstraint!( else i_b, i_g = init_matconstraint_mpc( model, transcription, Z̃min, Z̃max, nc, nϵ, - con.U0min, con.U0max, con.ΔUmin, con.ΔUmax, - con.Y0min, con.Y0max, con.Wmin, con.Wmax, - con.x̂0min, con.x̂0max + con.U0min, con.U0max, con.ΔUmin, con.ΔUmax, + con.Y0min, con.Y0max, con.Wmin, con.Wmax, + con.x̂0min, con.x̂0max ) diff_Z̃min, diff_Z̃max = diff_infs(Z̃min, con.Z̃min), diff_infs(Z̃max, con.Z̃max) if i_b ≠ con.i_b || i_g ≠ con.i_g || diff_Z̃min || diff_Z̃max @@ -625,7 +625,7 @@ The provided `nb` vector is modified to ensure `sum(nb) == Hp`: - If `sum(nb) < Hp`, a new element is pushed to `nb` with the value `Hp - sum(nb)`. - If `sum(nb) > Hp`, the intervals are truncated until `sum(nb) == Hp`. For example, if `Hp = 10` and `nb = [1, 2, 3, 6, 7]`, then `nb` is truncated to `[1, 2, 3, 4]`. -""" +""" function move_blocking(Hp_arg::Int, Hc_arg::AbstractVector{Int}) Hp = Hp_arg nb = Hc_arg @@ -637,7 +637,7 @@ function move_blocking(Hp_arg::Int, Hc_arg::AbstractVector{Int}) nb = nb[begin:findfirst(≥(Hp), cumsum(nb))] if sum(nb) > Hp # if the last block is too large, it is truncated to fit Hp: - nb[end] = Hp - @views sum(nb[begin:end-1]) + nb[end] = Hp - @views sum(nb[begin:(end-1)]) end end return nb @@ -701,10 +701,10 @@ Check the dimensions of the arguments of [`moveinput!`](@ref). """ function validate_args(mpc::PredictiveController, ry, d, lastu, D̂, R̂y, R̂u) ny, nd, nu, Hp = mpc.estim.model.ny, mpc.estim.model.nd, mpc.estim.model.nu, mpc.Hp - size(ry) ≠ (ny,) && throw(DimensionMismatch("ry size $(size(ry)) ≠ output size ($ny,)")) - size(d) ≠ (nd,) && throw(DimensionMismatch("d size $(size(d)) ≠ measured dist. size ($nd,)")) + size(ry) ≠ (ny,) && throw(DimensionMismatch("ry size $(size(ry)) ≠ output size ($ny,)")) + size(d) ≠ (nd,) && throw(DimensionMismatch("d size $(size(d)) ≠ measured dist. size ($nd,)")) size(lastu) ≠ (nu,) && throw(DimensionMismatch("lastu size $(size(lastu)) ≠ manip. input size ($nu,)")) - size(D̂) ≠ (nd*Hp,) && throw(DimensionMismatch("D̂ size $(size(D̂)) ≠ measured dist. size × Hp ($(nd*Hp),)")) + size(D̂) ≠ (nd*Hp,) && throw(DimensionMismatch("D̂ size $(size(D̂)) ≠ measured dist. size × Hp ($(nd*Hp),)")) size(R̂y) ≠ (ny*Hp,) && throw(DimensionMismatch("R̂y size $(size(R̂y)) ≠ output size × Hp ($(ny*Hp),)")) size(R̂u) ≠ (nu*Hp,) && throw(DimensionMismatch("R̂u size $(size(R̂u)) ≠ manip. input size × Hp ($(nu*Hp),)")) end @@ -734,7 +734,7 @@ function init_ZtoΔU( estim::StateEstimator{NT}, transcription::TranscriptionMethod, Hp, Hc ) where {NT<:Real} I_nu_Hc = sparse(Matrix{NT}(I, estim.model.nu*Hc, estim.model.nu*Hc)) - nZ = get_nZ(estim, transcription, Hp, Hc) + nZ = get_nZ_mpc(estim, transcription, Hp, Hc) nΔU = estim.model.nu*Hc PΔu = [I_nu_Hc spzeros(NT, nΔU, nZ - nΔU)] return PΔu @@ -796,13 +796,13 @@ function init_ZtoU( I_nu = sparse(Matrix{NT}(I, nu, nu)) PuDagger = Matrix{NT}(undef, nu*Hp, nu*Hc) for i=1:Hc - ni = nb[i] - Q_ni = repeat(I_nu, ni, 1) - iRows = (1:nu*ni) .+ @views nu*sum(nb[1:i-1]) + ni = nb[i] + Q_ni = repeat(I_nu, ni, 1) + iRows = (1:(nu*ni)) .+ @views nu*sum(nb[1:(i-1)]) PuDagger[iRows, :] = [repeat(Q_ni, 1, i) spzeros(nu*ni, nu*(Hc-i))] end PuDagger = sparse(PuDagger) - nZ = get_nZ(estim, transcription, Hp, Hc) + nZ = get_nZ_mpc(estim, transcription, Hp, Hc) Pu = [PuDagger spzeros(NT, nu*Hp, nZ - nu*Hc)] Tu = repeat(I_nu, Hp) return Pu, Tu @@ -835,7 +835,7 @@ if the condition number `cond(H̃) > warn_cond` and `transcription` is a `Single (`warn_cond=Inf` for no warning). """ function init_quadprog( - ::LinModel, transcription::TranscriptionMethod, weights::ControllerWeights, + ::LinModel, transcription::TranscriptionMethod, weights::ControllerWeights, Ẽ, P̃Δu, P̃u; warn_cond=1e6 ) M_Hp, Ñ_Hc, L_Hp = weights.M_Hp, weights.Ñ_Hc, weights.L_Hp @@ -856,8 +856,8 @@ function verify_cond(::SingleShooting, H̃, warn_cond) if !isinf(warn_cond) cond_H̃ = cond(H̃) cond_H̃ > warn_cond && @warn( - "The Hessian condition number cond_H̃ > $warn_cond. The optimization "* - "problem may be ill-conditioned.\n Consider changing the tunings, using "* + "The Hessian condition number cond_H̃ > $warn_cond. The optimization " * + "problem may be ill-conditioned.\n Consider changing the tunings, using " * "MultipleShooting, or using an optimizer more robust to this like DAQP.", cond_H̃, ) @@ -865,7 +865,7 @@ function verify_cond(::SingleShooting, H̃, warn_cond) return nothing end "No check if `transcription` is not a `SingleShooting`." -verify_cond(::TranscriptionMethod,_,_) = nothing +verify_cond(::TranscriptionMethod, _, _) = nothing """ init_defaultcon_mpc( @@ -885,37 +885,37 @@ Init `ControllerConstraint` struct with default parameters based on estimator `e Also return `P̃Δu`, `P̃u` and `Ẽ` matrices for the the augmented decision vector `Z̃`. """ function init_defaultcon_mpc( - estim::StateEstimator{NT}, + estim::StateEstimator{NT}, weights::ControllerWeights, transcription::TranscriptionMethod, - Hp, Hc, - PΔu, Pu, E, - ex̂, gx̂, jx̂, kx̂, vx̂, bx̂, + Hp, Hc, + PΔu, Pu, E, + ex̂, gx̂, jx̂, kx̂, vx̂, bx̂, ES, GS, JS, KS, VS, BS, Wy, Wu, Wd, Wr, - gc!::GCfunc = nothing, nc = 0 -) where {NT<:Real, GCfunc<:Union{Nothing, Function}} + gc!::GCfunc=nothing, nc=0 +) where {NT<:Real,GCfunc<:Union{Nothing,Function}} model = estim.model 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) - y0min, y0max = fill(convert(NT,-Inf), ny), fill(convert(NT,+Inf), ny) - wmin, wmax = fill(convert(NT,-Inf), nw), fill(convert(NT,+Inf), nw) - x̂0min, x̂0max = fill(convert(NT,-Inf), nx̂), fill(convert(NT,+Inf), nx̂) - c_umin, c_umax = fill(zero(NT), nu), fill(zero(NT), nu) - c_Δumin, c_Δumax = fill(zero(NT), nu), fill(zero(NT), nu) - c_ymin, c_ymax = fill(one(NT), ny), fill(one(NT), ny) - c_wmin, c_wmax = fill(one(NT), nw), fill(one(NT), nw) - c_x̂min, c_x̂max = fill(one(NT), nx̂), fill(one(NT), nx̂) - U0min, U0max, ΔUmin, ΔUmax, Y0min, Y0max, Wmin, Wmax = + u0min, u0max = fill(convert(NT, -Inf), nu), fill(convert(NT, +Inf), nu) + Δumin, Δumax = fill(convert(NT, -Inf), nu), fill(convert(NT, +Inf), nu) + y0min, y0max = fill(convert(NT, -Inf), ny), fill(convert(NT, +Inf), ny) + wmin, wmax = fill(convert(NT, -Inf), nw), fill(convert(NT, +Inf), nw) + x̂0min, x̂0max = fill(convert(NT, -Inf), nx̂), fill(convert(NT, +Inf), nx̂) + c_umin, c_umax = fill(zero(NT), nu), fill(zero(NT), nu) + c_Δumin, c_Δumax = fill(zero(NT), nu), fill(zero(NT), nu) + c_ymin, c_ymax = fill(one(NT), ny), fill(one(NT), ny) + c_wmin, c_wmax = fill(one(NT), nw), fill(one(NT), nw) + c_x̂min, c_x̂max = fill(one(NT), nx̂), fill(one(NT), nx̂) + U0min, U0max, ΔUmin, ΔUmax, Y0min, Y0max, Wmin, Wmax = repeat_constraints( Hp, Hc, u0min, u0max, Δumin, Δumax, y0min, y0max, wmin, wmax ) - C_umin, C_umax, C_Δumin, C_Δumax, C_ymin, C_ymax, C_wmin, C_wmax = + C_umin, C_umax, C_Δumin, C_Δumax, C_ymin, C_ymax, C_wmin, C_wmax = repeat_constraints( Hp, Hc, c_umin, c_umax, c_Δumin, c_Δumax, c_ymin, c_ymax, c_wmin, c_wmax ) @@ -923,55 +923,53 @@ function init_defaultcon_mpc( W̄u = sparse(repeatdiag(Wu, Hp+1)) W̄d = sparse(repeatdiag(Wd, Hp+1)) W̄r = sparse(repeatdiag(Wr, Hp+1)) - A_Umin, A_Umax, P̃u = relaxU(Pu, C_umin, C_umax, nϵ) + A_Umin, A_Umax, P̃u = relaxU(Pu, C_umin, C_umax, nϵ) A_ΔUmin, A_ΔUmax, P̃Δu = relaxΔU(PΔu, C_Δumin, C_Δumax, nϵ) - 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_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ϵ) Aeq, ẼS = augmentdefect(ES, nϵ) Z̃min, Z̃max = init_boxconstraint_mpc( estim, transcription, Hp, Hc, nϵ, - ΔUmin, ΔUmax, x̂0min, x̂0max, A_ΔUmin, A_ΔUmax, A_x̂min, A_x̂max + ΔUmin, ΔUmax, x̂0min, x̂0max, A_ΔUmin, A_ΔUmax, A_x̂min, A_x̂max ) i_b, i_g, A, Aeq, neq = init_matconstraint_mpc( model, transcription, Z̃min, Z̃max, nc, nϵ, - U0min, U0max, ΔUmin, ΔUmax, Y0min, Y0max, Wmin, Wmax, x̂0min, x̂0max, + U0min, U0max, ΔUmin, ΔUmax, Y0min, Y0max, Wmin, Wmax, x̂0min, x̂0max, A_Umin, A_Umax, A_ΔUmin, A_ΔUmax, A_Ymin, A_Ymax, A_Wmin, A_Wmax, A_x̂max, A_x̂min, Aeq ) - # dummy fx̂, Fw and FS vectors (updated just before optimization) + # dummy 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̂ , - ẼS , FS , GS , JS , KS , VS , BS , - Ẽw , Fw , W̄y , W̄u , W̄d , W̄r , nw , - U0min , U0max , ΔUmin , ΔUmax , - Y0min , Y0max , Wmin , Wmax , x̂0min , x̂0max , - Z̃min , Z̃max , - A_Umin , A_Umax , A_ΔUmin , A_ΔUmax , - A_Ymin , A_Ymax , A_Wmin , A_Wmax , A_x̂min , A_x̂max , - A , b , i_b , - Aeq , beq , - neq , - C_ymin , C_ymax , C_wmin , C_wmax , c_x̂min , c_x̂max , - i_g , - gc! , nc + con = ControllerConstraint{NT,GCfunc}( + ẽx̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂, + ẼS, FS, GS, JS, KS, VS, BS, + Ẽw, Fw, W̄y, W̄u, W̄d, W̄r, nw, + U0min, U0max, ΔUmin, ΔUmax, Y0min, Y0max, Wmin, Wmax, x̂0min, x̂0max, + Z̃min, Z̃max, + A_Umin, A_Umax, A_ΔUmin, A_ΔUmax, + A_Ymin, A_Ymax, A_Wmin, A_Wmax, A_x̂min, A_x̂max, + A, b, i_b, + Aeq, beq, + neq, + C_ymin, C_ymax, C_wmin, C_wmax, c_x̂min, c_x̂max, + i_g, + gc!, nc ) return con, nϵ, P̃Δu, P̃u, Ẽ end "Repeat predictive controller constraints over their respective horizons." function repeat_constraints(Hp, Hc, umin, umax, Δumin, Δumax, ymin, ymax, wmin, wmax) - Umin = repeat(umin, Hp) - Umax = repeat(umax, Hp) + Umin = repeat(umin, Hp) + Umax = repeat(umax, Hp) ΔUmin = repeat(Δumin, Hc) ΔUmax = repeat(Δumax, Hc) - Ymin = repeat(ymin, Hp) - Ymax = repeat(ymax, Hp) - Wmin = repeat(wmin, Hp+1) - Wmax = repeat(wmax, Hp+1) + Ymin = repeat(ymin, Hp) + Ymax = repeat(ymax, Hp) + Wmin = repeat(wmin, Hp+1) + Wmax = repeat(wmax, Hp+1) return Umin, Umax, ΔUmin, ΔUmax, Ymin, Ymax, Wmin, Wmax end @@ -1001,11 +999,11 @@ in which ``\mathbf{U_{min}}`` and ``\mathbf{U_{max}}`` vectors respectively cont function relaxU(Pu::AbstractMatrix{NT}, C_umin, C_umax, nϵ) where NT<:Real if nϵ == 1 # Z̃ = [Z; ϵ] # ϵ impacts Z → U conversion for constraint calculations: - A_Umin, A_Umax = -[Pu C_umin], [Pu -C_umax] + A_Umin, A_Umax = -[Pu C_umin], [Pu -C_umax] # ϵ has no impact on Z → U conversion for prediction calculations: P̃u = [Pu zeros(NT, size(Pu, 1))] else # Z̃ = Z (only hard constraints) - A_Umin, A_Umax = -Pu, Pu + A_Umin, A_Umax = -Pu, Pu P̃u = Pu end return A_Umin, A_Umax, P̃u @@ -1036,7 +1034,7 @@ It also returns the ``\mathbf{A}`` matrices for the inequality constraints: function relaxΔU(PΔu::AbstractMatrix{NT}, C_Δumin, C_Δumax, nϵ) where NT<:Real nZ = size(PΔu, 2) if nϵ == 1 # Z̃ = [Z; ϵ] - A_ΔUmin, A_ΔUmax = -[PΔu C_Δumin], [PΔu -C_Δumax] + A_ΔUmin, A_ΔUmax = -[PΔu C_Δumin], [PΔu -C_Δumax] P̃Δu = [PΔu zeros(NT, size(PΔu, 1), 1); zeros(NT, 1, size(PΔu, 2)) NT[1.0]] else # Z̃ = Z (only hard constraints) A_ΔUmin, A_ΔUmax = -PΔu, PΔu @@ -1074,12 +1072,12 @@ function relaxŶ(E::AbstractMatrix{NT}, C_ymin, C_ymax, nϵ) where NT<:Real C_ymin = C_ymax = zeros(NT, 0, 1) end # ϵ impacts predicted output constraint calculations: - A_Ymin, A_Ymax = -[E C_ymin], [E -C_ymax] + A_Ymin, A_Ymax = -[E C_ymin], [E -C_ymax] # ϵ has no impact on output predictions: - Ẽ = [E zeros(NT, size(E, 1), 1)] + Ẽ = [E zeros(NT, size(E, 1), 1)] else # Z̃ = Z (only hard constraints) Ẽ = E - A_Ymin, A_Ymax = -E, E + A_Ymin, A_Ymax = -E, E end return A_Ymin, A_Ymax, Ẽ end @@ -1140,18 +1138,18 @@ returns the ``\mathbf{Ẽ_w}`` matrix that appears in the custom constraint equa function relaxW(E::AbstractMatrix{NT}, Pu, Hp, W̄y, W̄u, C_wmin, C_wmax, nϵ) where {NT<:Real} nW = size(W̄y, 1) ny = size(W̄y, 2) ÷ (Hp + 1) - nu = size(Pu, 1) ÷ Hp + nu = size(Pu, 1) ÷ Hp if iszero(size(E, 1)) # model is not a LinModel, thus no Wy terms in the custom constraints: Wy_terms = zeros(NT, nW, size(E, 2)) else Wy_terms = W̄y*[zeros(NT, ny, size(E, 2)); E] end - Wu_terms = W̄u*[Pu; Pu[end-nu+1:end, :]] + Wu_terms = W̄u*[Pu; Pu[(end-nu+1):end, :]] Ew = Wy_terms + Wu_terms if nϵ == 1 # Z̃ = [Z; ϵ] # ϵ impacts custom constraint calculations: - A_Wmin, A_Wmax = -[Ew C_wmin], [Ew -C_wmax] + A_Wmin, A_Wmax = -[Ew C_wmin], [Ew -C_wmax] # ϵ has no impact on custom constraint predictions: Ẽw = [Ew zeros(NT, nW, 1)] else # Z̃ = Z (only hard constraints) @@ -1192,35 +1190,14 @@ function relaxterminal(ex̂::AbstractMatrix{NT}, c_x̂min, c_x̂max, nϵ) where # ϵ impacts terminal state constraint calculations: A_x̂min, A_x̂max = -[ex̂ c_x̂min], [ex̂ -c_x̂max] # ϵ has no impact on terminal state predictions: - ẽx̂ = [ex̂ zeros(NT, size(ex̂, 1), 1)] + ẽx̂ = [ex̂ zeros(NT, size(ex̂, 1), 1)] else # Z̃ = Z (only hard constraints) ẽx̂ = ex̂ - A_x̂min, A_x̂max = -ex̂, ex̂ + A_x̂min, A_x̂max = -ex̂, ex̂ end return A_x̂min, A_x̂max, ẽx̂ end -@doc raw""" - augmentdefect(ES, nϵ) -> Aeq, ẼS - -Augment defect equality constraints with slack variable ϵ if `nϵ == 1`. - -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_{eq} Z̃} = \mathbf{b_{eq}} = - \mathbf{F_S} -``` -""" -function augmentdefect(ES::AbstractMatrix{NT}, nϵ) where NT<:Real - if nϵ == 1 # Z̃ = [Z; ϵ] - ẼS = [ES zeros(NT, size(ES, 1), 1)] - else # Z̃ = Z (only hard constraints) - ẼS = ES - end - Aeq = ẼS - return Aeq, ẼS -end - """ init_boxconstraints_mpc( estim::StateEstimator, transcription::TranscriptionMethod, Hp, Hc, nϵ, @@ -1234,8 +1211,8 @@ function init_boxconstraint_mpc( ΔUmin, ΔUmax, x̂0min, x̂0max, A_ΔUmin, A_ΔUmax, A_x̂min, A_x̂max ) where {NT<:Real} nΔU, nX̂ = estim.model.nu*Hc, estim.nx̂*Hp - nZ̃ = get_nZ(estim, transcription, Hp, Hc) + nϵ - Z̃min, Z̃max = fill(convert(NT,-Inf), nZ̃), fill(convert(NT,+Inf), nZ̃) + nZ̃ = get_nZ_mpc(estim, transcription, Hp, Hc) + nϵ + Z̃min, Z̃max = fill(convert(NT, -Inf), nZ̃), fill(convert(NT, +Inf), nZ̃) nϵ > 0 && (Z̃min[end] = 0) if nϵ > 0 n_C_Δumin = @views A_ΔUmin[:, end] @@ -1276,20 +1253,20 @@ Current stochastic outputs ``\mathbf{ŷ_s}(k)`` comprises the measured outputs """ function init_stochpred(estim::InternalModel{NT}, Hp) where NT<:Real As, B̂s, Cs = estim.As, estim.B̂s, estim.Cs - ny = estim.model.ny + ny = estim.model.ny nxs = estim.nxs Ks = Matrix{NT}(undef, ny*Hp, nxs) Ps = Matrix{NT}(undef, ny*Hp, ny) for i = 1:Hp iRow = (1:ny) .+ ny*(i-1) Ms = Cs*As^(i-1)*B̂s - Ks[iRow,:] = Cs*As^i - Ms*Cs - Ps[iRow,:] = Ms + Ks[iRow, :] = Cs*As^i - Ms*Cs + Ps[iRow, :] = Ms end - return Ks, Ps + return Ks, Ps end "Return empty matrices if `estim` is not a [`InternalModel`](@ref)." -function init_stochpred(estim::StateEstimator{NT}, _ ) where NT<:Real +function init_stochpred(estim::StateEstimator{NT}, _) where NT<:Real return zeros(NT, 0, estim.nxs), zeros(NT, 0, estim.model.ny) end \ No newline at end of file diff --git a/src/controller/execute.jl b/src/controller/execute.jl index d25d799aa..3dec9e775 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -487,7 +487,7 @@ end Optimize the objective function of `mpc` [`PredictiveController`](@ref) and return the solution `Z̃`. -If first warm-starts the solver with [`set_warmstart!`](@ref). It then calls +If first warm-starts the solver with [`set_warmstart_mpc!`](@ref). It then calls `JuMP.optimize!(mpc.optim)` and extract the solution. A failed optimization prints an `@error` log in the REPL and returns the warm-start value. A failed optimization also prints [`getinfo`](@ref) results in the debug log [if activated](https://docs.julialang.org/en/v1/stdlib/Logging/#Example:-Enable-debug-level-messages). @@ -495,7 +495,7 @@ If first warm-starts the solver with [`set_warmstart!`](@ref). It then calls function optim_objective!(mpc::PredictiveController{NT}) where {NT<:Real} model, optim = mpc.estim.model, mpc.optim Z̃var::Vector{JuMP.VariableRef} = optim[:Z̃var] - Z̃s = set_warmstart!(mpc, mpc.transcription, Z̃var) + Z̃s = set_warmstart_mpc!(mpc, mpc.transcription, Z̃var) set_objective_linear_coef!(mpc, model, Z̃var) try JuMP.optimize!(optim) @@ -715,7 +715,7 @@ function setmodel_controller!(mpc::PredictiveController, uop_old, x̂op_old) weights = mpc.weights nu, ny, nd, Hp, Hc, nb = model.nu, model.ny, model.nd, mpc.Hp, mpc.Hc, mpc.nb optim, con = mpc.optim, mpc.con - nZ = get_nZ(estim, transcription, Hp, Hc) + nZ = get_nZ_mpc(estim, transcription, Hp, Hc) Pu = mpc.P̃u[:, 1:nZ] # --- prediction matrices --- E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat( diff --git a/src/controller/explicitmpc.jl b/src/controller/explicitmpc.jl index cd264cd8c..4f2c05365 100644 --- a/src/controller/explicitmpc.jl +++ b/src/controller/explicitmpc.jl @@ -65,7 +65,7 @@ struct ExplicitMPC{ # dummy vals (updated just before optimization): d0, D̂0, D̂e = zeros(NT, nd), zeros(NT, nd*Hp), zeros(NT, nd + nd*Hp) Uop, Yop, Dop = repeat(model.uop, Hp), repeat(model.yop, Hp), repeat(model.dop, Hp) - nZ̃ = get_nZ(estim, transcription, Hp, Hc) + nϵ + nZ̃ = get_nZ_mpc(estim, transcription, Hp, Hc) + nϵ Z̃ = zeros(NT, nZ̃) buffer = PredictiveControllerBuffer(estim, transcription, Hp, Hc, nϵ) mpc = new{NT, SE, CW}( diff --git a/src/controller/linmpc.jl b/src/controller/linmpc.jl index 1298c41d9..965c3f3a8 100644 --- a/src/controller/linmpc.jl +++ b/src/controller/linmpc.jl @@ -89,7 +89,7 @@ struct LinMPC{ # dummy vals (updated just before optimization): d0, D̂0, D̂e = zeros(NT, nd), zeros(NT, nd*Hp), zeros(NT, nd + nd*Hp) Uop, Yop, Dop = repeat(model.uop, Hp), repeat(model.yop, Hp), repeat(model.dop, Hp) - nZ̃ = get_nZ(estim, transcription, Hp, Hc) + nϵ + nZ̃ = get_nZ_mpc(estim, transcription, Hp, Hc) + nϵ Z̃ = zeros(NT, nZ̃) buffer = PredictiveControllerBuffer(estim, transcription, Hp, Hc, nϵ) mpc = new{NT, SE, CW, TM, JM}( diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 5b25ba213..d5ed95361 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -125,7 +125,7 @@ struct NonLinMPC{ d0, D̂0, D̂e = zeros(NT, nd), zeros(NT, nd*Hp), zeros(NT, nd + nd*Hp) Uop, Yop, Dop = repeat(model.uop, Hp), repeat(model.yop, Hp), repeat(model.dop, Hp) test_custom_function_mpc(NT, model, JE, gc!, nc, Uop, Yop, Dop, p) - nZ̃ = get_nZ(estim, transcription, Hp, Hc) + nϵ + nZ̃ = get_nZ_mpc(estim, transcription, Hp, Hc) + nϵ Z̃ = zeros(NT, nZ̃) buffer = PredictiveControllerBuffer(estim, transcription, Hp, Hc, nϵ) mpc = new{NT, SE, CW, TM, JM, GB, JB, HB, PT, JEfunc, GCfunc}( @@ -591,7 +591,7 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real hess = mpc.hessian transcription = mpc.transcription nu, ny, nx̂, nϵ = model.nu, model.ny, mpc.estim.nx̂, mpc.nϵ - nk = get_nk(model, transcription) + nk = get_nk_mpc(model, transcription) Hp, Hc = mpc.Hp, mpc.Hc i_g = findall(mpc.con.i_g) # convert to non-logical indices for non-allocating @views ng, ngi = length(mpc.con.i_g), sum(mpc.con.i_g) @@ -830,7 +830,7 @@ function get_nonlinobj_op(mpc::NonLinMPC, optim::JuMP.GenericModel{JNT}) where J transcription = mpc.transcription grad, hess = mpc.gradient, mpc.hessian nu, ny, nx̂, nϵ = model.nu, model.ny, mpc.estim.nx̂, mpc.nϵ - nk = get_nk(model, transcription) + nk = get_nk_mpc(model, transcription) Hp, Hc = mpc.Hp, mpc.Hc ng = length(mpc.con.i_g) nc, neq = mpc.con.nc, mpc.con.neq @@ -956,7 +956,7 @@ function get_nonlincon_oracle(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JN transcription = mpc.transcription jac, hess = mpc.jacobian, mpc.hessian nu, ny, nx̂, nϵ = model.nu, model.ny, mpc.estim.nx̂, mpc.nϵ - nk = get_nk(model, transcription) + nk = get_nk_mpc(model, transcription) Hp, Hc = mpc.Hp, mpc.Hc i_g = findall(mpc.con.i_g) # convert to non-logical indices for non-allocating @views ng, ngi = length(mpc.con.i_g), sum(mpc.con.i_g) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index c47a9a554..e1b9db70a 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1,324 +1,17 @@ -const COLLOCATION_NODE_TYPE = Float64 - -""" -Abstract supertype of all transcription methods of [`PredictiveController`](@ref). - -The module currently supports [`SingleShooting`](@ref), [`MultipleShooting`](@ref), -[`TrapezoidalCollocation`](@ref) and [`OrthogonalCollocation`](@ref) transcription methods. -""" -abstract type TranscriptionMethod end -abstract type ShootingMethod <: TranscriptionMethod end -abstract type CollocationMethod <: TranscriptionMethod end - -@doc raw""" - SingleShooting() - -Construct a direct single shooting [`TranscriptionMethod`](@ref). - -The decision variable in the optimization problem is (excluding the slack ``ϵ`` and without -any custom move blocking): -```math -\mathbf{Z} = \mathbf{ΔU} = \begin{bmatrix} - \mathbf{Δu}(k+0) \\ - \mathbf{Δu}(k+1) \\ - \vdots \\ - \mathbf{Δu}(k+H_c-1) \end{bmatrix} -``` -This method computes the predictions by calling the augmented discrete-time model -recursively over the prediction horizon ``H_p`` in the objective function, or by updating -the linear coefficients of the quadratic optimization for [`LinModel`](@ref). It is -generally more efficient for small control horizon ``H_c``, stable and mildly nonlinear -plant model/constraints. -""" -struct SingleShooting <: ShootingMethod end - -@doc raw""" - MultipleShooting(; f_threads=false, h_threads=false) - -Construct a direct multiple shooting [`TranscriptionMethod`](@ref). - -The decision variable is (excluding ``ϵ``): -```math -\mathbf{Z} = \begin{bmatrix} \mathbf{ΔU} \\ \mathbf{X̂_0} \end{bmatrix} -``` -thus it also includes the predicted states, expressed as deviation vectors from the -operating point ``\mathbf{x̂_{op}}`` (see [`augment_model`](@ref)): -```math -\mathbf{X̂_0} = \mathbf{X̂ - X̂_{op}} = \begin{bmatrix} - \mathbf{x̂}_i(k+1) - \mathbf{x̂_{op}} \\ - \mathbf{x̂}_i(k+2) - \mathbf{x̂_{op}} \\ - \vdots \\ - \mathbf{x̂}_i(k+H_p) - \mathbf{x̂_{op}} \end{bmatrix} -``` -where ``\mathbf{x̂}_i(k+j)`` is the state prediction for time ``k+j``, estimated by the -observer at time ``i=k`` or ``i=k-1`` depending on its `direct` flag. Note that -``\mathbf{X̂_0 = X̂}`` if the operating point is zero, which is typically the case in practice -for [`NonLinModel`](@ref). - -This transcription computes the predictions by calling the augmented discrete-time model -in the equality constraint function recursively over ``H_p``, or by updating the linear -equality constraint vector for [`LinModel`](@ref). It is generally more efficient for large -control horizon ``H_c``, unstable or highly nonlinear models/constraints. Multithreading -with `f_threads` or `h_threads` keyword arguments can be advantageous if ``\mathbf{f}`` or -``\mathbf{h}`` in the [`NonLinModel`](@ref) is expensive to evaluate, respectively. - -Sparse optimizers like `OSQP` or `Ipopt` and sparse Jacobian computations are recommended -for this transcription method. -""" -struct MultipleShooting <: ShootingMethod - f_threads::Bool - h_threads::Bool - function MultipleShooting(; f_threads=false, h_threads=false) - return new(f_threads, h_threads) - end -end - -@doc raw""" - TrapezoidalCollocation(h::Int=0; f_threads=false, h_threads=false) - -Construct an implicit trapezoidal [`TranscriptionMethod`](@ref) with `h`th order hold. - -This is the simplest collocation method. It supports continuous-time [`NonLinModel`](@ref)s -only. The decision variables are the same as for [`MultipleShooting`](@ref), hence similar -computational costs. See the same docstring for descriptions of `f_threads` and `h_threads` -keywords. The `h` argument is `0` or `1`, for piecewise constant or linear manipulated -inputs ``\mathbf{u}`` (`h=1` is slightly less expensive). Note that the various [`DiffSolver`](@ref) -here assume zero-order hold, so `h=1` will induce a plant-model mismatch if the plant is -simulated with these solvers. Measured disturbances ``\mathbf{d}`` are piecewise linear. - -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. See Extended Help for more details. - -!!! warning - 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). - -Sparse optimizers like `Ipopt` and sparse Jacobian computations are recommended for this -transcription method. - -# Extended Help -!!! details "Extended Help" - 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. See [`con_nonlinprogeq!`](@ref) - for more details. -""" -struct TrapezoidalCollocation <: CollocationMethod - h::Int - no::Int - f_threads::Bool - h_threads::Bool - function TrapezoidalCollocation(h::Int=0; f_threads=false, h_threads=false) - if !(h == 0 || h == 1) - throw(ArgumentError("h argument must be 0 or 1 for TrapezoidalCollocation.")) - end - no = 2 # 2 collocation points per intervals for trapezoidal rule - return new(h, no, f_threads, h_threads) - end -end - - -@doc raw""" - OrthogonalCollocation( - h::Int=0, no::Int=3; f_threads=false, h_threads=false, roots=:gaussradau - ) - -Construct an orthogonal collocation on finite elements [`TranscriptionMethod`](@ref). - -Also known as pseudo-spectral method. It supports continuous-time [`NonLinModel`](@ref)s -only. The `h` argument is the hold order for ``\mathbf{u}`` (`0` or `1`), and the `no` -argument, the number of collocation points ``n_o``. The decision variable is similar to -[`MultipleShooting`](@ref), but it also includes the collocation points: -```math -\mathbf{Z} = \begin{bmatrix} \mathbf{ΔU} \\ \mathbf{X̂_0} \\ \mathbf{K} \end{bmatrix} -``` -where ``\mathbf{K}`` encompasses all the intermediate stages of the deterministic states -(the first `nx` elements of ``\mathbf{x̂}``): -```math -\mathbf{K} = \begin{bmatrix} - \mathbf{k}_{1}(k+0) \\ - \mathbf{k}_{2}(k+0) \\ - \vdots \\ - \mathbf{k}_{n_o}(k+0) \\ - \mathbf{k}_{1}(k+1) \\ - \mathbf{k}_{2}(k+1) \\ - \vdots \\ - \mathbf{k}_{n_o}(k+H_p-1) \end{bmatrix} -``` -and ``\mathbf{k}_i(k+j)`` is the deterministic state prediction for the ``i``th collocation -point at the ``j``th stage/interval/finite element (details in Extended Help). The `roots` -keyword argument is either `:gaussradau` or `:gausslegendre`, for Gauss-Radau or -Gauss-Legendre quadrature, respectively. See [`MultipleShooting`](@ref) docstring for -descriptions of `f_threads` and `h_threads` keywords. This transcription computes the -predictions by enforcing the collocation and continuity constraints at the collocation -points. It is efficient for highly stiff systems, but generally more expensive than the -other methods for non-stiff systems. See Extended Help for more details. - -!!! warning - The built-in [`StateEstimator`](@ref) will still use the `solver` provided at the - construction of the [`NonLinModel`](@ref) to estimate the plant states, not orthogonal - collocation (see `supersample` option of [`RungeKutta`](@ref) for stiff systems). - -Sparse optimizers like `Ipopt` and sparse Jacobian computations are highly recommended for -this transcription method (sparser formulation than [`MultipleShooting`](@ref)). - -# Extended Help -!!! details "Extended Help" - As explained in the Extended Help of [`TrapezoidalCollocation`](@ref), the stochastic - states are left out of the ``\mathbf{K}`` vector since collocation methods require - continuous-time dynamics and the stochastic model is discrete. - - The collocation points are located at the roots of orthogonal polynomials, which is - "optimal" for approximating the state trajectories with polynomials of degree ``n_o``. - The method then enforces the system dynamics at these points. The Gauss-Legendre scheme - is more accurate than Gauss-Radau but only A-stable, while the latter being L-stable. - See [`con_nonlinprogeq!`](@ref) for implementation details. -""" -struct OrthogonalCollocation <: CollocationMethod - h::Int - no::Int - f_threads::Bool - h_threads::Bool - τ::Vector{COLLOCATION_NODE_TYPE} - function OrthogonalCollocation( - h::Int=0, no::Int=3; f_threads=false, h_threads=false, roots=:gaussradau - ) - if !(h == 0 || h == 1) - throw(ArgumentError("h argument must be 0 or 1 for OrthogonalCollocation.")) - end - if roots==:gaussradau - x, _ = FastGaussQuadrature.gaussradau(COLLOCATION_NODE_TYPE, no) - # we reverse the nodes to include the τ=1.0 node: - τ = (reverse(-x) .+ 1) ./ 2 - elseif roots==:gausslegendre - x, _ = FastGaussQuadrature.gausslegendre(COLLOCATION_NODE_TYPE, no) - # converting [-1, 1] to [0, 1] (see - # https://en.wikipedia.org/wiki/Gaussian_quadrature#Change_of_interval): - τ = (x .+ 1) ./ 2 - else - throw(ArgumentError("roots argument must be :gaussradau or :gausslegendre.")) - end - return new(h, no, f_threads, h_threads, τ) - end -end - -@doc raw""" - init_orthocolloc(model::SimModel, transcription::OrthogonalCollocation) -> Mo, Co, λo - -Init the differentiation and continuity matrices for [`OrthogonalCollocation`](@ref). - -Introducing ``τ_i``, the ``i``th root of the orthogonal polynomial normalized to the -interval ``[0, 1]``, and ``τ_0=0``, each state trajectories are approximated by a distinct -polynomial of degree ``n_o``. The differentiation matrix ``\mathbf{M_o}``, continuity -matrix ``\mathbf{C_o}`` and continuity coefficient ``λ_o`` are pre-computed with: -```math -\begin{aligned} - \mathbf{P_o} &= \begin{bmatrix} - τ_1^1 \mathbf{I} & τ_1^2 \mathbf{I} & \cdots & τ_1^{n_o} \mathbf{I} \\ - τ_2^1 \mathbf{I} & τ_2^2 \mathbf{I} & \cdots & τ_2^{n_o} \mathbf{I} \\ - \vdots & \vdots & \ddots & \vdots \\ - τ_{n_o}^1 \mathbf{I} & τ_{n_o}^2 \mathbf{I} & \cdots & τ_{n_o}^{n_o} \mathbf{I} \end{bmatrix} \\ - \mathbf{Ṗ_o} &= \begin{bmatrix} - τ_1^0 \mathbf{I} & 2τ_1^1 \mathbf{I} & \cdots & n_o τ_1^{n_o-1} \mathbf{I} \\ - τ_2^0 \mathbf{I} & 2τ_2^1 \mathbf{I} & \cdots & n_o τ_2^{n_o-1} \mathbf{I} \\ - \vdots & \vdots & \ddots & \vdots \\ - τ_{n_o}^0 \mathbf{I} & 2τ_{n_o}^1 \mathbf{I} & \cdots & n_o τ_{n_o}^{n_o-1} \mathbf{I} \end{bmatrix} \\ - \mathbf{M_o} &= \frac{1}{T_s} \mathbf{Ṗ_o} \mathbf{P_o}^{-1} \\ - \mathbf{C_o} &= \begin{bmatrix} - L_1(1) \mathbf{I} & L_2(1) \mathbf{I} & \cdots & L_{n_o}(1) \mathbf{I} \end{bmatrix} \\ - λ_o &= L_0(1) -\end{aligned} -``` -where ``\mathbf{P_o}`` is a matrix to evaluate the polynamial values w/o the coefficients -and Y-intercept, and ``\mathbf{Ṗ_o}``, to evaluate its derivatives. The Lagrange polynomial -``L_j(τ)`` bases are defined as: -```math -L_j(τ) = \prod_{i=0, i≠j}^{n_o} \frac{τ - τ_i}{τ_j - τ_i} -``` - -The collocation constraints are nonlinear, but the defects of deterministic states -``\mathbf{x̂_d}`` for the continuity constraints are in fact linear equality constraints: -```math -\mathbf{s_c}(k+j+1) = \mathbf{0} = - \mathbf{C_o} \begin{bmatrix} - \mathbf{k}_1(k+j) \\ - \mathbf{k}_2(k+j) \\ - \vdots \\ - \mathbf{k}_{n_o}(k+j) \end{bmatrix} - + λ_o \mathbf{x̂_d}(k+j) - \mathbf{x̂_d}(k+j+1) -``` -for ``j = 0, 1, ... , H_p-1``. The ``\mathbf{k}_i`` and ``\mathbf{x̂_d}`` vectors are all -directly extracted from the decision variable `Z̃`. -""" -function init_orthocolloc( - model::SimModel{NT}, transcription::OrthogonalCollocation -) where {NT<:Real} - nx, no = model.nx, transcription.no - τ = transcription.τ - Po = Matrix{NT}(undef, nx*no, nx*no) # polynomial matrix (w/o the Y-intercept term) - Ṗo = Matrix{NT}(undef, nx*no, nx*no) # polynomial derivative matrix - I_nx = I(nx) - for j=1:no, i=1:no - iRows = (1:nx) .+ nx*(i-1) - iCols = (1:nx) .+ nx*(j-1) - Po[iRows, iCols] = (τ[i]^j)*I_nx - Ṗo[iRows, iCols] = (j*τ[i]^(j-1))*I_nx - end - Mo = sparse((Ṗo/Po)/model.Ts) - Co = Matrix{NT}(undef, nx, nx*no) - for j=1:no - iCols = (1:nx) .+ nx*(j-1) - Co[:, iCols] = lagrange_end(j, transcription)*I_nx - end - Co = sparse(Co) - λo = lagrange_end(0, transcription) - return Mo, Co, λo -end -"Return empty sparse matrices and `NaN` for other [`TranscriptionMethod`](@ref)" -init_orthocolloc(::SimModel, ::TranscriptionMethod) = spzeros(0,0), spzeros(0,0), NaN - -"Evaluate the Lagrange basis polynomial ``L_j`` at `τ=1`." -function lagrange_end(j, transcription::OrthogonalCollocation) - τ_val = 1 - τ_values = [0; transcription.τ] # including the τ=0 node for the Lagrange polynomials - j_index = j + 1 # because of the τ=0 node - τj = τ_values[j_index] - Lj = 1 - for i in eachindex(τ_values) - i == j_index && continue - τi = τ_values[i] - Lj *= (τ_val - τi)/(τj - τi) - end - return Lj -end - -function validate_transcription(::LinModel, ::CollocationMethod) - throw(ArgumentError("Collocation methods are not supported for LinModel.")) - return nothing -end -function validate_transcription(::NonLinModel{<:Real, <:EmptySolver}, ::CollocationMethod) - throw(ArgumentError("Collocation methods require continuous-time NonLinModel.")) - return nothing -end -validate_transcription(::SimModel, ::TranscriptionMethod) = nothing - "Get the number of elements in the optimization decision vector `Z`." -function get_nZ(estim::StateEstimator, ::SingleShooting, Hp, Hc) +function get_nZ_mpc(estim::StateEstimator, ::SingleShooting, _ , Hc) return estim.model.nu*Hc end -function get_nZ(estim::StateEstimator, ::TranscriptionMethod, Hp, Hc) +function get_nZ_mpc(estim::StateEstimator, ::TranscriptionMethod, Hp, Hc) return estim.model.nu*Hc + estim.nx̂*Hp end -function get_nZ(estim::StateEstimator, transcription::OrthogonalCollocation, Hp, Hc) +function get_nZ_mpc(estim::StateEstimator, transcription::OrthogonalCollocation, Hp, Hc) return estim.model.nu*Hc + estim.nx̂*Hp + estim.model.nx*transcription.no*Hp end "Get length of the `k` vector with all the solver intermediate steps or all the collocation pts." -get_nk(model::SimModel, ::ShootingMethod) = model.nk -get_nk(model::SimModel, transcription::CollocationMethod) = model.nx*transcription.no +get_nk_mpc(model::SimModel, ::ShootingMethod) = model.nk +get_nk_mpc(model::SimModel, transcription::CollocationMethod) = model.nx*transcription.no @doc raw""" init_predmat( @@ -461,7 +154,7 @@ function init_predmat( V = Matrix{NT}(undef, Hp*ny, nu) Q!(V, 0, Hp, 0) # --- decision variables Z --- - nZ = get_nZ(estim, transcription, Hp, Hc) + nZ = get_nZ_mpc(estim, transcription, Hp, Hc) ex̂ = Matrix{NT}(undef, nx̂, nZ) E = zeros(NT, Hp*ny, nZ) for j=0:Hc-1 @@ -561,7 +254,7 @@ function init_predmat( model::NonLinModel, estim::StateEstimator{NT}, transcription::SingleShooting, Hp, Hc, _ ) where {NT<:Real} nu, nx̂, nd = model.nu, estim.nx̂, model.nd - nZ = get_nZ(estim, transcription, Hp, Hc) + nZ = get_nZ_mpc(estim, transcription, Hp, Hc) E = zeros(NT, 0, nZ) G = zeros(NT, 0, nd) J = zeros(NT, 0, nd*Hp) @@ -595,7 +288,7 @@ function init_predmat( nu, nx̂, nd = model.nu, estim.nx̂, model.nd nΔU = nu*Hc nX̂0 = nx̂*Hp - nZ = get_nZ(estim, transcription, Hp, Hc) + nZ = get_nZ_mpc(estim, transcription, Hp, Hc) E = zeros(NT, 0, nZ) G = zeros(NT, 0, nd) J = zeros(NT, 0, nd*Hp) @@ -868,7 +561,7 @@ function init_defectmat_orthocolloc( Hp, Hc, Co, λo, As, nxs ) where {NT<:Real} nu, nx, nd, nx̂ = model.nu, model.nx, model.nd, estim.nx̂ - nk = get_nk(model, transcription) + nk = get_nk_mpc(model, transcription) λo_I = λo*I(nx) # --- current state estimates x̂0 --- KS = zeros(NT, nx̂*Hp, nx̂) @@ -939,7 +632,7 @@ function init_defectmat_empty( ) where {NT<:Real} model = estim.model nx̂, nu, nd = estim.nx̂, model.nu, model.nd - nZ = get_nZ(estim, transcription, Hp, Hc) + nZ = get_nZ_mpc(estim, transcription, Hp, Hc) ES = zeros(NT, 0, nZ) GS = zeros(NT, 0, nd) JS = zeros(NT, 0, nd*Hp) @@ -964,7 +657,7 @@ The linear and nonlinear constraints are respectively defined as: \mathbf{A Z̃ } &≤ \mathbf{b} \\ \mathbf{A_{eq} Z̃} &= \mathbf{b_{eq}} \\ \mathbf{g(Z̃)} &≤ \mathbf{0} \\ - \mathbf{g_{eq}(Z̃)} &= \mathbf{0} \\ + \mathbf{g_{eq}(Z̃)} &= \mathbf{0} \end{aligned} ``` The argument `nc` is the number of custom nonlinear inequality constraints in @@ -1286,7 +979,7 @@ linconstrainteq!(::PredictiveController, ::SimModel, ::StateEstimator, ::Sing linconstrainteq!(::PredictiveController, ::NonLinModel, ::InternalModel, ::SingleShooting) = nothing @doc raw""" - set_warmstart!(mpc::PredictiveController, ::SingleShooting, Z̃var) -> Z̃s + set_warmstart_mpc!(mpc::PredictiveController, ::SingleShooting, Z̃var) -> Z̃s Set and return the warm-start value of `Z̃var` for [`SingleShooting`](@ref) transcription. @@ -1296,6 +989,7 @@ If supported by `mpc.optim`, it warm-starts the solver at: \mathbf{Δu}(k+0|k-1) \\ \mathbf{Δu}(k+1|k-1) \\ \vdots \\ + \mathbf{Δu}(k+H_c-3|k-1) \\ \mathbf{Δu}(k+H_c-2|k-1) \\ \mathbf{0} \\ ϵ_{k-1} @@ -1304,7 +998,7 @@ If supported by `mpc.optim`, it warm-starts the solver at: where ``\mathbf{Δu}(k+j|k-1)`` is the input increment for time ``k+j`` computed at the last control period ``k-1``, and ``ϵ_{k-1}``, the slack variable of the last control period. """ -function set_warmstart!(mpc::PredictiveController, ::SingleShooting, Z̃var) +function set_warmstart_mpc!(mpc::PredictiveController, ::SingleShooting, Z̃var) nu, Hc, Z̃s = mpc.estim.model.nu, mpc.Hc, mpc.buffer.Z̃ nΔU = nu*Hc # --- input increments ΔU --- @@ -1317,7 +1011,7 @@ function set_warmstart!(mpc::PredictiveController, ::SingleShooting, Z̃var) end @doc raw""" - set_warmstart!(mpc::PredictiveController, ::OrthogonalCollocation, Z̃var) -> Z̃s + set_warmstart_mpc!(mpc::PredictiveController, ::OrthogonalCollocation, Z̃var) -> Z̃s Set and return the warm-start value of `Z̃var` for [`OrthogonalCollocation`](@ref). @@ -1327,16 +1021,19 @@ It warm-starts the solver at: \mathbf{Δu}(k+0|k-1) \\ \mathbf{Δu}(k+1|k-1) \\ \vdots \\ + \mathbf{Δu}(k+H_c-3|k-1) \\ \mathbf{Δu}(k+H_c-2|k-1) \\ \mathbf{0} \\ \mathbf{x̂_0}(k+1|k-1) \\ \mathbf{x̂_0}(k+2|k-1) \\ \vdots \\ + \mathbf{x̂_0}(k+H_p-2|k-1) \\ \mathbf{x̂_0}(k+H_p-1|k-1) \\ \mathbf{x̂_0}(k+H_p-1|k-1) \\ \mathbf{k}(k+0|k-1) \\ \mathbf{k}(k+1|k-1) \\ \vdots \\ + \mathbf{k}(k+H_p-3|k-1) \\ \mathbf{k}(k+H_p-2|k-1) \\ \mathbf{k}(k+H_p-2|k-1) \\ ϵ_{k-1} @@ -1347,12 +1044,12 @@ last control period ``k-1``, expressed as a deviation from the operating point ``\mathbf{x̂_{op}}``. The vector ``\mathbf{k}(k+j|k-1)`` include the ``n_o`` intermediate stage predictions for the interval ``k+j``, and is also computed at the last control period. """ -function set_warmstart!( +function set_warmstart_mpc!( mpc::PredictiveController, transcription::OrthogonalCollocation, Z̃var ) nu, nx̂ = mpc.estim.model.nu, mpc.estim.nx̂ Hp, Hc, Z̃s = mpc.Hp, mpc.Hc, mpc.buffer.Z̃ - nk = get_nk(mpc.estim.model, transcription) + nk = get_nk_mpc(mpc.estim.model, transcription) nΔU, nX̂, nK = nu*Hc, nx̂*Hp, nk*Hp # --- input increments ΔU --- Z̃s[1:(nΔU-nu)] .= @views mpc.Z̃[(nu+1):(nΔU)] @@ -1370,7 +1067,7 @@ function set_warmstart!( end @doc raw""" - set_warmstart!(mpc::PredictiveController, ::TranscriptionMethod, Z̃var) -> Z̃s + set_warmstart_mpc!(mpc::PredictiveController, ::TranscriptionMethod, Z̃var) -> Z̃s Set and return the warm-start value of `Z̃var` for other [`TranscriptionMethod`](@ref). @@ -1380,21 +1077,20 @@ It warm-starts the solver at: \mathbf{Δu}(k+0|k-1) \\ \mathbf{Δu}(k+1|k-1) \\ \vdots \\ + \mathbf{Δu}(k+H_c-3|k-1) \\ \mathbf{Δu}(k+H_c-2|k-1) \\ \mathbf{0} \\ \mathbf{x̂_0}(k+1|k-1) \\ \mathbf{x̂_0}(k+2|k-1) \\ \vdots \\ + \mathbf{x̂_0}(k+H_p-2|k-1) \\ \mathbf{x̂_0}(k+H_p-1|k-1) \\ \mathbf{x̂_0}(k+H_p-1|k-1) \\ ϵ_{k-1} \end{bmatrix} ``` -where ``\mathbf{x̂_0}(k+j|k-1)`` is the predicted state for time ``k+j`` computed at the -last control period ``k-1``, expressed as a deviation from the operating point -``\mathbf{x̂_{op}}``. """ -function set_warmstart!(mpc::PredictiveController, ::TranscriptionMethod, Z̃var) +function set_warmstart_mpc!(mpc::PredictiveController, ::TranscriptionMethod, Z̃var) nu, nx̂, Hp, Hc, Z̃s = mpc.estim.model.nu, mpc.estim.nx̂, mpc.Hp, mpc.Hc, mpc.buffer.Z̃ nΔU, nX̂ = nu*Hc, nx̂*Hp # --- input increments ΔU --- @@ -1409,6 +1105,7 @@ function set_warmstart!(mpc::PredictiveController, ::TranscriptionMethod, Z̃var return Z̃s end +"Get the input increments `ΔŨ` augmented with the slack `ϵ` from the decision vector `Z̃`." getΔŨ!(ΔŨ, ::PredictiveController, ::SingleShooting, Z̃) = (ΔŨ .= Z̃) function getΔŨ!(ΔŨ, mpc::PredictiveController, ::TranscriptionMethod, Z̃) # avoid explicit matrix multiplication with mpc.P̃Δu for performance: @@ -1417,6 +1114,8 @@ function getΔŨ!(ΔŨ, mpc::PredictiveController, ::TranscriptionMethod, Z̃) mpc.nϵ == 1 && (ΔŨ[end] = Z̃[end]) return ΔŨ end + +"Get the manipulated input `U0` from the decision vector `Z̃`." getU0!(U0, mpc::PredictiveController, Z̃) = (mul!(U0, mpc.P̃u, Z̃) .+ mpc.Tu_lastu0) @doc raw""" @@ -1704,7 +1403,7 @@ function con_nonlinprogeq!( nΔU, nX̂ = nu*Hc, nx̂*Hp f_threads = transcription.f_threads Ts, p = model.Ts, model.p - nk = get_nk(model, transcription) + nk = get_nk_mpc(model, transcription) D̂0 = mpc.D̂0 X̂0_Z̃ = @views Z̃[(nΔU+1):(nΔU+nX̂)] disturbedinput!(Û0, mpc, mpc.estim, U0, X̂0_Z̃) @@ -1799,7 +1498,7 @@ function con_nonlinprogeq!( f_threads = transcription.f_threads p = model.p Mo, no, τ = mpc.Mo, transcription.no, transcription.τ - nk = get_nk(model, transcription) + nk = get_nk_mpc(model, transcription) 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̂ diff --git a/src/estimator/construct.jl b/src/estimator/construct.jl index 255c6fe4d..20006619f 100644 --- a/src/estimator/construct.jl +++ b/src/estimator/construct.jl @@ -5,6 +5,7 @@ struct StateEstimatorBuffer{NT<:Real} x̂ ::Vector{NT} Z̃ ::Vector{NT} V̂ ::Vector{NT} + Ŵ ::Vector{NT} X̂ ::Vector{NT} P̂ ::Matrix{NT} Q̂ ::Matrix{NT} @@ -17,17 +18,23 @@ struct StateEstimatorBuffer{NT<:Real} end @doc raw""" - StateEstimatorBuffer{NT}(nu::Int, nx̂::Int, nym::Int, ny::Int, nd::Int, nk::Int=0) + StateEstimatorBuffer{NT}( + nu::Int, nx̂::Int, nym::Int, ny::Int, nd::Int, nk::Int=0 + He::Int=0, nŵ::Int=nx̂, nε::Int=0, + transcription::TranscriptionMethod = SingleShooting() + ) Create a buffer for `StateEstimator` objects for estimated states and measured outputs. The buffer is used to store intermediate results during estimation without allocating. """ function StateEstimatorBuffer{NT}( - nu::Int, nx̂::Int, nym::Int, ny::Int, nd::Int, nk::Int=0, He::Int=0, nε::Int=0 + nu::Int, nx̂::Int, nym::Int, ny::Int, nd::Int, nk::Int=0, + He::Int=0, nŵ::Int=nx̂, nε::Int=0, + transcription::TranscriptionMethod = SingleShooting() ) where NT <: Real - nŵ = nx̂ - nZ̃ = nx̂ + nŵ*He + nε + nZ̃ = nε + get_nZ_mhe(transcription, He, nx̂, nŵ) + nŴ = nŵ*He nV̂ = nym*He nX̂ = nx̂*He u = Vector{NT}(undef, nu) @@ -36,6 +43,7 @@ function StateEstimatorBuffer{NT}( x̂ = Vector{NT}(undef, nx̂) Z̃ = Vector{NT}(undef, nZ̃) V̂ = Vector{NT}(undef, nV̂) + Ŵ = Vector{NT}(undef, nŴ) X̂ = Vector{NT}(undef, nX̂) P̂ = Matrix{NT}(undef, nx̂, nx̂) Q̂ = Matrix{NT}(undef, nx̂, nx̂) @@ -45,7 +53,7 @@ function StateEstimatorBuffer{NT}( ŷ = Vector{NT}(undef, ny) d = Vector{NT}(undef, nd) empty = Vector{NT}(undef, 0) - return StateEstimatorBuffer{NT}(u, û, k, x̂, Z̃, V̂, X̂, P̂, Q̂, R̂, K̂, ym, ŷ, d, empty) + return StateEstimatorBuffer{NT}(u, û, k, x̂, Z̃, V̂, Ŵ, X̂, P̂, Q̂, R̂, K̂, ym, ŷ, d, empty) end "Include all the covariance matrices for the Kalman filters and moving horizon estimator." diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index a23a01ac5..bd9c872f0 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -844,7 +844,7 @@ step is skipped if `estim.direct == true` since it's already done by the user. ```math \begin{aligned} \mathbf{X̂}_{k-1}(k) &= \bigg[\begin{matrix} \mathbf{x̂}_{k-1}(k) & \mathbf{x̂}_{k-1}(k) & \cdots & \mathbf{x̂}_{k-1}(k) \end{matrix}\bigg] + \bigg[\begin{matrix} \mathbf{0} & γ \sqrt{\mathbf{P̂}_{k-1}(k)} & -γ \sqrt{\mathbf{P̂}_{k-1}(k)} \end{matrix}\bigg] \\ - \mathbf{Ŷ^m}(k) &= \bigg[\begin{matrix} \mathbf{ĥ^m}\Big( \mathbf{X̂}_{k-1}^{1}(k) \Big) & \mathbf{ĥ^m}\Big( \mathbf{X̂}_{k-1}^{2}(k) \Big) & \cdots & \mathbf{ĥ^m}\Big( \mathbf{X̂}_{k-1}^{n_σ}(k) \Big) \end{matrix}\bigg] \\ + \mathbf{Ŷ^m}(k) &= \bigg[\begin{matrix} \mathbf{ĥ^m}\Big( \mathbf{X̂}_{k-1}^{1}(k), \mathbf{d}(k) \Big) & \mathbf{ĥ^m}\Big( \mathbf{X̂}_{k-1}^{2}(k), \mathbf{d}(k) \Big) & \cdots & \mathbf{ĥ^m}\Big( \mathbf{X̂}_{k-1}^{n_σ}(k), \mathbf{d}(k) \Big) \end{matrix}\bigg] \\ \mathbf{ŷ^m}(k) &= \mathbf{Ŷ^m}(k) \mathbf{m̂} \\ \mathbf{X̄}_{k-1}(k) &= \begin{bmatrix} \mathbf{X̂}_{k-1}^{1}(k) - \mathbf{x̂}_{k-1}(k) & \mathbf{X̂}_{k-1}^{2}(k) - \mathbf{x̂}_{k-1}(k) & \cdots & \mathbf{X̂}_{k-1}^{n_σ}(k) - \mathbf{x̂}_{k-1}(k) \end{bmatrix} \\ \mathbf{Ȳ^m}(k) &= \begin{bmatrix} \mathbf{Ŷ^m}^{1}(k) - \mathbf{ŷ^m}(k) & \mathbf{Ŷ^m}^{2}(k) - \mathbf{ŷ^m}(k) & \cdots & \mathbf{Ŷ^m}^{n_σ}(k) - \mathbf{ŷ^m}(k) \end{bmatrix} \\ diff --git a/src/estimator/mhe.jl b/src/estimator/mhe.jl index 3e1acfd7a..ddf475e59 100644 --- a/src/estimator/mhe.jl +++ b/src/estimator/mhe.jl @@ -1,5 +1,6 @@ include("mhe/construct.jl") include("mhe/execute.jl") +include("mhe/transcription.jl") "Return estimation horizon He and slack variables length nε for `MovingHorizonEstimator`." get_other_dims(estim::MovingHorizonEstimator) = (estim.He, estim.nε) @@ -7,6 +8,7 @@ get_other_dims(estim::MovingHorizonEstimator) = (estim.He, estim.nε) "Print optimizer and other information for `MovingHorizonEstimator`." function print_details(io::IO, estim::MovingHorizonEstimator) println(io, "├ optimizer: $(JuMP.solver_name(estim.optim)) ") + println(io, "├ transcription: $(nameof(typeof(estim.transcription)))") print_backends(io, estim, estim.model) println(io, "├ arrival covariance: $(nameof(typeof(estim.covestim))) ") println(io, "├ direct: $(estim.direct)") @@ -35,8 +37,8 @@ function print_estim_dim(io::IO, estim::MovingHorizonEstimator, n; firstchars=no print(io, " │ └$(lpad(nd, n)) measured disturbances d") if isnothing(firstchars) # the user prints the MHE object itself, not a controller: nZ̃, nε = length(estim.Z̃), estim.nε - nA = sum(estim.con.i_b) - ng, nc = sum(estim.con.i_g), estim.con.nc + nA, nAeq = sum(estim.con.i_b), size(estim.con.Aeq, 1) + ng, nc, neq = sum(estim.con.i_g), estim.con.nc, estim.con.neq m = maximum(ndigits.((nZ̃, nA, ng))) + 1 i_nZ̃min, i_nZ̃max = @. !isinf(estim.con.Z̃min), !isinf(estim.con.Z̃max) nZ̃bounds = sum(i_nZ̃min) + sum(i_nZ̃max) @@ -44,6 +46,8 @@ function print_estim_dim(io::IO, estim::MovingHorizonEstimator, n; firstchars=no println(io, " └ optimization:") println(io, " ├$(lpad(nZ̃, m)) decision variables Z̃ ($nε slack variable, $nZ̃bounds bounds)") println(io, " ├$(lpad(nA, m)) linear inequality constraints A") - print(io, " └$(lpad(ng, m)) nonlinear inequality constraints g ($nc custom)") + println(io, " ├$(lpad(nAeq, m)) linear equality constraints Aeq") + println(io, " ├$(lpad(ng, m)) nonlinear inequality constraints g ($nc custom)") + print(io, " └$(lpad(neq, m)) nonlinear equality constraints geq") end end \ No newline at end of file diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 7feee6fe4..6fa2a4575 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -1,3 +1,4 @@ +const DEFAULT_MHE_TRANSCRIPTION = SingleShooting() const DEFAULT_LINMHE_OPTIMIZER = OSQP.MathOptInterfaceOSQP.Optimizer const DEFAULT_NONLINMHE_OPTIMIZER = optimizer_with_attributes(Ipopt.Optimizer,"sb"=>"yes") const DEFAULT_NONLINMHE_GRADIENT = AutoForwardDiff() @@ -20,6 +21,12 @@ struct EstimatorConstraint{NT<:Real, GCfunc<:Union{Nothing, Function}} Gx̂ ::Matrix{NT} Jx̂ ::Matrix{NT} Bx̂ ::Vector{NT} + # matrices for the zero defect constraints (N/A for single shooting transcriptions): + ẼS ::Matrix{NT} + FS ::Vector{NT} + GS ::Matrix{NT} + JS ::Matrix{NT} + BS ::Vector{NT} # bounds over the estimation windows (deviation vectors from operating points): x̂0min ::Vector{NT} x̂0max ::Vector{NT} @@ -37,20 +44,26 @@ struct EstimatorConstraint{NT<:Real, GCfunc<:Union{Nothing, Function}} A_x̂max ::Matrix{NT} A_X̂min ::Matrix{NT} A_X̂max ::Matrix{NT} - A_Ŵmin ::Matrix{NT} - A_Ŵmax ::Matrix{NT} + A_Ŵmin ::SparseMatrixCSC{NT,Int} + A_Ŵmax ::SparseMatrixCSC{NT,Int} A_V̂min ::Matrix{NT} A_V̂max ::Matrix{NT} A ::Matrix{NT} # b vector for the linear inequality constraints: b ::Vector{NT} + # indices of finite numbers in the b vector (linear inequality constraints): + i_b ::BitVector + # Aeq matrix for the linear equality constraints: + Aeq ::Matrix{NT} + # beq vector for the linear equality constraints: + beq ::Vector{NT} + # number of nonlinear equality constraints: + neq ::Int # constraint softness parameter vectors needing separate storage: C_x̂min ::Vector{NT} C_x̂max ::Vector{NT} C_v̂min ::Vector{NT} C_v̂max ::Vector{NT} - # indices of finite numbers in the b vector (linear inequality constraints): - i_b ::BitVector # indices of finite numbers in the g vectors (nonlinear inequality constraints): i_g ::BitVector # custom nonlinear inequality constraints: @@ -62,6 +75,7 @@ struct MovingHorizonEstimator{ NT<:Real, SM<:SimModel, KC<:KalmanCovariances, + TM<:TranscriptionMethod, JM<:JuMP.GenericModel, GB<:AbstractADType, JB<:AbstractADType, @@ -71,7 +85,7 @@ struct MovingHorizonEstimator{ CE<:KalmanEstimator, } <: StateEstimator{NT} model::SM - cov ::KC + transcription::TM # note: `NT` and the number type `JNT` in `JuMP.GenericModel{JNT}` can be # different since solvers that support non-Float64 are scarce. optim::JM @@ -79,6 +93,7 @@ struct MovingHorizonEstimator{ gradient::GB jacobian::JB hessian::HB + cov::KC covestim::CE Z̃::Vector{NT} lastu0::Vector{NT} @@ -105,6 +120,7 @@ struct MovingHorizonEstimator{ D̂d ::Matrix{NT} Ĉm ::Matrix{NT} D̂dm ::Matrix{NT} + Tŵ::SparseMatrixCSC{NT, Int} Ẽ ::Matrix{NT} F ::Vector{NT} G ::Matrix{NT} @@ -123,7 +139,6 @@ struct MovingHorizonEstimator{ Ue ::Vector{NT} D0 ::Vector{NT} De ::Vector{NT} - Ŵ ::Vector{NT} X̂0_old ::Vector{NT} x̂0arr_old::Vector{NT} P̂arr_old ::Hermitian{NT, Matrix{NT}} @@ -135,12 +150,14 @@ struct MovingHorizonEstimator{ model::SM, He, i_ym, nint_u, nint_ym, cov::KC, Cwt, gc!::GCfunc, nc, p::PT, - optim::JM, gradient::GB, jacobian::JB, hessian::HB, covestim::CE; + transcription::TM, optim::JM, + gradient::GB, jacobian::JB, hessian::HB, covestim::CE; direct=true ) where { NT<:Real, SM<:SimModel{NT}, KC<:KalmanCovariances, + TM<:TranscriptionMethod, JM<:JuMP.GenericModel, GB<:AbstractADType, JB<:AbstractADType, @@ -155,20 +172,30 @@ struct MovingHorizonEstimator{ 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) nxs = size(As, 1) - nx̂ = model.nx + nxs + nx̂ = model.nx + nxs + nŵ = nx̂ Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = augment_model(model, As, Cs_u, Cs_y) Ĉm, D̂dm = Ĉ[i_ym, :], D̂d[i_ym, :] lastu0 = zeros(NT, nu) x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)] + Tŵ = init_ZtoŴ(model, transcription, He, nx̂, nŵ) E, G, J, B, ex̄, Ex̂, Gx̂, Jx̂, Bx̂ = init_predmat_mhe( - model, He, i_ym, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op, direct + model, transcription, He, i_ym, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op, direct ) + ES, GS, JS, BS = init_defectmat_mhe( + model, transcription, He, Â, B̂u, B̂d, x̂op, f̂op, direct + ) # dummy values (updated just before optimization): - F, fx̄, Fx̂ = zeros(NT, nym*He), zeros(NT, nx̂), zeros(NT, nx̂*He) + F, fx̄ = zeros(NT, nym*He), zeros(NT, nx̂) con, nε, Ẽ, ẽx̄ = init_defaultcon_mhe( - model, He, Cwt, nx̂, nym, E, ex̄, Ex̂, Fx̂, Gx̂, Jx̂, Bx̂, gc!, nc + model, transcription, + He, Cwt, nx̂, nym, + Tŵ, E, ex̄, + Ex̂, Gx̂, Jx̂, Bx̂, + ES, GS, JS, BS, + gc!, nc ) - nZ̃ = size(Ẽ, 2) + nZ̃ = nε + get_nZ_mhe(transcription, He, nx̂, nŵ) # dummy values, updated before optimization: H̃, q̃, r = Hermitian(zeros(NT, nZ̃, nZ̃), :L), zeros(NT, nZ̃), zeros(NT, 1) Z̃ = zeros(NT, nZ̃) @@ -176,7 +203,6 @@ struct MovingHorizonEstimator{ Y0m, Yem = fill(NT(NaN), nym*He), fill(NT(NaN), nym*(He+1)) U0, Ue = fill(NT(NaN), nu*He), fill(NT(NaN), nu*(He+1)) D0, De = fill(NT(NaN), nd*(He+1)), fill(NT(NaN), nd*(He+1)) - Ŵ = fill(NT(NaN), nx̂*He) X̂0_old = fill(NT(NaN), nx̂*He) D0[1:nd] .= 0 # D0 start with d0(-1) and it should not be NaN x̂0arr_old = zeros(NT, nx̂) @@ -184,12 +210,11 @@ struct MovingHorizonEstimator{ Nk = [0] prepared = [false] test_custom_function_mhe(NT, model, i_ym, He, gc!, nc, x̂op, p, direct) - buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk, He, nε) - estim = new{NT, SM, KC, JM, GB, JB, HB, PT, GCfunc, CE}( - model, - cov, - optim, con, + buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk, He, nŵ, nε, transcription) + estim = new{NT, SM, KC, TM, JM, GB, JB, HB, PT, GCfunc, CE}( + model, transcription, optim, con, gradient, jacobian, hessian, + cov, covestim, Z̃, lastu0, x̂op, f̂op, x̂0, He, nε, @@ -197,11 +222,12 @@ struct MovingHorizonEstimator{ p, As, Cs_u, Cs_y, nint_u, nint_ym, Â, B̂u, Ĉ, B̂d, D̂d, Ĉm, D̂dm, + Tŵ, Ẽ, F, G, J, B, ẽx̄, fx̄, H̃, q̃, r, Cwt, X̂op, - Y0m, Yem, U0, Ue, D0, De, Ŵ, + Y0m, Yem, U0, Ue, D0, De, X̂0_old, x̂0arr_old, P̂arr_old, Nk, direct, prepared, buffer @@ -478,6 +504,7 @@ function MovingHorizonEstimator( gc ::Function = gc!, nc ::Int = 0, p = model.p, + transcription::ShootingMethod = DEFAULT_MHE_TRANSCRIPTION, optim::JM = default_optim_mhe(model, nc), gradient::AbstractADType = DEFAULT_NONLINMHE_GRADIENT, jacobian::AbstractADType = DEFAULT_NONLINMHE_JACOBIAN, @@ -499,7 +526,8 @@ function MovingHorizonEstimator( isnothing(He) && throw(ArgumentError("Estimation horizon He must be explicitly specified")) return MovingHorizonEstimator( model, He, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, Cwt; - gc, gc!, nc, p, optim, gradient, jacobian, hessian, covestim, direct + gc, gc!, nc, p, + transcription, optim, gradient, jacobian, hessian, covestim, direct ) end @@ -529,6 +557,7 @@ function MovingHorizonEstimator( gc ::Function = gc!, nc = 0, p = model.p, + transcription::ShootingMethod = DEFAULT_MHE_TRANSCRIPTION, optim::JM = default_optim_mhe(model, nc), gradient::AbstractADType = DEFAULT_NONLINMHE_GRADIENT, jacobian::AbstractADType = DEFAULT_NONLINMHE_JACOBIAN, @@ -555,7 +584,7 @@ function MovingHorizonEstimator( model, He, i_ym, nint_u, nint_ym, cov, Cwt, gc!, nc, p, - optim, gradient, jacobian, hessian, covestim; + transcription, optim, gradient, jacobian, hessian, covestim; direct ) end @@ -811,6 +840,7 @@ function setconstraint!( C_v̂min = C_vhatmin, C_v̂max = C_vhatmax, ) model, optim, con = estim.model, estim.optim, estim.con + transcription = estim.transcription nε, nx̂, nŵ, nym, He = estim.nε, estim.nx̂, estim.nx̂, estim.nym, estim.He nX̂con = nx̂*(He+1) notSolvedYet = (JuMP.termination_status(optim) == JuMP.OPTIMIZE_NOT_CALLED) @@ -926,18 +956,19 @@ function setconstraint!( end end Z̃min, Z̃max = init_boxconstraint_mhe( - model, He, nx̂, nŵ, nε, + model, transcription, He, nx̂, nŵ, nε, con.x̂0min, con.x̂0max, con.Ŵmin, con.Ŵmax, con.A_x̂min, con.A_x̂max, con.A_Ŵmin, con.A_Ŵmax ) Z̃var = optim[:Z̃var] if notSolvedYet con.i_b[:], con.i_g[:], con.A[:] = init_matconstraint_mhe( - model, Z̃min, Z̃max, con.nc, + model, transcription, Z̃min, Z̃max, con.nc, con.x̂0min, con.x̂0max, con.X̂0min, con.X̂0max, con.Ŵmin, con.Ŵmax, con.V̂min, con.V̂max, con.A_x̂min, con.A_x̂max, con.A_X̂min, con.A_X̂max, - con.A_Ŵmin, con.A_Ŵmax, con.A_V̂min, con.A_V̂max + con.A_Ŵmin, con.A_Ŵmax, con.A_V̂min, con.A_V̂max, + con.Aeq ) con.Z̃min[:], con.Z̃max[:] = Z̃min, Z̃max A = con.A[con.i_b, :] @@ -954,7 +985,7 @@ function setconstraint!( reset_nonlincon!(estim, model) else i_b, i_g = init_matconstraint_mhe( - model, Z̃min, Z̃max, con.nc, + model, transcription, Z̃min, Z̃max, con.nc, con.x̂0min, con.x̂0max, con.X̂0min, con.X̂0max, con.Ŵmin, con.Ŵmax, con.V̂min, con.V̂max ) @@ -984,75 +1015,6 @@ function reset_nonlincon!(estim::MovingHorizonEstimator, model::NonLinModel) set_nonlincon!(estim, estim.optim, g_oracle) end -@doc raw""" - init_matconstraint_mhe( - model::LinModel, Z̃min, Z̃max, nc, - x̂0min, x̂0max, X̂0min, X̂0max, Ŵmin, Ŵmax, V̂min, V̂max, args... - ) -> b, g, A - -Init `i_b`, `g` and `A` matrices for the MHE linear inequality constraints. - -The linear and nonlinear inequality constraints are respectively defined as: -```math -\begin{aligned} - \mathbf{A Z̃ } &≤ \mathbf{b} \\ - \mathbf{g(Z̃)} &≤ \mathbf{0} -\end{aligned} -``` -The argument `nc` is the number of custom nonlinear inequality constraints in -``\mathbf{g_c}``. `b` is a `BitVector` including the indices of ``\mathbf{b}`` that are -finite numbers. `g` is a similar vector but for the indices of ``\mathbf{g}`` (empty if -`model` is a [`LinModel`](@ref)). The method also returns the ``\mathbf{A}`` matrix if -`args` is provided. In such a case, `args` needs to contain all the inequality constraint -matrices: `A_x̂min, A_x̂max, A_X̂min, A_X̂max, A_Ŵmin, A_Ŵmax, A_V̂min, A_V̂max`. -""" -function init_matconstraint_mhe( - model::LinModel{NT}, Z̃min, Z̃max, nc, - x̂0min, x̂0max, X̂0min, X̂0max, Ŵmin, Ŵmax, V̂min, V̂max, args... -) where {NT<:Real} - if isempty(args) - A = nothing - else - A_x̂min, A_x̂max, A_X̂min, A_X̂max, A_Ŵmin, A_Ŵmax, A_V̂min, A_V̂max = args - A = [A_x̂min; A_x̂max; A_X̂min; A_X̂max; A_Ŵmin; A_Ŵmax; A_V̂min; A_V̂max] - end - i_x̂min, i_x̂max = @. !isinf(x̂0min), !isinf(x̂0max) - i_X̂min, i_X̂max = @. !isinf(X̂0min), !isinf(X̂0max) - i_Ŵmin, i_Ŵmax = @. !isinf(Ŵmin), !isinf(Ŵmax) - i_V̂min, i_V̂max = @. !isinf(V̂min), !isinf(V̂max) - nx̂ = length(x̂0min) - nε = length(Z̃min) - length(Ŵmin) - nx̂ - deletex̂arr_lincon!(i_x̂min, i_x̂max, model, Z̃min, Z̃max, nε) - deleteŴ_lincon!(i_Ŵmin, i_Ŵmax, model, Z̃min, Z̃max, nx̂, nε) - i_b = [i_x̂min; i_x̂max; i_X̂min; i_X̂max; i_Ŵmin; i_Ŵmax; i_V̂min; i_V̂max] - g = trues(nc) - return i_b, g, A -end - -"Init `i_b, A` without state and sensor noise constraints if `model` is not a [`LinModel`](@ref)." -function init_matconstraint_mhe( - model::SimModel{NT}, Z̃min, Z̃max, nc, - x̂0min, x̂0max, X̂0min, X̂0max, Ŵmin, Ŵmax, V̂min, V̂max, args... -) where {NT<:Real} - if isempty(args) - A = nothing - else - A_x̂min, A_x̂max, _ , _ , A_Ŵmin, A_Ŵmax, _ , _ = args - A = [A_x̂min; A_x̂max; A_Ŵmin; A_Ŵmax] - end - i_x̂min, i_x̂max = @. !isinf(x̂0min), !isinf(x̂0max) - i_X̂min, i_X̂max = @. !isinf(X̂0min), !isinf(X̂0max) - i_Ŵmin, i_Ŵmax = @. !isinf(Ŵmin), !isinf(Ŵmax) - i_V̂min, i_V̂max = @. !isinf(V̂min), !isinf(V̂max) - nx̂ = length(x̂0min) - nε = length(Z̃min) - length(Ŵmin) - nx̂ - deletex̂arr_lincon!(i_x̂min, i_x̂max, model, Z̃min, Z̃max, nε) - deleteŴ_lincon!(i_Ŵmin, i_Ŵmax, model, Z̃min, Z̃max, nx̂, nε) - i_b = [i_x̂min; i_x̂max; i_Ŵmin; i_Ŵmax] - g = [i_X̂min; i_X̂max; i_V̂min; i_V̂max; trues(nc)] - return i_b, g, A -end - "Unset `i_x̂min` and `i_x̂min` elements if finite box constraints in `Z̃min` and `Z̃max`." function deletex̂arr_lincon!(i_x̂min, i_x̂max, ::SimModel, Z̃min, Z̃max, nε) nx̂ = length(i_x̂min) @@ -1070,9 +1032,36 @@ function deleteŴ_lincon!(i_Ŵmin, i_Ŵmax, ::SimModel, Z̃min, Z̃max, nx̂, return i_Ŵmin, i_Ŵmax end +@doc raw""" + init_ZtoŴ(model::SimModel, transcription::TranscriptionMethod, He, nx̂, nŵ) -> Tŵ + +Init decision variables to estimated process noise over ``H_e`` conversion matrix `Tŵ`. + +This is for the [`MovingHorizonEstimator`](@ref) only. The conversion from the decision +variables ``\mathbf{Z}`` to ``\mathbf{Ŵ}``, the estimated process noise over ``H_e``, is +computed by: +```math +\mathbf{Ŵ} = \mathbf{T_ŵ Z} +``` +in which ``\mathbf{T_{ŵ}} = [\begin{smallmatrix} \mathbf{0} & \mathbf{I} \end{smallmatrix}]`` +and ``\mathbf{0}`` is properly sized for the `transcription` instance. +""" +function init_ZtoŴ( + ::SimModel{NT}, transcription::TranscriptionMethod, He, nx̂, nŵ +) where {NT<:Real} + nŴ, nZ = nŵ*He, get_nZ_mhe(transcription, He, nx̂, nŵ) + Tŵ = [spzeros(NT, nŴ, nZ-nŴ) I] + return Tŵ +end + """ init_defaultcon_mhe( - model::SimModel, He, C, nx̂, nym, E, ex̄, Ex̂, Fx̂, Gx̂, Jx̂, Bx̂ + model::SimModel, transcription::TranscriptionMethod, + He, Cwt, nx̂, nym, + Tŵ, E, ex̄, + Ex̂, Gx̂, Jx̂, Bx̂, + ES, GS, JS, BS, + gc!::Function, nc ) -> con, Ẽ, ẽx̄ Init `EstimatatorConstraint` struct with default parameters based on model `model`. @@ -1080,12 +1069,17 @@ end Also return `Ẽ` and `ẽx̄` matrices for the the augmented decision vector `Z̃`. """ function init_defaultcon_mhe( - model::SimModel{NT}, He, C, nx̂, nym, E, ex̄, Ex̂, Fx̂, Gx̂, Jx̂, Bx̂, - gc!::GCfunc = nothing, nc = 0 -) where {NT<:Real, GCfunc<:Union{Nothing, Function}} + model::SimModel{NT}, transcription::TranscriptionMethod, + He, Cwt, nx̂, nym, + Tŵ, E, ex̄, + Ex̂, Gx̂, Jx̂, Bx̂, + ES, GS, JS, BS, + gc!::GCfunc, nc +) where {NT<:Real, GCfunc<:Function} nŵ = nx̂ nX̂, nŴ, nYm = nx̂*He, nŵ*He, nym*He - nε = isinf(C) ? 0 : 1 + nε = isinf(Cwt) ? 0 : 1 + nS = size(ES, 1) x̂0min, x̂0max = fill(convert(NT,-Inf), nx̂), fill(convert(NT,+Inf), nx̂) X̂0min, X̂0max = fill(convert(NT,-Inf), nX̂), fill(convert(NT,+Inf), nX̂) Ŵmin, Ŵmax = fill(convert(NT,-Inf), nŴ), fill(convert(NT,+Inf), nŴ) @@ -1094,35 +1088,41 @@ function init_defaultcon_mhe( C_x̂min, C_x̂max = fill(0.0, nX̂), fill(0.0, nX̂) C_ŵmin, C_ŵmax = fill(0.0, nŴ), fill(0.0, nŴ) C_v̂min, C_v̂max = fill(0.0, nYm), fill(0.0, nYm) - A_x̂min, A_x̂max, ẽx̄ = relaxarrival(model, nε, c_x̂min, c_x̂max, ex̄) - A_X̂min, A_X̂max, Ẽx̂ = relaxX̂(model, nε, C_x̂min, C_x̂max, Ex̂) - A_Ŵmin, A_Ŵmax = relaxŴ(model, nε, C_ŵmin, C_ŵmax, nx̂) - A_V̂min, A_V̂max, Ẽ = relaxV̂(model, nε, C_v̂min, C_v̂max, E) + A_x̂min, A_x̂max, ẽx̄ = relaxarrival(ex̄, c_x̂min, c_x̂max, nε) + A_X̂min, A_X̂max, Ẽx̂ = relaxX̂(Ex̂, C_x̂min, C_x̂max, nε) + A_Ŵmin, A_Ŵmax = relaxŴ(Tŵ, C_ŵmin, C_ŵmax, nε) + A_V̂min, A_V̂max, Ẽ = relaxV̂(E, C_v̂min, C_v̂max , nε) + Aeq, ẼS = augmentdefect(ES, nε; slackfirst=true) Z̃min, Z̃max = init_boxconstraint_mhe( - model, He, nx̂, nŵ, nε, + model, transcription, He, nx̂, nŵ, nε, x̂0min, x̂0max, Ŵmin, Ŵmax, A_x̂min, A_x̂max, A_Ŵmin, A_Ŵmax ) - i_b, i_g, A = init_matconstraint_mhe( - model, Z̃min, Z̃max, nc, + i_b, i_g, A, Aeq, neq = init_matconstraint_mhe( + model, transcription, Z̃min, Z̃max, nc, x̂0min, x̂0max, X̂0min, X̂0max, Ŵmin, Ŵmax, V̂min, V̂max, - A_x̂min, A_x̂max, A_X̂min, A_X̂max, A_Ŵmin, A_Ŵmax, A_V̂min, A_V̂max + A_x̂min, A_x̂max, A_X̂min, A_X̂max, A_Ŵmin, A_Ŵmax, A_V̂min, A_V̂max, Aeq ) - b = zeros(NT, size(A, 1)) # dummy b vector (updated just before optimization) + # dummy vectors (updated just before optimization): + Fx̂, FS = zeros(NT, nx̂*He), zeros(NT, nS) + b, beq = zeros(NT, size(A, 1)), zeros(NT, size(Aeq, 1)) con = EstimatorConstraint{NT, GCfunc}( Ẽx̂, Fx̂, Gx̂, Jx̂, Bx̂, + ẼS, FS, GS, JS, BS, x̂0min, x̂0max, X̂0min, X̂0max, Ŵmin, Ŵmax, V̂min, V̂max, Z̃min, Z̃max, A_x̂min, A_x̂max, A_X̂min, A_X̂max, A_Ŵmin, A_Ŵmax, A_V̂min, A_V̂max, - A, b, + A, b, i_b, + Aeq, beq, + neq, C_x̂min, C_x̂max, C_v̂min, C_v̂max, - i_b, i_g, + i_g, gc!, nc ) return con, nε, Ẽ, ẽx̄ end @doc raw""" - relaxarrival(model::SimModel, nε, c_x̂min, c_x̂max, ex̄) -> A_x̂min, A_x̂max, ẽx̄ + relaxarrival(ex̄, c_x̂min, c_x̂maxm, nε) -> A_x̂min, A_x̂max, ẽx̄ Augment arrival state constraints with slack variable ε for softening the MHE. @@ -1142,7 +1142,7 @@ matrices for the inequality constraints: \end{bmatrix} ``` """ -function relaxarrival(::SimModel{NT}, nε, c_x̂min, c_x̂max, ex̄) where NT<:Real +function relaxarrival(ex̄::AbstractMatrix{NT}, c_x̂min, c_x̂max, nε) where NT<:Real ex̂ = -ex̄ if nε ≠ 0 # Z̃ = [ε; Z] # ε impacts arrival state constraint calculations: @@ -1157,7 +1157,7 @@ function relaxarrival(::SimModel{NT}, nε, c_x̂min, c_x̂max, ex̄) where NT<:R end @doc raw""" - relaxX̂(model::SimModel, nε, C_x̂min, C_x̂max, Ex̂) -> A_X̂min, A_X̂max, Ẽx̂ + relaxX̂(Ex̂, C_x̂min, C_x̂max, nε) -> A_X̂min, A_X̂max, Ẽx̂ Augment estimated state constraints with slack variable ε for softening the MHE. @@ -1178,8 +1178,12 @@ also returns the ``\mathbf{A}`` matrices for the inequality constraints: in which ``\mathbf{X̂_{min}, X̂_{max}}`` and ``\mathbf{X̂_{op}}`` vectors respectively contains ``\mathbf{x̂_{min}, x̂_{max}}`` and ``\mathbf{x̂_{op}}`` repeated ``H_e`` times. """ -function relaxX̂(::LinModel{NT}, nε, C_x̂min, C_x̂max, Ex̂) where {NT<:Real} +function relaxX̂(Ex̂::AbstractMatrix{NT}, C_x̂min, C_x̂max, nε) where NT<:Real if nε ≠ 0 # Z̃ = [ε; Z] + if iszero(size(Ex̂, 1)) + # model is not a LinModel, thus X̂ constraints are not linear: + C_x̂min = C_x̂max = zeros(NT, 0, 1) + end # ε impacts estimated process noise constraint calculations: A_X̂min, A_X̂max = -[C_x̂min Ex̂], [-C_x̂max Ex̂] # ε has no impact on estimated process noises: @@ -1191,15 +1195,8 @@ function relaxX̂(::LinModel{NT}, nε, C_x̂min, C_x̂max, Ex̂) where {NT<:Real return A_X̂min, A_X̂max, Ẽx̂ end -"Return empty matrices if model is not a [`LinModel`](@ref)" -function relaxX̂(::SimModel{NT}, nε, C_x̂min, C_x̂max, Ex̂) where {NT<:Real} - Ẽx̂ = [zeros(NT, 0, nε) Ex̂] - A_X̂min, A_X̂max = -Ẽx̂, Ẽx̂ - return A_X̂min, A_X̂max, Ẽx̂ -end - @doc raw""" - relaxŴ(model::SimModel, nε, C_ŵmin, C_ŵmax, nx̂) -> A_Ŵmin, A_Ŵmax + relaxŴ(Tŵ, C_ŵmin, C_ŵmax, nε) -> A_Ŵmin, A_Ŵmax Augment estimated process noise constraints with slack variable ε for softening the MHE. @@ -1207,7 +1204,7 @@ Denoting the MHE decision variable augmented with the slack variable ``\mathbf{Z [\begin{smallmatrix} ε \\ \mathbf{Z} \end{smallmatrix}]``, it returns the ``\mathbf{A}`` matrices for the inequality constraints: ```math -\begin{bmatrix} +\begin{bmatrix} \mathbf{A_{Ŵ_{min}}} \\ \mathbf{A_{Ŵ_{max}}} \end{bmatrix} \mathbf{Z̃} ≤ @@ -1217,18 +1214,17 @@ matrices for the inequality constraints: \end{bmatrix} ``` """ -function relaxŴ(::SimModel{NT}, nε, C_ŵmin, C_ŵmax, nx̂) where {NT<:Real} - A = [zeros(NT, length(C_ŵmin), nx̂) I] +function relaxŴ(Tŵ::AbstractMatrix{NT}, C_ŵmin, C_ŵmax, nε) where NT<:Real if nε ≠ 0 # Z̃ = [ε; Z] - A_Ŵmin, A_Ŵmax = -[C_ŵmin A], [-C_ŵmax A] + A_Ŵmin, A_Ŵmax = -[C_ŵmin Tŵ], [-C_ŵmax Tŵ] else # Z̃ = Z (only hard constraints) - A_Ŵmin, A_Ŵmax = -A, A + A_Ŵmin, A_Ŵmax = -Tŵ, Tŵ end return A_Ŵmin, A_Ŵmax end @doc raw""" - relaxV̂(model::SimModel, nε, C_v̂min, C_v̂max, E) -> A_V̂min, A_V̂max, Ẽ + relaxV̂(E, C_v̂min, C_v̂max, nε) -> A_V̂min, A_V̂max, Ẽ Augment estimated sensor noise constraints with slack variable ε for softening the MHE. @@ -1247,8 +1243,12 @@ also returns the ``\mathbf{A}`` matrices for the inequality constraints: \end{bmatrix} ``` """ -function relaxV̂(::LinModel{NT}, nε, C_v̂min, C_v̂max, E) where {NT<:Real} +function relaxV̂(E::AbstractMatrix{NT}, C_v̂min, C_v̂max, nε) where NT<:Real if nε ≠ 0 # Z̃ = [ε; Z] + if iszero(size(E, 1)) + # model is not a LinModel, thus V̂ constraints are not linear: + C_v̂min = C_v̂max = zeros(NT, 0, 1) + end # ε impacts estimated sensor noise constraint calculations: A_V̂min, A_V̂max = -[C_v̂min E], [-C_v̂max E] # ε has no impact on estimated sensor noises: @@ -1260,16 +1260,9 @@ function relaxV̂(::LinModel{NT}, nε, C_v̂min, C_v̂max, E) where {NT<:Real} return A_V̂min, A_V̂max, Ẽ end -"Return empty matrices if model is not a [`LinModel`](@ref)" -function relaxV̂(::SimModel{NT}, nε, C_v̂min, C_v̂max, E) where {NT<:Real} - Ẽ = [zeros(NT, 0, nε) E] - A_V̂min, A_V̂max = -Ẽ, Ẽ - return A_V̂min, A_V̂max, Ẽ -end - """ init_boxconstraint_mhe( - model::SimModel, He, nx̂, nŵ, nε, + model::SimModel, transcription::TranscriptionMethod, He, nx̂, nŵ, nε, x̂0min, x̂0max, Ŵmin, Ŵmax, A_x̂min, A_x̂max, A_Ŵmin, A_Ŵmin ) -> Z̃min, Z̃max @@ -1277,12 +1270,13 @@ end Init the decision variable box constraints `Z̃min` and `Z̃max` for [`MovingHorizonEstimator`](@ref). """ function init_boxconstraint_mhe( - ::SimModel{NT}, He, nx̂, nŵ, nε, + ::SimModel{NT}, transcription::TranscriptionMethod, He, nx̂, nŵ, nε, x̂0min, x̂0max, Ŵmin, Ŵmax, A_x̂min, A_x̂max, A_Ŵmin, A_Ŵmax ) where {NT<:Real} - nZ̃ = nε + nx̂ + nŵ*He + nZ̃ = nε + get_nZ_mhe(transcription, He, nx̂, nŵ) Z̃min, Z̃max = fill(convert(NT,-Inf), nZ̃), fill(convert(NT,+Inf), nZ̃) nε > 0 && (Z̃min[begin] = 0) + nŴ = nŵ*He if nε > 0 n_C_x̂min = @views A_x̂min[:, begin] n_C_x̂max = @views A_x̂max[:, begin] @@ -1295,280 +1289,20 @@ function init_boxconstraint_mhe( iszero(n_C_x̂max[i]) && (Z̃max[nε + i] = x̂0max[i]) end for i in eachindex(Ŵmin) - iszero(n_C_Ŵmin[i]) && (Z̃min[nε + nx̂ + i] = Ŵmin[i]) + iszero(n_C_Ŵmin[i]) && (Z̃min[nZ̃ - nŴ + i] = Ŵmin[i]) end for i in eachindex(Ŵmax) - iszero(n_C_Ŵmax[i]) && (Z̃min[nε + nx̂ + i] = Ŵmax[i]) + iszero(n_C_Ŵmax[i]) && (Z̃min[nZ̃ - nŴ + i] = Ŵmax[i]) end else Z̃min[1:nx̂] .= x̂0min Z̃max[1:nx̂] .= x̂0max - Z̃min[nx̂+1:end] .= Ŵmin - Z̃max[nx̂+1:end] .= Ŵmax + Z̃min[nZ̃-nŴ+1:end] .= Ŵmin + Z̃max[nZ̃-nŴ+1:end] .= Ŵmax end return Z̃min, Z̃max end -@doc raw""" - init_predmat_mhe( - model::LinModel, He, i_ym, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op, direct - ) -> E, G, J, B, ex̄, Ex̂, Gx̂, Jx̂, Bx̂ - -Construct the [`MovingHorizonEstimator`](@ref) prediction matrices for [`LinModel`](@ref) `model`. - -We first introduce the deviation vector of the estimated state at arrival -``\mathbf{x̂_0}(k-N_k+p) = \mathbf{x̂}_k(k-N_k+p) - \mathbf{x̂_{op}}`` (see [`setop!`](@ref)), -and the vector ``\mathbf{Z} = [\begin{smallmatrix} \mathbf{x̂_0}(k-N_k+p) -\\ \mathbf{Ŵ} \end{smallmatrix}]`` with the decision variables. Setting the constant ``p=0`` -produces an estimator in the current form, while the prediction form is obtained with -``p=1``. The estimated sensor noises from time ``k-N_k+1`` to ``k`` are computed by: -```math -\begin{aligned} - \mathbf{V̂} = \mathbf{Y_0^m - Ŷ_0^m} &= \mathbf{E Z + G U_0 + J D_0 + Y_0^m + B} \\ - &= \mathbf{E Z + F} -\end{aligned} -``` -in which ``\mathbf{U_0}`` and ``\mathbf{Y_0^m}`` respectively include the deviation values -of the manipulated inputs ``\mathbf{u_0}(k-j+p)`` from ``j=N_k`` to ``1`` and measured -outputs ``\mathbf{y_0^m}(k-j+1)`` from ``j=N_k`` to ``1``. The vector ``\mathbf{D_0}`` -includes the the measured disturbance deviation values ``\mathbf{d_0}(k-j)`` from from -``j=N_k`` to ``0``, thus one additional data point. The constant ``\mathbf{B}`` is the -contribution for non-zero state ``\mathbf{x̂_{op}}`` and state update ``\mathbf{f̂_{op}}`` -operating points (for linearization, see [`augment_model`](@ref) and [`linearize`](@ref)). -The method also returns the matrices for the estimation error at arrival: -```math - \mathbf{x̄} = \mathbf{x̂_0^†}(k-N_k+p) - \mathbf{x̂_0}(k-N_k+p) = \mathbf{e_x̄ Z + f_x̄} -``` -in which ``\mathbf{e_x̄} = [\begin{smallmatrix} -\mathbf{I} & \mathbf{0} & \cdots & \mathbf{0} \end{smallmatrix}]``, -and ``\mathbf{f_x̄} = \mathbf{x̂_0^†}(k-N_k+p)``. The latter is the deviation vector of the -state at arrival, estimated at time ``k-N_k``, i.e. ``\mathbf{x̂_0^†}(k-N_k+p) = -\mathbf{x̂}_{k-N_k}(k-N_k+p) - \mathbf{x̂_{op}}``. Lastly, the estimates ``\mathbf{x̂_0}(k-j+p)`` -from ``j=N_k-1`` to ``0``, also in deviation form, are computed with: -```math -\begin{aligned} - \mathbf{X̂_0} &= \mathbf{E_x̂ Z + G_x̂ U_0 + J_x̂ D_0 + B_x̂} \\ - &= \mathbf{E_x̂ Z + F_x̂} -\end{aligned} -``` -The matrices ``\mathbf{E, G, J, B, E_x̂, G_x̂, J_x̂, B_x̂}`` are defined in the Extended Help -section. The vectors ``\mathbf{F, F_x̂, f_x̄}`` are recalculated at each discrete time step, -see [`initpred!(::MovingHorizonEstimator, ::LinModel)`](@ref) and [`linconstraint!(::MovingHorizonEstimator, ::LinModel)`](@ref). - -# Extended Help -!!! details "Extended Help" - Using the augmented process model matrices ``\mathbf{Â, B̂_u, Ĉ^m, B̂_d, D̂_d^m}``, and the - function ``\mathbf{S}(j) = ∑_{i=0}^j \mathbf{Â}^i``, the prediction matrices for the - sensor noises depend on the constant ``p``. For ``p=0``, the matrices are computed by - (notice the minus signs after the equalities): - ```math - \begin{aligned} - \mathbf{E} &= - \begin{bmatrix} - \mathbf{Ĉ^m}\mathbf{Â}^{1} & \mathbf{Ĉ^m}\mathbf{Â}^{0} & \cdots & \mathbf{0} \\ - \mathbf{Ĉ^m}\mathbf{Â}^{2} & \mathbf{Ĉ^m}\mathbf{Â}^{1} & \cdots & \mathbf{0} \\ - \vdots & \vdots & \ddots & \vdots \\ - \mathbf{Ĉ^m}\mathbf{Â}^{H_e} & \mathbf{Ĉ^m}\mathbf{Â}^{H_e-1} & \cdots & \mathbf{Ĉ^m}\mathbf{Â}^{0} \end{bmatrix} \\ - \mathbf{G} &= - \begin{bmatrix} - \mathbf{Ĉ^m}\mathbf{Â}^{0}\mathbf{B̂_u} & \mathbf{0} & \cdots & \mathbf{0} \\ - \mathbf{Ĉ^m}\mathbf{Â}^{1}\mathbf{B̂_u} & \mathbf{Ĉ^m}\mathbf{Â}^{0}\mathbf{B̂_u} & \cdots & \mathbf{0} \\ - \vdots & \vdots & \ddots & \vdots \\ - \mathbf{Ĉ^m}\mathbf{Â}^{H_e-1}\mathbf{B̂_u} & \mathbf{Ĉ^m}\mathbf{Â}^{H_e-2}\mathbf{B̂_u} & \cdots & \mathbf{Ĉ^m}\mathbf{Â}^{0}\mathbf{B̂_u} \end{bmatrix} \\ - \mathbf{J} &= - \begin{bmatrix} - \mathbf{Ĉ^m}\mathbf{Â}^{0}\mathbf{B̂_d} & \mathbf{D̂_d^m} & \mathbf{0} & \cdots & \mathbf{0} \\ - \mathbf{Ĉ^m}\mathbf{Â}^{1}\mathbf{B̂_d} & \mathbf{Ĉ^m}\mathbf{Â}^{0}\mathbf{B̂_d} & \mathbf{D̂_d^m} & \cdots & \mathbf{0} \\ - \vdots & \vdots & \vdots & \ddots & \vdots \\ - \mathbf{Ĉ^m}\mathbf{Â}^{H_e-1}\mathbf{B̂_d} & \mathbf{Ĉ^m}\mathbf{Â}^{H_e-2}\mathbf{B̂_d} & \mathbf{Ĉ^m}\mathbf{Â}^{H_e-3}\mathbf{B̂_d} & \cdots & \mathbf{D̂_d^m} \end{bmatrix} \\ - \mathbf{B} &= - \begin{bmatrix} - \mathbf{Ĉ^m S}(0) \\ - \mathbf{Ĉ^m S}(1) \\ - \vdots \\ - \mathbf{Ĉ^m S}(H_e-1) \end{bmatrix} \mathbf{\big(f̂_{op} - x̂_{op}\big)} - \end{aligned} - ``` - or, for ``p=1``, the matrices are given by: - ```math - \begin{aligned} - \mathbf{E} &= - \begin{bmatrix} - \mathbf{Ĉ^m}\mathbf{Â}^{0} & \mathbf{0} & \cdots & \mathbf{0} \\ - \mathbf{Ĉ^m}\mathbf{Â}^{1} & \mathbf{Ĉ^m}\mathbf{Â}^{0} & \cdots & \mathbf{0} \\ - \vdots & \vdots & \ddots & \vdots \\ - \mathbf{Ĉ^m}\mathbf{Â}^{H_e-1} & \mathbf{Ĉ^m}\mathbf{Â}^{H_e-2} & \cdots & \mathbf{0} \end{bmatrix} \\ - \mathbf{G} &= - \begin{bmatrix} - \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} \\ - \mathbf{Ĉ^m}\mathbf{Â}^{0}\mathbf{B̂_u} & \mathbf{0} & \cdots & \mathbf{0} \\ - \vdots & \vdots & \ddots & \vdots \\ - \mathbf{Ĉ^m}\mathbf{Â}^{H_e-2}\mathbf{B̂_u} & \mathbf{Ĉ^m}\mathbf{Â}^{H_e-3}\mathbf{B̂_u} & \cdots & \mathbf{0} \end{bmatrix} \\ - \mathbf{J} &= - \begin{bmatrix} - \mathbf{0} & \mathbf{D̂_d^m} & \mathbf{0} & \cdots & \mathbf{0} \\ - \mathbf{0} & \mathbf{Ĉ^m}\mathbf{Â}^{0}\mathbf{B̂_d} & \mathbf{D̂_d^m} & \cdots & \mathbf{0} \\ - \vdots & \vdots & \vdots & \ddots & \vdots \\ - \mathbf{0} & \mathbf{Ĉ^m}\mathbf{Â}^{H_e-2}\mathbf{B̂_d} & \mathbf{Ĉ^m}\mathbf{Â}^{H_e-3}\mathbf{B̂_d} & \cdots & \mathbf{D̂_d^m} \end{bmatrix} \\ - \mathbf{B} &= - \begin{bmatrix} - \mathbf{0} \\ - \mathbf{Ĉ^m S}(0) \\ - \vdots \\ - \mathbf{Ĉ^m S}(H_e-2) \end{bmatrix} \mathbf{\big(f̂_{op} - x̂_{op}\big)} - \end{aligned} - ``` - The matrices for the estimated states are computed by: - ```math - \begin{aligned} - \mathbf{E_x̂} &= \begin{bmatrix} - \mathbf{Â}^{1} & \mathbf{A}^{0} & \cdots & \mathbf{0} \\ - \mathbf{Â}^{2} & \mathbf{Â}^{1} & \cdots & \mathbf{0} \\ - \vdots & \vdots & \ddots & \vdots \\ - \mathbf{Â}^{H_e} & \mathbf{Â}^{H_e-1} & \cdots & \mathbf{Â}^{0} \end{bmatrix} \\ - \mathbf{G_x̂} &= \begin{bmatrix} - \mathbf{Â}^{0}\mathbf{B̂_u} & \mathbf{0} & \cdots & \mathbf{0} \\ - \mathbf{Â}^{1}\mathbf{B̂_u} & \mathbf{Â}^{0}\mathbf{B̂_u} & \cdots & \mathbf{0} \\ - \vdots & \vdots & \ddots & \vdots \\ - \mathbf{Â}^{H_e-1}\mathbf{B̂_u} & \mathbf{Â}^{H_e-2}\mathbf{B̂_u} & \cdots & \mathbf{Â}^{0}\mathbf{B̂_u} \end{bmatrix} \\ - \mathbf{J_x̂^†} &= \begin{bmatrix} - \mathbf{Â}^{0}\mathbf{B̂_d} & \mathbf{0} & \cdots & \mathbf{0} \\ - \mathbf{Â}^{1}\mathbf{B̂_d} & \mathbf{Â}^{0}\mathbf{B̂_d} & \cdots & \mathbf{0} \\ - \vdots & \vdots & \ddots & \vdots \\ - \mathbf{Â}^{H_e-1}\mathbf{B̂_d} & \mathbf{Â}^{H_e-2}\mathbf{B̂_d} & \cdots & \mathbf{Â}^{0}\mathbf{B̂_d} \end{bmatrix} \ , \quad - \mathbf{J_x̂} = \begin{cases} - [\begin{smallmatrix} \mathbf{J_x̂^†} & \mathbf{0} \end{smallmatrix}] & p=0 \\ - [\begin{smallmatrix} \mathbf{0} & \mathbf{J_x̂^†} \end{smallmatrix}] & p=1 \end{cases} \\ - \mathbf{B_x̂} &= \begin{bmatrix} - \mathbf{S}(0) \\ - \mathbf{S}(1) \\ - \vdots \\ - \mathbf{S}(H_e-1) \end{bmatrix} \mathbf{\big(f̂_{op} - x̂_{op}\big)} - \end{aligned} - ``` - All these matrices are truncated when ``N_k < H_e`` (at the beginning). -""" -function init_predmat_mhe( - model::LinModel{NT}, He, i_ym, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op, direct -) where {NT<:Real} - nu, nd = model.nu, model.nd - nym, nx̂ = length(i_ym), size(Â, 2) - nŵ = nx̂ - p = direct ? 0 : 1 - # --- pre-compute matrix powers --- - # Apow3D array : Apow[:,:,1] = A^0, Apow[:,:,2] = A^1, ... , Apow[:,:,He+1] = A^He - Âpow3D = Array{NT}(undef, nx̂, nx̂, He+1) - Âpow3D[:,:,1] = I(nx̂) - for j=2:He+1 - Âpow3D[:,:,j] = @views Âpow3D[:,:,j-1]*Â - end - # nĈm_Âpow3D array : similar indices as Apow3D - nĈm_Âpow3D = Array{NT}(undef, nym, nx̂, He+1) - nĈm_Âpow3D[:,:,1] = -Ĉm - for j=2:He+1 - nĈm_Âpow3D[:,:,j] = @views -Ĉm*Âpow3D[:,:,j] - end - # helper function to improve code clarity and be similar to eqs. in docstring: - getpower(array3D, power) = @views array3D[:,:, power+1] - # --- decision variables Z --- - nĈm_Âpow = reduce(vcat, getpower(nĈm_Âpow3D, i) for i=0:He) - E = zeros(NT, nym*He, nx̂ + nŵ*He) - col_begin = iszero(p) ? 1 : 0 - col_end = iszero(p) ? He : He-1 - i = 0 - for j=col_begin:col_end - iRow = (1 + i*nym):(nym*He) - iCol = (1:nŵ) .+ j*nŵ - E[iRow, iCol] = @views nĈm_Âpow[1:length(iRow) ,:] - i += 1 - end - iszero(p) && @views (E[:, 1:nx̂] = @views nĈm_Âpow[nym+1:end, :]) - ex̄ = [-I zeros(NT, nx̂, nŵ*He)] - Âpow_vec = reduce(vcat, getpower(Âpow3D, i) for i=0:He) - Ex̂ = zeros(NT, nx̂*He, nx̂ + nŵ*He) - i=0 - for j=1:He - iRow = (1 + i*nx̂):(nx̂*He) - iCol = (1:nŵ) .+ j*nŵ - Ex̂[iRow, iCol] = @views Âpow_vec[1:length(iRow) ,:] - i+=1 - end - Ex̂[:, 1:nx̂] = @views Âpow_vec[nx̂+1:end, :] - # --- manipulated inputs U --- - nĈm_Âpow_B̂u = reduce(vcat, getpower(nĈm_Âpow3D, i)*B̂u for i=0:He-1) - nĈm_Âpow_B̂u = [zeros(nym, nu) ; nĈm_Âpow_B̂u] - G = zeros(NT, nym*He, nu*He) - i=0 - col_begin = iszero(p) ? 1 : 0 - col_end = iszero(p) ? He-1 : He-2 - for j=col_begin:col_end - iRow = (1 + i*nym):(nym*He) - iCol = (1:nu) .+ j*nu - G[iRow, iCol] = @views nĈm_Âpow_B̂u[1:length(iRow) ,:] - i+=1 - end - iszero(p) && @views (G[:, 1:nu] = nĈm_Âpow_B̂u[nym+1:end, :]) - Âpow_B̂u = reduce(vcat, getpower(Âpow3D, i)*B̂u for i=0:He-1) - Gx̂ = zeros(NT, nx̂*He, nu*He) - for j=0:He-1 - iRow = (1 + j*nx̂):(nx̂*He) - iCol = (1:nu) .+ j*nu - Gx̂[iRow, iCol] = @views Âpow_B̂u[1:length(iRow) ,:] - end - # --- measured disturbances D --- - nĈm_Âpow_B̂d = reduce(vcat, getpower(nĈm_Âpow3D, i)*B̂d for i=0:He-1) - nĈm_Âpow_B̂d = [-D̂dm; nĈm_Âpow_B̂d] - J = zeros(NT, nym*He, nd*(He+1)) - i = 0 - for j=1:He - iRow = (1 + i*nym):(nym*He) - iCol = (1:nd) .+ j*nd - J[iRow, iCol] = nĈm_Âpow_B̂d[1:length(iRow) ,:] - i+=1 - end - iszero(p) && @views (J[:, 1:nd] = nĈm_Âpow_B̂d[nym+1:end, :]) - Âpow_B̂d = reduce(vcat, getpower(Âpow3D, i)*B̂d for i=0:He-1) - Jx̂ = zeros(NT, nx̂*He, nd*(He+1)) - for j=0:He-1 - iRow = (1 + j*nx̂):(nx̂*He) - iCol = (1:nd) .+ j*nd .+ p - Jx̂[iRow, iCol] = Âpow_B̂d[1:length(iRow) ,:] - end - # --- state x̂op and state update f̂op operating points --- - # Apow_csum 3D array : Apow_csum[:,:,1] = A^0, Apow_csum[:,:,2] = A^1 + A^0, ... - Âpow_csum = cumsum(Âpow3D, dims=3) - # helper function to improve code clarity and be similar to eqs. in docstring: - S(j) = @views Âpow_csum[:,:, j+1] - f̂_op_n_x̂op = (f̂op - x̂op) - coef_B = zeros(NT, nym*He, nx̂) - row_begin = iszero(p) ? 0 : 1 - row_end = iszero(p) ? He-1 : He-2 - j=0 - for i=row_begin:row_end - iRow = (1:nym) .+ nym*i - coef_B[iRow,:] = -Ĉm*S(j) - j+=1 - end - B = coef_B*f̂_op_n_x̂op - coef_Bx̂ = Matrix{NT}(undef, nx̂*He, nx̂) - for j=0:He-1 - iRow = (1:nx̂) .+ nx̂*j - coef_Bx̂[iRow,:] = S(j) - end - Bx̂ = coef_Bx̂*f̂_op_n_x̂op - return E, G, J, B, ex̄, Ex̂, Gx̂, Jx̂, Bx̂ -end - -"Return empty matrices if `model` is not a [`LinModel`](@ref), except for `ex̄`." -function init_predmat_mhe( - model::SimModel{NT}, He, i_ym, Â, _ , _ , _ , _ , _ , _ , direct -) where {NT<:Real} - nym, nx̂ = length(i_ym), size(Â, 2) - nŵ = nx̂ - p = direct ? 0 : 1 - E = zeros(NT, 0, nx̂ + nŵ*He) - ex̄ = [-I zeros(NT, nx̂, nŵ*He)] - Ex̂ = zeros(NT, 0, nx̂ + nŵ*He) - G = zeros(NT, 0, model.nu*He) - Gx̂ = zeros(NT, 0, model.nu*He) - J = zeros(NT, 0, model.nd*(He+1-p)) - Jx̂ = zeros(NT, 0, model.nd*He) - B = zeros(NT, nym*He) - Bx̂ = zeros(NT, nx̂*He) - return E, G, J, B, ex̄, Ex̂, Gx̂, Jx̂, Bx̂ -end - """ init_optimization!( estim::MovingHorizonEstimator, model::LinModel, optim::JuMP.GenericModel @@ -1589,6 +1323,9 @@ function init_optimization!( A = con.A[con.i_b, :] b = con.b[con.i_b] @constraint(optim, linconstraint, A*Z̃var .≤ b) + Aeq = con.Aeq + beq = con.beq + @constraint(optim, linconstrainteq, Aeq*Z̃var .== beq) @objective(optim, Min, obj_quadprog(Z̃var, estim.H̃, estim.q̃)) if con.nc > 0 # --- nonlinear optimization init for the custom NL constraints --- @@ -1620,6 +1357,9 @@ function init_optimization!( A = con.A[con.i_b, :] b = con.b[con.i_b] @constraint(optim, linconstraint, A*Z̃var .≤ b) + Aeq = con.Aeq + beq = con.beq + @constraint(optim, linconstrainteq, Aeq*Z̃var .== beq) # --- nonlinear optimization init --- set_scaling_gradient!(optim, C) # constraints with vector nonlinear oracle, objective function with splatting: diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 7d2fbcb80..b27e57979 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -10,7 +10,6 @@ function init_estimate_cov!(estim::MovingHorizonEstimator, y0m, d0, u0) estim.Ue .= NaN estim.D0 .= NaN estim.De .= NaN - estim.Ŵ .= NaN estim.X̂0_old .= NaN estim.Nk .= 0 estim.F .= 0 @@ -46,8 +45,10 @@ function correct_estimate!(estim::MovingHorizonEstimator, y0m, d0) ismoving = add_data_windows!(estim, y0m, d0) ismoving && correct_cov!(estim) initpred!(estim, estim.model) - linconstraint!(estim, estim.model) - optim_objective!(estim) + linconstraint!(estim, estim.model, estim.transcription) + linconstrainteq!(estim, estim.model, estim.transcription) + Z̃ = optim_objective!(estim) + getstate!(estim, Z̃) end return nothing end @@ -71,8 +72,10 @@ function update_estimate!(estim::MovingHorizonEstimator, u0, y0m, d0) if !estim.direct add_data_windows!(estim, y0m, d0, u0) initpred!(estim, estim.model) - linconstraint!(estim, estim.model) - optim_objective!(estim) + linconstraint!(estim, estim.model, estim.transcription) + linconstrainteq!(estim, estim.model, estim.transcription) + Z̃ = optim_objective!(estim) + getstate!(estim, Z̃) end (estim.Nk[] == estim.He) && update_cov!(estim) estim.lastu0 .= u0 @@ -138,17 +141,18 @@ function getinfo(estim::MovingHorizonEstimator{NT}) where NT<:Real model, buffer, Nk = estim.model, estim.buffer, estim.Nk[] nu, ny, nd = model.nu, model.ny, model.nd nx̂, nym, nŵ = estim.nx̂, estim.nym, estim.nx̂ - Z̃, Ŵ = estim.Z̃, estim.Ŵ + Z̃ = estim.Z̃ info = Dict{Symbol, Any}() Ŷ0 = Vector{NT}(undef, ny*Nk) - V̂, X̂0 = buffer.V̂, buffer.X̂ + V̂, Ŵ, X̂0 = buffer.V̂, buffer.Ŵ, buffer.X̂ x̂0arr, û0, k, ŷ0 = buffer.x̂, buffer.û, buffer.k, buffer.ŷ - x̂0arr = getarrival!(x̂0arr, estim, Z̃) - x̄ = estim.x̂0arr_old - x̂0arr - V̂, X̂0 = predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim, model, x̂0arr, Ŵ, Z̃) - Ŷ0 = predict_outputs_mhe!(Ŷ0, estim, X̂0, x̂0arr) - J = obj_nonlinprog(estim, estim.model, x̄, V̂, Ŵ, Z̃) - yopm = model.yop[estim.i_ym] + x̂0arr = getarrival!(x̂0arr, estim, Z̃) + Ŵ = getŴ!(Ŵ, estim, Z̃) + x̄ = estim.x̂0arr_old - x̂0arr + V̂, X̂0 = predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim, model, x̂0arr, Ŵ, Z̃) + Ŷ0 = predict_outputs_mhe!(Ŷ0, estim, X̂0, x̂0arr) + J = obj_nonlinprog(estim, estim.model, x̄, V̂, Ŵ, Z̃) + yopm = model.yop[estim.i_ym] Ym0, U0, D0 = estim.Y0m[1:nym*Nk], estim.U0[1:nu*Nk], estim.D0[1:nd*(Nk+1)] Ym, U, D, Ŷ, X̂, x̂arr = Ym0, U0, D0, Ŷ0, X̂0, x̂0arr for i=1:Nk @@ -303,6 +307,77 @@ function addinfo!(info, estim::MovingHorizonEstimator{NT}, model::SimModel) wher return info end +@doc raw""" + initpred!(estim::MovingHorizonEstimator, model::LinModel) -> nothing + +Init quadratic optimization matrices `F, fx̄, H̃, q̃, r` for [`MovingHorizonEstimator`](@ref). + +See [`init_predmat_mhe`](@ref) for the definition of the vectors ``\mathbf{F, f_x̄}``. It +also inits `estim.optim` objective function, expressed as the quadratic general form: +```math + J = \min_{\mathbf{Z̃}} \frac{1}{2}\mathbf{Z̃' H̃ Z̃} + \mathbf{q̃' Z̃} + r +``` +in which ``\mathbf{Z̃} = [\begin{smallmatrix} ε \\ \mathbf{Z} \end{smallmatrix}]``. Note that +``r`` is useless at optimization but required to evaluate the objective minima ``J``. The +Hessian ``\mathbf{H̃}`` matrix of the quadratic general form is not constant here because +of the time-varying ``\mathbf{P̄}`` covariance . The computed variables are: +```math +\begin{aligned} + \mathbf{F} &= \mathbf{G U_0} + \mathbf{J D_0} + \mathbf{Y_0^m} + \mathbf{B} \\ + \mathbf{f_x̄} &= \mathbf{x̂_0^†}(k-N_k+1) \\ + \mathbf{F_Z̃} &= [\begin{smallmatrix}\mathbf{f_x̄} \\ \mathbf{F} \end{smallmatrix}] \\ + \mathbf{Ẽ_Z̃} &= [\begin{smallmatrix}\mathbf{ẽ_x̄} \\ \mathbf{Ẽ} \end{smallmatrix}] \\ + \mathbf{M}_{N_k} &= \mathrm{diag}(\mathbf{P̄}^{-1}, \mathbf{R̂}_{N_k}^{-1}) \\ + \mathbf{Ñ}_{N_k} &= \mathrm{diag}(C, \mathbf{T_ŵ}'\mathbf{Q̂}_{N_k}^{-1}\mathbf{T_ŵ}) \\ + \mathbf{H̃} &= 2(\mathbf{Ẽ_Z̃}' \mathbf{M}_{N_k} \mathbf{Ẽ_Z̃} + \mathbf{Ñ}_{N_k}) \\ + \mathbf{q̃} &= 2(\mathbf{M}_{N_k} \mathbf{Ẽ_Z̃})' \mathbf{F_Z̃} \\ + r &= \mathbf{F_Z̃}' \mathbf{M}_{N_k} \mathbf{F_Z̃} +\end{aligned} +See [`init_ZtoŴ`](@ref) for the definition of the conversion matrix ``\mathbf{T_ŵ}``. +``` +""" +function initpred!(estim::MovingHorizonEstimator{NT}, model::LinModel) where NT<:Real + invP̄, invQ̂_He, invR̂_He = estim.cov.invP̄, estim.cov.invQ̂_He, estim.cov.invR̂_He + F, C, optim = estim.F, estim.C, estim.optim + fx̄, r = estim.fx̄, estim.r + nx̂, nŵ, nym, nε, Nk = estim.nx̂, estim.nx̂, estim.nym, estim.nε, estim.Nk[] + nYm, nZ = estim.nym*Nk, get_nZ_mhe(estim.transcription, Nk, nx̂, nŵ) + # --- truncate vectors and matrices if Nk < He --- + U0, D0, Y0m = trunc_windows(estim) + Ẽ, F, G, J, B, ẽx̄, Tŵ, H̃, H̃_data, q̃, Z̃var = trunc_predmat(estim, estim.transcription) + invQ̂_Nk = trunc_cov(invQ̂_He, nx̂, Nk, estim.He) + invR̂_Nk = trunc_cov(invR̂_He, nym, Nk, estim.He) + # --- update F and fx̄ vectors for MHE predictions --- + F .= Y0m .+ B + mul!(F, G, U0, 1, 1) + (model.nd > 0) && mul!(F, J, D0, 1, 1) + fx̄ .= estim.x̂0arr_old + # --- handle NaN values in V̂ for the objective function --- + if any(isnan, F) + i_nan = findall(isnan, F) + Ẽ, F = copy(Ẽ), copy(F) + Ẽ[i_nan, :] .= 0 + F[i_nan] .= 0 + end + # --- update H̃, q̃ and r vectors for quadratic optimization --- + ẼZ̃ = [ẽx̄; Ẽ] + FZ̃ = [fx̄; F] + M_Nk = [invP̄ zeros(NT, nx̂, nYm); zeros(NT, nYm, nx̂) invR̂_Nk] + Ñ_Nk = [fill(C, nε, nε) zeros(NT, nε, nZ); zeros(NT, nZ, nε) Tŵ'invQ̂_Nk*Tŵ] + M_Nk_ẼZ̃ = M_Nk*ẼZ̃ + mul!(q̃, M_Nk_ẼZ̃', FZ̃) + lmul!(2, q̃) + r .= dot(FZ̃, M_Nk, FZ̃) + H̃_data .= Ñ_Nk + mul!(H̃_data, ẼZ̃', M_Nk_ẼZ̃, 1, 1) + lmul!(2, H̃_data) + # --- update the quadratic objective function --- + JuMP.set_objective_function(optim, obj_quadprog(Z̃var, H̃, q̃)) + return nothing +end +"Does nothing if `model` is not a [`LinModel`](@ref)." +initpred!(::MovingHorizonEstimator, ::SimModel) = nothing + "Get the estimated state at arrival from the decision vector `Z̃`." function getarrival!(x̂0arr, estim::MovingHorizonEstimator, Z̃) nx̃ = estim.nε + estim.nx̂ @@ -348,7 +423,6 @@ function add_data_windows!(estim::MovingHorizonEstimator, y0m, d0, u0=estim.last Nk = estim.Nk[] p = estim.direct ? 0 : 1 # u0 argument is u0(k-1) if estim.direct, else u0(k) x̂0_old = estim.x̂0 # x̂0_old is x̂0(k-1|k-1) if estim.direct, else x̂0(k|k-1) - ŵ = 0 # ŵ(k-1+p) = 0 for warm-start estim.Nk .+= 1 Nk = estim.Nk[] ismoving = (Nk > estim.He) @@ -368,8 +442,6 @@ function add_data_windows!(estim::MovingHorizonEstimator, y0m, d0, u0=estim.last estim.Ue[1:end-nu] .= @views estim.Ue[nu+1:end] estim.U0[end-nu+1:end] .= u0 estim.Ue[(end-nu+1 - nu):(end - nu)] .= u0 .+ model.uop - estim.Ŵ[1:end-nŵ] .= @views estim.Ŵ[nŵ+1:end] - estim.Ŵ[end-nŵ+1:end] .= ŵ estim.X̂0_old[1:end-nx̂] .= @views estim.X̂0_old[nx̂+1:end] estim.X̂0_old[end-nx̂+1:end] .= x̂0_old estim.Nk .= estim.He @@ -382,179 +454,12 @@ function add_data_windows!(estim::MovingHorizonEstimator, y0m, d0, u0=estim.last end estim.U0[(1 + nu*(Nk-1)):(nu*Nk)] .= u0 estim.Ue[(1 + nu*(Nk-1)):(nu*Nk)] .= u0 .+ model.uop - estim.Ŵ[(1 + nŵ*(Nk-1)):(nŵ*Nk)] .= ŵ estim.X̂0_old[(1 + nx̂*(Nk-1)):(nx̂*Nk)] .= x̂0_old end # --- update the arrival state estimated at k-Nk --- estim.x̂0arr_old .= @views estim.X̂0_old[1:nx̂] return ismoving end - -@doc raw""" - initpred!(estim::MovingHorizonEstimator, model::LinModel) -> nothing - -Init quadratic optimization matrices `F, fx̄, H̃, q̃, r` for [`MovingHorizonEstimator`](@ref). - -See [`init_predmat_mhe`](@ref) for the definition of the vectors ``\mathbf{F, f_x̄}``. It -also inits `estim.optim` objective function, expressed as the quadratic general form: -```math - J = \min_{\mathbf{Z̃}} \frac{1}{2}\mathbf{Z̃' H̃ Z̃} + \mathbf{q̃' Z̃} + r -``` -in which ``\mathbf{Z̃} = [\begin{smallmatrix} ε \\ \mathbf{Z} \end{smallmatrix}]``. Note that -``r`` is useless at optimization but required to evaluate the objective minima ``J``. The -Hessian ``\mathbf{H̃}`` matrix of the quadratic general form is not constant here because -of the time-varying ``\mathbf{P̄}`` covariance . The computed variables are: -```math -\begin{aligned} - \mathbf{F} &= \mathbf{G U_0} + \mathbf{J D_0} + \mathbf{Y_0^m} + \mathbf{B} \\ - \mathbf{f_x̄} &= \mathbf{x̂_0^†}(k-N_k+1) \\ - \mathbf{F_Z̃} &= [\begin{smallmatrix}\mathbf{f_x̄} \\ \mathbf{F} \end{smallmatrix}] \\ - \mathbf{Ẽ_Z̃} &= [\begin{smallmatrix}\mathbf{ẽ_x̄} \\ \mathbf{Ẽ} \end{smallmatrix}] \\ - \mathbf{M}_{N_k} &= \mathrm{diag}(\mathbf{P̄}^{-1}, \mathbf{R̂}_{N_k}^{-1}) \\ - \mathbf{Ñ}_{N_k} &= \mathrm{diag}(C, \mathbf{0}, \mathbf{Q̂}_{N_k}^{-1}) \\ - \mathbf{H̃} &= 2(\mathbf{Ẽ_Z̃}' \mathbf{M}_{N_k} \mathbf{Ẽ_Z̃} + \mathbf{Ñ}_{N_k}) \\ - \mathbf{q̃} &= 2(\mathbf{M}_{N_k} \mathbf{Ẽ_Z̃})' \mathbf{F_Z̃} \\ - r &= \mathbf{F_Z̃}' \mathbf{M}_{N_k} \mathbf{F_Z̃} -\end{aligned} -``` -""" -function initpred!(estim::MovingHorizonEstimator, model::LinModel) - invP̄, invQ̂_He, invR̂_He = estim.cov.invP̄, estim.cov.invQ̂_He, estim.cov.invR̂_He - F, C, optim = estim.F, estim.C, estim.optim - nx̂, nŵ, nym, nε, Nk = estim.nx̂, estim.nx̂, estim.nym, estim.nε, estim.Nk[] - nU, nYm, nŴ, nD = model.nu*Nk, estim.nym*Nk, nŵ*Nk, model.nd*(Nk+1) - nZ̃ = nε + nx̂ + nŴ - # --- truncate vector and matrices if necessary --- - if Nk < estim.He - # avoid views since allocations only when Nk < He and we want fast mul!: - Y0m, B = estim.Y0m[1:nYm], estim.B[1:nYm] - G, U0 = estim.G[1:nYm, 1:nU], estim.U0[1:nU] - J, D0 = estim.J[1:nYm, 1:nD], estim.D0[1:nD] - Ẽ, ẽx̄ = estim.Ẽ[1:nYm, 1:nZ̃], estim.ẽx̄[:, 1:nZ̃] - F, q̃ = @views estim.F[1:nYm], estim.q̃[1:nZ̃] - H̃_data = @views estim.H̃.data[1:nZ̃, 1:nZ̃] - H̃ = @views estim.H̃[1:nZ̃, 1:nZ̃] - Z̃var = @views optim[:Z̃var][1:nZ̃] - else - Y0m, B = estim.Y0m, estim.B - G, U0 = estim.G, estim.U0 - J, D0 = estim.J, estim.D0 - Ẽ, ẽx̄ = estim.Ẽ, estim.ẽx̄ - F, q̃ = estim.F, estim.q̃ - H̃_data = estim.H̃.data - H̃ = estim.H̃ - Z̃var = optim[:Z̃var] - end - invQ̂_Nk = trunc_cov(invQ̂_He, nx̂, Nk, estim.He) - invR̂_Nk = trunc_cov(invR̂_He, nym, Nk, estim.He) - fx̄ = estim.fx̄ - r = estim.r - # --- update F and fx̄ vectors for MHE predictions --- - F .= Y0m .+ B - mul!(F, G, U0, 1, 1) - (model.nd > 0) && mul!(F, J, D0, 1, 1) - fx̄ .= estim.x̂0arr_old - if any(isnan, F) # ignore NaN values in V̂ for the objective function: - i_nan = findall(isnan, F) - Ẽ, F = copy(Ẽ), copy(F) - Ẽ[i_nan, :] .= 0 - F[i_nan] .= 0 - end - # --- update H̃, q̃ and p vectors for quadratic optimization --- - ẼZ̃ = [ẽx̄; Ẽ] - FZ̃ = [fx̄; F] - M_Nk = [invP̄ zeros(nx̂, nYm); zeros(nYm, nx̂) invR̂_Nk] - Ñ_Nk = [fill(C, nε, nε) zeros(nε, nx̂+nŴ); zeros(nx̂, nε+nx̂+nŴ); zeros(nŴ, nε+nx̂) invQ̂_Nk] - M_Nk_ẼZ̃ = M_Nk*ẼZ̃ - mul!(q̃, M_Nk_ẼZ̃', FZ̃) - lmul!(2, q̃) - r .= dot(FZ̃, M_Nk, FZ̃) - H̃_data .= Ñ_Nk - mul!(H̃_data, ẼZ̃', M_Nk_ẼZ̃, 1, 1) - lmul!(2, H̃_data) - JuMP.set_objective_function(optim, obj_quadprog(Z̃var, H̃, q̃)) - return nothing -end -"Does nothing if `model` is not a [`LinModel`](@ref)." -initpred!(::MovingHorizonEstimator, ::SimModel) = nothing - -@doc raw""" - linconstraint!(estim::MovingHorizonEstimator, model::LinModel) - -Set `b` vector for the linear model inequality constraints (``\mathbf{A Z̃ ≤ b}``) of MHE. - -Also init ``\mathbf{F_x̂ = G_x̂ U_0 + J_x̂ D_0 + B_x̂}`` vector for the state constraints, see -[`init_predmat_mhe`](@ref). -""" -function linconstraint!(estim::MovingHorizonEstimator, model::LinModel) - nx̂, nŵ, nym, Nk = estim.nx̂, estim.nx̂, estim.nym, estim.Nk[] - nU, nX̂, nD = model.nu*Nk, estim.nx̂*Nk, model.nd*(Nk+1) - # --- truncate vector and matrices if necessary --- - if Nk < estim.He - # avoid views since allocations only when Nk < He and we want fast mul!: - Bx̂ = estim.con.Bx̂[1:nX̂] - Gx̂, U0 = estim.con.Gx̂[1:nX̂, 1:nU], estim.U0[1:nU] - Jx̂, D0 = estim.con.Jx̂[1:nX̂, 1:nD], estim.D0[1:nD] - Fx̂ = @views estim.con.Fx̂[1:nX̂] - else - Bx̂ = estim.con.Bx̂ - Gx̂, U0 = estim.con.Gx̂, estim.U0 - Jx̂, D0 = estim.con.Jx̂, estim.D0 - Fx̂ = estim.con.Fx̂ - end - X̂0min, X̂0max = trunc_bounds(estim, estim.con.X̂0min, estim.con.X̂0max, nx̂) - Ŵmin, Ŵmax = trunc_bounds(estim, estim.con.Ŵmin, estim.con.Ŵmax, nŵ) - V̂min, V̂max = trunc_bounds(estim, estim.con.V̂min, estim.con.V̂max, nym) - # --- update Fx̂ vectors for MHE state constraints --- - Fx̂ .= Bx̂ - mul!(Fx̂, Gx̂, U0, 1, 1) - model.nd > 0 && mul!(Fx̂, Jx̂, D0, 1, 1) - # --- update b vector for linear inequality constraints --- - nX̂_He, nŴ_He, nV̂_He = length(X̂0min), length(Ŵmin), length(V̂min) - nx̂ = length(estim.con.x̂0min) - n = 0 - estim.con.b[(n+1):(n+nx̂)] .= @. -estim.con.x̂0min - n += nx̂ - estim.con.b[(n+1):(n+nx̂)] .= @. +estim.con.x̂0max - n += nx̂ - estim.con.b[(n+1):(n+nX̂_He)] .= @. -X̂0min + estim.con.Fx̂ - n += nX̂_He - estim.con.b[(n+1):(n+nX̂_He)] .= @. +X̂0max - estim.con.Fx̂ - n += nX̂_He - estim.con.b[(n+1):(n+nŴ_He)] .= @. -Ŵmin - n += nŴ_He - estim.con.b[(n+1):(n+nŴ_He)] .= @. +Ŵmax - n += nŴ_He - estim.con.b[(n+1):(n+nV̂_He)] .= @. -V̂min + estim.F - n += nV̂_He - estim.con.b[(n+1):(n+nV̂_He)] .= @. +V̂max - estim.F - if any(estim.con.i_b) - lincon = estim.optim[:linconstraint] - JuMP.set_normalized_rhs(lincon, estim.con.b[estim.con.i_b]) - end - return nothing -end - -"Set `b` excluding state and sensor noise bounds if `model` is not a [`LinModel`](@ref)." -function linconstraint!(estim::MovingHorizonEstimator, ::SimModel) - # --- truncate vector and matrices if necessary --- - Ŵmin, Ŵmax = trunc_bounds(estim, estim.con.Ŵmin, estim.con.Ŵmax, estim.nx̂) - # --- update b vector for linear inequality constraints --- - nx̂, nŴ_He = length(estim.con.x̂0min), length(Ŵmin) - n = 0 - estim.con.b[(n+1):(n+nx̂)] .= @. -estim.con.x̂0min - n += nx̂ - estim.con.b[(n+1):(n+nx̂)] .= @. +estim.con.x̂0max - n += nx̂ - estim.con.b[(n+1):(n+nŴ_He)] .= @. -Ŵmin - n += nŴ_He - estim.con.b[(n+1):(n+nŴ_He)] .= @. +Ŵmax - if any(estim.con.i_b) - lincon = estim.optim[:linconstraint] - JuMP.set_normalized_rhs(lincon, estim.con.b[estim.con.i_b]) - end - return nothing -end "Truncate the bounds `Bmin` and `Bmax` to the window size `Nk` if `Nk < He`." function trunc_bounds(estim::MovingHorizonEstimator{NT}, Bmin, Bmax, n) where NT<:Real @@ -584,11 +489,9 @@ If first warm-starts the solver with [`set_warmstart_mhe!`](@ref). It then calls [`getinfo`](@ref) results in the debug log [if activated](@extref Julia Example:-Enable-debug-level-messages). """ function optim_objective!(estim::MovingHorizonEstimator{NT}) where NT<:Real - model, optim, buffer = estim.model, estim.optim, estim.buffer - nŵ, nx̂, Nk = estim.nx̂, estim.nx̂, estim.Nk[] - nx̃ = estim.nε + nx̂ + optim = estim.optim Z̃var::Vector{JuMP.VariableRef} = optim[:Z̃var] - Z̃s = set_warmstart_mhe!(estim, Z̃var) + Z̃s = set_warmstart_mhe!(estim, estim.transcription, Z̃var) # ------- solve optimization problem -------------- try JuMP.optimize!(optim) @@ -620,72 +523,49 @@ function optim_objective!(estim::MovingHorizonEstimator{NT}) where NT<:Real @debug info2debugstr(getinfo(estim)) end if iserror(optim) - estim.Z̃[1:nx̃+nŵ*Nk] .= @views Z̃s[1:nx̃+nŵ*Nk] + estim.Z̃ .= Z̃s else - estim.Z̃[1:nx̃+nŵ*Nk] .= @views JuMP.value.(Z̃var[1:nx̃+nŵ*Nk]) + estim.Z̃ .= JuMP.value.(Z̃var) end - # --------- update estimate ----------------------- - x̂0arr, û0, ŷ0, k = buffer.x̂, buffer.û, buffer.ŷ, buffer.k - V̂, X̂0 = buffer.V̂, buffer.X̂ - estim.Ŵ[1:nŵ*Nk] .= @views estim.Z̃[nx̃+1:nx̃+nŵ*Nk] # update Ŵ with optimum for warm-start - getarrival!(x̂0arr, estim, estim.Z̃) - predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim, model, x̂0arr, estim.Ŵ, estim.Z̃) - x̂0corrORnext = @views X̂0[((Nk-1)*nx̂+1):(Nk*nx̂)] - estim.x̂0 .= x̂0corrORnext + fill0unused!(estim.Z̃, estim, estim.transcription) return estim.Z̃ end @doc raw""" - set_warmstart_mhe!(estim::MovingHorizonEstimator, Z̃var) -> Z̃s + getstate!(estim::MovingHorizonEstimator, Z̃) -Set and return the warm-start value of `Z̃var` for [`MovingHorizonEstimator`](@ref). +Get current or next state estimate `x̂0corrORnext` from the solution `Z̃` and store it. -If supported by `estim.optim`, it warm-starts the solver at: -```math -\mathbf{Z̃_s} = -\begin{bmatrix} - ε_{k-1} \\ - \mathbf{x̂}_{k-1}(k-N_k+p) \\ - \mathbf{ŵ}_{k-1}(k-N_k+p+0) \\ - \mathbf{ŵ}_{k-1}(k-N_k+p+1) \\ - \vdots \\ - \mathbf{ŵ}_{k-1}(k-p-2) \\ - \mathbf{0} \\ -\end{bmatrix} -``` -where ``ε_{k-1}``, ``\mathbf{x̂}_{k-1}(k-N_k+p)`` and ``\mathbf{ŵ}_{k-1}(k-j)`` are -respectively the slack variable, the arrival state estimate and the process noise estimates -computed at the last time step ``k-1``. If the objective function is not finite at this -point, all the process noises ``\mathbf{ŵ}_{k-1}(k-j)`` are warm-started at zeros. The -method mutates all the arguments. +It extract and store in `estim.x̂0` the current corrected state if `estim.direct`, or the +next one otherwise. """ -function set_warmstart_mhe!(estim::MovingHorizonEstimator{NT}, Z̃var) where NT<:Real - model, buffer = estim.model, estim.buffer - nε, nx̂, nŵ, Nk = estim.nε, estim.nx̂, estim.nx̂, estim.Nk[] - nx̃ = nε + nx̂ - Z̃s = estim.buffer.Z̃ - û0, ŷ0, x̄, k = buffer.û, buffer.ŷ, buffer.x̂, buffer.k - # --- slack variable ε --- - estim.nε == 1 && (Z̃s[begin] = estim.Z̃[begin]) - # --- arrival state estimate x̂0arr --- - Z̃s[nε+1:nx̃] = estim.x̂0arr_old - # --- process noise estimates Ŵ --- - Z̃s[nx̃+1:end] = estim.Ŵ - # verify definiteness of objective function: - V̂, X̂0 = estim.buffer.V̂, estim.buffer.X̂ - x̄ .= 0 # x̂0arr == x̂arr_old implies the error at arrival x̄ is zero - predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim, model, estim.x̂0arr_old, estim.Ŵ, Z̃s) - Js = obj_nonlinprog(estim, model, x̄, V̂, estim.Ŵ, Z̃s) - if !isfinite(Js) - Z̃s[nx̃+1:end] .= 0 +function getstate!(estim::MovingHorizonEstimator, Z̃) + buffer, nx̂, Nk = estim.buffer, estim.nx̂, estim.Nk[] + x̂0arr, û0, ŷ0, k = buffer.x̂, buffer.û, buffer.ŷ, buffer.k + V̂, Ŵ, X̂0 = buffer.V̂, buffer.Ŵ, buffer.X̂ + getŴ!(Ŵ, estim, estim.transcription, estim.Z̃) + getarrival!(x̂0arr, estim, Z̃) + predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim, estim.model, x̂0arr, Ŵ, Z̃) + x̂0corrORnext = @views X̂0[((Nk-1)*nx̂+1):(Nk*nx̂)] + estim.x̂0 .= x̂0corrORnext + return nothing +end + +"Truncate and return the data windows if `Nk < He" +function trunc_windows(estim::MovingHorizonEstimator) + model, Nk = estim.model, estim.Nk[] + nym, nu, nd = estim.nym, model.nu, model.nd + nU, nYm, nD = nu*Nk, nym*Nk, nd*(Nk+1) + if Nk < estim.He # avoid views since allocations only when Nk < He and we want fast mul! + U0 = estim.U0[1:nU] + D0 = estim.D0[1:nD] + Y0m = estim.Y0m[1:nYm] + else + U0 = estim.U0 + D0 = estim.D0 + Y0m = estim.Y0m end - # --- unused variable in Z̃ (applied only when Nk ≠ He) --- - # We force the update of the NLP gradient and jacobian by warm-starting the unused - # variable in Z̃ at 1. Since estim.Ŵ is initialized with 0s, at least 1 variable in Z̃s - # will be inevitably different at the following time step. - Z̃s[nx̃+Nk*nŵ+1:end] .= 1 - JuMP.set_start_value.(Z̃var, Z̃s) - return Z̃s + return U0, D0, Y0m end "Truncate the inverse covariance `invA_He` to the window size `Nk` if `Nk < He`." @@ -1095,12 +975,13 @@ function setmodel_estimator!( estim.x̂0 .-= estim.x̂op # convert x̂ to x̂0 with the new operating point # --- predictions matrices --- E, G, J, B, _ , Ex̂, Gx̂, Jx̂, Bx̂ = init_predmat_mhe( - model, He, estim.i_ym, + model, transcription, + He, estim.i_ym, estim.Â, estim.B̂u, estim.Ĉm, estim.B̂d, estim.D̂dm, estim.x̂op, estim.f̂op, estim.direct ) - A_X̂min, A_X̂max, Ẽx̂ = relaxX̂(model, nε, con.C_x̂min, con.C_x̂max, Ex̂) - A_V̂min, A_V̂max, Ẽ = relaxV̂(model, nε, con.C_v̂min, con.C_v̂max, E) + A_X̂min, A_X̂max, Ẽx̂ = relaxX̂(Ex̂, con.C_x̂min, con.C_x̂max, nε) + A_V̂min, A_V̂max, Ẽ = relaxV̂(E, con.C_v̂min, con.C_v̂max, nε) estim.Ẽ .= Ẽ estim.G .= G estim.J .= J diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl new file mode 100644 index 000000000..4f3c97a73 --- /dev/null +++ b/src/estimator/mhe/transcription.jl @@ -0,0 +1,781 @@ +"Get the number of elements in the optimization decision vector `Z`" +get_nZ_mhe(::SingleShooting, He, nx̂, nŵ) = nx̂ + nŵ*He +get_nZ_mhe(::TranscriptionMethod, He, nx̂, nŵ) = nx̂ + nx̂*He + nŵ*He + +@doc raw""" + init_predmat_mhe( + model::LinModel, transcription::SingleShooting, + He, i_ym, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op, direct + ) -> E, G, J, B, ex̄, Ex̂, Gx̂, Jx̂, Bx̂ + +Construct the MHE prediction matrices for [`LinModel`](@ref) and [`SingleShooting`](@ref). + +We first introduce the deviation vector of the estimated state at arrival +``\mathbf{x̂_0}(k-N_k+p) = \mathbf{x̂}_k(k-N_k+p) - \mathbf{x̂_{op}}`` (see [`setop!`](@ref)), +and the vector ``\mathbf{Z} = [\begin{smallmatrix} \mathbf{x̂_0}(k-N_k+p) +\\ \mathbf{Ŵ} \end{smallmatrix}]`` with the decision variables. Setting the constant ``p=0`` +produces an estimator in the current form, while the prediction form is obtained with +``p=1``. The estimated sensor noises from time ``k-N_k+1`` to ``k`` are computed by: +```math +\begin{aligned} + \mathbf{V̂} = \mathbf{Y_0^m - Ŷ_0^m} &= \mathbf{E Z + G U_0 + J D_0 + Y_0^m + B} \\ + &= \mathbf{E Z + F} +\end{aligned} +``` +in which ``\mathbf{U_0}`` and ``\mathbf{Y_0^m}`` respectively include the deviation values +of the manipulated inputs ``\mathbf{u_0}(k-j+p)`` from ``j=N_k`` to ``1`` and measured +outputs ``\mathbf{y_0^m}(k-j+1)`` from ``j=N_k`` to ``1``. The vector ``\mathbf{D_0}`` +includes the the measured disturbance deviation values ``\mathbf{d_0}(k-j)`` from from +``j=N_k`` to ``0``, thus one additional data point. The constant ``\mathbf{B}`` is the +contribution for non-zero state ``\mathbf{x̂_{op}}`` and state update ``\mathbf{f̂_{op}}`` +operating points (for linearization, see [`augment_model`](@ref) and [`linearize`](@ref)). +The method also returns the matrices for the estimation error at arrival: +```math + \mathbf{x̄} = \mathbf{x̂_0^†}(k-N_k+p) - \mathbf{x̂_0}(k-N_k+p) = \mathbf{e_x̄ Z + f_x̄} +``` +in which ``\mathbf{e_x̄} = [\begin{smallmatrix} -\mathbf{I} & \mathbf{0} & \cdots & \mathbf{0} \end{smallmatrix}]``, +and ``\mathbf{f_x̄} = \mathbf{x̂_0^†}(k-N_k+p)``. The latter is the deviation vector of the +state at arrival, estimated at time ``k-N_k``, i.e. ``\mathbf{x̂_0^†}(k-N_k+p) = +\mathbf{x̂}_{k-N_k}(k-N_k+p) - \mathbf{x̂_{op}}``. Lastly, the estimates ``\mathbf{x̂_0}(k-j+p)`` +from ``j=N_k-1`` to ``0``, also in deviation form, are computed with: +```math +\begin{aligned} + \mathbf{X̂_0} &= \mathbf{E_x̂ Z + G_x̂ U_0 + J_x̂ D_0 + B_x̂} \\ + &= \mathbf{E_x̂ Z + F_x̂} +\end{aligned} +``` +The matrices ``\mathbf{E, G, J, B, E_x̂, G_x̂, J_x̂, B_x̂}`` are defined in the Extended Help +section. The vectors ``\mathbf{F, F_x̂, f_x̄}`` are recalculated at each discrete time step, +see [`initpred!(::MovingHorizonEstimator, ::LinModel)`](@ref) and [`linconstraint!(::MovingHorizonEstimator, ::LinModel)`](@ref). + +# Extended Help +!!! details "Extended Help" + Using the augmented process model matrices ``\mathbf{Â, B̂_u, Ĉ^m, B̂_d, D̂_d^m}``, and the + function ``\mathbf{S}(j) = ∑_{i=0}^j \mathbf{Â}^i``, the prediction matrices for the + sensor noises depend on the constant ``p``. For ``p=0``, the matrices are computed by + (notice the minus signs after the equalities): + ```math + \begin{aligned} + \mathbf{E} &= - \begin{bmatrix} + \mathbf{Ĉ^m}\mathbf{Â}^{1} & \mathbf{Ĉ^m}\mathbf{Â}^{0} & \cdots & \mathbf{0} \\ + \mathbf{Ĉ^m}\mathbf{Â}^{2} & \mathbf{Ĉ^m}\mathbf{Â}^{1} & \cdots & \mathbf{0} \\ + \vdots & \vdots & \ddots & \vdots \\ + \mathbf{Ĉ^m}\mathbf{Â}^{H_e} & \mathbf{Ĉ^m}\mathbf{Â}^{H_e-1} & \cdots & \mathbf{Ĉ^m}\mathbf{Â}^{0} \end{bmatrix} \\ + \mathbf{G} &= - \begin{bmatrix} + \mathbf{Ĉ^m}\mathbf{Â}^{0}\mathbf{B̂_u} & \mathbf{0} & \cdots & \mathbf{0} \\ + \mathbf{Ĉ^m}\mathbf{Â}^{1}\mathbf{B̂_u} & \mathbf{Ĉ^m}\mathbf{Â}^{0}\mathbf{B̂_u} & \cdots & \mathbf{0} \\ + \vdots & \vdots & \ddots & \vdots \\ + \mathbf{Ĉ^m}\mathbf{Â}^{H_e-1}\mathbf{B̂_u} & \mathbf{Ĉ^m}\mathbf{Â}^{H_e-2}\mathbf{B̂_u} & \cdots & \mathbf{Ĉ^m}\mathbf{Â}^{0}\mathbf{B̂_u} \end{bmatrix} \\ + \mathbf{J} &= - \begin{bmatrix} + \mathbf{Ĉ^m}\mathbf{Â}^{0}\mathbf{B̂_d} & \mathbf{D̂_d^m} & \mathbf{0} & \cdots & \mathbf{0} \\ + \mathbf{Ĉ^m}\mathbf{Â}^{1}\mathbf{B̂_d} & \mathbf{Ĉ^m}\mathbf{Â}^{0}\mathbf{B̂_d} & \mathbf{D̂_d^m} & \cdots & \mathbf{0} \\ + \vdots & \vdots & \vdots & \ddots & \vdots \\ + \mathbf{Ĉ^m}\mathbf{Â}^{H_e-1}\mathbf{B̂_d} & \mathbf{Ĉ^m}\mathbf{Â}^{H_e-2}\mathbf{B̂_d} & \mathbf{Ĉ^m}\mathbf{Â}^{H_e-3}\mathbf{B̂_d} & \cdots & \mathbf{D̂_d^m} \end{bmatrix} \\ + \mathbf{B} &= - \begin{bmatrix} + \mathbf{Ĉ^m S}(0) \\ + \mathbf{Ĉ^m S}(1) \\ + \vdots \\ + \mathbf{Ĉ^m S}(H_e-1) \end{bmatrix} \mathbf{\big(f̂_{op} - x̂_{op}\big)} + \end{aligned} + ``` + or, for ``p=1``, the matrices are given by: + ```math + \begin{aligned} + \mathbf{E} &= - \begin{bmatrix} + \mathbf{Ĉ^m}\mathbf{Â}^{0} & \mathbf{0} & \cdots & \mathbf{0} \\ + \mathbf{Ĉ^m}\mathbf{Â}^{1} & \mathbf{Ĉ^m}\mathbf{Â}^{0} & \cdots & \mathbf{0} \\ + \vdots & \vdots & \ddots & \vdots \\ + \mathbf{Ĉ^m}\mathbf{Â}^{H_e-1} & \mathbf{Ĉ^m}\mathbf{Â}^{H_e-2} & \cdots & \mathbf{0} \end{bmatrix} \\ + \mathbf{G} &= - \begin{bmatrix} + \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} \\ + \mathbf{Ĉ^m}\mathbf{Â}^{0}\mathbf{B̂_u} & \mathbf{0} & \cdots & \mathbf{0} \\ + \vdots & \vdots & \ddots & \vdots \\ + \mathbf{Ĉ^m}\mathbf{Â}^{H_e-2}\mathbf{B̂_u} & \mathbf{Ĉ^m}\mathbf{Â}^{H_e-3}\mathbf{B̂_u} & \cdots & \mathbf{0} \end{bmatrix} \\ + \mathbf{J} &= - \begin{bmatrix} + \mathbf{0} & \mathbf{D̂_d^m} & \mathbf{0} & \cdots & \mathbf{0} \\ + \mathbf{0} & \mathbf{Ĉ^m}\mathbf{Â}^{0}\mathbf{B̂_d} & \mathbf{D̂_d^m} & \cdots & \mathbf{0} \\ + \vdots & \vdots & \vdots & \ddots & \vdots \\ + \mathbf{0} & \mathbf{Ĉ^m}\mathbf{Â}^{H_e-2}\mathbf{B̂_d} & \mathbf{Ĉ^m}\mathbf{Â}^{H_e-3}\mathbf{B̂_d} & \cdots & \mathbf{D̂_d^m} \end{bmatrix} \\ + \mathbf{B} &= - \begin{bmatrix} + \mathbf{0} \\ + \mathbf{Ĉ^m S}(0) \\ + \vdots \\ + \mathbf{Ĉ^m S}(H_e-2) \end{bmatrix} \mathbf{\big(f̂_{op} - x̂_{op}\big)} + \end{aligned} + ``` + The matrices for the estimated states are computed by: + ```math + \begin{aligned} + \mathbf{E_x̂} &= \begin{bmatrix} + \mathbf{Â}^{1} & \mathbf{A}^{0} & \cdots & \mathbf{0} \\ + \mathbf{Â}^{2} & \mathbf{Â}^{1} & \cdots & \mathbf{0} \\ + \vdots & \vdots & \ddots & \vdots \\ + \mathbf{Â}^{H_e} & \mathbf{Â}^{H_e-1} & \cdots & \mathbf{Â}^{0} \end{bmatrix} \\ + \mathbf{G_x̂} &= \begin{bmatrix} + \mathbf{Â}^{0}\mathbf{B̂_u} & \mathbf{0} & \cdots & \mathbf{0} \\ + \mathbf{Â}^{1}\mathbf{B̂_u} & \mathbf{Â}^{0}\mathbf{B̂_u} & \cdots & \mathbf{0} \\ + \vdots & \vdots & \ddots & \vdots \\ + \mathbf{Â}^{H_e-1}\mathbf{B̂_u} & \mathbf{Â}^{H_e-2}\mathbf{B̂_u} & \cdots & \mathbf{Â}^{0}\mathbf{B̂_u} \end{bmatrix} \\ + \mathbf{J_x̂^†} &= \begin{bmatrix} + \mathbf{Â}^{0}\mathbf{B̂_d} & \mathbf{0} & \cdots & \mathbf{0} \\ + \mathbf{Â}^{1}\mathbf{B̂_d} & \mathbf{Â}^{0}\mathbf{B̂_d} & \cdots & \mathbf{0} \\ + \vdots & \vdots & \ddots & \vdots \\ + \mathbf{Â}^{H_e-1}\mathbf{B̂_d} & \mathbf{Â}^{H_e-2}\mathbf{B̂_d} & \cdots & \mathbf{Â}^{0}\mathbf{B̂_d} \end{bmatrix} \ , \quad + \mathbf{J_x̂} = \begin{cases} + [\begin{smallmatrix} \mathbf{J_x̂^†} & \mathbf{0} \end{smallmatrix}] & p=0 \\ + [\begin{smallmatrix} \mathbf{0} & \mathbf{J_x̂^†} \end{smallmatrix}] & p=1 \end{cases} \\ + \mathbf{B_x̂} &= \begin{bmatrix} + \mathbf{S}(0) \\ + \mathbf{S}(1) \\ + \vdots \\ + \mathbf{S}(H_e-1) \end{bmatrix} \mathbf{\big(f̂_{op} - x̂_{op}\big)} + \end{aligned} + ``` + All these matrices are truncated when ``N_k < H_e`` (at the beginning). +""" +function init_predmat_mhe( + model::LinModel{NT}, ::SingleShooting, He, i_ym, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op, direct +) where {NT<:Real} + nu, nd = model.nu, model.nd + nym, nx̂ = length(i_ym), size(Â, 2) + nŵ = nx̂ + p = direct ? 0 : 1 + # --- pre-compute matrix powers --- + # Apow3D array : Apow[:,:,1] = A^0, Apow[:,:,2] = A^1, ... , Apow[:,:,He+1] = A^He + Âpow3D = Array{NT}(undef, nx̂, nx̂, He+1) + Âpow3D[:,:,1] = I(nx̂) + for j=2:He+1 + Âpow3D[:,:,j] = @views Âpow3D[:,:,j-1]*Â + end + # nĈm_Âpow3D array : similar indices as Apow3D + nĈm_Âpow3D = Array{NT}(undef, nym, nx̂, He+1) + nĈm_Âpow3D[:,:,1] = -Ĉm + for j=2:He+1 + nĈm_Âpow3D[:,:,j] = @views -Ĉm*Âpow3D[:,:,j] + end + # helper function to improve code clarity and be similar to eqs. in docstring: + getpower(array3D, power) = @views array3D[:,:, power+1] + # --- decision variables Z --- + nĈm_Âpow = reduce(vcat, getpower(nĈm_Âpow3D, i) for i=0:He) + E = zeros(NT, nym*He, nx̂ + nŵ*He) + col_begin = iszero(p) ? 1 : 0 + col_end = iszero(p) ? He : He-1 + i = 0 + for j=col_begin:col_end + iRow = (1 + i*nym):(nym*He) + iCol = (1:nŵ) .+ j*nŵ + E[iRow, iCol] = @views nĈm_Âpow[1:length(iRow) ,:] + i += 1 + end + iszero(p) && @views (E[:, 1:nx̂] = @views nĈm_Âpow[nym+1:end, :]) + ex̄ = [-I zeros(NT, nx̂, nŵ*He)] + Âpow_vec = reduce(vcat, getpower(Âpow3D, i) for i=0:He) + Ex̂ = zeros(NT, nx̂*He, nx̂ + nŵ*He) + i=0 + for j=1:He + iRow = (1 + i*nx̂):(nx̂*He) + iCol = (1:nŵ) .+ j*nŵ + Ex̂[iRow, iCol] = @views Âpow_vec[1:length(iRow) ,:] + i+=1 + end + Ex̂[:, 1:nx̂] = @views Âpow_vec[nx̂+1:end, :] + # --- manipulated inputs U --- + nĈm_Âpow_B̂u = reduce(vcat, getpower(nĈm_Âpow3D, i)*B̂u for i=0:He-1) + nĈm_Âpow_B̂u = [zeros(nym, nu) ; nĈm_Âpow_B̂u] + G = zeros(NT, nym*He, nu*He) + i=0 + col_begin = iszero(p) ? 1 : 0 + col_end = iszero(p) ? He-1 : He-2 + for j=col_begin:col_end + iRow = (1 + i*nym):(nym*He) + iCol = (1:nu) .+ j*nu + G[iRow, iCol] = @views nĈm_Âpow_B̂u[1:length(iRow) ,:] + i+=1 + end + iszero(p) && @views (G[:, 1:nu] = nĈm_Âpow_B̂u[nym+1:end, :]) + Âpow_B̂u = reduce(vcat, getpower(Âpow3D, i)*B̂u for i=0:He-1) + Gx̂ = zeros(NT, nx̂*He, nu*He) + for j=0:He-1 + iRow = (1 + j*nx̂):(nx̂*He) + iCol = (1:nu) .+ j*nu + Gx̂[iRow, iCol] = @views Âpow_B̂u[1:length(iRow) ,:] + end + # --- measured disturbances D --- + nĈm_Âpow_B̂d = reduce(vcat, getpower(nĈm_Âpow3D, i)*B̂d for i=0:He-1) + nĈm_Âpow_B̂d = [-D̂dm; nĈm_Âpow_B̂d] + J = zeros(NT, nym*He, nd*(He+1)) + i = 0 + for j=1:He + iRow = (1 + i*nym):(nym*He) + iCol = (1:nd) .+ j*nd + J[iRow, iCol] = nĈm_Âpow_B̂d[1:length(iRow) ,:] + i+=1 + end + iszero(p) && @views (J[:, 1:nd] = nĈm_Âpow_B̂d[nym+1:end, :]) + Âpow_B̂d = reduce(vcat, getpower(Âpow3D, i)*B̂d for i=0:He-1) + Jx̂ = zeros(NT, nx̂*He, nd*(He+1)) + for j=0:He-1 + iRow = (1 + j*nx̂):(nx̂*He) + iCol = (1:nd) .+ j*nd .+ p + Jx̂[iRow, iCol] = Âpow_B̂d[1:length(iRow) ,:] + end + # --- state x̂op and state update f̂op operating points --- + # Apow_csum 3D array : Apow_csum[:,:,1] = A^0, Apow_csum[:,:,2] = A^1 + A^0, ... + Âpow_csum = cumsum(Âpow3D, dims=3) + # helper function to improve code clarity and be similar to eqs. in docstring: + S(j) = @views Âpow_csum[:,:, j+1] + f̂_op_n_x̂op = (f̂op - x̂op) + coef_B = zeros(NT, nym*He, nx̂) + row_begin = iszero(p) ? 0 : 1 + row_end = iszero(p) ? He-1 : He-2 + j=0 + for i=row_begin:row_end + iRow = (1:nym) .+ nym*i + coef_B[iRow,:] = -Ĉm*S(j) + j+=1 + end + B = coef_B*f̂_op_n_x̂op + coef_Bx̂ = Matrix{NT}(undef, nx̂*He, nx̂) + for j=0:He-1 + iRow = (1:nx̂) .+ nx̂*j + coef_Bx̂[iRow,:] = S(j) + end + Bx̂ = coef_Bx̂*f̂_op_n_x̂op + return E, G, J, B, ex̄, Ex̂, Gx̂, Jx̂, Bx̂ +end + +""" + init_predmat_mhe( + model::LinModel, transcription::MultipleShooting, + He, i_ym, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op, direct + ) -> E, G, J, B, ex̄, Ex̂, Gx̂, Jx̂, Bx̂ + +TBW +""" +function init_predmat_mhe( + model::LinModel{NT}, ::MultipleShooting, + He, i_ym, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op, direct +) where {NT<:Real} + nu, nd = model.nu, model.nd + nym, nx̂ = length(i_ym), size(Â, 2) + nŵ = nx̂ + p = direct ? 0 : 1 + nX̂, nŴ, nV̂, nU, nD = nx̂*He, nŵ*He, nym*He, nu*He, nd*(He+1) + # --- decision variables Z --- + E = [zeros(NT, nV̂, (1-p)*nx̂) repeatdiag(-Ĉm, He) zeros(NT, nV̂, p*nx̂ + nŴ)] + ex̄ = [-I zeros(NT, nx̂, nX̂ + nŴ)] + Ex̂ = [zeros(NT, nX̂, nx̂) I zeros(NT, nX̂, nŴ)] + # --- manipulated inputs U --- + G = zeros(NT, nV̂, nU) + Gx̂ = zeros(NT, nX̂, nU) + # --- measured disturbances D --- + J = [zeros(NT, nV̂, nd) repeatdiag(-D̂dm, He)] + Jx̂ = zeros(NT, nX̂, nD) + # --- state x̂op and state update f̂op operating points --- + B = zeros(NT, nV̂) + Bx̂ = zeros(NT, nX̂) + return E, G, J, B, ex̄, Ex̂, Gx̂, Jx̂, Bx̂ +end + +""" + init_predmat_mhe( + model::SimModel, ::SingleShooting, He, i_ym, Â, _ , _ , _ , _ , _ , _ , _ + ) -> E, G, J, B, ex̄, Ex̂, Gx̂, Jx̂, Bx̂ + +Return empty matrices if `model` is not a [`LinModel`](@ref), except for `ex̄`. +""" +function init_predmat_mhe( + model::SimModel{NT}, ::SingleShooting, He, i_ym, Â, _ , _ , _ , _ , _ , _ , _ +) where {NT<:Real} + nym, nx̂ = length(i_ym), size(Â, 2) + nŵ = nx̂ + E = zeros(NT, 0, nx̂ + nŵ*He) + ex̄ = [-I zeros(NT, nx̂, nŵ*He)] + Ex̂ = zeros(NT, 0, nx̂ + nŵ*He) + G = zeros(NT, 0, model.nu*He) + Gx̂ = zeros(NT, 0, model.nu*He) + J = zeros(NT, 0, model.nd*(He+1)) + Jx̂ = zeros(NT, 0, model.nd*(He+1)) + B = zeros(NT, nym*He) + Bx̂ = zeros(NT, nx̂*He) + return E, G, J, B, ex̄, Ex̂, Gx̂, Jx̂, Bx̂ +end + +""" + init_defectmat_mhe( + model::LinModel, transcription::MultipleShooting, + He, i_ym, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op, direct + ) -> ES, GS, JS, BS + +TBW +""" +function init_defectmat_mhe( + model::LinModel{NT}, ::MultipleShooting, He, Â, B̂u, B̂d, x̂op, f̂op, direct +) where {NT<:Real} + nd = model.nd + nx̂ = size(Â, 2) + nŵ = nx̂ + nX̂ = nx̂*He + p = direct ? 0 : 1 + # --- decision variables Z --- + nI_nx̂ = Matrix{NT}(-I, nx̂, nx̂) + I_nx̂ = Matrix{NT}(I, nŵ, nŵ) + ES = [zeros(NT, nX̂, nx̂) repeatdiag(nI_nx̂, He) repeatdiag(I_nx̂, He)] + for j=1:He + iRowCol = (1:nx̂) .+ nx̂*(j-1) + ES[iRowCol, iRowCol] = Â + end + # --- manipulated inputs U --- + GS = repeatdiag(B̂u, He) + # --- measured disturbances D --- + JS = [zeros(NT, nX̂, p*nd) repeatdiag(B̂d, He) zeros(NT, nX̂, (1-p)*nd)] + # --- state x̂op and state update f̂op operating points --- + BS = repeat(f̂op - x̂op, He) + return ES, GS, JS, BS +end + +"Return empty matrices for [`SingleShooting`](@ref) transcription (N/A)." +function init_defectmat_mhe( + model::SimModel{NT}, ::SingleShooting, He, Â, _ , _ , _ , _ , _ +) where {NT<:Real} + nu, nd = model.nu, model.nd + nx̂ = size(Â, 2) + nŵ = nx̂ + ES = zeros(NT, 0, nx̂ + nŵ*He) + GS = zeros(NT, 0, nu*He) + JS = zeros(NT, 0, nd*(He+1)) + BS = zeros(NT, 0) + return ES, GS, JS, BS +end + +@doc raw""" + init_matconstraint_mhe( + model::LinModel, transcription::SingleShooting, Z̃min, Z̃max, nc, + x̂0min, x̂0max, X̂0min, X̂0max, Ŵmin, Ŵmax, V̂min, V̂max, args... + ) -> i_b, i_g, A, Aeq, neq + +Init `i_b`, `i_g`, `neq`, and `A` and `Aeq` matrices for all the MHE constraints. + +The linear and nonlinear inequality constraints are respectively defined as: +```math +\begin{aligned} + \mathbf{A Z̃ } &≤ \mathbf{b} \\ + \mathbf{A_{eq} Z̃} &= \mathbf{b_{eq}} \\ + \mathbf{g(Z̃)} &≤ \mathbf{0} \\ + \mathbf{g_{eq}(Z̃)} &= \mathbf{0} +\end{aligned} +``` +The argument `nc` is the number of custom nonlinear inequality constraints in +``\mathbf{g_c}``. `i_b` is a `BitVector` including the indices of ``\mathbf{b}`` that are +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_x̂min, A_x̂max, A_X̂min, A_X̂max, A_Ŵmin, A_Ŵmax, A_V̂min, A_V̂max, Aeq`. The integer `neq` is +the number of nonlinear equality constraints in ``\mathbf{g_{eq}}``. +""" +function init_matconstraint_mhe( + model::LinModel{NT}, transcription::TranscriptionMethod, Z̃min, Z̃max, nc, + x̂0min, x̂0max, X̂0min, X̂0max, Ŵmin, Ŵmax, V̂min, V̂max, args... +) where {NT<:Real} + if isempty(args) + A, Aeq, neq = nothing, nothing, nothing + else + A_x̂min, A_x̂max, A_X̂min, A_X̂max, A_Ŵmin, A_Ŵmax, A_V̂min, A_V̂max, Aeq = args + A = [A_x̂min; A_x̂max; A_X̂min; A_X̂max; A_Ŵmin; A_Ŵmax; A_V̂min; A_V̂max] + neq = 0 # number of nonlinear equality constraints + end + i_x̂min, i_x̂max = @. !isinf(x̂0min), !isinf(x̂0max) + i_X̂min, i_X̂max = @. !isinf(X̂0min), !isinf(X̂0max) + i_Ŵmin, i_Ŵmax = @. !isinf(Ŵmin), !isinf(Ŵmax) + i_V̂min, i_V̂max = @. !isinf(V̂min), !isinf(V̂max) + nx̂ = length(x̂0min) + nε = length(Z̃min) - length(Ŵmin) - nx̂ + deletex̂arr_lincon!(i_x̂min, i_x̂max, model, Z̃min, Z̃max, nε) + deleteŴ_lincon!(i_Ŵmin, i_Ŵmax, model, Z̃min, Z̃max, nx̂, nε) + i_b = [i_x̂min; i_x̂max; i_X̂min; i_X̂max; i_Ŵmin; i_Ŵmax; i_V̂min; i_V̂max] + i_g = trues(nc) + return i_b, i_g, A, Aeq, neq +end + +"Init `i_b, A` without state and sensor noise constraints if `model` is not a [`LinModel`](@ref)." +function init_matconstraint_mhe( + model::NonLinModel{NT}, transcription::SingleShooting, Z̃min, Z̃max, nc, + x̂0min, x̂0max, X̂0min, X̂0max, Ŵmin, Ŵmax, V̂min, V̂max, args... +) where {NT<:Real} + if isempty(args) + A, Aeq, neq = nothing, nothing, nothing + else + A_x̂min, A_x̂max, _ , _ , A_Ŵmin, A_Ŵmax, _ , _ , Aeq = args + A = [A_x̂min; A_x̂max; A_Ŵmin; A_Ŵmax] + neq = 0 # number of nonlinear equality constraints + end + i_x̂min, i_x̂max = @. !isinf(x̂0min), !isinf(x̂0max) + i_X̂min, i_X̂max = @. !isinf(X̂0min), !isinf(X̂0max) + i_Ŵmin, i_Ŵmax = @. !isinf(Ŵmin), !isinf(Ŵmax) + i_V̂min, i_V̂max = @. !isinf(V̂min), !isinf(V̂max) + nx̂ = length(x̂0min) + nε = length(Z̃min) - length(Ŵmin) - nx̂ + deletex̂arr_lincon!(i_x̂min, i_x̂max, model, Z̃min, Z̃max, nε) + deleteŴ_lincon!(i_Ŵmin, i_Ŵmax, model, Z̃min, Z̃max, nx̂, nε) + i_b = [i_x̂min; i_x̂max; i_Ŵmin; i_Ŵmax] + i_g = [i_X̂min; i_X̂max; i_V̂min; i_V̂max; trues(nc)] + return i_b, i_g, i_g, A, Aeq, neq +end + +"For [`SingleShooting`](@ref), truncate the end of prediction matrices if `Nk < He`" +function trunc_predmat(estim::MovingHorizonEstimator, transcription::SingleShooting) + model = estim.model + nx̂, nŵ, nym, nε, Nk = estim.nx̂, estim.nx̂, estim.nym, estim.nε, estim.Nk[] + nU, nYm, nŴ, nD = model.nu*Nk, nym*Nk, nŵ*Nk, model.nd*(Nk+1) + nZ = get_nZ_mhe(transcription, Nk, nx̂, nŵ) + nZ̃ = nε + nZ + if Nk < estim.He # avoid views since allocations only when Nk < He and we want fast mul! + Ẽ = estim.Ẽ[1:nYm, 1:nZ̃] + G, J, B = estim.G[1:nYm, 1:nU], estim.J[1:nYm, 1:nD], estim.B[1:nYm] + ẽx̄ = estim.ẽx̄[:, 1:nZ̃] + Tŵ = estim.Tŵ[1:nŴ, 1:nZ] + F = @views estim.F[1:nYm] # views here since they will store results + H̃_data = @views estim.H̃.data[1:nZ̃, 1:nZ̃] + H̃ = @views estim.H̃[1:nZ̃, 1:nZ̃] + q̃ = @views estim.q̃[1:nZ̃] + Z̃var = @views estim.optim[:Z̃var][1:nZ̃] + else + Ẽ, F, G, J, B = estim.Ẽ, estim.F, estim.G, estim.J, estim.B + ẽx̄, Tŵ = estim.ẽx̄, estim.Tŵ + H̃, H̃_data, q̃ = estim.H̃, estim.H̃.data, estim.q̃ + Z̃var = estim.optim[:Z̃var] + end + return Ẽ, F, G, J, B, ẽx̄, Tŵ, H̃, H̃_data, q̃, Z̃var +end + +"For [`MultipleShooting`](@ref), extract subparts of the prediction matrices if `Nk < He`." +function trunc_predmat(estim::MovingHorizonEstimator, ::MultipleShooting) + model = estim.model + nx̂, nŵ, nym, nε, Nk = estim.nx̂, estim.nx̂, estim.nym, estim.nε, estim.Nk[] + nU, nYm, nŴ, nD = model.nu*Nk, nym*Nk, nŵ*Nk, model.nd*(Nk+1) + nx̂_nX̂ = nx̂ + nx̂*Nk + nx̂_nX̂_He = nx̂ + nx̂*estim.He + if Nk < estim.He # avoid views since allocations only when Nk < He and we want fast mul! + i_Z̃_He = [(1):(nε + nx̂_nX̂); (nε + nx̂_nX̂_He + 1):(nε + nx̂_nX̂_He + nŴ)] + i_Z_He = [(1):(nx̂_nX̂); (nx̂_nX̂_He + 1):(nx̂_nX̂_He + nŴ)] + Ẽ = estim.Ẽ[1:nYm, i_Z̃_He] + G, J, B = estim.G[1:nYm, 1:nU], estim.J[1:nYm, 1:nD], estim.B[1:nYm] + ẽx̄ = estim.ẽx̄[:, i_Z̃_He] + Tŵ = estim.Tŵ[1:nŴ, i_Z_He] + F = @views estim.F[1:nYm] # views here since they will store results + H̃_data = @views estim.H̃.data[i_Z̃_He, i_Z̃_He] + H̃ = @views estim.H̃[i_Z̃_He, i_Z̃_He] + q̃ = @views estim.q̃[i_Z̃_He] + Z̃var = @views estim.optim[:Z̃var][i_Z̃_He] + else + Ẽ, F, G, J, B = estim.Ẽ, estim.F, estim.G, estim.J, estim.B + ẽx̄, Tŵ = estim.ẽx̄, estim.Tŵ + H̃, H̃_data, q̃ = estim.H̃, estim.H̃.data, estim.q̃ + Z̃var = estim.optim[:Z̃var] + end + return Ẽ, F, G, J, B, ẽx̄, Tŵ, H̃, H̃_data, q̃, Z̃var +end + +function trunc_defectmat(estim::MovingHorizonEstimator) + model, con = estim.model, estim.con + FS = con.FS + nx̂, nŵ, nε, Nk = estim.nx̂, estim.nx̂, estim.nε, estim.Nk[] + nU, nŴ, nX̂, nD = model.nu*Nk, nŵ*Nk, nx̂*Nk, model.nd*(Nk+1) + nx̂_nX̂ = nx̂ + nX̂ + nx̂_nX̂_He = nx̂ + nx̂*estim.He + if Nk < estim.He # avoid views since allocations only when Nk < He and we want fast mul! + i_Z̃_He = [(1):(nε + nx̂_nX̂); (nε + nx̂_nX̂_He + 1):(nε + nx̂_nX̂_He + nŴ)] + ẼS = con.ẼS[1:nX̂, i_Z̃_He] + GS, JS, BS = con.GS[1:nX̂, 1:nU], con.JS[1:nX̂, 1:nD], con.BS[1:nX̂] + FS = @views con.FS[1:nX̂] # views here since they will store results + Aeq = @views con.Aeq[1:nX̂, i_Z̃_He] + beq = @views con.beq[1:nX̂] + Z̃var = @views estim.optim[:Z̃var][i_Z̃_He] + else + ẼS, FS, GS, JS, BS = con.ẼS, con.FS, con.GS, con.JS, con.BS + Aeq = con.Aeq + beq = con.beq + Z̃var = estim.optim[:Z̃var] + end + return ẼS, FS, GS, JS, BS, Aeq, beq, Z̃var +end + +@doc raw""" + linconstraint!( + estim::MovingHorizonEstimator, model::LinModel, transcription::TranscriptionMethod + ) + +Set `b` vector for the linear model inequality constraints (``\mathbf{A Z̃ ≤ b}``) of MHE. + +Also init ``\mathbf{F_x̂ = G_x̂ U_0 + J_x̂ D_0 + B_x̂}`` vector for the state constraints, see +[`init_predmat_mhe`](@ref). +""" +function linconstraint!( + estim::MovingHorizonEstimator, model::LinModel, ::TranscriptionMethod +) + nx̂, nŵ, nym, Nk = estim.nx̂, estim.nx̂, estim.nym, estim.Nk[] + nU, nX̂, nD = model.nu*Nk, estim.nx̂*Nk, model.nd*(Nk+1) + # --- truncate vector and matrices if necessary --- + if Nk < estim.He + # avoid views since allocations only when Nk < He and we want fast mul!: + Bx̂ = estim.con.Bx̂[1:nX̂] + Gx̂, U0 = estim.con.Gx̂[1:nX̂, 1:nU], estim.U0[1:nU] + Jx̂, D0 = estim.con.Jx̂[1:nX̂, 1:nD], estim.D0[1:nD] + Fx̂ = @views estim.con.Fx̂[1:nX̂] + else + Bx̂ = estim.con.Bx̂ + Gx̂, U0 = estim.con.Gx̂, estim.U0 + Jx̂, D0 = estim.con.Jx̂, estim.D0 + Fx̂ = estim.con.Fx̂ + end + X̂0min, X̂0max = trunc_bounds(estim, estim.con.X̂0min, estim.con.X̂0max, nx̂) + Ŵmin, Ŵmax = trunc_bounds(estim, estim.con.Ŵmin, estim.con.Ŵmax, nŵ) + V̂min, V̂max = trunc_bounds(estim, estim.con.V̂min, estim.con.V̂max, nym) + # --- update Fx̂ vectors for MHE state constraints --- + Fx̂ .= Bx̂ + mul!(Fx̂, Gx̂, U0, 1, 1) + model.nd > 0 && mul!(Fx̂, Jx̂, D0, 1, 1) + # --- update b vector for linear inequality constraints --- + nX̂_He, nŴ_He, nV̂_He = length(X̂0min), length(Ŵmin), length(V̂min) + nx̂ = length(estim.con.x̂0min) + n = 0 + estim.con.b[(n+1):(n+nx̂)] .= @. -estim.con.x̂0min + n += nx̂ + estim.con.b[(n+1):(n+nx̂)] .= @. +estim.con.x̂0max + n += nx̂ + estim.con.b[(n+1):(n+nX̂_He)] .= @. -X̂0min + estim.con.Fx̂ + n += nX̂_He + estim.con.b[(n+1):(n+nX̂_He)] .= @. +X̂0max - estim.con.Fx̂ + n += nX̂_He + estim.con.b[(n+1):(n+nŴ_He)] .= @. -Ŵmin + n += nŴ_He + estim.con.b[(n+1):(n+nŴ_He)] .= @. +Ŵmax + n += nŴ_He + estim.con.b[(n+1):(n+nV̂_He)] .= @. -V̂min + estim.F + n += nV̂_He + estim.con.b[(n+1):(n+nV̂_He)] .= @. +V̂max - estim.F + if any(estim.con.i_b) + lincon = estim.optim[:linconstraint] + JuMP.set_normalized_rhs(lincon, estim.con.b[estim.con.i_b]) + end + return nothing +end + +"Set `b` excluding state and sensor noise bounds if `model` is not a [`LinModel`](@ref)." +function linconstraint!( + estim::MovingHorizonEstimator, ::SimModel, ::TranscriptionMethod +) + # --- truncate vector and matrices if necessary --- + Ŵmin, Ŵmax = trunc_bounds(estim, estim.con.Ŵmin, estim.con.Ŵmax, estim.nx̂) + # --- update b vector for linear inequality constraints --- + nx̂, nŴ_He = length(estim.con.x̂0min), length(Ŵmin) + n = 0 + estim.con.b[(n+1):(n+nx̂)] .= @. -estim.con.x̂0min + n += nx̂ + estim.con.b[(n+1):(n+nx̂)] .= @. +estim.con.x̂0max + n += nx̂ + estim.con.b[(n+1):(n+nŴ_He)] .= @. -Ŵmin + n += nŴ_He + estim.con.b[(n+1):(n+nŴ_He)] .= @. +Ŵmax + if any(estim.con.i_b) + lincon = estim.optim[:linconstraint] + JuMP.set_normalized_rhs(lincon, estim.con.b[estim.con.i_b]) + end + return nothing +end + +""" + linconstrainteq!( + estim::MovingHorizonEstimator, model::LinModel, ::TranscriptionMethod + ) + +TBW +""" +function linconstrainteq!( + estim::MovingHorizonEstimator, model::LinModel, transcription::TranscriptionMethod +) + optim = estim.optim + ẼS, FS, GS, JS, BS, Aeq, beq, Z̃var = trunc_defectmat(estim) + U0, D0 = trunc_windows(estim) + FS .= BS + mul!(FS, GS, U0, 1, 1) + if model.nd > 0 + mul!(FS, JS, D0, 1, 1) + end + beq .= @. -FS + Aeq .= @. ẼS + if haskey(optim, :linconstrainteq_temp) # temporary since only used once when Nk < He + JuMP.delete(optim, optim[:linconstrainteq_temp]) + JuMP.unregister(optim, :linconstrainteq_temp) + end + if estim.Nk[] < estim.He + if haskey(optim, :linconstrainteq) + JuMP.delete(optim, optim[:linconstrainteq]) + JuMP.unregister(optim, :linconstrainteq) + end + @constraint(optim, linconstrainteq_temp, Aeq*Z̃var .== beq) + else + if haskey(optim, :linconstrainteq) + JuMP.set_normalized_rhs(optim[:linconstrainteq], beq) + else + @constraint(optim, linconstrainteq, Aeq*Z̃var .== beq) + end + end + return nothing +end +linconstrainteq!(::MovingHorizonEstimator, ::LinModel, ::SingleShooting) = nothing + + + + +@doc raw""" + set_warmstart_mhe!( + estim::MovingHorizonEstimator, transcription::SingleShooting, Z̃var + ) -> Z̃s + +Set and return the warm-start value of `Z̃var` for [`MovingHorizonEstimator`](@ref). + +If supported by `estim.optim` and based a [`SingleShooting`](@ref) transcription, it +warm-starts the solver at: +```math +\mathbf{Z̃_s} = +\begin{bmatrix} + ε_{k-1} \\ + \mathbf{x̂_0^†}(k-N_k+p) \\ + \mathbf{ŵ}(k-N_k+p+0|k-1) \\ + \mathbf{ŵ}(k-N_k+p+1|k-1) \\ + \vdots \\ + \mathbf{ŵ}(k+p-3|k-1) \\ + \mathbf{ŵ}(k+p-2|k-1) \\ + \mathbf{0} \\ +\end{bmatrix} +``` +where ``ε_{k-1}`` and ``\mathbf{ŵ}(k-j|k-1)`` are respectively the slack variable and the +process noise estimates computed at the last time step ``k-1``. The vector +``\mathbf{x̂_0^†}(k-N_k+p)`` is the deviation vector of the state at the arrival estimated +at time ``k-N_k``. If the objective function is not finite at this point, all the process +noises ``\mathbf{ŵ}_{k-1}(k-j)`` are warm-started at zeros. The method mutates all the +arguments. +""" +function set_warmstart_mhe!( + estim::MovingHorizonEstimator{NT}, transcription::SingleShooting, Z̃var +) where NT<:Real + model, buffer = estim.model, estim.buffer + nε, nx̂, nŵ, He, Nk = estim.nε, estim.nx̂, estim.nx̂, estim.He, estim.Nk[] + nx̃, nŴ = nε + nx̂, nŵ*He + Z̃s = estim.buffer.Z̃ + û0, ŷ0, x̄, k = buffer.û, buffer.ŷ, buffer.x̂, buffer.k + # --- slack variable ε --- + estim.nε == 1 && (Z̃s[begin] = estim.Z̃[begin]) + # --- arrival state estimate x̂0arr --- + Z̃s[nε+1:nx̃] = estim.x̂0arr_old + # --- process noise estimates Ŵ --- + Z̃s[(nx̃+1):(nx̃+nŴ-nŵ)] .= @views estim.Z̃[(nx̃+nŵ+1):(nx̃+nŴ)] + Z̃s[(nx̃+nŴ-nŵ+1):end] .= 0 + # --- verify definiteness of objective function --- + V̂, Ŵ, X̂0 = estim.buffer.V̂, estim.buffer.Ŵ, estim.buffer.X̂ + x̄ .= 0 # x̂0arr == x̂arr_old implies the error at arrival x̄ is zero + getŴ!(Ŵ, estim, transcription, Z̃s) + predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim, model, estim.x̂0arr_old, Ŵ, Z̃s) + Js = obj_nonlinprog(estim, model, x̄, V̂, Ŵ, Z̃s) + if !isfinite(Js) + Z̃s[nx̃+1:end] .= 0 + end + # --- unused variable in Z̃ (applied only when Nk < He) --- + # We force the update of the NLP gradient and jacobian by warm-starting the unused + # variable in Z̃ at 1. Since estim.Ŵ is initialized with 0s, at least 1 variable in Z̃s + # will be inevitably different at the following time step. + Z̃s[nx̃+nŵ*Nk+1:end] .= 1 + JuMP.set_start_value.(Z̃var, Z̃s) + return Z̃s +end + +@doc raw""" + set_warmstart_mhe!( + estim::MovingHorizonEstimator, transcription::MultipleShooting, Z̃var + ) -> Z̃s + +Do the same but based on a [`MultipleShooting`](@ref) transcription. + +If supported by `estim.optim`, it warm-starts the solver at: +```math +\mathbf{Z̃_s} = +\begin{bmatrix} + ε_{k-1} \\ + \mathbf{x̂_0^†}(k-N_k+p) \\ + \mathbf{x̂_0}(k-N_k+p+1|k-1) \\ + \mathbf{x̂_0}(k-N_k+p+2|k-1) \\ + \vdots \\ + \mathbf{x̂_0}(k+p-2|k-1) \\ + \mathbf{x̂_0}(k+p-1|k-1) \\ + \mathbf{x̂_0}(k+p-1|k-1) \\ + \mathbf{ŵ}(k-N_k+p+0|k-1) \\ + \mathbf{ŵ}(k-N_k+p+1|k-1) \\ + \vdots \\ + \mathbf{ŵ}(k+p-3|k-1) \\ + \mathbf{ŵ}(k+p-2|k-1) \\ + \mathbf{0} \\ +\end{bmatrix} +``` +where ``\mathbf{x̂_0}(k-j|k-1)`` is the predicted state for time ``k-j`` computed at the +last control period ``k-1``, expressed as a deviation from the operating point +``\mathbf{x̂_{op}}``. +""" +function set_warmstart_mhe!( + estim::MovingHorizonEstimator{NT}, transcription::MultipleShooting, Z̃var +) where NT<:Real + model, buffer = estim.model, estim.buffer + nε, nx̂, nŵ, He, Nk = estim.nε, estim.nx̂, estim.nx̂, estim.He, estim.Nk[] + nx̃, nŴ, nX̂ = nε + nx̂, nŵ*He, nx̂*He + Z̃s = estim.buffer.Z̃ + û0, ŷ0, x̄, k = buffer.û, buffer.ŷ, buffer.x̂, buffer.k + # --- slack variable ε --- + estim.nε == 1 && (Z̃s[begin] = estim.Z̃[begin]) + # --- arrival state estimate x̂0arr --- + Z̃s[nε+1:nx̃] = estim.x̂0arr_old + # --- state estimates X̂0 --- + Z̃s[(nx̃+1):(nx̃+nX̂-nx̂)] .= @views estim.Z̃[(nx̃+nx̂+1):(nx̃+nX̂)] + Z̃s[(nx̃+nX̂-nx̂+1):(nx̃+nX̂)] .= @views estim.Z̃[(nx̃+nX̂-nx̂+1):(nx̃+nX̂)] + # --- process noise estimates Ŵ --- + Z̃s[(nx̃+nX̂+1):(nx̃+nX̂+nŴ-nŵ)] .= @views estim.Z̃[(nx̃+nX̂+nŵ+1):(nx̃+nX̂+nŴ)] + Z̃s[(nx̃+nX̂+nŴ-nŵ+1):end] .= 0 + # --- verify definiteness of objective function --- + V̂, Ŵ, X̂0 = estim.buffer.V̂, estim.buffer.Ŵ, estim.buffer.X̂ + x̄ .= 0 # x̂0arr == x̂arr_old implies the error at arrival x̄ is zero + getŴ!(Ŵ, estim, transcription, Z̃s) + predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim, model, estim.x̂0arr_old, Ŵ, Z̃s) + Js = obj_nonlinprog(estim, model, x̄, V̂, Ŵ, Z̃s) + if !isfinite(Js) + Z̃s[nx̃+nX̂+1:end] .= 0 + end + # --- unused variable in Z̃ (applied only when Nk < He) --- + # We force the update of the NLP gradient and jacobian by warm-starting the unused + # variable in Z̃ at 1. Since estim.Ŵ is initialized with 0s, at least 1 variable in Z̃s + # will be inevitably different at the following time step. + Z̃s[nx̃+nX̂+nŵ*Nk+1:end] .= 1 + JuMP.set_start_value.(Z̃var, Z̃s) + return Z̃s +end + +"Get the estimated process noise from the decision vector `Z̃`." +function getŴ!(Ŵ, estim::MovingHorizonEstimator, transcription::TranscriptionMethod, Z̃) + He, nx̂, nŵ, Nk = estim.He, estim.nx̂, estim.nx̂, estim.Nk[] + nZ̃ = estim.nε + get_nZ_mhe(transcription, He, nx̂, nŵ) + Ŵ[1:nŵ*Nk] .= @views Z̃[(nZ̃ - nŵ*He + 1):(nZ̃ - nŵ*He + nŵ*Nk)] + return Ŵ +end + +"Fill the unused decision variables in `Z̃` with `0`s (only when `Nk < He`)." +function fill0unused!(Z̃, estim::MovingHorizonEstimator, ::SingleShooting) + nŵ, nx̂, Nk = estim.nx̂, estim.nx̂, estim.Nk[] + nx̃ = estim.nε + nx̂ + Z̃[(nx̃ + nŵ*Nk + 1):end] .= 0 # unused decision variables after Ŵ vector + return nothing +end +function fill0unused!(Z̃, estim::MovingHorizonEstimator, ::TranscriptionMethod) + nŵ, nx̂, He, Nk = estim.nx̂, estim.nx̂, estim.He, estim.Nk[] + nx̃ = estim.nε + nx̂ + Z̃[(nx̃ + nx̂*Nk + 1):(nx̃ + nx̂*He)] .= 0 # unused decision variables after X̂0 vector + Z̃[(nx̃ + nx̂*He + nŵ*Nk + 1):end] .= 0 # unused decision variables after Ŵ vector + return nothing +end \ No newline at end of file diff --git a/src/general.jl b/src/general.jl index 203ab76b7..48631cae8 100644 --- a/src/general.jl +++ b/src/general.jl @@ -69,6 +69,29 @@ function info2debugstr(info) return mystr end +@doc raw""" + augmentdefect(ES, nϵ; slackfirst=false) -> Aeq, ẼS + +Augment defect equality constraints with slack variable ϵ if `nϵ == 1`. + +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_{eq} Z̃} = \mathbf{b_{eq}} = - \mathbf{F_S} +``` +The slack is assumed to be 1st element of the decision vector if `slackfirst`, else the last. +""" +function augmentdefect(ES::AbstractMatrix{NT}, nϵ; slackfirst=false) where NT<:Real + if nϵ == 1 # Z̃ = [Z; ϵ] (or Z̃ = [ϵ; Z] if slackfirst == true) + zero_vec = zeros(NT, size(ES, 1), 1) + ẼS = slackfirst ? [zero_vec ES] : [ES zero_vec] + else # Z̃ = Z (only hard constraints) + ẼS = ES + end + Aeq = ẼS + return Aeq, ẼS +end + "Evaluate the quadratic programming objective function `0.5x'*H*x + q'*x` at `x`." obj_quadprog(x, H, q) = 0.5*dot(x, H, x) + q'*x # dot(x, H, x) is faster than x'*H*x diff --git a/src/predictive_control.jl b/src/predictive_control.jl index 964ce1ff7..765e5b00a 100644 --- a/src/predictive_control.jl +++ b/src/predictive_control.jl @@ -19,12 +19,12 @@ julia> u = mpc([5]); round.(u, digits=3) """ abstract type PredictiveController{NT<:Real} end -include("controller/transcription.jl") include("controller/construct.jl") include("controller/execute.jl") include("controller/explicitmpc.jl") include("controller/linmpc.jl") include("controller/nonlinmpc.jl") +include("controller/transcription.jl") function Base.show(io::IO, mpc::PredictiveController) estim, model = mpc.estim, mpc.estim.model diff --git a/src/transcription.jl b/src/transcription.jl new file mode 100644 index 000000000..6f2abafd1 --- /dev/null +++ b/src/transcription.jl @@ -0,0 +1,350 @@ +const COLLOCATION_NODE_TYPE = Float64 + +""" +Abstract supertype of all transcription methods for the optimization problems. + +The module currently supports [`SingleShooting`](@ref), [`MultipleShooting`](@ref), +[`TrapezoidalCollocation`](@ref) and [`OrthogonalCollocation`](@ref) transcription methods. +""" +abstract type TranscriptionMethod end +abstract type ShootingMethod <: TranscriptionMethod end +abstract type CollocationMethod <: TranscriptionMethod end + +@doc raw""" + SingleShooting() + +Construct a direct single shooting [`TranscriptionMethod`](@ref). + +In the case of [`PredictiveController`](@ref) types, the decision variable in the +optimization problem is (excluding the slack ``ϵ``, and without any custom move blocking): +```math +\mathbf{Z} = \mathbf{ΔU} = \begin{bmatrix} + \mathbf{Δu}(k+0) \\ + \mathbf{Δu}(k+1) \\ + \vdots \\ + \mathbf{Δu}(k+H_c-1) \end{bmatrix} +``` +This method computes the predictions by calling the augmented discrete-time model +recursively over the prediction horizon ``H_p`` in the objective function, or by updating +the linear coefficients of the quadratic optimization for [`LinModel`](@ref). It is +generally more efficient for small control horizon ``H_c``, stable and mildly nonlinear +plant model/constraints. The Extended Help details transcription of +[`MovingHorizonEstimator`](@ref) objects. + +# Extended Help +!!! details "Extended Help" + For [`MovingHorizonEstimator`](@ref), the decision variable is (excluding slack `ε`): + ```math + \mathbf{Z} + = \begin{bmatrix} + \mathbf{x̂_0}(k-N_k+p) \\ + \mathbf{Ŵ} \\ + \mathbf{0} + \end{bmatrix} + = \begin{bmatrix} + \mathbf{x̂}_k(k-N_k+p) - \mathbf{x̂_{op}} \\ + \mathbf{Ŵ} \\ + \mathbf{0} \end{bmatrix} + ``` + The vector ``\mathbf{0}`` with `nx̂*(He-Nk)` zeros is for the unused decision variables + at the beginning, when the data windows are growing (``N_k < H_e``). The number of + elements in ``\mathbf{Z}`` is therefore constant. The ``\mathbf{Ŵ}`` vector is defined + in the Extended Help of [`MovingHorizonEstimator`](@ref). +""" +struct SingleShooting <: ShootingMethod end + +@doc raw""" + MultipleShooting(; f_threads=false, h_threads=false) + +Construct a direct multiple shooting [`TranscriptionMethod`](@ref). + +The decision variable of [`PredictiveController`](@ref) is (excluding ``ϵ``): +```math +\mathbf{Z} = \begin{bmatrix} \mathbf{ΔU} \\ \mathbf{X̂_0} \end{bmatrix} +``` +thus it also includes the predicted states, expressed as deviation vectors from the +operating point ``\mathbf{x̂_{op}}`` (see [`augment_model`](@ref)): +```math +\mathbf{X̂_0} = \mathbf{X̂ - X̂_{op}} = \begin{bmatrix} + \mathbf{x̂}_i(k+1) - \mathbf{x̂_{op}} \\ + \mathbf{x̂}_i(k+2) - \mathbf{x̂_{op}} \\ + \vdots \\ + \mathbf{x̂}_i(k+H_p) - \mathbf{x̂_{op}} \end{bmatrix} +``` +where ``\mathbf{x̂}_i(k+j)`` is the state prediction for time ``k+j``, estimated by the +observer at time ``i=k`` or ``i=k-1`` depending on its `direct` flag. Note that +``\mathbf{X̂_0 = X̂}`` if the operating point is zero, which is typically the case in practice +for [`NonLinModel`](@ref). + +This transcription computes the predictions by calling the augmented discrete-time model +in the equality constraint function recursively over ``H_p``, or by updating the linear +equality constraint vector for [`LinModel`](@ref). It is generally more efficient for large +control horizon ``H_c``, unstable or highly nonlinear models/constraints. Multithreading +with `f_threads` or `h_threads` keyword arguments can be advantageous if ``\mathbf{f}`` or +``\mathbf{h}`` in the [`NonLinModel`](@ref) is expensive to evaluate, respectively. + +Sparse optimizers like `OSQP` or `Ipopt` and sparse Jacobian computations are recommended +for this transcription method. The transcription of [`MovingHorizonEstimator`](@ref) is +provided in the Extended Help. + +!!! details "Extended Help" + For [`MovingHorizonEstimator`](@ref), the decision variable is (excluding slack `ε`): + ```math + \mathbf{Z} = \begin{bmatrix} + \mathbf{x̂_0}(k-N_k+p) \\ + \mathbf{X̂_0} \\ + \mathbf{0} \\ + \mathbf{Ŵ} \\ + \mathbf{0} \end{bmatrix} + ``` + thus the deviation value of arrival state estimate ``\mathbf{x̂_0}(k-N_k+p)`` is kept out + of the estimated states over ``N_k``: + ```math + \mathbf{X̂_0} = \mathbf{X̂ - X̂_{op}} = \begin{bmatrix} + \mathbf{x̂}_k(k-N_k+p+1) - \mathbf{x̂_{op}} \\ + \mathbf{x̂}_k(k-N_k+p+2) - \mathbf{x̂_{op}} \\ + \vdots \\ + \mathbf{x̂}_k(k+p) - \mathbf{x̂_{op}} \end{bmatrix} + ``` + Similarly to [`SingleShooting`](@ref), the two ``\mathbf{0}`` vectors with zeros is + for the unused decision variables at the beginning. +""" +struct MultipleShooting <: ShootingMethod + f_threads::Bool + h_threads::Bool + function MultipleShooting(; f_threads=false, h_threads=false) + return new(f_threads, h_threads) + end +end + +@doc raw""" + TrapezoidalCollocation(h::Int=0; f_threads=false, h_threads=false) + +Construct an implicit trapezoidal [`TranscriptionMethod`](@ref) with `h`th order hold. + +This is the simplest collocation method. It supports continuous-time [`NonLinModel`](@ref)s +only. The decision variables are the same as for [`MultipleShooting`](@ref), hence similar +computational costs. See the same docstring for descriptions of `f_threads` and `h_threads` +keywords. The `h` argument is `0` or `1`, for piecewise constant or linear manipulated +inputs ``\mathbf{u}`` (`h=1` is slightly less expensive). Note that the various [`DiffSolver`](@ref) +here assume zero-order hold, so `h=1` will induce a plant-model mismatch if the plant is +simulated with these solvers. Measured disturbances ``\mathbf{d}`` are piecewise linear. + +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. See Extended Help for more details. + +!!! warning + 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). + +Sparse optimizers like `Ipopt` and sparse Jacobian computations are recommended for this +transcription method. + +# Extended Help +!!! details "Extended Help" + 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. See [`con_nonlinprogeq!`](@ref) + for more details. +""" +struct TrapezoidalCollocation <: CollocationMethod + h::Int + no::Int + f_threads::Bool + h_threads::Bool + function TrapezoidalCollocation(h::Int=0; f_threads=false, h_threads=false) + if !(h == 0 || h == 1) + throw(ArgumentError("h argument must be 0 or 1 for TrapezoidalCollocation.")) + end + no = 2 # 2 collocation points per intervals for trapezoidal rule + return new(h, no, f_threads, h_threads) + end +end + + +@doc raw""" + OrthogonalCollocation( + h::Int=0, no::Int=3; f_threads=false, h_threads=false, roots=:gaussradau + ) + +Construct an orthogonal collocation on finite elements [`TranscriptionMethod`](@ref). + +Also known as pseudo-spectral method. It supports continuous-time [`NonLinModel`](@ref)s +only. The `h` argument is the hold order for ``\mathbf{u}`` (`0` or `1`), and the `no` +argument, the number of collocation points ``n_o``. The decision variable is similar to +[`MultipleShooting`](@ref), but it also includes the collocation points: +```math +\mathbf{Z} = \begin{bmatrix} \mathbf{ΔU} \\ \mathbf{X̂_0} \\ \mathbf{K} \end{bmatrix} +``` +where ``\mathbf{K}`` encompasses all the intermediate stages of the deterministic states +(the first `nx` elements of ``\mathbf{x̂}``): +```math +\mathbf{K} = \begin{bmatrix} + \mathbf{k}_{1}(k+0) \\ + \mathbf{k}_{2}(k+0) \\ + \vdots \\ + \mathbf{k}_{n_o}(k+0) \\ + \mathbf{k}_{1}(k+1) \\ + \mathbf{k}_{2}(k+1) \\ + \vdots \\ + \mathbf{k}_{n_o}(k+H_p-1) \end{bmatrix} +``` +and ``\mathbf{k}_i(k+j)`` is the deterministic state prediction for the ``i``th collocation +point at the ``j``th stage/interval/finite element (details in Extended Help). The `roots` +keyword argument is either `:gaussradau` or `:gausslegendre`, for Gauss-Radau or +Gauss-Legendre quadrature, respectively. See [`MultipleShooting`](@ref) docstring for +descriptions of `f_threads` and `h_threads` keywords. This transcription computes the +predictions by enforcing the collocation and continuity constraints at the collocation +points. It is efficient for highly stiff systems, but generally more expensive than the +other methods for non-stiff systems. See Extended Help for more details. + +!!! warning + The built-in [`StateEstimator`](@ref) will still use the `solver` provided at the + construction of the [`NonLinModel`](@ref) to estimate the plant states, not orthogonal + collocation (see `supersample` option of [`RungeKutta`](@ref) for stiff systems). + +Sparse optimizers like `Ipopt` and sparse Jacobian computations are highly recommended for +this transcription method (sparser formulation than [`MultipleShooting`](@ref)). + +# Extended Help +!!! details "Extended Help" + As explained in the Extended Help of [`TrapezoidalCollocation`](@ref), the stochastic + states are left out of the ``\mathbf{K}`` vector since collocation methods require + continuous-time dynamics and the stochastic model is discrete. + + The collocation points are located at the roots of orthogonal polynomials, which is + "optimal" for approximating the state trajectories with polynomials of degree ``n_o``. + The method then enforces the system dynamics at these points. The Gauss-Legendre scheme + is more accurate than Gauss-Radau but only A-stable, while the latter being L-stable. + See [`con_nonlinprogeq!`](@ref) for implementation details. +""" +struct OrthogonalCollocation <: CollocationMethod + h::Int + no::Int + f_threads::Bool + h_threads::Bool + τ::Vector{COLLOCATION_NODE_TYPE} + function OrthogonalCollocation( + h::Int=0, no::Int=3; f_threads=false, h_threads=false, roots=:gaussradau + ) + if !(h == 0 || h == 1) + throw(ArgumentError("h argument must be 0 or 1 for OrthogonalCollocation.")) + end + if roots==:gaussradau + x, _ = FastGaussQuadrature.gaussradau(COLLOCATION_NODE_TYPE, no) + # we reverse the nodes to include the τ=1.0 node: + τ = (reverse(-x) .+ 1) ./ 2 + elseif roots==:gausslegendre + x, _ = FastGaussQuadrature.gausslegendre(COLLOCATION_NODE_TYPE, no) + # converting [-1, 1] to [0, 1] (see + # https://en.wikipedia.org/wiki/Gaussian_quadrature#Change_of_interval): + τ = (x .+ 1) ./ 2 + else + throw(ArgumentError("roots argument must be :gaussradau or :gausslegendre.")) + end + return new(h, no, f_threads, h_threads, τ) + end +end + +@doc raw""" + init_orthocolloc(model::SimModel, transcription::OrthogonalCollocation) -> Mo, Co, λo + +Init the differentiation and continuity matrices for [`OrthogonalCollocation`](@ref). + +Introducing ``τ_i``, the ``i``th root of the orthogonal polynomial normalized to the +interval ``[0, 1]``, and ``τ_0=0``, each state trajectories are approximated by a distinct +polynomial of degree ``n_o``. The differentiation matrix ``\mathbf{M_o}``, continuity +matrix ``\mathbf{C_o}`` and continuity coefficient ``λ_o`` are pre-computed with: +```math +\begin{aligned} + \mathbf{P_o} &= \begin{bmatrix} + τ_1^1 \mathbf{I} & τ_1^2 \mathbf{I} & \cdots & τ_1^{n_o} \mathbf{I} \\ + τ_2^1 \mathbf{I} & τ_2^2 \mathbf{I} & \cdots & τ_2^{n_o} \mathbf{I} \\ + \vdots & \vdots & \ddots & \vdots \\ + τ_{n_o}^1 \mathbf{I} & τ_{n_o}^2 \mathbf{I} & \cdots & τ_{n_o}^{n_o} \mathbf{I} \end{bmatrix} \\ + \mathbf{Ṗ_o} &= \begin{bmatrix} + τ_1^0 \mathbf{I} & 2τ_1^1 \mathbf{I} & \cdots & n_o τ_1^{n_o-1} \mathbf{I} \\ + τ_2^0 \mathbf{I} & 2τ_2^1 \mathbf{I} & \cdots & n_o τ_2^{n_o-1} \mathbf{I} \\ + \vdots & \vdots & \ddots & \vdots \\ + τ_{n_o}^0 \mathbf{I} & 2τ_{n_o}^1 \mathbf{I} & \cdots & n_o τ_{n_o}^{n_o-1} \mathbf{I} \end{bmatrix} \\ + \mathbf{M_o} &= \frac{1}{T_s} \mathbf{Ṗ_o} \mathbf{P_o}^{-1} \\ + \mathbf{C_o} &= \begin{bmatrix} + L_1(1) \mathbf{I} & L_2(1) \mathbf{I} & \cdots & L_{n_o}(1) \mathbf{I} \end{bmatrix} \\ + λ_o &= L_0(1) +\end{aligned} +``` +where ``\mathbf{P_o}`` is a matrix to evaluate the polynamial values w/o the coefficients +and Y-intercept, and ``\mathbf{Ṗ_o}``, to evaluate its derivatives. The Lagrange polynomial +``L_j(τ)`` bases are defined as: +```math +L_j(τ) = \prod_{i=0, i≠j}^{n_o} \frac{τ - τ_i}{τ_j - τ_i} +``` + +The collocation constraints are nonlinear, but the defects of deterministic states +``\mathbf{x̂_d}`` for the continuity constraints are in fact linear equality constraints: +```math +\mathbf{s_c}(k+j+1) = \mathbf{0} = + \mathbf{C_o} \begin{bmatrix} + \mathbf{k}_1(k+j) \\ + \mathbf{k}_2(k+j) \\ + \vdots \\ + \mathbf{k}_{n_o}(k+j) \end{bmatrix} + + λ_o \mathbf{x̂_d}(k+j) - \mathbf{x̂_d}(k+j+1) +``` +for ``j = 0, 1, ... , H_p-1``. The ``\mathbf{k}_i`` and ``\mathbf{x̂_d}`` vectors are all +directly extracted from the decision variable `Z̃`. +""" +function init_orthocolloc( + model::SimModel{NT}, transcription::OrthogonalCollocation +) where {NT<:Real} + nx, no = model.nx, transcription.no + τ = transcription.τ + Po = Matrix{NT}(undef, nx*no, nx*no) # polynomial matrix (w/o the Y-intercept term) + Ṗo = Matrix{NT}(undef, nx*no, nx*no) # polynomial derivative matrix + I_nx = I(nx) + for j=1:no, i=1:no + iRows = (1:nx) .+ nx*(i-1) + iCols = (1:nx) .+ nx*(j-1) + Po[iRows, iCols] = (τ[i]^j)*I_nx + Ṗo[iRows, iCols] = (j*τ[i]^(j-1))*I_nx + end + Mo = sparse((Ṗo/Po)/model.Ts) + Co = Matrix{NT}(undef, nx, nx*no) + for j=1:no + iCols = (1:nx) .+ nx*(j-1) + Co[:, iCols] = lagrange_end(j, transcription)*I_nx + end + Co = sparse(Co) + λo = lagrange_end(0, transcription) + return Mo, Co, λo +end +"Return empty sparse matrices and `NaN` for other [`TranscriptionMethod`](@ref)" +init_orthocolloc(::SimModel, ::TranscriptionMethod) = spzeros(0,0), spzeros(0,0), NaN + +"Evaluate the Lagrange basis polynomial ``L_j`` at `τ=1`." +function lagrange_end(j, transcription::OrthogonalCollocation) + τ_val = 1 + τ_values = [0; transcription.τ] # including the τ=0 node for the Lagrange polynomials + j_index = j + 1 # because of the τ=0 node + τj = τ_values[j_index] + Lj = 1 + for i in eachindex(τ_values) + i == j_index && continue + τi = τ_values[i] + Lj *= (τ_val - τi)/(τj - τi) + end + return Lj +end + +function validate_transcription(::LinModel, ::CollocationMethod) + throw(ArgumentError("Collocation methods are not supported for LinModel.")) + return nothing +end +function validate_transcription(::NonLinModel{<:Real, <:EmptySolver}, ::CollocationMethod) + throw(ArgumentError("Collocation methods require continuous-time NonLinModel.")) + return nothing +end +validate_transcription(::SimModel, ::TranscriptionMethod) = nothing