Merge pull request #15 from franckgaga/nint_u

Added support for unmeasured disturbances at model manipulated inputs.

Author: franckgaga

Project.toml

@@ -1,7 +1,7 @@
 name = "ModelPredictiveControl"
 uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c"
 authors = ["Francis Gagnon"]
-version = "0.7.1"
+version = "0.8.0"
 
 [deps]
 ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e"

README.md

@@ -93,7 +93,7 @@ for more detailed examples.
 - [x] supported feedback strategy:
   - [x] state estimator (see State Estimation features)
   - [x] internal model structure with a custom stochastic model
-- [x] offset-free tracking with a single or multiple integrators on measured outputs
+- [x] automatic model augmentation with integrating states for offset-free tracking
 - [x] support for unmeasured model outputs
 - [x] feedforward action with measured disturbances that supports direct transmission
 - [x] custom predictions for:
@@ -115,7 +115,9 @@ for more detailed examples.
   - [x] extended Kalman filter
   - [x] unscented Kalman filter
   - [ ] moving horizon estimator
-- [x] automatic model augmentation for offset-free tracking
+- [x] easily estimate unmeasured disturbances by adding one or more integrators at the:
+  - [x] manipulated inputs
+  - [x] measured outputs
 - [x] observers in predictor form to ease control applications
 - [ ] moving horizon estimator that supports:
   - [ ] inequality state constraints

docs/src/internals/state_estim.md

@@ -3,6 +3,7 @@
 ## Estimator Initialization
 
 ```@docs
+ModelPredictiveControl.init_estimstoch
 ModelPredictiveControl.init_integrators
 ModelPredictiveControl.augment_model
 ModelPredictiveControl.init_ukf

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], σR=[0.5], nint_ym=[1], σQ_int=[5.0])
+estim = UnscentedKalmanFilter(model, σQ=[0.1, 0.5], σR=[0.5], nint_ym=[1], σQint_ym=[5.0])
 ```
 
 The standard deviation of the angular velocity ``ω`` is higher here (`σQ` second value)
@@ -75,7 +75,7 @@ plot(res, plotu=false, plotxwithx̂=true)
 ```
 
 The estimate ``x̂_3`` is the integrator state that compensates for static errors (`nint_ym`
-and `σQ_int` parameters of [`UnscentedKalmanFilter`](@ref)). The Kalman filter performance
+and `σQint_ym` parameters of [`UnscentedKalmanFilter`](@ref)). The Kalman filter performance
 seems sufficient for control. As the motor torque is limited to -1.5 to 1.5 N m, we
 incorporate the input constraints in a [`NonLinMPC`](@ref):
 

src/controller/nonlinmpc.jl

@@ -168,7 +168,7 @@ Use custom state estimator `estim` to construct `NonLinMPC`.
 ```jldoctest
 julia> model = NonLinModel((x,u,_)->0.5x+u, (x,_)->2x, 10.0, 1, 1, 1);
 
-julia> estim = UnscentedKalmanFilter(model, σQ_int=[0.05]);
+julia> estim = UnscentedKalmanFilter(model, σQint_ym=[0.05]);
 
 julia> mpc = NonLinMPC(estim, Hp=20, Hc=1, Cwt=1e6)
 NonLinMPC controller with a sample time Ts = 10.0 s, Ipopt optimizer, UnscentedKalmanFilter estimator and:

src/estimator/kalman.jl

@@ -7,8 +7,10 @@ struct Luenberger <: StateEstimator
     nym::Int
     nyu::Int
     nxs::Int
-    As::Matrix{Float64}
-    Cs::Matrix{Float64}
+    As  ::Matrix{Float64}
+    Cs_u::Matrix{Float64}
+    Cs_y::Matrix{Float64}
+    nint_u ::Vector{Int}
     nint_ym::Vector{Int}
     Â   ::Matrix{Float64}
     B̂u  ::Matrix{Float64}
@@ -18,9 +20,11 @@ struct Luenberger <: StateEstimator
     Ĉm  ::Matrix{Float64}
     D̂dm ::Matrix{Float64}
     K::Matrix{Float64}
-    function Luenberger(model, i_ym, nint_ym, p̂)
-        nym, nyu, nxs, nx̂, As, Cs, nint_ym = init_estimstoch(model, i_ym, nint_ym)
-        Â, B̂u, Ĉ, B̂d, D̂d = augment_model(model, As, Cs)
+    function Luenberger(model, i_ym, nint_u, nint_ym, p̂)
+        nym, nyu = validate_ym(model, i_ym)
+        As, Cs_u, Cs_y, nxs, nint_u, nint_ym = init_estimstoch(model, i_ym, nint_u, nint_ym)
+        nx̂ = model.nx + nxs
+        Â, B̂u, Ĉ, B̂d, D̂d = augment_model(model, As, Cs_u, Cs_y)
         K = try
             place(Â, Ĉ, p̂, :o)[:, i_ym]
         catch
@@ -33,7 +37,7 @@ struct Luenberger <: StateEstimator
             model, 
             lastu0, x̂,
             i_ym, nx̂, nym, nyu, nxs, 
-            As, Cs, nint_ym,
+            As, Cs_u, Cs_y, nint_u, nint_ym,
             Â, B̂u, B̂d, Ĉ, D̂d, 
             Ĉm, D̂dm,
             K
@@ -45,6 +49,7 @@ end
     Luenberger(
         model::LinModel; 
         i_ym = 1:model.ny, 
+        nint_u  = 0,
         nint_ym = default_nint(model, i_ym),
         p̂ = 1e-3*(0:(model.nx + sum(nint_ym)-1)) .+ 0.5)
     )
@@ -53,7 +58,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 output integrator `nint_ym` (see [`SteadyKalmanFilter`](@ref)
+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_ym)` elements 
 specifying the observer poles/eigenvalues (near ``z=0.5`` by default). The method computes 
 the observer gain ``\mathbf{K}`` with [`place`](https://juliacontrol.github.io/ControlSystems.jl/stable/lib/synthesis/#ControlSystemsBase.place).
@@ -74,15 +79,16 @@ 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 = default_nint(model, i_ym),
+    nint_u ::IntVectorOrInt = 0,
+    nint_ym::IntVectorOrInt = default_nint(model, i_ym, nint_u),
     p̂ = 1e-3*(0:(model.nx + sum(nint_ym)-1)) .+ 0.5
 )
     nx = model.nx
     if length(p̂) ≠ model.nx + sum(nint_ym)
         error("p̂ length ($(length(p̂))) ≠ nx ($nx) + integrator quantity ($(sum(nint_ym)))")
     end
     any(abs.(p̂) .≥ 1) && error("Observer poles p̂ should be inside the unit circles.")
-    return Luenberger(model, i_ym, nint_ym, p̂)
+    return Luenberger(model, i_ym, nint_u, nint_ym, p̂)
 end
 
 

src/estimator/luenberger.jl

@@ -913,14 +913,14 @@ with [`getestimates!`](@ref). The method also returns an empty ``\mathbf{P_s}``
 it is useless except for [`InternalModel`](@ref) estimators.
 """
 function init_stochpred(estim::StateEstimator, Hp)
-    As, Cs = estim.As, estim.Cs
+    As, Cs_y = estim.As, estim.Cs_y
     nxs = estim.nxs
     Ms = Matrix{Float64}(undef, Hp*nxs, nxs) 
     for i = 1:Hp
         iRow = (1:nxs) .+ nxs*(i-1)
         Ms[iRow, :] = As^i
     end
-    Js = repeatdiag(Cs, Hp)
+    Js = repeatdiag(Cs_y, Hp)
     Ks = Js*Ms
     Ps = zeros(estim.model.ny*Hp, 0)
     return Ks, Ps

src/predictive_control.jl

@@ -70,26 +70,36 @@ function remove_op!(estim::StateEstimator, u, d, ym)
 end
 
 """
-    init_estimstoch(model, i_ym, nint_ym)
+    init_estimstoch(model, i_ym, nint_u, nint_ym) -> As, Cs_u, Cs_y, nxs, nint_u, nint_ym
 
 Init stochastic model matrices from integrator specifications for state estimation.
+
+TBW. 
+
+The function [`init_integrators`](@ref) builds the state-space matrice of the unmeasured
+disturbance models.
 """
-function init_estimstoch(model, i_ym, nint_ym)
-    validate_ym(model, i_ym)
-    nx, ny = model.nx, model.ny
-    nym, nyu = length(i_ym), ny - length(i_ym)
-    Asm, Csm, nint_ym = init_integrators(i_ym, nint_ym)
-    nxs = size(Asm,1)
-    nx̂ = nx + nxs
-    As, _ , Cs  = stoch_ym2y(model, i_ym, Asm, [], Csm, [])
-    return nym, nyu, nxs, nx̂, As, Cs, nint_ym
+function init_estimstoch(model, i_ym, nint_u::IntVectorOrInt, nint_ym::IntVectorOrInt)
+    nu, ny, nym = model.nu, model.ny, length(i_ym)
+    As_u , Cs_u , nint_u  = init_integrators(nint_u , nu , "u")
+    As_ym, Cs_ym, nint_ym = init_integrators(nint_ym, nym, "ym")
+    As_y, _ , Cs_y  = stoch_ym2y(model, i_ym, As_ym, [], Cs_ym, [])
+    nxs_u, nxs_y = size(As_u, 1), size(As_y, 1)
+    # combines input and output stochastic models:
+    As   = [As_u zeros(nxs_u, nxs_y); zeros(nxs_y, nxs_u) As_y]
+    Cs_u = [Cs_u zeros(nu, nxs_y)]
+    Cs_y = [zeros(ny, nxs_u) Cs_y]
+    nxs = nxs_u + nxs_y
+    return As, Cs_u, Cs_y, nxs, nint_u, nint_ym
 end
 
 "Validate the specified measured output indices `i_ym`."
 function validate_ym(model::SimModel, i_ym)
     if length(unique(i_ym)) ≠ length(i_ym) || maximum(i_ym) > model.ny
         error("Measured output indices i_ym should contains valid and unique indices")
     end
+    nym, nyu = length(i_ym), model.ny - length(i_ym)
+    return nym, nyu
 end
 
 "Convert the measured outputs stochastic model `stoch_ym` to all outputs `stoch_y`."
@@ -108,74 +118,49 @@ function stoch_ym2y(model::SimModel, i_ym, Asm, Bsm, Csm, Dsm)
 end
 
 @doc raw"""
-    init_integrators(i_ym, nint_ym::Vector{Int}) -> Asm, Csm, nint_ym
+    init_integrators(nint, nys, varname::String) -> As, Cs, nint
 
-Calc stochastic model matrices from output integrators specifications for state estimation.
+Calc state-space matrices `As, Cs` (stochastic part) from integrator specifications `nint`.
 
-For closed-loop state estimators. `nint_ym` is a vector providing how many integrator should 
-be added for each measured output ``\mathbf{y^m}``. The argument generates the `Asm` and 
-`Csm` matrices:
+This function is used to initialize the stochastic part of the augmented model for the
+design of state estimators. The vector `nint` provides how many integrators (in series) 
+should be incorporated for each stochastic output ``\mathbf{y_s}``:
 ```math
 \begin{aligned}
-\mathbf{x_s}(k+1) &= \mathbf{A_s^m x_s}(k) + \mathbf{B_s^m e}(k) \\
-\mathbf{y_s^m}(k) &= \mathbf{C_s^m x_s}(k)
+\mathbf{x_s}(k+1)   &= \mathbf{A_s x_s}(k) + \mathbf{B_s e}(k) \\
+\mathbf{y_s}(k)     &= \mathbf{C_s x_s}(k)
 \end{aligned}
 ```
-where ``\mathbf{e}(k)`` is a conceptual and unknown zero mean white noise. 
-``\mathbf{B_s^m}`` is not used for closed-loop state estimators thus ignored.
-"""
-function init_integrators(i_ym, nint_ym)
-    if nint_ym == 0 # alias for no output integrator at all
-        nint_ym = fill(0, length(i_ym))
-    end
-    nym = length(i_ym);
-    if length(nint_ym) ≠ nym
-        error("nint_ym size ($(length(nint_ym))) ≠ measured output quantity nym ($nym)")
-    end
-    any(nint_ym .< 0) && error("nint_ym values should be ≥ 0")
-    nxs = sum(nint_ym)
-    Asm, Csm = zeros(nxs, nxs), zeros(nym, nxs)
-    if nxs ≠ 0 # construct stochastic model state-space matrices (integrators) :
-        i_Asm, i_Csm = 1, 1
-        for iym = 1:nym
-            nint = nint_ym[iym]
-            if nint ≠ 0
-                rows_Asm = (i_Asm):(i_Asm + nint - 1)
-                Asm[rows_Asm, rows_Asm] = Bidiagonal(ones(nint), ones(nint-1), :L)
-                Csm[iym, i_Csm+nint-1] = 1
-                i_Asm += nint
-                i_Csm += nint
-            end
-        end
-    end
-    return Asm, Csm, nint_ym
-end
+where ``\mathbf{e}(k)`` is an unknown zero mean white noise. The estimations does not use
+``\mathbf{B_s}``, it is thus ignored. Note that this function is called twice :
 
-function init_integrators_u(nu, nint_u)
-    if nint_u == 0 # alias for no output integrator at all
-        nint_u = fill(0, length(nu))
+1. for the unmodeled disturbances at measured outputs ``\mathbf{y^m}``
+2. for the unmodeled disturbances at manipulated inputs ``\mathbf{u}``
+"""
+function init_integrators(nint::IntVectorOrInt, nys, varname::String)
+    if nint == 0 # alias for no integrator at all
+        nint = fill(0, nys)
     end
-    #nym = length(i_ym);
-    if length(nint_u) ≠ nu
-        error("nint_u size ($(length(nint_u))) ≠ manipulated input quantity nu ($nu)")
+    if length(nint) ≠ nys
+        error("nint_$(varname) size ($(length(nint))) ≠ n$(varname) ($nys)")
     end
-    any(nint_u .< 0) && error("nint_u values should be ≥ 0")
-    nxs = sum(nint_u)
-    Asm, Csm = zeros(nxs, nxs), zeros(nym, nxs)
+    any(nint .< 0) && error("nint_$(varname) values should be ≥ 0")
+    nxs = sum(nint)
+    As, Cs = zeros(nxs, nxs), zeros(nys, nxs)
     if nxs ≠ 0 # construct stochastic model state-space matrices (integrators) :
-        i_Asm, i_Csm = 1, 1
-        for iym = 1:nym
-            nint = nint_u[iym]
-            if nint ≠ 0
-                rows_Asm = (i_Asm):(i_Asm + nint - 1)
-                Asm[rows_Asm, rows_Asm] = Bidiagonal(ones(nint), ones(nint-1), :L)
-                Csm[iym, i_Csm+nint-1] = 1
-                i_Asm += nint
-                i_Csm += nint
+        i_As, i_Cs = 1, 1
+        for i = 1:nys
+            nint_i = nint[i]
+            if nint_i ≠ 0
+                rows_As = (i_As):(i_As + nint_i - 1)
+                As[rows_As, rows_As] = Bidiagonal(ones(nint_i), ones(nint_i-1), :L)
+                Cs[i, i_Cs+nint_i-1] = 1
+                i_As += nint_i
+                i_Cs += nint_i
             end
         end
     end
-    return Asm, Csm, nint_u
+    return As, Cs, nint
 end
 
 @doc raw"""
@@ -195,26 +180,27 @@ returns the augmented matrices `Â`, `B̂u`, `Ĉ`, `B̂d` and `D̂d`:
 ```
 An error is thrown if the augmented model is not observable and `verify_obsv == true`.
 """
-function augment_model(model::LinModel, As, Cs; verify_obsv=true)
+function augment_model(model::LinModel, As, Cs_u, Cs_y; verify_obsv=true)
     nu, nx, nd = model.nu, model.nx, model.nd
     nxs = size(As, 1)
-    Â   = [model.A zeros(nx,nxs); zeros(nxs,nx) As]
-    B̂u  = [model.Bu; zeros(nxs,nu)]
-    Ĉ   = [model.C Cs]
-    B̂d  = [model.Bd; zeros(nxs,nd)]
+    Â   = [model.A model.Bu*Cs_u; zeros(nxs,nx) As]
+    B̂u  = [model.Bu; zeros(nxs, nu)]
+    Ĉ   = [model.C Cs_y]
+    B̂d  = [model.Bd; zeros(nxs, nd)]
     D̂d  = model.Dd
     # observability on Ĉ instead of Ĉm, since it would always return false when nym ≠ ny:
     if verify_obsv && !observability(Â, Ĉ)[:isobservable]
-        error("The augmented model is unobservable. You may try to use 0 "*
-              "integrator on model integrating outputs with nint_ym parameter.")
+        error("The augmented model is unobservable. You may try to use 0 integrator on "*
+              "model integrating outputs with nint_ym parameter. Adding integrators at both "*
+              "inputs (nint_u) and outputs (nint_ym) can also violate observability.")
     end
     return Â, B̂u, Ĉ, B̂d, D̂d
 end
 "No need to augment the model if `model` is not a [`LinModel`](@ref)."
-augment_model(::SimModel, _ , _ ) = nothing
+augment_model(::SimModel, _ , _ , _ ) = nothing
 
 @doc raw"""
-    default_nint(model::LinModel, i_ym=1:model.ny)
+    default_nint(model::LinModel, i_ym=1:model.ny, nint_u)
 
 Get default integrator quantity per measured outputs `nint_ym` for [`LinModel`](@ref).
 
@@ -234,24 +220,27 @@ julia> nint_ym = default_nint(model)
  1
 ```
 """
-function default_nint(model::LinModel, i_ym::IntRangeOrVector = 1:model.ny)
+function default_nint(model::LinModel, i_ym=1:model.ny, nint_u=0)
+    validate_ym(model, i_ym)
     nint_ym = fill(0, length(i_ym))
     for i in eachindex(i_ym)
         nint_ym[i]  = 1
-        Asm, Csm    = init_integrators(i_ym, nint_ym)
-        As , _ , Cs = stoch_ym2y(model, i_ym, Asm, [], Csm, [])
-        Â  , _ , Ĉ  = augment_model(model, As, Cs, verify_obsv=false)
+        As, Cs_u, Cs_y = init_estimstoch(model, i_ym, nint_u, nint_ym)
+        Â, _ , Ĉ = augment_model(model, As, Cs_u, Cs_y, verify_obsv=false)
         # observability on Ĉ instead of Ĉm, since it would always return false when nym ≠ ny
         observability(Â, Ĉ)[:isobservable] || (nint_ym[i] = 0)
     end
     return nint_ym
 end
 """
-    default_nint(model::SimModel, i_ym=1:model.ny)
+    default_nint(model::SimModel, i_ym=1:model.ny, nint_u=0)
 
 One integrator on each measured output by default if `model` is not a  [`LinModel`](@ref).
 """
-default_nint(::SimModel, i_ym::IntRangeOrVector = 1:model.ny) = fill(1, length(i_ym))
+function default_nint(model::SimModel, i_ym=1:model.ny, nint_u=0)
+    validate_ym(model, i_ym)
+    return fill(1, length(i_ym))
+end
 
 @doc raw"""
     f̂(estim::StateEstimator, x̂, u, d)
@@ -269,8 +258,8 @@ function returns the next state of the augmented model, defined as:
 """
 function f̂(estim::StateEstimator, x̂, u, d)
     # `@views` macro avoid copies with matrix slice operator e.g. [a:b]
-    nx = estim.model.nx
-    @views return [f(estim.model, x̂[1:nx], u, d); estim.As*x̂[nx+1:end]]
+    @views x̂d, x̂s = x̂[1:estim.model.nx], x̂[estim.model.nx+1:end]
+    return [f(estim.model, x̂d, u + estim.Cs_u*x̂s, d); estim.As*x̂s]
 end
 
 @doc raw"""
@@ -280,8 +269,8 @@ Output function ``\mathbf{ĥ}`` of the augmented model, see [`f̂`](@ref) for d
 """
 function ĥ(estim::StateEstimator, x̂, d)
     # `@views` macro avoid copies with matrix slice operator e.g. [a:b]
-    nx = estim.model.nx
-    @views return h(estim.model, x̂[1:nx], d) + estim.Cs*x̂[nx+1:end]
+    @views x̂d, x̂s = x̂[1:estim.model.nx], x̂[estim.model.nx+1:end]
+    return h(estim.model, x̂d, d) + estim.Cs_y*x̂s
 end