removed: DiffCaches in RungeKutta

also renamed `nonlinmodel.f!` and `nonlinmodel.h!` to `nonlinmodel.solver_f!` and `nonlinmodel.solver_h!`, to be more explicit that it's the function for a specific `DiffSolver` instance

Author: franckgaga

src/controller/execute.jl

@@ -113,24 +113,27 @@ julia> round.(getinfo(mpc)[:Ŷ], digits=3)
 ```
 """
 function getinfo(mpc::PredictiveController{NT}) where NT<:Real
-    model, transcription = mpc.estim.model, mpc.transcription
-    nΔŨ      = mpc.Hc*model.nu + mpc.nϵ
+    model, buffer, transcription = mpc.estim.model, mpc.buffer, mpc.transcription
+    nΔŨ, nXi = mpc.Hc*model.nu + mpc.nϵ, mpc.Hp*model.nxi
     nŶe, nUe = (mpc.Hp+1)*model.ny, (mpc.Hp+1)*model.nu
     nX̂0, nÛ0 = mpc.estim.nx̂*mpc.Hp, model.nu*mpc.Hp 
     Z̃ = mpc.Z̃
     info = Dict{Symbol, Any}()
-    ΔŨ = Vector{NT}(undef, nΔŨ)
-    x̂0end = similar(mpc.estim.x̂0)
+    ΔŨ     = Vector{NT}(undef, nΔŨ)
+    x̂0end  = similar(mpc.estim.x̂0)
+    X0i    = Vector{NT}(undef, nXi) 
     Ue, Ŷe = Vector{NT}(undef, nUe), Vector{NT}(undef, nŶe)
     U0, Ŷ0 = similar(mpc.Uop), similar(mpc.Yop)
     Û0, X̂0 = Vector{NT}(undef, nÛ0), Vector{NT}(undef, nX̂0)
-    U,  Ŷ  = similar(mpc.Uop), similar(mpc.Yop)
+    U,  Ŷ  = buffer.U, buffer.Ŷ
+    D̂      = buffer.D̂
     U0 = getU0!(U0, mpc, Z̃)
     ΔŨ = getΔŨ!(ΔŨ, mpc, transcription, Z̃)
-    Ŷ0, x̂0end  = predict!(Ŷ0, x̂0end, X̂0, Û0, mpc, model, transcription, U0, Z̃)
+    Ŷ0, x̂0end  = predict!(Ŷ0, x̂0end, X̂0, Û0, X0i, mpc, model, transcription, U0, Z̃)
     Ue, Ŷe = extended_vectors!(Ue, Ŷe, mpc, U0, Ŷ0)
     U .= U0 .+ mpc.Uop
     Ŷ .= Ŷ0 .+ mpc.Yop
+    D̂ .= mpc.D̂0 + mpc.Dop
     J = obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ)
     Ŷs = similar(mpc.Yop)
     predictstoch!(Ŷs, mpc, mpc.estim)
@@ -140,7 +143,7 @@ function getinfo(mpc::PredictiveController{NT}) where NT<:Real
     info[:U]    = U
     info[:u]    = info[:U][1:model.nu]
     info[:d]    = mpc.d0 + model.dop
-    info[:D̂]    = mpc.D̂0 + mpc.Dop
+    info[:D̂]    = D̂
     info[:ŷ]    = mpc.ŷ
     info[:Ŷ]    = Ŷ
     info[:x̂end] = x̂0end + mpc.estim.x̂op

src/controller/nonlinmpc.jl

@@ -578,15 +578,17 @@ Inspired from: [User-defined operators with vector outputs](@extref JuMP User-de
 function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT<:Real
     # ----- common cache for Jfunc, gfuncs, geqfuncs called with floats -------------------
     model = mpc.estim.model
-    nu, ny, nx̂, nϵ, Hp, Hc = model.nu, model.ny, mpc.estim.nx̂, mpc.nϵ, mpc.Hp, mpc.Hc
+    nu, ny, nx̂, nϵ, nxi = model.nu, model.ny, mpc.estim.nx̂, mpc.nϵ, model.nxi
+    Hp, Hc = mpc.Hp, mpc.Hc
     ng, nc, neq = length(mpc.con.i_g), mpc.con.nc, mpc.con.neq
-    nZ̃, nU, nŶ, nX̂ = length(mpc.Z̃), Hp*nu, Hp*ny, Hp*nx̂
+    nZ̃, nU, nŶ, nX̂, nXi = length(mpc.Z̃), Hp*nu, Hp*ny, Hp*nx̂, Hp*nxi
     nΔŨ, nUe, nŶe = nu*Hc + nϵ, nU + nu, nŶ + ny  
     strict = Val(true)
     myNaN  = convert(JNT, NaN)  # NaN to force update_simulations! at first call:
     Z̃ ::Vector{JNT}                  = fill(myNaN, nZ̃)
     ΔŨ::Vector{JNT}                  = zeros(JNT, nΔŨ)
     x̂0end::Vector{JNT}               = zeros(JNT, nx̂)
+    X0i::Vector{JNT}                 = zeros(JNT, nXi)
     Ue::Vector{JNT}, Ŷe::Vector{JNT} = zeros(JNT, nUe), zeros(JNT, nŶe)
     U0::Vector{JNT}, Ŷ0::Vector{JNT} = zeros(JNT, nU),  zeros(JNT, nŶ)
     Û0::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nU),  zeros(JNT, nX̂)
@@ -596,18 +598,18 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT
     function Jfunc(Z̃arg::Vararg{T, N}) where {N, T<:Real}
         if isdifferent(Z̃arg, Z̃)
             Z̃ .= Z̃arg
-            update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq, mpc, Z̃)
+            update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X0i, X̂0, gc, g, geq, mpc, Z̃)
         end
         return obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ)::T
     end
-    function Jfunc!(Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq)
-        update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq, mpc, Z̃)
+    function Jfunc!(Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X0i, X̂0, gc, g, geq)
+        update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X0i, X̂0, gc, g, geq, mpc, Z̃)
         return obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ)
     end
     Z̃_∇J = fill(myNaN, nZ̃) 
     ∇J_context = (
         Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), 
-        Cache(Û0), Cache(X̂0), 
+        Cache(Û0), Cache(X0i), Cache(X̂0), 
         Cache(gc), Cache(g), Cache(geq),
     )
     ∇J_prep = prepare_gradient(Jfunc!, mpc.gradient, Z̃_∇J, ∇J_context...; strict)
@@ -631,19 +633,23 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT
         gfunc_i = function (Z̃arg::Vararg{T, N}) where {N, T<:Real}
             if isdifferent(Z̃arg, Z̃)
                 Z̃ .= Z̃arg
-                update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq, mpc, Z̃)
+                update_predictions!(
+                    ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X0i, X̂0, gc, g, geq, mpc, Z̃
+                )
             end
             return g[i]::T
         end
         gfuncs[i] = gfunc_i
     end
-    function gfunc!(g, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, geq)
-        return update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq, mpc, Z̃)
+    function gfunc!(g, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X0i, X̂0, gc, geq)
+        return update_predictions!(
+            ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X0i, X̂0, gc, g, geq, mpc, Z̃
+        )
     end
     Z̃_∇g = fill(myNaN, nZ̃)
     ∇g_context = (
         Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), 
-        Cache(Û0), Cache(X̂0), 
+        Cache(Û0), Cache(X0i),   Cache(X̂0), 
         Cache(gc), Cache(geq),
     )
     # temporarily enable all the inequality constraints for sparsity detection:
@@ -678,19 +684,23 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT
         geqfunc_i = function (Z̃arg::Vararg{T, N}) where {N, T<:Real}
             if isdifferent(Z̃arg, Z̃)
                 Z̃ .= Z̃arg
-                update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq, mpc, Z̃)
+                update_predictions!(
+                    ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X0i, X̂0, gc, g, geq, mpc, Z̃
+                )
             end
             return geq[i]::T
         end
         geqfuncs[i] = geqfunc_i          
     end
-    function geqfunc!(geq, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g) 
-        return update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq, mpc, Z̃)
+    function geqfunc!(geq, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X0i, X̂0, gc, g) 
+        return update_predictions!(
+            ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X0i, X̂0, gc, g, geq, mpc, Z̃
+        )
     end
     Z̃_∇geq = fill(myNaN, nZ̃)
     ∇geq_context = (
         Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0),
-        Cache(Û0), Cache(X̂0),
+        Cache(Û0), Cache(X0i),   Cache(X̂0),
         Cache(gc), Cache(g)
     )
     ∇geq_prep = prepare_jacobian(geqfunc!, geq, mpc.jacobian, Z̃_∇geq, ∇geq_context...; strict)
@@ -716,7 +726,7 @@ end
 
 """
     update_predictions!(
-        ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq, 
+        ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X0i, X̂0, gc, g, geq, 
         mpc::PredictiveController, Z̃
     ) -> nothing
 
@@ -725,17 +735,17 @@ Update in-place all vectors for the predictions of `mpc` controller at decision
 The method mutates all the arguments before the `mpc` argument.
 """
 function update_predictions!(
-    ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq, mpc::PredictiveController, Z̃
+    ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X0i, X̂0, gc, g, geq, mpc::PredictiveController, Z̃
 )
     model, transcription = mpc.estim.model, mpc.transcription
     U0 = getU0!(U0, mpc, Z̃)
     ΔŨ = getΔŨ!(ΔŨ, mpc, transcription, Z̃)
-    Ŷ0, x̂0end  = predict!(Ŷ0, x̂0end, X̂0, Û0, mpc, model, transcription, U0, Z̃)
+    Ŷ0, x̂0end  = predict!(Ŷ0, x̂0end, X̂0, Û0, X0i, mpc, model, transcription, U0, Z̃)
     Ue, Ŷe = extended_vectors!(Ue, Ŷe, mpc, U0, Ŷ0)
     ϵ = getϵ(mpc, Z̃)
     gc  = con_custom!(gc, mpc, Ue, Ŷe, ϵ)
     g   = con_nonlinprog!(g, mpc, model, transcription, x̂0end, Ŷ0, gc, ϵ)
-    geq = con_nonlinprogeq!(geq, X̂0, Û0, mpc, model, transcription, U0, Z̃)
+    geq = con_nonlinprogeq!(geq, X̂0, Û0, X0i, mpc, model, transcription, U0, Z̃)
     return nothing
 end
 

src/controller/transcription.jl

@@ -1028,7 +1028,7 @@ getU0!(U0, mpc::PredictiveController, Z̃) = (mul!(U0, mpc.P̃u, Z̃) .+ mpc.Tu_
 
 @doc raw"""
     predict!(
-        Ŷ0, x̂0end, _ , _ ,
+        Ŷ0, x̂0end, _ , _ , _ ,
         mpc::PredictiveController, model::LinModel, transcription::TranscriptionMethod, 
         _ , Z̃
     ) -> Ŷ0, x̂0end
@@ -1040,7 +1040,7 @@ the terminal constraints applied on ``\mathbf{x̂}_{k-1}(k+H_p)``. The computati
 identical for any [`TranscriptionMethod`](@ref) if the model is linear.
 """
 function predict!(
-    Ŷ0, x̂0end, _ , _ , 
+    Ŷ0, x̂0end, _, _, _,
     mpc::PredictiveController, ::LinModel, ::TranscriptionMethod, 
     _ , Z̃
 )
@@ -1052,29 +1052,31 @@ end
 
 @doc raw"""
     predict!(
-        Ŷ0, x̂0end, X̂0, Û0, 
+        Ŷ0, x̂0end, X̂0, Û0, X0i,
         mpc::PredictiveController, model::NonLinModel, transcription::SingleShooting,
         U0, _
     ) -> Ŷ0, x̂0end
 
 Compute vectors if `model` is a [`NonLinModel`](@ref) and for [`SingleShooting`](@ref).
     
-The method mutates `Ŷ0`, `x̂0end`, `X̂0` and `Û0` arguments.
+The method mutates `Ŷ0`, `x̂0end`, `X̂0`, `Û0` and `X0i` arguments.
 """
 function predict!(
-    Ŷ0, x̂0end, X̂0, Û0,
+    Ŷ0, x̂0end, X̂0, Û0, X0i,
     mpc::PredictiveController, model::NonLinModel, ::SingleShooting,
     U0, _
 )
-    nu, nx̂, ny, nd, Hp, Hc = model.nu, mpc.estim.nx̂, model.ny, model.nd, mpc.Hp, mpc.Hc
+    nu, nx̂, ny, nd, nxi = model.nu, mpc.estim.nx̂, model.ny, model.nd, model.nxi
+    Hp, Hc = mpc.Hp, mpc.Hc
     D̂0 = mpc.D̂0
     x̂0 = @views mpc.estim.x̂0[1:nx̂]
     d0 = @views mpc.d0[1:nd]
     for j=1:Hp
-        u0     = @views U0[(1 + nu*(j-1)):(nu*j)]
-        û0     = @views Û0[(1 + nu*(j-1)):(nu*j)]
-        x̂0next = @views X̂0[(1 + nx̂*(j-1)):(nx̂*j)]
-        f̂!(x̂0next, û0, mpc.estim, model, x̂0, u0, d0)
+        u0     = @views  U0[(1 +  nu*(j-1)):(nu*j)]
+        û0     = @views  Û0[(1 +  nu*(j-1)):(nu*j)]
+        x0i    = @views X0i[(1 + nxi*(j-1)):(nxi*j)]
+        x̂0next = @views  X̂0[(1 +  nx̂*(j-1)):(nx̂*j)]
+        f̂!(x̂0next, û0, x0i, 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)]
@@ -1088,7 +1090,7 @@ end
 
 @doc raw"""
     predict!(
-        Ŷ0, x̂0end, _ , _ , 
+        Ŷ0, x̂0end, _ , _ , _ , 
         mpc::PredictiveController, model::NonLinModel, transcription::MultipleShooting,
         U0, Z̃
     ) -> Ŷ0, x̂0end
@@ -1098,7 +1100,7 @@ Compute vectors if `model` is a [`NonLinModel`](@ref) and for [`MultipleShooting
 The method mutates `Ŷ0` and `x̂0end` arguments.
 """
 function predict!(
-    Ŷ0, x̂0end, _, _,
+    Ŷ0, x̂0end, _, _, _,
     mpc::PredictiveController, model::NonLinModel, ::MultipleShooting,
     U0, Z̃
 )
@@ -1204,17 +1206,20 @@ end
 
 """
     con_nonlinprogeq!(
-        geq, X̂0, Û0, mpc::PredictiveController, model::NonLinModel, ::MultipleShooting, U0, Z̃
+        geq, X̂0, Û0, X0i
+        mpc::PredictiveController, model::NonLinModel, ::MultipleShooting, U0, Z̃
     )
 
 Nonlinear equality constrains for [`NonLinModel`](@ref) and [`MultipleShooting`](@ref).
 
-The method mutates the `geq`, `X̂0` and `Û0` vectors in argument.
+The method mutates the `geq`, `X̂0`, `Û0` and `X0i` vectors in argument.
 """
 function con_nonlinprogeq!(
-    geq, X̂0, Û0, mpc::PredictiveController, model::NonLinModel, ::MultipleShooting, U0, Z̃
+    geq, X̂0, Û0, X0i, 
+    mpc::PredictiveController, model::NonLinModel, ::MultipleShooting, U0, Z̃
 )
-    nx̂, nu, nd, Hp, Hc = mpc.estim.nx̂, model.nu, model.nd, mpc.Hp, mpc.Hc
+    nu, nx̂, ny, nd, nxi = model.nu, mpc.estim.nx̂, model.ny, model.nd, model.nxi
+    Hp, Hc = mpc.Hp, mpc.Hc
     nΔU, nX̂ = nu*Hc, nx̂*Hp
     D̂0 = mpc.D̂0
     X̂0_Z̃ = @views Z̃[(nΔU+1):(nΔU+nX̂)]
@@ -1225,7 +1230,8 @@ function con_nonlinprogeq!(
         u0     = @views U0[(1 + nu*(j-1)):(nu*j)]
         û0     = @views Û0[(1 + nu*(j-1)):(nu*j)]
         x̂0next = @views X̂0[(1 + nx̂*(j-1)):(nx̂*j)]
-        f̂!(x̂0next, û0, mpc.estim, model, x̂0, u0, d0)
+        x0i    = @views X0i[(1 + nxi*(j-1)):(nxi*j)]
+        f̂!(x̂0next, û0, x0i, mpc.estim, model, x̂0, u0, d0)
         x̂0next .+= mpc.estim.f̂op .- mpc.estim.x̂op
         x̂0next_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*j)]       
         ŝnext    = @views  geq[(1 + nx̂*(j-1)):(nx̂*j)]
@@ -1235,5 +1241,5 @@ function con_nonlinprogeq!(
     end
     return geq
 end
-con_nonlinprogeq!(geq,_,_,::PredictiveController,::NonLinModel,::SingleShooting,  _,_) = geq
-con_nonlinprogeq!(geq,_,_,::PredictiveController,::LinModel,::TranscriptionMethod,_,_) = geq
+con_nonlinprogeq!(geq,_,_,_,::PredictiveController,::NonLinModel,::SingleShooting,  _,_)=geq
+con_nonlinprogeq!(geq,_,_,_,::PredictiveController,::LinModel,::TranscriptionMethod,_,_)=geq

src/estimator/construct.jl

@@ -1,6 +1,7 @@
 struct StateEstimatorBuffer{NT<:Real}
     u ::Vector{NT}
     û ::Vector{NT}
+    xi::Vector{NT}
     x̂ ::Vector{NT}
     P̂ ::Matrix{NT}
     Q̂ ::Matrix{NT}
@@ -13,17 +14,18 @@ struct StateEstimatorBuffer{NT<:Real}
 end
 
 @doc raw"""
-    StateEstimatorBuffer{NT}(nu::Int, nx̂::Int, nym::Int, ny::Int, nd::Int)
+    StateEstimatorBuffer{NT}(nu::Int, nx̂::Int, nym::Int, ny::Int, nd::Int, nxi::Int=0)
 
 Create a buffer for `StateEstimator` objects for estimated states and measured outputs.
 
 The buffer is used to store intermediate results during estimation without allocating.
 """
 function StateEstimatorBuffer{NT}(
-    nu::Int, nx̂::Int, nym::Int, ny::Int, nd::Int
+    nu::Int, nx̂::Int, nym::Int, ny::Int, nd::Int, nxi::Int=0
 ) where NT <: Real
     u  = Vector{NT}(undef, nu)
     û  = Vector{NT}(undef, nu)
+    xi = Vector{NT}(undef, nxi)
     x̂  = Vector{NT}(undef, nx̂)
     P̂  = Matrix{NT}(undef, nx̂, nx̂)
     Q̂  = Matrix{NT}(undef, nx̂, nx̂)
@@ -33,7 +35,7 @@ function StateEstimatorBuffer{NT}(
     ŷ  = Vector{NT}(undef, ny)
     d  = Vector{NT}(undef, nd)
     empty = Vector{NT}(undef, 0)
-    return StateEstimatorBuffer{NT}(u, û, x̂, P̂, Q̂, R̂, K̂, ym, ŷ, d, empty)
+    return StateEstimatorBuffer{NT}(u, û, xi, x̂, P̂, Q̂, R̂, K̂, ym, ŷ, d, empty)
 end
 
 @doc raw"""

src/estimator/internal_model.jl

@@ -32,7 +32,7 @@ struct InternalModel{NT<:Real, SM<:SimModel} <: StateEstimator{NT}
     function InternalModel{NT}(
         model::SM, i_ym, Asm, Bsm, Csm, Dsm
     ) where {NT<:Real, SM<:SimModel}
-        nu, ny, nd = model.nu, model.ny, model.nd
+        nu, ny, nd, nxi = model.nu, model.ny, model.nd, model.nxi
         nym, nyu = validate_ym(model, i_ym)
         validate_internalmodel(model, nym, Csm, Dsm)
         As, Bs, Cs, Ds = stoch_ym2y(model, i_ym, Asm, Bsm, Csm, Dsm)
@@ -48,7 +48,7 @@ struct InternalModel{NT<:Real, SM<:SimModel} <: StateEstimator{NT}
         ŷs = zeros(NT, ny)
         direct = true # InternalModel always uses direct transmission from ym
         corrected = [false]
-        buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd)
+        buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nxi)
         return new{NT, SM}(
             model, 
             lastu0, x̂op, f̂op, x̂0, x̂d, x̂s, ŷs, x̂snext,
@@ -284,8 +284,8 @@ function update_estimate!(estim::InternalModel, _ , d0, u0)
     model = estim.model
     x̂d, x̂s, ŷs = estim.x̂d, estim.x̂s, estim.ŷs
     # -------------- deterministic model ---------------------
-    x̂dnext = estim.buffer.x̂
-    f!(x̂dnext, model, x̂d, u0, d0, model.p) 
+    x̂dnext, x0i = estim.buffer.x̂, estim.buffer.xi
+    f!(x̂dnext, x0i, model, x̂d, u0, d0, model.p) 
     x̂d .= x̂dnext # this also updates estim.x̂0 (they are the same object)
     # --------------- stochastic model -----------------------
     x̂snext = estim.x̂snext