diff --git a/Project.toml b/Project.toml index 12249dea9..ab3ed796b 100644 --- a/Project.toml +++ b/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] diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 7d2fbcb80..dbb54ddad 100644 --- a/src/estimator/mhe/execute.jl +++ b/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) diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index 9b8912ffa..2b870a5a4 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -1113,7 +1113,8 @@ 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 @@ -1121,7 +1122,6 @@ end 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])