added: linearize NonLinModel with ForwardDiff

Author: franckgaga

docs/src/public/sim_model.md

@@ -34,3 +34,9 @@ NonLinModel
 ```@docs
 setop!
 ```
+
+## Linearize
+
+```@docs
+linearize
+```

src/ModelPredictiveControl.jl

@@ -11,7 +11,8 @@ using PreallocationTools
 import OSQP, Ipopt
 
 
-export SimModel, LinModel, NonLinModel, setop!, setstate!, updatestate!, evaloutput
+export SimModel, LinModel, NonLinModel
+export setop!, setstate!, updatestate!, evaloutput, linearize
 export StateEstimator, InternalModel, Luenberger
 export SteadyKalmanFilter, KalmanFilter, UnscentedKalmanFilter, ExtendedKalmanFilter
 export default_nint, initstate!

src/model/conversion.jl renamed to src/model/linearization.jl

@@ -1,7 +1,15 @@
 
-@doc raw"""
+"""
     LinModel(model::NonLinModel; x=model.x, u=model.uop, d=model.dop)
 
+Call [`linearize(model; x, u, d)`](@ref) and return the resulting linear model.
+```
+"""
+LinModel(model::NonLinModel; kwargs...) = linearize(model; kwargs...)
+
+@doc raw"""
+    linearize(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}``,
@@ -11,14 +19,16 @@ Jacobians of ``\mathbf{f}`` and ``\mathbf{h}`` functions are automatically compu
 
 ## Examples
 ```jldoctest
-julia> model = NonLinModel((x,u,_)->x.^3+u, (x,_)->x, 0.1, 1, 1, 1);
+julia> model = NonLinModel((x,u,_)->x.^3 + u, (x,_)->x, 0.1, 1, 1, 1);
+
+julia> linmodel = linearize(model, x=[10.0], u=[0.0]); 
 
-julia> linmodel = LinModel(model, x=[1.0]); linmodel.A
+julia> linmodel.A
 1×1 Matrix{Float64}:
- 3.0
+ 300.0
 ```
 """
-function LinModel(model::NonLinModel; x=model.x, u=model.uop, d=model.dop)
+function linearize(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

src/sim_model.jl

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

test/test_sim_model.jl

@@ -97,7 +97,6 @@ end
     @test_throws DimensionMismatch evaloutput(linmodel1, zeros(1))
 end
 
-
 @testset "NonLinModel construction" begin
     linmodel1 = LinModel(sys,Ts,i_u=[1,2])
     f1(x,u,_) = linmodel1.A*x + linmodel1.Bu*u
@@ -147,4 +146,24 @@ end
 
     @test_throws DimensionMismatch updatestate!(nonlinmodel, zeros(2), zeros(1))
     @test_throws DimensionMismatch evaloutput(nonlinmodel, zeros(1))
+end
+
+@testset "NonLinModel linearization" begin
+    Ts = 0.1
+    f1(x,u,d) = x.^5 + u.^4 + d.^3
+    h1(x,d)   = x.^2 + d
+    nonlinmodel1 = NonLinModel(f1,h1,0.1,1,1,1,1)
+    x, u, d = [2.0], [3.0], [4.0]
+    linmodel1 = linearize(nonlinmodel1; x, u, d)
+    @test linmodel1.A  ≈ 5*x.^4
+    @test linmodel1.Bu ≈ 4*u.^3
+    @test linmodel1.Bd ≈ 3*d.^2
+    @test linmodel1.C  ≈ 2*x.^1
+    @test linmodel1.Dd ≈ 1*d.^0
+    linmodel2 = LinModel(nonlinmodel1; x, u, d)
+    @test linmodel1.A  ≈ linmodel2.A
+    @test linmodel1.Bu ≈ linmodel2.Bu
+    @test linmodel1.Bd ≈ linmodel2.Bd
+    @test linmodel1.C  ≈ linmodel2.C
+    @test linmodel1.Dd ≈ linmodel2.Dd 
 end