added: ensure that sum(nb)==Hp

Author: franckgaga

src/controller/construct.jl

@@ -457,9 +457,53 @@ end
 estimate_delays(::SimModel) = 0
 
 
-"Get move blocking vector `nb` and actual `Hc` value from provided `Hc_arg` argument" 
-move_blocking(Hc_arg::AbstractVector{Int}) = (Hc_arg, length(nb))
-move_blocking(Hc_arg::Int) = (fill(1, length(Hc_arg)), Hc_arg)
+"""
+    move_blocking(Hp::Int, Hc::AbstractVector{Int}) -> nb, Hc
+
+Get move blocking vector `nb` and actual control horizon from `Hp` and `Hc` arguments.
+
+The argument `Hc` is in fact the move blocking vector `nb` provided by the user. The `nb`
+vector is modified in these two cases:
+
+- If `sum(nb) < Hp`, a new element is added to `nb` with the value `Hp - sum(nb)`.
+- If `sum(nb) > Hp`, the intervals are truncated until the sum is `Hp`. For example, if
+  `Hp = 10` and `nb = [1, 2, 3, 6, 7]`, then `nb` is truncated to `[1, 2, 3, 4]`.
+""" 
+function move_blocking(Hp_arg::Int, Hc_arg::AbstractVector{Int})
+    Hp = Hp_arg
+    nb = Hc_arg
+    if sum(nb) < Hp
+        newblock = [Hp - sum(nb)]
+        nb = [nb; newblock]
+    elseif sum(nb) > Hp
+        #For example, if Hp = 10 and nb = [1, 2, 3, 6, 7], than nb is truncated to [1, 2, 3, 4]
+        nb = nb[begin:findfirst(≥(Hp), cumsum(nb))]
+        if sum(nb) > Hp
+            # if the last block is too large, it is truncated to fit Hp:
+            nb[end] = Hp - @views sum(nb[begin:end-1])
+        end
+    end
+    Hc = length(nb)
+    return nb, Hc
+end
+
+"""
+    move_blocking(Hp::Int, Hc::Int) -> nb, Hc
+
+Construct a move blocking vector `nb` to fit the provides `Hp` and `Hc` horizons.
+
+The vector is filled with `1`s for the first `Hc` blocks. If `Hc < Hp`, the last block in 
+`nb` is set to `Hp - Hc` to ensure the total sum is `Hp`.
+"""
+function move_blocking(Hp_arg::Int, Hc_arg::Int)
+    Hp, Hc = Hp_arg, Hc_arg
+    nb = fill(1, Hc_arg)
+    if Hc < Hp
+        newblock = Hp - Hc
+        nb = [nb; newblock]
+    end
+    return nb, Hc
+end
 
 
 """

src/controller/explicitmpc.jl

@@ -51,7 +51,7 @@ struct ExplicitMPC{
         lastu0 = zeros(NT, nu)
         transcription = SingleShooting() # explicit MPC only supports SingleShooting
         PΔu = init_ZtoΔU(estim, transcription, Hp, Hc)
-        Pu, Tu = init_ZtoU(estim, transcription, Hp, Hc)
+        Pu, Tu = init_ZtoU(estim, transcription, Hp, Hc, nb)
         E, G, J, K, V, B = init_predmat(model, estim, transcription, Hp, Hc)
         # dummy val (updated just before optimization):
         F = zeros(NT, ny*Hp)
@@ -167,7 +167,7 @@ function ExplicitMPC(
         @warn("prediction horizon Hp ($Hp) ≤ estimated number of delays in model "*
               "($nk), the closed-loop system may be unstable or zero-gain (unresponsive)")
     end
-    nb, Hc = move_blocking(Hc)
+    nb, Hc = move_blocking(Hp, Hc)
     weights = ControllerWeights{NT}(estim.model, Hp, Hc, M_Hp, N_Hc, L_Hp)
     return ExplicitMPC{NT}(estim, Hp, Hc, nb, weights)
 end

src/controller/linmpc.jl

@@ -64,7 +64,7 @@ struct LinMPC{
         R̂y, R̂u, Tu_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
         lastu0 = zeros(NT, nu)
         PΔu = init_ZtoΔU(estim, transcription, Hp, Hc)
-        Pu, Tu = init_ZtoU(estim, transcription, Hp, Hc)
+        Pu, Tu = init_ZtoU(estim, transcription, Hp, Hc, nb)
         E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat(
             model, estim, transcription, Hp, Hc
         )
@@ -267,7 +267,7 @@ function LinMPC(
         @warn("prediction horizon Hp ($Hp) ≤ estimated number of delays in model "*
               "($nk), the closed-loop system may be unstable or zero-gain (unresponsive)")
     end
-    nb, Hc = move_blocking(Hc)
+    nb, Hc = move_blocking(Hp, Hc)
     weights = ControllerWeights{NT}(estim.model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt)
     return LinMPC{NT}(estim, Hp, Hc, nb, weights, transcription, optim)
 end

src/controller/nonlinmpc.jl

@@ -87,7 +87,7 @@ struct NonLinMPC{
         R̂y, R̂u, Tu_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
         lastu0 = zeros(NT, nu)
         PΔu = init_ZtoΔU(estim, transcription, Hp, Hc)
-        Pu, Tu = init_ZtoU(estim, transcription, Hp, Hc)
+        Pu, Tu = init_ZtoU(estim, transcription, Hp, Hc, nb)
         E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat(
             model, estim, transcription, Hp, Hc
         )
@@ -366,7 +366,7 @@ function NonLinMPC(
         @warn("prediction horizon Hp ($Hp) ≤ estimated number of delays in model "*
               "($nk), the closed-loop system may be unstable or zero-gain (unresponsive)")
     end
-    nb, Hc = move_blocking(Hc)
+    nb, Hc = move_blocking(Hp, Hc)
     validate_JE(NT, JE)
     gc! = get_mutating_gc(NT, gc)
     weights = ControllerWeights{NT}(estim.model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt)

src/controller/transcription.jl

@@ -121,7 +121,7 @@ function init_ZtoΔU(
 end
 
 @doc raw"""
-    init_ZtoU(estim, transcription, Hp, Hc) -> Pu, Tu
+    init_ZtoU(estim, transcription, Hp, Hc, nb) -> Pu, Tu
 
 Init decision variables to inputs over ``H_p`` conversion matrices.
 
@@ -164,7 +164,7 @@ The ``\mathbf{P_u}`` and ``\mathbf{T_u}`` matrices are defined in the Extended H
       if `transcription` is a [`MultipleShooting`](@ref)
 """
 function init_ZtoU(
-    estim::StateEstimator{NT}, transcription::TranscriptionMethod, Hp, Hc
+    estim::StateEstimator{NT}, transcription::TranscriptionMethod, Hp, Hc, nb
 ) where {NT<:Real}
     model = estim.model
     # Pu and Tu are `Matrix{NT}`, conversion is faster than `Matrix{Bool}` or `BitMatrix`