From ed9088ff746aeeba24f254872f739f7b171f1519 Mon Sep 17 00:00:00 2001
From: franckgaga <franckgaga2@gmail.com>
Date: Thu, 13 Apr 2023 16:56:20 -0400
Subject: [PATCH] improve performance with @views for fhat and hhat functions

---
 example/juMPC.jl          |  3 ++-
 src/predictive_control.jl |  2 +-
 src/sim_model.jl          |  8 ++++++--
 src/state_estim.jl        | 17 ++++++++++-------
 4 files changed, 19 insertions(+), 11 deletions(-)

diff --git a/example/juMPC.jl b/example/juMPC.jl
index f352c5e38..0a04af23f 100644
--- a/example/juMPC.jl
+++ b/example/juMPC.jl
@@ -62,6 +62,7 @@ updatestate!(ssKalmanFilter2,[1, 1],[1,1])
 initstate!(ssKalmanFilter1,[0,0],[2,1])
 
 uscKalmanFilter1 = UnscentedKalmanFilter(linModel1)
+
 updatestate!(uscKalmanFilter1,[0,0],[2,1])
 
 initstate!(uscKalmanFilter1,[0,0],[2,1])
@@ -130,7 +131,7 @@ function test_mpc(model, mpc)
 end
 
 @time u_data, y_data, r_data, d_data = test_mpc(linModel4, mpc)
-#@profview u_data, y_data, r_data, d_data = test_mpc(linModel4, mpc)
+@profview u_data, y_data, r_data, d_data = test_mpc(linModel4, mpc)
 #=
 using PlotThemes, Plots
 #theme(:default)
diff --git a/src/predictive_control.jl b/src/predictive_control.jl
index 9b18e23c4..6cfb09d27 100644
--- a/src/predictive_control.jl
+++ b/src/predictive_control.jl
@@ -782,7 +782,7 @@ end
 
 Init `b` vector for the linear model inequality constraints (``\mathbf{A ΔŨ ≤ b}``).
 """
-function init_constraint(mpc, ::LinModel, F, lastu)
+function init_constraint(mpc::C, ::LinModel, F, lastu) where {C<:PredictiveController}
     b = [
         -mpc.Umin + mpc.T_Hc*lastu
         +mpc.Umax - mpc.T_Hc*lastu 
diff --git a/src/sim_model.jl b/src/sim_model.jl
index 2a1e52c4c..6773a401e 100644
--- a/src/sim_model.jl
+++ b/src/sim_model.jl
@@ -161,8 +161,12 @@ function LinModel(
     C   = sys_dis.C
     Dd  = sys_dis.D[:,nu+1:end]
     # the `let` block captures and fixes A, Bu, Bd, C, Dd values (faster computations):
-    f(x, u, d) = A*x + Bu*u + Bd*d
-    h(x, d) = C*x + Dd*d
+    f = let A=A, Bu=Bu, Bd=Bd 
+        (x, u, d) -> A*x + Bu*u + Bd*d
+    end
+    h = let C=C, Dd=Dd
+        (x, d) -> C*x + Dd*d
+    end
     return LinModel_ssfunc(A, Bu, C, Bd, Dd, f, h, Ts, nu, nx, ny, nd)
 end
 
diff --git a/src/state_estim.jl b/src/state_estim.jl
index 24aba1857..32d0cab54 100644
--- a/src/state_estim.jl
+++ b/src/state_estim.jl
@@ -73,7 +73,7 @@ function stoch_ym2y(model::SimModel, i_ym, Asm, Bsm, Csm, Dsm)
 end
 
 @doc raw"""
-    init_estimstoch(model::SimModel, i_ym, nint_ym::Vector{Int})
+    init_estimstoch(i_ym, nint_ym::Vector{Int})
 
 Calc stochastic model matrices from output integrators specifications for state estimation.
 
@@ -150,10 +150,10 @@ end
 @doc raw"""
     f̂(estim::StateEstimator, x̂, u, d)
 
-Update the augmented model state for estimation.
+State function ``\mathbf{f̂}`` of the augmented model.
 
-By introducing an augmented state vector ``\mathbf{x}`` like in [`augment_model`](@ref) doc,
-the ``\mathbf{f̂}`` method updates it from the augmented model, defined as :
+By introducing an augmented state vector ``\mathbf{x}`` like in [`augment_model`](@ref), the
+function returns the next state of the augmented model, defined as:
 ```math
 \begin{aligned}
     \mathbf{x}(k+1) &= \mathbf{f̂}\Big(\mathbf{x}(k), \mathbf{u}(k), \mathbf{d}(k)\Big) \\
@@ -162,8 +162,9 @@ the ``\mathbf{f̂}`` method updates it from the augmented model, defined as :
 ```
 """
 function f̂(estim::E, x̂, u, d) where {E<:StateEstimator}
-    # TODO: consider using views : https://docs.julialang.org/en/v1/manual/performance-tips/#man-performance-views
-    return [estim.model.f(x̂[1:estim.model.nx], u, d); estim.As*x̂[estim.model.nx+1:end]]
+    # `@views` macro avoid copies with matrix slice operator e.g. [a:b]
+    nx = estim.model.nx
+    @views return [estim.model.f(x̂[1:nx], u, d); estim.As*x̂[nx+1:end]]
 end
 
 @doc raw"""
@@ -172,7 +173,9 @@ end
 Output function ``\mathbf{ĥ}`` of the augmented model, see [`f̂`](@ref) for details.
 """
 function ĥ(estim::E, x̂, d) where {E<:StateEstimator}
-    return estim.model.h(x̂[1:estim.model.nx], d) + estim.Cs*x̂[estim.model.nx+1:end]
+    # `@views` macro avoid copies with matrix slice operator e.g. [a:b]
+    nx = estim.model.nx
+    @views return estim.model.h(x̂[1:nx], d) + estim.Cs*x̂[nx+1:end]
 end