Skip to content

debug: setconstraint! for MHE and softness parameter arguments now works #387

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
franckgaga merged 3 commits into from
Jun 23, 2026
Merged

debug: setconstraint! for MHE and softness parameter arguments now works #387

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
5c9e829
debug: `setconstraint!` for MHE and softness parameters
franckgaga Jun 23, 2026
91f8801
test: soft constraints in MHE constraint violation test
franckgaga Jun 23, 2026
c9e0646
test: better coverage of `MHE` constraint violation
franckgaga Jun 23, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
16 changes: 8 additions & 8 deletions src/estimator/mhe/construct.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
137 changes: 76 additions & 61 deletions test/2_test_state_estim.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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))
Expand All @@ -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
Expand Down Expand Up @@ -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])
Expand Down