JuliaControl · franckgaga · Apr 27, 2024 · Apr 27, 2024 · Apr 27, 2024
diff --git a/docs/src/assets/imc.svg b/docs/src/assets/imc.svg
diff --git a/docs/src/internals/state_estim.md b/docs/src/internals/state_estim.md
@@ -55,9 +55,12 @@ ModelPredictiveControl.init_estimate!
 ## Update Estimate
 
 !!! info
-    All these methods assume that the operating points are already removed in `u`, `ym`
-    and `d` arguments. Strickly speaking, the arguments should be called `u0`, `ym0` and
-    `d0`, following [`setop!`](@ref) notation. The `0` is dropped to simplify the notation.
+    All these methods assume that the `u0`, `y0m` and `d0` arguments are deviation vectors
+    from their respective operating points (see [`setop!`](@ref)). The associated equations
+    in the documentation drops the ``\mathbf{0}`` in subscript to simplify the notation.
+    Strictly speaking, the manipulated inputs, measured outputs, measured disturbances and
+    estimated states should be denoted with ``\mathbf{u_0, y_0^m, d_0}`` and
+    ``\mathbf{x̂_0}``, respectively.
 
 ```@docs
 ModelPredictiveControl.update_estimate!

diff --git a/src/controller/execute.jl b/src/controller/execute.jl
@@ -348,30 +348,33 @@ function predict!(Ŷ0, x̂0, x̂0next, u0, û0, mpc::PredictiveController, mod
 end
 
 """
-    obj_nonlinprog!(U0 , _ , _ , mpc::PredictiveController, model::LinModel, Ŷ0, ΔŨ)
+    obj_nonlinprog!(U0 , Ȳ, _ , mpc::PredictiveController, model::LinModel, Ŷ0, ΔŨ)
 
 Nonlinear programming objective function when `model` is a [`LinModel`](@ref).
 
 The function is called by the nonlinear optimizer of [`NonLinMPC`](@ref) controllers. It can
 also be called on any [`PredictiveController`](@ref)s to evaluate the objective function `J`
-at specific input increments `ΔŨ` and predictions `Ŷ0` values. It mutates the `U0` argument.
+at specific input increments `ΔŨ` and predictions `Ŷ0` values. It mutates the `U0` and
+`Ȳ` arguments.
 """
 function obj_nonlinprog!(
-    U0, _ , _ , mpc::PredictiveController, model::LinModel, Ŷ0, ΔŨ::AbstractVector{NT}
+    U0, Ȳ, _ , mpc::PredictiveController, model::LinModel, Ŷ0, ΔŨ::AbstractVector{NT}
 ) where NT <: Real
     J = obj_quadprog(ΔŨ, mpc.H̃, mpc.q̃) + mpc.p[]
     if !iszero(mpc.E)
-        ny, Hp, D̂E = model.ny, mpc.Hp, mpc.D̂E
-        U  = U0
-        mul!(U, mpc.S̃, ΔŨ)
-        U .+= mpc.T_lastu0 .+ mpc.Uop
-        UE = @views [U; U[(end - model.nu + 1):end]]
-        ŶE = Vector{NT}(undef, ny + ny*Hp)
-        ŶE[1:ny]     .= mpc.ŷ
-        ŶE[1+ny:end] .= Ŷ0 .+ mpc.Yop
-        J += mpc.E*mpc.JE(UE, ŶE, D̂E)
+        ny, Hp, ŷ, D̂E = model.ny, mpc.Hp, mpc.ŷ, mpc.D̂E
+        U = U0
+        U  .+= mpc.Uop
+        uend = @views U[(end-model.nu+1):end]
+        Ŷ  = Ȳ
+        Ŷ .= Ŷ0 .+ mpc.Yop
+        UE = [U; uend]
+        ŶE = [ŷ; Ŷ]
+        E_JE = mpc.E*mpc.JE(UE, ŶE, D̂E)
+    else
+        E_JE = 0.0
     end
-    return J
+    return J + E_JE
 end
 
 """
@@ -403,13 +406,14 @@ function obj_nonlinprog!(
     end
     # --- economic term ---
     if !iszero(mpc.E)
-        ny, Hp, D̂E = model.ny, mpc.Hp, mpc.D̂E
+        ny, Hp, ŷ, D̂E = model.ny, mpc.Hp, mpc.ŷ, mpc.D̂E
         U = U0
-        U .+= mpc.Uop
-        UE = @views [U; U[(end - model.nu + 1):end]]
-        ŶE = Vector{NT}(undef, ny + ny*Hp)
-        ŶE[1:ny]     .= mpc.ŷ
-        ŶE[1+ny:end] .= Ŷ0 .+ mpc.Yop
+        U  .+= mpc.Uop
+        uend = @views U[(end-model.nu+1):end]
+        Ŷ  = Ȳ
+        Ŷ .= Ŷ0 .+ mpc.Yop
+        UE = [U; uend]
+        ŶE = [ŷ; Ŷ]
         E_JE = mpc.E*mpc.JE(UE, ŶE, D̂E)
     else
         E_JE = 0.0

diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl
@@ -245,10 +245,10 @@ Set model and operating points of `estim` [`StateEstimator`](@ref) to `model` va
 
 Allows model adaptation of estimators based on [`LinModel`](@ref) at runtime ([`NonLinModel`](@ref)
 is not supported). Not supported by [`Luenberger`](@ref) and [`SteadyKalmanFilter`](@ref),
-use the time-varying [`KalmanFilter`](@ref) instead.  The [`MovingHorizonEstimator`] model 
-is kept constant over the estimation horizon ``H_e``. The matrix dimensions and sample time
-must stay the same. Note that the observability and controllability of the new augmented
-model is not verified (see Extended Help for details).
+use the time-varying [`KalmanFilter`](@ref) instead.  The [`MovingHorizonEstimator`](@ref)
+model is kept constant over the estimation horizon ``H_e``. The matrix dimensions and sample
+time must stay the same. Note that the observability and controllability of the new
+augmented model is not verified (see Extended Help for more info).
 
 # Examples
 ```jldoctest

diff --git a/src/estimator/internal_model.jl b/src/estimator/internal_model.jl
@@ -56,7 +56,7 @@ Construct an internal model estimator based on `model` ([`LinModel`](@ref) or [`
 unmeasured ``\mathbf{y^u}``. `model` evaluates the deterministic predictions 
 ``\mathbf{ŷ_d}``, and `stoch_ym`, the stochastic predictions of the measured outputs 
 ``\mathbf{ŷ_s^m}`` (the unmeasured ones being ``\mathbf{ŷ_s^u=0}``). The predicted outputs
-sum both values : ``\mathbf{ŷ = ŷ_d + ŷ_s}``.
+sum both values : ``\mathbf{ŷ = ŷ_d + ŷ_s}``. See the Extended Help for more details.
 
 !!! warning
     `InternalModel` estimator does not work if `model` is integrating or unstable. The 
@@ -81,7 +81,10 @@ InternalModel estimator with a sample time Ts = 0.5 s, LinModel and:
     supposes 1 integrator per measured outputs by default, assuming that the current stochastic
     estimate ``\mathbf{ŷ_s^m}(k) = \mathbf{y^m}(k) - \mathbf{ŷ_d^m}(k)`` is constant in the 
     future. This is the dynamic matrix control (DMC) strategy, which is simple but sometimes
-    too aggressive. Additional poles and zeros in `stoch_ym` can mitigate this.
+    too aggressive. Additional poles and zeros in `stoch_ym` can mitigate this. The following
+    block diagram summarizes the internal model structure.
+
+    ![block diagram of the internal model structure](../assets/imc.svg)
 """
 function InternalModel(
     model::SM;
@@ -219,9 +222,9 @@ function setmodel_estimator!(estim::InternalModel, model::LinModel, _ , _ , _)
 end
 
 @doc raw"""
-    update_estimate!(estim::InternalModel, u, ym, d=[])
+    update_estimate!(estim::InternalModel, u0, y0m, d0=[])
 
-Update `estim.x̂0`/`x̂d`/`x̂s` with current inputs `u`, measured outputs `ym` and dist. `d`.
+Update `estim.x̂0`/`x̂d`/`x̂s` with current inputs `u0`, measured outputs `y0m` and dist. `d0`.
 
 The [`InternalModel`](@ref) updates the deterministic `x̂d` and stochastic `x̂s` estimates with:
 ```math
@@ -234,19 +237,19 @@ This estimator does not augment the state vector, thus ``\mathbf{x̂ = x̂_d}``.
 [`init_internalmodel`](@ref) for details. 
 """
 function update_estimate!(
-    estim::InternalModel{NT, SM}, u, ym, d=empty(estim.x̂0)
+    estim::InternalModel{NT, SM}, u0, y0m, d0=empty(estim.x̂0)
 ) where {NT<:Real, SM}
     model = estim.model
     x̂d, x̂s = estim.x̂d, estim.x̂s
     # -------------- deterministic model ---------------------
-    ŷd, x̂dnext = Vector{NT}(undef, model.ny), Vector{NT}(undef, model.nx)
-    h!(ŷd, model, x̂d, d)
-    f!(x̂dnext, model, x̂d, u, d) 
+    ŷ0d, x̂dnext = Vector{NT}(undef, model.ny), Vector{NT}(undef, model.nx)
+    h!(ŷ0d, model, x̂d, d0)
+    f!(x̂dnext, model, x̂d, u0, d0) 
     x̂d .= x̂dnext # this also updates estim.x̂0 (they are the same object)
     # --------------- stochastic model -----------------------
     x̂snext = Vector{NT}(undef, estim.nxs)
     ŷs = zeros(NT, model.ny)
-    ŷs[estim.i_ym] = ym - ŷd[estim.i_ym]   # ŷs=0 for unmeasured outputs
+    ŷs[estim.i_ym] = y0m - ŷ0d[estim.i_ym]   # ŷs=0 for unmeasured outputs
     mul!(x̂snext, estim.Âs, x̂s)
     mul!(x̂snext, estim.B̂s, ŷs, 1, 1)
     x̂s .= x̂snext