changed: f̂ now internally handle the operating points

This is simpler since the users of the function, that is `PredictiveController` and `StateEstimator` objects, no longer need to add `f̂op` subtract `x̂op` after calling `f̂`

Author: franckgaga

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,10 +19,10 @@ 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}
 ```
@@ -41,7 +41,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,7 +56,9 @@ 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)
@@ -64,12 +67,13 @@ 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
 
@@ -86,6 +90,7 @@ function f̂!(x̂0next, û0, k0, model::SimModel, As, Cs_u, x̂0, u0, d0)
     û0 .+= u0               # û0 = u0 + ys_u  
     f!(xdnext, k0, model, xd, û0, d0, model.p)
     mul!(xsnext, As, xs)
+    x̂0next .+= estim.f̂op .- estim.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

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