added : MHE tests for constraints and getinfo

Author: franckgaga

src/controller/execute.jl

@@ -73,19 +73,19 @@ Get additional info about `mpc` controller optimum for troubleshooting.
 The function should be called after calling [`moveinput!`](@ref). It returns the dictionary
 `info` with the following fields:
 
-- `:ΔU`  : optimal manipulated input increments over `Hc`, ``\mathbf{ΔU}``.
+- `:ΔU`  : optimal manipulated input increments over ``H_c``, ``\mathbf{ΔU}``.
 - `:ϵ`   : optimal slack variable, ``ϵ``.
 - `:J`   : objective value optimum, ``J``.
-- `:U`   : optimal manipulated inputs over `Hp`, ``\mathbf{U}``.
+- `:U`   : optimal manipulated inputs over ``H_p``, ``\mathbf{U}``.
 - `:u`   : current optimal manipulated input, ``\mathbf{u}(k)``.
 - `:d`   : current measured disturbance, ``\mathbf{d}(k)``.
-- `:D̂`   : predicted measured disturbances over `Hp`, ``\mathbf{D̂}``.
+- `:D̂`   : predicted measured disturbances over ``H_p``, ``\mathbf{D̂}``.
 - `:ŷ`   : current estimated output, ``\mathbf{ŷ}(k)``.
-- `:Ŷ`   : optimal predicted outputs over `Hp`, ``\mathbf{Ŷ}``.
+- `:Ŷ`   : optimal predicted outputs over ``H_p``, ``\mathbf{Ŷ}``.
 - `:x̂end`: optimal terminal states, ``\mathbf{x̂}_{k-1}(k+H_p)``.
-- `:Ŷs`  : predicted stochastic output over `Hp` of [`InternalModel`](@ref), ``\mathbf{Ŷ_s}``.
-- `:R̂y`  : predicted output setpoint over `Hp`, ``\mathbf{R̂_y}``.
-- `:R̂u`  : predicted manipulated input setpoint over `Hp`, ``\mathbf{R̂_u}``.
+- `:Ŷs`  : predicted stochastic output over ``H_p`` of [`InternalModel`](@ref), ``\mathbf{Ŷ_s}``.
+- `:R̂y`  : predicted output setpoint over ``H_p``, ``\mathbf{R̂_y}``.
+- `:R̂u`  : predicted manipulated input setpoint over ``H_p``, ``\mathbf{R̂_u}``.
 
 For [`LinMPC`](@ref) and [`NonLinMPC`](@ref), the field `:sol` also contains the optimizer
 solution summary that can be printed. Lastly, the optimal economic cost `:JE` is also

src/estimator/mhe/execute.jl

@@ -91,43 +91,59 @@ Get additional info on `estim` [`MovingHorizonEstimator`](@ref) optimum for trou
 The function should be called after calling [`updatestate!`](@ref). It returns the
 dictionary `info` with the following fields:
 
-- `:Ŵ`   : optimal estimated process noise over `Nk`, ``\mathbf{ΔU}``.
+- `:Ŵ`   : optimal estimated process noise over ``N_k``, ``\mathbf{Ŵ}``.
 - `:x̂arr`: optimal estimated state at arrival, ``\mathbf{x̂}_k(k-N_k+1)``.
 - `:J`   : objective value optimum, ``J``.
-- `:X̂`   : optimal estimated states over `Nk+1`.
+- `:X̂`   : optimal estimated states over ``N_k+1``, ``\mathbf{X̂}``.
 - `:x̂`   : optimal estimated state for the next time step, ``\mathbf{x̂}_k(k+1)``.
-- `:V̂`   : optimal estimated sensor noise over `Nk`, ``\mathbf{V̂}``.
+- `:V̂`   : optimal estimated sensor noise over ``N_k``, ``\mathbf{V̂}``.
 - `:P̄`   : estimation error covariance at arrival, ``\mathbf{P̄}``.
 - `:x̄`   : optimal estimation error at arrival, ``\mathbf{x̄}``.
-- `:Ŷm`  : optimal estimated measured outputs over `Nk`, ``\mathbf{Ŷ^m}``.
-- `:Ym`  : measured outputs over `Nk`, ``\mathbf{Y^m}``.
-- `:U`   : manipulated inputs over `Hp`, ``\mathbf{U}``.
-- `:D`   : measured disturbances over `Nk`, ``\mathbf{D}``.
+- `:Ŷ`   : optimal estimated outputs over ``N_k``, ``\mathbf{Ŷ}``.
+- `:Ŷm`  : optimal estimated measured outputs over ``N_k``, ``\mathbf{Ŷ^m}``.
+- `:Ym`  : measured outputs over ``N_k``, ``\mathbf{Y^m}``.
+- `:U`   : manipulated inputs over ``N_k``, ``\mathbf{U}``.
+- `:D`   : measured disturbances over ``N_k``, ``\mathbf{D}``.
 - `:sol` : solution summary of the optimizer for printing.
 
 # Examples
 ```jldoctest
-julia> a=1;
+julia> estim = MovingHorizonEstimator(LinModel(ss(1.0, 1.0, 1.0, 0, 1)), He=1, nint_ym=0);
+
+julia> updatestate!(estim, [0], [1]);
+
+julia> round.(getinfo(estim)[:Ŷ], digits=3)
+1-element Vector{Float64}:
+ 0.5
 ```
 """
 function getinfo(estim::MovingHorizonEstimator{NT}) where NT<:Real
     model, Nk = estim.model, estim.Nk[]
+    nu, ny, nd = model.nu, model.ny, model.nd
     nx̂, nym, nŵ = estim.nx̂, estim.nym, estim.nx̂
     MyTypes = Union{JuMP._SolutionSummary, Hermitian{NT, Matrix{NT}}, Vector{NT}, NT}
     info = Dict{Symbol, MyTypes}()
     V̂, X̂ = similar(estim.Ym[1:nym*Nk]), similar(estim.X̂[1:nx̂*Nk])
-    V̂, X̂ = predict!(V̂, X̂, estim, estim.model, estim.Z̃)
+    V̂, X̂ = predict!(V̂, X̂, estim, model, estim.Z̃)
     x̂arr = estim.Z̃[1:nx̂]
-    Ym, U, D = estim.Ym[1:nym*Nk], estim.U[1:model.nu*Nk], estim.D[1:model.nd*Nk]
+    X̂ = [x̂arr; X̂]
+    Ym, U, D = estim.Ym[1:nym*Nk], estim.U[1:nu*Nk], estim.D[1:nd*Nk]
+    Ŷ = Vector{NT}(undef, ny*Nk)
+    for i=1:Nk
+        d = @views D[(1 + nd*(i-1)):(nd*i)] # operating point already removed in estim.D
+        x̂ = @views X̂[(1 + nx̂*(i-1)):(nx̂*i)]
+        Ŷ[(1 + ny*(i-1)):(ny*i)] = ĥ(estim, model, x̂, d) + model.yop
+    end
     Ŷm = Ym - V̂
     info[:Ŵ] = estim.Ŵ[1:Nk*nŵ]
     info[:x̂arr] = x̂arr
     info[:J] = obj_nonlinprog(estim, estim.model, V̂, estim.Z̃)
-    info[:X̂] = [x̂arr; X̂]
-    info[:x̂] = X̂[end-estim.nx̂+1:end]
+    info[:X̂] = X̂
+    info[:x̂] = estim.x̂
     info[:V̂] = V̂
     info[:P̄] = estim.P̂arr_old
     info[:x̄] = estim.x̂arr_old - x̂arr
+    info[:Ŷ] = Ŷ
     info[:Ŷm] = Ŷm
     info[:Ym] = Ym
     info[:U] = U

test/test_state_estim.jl

@@ -1,7 +1,7 @@
 Ts = 4.0
 sys = [ tf(1.90,[18.0,1])   tf(1.90,[18.0,1])   tf(1.90,[18.0,1]);
         tf(-0.74,[8.0,1])   tf(0.74,[8.0,1])    tf(-0.74,[8.0,1])   ] 
-#=
+
 @testset "SteadyKalmanFilter construction" begin
     linmodel1 = LinModel(sys,Ts,i_u=[1,2])
     skalmanfilter1 = SteadyKalmanFilter(linmodel1)
@@ -618,16 +618,21 @@ end
     @test_throws ArgumentError MovingHorizonEstimator(linmodel1)
     @test_throws ArgumentError MovingHorizonEstimator(linmodel1, He=0)
 end
-=#
-@testset "MovingHorizonEstimator estimator methods" begin
-    linmodel1 = LinModel(sys,Ts,i_u=[1,2], i_d=[3])
+
+@testset "MovingHorizonEstimator estimation and getinfo" begin
+    linmodel1 = setop!(LinModel(sys,Ts,i_u=[1,2], i_d=[3]), uop=[10,50], yop=[50,30], dop=[5])
     f(x,u,d) = linmodel1.A*x + linmodel1.Bu*u + linmodel1.Bd*d
     h(x,d)   = linmodel1.C*x + linmodel1.Dd*d
     nonlinmodel = setop!(NonLinModel(f, h, Ts, 2, 4, 2, 1), uop=[10,50], yop=[50,30], dop=[5])
     mhe1 = MovingHorizonEstimator(nonlinmodel, He=2)
-    @test updatestate!(mhe1, [10, 50], [50, 30], [5]) ≈ zeros(6) atol=1e-9
+    x̂ = updatestate!(mhe1, [10, 50], [50, 30], [5])
+    @test x̂ ≈ zeros(6) atol=1e-9
     @test mhe1.x̂ ≈ zeros(6) atol=1e-9
     @test evaloutput(mhe1, [5]) ≈ mhe1([5]) ≈ [50, 30]
+    info = getinfo(mhe1)
+    @test info[:x̂] ≈ x̂ atol=1e-9
+    @test info[:Ŷ][end-1:end] ≈ [50, 30] atol=1e-9
+
     @test initstate!(mhe1, [10, 50], [50, 30+1], [5]) ≈ zeros(6) atol=1e-9
     setstate!(mhe1, [1,2,3,4,5,6])
     @test mhe1.x̂ ≈ [1,2,3,4,5,6]
@@ -639,7 +644,15 @@ end
         updatestate!(mhe1, [10, 50], [51, 32], [5])
     end
     @test mhe1([5]) ≈ [51, 32] atol=1e-3
+    
     mhe2 = MovingHorizonEstimator(linmodel1, He=2, nint_u=[1, 1], nint_ym=[0, 0])
+    x̂ = updatestate!(mhe2, [10, 50], [50, 30], [5])
+    @test x̂ ≈ zeros(6) atol=1e-9
+    @test mhe2.x̂ ≈ zeros(6) atol=1e-9
+    @test evaloutput(mhe2, [5]) ≈ mhe2([5]) ≈ [50, 30]
+    info = getinfo(mhe2)
+    @test info[:x̂] ≈ x̂ atol=1e-9
+    @test info[:Ŷ][end-1:end] ≈ [50, 30] atol=1e-9
     for i in 1:100
         updatestate!(mhe2, [11, 52], [50, 30], [5])
     end
@@ -698,41 +711,87 @@ end
 end
 
 @testset "MovingHorizonEstimator constraint violation" begin
-    linmodel1 = LinModel(sys,Ts,i_u=[1,2])
-    f(x,u,_) = linmodel1.A*x + linmodel1.Bu*u
-    h(x,_)   = linmodel1.C*x
-    nonlinmodel = setop!(NonLinModel(f, h, Ts, 2, 2, 2), uop=[10,50], yop=[50,30])
-    mhe = MovingHorizonEstimator(nonlinmodel, He=1, nint_ym=0)
+    linmodel1 = setop!(LinModel(sys,Ts,i_u=[1,2]), uop=[10,50], yop=[50,30])
+    mhe = MovingHorizonEstimator(linmodel1, He=1, nint_ym=0)
 
     setconstraint!(mhe, x̂min=[-100,-100], x̂max=[100,100])
     setconstraint!(mhe, ŵmin=[-100,-100], ŵmax=[100,100])
     setconstraint!(mhe, v̂min=[-100,-100], v̂max=[100,100])
 
     setconstraint!(mhe, x̂min=[1,1], x̂max=[100,100])
     x̂ = updatestate!(mhe, [10, 50], [50, 30])
-    @test x̂ ≈ [1, 1] atol=1e-3
+    @test x̂ ≈ [1, 1] atol=1e-2
 
     setconstraint!(mhe, x̂min=[-100,-100], x̂max=[-1,-1])
     x̂ = updatestate!(mhe, [10, 50], [50, 30])
-    @test x̂ ≈ [-1, -1] atol=1e-3
+    @test x̂ ≈ [-1, -1] atol=1e-2
 
     setconstraint!(mhe, x̂min=[-100,-100], x̂max=[100,100])
     setconstraint!(mhe, ŵmin=[-100,-100], ŵmax=[100,100])
     setconstraint!(mhe, v̂min=[-100,-100], v̂max=[100,100])
 
     setconstraint!(mhe, ŵmin=[1,1], ŵmax=[100,100])
     x̂ = updatestate!(mhe, [10, 50], [50, 30])
-    @test mhe.Ŵ ≈ [1,1] atol=1e-3
+    @test mhe.Ŵ ≈ [1,1] atol=1e-2
 
     setconstraint!(mhe, ŵmin=[-100,-100], ŵmax=[-1,-1])
     x̂ = updatestate!(mhe, [10, 50], [50, 30])
-    @test mhe.Ŵ ≈ [-1,-1] atol=1e-3
+    @test mhe.Ŵ ≈ [-1,-1] atol=1e-2
 
     setconstraint!(mhe, x̂min=[-100,-100], x̂max=[100,100])
     setconstraint!(mhe, ŵmin=[-100,-100], ŵmax=[100,100])
     setconstraint!(mhe, v̂min=[-100,-100], v̂max=[100,100])
 
-    #setconstraint!(mhe, v̂min=[1,1], v̂max=[100,100])
-    #x̂ = updatestate!(mhe, [10, 50], [50, 30])
-    #@test [50,30] - ModelPredictiveControl.ĥ(mhe,mhe.mode,X̂ ≈ [1,1] atol=1e-3
+    setconstraint!(mhe, v̂min=[1,1], v̂max=[100,100])
+    x̂ = updatestate!(mhe, [10, 50], [50, 30])
+    info = getinfo(mhe)
+    @test info[:V̂] ≈ [1,1] atol=1e-2
+
+    setconstraint!(mhe, v̂min=[-100,-100], v̂max=[-1,-1])
+    x̂ = updatestate!(mhe, [10, 50], [50, 30])
+    info = getinfo(mhe)
+    @test info[:V̂] ≈ [-1,-1] atol=1e-2
+
+    f(x,u,_) = linmodel1.A*x + linmodel1.Bu*u
+    h(x,_)   = linmodel1.C*x
+    nonlinmodel = setop!(NonLinModel(f, h, Ts, 2, 2, 2), uop=[10,50], yop=[50,30])
+    mhe2 = MovingHorizonEstimator(nonlinmodel, He=1, nint_ym=0)
+
+    setconstraint!(mhe2, x̂min=[-100,-100], x̂max=[100,100])
+    setconstraint!(mhe2, ŵmin=[-100,-100], ŵmax=[100,100])
+    setconstraint!(mhe2, v̂min=[-100,-100], v̂max=[100,100])
+
+    setconstraint!(mhe2, x̂min=[1,1], x̂max=[100,100])
+    x̂ = updatestate!(mhe2, [10, 50], [50, 30])
+    @test x̂ ≈ [1, 1] atol=1e-2
+
+    setconstraint!(mhe2, x̂min=[-100,-100], x̂max=[-1,-1])
+    x̂ = updatestate!(mhe2, [10, 50], [50, 30])
+    @test x̂ ≈ [-1, -1] atol=1e-2
+
+    setconstraint!(mhe2, x̂min=[-100,-100], x̂max=[100,100])
+    setconstraint!(mhe2, ŵmin=[-100,-100], ŵmax=[100,100])
+    setconstraint!(mhe2, v̂min=[-100,-100], v̂max=[100,100])
+
+    setconstraint!(mhe2, ŵmin=[1,1], ŵmax=[100,100])
+    x̂ = updatestate!(mhe2, [10, 50], [50, 30])
+    @test mhe2.Ŵ ≈ [1,1] atol=1e-2
+
+    setconstraint!(mhe2, ŵmin=[-100,-100], ŵmax=[-1,-1])
+    x̂ = updatestate!(mhe2, [10, 50], [50, 30])
+    @test mhe2.Ŵ ≈ [-1,-1] atol=1e-2
+
+    setconstraint!(mhe2, x̂min=[-100,-100], x̂max=[100,100])
+    setconstraint!(mhe2, ŵmin=[-100,-100], ŵmax=[100,100])
+    setconstraint!(mhe2, v̂min=[-100,-100], v̂max=[100,100])
+
+    setconstraint!(mhe2, v̂min=[1,1], v̂max=[100,100])
+    x̂ = updatestate!(mhe2, [10, 50], [50, 30])
+    info = getinfo(mhe2)
+    @test info[:V̂] ≈ [1,1] atol=1e-2
+
+    setconstraint!(mhe2, v̂min=[-100,-100], v̂max=[-1,-1])
+    x̂ = updatestate!(mhe2, [10, 50], [50, 30])
+    info = getinfo(mhe2)
+    @test info[:V̂] ≈ [-1,-1] atol=1e-2
 end