debug NonLinMPC tests

Author: franckgaga

src/controller/execute.jl

@@ -226,7 +226,9 @@ function predictstoch!(
     isnothing(ym) && error("Predictive controllers with InternalModel need the measured "*
                            "outputs ym in keyword argument to compute control actions u")
     Ŷop, ny, yop = mpc.Ŷop, estim.model.ny, estim.model.yop
-    ŷd = h(estim.model, estim.x̂d, d - estim.model.dop) .+ estim.model.yop 
+    ŷd = similar(estim.model.yop)
+    h!(ŷd, estim.model, estim.x̂d, d - estim.model.dop)
+    ŷd .+= estim.model.yop 
     ŷs = zeros(NT, estim.model.ny)
     ŷs[estim.i_ym] .= @views ym .- ŷd[estim.i_ym]  # ŷs=0 for unmeasured outputs
     Ŷop_LHS = similar(Ŷop)

src/estimator/internal_model.jl

@@ -272,7 +272,8 @@ estimate its outputs ``\mathbf{ŷ}(k)``, since the strategy imposes that
 """
 function evalŷ(estim::InternalModel{NT}, ym, d) where NT<:Real
     ŷ = Vector{NT}(undef, estim.model.ny)
-    ŷ = h!(ŷ, estim.model, estim.x̂d, d - estim.model.dop) .+ estim.model.yop
+    h!(ŷ, estim.model, estim.x̂d, d - estim.model.dop) 
+    ŷ .+= estim.model.yop
     ŷ[estim.i_ym] = ym
     return ŷ
 end

src/model/nonlinmodel.jl

@@ -23,7 +23,8 @@ struct NonLinModel{NT<:Real, F<:Function, H<:Function} <: SimModel{NT}
 end
 
 @doc raw"""
-    NonLinModel{NT}(f::Function, h::Function, Ts, nu, nx, ny, nd=0)
+    NonLinModel{NT}( f::Function,  h::Function, Ts, nu, nx, ny, nd=0)
+    NonLinModel{NT}(f!::Function, h!::Function, Ts, nu, nx, ny, nd=0)
 
 Construct a nonlinear model from discrete-time state-space functions `f` and `h`.
 
@@ -34,6 +35,14 @@ The state update ``\mathbf{f}`` and output ``\mathbf{h}`` functions are defined
     \mathbf{y}(k)   &= \mathbf{h}\Big( \mathbf{x}(k), \mathbf{d}(k) \Big)
     \end{aligned}
 ```
+They can be specified in two forms:
+
+- non-mutating functions (out-of-place): they must be defined as `f(x, u, d) -> xnext` and
+  `h(x, d) -> y`
+- mutating functions (in-place): they must be defined as `f!(xnext, x, u, d) -> nothing` and
+  `h!(y, x, d) -> nothing`. This syntax reduces the allocations and potentially the 
+  computational burden as well.
+  
 `Ts` is the sampling time in second. `nu`, `nx`, `ny` and `nd` are the respective number of 
 manipulated inputs, states, outputs and measured disturbances. The optional parameter `NT`
 explicitly specifies the number type of vectors (default to `Float64`).

test/test_predictive_control.jl

@@ -462,11 +462,12 @@ end
     @test ForwardDiff.gradient(g_Ymax_end, [1.0, 1.0]) ≈ [0.0, 0.0]
     linmodel3 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), zeros(1,0), zeros(1,0), 1.0)
     nmpc6  = NonLinMPC(linmodel3, Hp=10)
-    moveinput!(nmpc6, [0]) ≈ [0.0]
+    @test moveinput!(nmpc6, [0]) ≈ [0.0]
     nonlinmodel2 = NonLinModel{Float32}(f, h, 3000.0, 1, 2, 1, 1)
     nmpc7  = NonLinMPC(nonlinmodel2, Hp=10)
-    nonlinmodel2.h(Float32[0,0], Float32[0])
-    moveinput!(nmpc7, [0], [0]) ≈ [0.0]
+    y = similar(nonlinmodel2.yop)
+    nonlinmodel2.h!(y, Float32[0,0], Float32[0])
+    @test moveinput!(nmpc7, [0], [0]) ≈ [0.0]
 end
 
 @testset "NonLinMPC step disturbance rejection" begin

test/test_sim_model.jl

@@ -112,8 +112,11 @@ end
     @test nonlinmodel1.nu == 2
     @test nonlinmodel1.nd == 0
     @test nonlinmodel1.ny == 2
-    @test nonlinmodel1.f!([0,0],[0,0],[0,0],[1]) ≈ zeros(2,)
-    @test nonlinmodel1.h!([0,0],[0,0],[1]) ≈ zeros(2,)
+    xnext, y = similar(nonlinmodel1.x), similar(nonlinmodel1.yop)
+    nonlinmodel1.f!(xnext,[0,0],[0,0],[1])
+    @test xnext ≈ zeros(2,)
+    nonlinmodel1.h!(y,[0,0],[1])
+    @test y ≈ zeros(2,)
 
     linmodel2 = LinModel(sys,Ts,i_d=[3])
     f2(x,u,d) = linmodel2.A*x + linmodel2.Bu*u + linmodel2.Bd*d
@@ -124,8 +127,11 @@ end
     @test nonlinmodel2.nu == 2
     @test nonlinmodel2.nd == 1
     @test nonlinmodel2.ny == 2
-    @test nonlinmodel2.f!([0,0,0,0],[0,0,0,0],[0,0],[0]) ≈ zeros(4,)
-    @test nonlinmodel2.h!([0,0],[0,0,0,0],[0]) ≈ zeros(2,)
+    xnext, y = similar(nonlinmodel2.x), similar(nonlinmodel2.yop)
+    nonlinmodel2.f!(xnext,[0,0,0,0],[0,0],[0])
+    @test xnext ≈ zeros(4,)
+    nonlinmodel2.h!(y,[0,0,0,0],[0])
+    @test y ≈ zeros(2,)
 
     nonlinemodel3 = NonLinModel{Float32}(f2,h2,Ts,2,4,2,1)
     @test isa(nonlinemodel3, NonLinModel{Float32})