sim function for PredictiveController is working

Author: franckgaga

Project.toml

@@ -11,6 +11,7 @@ LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
 OSQP = "ab2f91bb-94b4-55e3-9ba0-7f65df51de79"
 PreallocationTools = "d236fae5-4411-538c-8e31-a6e3d9e00b46"
 PrecompileTools = "aea7be01-6a6a-4083-8856-8a6e6704d82a"
+RecipesBase = "3cdcf5f2-1ef4-517c-9805-6587b60abb01"
 
 [compat]
 ControlSystemsBase = "1"

example/juMPC.jl

@@ -149,6 +149,8 @@ end
 test_mpc(linModel4, mpc)
 @time u_data, y_data, r_data, d_data = test_mpc(linModel4, mpc)
 
+res =  sim(mpc)
+
 test_mpc(linModel4, nmpc)
 @time u_data, y_data, r_data, d_data = test_mpc(linModel4, nmpc)
 

src/ModelPredictiveControl.jl

@@ -13,8 +13,9 @@ export SimModel, LinModel, NonLinModel, setop!, setstate!, updatestate!, evalout
 export StateEstimator, InternalModel
 export SteadyKalmanFilter, KalmanFilter, UnscentedKalmanFilter
 export initstate!
-export PredictiveController, LinMPC, NonLinMPC, setconstraint!, moveinput!, getinfo
+export PredictiveController, LinMPC, NonLinMPC, setconstraint!, moveinput!, getinfo, sim
 
+include("plots.jl")
 include("sim_model.jl")
 include("state_estim.jl")
 include("predictive_control.jl")

src/plots.jl

@@ -0,0 +1,20 @@
+struct SimResult
+    T_data ::Vector{Float64}
+    Y_data ::Matrix{Float64}
+    Ry_data::Matrix{Float64}
+    Ŷ_data ::Matrix{Float64}
+    U_data ::Matrix{Float64}
+    Ru_data::Matrix{Float64}
+    D_data ::Matrix{Float64}
+    X_data ::Matrix{Float64}
+    X̂_data ::Matrix{Float64}
+end
+
+#=
+@recipe function f(res::SimResult)
+    y = map(xi -> pdf(dist,xi), x)
+    seriestype --> :path  # there is always an attribute dictionary `d` available...
+                          # If the user didn't specify a seriestype, we choose :path
+    return x, y
+end
+=#

src/predictive_control.jl

@@ -333,18 +333,46 @@ function sim(
 )
     model = mpc.estim.model
     model.Ts ≈ plant.Ts || error("Sampling time Ts of mpc and plant must be equal")
+    T_data  = collect(plant.Ts*(0:(N-1)))
+    Y_data  = Matrix{Float64}(undef, plant.ny, N)
+    Ŷ_data  = Matrix{Float64}(undef, model.ny, N)
+    Ry_data = Matrix{Float64}(undef, plant.ny, N)
+    U_data  = Matrix{Float64}(undef, plant.nu, N)
+    Ru_data = Matrix{Float64}(undef, plant.nu, N)
+    D_data  = Matrix{Float64}(undef, plant.nd, N)
+    X_data  = Matrix{Float64}(undef, plant.nx, N) 
+    X̂_data  = Matrix{Float64}(undef, mpc.estim.nx̂, N) 
     setstate!(plant, x0)
     if isnothing(x̂0)
-        initstate!(mpc, lastu, plant(), d)
+        initstate!(mpc, lastu, plant(d), d)
+    else
+        setstate!(mpc, x̂0)
     end
-    ry_data = Matrix{Float64}(undef, plant.ny, N)
-    u_data  = Matrix{Float64}(undef, plant.nu, N)
-    y_data  = Matrix{Float64}(undef, plant.ny, N)
-    d_data  = Matrix{Float64}(undef, plant.nd, N)
-    t_data  = plant.Ts*(0:(N-1))
+    lastd = d
+    ru = !isempty(mpc.R̂u) ? mpc.R̂u[:, begin] : fill(NaN, plant.nu)
+    x = plant.x
+    x̂ = mpc.estim.x̂
     for i=1:N
+        d = lastd + d_step + d_noise.*randn(plant.nd)
+        y = plant(d) + y_step + y_noise.*randn(plant.ny)
+        ym = y[mpc.estim.i_ym]
+        u  = moveinput!(mpc, ry, d; ym)
+        up = u + u_step + u_noise.*randn(plant.nu)
+        Y_data[:, i] = y
+        Ŷ_data[:, i] = mpc.ŷ
+        Ry_data[:, i] = ry
+        U_data[:, i] = u
+        Ru_data[:, i] = ru
+        D_data[:, i] = d
+        X_data[:, i] = x
+        X̂_data[:, i] = x̂
+        x = updatestate!(plant, up, d)
+        x̂ = updatestate!(mpc, u, ym, d)
     end
-    return nothing
+    res = SimResult(
+        T_data, Y_data, Ry_data, Ŷ_data, U_data, Ru_data, D_data, X_data, X̂_data
+    )
+    return res
 end