test: wip new tests for MHE custom constraints

Author: franckgaga

test/2_test_state_estim.jl

@@ -1566,6 +1566,142 @@ end
     @test X̂_lin ≈ X̂_nonlin atol=1e-3 rtol=1e-3
 end
 
+
+@testitem "MovingHorizonEstimator construction with custom constraint (LinModel)" setup=[SetupMPCtests] begin
+    using .SetupMPCtests, ControlSystemsBase, LinearAlgebra
+    linmodel = LinModel(sys, Ts, i_u=[1,2], i_d=[3])
+    linmodel = setop!(linmodel, uop=[10,50], yop=[50,30], dop=[5])
+    
+    # Custom constraint: simple bound on states using only X̂e and ε
+    function gc_lin(X̂e, _ , _ , _ , _ , _ , _ , _ ,nX̂e , ε)
+        gc = X̂e .- 100 .- ε    # all states must be <= 100
+        return [gc; zeros(nX̂e .- length(X̂e))]
+    end
+    He = 3
+    nx̂ = length(linmodel.A) + 2  # number of states (with augmentation)
+    nc = nx̂ * (He+1) # Approximate constraint count for He=3
+    nX̂e = nx̂ * (He+1)
+    mhe = MovingHorizonEstimator(linmodel, Cwt=1e5, He=He, gc=gc_lin, nc=nc, p=nX̂e)
+    
+    @test mhe.con.nc == nc
+    @test mhe.nε > 0
+    @test mhe.He == 3
+end
+
+@testitem "MovingHorizonEstimator custom constraint violation (LinModel)" setup=[SetupMPCtests] begin
+    using .SetupMPCtests, ControlSystemsBase, LinearAlgebra
+    linmodel = LinModel(sys, Ts, i_u=[1,2], i_d=[3])
+    linmodel = setop!(linmodel, uop=[10,50], yop=[50,30], dop=[5])
+    
+    # Custom constraint function using only X̂e and ε
+    # This constraint enforces that the first state must be >= 0.5
+    function gc_constraint!(LHS, X̂e, V̂e, Ŵe, Ue, Yem, De, P̄, x̄, p, ε)
+        nx̂ = 4  # number of states (with augmentation)
+        # Extract and constrain first state at each time step
+        for i in 1:div(length(X̂e), nx̂)
+            LHS[(i-1)+1] = 0.5 - X̂e[(i-1)*nx̂ + 1]  # First state >= 0.5
+        end
+        return nothing
+    end
+    
+    nc = 3  # One constraint per time step (He=1 initially, but grows)
+    mhe = MovingHorizonEstimator(linmodel, He=1, gc=gc_constraint!, nc=nc, Cwt=1e3, nint_ym=0)
+    
+    preparestate!(mhe, [50, 30], [5])
+    x̂ = updatestate!(mhe, [10, 50], [50, 30], [5])
+    # The constraint should be satisfied (first state should be >= 0.5)
+    @test x̂[1] >= 0.5 - 0.1  # Allow some tolerance
+end
+
+@testitem "MovingHorizonEstimator custom constraint violation (NonLinModel)" setup=[SetupMPCtests] begin
+    using .SetupMPCtests, ControlSystemsBase, LinearAlgebra
+    using JuMP, Ipopt
+    linmodel = LinModel(sys, Ts, i_u=[1,2], i_d=[3])
+    linmodel = setop!(linmodel, uop=[10,50], yop=[50,30], dop=[5])
+    
+    f = (x,u,d,model) -> model.A*x + model.Bu*u + model.Bd*d
+    h = (x,d,model)   -> model.C*x + model.Dd*d
+    nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing, p=linmodel)
+    nonlinmodel = setop!(nonlinmodel, uop=[10,50], yop=[50,30], dop=[5])
+    
+    # Custom constraint function using only X̂e and ε
+    # This constraint enforces that the first state must be >= 0.5
+    function gc_constraint_nl!(LHS, X̂e, V̂e, Ŵe, Ue, Yem, De, P̄, x̄, p, ε)
+        nx̂ = 6  # number of states (with augmentation)
+        # Extract and constrain first state at each time step
+        for i in 1:div(length(X̂e), nx̂)
+            LHS[(i-1)+1] = 0.5 - X̂e[(i-1)*nx̂ + 1]  # First state >= 0.5
+        end
+        return nothing
+    end
+    
+    nc = 3  # One constraint per time step
+    optim = Model(Ipopt.Optimizer)
+    mhe = MovingHorizonEstimator(nonlinmodel, He=1, gc=gc_constraint_nl!, nc=nc, Cwt=1e3, nint_ym=0; optim)
+    
+    preparestate!(mhe, [50, 30], [5])
+    x̂ = updatestate!(mhe, [10, 50], [50, 30], [5])
+    # The constraint should be satisfied (first state should be >= 0.5)
+    @test x̂[1] >= 0.5 - 0.1  # Allow some tolerance
+end
+
+@testitem "MovingHorizonEstimator custom constraint on noise (LinModel)" setup=[SetupMPCtests] begin
+    using .SetupMPCtests, ControlSystemsBase, LinearAlgebra
+    linmodel = LinModel(sys, Ts, i_u=[1,2], i_d=[3])
+    linmodel = setop!(linmodel, uop=[10,50], yop=[50,30], dop=[5])
+    
+    # Custom constraint function using Ŵe and ε
+    # This constraint enforces that process noise must be <= 0
+    function gc_noise_constraint!(LHS, X̂e, V̂e, Ŵe, Ue, Yem, De, P̄, x̄, p, ε)
+        nx̂ = 4  # number of states (with augmentation)
+        # Constraint: all process noise components must be <= 0
+        for i in 1:length(Ŵe)
+            LHS[i] = Ŵe[i]  # Ŵe <= 0
+        end
+        return nothing
+    end
+    
+    nc = 4 * 2  # Two time steps with 4 states each
+    mhe = MovingHorizonEstimator(linmodel, He=1, gc=gc_noise_constraint!, nc=nc, Cwt=1e3, nint_ym=0)
+    
+    preparestate!(mhe, [50, 30], [5])
+    x̂ = updatestate!(mhe, [10, 50], [50, 30], [5])
+    # The constraint should be satisfied (process noise should be <= 0)
+    @test all(mhe.Ŵ .<= 0.1)  # Allow some tolerance
+end
+
+@testitem "MovingHorizonEstimator custom constraint on noise (NonLinModel)" setup=[SetupMPCtests] begin
+    using .SetupMPCtests, ControlSystemsBase, LinearAlgebra
+    using JuMP, Ipopt
+    linmodel = LinModel(sys, Ts, i_u=[1,2], i_d=[3])
+    linmodel = setop!(linmodel, uop=[10,50], yop=[50,30], dop=[5])
+    
+    f = (x,u,d,model) -> model.A*x + model.Bu*u + model.Bd*d
+    h = (x,d,model)   -> model.C*x + model.Dd*d
+    nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing, p=linmodel)
+    nonlinmodel = setop!(nonlinmodel, uop=[10,50], yop=[50,30], dop=[5])
+    
+    # Custom constraint function using Ŵe and ε
+    # This constraint enforces that process noise must be <= 0
+    function gc_noise_constraint_nl!(LHS, X̂e, V̂e, Ŵe, Ue, Yem, De, P̄, x̄, p, ε)
+        nx̂ = 6  # number of states (with augmentation)
+        # Constraint: all process noise components must be <= 0
+        for i in 1:length(Ŵe)
+            LHS[i] = Ŵe[i]  # Ŵe <= 0
+        end
+        return nothing
+    end
+    
+    nc = 6 * 2  # Two time steps with 6 states each
+    optim = Model(Ipopt.Optimizer)
+    mhe = MovingHorizonEstimator(nonlinmodel, He=1, gc=gc_noise_constraint_nl!, nc=nc, Cwt=1e3, nint_ym=0; optim)
+    
+    preparestate!(mhe, [50, 30], [5])
+    x̂ = updatestate!(mhe, [10, 50], [50, 30], [5])
+    # The constraint should be satisfied (process noise should be <= 0)
+    @test all(mhe.Ŵ .<= 0.1)  # Allow some tolerance
+end
+
 @testitem "ManualEstimator construction" setup=[SetupMPCtests] begin
     using .SetupMPCtests, ControlSystemsBase, LinearAlgebra
     linmodel = LinModel(sys,Ts,i_u=[1,2])