debug: pass f̂op, x̂op as arguments

Author: franckgaga

src/estimator/execute.jl

@@ -29,8 +29,9 @@ the function returns the next state of the augmented model, as deviation vectors
 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"
@@ -61,7 +62,7 @@ Extended Help for details on ``\mathbf{û_0, f̂}`` and ``\mathbf{ĥ}`` implem
     [`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
 
 """
@@ -78,19 +79,19 @@ function f̂!(x̂0next, _ , _ , estim::StateEstimator, ::LinModel, x̂0, u0, d0)
 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 .+= estim.f̂op .- estim.x̂op
+    x̂0next .+= f̂op .- x̂op
     return nothing
 end
 

src/estimator/kalman.jl

@@ -1067,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̂)