debug: evaloutput for InternalModel now include current stochastic estimate

removed: `evalŷ` method (useless with new `preparestate!`)
added: warning if `evaloutput` is called before `preparestate!` with current estimators
added: warning if `moveinput!` is called before `preparestate!`with current estimators
tests: calling `preparestate!` where needed to avoid the warnings

Author: franckgaga

docs/src/internals/state_estim.md

@@ -40,12 +40,6 @@ ModelPredictiveControl.linconstraint!(::MovingHorizonEstimator, ::LinModel)
 ModelPredictiveControl.optim_objective!(::MovingHorizonEstimator)
 ```
 
-## Evaluate Estimated Output
-
-```@docs
-ModelPredictiveControl.evalŷ
-```
-
 ## Remove Operating Points
 
 ```@docs

src/controller/execute.jl

@@ -42,7 +42,9 @@ See also [`LinMPC`](@ref), [`ExplicitMPC`](@ref), [`NonLinMPC`](@ref).
 ```jldoctest
 julia> mpc = LinMPC(LinModel(tf(5, [2, 1]), 3), Nwt=[0], Hp=1000, Hc=1);
 
-julia> ry = [5]; u = moveinput!(mpc, ry); round.(u, digits=3)
+julia> preparestate!(mpc, [0]); ry = [5];
+
+julia> u = moveinput!(mpc, ry); round.(u, digits=3)
 1-element Vector{Float64}:
  1.0
 ```
@@ -58,6 +60,9 @@ function moveinput!(
     R̂y = Rhaty,
     R̂u = Rhatu
 )
+    if mpc.estim.direct && !mpc.estim.corrected[]
+        @warn "preparestate! should be called before moveinput! with current estimators"
+    end
     validate_args(mpc, ry, d, D̂, R̂y, R̂u)
     initpred!(mpc, mpc.estim.model, d, D̂, R̂y, R̂u)
     linconstraint!(mpc, mpc.estim.model)
@@ -102,7 +107,7 @@ available for [`NonLinMPC`](@ref).
 ```jldoctest
 julia> mpc = LinMPC(LinModel(tf(5, [2, 1]), 3), Nwt=[0], Hp=1, Hc=1);
 
-julia> u = moveinput!(mpc, [10]);
+julia> preparestate!(mpc, [0]); u = moveinput!(mpc, [10]);
 
 julia> round.(getinfo(mpc)[:Ŷ], digits=3)
 1-element Vector{Float64}:
@@ -184,7 +189,7 @@ They are computed with these equations using in-place operations:
 function initpred!(mpc::PredictiveController, model::LinModel, d, D̂, R̂y, R̂u)
     mul!(mpc.T_lastu0, mpc.T, mpc.estim.lastu0)
     ŷ, F, q̃, r = mpc.ŷ, mpc.F, mpc.q̃, mpc.r
-    ŷ .= evalŷ(mpc.estim, d)
+    ŷ .= evaloutput(mpc.estim, d)
     predictstoch!(mpc, mpc.estim) # init mpc.F with Ŷs for InternalModel
     F .+= mpc.B
     mul!(F, mpc.K, mpc.estim.x̂0, 1, 1) 
@@ -220,7 +225,7 @@ Init `ŷ, F, d0, D̂0, D̂E, R̂y0, R̂u0` vectors when model is not a [`LinMod
 """
 function initpred!(mpc::PredictiveController, model::SimModel, d, D̂, R̂y, R̂u)
     mul!(mpc.T_lastu0, mpc.T, mpc.estim.lastu0)
-    mpc.ŷ .= evalŷ(mpc.estim, d)
+    mpc.ŷ .= evaloutput(mpc.estim, d)
     predictstoch!(mpc, mpc.estim) # init mpc.F with Ŷs for InternalModel
     if model.nd ≠ 0
         mpc.d0 .= d .- model.dop

src/estimator/execute.jl

@@ -100,16 +100,14 @@ If applicable, it also sets the error covariance `estim.P̂` to `estim.P̂_0`.
 
 # Examples
 ```jldoctest
-julia> estim = SteadyKalmanFilter(LinModel(tf(3, [10, 1]), 0.5), nint_ym=[2]);
+julia> estim = SteadyKalmanFilter(LinModel(tf(3, [10, 1]), 0.5), nint_ym=[2], direct=false);
 
 julia> u = [1]; y = [3 - 0.1]; x̂ = round.(initstate!(estim, u, y), digits=3)
 3-element Vector{Float64}:
   5.0
   0.0
  -0.1
 
-julia> preparestate!(estim, y); 
-
 julia> x̂ ≈ updatestate!(estim, u, y)
 true
 
@@ -172,19 +170,25 @@ init_estimate!(::StateEstimator, ::SimModel, _ , _ , _ ) = nothing
 
 Evaluate `StateEstimator` outputs `ŷ` from `estim.x̂0` states and disturbances `d`.
 
-It returns `estim` output at the current time step ``\mathbf{ŷ}(k)``. Calling a
-[`StateEstimator`](@ref) object calls this `evaloutput` method.
+It returns `estim` output at the current time step ``\mathbf{ŷ}(k)``. If `estim.direct` is
+`true`, the method [`preparestate!`](@ref) should be called beforehand to correct the state
+estimate. 
+
+Calling a [`StateEstimator`](@ref) object calls this `evaloutput` method.
 
 # Examples
 ```jldoctest
-julia> kf = SteadyKalmanFilter(setop!(LinModel(tf(2, [10, 1]), 5), yop=[20]));
+julia> kf = SteadyKalmanFilter(setop!(LinModel(tf(2, [10, 1]), 5), yop=[20]), direct=false);
 
 julia> ŷ = evaloutput(kf)
 1-element Vector{Float64}:
  20.0
 ```
 """
 function evaloutput(estim::StateEstimator{NT}, d=estim.buffer.empty) where NT <: Real
+    if estim.direct && !estim.corrected[]
+        @warn "preparestate! should be called before evaloutput with current estimators"
+    end
     validate_args(estim.model, d)
     ŷ0, d0 = estim.buffer.ŷ, estim.buffer.d
     d0 .= d .- estim.model.dop

src/estimator/internal_model.jl

@@ -326,23 +326,16 @@ function init_estimate!(estim::InternalModel, model::LinModel{NT}, y0m, d0, u0)
     return nothing
 end
 
-@doc raw"""
-    evalŷ(estim::InternalModel, d) -> ŷ
-
-Get [`InternalModel`](@ref) estimated output `ŷ`.
-
-[`InternalModel`](@ref) estimator needs current stochastic output ``\mathbf{ŷ_s}(k)`` to 
-estimate its outputs ``\mathbf{ŷ}(k)``. The method [`preparestate!`](@ref) store this value
-inside `estim` object, it should be thus called before `evalŷ`.
-"""
-function evalŷ(estim::InternalModel, d)
+# Compute estimated output with current stochastic estimate `estim.ŷs` for `InternalModel`
+function evaloutput(estim::InternalModel, d)
     if !estim.corrected[]
-        error("InternalModel: preparestate! must be called before evalŷ")
+        @warn "preparestate! should be called before evaloutput with InternalModel"
     end
+    validate_args(estim.model, d)
     ŷ0d, d0 = estim.buffer.ŷ, estim.buffer.d
     d0 .= d .- estim.model.dop
-    h!(ŷ0d, estim.model, estim.x̂d, d0) 
-    ŷ = ŷ0d
+    ĥ!(ŷ0d, estim, estim.model, estim.x̂0, d0)
+    ŷ   = ŷ0d
     ŷ .+= estim.model.yop .+ estim.ŷs
     return ŷ
 end

src/plot_sim.jl

@@ -290,7 +290,7 @@ function sim_closedloop!(
         u  = sim_getu!(est_mpc, u_ry, d, ru)
         ud = u + u_step + u_noise.*randn(plant.nu)
         Y_data[:, i]        .= y
-        Ŷ_data[:, i]        .= evalŷ(estim, d)
+        Ŷ_data[:, i]        .= evaloutput(estim, d)
         U_Ry_data[:, i]     .= u_ry
         U_data[:, i]        .= u
         Ud_data[:, i]       .= ud

src/predictive_control.jl

@@ -9,7 +9,7 @@ Functor allowing callable `PredictiveController` object as an alias for [`movein
 
 # Examples
 ```jldoctest
-julia> mpc = LinMPC(LinModel(tf(5, [2, 1]), 3), Nwt=[0], Hp=1000, Hc=1);
+julia> mpc = LinMPC(LinModel(tf(5, [2, 1]), 3), Nwt=[0], Hp=1000, Hc=1, direct=false);
 
 julia> u = mpc([5]); round.(u, digits=3)
 1-element Vector{Float64}:

src/state_estim.jl

@@ -9,7 +9,7 @@ Functor allowing callable `StateEstimator` object as an alias for [`evaloutput`]
 
 # Examples
 ```jldoctest
-julia> kf = KalmanFilter(setop!(LinModel(tf(3, [10, 1]), 2), yop=[20]));
+julia> kf = KalmanFilter(setop!(LinModel(tf(3, [10, 1]), 2), yop=[20]), direct=false);
 
 julia> ŷ = kf() 
 1-element Vector{Float64}:
@@ -83,12 +83,4 @@ include("estimator/execute.jl")
 include("estimator/kalman.jl")
 include("estimator/luenberger.jl")
 include("estimator/mhe.jl")
-include("estimator/internal_model.jl")
-
-"""
-    evalŷ(estim::StateEstimator, d) -> ŷ
-
-Evaluate [`StateEstimator`](@ref) output `ŷ` from measured disturbance `d` and `estim.x̂0`.
-"""
-evalŷ(estim::StateEstimator, d) = evaloutput(estim, d)
-    
+include("estimator/internal_model.jl")

test/test_predictive_control.jl

@@ -54,6 +54,7 @@ end
     linmodel = setop!(LinModel(tf(5, [2, 1]), 3), yop=[10])
     mpc1 = LinMPC(linmodel, Nwt=[0], Hp=1000, Hc=1)
     r = [15]
+    preparestate!(mpc1, [10])
     u = moveinput!(mpc1, r)
     @test u ≈ [1] atol=1e-2
     u = mpc1(r)
@@ -62,13 +63,16 @@ end
     @test info[:u] ≈ u
     @test info[:Ŷ][end] ≈ r[1] atol=1e-2
     mpc2 = LinMPC(linmodel, Nwt=[0], Cwt=Inf, Hp=1000, Hc=1)
+    preparestate!(mpc2, [10])
     u = moveinput!(mpc2, r)
     @test u ≈ [1] atol=1e-2
     mpc3 = LinMPC(linmodel, Mwt=[0], Nwt=[0], Lwt=[1])
+    preparestate!(mpc3, [10])
     u = moveinput!(mpc3, [0], R̂u=fill(12, mpc3.Hp))
     @test u ≈ [12] atol=1e-2
     model2 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), zeros(1,0), zeros(1,0), 1.0)
     mpc4  = LinMPC(model2)
+    preparestate!(mpc4, [0])
     moveinput!(mpc4, [0]) ≈ [0.0]
 
     @test_throws DimensionMismatch moveinput!(mpc1, [0,0,0])
@@ -127,10 +131,10 @@ end
     setstate!(mpc1, [1,2,3,4])
     @test mpc1.estim.x̂0 ≈ [1,2,3,4]
     setstate!(mpc1, [0,0,0,0])
-    preparestate!(mpc1, mpc1.estim())
-    updatestate!(mpc1, mpc1.estim.model.uop, mpc1.estim())
+    preparestate!(mpc1, [50, 30])
+    updatestate!(mpc1, mpc1.estim.model.uop, [50, 30])
     @test mpc1.estim.x̂0 ≈ [0,0,0,0]
-    preparestate!(mpc1, mpc1.estim())
+    preparestate!(mpc1, [50, 30])
     @test_throws ArgumentError updatestate!(mpc1, [0,0])
 end
 
@@ -187,6 +191,7 @@ end
     @test_throws ArgumentError setconstraint!(mpc, c_ymin=[0,0,0])
     @test_throws ArgumentError setconstraint!(mpc, c_ymax=[0,0,0])
 
+    preparestate!(mpc, mpc.estim.model.yop, mpc.estim.model.dop)
     moveinput!(mpc, [0, 0], [0])
     @test_throws ErrorException setconstraint!(mpc, c_umin=[1, 1], c_umax=[1, 1])
     @test_throws ErrorException setconstraint!(mpc, umin=[-Inf,-Inf], umax=[+Inf,+Inf])
@@ -208,6 +213,7 @@ end
     setconstraint!(mpc, umin=[-3], umax=[3])
     setconstraint!(mpc, Δumin=[-1.5], Δumax=[1.5])
     setconstraint!(mpc, ymin=[-100], ymax=[100])
+    preparestate!(mpc, [0])
     moveinput!(mpc, [-10])
     info = getinfo(mpc)
     @test info[:ΔU][begin] ≈ -1.5 atol=1e-1
@@ -276,6 +282,7 @@ end
     @test mpc.con.Y0min ≈ fill(-54.0 -10, 1000)
     @test mpc.con.Y0max ≈ fill(56.0  -10, 1000)
     r = [15]
+    preparestate!(mpc, [10])
     u = moveinput!(mpc, r)
     @test u ≈ [2] atol=1e-2
     setmodel!(mpc, setop!(LinModel(tf(5, [2, 1]), 3), yop=[20], uop=[11]))
@@ -346,6 +353,7 @@ end
 @testset "ExplicitMPC moves and getinfo" begin
     mpc1 = ExplicitMPC(LinModel(tf(5, [2, 1]), 3), Nwt=[0], Hp=1000, Hc=1)
     r = [5]
+    preparestate!(mpc1, [0])
     u = moveinput!(mpc1, r)
     @test u ≈ [1] atol=1e-2
     u = mpc1(r)
@@ -354,13 +362,16 @@ end
     @test info[:u] ≈ u
     @test info[:Ŷ][end] ≈ r[1] atol=1e-2
     mpc2 = ExplicitMPC(LinModel(tf(5, [2, 1]), 3), Nwt=[0], Hp=1000, Hc=1)
+    preparestate!(mpc2, [0])
     u = moveinput!(mpc2, [5])
     @test u ≈ [1] atol=1e-2
     mpc3 = ExplicitMPC(LinModel(tf(5, [2, 1]), 3), Mwt=[0], Nwt=[0], Lwt=[1])
+    preparestate!(mpc3, [0])
     u = moveinput!(mpc3, [0], R̂u=fill(12, mpc3.Hp))
     @test u ≈ [12] atol=1e-2
     model2 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), zeros(1,0), zeros(1,0), 1.0)
     mpc4  = ExplicitMPC(model2)
+    preparestate!(mpc4, [0])
     moveinput!(mpc4, [0]) ≈ [0.0]
 end
 
@@ -414,10 +425,10 @@ end
     setstate!(mpc1, [1,2,3,4])
     @test mpc1.estim.x̂0 ≈ [1,2,3,4]
     setstate!(mpc1, [0,0,0,0])
-    preparestate!(mpc1, mpc1.estim())
-    updatestate!(mpc1, mpc1.estim.model.uop, mpc1.estim())
+    preparestate!(mpc1, [50, 30])
+    updatestate!(mpc1, mpc1.estim.model.uop, [50, 30])
     @test mpc1.estim.x̂0 ≈ [0,0,0,0]
-    preparestate!(mpc1, mpc1.estim())
+    preparestate!(mpc1, [50, 30])
     @test_throws ArgumentError updatestate!(mpc1, [0,0])
 end
 
@@ -433,6 +444,7 @@ end
     @test mpc.Yop ≈ fill(10.0, 1000)
     @test mpc.Uop ≈ fill(1.0, 1000)
     r = [15]
+    preparestate!(mpc, [10])
     u = moveinput!(mpc, r)
     @test u ≈ [2] atol=1e-2
     setmodel!(mpc, setop!(LinModel(tf(5, [2, 1]), 3), yop=[20], uop=[11]))
@@ -512,6 +524,7 @@ end
     linmodel = setop!(LinModel(tf(5, [2000, 1]), 3000.0), yop=[10])
     nmpc_lin = NonLinMPC(linmodel, Nwt=[0], Hp=1000, Hc=1)
     r = [15]
+    preparestate!(nmpc_lin, [10])
     u = moveinput!(nmpc_lin, r)
     @test u ≈ [1] atol=5e-2
     u = nmpc_lin(r)
@@ -523,16 +536,18 @@ end
     R̂y = fill(r[1], Hp)
     JE = (_ , ŶE, _ ) -> sum((ŶE[2:end] - R̂y).^2)
     nmpc = NonLinMPC(linmodel, Mwt=[0], Nwt=[0], Cwt=Inf, Ewt=1, JE=JE, Hp=Hp, Hc=1)
+    preparestate!(nmpc, [10])
     u = moveinput!(nmpc)
     @test u ≈ [1] atol=5e-2
     # ensure that the current estimated output is updated for correct JE values:
-    @test nmpc.ŷ ≈ ModelPredictiveControl.evalŷ(nmpc.estim, Float64[])
+    @test nmpc.ŷ ≈ evaloutput(nmpc.estim, Float64[])
     linmodel2 = LinModel([tf(5, [2000, 1]) tf(7, [8000,1])], 3000.0, i_d=[2])
     f = (x,u,d) -> linmodel2.A*x + linmodel2.Bu*u + linmodel2.Bd*d
     h = (x,d)   -> linmodel2.C*x + linmodel2.Dd*d
     nonlinmodel = NonLinModel(f, h, 3000.0, 1, 2, 1, 1, solver=nothing)
     nmpc2 = NonLinMPC(nonlinmodel, Nwt=[0], Hp=1000, Hc=1)
     d = [0.1]
+    preparestate!(nmpc2, [0], [0])
     u = moveinput!(nmpc2, 7d, d)
     @test u ≈ [0] atol=5e-2
     u = nmpc2(7d, d)
@@ -541,9 +556,11 @@ end
     @test info[:u] ≈ u
     @test info[:Ŷ][end] ≈ 7d[1] atol=5e-2
     nmpc3 = NonLinMPC(nonlinmodel, Nwt=[0], Cwt=Inf, Hp=1000, Hc=1)
+    preparestate!(nmpc3, [0], [0])
     u = moveinput!(nmpc3, 7d, d)
     @test u ≈ [0] atol=5e-2
     nmpc4 = NonLinMPC(nonlinmodel, Hp=15, Mwt=[0], Nwt=[0], Lwt=[1])
+    preparestate!(nmpc4, [0], [0])
     u = moveinput!(nmpc4, [0], d, R̂u=fill(12, nmpc4.Hp))
     @test u ≈ [12] atol=5e-2
     nmpc5 = setconstraint!(NonLinMPC(nonlinmodel, Hp=15, Cwt=Inf), ymin=[1])
@@ -554,11 +571,13 @@ end
     @test ForwardDiff.gradient(vec->g_Y0min_end(vec...), [20.0, 10.0]) ≈ [-5, -5] atol=1e-3
     linmodel3 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), zeros(1,0), zeros(1,0), 1.0)
     nmpc6  = NonLinMPC(linmodel3, Hp=10)
+    preparestate!(nmpc6, [0])
     @test moveinput!(nmpc6, [0]) ≈ [0.0]
     nonlinmodel2 = NonLinModel{Float32}(f, h, 3000.0, 1, 2, 1, 1, solver=nothing)
     nmpc7  = NonLinMPC(nonlinmodel2, Hp=10)
     y = similar(nonlinmodel2.yop)
     nonlinmodel2.h!(y, Float32[0,0], Float32[0])
+    preparestate!(nmpc7, [0], [0])
     @test moveinput!(nmpc7, [0], [0]) ≈ [0.0]
 end
 
@@ -608,14 +627,16 @@ end
     linmodel = setop!(LinModel(sys,Ts,i_u=[1,2]), uop=[10,50], yop=[50,30])
     f = (x,u,_) -> linmodel.A*x + linmodel.Bu*u
     h = (x,_)   -> linmodel.C*x
-    nonlinmodel = NonLinModel(f, h, Ts, 2, 2, 2, solver=nothing) 
+    nonlinmodel = setop!(
+        NonLinModel(f, h, Ts, 2, 2, 2, solver=nothing), uop=[10,50], yop=[50,30]
+    )
     nmpc1 = NonLinMPC(nonlinmodel, Hp=15)
     @test initstate!(nmpc1, [10, 50], [20, 25]) ≈ zeros(4)
     setstate!(nmpc1, [1,2,3,4])
     @test nmpc1.estim.x̂0 ≈ [1,2,3,4]
     setstate!(nmpc1, [0,0,0,0])
-    preparestate!(nmpc1, nmpc1.estim())
-    updatestate!(nmpc1, nmpc1.estim.model.uop, nmpc1.estim())
+    preparestate!(nmpc1, [50, 30])
+    updatestate!(nmpc1, nmpc1.estim.model.uop, [50, 30])
     @test nmpc1.estim.x̂0 ≈ [0,0,0,0] atol=1e-6
 end
 
@@ -662,6 +683,7 @@ end
     setconstraint!(nmpc_lin, umin=[-3], umax=[3])
     setconstraint!(nmpc_lin, Δumin=[-1.5], Δumax=[1.5])
     setconstraint!(nmpc_lin, ymin=[-100], ymax=[100])
+    preparestate!(nmpc_lin, [0])
     moveinput!(nmpc_lin, [-20])
     info = getinfo(nmpc_lin)
     @test info[:ΔU][begin] ≈ -1.5 atol=1e-2
@@ -699,6 +721,7 @@ end
     setconstraint!(nmpc, umin=[-3], umax=[3])
     setconstraint!(nmpc, Δumin=[-1.5], Δumax=[1.5])
     setconstraint!(nmpc, ymin=[-100], ymax=[100])
+    preparestate!(nmpc, [0])
     moveinput!(nmpc, [-20])
     info = getinfo(nmpc)
     @test info[:ΔU][begin] ≈ -1.5 atol=1e-2
@@ -740,6 +763,7 @@ end
     @test mpc.con.Y0min ≈ fill(-54.0 -10, 1000)
     @test mpc.con.Y0max ≈ fill(56.0  -10, 1000)
     r = [15]
+    preparestate!(mpc, [10])
     u = moveinput!(mpc, r)
     @test u ≈ [2] atol=1e-2
     setmodel!(mpc, setop!(LinModel(tf(5, [2, 1]), 3), yop=[20], uop=[11]))