debug: separate SimModel and StateEstimator buffers

Author: franckgaga

src/controller/execute.jl

@@ -3,7 +3,7 @@
 
 Init the states of `mpc.estim` [`StateEstimator`](@ref) and warm start `mpc.ΔŨ` at zero.
 """
-function initstate!(mpc::PredictiveController, u, ym, d=mpc.estim.model.buffer.empty)
+function initstate!(mpc::PredictiveController, u, ym, d=mpc.estim.buffer.empty)
     mpc.ΔŨ .= 0
     return initstate!(mpc.estim, u, ym, d)
 end
@@ -51,7 +51,7 @@ julia> ry = [5]; u = moveinput!(mpc, ry); round.(u, digits=3)
 function moveinput!(
     mpc::PredictiveController, 
     ry::Vector = mpc.estim.model.yop, 
-    d ::Vector = mpc.estim.model.buffer.empty;
+    d ::Vector = mpc.estim.buffer.empty;
     Dhat ::Vector = repeat(d,  mpc.Hp),
     Rhaty::Vector = repeat(ry, mpc.Hp),
     Rhatu::Vector = mpc.Uop,
@@ -509,7 +509,7 @@ set_objective_linear_coef!(::PredictiveController, _ ) = nothing
 
 Call [`updatestate!`](@ref) on `mpc.estim` [`StateEstimator`](@ref).
 """
-function updatestate!(mpc::PredictiveController, u, ym, d=mpc.estim.model.buffer.empty)
+function updatestate!(mpc::PredictiveController, u, ym, d=mpc.estim.buffer.empty)
     return updatestate!(mpc.estim, u, ym, d)
 end
 updatestate!(::PredictiveController, _ ) = throw(ArgumentError("missing measured outputs ym"))

src/estimator/execute.jl

@@ -113,7 +113,7 @@ julia> evaloutput(estim) ≈ y
 true
 ```
 """
-function initstate!(estim::StateEstimator, u, ym, d=estim.model.buffer.empty)
+function initstate!(estim::StateEstimator, u, ym, d=estim.buffer.empty)
     # --- validate arguments ---
     validate_args(estim, u, ym, d)
     # --- init state estimate ----
@@ -179,18 +179,18 @@ julia> ŷ = evaloutput(kf)
  20.0
 ```
 """
-function evaloutput(estim::StateEstimator{NT}, d=estim.model.buffer.empty) where NT <: Real
+function evaloutput(estim::StateEstimator{NT}, d=estim.buffer.empty) where NT <: Real
     validate_args(estim.model, d)
-    ŷ0, d0 = estim.model.buffer.y, estim.model.buffer.d
+    ŷ0, d0 = estim.buffer.ŷ, estim.buffer.d
     d0 .= d .- estim.model.dop
     ĥ!(ŷ0, estim, estim.model, estim.x̂0, d0)
-    ŷ  = estim.model.buffer.y
-    ŷ .= ŷ0 .+ estim.model.yop
+    ŷ   = ŷ0
+    ŷ .+= estim.model.yop
     return ŷ
 end
 
 "Functor allowing callable `StateEstimator` object as an alias for `evaloutput`."
-(estim::StateEstimator)(d=estim.model.buffer.empty) = evaloutput(estim, d)
+(estim::StateEstimator)(d=estim.buffer.empty) = evaloutput(estim, d)
 
 @doc raw"""
     updatestate!(estim::StateEstimator, u, ym, d=[]) -> x̂
@@ -210,10 +210,10 @@ julia> x̂ = updatestate!(kf, [1], [0]) # x̂[2] is the integrator state (nint_y
  0.0
 ```
 """
-function updatestate!(estim::StateEstimator, u, ym, d=estim.model.buffer.empty)
+function updatestate!(estim::StateEstimator, u, ym, d=estim.buffer.empty)
     validate_args(estim, u, ym, d)
     u0, ym0, d0 = remove_op!(estim, u, ym, d)
-    x̂0next = update_estimate!(estim, u0, ym0, d0)
+    x̂0next  = update_estimate!(estim, u0, ym0, d0)
     x̂next   = x̂0next
     x̂next .+= estim.x̂op
     return x̂next

src/predictive_control.jl

@@ -42,7 +42,7 @@ end
 "Functor allowing callable `PredictiveController` object as an alias for `moveinput!`."
 function (mpc::PredictiveController)(
     ry::Vector = mpc.estim.model.yop, 
-    d ::Vector = mpc.estim.model.buffer.empty;
+    d ::Vector = mpc.estim.buffer.empty;
     kwargs...
 )
     return moveinput!(mpc, ry, d; kwargs...)

src/sim_model.jl

@@ -241,8 +241,8 @@ function updatestate!(model::SimModel{NT}, u, d=model.buffer.empty) where NT <:
     f!(xnext0, model, model.x0, u0, d0)
     xnext0  .+= model.fop .- model.xop
     model.x0 .= xnext0
-    xnext  = model.buffer.x
-    xnext .= xnext0 .+ model.xop
+    xnext   = xnext0
+    xnext .+= model.xop
     return xnext
 end
 
@@ -268,8 +268,8 @@ function evaloutput(model::SimModel{NT}, d=model.buffer.empty) where NT <: Real
     d0 .= d .- model.dop
     y0 = model.buffer.y
     h!(y0, model, model.x0, d0)
-    y  = model.buffer.y
-    y .= y0 .+ model.yop
+    y   = y0
+    y .+= model.yop
     return y
 end