added : linearize NonLinModel with ForwardDiff

Author: franckgaga

src/model/conversion.jl

@@ -0,0 +1,33 @@
+
+@doc raw"""
+    LinModel(model::NonLinModel; x=model.x, u=model.uop, d=model.dop)
+
+Linearize `model` around the operating points `x`, `u` and `d`.
+
+The arguments `x`, `u` and `d` are the linearization points for the state ``\mathbf{x}``,
+manipulated input ``\mathbf{u}`` and measured disturbance ``\mathbf{d}``, respectively. The
+Jacobians of ``\mathbf{f}`` and ``\mathbf{h}`` functions are automatically computed with
+[`ForwardDiff.jl`](https://github.com/JuliaDiff/ForwardDiff.jl).
+
+## Examples
+```jldoctest
+julia> model = NonLinModel((x,u,_)->x.^3+u, (x,_)->x, 0.1, 1, 1, 1);
+
+julia> linmodel = LinModel(model, x=[1.0]); linmodel.A
+1×1 Matrix{Float64}:
+ 3.0
+```
+"""
+function LinModel(model::NonLinModel; x=model.x, u=model.uop, d=model.dop)
+    nu, nx, ny, nd = model.nu, model.nx, model.ny, model.nd
+    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)
+    linmodel = LinModel(A, Bu, C, Bd, Dd, model.Ts, nu, nx, ny, nd)
+    setop!(linmodel, uop=u, yop=y, dop=d)
+    return linmodel
+end

src/plot_sim.jl

@@ -163,7 +163,7 @@ vectors. The simulated sensor and process noises of `plant` are specified by `y_
 - `d_step  = zeros(plant.nd)` : step on measured disturbances ``\mathbf{d}``.
 - `d_noise = zeros(plant.nd)` : additive gaussian noise on measured dist. ``\mathbf{d}``.
 - `x_noise = zeros(plant.nx)` : additive gaussian noise on plant states ``\mathbf{x}``.
-- `x0 = zeros(plant.nx)` : plant initial state ``\mathbf{x}(0)``.
+- `x0 = plant.x` : plant initial state ``\mathbf{x}(0)``.
 - `x̂0 = nothing` : initial estimate ``\mathbf{x̂}(0)``, [`initstate!`](@ref) is used if `nothing`.
 - `lastu = plant.uop` : last plant input ``\mathbf{u}`` for ``\mathbf{x̂}`` initialization.
 
@@ -245,7 +245,7 @@ function sim_closedloop!(
     d_step ::Vector = zeros(plant.nd),
     d_noise::Vector = zeros(plant.nd),
     x_noise::Vector = zeros(plant.nx),
-    x0 = zeros(plant.nx),
+    x0 = plant.x,
     x̂0 = nothing,
     lastu = plant.uop,
 )

src/sim_model.jl

@@ -168,4 +168,5 @@ evaloutput(model::SimModel, d=empty(model.x)) = h(model, model.x, d - model.dop)
 (model::SimModel)(d=empty(model.x)) = evaloutput(model::SimModel, d)
 
 include("model/linmodel.jl")
-include("model/nonlinmodel.jl")
+include("model/nonlinmodel.jl")
+include("model/conversion.jl")