diff --git a/docs/src/internals/predictive_control.md b/docs/src/internals/predictive_control.md index 51dee94c1..415927e83 100644 --- a/docs/src/internals/predictive_control.md +++ b/docs/src/internals/predictive_control.md @@ -40,5 +40,5 @@ ModelPredictiveControl.linconstrainteq! ```@docs ModelPredictiveControl.optim_objective!(::PredictiveController) ModelPredictiveControl.set_warmstart! -ModelPredictiveControl.getinput +ModelPredictiveControl.getinput! ``` diff --git a/docs/src/public/state_estim.md b/docs/src/public/state_estim.md index 2d2d9a794..07a28b0fd 100644 --- a/docs/src/public/state_estim.md +++ b/docs/src/public/state_estim.md @@ -81,6 +81,12 @@ MovingHorizonEstimator InternalModel ``` +## ManualEstimator + +```@docs +ManualEstimator +``` + ## Default Model Augmentation ```@docs diff --git a/src/ModelPredictiveControl.jl b/src/ModelPredictiveControl.jl index adefdfef2..b3a74ad61 100644 --- a/src/ModelPredictiveControl.jl +++ b/src/ModelPredictiveControl.jl @@ -36,6 +36,7 @@ export savetime!, periodsleep export StateEstimator, InternalModel, Luenberger export SteadyKalmanFilter, KalmanFilter, UnscentedKalmanFilter, ExtendedKalmanFilter export MovingHorizonEstimator +export ManualEstimator export default_nint, initstate! export PredictiveController, ExplicitMPC, LinMPC, NonLinMPC, setconstraint!, moveinput! export TranscriptionMethod, SingleShooting, MultipleShooting diff --git a/src/controller/construct.jl b/src/controller/construct.jl index 2e6d11eea..294b32d50 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -1,3 +1,6 @@ +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} diff --git a/src/controller/execute.jl b/src/controller/execute.jl index c4a303918..7bd29861d 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -2,9 +2,13 @@ initstate!(mpc::PredictiveController, u, ym, d=[]) -> x̂ Init the states of `mpc.estim` [`StateEstimator`](@ref) and warm start `mpc.Z̃` at zero. + +It also stores `u - mpc.estim.model.uop` at `mpc.lastu0` for converting the input increments +``\mathbf{ΔU}`` to inputs ``\mathbf{U}``. """ function initstate!(mpc::PredictiveController, u, ym, d=mpc.estim.buffer.empty) mpc.Z̃ .= 0 + mpc.lastu0 .= u .- mpc.estim.model.uop return initstate!(mpc.estim, u, ym, d) end @@ -16,10 +20,11 @@ Compute the optimal manipulated input value `u` for the current control period. Solve the optimization problem of `mpc` [`PredictiveController`](@ref) and return the results ``\mathbf{u}(k)``. Following the receding horizon principle, the algorithm discards the optimal future manipulated inputs ``\mathbf{u}(k+1), \mathbf{u}(k+2), ...`` Note that -the method mutates `mpc` internal data but it does not modifies `mpc.estim` states. Call -[`preparestate!(mpc, ym, d)`](@ref) before `moveinput!`, and [`updatestate!(mpc, u, ym, d)`](@ref) -after, to update `mpc` state estimates. Setpoint and measured disturbance previews can -be implemented with the `R̂y`, `R̂u` and `D̂` keyword arguments. +the method mutates `mpc` internal data (it stores `u - mpc.estim.model.uop` at `mpc.lastu0` +for instance) but it does not modifies `mpc.estim` states. Call [`preparestate!(mpc, ym, d)`](@ref) +before `moveinput!`, and [`updatestate!(mpc, u, ym, d)`](@ref) after, to update `mpc` state +estimates. Setpoint and measured disturbance previews can be implemented with the `R̂y`, `R̂u` +and `D̂` keyword arguments. Calling a [`PredictiveController`](@ref) object calls this method. @@ -69,7 +74,7 @@ function moveinput!( linconstraint!(mpc, mpc.estim.model, mpc.transcription) linconstrainteq!(mpc, mpc.estim.model, mpc.transcription) Z̃ = optim_objective!(mpc) - return getinput(mpc, Z̃) + return getinput!(mpc, Z̃) end @doc raw""" @@ -207,7 +212,7 @@ function initpred!(mpc::PredictiveController, model::LinModel, d, D̂, R̂y, R̂ F = initpred_common!(mpc, model, d, D̂, R̂y, R̂u) F .+= mpc.B # F = F + B mul!(F, mpc.K, mpc.estim.x̂0, 1, 1) # F = F + K*x̂0 - mul!(F, mpc.V, mpc.estim.lastu0, 1, 1) # F = F + V*lastu0 + mul!(F, mpc.V, mpc.lastu0, 1, 1) # F = F + V*lastu0 if model.nd > 0 mul!(F, mpc.G, mpc.d0, 1, 1) # F = F + G*d0 mul!(F, mpc.J, mpc.D̂0, 1, 1) # F = F + J*D̂0 @@ -254,7 +259,7 @@ Will also init `mpc.F` with 0 values, or with the stochastic predictions `Ŷs` is an [`InternalModel`](@ref). The function returns `mpc.F`. """ function initpred_common!(mpc::PredictiveController, model::SimModel, d, D̂, R̂y, R̂u) - mul!(mpc.Tu_lastu0, mpc.Tu, mpc.estim.lastu0) + mul!(mpc.Tu_lastu0, mpc.Tu, mpc.lastu0) mpc.ŷ .= evaloutput(mpc.estim, d) if model.nd > 0 mpc.d0 .= d .- model.dop @@ -446,20 +451,23 @@ function preparestate!(mpc::PredictiveController, ym, d=mpc.estim.buffer.empty) end @doc raw""" - getinput(mpc::PredictiveController, Z̃) -> u + getinput!(mpc::PredictiveController, Z̃) -> u -Get current manipulated input `u` from a [`PredictiveController`](@ref) solution `Z̃`. +Get current manipulated input `u` from the solution `Z̃`, store it and return it. The first manipulated input ``\mathbf{u}(k)`` is extracted from the decision vector -``\mathbf{Z̃}`` and applied on the plant (from the receding horizon principle). +``\mathbf{Z̃}`` and applied on the plant (from the receding horizon principle). It also +stores `u - mpc.estim.model.uop` at `mpc.lastu0`. """ -function getinput(mpc, Z̃) +function getinput!(mpc, Z̃) + model = mpc.estim.model Δu = mpc.buffer.u - for i in 1:mpc.estim.model.nu + for i in 1:model.nu Δu[i] = Z̃[i] end u = Δu - u .+= mpc.estim.lastu0 .+ mpc.estim.model.uop + u .+= mpc.lastu0 .+ model.uop + mpc.lastu0 .= u .- model.uop return u end @@ -548,6 +556,7 @@ function setmodel!( Ñ_Hc = Ntilde_Hc, kwargs... ) + uop_old = copy(mpc.estim.model.uop) x̂op_old = copy(mpc.estim.x̂op) nu, ny, Hp, Hc, nϵ = model.nu, model.ny, mpc.Hp, mpc.Hc, mpc.nϵ setmodel!(mpc.estim, model; kwargs...) @@ -593,12 +602,12 @@ function setmodel!( mpc.weights.L_Hp .= L_Hp mpc.weights.iszero_L_Hp[] = iszero(mpc.weights.L_Hp) end - setmodel_controller!(mpc, x̂op_old) + setmodel_controller!(mpc, uop_old, x̂op_old) return mpc end "Update the prediction matrices, linear constraints and JuMP optimization." -function setmodel_controller!(mpc::PredictiveController, x̂op_old) +function setmodel_controller!(mpc::PredictiveController, uop_old, x̂op_old) model, estim, transcription = mpc.estim.model, mpc.estim, mpc.transcription nu, ny, nd, Hp, Hc = model.nu, model.ny, model.nd, mpc.Hp, mpc.Hc optim, con = mpc.optim, mpc.con @@ -628,6 +637,7 @@ function setmodel_controller!(mpc::PredictiveController, x̂op_old) con.x̂0min .+= x̂op_old # convert x̂0 to x̂ with the old operating point con.x̂0max .+= x̂op_old # convert x̂0 to x̂ with the old operating point # --- operating points --- + mpc.lastu0 .+= uop_old .- model.uop for i in 0:Hp-1 mpc.Uop[(1+nu*i):(nu+nu*i)] .= model.uop mpc.Yop[(1+ny*i):(ny+ny*i)] .= model.yop diff --git a/src/controller/explicitmpc.jl b/src/controller/explicitmpc.jl index f6f646792..cfb4dac7c 100644 --- a/src/controller/explicitmpc.jl +++ b/src/controller/explicitmpc.jl @@ -13,6 +13,7 @@ struct ExplicitMPC{ weights::CW R̂u::Vector{NT} R̂y::Vector{NT} + lastu0::Vector{NT} P̃Δu::Matrix{NT} P̃u ::Matrix{NT} Tu ::Matrix{NT} @@ -46,12 +47,13 @@ struct ExplicitMPC{ nϵ = 0 # no slack variable ϵ for ExplicitMPC # dummy vals (updated just before optimization): R̂y, R̂u, Tu_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp) + lastu0 = zeros(NT, nu) transcription = SingleShooting() # explicit MPC only supports SingleShooting PΔu = init_ZtoΔU(estim, transcription, Hp, Hc) Pu, Tu = init_ZtoU(estim, transcription, Hp, Hc) E, G, J, K, V, B = init_predmat(model, estim, transcription, Hp, Hc) # dummy val (updated just before optimization): - F, fx̂ = zeros(NT, ny*Hp), zeros(NT, nx̂) + F = zeros(NT, ny*Hp) P̃Δu, P̃u, Ẽ = PΔu, Pu, E # no slack variable ϵ for ExplicitMPC H̃ = init_quadprog(model, weights, Ẽ, P̃Δu, P̃u) # dummy vals (updated just before optimization): @@ -71,6 +73,7 @@ struct ExplicitMPC{ Hp, Hc, nϵ, weights, R̂u, R̂y, + lastu0, P̃Δu, P̃u, Tu, Tu_lastu0, Ẽ, F, G, J, K, V, B, H̃, q̃, r, @@ -157,7 +160,7 @@ function ExplicitMPC( N_Hc = Diagonal(repeat(Nwt, Hc)), L_Hp = Diagonal(repeat(Lwt, Hp)), ) where {NT<:Real, SE<:StateEstimator{NT}} - isa(estim.model, LinModel) || error("estim.model type must be a LinModel") + isa(estim.model, LinModel) || error(MSG_LINMODEL_ERR) nk = estimate_delays(estim.model) if Hp ≤ nk @warn("prediction horizon Hp ($Hp) ≤ estimated number of delays in model "* @@ -215,7 +218,7 @@ addinfo!(info, mpc::ExplicitMPC) = info "Update the prediction matrices and Cholesky factorization." -function setmodel_controller!(mpc::ExplicitMPC, _ ) +function setmodel_controller!(mpc::ExplicitMPC, uop_old, _ ) model, estim, transcription = mpc.estim.model, mpc.estim, mpc.transcription nu, ny, nd, Hp, Hc = model.nu, model.ny, model.nd, mpc.Hp, mpc.Hc # --- predictions matrices --- @@ -232,6 +235,7 @@ function setmodel_controller!(mpc::ExplicitMPC, _ ) mpc.H̃ .= H̃ set_objective_hessian!(mpc) # --- operating points --- + mpc.lastu0 .+= uop_old .- model.uop for i in 0:Hp-1 mpc.Uop[(1+nu*i):(nu+nu*i)] .= model.uop mpc.Yop[(1+ny*i):(ny+ny*i)] .= model.yop diff --git a/src/controller/linmpc.jl b/src/controller/linmpc.jl index 03122b651..3d54586d2 100644 --- a/src/controller/linmpc.jl +++ b/src/controller/linmpc.jl @@ -22,6 +22,7 @@ struct LinMPC{ weights::CW R̂u::Vector{NT} R̂y::Vector{NT} + lastu0::Vector{NT} P̃Δu::Matrix{NT} P̃u ::Matrix{NT} Tu ::Matrix{NT} @@ -60,6 +61,7 @@ struct LinMPC{ ŷ = copy(model.yop) # dummy vals (updated just before optimization) # dummy vals (updated just before optimization): R̂y, R̂u, Tu_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp) + lastu0 = zeros(NT, nu) PΔu = init_ZtoΔU(estim, transcription, Hp, Hc) Pu, Tu = init_ZtoU(estim, transcription, Hp, Hc) E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat( @@ -91,6 +93,7 @@ struct LinMPC{ Hp, Hc, nϵ, weights, R̂u, R̂y, + lastu0, P̃Δu, P̃u, Tu, Tu_lastu0, Ẽ, F, G, J, K, V, B, H̃, q̃, r, @@ -256,7 +259,7 @@ function LinMPC( transcription::TranscriptionMethod = DEFAULT_LINMPC_TRANSCRIPTION, optim::JM = JuMP.Model(DEFAULT_LINMPC_OPTIMIZER, add_bridges=false), ) where {NT<:Real, SE<:StateEstimator{NT}, JM<:JuMP.GenericModel} - isa(estim.model, LinModel) || error("estim.model type must be a LinModel") + isa(estim.model, LinModel) || error(MSG_LINMODEL_ERR) nk = estimate_delays(estim.model) if Hp ≤ nk @warn("prediction horizon Hp ($Hp) ≤ estimated number of delays in model "* diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 4f95a8083..e39f8d8d8 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -38,6 +38,7 @@ struct NonLinMPC{ p::PT R̂u::Vector{NT} R̂y::Vector{NT} + lastu0::Vector{NT} P̃Δu::Matrix{NT} P̃u ::Matrix{NT} Tu ::Matrix{NT} @@ -83,6 +84,7 @@ struct NonLinMPC{ ŷ = copy(model.yop) # dummy vals (updated just before optimization) # dummy vals (updated just before optimization): R̂y, R̂u, Tu_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp) + lastu0 = zeros(NT, nu) PΔu = init_ZtoΔU(estim, transcription, Hp, Hc) Pu, Tu = init_ZtoU(estim, transcription, Hp, Hc) E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat( @@ -118,6 +120,7 @@ struct NonLinMPC{ weights, JE, p, R̂u, R̂y, + lastu0, P̃Δu, P̃u, Tu, Tu_lastu0, Ẽ, F, G, J, K, V, B, H̃, q̃, r, diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 9048e22a6..c81e8b3fe 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -852,7 +852,7 @@ function linconstraint!(mpc::PredictiveController, model::LinModel, ::Transcript nx̂, fx̂ = mpc.estim.nx̂, mpc.con.fx̂ fx̂ .= mpc.con.bx̂ mul!(fx̂, mpc.con.kx̂, mpc.estim.x̂0, 1, 1) - mul!(fx̂, mpc.con.vx̂, mpc.estim.lastu0, 1, 1) + mul!(fx̂, mpc.con.vx̂, mpc.lastu0, 1, 1) if model.nd > 0 mul!(fx̂, mpc.con.gx̂, mpc.d0, 1, 1) mul!(fx̂, mpc.con.jx̂, mpc.D̂0, 1, 1) @@ -938,7 +938,7 @@ function linconstrainteq!(mpc::PredictiveController, model::LinModel, ::Multiple Fŝ = mpc.con.Fŝ Fŝ .= mpc.con.Bŝ mul!(Fŝ, mpc.con.Kŝ, mpc.estim.x̂0, 1, 1) - mul!(Fŝ, mpc.con.Vŝ, mpc.estim.lastu0, 1, 1) + mul!(Fŝ, mpc.con.Vŝ, mpc.lastu0, 1, 1) if model.nd > 0 mul!(Fŝ, mpc.con.Gŝ, mpc.d0, 1, 1) mul!(Fŝ, mpc.con.Jŝ, mpc.D̂0, 1, 1) diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index 98b63c9e2..560eab897 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -2,9 +2,6 @@ remove_op!(estim::StateEstimator, ym, d, u=nothing) -> y0m, d0, u0 Remove operating pts on measured outputs `ym`, disturbances `d` and inputs `u` (if provided). - -If `u` is provided, also store current inputs without operating points `u0` in -`estim.lastu0`. This field is used for [`PredictiveController`](@ref) computations. """ function remove_op!(estim::StateEstimator, ym, d, u=nothing) y0m, u0, d0 = estim.buffer.ym, estim.buffer.u, estim.buffer.d @@ -12,7 +9,6 @@ function remove_op!(estim::StateEstimator, ym, d, u=nothing) d0 .= d .- estim.model.dop if !isnothing(u) u0 .= u .- estim.model.uop - estim.lastu0 .= u0 end return y0m, d0, u0 end @@ -108,8 +104,7 @@ end Init `estim.x̂0` states from current inputs `u`, measured outputs `ym` and disturbances `d`. The method tries to find a good steady-state for the initial estimate ``\mathbf{x̂}``. It -stores `u - estim.model.uop` at `estim.lastu0` and removes the operating points with -[`remove_op!`](@ref), and call [`init_estimate!`](@ref): +removes the operating points with [`remove_op!`](@ref) and call [`init_estimate!`](@ref): - If `estim.model` is a [`LinModel`](@ref), it finds the steady-state of the augmented model using `u` and `d` arguments, and uses the `ym` argument to enforce that @@ -408,7 +403,6 @@ function setmodel!( yop_old = copy(estim.model.yop) dop_old = copy(estim.model.dop) setmodel_linmodel!(estim.model, model) - estim.lastu0 .+= uop_old .- model.uop setmodel_estimator!(estim, model, uop_old, yop_old, dop_old, Q̂, R̂) return estim end diff --git a/src/estimator/internal_model.jl b/src/estimator/internal_model.jl index e5ab37ef6..f7b255b67 100644 --- a/src/estimator/internal_model.jl +++ b/src/estimator/internal_model.jl @@ -1,6 +1,5 @@ struct InternalModel{NT<:Real, SM<:SimModel} <: StateEstimator{NT} model::SM - lastu0::Vector{NT} x̂op::Vector{NT} f̂op::Vector{NT} x̂0 ::Vector{NT} @@ -41,7 +40,6 @@ struct InternalModel{NT<:Real, SM<:SimModel} <: StateEstimator{NT} Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = matrices_internalmodel(model) Ĉm, D̂dm = Ĉ[i_ym,:], D̂d[i_ym,:] Âs, B̂s = init_internalmodel(As, Bs, Cs, Ds) - lastu0 = zeros(NT, nu) # x̂0 and x̂d are same object (updating x̂d will update x̂0): x̂d = x̂0 = zeros(NT, model.nx) x̂s, x̂snext = zeros(NT, nxs), zeros(NT, nxs) @@ -51,7 +49,7 @@ struct InternalModel{NT<:Real, SM<:SimModel} <: StateEstimator{NT} buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk) return new{NT, SM}( model, - lastu0, x̂op, f̂op, x̂0, x̂d, x̂s, ŷs, x̂snext, + x̂op, f̂op, x̂0, x̂d, x̂s, ŷs, x̂snext, i_ym, nx̂, nym, nyu, nxs, As, Bs, Cs, Ds, Â, B̂u, Ĉ, B̂d, D̂d, Ĉm, D̂dm, diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index 6f1ceedeb..3986b3ce7 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -1,6 +1,5 @@ struct SteadyKalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT} model::SM - lastu0::Vector{NT} x̂op ::Vector{NT} f̂op ::Vector{NT} x̂0 ::Vector{NT} @@ -56,14 +55,13 @@ struct SteadyKalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT} rethrow() end end - lastu0 = zeros(NT, nu) x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)] Q̂, R̂ = Hermitian(Q̂, :L), Hermitian(R̂, :L) corrected = [false] buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk) return new{NT, SM}( model, - lastu0, x̂op, f̂op, x̂0, + x̂op, f̂op, x̂0, i_ym, nx̂, nym, nyu, nxs, As, Cs_u, Cs_y, nint_u, nint_ym, Â, B̂u, Ĉ, B̂d, D̂d, Ĉm, D̂dm, @@ -279,7 +277,6 @@ end struct KalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT} model::SM - lastu0::Vector{NT} x̂op::Vector{NT} f̂op::Vector{NT} x̂0 ::Vector{NT} @@ -319,7 +316,6 @@ struct KalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT} Â, 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, :] validate_kfcov(nym, nx̂, Q̂, R̂, P̂_0) - lastu0 = zeros(NT, nu) x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)] Q̂, R̂ = Hermitian(Q̂, :L), Hermitian(R̂, :L) P̂_0 = Hermitian(P̂_0, :L) @@ -329,7 +325,7 @@ struct KalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT} buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk) return new{NT, SM}( model, - lastu0, x̂op, f̂op, x̂0, P̂, + x̂op, f̂op, x̂0, P̂, i_ym, nx̂, nym, nyu, nxs, As, Cs_u, Cs_y, nint_u, nint_ym, Â, B̂u, Ĉ, B̂d, D̂d, Ĉm, D̂dm, @@ -493,7 +489,6 @@ end struct UnscentedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT} model::SM - lastu0::Vector{NT} x̂op ::Vector{NT} f̂op ::Vector{NT} x̂0 ::Vector{NT} @@ -543,7 +538,6 @@ struct UnscentedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT} Ĉm, D̂dm = Ĉ[i_ym, :], D̂d[i_ym, :] validate_kfcov(nym, nx̂, Q̂, R̂, P̂_0) nσ, γ, m̂, Ŝ = init_ukf(model, nx̂, α, β, κ) - lastu0 = zeros(NT, nu) x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)] Q̂, R̂ = Hermitian(Q̂, :L), Hermitian(R̂, :L) P̂_0 = Hermitian(P̂_0, :L) @@ -556,7 +550,7 @@ struct UnscentedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT} buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk) return new{NT, SM}( model, - lastu0, x̂op, f̂op, x̂0, P̂, + x̂op, f̂op, x̂0, P̂, i_ym, nx̂, nym, nyu, nxs, As, Cs_u, Cs_y, nint_u, nint_ym, Â, B̂u, Ĉ, B̂d, D̂d, Ĉm, D̂dm, @@ -879,7 +873,6 @@ struct ExtendedKalmanFilter{ LF<:Function } <: StateEstimator{NT} model::SM - lastu0::Vector{NT} x̂op ::Vector{NT} f̂op ::Vector{NT} x̂0 ::Vector{NT} @@ -925,7 +918,6 @@ struct ExtendedKalmanFilter{ Â, 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, :] validate_kfcov(nym, nx̂, Q̂, R̂, P̂_0) - lastu0 = zeros(NT, nu) x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)] Q̂, R̂ = Hermitian(Q̂, :L), Hermitian(R̂, :L) P̂_0 = Hermitian(P̂_0, :L) @@ -937,7 +929,7 @@ struct ExtendedKalmanFilter{ buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk) return new{NT, SM, JB, LF}( model, - lastu0, x̂op, f̂op, x̂0, P̂, + x̂op, f̂op, x̂0, P̂, i_ym, nx̂, nym, nyu, nxs, As, Cs_u, Cs_y, nint_u, nint_ym, Â, B̂u, Ĉ, B̂d, D̂d, Ĉm, D̂dm, @@ -978,9 +970,9 @@ differentiation. This estimator is allocation-free if `model` simulations do not - `σR=fill(1,length(i_ym))` or *`sigmaR`* : main diagonal of the sensor noise covariance ``\mathbf{R}`` of `model` measured outputs, specified as a standard deviation vector. - `nint_u=0`: integrator quantity for the stochastic model of the unmeasured disturbances at - the manipulated inputs (vector), use `nint_u=0` for no integrator (see Extended Help). + the manipulated inputs (vector), use `nint_u=0` for no integrator. - `nint_ym=default_nint(model,i_ym,nint_u)` : same than `nint_u` but for the unmeasured - disturbances at the measured outputs, use `nint_ym=0` for no integrator (see Extended Help). + disturbances at the measured outputs, use `nint_ym=0` for no integrator. - `σQint_u=fill(1,sum(nint_u))` or *`sigmaQint_u`* : same than `σQ` but for the unmeasured disturbances at manipulated inputs ``\mathbf{Q_{int_u}}`` (composed of integrators). - `σPint_u_0=fill(1,sum(nint_u))` or *`sigmaPint_u_0`* : same than `σP_0` but for the unmeasured diff --git a/src/estimator/luenberger.jl b/src/estimator/luenberger.jl index 93f9d749c..8c574350f 100644 --- a/src/estimator/luenberger.jl +++ b/src/estimator/luenberger.jl @@ -1,6 +1,5 @@ struct Luenberger{NT<:Real, SM<:LinModel} <: StateEstimator{NT} model::SM - lastu0::Vector{NT} x̂op::Vector{NT} f̂op::Vector{NT} x̂0 ::Vector{NT} @@ -41,13 +40,12 @@ struct Luenberger{NT<:Real, SM<:LinModel} <: StateEstimator{NT} catch error("Cannot compute the Luenberger gain K̂ with specified poles.") end - lastu0 = zeros(NT, nu) x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)] corrected = [false] buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk) return new{NT, SM}( model, - lastu0, x̂op, f̂op, x̂0, + x̂op, f̂op, x̂0, i_ym, nx̂, nym, nyu, nxs, As, Cs_u, Cs_y, nint_u, nint_ym, Â, B̂u, Ĉ, B̂d, D̂d, Ĉm, D̂dm, diff --git a/src/estimator/manual.jl b/src/estimator/manual.jl new file mode 100644 index 000000000..ebd4b7bc4 --- /dev/null +++ b/src/estimator/manual.jl @@ -0,0 +1,109 @@ +struct ManualEstimator{NT<:Real, SM<:SimModel} <: StateEstimator{NT} + model::SM + x̂op ::Vector{NT} + f̂op ::Vector{NT} + x̂0 ::Vector{NT} + i_ym::Vector{Int} + nx̂ ::Int + nym::Int + nyu::Int + nxs::Int + As ::Matrix{NT} + Cs_u::Matrix{NT} + Cs_y::Matrix{NT} + nint_u ::Vector{Int} + nint_ym::Vector{Int} +  ::Matrix{NT} + B̂u ::Matrix{NT} + Ĉ ::Matrix{NT} + B̂d ::Matrix{NT} + D̂d ::Matrix{NT} + Ĉm ::Matrix{NT} + D̂dm ::Matrix{NT} + direct::Bool + corrected::Vector{Bool} + buffer::StateEstimatorBuffer{NT} + function ManualEstimator{NT}( + model::SM, i_ym, nint_u, nint_ym + ) where {NT<:Real, SM<:SimModel{NT}} + nu, ny, nd, nk = model.nu, model.ny, model.nd, model.nk + nym, nyu = validate_ym(model, i_ym) + As, Cs_u, Cs_y, nint_u, nint_ym = init_estimstoch(model, i_ym, nint_u, nint_ym) + nxs = size(As, 1) + nx̂ = model.nx + nxs + Â, 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, :] + x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)] + direct = false + corrected = [true] + buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk) + return new{NT, SM}( + model, + x̂op, f̂op, x̂0, + i_ym, nx̂, nym, nyu, nxs, + As, Cs_u, Cs_y, nint_u, nint_ym, + Â, B̂u, Ĉ, B̂d, D̂d, Ĉm, D̂dm, + direct, corrected, + buffer + ) + end +end + +@doc raw""" + ManualEstimator(model::SimModel; ) + +Construct a manual state estimator for `model` ([`LinModel`](@ref) or [`NonLinModel`](@ref)). + +This [`StateEstimator`](@ref) type allows the construction of [`PredictiveController`](@ref) +objects but turns off the built-in state estimation. The user must manually provides the +estimate ``\mathbf{x̂}_{k}(k)`` or ``\mathbf{x̂}_{k-1}(k)`` through [`setstate!`](@ref) at +each time step. Calling [`preparestate!`](@ref) and [`updatestate!`](@ref) will not modify +the estimate. See Extended Help for usage examples. + +# Arguments +- `model::SimModel` : (deterministic) model for the estimations. +- `i_ym=1:model.ny` : `model` output indices that are measured ``\mathbf{y^m}``, the rest + are unmeasured ``\mathbf{y^u}``. +- `nint_u=0`: integrator quantity for the stochastic model of the unmeasured disturbances at + the manipulated inputs (vector), use `nint_u=0` for no integrator (see Extended Help). +- `nint_ym=default_nint(model,i_ym,nint_u)` : same than `nint_u` but for the unmeasured + disturbances at the measured outputs, use `nint_ym=0` for no integrator (see Extended Help). + +# Examples +```jldoctest +julia> model = LinModel([tf(3, [30, 1]); tf(-2, [5, 1])], 0.5); + +julia> estim = ManualEstimator(model, nint_ym=0) # disable augmentation with integrators +ManualEstimator estimator with a sample time Ts = 0.5 s, LinModel and: + 1 manipulated inputs u (0 integrating states) + 2 estimated states x̂ + 2 measured outputs ym (0 integrating states) + 0 unmeasured outputs yu + 0 measured disturbances d +``` + +# Extended Help +!!! details "Extended Help" + A first use case is a linear predictive controller based on nonlinear state estimation. + The [`ManualEstimator`](@ref) serves as a wrapper to provide the minimal required + information to construct a [`PredictiveController`](@ref). e.g.: + + ```julia + a=1 + ``` +""" +function ManualEstimator( + model::SM; + i_ym::AbstractVector{Int} = 1:model.ny, + nint_u ::IntVectorOrInt = 0, + nint_ym::IntVectorOrInt = default_nint(model, i_ym, nint_u), +) where {NT<:Real, SM<:SimModel{NT}} + return ManualEstimator{NT}(model, i_ym, nint_u, nint_ym) +end + +""" + update_estimate!(estim::ManualEstimator, y0m, d0, u0) + +Do nothing for [`ManualEstimator`](@ref). +""" +update_estimate!(estim::ManualEstimator, y0m, d0, u0) = nothing \ No newline at end of file diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 71bbaa52e..f163d9071 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -43,13 +43,15 @@ end Update [`MovingHorizonEstimator`](@ref) state `estim.x̂0`. -The optimization problem of [`MovingHorizonEstimator`](@ref) documentation is solved at -each discrete time ``k``. The prediction matrices are provided at [`init_predmat_mhe`](@ref) -documentation. Once solved, the optimal estimate ``\mathbf{x̂}_k(k+p)`` is computed by -inserting the optimal values of ``\mathbf{x̂}_k(k-N_k+p)`` and ``\mathbf{Ŵ}`` in the -augmented model from ``j = N_k-1`` to ``0`` inclusively. Afterward, if ``N_k = H_e``, the -arrival covariance for the next time step ``\mathbf{P̂}_{k-N_k}(k-N_k+1)`` is estimated using -`estim.covestim` object. +The optimization problem of [`MovingHorizonEstimator`](@ref) documentation is solved if +`estim.direct` is `false` (otherwise solved in [`correct_estimate!`](@ref)). The prediction +matrices are provided at [`init_predmat_mhe`](@ref) documentation. Once solved, the optimal +estimate ``\mathbf{x̂}_k(k+p)`` is computed by inserting the optimal values of +``\mathbf{x̂}_k(k-N_k+p)`` and ``\mathbf{Ŵ}`` in the augmented model from ``j = N_k-1`` to +``0`` inclusively. Afterward, if ``N_k = H_e``, the arrival covariance for the next time +step ``\mathbf{P̂}_{k-N_k}(k-N_k+1)`` is estimated using `estim.covestim` object. It +also stores `u0` at `estim.lastu0`, so it can be added to the data window at the next time +step in [`correct_estimate!`](@ref). """ function update_estimate!(estim::MovingHorizonEstimator, y0m, d0, u0) if !estim.direct @@ -59,6 +61,7 @@ function update_estimate!(estim::MovingHorizonEstimator, y0m, d0, u0) optim_objective!(estim) end (estim.Nk[] == estim.He) && update_cov!(estim) + estim.lastu0 .= u0 return nothing end @@ -779,8 +782,10 @@ function setmodel_estimator!( # convert d to d0 with the new operating point: estim.D0[(1+nd*(i-1)):(nd*i)] .-= model.dop end + estim.lastu0 .+= uop_old estim.Z̃[nϵ+1:nϵ+nx̂] .+= x̂op_old estim.x̂0arr_old .+= x̂op_old + estim.lastu0 .-= model.uop estim.Z̃[nϵ+1:nϵ+nx̂] .-= x̂op estim.x̂0arr_old .-= x̂op # --- covariance matrices --- diff --git a/src/model/nonlinmodel.jl b/src/model/nonlinmodel.jl index 25566a7c6..a1d9c9ccc 100644 --- a/src/model/nonlinmodel.jl +++ b/src/model/nonlinmodel.jl @@ -90,7 +90,7 @@ functions are defined as: ``` where ``\mathbf{x}``, ``\mathbf{y}``, ``\mathbf{u}``, ``\mathbf{d}`` and ``\mathbf{p}`` are respectively the state, output, manipulated input, measured disturbance and parameter -vectors. As a mather of fact, the parameter argument `p` can be any Julia objects but use a +vectors. As a matter of fact, the parameter argument `p` can be any Julia objects but use a mutable type if you want to change them later e.g.: a vector. If the dynamics is a function of the time, simply add a measured disturbance defined as ``d(t) = t``. The functions can be implemented in two possible ways: diff --git a/src/state_estim.jl b/src/state_estim.jl index 93420831e..b681ef2d1 100644 --- a/src/state_estim.jl +++ b/src/state_estim.jl @@ -26,6 +26,7 @@ include("estimator/kalman.jl") include("estimator/luenberger.jl") include("estimator/mhe.jl") include("estimator/internal_model.jl") +include("estimator/manual.jl") function Base.show(io::IO, estim::StateEstimator) nu, nd = estim.model.nu, estim.model.nd diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index a81d0e62a..08d7874be 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -264,7 +264,6 @@ end @test kalmanfilter. ≈ [0.2] preparestate!(kalmanfilter, [55.0]) @test evaloutput(kalmanfilter) ≈ [55.0] - @test kalmanfilter.lastu0 ≈ [2.0 - 3.0] preparestate!(kalmanfilter, [55.0]) x̂ = updatestate!(kalmanfilter, [3.0], [55.0]) @test x̂ ≈ [3.0] @@ -507,7 +506,6 @@ end @test internalmodel. ≈ [0.2] preparestate!(internalmodel, [55.0]) @test evaloutput(internalmodel) ≈ [55.0] - @test internalmodel.lastu0 ≈ [2.0 - 3.0] preparestate!(internalmodel, [55.0]) x̂ = updatestate!(internalmodel, [3.0], [55.0]) @test x̂ ≈ [3.0] @@ -653,7 +651,6 @@ end @test ukf1. ≈ [0.2] preparestate!(ukf1, [55.0]) @test evaloutput(ukf1) ≈ [55.0] - @test ukf1.lastu0 ≈ [2.0 - 3.0] preparestate!(ukf1, [55.0]) x̂ = updatestate!(ukf1, [3.0], [55.0]) @test x̂ ≈ [3.0] @@ -818,7 +815,6 @@ end @test ekf1. ≈ [0.2] preparestate!(ekf1, [55.0]) @test evaloutput(ekf1) ≈ [55.0] - @test ekf1.lastu0 ≈ [2.0 - 3.0] preparestate!(ekf1, [55.0]) x̂ = updatestate!(ekf1, [3.0], [55.0]) @test x̂ ≈ [3.0] diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index fc06dd513..d9b91cf92 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -361,6 +361,7 @@ end preparestate!(mpc, [10]) u = moveinput!(mpc, r) @test u ≈ [2] atol=1e-2 + @test mpc.lastu0 ≈ [2] - [1] atol=1e-2 setmodel!(mpc, setop!(LinModel(tf(5, [2, 1]), 3), yop=[20], uop=[11])) @test mpc.Yop ≈ fill(20.0, 1000) @test mpc.Uop ≈ fill(11.0, 1000) @@ -368,6 +369,7 @@ end @test mpc.con.U0max ≈ fill(26.0 - 1 + 1 - 11, 1000) @test mpc.con.Y0min ≈ fill(-54.0 - 10 + 10 - 20, 1000) @test mpc.con.Y0max ≈ fill(56.0 - 10 + 10 - 20, 1000) + @test mpc.lastu0 ≈ [2] - [11] atol=1e-2 r = [40] u = moveinput!(mpc, r) @test u ≈ [15] atol=1e-2 @@ -556,9 +558,11 @@ end preparestate!(mpc, [10]) u = moveinput!(mpc, r) @test u ≈ [2] atol=1e-2 + @test mpc.lastu0 ≈ [2] - [1] atol=1e-2 setmodel!(mpc, setop!(LinModel(tf(5, [2, 1]), 3), yop=[20], uop=[11])) @test mpc.Yop ≈ fill(20.0, 1000) @test mpc.Uop ≈ fill(11.0, 1000) + @test mpc.lastu0 ≈ [2] - [11] atol=1e-2 r = [40] u = moveinput!(mpc, r) @test u ≈ [15] atol=1e-2 @@ -1112,6 +1116,7 @@ end preparestate!(mpc, [10]) u = moveinput!(mpc, r) @test u ≈ [2] atol=1e-2 + @test mpc.lastu0 ≈ [2] - [1] atol=1e-2 setmodel!(mpc, setop!(LinModel(tf(5, [200, 1]), 300), yop=[20], uop=[11])) @test mpc.Yop ≈ fill(20.0, 1000) @test mpc.Uop ≈ fill(11.0, 1000) @@ -1119,6 +1124,7 @@ end @test mpc.con.U0max ≈ fill(26.0 - 1 + 1 - 11, 1000) @test mpc.con.Y0min ≈ fill(-54.0 - 10 + 10 - 20, 1000) @test mpc.con.Y0max ≈ fill(56.0 - 10 + 10 - 20, 1000) + @test mpc.lastu0 ≈ [2] - [11] atol=1e-2 r = [40] u = moveinput!(mpc, r) @test u ≈ [15] atol=1e-2