debug: last sdnext after loop for nh=1 (special case)

Author: franckgaga

src/controller/transcription.jl

@@ -1397,7 +1397,7 @@ function con_nonlinprogeq!(
     mpc::PredictiveController, model::NonLinModel, transcription::TrapezoidalCollocation, 
     U0, Z̃
 )
-    nu, nx̂, nd, nx = model.nu, mpc.estim.nx̂, model.nd, model.nx
+    nu, nx̂, nd, nx, nh = model.nu, mpc.estim.nx̂, model.nd, model.nx, transcription.nh
     Hp, Hc = mpc.Hp, mpc.Hc
     nΔU, nX̂ = nu*Hc, nx̂*Hp
     Ts, p = model.Ts, model.p
@@ -1407,12 +1407,12 @@ function con_nonlinprogeq!(
     X̂0_Z̃ = @views Z̃[(nΔU+1):(nΔU+nX̂)]
     x̂0 = @views mpc.estim.x̂0[1:nx̂]
     d0 = @views mpc.d0[1:nd]
-    if !iszero(transcription.nh)
+    if !iszero(nh)
         k1, u0, û0 = @views K0[1:nx], U0[1:nu], Û0[1:nu]
-        xd, xs     = @views x̂0[1:nx], x̂0[nx+1:end]
+        x0, xs     = @views x̂0[1:nx], x̂0[nx+1:end]
         mul!(û0, Cs_u, xs)                 
         û0 .+= u0                          
-        model.f!(k1, xd, û0, d0, p)
+        model.f!(k1, x0, û0, d0, p)
         lastk2 = k1
     end
     #TODO: allow parallel for loop or threads? 
@@ -1422,38 +1422,49 @@ function con_nonlinprogeq!(
         x̂0next   = @views   X̂0[(1 + nx̂*(j-1)):(nx̂*j)]
         x̂0next_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*j)]  
         ŝnext    = @views  geq[(1 + nx̂*(j-1)):(nx̂*j)]  
-        xd, xs              = @views x̂0[1:nx], x̂0[nx+1:end]
-        xdnext_Z̃, xsnext_Z̃  = @views x̂0next_Z̃[1:nx], x̂0next_Z̃[nx+1:end]
+        x0, xs              = @views x̂0[1:nx], x̂0[nx+1:end]
+        x0next_Z̃, xsnext_Z̃  = @views x̂0next_Z̃[1:nx], x̂0next_Z̃[nx+1:end]
         sdnext, ssnext      = @views ŝnext[1:nx], ŝnext[nx+1:end]
-        k1, k2 = @views k0[1:nx], k0[nx+1:2*nx]
-        if iszero(transcription.nh) # piecewise constant manipulated inputs u:
+        k1, k2              = @views k0[1:nx], k0[nx+1:2*nx]
+        # ----------------- stochastic defects -----------------------------------------
+        xsnext = @views x̂0next[nx+1:end]
+        mul!(xsnext, As, xs)
+        ssnext .= @. xsnext - xsnext_Z̃
+        # ----------------- deterministic defects --------------------------------------
+        if iszero(nh) # piecewise constant manipulated inputs u:
             u0 = @views U0[(1 + nu*(j-1)):(nu*j)]
             û0 = @views Û0[(1 + nu*(j-1)):(nu*j)]
             mul!(û0, Cs_u, xs)                 # ys_u(k) = Cs_u*xs(k)
             û0 .+= u0                          #   û0(k) = u0(k) + ys_u(k)
-            model.f!(k1, xd, û0, d0, p)
-            model.f!(k2, xdnext_Z̃, û0, d0next, p)
+            model.f!(k1, x0, û0, d0, p)
+            model.f!(k2, x0next_Z̃, û0, d0next, p)
         else # piecewise linear manipulated inputs u:
-            if j < Hp 
-                u0next = @views U0[(1 + nu*j):(nu*(j+1))]
-                û0next = @views Û0[(1 + nu*j):(nu*(j+1))]
-            else # u(k+Hp) = u(k+Hp-1) since Hc ≤ Hp implies that Δu(k+Hp)=0
-                u0next = @views U0[(1 + nu*(j-1)):(nu*j)]
-                û0next = @views Û0[(1 + nu*(j-1)):(nu*j)]
-            end
+            k1 .= lastk2
+            j == Hp && break # special case, treated after the loop
+            u0next = @views U0[(1 + nu*j):(nu*(j+1))]
+            û0next = @views Û0[(1 + nu*j):(nu*(j+1))]
             mul!(û0next, Cs_u, xsnext_Z̃)      # ys_u(k+1) = Cs_u*xs(k+1)
             û0next .+= u0next                 #   û0(k+1) = u0(k+1) + ys_u(k+1)
-            k1 .= lastk2
-            model.f!(k2, xdnext_Z̃, û0next, d0next, p)
+            model.f!(k2, x0next_Z̃, û0next, d0next, p)
             lastk2 = k2
-        end 
-        xsnext = @views x̂0next[nx+1:end]
-        mul!(xsnext, As, xs)
-        sdnext .= @. xd - xdnext_Z̃ + (Ts/2)*(k1 + k2)
-        ssnext .= @. xsnext - xsnext_Z̃
+        end
+        sdnext .= @. x0 - x0next_Z̃ + 0.5*Ts*(k1 + k2)
         x̂0 = x̂0next_Z̃ # using states in Z̃ for next iteration (allow parallel for)
         d0 = d0next
     end
+    if !iszero(nh)
+        # j = Hp special case: u(k+Hp-1) = u(k+Hp) since Hc ≤ Hp implies Δu(k+Hp)=0
+        x̂0, x̂0next_Z̃   = @views X̂0_Z̃[end-2nx̂+1:end-nx̂], X̂0_Z̃[end-nx̂+1:end]
+        k1, k2         = @views K0[end-2nx+1:end-nx], K0[end-nx+1:end] # k1 already filled
+        d0next         = @views D̂0[end-nd+1:end]
+        û0next, u0next = @views Û0[end-nu+1:end], U0[end-nu+1:end] # correspond to u(k+Hp-1)
+        x0, x0next_Z̃, xsnext_Z̃ = @views x̂0[1:nx], x̂0next_Z̃[1:nx], x̂0next_Z̃[nx+1:end]
+        sdnext = @views geq[end-nx̂+1:end-nx̂+nx] # ssnext already filled
+        mul!(û0next, Cs_u, xsnext_Z̃)                 
+        û0next .+= u0next                          
+        model.f!(k2, x0next_Z̃, û0next, d0next, p)
+        sdnext .= @. x0 - x0next_Z̃ + (Ts/2)*(k1 + k2)
+    end
     return geq
 end