debug: AutoSparse hessian with MovingHorizonEstimator now works

Project.toml

@@ -1,6 +1,6 @@
 name = "ModelPredictiveControl"
 uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c"
-version = "2.8.2"
+version = "2.8.3"
 authors = ["Francis Gagnon"]
 
 [deps]

src/estimator/mhe/execute.jl

@@ -810,9 +810,6 @@ function obj_nonlinprog(estim::MovingHorizonEstimator, ::SimModel, x̄, V̂, Ŵ
         nŴ, nYm = Nk*estim.nx̂, Nk*estim.nym
         Ŵ, V̂ = Ŵ[1:nŴ], V̂[1:nYm]
     end
-    if any(isnan, V̂) # ignore NaN values in V̂ for the objective function:
-        V̂ = [isnan(v) ? 0 : v for v in V̂]
-    end
     Jε = estim.nε > 0 ? estim.C*Z̃[begin]^2 : 0
     return dot(x̄, invP̄, x̄) + dot(Ŵ, invQ̂_Nk, Ŵ) + dot(V̂, invR̂_Nk, V̂) + Jε
 end
@@ -889,6 +886,9 @@ function predict_mhe!(
             d0next  = @views estim.D0[(1 + nd*j):(nd*(j+1))]
             ĥ!(ŷ0next, estim, model, x̂0next, d0next)
             ŷ0nextm = @views ŷ0next[estim.i_ym]
+            if any(isnan, y0nextm)
+                y0nextm = [isnan(y) ? ŷ : y for (y, ŷ) in zip(y0nextm, ŷ0nextm)]
+            end
             V̂[(1 + nym*(j-1)):(nym*j)] .= y0nextm .- ŷ0nextm
             x̂0, d0 = x̂0next, d0next
         end        
@@ -900,6 +900,9 @@ function predict_mhe!(
             ŵ   = @views Ŵ[(1 + nŵ*(j-1)):(nŵ*j)]
             ĥ!(ŷ0, estim, model, x̂0, d0)
             ŷ0m = @views ŷ0[estim.i_ym]
+            if any(isnan, y0m)
+                y0m = [isnan(y) ? ŷ : y for (y, ŷ) in zip(y0m, ŷ0m)]
+            end
             V̂[(1 + nym*(j-1)):(nym*j)] .= y0m .- ŷ0m
             x̂0next = @views X̂0[(1 + nx̂ *(j-1)):(nx̂ *j)]
             f̂!(x̂0next, û0, k, estim, model, x̂0, u0, d0)

test/2_test_state_estim.jl

@@ -1113,15 +1113,15 @@ end
 
 @testitem "MHE estimation and getinfo (NonLinModel)" setup=[SetupMPCtests] begin
     using .SetupMPCtests, ControlSystemsBase, LinearAlgebra, ForwardDiff
-    using JuMP, Ipopt
+    using JuMP, Ipopt, DifferentiationInterface, SparseMatrixColorings, SparseConnectivityTracer
+    import ForwardDiff
     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])
 
-
     mhe1 = MovingHorizonEstimator(nonlinmodel, He=2)
     JuMP.set_attribute(mhe1.optim, "tol", 1e-7)
     preparestate!(mhe1, [50, 30], [5])
@@ -1218,7 +1218,13 @@ end
     @test_nowarn ModelPredictiveControl.info2debugstr(info)
     @test_throws ErrorException setstate!(mhe1, [1,2,3,4,5,6], diagm(.1:.1:.6))
 
-    mhe7 = MovingHorizonEstimator(nonlinmodel, He=2)
+    hessian = AutoSparse(
+        AutoForwardDiff();
+        sparsity_detector=TracerSparsityDetector(),
+        coloring_algorithm=GreedyColoringAlgorithm(),
+    )
+
+    mhe7 = MovingHorizonEstimator(nonlinmodel; He=2, hessian)
     @test_logs(
         (:warn, "NaN values in the MHE measurements ym: ignoring them in the objective"),
         preparestate!(mhe7, [50, NaN], [5])