added : automatic model augmentation based on observability

Author: franckgaga

docs/src/internals/state_estim.md

@@ -5,6 +5,7 @@
 ```@docs
 ModelPredictiveControl.init_estimstoch
 ModelPredictiveControl.augment_model
+ModelPredictiveControl.default_nint
 ModelPredictiveControl.init_ukf
 ModelPredictiveControl.init_internalmodel
 ```

docs/src/manual/nonlinmpc.md

@@ -59,7 +59,7 @@ plot(sim!(model, 60, u), plotu=false)
 An [`UnscentedKalmanFilter`](@ref) estimates the plant state :
 
 ```@example 1
-estim = UnscentedKalmanFilter(model, σQ=[0.1, 0.5], σQ_int=[5.0], σR=[0.5])
+estim = UnscentedKalmanFilter(model, σQ=[0.1, 0.5], σR=[0.5], nint_ym=[1], σQ_int=[5.0])
 ```
 
 The standard deviation of the angular velocity ``ω`` is higher here (`σQ` second value)

example/juMPC.jl

@@ -59,6 +59,7 @@ mpcluen = LinMPC(luenberger)
 
 
 
+#=
 kalmanFilter1 = KalmanFilter(linModel1)
 kalmanFilter2 = KalmanFilter(linModel1,nint_ym=0)
 
@@ -210,3 +211,4 @@ display(pu)
 display(py)
 
 =#
+=#

src/controller/nonlinmpc.jl

@@ -198,7 +198,7 @@ function NonLinMPC(
 end
 
 """
-    getinfo(mpc::NonLinMPC)
+    getinfo(mpc::NonLinMPC) -> sol_summary, info
 
 Invoke [`getinfo(::PredictiveController)`](@ref) and add `:JE` the economic optimum ``J_E``.
 """

src/estimator/internal_model.jl

@@ -110,7 +110,7 @@ end
 validate_internalmodel(::SimModel) = nothing
 
 @doc raw"""
-    init_internalmodel(As, Bs, Cs, Ds)
+    init_internalmodel(As, Bs, Cs, Ds) -> Âs, B̂s
 
 Calc stochastic model update matrices `Âs` and `B̂s` for `InternalModel` estimator.
 

src/estimator/kalman.jl

@@ -27,11 +27,14 @@ struct SteadyKalmanFilter <: StateEstimator
         nxs = size(Asm,1)
         nx̂ = nx + nxs
         validate_kfcov(nym, nx̂, Q̂, R̂)
-        As, _ , Cs, _  = stoch_ym2y(model, i_ym, Asm, [], Csm, [])
-        Â, B̂u, Ĉ, B̂d, D̂d = augment_model(model, As, Cs)
-        Ĉm, D̂dm = Ĉ[i_ym, :], D̂d[i_ym, :] # measured outputs ym only
+        As, _ , Cs  = stoch_ym2y(model, i_ym, Asm, [], Csm, [])
+        validate_obsv(model, As, Cs)
+        Â , B̂u, Ĉ, B̂d, D̂d = augment_model(model, As, Cs)
         K = try
-            kalman(Discrete, Â, Ĉm, Matrix(Q̂), Matrix(R̂)) # Matrix() required for Julia 1.6
+            Q̂_kalman = Matrix(Q̂) # Matrix() required for Julia 1.6
+            R̂_kalman = zeros(eltype(R̂), ny, ny)
+            R̂_kalman[i_ym, i_ym] = R̂
+            kalman(Discrete, Â, Ĉ, Q̂_kalman, R̂_kalman)[:, i_ym] 
         catch my_error
             if isa(my_error, ErrorException)
                 error("Cannot compute the optimal Kalman gain K for the "* 
@@ -41,6 +44,7 @@ struct SteadyKalmanFilter <: StateEstimator
                 rethrow()
             end
         end
+        Ĉm, D̂dm = Ĉ[i_ym, :], D̂d[i_ym, :] # measured outputs ym only
         i_ym = collect(i_ym)
         lastu0 = zeros(nu)
         x̂ = [zeros(model.nx); zeros(nxs)]
@@ -92,8 +96,8 @@ ones, for ``\mathbf{Ĉ^u, D̂_d^u}``).
     ``\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.
-- `nint_ym=fill(1,length(i_ym))` : integrator quantity per measured outputs (vector) for the 
-    stochastic model, use `nint_ym=0` for no integrator at all.
+- `nint_ym=default_nint(model,i_ym)` : integrator quantity per measured outputs (vector) for
+    the stochastic model, use `nint_ym=0` for no integrator at all (see Extended Help).
 - `σQ_int=fill(1,sum(nint_ym))` : same than `σQ` but for the stochastic model covariance
     ``\mathbf{Q_{int}}`` (composed of output integrators).
 
@@ -111,25 +115,30 @@ SteadyKalmanFilter estimator with a sample time Ts = 0.5 s, LinModel and:
 ```
 
 # Extended Help
-The model augmentation with `nint_ym` vector produces the integral action when the estimator
-is used in a controller as state feedback. The default is 1 integrator per measured outputs,
-resulting in an offset-free tracking for "step-like" unmeasured output disturbances. Use 2 
-integrators for "ramp-like" disturbances. See [`init_estimstoch`](@ref).
+The model augmentation with `nint_ym` vector adds integrators at model measured outputs
+``\mathbf{y^m}``, producing the integral action when the estimator is used in a controller
+as state feedback. The method [`default_nint`](@ref) computes the default value of 
+`nint_ym`. It can also be tweaked by following these rules on each measured output:
+
+- Use 0 integrator if the model output is integrating (else it will be unobservable)
+- Use 1 integrator if the disturbances on the output are typically "step-like"
+- Use 2 integrators if the disturbances on the output are typically "ramp-like" 
+
+The function [`init_estimstoch`](@ref) builds the stochastic model from `nint_ym`.
 
 !!! tip
     Increasing `σQ_int` values increases the integral action "gain".
 
 The constructor pre-compute the steady-state Kalman gain `K` with the [`kalman`](https://juliacontrol.github.io/ControlSystems.jl/stable/lib/synthesis/#ControlSystemsBase.kalman-Tuple{Any,%20Any,%20Any,%20Any,%20Any,%20Vararg{Any}})
-function. It can sometimes fail, for example when `model` is integrating. In such a case,
-you can use 0 integrator on `model` integrating outputs, or the alternative time-varying 
-[`KalmanFilter`](@ref).
+function. It can sometimes fail, for example when `model` matrices are ill-conditioned. In
+such a case, you can try the alternative time-varying [`KalmanFilter`](@ref).
 """
 function SteadyKalmanFilter(
     model::LinModel;
     i_ym::IntRangeOrVector = 1:model.ny,
     σQ::Vector = fill(1/model.nx, model.nx),
     σR::Vector = fill(1, length(i_ym)),
-    nint_ym::IntVectorOrInt = fill(1, length(i_ym)),
+    nint_ym::IntVectorOrInt = default_nint(model, i_ym),
     σQ_int::Vector = fill(1, max(sum(nint_ym), 0))
 )
     # estimated covariances matrices (variance = σ²) :
@@ -196,8 +205,9 @@ struct KalmanFilter <: StateEstimator
         nxs = size(Asm,1)
         nx̂ = nx + nxs
         validate_kfcov(nym, nx̂, Q̂, R̂, P̂0)
-        As, _ , Cs, _  = stoch_ym2y(model, i_ym, Asm, [], Csm, [])
-        Â, B̂u, Ĉ, B̂d, D̂d = augment_model(model, As, Cs)
+        As, _ , Cs = stoch_ym2y(model, i_ym, Asm, [], Csm, [])
+        Â , B̂u, Ĉ, B̂d, D̂d = augment_model(model, As, Cs)
+        validate_obsv(model, As, Cs)
         Ĉm, D̂dm = Ĉ[i_ym, :], D̂d[i_ym, :] # measured outputs ym only
         i_ym = collect(i_ym)
         lastu0 = zeros(nu)
@@ -255,7 +265,7 @@ function KalmanFilter(
     σP0::Vector = fill(1/model.nx, model.nx),
     σQ::Vector  = fill(1/model.nx, model.nx),
     σR::Vector  = fill(1, length(i_ym)),
-    nint_ym::IntVectorOrInt = fill(1, length(i_ym)),
+    nint_ym::IntVectorOrInt = default_nint(model, i_ym),
     σP0_int::Vector = fill(1, max(sum(nint_ym), 0)),
     σQ_int::Vector  = fill(1, max(sum(nint_ym), 0))
 )
@@ -333,7 +343,8 @@ struct UnscentedKalmanFilter{M<:SimModel} <: StateEstimator
         nxs = size(Asm,1)
         nx̂ = nx + nxs
         validate_kfcov(nym, nx̂, Q̂, R̂, P̂0)
-        As, _ , Cs, _  = stoch_ym2y(model, i_ym, Asm, [], Csm, [])
+        As, _ , Cs = stoch_ym2y(model, i_ym, Asm, [], Csm, [])
+        validate_obsv(model, As, Cs)
         nσ, γ, m̂, Ŝ = init_ukf(nx̂, α, β, κ)
         i_ym = collect(i_ym)
         lastu0 = zeros(nu)
@@ -402,7 +413,7 @@ function UnscentedKalmanFilter(
     σP0::Vector = fill(1/model.nx, model.nx),
     σQ::Vector  = fill(1/model.nx, model.nx),
     σR::Vector  = fill(1, length(i_ym)),
-    nint_ym::IntVectorOrInt = fill(1, length(i_ym)),
+    nint_ym::IntVectorOrInt = default_nint(model, i_ym),
     σP0_int::Vector = fill(1, max(sum(nint_ym), 0)),
     σQ_int::Vector  = fill(1, max(sum(nint_ym), 0)),
     α::Real = 1e-3,
@@ -427,7 +438,7 @@ UnscentedKalmanFilter{M}(model::M, i_ym, nint_ym, P̂0, Q̂, R̂, α, β, κ) wh
 
 
 @doc raw"""
-    init_ukf(nx̂, α, β, κ)
+    init_ukf(nx̂, α, β, κ) -> nσ, γ, m̂, Ŝ
 
 Compute the [`UnscentedKalmanFilter`](@ref) constants from ``α, β`` and ``κ``.
 
@@ -553,6 +564,7 @@ struct ExtendedKalmanFilter{M<:SimModel} <: StateEstimator
         nx̂ = nx + nxs
         validate_kfcov(nym, nx̂, Q̂, R̂, P̂0)
         As, _ , Cs, _  = stoch_ym2y(model, i_ym, Asm, [], Csm, [])
+        validate_obsv(model, As, Cs)
         i_ym = collect(i_ym)
         lastu0 = zeros(nu)
         x̂ = [zeros(model.nx); zeros(nxs)]
@@ -614,7 +626,7 @@ function ExtendedKalmanFilter(
     σP0::Vector = fill(1/model.nx, model.nx),
     σQ::Vector  = fill(1/model.nx, model.nx),
     σR::Vector  = fill(1, length(i_ym)),
-    nint_ym::IntVectorOrInt = fill(1, length(i_ym)),
+    nint_ym::IntVectorOrInt = default_nint(model, i_ym),
     σP0_int::Vector = fill(1, max(sum(nint_ym), 0)),
     σQ_int::Vector  = fill(1, max(sum(nint_ym), 0))
 ) where {M<:SimModel}
@@ -695,7 +707,7 @@ function validate_kfcov(nym, nx̂, Q̂, R̂, P̂0=nothing)
 end
 
 """
-    update_estimate_kf!(estim, Â, Ĉm, u, ym, d)
+    update_estimate_kf!(estim, Â, Ĉm, u, ym, d) -> x̂, P̂
 
 Update time-varying/extended Kalman Filter estimates with augmented `Â` and `Ĉm` matrices.
 

src/estimator/luenberger.jl

@@ -24,14 +24,15 @@ struct Luenberger <: StateEstimator
         Asm, Csm, nint_ym = init_estimstoch(i_ym, nint_ym)
         nxs = size(Asm,1)
         nx̂ = nx + nxs
-        As, _ , Cs, _  = stoch_ym2y(model, i_ym, Asm, [], Csm, [])
-        Â, B̂u, Ĉ, B̂d, D̂d = augment_model(model, As, Cs)
-        Ĉm, D̂dm = Ĉ[i_ym, :], D̂d[i_ym, :] # measured outputs ym only
+        As, _ , Cs = stoch_ym2y(model, i_ym, Asm, [], Csm, [])
+        Â , B̂u, Ĉ, B̂d, D̂d = augment_model(model, As, Cs)
+        validate_obsv(model, As, Cs)
         K = try
-            place(Â, Ĉ, p̂, :o)
+            place(Â, Ĉ, p̂, :o)[:, i_ym]
         catch
-            error("Cannot compute the Luenberger gain L with specified poles p̂.")        
+            error("Cannot compute the Luenberger gain L with specified poles p̂.")
         end
+        Ĉm, D̂dm = Ĉ[i_ym, :], D̂d[i_ym, :] # measured outputs ym only
         i_ym = collect(i_ym)
         lastu0 = zeros(nu)
         x̂ = [zeros(model.nx); zeros(nxs)]
@@ -51,7 +52,7 @@ end
     Luenberger(
         model::LinModel; 
         i_ym = 1:model.ny, 
-        nint_ym = fill(1, length(i_ym)),
+        nint_ym = default_nint(model, i_ym),
         p̂ = 1e-3*(0:(model.nx + sum(nint_ym)-1)) .+ 0.5)
     )
 
@@ -80,7 +81,7 @@ Luenberger estimator with a sample time Ts = 0.5 s, LinModel and:
 function Luenberger(
     model::LinModel;
     i_ym::IntRangeOrVector  = 1:model.ny,
-    nint_ym::IntVectorOrInt = fill(1, length(i_ym)),
+    nint_ym::IntVectorOrInt = default_nint(model, i_ym),
     p̂ = 1e-3*(0:(model.nx + sum(nint_ym)-1)) .+ 0.5
 )
     nx = model.nx
@@ -101,5 +102,5 @@ function update_estimate!(estim::Luenberger, u, ym, d=Float64[])
     Â, B̂u, B̂d, Ĉm, D̂dm = estim.Â, estim.B̂u, estim.B̂d, estim.Ĉm, estim.D̂dm
     x̂, K = estim.x̂, estim.K
     x̂[:] = Â*x̂ + B̂u*u + B̂d*d + K*(ym - Ĉm*x̂ - D̂dm*d)
-    return x̂    
+    return x̂
 end

src/predictive_control.jl

@@ -270,7 +270,7 @@ function moveinput!(
 end
 
 @doc raw"""
-    getinfo(mpc::PredictiveController)
+    getinfo(mpc::PredictiveController) -> sol_summary, info
 
 Get additional information about `mpc` controller optimum to ease troubleshooting.
 
@@ -355,7 +355,7 @@ function validate_setpointdist(mpc::PredictiveController, ry, d, R̂y, D̂)
 end
 
 """
-    getestimates!(mpc::PredictiveController, estim::StateEstimator)
+    getestimates!(mpc::PredictiveController, estim::StateEstimator) -> x̂d, x̂s, ŷ
 
 Get estimator output and split `x̂` into the deterministic `x̂d` and stochastic `x̂s` states.
 """
@@ -368,7 +368,7 @@ function getestimates!(mpc::PredictiveController, estim::StateEstimator, ym , d)
 end
 
 """
-    getestimates!(mpc::PredictiveController, estim::InternalModel)
+    getestimates!(mpc::PredictiveController, estim::InternalModel) -> x̂d, x̂s, ŷ
 
 Get the internal model deterministic `estim.x̂d` and stochastic `estim.x̂s` states.
 """
@@ -513,7 +513,7 @@ end
 """
     optim_objective!(mpc::PredictiveController, b, p)
 
-Optimize the objective function ``J`` of `mpc` controller. 
+Optimize the objective function ``J`` of `mpc` controller and return the solution `ΔŨ`.
 """
 function optim_objective!(mpc::PredictiveController)
     optim = mpc.optim
@@ -550,7 +550,7 @@ end
 set_objective_linear_coef!(::PredictiveController, _ ) = nothing
 
 @doc raw"""
-    init_ΔUtoU(nu, Hp, Hc, C, c_Umin, c_Umax)
+    init_ΔUtoU(nu, Hp, Hc, C, c_Umin, c_Umax) -> S_Hp, T_Hp, S_Hc, T_Hc
 
 Init manipulated input increments to inputs conversion matrices.
 
@@ -574,7 +574,7 @@ end
 
 
 @doc raw"""
-    init_deterpred(model::LinModel, Hp, Hc)
+    init_deterpred(model::LinModel, Hp, Hc) -> E, G, J, Kd, Q
 
 Construct deterministic prediction matrices for [`LinModel`](@ref) `model`.
 
@@ -699,7 +699,7 @@ function init_deterpred(model::SimModel, Hp, Hc)
 end
 
 @doc raw"""
-    init_quadprog(model::LinModel, Ẽ, S_Hp, M_Hp, N_Hc, L_Hp)
+    init_quadprog(model::LinModel, Ẽ, S_Hp, M_Hp, N_Hc, L_Hp) -> P̃, q̃, p
 
 Init the quadratic programming optimization matrix `P̃` and `q̃`.
 
@@ -730,7 +730,7 @@ end
 obj_quadprog(ΔŨ, P̃, q̃) = 0.5*ΔŨ'*P̃*ΔŨ + q̃'*ΔŨ
 
 """
-    init_defaultcon(model, C, S_Hp, S_Hc, N_Hc, E)
+    init_defaultcon(model, C, S_Hp, S_Hc, N_Hc, E) -> con, S̃_Hp, Ñ_Hc, Ẽ
 
 Init `ControllerConstraint` struct with default parameters.
 
@@ -779,7 +779,7 @@ function repeat_constraints(Hp, Hc, umin, umax, Δumin, Δumax, ŷmin, ŷmax)
 end
 
 @doc raw"""
-    relaxU(C, c_Umin, c_Umax, S_Hp, S_Hc)
+    relaxU(C, c_Umin, c_Umax, S_Hp, S_Hc) -> A_Umin, A_Umax, S̃_Hp
 
 Augment manipulated inputs constraints with slack variable ϵ for softening.
 
@@ -813,7 +813,7 @@ function relaxU(C, c_Umin, c_Umax, S_Hp, S_Hc)
 end
 
 @doc raw"""
-    relaxΔU(C, c_ΔUmin, c_ΔUmax, ΔUmin, ΔUmax, N_Hc)
+    relaxΔU(C, c_ΔUmin, c_ΔUmax, ΔUmin, ΔUmax, N_Hc) -> A_ΔŨmin, A_ΔŨmax, ΔŨmin, ΔŨmax, Ñ_Hc
 
 Augment input increments constraints with slack variable ϵ for softening.
 
@@ -850,7 +850,7 @@ function relaxΔU(C, c_ΔUmin, c_ΔUmax, ΔUmin, ΔUmax, N_Hc)
 end
 
 @doc raw"""
-    relaxŶ(::LinModel, C, c_Ŷmin, c_Ŷmax, E)
+    relaxŶ(::LinModel, C, c_Ŷmin, c_Ŷmax, E) -> A_Ŷmin, A_Ŷmax, Ẽ
 
 Augment linear output prediction constraints with slack variable ϵ for softening.
 
@@ -890,7 +890,7 @@ function relaxŶ(::SimModel, C, c_Ŷmin, c_Ŷmax, E)
 end
 
 @doc raw"""
-    init_stochpred(estim::StateEstimator, Hp)
+    init_stochpred(estim::StateEstimator, Hp) -> Ks, Ps
 
 Init the stochastic prediction matrix `Ks` from `estim` estimator for predictive control.
 
@@ -919,7 +919,7 @@ function init_stochpred(estim::StateEstimator, Hp)
 end
 
 @doc raw"""
-    init_stochpred(estim::InternalModel, Hp)
+    init_stochpred(estim::InternalModel, Hp) -> Ks, Ps
 
 Init the stochastic prediction matrices for [`InternalModel`](@ref).
 
@@ -955,7 +955,7 @@ end
     init_linconstraint(model::LinModel, 
         A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ŷmin, A_Ŷmax,
         i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ŷmin, i_Ŷmax
-    )
+    ) -> A, i_b, b
 
 Init `A`, `b` and `i_b` for the linear inequality constraints (``\mathbf{A ΔŨ ≤ b}``).