diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 73abc47f2..3b864817e 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -920,39 +920,39 @@ function setconstraint!( size(C_x̂min) == (nX̂con,) || throw(DimensionMismatch("C_x̂min size must be $((nX̂con,))")) any(C_x̂min .< 0) && error("C_x̂min weights should be non-negative") # if C is finite : x̃ = [ε; x̂] - con.A_x̃min[end-nx̂+1:end, end] .= @views -C_x̂min[1:nx̂] + con.A_x̃min[end-nx̂+1:end, begin] .= @views -C_x̂min[1:nx̂] con.C_x̂min .= @views C_x̂min[nx̂+1:end] - size(con.A_X̂min, 1) ≠ 0 && (con.A_X̂min[:, end] = -con.C_x̂min) # for LinModel + size(con.A_X̂min, 1) ≠ 0 && (con.A_X̂min[:, begin] = -con.C_x̂min) # for LinModel end if !isnothing(C_x̂max) size(C_x̂max) == (nX̂con,) || throw(DimensionMismatch("C_x̂max size must be $((nX̂con,))")) any(C_x̂max .< 0) && error("C_x̂max weights should be non-negative") # if C is finite : x̃ = [ε; x̂] : - con.A_x̃max[end-nx̂+1:end, end] .= @views -C_x̂max[1:nx̂] + con.A_x̃max[end-nx̂+1:end, begin] .= @views -C_x̂max[1:nx̂] con.C_x̂max .= @views C_x̂max[nx̂+1:end] - size(con.A_X̂max, 1) ≠ 0 && (con.A_X̂max[:, end] = -con.C_x̂max) # for LinModel + size(con.A_X̂max, 1) ≠ 0 && (con.A_X̂max[:, begin] = -con.C_x̂max) # for LinModel end if !isnothing(C_ŵmin) size(C_ŵmin) == (nŵ*He,) || throw(DimensionMismatch("C_ŵmin size must be $((nŵ*He,))")) any(C_ŵmin .< 0) && error("C_ŵmin weights should be non-negative") - con.A_Ŵmin[:, end] .= -C_ŵmin + con.A_Ŵmin[:, begin] .= -C_ŵmin end if !isnothing(C_ŵmax) size(C_ŵmax) == (nŵ*He,) || throw(DimensionMismatch("C_ŵmax size must be $((nŵ*He,))")) any(C_ŵmax .< 0) && error("C_ŵmax weights should be non-negative") - con.A_Ŵmax[:, end] .= -C_ŵmax + con.A_Ŵmax[:, begin] .= -C_ŵmax end if !isnothing(C_v̂min) size(C_v̂min) == (nym*He,) || throw(DimensionMismatch("C_v̂min size must be $((nym*He,))")) any(C_v̂min .< 0) && error("C_v̂min weights should be non-negative") con.C_v̂min .= C_v̂min - size(con.A_V̂min, 1) ≠ 0 && (con.A_V̂min[:, end] = -con.C_v̂min) # for LinModel + size(con.A_V̂min, 1) ≠ 0 && (con.A_V̂min[:, begin] = -con.C_v̂min) # for LinModel end if !isnothing(C_v̂max) size(C_v̂max) == (nym*He,) || throw(DimensionMismatch("C_v̂max size must be $((nym*He,))")) any(C_v̂max .< 0) && error("C_v̂max weights should be non-negative") con.C_v̂max .= C_v̂max - size(con.A_V̂max, 1) ≠ 0 && (con.A_V̂max[:, end] = -con.C_v̂max) # for LinModel + size(con.A_V̂max, 1) ≠ 0 && (con.A_V̂max[:, begin] = -con.C_v̂max) # for LinModel end end i_x̃min, i_x̃max = .!isinf.(con.x̃0min), .!isinf.(con.x̃0max) diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index 8b37902db..84050a8ef 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -1227,16 +1227,16 @@ end @test mhe1.con.V̂min ≈ [-59,-60] @test mhe1.con.V̂max ≈ [61,62] setconstraint!(mhe1, c_x̂min=[0.01,0.02], c_x̂max=[0.03,0.04]) - @test -mhe1.con.A_X̂min[:, end] ≈ [0.01, 0.02] - @test -mhe1.con.A_X̂max[:, end] ≈ [0.03,0.04] - @test -mhe1.con.A_x̃min[2:end, end] ≈ [0.01,0.02] - @test -mhe1.con.A_x̃max[2:end, end] ≈ [0.03,0.04] + @test -mhe1.con.A_X̂min[:, begin] ≈ [0.01, 0.02] + @test -mhe1.con.A_X̂max[:, begin] ≈ [0.03,0.04] + @test -mhe1.con.A_x̃min[2:end, begin] ≈ [0.01,0.02] + @test -mhe1.con.A_x̃max[2:end, begin] ≈ [0.03,0.04] setconstraint!(mhe1, c_ŵmin=[0.05,0.06], c_ŵmax=[0.07,0.08]) - @test -mhe1.con.A_Ŵmin[:, end] ≈ [0.05, 0.06] - @test -mhe1.con.A_Ŵmax[:, end] ≈ [0.07,0.08] + @test -mhe1.con.A_Ŵmin[:, begin] ≈ [0.05, 0.06] + @test -mhe1.con.A_Ŵmax[:, begin] ≈ [0.07,0.08] setconstraint!(mhe1, c_v̂min=[0.09,0.10], c_v̂max=[0.11,0.12]) - @test -mhe1.con.A_V̂min[:, end] ≈ [0.09, 0.10] - @test -mhe1.con.A_V̂max[:, end] ≈ [0.11,0.12] + @test -mhe1.con.A_V̂min[:, begin] ≈ [0.09, 0.10] + @test -mhe1.con.A_V̂max[:, begin] ≈ [0.11,0.12] mhe2 = MovingHorizonEstimator(linmodel, He=4, nint_ym=0, Cwt=1e3) setconstraint!(mhe2, X̂min=-1(1:10), X̂max=1(1:10)) @@ -1251,16 +1251,16 @@ end @test mhe2.con.V̂min ≈ -1(31:38) @test mhe2.con.V̂max ≈ 1(31:38) setconstraint!(mhe2, C_x̂min=0.01(1:10), C_x̂max=0.02(1:10)) - @test -mhe2.con.A_X̂min[:, end] ≈ 0.01(3:10) - @test -mhe2.con.A_X̂max[:, end] ≈ 0.02(3:10) - @test -mhe2.con.A_x̃min[2:end, end] ≈ 0.01(1:2) - @test -mhe2.con.A_x̃max[2:end, end] ≈ 0.02(1:2) + @test -mhe2.con.A_X̂min[:, begin] ≈ 0.01(3:10) + @test -mhe2.con.A_X̂max[:, begin] ≈ 0.02(3:10) + @test -mhe2.con.A_x̃min[2:end, begin] ≈ 0.01(1:2) + @test -mhe2.con.A_x̃max[2:end, begin] ≈ 0.02(1:2) setconstraint!(mhe2, C_ŵmin=0.03(11:18), C_ŵmax=0.04(11:18)) - @test -mhe2.con.A_Ŵmin[:, end] ≈ 0.03(11:18) - @test -mhe2.con.A_Ŵmax[:, end] ≈ 0.04(11:18) + @test -mhe2.con.A_Ŵmin[:, begin] ≈ 0.03(11:18) + @test -mhe2.con.A_Ŵmax[:, begin] ≈ 0.04(11:18) setconstraint!(mhe2, C_v̂min=0.05(31:38), C_v̂max=0.06(31:38)) - @test -mhe2.con.A_V̂min[:, end] ≈ 0.05(31:38) - @test -mhe2.con.A_V̂max[:, end] ≈ 0.06(31:38) + @test -mhe2.con.A_V̂min[:, begin] ≈ 0.05(31:38) + @test -mhe2.con.A_V̂max[:, begin] ≈ 0.06(31:38) f(x,u,d,model) = model.A*x + model.Bu*u h(x,d,model) = model.C*x @@ -1315,51 +1315,66 @@ end @testitem "MHE constraint violation (LinModel)" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel = setop!(LinModel(sys,Ts,i_u=[1,2]), uop=[10,50], yop=[50,30]) - mhe = MovingHorizonEstimator(linmodel, 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]) - preparestate!(mhe, [50, 30]) - x̂ = updatestate!(mhe, [10, 50], [50, 30]) - @test x̂ ≈ [1, 1] atol=5e-2 - - setconstraint!(mhe, x̂min=[-100,-100], x̂max=[-1,-1]) - preparestate!(mhe, [50, 30]) - x̂ = updatestate!(mhe, [10, 50], [50, 30]) - @test x̂ ≈ [-1, -1] atol=5e-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]) - preparestate!(mhe, [50, 30]) - x̂ = updatestate!(mhe, [10, 50], [50, 30]) - @test mhe.Ŵ ≈ [1,1] atol=5e-2 - - setconstraint!(mhe, ŵmin=[-100,-100], ŵmax=[-1,-1]) - preparestate!(mhe, [50, 30]) - x̂ = updatestate!(mhe, [10, 50], [50, 30]) - @test mhe.Ŵ ≈ [-1,-1] atol=5e-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]) - preparestate!(mhe, [50, 30]) - x̂ = updatestate!(mhe, [10, 50], [50, 30]) - info = getinfo(mhe) - @test info[:V̂] ≈ [1,1] atol=5e-2 - - setconstraint!(mhe, v̂min=[-100,-100], v̂max=[-1,-1]) - preparestate!(mhe, [50, 30]) - x̂ = updatestate!(mhe, [10, 50], [50, 30]) - info = getinfo(mhe) - @test info[:V̂] ≈ [-1,-1] atol=5e-2 + mhe_soft = MovingHorizonEstimator(linmodel, He=1, nint_ym=0, Cwt=1e5) + + setconstraint!(mhe_soft, x̂min=[-100,-100], x̂max=[100,100]) + setconstraint!(mhe_soft, ŵmin=[-100,-100], ŵmax=[100,100]) + setconstraint!(mhe_soft, v̂min=[-100,-100], v̂max=[100,100]) + + # activating all soft constraints to ensure that they work as intended: + setconstraint!(mhe_soft, c_x̂min=[1, 1], c_x̂max=[1, 1]) + setconstraint!(mhe_soft, c_ŵmin=[0.1, 0.1], c_ŵmax=[0.1, 0.1]) + setconstraint!(mhe_soft, c_v̂min=[1, 1], c_v̂max=[1, 1]) + + mhe_hard = MovingHorizonEstimator(linmodel, He=1, nint_ym=0, Cwt=Inf) + + setconstraint!(mhe_hard, x̂min=[-100,-100], x̂max=[100,100]) + setconstraint!(mhe_hard, ŵmin=[-100,-100], ŵmax=[100,100]) + setconstraint!(mhe_hard, v̂min=[-100,-100], v̂max=[100,100]) + + function test_bound_violation(mhe) + setconstraint!(mhe, x̂min=[1,1], x̂max=[100,100]) + preparestate!(mhe, [50, 30]) + x̂ = updatestate!(mhe, [10, 50], [50, 30]) + @test x̂ ≈ [1, 1] atol=5e-2 + + setconstraint!(mhe, x̂min=[-100,-100], x̂max=[-1,-1]) + preparestate!(mhe, [50, 30]) + x̂ = updatestate!(mhe, [10, 50], [50, 30]) + @test x̂ ≈ [-1, -1] atol=5e-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]) + preparestate!(mhe, [50, 30]) + x̂ = updatestate!(mhe, [10, 50], [50, 30]) + @test mhe.Ŵ ≈ [1,1] atol=5e-2 + + setconstraint!(mhe, ŵmin=[-100,-100], ŵmax=[-1,-1]) + preparestate!(mhe, [50, 30]) + x̂ = updatestate!(mhe, [10, 50], [50, 30]) + @test mhe.Ŵ ≈ [-1,-1] atol=5e-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]) + preparestate!(mhe, [50, 30]) + x̂ = updatestate!(mhe, [10, 50], [50, 30]) + info = getinfo(mhe) + @test info[:V̂] ≈ [1,1] atol=5e-2 + + setconstraint!(mhe, v̂min=[-100,-100], v̂max=[-1,-1]) + preparestate!(mhe, [50, 30]) + x̂ = updatestate!(mhe, [10, 50], [50, 30]) + info = getinfo(mhe) + @test info[:V̂] ≈ [-1,-1] atol=5e-2 + end + test_bound_violation(mhe_soft) + test_bound_violation(mhe_hard) linmodel2 = LinModel(sys, Ts, i_u=[1,2], i_d=[3]) linmodel2 = setop!(linmodel2, uop=[10,50], yop=[50,30], dop=[5])