changed: f̂ now internally handle the operating points

src/controller/transcription.jl

@@ -1159,7 +1159,6 @@ function predict!(
         k0     = @views K0[(1 + nk*(j-1)):(nk*j)]
         x̂0next = @views X̂0[(1 + nx̂*(j-1)):(nx̂*j)]
         f̂!(x̂0next, û0, k0, mpc.estim, model, x̂0, u0, d0)
-        x̂0next .+= mpc.estim.f̂op .- mpc.estim.x̂op
         x̂0 = @views X̂0[(1 + nx̂*(j-1)):(nx̂*j)]
         d0 = @views D̂0[(1 + nd*(j-1)):(nd*j)]
         ŷ0 = @views Ŷ0[(1 + ny*(j-1)):(ny*j)]
@@ -1318,8 +1317,6 @@ function con_nonlinprogeq!(
         x̂0next_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*j)]
         ŝnext    = @views  geq[(1 + nx̂*(j-1)):(nx̂*j)]
         f̂!(x̂0next, û0, k0, mpc.estim, model, x̂0, u0, d0)
-        # handle operating points (but should be zeros for NonLinModel):
-        x̂0next .+= mpc.estim.f̂op .- mpc.estim.x̂op
         ŝnext .= x̂0next .- x̂0next_Z̃
         x̂0 = x̂0next_Z̃ # using states in Z̃ for next iteration (allow parallel for)
         d0 = d0next

src/estimator/execute.jl

@@ -19,18 +19,19 @@ end
 Mutating state function ``\mathbf{f̂}`` of the augmented model.
 
 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:
+the function returns the next state of the augmented model, as deviation vectors:
 ```math
 \begin{aligned}
-    \mathbf{x̂_0}(k+1) &= \mathbf{f̂}\Big(\mathbf{x̂_0}(k), \mathbf{u_0}(k), \mathbf{d_0}(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̂_0}(k+1)`` is stored in `x̂0next` argument. The method mutates `x̂0next`, 
 `û0` and `k0` in place. The argument `û0` stores the disturbed input of the augmented model
 ``\mathbf{û_0}``, and `k0`, the intermediate stage values of `model.solver`, when applicable.
-The model parameter `model.p` is not included in the function signature for conciseness. See
-Extended Help for details on ``\mathbf{û_0, f̂}`` and ``\mathbf{ĥ}`` implementations.
+The model parameter `model.p` is not included in the function signature for conciseness. 
+The operating points are handled inside ``\mathbf{f̂}``. See Extended Help for details on 
+``\mathbf{û_0, f̂}`` and ``\mathbf{ĥ}`` implementations.
 
 # Extended Help
 !!! details "Extended Help"
@@ -41,7 +42,8 @@ Extended Help for details on ``\mathbf{û_0, f̂}`` and ``\mathbf{ĥ}`` implem
     \begin{aligned}
     \mathbf{f̂}\Big(\mathbf{x̂_0}(k), \mathbf{u_0}(k), \mathbf{d_0}(k)\Big)  &=               \begin{bmatrix}
         \mathbf{f}\Big(\mathbf{x_0}(k), \mathbf{û_0}(k), \mathbf{d_0}(k), \mathbf{p}\Big)   \\
-        \mathbf{A_s} \mathbf{x_s}(k)                                                        \end{bmatrix} \\
+        \mathbf{A_s} \mathbf{x_s}(k)                                                        \end{bmatrix} 
+        + \mathbf{f̂_{op}} - \mathbf{x̂_{op}}                                                 \\
     \mathbf{ĥ}\Big(\mathbf{x̂_0}(k), \mathbf{d_0}(k)\Big)                   &=
         \mathbf{h}\Big(\mathbf{x_0}(k), \mathbf{d_0}(k), \mathbf{p}\Big) + \mathbf{y_{s_y}}(k)
     \end{aligned}
@@ -55,37 +57,41 @@ Extended Help for details on ``\mathbf{û_0, f̂}`` and ``\mathbf{ĥ}`` implem
     \end{aligned}
     ```
     The ``\mathbf{f}`` and ``\mathbf{h}`` functions above are in fact the [`f!`](@ref) and 
-    [`h!`](@ref) methods, respectively.
+    [`h!`](@ref) methods, respectively. The operating points ``\mathbf{x̂_{op}, f̂_{op}}``
+    are computed by [`augment_model`](@ref) (almost always zeros in practice for 
+    [`NonLinModel`](@ref)).
 """
 function f̂!(x̂0next, û0, k0, estim::StateEstimator, model::SimModel, x̂0, u0, d0)
-    return f̂!(x̂0next, û0, k0, model, estim.As, estim.Cs_u, x̂0, u0, d0)
+    return f̂!(x̂0next, û0, k0, model, estim.As, estim.Cs_u, estim.f̂op, estim.x̂op, x̂0, u0, d0)
 end
 
 """
     f̂!(x̂0next, _ , _ , estim::StateEstimator, model::LinModel, x̂0, u0, d0) -> nothing
 
-Use the augmented model matrices if `model` is a [`LinModel`](@ref).
+Use the augmented model matrices and operating points if `model` is a [`LinModel`](@ref).
 """
 function f̂!(x̂0next, _ , _ , estim::StateEstimator, ::LinModel, x̂0, u0, d0)
     mul!(x̂0next, estim.Â,  x̂0)
     mul!(x̂0next, estim.B̂u, u0, 1, 1)
     mul!(x̂0next, estim.B̂d, d0, 1, 1)
+    x̂0next .+= estim.f̂op .- estim.x̂op
     return nothing
 end
 
 """
-    f̂!(x̂0next, û0, k0, model::SimModel, As, Cs_u, x̂0, u0, d0)
+    f̂!(x̂0next, û0, k0, model::SimModel, As, Cs_u, f̂op, x̂op, x̂0, u0, d0)
 
 Same than [`f̂!`](@ref) for [`SimModel`](@ref) but without the `estim` argument.
 """
-function f̂!(x̂0next, û0, k0, model::SimModel, As, Cs_u, x̂0, u0, d0)
+function f̂!(x̂0next, û0, k0, model::SimModel, As, Cs_u, f̂op, x̂op, x̂0, u0, d0)
     # `@views` macro avoid copies with matrix slice operator e.g. [a:b]
     @views xd, xs = x̂0[1:model.nx], x̂0[model.nx+1:end]
     @views xdnext, xsnext = x̂0next[1:model.nx], x̂0next[model.nx+1:end]
     mul!(û0, Cs_u, xs)      # ys_u = Cs_u*xs
     û0 .+= u0               # û0 = u0 + ys_u  
     f!(xdnext, k0, model, xd, û0, d0, model.p)
     mul!(xsnext, As, xs)
+    x̂0next .+= f̂op .- x̂op
     return nothing
 end
 

src/estimator/internal_model.jl

@@ -172,8 +172,10 @@ State function ``\mathbf{f̂}`` of [`InternalModel`](@ref) for [`NonLinModel`](@
 
 It calls [`f!`](@ref) directly since this estimator does not augment the states.
 """
-function f̂!(x̂0next, _ , k0, ::InternalModel, model::NonLinModel, x̂0, u0, d0)
-    return f!(x̂0next, k0, model, x̂0, u0, d0, model.p)
+function f̂!(x̂0next, _ , k0, estim::InternalModel, model::NonLinModel, x̂0, u0, d0)
+    f!(x̂0next, k0, model, x̂0, u0, d0, model.p)
+    x̂0next .+= estim.f̂op .- estim.x̂op
+    return x̂0next
 end
 
 @doc raw"""

src/estimator/kalman.jl

@@ -867,7 +867,6 @@ function update_estimate!(estim::UnscentedKalmanFilter, y0m, d0, u0)
     P̂next = estim.buffer.P̂
     mul!(P̂next, X̄0next, Ŝ_X̄0nextᵀ) 
     P̂next   .+= Q̂
-    x̂0next  .+= estim.f̂op .- estim.x̂op
     estim.x̂0 .= x̂0next
     estim.cov.P̂  .= Hermitian(P̂next, :L)
     return nothing
@@ -1068,11 +1067,17 @@ objects with the linearization points.
 """
 function get_ekf_linfunc(NT, model, i_ym, nint_u, nint_ym, jacobian)
     As, Cs_u, Cs_y = init_estimstoch(model, i_ym, nint_u, nint_ym)
-    f̂_ekf!(x̂0next, x̂0, û0, k0, u0, d0) = f̂!(x̂0next, û0, k0, model, As, Cs_u, x̂0, u0, d0)
-    ĥ_ekf!(ŷ0, x̂0, d0) = ĥ!(ŷ0, model, Cs_y, x̂0, d0)
+    nxs = size(As, 1)
+    x̂op, f̂op = [model.xop; zeros(nxs)], [model.fop; zeros(nxs)]
+    f̂_ekf!(x̂0next, x̂0, û0, k0, u0, d0) = f̂!(
+        x̂0next, û0, k0, model, As, Cs_u, f̂op, x̂op, x̂0, u0, d0
+    )
+    ĥ_ekf!(ŷ0, x̂0, d0) = ĥ!(
+        ŷ0, model, Cs_y, x̂0, d0
+    )
     strict  = Val(true)
     nu, ny, nd, nk = model.nu, model.ny, model.nd, model.nk
-    nx̂ = model.nx + size(As, 1)
+    nx̂ = model.nx + nxs
     x̂0next = zeros(NT, nx̂)
     ŷ0 = zeros(NT, ny)
     x̂0 = zeros(NT, nx̂)
@@ -1232,7 +1237,6 @@ function predict_estimate_kf!(estim::Union{KalmanFilter, ExtendedKalmanFilter},
     mul!(Â_P̂corr_Âᵀ, Â, P̂corr_Âᵀ)
     P̂next  = estim.buffer.P̂
     P̂next .= Â_P̂corr_Âᵀ .+ Q̂
-    x̂0next  .+= estim.f̂op .- estim.x̂op
     estim.x̂0 .= x̂0next
     estim.cov.P̂  .= Hermitian(P̂next, :L)
     return nothing

src/estimator/mhe/execute.jl

@@ -610,7 +610,7 @@ function predict!(V̂, X̂0, û0, k0, ŷ0, estim::MovingHorizonEstimator, mode
             ŵ   = @views Z̃[(1 + nx̃ + nŵ*(j-1)):(nx̃ + nŵ*j)]
             x̂0next = @views X̂0[(1 + nx̂ *(j-1)):(nx̂ *j)]
             f̂!(x̂0next, û0, k0, estim, model, x̂0, u0, d0)
-            x̂0next .+= ŵ .+ estim.f̂op .- estim.x̂op
+            x̂0next .+= ŵ
             y0nextm = @views estim.Y0m[(1 + nym * (j-1)):(nym*j)]
             d0next  = @views estim.D0[(1 + nd*j):(nd*(j+1))]
             ĥ!(ŷ0next, estim, model, x̂0next, d0next)
@@ -629,7 +629,7 @@ function predict!(V̂, X̂0, û0, k0, ŷ0, estim::MovingHorizonEstimator, mode
             V̂[(1 + nym*(j-1)):(nym*j)] .= y0m .- ŷ0m
             x̂0next = @views X̂0[(1 + nx̂ *(j-1)):(nx̂ *j)]
             f̂!(x̂0next, û0, k0, estim, model, x̂0, u0, d0)
-            x̂0next .+= ŵ .+ estim.f̂op .- estim.x̂op
+            x̂0next .+= ŵ
             x̂0 = x̂0next
         end
     end