Merge pull request #226 from JuliaControl/debug_mhe

debug: `MovingHorizonEstimator` no longer crash with error termination flag

Author: franckgaga

src/estimator/mhe/execute.jl

@@ -472,6 +472,7 @@ function set_warmstart_mhe!(V̂, X̂0, estim::MovingHorizonEstimator{NT}, Z̃var
     # will be inevitably different at the following time step.
     Z̃s[nx̃+Nk*nŵ+1:end] .= 1
     JuMP.set_start_value.(Z̃var, Z̃s)
+    return Z̃s
 end
 
 "Correct the covariance estimate at arrival using `covestim` [`StateEstimator`](@ref)."

src/general.jl

@@ -49,7 +49,8 @@ function limit_solve_time(optim::GenericModel, Ts)
         JuMP.set_time_limit_sec(optim, Ts)
     catch err
         if isa(err, MOI.UnsupportedAttribute{MOI.TimeLimitSec})
-            @warn "Solving time limit is not supported by the optimizer."
+            @warn "Solving time limit is not supported by the $(JuMP.solver_name(optim)) "*
+                  "optimizer."
         else
             rethrow()
         end

src/precompile.jl

@@ -79,11 +79,18 @@ exmpc.estim()
 u = exmpc([55, 30])
 sim!(exmpc, 2, [55, 30])
 
-f(x,u,_,model) = model.A*x + model.Bu*u
-h(x,_,model) = model.C*x
+function f!(xnext, x, u, _, model)
+    mul!(xnext, model.A , x)
+    mul!(xnext, model.Bu, u, 1, 1)
+    return nothing
+end
+function h!(y, x, _, model)
+    mul!(y, model.C, x)
+    return nothing
+end
 
 nlmodel = setop!(
-    NonLinModel(f, h, Ts, 2, 2, 2, solver=nothing, p=model), 
+    NonLinModel(f!, h!, Ts, 2, 2, 2, solver=nothing, p=model), 
     uop=[10, 10], yop=[50, 30]
 )
 y = nlmodel()
@@ -118,7 +125,7 @@ u = nmpc_mhe([55, 30])
 sim!(nmpc_mhe, 2, [55, 30])
 
 function JE( _ , Ŷe, _ , R̂y)
-    Ŷ = Ŷe[3:end]
+    Ŷ = @views Ŷe[3:end]
     Ȳ = R̂y - Ŷ
     return dot(Ȳ, Ȳ)
 end

test/2_test_state_estim.jl

@@ -1035,6 +1035,16 @@ end
     info = getinfo(mhe5)
     @test info[:x̂] ≈ x̂ atol=1e-9
     @test info[:Ŷ][end-1:end] ≈ [50, 30] atol=1e-9
+
+    # coverage of the branch with error termination status (with an infeasible problem):
+    mhe_infeas = MovingHorizonEstimator(nonlinmodel, He=1, Cwt=Inf)
+    mhe_infeas = setconstraint!(mhe_infeas, v̂min=[1, 1], v̂max=[-1, -1]) 
+    @test_logs(
+        (:error, "MHE terminated without solution: estimation in open-loop "*
+                 "(more info in debug log)"), 
+        preparestate!(mhe_infeas, [0, 0], [0])
+    )
+
     # for coverage of NLP functions, the univariate syntax of JuMP.@operator
     mhe6 = MovingHorizonEstimator(nonlinmodel, He=1, Cwt=Inf)
     setconstraint!(mhe6, v̂min=[-51,-52], v̂max=[53,54])

test/3_test_predictive_control.jl

@@ -19,7 +19,7 @@
     @test mpc6.weights.L_Hp ≈ Diagonal(diagm(repeat(Float64[0, 1], 15)))
     @test mpc6.weights.L_Hp isa Hermitian{Float64, Diagonal{Float64, Vector{Float64}}}
     mpc7 = @test_logs(
-        (:warn, "Solving time limit is not supported by the optimizer."), 
+        (:warn, "Solving time limit is not supported by the DAQP optimizer."), 
         LinMPC(model, optim=JuMP.Model(DAQP.Optimizer))
     )
     @test solver_name(mpc7.optim) == "DAQP"
@@ -121,6 +121,16 @@ end
     ΔU_diff = diff(getinfo(mpc7)[:U])
     @test ΔU_diff[[2, 4, 5, 7, 8, 9]] ≈ zeros(6) atol=1e-9
 
+    # coverage of the branch with error termination status (with an infeasible problem):
+    mpc_infeas = LinMPC(linmodel2, Hp=1, Hc=1, Cwt=Inf)
+    mpc_infeas = setconstraint!(mpc_infeas, umin=[+1], umax=[-1])
+    preparestate!(mpc_infeas, [0], [0])
+    @test_logs(
+        (:error, "MPC terminated without solution: returning last solution shifted "*
+                 "(more info in debug log)"), 
+        moveinput!(mpc_infeas, [0], [0])
+    )
+
     @test_throws DimensionMismatch moveinput!(mpc1, [0,0,0])
     @test_throws DimensionMismatch moveinput!(mpc1, [0], [0,0])
     @test_throws DimensionMismatch moveinput!(mpc1; D̂  = fill(0, mpc1.Hp+1))
@@ -823,6 +833,17 @@ end
     ΔU_diff = diff(getinfo(nmpc11)[:U])
     @test ΔU_diff[[2, 4, 5, 7, 8, 9]] ≈ zeros(6) atol=1e-9
 
+    # coverage of the branch with error termination status (with an infeasible problem):
+    nmpc_infeas = NonLinMPC(nonlinmodel, Hp=1, Hc=1, Cwt=Inf)
+    nmpc_infeas = setconstraint!(nmpc_infeas, umin=[+1], umax=[-1])
+    preparestate!(nmpc_infeas, [0], [0])
+    @test_logs(
+        (:error, "MPC terminated without solution: returning last solution shifted "*
+                 "(more info in debug log)"), 
+        moveinput!(nmpc_infeas, [0], [0])
+    )
+
+
     @test_nowarn ModelPredictiveControl.info2debugstr(info)
 end