Merge pull request #372 from JuliaControl/code_gen

doc: added C code generation example in "Manual: Linear Design"

Author: franckgaga

Project.toml

@@ -1,6 +1,6 @@
 name = "ModelPredictiveControl"
 uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c"
-version = "2.4.1"
+version = "2.4.2"
 authors = ["Francis Gagnon"]
 
 [deps]

docs/src/manual/linmpc.md

@@ -293,3 +293,102 @@ savefig("plot4_LinMPC.svg"); nothing # hide
 Note that measured disturbances are assumed constant in the future by default but custom
 ``\mathbf{D̂}`` predictions are possible. The same applies for the setpoint predictions
 ``\mathbf{R̂_y}``.
+
+## Generating C code
+
+The [`LinearMPC.jl`](@extref LinearMPC) package extension provides code generation
+capabilities to export the controller as optimized and standalone C code. First, install the
+package with:
+
+```test
+using Pkg; Pkg.add("LinearMPC")
+```
+
+The feedforward MPC controller can be converted to a [`LinearMPC.MPC`](@ref) object using:
+
+```@example 1
+import LinearMPC
+c_mpc_d = LinearMPC.MPC(mpc_d);
+nothing # hide
+```
+
+We test the converted controller in closed-loop to verify that it behaves identically to the
+original one, notably because of the two different solvers:
+
+```@example 1
+function test_c_mpc_d(c_mpc_d, model)
+    N = 200
+    ry, ul = [50, 30], 0
+    dop = 20
+    u = model.uop
+    u_data, y_data, ry_data = zeros(model.nu, N), zeros(model.ny, N), zeros(model.ny, N)
+    for i = 1:N
+        i == 51  && (ry = [50, 35])
+        i == 101 && (ry = [54, 30])
+        i == 151 && (ul = -20)
+        d = [ul .+ dop]
+        y = model()
+        x̂ = LinearMPC.correct_state!(c_mpc_d, y, d)
+        u = LinearMPC.compute_control(c_mpc_d, x̂; r=ry, d=d, uprev=u)
+        u_data[:,i], y_data[:,i], ry_data[:,i] = u, y, ry
+        LinearMPC.predict_state!(c_mpc_d, u, d)
+        updatestate!(model, u + [0; ul])
+    end
+    return u_data, y_data, ry_data
+end
+setstate!(model, zeros(model.nx))
+LinearMPC.set_state!(c_mpc_d, zeros(c_mpc_d.model.nx))
+u_data, y_data, ry_data = test_c_mpc_d(c_mpc_d, model)
+plot_data(t_data, u_data, y_data, ry_data)
+savefig("plot5_LinMPC.svg"); nothing # hide
+```
+
+![plot5_LinMPC](plot5_LinMPC.svg)
+
+The closed-loop simulation matches the results of the previous section, as expected. We
+can now generate the C code using:
+
+```julia
+LinearMPC.codegen(c_mpc_d; dir="codegen", fname="mpc_funcs")
+```
+
+The three C functions to call at each control period are declared in the generated
+`codegen/mpc_funcs.h` file, and they receive pointers of `c_float` arrays:
+
+```C
+void mpc_correct_state(c_float* state, c_float* measurement, c_float* disturbance);  
+int mpc_compute_control(c_float* control, c_float* state, c_float* reference, c_float* disturbance);
+void mpc_predict_state(c_float* state, c_float* control, c_float* disturbance);
+```
+
+For example, on Linux, you can add the following code in a new `main.c` file:
+
+```C
+#include "mpc_funcs.h" 
+#include <stdio.h>
+int main(){
+    // initialize arrays:
+    c_float u[2] = {20, 20};
+    c_float x[6] = {0, 0, 0, 0, 0, 0};
+    c_float r[2] = {50, 35};
+    c_float y[2] = {50, 30};
+    c_float d[1] = {20};
+    // execute one control period:
+    mpc_correct_state(x, y, d);
+    mpc_compute_control(u, x, r, d);
+    mpc_predict_state(x, u, d);
+    // print the computed control:
+    printf("The computed u value is: [%f, %f]\n", u[0], u[1]);
+    return 0;
+}
+```
+
+compile with using `gcc *.c -o main.bin` and run it with `./main.bin`. The printed `u` value
+should be identical to:
+
+```@example 1
+LinearMPC.set_state!(c_mpc_d, zeros(c_mpc_d.model.nx))
+x̂ = LinearMPC.correct_state!(c_mpc_d, [50, 30], [20])
+u = LinearMPC.compute_control(c_mpc_d, x̂; r=[50, 35], d=[20], uprev=[20, 20])
+println("The computed u value is: $(round.(u, digits=6))")
+```

src/controller/transcription.jl

@@ -1561,7 +1561,7 @@ deterministic states extracted from ``\mathbf{X̂_0}`` also in `Z̃`, and they c
 states at the beginning of the interval ``τ_0=0``. The ``\mathbf{k̇}_i`` derivative for the
 ``i``th collocation point is computed from the continuous-time function `model.f!` and:
 ```math
-\mathbf{k̇}_i(k+j) =  \mathbf{f}\Big(\mathbf{k}_i(k+j), \mathbf{û_i}(k+j), \mathbf{d̂}_i(k+j), \mathbf{p}\Big)
+\mathbf{k̇}_i(k+j) =  \mathbf{f}\Big(\mathbf{k}_i(k+j), \mathbf{û}_i(k+j), \mathbf{d̂}_i(k+j), \mathbf{p}\Big)
 ```
 Based on the normalized time ``τ_i ∈ [0, 1]`` and hold order `transcription.h`, the inputs
 and disturbances are piecewise constant or linear:

src/estimator/internal_model.jl

@@ -281,7 +281,7 @@ Update `estim.x̂0`/`x̂d`/`x̂s` with current inputs `u0`, measured outputs `y0
 The [`InternalModel`](@ref) updates the deterministic `x̂d` and stochastic `x̂s` estimates with:
 ```math
 \begin{aligned}
-    \mathbf{x̂_d}(k+1) &= \mathbf{f}\Big( \mathbf{x̂_d}(k), \mathbf{u}(k), \mathbf{d}(k) \Big) \\
+    \mathbf{x̂_d}(k+1) &= \mathbf{f}\Big( \mathbf{x̂_d}(k), \mathbf{u}(k), \mathbf{d}(k), \mathbf{p} \Big) \\
     \mathbf{x̂_s}(k+1) &= \mathbf{Â_s x̂_s}(k) + \mathbf{B̂_s ŷ_s}(k)
 \end{aligned}
 ```

src/estimator/mhe/construct.jl

@@ -438,9 +438,10 @@ MovingHorizonEstimator estimator with a sample time Ts = 10.0 s:
 
     The optimization and the update of the arrival covariance depend on `model`:
 
-    - If `model` is a [`LinModel`](@ref), the optimization is treated as a quadratic program
-      with a time-varying Hessian, which is generally cheaper than nonlinear programming. By
-      default, a [`KalmanFilter`](@ref) estimates the arrival covariance (customizable).
+    - If `model` is a [`LinModel`](@ref) and `nc=0`, the optimization is treated as a
+      quadratic program with a time-varying Hessian, which is generally cheaper than
+      nonlinear programming. By default, a [`KalmanFilter`](@ref) estimates the arrival
+      covariance (customizable).
     - Else, a nonlinear program with dense [`ForwardDiff`](@extref ForwardDiff) automatic
       differentiation (AD) compute the objective and constraint derivatives by default 
       (customizable). Optimizers generally benefit from exact derivatives like AD. However,