debug: linear constraints MHE now works

Author: franckgaga

src/estimator/mhe/execute.jl

@@ -253,11 +253,11 @@ end
 
 "Set `b` excluding state and sensor noise bounds if `model` is not a [`LinModel`](@ref)."
 function linconstraint!(estim::MovingHorizonEstimator, ::SimModel)
-    Ŵmax, Ŵmax = trunc_bounds(estim, estim.con.Ŵmax, estim.con.Ŵmax, estim.nx̂)
+    Ŵmin, Ŵmax = trunc_bounds(estim, estim.con.Ŵmin, estim.con.Ŵmax, estim.nx̂)
     estim.con.b[:] = [
         -estim.con.x̃min
         +estim.con.x̃max
-        -Ŵmax
+        -Ŵmin
         +Ŵmax
     ]
     lincon = estim.optim[:linconstraint]

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)
@@ -549,7 +549,7 @@ end
     @test x̂ ≈ [0, 0]
     @test isa(x̂, Vector{Float32})
 end
-=#
+
 @testset "MovingHorizonEstimator construction" begin
     linmodel1 = LinModel(sys,Ts,i_d=[3])
     f(x,u,d) = linmodel1.A*x + linmodel1.Bu*u + linmodel1.Bd*d