added : linearize function

- automatic linearization based on `ForwardDiff`
- unit test for linearization
- doc: example for the pendulum in the manual

Author: franckgaga

docs/src/manual/linmpc.md

@@ -154,26 +154,17 @@ savefig(ans, "plot1_LinMPC.svg"); nothing # hide
 
 ![plot1_LinMPC](plot1_LinMPC.svg)
 
-For some situations, when [`LinMPC`](@ref) matrices are small/medium and dense, [`DAQP`](https://darnstrom.github.io/daqp/)
-optimizer may be more efficient. To install it, run:
-
-```text
-using Pkg; Pkg.add("DAQP")
-```
-
-Also, compared to the default setting, adding the integrating states at the model inputs may
-improve the closed-loop performance. Load disturbances are indeed very frequent in many
-real-life control problems. Constructing a [`LinMPC`](@ref) with `DAQP` and input integrators:
+Compared to the default setting, adding the integrating states at the model inputs may
+improve the closed-loop performance. Load disturbances are indeed very common in many
+real-life control problems. Constructing a [`LinMPC`](@ref) with input integrators:
 
 ```@example 1
-using JuMP, DAQP
-daqp = Model(DAQP.Optimizer, add_bridges=false)
-mpc2 = LinMPC(model, Hp=10, Hc=2, Mwt=[1, 1], Nwt=[0.1, 0.1], optim=daqp, nint_u=[1, 1])
+mpc2 = LinMPC(model, Hp=10, Hc=2, Mwt=[1, 1], Nwt=[0.1, 0.1], nint_u=[1, 1])
 mpc2 = setconstraint!(mpc2, ymin=[45, -Inf])
 ```
 
-leads to similar computational times, but it does accelerate the rejection of the load
-disturbance and eliminates the level constraint violation:
+does accelerate the rejection of the load disturbance and eliminates the level constraint
+violation:
 
 ```@example 1
 setstate!(model, zeros(model.nx))

docs/src/manual/nonlinmpc.md

@@ -130,12 +130,91 @@ satisfactory:
 
 ```@example 1
 res_yd = sim!(nmpc, N, [180.0], plant=plant, x0=[π, 0], x̂0=[π, 0, 0], y_step=[10])
+using BenchmarkTools
+@btime sim!(nmpc, N, [180.0], plant=plant, x0=[π, 0], x̂0=[π, 0, 0], y_step=[10])
 plot(res_yd)
 savefig(ans, "plot4_NonLinMPC.svg"); nothing # hide
 ```
 
 ![plot4_NonLinMPC](plot4_NonLinMPC.svg)
 
+## Linearizing the Model
+
+Nonlinear MPC are more computationally expensive than [`LinMPC`](@ref). Solving the problem
+should always be faster than the sampling time ``T_s = 0.1`` s for real-time operation. For
+electronic and mechanical systems like here, this requirement is sometimes harder to achieve
+because of their fast dynamics. To ease the design and comparison with [`LinMPC`](@ref), the
+[`linearize`](@ref) function allows automatic linearization of [`NonLinModel`](@ref) based
+on [`ForwardDiff.jl`](https://juliadiff.org/ForwardDiff.jl/stable/). We first linearize
+`model` at the operating points ``\mathbf{x} = [\begin{smallmatrix} π\\0 \end{smallmatrix}]``
+and ``\mathbf{u} = 0`` (inverted position):
+
+```@example 1
+linmodel = linearize(model, x=[π, 0], u=[0])
+```
+
+It is worth mentionning that the Euler method in `model` object is not the best choice for
+linearization since its accuracy is low (i.e. approximation of a bad approximation). A
+[`SteadyKalmanFilter`](@ref) and a [`LinMPC`](@ref) is designed from `linmodel`:
+
+```@example 1
+kf  = SteadyKalmanFilter(linmodel; σQ, σR, nint_u, σQint_u)
+mpc = LinMPC(kf, Hp=20, Hc=2, Mwt=[0.5], Nwt=[2.5])
+mpc = setconstraint!(mpc, umin=[-1.5], umax=[+1.5])
+```
+
+The linear controller has difficulties to reject the 10° step disturbance:
+
+```@example 1
+res_lin = sim!(mpc, N, [180.0]; plant, x0=[π, 0], y_step=[10])
+plot(res_lin)
+savefig(ans, "plot5_NonLinMPC.svg"); nothing # hide
+```
+
+![plot5_NonLinMPC](plot5_NonLinMPC.svg)
+
+Some improvements can be achieved by solving the optimization problem of `mpc` with [`DAQP`](https://darnstrom.github.io/daqp/)
+optimizer instead of the default `OSQP` solver. It is indeed documented that `DAQP` can
+perform better on small/medium dense matrices and unstable poles, which is obviously the
+case here (the absolute value of unstable poles are greater than one).:
+
+```@example 1
+using LinearAlgebra; poles = eigvals(linmodel.A)
+```
+
+To install it, run:
+
+```text
+using Pkg; Pkg.add("DAQP")
+```
+
+Constructing a [`LinMPC`](@ref) with `DAQP`:
+
+```@example 1
+using JuMP, DAQP
+daqp = Model(DAQP.Optimizer, add_bridges=false)
+mpc2 = LinMPC(kf, Hp=20, Hc=2, Mwt=[0.5], Nwt=[2.5], optim=daqp)
+mpc2 = setconstraint!(mpc, umin=[-1.5], umax=[+1.5])
+```
+
+does improve the rejection of the step disturbance:
+
+```@example 1
+res_lin2 = sim!(mpc2, N, [180.0]; plant, x0=[π, 0], y_step=[10])
+using BenchmarkTools
+@btime sim!(mpc2, N, [180.0]; plant, x0=[π, 0], y_step=[10])
+plot(res_lin2)
+savefig(ans, "plot6_NonLinMPC.svg"); nothing # hide
+```
+
+![plot6_NonLinMPC](plot6_NonLinMPC.svg)
+
+The performance is still lower than the nonlinear controller, as expected, but computations
+are about 2000 times faster (0.00002 s versus 0.04 s per time steps on average). Note that
+`linmodel` is only valid for angular position near 180°. Multiple linearized models and
+controllers are required for large deviations from this operating point. This is known as
+gain scheduling.
+
 ## Economic Model Predictive Controller
 
 Economic MPC can achieve the same objective but with lower economical costs. For this case
@@ -198,10 +277,10 @@ setpoint is:
 unset_time_limit_sec(empc.optim) # hide
 res2_ry = sim!(empc, N, [180, 0], plant=plant2, x0=[0, 0], x̂0=[0, 0, 0])
 plot(res2_ry)
-savefig(ans, "plot5_NonLinMPC.svg"); nothing # hide
+savefig(ans, "plot7_NonLinMPC.svg"); nothing # hide
 ```
 
-![plot5_NonLinMPC](plot5_NonLinMPC.svg)
+![plot7_NonLinMPC](plot7_NonLinMPC.svg)
 
 and the energy consumption is slightly lower:
 
@@ -218,10 +297,10 @@ Also, for a 10° step disturbance:
 ```@example 1
 res2_yd = sim!(empc, N, [180; 0]; plant=plant2, x0=[π, 0], x̂0=[π, 0, 0], y_step=[10, 0])
 plot(res2_yd)
-savefig(ans, "plot6_NonLinMPC.svg"); nothing # hide
+savefig(ans, "plot8_NonLinMPC.svg"); nothing # hide
 ```
 
-![plot6_NonLinMPC](plot6_NonLinMPC.svg)
+![plot8_NonLinMPC](plot8_NonLinMPC.svg)
 
 the new controller is able to recuperate more energy from the pendulum (i.e. negative work):
 

src/estimator/kalman.jl

@@ -632,7 +632,7 @@ identical to [`UnscentedKalmanFilter`](@ref). The Jacobians of the augmented mod
 automatic differentiation.
 
 !!! warning
-    See Extended Help if you get an error like:    
+    See the Extended Help of [`linearize`](@ref) function if you get an error like:    
     `MethodError: no method matching (::var"##")(::Vector{ForwardDiff.Dual})`.
 
 # Arguments
@@ -652,11 +652,6 @@ ExtendedKalmanFilter estimator with a sample time Ts = 5.0 s, NonLinModel and:
  0 unmeasured outputs yu
  0 measured disturbances d
 ```
-
-# Extended Help
-Automatic differentiation (AD) allows exact Jacobians. The [`NonLinModel`](@ref) `f` and `h`
-functions must be compatible with this feature though. See [Automatic differentiation](https://jump.dev/JuMP.jl/stable/manual/nlp/#Automatic-differentiation)
-for common mistakes when writing these functions.
 """
 function ExtendedKalmanFilter(
     model::SM;

src/model/linearization.jl

@@ -17,6 +17,10 @@ manipulated input ``\mathbf{u}`` and measured disturbance ``\mathbf{d}``, respec
 Jacobians of ``\mathbf{f}`` and ``\mathbf{h}`` functions are automatically computed with
 [`ForwardDiff.jl`](https://github.com/JuliaDiff/ForwardDiff.jl).
 
+!!! warning
+    See Extended Help if you get an error like:    
+    `MethodError: no method matching (::var"##")(::Vector{ForwardDiff.Dual})`.
+
 ## Examples
 ```jldoctest
 julia> model = NonLinModel((x,u,_)->x.^3 + u, (x,_)->x, 0.1, 1, 1, 1);
@@ -27,6 +31,11 @@ julia> linmodel.A
 1×1 Matrix{Float64}:
  300.0
 ```
+
+## Extended Help
+Automatic differentiation (AD) allows exact Jacobians. The [`NonLinModel`](@ref) `f` and `h`
+functions must be compatible with this feature though. See [Automatic differentiation](https://jump.dev/JuMP.jl/stable/manual/nlp/#Automatic-differentiation)
+for common mistakes when writing these functions.
 """
 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