added: Jacobian and Hessians in getinfo(mhe) if nc>0

Project.toml

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

src/controller/nonlinmpc.jl

@@ -834,7 +834,7 @@ function get_nonlinobj_op(mpc::NonLinMPC, optim::JuMP.GenericModel{JNT}) where J
             end
         end
     else
-        update_objective! = function (J, ∇J, Z̃_∇J, Z̃_arg)
+        function (J, ∇J, Z̃_∇J, Z̃_arg)
             if isdifferent(Z̃_arg, Z̃_∇J)
                 Z̃_∇J .= Z̃_arg
                 J[], _ = value_and_gradient!(

src/estimator/mhe/construct.jl

@@ -1250,10 +1250,10 @@ in which ``\mathbf{U_0}`` and ``\mathbf{Y_0^m}`` respectively include the deviat
 of the manipulated inputs ``\mathbf{u_0}(k-j+p)`` from ``j=N_k`` to ``1`` and measured
 outputs ``\mathbf{y_0^m}(k-j+1)`` from ``j=N_k`` to ``1``. The vector ``\mathbf{D_0}``
 includes the the measured disturbance deviation values ``\mathbf{d_0}(k-j)`` from from
-``j=N_k`` to ``0``. The constant ``\mathbf{B}`` is the contribution for non-zero state
-``\mathbf{x̂_{op}}`` and state update ``\mathbf{f̂_{op}}`` operating points (for linearization,
-see [`augment_model`](@ref) and [`linearize`](@ref)). The method also returns the matrices
-for the estimation error at arrival:
+``j=N_k`` to ``0``, thus one additional data point. The constant ``\mathbf{B}`` is the
+contribution for non-zero state ``\mathbf{x̂_{op}}`` and state update ``\mathbf{f̂_{op}}``
+operating points (for linearization, see [`augment_model`](@ref) and [`linearize`](@ref)).
+The method also returns the matrices for the estimation error at arrival:
 ```math
     \mathbf{x̄} = \mathbf{x̂_0^†}(k-N_k+p) - \mathbf{x̂_0}(k-N_k+p) = \mathbf{e_x̄ Z + f_x̄}
 ```

src/estimator/mhe/execute.jl

@@ -93,7 +93,7 @@ following fields:
 
 - `:Ŵ` or *`:What`* : optimal estimated process noise over ``N_k``, ``\mathbf{Ŵ}``
 - `:ε` or *`:epsilon`* : optimal slack variable, ``ε``
-- `:X̂` or *`:Xhat`* : optimal estimated states over ``N_k+1``, ``\mathbf{X̂}``
+- `:X̂` or *`:Xhat`* : optimal estimated states over ``N_k``, ``\mathbf{X̂}``
 - `:x̂` or *`:xhat`* : optimal estimated state, ``\mathbf{x̂}_k(k+p)``
 - `:V̂` or *`:Vhat`* : optimal estimated sensor noise over ``N_k``, ``\mathbf{V̂}``
 - `:P̄` or *`:Pbar`* : estimation error covariance at arrival, ``\mathbf{P̄}``
@@ -104,10 +104,11 @@ following fields:
 - `:J`   : objective value optimum, ``J``
 - `:Ym`  : measured outputs over ``N_k``, ``\mathbf{Y^m}``
 - `:U`   : manipulated inputs over ``N_k``, ``\mathbf{U}``
-- `:D`   : measured disturbances over ``N_k``, ``\mathbf{D}``
+- `:D`   : measured disturbances over ``N_k+1``, ``\mathbf{D}``
 - `:sol` : solution summary of the optimizer for printing
 
-For [`NonLinModel`](@ref), it also includes the following fields:
+For [`NonLinModel`](@ref) or under custom nonlinear inequality constraints (`nc>0`), it also
+includes the following fields:
 
 - `:∇J` or *`:nablaJ`* : optimal gradient of the objective function, ``\mathbf{\nabla} J``
 - `:∇²J` or *`:nabla2J`* : optimal Hessian of the objective function, ``\mathbf{\nabla^2}J``
@@ -136,42 +137,39 @@ julia> round.(getinfo(estim)[:Ŷ], digits=3)
 function getinfo(estim::MovingHorizonEstimator{NT}) where NT<:Real
     model, buffer, Nk = estim.model, estim.buffer, estim.Nk[]
     nu, ny, nd = model.nu, model.ny, model.nd
-    nx̂, nym, nŵ, nε = estim.nx̂, estim.nym, estim.nx̂, estim.nε
-    nx̃ = nε + nx̂
+    nx̂, nym, nŵ = estim.nx̂, estim.nym, estim.nx̂
+    Z̃, Ŵ = estim.Z̃, estim.Ŵ
     info = Dict{Symbol, Any}()
+    Ŷ0 = Vector{NT}(undef, ny*Nk)
     V̂,  X̂0 = buffer.V̂, buffer.X̂
     x̂0arr, û0, k, ŷ0 = buffer.x̂, buffer.û, buffer.k, buffer.ŷ
-    x̂0arr  = getarrival!(x̂0arr, estim, estim.Z̃)
+    x̂0arr  = getarrival!(x̂0arr, estim, Z̃)
     x̄      = estim.x̂0arr_old - x̂0arr
-    V̂,  X̂0 = predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim, model, x̂0arr, estim.Ŵ, estim.Z̃)
-    X̂0 = [x̂0arr; X̂0]
-    Ym0, U0, D0 = estim.Y0m[1:nym*Nk], estim.U0[1:nu*Nk], estim.D0[1:nd*Nk]
-    Ŷ0m, Ŷ0 = Vector{NT}(undef, nym*Nk), Vector{NT}(undef, ny*Nk)
-    for i=1:Nk
-        d0 = @views D0[(1 + nd*(i-1)):(nd*i)]
-        x̂0 = @views X̂0[(1 + nx̂*(i-1)):(nx̂*i)]
-        @views ĥ!(Ŷ0[(1 + ny*(i-1)):(ny*i)], estim, model, x̂0, d0)
-        Ŷ0m[(1 + nym*(i-1)):(nym*i)] .= @views Ŷ0[(1 + ny*(i-1)):(ny*i)][estim.i_ym]
-    end
-    Ym, U, D, Ŷm, Ŷ = Ym0, U0, D0, Ŷ0m, Ŷ0
+    V̂,  X̂0 = predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim, model, x̂0arr, Ŵ, Z̃)
+    Ŷ0     = predict_outputs_mhe!(Ŷ0, estim, X̂0, x̂0arr)
+    J      = obj_nonlinprog(estim, estim.model, x̄, V̂, Ŵ, Z̃)
+    Ym0, U0, D0 = estim.Y0m[1:nym*Nk], estim.U0[1:nu*Nk], estim.D0[1:nd*(Nk+1)]
+    Ym, U, D, Ŷ, X̂, x̂arr = Ym0, U0, D0, Ŷ0, X̂0, x̂0arr
     for i=1:Nk
-        Ŷ[(1 + ny*(i-1)):(ny*i)]    .+= model.yop
-        Ŷm[(1 + nym*(i-1)):(nym*i)] .+= @views model.yop[estim.i_ym]
+        X̂[( 1 +  nx̂*(i-1)):(nx̂*i)]  .+= estim.x̂op
+        Ŷ[( 1 +  ny*(i-1)):(ny*i)]  .+= model.yop
         Ym[(1 + nym*(i-1)):(nym*i)] .+= @views model.yop[estim.i_ym]
-        U[(1 + nu*(i-1)):(nu*i)]    .+= model.uop
-        D[(1 + nd*(i-1)):(nd*i)]    .+= model.dop
+        U[( 1 +  nu*(i-1)):(nu*i)]  .+= model.uop
+        D[( 1 +  nd*(i-1)):(nd*i)]  .+= model.dop
     end
-    info[:Ŵ] = estim.Ŵ[1:Nk*nŵ]
-    info[:x̂arr] = x̂0arr + estim.x̂op
-    info[:ε]  = nε ≠ 0 ? estim.Z̃[begin] : zero(NT)
-    info[:J]  = obj_nonlinprog(estim, estim.model, x̄, V̂, estim.Ŵ, estim.Z̃)
-    info[:X̂]  = (X̂0       .+ @views [estim.x̂op; estim.X̂op])[1:nx̂*(Nk+1)]
+    D[end-nd+1:end] .+= model.dop
+    x̂arr            .+= estim.x̂op
+    info[:Ŵ]  = Ŵ[1:nŵ*Nk]
+    info[:ε]  = getε(estim, Z̃)
+    info[:X̂]  = X̂
     info[:x̂]  = estim.x̂0 .+ estim.x̂op
     info[:V̂]  = V̂
     info[:P̄]  = estim.P̂arr_old
     info[:x̄]  = x̄
     info[:Ŷ]  = Ŷ
-    info[:Ŷm] = Ŷm
+    info[:Ŷm] = Ŷ[vec(estim.i_ym .+ ny.*(0:Nk-1)')]
+    info[:x̂arr] = x̂arr
+    info[:J]  = J
     info[:Ym] = Ym
     info[:U]  = U 
     info[:D]  = D
@@ -195,13 +193,12 @@ end
 
 
 """
-    addinfo!(info, estim::MovingHorizonEstimator, model::NonLinModel)
+    addinfo!(info, estim::MovingHorizonEstimator, model::SimModel) -> info
 
-For [`NonLinModel`](@ref), add the various derivatives.
+Add the various derivatives if model is *not* a [`LinModel`](@ref) or if `nc > 0`.
 """
-function addinfo!(
-    info, estim::MovingHorizonEstimator{NT}, model::NonLinModel
-) where NT <:Real
+function addinfo!(info, estim::MovingHorizonEstimator{NT}, model::SimModel) where NT <:Real
+    model isa LinModel && iszero(estim.con.nc) && return info
     # --- objective derivatives ---
     optim, con = estim.optim, estim.con
     hess = estim.hessian
@@ -305,9 +302,6 @@ function addinfo!(
     return info
 end
 
-"Nothing to add in the `info` dict for [`LinModel`](@ref)."
-addinfo!(info, ::MovingHorizonEstimator, ::LinModel) = info
-
 "Get the estimated state at arrival from the decision vector `Z̃`."
 function getarrival!(x̂0arr, estim::MovingHorizonEstimator, Z̃) 
     nx̃ = estim.nε + estim.nx̂
@@ -856,9 +850,9 @@ function predict_mhe!(
     V̂, X̂0, û0, k, ŷ0, estim::MovingHorizonEstimator, model::SimModel, x̂0arr, Ŵ, _ 
 )
     nu, nd, nx̂, nŵ, nym, Nk = model.nu, model.nd, estim.nx̂, estim.nx̂, estim.nym, estim.Nk[]
-    x̂0 = x̂0arr
+    x̂0 = @views x̂0arr[1:nx̂]
     if Nk < estim.He
-        V̂  .= 0 # fill unused values a 0 for tracer sparsity detection
+        V̂  .= 0 # fill unused values with 0s for tracer sparsity detection
         X̂0 .= 0
     end
     if estim.direct     # p = 0
@@ -895,6 +889,32 @@ function predict_mhe!(
     return V̂, X̂0
 end
 
+@doc raw"""
+    predict_outputs_mhe!(Ŷ0, estim::MovingHorizonEstimator, X̂0, x̂0arr) -> Ŷ0
+
+Predict in-place the outputs of `estim` [`MovingHorizonEstimator`](@ref).
+
+This function is not used for the optimization, but it can be useful to predict the
+estimated outputs ``\mathbf{ŷ_0}(k-j+1)`` from ``j=N_k`` to ``1``, stored in-place in the
+`Ŷ0` vector. The argument `X̂0` is computed from [`predict_mhe!`](@ref) and contains the
+estimated states from ``k-N_k+1+p`` to ``k+p``. The argument `x̂0arr` is computed from
+[`getarrival!`](@ref) and contains the arrival state estimate for the time step ``k-N_k+p``. 
+"""
+function predict_outputs_mhe!(Ŷ0, estim::MovingHorizonEstimator, X̂0, x̂0arr)
+    model = estim.model
+    nd, ny, nx̂, Nk = model.nd, model.ny, estim.nx̂, estim.Nk[]
+    D0 = estim.D0
+    p = estim.direct ? 0 : 1
+    x̂0 = @views estim.direct ? X̂0[1:nx̂] : x̂0arr[1:nx̂]
+    for j=1:Nk
+        d0 = @views D0[(1 + nd*j):(nd*(j+1))] # 1st data in D0 is d0(k-Nk), not used here
+        ŷ0 = @views Ŷ0[(1 + ny*(j-1)):(ny*j)]
+        ĥ!(ŷ0, estim, estim.model, x̂0, d0)
+        j < Nk || break
+        x̂0 = @views X̂0[(1 + nx̂*(j-p)):(nx̂*(j-p+1))]
+    end
+    return Ŷ0
+end
 
 """
     update_predictions!(

test/2_test_state_estim.jl

@@ -1287,7 +1287,7 @@ end
     @test_throws ArgumentError setconstraint!(mhe4, c_v̂max=[1,1])
 end
 
-@testitem "MHE constraint violation" setup=[SetupMPCtests] begin
+@testitem "MHE constraint violation (LinModel)" setup=[SetupMPCtests] begin
     using .SetupMPCtests, ControlSystemsBase, LinearAlgebra
     linmodel = setop!(LinModel(sys,Ts,i_u=[1,2]), uop=[10,50], yop=[50,30])
     mhe = MovingHorizonEstimator(linmodel, He=1, nint_ym=0)
@@ -1336,6 +1336,37 @@ end
     info = getinfo(mhe)
     @test info[:V̂] ≈ [-1,-1] atol=5e-2
 
+    linmodel2 = LinModel(sys, Ts, i_u=[1,2], i_d=[3])
+    linmodel2 = setop!(linmodel2, uop=[10,50], yop=[50,30], dop=[5])
+    function gclv!(LHS, X̂e, _, _, _, _, _, _, _, nx̂, _ )
+        for i in 1:div(length(X̂e), nx̂)
+            LHS[(i-1)+1] = 0.5 - X̂e[(i-1)*nx̂ + 1]  # First state >= 0.5
+        end
+        return nothing
+    end
+    nx̂ = linmodel2.nx
+    nc = 6
+    mhe = MovingHorizonEstimator(linmodel2, He=5, gc=gclv!, nc=nc, nint_ym=0, p=nx̂)
+    preparestate!(mhe, [50, 30], [5])
+    x̂ = updatestate!(mhe, [10, 50], [50, 30], [5])
+    @test x̂[1] ≈ 0.5 atol = 5e-2
+
+    function gcln!(LHS, _, _, Ŵe, _, _, _, _,_, nx̂, _)
+            LHS .= Ŵe[1:nx̂]
+        return nothing
+    end
+    nx̂ = linmodel2.nx
+    nc = linmodel2.nx
+    mhe = MovingHorizonEstimator(linmodel2, He=1, gc=gcln!, nc=nc, nint_ym=0, direct=false, p=nx̂)
+    preparestate!(mhe, [50, 30], [5])
+    x̂ = updatestate!(mhe, [10, 50], [50, 30], [5])
+    @test mhe.Ŵ ≈ zeros(nx̂) atol=5e-2
+end
+
+@testitem "MHE constraint violation (NonLinModel)" setup=[SetupMPCtests] begin
+    using .SetupMPCtests, ControlSystemsBase, LinearAlgebra
+    linmodel = setop!(LinModel(sys,Ts,i_u=[1,2]), uop=[10,50], yop=[50,30])
+
     f = (x,u,_,model) -> model.A*x + model.Bu*u
     h = (x,_,model)   -> model.C*x
     nonlinmodel = NonLinModel(f, h, Ts, 2, 2, 2, p=linmodel, solver=nothing)
@@ -1388,30 +1419,7 @@ end
 
     linmodel2 = LinModel(sys, Ts, i_u=[1,2], i_d=[3])
     linmodel2 = setop!(linmodel2, uop=[10,50], yop=[50,30], dop=[5])
-    function gclv!(LHS, X̂e, _, _, _, _, _, _, _, nx̂, _ )
-        for i in 1:div(length(X̂e), nx̂)
-            LHS[(i-1)+1] = 0.5 - X̂e[(i-1)*nx̂ + 1]  # First state >= 0.5
-        end
-        return nothing
-    end
-    nx̂ = linmodel2.nx
-    nc = 6
-    mhe = MovingHorizonEstimator(linmodel2, He=5, gc=gclv!, nc=nc, nint_ym=0, p=nx̂)
-    preparestate!(mhe, [50, 30], [5])
-    x̂ = updatestate!(mhe, [10, 50], [50, 30], [5])
-    @test x̂[1] ≈ 0.5 atol = 5e-2
 
-    function gcln!(LHS, _, _, Ŵe, _, _, _, _,_, nx̂, _)
-            LHS .= Ŵe[1:nx̂]
-        return nothing
-    end
-    nx̂ = linmodel2.nx
-    nc = linmodel2.nx
-    mhe = MovingHorizonEstimator(linmodel2, He=1, gc=gcln!, nc=nc, nint_ym=0, direct=false, p=nx̂)
-    preparestate!(mhe, [50, 30], [5])
-    x̂ = updatestate!(mhe, [10, 50], [50, 30], [5])
-    @test mhe.Ŵ ≈ zeros(nx̂) atol=5e-2
-
     f = (x,u,d,model) -> model.A*x + model.Bu*u + model.Bd*d
     h = (x,d,model)   -> model.C*x + model.Dd*d
     nonlinmodel2 = NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing, p=linmodel2)
@@ -1424,9 +1432,9 @@ end
     end
     nc = 2
     nx̂ = nonlinmodel2.nx
-    mhe = MovingHorizonEstimator(nonlinmodel2, He=1, gc=gcnlv!, nc=nc, nint_ym=0, p=nx̂)
-    preparestate!(mhe, [50, 30], [5])
-    x̂ = updatestate!(mhe, [10, 50], [50, 30], [5])
+    mhe2 = MovingHorizonEstimator(nonlinmodel2, He=1, gc=gcnlv!, nc=nc, nint_ym=0, p=nx̂)
+    preparestate!(mhe2, [50, 30], [5])
+    x̂ = updatestate!(mhe2, [10, 50], [50, 30], [5])
     @test x̂[1] ≈ 0.5 atol = 5e-2
 
     function gclnl!(LHS, _, _, Ŵe, _, _, _, _,_, nx̂, _)
@@ -1435,11 +1443,10 @@ end
     end
     nx̂ = nonlinmodel2.nx
     nc = nonlinmodel2.nx
-    mhe = MovingHorizonEstimator(nonlinmodel2, He=1, gc=gclnl!, nc=nc, nint_ym=0, p=nx̂)
-    preparestate!(mhe, [50, 30], [5])
-    x̂ = updatestate!(mhe, [10, 50], [50, 30], [5])
-    @test mhe.Ŵ ≈ zeros(nx̂) atol=5e-2
-
+    mhe2 = MovingHorizonEstimator(nonlinmodel2, He=1, gc=gclnl!, nc=nc, nint_ym=0, p=nx̂)
+    preparestate!(mhe2, [50, 30], [5])
+    x̂ = updatestate!(mhe2, [10, 50], [50, 30], [5])
+    @test mhe2.Ŵ ≈ zeros(nx̂) atol=5e-2
 end
 
 @testitem "MHE set model" setup=[SetupMPCtests] begin