changed: keep lastu0 in MHE since needed when direct==true

`preparestate!` add the new data in the windows and solve the optimization problem in this case. Since the function do not receive `u0`, it needs to be stored at the last time step in `update_estimate!`

Author: franckgaga

src/controller/execute.jl

@@ -556,6 +556,7 @@ function setmodel!(
         Ñ_Hc      = Ntilde_Hc,
         kwargs...
     )
+    uop_old = copy(mpc.estim.model.uop)
     x̂op_old = copy(mpc.estim.x̂op)
     nu, ny, Hp, Hc, nϵ = model.nu, model.ny, mpc.Hp, mpc.Hc, mpc.nϵ
     setmodel!(mpc.estim, model; kwargs...)
@@ -601,12 +602,12 @@ function setmodel!(
         mpc.weights.L_Hp .= L_Hp
         mpc.weights.iszero_L_Hp[] = iszero(mpc.weights.L_Hp)
     end
-    setmodel_controller!(mpc, x̂op_old)
+    setmodel_controller!(mpc, uop_old, x̂op_old)
     return mpc
 end
 
 "Update the prediction matrices, linear constraints and JuMP optimization."
-function setmodel_controller!(mpc::PredictiveController, x̂op_old)
+function setmodel_controller!(mpc::PredictiveController, uop_old, x̂op_old)
     model, estim, transcription = mpc.estim.model, mpc.estim, mpc.transcription
     nu, ny, nd, Hp, Hc = model.nu, model.ny, model.nd, mpc.Hp, mpc.Hc
     optim, con = mpc.optim, mpc.con
@@ -636,6 +637,7 @@ function setmodel_controller!(mpc::PredictiveController, x̂op_old)
     con.x̂0min .+= x̂op_old # convert x̂0 to x̂ with the old operating point
     con.x̂0max .+= x̂op_old # convert x̂0 to x̂ with the old operating point
     # --- operating points ---
+    mpc.lastu0 .+= uop_old .- model.uop
     for i in 0:Hp-1
         mpc.Uop[(1+nu*i):(nu+nu*i)] .= model.uop
         mpc.Yop[(1+ny*i):(ny+ny*i)] .= model.yop

src/estimator/execute.jl

@@ -403,7 +403,6 @@ function setmodel!(
     yop_old = copy(estim.model.yop)
     dop_old = copy(estim.model.dop)
     setmodel_linmodel!(estim.model, model)
-    estim.lastu0 .+= uop_old .- model.uop
     setmodel_estimator!(estim, model, uop_old, yop_old, dop_old, Q̂, R̂)
     return estim
 end

src/estimator/mhe/execute.jl

@@ -43,13 +43,15 @@ end
     
 Update [`MovingHorizonEstimator`](@ref) state `estim.x̂0`.
 
-The optimization problem of [`MovingHorizonEstimator`](@ref) documentation is solved at
-each discrete time ``k``. The prediction matrices are provided at [`init_predmat_mhe`](@ref)
-documentation. Once solved, the optimal estimate ``\mathbf{x̂}_k(k+p)`` is computed by 
-inserting the optimal values of ``\mathbf{x̂}_k(k-N_k+p)`` and ``\mathbf{Ŵ}`` in the
-augmented model from ``j = N_k-1`` to ``0`` inclusively. Afterward, if ``N_k = H_e``, the
-arrival covariance for the next time step ``\mathbf{P̂}_{k-N_k}(k-N_k+1)`` is estimated using 
-`estim.covestim` object.
+The optimization problem of [`MovingHorizonEstimator`](@ref) documentation is solved if
+`estim.direct` is `false` (otherwise solved in [`correct_estimate!`](@ref)). The prediction
+matrices are provided at [`init_predmat_mhe`](@ref) documentation. Once solved, the optimal
+estimate ``\mathbf{x̂}_k(k+p)`` is computed by inserting the optimal values of 
+``\mathbf{x̂}_k(k-N_k+p)`` and ``\mathbf{Ŵ}`` in the augmented model from ``j = N_k-1`` to
+``0`` inclusively. Afterward, if ``N_k = H_e``, the arrival covariance for the next time
+step ``\mathbf{P̂}_{k-N_k}(k-N_k+1)`` is estimated using `estim.covestim` object. It
+also stores `u0` at `estim.lastu0`, so it can be added to the data window at the next time
+step in [`correct_estimate!`](@ref).
 """
 function update_estimate!(estim::MovingHorizonEstimator, y0m, d0, u0)
     if !estim.direct
@@ -59,6 +61,7 @@ function update_estimate!(estim::MovingHorizonEstimator, y0m, d0, u0)
         optim_objective!(estim)
     end
     (estim.Nk[] == estim.He) && update_cov!(estim)
+    estim.lastu0 .= u0
     return nothing
 end
 
@@ -779,8 +782,10 @@ function setmodel_estimator!(
         # convert d to d0 with the new operating point:
         estim.D0[(1+nd*(i-1)):(nd*i)]    .-= model.dop
     end
+    estim.lastu0        .+= uop_old
     estim.Z̃[nϵ+1:nϵ+nx̂] .+= x̂op_old
     estim.x̂0arr_old     .+= x̂op_old
+    estim.lastu0        .-= model.uop
     estim.Z̃[nϵ+1:nϵ+nx̂] .-= x̂op
     estim.x̂0arr_old     .-= x̂op
     # --- covariance matrices ---