combine plant and estimator states in plot if same model

Author: franckgaga

README.md

@@ -28,9 +28,6 @@ using Pkg; Pkg.add("ModelPredictiveControl")
 ### Model Predictive Control Features
 
 - ✅ linear and nonlinear plant models exploiting multiple dispatch
-- ✅ model predictive controllers based on:
-  - ✅ linear plant models
-  - ✅ nonlinear plant models
 - ✅ supported objective function terms:
   - ✅ output setpoint tracking
   - ✅ move suppression

docs/src/index.md

@@ -33,9 +33,6 @@ Pages = [
 ### Model Predictive Control Features
 
 - ✅ linear and nonlinear plant models exploiting multiple dispatch
-- ✅ model predictive controllers based on:
-  - ✅ linear plant models
-  - ✅ nonlinear plant models
 - ✅ supported objective function terms:
   - ✅ output setpoint tracking
   - ✅ move suppression

example/juMPC.jl

@@ -159,13 +159,18 @@ psM  = plot(resM)
 display(psM)
 
 res = sim!(mpc, mpc.Hp+10)
-ps = plot(res)
+ps = plot(res, plotx=true)
 display(ps)
 
 res2 = sim!(uscKalmanFilter1, mpc.Hp+10)
 ps2 = plot(res2)
 display(ps2)
 
+res2 = sim!(uscKalmanFilter1, mpc.Hp+10, plant=deepcopy(uscKalmanFilter1.model))
+ps2 = plot(res2, plotx=true, plotx̂=true)
+display(ps2)
+
+
 #=
 test_mpc(linModel4, nmpc)
 @time u_data, y_data, r_data, d_data = test_mpc(linModel4, nmpc)

src/plot_sim.jl

@@ -1,15 +1,16 @@
 "Includes all signals of [`sim!`](@ref), view them with `plot` on `SimResult` instances."
 struct SimResult{O<:Union{SimModel, StateEstimator, PredictiveController}}
-    T_data ::Vector{Float64} # time in seconds
-    Y_data ::Matrix{Float64} # plant outputs (both measured and unmeasured)
-    Ry_data::Matrix{Float64} # output setpoints
-    Ŷ_data ::Matrix{Float64} # estimated outputs
-    U_data ::Matrix{Float64} # manipulated inputs
-    Ud_data::Matrix{Float64} # manipulated inputs including load disturbances
-    D_data ::Matrix{Float64} # measured disturbances
-    X_data ::Matrix{Float64} # plant states
-    X̂_data ::Matrix{Float64} # estimated states
-    obj    ::O               # simulated instance
+    T_data ::Vector{Float64}    # time in seconds
+    Y_data ::Matrix{Float64}    # plant outputs (both measured and unmeasured)
+    Ry_data::Matrix{Float64}    # output setpoints
+    Ŷ_data ::Matrix{Float64}    # estimated outputs
+    U_data ::Matrix{Float64}    # manipulated inputs
+    Ud_data::Matrix{Float64}    # manipulated inputs including load disturbances
+    D_data ::Matrix{Float64}    # measured disturbances
+    X_data ::Matrix{Float64}    # plant states
+    X̂_data ::Matrix{Float64}    # estimated states
+    plantIsModel::Bool          # true if simulated plant is identical to estimation model
+    obj::O                      # simulated instance
 end
 
 @doc raw"""
@@ -55,7 +56,7 @@ function sim!(
         updatestate!(plant, u, d); 
     end
     return SimResult(
-        T_data, Y_data, U_data, Y_data, U_data, U_data, D_data, X_data, X_data, plant
+        T_data, Y_data, U_data, Y_data, U_data, U_data, D_data, X_data, X_data, true, plant
     )
 end
 
@@ -96,7 +97,7 @@ vectors. The simulated sensor and process noises of `plant` are specified by `y_
 ```julia-repl
 julia> model = LinModel(tf(3, [30, 1]), 0.5);
 
-julia> estim = SteadyKalmanFilter(model, σR=[0.5], σQ=[0.25], σQ_int=[0.01]);
+julia> estim = KalmanFilter(model, σR=[0.5], σQ=[0.25], σQ_int=[0.01], σP0_int=[0.1]);
 
 julia> res = sim!(estim, 50, [0], y_noise=[0.5], x_noise=[0.25], x0=[-10], x̂0=[0, 0]);
 
@@ -170,6 +171,7 @@ function sim_closedloop!(
     lastu = plant.uop,
 )
     model = estim.model
+    plantIsModel = (plant === model)
     model.Ts ≈ plant.Ts || error("Sampling time of controller/estimator ≠ plant.Ts")
     old_x0 = copy(plant.x)
     T_data  = collect(plant.Ts*(0:(N-1)))
@@ -207,7 +209,9 @@ function sim_closedloop!(
         updatestate!(est_mpc, u, ym, d)
     end
     res = SimResult(
-        T_data, Y_data, U_Ry_data, Ŷ_data, U_data, Ud_data, D_data, X_data, X̂_data, est_mpc
+        T_data, Y_data, U_Ry_data, Ŷ_data, 
+        U_data, Ud_data, D_data, X_data, X̂_data, 
+        plantIsModel, est_mpc
     )
     setstate!(plant, old_x0)
     return res
@@ -304,6 +308,7 @@ end
     plotx̂           = false
 )
     t   = res.T_data
+    plantIsModel = res.plantIsModel
     ny = size(res.Y_data, 1)
     nu = size(res.U_data, 1)
     nd = size(res.D_data, 1)
@@ -312,8 +317,14 @@ end
     layout_mat = [(ny, 1)]
     plotu && (layout_mat = [layout_mat (nu, 1)])
     (plotd && nd ≠ 0) && (layout_mat = [layout_mat (nd, 1)])
-    plotx && (layout_mat = [layout_mat (nx, 1)])
-    plotx̂ && (layout_mat = [layout_mat (nx̂, 1)])
+    if plantIsModel && plotx && !plotx̂
+        layout_mat = [layout_mat (nx, 1)]
+    elseif plantIsModel && plotx̂
+        layout_mat = [layout_mat (nx̂, 1)]
+    elseif !plantIsModel
+        plotx && (layout_mat = [layout_mat (nx, 1)])
+        plotx̂ && (layout_mat = [layout_mat (nx̂, 1)])
+    end
     layout := layout_mat
     xguide    --> "Time (s)"
     # --- outputs y ---
@@ -369,6 +380,7 @@ end
                 t, res.D_data[i, :]
             end
         end
+        subplot_base += nd
     end
     # --- plant states x ---
     if plotx
@@ -382,19 +394,20 @@ end
                 t, res.X_data[i, :]
             end
         end
-        subplot_base += nx
+        !plantIsModel && (subplot_base += nx)
     end
     # --- estimated states x̂ ---
     if plotx̂
         for i in 1:nx̂
             @series begin
-                yguide     --> "\$\\hat{x}_$i\$"
+                withPlantState = plantIsModel && plotx && i ≤ nx
+                yguide     --> (withPlantState ? "\$x_$i\$" : "\$\\hat{x}_$i\$")
                 color      --> 2
                 subplot    --> subplot_base + i
                 linestyle --> :dashdot
                 linewidth --> 0.75
                 label      --> "\$\\mathbf{\\hat{x}}\$"
-                legend     --> false
+                legend     --> (withPlantState ? true : false)
                 t, res.X̂_data[i, :]
             end
         end
@@ -417,8 +430,8 @@ end
     plotx̂    = false
 )
     mpc = res.obj
-
     t  = res.T_data
+    plantIsModel = res.plantIsModel
     ny = size(res.Y_data, 1)
     nu = size(res.U_data, 1)
     nd = size(res.D_data, 1)
@@ -427,12 +440,16 @@ end
     layout_mat = [(ny, 1)]
     plotu && (layout_mat = [layout_mat (nu, 1)])
     (plotd && nd ≠ 0) && (layout_mat = [layout_mat (nd, 1)])
-    plotx && (layout_mat = [layout_mat (nx, 1)])
-    plotx̂ && (layout_mat = [layout_mat (nx̂, 1)])
-
+    if plantIsModel && plotx && !plotx̂
+        layout_mat = [layout_mat (nx, 1)]
+    elseif plantIsModel && plotx̂
+        layout_mat = [layout_mat (nx̂, 1)]
+    elseif !plantIsModel
+        plotx && (layout_mat = [layout_mat (nx, 1)])
+        plotx̂ && (layout_mat = [layout_mat (nx̂, 1)])
+    end
     layout := layout_mat
     xguide --> "Time (s)"
-
     # --- outputs y ---
     subplot_base = 0
     for i in 1:ny
@@ -475,7 +492,7 @@ end
                 color     --> 4
                 subplot   --> subplot_base + i
                 linestyle --> :dot
-                linewidth --> 2.0
+                linewidth --> 1.5
                 label     --> "\$\\mathbf{\\hat{y}_{min}}\$"
                 legend    --> true
                 t, fill(mpc.con.Ŷmin[i], length(t))
@@ -487,7 +504,7 @@ end
                 color     --> 5
                 subplot   --> subplot_base + i
                 linestyle --> :dot
-                linewidth --> 2.0
+                linewidth --> 1.5
                 label     --> "\$\\mathbf{\\hat{y}_{max}}\$"
                 legend    --> true
                 t, fill(mpc.con.Ŷmax[i], length(t))
@@ -525,7 +542,7 @@ end
                     color     --> 4
                     subplot   --> subplot_base + i
                     linestyle --> :dot
-                    linewidth --> 2.0
+                    linewidth --> 1.5
                     label     --> "\$\\mathbf{u_{min}}\$"
                     legend    --> true
                     t, fill(mpc.con.Umin[i], length(t))
@@ -537,7 +554,7 @@ end
                     color     --> 5
                     subplot   --> subplot_base + i
                     linestyle --> :dot
-                    linewidth --> 2.0
+                    linewidth --> 1.5
                     label     --> "\$\\mathbf{u_{max}}\$"
                     legend    --> true
                     t, fill(mpc.con.Umax[i], length(t))
@@ -559,6 +576,7 @@ end
                 t, res.D_data[i, :]
             end
         end
+        subplot_base += nd
     end
     # --- plant states x ---
     if plotx
@@ -572,19 +590,20 @@ end
                 t, res.X_data[i, :]
             end
         end
-        subplot_base += nx
+        !plantIsModel && (subplot_base += nx)
     end
     # --- estimated states x̂ ---
     if plotx̂
         for i in 1:nx̂
             @series begin
-                yguide     --> "\$\\hat{x}_$i\$"
+                withPlantState = plantIsModel && plotx && i ≤ nx
+                yguide     --> (withPlantState ? "\$x_$i\$" : "\$\\hat{x}_$i\$")
                 color      --> 2
                 subplot    --> subplot_base + i
                 linestyle --> :dashdot
                 linewidth --> 0.75
                 label      --> "\$\\mathbf{\\hat{x}}\$"
-                legend     --> false
+                legend     --> (withPlantState ? true : false)
                 t, res.X̂_data[i, :]
             end
         end

src/predictive_control.jl

@@ -290,11 +290,11 @@ fields:
 ```jldoctest
 julia> mpc = LinMPC(LinModel(tf(5, [2, 1]), 3), Nwt=[0], Hp=1, Hc=1);
 
-julia> u = moveinput!(mpc, [5]);
+julia> u = moveinput!(mpc, [10]);
 
 julia> info, sol_summary = getinfo(mpc); round.(info[:Ŷ], digits=3)
 1-element Vector{Float64}:
- 5.0
+ 10.0
 ```
 """
 function getinfo(mpc::PredictiveController)