Merge pull request #207 from JuliaControl/debug_mhe

debug: force update of gradient/jacobian in `MovingHorzionEstimator` when window not filled

Author: franckgaga

Project.toml

@@ -1,7 +1,7 @@
 name = "ModelPredictiveControl"
 uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c"
 authors = ["Francis Gagnon"]
-version = "1.6.1"
+version = "1.6.2"
 
 [deps]
 ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e"

docs/src/internals/state_estim.md

@@ -39,6 +39,7 @@ ModelPredictiveControl.linconstraint!(::MovingHorizonEstimator, ::LinModel)
 
 ```@docs
 ModelPredictiveControl.optim_objective!(::MovingHorizonEstimator)
+ModelPredictiveControl.set_warmstart_mhe!
 ```
 
 ## Remove Operating Points

src/controller/nonlinmpc.jl

@@ -583,9 +583,9 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT
     )
     ∇J_prep = prepare_gradient(Jfunc!, grad, Z̃_∇J, ∇J_context...; strict)
     ∇J = Vector{JNT}(undef, nZ̃)
-    function update_objective!(J, ∇J, Z̃, Z̃arg)
-        if isdifferent(Z̃arg, Z̃)
-            Z̃ .= Z̃arg
+    function update_objective!(J, ∇J, Z̃_∇J, Z̃arg)
+        if isdifferent(Z̃arg, Z̃_∇J)
+            Z̃_∇J .= Z̃arg
             J[], _ = value_and_gradient!(Jfunc!, ∇J, ∇J_prep, grad, Z̃_∇J, ∇J_context...)
         end
     end    
@@ -620,10 +620,10 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT
     ∇g_prep  = prepare_jacobian(gfunc!, g, jac, Z̃_∇g, ∇g_context...; strict)
     mpc.con.i_g[1:end-nc] .= false
     ∇g = init_diffmat(JNT, jac, ∇g_prep, nZ̃, ng)
-    function update_con!(g, ∇g, Z̃, Z̃arg)
-        if isdifferent(Z̃arg, Z̃)
-            Z̃ .= Z̃arg
-            value_and_jacobian!(gfunc!, g, ∇g, ∇g_prep, jac, Z̃, ∇g_context...)
+    function update_con!(g, ∇g, Z̃_∇g, Z̃arg)
+        if isdifferent(Z̃arg, Z̃_∇g)
+            Z̃_∇g .= Z̃arg
+            value_and_jacobian!(gfunc!, g, ∇g, ∇g_prep, jac, Z̃_∇g, ∇g_context...)
         end
     end
     gfuncs = Vector{Function}(undef, ng)
@@ -662,10 +662,10 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT
     )
     ∇geq_prep = prepare_jacobian(geqfunc!, geq, jac, Z̃_∇geq, ∇geq_context...; strict)
     ∇geq = init_diffmat(JNT, jac, ∇geq_prep, nZ̃, neq)
-    function update_con_eq!(geq, ∇geq, Z̃, Z̃arg)
-        if isdifferent(Z̃arg, Z̃)
-            Z̃ .= Z̃arg
-            value_and_jacobian!(geqfunc!, geq, ∇geq, ∇geq_prep, jac, Z̃, ∇geq_context...)
+    function update_con_eq!(geq, ∇geq, Z̃_∇geq, Z̃arg)
+        if isdifferent(Z̃arg, Z̃_∇geq)
+            Z̃_∇geq .= Z̃arg
+            value_and_jacobian!(geqfunc!, geq, ∇geq, ∇geq_prep, jac, Z̃_∇geq, ∇geq_context...)
         end
     end
     geqfuncs = Vector{Function}(undef, neq)

src/controller/transcription.jl

@@ -954,7 +954,7 @@ linconstrainteq!(::PredictiveController, ::SimModel, ::MultipleShooting) = nothi
 @doc raw"""
     set_warmstart!(mpc::PredictiveController, transcription::SingleShooting, Z̃var) -> Z̃s
 
-Set and return the warm start value of `Z̃var` for [`SingleShooting`](@ref) transcription.
+Set and return the warm-start value of `Z̃var` for [`SingleShooting`](@ref) transcription.
 
 If supported by `mpc.optim`, it warm-starts the solver at:
 ```math
@@ -985,7 +985,7 @@ end
 @doc raw"""
     set_warmstart!(mpc::PredictiveController, transcription::MultipleShooting, Z̃var) -> Z̃s
 
-Set and return the warm start value of `Z̃var` for [`MultipleShooting`](@ref) transcription.
+Set and return the warm-start value of `Z̃var` for [`MultipleShooting`](@ref) transcription.
 
 It warm-starts the solver at:
 ```math

src/estimator/internal_model.jl

@@ -168,15 +168,15 @@ function matrices_internalmodel(model::SimModel{NT}) where NT<:Real
 end
 
 @doc raw"""
-    f̂!(x̂0next, _ , x̂0i, estim::InternalModel, model::NonLinModel, x̂0, u0, d0)
+    f̂!(x̂0next, _ , k0, estim::InternalModel, model::NonLinModel, x̂0, u0, d0)
 
 State function ``\mathbf{f̂}`` of [`InternalModel`](@ref) for [`NonLinModel`](@ref).
 
-It calls `model.solver_f!(x̂0next, x̂0i, x̂0, u0 ,d0, model.p)` directly since this estimator
+It calls `model.solver_f!(x̂0next, k0, x̂0, u0 ,d0, model.p)` directly since this estimator
 does not augment the states.
 """
-function f̂!(x̂0next, _ , x̂0i, ::InternalModel, model::NonLinModel, x̂0, u0, d0)
-    return model.solver_f!(x̂0next, x̂0i, x̂0, u0, d0, model.p)
+function f̂!(x̂0next, _ , k0, ::InternalModel, model::NonLinModel, x̂0, u0, d0)
+    return model.solver_f!(x̂0next, k0, x̂0, u0, d0, model.p)
 end
 
 @doc raw"""

src/estimator/mhe/construct.jl

@@ -1356,9 +1356,9 @@ function get_optim_functions(
     ∇J_prep = prepare_gradient(Jfunc!, grad, Z̃_∇J, ∇J_context...; strict)
     estim.Nk[] = 0
     ∇J = Vector{JNT}(undef, nZ̃)
-    function update_objective!(J, ∇J, Z̃, Z̃arg)
-        if isdifferent(Z̃arg, Z̃)
-            Z̃ .= Z̃arg
+    function update_objective!(J, ∇J, Z̃_∇J, Z̃arg)
+        if isdifferent(Z̃arg, Z̃_∇J)
+            Z̃_∇J .= Z̃arg
             J[], _ = value_and_gradient!(Jfunc!, ∇J, ∇J_prep, grad, Z̃_∇J, ∇J_context...)
         end
     end
@@ -1387,10 +1387,10 @@ function get_optim_functions(
     estim.con.i_g .= false
     estim.Nk[] = 0
     ∇g = init_diffmat(JNT, jac, ∇g_prep, nZ̃, ng)
-    function update_con!(g, ∇g, Z̃, Z̃arg)
-        if isdifferent(Z̃arg, Z̃)
-            Z̃ .= Z̃arg
-            value_and_jacobian!(gfunc!, g, ∇g, ∇g_prep, jac, Z̃, ∇g_context...)
+    function update_con!(g, ∇g, Z̃_∇g, Z̃arg)
+        if isdifferent(Z̃arg, Z̃_∇g)
+            Z̃_∇g .= Z̃arg
+            value_and_jacobian!(gfunc!, g, ∇g, ∇g_prep, jac, Z̃_∇g, ∇g_context...)
         end
     end
     gfuncs = Vector{Function}(undef, ng)

src/estimator/mhe/execute.jl

@@ -362,41 +362,19 @@ end
 
 Optimize objective of `estim` [`MovingHorizonEstimator`](@ref) and return the solution `Z̃`.
 
-If supported by `estim.optim`, it warm-starts the solver at:
-```math
-\mathbf{Z̃_s} = 
-\begin{bmatrix}
-    ϵ_{k-1}                         \\
-    \mathbf{x̂}_{k-1}(k-N_k+p)       \\ 
-    \mathbf{ŵ}_{k-1}(k-N_k+p+0)     \\ 
-    \mathbf{ŵ}_{k-1}(k-N_k+p+1)     \\ 
-    \vdots                          \\
-    \mathbf{ŵ}_{k-1}(k-p-2)         \\
-    \mathbf{0}                      \\
-\end{bmatrix}
-```
-where ``\mathbf{ŵ}_{k-1}(k-j)`` is the input increment for time ``k-j`` computed at the 
-last time step ``k-1``. It then calls `JuMP.optimize!(estim.optim)` and extract the
-solution. A failed optimization prints an `@error` log in the REPL and returns the 
-warm-start value. A failed optimization also prints [`getinfo`](@ref) results in
-the debug log [if activated](@extref Julia Example:-Enable-debug-level-messages).
+If first warm-starts the solver with [`set_warmstart_mhe!`](@ref). It then calls 
+`JuMP.optimize!(estim.optim)` and extract the solution. A failed optimization prints an 
+`@error` log in the REPL and returns the warm-start value. A failed optimization also prints
+[`getinfo`](@ref) results in the debug log [if activated](@extref Julia Example:-Enable-debug-level-messages).
 """
 function optim_objective!(estim::MovingHorizonEstimator{NT}) where NT<:Real
     model, optim, buffer = estim.model, estim.optim, estim.buffer
-    nu, ny, nk = model.nu, model.ny, model.nk
-    nx̂, nym, nŵ, nϵ, Nk = estim.nx̂, estim.nym, estim.nx̂, estim.nϵ, estim.Nk[]
+    nym, nx̂, nŵ, nϵ, Nk = estim.nym, estim.nx̂, estim.nx̂, estim.nϵ, estim.Nk[]
     nx̃ = nϵ + nx̂
     Z̃var::Vector{JuMP.VariableRef} = optim[:Z̃var]
-    V̂   = Vector{NT}(undef, nym*Nk)
-    X̂0  = Vector{NT}(undef, nx̂*Nk)
-    û0, ŷ0, x̄, k0 = buffer.û, buffer.ŷ, buffer.x̂, buffer.k
-    ϵ_0 = estim.nϵ ≠ 0 ? estim.Z̃[begin] : empty(estim.Z̃)
-    Z̃s = [ϵ_0; estim.x̂0arr_old; estim.Ŵ]
-    V̂, X̂0 = predict!(V̂, X̂0, û0, k0, ŷ0, estim, model, Z̃s)
-    J_0 = obj_nonlinprog!(x̄, estim, model, V̂, Z̃s)
-    # warm-start Z̃s with Ŵ=0 if objective or constraint function not finite :
-    isfinite(J_0) || (Z̃s = [ϵ_0; estim.x̂0arr_old; zeros(NT, nŵ*estim.He)])
-    JuMP.set_start_value.(Z̃var, Z̃s)
+    V̂   = Vector{NT}(undef, nym*Nk)     # TODO: remove this allocation
+    X̂0  = Vector{NT}(undef, nx̂*Nk)      # TODO: remove this allocation
+    Z̃s = set_warmstart_mhe!(V̂, X̂0, estim, Z̃var)
     # ------- solve optimization problem --------------
     try
         JuMP.optimize!(optim)
@@ -433,13 +411,64 @@ function optim_objective!(estim::MovingHorizonEstimator{NT}) where NT<:Real
         estim.Z̃ .= JuMP.value.(Z̃var)
     end
     # --------- update estimate -----------------------
+    û0, ŷ0, k0 = buffer.û, buffer.ŷ, buffer.k
     estim.Ŵ[1:nŵ*Nk] .= @views estim.Z̃[nx̃+1:nx̃+nŵ*Nk] # update Ŵ with optimum for warm-start
     V̂, X̂0 = predict!(V̂, X̂0, û0, k0, ŷ0, estim, model, estim.Z̃)
     x̂0next    = @views X̂0[end-nx̂+1:end] 
     estim.x̂0 .= x̂0next
     return estim.Z̃
 end
 
+@doc raw"""
+    set_warmstart_mhe!(V̂, X̂0, estim::MovingHorizonEstimator, Z̃var) -> Z̃s
+
+Set and return the warm-start value of `Z̃var` for [`MovingHorizonEstimator`](@ref).
+
+If supported by `estim.optim`, it warm-starts the solver at:
+```math
+\mathbf{Z̃_s} = 
+\begin{bmatrix}
+    ϵ_{k-1}                         \\
+    \mathbf{x̂}_{k-1}(k-N_k+p)       \\ 
+    \mathbf{ŵ}_{k-1}(k-N_k+p+0)     \\ 
+    \mathbf{ŵ}_{k-1}(k-N_k+p+1)     \\ 
+    \vdots                          \\
+    \mathbf{ŵ}_{k-1}(k-p-2)         \\
+    \mathbf{0}                      \\
+\end{bmatrix}
+```
+where ``ϵ(k-1)``, ``\mathbf{x̂}_{k-1}(k-N_k+p)`` and ``\mathbf{ŵ}_{k-1}(k-j)`` are
+respectively the slack variable, the arrival state estimate and the process noise estimates
+computed at the last time step ``k-1``. If the objective function is not finite at this
+point, all the process noises ``\mathbf{ŵ}_{k-1}(k-j)`` are warm-started at zeros. The
+method mutates all the arguments.
+"""
+function set_warmstart_mhe!(V̂, X̂0, estim::MovingHorizonEstimator{NT}, Z̃var) where NT<:Real
+    model, buffer = estim.model, estim.buffer
+    nϵ, nx̂, nŵ, nZ̃, Nk = estim.nϵ, estim.nx̂, estim.nx̂, length(estim.Z̃), estim.Nk[]
+    nx̃ = nϵ + nx̂
+    Z̃s  = Vector{NT}(undef, nZ̃)  # TODO: remove this allocation
+    û0, ŷ0, x̄, k0 = buffer.û, buffer.ŷ, buffer.x̂, buffer.k
+    # --- slack variable ϵ ---
+    estim.nϵ == 1 && (Z̃s[begin] = estim.Z̃[begin])
+    # --- arrival state estimate x̂0arr ---
+    Z̃s[nϵ+1:nx̃] = estim.x̂0arr_old
+    # --- process noise estimates Ŵ ---
+    Z̃s[nx̃+1:end] = estim.Ŵ
+    # verify definiteness of objective function:
+    V̂, X̂0 = predict!(V̂, X̂0, û0, k0, ŷ0, estim, model, Z̃s)
+    Js = obj_nonlinprog!(x̄, estim, model, V̂, Z̃s)
+    if !isfinite(Js)
+        Z̃s[nx̃+1:end] = 0
+    end
+    # --- unused variable in Z̃ (applied only when Nk ≠ He) ---
+    # We force the update of the NLP gradient and jacobian by warm-starting the unused 
+    # variable in Z̃ at 1. Since estim.Ŵ is initialized with 0s, at least 1 variable in Z̃s
+    # will be inevitably different at the following time step.
+    Z̃s[nx̃+Nk*nŵ+1:end] .= 1
+    JuMP.set_start_value.(Z̃var, Z̃s)
+end
+
 "Correct the covariance estimate at arrival using `covestim` [`StateEstimator`](@ref)."
 function correct_cov!(estim::MovingHorizonEstimator)
     nym, nd = estim.nym, estim.model.nd

test/2_test_state_estim.jl

@@ -1064,6 +1064,29 @@ end
     @test_throws ErrorException setstate!(mhe1, [1,2,3,4,5,6], diagm(.1:.1:.6))
 end
 
+@testitem "MovingHorizonEstimator estimation with unfilled window" setup=[SetupMPCtests] begin
+    f(x,u,_,_) = 0.5x + u
+    h(x,_,_) = x
+    model = NonLinModel(f, h, 10.0, 1, 1, 1, solver=nothing)
+    mhe1 = MovingHorizonEstimator(model, nint_u=[1], He=3, direct=true) 
+    for i = 1:40
+        y = model()
+        x̂ = preparestate!(mhe1, y)
+        updatestate!(mhe1, [0.0], y)
+        updatestate!(model, [0.1])
+    end
+    @test mhe1() ≈ model() atol = 1e-9
+    model = NonLinModel(f, h, 10.0, 1, 1, 1, solver=nothing)
+    mhe2 = MovingHorizonEstimator(model, nint_u=[1], He=3, direct=false) 
+    for i = 1:40
+        y = model()
+        x̂ = preparestate!(mhe2, y)
+        updatestate!(mhe2, [0.0], y)
+        updatestate!(model, [0.1])
+    end
+    @test mhe2() ≈ model() atol = 1e-9
+end
+
 @testitem "MovingHorizonEstimator fallbacks for arrival covariance estimation" setup=[SetupMPCtests] begin
     using .SetupMPCtests, ControlSystemsBase, LinearAlgebra
     linmodel = setop!(LinModel(sys,Ts,i_u=[1,2], i_d=[3]), uop=[10,50], yop=[50,30], dop=[5])