From b1f260684fc1747d76f717c31a2d0a637567eec9 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 2 May 2024 10:45:01 -0400 Subject: [PATCH 1/2] minor doc correction --- docs/src/manual/nonlinmpc.md | 4 ++-- src/controller/execute.jl | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/docs/src/manual/nonlinmpc.md b/docs/src/manual/nonlinmpc.md index c93a5b7b..38d2fb1e 100644 --- a/docs/src/manual/nonlinmpc.md +++ b/docs/src/manual/nonlinmpc.md @@ -73,10 +73,10 @@ plot(res, plotu=false) savefig(ans, "plot1_NonLinMPC.svg"); nothing # hide ``` -The [`setname!`](@ref) function allows customizing the Y-axis labels. - ![plot1_NonLinMPC](plot1_NonLinMPC.svg) +The [`setname!`](@ref) function allows customizing the Y-axis labels. + ## Nonlinear Model Predictive Controller An [`UnscentedKalmanFilter`](@ref) estimates the plant state : diff --git a/src/controller/execute.jl b/src/controller/execute.jl index 1223f6e8..74331059 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -498,7 +498,7 @@ updatestate!(::PredictiveController, _ ) = throw(ArgumentError("missing measured """ setstate!(mpc::PredictiveController, x̂) -> mpc -Set the estimate at `mpc.estim.x̂`. +Set `mpc.estim.x̂0` to `x̂ - estim.x̂op` from the argument `x̂`. """ setstate!(mpc::PredictiveController, x̂) = (setstate!(mpc.estim, x̂); return mpc) From 4f2410e2b5e5f586bc1f0f9d5b2cb4e190a79e21 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 2 May 2024 16:23:49 -0400 Subject: [PATCH 2/2] added: non-unicode alternative in API keyword args and dict fields --- src/controller/construct.jl | 42 ++++++---- src/controller/execute.jl | 58 +++++++++----- src/estimator/kalman.jl | 139 +++++++++++++++++++++------------ src/estimator/luenberger.jl | 28 +++---- src/estimator/mhe/construct.jl | 70 +++++++++++------ src/estimator/mhe/execute.jl | 34 +++++--- src/plot_sim.jl | 13 ++- test/test_state_estim.jl | 6 +- 8 files changed, 250 insertions(+), 140 deletions(-) diff --git a/src/controller/construct.jl b/src/controller/construct.jl index c0b482f4..819be9c6 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -53,6 +53,14 @@ for details on bounds and softness parameters ``\mathbf{c}``. The output and ter constraints are all soft by default. See Extended Help for time-varying constraints. # Arguments +!!! info + The keyword arguments `Δumin`, `Δumax`, `c_Δumin`, `c_Δumax`, `x̂min`, `x̂max`, `c_x̂min`, + `c_x̂max` and their capital letter versions have non-Unicode alternatives e.g. + *`Deltaumin`*, *`xhatmax`* and *`C_Deltaumin`* + + The default constraints are mentioned here for clarity but omitting a keyword argument + will not re-assign to its default value (defaults are set at construction only). + - `mpc::PredictiveController` : predictive controller to set constraints - `umin=fill(-Inf,nu)` / `umax=fill(+Inf,nu)` : manipulated input bound ``\mathbf{u_{min/max}}`` - `Δumin=fill(-Inf,nu)` / `Δumax=fill(+Inf,nu)` : manipulated input increment bound ``\mathbf{Δu_{min/max}}`` @@ -114,20 +122,26 @@ LinMPC controller with a sample time Ts = 4.0 s, OSQP optimizer, SteadyKalmanFil """ function setconstraint!( mpc::PredictiveController; - umin = nothing, umax = nothing, - Δumin = nothing, Δumax = nothing, - ymin = nothing, ymax = nothing, - x̂min = nothing, x̂max = nothing, - c_umin = nothing, c_umax = nothing, - c_Δumin = nothing, c_Δumax = nothing, - c_ymin = nothing, c_ymax = nothing, - c_x̂min = nothing, c_x̂max = nothing, - Umin = nothing, Umax = nothing, - ΔUmin = nothing, ΔUmax = nothing, - Ymin = nothing, Ymax = nothing, - C_umax = nothing, C_umin = nothing, - C_Δumax = nothing, C_Δumin = nothing, - C_ymax = nothing, C_ymin = nothing, + umin = nothing, umax = nothing, + Deltaumin = nothing, Deltaumax = nothing, + ymin = nothing, ymax = nothing, + xhatmin = nothing, xhatmax = nothing, + c_umin = nothing, c_umax = nothing, + c_Deltaumin = nothing, c_Deltaumax = nothing, + c_ymin = nothing, c_ymax = nothing, + c_xhatmin = nothing, c_xhatmax = nothing, + Umin = nothing, Umax = nothing, + DeltaUmin = nothing, DeltaUmax = nothing, + Ymin = nothing, Ymax = nothing, + C_umax = nothing, C_umin = nothing, + C_Deltaumax = nothing, C_Deltaumin = nothing, + C_ymax = nothing, C_ymin = 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, con, optim = mpc.estim.model, mpc.con, mpc.optim nu, ny, nx̂, Hp, Hc = model.nu, model.ny, mpc.estim.nx̂, mpc.Hp, mpc.Hc diff --git a/src/controller/execute.jl b/src/controller/execute.jl index 74331059..1cdfa722 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -24,15 +24,19 @@ Calling a [`PredictiveController`](@ref) object calls this method. See also [`LinMPC`](@ref), [`ExplicitMPC`](@ref), [`NonLinMPC`](@ref). # Arguments + +Keyword arguments in *`italic`* are non-Unicode alternatives. + - `mpc::PredictiveController` : solve optimization problem of `mpc`. - `ry=mpc.estim.model.yop` : current output setpoints ``\mathbf{r_y}(k)``. - `d=[]` : current measured disturbances ``\mathbf{d}(k)``. -- `D̂=repeat(d, mpc.Hp)` : predicted measured disturbances ``\mathbf{D̂}``, constant in the - future by default or ``\mathbf{d̂}(k+j)=\mathbf{d}(k)`` for ``j=1`` to ``H_p``. -- `R̂y=repeat(ry, mpc.Hp)` : predicted output setpoints ``\mathbf{R̂_y}``, constant in the - future by default or ``\mathbf{r̂_y}(k+j)=\mathbf{r_y}(k)`` for ``j=1`` to ``H_p``. -- `R̂u=repeat(mpc.estim.model.uop, mpc.Hp)` : predicted manipulated input setpoints, constant - in the future by default or ``\mathbf{r̂_u}(k+j)=\mathbf{u_{op}}`` for ``j=0`` to ``H_p-1``. +- `D̂=repeat(d, mpc.Hp)` or *`Dhat`* : predicted measured disturbances ``\mathbf{D̂}``, constant + in the future by default or ``\mathbf{d̂}(k+j)=\mathbf{d}(k)`` for ``j=1`` to ``H_p``. +- `R̂y=repeat(ry, mpc.Hp)` or *`Rhaty`* : predicted output setpoints ``\mathbf{R̂_y}``, constant + in the future by default or ``\mathbf{r̂_y}(k+j)=\mathbf{r_y}(k)`` for ``j=1`` to ``H_p``. +- `R̂u=repeat(mpc.estim.model.uop, mpc.Hp)` or *`Rhatu`* : predicted manipulated input + setpoints, constant in the future by default or ``\mathbf{r̂_u}(k+j)=\mathbf{u_{op}}`` for + ``j=0`` to ``H_p-1``. - `ym=nothing` : current measured outputs ``\mathbf{y^m}(k)``, only required if `mpc.estim` is an [`InternalModel`](@ref). @@ -49,10 +53,13 @@ function moveinput!( mpc::PredictiveController, ry::Vector = mpc.estim.model.yop, d ::Vector = empty(mpc.estim.x̂0); - D̂ ::Vector = repeat(d, mpc.Hp), - R̂y::Vector = repeat(ry, mpc.Hp), - R̂u::Vector = mpc.noR̂u ? empty(mpc.estim.x̂0) : repeat(mpc.estim.model.uop, mpc.Hp), - ym::Union{Vector, Nothing} = nothing + Dhat ::Vector = repeat(d, mpc.Hp), + Rhaty::Vector = repeat(ry, mpc.Hp), + Rhatu::Vector = mpc.noR̂u ? empty(mpc.estim.x̂0) : repeat(mpc.estim.model.uop, mpc.Hp), + ym::Union{Vector, Nothing} = nothing, + D̂ = Dhat, + R̂y = Rhaty, + R̂u = Rhatu ) validate_args(mpc, ry, d, D̂, R̂y, R̂u) initpred!(mpc, mpc.estim.model, d, ym, D̂, R̂y, R̂u) @@ -73,19 +80,22 @@ Get additional info about `mpc` [`PredictiveController`](@ref) optimum for troub The function should be called after calling [`moveinput!`](@ref). It returns the dictionary `info` with the following fields: -- `:ΔU` : optimal manipulated input increments over ``H_c``, ``\mathbf{ΔU}`` -- `:ϵ` : optimal slack variable, ``ϵ`` +!!! info + Fields in *`italic`* are non-Unicode alternatives. + +- `:ΔU` or *`:DeltaU`* : optimal manipulated input increments over ``H_c``, ``\mathbf{ΔU}`` +- `:ϵ` or *`:epsilon`* : optimal slack variable, ``ϵ`` +- `:D̂` or *`:Dhat`* : predicted measured disturbances over ``H_p``, ``\mathbf{D̂}`` +- `:ŷ` or *`:yhat`* : current estimated output, ``\mathbf{ŷ}(k)`` +- `:Ŷ` or *`:Yhat`* : optimal predicted outputs over ``H_p``, ``\mathbf{Ŷ}`` +- `:Ŷs` or *`:Yhats`* : predicted stochastic output over ``H_p`` of [`InternalModel`](@ref), ``\mathbf{Ŷ_s}`` +- `:R̂y` or *`:Rhaty`* : predicted output setpoint over ``H_p``, ``\mathbf{R̂_y}`` +- `:R̂u` or *`:Rhatu`* : predicted manipulated input setpoint over ``H_p``, ``\mathbf{R̂_u}`` +- `:x̂end` or *`:xhatend`* : optimal terminal states, ``\mathbf{x̂}_{k-1}(k+H_p)`` - `:J` : objective value optimum, ``J`` - `:U` : optimal manipulated inputs over ``H_p``, ``\mathbf{U}`` - `:u` : current optimal manipulated input, ``\mathbf{u}(k)`` - `:d` : current measured disturbance, ``\mathbf{d}(k)`` -- `:D̂` : predicted measured disturbances over ``H_p``, ``\mathbf{D̂}`` -- `:ŷ` : current estimated output, ``\mathbf{ŷ}(k)`` -- `:Ŷ` : optimal predicted outputs over ``H_p``, ``\mathbf{Ŷ}`` -- `:x̂end`: optimal terminal states, ``\mathbf{x̂}_{k-1}(k+H_p)`` -- `:Ŷs` : predicted stochastic output over ``H_p`` of [`InternalModel`](@ref), ``\mathbf{Ŷ_s}`` -- `:R̂y` : predicted output setpoint over ``H_p``, ``\mathbf{R̂_y}`` -- `:R̂u` : predicted manipulated input setpoint over ``H_p``, ``\mathbf{R̂_u}`` For [`LinMPC`](@ref) and [`NonLinMPC`](@ref), the field `:sol` also contains the optimizer solution summary that can be printed. Lastly, the optimal economic cost `:JE` is also @@ -129,6 +139,16 @@ function getinfo(mpc::PredictiveController{NT}) where NT<:Real info[:Ŷs] = Ŷs info[:R̂y] = mpc.R̂y0 + mpc.Yop info[:R̂u] = mpc.R̂u0 + mpc.Uop + # --- non-Unicode fields --- + info[:DeltaU] = info[:ΔU] + info[:epsilon] = info[:ϵ] + info[:Dhat] = info[:D̂] + info[:yhat] = info[:ŷ] + info[:Yhat] = info[:Ŷ] + info[:xhatend] = info[:x̂end] + info[:Yhats] = info[:Ŷs] + info[:Rhaty] = info[:R̂y] + info[:Rhatu] = info[:R̂u] info = addinfo!(info, mpc) return info end diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index 3b0c19f7..1f04ce9e 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -86,21 +86,24 @@ the rows of ``\mathbf{Ĉ, D̂_d}`` that correspond to measured outputs ``\mathb unmeasured ones, for ``\mathbf{Ĉ^u, D̂_d^u}``). # Arguments +!!! info + Keyword arguments in *`italic`* are non-Unicode alternatives. + - `model::LinModel` : (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}``. -- `σQ=fill(1/model.nx,model.nx)` : main diagonal of the process noise covariance - ``\mathbf{Q}`` of `model`, specified as a standard deviation vector. -- `σR=fill(1,length(i_ym))` : main diagonal of the sensor noise covariance ``\mathbf{R}`` - of `model` measured outputs, specified as a standard deviation vector. +- `σQ=fill(1/model.nx,model.nx)` or *`sigmaQ`* : main diagonal of the process noise + covariance ``\mathbf{Q}`` of `model`, specified as a standard deviation vector. +- `σ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). -- `σQint_u=fill(1,sum(nint_u))`: same than `σQ` but for the unmeasured disturbances at - manipulated inputs ``\mathbf{Q_{int_u}}`` (composed of integrators). - `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). -- `σQint_ym=fill(1,sum(nint_ym))` : same than `σQ` for the unmeasured disturbances at - measured outputs ``\mathbf{Q_{int_{ym}}}`` (composed of integrators). +- `σ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). +- `σQint_ym=fill(1,sum(nint_ym))` or *`sigmaQint_u`* : same than `σQ` for the unmeasured + disturbances at measured outputs ``\mathbf{Q_{int_{ym}}}`` (composed of integrators). # Examples ```jldoctest @@ -139,12 +142,16 @@ SteadyKalmanFilter estimator with a sample time Ts = 0.5 s, LinModel and: function SteadyKalmanFilter( model::SM; i_ym::IntRangeOrVector = 1:model.ny, - σQ::Vector = fill(1/model.nx, model.nx), - σR::Vector = fill(1, length(i_ym)), + sigmaQ = fill(1/model.nx, model.nx), + sigmaR = fill(1, length(i_ym)), nint_u ::IntVectorOrInt = 0, - σQint_u::Vector = fill(1, max(sum(nint_u), 0)), nint_ym::IntVectorOrInt = default_nint(model, i_ym, nint_u), - σQint_ym::Vector = fill(1, max(sum(nint_ym), 0)) + sigmaQint_u = fill(1, max(sum(nint_u), 0)), + sigmaQint_ym = fill(1, max(sum(nint_ym), 0)), + σQ = sigmaQ, + σR = sigmaR, + σQint_u = sigmaQint_u, + σQint_ym = sigmaQint_ym, ) where {NT<:Real, SM<:LinModel{NT}} # estimated covariances matrices (variance = σ²) : Q̂ = Hermitian(diagm(NT[σQ; σQint_u; σQint_ym ].^2), :L) @@ -266,13 +273,16 @@ its initial value with ``\mathbf{P̂}_{-1}(0) = \mathrm{diag}\{ \mathbf{P}(0), \mathbf{P_{int_{u}}}(0), \mathbf{P_{int_{ym}}}(0) \}``. # Arguments +!!! info + Keyword arguments in *`italic`* are non-Unicode alternatives. + - `model::LinModel` : (deterministic) model for the estimations. -- `σP_0=fill(1/model.nx,model.nx)` : main diagonal of the initial estimate covariance - ``\mathbf{P}(0)``, specified as a standard deviation vector. -- `σPint_u_0=fill(1,sum(nint_u))` : same than `σP_0` but for the unmeasured disturbances at - manipulated inputs ``\mathbf{P_{int_u}}(0)`` (composed of integrators). -- `σPint_ym_0=fill(1,sum(nint_ym))` : same than `σP_0` but for the unmeasured disturbances - at measured outputs ``\mathbf{P_{int_{ym}}}(0)`` (composed of integrators). +- `σP_0=fill(1/model.nx,model.nx)` or *`sigmaP_0`* : main diagonal of the initial estimate + covariance ``\mathbf{P}(0)``, specified as a standard deviation vector. +- `σPint_u_0=fill(1,sum(nint_u))` or *`sigmaPint_u_0`* : same than `σP_0` but for the unmeasured + disturbances at manipulated inputs ``\mathbf{P_{int_u}}(0)`` (composed of integrators). +- `σPint_ym_0=fill(1,sum(nint_ym))` or *`sigmaPint_ym_0`* : same than `σP_0` but for the unmeasured + disturbances at measured outputs ``\mathbf{P_{int_{ym}}}(0)`` (composed of integrators). - `` of [`SteadyKalmanFilter`](@ref) constructor. # Examples @@ -291,15 +301,22 @@ KalmanFilter estimator with a sample time Ts = 0.5 s, LinModel and: function KalmanFilter( model::SM; i_ym::IntRangeOrVector = 1:model.ny, - σP_0::Vector = fill(1/model.nx, model.nx), - σQ ::Vector = fill(1/model.nx, model.nx), - σR ::Vector = fill(1, length(i_ym)), - nint_u ::IntVectorOrInt = 0, - σQint_u ::Vector = fill(1, max(sum(nint_u), 0)), - σPint_u_0 ::Vector = fill(1, max(sum(nint_u), 0)), - nint_ym ::IntVectorOrInt = default_nint(model, i_ym, nint_u), - σQint_ym ::Vector = fill(1, max(sum(nint_ym), 0)), - σPint_ym_0::Vector = fill(1, max(sum(nint_ym), 0)), + sigmaP_0 = fill(1/model.nx, model.nx), + sigmaQ = fill(1/model.nx, model.nx), + sigmaR = fill(1, length(i_ym)), + nint_u ::IntVectorOrInt = 0, + nint_ym::IntVectorOrInt = default_nint(model, i_ym, nint_u), + sigmaPint_u_0 = fill(1, max(sum(nint_u), 0)), + sigmaQint_u = fill(1, max(sum(nint_u), 0)), + sigmaPint_ym_0 = fill(1, max(sum(nint_ym), 0)), + sigmaQint_ym = fill(1, max(sum(nint_ym), 0)), + σP_0 = sigmaP_0, + σQ = sigmaQ, + σR = sigmaR, + σPint_u_0 = sigmaPint_u_0, + σQint_u = sigmaQint_u, + σPint_ym_0 = sigmaPint_ym_0, + σQint_ym = sigmaQint_ym, ) where {NT<:Real, SM<:LinModel{NT}} # estimated covariances matrices (variance = σ²) : P̂_0 = Hermitian(diagm(NT[σP_0; σPint_u_0; σPint_ym_0].^2), :L) @@ -440,10 +457,13 @@ represents the measured outputs of ``\mathbf{ĥ}`` function (and unmeasured one ``\mathbf{ĥ^u}``). # Arguments +!!! info + Keyword arguments in *`italic`* are non-Unicode alternatives. + - `model::SimModel` : (deterministic) model for the estimations. -- `α=1e-3` : alpha parameter, spread of the state distribution ``(0 < α ≤ 1)``. -- `β=2` : beta parameter, skewness and kurtosis of the states distribution ``(β ≥ 0)``. -- `κ=0` : kappa parameter, another spread parameter ``(0 ≤ κ ≤ 3)``. +- `α=1e-3` or *`alpha`* : alpha parameter, spread of the state distribution ``(0 < α ≤ 1)``. +- `β=2` or *`beta`* : beta parameter, skewness and kurtosis of the states distribution ``(β ≥ 0)``. +- `κ=0` or *`kappa`* : kappa parameter, another spread parameter ``(0 ≤ κ ≤ 3)``. - `` of [`SteadyKalmanFilter`](@ref) constructor. - `` of [`KalmanFilter`](@ref) constructor. @@ -470,18 +490,28 @@ UnscentedKalmanFilter estimator with a sample time Ts = 10.0 s, NonLinModel and: function UnscentedKalmanFilter( model::SM; i_ym::IntRangeOrVector = 1:model.ny, - σP_0::Vector = fill(1/model.nx, model.nx), - σQ ::Vector = fill(1/model.nx, model.nx), - σR ::Vector = fill(1, length(i_ym)), - nint_u ::IntVectorOrInt = 0, - σQint_u ::Vector = fill(1, max(sum(nint_u), 0)), - σPint_u_0 ::Vector = fill(1, max(sum(nint_u), 0)), - nint_ym ::IntVectorOrInt = default_nint(model, i_ym, nint_u), - σQint_ym ::Vector = fill(1, max(sum(nint_ym), 0)), - σPint_ym_0::Vector = fill(1, max(sum(nint_ym), 0)), - α::Real = 1e-3, - β::Real = 2, - κ::Real = 0 + sigmaP_0 = fill(1/model.nx, model.nx), + sigmaQ = fill(1/model.nx, model.nx), + sigmaR = fill(1, length(i_ym)), + nint_u ::IntVectorOrInt = 0, + nint_ym::IntVectorOrInt = default_nint(model, i_ym, nint_u), + sigmaPint_u_0 = fill(1, max(sum(nint_u), 0)), + sigmaQint_u = fill(1, max(sum(nint_u), 0)), + sigmaPint_ym_0 = fill(1, max(sum(nint_ym), 0)), + sigmaQint_ym = fill(1, max(sum(nint_ym), 0)), + alpha::Real = 1e-3, + beta ::Real = 2, + kappa::Real = 0, + σP_0 = sigmaP_0, + σQ = sigmaQ, + σR = sigmaR, + σPint_u_0 = sigmaPint_u_0, + σQint_u = sigmaQint_u, + σPint_ym_0 = sigmaPint_ym_0, + σQint_ym = sigmaQint_ym, + α = alpha, + β = beta, + κ = kappa, ) where {NT<:Real, SM<:SimModel{NT}} # estimated covariances matrices (variance = σ²) : P̂_0 = Hermitian(diagm(NT[σP_0; σPint_u_0; σPint_ym_0].^2), :L) @@ -732,15 +762,22 @@ ExtendedKalmanFilter estimator with a sample time Ts = 5.0 s, NonLinModel and: function ExtendedKalmanFilter( model::SM; i_ym::IntRangeOrVector = 1:model.ny, - σP_0::Vector = fill(1/model.nx, model.nx), - σQ ::Vector = fill(1/model.nx, model.nx), - σR ::Vector = fill(1, length(i_ym)), - nint_u ::IntVectorOrInt = 0, - σQint_u ::Vector = fill(1, max(sum(nint_u), 0)), - σPint_u_0 ::Vector = fill(1, max(sum(nint_u), 0)), - nint_ym ::IntVectorOrInt = default_nint(model, i_ym, nint_u), - σQint_ym ::Vector = fill(1, max(sum(nint_ym), 0)), - σPint_ym_0::Vector = fill(1, max(sum(nint_ym), 0)), + sigmaP_0 = fill(1/model.nx, model.nx), + sigmaQ = fill(1/model.nx, model.nx), + sigmaR = fill(1, length(i_ym)), + nint_u ::IntVectorOrInt = 0, + nint_ym::IntVectorOrInt = default_nint(model, i_ym, nint_u), + sigmaPint_u_0 = fill(1, max(sum(nint_u), 0)), + sigmaQint_u = fill(1, max(sum(nint_u), 0)), + sigmaPint_ym_0 = fill(1, max(sum(nint_ym), 0)), + sigmaQint_ym = fill(1, max(sum(nint_ym), 0)), + σP_0 = sigmaP_0, + σQ = sigmaQ, + σR = sigmaR, + σPint_u_0 = sigmaPint_u_0, + σQint_u = sigmaQint_u, + σPint_ym_0 = sigmaPint_ym_0, + σQint_ym = sigmaQint_ym, ) where {NT<:Real, SM<:SimModel{NT}} # estimated covariances matrices (variance = σ²) : P̂_0 = Hermitian(diagm(NT[σP_0; σPint_u_0; σPint_ym_0].^2), :L) diff --git a/src/estimator/luenberger.jl b/src/estimator/luenberger.jl index 3bed0489..43d08aa6 100644 --- a/src/estimator/luenberger.jl +++ b/src/estimator/luenberger.jl @@ -21,18 +21,18 @@ struct Luenberger{NT<:Real, SM<:LinModel} <: StateEstimator{NT} D̂d ::Matrix{NT} K̂::Matrix{NT} function Luenberger{NT, SM}( - model, i_ym, nint_u, nint_ym, p̂ + model, i_ym, nint_u, nint_ym, poles ) where {NT<:Real, SM<:LinModel} nym, nyu = validate_ym(model, i_ym) - validate_luenberger(model, nint_u, nint_ym, p̂) + validate_luenberger(model, nint_u, nint_ym, poles) 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) K̂ = try - ControlSystemsBase.place(Â, Ĉ, p̂, :o)[:, i_ym] + ControlSystemsBase.place(Â, Ĉ, poles, :o)[:, i_ym] catch - error("Cannot compute the Luenberger gain K̂ with specified poles p̂.") + error("Cannot compute the Luenberger gain K̂ with specified poles.") end lastu0 = zeros(NT, model.nu) x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)] @@ -53,7 +53,7 @@ end i_ym = 1:model.ny, nint_u = 0, nint_ym = default_nint(model, i_ym), - p̂ = 1e-3*(1:(model.nx + sum(nint_u) + sum(nint_ym))) .+ 0.5 + poles = 1e-3*(1:(model.nx + sum(nint_u) + sum(nint_ym))) .+ 0.5 ) Construct a Luenberger observer with the [`LinModel`](@ref) `model`. @@ -61,7 +61,7 @@ Construct a Luenberger observer with the [`LinModel`](@ref) `model`. `i_ym` provides the `model` output indices that are measured ``\mathbf{y^m}``, the rest are unmeasured ``\mathbf{y^u}``. `model` matrices are augmented with the stochastic model, which is specified by the numbers of integrator `nint_u` and `nint_ym` (see [`SteadyKalmanFilter`](@ref) -Extended Help). The argument `p̂` is a vector of `model.nx + sum(nint_u) + sum(nint_ym)` +Extended Help). The argument `poles` is a vector of `model.nx + sum(nint_u) + sum(nint_ym)` elements specifying the observer poles/eigenvalues (near ``z=0.5`` by default). The method computes the observer gain `K̂` with [`place`](https://juliacontrol.github.io/ControlSystems.jl/stable/lib/synthesis/#ControlSystemsBase.place). @@ -69,7 +69,7 @@ computes the observer gain `K̂` with [`place`](https://juliacontrol.github.io/C ```jldoctest julia> model = LinModel([tf(3, [30, 1]); tf(-2, [5, 1])], 0.5); -julia> estim = Luenberger(model, nint_ym=[1, 1], p̂=[0.61, 0.62, 0.63, 0.64]) +julia> estim = Luenberger(model, nint_ym=[1, 1], poles=[0.61, 0.62, 0.63, 0.64]) Luenberger estimator with a sample time Ts = 0.5 s, LinModel and: 1 manipulated inputs u (0 integrating states) 4 estimated states x̂ @@ -83,18 +83,18 @@ function Luenberger( i_ym::IntRangeOrVector = 1:model.ny, nint_u ::IntVectorOrInt = 0, nint_ym::IntVectorOrInt = default_nint(model, i_ym, nint_u), - p̂ = 1e-3*(1:(model.nx + sum(nint_u) + sum(nint_ym))) .+ 0.5 + poles = 1e-3*(1:(model.nx + sum(nint_u) + sum(nint_ym))) .+ 0.5 ) where{NT<:Real, SM<:LinModel{NT}} - return Luenberger{NT, SM}(model, i_ym, nint_u, nint_ym, p̂) + return Luenberger{NT, SM}(model, i_ym, nint_u, nint_ym, poles) end -"Validate the quantity and stability of the Luenberger poles `p̂`." -function validate_luenberger(model, nint_u, nint_ym, p̂) - if length(p̂) ≠ model.nx + sum(nint_u) + sum(nint_ym) - error("p̂ length ($(length(p̂))) ≠ nx ($(model.nx)) + "* +"Validate the quantity and stability of the Luenberger `poles`." +function validate_luenberger(model, nint_u, nint_ym, poles) + if length(poles) ≠ model.nx + sum(nint_u) + sum(nint_ym) + error("poles length ($(length(poles))) ≠ nx ($(model.nx)) + "* "integrator quantity ($(sum(nint_ym)))") end - any(abs.(p̂) .≥ 1) && error("Observer poles p̂ should be inside the unit circles.") + any(abs.(poles) .≥ 1) && error("Observer poles should be inside the unit circles.") end diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index b91e54a6..52353727 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -277,17 +277,24 @@ function MovingHorizonEstimator( model::SM; He::Union{Int, Nothing} = nothing, i_ym::IntRangeOrVector = 1:model.ny, - σP_0::Vector = fill(1/model.nx, model.nx), - σQ ::Vector = fill(1/model.nx, model.nx), - σR ::Vector = fill(1, length(i_ym)), - nint_u ::IntVectorOrInt = 0, - σQint_u ::Vector = fill(1, max(sum(nint_u), 0)), - σPint_u_0 ::Vector = fill(1, max(sum(nint_u), 0)), - nint_ym ::IntVectorOrInt = default_nint(model, i_ym, nint_u), - σQint_ym ::Vector = fill(1, max(sum(nint_ym), 0)), - σPint_ym_0::Vector = fill(1, max(sum(nint_ym), 0)), + sigmaP_0 = fill(1/model.nx, model.nx), + sigmaQ = fill(1/model.nx, model.nx), + sigmaR = fill(1, length(i_ym)), + nint_u ::IntVectorOrInt = 0, + nint_ym::IntVectorOrInt = default_nint(model, i_ym, nint_u), + sigmaPint_u_0 = fill(1, max(sum(nint_u), 0)), + sigmaQint_u = fill(1, max(sum(nint_u), 0)), + sigmaPint_ym_0 = fill(1, max(sum(nint_ym), 0)), + sigmaQint_ym = fill(1, max(sum(nint_ym), 0)), Cwt::Real = Inf, optim::JM = default_optim_mhe(model), + σP_0 = sigmaP_0, + σQ = sigmaQ, + σR = sigmaR, + σPint_u_0 = sigmaPint_u_0, + σQint_u = sigmaQint_u, + σPint_ym_0 = sigmaPint_ym_0, + σQint_ym = sigmaQint_ym, ) where {NT<:Real, SM<:SimModel{NT}, JM<:JuMP.GenericModel} # estimated covariances matrices (variance = σ²) : P̂_0 = Hermitian(diagm(NT[σP_0; σPint_u_0; σPint_ym_0].^2), :L) @@ -352,9 +359,10 @@ model augmentation and time-varying constraints. # Arguments !!! info + All the keyword arguments have non-Unicode alternatives e.g. *`xhatmin`* or *`Vhatmax`*. + The default constraints are mentioned here for clarity but omitting a keyword argument - will not re-assign to its default value (defaults are set at construction only). The - same applies for [`PredictiveController`](@ref). + will not re-assign to its default value (defaults are set at construction only). - `estim::MovingHorizonEstimator` : moving horizon estimator to set constraints - `x̂min=fill(-Inf,nx̂)` / `x̂max=fill(+Inf,nx̂)` : estimated state bound ``\mathbf{x̂_{min/max}}`` @@ -363,8 +371,8 @@ model augmentation and time-varying constraints. - `c_x̂min=fill(0.0,nx̂)` / `c_x̂max=fill(0.0,nx̂)` : `x̂min` / `x̂max` softness weight ``\mathbf{c_{x̂_{min/max}}}`` - `c_ŵmin=fill(0.0,nx̂)` / `c_ŵmax=fill(0.0,nx̂)` : `ŵmin` / `ŵmax` softness weight ``\mathbf{c_{ŵ_{min/max}}}`` - `c_v̂min=fill(0.0,nym)` / `c_v̂max=fill(0.0,nym)` : `v̂min` / `v̂max` softness weight ``\mathbf{c_{v̂_{min/max}}}`` -- all the keyword arguments above but with a first capital letter, e.g. `X̂max` or `C_ŵmax`: - for time-varying constraints (see Extended Help) +- all the keyword arguments above but with a first capital letter, e.g. `X̂max` or `C_ŵmax`: + for time-varying constraints (see Extended Help) # Examples ```jldoctest @@ -402,18 +410,30 @@ MovingHorizonEstimator estimator with a sample time Ts = 1.0 s, OSQP optimizer, """ function setconstraint!( estim::MovingHorizonEstimator; - x̂min = nothing, x̂max = nothing, - ŵmin = nothing, ŵmax = nothing, - v̂min = nothing, v̂max = nothing, - c_x̂min = nothing, c_x̂max = nothing, - c_ŵmin = nothing, c_ŵmax = nothing, - c_v̂min = nothing, c_v̂max = nothing, - X̂min = nothing, X̂max = nothing, - Ŵmin = nothing, Ŵmax = nothing, - V̂min = nothing, V̂max = nothing, - C_x̂min = nothing, C_x̂max = nothing, - C_ŵmin = nothing, C_ŵmax = nothing, - C_v̂min = nothing, C_v̂max = nothing, + xhatmin = nothing, xhatmax = nothing, + whatmin = nothing, whatmax = nothing, + vhatmin = nothing, vhatmax = nothing, + c_xhatmin = nothing, c_xhatmax = nothing, + c_whatmin = nothing, c_whatmax = nothing, + c_vhatmin = nothing, c_vhatmax = nothing, + Xhatmin = nothing, Xhatmax = nothing, + Whatmin = nothing, Whatmax = nothing, + Vhatmin = nothing, Vhatmax = nothing, + C_xhatmin = nothing, C_xhatmax = nothing, + C_whatmin = nothing, C_whatmax = nothing, + C_vhatmin = nothing, C_vhatmax = nothing, + x̂min = xhatmin, x̂max = xhatmax, + ŵmin = whatmin, ŵmax = whatmax, + v̂min = vhatmin, v̂max = vhatmax, + c_x̂min = c_xhatmin, c_x̂max = c_xhatmax, + c_ŵmin = c_whatmin, c_ŵmax = c_whatmax, + c_v̂min = c_vhatmin, c_v̂max = c_vhatmax, + X̂min = Xhatmin, X̂max = Xhatmax, + Ŵmin = Whatmin, Ŵmax = Whatmax, + V̂min = Vhatmin, V̂max = Vhatmax, + C_x̂min = C_xhatmin, C_x̂max = C_xhatmax, + C_ŵmin = C_whatmin, C_ŵmax = C_whatmax, + C_v̂min = C_vhatmin, C_v̂max = C_vhatmax, ) model, optim, con = estim.model, estim.optim, estim.con nx̂, nŵ, nym, He = estim.nx̂, estim.nx̂, estim.nym, estim.He diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index e6558dd2..c3ef5d26 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -95,17 +95,20 @@ Get additional info on `estim` [`MovingHorizonEstimator`](@ref) optimum for trou The function should be called after calling [`updatestate!`](@ref). It returns the dictionary `info` with the following fields: -- `:Ŵ` : optimal estimated process noise over ``N_k``, ``\mathbf{Ŵ}`` -- `:x̂arr`: optimal estimated state at arrival, ``\mathbf{x̂}_k(k-N_k+1)`` -- `:ϵ` : optimal slack variable, ``ϵ`` +!!! info + Fields in *`italic`* are non-Unicode alternatives. + +- `:Ŵ` or *`:What`* : optimal estimated process noise over ``N_k``, ``\mathbf{Ŵ}`` +- `:ϵ` or *`:epsilon`* : optimal slack variable, ``ϵ`` +- `:X̂` or *`:Xhat`* : optimal estimated states over ``N_k+1``, ``\mathbf{X̂}`` +- `:x̂` or *`:xhat`* : optimal estimated state for the next time step, ``\mathbf{x̂}_k(k+1)`` +- `:V̂` or *`:Vhat`* : optimal estimated sensor noise over ``N_k``, ``\mathbf{V̂}`` +- `:P̄` or *`:Pbar`* : estimation error covariance at arrival, ``\mathbf{P̄}`` +- `:x̄` or *`:xbar`* : optimal estimation error at arrival, ``\mathbf{x̄}`` +- `:Ŷ` or *`:Yhat`* : optimal estimated outputs over ``N_k``, ``\mathbf{Ŷ}`` +- `:Ŷm` or *`:Yhatm`* : optimal estimated measured outputs over ``N_k``, ``\mathbf{Ŷ^m}`` +- `:x̂arr` or *`:xhatarr`* : optimal estimated state at arrival, ``\mathbf{x̂}_k(k-N_k+1)`` - `:J` : objective value optimum, ``J`` -- `:X̂` : optimal estimated states over ``N_k+1``, ``\mathbf{X̂}`` -- `:x̂` : optimal estimated state for the next time step, ``\mathbf{x̂}_k(k+1)`` -- `:V̂` : optimal estimated sensor noise over ``N_k``, ``\mathbf{V̂}`` -- `:P̄` : estimation error covariance at arrival, ``\mathbf{P̄}`` -- `:x̄` : optimal estimation error at arrival, ``\mathbf{x̄}`` -- `:Ŷ` : optimal estimated outputs over ``N_k``, ``\mathbf{Ŷ}`` -- `:Ŷm` : optimal estimated measured outputs over ``N_k``, ``\mathbf{Ŷ^m}`` - `:Ym` : measured outputs over ``N_k``, ``\mathbf{Y^m}`` - `:U` : manipulated inputs over ``N_k``, ``\mathbf{U}`` - `:D` : measured disturbances over ``N_k``, ``\mathbf{D}`` @@ -166,6 +169,17 @@ function getinfo(estim::MovingHorizonEstimator{NT}) where NT<:Real info[:U] = U info[:D] = D info[:sol] = JuMP.solution_summary(estim.optim, verbose=true) + # --- non-Unicode fields --- + info[:What] = info[:Ŵ] + info[:xhatarr] = info[:x̂arr] + info[:epsilon] = info[:ϵ] + info[:Xhat] = info[:X̂] + info[:xhat] = info[:x̂] + info[:Vhat] = info[:V̂] + info[:Pbar] = info[:P̄] + info[:xbar] = info[:x̄] + info[:Yhat] = info[:Ŷ] + info[:Yhatm] = info[:Ŷm] return info end diff --git a/src/plot_sim.jl b/src/plot_sim.jl index 42c4aa04..12d83338 100644 --- a/src/plot_sim.jl +++ b/src/plot_sim.jl @@ -154,6 +154,9 @@ vectors. The simulated sensor and process noises of `plant` are specified by `y_ `x_noise` arguments, respectively. # Arguments +!!! info + Keyword arguments in *`italic`* are non-Unicode alternatives. + - `estim::StateEstimator` : state estimator to simulate - `N::Int` : simulation length in time steps - `u = estim.model.uop .+ 1` : manipulated input ``\mathbf{u}`` value @@ -167,7 +170,8 @@ vectors. The simulated sensor and process noises of `plant` are specified by `y_ - `d_noise = zeros(plant.nd)` : additive gaussian noise on measured dist. ``\mathbf{d}`` - `x_noise = zeros(plant.nx)` : additive gaussian noise on plant states ``\mathbf{x}`` - `x_0 = plant.xop` : plant initial state ``\mathbf{x}(0)`` -- `x̂_0 = nothing` : initial estimate ``\mathbf{x̂}(0)``, [`initstate!`](@ref) is used if `nothing` +- `x̂_0 = nothing` or *`xhat_0`* : initial estimate ``\mathbf{x̂}(0)``, [`initstate!`](@ref) + is used if `nothing` - `lastu = plant.uop` : last plant input ``\mathbf{u}`` for ``\mathbf{x̂}`` initialization # Examples @@ -248,9 +252,10 @@ function sim_closedloop!( d_step ::Vector = zeros(NT, plant.nd), d_noise::Vector = zeros(NT, plant.nd), x_noise::Vector = zeros(NT, plant.nx), - x_0 = plant.xop, - x̂_0 = nothing, - lastu = plant.uop, + x_0 = plant.xop, + xhat_0 = nothing, + lastu = plant.uop, + x̂_0 = xhat_0 ) where {NT<:Real} model = estim.model model.Ts ≈ plant.Ts || error("Sampling time of controller/estimator ≠ plant.Ts") diff --git a/test/test_state_estim.jl b/test/test_state_estim.jl index 3e6f272d..6e79f8ea 100644 --- a/test/test_state_estim.jl +++ b/test/test_state_estim.jl @@ -255,9 +255,9 @@ end @test_throws ErrorException Luenberger(linmodel1, nint_ym=[1,1,1]) @test_throws ErrorException Luenberger(linmodel1, nint_ym=[-1,0]) - @test_throws ErrorException Luenberger(linmodel1, p̂=[0.5]) - @test_throws ErrorException Luenberger(linmodel1, p̂=fill(1.5, lo1.nx̂)) - @test_throws ErrorException Luenberger(LinModel(tf(1,[1, 0]),0.1), p̂=[0.5,0.6]) + @test_throws ErrorException Luenberger(linmodel1, poles=[0.5]) + @test_throws ErrorException Luenberger(linmodel1, poles=fill(1.5, lo1.nx̂)) + @test_throws ErrorException Luenberger(LinModel(tf(1,[1, 0]),0.1), poles=[0.5,0.6]) end @testset "Luenberger estimator methods" begin