Merge pull request #33 from JuliaControl/mutating_nonlinmodel

Added: mutating `NonLinModel`

Author: franckgaga

.github/workflows/CI.yml

@@ -36,4 +36,4 @@ jobs:
       - uses: julia-actions/julia-buildpkg@v1
       - uses: julia-actions/julia-runtest@v1
       - uses: julia-actions/julia-processcoverage@v1
-      - uses: codecov/codecov-action@v3
+      - uses: codecov/codecov-action@v4

docs/src/internals/sim_model.md

@@ -7,8 +7,8 @@ Pages = ["sim_model.md"]
 ## State-Space Functions
 
 ```@docs
-ModelPredictiveControl.f
-ModelPredictiveControl.h
+ModelPredictiveControl.f!
+ModelPredictiveControl.h!
 ```
 
 ## Steady-State Calculation

docs/src/internals/state_estim.md

@@ -7,8 +7,8 @@ Pages = ["state_estim.md"]
 ## Augmented Model
 
 ```@docs
-ModelPredictiveControl.f̂
-ModelPredictiveControl.ĥ
+ModelPredictiveControl.f̂!
+ModelPredictiveControl.ĥ!
 ```
 
 ## Constraint Relaxation

src/controller/execute.jl

@@ -226,7 +226,9 @@ function predictstoch!(
     isnothing(ym) && error("Predictive controllers with InternalModel need the measured "*
                            "outputs ym in keyword argument to compute control actions u")
     Ŷop, ny, yop = mpc.Ŷop, estim.model.ny, estim.model.yop
-    ŷd = h(estim.model, estim.x̂d, d - estim.model.dop) .+ estim.model.yop 
+    ŷd = similar(estim.model.yop)
+    h!(ŷd, estim.model, estim.x̂d, d - estim.model.dop)
+    ŷd .+= estim.model.yop 
     ŷs = zeros(NT, estim.model.ny)
     ŷs[estim.i_ym] .= @views ym .- ŷd[estim.i_ym]  # ŷs=0 for unmeasured outputs
     Ŷop_LHS = similar(Ŷop)
@@ -325,15 +327,18 @@ function predict!(
     nu, ny, nd, Hp, Hc = model.nu, model.ny, model.nd, mpc.Hp, mpc.Hc
     u0 = u
     x̂  .= mpc.estim.x̂
+    x̂next = similar(x̂) # TODO: avoid this allocation if possible
     u0 .= mpc.estim.lastu0
     d0  = @views mpc.d0[1:end]
     for j=1:Hp
         if j ≤ Hc
             u0 .+= @views ΔŨ[(1 + nu*(j-1)):(nu*j)]
         end
-        x̂[:]  = f̂(mpc.estim, model, x̂, u0, d0)
-        d0    = @views mpc.D̂0[(1 + nd*(j-1)):(nd*j)]
-        Ŷ[(1 + ny*(j-1)):(ny*j)] = ĥ(mpc.estim, model, x̂, d0)
+        f̂!(x̂next, mpc.estim, model, x̂, u0, d0)
+        x̂ .= x̂next
+        d0 = @views mpc.D̂0[(1 + nd*(j-1)):(nd*j)]
+        ŷ  = @views Ŷ[(1 + ny*(j-1)):(ny*j)]
+        ĥ!(ŷ, mpc.estim, model, x̂, d0)
     end
     Ŷ .= Ŷ .+ mpc.Ŷop # Ŷop = Ŷs + Yop, and Ŷs=0 if mpc.estim is not an InternalModel
     x̂end = x̂

src/estimator/execute.jl

@@ -15,9 +15,9 @@ function remove_op!(estim::StateEstimator, u, ym, d)
 end
 
 @doc raw"""
-    f̂(estim::StateEstimator, model::SimModel, x̂, u, d)
+    f̂!(x̂next, estim::StateEstimator, model::SimModel, x̂, u, d) -> x̂next
 
-State function ``\mathbf{f̂}`` of the augmented model.
+Mutating state function ``\mathbf{f̂}`` of the augmented model.
 
 By introducing an augmented state vector ``\mathbf{x̂}`` like in [`augment_model`](@ref), the
 function returns the next state of the augmented model, defined as:
@@ -26,28 +26,48 @@ function returns the next state of the augmented model, defined as:
     \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) 
 \end{aligned}
+where ``\mathbf{x̂}(k+1)`` is stored in `x̂next` argument.
 ```
 """
-function f̂(estim::StateEstimator, model::SimModel, x̂, u, d)
+function f̂!(x̂next, estim::StateEstimator, model::SimModel, x̂, u, d)
     # `@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]
-    return [f(model, x̂d, u + estim.Cs_u*x̂s, d); estim.As*x̂s]
+    @views x̂d_next, x̂s_next = x̂next[1:model.nx], x̂next[model.nx+1:end]
+    T = promote_type(eltype(x̂), eltype(u), eltype(d))
+    u_us = Vector{T}(undef, model.nu) # TODO: avoid this allocation if possible
+    u_us .= u .+ mul!(u_us, estim.Cs_u, x̂s)
+    f!(x̂d_next, model, x̂d, u_us, d)
+    mul!(x̂s_next, estim.As, x̂s)
+    return x̂next
 end
 "Use the augmented model matrices if `model` is a [`LinModel`](@ref)."
-f̂(estim::StateEstimator, ::LinModel, x̂, u, d) = estim. * x̂ + estim.B̂u * u + estim.B̂d * d
+function f̂!(x̂next, estim::StateEstimator, ::LinModel, x̂, u, d)
+    x̂next .= 0
+    mul!(x̂next, estim.Â,  x̂, 1, 1)
+    mul!(x̂next, estim.B̂u, u, 1, 1)
+    mul!(x̂next, estim.B̂d, d, 1, 1)
+    return x̂next
+end
 
 @doc raw"""
-    ĥ(estim::StateEstimator, model::SimModel, x̂, d)
+    ĥ!(ŷ, estim::StateEstimator, model::SimModel, x̂, d) -> ŷ
 
-Output function ``\mathbf{ĥ}`` of the augmented model, see [`f̂`](@ref) for details.
+Mutating output function ``\mathbf{ĥ}`` of the augmented model, see [`f̂!`](@ref).
 """
-function ĥ(estim::StateEstimator, model::SimModel, x̂, d)
+function ĥ!(ŷ, estim::StateEstimator, model::SimModel, x̂, d)
     # `@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]
-    return h(model, x̂d, d) + estim.Cs_y*x̂s
+    h!(ŷ, model, x̂d, d)
+    mul!(ŷ, estim.Cs_y, x̂s, 1, 1)
+    return ŷ
 end
 "Use the augmented model matrices if `model` is a [`LinModel`](@ref)."
-ĥ(estim::StateEstimator, ::LinModel, x̂, d) = estim.Ĉ * x̂ + estim.D̂d * d
+function ĥ!(ŷ, estim::StateEstimator, ::LinModel, x̂, d)
+    ŷ .= 0
+    mul!(ŷ, estim.Ĉ,  x̂, 1, 1)
+    mul!(ŷ, estim.D̂d, d, 1, 1)
+    return ŷ
+end
 
 
 @doc raw"""
@@ -147,8 +167,12 @@ julia> ŷ = evaloutput(kf)
  20.0
 ```
 """
-function evaloutput(estim::StateEstimator, d=empty(estim.x̂)) 
-    return ĥ(estim, estim.model, estim.x̂, d - estim.model.dop) + estim.model.yop
+function evaloutput(estim::StateEstimator{NT}, d=empty(estim.x̂)) where NT <: Real
+    validate_args(estim.model, d)
+    ŷ = Vector{NT}(undef, estim.model.ny)
+    ĥ!(ŷ, estim, estim.model, estim.x̂, d - estim.model.dop)
+    ŷ .+= estim.model.yop
+    return ŷ
 end
 
 "Functor allowing callable `StateEstimator` object as an alias for `evaloutput`."
@@ -186,7 +210,7 @@ updatestate!(::StateEstimator, _ ) = throw(ArgumentError("missing measured outpu
 Check `u`, `ym` and `d` sizes against `estim` dimensions.
 """
 function validate_args(estim::StateEstimator, u, ym, d)
-    validate_args(estim.model, u, d)
+    validate_args(estim.model, d, u)
     nym = estim.nym
     size(ym) ≠ (nym,) && throw(DimensionMismatch("ym size $(size(ym)) ≠ meas. output size ($nym,)"))
 end

src/estimator/internal_model.jl

@@ -144,20 +144,20 @@ function matrices_internalmodel(model::SimModel{NT}) where NT<:Real
 end
 
 @doc raw"""
-    f̂(::InternalModel, model::NonLinModel, x̂, u, d)
+    f̂!(x̂next, ::InternalModel, model::NonLinModel, x̂, u, d)
 
 State function ``\mathbf{f̂}`` of [`InternalModel`](@ref) for [`NonLinModel`](@ref).
 
-It calls `model.f(x̂, u ,d)` since this estimator does not augment the states.
+It calls `model.f!(x̂next, x̂, u ,d)` since this estimator does not augment the states.
 """
-f̂(::InternalModel, model::NonLinModel, x̂, u, d) = model.f(x̂, u, d)
+f̂!(x̂next, ::InternalModel, model::NonLinModel, x̂, u, d) = model.f!(x̂next, x̂, u, d)
 
 @doc raw"""
-    ĥ(::InternalModel, model::NonLinModel, x̂, d)
+    ĥ!(ŷ, ::InternalModel, model::NonLinModel, x̂, d)
 
-Output function ``\mathbf{ĥ}`` of [`InternalModel`](@ref), it calls `model.h`.
+Output function ``\mathbf{ĥ}`` of [`InternalModel`](@ref), it calls `model.h!`.
 """
-ĥ(::InternalModel, model::NonLinModel, x̂, d) = model.h(x̂, d)
+ĥ!(x̂next, ::InternalModel, model::NonLinModel, x̂, d) = model.h!(x̂next, x̂, d)
 
 
 @doc raw"""
@@ -218,12 +218,17 @@ function update_estimate!(
     model = estim.model
     x̂d, x̂s = estim.x̂d, estim.x̂s
     # -------------- deterministic model ---------------------
-    ŷd = h(model, x̂d, d)
-    x̂d[:] = f(model, x̂d, u, d) # this also updates estim.x̂ (they are the same object)
+    ŷd, x̂dnext = Vector{NT}(undef, model.ny), Vector{NT}(undef, model.nx)
+    h!(ŷd, model, x̂d, d)
+    f!(x̂dnext, model, x̂d, u, d) 
+    x̂d .= x̂dnext # this also updates estim.x̂ (they are the same object)
     # --------------- stochastic model -----------------------
+    x̂snext = Vector{NT}(undef, estim.nxs)
     ŷs = zeros(NT, model.ny)
     ŷs[estim.i_ym] = ym - ŷd[estim.i_ym]   # ŷs=0 for unmeasured outputs
-    x̂s[:] = estim.Âs*x̂s + estim.B̂s*ŷs
+    mul!(x̂snext, estim.Âs, x̂s)
+    mul!(x̂snext, estim.B̂s, ŷs, 1, 1)
+    x̂s .= x̂snext
     return nothing
 end
 
@@ -247,14 +252,32 @@ This estimator does not augment the state vector, thus ``\mathbf{x̂ = x̂_d}``.
 """
 function init_estimate!(estim::InternalModel, model::LinModel{NT}, u, ym, d) where NT<:Real
     x̂d, x̂s = estim.x̂d, estim.x̂s
-    x̂d[:] = (I - model.A)\(model.Bu*u + model.Bd*d)
-    ŷd = h(model, x̂d, d)
+    x̂d .= (I - model.A)\(model.Bu*u + model.Bd*d)
+    ŷd = Vector{NT}(undef, model.ny)
+    h!(ŷd, model, x̂d, d)
     ŷs = zeros(NT, model.ny)
     ŷs[estim.i_ym] = ym - ŷd[estim.i_ym]  # ŷs=0 for unmeasured outputs
-    x̂s[:] = (I-estim.Âs)\estim.B̂s*ŷs
+    x̂s .= (I-estim.Âs)\estim.B̂s*ŷs
     return nothing
 end
 
+@doc raw"""
+    evalŷ(estim::InternalModel, ym, d) -> ŷ
+
+Get [`InternalModel`](@ref) output `ŷ` from current measured outputs `ym` and dist. `d`.
+
+[`InternalModel`](@ref) estimator needs current measured outputs ``\mathbf{y^m}(k)`` to 
+estimate its outputs ``\mathbf{ŷ}(k)``, since the strategy imposes that 
+``\mathbf{ŷ^m}(k) = \mathbf{y^m}(k)`` is always true.
+"""
+function evalŷ(estim::InternalModel{NT}, ym, d) where NT<:Real
+    ŷ = Vector{NT}(undef, estim.model.ny)
+    h!(ŷ, estim.model, estim.x̂d, d - estim.model.dop) 
+    ŷ .+= estim.model.yop
+    ŷ[estim.i_ym] = ym
+    return ŷ
+end
+
 "Print InternalModel information without i/o integrators."
 function print_estim_dim(io::IO, estim::InternalModel, n)
     nu, nd = estim.model.nu, estim.model.nd

src/estimator/kalman.jl

@@ -567,8 +567,9 @@ function update_estimate!(estim::UnscentedKalmanFilter{NT}, u, ym, d) where NT<:
     nym, nx̂, nσ = estim.nym, estim.nx̂, estim.nσ
     γ, m̂, Ŝ = estim.γ, estim.m̂, estim.Ŝ
     # --- initialize matrices ---
-    X̂  = Matrix{NT}(undef, nx̂, nσ)
+    X̂, X̂_next = Matrix{NT}(undef, nx̂, nσ), Matrix{NT}(undef, nx̂, nσ)
     ŷm = Vector{NT}(undef, nym)
+    ŷ  = Vector{NT}(undef, estim.model.ny)
     Ŷm = Matrix{NT}(undef, nym, nσ)
     sqrt_P̂ = LowerTriangular{NT, Matrix{NT}}(Matrix{NT}(undef, nx̂, nx̂))
     # --- correction step ---
@@ -578,7 +579,8 @@ function update_estimate!(estim::UnscentedKalmanFilter{NT}, u, ym, d) where NT<:
     X̂[:, 2:nx̂+1]   .+= γ_sqrt_P̂
     X̂[:, nx̂+2:end] .-= γ_sqrt_P̂
     for j in axes(Ŷm, 2)
-        Ŷm[:, j] = @views ĥ(estim, estim.model, X̂[:, j], d)[estim.i_ym]
+        @views ĥ!(ŷ, estim, estim.model, X̂[:, j], d)
+        @views Ŷm[:, j] .= ŷ[estim.i_ym]
     end
     mul!(ŷm, Ŷm, m̂)
     X̄, Ȳm = X̂, Ŷm
@@ -598,9 +600,9 @@ function update_estimate!(estim::UnscentedKalmanFilter{NT}, u, ym, d) where NT<:
     X̂_cor .= x̂_cor
     X̂_cor[:, 2:nx̂+1]   .+= γ_sqrt_P̂_cor
     X̂_cor[:, nx̂+2:end] .-= γ_sqrt_P̂_cor
-    X̂_next = X̂_cor
+    X̂_next = similar(X̂_cor)
     for j in axes(X̂_next, 2)
-        X̂_next[:, j] = @views f̂(estim, estim.model, X̂_cor[:, j], u, d)
+        @views f̂!(X̂_next[:, j], estim, estim.model, X̂_cor[:, j], u, d)
     end
     x̂_next = mul!(x̂, X̂_next, m̂)
     X̄_next = X̂_next
@@ -757,9 +759,13 @@ automatically computes the Jacobians:
 ```
 The matrix ``\mathbf{Ĥ^m}`` is the rows of ``\mathbf{Ĥ}`` that are measured outputs.
 """
-function update_estimate!(estim::ExtendedKalmanFilter, u, ym, d=empty(estim.x̂))
-    F̂ = ForwardDiff.jacobian(x̂ -> f̂(estim, estim.model, x̂, u, d), estim.x̂)
-    Ĥ = ForwardDiff.jacobian(x̂ -> ĥ(estim, estim.model, x̂, d), estim.x̂)
+function update_estimate!(
+    estim::ExtendedKalmanFilter{NT}, u, ym, d=empty(estim.x̂)
+) where NT<:Real
+    model = estim.model
+    x̂next, ŷ = Vector{NT}(undef, estim.nx̂), Vector{NT}(undef, model.ny)
+    F̂ = ForwardDiff.jacobian((x̂next, x̂) -> f̂!(x̂next, estim, model, x̂, u, d), x̂next, estim.x̂)
+    Ĥ = ForwardDiff.jacobian((ŷ, x̂)     -> ĥ!(ŷ, estim, model, x̂, d), ŷ, estim.x̂)
     return update_estimate_kf!(estim, u, ym, d, F̂, Ĥ[estim.i_ym, :], estim.P̂, estim.x̂)
 end
 
@@ -790,7 +796,7 @@ function validate_kfcov(nym, nx̂, Q̂, R̂, P̂0=nothing)
 end
 
 """
-    update_estimate_kf!(estim, u, ym, d, Â, Ĉm, P̂, x̂=nothing)
+    update_estimate_kf!(estim::StateEstimator, u, ym, d, Â, Ĉm, P̂, x̂=nothing)
 
 Update time-varying/extended Kalman Filter estimates with augmented `Â` and `Ĉm` matrices.
 
@@ -801,16 +807,22 @@ The implementation uses in-place operations and explicit factorization to reduce
 allocations. See e.g. [`KalmanFilter`](@ref) docstring for the equations. If `isnothing(x̂)`,
 only the covariance `P̂` is updated.
 """
-function update_estimate_kf!(estim, u, ym, d, Â, Ĉm, P̂, x̂=nothing)
+function update_estimate_kf!(
+    estim::StateEstimator{NT}, u, ym, d, Â, Ĉm, P̂, x̂=nothing
+) where NT<:Real
     Q̂, R̂, M̂ = estim.Q̂, estim.R̂, estim.M̂
     mul!(M̂, P̂, Ĉm')
     rdiv!(M̂, cholesky!(Hermitian(Ĉm * P̂ * Ĉm' .+ R̂)))
     if !isnothing(x̂)
         mul!(estim.K̂, Â, M̂)
-        ŷm = @views ĥ(estim, estim.model, x̂, d)[estim.i_ym]
+        x̂next, ŷ = Vector{NT}(undef, estim.nx̂), Vector{NT}(undef, estim.model.ny)
+        ĥ!(ŷ, estim, estim.model, x̂, d)
+        ŷm = @views ŷ[estim.i_ym]
         v̂  = ŷm
         v̂ .= ym .- ŷm
-        x̂ .= f̂(estim, estim.model, x̂, u, d) .+ mul!(x̂, estim.K̂, v̂)
+        f̂!(x̂next, estim, estim.model, x̂, u, d)
+        mul!(x̂next, estim.K̂, v̂, 1, 1)
+        estim.x̂ .= x̂next
     end
     P̂.data .= Â * (P̂ .- M̂ * Ĉm * P̂) * Â' .+ Q̂ # .data is necessary for Hermitians
     return nothing

src/estimator/mhe/execute.jl

@@ -137,8 +137,9 @@ function getinfo(estim::MovingHorizonEstimator{NT}) where NT<:Real
     Ŷ = Vector{NT}(undef, ny*Nk)
     for i=1:Nk
         d0 = @views D[(1 + nd*(i-1)):(nd*i)] # operating point already removed in estim.D
-        x̂ = @views X̂[(1 + nx̂*(i-1)):(nx̂*i)]
-        Ŷ[(1 + ny*(i-1)):(ny*i)] = ĥ(estim, model, x̂, d0) + model.yop
+        x̂  = @views X̂[(1 + nx̂*(i-1)):(nx̂*i)]
+        @views ĥ!(Ŷ[(1 + ny*(i-1)):(ny*i)], estim, model, x̂, d0)
+        Ŷ[(1 + ny*(i-1)):(ny*i)] .+= model.yop
     end
     Ŷm = Ym - V̂
     info[:Ŵ] = estim.Ŵ[1:Nk*nŵ]
@@ -326,10 +327,11 @@ function update_cov!(P̂, estim::MovingHorizonEstimator, ::LinModel, u, ym, d)
     return update_estimate_kf!(estim, u, ym, d, estim.Â, estim.Ĉ[estim.i_ym, :], P̂)
 end
 "Update it with the `ExtendedKalmanFilter` if model is not a `LinModel`."
-function update_cov!(P̂, estim::MovingHorizonEstimator, ::SimModel, u, ym, d) 
+function update_cov!(P̂, estim::MovingHorizonEstimator, model::SimModel, u, ym, d) 
     # TODO: also support UnscentedKalmanFilter
-    F̂ = ForwardDiff.jacobian(x̂ -> f̂(estim, estim.model, x̂, u, d), estim.x̂)
-    Ĥ = ForwardDiff.jacobian(x̂ -> ĥ(estim, estim.model, x̂, d), estim.x̂)
+    x̂next, ŷ = similar(estim.x̂), similar(model.yop)
+    F̂ = ForwardDiff.jacobian((x̂next, x̂) -> f̂!(x̂next, estim, estim.model, x̂, u, d), x̂next, estim.x̂)
+    Ĥ = ForwardDiff.jacobian((ŷ, x̂)     -> ĥ!(ŷ, estim, estim.model, x̂, d), ŷ, estim.x̂)
     return update_estimate_kf!(estim, u, ym, d, F̂, Ĥ[estim.i_ym, :],  P̂)
 end
 
@@ -394,16 +396,22 @@ function predict!(
     V̂, X̂, estim::MovingHorizonEstimator, model::SimModel, Z̃::Vector{T}
 ) where {T<:Real}
     Nk = estim.Nk[]
-    nu, nd, nx̂, nŵ, nym = model.nu, model.nd, estim.nx̂, estim.nx̂, estim.nym
+    nu, nd, ny, nx̂, nŵ, nym = model.nu, model.nd, model.ny, estim.nx̂, estim.nx̂, estim.nym
     nx̃ = !isinf(estim.C) + nx̂
+    # TODO: avoid these two allocations if possible:
+    ŷ, x̂next = Vector{T}(undef, ny), Vector{T}(undef, nx̂)
     x̂ = @views Z̃[nx̃-nx̂+1:nx̃]
     for j=1:Nk
         u  = @views estim.U[ (1 + nu  * (j-1)):(nu*j)]
         ym = @views estim.Ym[(1 + nym * (j-1)):(nym*j)]
         d  = @views estim.D[ (1 + nd  * (j-1)):(nd*j)]
         ŵ  = @views Z̃[(1 + nx̃ + nŵ*(j-1)):(nx̃ + nŵ*j)]
-        V̂[(1 + nym*(j-1)):(nym*j)] = ym - ĥ(estim, model, x̂, d)[estim.i_ym]
-        X̂[(1 + nx̂ *(j-1)):(nx̂ *j)] = f̂(estim, model, x̂, u, d) + ŵ
+        ĥ!(ŷ, estim, model, x̂, d)
+        ŷm = @views ŷ[estim.i_ym]
+        V̂[(1 + nym*(j-1)):(nym*j)] .= ym .- ŷm
+        f̂!(x̂next, estim, model, x̂, u, d)
+        x̂next .+= ŵ
+        X̂[(1 + nx̂ *(j-1)):(nx̂ *j)] = x̂next
         x̂ = @views X̂[(1 + nx̂*(j-1)):(nx̂*j)]
     end
     return V̂, X̂

src/model/linearization.jl

@@ -39,12 +39,13 @@ julia> linmodel.A
 """
 function linearize(model::NonLinModel; x=model.x, u=model.uop, d=model.dop)
     u0, d0 = u - model.uop, d - model.dop
-    y  = model.h(x, d0) + model.yop
-    A  = ForwardDiff.jacobian(x  -> model.f(x, u0, d0), x)
-    Bu = ForwardDiff.jacobian(u0 -> model.f(x, u0, d0), u0)
-    Bd = ForwardDiff.jacobian(d0 -> model.f(x, u0, d0), d0)
-    C  = ForwardDiff.jacobian(x  -> model.h(x, d0), x)
-    Dd = ForwardDiff.jacobian(d0 -> model.h(x, d0), d0)
+    xnext, y = similar(x), similar(model.yop)
+    y = model.h!(y, x, d0) .+ model.yop
+    A  = ForwardDiff.jacobian((xnext, x)  -> model.f!(xnext, x, u0, d0), xnext, x)
+    Bu = ForwardDiff.jacobian((xnext, u0) -> model.f!(xnext, x, u0, d0), xnext, u0)
+    Bd = ForwardDiff.jacobian((xnext, d0) -> model.f!(xnext, x, u0, d0), xnext, d0)
+    C  = ForwardDiff.jacobian((y, x)  -> model.h!(y, x, d0), y, x)
+    Dd = ForwardDiff.jacobian((y, d0) -> model.h!(y, x, d0), y, d0)
     linmodel = LinModel(A, Bu, C, Bd, Dd, model.Ts)
     setop!(linmodel, uop=u, yop=y, dop=d)
     return linmodel