added: derivatives in getinfo(::MovingHorizonEstimator)

Author: franckgaga

src/controller/nonlinmpc.jl

@@ -530,13 +530,18 @@ For [`NonLinMPC`](@ref), add `:sol`, the custom nonlinear objective `:JE`, the c
 constraint `:gc`, and the various derivatives.
 """
 function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real
+    # --- variables specific to NonLinMPC ---
     U, Ŷ, D̂, ŷ, d, ϵ = info[:U], info[:Ŷ], info[:D̂], info[:ŷ], info[:d], info[:ϵ]
     Ue = [U; U[(end - mpc.estim.model.nu + 1):end]]
     Ŷe = [ŷ; Ŷ]
     D̂e = [d; D̂] 
     JE = mpc.JE(Ue, Ŷe, D̂e, mpc.p)
     LHS = Vector{NT}(undef, mpc.con.nc)
     mpc.con.gc!(LHS, Ue, Ŷe, D̂e, mpc.p, ϵ)
+    info[:JE]  = JE 
+    info[:gc] = LHS
+    info[:sol] = JuMP.solution_summary(mpc.optim, verbose=true)
+    # --- derivatives ---
     model, optim = mpc.estim.model, mpc.optim
     transcription = mpc.transcription
     nu, ny, nx̂, nϵ = model.nu, model.ny, mpc.estim.nx̂, mpc.nϵ
@@ -568,15 +573,19 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real
     else
         ∇J, ∇²J = gradient(J!, mpc.gradient, mpc.Z̃, J_cache...), nothing
     end
-    JNT = typeof(mpc.optim).parameters[1]
+    JNT = typeof(optim).parameters[1]
     nonlin_constraints = JuMP.all_constraints(
         optim, JuMP.Vector{JuMP.VariableRef}, MOI.VectorNonlinearOracle{JNT}
     )
-    g_con, geq_func = nonlin_constraints
-    λ, λeq = JuMP.dual.(g_con), JuMP.dual.(geq_func)
+    g_con, geq_con = nonlin_constraints
+    display(g_con)
+    display(geq_con)
+    λ, λeq = JuMP.dual.(g_con), JuMP.dual.(geq_con)
+    println(JuMP.dual.(JuMP.FixBoundRef.(optim[:Z̃var])))
     display(λ)
     display(λeq)
-    g_cache = (
+
+    ∇g_cache = (
         Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), 
         Cache(Û0), Cache(K0), Cache(X̂0), 
         Cache(gc), Cache(geq),
@@ -585,7 +594,7 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real
         update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃)
         return nothing
     end
-    ∇g = jacobian(g!, g, mpc.jacobian, mpc.Z̃, g_cache...)
+    ∇g = jacobian(g!, g, mpc.jacobian, mpc.Z̃, ∇g_cache...)
     #=
     if !isnothing(mpc.hessian)
         function ℓ_g(Z̃, λ, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, geq, g)
@@ -614,15 +623,13 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real
     ∇geq = jacobian(geq!, geq, mpc.jacobian, mpc.Z̃, geq_cache...)
     ∇²ℓgeq = nothing # TODO: implement later
 
-    info[:JE]  = JE 
-    info[:gc] = LHS
+
     info[:∇J] = ∇J
     info[:∇²J] = ∇²J
     info[:∇g] = ∇g
     info[:∇²ℓg] = ∇²ℓg
     info[:∇geq] = ∇geq
     info[:∇²ℓgeq] = ∇²ℓgeq
-    info[:sol] = JuMP.solution_summary(mpc.optim, verbose=true)
     # --- non-Unicode fields ---
     info[:nablaJ] = ∇J
     info[:nabla2J] = ∇²J

src/estimator/mhe/construct.jl

@@ -1504,9 +1504,7 @@ function get_nonlincon_oracle(
         return dot(λi, gi)
     end
     Z̃_∇gi = fill(myNaN, nZ̃)      # NaN to force update_predictions! at first call
-    ∇gi_cache = (
-        Cache(V̂), Cache(X̂0), Cache(û0), Cache(k0), Cache(ŷ0), Cache(g)
-    )
+    ∇gi_cache = (Cache(V̂), Cache(X̂0), Cache(û0), Cache(k0), Cache(ŷ0), Cache(g))
     # temporarily "fill" the estimation window for the preparation of the gradient: 
     estim.Nk[] = He
     ∇gi_prep = prepare_jacobian(gi!, gi, jac, Z̃_∇gi, ∇gi_cache...; strict)

src/estimator/mhe/execute.jl

@@ -163,9 +163,85 @@ function getinfo(estim::MovingHorizonEstimator{NT}) where NT<:Real
     info[:Yhatm] = info[:Ŷm]
     # --- deprecated fields ---
     info[:ϵ] = info[:ε]
+    info = addinfo!(info, estim, model)
     return info
 end
 
+
+"""
+    addinfo!(info, estim::MovingHorizonEstimator, model::NonLinModel)
+
+For [`NonLinModel`](@ref), add the various derivatives.
+"""
+function addinfo!(
+    info, estim::MovingHorizonEstimator{NT}, model::NonLinModel
+) where NT <:Real
+    # --- derivatives ---
+    optim, con = estim.optim, estim.con
+    nx̂, nym, nŷ, nu, nk = estim.nx̂, estim.nym, model.ny, model.nu, model.nk
+    He = estim.He
+    ng = length(con.i_g)
+    nV̂, nX̂, ng = He*nym, He*nx̂, length(con.i_g)
+    V̂,  X̂0 = zeros(NT, nV̂), zeros(NT, nX̂)
+    k0     = zeros(NT, nk)
+    û0, ŷ0 = zeros(NT, nu), zeros(NT, nŷ)
+    g      = zeros(NT, ng) 
+    x̄      = zeros(NT, nx̂)
+    J_cache = (
+        Cache(V̂),  Cache(X̂0), Cache(û0), Cache(k0), Cache(ŷ0),
+        Cache(g),
+        Cache(x̄),
+    )
+    function J!(Z̃, V̂, X̂0, û0, k0, ŷ0, g, x̄)
+        update_prediction!(V̂, X̂0, û0, k0, ŷ0, g, estim, Z̃)
+        return obj_nonlinprog!(x̄, estim, model, V̂, Z̃)
+    end
+    if !isnothing(estim.hessian)
+        _, ∇J, ∇²J = value_gradient_and_hessian(J!, estim.hessian, estim.Z̃, J_cache...)
+    else
+        ∇J, ∇²J = gradient(J!, estim.gradient, estim.Z̃, J_cache...), nothing
+    end
+    JNT = typeof(optim).parameters[1]
+    nonlin_constraints = JuMP.all_constraints(
+        optim, JuMP.Vector{JuMP.VariableRef}, MOI.VectorNonlinearOracle{JNT}
+    )
+    g_con = nonlin_constraints[1]
+    λ = JuMP.dual.(g_con)
+    display(λ)
+    ∇g_cache = (Cache(V̂), Cache(X̂0), Cache(û0), Cache(k0), Cache(ŷ0))
+    function g!(g, Z̃, V̂, X̂0, û0, k0, ŷ0)
+        update_prediction!(V̂, X̂0, û0, k0, ŷ0, g, estim, Z̃)
+        return nothing
+    end
+    ∇g = jacobian(g!, g, estim.jacobian, estim.Z̃, ∇g_cache...)
+    #=
+    if !isnothing(estim.hessian)
+        function ℓ_g(Z̃, λ, V̂, X̂0, û0, k0, ŷ0, g)
+            update_prediction!(V̂, X̂0, û0, k0, ŷ0, g, estim, Z̃)
+            return dot(λ, g)
+        end
+        ∇²g_cache = (Cache(V̂), Cache(X̂0), Cache(û0), Cache(k0), Cache(ŷ0), Cache(g))
+        ∇²ℓg = hessian(ℓ_g, estim.hessian, estim.Z̃, Constant(λ), ∇²g_cache...)
+    else
+        ∇²ℓg = nothing
+    end
+    =# ∇²ℓg = nothing #TODO: delete this line when enabling the above block
+
+    info[:∇J] = ∇J
+    info[:∇²J] = ∇²J
+    info[:∇g] = ∇g
+    info[:∇²ℓg] = ∇²ℓg
+    # --- non-Unicode fields ---
+    info[:nablaJ] = ∇J
+    info[:nabla2J] = ∇²J
+    info[:nablag] = ∇g
+    info[:nabla2lg] = ∇²ℓg
+    return info
+end
+
+"Nothing to add in the `info` dict for [`LinModel`](@ref)."
+addinfo!(info, ::MovingHorizonEstimator, ::LinModel) = info
+
 """
     getε(estim::MovingHorizonEstimator, Z̃) -> ε