changed: MHE without collect on decision var.

Author: franckgaga

src/controller/nonlinmpc.jl

@@ -314,24 +314,20 @@ function init_optimization!(mpc::NonLinMPC, optim::JuMP.GenericModel{JNT}) where
         û_cache::DiffCache{Vector{JNT}, Vector{JNT}}     = DiffCache(zeros(JNT, nu), Nc)
         Ȳ_cache::DiffCache{Vector{JNT}, Vector{JNT}}     = DiffCache(zeros(JNT, nŶ), Nc)
         Ū_cache::DiffCache{Vector{JNT}, Vector{JNT}}     = DiffCache(zeros(JNT, nU), Nc)
-        function Jfunc(ΔŨtup::T...)::T where T <: Real
+        function Jfunc(ΔŨtup::T...)::T where {T <: Real}
             ΔŨ1 = ΔŨtup[begin]
-            ΔŨ, Ŷ = get_tmp(ΔŨ_cache, ΔŨ1), get_tmp(Ŷ_cache, ΔŨ1)
             if T == JNT
-                isnewvalue = (ΔŨtup !== last_ΔŨtup_float)
-                isnewvalue && (last_ΔŨtup_float = ΔŨtup)
+                last_ΔŨtup_float = ΔŨtup
             else
-                isnewvalue = (ΔŨtup !== last_ΔŨtup_dual)
-                isnewvalue && (last_ΔŨtup_dual = ΔŨtup)
-            end
-            if isnewvalue
-                x̂, x̂next = get_tmp(x̂_cache, ΔŨ1), get_tmp(x̂next_cache, ΔŨ1)
-                u, û = get_tmp(u_cache, ΔŨ1), get_tmp(û_cache, ΔŨ1)
-                ΔŨ .= ΔŨtup
-                Ŷ, x̂end = predict!(Ŷ, x̂, x̂next, u, û, mpc, model, ΔŨ)
-                g = get_tmp(g_cache, ΔŨ1)
-                g = con_nonlinprog!(g, mpc, model, x̂end, Ŷ, ΔŨ)
+                last_ΔŨtup_dual  = ΔŨtup
             end
+            ΔŨ, Ŷ = get_tmp(ΔŨ_cache, ΔŨ1), get_tmp(Ŷ_cache, ΔŨ1)
+            x̂, x̂next = get_tmp(x̂_cache, ΔŨ1), get_tmp(x̂next_cache, ΔŨ1)
+            u, û = get_tmp(u_cache, ΔŨ1), get_tmp(û_cache, ΔŨ1)
+            ΔŨ .= ΔŨtup
+            Ŷ, x̂end = predict!(Ŷ, x̂, x̂next, u, û, mpc, model, ΔŨ)
+            g = get_tmp(g_cache, ΔŨ1)
+            g = con_nonlinprog!(g, mpc, model, x̂end, Ŷ, ΔŨ)
             U, Ȳ, Ū = get_tmp(U_cache, ΔŨ1), get_tmp(Ȳ_cache, ΔŨ1), get_tmp(Ū_cache, ΔŨ1)
             return obj_nonlinprog!(U, Ȳ, Ū, mpc, model, Ŷ, ΔŨ)::T
         end

src/estimator/mhe/construct.jl

@@ -1008,72 +1008,52 @@ function init_optimization!(
     Jfunc, gfunc = let estim=estim, model=model, nZ̃=nZ̃, nV̂=nV̂, nX̂=nX̂, ng=ng, nx̂=nx̂, nu=nu, nŷ=nŷ
         Nc = nZ̃ + 3
         last_Z̃tup_float, last_Z̃tup_dual = nothing, nothing
+        Z̃_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nZ̃), Nc)
         V̂_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nV̂), Nc)
         g_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, ng), Nc)
         X̂_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nX̂), Nc)
         x̄_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nx̂), Nc)
         û_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nu), Nc)
         ŷ_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nŷ), Nc)
-        function Jfunc(Z̃tup::JNT...)
+        function Jfunc(Z̃tup::T...)::T where {T <: Real}
             Z̃1 = Z̃tup[begin]
-            V̂ = get_tmp(V̂_cache, Z̃1)
-            Z̃ = collect(Z̃tup)
-            if Z̃tup !== last_Z̃tup_float
-                g = get_tmp(g_cache, Z̃1)
-                X̂ = get_tmp(X̂_cache, Z̃1)
-                û, ŷ = get_tmp(û_cache, Z̃1), get_tmp(ŷ_cache, Z̃1)
-                V̂, X̂ = predict!(V̂, X̂, û, ŷ, estim, model, Z̃)
-                g = con_nonlinprog!(g, estim, model, X̂, V̂, Z̃)
+            if T == JNT
                 last_Z̃tup_float = Z̃tup
-            end
-            x̄ = get_tmp(x̄_cache, Z̃1)
-            return obj_nonlinprog!(x̄, estim, model, V̂, Z̃)
-        end
-        function Jfunc(Z̃tup::ForwardDiff.Dual...)
-            Z̃1 = Z̃tup[begin]
-            V̂ = get_tmp(V̂_cache, Z̃1)
-            Z̃ = collect(Z̃tup)
-            if Z̃tup !== last_Z̃tup_dual
-                g = get_tmp(g_cache, Z̃1)
-                X̂ = get_tmp(X̂_cache, Z̃1)
-                û, ŷ = get_tmp(û_cache, Z̃1), get_tmp(ŷ_cache, Z̃1)
-                V̂, X̂ = predict!(V̂, X̂, û, ŷ, estim, model, Z̃)
-                g = con_nonlinprog!(g, estim, model, X̂, V̂, Z̃)
+            else
                 last_Z̃tup_dual = Z̃tup
             end
+            Z̃, V̂ = get_tmp(Z̃_cache, Z̃1), get_tmp(V̂_cache, Z̃1)
+            X̂ = get_tmp(X̂_cache, Z̃1)
+            û, ŷ = get_tmp(û_cache, Z̃1), get_tmp(ŷ_cache, Z̃1)
+            Z̃ .= Z̃tup
+            V̂, X̂ = predict!(V̂, X̂, û, ŷ, estim, model, Z̃)
+            g = get_tmp(g_cache, Z̃1)
+            g = con_nonlinprog!(g, estim, model, X̂, V̂, Z̃)
             x̄ = get_tmp(x̄_cache, Z̃1)
-            return obj_nonlinprog!(x̄, estim, model, V̂, Z̃)
+            return obj_nonlinprog!(x̄, estim, model, V̂, Z̃)::T
         end
-        function gfunc_i(i, Z̃tup::NTuple{N, JNT}) where N
+        function gfunc_i(i, Z̃tup::NTuple{N, T})::T where {N, T <:Real}
             Z̃1 = Z̃tup[begin]
             g = get_tmp(g_cache, Z̃1)
-            if Z̃tup !== last_Z̃tup_float
-                Z̃ = collect(Z̃tup)
-                V̂ = get_tmp(V̂_cache, Z̃1)
-                X̂ = get_tmp(X̂_cache, Z̃1)
-                û, ŷ = get_tmp(û_cache, Z̃1), get_tmp(ŷ_cache, Z̃1)
-                V̂, X̂ = predict!(V̂, X̂, û, ŷ, estim, model, Z̃)
-                g = con_nonlinprog!(g, estim, model, X̂, V̂, Z̃)
-                last_Z̃tup_float = Z̃tup
+            if T == JNT
+                isnewvalue = (Z̃tup !== last_Z̃tup_float)
+                isnewvalue && (last_Z̃tup_float = Z̃tup)
+            else
+                isnewvalue = (Z̃tup !== last_Z̃tup_dual)
+                isnewvalue && (last_Z̃tup_dual = Z̃tup)
             end
-            return g[i]
-        end 
-        function gfunc_i(i, Z̃tup::NTuple{N, ForwardDiff.Dual}) where N
-            Z̃1 = Z̃tup[begin]
-            g = get_tmp(g_cache, Z̃1)
-            if Z̃tup !== last_Z̃tup_dual
-                Z̃ = collect(Z̃tup)
-                V̂ = get_tmp(V̂_cache, Z̃1)
+            if isnewvalue
+                Z̃, V̂ = get_tmp(Z̃_cache, Z̃1), get_tmp(V̂_cache, Z̃1)
                 X̂ = get_tmp(X̂_cache, Z̃1)
                 û, ŷ = get_tmp(û_cache, Z̃1), get_tmp(ŷ_cache, Z̃1)
+                Z̃ .= Z̃tup
                 V̂, X̂ = predict!(V̂, X̂, û, ŷ, estim, model, Z̃)
                 g = con_nonlinprog!(g, estim, model, X̂, V̂, Z̃)
-                last_Z̃tup_dual = Z̃tup
             end
             return g[i]
         end
         gfunc = [(Z̃...) -> gfunc_i(i, Z̃) for i in 1:ng]
-        Jfunc, gfunc
+        (Jfunc, gfunc)
     end
     register(optim, :Jfunc, nZ̃, Jfunc, autodiff=true)
     @NLobjective(optim, Min, Jfunc(Z̃var...))

src/general.jl

@@ -7,11 +7,11 @@ const DEFAULT_CWT = 1e5
 const DEFAULT_EWT = 0.0
 
 "Termination status that means 'no solution available'."
-const ERROR_STATUSES = [
+const ERROR_STATUSES = (
     INFEASIBLE, DUAL_INFEASIBLE, LOCALLY_INFEASIBLE, INFEASIBLE_OR_UNBOUNDED, 
     NUMERICAL_ERROR, INVALID_MODEL, INVALID_OPTION, INTERRUPTED, 
     OTHER_ERROR
-]
+)
 
 "Verify that `optim` termination status is `OPTIMAL` or `LOCALLY_SOLVED`."
 function issolved(optim::JuMP.GenericModel)
@@ -22,7 +22,7 @@ end
 "Verify that `optim` termination status means 'no solution available'."
 function iserror(optim::JuMP.GenericModel) 
     status = termination_status(optim)
-    return any(status .== ERROR_STATUSES)
+    return any(errstatus->isequal(status, errstatus), ERROR_STATUSES)
 end
 
 "Evaluate the quadratic programming objective function `0.5x'*H*x + q'*x` at `x`."

src/plot_sim.jl

@@ -129,7 +129,7 @@ function sim!(
         U_data[:, i]  = u
         D_data[:, i]  = d
         X_data[:, i]  = plant.x
-        x = updatestate!(plant, u, d)
+        updatestate!(plant, u, d)
     end
     return SimResult(plant, T_data, Y_data, U_data, Y_data, 
                      U_data, U_data, U_data, D_data, X_data, X_data)

test/test_state_estim.jl

@@ -682,10 +682,12 @@ end
 
     mhe4 = setconstraint!(MovingHorizonEstimator(nonlinmodel, He=1, nint_ym=0), x̂max=[50,50,50,50])
     g_X̂max_end = mhe4.optim.nlp_model.operators.registered_multivariate_operators[end].f
-    @test g_X̂max_end((1.0, 1.0, 1.0, 1.0)) ≤ 0.0 # test gfunc_i(i,::NTuple{N, Float64})
-    # test gfunc_i(i,::NTuple{N, ForwardDiff.Dual}) : 
-    @test ForwardDiff.gradient(g_X̂max_end, [1.0, 1.0, 1.0, 1.0]) ≈ [0.0, 0.0, 0.0, 0.0]
-
+    # test gfunc_i(i,::NTuple{N, Float64}):
+    @test g_X̂max_end(
+        (1.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0)) ≤ 0.0 
+    # test gfunc_i(i,::NTuple{N, ForwardDiff.Dual}): 
+    @test ForwardDiff.gradient(
+        g_X̂max_end, [1.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0]) ≈ [0, 0, 0, 0, 0, 0, 0, 0]
     Q̂ = diagm([1/4, 1/4, 1/4, 1/4].^2) 
     R̂ = diagm([1, 1].^2)
     optim = Model(Ipopt.Optimizer)