Merge pull request #38 from JuliaControl/continuous_nonlinmodel

starting support of continuous `NonLinModel` (RK4 only for now)

Author: franckgaga

Project.toml

@@ -1,7 +1,7 @@
 name = "ModelPredictiveControl"
 uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c"
 authors = ["Francis Gagnon"]
-version = "0.18.1"
+version = "0.19.0"
 
 [deps]
 ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e"

docs/src/manual/nonlinmpc.md

@@ -35,8 +35,8 @@ The plant model is nonlinear:
 
 in which ``g`` is the gravitational acceleration in m/s², ``L``, the pendulum length in m,
 ``K``, the friction coefficient at the pivot point in kg/s, and ``m``, the mass attached at
-the end of the pendulum in kg. Here, the explicit Euler method discretizes the system to
-construct a [`NonLinModel`](@ref):
+the end of the pendulum in kg. Here, a [fourth order Runge-Kutta method](https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods)
+solves the differential equations to construct a [`NonLinModel`](@ref):
 
 ```@example 1
 using ModelPredictiveControl
@@ -49,17 +49,17 @@ function pendulum(par, x, u)
     return [dθ, dω]
 end
 # declared constants, to avoid type-instability in the f function, for speed:
-const par, Ts = (9.8, 0.4, 1.2, 0.3), 0.1
-f(x, u, _ ) = x + Ts*pendulum(par, x, u) # Euler method
+const par = (9.8, 0.4, 1.2, 0.3)
+f(x, u, _ ) = pendulum(par, x, u)
 h(x, _ )    = [180/π*x[1]]  # [°]
-nu, nx, ny = 1, 2, 1
+Ts, nu, nx, ny = 0.1, 1, 2, 1
 model = NonLinModel(f, h, Ts, nu, nx, ny)
 ```
 
 The output function ``\mathbf{h}`` converts the ``θ`` angle to degrees. Note that special
 characters like ``θ`` can be typed in the Julia REPL or VS Code by typing `\theta` and
-pressing the `<TAB>` key. The tuple `par` and `Ts` are declared as constants here to improve
-the [performance](https://docs.julialang.org/en/v1/manual/performance-tips/#Avoid-untyped-global-variables).
+pressing the `<TAB>` key. The tuple `par` is constant here to improve the [performance](https://docs.julialang.org/en/v1/manual/performance-tips/#Avoid-untyped-global-variables).
+Note that a 4th order [`RungeKutta`](@ref) differential equation solver is used by default.
 It is good practice to first simulate `model` using [`sim!`](@ref) as a quick sanity check:
 
 ```@example 1
@@ -91,7 +91,7 @@ estimator tuning is tested on a plant with a 25 % larger friction coefficient ``
 
 ```@example 1
 const par_plant = (par[1], par[2], 1.25*par[3], par[4])
-f_plant(x, u, _) = x + Ts*pendulum(par_plant, x, u)
+f_plant(x, u, _ ) = pendulum(par_plant, x, u)
 plant = NonLinModel(f_plant, h, Ts, nu, nx, ny)
 res = sim!(estim, N, [0.5], plant=plant, y_noise=[0.5])
 plot(res, plotu=false, plotxwithx̂=true)
@@ -184,7 +184,7 @@ function JE(UE, ŶE, _ )
     τ, ω = UE[1:end-1], ŶE[2:2:end-1]
     return Ts*sum(τ.*ω)
 end
-empc = NonLinMPC(estim2, Hp=20, Hc=2, Mwt=[0.5, 0], Nwt=[2.5], Ewt=4.5e3, JE=JE)
+empc = NonLinMPC(estim2, Hp=20, Hc=2, Mwt=[0.5, 0], Nwt=[2.5], Ewt=3.5e3, JE=JE)
 empc = setconstraint!(empc, umin=[-1.5], umax=[+1.5])
 ```
 
@@ -245,9 +245,7 @@ We first linearize `model` at the point ``θ = π`` rad and ``ω = τ = 0`` (inv
 linmodel = linearize(model, x=[π, 0], u=[0])
 ```
 
-It is worth mentioning that the Euler method in `model` object is not the best choice for
-linearization since its accuracy is low (approximation of a poor approximation). A
-[`SteadyKalmanFilter`](@ref) and a [`LinMPC`](@ref) are designed from `linmodel`:
+A [`SteadyKalmanFilter`](@ref) and a [`LinMPC`](@ref) are designed from `linmodel`:
 
 ```@example 1
 kf  = SteadyKalmanFilter(linmodel; σQ, σR, nint_u, σQint_u)

docs/src/public/sim_model.md

@@ -29,6 +29,20 @@ LinModel
 NonLinModel
 ```
 
+## Differential Equation Solvers
+
+### DiffSolver
+
+```@docs
+ModelPredictiveControl.DiffSolver
+```
+
+### RungeKutta
+
+```@docs
+RungeKutta
+```
+
 ## Set Operating Points
 
 ```@docs

src/ModelPredictiveControl.jl

@@ -12,6 +12,7 @@ import OSQP, Ipopt
 
 
 export SimModel, LinModel, NonLinModel
+export DiffSolver, RungeKutta
 export setop!, setstate!, updatestate!, evaloutput, linearize
 export StateEstimator, InternalModel, Luenberger
 export SteadyKalmanFilter, KalmanFilter, UnscentedKalmanFilter, ExtendedKalmanFilter

src/controller/nonlinmpc.jl

@@ -145,7 +145,7 @@ This method uses the default state estimator :
 
 # Examples
 ```jldoctest
-julia> model = NonLinModel((x,u,_)->0.5x+u, (x,_)->2x, 10.0, 1, 1, 1);
+julia> model = NonLinModel((x,u,_)->0.5x+u, (x,_)->2x, 10.0, 1, 1, 1, solver=nothing);
 
 julia> mpc = NonLinMPC(model, Hp=20, Hc=1, Cwt=1e6)
 NonLinMPC controller with a sample time Ts = 10.0 s, Ipopt optimizer, UnscentedKalmanFilter estimator and:
@@ -166,8 +166,8 @@ NonLinMPC controller with a sample time Ts = 10.0 s, Ipopt optimizer, UnscentedK
 
     The optimization relies on [`JuMP`](https://github.com/jump-dev/JuMP.jl) automatic 
     differentiation (AD) to compute the objective and constraint derivatives. Optimizers 
-    generally benefit from exact derivatives like AD. However, the [`NonLinModel`](@ref) `f` 
-    and `h` functions must be compatible with this feature. See [Automatic differentiation](https://jump.dev/JuMP.jl/stable/manual/nlp/#Automatic-differentiation)
+    generally benefit from exact derivatives like AD. However, the [`NonLinModel`](@ref) 
+    state-space functions must be compatible with this feature. See [Automatic differentiation](https://jump.dev/JuMP.jl/stable/manual/nlp/#Automatic-differentiation)
     for common mistakes when writing these functions.
 
     Note that if `Cwt≠Inf`, the attribute `nlp_scaling_max_gradient` of `Ipopt` is set to 
@@ -221,7 +221,7 @@ Use custom state estimator `estim` to construct `NonLinMPC`.
 
 # Examples
 ```jldoctest
-julia> model = NonLinModel((x,u,_)->0.5x+u, (x,_)->2x, 10.0, 1, 1, 1);
+julia> model = NonLinModel((x,u,_)->0.5x+u, (x,_)->2x, 10.0, 1, 1, 1, solver=nothing);
 
 julia> estim = UnscentedKalmanFilter(model, σQint_ym=[0.05]);
 

src/estimator/kalman.jl

@@ -434,7 +434,7 @@ represents the measured outputs of ``\mathbf{ĥ}`` function (and unmeasured one
 
 # Examples
 ```jldoctest
-julia> model = NonLinModel((x,u,_)->0.1x+u, (x,_)->2x, 10.0, 1, 1, 1);
+julia> model = NonLinModel((x,u,_)->0.1x+u, (x,_)->2x, 10.0, 1, 1, 1, solver=nothing);
 
 julia> estim = UnscentedKalmanFilter(model, σR=[1], nint_ym=[2], σP0int_ym=[1, 1])
 UnscentedKalmanFilter estimator with a sample time Ts = 10.0 s, NonLinModel and:
@@ -685,7 +685,7 @@ automatic differentiation.
 
 # Examples
 ```jldoctest
-julia> model = NonLinModel((x,u,_)->0.2x+u, (x,_)->-3x, 5.0, 1, 1, 1);
+julia> model = NonLinModel((x,u,_)->0.2x+u, (x,_)->-3x, 5.0, 1, 1, 1, solver=nothing);
 
 julia> estim = ExtendedKalmanFilter(model, σQ=[2], σQint_ym=[2], σP0=[0.1], σP0int_ym=[0.1])
 ExtendedKalmanFilter estimator with a sample time Ts = 5.0 s, NonLinModel and:

src/estimator/mhe/construct.jl

@@ -206,7 +206,7 @@ matrix ``\mathbf{P̂}_{k-N_k}(k-N_k+1)`` is estimated with an [`ExtendedKalmanFi
 
 # Examples
 ```jldoctest
-julia> model = NonLinModel((x,u,_)->0.1x+u, (x,_)->2x, 10.0, 1, 1, 1);
+julia> model = NonLinModel((x,u,_)->0.1x+u, (x,_)->2x, 10.0, 1, 1, 1, solver=nothing);
 
 julia> estim = MovingHorizonEstimator(model, He=5, σR=[1], σP0=[0.01])
 MovingHorizonEstimator estimator with a sample time Ts = 10.0 s, Ipopt optimizer, NonLinModel and:

src/model/linearization.jl

@@ -22,7 +22,7 @@ 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, solver=nothing);
 
 julia> linmodel = linearize(model, x=[10.0], u=[0.0]); 
 
@@ -40,7 +40,8 @@ julia> linmodel.A
 function linearize(model::NonLinModel; x=model.x, u=model.uop, d=model.dop)
     u0, d0 = u - model.uop, d - model.dop
     xnext, y = similar(x), similar(model.yop)
-    y = model.h!(y, x, d0) .+ model.yop
+    model.h!(y, x, d0)
+    y .+= 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)

src/model/linmodel.jl

@@ -35,26 +35,35 @@ end
 
 Construct a linear model from state-space model `sys` with sampling time `Ts` in second.
 
-`Ts` can be omitted when `sys` is discrete-time. Its state-space matrices are:
+The system `sys` can be continuous or discrete-time (`Ts` can be omitted for the latter).
+For continuous dynamics, its state-space equations are (discrete case in Extended Help):
 ```math
 \begin{aligned}
-    \mathbf{x}(k+1) &= \mathbf{A x}(k) + \mathbf{B z}(k) \\
-    \mathbf{y}(k)   &= \mathbf{C x}(k) + \mathbf{D z}(k)
+    \mathbf{ẋ}(t) &= \mathbf{A x}(t) + \mathbf{B z}(t) \\
+    \mathbf{y}(t) &= \mathbf{C x}(t) + \mathbf{D z}(t)
 \end{aligned}
 ```
 with the state ``\mathbf{x}`` and output ``\mathbf{y}`` vectors. The ``\mathbf{z}`` vector 
 comprises the manipulated inputs ``\mathbf{u}`` and measured disturbances ``\mathbf{d}``, 
 in any order. `i_u` provides the indices of ``\mathbf{z}`` that are manipulated, and `i_d`, 
-the measured disturbances. See Extended Help if `sys` is continuous-time, or discrete-time
-with `Ts ≠ sys.Ts`.
+the measured disturbances. The constructor automatically discretizes continuous systems,
+resamples discrete ones if `Ts ≠ sys.Ts`, compute a new realization if not minimal, and 
+separate the ``\mathbf{z}`` terms in two parts (details in Extended Help). The rest of the
+documentation assumes discrete dynamics since all systems end up in this form.
 
-See also [`ss`](https://juliacontrol.github.io/ControlSystems.jl/stable/lib/constructors/#ControlSystemsBase.ss),
-[`tf`](https://juliacontrol.github.io/ControlSystems.jl/stable/lib/constructors/#ControlSystemsBase.tf).
+See also [`ss`](https://juliacontrol.github.io/ControlSystems.jl/stable/lib/constructors/#ControlSystemsBase.ss)
 
 # Examples
 ```jldoctest
-julia> model = LinModel(ss(0.4, 0.2, 0.3, 0, 0.1))
-Discrete-time linear model with a sample time Ts = 0.1 s and:
+julia> model1 = LinModel(ss(-0.1, 1.0, 1.0, 0), 2.0) # continuous-time StateSpace
+LinModel with a sample time Ts = 2.0 s and:
+ 1 manipulated inputs u
+ 1 states x
+ 1 outputs y
+ 0 measured disturbances d
+
+julia> model2 = LinModel(ss(0.4, 0.2, 0.3, 0, 0.1)) # discrete-time StateSpace
+LinModel with a sample time Ts = 0.1 s and:
  1 manipulated inputs u
  1 states x
  1 outputs y
@@ -63,9 +72,9 @@ Discrete-time linear model with a sample time Ts = 0.1 s and:
 
 # Extended Help
 !!! details "Extended Help"
-    State-space matrices are similar if `sys` is continuous (replace ``\mathbf{x}(k+1)``
-    with ``\mathbf{ẋ}(t)`` and ``k`` with ``t`` on the LHS). In such a case, it's 
-    discretized with [`c2d`](https://juliacontrol.github.io/ControlSystems.jl/stable/lib/constructors/#ControlSystemsBase.c2d)
+    State-space equations are similar if `sys` is discrete-time (replace ``\mathbf{ẋ}(t)``
+    with ``\mathbf{x}(k+1)`` and ``k`` with ``t`` on the LHS). Continuous dynamics are 
+    internally discretized using [`c2d`](https://juliacontrol.github.io/ControlSystems.jl/stable/lib/constructors/#ControlSystemsBase.c2d)
     and `:zoh` for manipulated inputs, and `:tustin`, for measured disturbances. Lastly, if 
     `sys` is discrete and the provided argument `Ts ≠ sys.Ts`, the system is resampled by
     using the aforementioned discretization methods.
@@ -124,8 +133,8 @@ function LinModel(
             sysd_dis = sysd
         end     
     end
-    # minreal to merge common poles if possible and ensure observability
-    sys_dis = minreal([sysu_dis sysd_dis])
+    # minreal to merge common poles if possible and ensure controllability/observability:
+    sys_dis = minreal([sysu_dis sysd_dis]) # same realization if already minimal
     nu  = length(i_u)
     A   = sys_dis.A
     Bu  = sys_dis.B[:,1:nu]
@@ -144,10 +153,12 @@ Convert to minimal realization state-space when `sys` is a transfer function.
 `sys` is equal to ``\frac{\mathbf{y}(s)}{\mathbf{z}(s)}`` for continuous-time, and 
 ``\frac{\mathbf{y}(z)}{\mathbf{z}(z)}``, for discrete-time.
 
+See also [`tf`](https://juliacontrol.github.io/ControlSystems.jl/stable/lib/constructors/#ControlSystemsBase.tf)
+
 # Examples
 ```jldoctest
 julia> model = LinModel([tf(3, [30, 1]) tf(-2, [5, 1])], 0.5, i_d=[2])
-Discrete-time linear model with a sample time Ts = 0.5 s and:
+LinModel with a sample time Ts = 0.5 s and:
  1 manipulated inputs u
  2 states x
  1 outputs y
@@ -177,9 +188,10 @@ end
 
 Construct the model from the discrete state-space matrices `A, Bu, C, Bd, Dd` directly.
 
-This syntax do not modify the state-space representation provided in argument (`minreal`
-is not called). Care must be taken to ensure that the model is controllable and observable.
-The optional parameter `NT` explicitly specifies the number type of the matrices.
+See [`LinModel(::StateSpace)`](@ref) Extended Help for the meaning of the matrices. This
+syntax do not modify the state-space representation provided in argument (`minreal` is not
+called). Care must be taken to ensure that the model is controllable and observable. The
+optional parameter `NT` explicitly specifies the number type of the matrices.
 """
 LinModel{NT}(A, Bu, C, Bd, Dd, Ts) where NT<:Real
 
@@ -245,6 +257,4 @@ function h!(y, model::LinModel, x, d)
     mul!(y, model.C,  x, 1, 1)
     mul!(y, model.Dd, d, 1, 1)
     return nothing
-end
-
-typestr(model::LinModel) = "linear"
+end