debug: correct steady-state formula for initstate on InternalModel

and minor doc corrections

Author: franckgaga

src/controller/execute.jl

@@ -247,9 +247,8 @@ predictstoch!(mpc::PredictiveController, ::StateEstimator, _ , _ ) = (mpc.F .= 0
 
 Set `b` vector for the linear model inequality constraints (``\mathbf{A ΔŨ ≤ b}``).
 
-Also init ``\mathbf{f_x̂} = \mathbf{g_x̂ d}(k) + \mathbf{j_x̂ D̂} + \mathbf{k_x̂ x̂}_{k-1}(k) 
-+ \mathbf{v_x̂ u}(k-1) + \mathbf{b_x̂}`` vector for the terminal constraints, see 
-[`init_predmat`](@ref).
+Also init ``\mathbf{f_x̂} = \mathbf{g_x̂ d}(k) + \mathbf{j_x̂ D̂} + \mathbf{k_x̂ x̂}_{k-1}(k) + \mathbf{v_x̂ u}(k-1) + \mathbf{b_x̂}``
+vector for the terminal constraints, see [`init_predmat`](@ref).
 """
 function linconstraint!(mpc::PredictiveController, model::LinModel)
     nU, nΔŨ, nY = length(mpc.con.U0min), length(mpc.con.ΔŨmin), length(mpc.con.Y0min)

src/estimator/construct.jl

@@ -105,18 +105,18 @@ end
 
 Augment [`LinModel`](@ref) state-space matrices with the stochastic ones `As` and `Cs`.
 
-If ``\mathbf{x}`` are `model.x0` states, and ``\mathbf{x_s}``, the states defined at
+If ``\mathbf{x_0}`` are `model.x0` states, and ``\mathbf{x_s}``, the states defined at
 [`init_estimstoch`](@ref), we define an augmented state vector ``\mathbf{x̂} = 
-[ \begin{smallmatrix} \mathbf{x} \\ \mathbf{x_s} \end{smallmatrix} ]``. The method
+[ \begin{smallmatrix} \mathbf{x_0} \\ \mathbf{x_s} \end{smallmatrix} ]``. The method
 returns the augmented matrices `Â`, `B̂u`, `Ĉ`, `B̂d` and `D̂d`:
 ```math
 \begin{aligned}
-    \mathbf{x̂}(k+1) &= \mathbf{Â x̂}(k) + \mathbf{B̂_u u}(k) + \mathbf{B̂_d d}(k) \\
-    \mathbf{ŷ}(k)   &= \mathbf{Ĉ x̂}(k) + \mathbf{D̂_d d}(k)
+    \mathbf{x̂_0}(k+1) &= \mathbf{Â x̂_0}(k) + \mathbf{B̂_u u_0}(k) + \mathbf{B̂_d d_0}(k) \\
+    \mathbf{ŷ_0}(k)   &= \mathbf{Ĉ x̂_0}(k) + \mathbf{D̂_d d_0}(k)
 \end{aligned}
 ```
 An error is thrown if the augmented model is not observable and `verify_obsv == true`. The
-augmented operating points `x̂_{op}` and `f̂_{op}` are simply ``\mathbf{x_{op}}`` and
+augmented operating points `x̂op` and `f̂op` are simply ``\mathbf{x_{op}}`` and
 ``\mathbf{f_{op}}`` vectors appended with zeros (see [`setop!`](@ref)).
 """
 function augment_model(model::LinModel{NT}, As, Cs_u, Cs_y; verify_obsv=true) where NT<:Real

src/estimator/execute.jl

@@ -15,64 +15,65 @@ function remove_op!(estim::StateEstimator, u, ym, d)
 end
 
 @doc raw"""
-    f̂!(x̂next, û, estim::StateEstimator, model::SimModel, x̂, u, d) -> nothing
+    f̂!(x̂next0, û0, estim::StateEstimator, model::SimModel, x̂0, u0, d0) -> nothing
 
 Mutating state function ``\mathbf{f̂}`` of the augmented model.
 
-By introducing an augmented state vector ``\mathbf{x̂}`` like in [`augment_model`](@ref), the
+By introducing an augmented state vector ``\mathbf{x̂_0}`` like in [`augment_model`](@ref), the
 function returns the next state of the augmented model, defined as:
 ```math
 \begin{aligned}
-    \mathbf{x̂}(k+1) &= \mathbf{f̂}\Big(\mathbf{x̂}(k), \mathbf{u}(k), \mathbf{d}(k)\Big) \\
-    \mathbf{ŷ}(k)   &= \mathbf{ĥ}\Big(\mathbf{x̂}(k), \mathbf{d}(k)\Big) 
+    \mathbf{x̂_0}(k+1) &= \mathbf{f̂}\Big(\mathbf{x̂_0}(k), \mathbf{u_0}(k), \mathbf{d_0}(k)\Big) \\
+    \mathbf{ŷ_0}(k)   &= \mathbf{ĥ}\Big(\mathbf{x̂_0}(k), \mathbf{d_0}(k)\Big) 
 \end{aligned}
 ```
-where ``\mathbf{x̂}(k+1)`` is stored in `x̂next` argument. The method mutates `x̂next` and `û`
-in place, the latter stores the input vector of the augmented model ``\mathbf{u + ŷ_{s_u}}``.
+where ``\mathbf{x̂_0}(k+1)`` is stored in `x̂next0` argument. The method mutates `x̂next0` and
+`û0` in place, the latter stores the input vector of the augmented model 
+``\mathbf{u0 + ŷ_{s_u}}``.
 """
-function f̂!(x̂next, û, estim::StateEstimator, model::SimModel, x̂, u, d)
+function f̂!(x̂next0, û0, estim::StateEstimator, model::SimModel, x̂0, u0, d0)
     # `@views` macro avoid copies with matrix slice operator e.g. [a:b]
-    @views x̂d, x̂s = x̂[1:model.nx], x̂[model.nx+1:end]
-    @views x̂d_next, x̂s_next = x̂next[1:model.nx], x̂next[model.nx+1:end]
-    mul!(û, estim.Cs_u, x̂s)
-    û .+= u
-    f!(x̂d_next, model, x̂d, û, d)
+    @views x̂d, x̂s = x̂0[1:model.nx], x̂0[model.nx+1:end]
+    @views x̂d_next, x̂s_next = x̂next0[1:model.nx], x̂next0[model.nx+1:end]
+    mul!(û0, estim.Cs_u, x̂s)
+    û0 .+= u0
+    f!(x̂d_next, model, x̂d, û0, d0)
     mul!(x̂s_next, estim.As, x̂s)
     return nothing
 end
 
 """
-    f̂!(x̂next, _ , estim::StateEstimator, model::LinModel, x̂, u, d) -> nothing
+    f̂!(x̂next0, _ , estim::StateEstimator, model::LinModel, x̂0, u0, d0) -> nothing
 
 Use the augmented model matrices if `model` is a [`LinModel`](@ref).
 """
-function f̂!(x̂next, _ , estim::StateEstimator, ::LinModel, x̂, u, d)
-    mul!(x̂next, estim.Â,  x̂)
-    mul!(x̂next, estim.B̂u, u, 1, 1)
-    mul!(x̂next, estim.B̂d, d, 1, 1)
+function f̂!(x̂next0, _ , estim::StateEstimator, ::LinModel, x̂0, u0, d0)
+    mul!(x̂next0, estim.Â,  x̂0)
+    mul!(x̂next0, estim.B̂u, u0, 1, 1)
+    mul!(x̂next0, estim.B̂d, d0, 1, 1)
     return nothing
 end
 
 @doc raw"""
-    ĥ!(ŷ, estim::StateEstimator, model::SimModel, x̂, d) -> nothing
+    ĥ!(ŷ0, estim::StateEstimator, model::SimModel, x̂0, d0) -> nothing
 
 Mutating output function ``\mathbf{ĥ}`` of the augmented model, see [`f̂!`](@ref).
 """
-function ĥ!(ŷ, estim::StateEstimator, model::SimModel, x̂, d)
+function ĥ!(ŷ0, estim::StateEstimator, model::SimModel, x̂0, d0)
     # `@views` macro avoid copies with matrix slice operator e.g. [a:b]
-    @views x̂d, x̂s = x̂[1:model.nx], x̂[model.nx+1:end]
-    h!(ŷ, model, x̂d, d)
-    mul!(ŷ, estim.Cs_y, x̂s, 1, 1)
+    @views x̂d, x̂s = x̂0[1:model.nx], x̂0[model.nx+1:end]
+    h!(ŷ0, model, x̂d, d0)
+    mul!(ŷ0, estim.Cs_y, x̂s, 1, 1)
     return nothing
 end
 """
-    ĥ!(ŷ, estim::StateEstimator, model::LinModel, x̂, d) -> nothing
+    ĥ!(ŷ0, estim::StateEstimator, model::LinModel, x̂0, d0) -> nothing
 
 Use the augmented model matrices if `model` is a [`LinModel`](@ref).
 """
-function ĥ!(ŷ, estim::StateEstimator, ::LinModel, x̂, d)
-    mul!(ŷ, estim.Ĉ,  x̂)
-    mul!(ŷ, estim.D̂d, d, 1, 1)
+function ĥ!(ŷ0, estim::StateEstimator, ::LinModel, x̂0, d0)
+    mul!(ŷ0, estim.Ĉ,  x̂0)
+    mul!(ŷ0, estim.D̂d, d0, 1, 1)
     return nothing
 end
 
@@ -130,8 +131,9 @@ init_estimate_cov!(::StateEstimator, _ , _ , _ ) = nothing
 
 Init `estim.x̂0` estimate with the steady-state solution if `model` is a [`LinModel`](@ref).
 
-Using `u0`, `ym0` and `d0` arguments, the steady-state problem combined to the equality 
-constraint ``\mathbf{ŷ_0^m} = \mathbf{y_0^m}`` engenders the following system to solve:
+Using `u0`, `ym0` and `d0` arguments (deviation values, see [`setop!`](@ref)), the
+steadystate problem combined to the equality constraint ``\mathbf{ŷ_0^m} = \mathbf{y_0^m}``
+engenders the following system to solve:
 ```math
 \begin{bmatrix}
     \mathbf{I} - \mathbf{Â}                         \\

src/estimator/internal_model.jl

@@ -149,20 +149,20 @@ function matrices_internalmodel(model::SimModel{NT}) where NT<:Real
 end
 
 @doc raw"""
-    f̂!(x̂next, _ , estim::InternalModel, model::NonLinModel, x̂, u, d)
+    f̂!(x̂next0, _ , estim::InternalModel, model::NonLinModel, x̂0, u0, d0)
 
 State function ``\mathbf{f̂}`` of [`InternalModel`](@ref) for [`NonLinModel`](@ref).
 
-It calls `model.f!(x̂next, x̂, u ,d)` since this estimator does not augment the states.
+It calls `model.f!(x̂next0, x̂0, u0 ,d0)` since this estimator does not augment the states.
 """
-f̂!(x̂next, _ , ::InternalModel, model::NonLinModel, x̂, u, d) = model.f!(x̂next, x̂, u, d)
+f̂!(x̂next0, _, ::InternalModel, model::NonLinModel, x̂0, u0, d0) = model.f!(x̂next0, x̂0, u0, d0)
 
 @doc raw"""
-    ĥ!(ŷ, estim::InternalModel, model::NonLinModel, x̂, d)
+    ĥ!(ŷ0, estim::InternalModel, model::NonLinModel, x̂0, d0)
 
 Output function ``\mathbf{ĥ}`` of [`InternalModel`](@ref), it calls `model.h!`.
 """
-ĥ!(x̂next, ::InternalModel, model::NonLinModel, x̂, d) = model.h!(x̂next, x̂, d)
+ĥ!(x̂next0, ::InternalModel, model::NonLinModel, x̂0, d0) = model.h!(x̂next0, x̂0, d0)
 
 
 @doc raw"""
@@ -260,10 +260,10 @@ Init `estim.x̂0`/`x̂d`/`x̂s` estimate at steady-state for [`InternalModel`](@
 
 The deterministic estimates `estim.x̂d` start at steady-state using `u0` and `d0` arguments:
 ```math
-    \mathbf{x̂_d} = \mathbf{(I - A)^{-1} (B_u u + B_d d)}
+    \mathbf{x̂_d} = \mathbf{(I - A)^{-1} (B_u u_0 + B_d d_0 + f_{op} - x_{op})}
 ```
-Based on `ym` argument and current stochastic outputs estimation ``\mathbf{ŷ_s}``, composed
-of the measured ``\mathbf{ŷ_s^m} = \mathbf{y^m} - \mathbf{ŷ_d^m}`` and unmeasured 
+Based on `ym0` argument and current stochastic outputs estimation ``\mathbf{ŷ_s}``, composed
+of the measured ``\mathbf{ŷ_s^m} = \mathbf{y_0^m} - \mathbf{ŷ_{d0}^m}`` and unmeasured 
 ``\mathbf{ŷ_s^u = 0}`` outputs, the stochastic estimates also start at steady-state:
 ```math
     \mathbf{x̂_s} = \mathbf{(I - Â_s)^{-1} B̂_s ŷ_s}
@@ -274,7 +274,7 @@ This estimator does not augment the state vector, thus ``\mathbf{x̂ = x̂_d}``.
 function init_estimate!(estim::InternalModel, model::LinModel{NT}, u0, ym0, d0) where NT<:Real
     x̂d, x̂s = estim.x̂d, estim.x̂s
     # also updates estim.x̂0 (they are the same object):
-    x̂d .= (I - model.A)\(model.Bu*u0 + model.Bd*d0 - estim.f̂op + estim.x̂op)
+    x̂d .= (I - model.A)\(model.Bu*u0 + model.Bd*d0 + model.fop - model.xop)
     ŷd0 = Vector{NT}(undef, model.ny)
     h!(ŷd0, model, x̂d, d0)
     ŷs = zeros(NT, model.ny)