added: bumpless transfer for all controller based on LinModel

It supports all `StateEstimator`s including `InternalModel` (new feature).

Author: franckgaga

README.md

@@ -118,6 +118,7 @@ for more detailed examples.
 - [x] easily estimate unmeasured disturbances by adding one or more integrators at the:
   - [x] manipulated inputs
   - [x] measured outputs
+- [x] bumpless manual to automatic transfer for control with a proper intial estimate
 - [x] observers in predictor form to ease control applications
 - [ ] moving horizon estimator that supports:
   - [ ] inequality state constraints

src/estimator/internal_model.jl

@@ -208,16 +208,36 @@ function update_estimate!(estim::InternalModel, u, ym, d=empty(estim.x̂))
     ŷd = h(model, x̂d, d)
     x̂d[:] = f(model, x̂d, u, d) # this also updates estim.x̂ (they are the same object)
     # --------------- stochastic model -----------------------
-    ŷs = zeros(model.ny,1)
+    ŷs = zeros(model.ny)
     ŷs[estim.i_ym] = ym - ŷd[estim.i_ym]   # ŷs=0 for unmeasured outputs
     x̂s[:] = estim.Âs*x̂s + estim.B̂s*ŷs
     return x̂d
 end
 
-"Init the stochastic states `estim.x̂s` of [`InternalModel`](@ref) at 0. " 
-function initstate_post!(estim::InternalModel)
-    # TODO: best method to initialize internal model stochastic states ? not sure...
-    estim.x̂s[:] = zeros(estim.nxs)
+@doc raw"""
+    init_estimate!(estim::InternalModel, ::LinModel, u, ym, d)
+
+Init `estim.x̂` \ `x̂d` \ `x̂s` estimate at steady-state for [`InternalModel`](@ref)s.
+
+The deterministic estimates `estim.x̂d` start at steady-state using `u` and `d` arguments:
+```math
+\mathbf{x̂_d} = \mathbf{(I-Â)^{-1} B̂_u u}
+```
+Based on `ym` argument and current stochastic outputs estimation ``\mathbf{ŷ_s}``, composed
+of the measured ``\mathbf{ŷ_s^m} = \mathbf{y^m} - \mathbf{ŷ_d^m}`` and unmeasured 
+``\mathbf{ŷ_s^u = 0}`` outputs, the stochastic estimates also start at steady-state:
+```math
+    \mathbf{x̂_s} = \mathbf{(I - Â_s)^{-1} B̂_s ŷ_s}
+```
+See [`init_internalmodel`](@ref) for details.
+"""
+function init_estimate!(estim::InternalModel, ::LinModel, u, ym, d)
+    x̂d, x̂s = estim.x̂d, estim.x̂s
+    x̂d[:] = (I - estim.Â)\(estim.B̂u*u + estim.B̂d*d)
+    ŷd = h(estim.model, x̂d, d)
+    ŷs = zeros(estim.model.ny)
+    ŷs[estim.i_ym] = ym - ŷd[estim.i_ym]  # ŷs=0 for unmeasured outputs
+    x̂s[:] = (I-estim.Âs)\estim.B̂s*ŷs
     return nothing
 end
 
@@ -232,5 +252,3 @@ function print_estim_dim(io::IO, estim::InternalModel, n)
     print(io,   "$(lpad(nd, n)) measured disturbances d")
 end
 
-(estim::InternalModel)(ym, d=empty(estim.x̂)) = evaloutput(estim::InternalModel, ym, d)
-

src/estimator/kalman.jl

@@ -724,11 +724,11 @@ function update_estimate!(estim::ExtendedKalmanFilter, u, ym, d=empty(estim.x̂)
     return update_estimate_kf!(estim, F̂, Ĥm, u, ym, d)
 end
 
-"Initialize the covariance estimate `P̂` for the time-varying Kalman Filters" 
-function initstate_post!(
-    estim::Union{KalmanFilter, UnscentedKalmanFilter, ExtendedKalmanFilter}
+"Set `estim.P̂` to `estim.P̂0` for the time-varying Kalman Filters."
+function init_estimate_cov!(
+    estim::Union{KalmanFilter, UnscentedKalmanFilter, ExtendedKalmanFilter}, _ , _ , _
 ) 
-    estim.P̂.data[:] = estim.P̂0
+    estim.P̂.data[:] = estim.P̂0 # .data is necessary for Hermitians
     return nothing
 end
 

src/state_estim.jl

@@ -54,19 +54,19 @@ function print_estim_dim(io::IO, estim::StateEstimator, n)
 end
 
 """
-    remove_op!(estim::StateEstimator, u, d, ym) -> u0, d0, ym0
+    remove_op!(estim::StateEstimator, u, ym, d) -> u0, ym0, d0
 
 Remove operating points on inputs `u`, measured outputs `ym` and disturbances `d`.
 
 Also store current inputs without operating points `u0` in `estim.lastu0`. This field is 
 used for [`PredictiveController`](@ref) computations.
 """
-function remove_op!(estim::StateEstimator, u, d, ym)
+function remove_op!(estim::StateEstimator, u, ym, d)
     u0  = u  - estim.model.uop
-    d0  = d  - estim.model.dop
     ym0 = ym - estim.model.yop[estim.i_ym]
+    d0  = d  - estim.model.dop
     estim.lastu0[:] = u0
-    return u0, d0, ym0
+    return u0, ym0, d0 
 end
 
 @doc raw"""
@@ -302,7 +302,7 @@ removes the operating points with [`remove_op!`](@ref) and call [`init_estimate!
   bumpless manual to automatic transfer. See [`init_estimate!`](@ref) for details.
 - Else, `estim.x̂` is left unchanged. Use [`setstate!`](@ref) to manually modify it.
 
-If applicable, it also sets the error covariance `estim.P̂` to ``\mathbf{P̂}(0)``.
+If applicable, it also sets the error covariance `estim.P̂` to `estim.P̂0`.
 
 # Examples
 ```jldoctest
@@ -324,15 +324,15 @@ true
 """
 function initstate!(estim::StateEstimator, u, ym, d=empty(estim.x̂))
     # --- init state estimate ----
-    u0, d0, ym0 = remove_op!(estim, u, d, ym)
+    u0, ym0, d0 = remove_op!(estim, u, ym, d)
     init_estimate!(estim, estim.model, u0, ym0, d0)
     # --- init covariance error estimate, if applicable ---
-    initstate_post!(estim)
+    init_estimate_cov!(estim, u0, ym0, d0)
     return estim.x̂
 end
 
-"By default, do nothing at the end of `initstate!` (used to init the covariance estimate)."
-initstate_post!(::StateEstimator) = nothing
+"By default, [`StateEstimator`](@ref)s do not need covariance error estimate."
+init_estimate_cov!(::StateEstimator, _ , _ , _ ) = nothing
 
 @doc raw"""
     init_estimate!(estim::StateEstimator, model::LinModel, u, ym, d)
@@ -347,7 +347,7 @@ constraint ``\mathbf{ŷ^m} = \mathbf{y^m}`` engenders the following system to s
     \mathbf{Ĉ^m}
 \end{bmatrix} \mathbf{x̂} =
 \begin{bmatrix}
-    \mathbf{B̂_u u} + \mathbf{B̂_d d} \\
+    \mathbf{B̂_u u} + \mathbf{B̂_d d}     \\
     \mathbf{y^m} - \mathbf{D̂_d^m d}
 \end{bmatrix}
 ```
@@ -358,8 +358,13 @@ function init_estimate!(estim::StateEstimator, ::LinModel, u, ym, d)
     Â, B̂u, Ĉ, B̂d, D̂d = estim.Â, estim.B̂u, estim.Ĉ, estim.B̂d, estim.D̂d
     Ĉm, D̂dm = Ĉ[estim.i_ym, :], D̂d[estim.i_ym, :] # measured outputs ym only
     estim.x̂[:] = [(I - Â); Ĉm]\[B̂u*u + B̂d*d; ym - D̂dm*d]
+    return nothing
 end
-"Left `estim.x̂` estimate unchanged if `model` is not a [`LinModel`](@ref)."
+"""
+    init_estimate!(::StateEstimator, ::SimModel, _ , _ , _ )
+
+Left `estim.x̂` estimate unchanged if `model` is not a [`LinModel`](@ref).
+"""
 init_estimate!(::StateEstimator, ::SimModel, _ , _ , _ ) = nothing
 
 @doc raw"""
@@ -405,7 +410,7 @@ julia> x̂ = updatestate!(kf, [1], [0]) # x̂[2] is the integrator state (nint_y
 ```
 """
 function updatestate!(estim::StateEstimator, u, ym, d=empty(estim.x̂))
-    u0, d0, ym0 = remove_op!(estim, u, d, ym) 
+    u0, ym0, d0 = remove_op!(estim, u, ym, d) 
     update_estimate!(estim, u0, ym0, d0)
     return estim.x̂
 end

test/test_sim_model.jl

@@ -80,16 +80,18 @@ end
 
 @testset "LinModel sim methods" begin
     linmodel1 = setop!(LinModel(Gss), uop=[10,50], yop=[50,30])
-
-    @test updatestate!(linmodel1, [10, 50]) ≈ zeros(2) 
+    @test updatestate!(linmodel1, [10, 50]) ≈ zeros(2)
     @test updatestate!(linmodel1, [10, 50], Float64[]) ≈ zeros(2)
     @test linmodel1.x ≈ zeros(2)
-    @test evaloutput(linmodel1) ≈ linmodel1() ≈ [50,30] 
+    @test evaloutput(linmodel1) ≈ linmodel1() ≈ [50,30]
     @test evaloutput(linmodel1, Float64[]) ≈ linmodel1(Float64[]) ≈ [50,30]
-
     x = initstate!(linmodel1, [10, 60])
     @test evaloutput(linmodel1) ≈ [50 + 19.0, 30 + 7.4]
     @test updatestate!(linmodel1, [10, 60]) ≈ x
+    linmodel2 = LinModel(append(tf(1, [1, 0]), tf(2, [10, 1])), 1.0)
+    x = initstate!(linmodel2, [10, 3])
+    @test evaloutput(linmodel2) ≈ [0, 6]
+    @test updatestate!(linmodel2, [0, 3]) ≈ x
 
     @test_throws DimensionMismatch updatestate!(linmodel1, zeros(2), zeros(1))
     @test_throws DimensionMismatch evaloutput(linmodel1, zeros(1))

test/test_state_estim.jl

@@ -64,6 +64,11 @@ end
     @test evaloutput(skalmanfilter1) ≈ skalmanfilter1() ≈ [50, 30]
     @test evaloutput(skalmanfilter1, Float64[]) ≈ skalmanfilter1(Float64[]) ≈ [50, 30]
     @test initstate!(skalmanfilter1, [10, 50], [50, 30+1]) ≈ [zeros(3); [1]]
+    linmodel2 = LinModel(append(tf(1, [1, 0]), tf(2, [10, 1])), 1.0)
+    skalmanfilter2 = SteadyKalmanFilter(linmodel2, nint_u=[1, 1])
+    x = initstate!(skalmanfilter2, [10, 3], [0.5, 6+0.1])
+    @test evaloutput(skalmanfilter2) ≈ [0.5, 6+0.1]
+    @test updatestate!(skalmanfilter2, [0, 3], [0.5, 6+0.1]) ≈ x
     setstate!(skalmanfilter1, [1,2,3,4])
     @test skalmanfilter1.x̂ ≈ [1,2,3,4]
     for i in 1:100
@@ -299,6 +304,13 @@ end
     @test internalmodel1.x̂s ≈ ones(2)
     @test ModelPredictiveControl.evalŷ(internalmodel1, [51,31], Float64[]) ≈ [51,31]
     @test initstate!(internalmodel1, [10, 50], [50, 30]) ≈ zeros(2)
+    linmodel2 = LinModel(append(tf(3, [5, 1]), tf(2, [10, 1])), 1.0)
+    stoch_ym = append(tf([2.5, 1],[1.2, 1, 0]),tf([1.5, 1], [1.3, 1, 0]))
+    estim = InternalModel(linmodel2; stoch_ym)
+    initstate!(estim, [1, 2], [3+0.1, 4+0.5])
+    @test estim.x̂d ≈ estim.Â*estim.x̂d + estim.B̂u*[1, 2]
+    ŷs = [3+0.1, 4+0.5] - estim()
+    @test estim.x̂s ≈ estim.Âs*estim.x̂s + estim.B̂s*ŷs
     @test internalmodel1.x̂s ≈ zeros(2)
     setstate!(internalmodel1, [1,2])
     @test internalmodel1.x̂ ≈ [1,2]