Merge pull request #243 from JuliaControl/tc_foh

added: piecewise linear manipulated inputs with `TrapezoidalCollocation(1)`

Author: franckgaga

src/controller/transcription.jl

@@ -64,14 +64,16 @@ for this transcription method.
 struct MultipleShooting <: ShootingMethod end
 
 @doc raw"""
-    TrapezoidalCollocation()
+    TrapezoidalCollocation(h::Int=0)
 
-Construct an implicit trapezoidal [`TranscriptionMethod`](@ref).
+Construct an implicit trapezoidal [`TranscriptionMethod`](@ref) with `h`th order hold.
 
 This is the simplest collocation method. It supports continuous-time [`NonLinModel`](@ref)s
 only. The decision variables are the same as for [`MultipleShooting`](@ref), hence similar
-computational costs. It currently assumes piecewise constant manipulated inputs (or zero-
-order hold) between the samples, but linear interpolation will be added soon.
+computational costs. The `h` argument is `0` or `1`, for piecewise constant or linear
+manipulated inputs ``\mathbf{u}`` (`h=1` is slightly less expensive). Note that the various
+[`DiffSolver`](@ref) here assume zero-order hold, so `h=1` will induce a plant-model 
+mismatch if the plant is simulated with these solvers.
 
 This transcription computes the predictions by calling the continuous-time model in the
 equality constraint function and by using the implicit trapezoidal rule. It can handle
@@ -92,10 +94,14 @@ transcription method.
     for more details.
 """
 struct TrapezoidalCollocation <: CollocationMethod
+    h::Int
     nc::Int
-    function TrapezoidalCollocation() 
+    function TrapezoidalCollocation(h::Int=0)
+        if !(h == 0 || h == 1)
+            throw(ArgumentError("h argument must be 0 or 1 for TrapezoidalCollocation."))
+        end
         nc = 2 # 2 collocation points per interval for trapezoidal rule
-        return new(nc)
+        return new(h, nc)
     end
 end
 
@@ -1376,11 +1382,11 @@ extracted from the decision variables `Z̃`. The ``\mathbf{k}`` coefficients are
 evaluated from the continuous-time function `model.f!` and:
 ```math
 \begin{aligned}
-\mathbf{k}_1 &= \mathbf{f}\Big(\mathbf{x_0}(k),   \mathbf{û_0}(k), \mathbf{d_0}(k)  \Big) \\
-\mathbf{k}_2 &= \mathbf{f}\Big(\mathbf{x_0}(k+1), \mathbf{û_0}(k), \mathbf{d_0}(k+1)\Big) 
+\mathbf{k}_1 &= \mathbf{f}\Big(\mathbf{x_0}(k),   \mathbf{û_0}(k),   \mathbf{d_0}(k)  \Big) \\
+\mathbf{k}_2 &= \mathbf{f}\Big(\mathbf{x_0}(k+1), \mathbf{û_0}(k+h), \mathbf{d_0}(k+1)\Big) 
 \end{aligned}
 ```
-and the input of the augmented model is:
+in which ``h`` is the hold order `transcription.h` and the disturbed input is:
 ```math
 \mathbf{û_0}(k) = \mathbf{u_0}(k) + \mathbf{C_{s_u} x_s}(k)
 ``` 
@@ -1391,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, h = model.nu, mpc.estim.nx̂, model.nd, model.nx, transcription.h
     Hp, Hc = mpc.Hp, mpc.Hc
     nΔU, nX̂ = nu*Hc, nx̂*Hp
     Ts, p = model.Ts, model.p
@@ -1401,30 +1407,64 @@ 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(h)
+        k1, u0, û0 = @views K0[1:nx], U0[1:nu], Û0[1:nu]
+        x0, xs     = @views x̂0[1:nx], x̂0[nx+1:end]
+        mul!(û0, Cs_u, xs)                 
+        û0 .+= u0                          
+        model.f!(k1, x0, û0, d0, p)
+        lastk2 = k1
+    end
     #TODO: allow parallel for loop or threads? 
     for j=1:Hp
-        u0       = @views   U0[(1 + nu*(j-1)):(nu*j)]
-        û0       = @views   Û0[(1 + nu*(j-1)):(nu*j)]
         k0       = @views   K0[(1 + nk*(j-1)):(nk*j)]
         d0next   = @views   D̂0[(1 + nd*(j-1)):(nd*j)]
         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]
-        mul!(û0, Cs_u, xs)      # ys_u = Cs_u*xs
-        û0 .+= u0               # û0 = u0 + ys_u
-        k1, k2 = @views k0[1:nx], k0[nx+1:2*nx]
-        model.f!(k1, xd, û0, d0, p)
-        model.f!(k2, xdnext_Z̃, û0, d0next, p) # assuming ZOH on 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)
-        sdnext .= @. xd - xdnext_Z̃ + (Ts/2)*(k1 + k2)
         ssnext .= @. xsnext - xsnext_Z̃
+        # ----------------- deterministic defects --------------------------------------
+        if iszero(h) # 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, x0, û0, d0, p)
+            model.f!(k2, x0next_Z̃, û0, d0next, p)
+        else # piecewise linear manipulated inputs u:
+            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)
+            model.f!(k2, x0next_Z̃, û0next, d0next, p)
+            lastk2 = k2
+        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(h)
+        # 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
 

test/3_test_predictive_control.jl

@@ -728,6 +728,7 @@ end
     @test_throws ErrorException NonLinMPC(nonlinmodel, Hp=15, gc  = (_,_,_,_)->[0.0], nc=1)
     @test_throws ErrorException NonLinMPC(nonlinmodel, Hp=15, gc! = (_,_,_,_)->[0.0], nc=1)
     @test_throws ArgumentError NonLinMPC(nonlinmodel, transcription=TrapezoidalCollocation())
+    @test_throws ArgumentError NonLinMPC(nonlinmodel, transcription=TrapezoidalCollocation(2))
 
     @test_logs (:warn, Regex(".*")) NonLinMPC(nonlinmodel, Hp=15, JE=(Ue,_,_,_)->Ue)
     @test_logs (:warn, Regex(".*")) NonLinMPC(nonlinmodel, Hp=15, gc=(Ue,_,_,_,_)->Ue, nc=0)    
@@ -806,6 +807,10 @@ end
     preparestate!(nmpc5, [0.0])
     u = moveinput!(nmpc5, [1/0.001])
     @test u ≈ [1.0] atol=5e-2
+    nmpc5_1 = NonLinMPC(nonlinmodel_c, Nwt=[0], Hp=100, Hc=1, transcription=TrapezoidalCollocation(1))
+    preparestate!(nmpc5_1, [0.0])
+    u = moveinput!(nmpc5_1, [1/0.001])
+    @test u ≈ [1.0] atol=5e-2
     nmpc6  = NonLinMPC(linmodel3, Hp=10)
     preparestate!(nmpc6, [0])
     @test moveinput!(nmpc6, [0]) ≈ [0.0] atol=5e-2