added unmeasured disturbances at input for all StateEstimator

Author: franckgaga

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

src/estimator/kalman.jl

@@ -20,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
@@ -35,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
@@ -47,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)
     )
@@ -55,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).
@@ -76,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,38 +70,31 @@ 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)
 
 Init stochastic model matrices from integrator specifications for state estimation.
 """
-function init_estimstoch(model, i_ym, nint_ym; nint_u=0)
-    validate_ym(model, i_ym)
-    nu, nx, ny = model.nu, model.nx, model.ny
-    nym, nyu = length(i_ym), ny - length(i_ym)
+function init_estimstoch(model, i_ym, nint_u, nint_ym)
+    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]
+    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]
-
-    #Bs = B*Ks (nx × nxs) 
-    #Cs_u # nu x nxs_u
-    #B # nx x nu
-    #K_res = B*Cs_u # (nx × nu) * (nu × nxs_u) = (nx × nxs_u)
-
     nxs = nxs_u + nxs_y
-    nx̂ = nx + nxs
-    return nym, nyu, nx̂, nxs, As, Cs_u, Cs_y, nint_u, nint_ym
+    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`."
@@ -139,7 +132,7 @@ where ``\mathbf{e}(k)`` is an unknown zero mean white noise. The estimations doe
 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::Vector{Int}, nys, varname::String)
+function init_integrators(nint::IntVectorOrInt, nys, varname::String)
     if nint == 0 # alias for no integrator at all
         nint = fill(0, nys)
     end
@@ -192,16 +185,17 @@ function augment_model(model::LinModel, As, Cs_u, Cs_y; verify_obsv=true)
     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).
 
@@ -221,24 +215,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)

src/state_estim.jl

@@ -45,6 +45,8 @@ sys = [ tf(1.90,[18.0,1])   tf(1.90,[18.0,1])   tf(1.90,[18.0,1]);
     @test_throws ErrorException SteadyKalmanFilter(linmodel3, nint_ym=[1, 0, 0])
     model_unobs = LinModel([1 0;0 1.5], [1;0][:,:], [1 0], zeros(2,0), zeros(1,0),1,1,2,1,0)
     @test_throws ErrorException SteadyKalmanFilter(model_unobs, nint_ym=[1])
+    @test_throws ErrorException SteadyKalmanFilter(LinModel(tf(1, [1,0]), 1), nint_ym=[1])
+    @test_throws ErrorException SteadyKalmanFilter(linmodel1, nint_u=[1,1], nint_ym=[1,1])
 end
 
 @testset "SteadyKalmanFilter estimator methods" begin