text:
# Arduino_Constrained_MPC_Library
This is a compact **sub-millisecond** Constrained (linear) Model Predictive Control (MPC) library for Teensy4.0/Arduino system (or real time embedded system in general).
- It's not using Eigen (small source code - more simple to understand).
- It's not using C++ Standard Library/std (for embedded consideration).
- If you set `SYSTEM_IMPLEMENTATION` to `SYSTEM_IMPLEMENTATION_EMBEDDED_NO_PRINT` in `konfig.h`, the code is platform agnostic (not using any library beside these C header files: `stdlib.h`, `stdint.h`, `stdbool.h`, `string.h`, and `math.h`).
- There's no malloc/new/free dynamic memory allocation for real time application (but using heavy stack local variables, so you need to run it through static memory analyzer if you are really concerned about implement this in mission critical hard real time application).

The unconstrained MPC version can be found in [my other repository](https://github.com/pronenewbits/Arduino_Unconstrained_MPC_Library/).

# The Background
This is the continuation of my previous repo [Arduino_Unconstrained_MPC_Library](https://github.com/pronenewbits/Arduino_Unconstrained_MPC_Library/), so you should read it before continue. As with my other repo, the main goal is for the student to learn the MPC concept (I've made decision to sacrifice speed to get best code readability I could get) while still capable of tackling real-time control system implementation. For non-trivia application, the code is computed in around **886 us** for naive implementation, **327 us** for the optimized version, and **127 us** for the optimized version without matrix bounds checking (even as low as **13-28 us** for a lower quality control output)! See [*Some Benchmark*](README.md#some-benchmark) section below.

To recap, the MPC formula derivation can be described as (I'm using Jan Maciejowski's *Predictive Control with Constraints* as reference, great book btw) :

As we can see here, the formulation of constrained MPC is exactly the same as
the unconstrained version. The difference is on the optimal control problem definition where the constrained version, well, has constraints.

We have 3 type of constraints:
1.  constraint (i.e. ), or the actuator slew rate constraint.
2.  constraint (i.e. ), or the actuator output constraint.
3.  constraint (i.e. ), or the system output constraint.

**Remark**: To make the explanation simple and complete, in this implementation I implemented all constraints. But in the real implementation, you don't need to. For example, for  you can implement the minimum constraint only (without the maximum constraint) . Or for example if you have 3 input, you can limit only the second input  and set free the other. *Actually it is preferred (best practice) to implement hard-contraints as little as possible to ensure maximum feasible search space*.

The constraints formulation can be described as:

welp, I guess that's all XD

Then we can describe the full formulation of constrained MPC problem:

**Note**: From this point onward, sometimes I'll use `MPC` subscript to denote MPC variables and `QP` subscript to denote Quadratic Programming solver variables. But if the context is known (e.g. In the MPC subsection/Active Set subsection), I'll drop the subscript to shorten the explanation.

In the formulation above,  means when we call the QP solver from MPC subroutine, we need to scale the MPC variables accordingly .

# The Quadratic Programming Solver
In this implementation, I use (one of [several](https://en.wikipedia.org/wiki/Quadratic_programming#Solution_methods)) QP solver called [Active Set methods (ASM)](https://en.wikipedia.org/wiki/Active-set_method). The big idea of active set algorithm is searching the optimal  value by solving the QP problem (with inequality constraints) as QP problem with equality constraints (EQP). More concretely, we search  by iterating through EQP subproblem, and construct the EQP subproblem (more specifically the constraint matrix) from the set of QP constraints that is active at that iteration (hence the name).

In the wild, many recent reseach are using the more 'sexy' solver like [Interior Point methods (IPM)](https://en.wikipedia.org/wiki/Interior-point_method) because the IPM is faster for big QP problem. But in our case (i.e. MPC implementation for embedded system) we mainly deal with small (less than ten) state variables/actuator and small (less than twenty) prediction horizon (we're limited by the RAM size after all). In that case, the ASM will be faster than IPM. And by exploiting the MPC structure, we can reduce the computation time even further while increase the numerical stability (see [Active Set note: Solving the Linear Equation of KKT system](README.md#active-set-note-solving-the-linear-equation-of-kkt-system) subsection and [The MPC Implementation](README.md#the-second-implementation-optimized-active-set-with-schur-complement-of-kkt) subsection below).

On the [Wikipedia page](https://en.wikipedia.org/wiki/Active-set_method), the general structure of an active set algorithm is:
```
Find a feasible starting point
repeat until "optimal enough"
    solve the equality problem defined by the active set (approximately)
    compute the Lagrange multipliers of the active set
    remove a subset of the constraints with negative Lagrange multipliers
    search for infeasible constraints
end repeat
```

Using above structure, the active set algorithm used in this implementation can be described as:
![Active Set Algorithm](documentation_resources/ActiveSet.png "Click to maximize if the image rescaling make you dizzy")

**Remark**: If the global optimum is inside the feasible region (i.e. the global optimum doesn't violate any constraints) and is not on the edge of any constraint, then it means there is no constraint active and the active set matrix  disappear. The KKT system equation simply becomes:

As , then the optimal control calculation becomes  which exactly is the equation for [unconstrained MPC](https://github.com/pronenewbits/Arduino_Unconstrained_MPC_Library/blob/master/Formulation_of_Optimal_Control.png)! With this we can prove that the constrained MPC using active set solver is a generalization of unconstrained MPC (math is pretty nifty huh).

#### Active Set note: The Termination Condition
The termination condition on the last line of algorithm above  is called [Karush–Kuhn–Tucker conditions](https://en.wikipedia.org/wiki/Karush%E2%80%93Kuhn%E2%80%93Tucker_conditions). As the condition is applied on the solution of the KKT system on EQP subproblem (first line on iteration), then we can move the termination check just after we solve the KKT system.

#### Active Set note: Programming Implementation
The simplest way to implement the active set  is by using an array of boolean, where the size of the array is as big as the number of constraints. That way, the operation of adding or removing the *i*-th constraint into/from  (i.e. the  and  operations) simply becomes the operation of setting the *i*-th value of the array as `true` or `false`, accordingly.

#### Active Set note: Generating the Feasible Starting Point
todo: calculate the starting point using linear programming. For now we set the starting point as zero (ok if the null space of x is inside the feasible search space, e.g. du_mpc,min < 0 and du_mpc,max > 0).

#### Active Set note: Solving the Linear Equation of KKT system
The most obvious way to solve the KKT system is by inverting the LHS matrix of the KKT equation (e.g. using Gauss-Jordan elimination):

Another not so obvious but very efficient solution is by using [Schur complement](https://en.wikipedia.org/wiki/Schur_complement) of the LHS matrix. Exploiting the fact that we can make the  matrix as a symmetric positive definite matrix, and moreover, a constant matrix.

Both approach to solve the KKT system is implemented below (as [naive implementation](README.md#the-first-implementation-the-naive-implementation) and [Schur complement implementation](README.md#the-second-implementation-optimized-active-set-with-schur-complement-of-kkt)).

#### Active Set note: Additional Termination Condition
Unlike unconstrained MPC (where the solver is [basically a gain matrix](https://github.com/pronenewbits/Arduino_Unconstrained_MPC_Library#the-second-implementation-description-optimized-version-of-the-naive-implementation)), the constrained MPC solver use iteration to calculate the optimal control value (QP in worst case is NP-hard). It means in a real-time system, in worst case we need to terminate the iteration before the optimal control value can be calculated.

The nifty thing about ASM is that once we have optimal variable  candidate that is inside feasible region, the next iteration of the variable  will be both:
a) Also inside the feasible region; and
b) More optimal than the previous iteration.

Using that fact, we can easily add another termination condition  beside KKT condition  to make sure the MPC calculation still satisfy the real-time requirement. The other option is to use a real-time timer as the termination condition. We then can use the not-the-most-optimal-but-still-feasible solution as the controller output.

#### Active Set: Wrap it up
The Active Set pseudo code then can be redescribed as:

For more explanation about ASM, [these slides](https://people.cs.umu.se/eddiew/optpde2016/QP.pdf) or these pages ([here](https://neos-guide.org/content/quadratic-programming) and [here](https://neos-guide.org/content/quadratic-programming-algorithms)) are quite good at explaining the mechanism behind active set for quadratic programming problem. But for more in-depth treatment of this algorithm, the best resource is of course [Nocedal & Wright's Numerical Optimization](https://www.google.com/books/edition/Numerical_Optimization/7wDpBwAAQBAJ?hl=en&gbpv=0), chapter 16. Just be aware, the tutorials I linked (and many QP formulation) use inequality constraint in the form  while we use  (just multiply the inequality with  to change from one form to another).

# The MPC Implementation
The implementations of the MPC control calculation consist of two main implementations, each of the implementation is self contained in each folder and calculate the same control output (theoretically).

### The first implementation: The Naive Implementation
The Naive Implementation algorithm is just a direct implementation of the MPC derivation above. The MPC algorithm can be described as (the source code can be found in "[mpc_constrained_naive_engl](mpc_constrained_naive_engl)" folder, especially see "[mpc.cpp](mpc_constrained_naive_engl/mpc.cpp)" file):
![MPC Naive algorithm](documentation_resources/Naive_MPC_Calculation.png "Click to maximize if the image rescaling make you dizzy")

The active set subroutine is exactly [the same as explained above](README.md#active-set-wrap-it-up) where the KKT solution is calculated by matrix inversion (the code is also inside "[mpc.cpp](mpc_constrained_naive_engl/mpc.cpp#L240)" file):

### The second implementation: Optimized Active Set with Schur Complement of KKT
There are several things we can do to lower the computation cost:
1. The LHS of constraint calculation at (MPC_5a)-(MPC_5c) of naive implementation is constant. We should move them into MPC initialization step.
2. (Some of calculation inside) `G` matrix at (MPC_4) of naive implementation (specifically the  portion) is actually constant. We should move them into MPC initialization step.
3. If we limit the selection of the MPC `Q` and `R` weight matrices as diagonal and all its entry positive (in fact we've been doing it since [the unconstrained implementation](https://github.com/pronenewbits/Arduino_Unconstrained_MPC_Library#the-background)), then  is also a symmetric positive definite matrix (here I denote `Q` and `R` weight matrices with `MPC` subscript to differentiate them with `Q` matrix of QP solver).

Based on the last point, we can speed up the solution calculation of the KKT system:

First we apply block Gaussian elimination to the KKT system using  as the pivot. Then the KKT system will becomes:

Where the left matrix of the LHS equation is the [Schur complement](https://en.wikipedia.org/wiki/Schur_complement) of , and the solution of KKT system then can be calculated as:

At first glance, the calculation become more complicated than the naive inversion, but:
1. The  matrix is constant, so also its inverse.
2. As the  is positive definite, so is the matrix . It means we can use [Cholesky decomposition to solve the first equation](https://en.wikipedia.org/wiki/Cholesky_decomposition#Applications) instead of calculating the inversion of it. Whenever applicable, the Cholesky decomposition is faster and more stable numerically than Gaussian elimination.
3. In many cases, the active set matrix  dimension is smaller than  dimension, so the matrix  will be smaller than  and obviously smaller than , making the computation even more faster, relative to the naive inversion.
4. For the MPC calculation, the KKT matrix  is the biggest matrix constructed. By avoid constructing it, we can reduce the memory used by the MPC.

\* As [stated on the unconstrained version](https://github.com/pronenewbits/Arduino_Unconstrained_MPC_Library#the-third-implementation-description-the-numerically-robust-version), if we don't take care of it, the  (although a constant) is often ill conditioned. So care must be taken to make sure the  will behave as good as possible.

The MPC algorithm then can be described as (the source code can be found in "[mpc_constrained_schur_engl](mpc_constrained_schur_engl)" folder, especially see "[mpc.cpp](mpc_constrained_schur_engl/mpc.cpp)" file):
![MPC schur complement algorithm](documentation_resources/Schur_comp_MPC_Calculation.png "Click to maximize if the image rescaling make you dizzy")

Where the active set subroutine can be described as (the code is also inside "[mpc.cpp](mpc_constrained_schur_engl/mpc.cpp#L256)" file):

# How to Use
The MPC code is self contained and can be accessed in the folder [mpc_constrained_naive_engl](mpc_constrained_naive_engl) or [mpc_constrained_schur_engl](mpc_constrained_schur_engl). Inside you will find these files:
- `matrix.h/cpp` : The backbone of all my code in this account. This files contain the class for Matrix operation.
- `mpc.h/cpp` : The source files of the MPC Class.
- `konfig.h` : The configuration file.
- `mpc_constrained_*_engl.ino` : The arduino main file (for naive/schur version).

For custom implementation, typically you only need to modify `konfig.h` and `*.ino` files. Where basically you need to:
1. Set the length of `X, U, Z` vectors and sampling time `dt` in `konfig.h`, depend on your model.
2. Set the MPC parameters like `Hp (Prediction Horizon)` or `Hu (Control Horizon)` in `konfig.h`, depend on your application.
3. Enable/disable and set the MPC constraints parameters like `DU`, `U`, or `Z` in `konfig.h`, depend on your application.
4. Define the (linear) matrix system `A, B, C` and MPC initialization value `weightQ, weightR` in the `*.ino` file.

After that, you only need to initialize the MPC class, set the non-zero initialization matrix by calling `MPC::vReInit(A, B, C, weightQ, weightR)` function at initialization, and call the function `MPC::bUpdate(SP, x, u)` at every sampling time to calculate the control value `u(k)`.

*For Arduino configuration (`SYSTEM_IMPLEMENTATION` is set to `SYSTEM_IMPLEMENTATION_EMBEDDED_ARDUINO` in `konfig.h`):
The code is tested on compiler Arduino IDE 1.8.10 and hardware Teensy 4.0 Platform.

*For PC configuration (`SYSTEM_IMPLEMENTATION` is set to `SYSTEM_IMPLEMENTATION_PC` in `konfig.h`):
The code is tested on compiler Qt Creator 4.8.2 and typical PC Platform.

**Important note: For Teensy 4.0, I encounter RAM limitation where the `MATRIX_MAXIMUM_SIZE` can't be more than 28 (if you are using double precision) or 40 (if using single precision). If you already set more than that, your Teensy might be unable to be programmed (stack overflow make the bootloader program goes awry?). The solution is simply to change the `MATRIX_MAXIMUM_SIZE` to be less than that, compile & upload the code from the compiler. The IDE then will protest that it cannot find the Teensy board. DON'T PANIC. Click the program button on the Teensy board to force the bootloader to restart and download the firmware from the computer.**

(The maximum matrix size 28 very much limit the `HP, Hu`, and constraints implementation, I guess [500 kB of Teensy stack](https://www.pjrc.com/store/teensy4_memory.png) is not enough for constrained MPC huh...)
next step: implement [memory pool](https://en.wikipedia.org/wiki/Memory_pool) for Matrix library!

# Some Benchmark
To demonstrate the code, I've made the MPC control a state-space model (HIL style) for Jet Transport Aircraft (ref: https://www.mathworks.com/help/control/ug/mimo-state-space-models.html#buv3tp8-1), where the configuration is (4 state, 2 input, 2 output LTI system) + Hp=4 & Hu=3. The compiler is Arduino IDE 1.8.10 with default setting (compiler optimization setting: faster) and the hardware is Teensy 4.0. Note that I'm somewhat 'cheating' by using the state variable `x` directly from the HIL system into the MPC calculation (in the real world it should be inferred using some kind of observer. But for benchmark purpose, the result should be the same).

These are the results of simulating the model with several constraint configurations (the calculation benchmark below):
1. The result for no-constraint. The MPC behave like [the unconstrained version](https://github.com/pronenewbits/Arduino_Unconstrained_MPC_Library#some-benchmark).

2. The result with slew rate constraints added : Maximum 4 active-set iterations. Take attention that the  signals become slower to change.

3. The result when slew rate constraints *and* output constrains added : Maximum 5 active-set iterations. Take attention that beside the  signals become slower to change, the output  at ~300th sampling time don't overshoot anymore.

The result when zoomed at 300-th sampling time:

All result are plotted using [Scilab](https://www.scilab.org/).

The most interesting result came from analyzing the computation needed by the MPC, the data are collected using this code snippet:
```
u64compuTime = micros();
MPC_HIL.bUpdate(SP, x, u);
u64compuTime = (micros() - u64compuTime);
_maxu64compuTime < u64compuTime? _maxu64compuTime = u64compuTime: 0;
```

The benchmark data variable `_maxu64compuTime`, in microseconds (lower is better):

|Constraints Configuration|Maximum Active Set iteration|Naive Implementation|Implementation using Schur Complement|Schur Complement without Bounds Checking|
|:---:|:---:|:----:|:---:|:---:|
|No-constraint|2|169 us|35 us|11 us|
||4|556 us|170 us|67 us|
||5|886 us
(117.2 us/iteration)|327 us
(65.4 us/iteration)|127 us
(25.4 us/iteration)|

(Nice)

As can be seen here, the optimized version using Schur complement implementation gives computational boost up to 2 to 3 times compared the naive version. Even if we optimize the naive version by doing [the first two optimization of the second implementation](README.md#the-second-implementation-optimized-active-set-with-schur-complement-of-kkt) (moving the LHS constraint and some of `G` matrix calculation into initialization step), we can only cut the computation time around 40 us (try it!). It means the bulk of the speed up came from the choice of the KKT system solver. Note that using Schur complement method (some publication called it 'Range-space methods') is not the only way to solve the KKT system of EQP, there are Null-Space methods (where the  matrix doesn't need to be positive definite), Conjugate gradient method, etc. Refer to mathematical optimization textbooks to read more on this topic.

The interesting thing happens when we disabled the matrix bounds checking in `konfig.h` (if you customize/change the plant system, you need to harden the system before disable it), we can get further increase throughput 2 to 4 times compared with bounds checking enabled, where the best result is **127 us** for a fully functioning constrained MPC. That means this MPC library is capable to tackle a reasonably complex very fast system at several hundred (or even thousand) Hz sampling rate (and in a $20 hardware form!).

Using static analyzer, the biggest matrix constructed is  matrix where the worst-case size is `(2xHuxM) + (2xHpxZ)` when we enable  constraints. The matrix maximum size will be `(2x4x2)+(2x3x2) = 28`. This is the `MATRIX_MAXIMUM_SIZE` value we used in this benchmark.

Note that when we use the naive version, the worst case matrix size is actually the dimension of  which is `(HpxZ) + (2xHuxM) + (2xHpxZ)` or 36 (we don't use it here because of stack overflow).

### More Interesting Benchmark
Sometimes I just want to see how far I can push this system to the extreme. Using the same model (Jet Transport Aircraft), the lowest `Hp` & `Hu` values that still capable to tracking the set point are `Hp=3` and `Hu=1` (with degraded transient response). The biggest matrix is  with  constraints at the size `(2*Hu*M)+(2*Hp*Z) x (Hu*M) = (2*1*2)+(2*3*2)x(1*2) = 16x2 matrix`.

The benchmark data variable `_maxu64compuTime`, in microseconds are (lower is better):

|Constraints Configuration|Maximum Active Set iteration|Implementation using Schur Complement|Schur Complement without Bounds Checking|
|:---:|:---:|:----:|:---:|
|No-constraint|1|11 us|4 us|
||2|26 us|13 us|
||2|52 us|28 us|

(Noice) With this we're entering the tens to (almost) hundred kHz application (\*⌄\*)

This is the result:

And when zoomed for  and  constraints (the MPC still able to control the system while still comply with the constraints):

**Note** that this benchmark still using [zero vector as the ASM starting value](README.md#active-set-note-generating-the-feasible-starting-point). Expect additional 20-30% of computation time when we use linear programming to search for the starting point (will be implemented soon!).

# Closing Remark
I hope you can test & validate my result or inform me if there are some bugs / mathematical error you encounter along the way! (or if you notice some grammar error in the documentation).

I published the code under CC0 license, effectively placed the code on public domain. But it will be great if you can tell me if you use the code, for what/why. That means a lot to me and give me motivation to expand the work (⌒▽⌒)

mime_type: text/x-tex


text:
/************************************************************************************
 * Class Matrix
 *  See matrix.h for description
 *
 * See https://github.com/pronenewbits for more!
 *************************************************************************************/

#include "matrix.h"

/* There's nothing here yet! */


text:
/*************************************************************************************************************
 * Matrix Class
 *  Contain the matrix class definition and operation.
 *
 *  Notes:
 *    a. The matrix data is a 2 dimensional array, with structure:
 *      ->  0 <= i16row <= MATRIX_MAXIMUM_SIZE
 *      ->  0 <= i16col <= MATRIX_MAXIMUM_SIZE
 *      ->  floatData[MATRIX_MAXIMUM_SIZE][MATRIX_MAXIMUM_SIZE] is the memory
 *           representation of the matrix. We only use the first i16row-th
 *           and first i16col-th memory for the matrix data. The rest is unused.
 *
 *    b. Indexing start from 0, there are 3 ways to access the data:
 *          1. A(idxRow,idxCol)       <-- The preferred way. With bounds checking.
 *          2. A[idxRow][idxCol]      <-- Slow, not recommended, but makes a cute code. With bounds checking.
 *          3. A._at(idxRow,idxCol)   <-- Just for internal function usage. Without bounds checking.
 *
 *       Accessing index start from 0 until i16row/i16col, that is:
 *          (0 <= idxRow < i16row)     and     (0 <= idxRow < i16col).
 *
 *    See below at "Data structure of Matrix class" at private member class definition for more information!
 *
 *
 * Class Matrix Versioning:
 *    v0.10 (2020-04-29), {PNb}:
 *      - Add NoInitMatZero for _outp matrix initialization in ForwardSubtitution.
 *      - Fixing bug in HouseholderTransformQR function where _outp & _vectTemp need to be initialized
 *          with zero.
 *      - Change comparison with zero in these functions using float_prec_ZERO_ECO:
 *          -> operator ==
 *          -> operator /
 *          -> vRoundingElementToZero
 *          -> RoundingMatrixToZero
 *      - No need to check fabs(_normM) as non-zero in bNormVector().
 *      - Add operator != (checking with float_prec_ZERO_ECO).
 *      - Add MatIdentity() function.
 *
 *    v0.9 (2020-04-28), {PNb}:
 *      - Set all function as inline function (insert oprah meme here).
 *      - Rework matrix library to increase readability.
 *      - Make every parameters and function as reference as much as possible.
 *      - Make every member function as a const function as possible.
 *      - Add () operator overload to access the matrix data, so we can do both:
 *         -> A[0][3] = access 1st row, 4th column of matrix data.
 *         -> A(2,0)  = access 3rd row, 1st column of matrix data. < this is the way.
 *       - Remove (bug) a remnant of ancient code where we haven't implemented NoInitMatZero:
 *          float_prec floatData[MATRIX_MAXIMUM_SIZE][MATRIX_MAXIMUM_SIZE] = {{0}};
 *         Into:
 *          float_prec floatData[MATRIX_MAXIMUM_SIZE][MATRIX_MAXIMUM_SIZE];
 *         We can get very nice speed up (MPC benchmark @2020-04-27) from 698 us to 414 us!
 *       - Implement copy constructor               (more sweet speed up, 414 us -> 382 us!).
 *       - Implement assignment operator       (more more sweet speed up, 382 us -> 327 us!).
 *       - Remove Copy() function, use assignment operator instead.
 *       - Change MATRIX_USE_BOUND_CHECKING -> MATRIX_USE_BOUNDS_CHECKING (see ...BOUND*S*_CH...).
 *
 *    v0.8 (2020-03-26), {PNb}:
 *      - Change indexing from int32_t to int16_t.
 *      - Add way to initialize matrix with existing float_prec array.
 *      - Add enum InitZero.
 *      - Make temporary matrix initialization inside almost all method with
 *          NoInitMatZero argument.
 *      - Remove the 1 index buffer reserve in bMatrixIsValid function.
 *      - Add bMatrixIsPositiveDefinite method to check the positive
 *          (semi)definiteness of a matrix.
 *      - Add GetDiagonalEntries method.
 *      - Change SYSTEM_IMPLEMENTATION_EMBEDDED_NO_PRINT into
 *          SYSTEM_IMPLEMENTATION_EMBEDDED_CUSTOM, and make vPrint and
 *          vPrintFull as function declaration (the user must define that
 *          function somewhere).
 *
 *    v0.7 (2020-02-23), {PNb}:
 *      - Make the matrix class interface in English (at long last, yay?).
 *
 *
 *** Documentation below is for tracking purpose *************************************
 *
 *    v0.6 (2020-01-16), {PNb}:
 *      - Tambahkan sanity check saat pengecekan MATRIX_PAKAI_BOUND_CHECKING
 *          dengan membandingkan baris & kolom dengan MATRIX_MAXIMUM_SIZE.
 *      - Menambahkan pengecekan matrix untuk operasi dasar antar matrix (*,+,-).
 *
 *    v0.5 (2020-01-14), {PNb}:
 *      - Buat file matrix.cpp (akhirnya!) untuk definisi fungsi di luar class.
 *      - Tambahkan operator overloading untuk operasi negatif matrix (mis. a = -b).
 *      - Tambahkan operator overloading untuk operasi penjumlahan & pengurangan
 *          dengan scalar.
 *      - Ubah evaluasi MATRIX_PAKAI_BOUND_CHECKING menggunakan ASSERT.
 *      - Tambahkan pengecekan index selalu positif di MATRIX_PAKAI_BOUND_CHECKING.
 *
 *    v0.4 (2020-01-10), {PNb}:
 *      - Tambahkan rounding to zero sebelum operasi sqrt(x) untuk menghindari
 *          kasus x = 0-
 *      - Fungsi QRDec mengembalikan Q' dan R (user perlu melakukan transpose
 *          lagi setelah memanggil QRDec untuk mendapatkan Q).
 *      - Menambahkan pengecekan hasil HouseholderTransformQR di dalam QRDec.
 *      - Tambah warning jika MATRIX_PAKAI_BOUND_CHECKING dinonaktifkan.
 *
 *    v0.3_engl (2019-12-31), {PNb}:
 *      - Modifikasi dokumentasi kode buat orang asing.
 *
 *    v0.3 (2019-12-25), {PNb}:
 *      - Menambahkan fungsi back subtitution untuk menyelesaikan permasalahan
 *          persamaan linear Ax = B. Dengan A matrix segitiga atas & B vektor.
 *      - Memperbaiki bug pengecekan MATRIX_PAKAI_BOUND_CHECKING pada indexing kolom.
 *      - Menambahkan fungsi QR Decomposition (via Householder Transformation).
 *      - Menambahkan fungsi Householder Transformation.
 *      - Menghilangkan warning 'implicit conversion' untuk operasi pembandingan
 *          dengan float_prec_ZERO.
 *      - Menambahkan function overloading operasi InsertSubMatrix, untuk
 *          operasi insert dari SubMatrix ke SubMatrix.
 *      - Saat inisialisasi, matrix diisi nol (melalui vIsiHomogen(0.0)).
 *      - Menambahkan function overloading operator '/' dengan scalar.
 *
 *    v0.2 (2019-11-30), {PNb}:
 *      - Fungsi yang disupport:
 *          - Operator ==
 *          - Normalisasi matrix
 *          - Cholesky Decomposition
 *          - InsertSubMatrix
 *          - InsertVector
 *
 *    v0.1 (2019-11-29), {PNb}:
 *      - Fungsi yang disupport:
 *          - Operasi matrix dasar
 *          - Invers
 *          - Cetak
 *
 * See https://github.com/pronenewbits for more!
 ************************************************************************************************************/
#ifndef MATRIX_H
#define MATRIX_H

#include "konfig.h"

#if (SYSTEM_IMPLEMENTATION == SYSTEM_IMPLEMENTATION_PC)
    #include <iostream>
    #include <iomanip>      // std::setprecision

    using namespace std;
#elif (SYSTEM_IMPLEMENTATION == SYSTEM_IMPLEMENTATION_EMBEDDED_ARDUINO)
    #include <Wire.h>
#endif

class Matrix
{
public:
    typedef enum {
        InitMatWithZero,    /* Initialize matrix with zero data */
        NoInitMatZero
    } InitZero;

    /* --------------------------------------------- Basic Matrix Class functions --------------------------------------------- */
    /* Init empty matrix size _i16row x _i16col */
    Matrix(const int16_t _i16row, const int16_t _i16col, const InitZero _init = InitMatWithZero);
    /* Init matrix size _i16row x _i16col with entries initData */
    Matrix(const int16_t _i16row, const int16_t _i16col, const float_prec* initData, const InitZero _init = InitMatWithZero);
    /* Copy constructor (for this operation --> A(B)) (copy B into A) */
    Matrix(const Matrix& old_obj);
    /* Assignment operator (for this operation --> A = B) (copy B into A) */
    Matrix& operator = (const Matrix& obj);
    /* Destructor */
    ~Matrix(void);
    /* Get internal state */
    inline int16_t i16getRow(void) const { return this->i16row; }
    inline int16_t i16getCol(void) const { return this->i16col; }

    /* ------------------------------------------- Matrix entry accessing functions ------------------------------------------- */
    /* For example: A(1,2) access the 1st row and 2nd column data of matrix A <--- The preferred way to access the matrix */
    float_prec& operator () (const int16_t _row, const int16_t _col);
    float_prec operator () (const int16_t _row, const int16_t _col) const;

    /* For example: A[1][2] access the 1st row and 2nd column data of matrix A <-- The awesome way */
    class Proxy {
        public:
            Proxy(float_prec* _inpArr, const int16_t _maxCol) { _array.ptr = _inpArr; this->_maxCol = _maxCol; }
            Proxy(const float_prec* _inpArr, const int16_t _maxCol) { _array.cptr = _inpArr; this->_maxCol = _maxCol; }
            float_prec & operator [] (const int16_t _col);
            float_prec operator [] (const int16_t _col) const;
        private:
            union { /* teehee xp */
                const float_prec* cptr;
                float_prec* ptr;
            } _array;
            int16_t _maxCol;
    };
    Proxy operator [] (const int16_t _row);
    const Proxy operator [] (const int16_t _row) const;

    /* ----------------------------------------- Matrix checking function declaration ----------------------------------------- */
    bool bMatrixIsValid(void);
    void vSetMatrixInvalid(void);
    bool bMatrixIsSquare();
    /* --------------------------------------------- Matrix elementary operations --------------------------------------------- */
    bool operator == (const Matrix& _compare) const;
    bool operator != (const Matrix& _compare) const;
    Matrix operator - (void) const;
    Matrix operator + (const float_prec _scalar) const;
    Matrix operator - (const float_prec _scalar) const;
    Matrix operator * (const float_prec _scalar) const;
    Matrix operator / (const float_prec _scalar) const;
    Matrix operator + (const Matrix& _matAdd) const;
    Matrix operator - (const Matrix& _matSub) const;
    Matrix operator * (const Matrix& _matMul) const;
    /* Declared outside class below */
    /* inline Matrix operator + (const float_prec _scalar, Matrix _mat); */
    /* inline Matrix operator - (const float_prec _scalar, Matrix _mat); */
    /* inline Matrix operator * (const float_prec _scalar, Matrix _mat); */
    /* ----------------------------------------------- Simple Matrix operations ----------------------------------------------- */
    void vRoundingElementToZero(const int16_t _i, const int16_t _j);
    Matrix RoundingMatrixToZero(void);
    void vSetHomogen(const float_prec _val);
    void vSetToZero(void);
    void vSetRandom(const int32_t _maxRand, const int32_t _minRand);
    void vSetDiag(const float_prec _val);
    void vSetIdentity(void);
    Matrix Transpose(void);
    bool bNormVector(void);
    /* ------------------------------------------ Matrix/Vector insertion operations ------------------------------------------ */
    Matrix InsertVector(const Matrix& _Vector, const int16_t _posCol);
    Matrix InsertSubMatrix(const Matrix& _subMatrix, const int16_t _posRow, const int16_t _posCol);
    Matrix InsertSubMatrix(const Matrix& _subMatrix, const int16_t _posRow, const int16_t _posCol,
                           const int16_t _lenRow, const int16_t _lenColumn);
    Matrix InsertSubMatrix(const Matrix& _subMatrix, const int16_t _posRow, const int16_t _posCol,
                           const int16_t _posRowSub, const int16_t _posColSub,
                           const int16_t _lenRow, const int16_t _lenColumn);
    /* ---------------------------------------------------- Big operations ---------------------------------------------------- */
    /* Matrix invertion using Gauss-Jordan algorithm */
    Matrix Invers(void) const;
    /* Check the definiteness of a matrix */
    bool bMatrixIsPositiveDefinite(const bool checkPosSemidefinite = false) const;
    /* Return the vector (Mx1 matrix) correspond with the diagonal entries of 'this' */
    Matrix GetDiagonalEntries(void) const;
    /* Do the Cholesky Decomposition using Cholesky-Crout algorithm, return 'L' matrix */
    Matrix CholeskyDec(void) const;
    /* Do Householder Transformation for QR Decomposition operation */
    Matrix HouseholderTransformQR(const int16_t _rowTransform, const int16_t _colTransform);
    /* Do QR Decomposition for matrix using Householder Transformation */
    bool QRDec(Matrix& Qt, Matrix& R) const;
    /* Do back-subtitution for upper triangular matrix A & column matrix B:
     * x = BackSubtitution(&A, &B)          ; for Ax = B
     */
    Matrix BackSubtitution(const Matrix& A, const Matrix& B) const;
    /* Do forward-subtitution for lower triangular matrix A & column matrix B:
     * x = ForwardSubtitution(&A, &B)       ; for Ax = B
     */
    Matrix ForwardSubtitution(const Matrix& A, const Matrix& B) const;
    /* ----------------------------------------------- Matrix printing function ----------------------------------------------- */
    void vPrint(void);
    void vPrintFull(void);

private:
    /* Data structure of Matrix class:
     *  0 <= i16row <= MATRIX_MAXIMUM_SIZE      ; i16row is the row of the matrix. i16row is invalid if (i16row == -1)
     *  0 <= i16col <= MATRIX_MAXIMUM_SIZE      ; i16col is the column of the matrix. i16col is invalid if (i16col == -1)
     *
     * Accessing index start from 0 until i16row/i16col, that is:
     *  (0 <= idxRow < i16row)     and     (0 <= idxCol < i16col).
     * There are 3 ways to access the data:
     *  1. A[idxRow][idxCol]          <-- Slow, not recommended, but make a cute code. With bounds checking.
     *  2. A(idxRow, idxCol)          <-- The preferred way. With bounds checking.
     *  3. A._at(idxRow, idxCol)      <-- Just for internal function usage. Without bounds checking.
     *
     * floatData[MATRIX_MAXIMUM_SIZE][MATRIX_MAXIMUM_SIZE] is the memory representation of the matrix. We only use the
     *  first i16row-th and first i16col-th memory for the matrix data. The rest is unused.
     *
     * This configuration might seems wasteful (yes it is). But with this, we can make the matrix library code as cleanly
     *  as possible (like I said in the github page, I've made decision to sacrifice speed & performance to get best code
     *  readability I could get).
     *
     * You could change the data structure of floatData if you want to make the implementation more memory efficient.
     */
    int16_t i16row;
    int16_t i16col;
    float_prec floatData[MATRIX_MAXIMUM_SIZE][MATRIX_MAXIMUM_SIZE];

    /* Private way to access floatData without bound checking.
     *  TODO: For Matrix member function we could do the bound checking once at the beginning of the function, and use this
     *          to access the floatData instead of (i,j) operator. From preliminary experiment doing this only on elementary
     *          operation (experiment @2020-04-27), we can get up to 45% computation boost!!! (MPC benchmark 414 us -> 226 us)!
     */
    float_prec& _at(const int16_t _row, const int16_t _col) { return this->floatData[_row][_col]; }
    float_prec _at(const int16_t _row, const int16_t _col) const { return this->floatData[_row][_col]; }
};

inline Matrix operator + (const float_prec _scalar, const Matrix& _mat);
inline Matrix operator - (const float_prec _scalar, const Matrix& _mat);
inline Matrix operator * (const float_prec _scalar, const Matrix& _mat);
inline Matrix MatIdentity(const int16_t _i16size);

/* ================================================= inline definition below: ================================================= */
/* ================================================= inline definition below: ================================================= */

/* ------------------------------------------ Basic Matrix Class functions ------------------------------------------ */
/* ------------------------------------------ Basic Matrix Class functions ------------------------------------------ */

inline Matrix::Matrix(const int16_t _i16row, const int16_t _i16col, const InitZero _init) {
    this->i16row = _i16row;
    this->i16col = _i16col;

    if (_init == InitMatWithZero) {
        this->vSetHomogen(0.0);
    }
}

inline Matrix::Matrix(const int16_t _i16row, const int16_t _i16col, const float_prec* initData, const InitZero _init) {
    this->i16row = _i16row;
    this->i16col = _i16col;

    if (_init == InitMatWithZero) {
        this->vSetHomogen(0.0);
    }
    for (int16_t _i = 0; _i < this->i16row; _i++) {
        for (int16_t _j = 0; _j < this->i16col; _j++) {
            (*this)(_i,_j) = *initData;
            initData++;
        }
    }
}

inline Matrix::Matrix(const Matrix& old_obj) {
    /* For copy contructor, we only need to copy (_i16row x _i16col) submatrix, there's no need to copy all data */
    this->i16row = old_obj.i16row;
    this->i16col = old_obj.i16col;

    const float_prec *sourc = old_obj.floatData[0];
    float_prec *desti = this->floatData[0];

    for (int16_t _i = 0; _i < i16row; _i++) {
        /* Still valid with invalid matrix ((i16row == -1) or (i16col == -1)) */
        memcpy(desti, sourc, sizeof(float_prec)*size_t((this->i16col)));
        sourc += (MATRIX_MAXIMUM_SIZE);
        desti += (MATRIX_MAXIMUM_SIZE);
    }
}

inline Matrix& Matrix::operator = (const Matrix& obj) {
    /* For assignment operator, we only need to copy (_i16row x _i16col) submatrix, there's no need to copy all data */
    this->i16row = obj.i16row;
    this->i16col = obj.i16col;

    const float_prec *sourc = obj.floatData[0];
    float_prec *desti = this->floatData[0];

    for (int16_t _i = 0; _i < i16row; _i++) {
        /* Still valid with invalid matrix ((i16row == -1) or (i16col == -1)) */
        memcpy(desti, sourc, sizeof(float_prec)*size_t((this->i16col)));
        sourc += (MATRIX_MAXIMUM_SIZE);
        desti += (MATRIX_MAXIMUM_SIZE);
    }

    return *this;
}

inline Matrix::~Matrix(void) {
    /* haven't implemented anything here, definition needed for the rule of three */
    /* (´・ω・`) */
}

/* ---------------------------------------- Matrix entry accessing functions ---------------------------------------- */
/* ---------------------------------------- Matrix entry accessing functions ---------------------------------------- */

/* The preferred method to access the matrix data (boring code) */
inline float_prec& Matrix::operator () (const int16_t _row, const int16_t _col) {
    #if (defined(MATRIX_USE_BOUNDS_CHECKING))
        ASSERT((_row >= 0) && (_row < this->i16row) && (_row < MATRIX_MAXIMUM_SIZE),
               "Matrix index out-of-bounds (at row evaluation)");
        ASSERT((_col >= 0) && (_col < this->i16col) && (_col < MATRIX_MAXIMUM_SIZE),
               "Matrix index out-of-bounds (at column _column)");
    #else
        #warning("Matrix bounds checking is disabled... good luck >:3");
    #endif
    return this->floatData[_row][_col];
}
inline float_prec Matrix::operator () (const int16_t _row, const int16_t _col) const {
    #if (defined(MATRIX_USE_BOUNDS_CHECKING))
        ASSERT((_row >= 0) && (_row < this->i16row) && (_row < MATRIX_MAXIMUM_SIZE),
               "Matrix index out-of-bounds (at row evaluation)");
        ASSERT((_col >= 0) && (_col < this->i16col) && (_col < MATRIX_MAXIMUM_SIZE),
               "Matrix index out-of-bounds (at column _column)");
    #else
        #warning("Matrix bounds checking is disabled... good luck >:3");
    #endif
    return this->floatData[_row][_col];
}

/* Ref: https://stackoverflow.com/questions/6969881/operator-overload
 * Modified to be lvalue modifiable (I know this is so dirty, but it makes the code so FABULOUS XD)
 */
inline float_prec & Matrix::Proxy::operator [] (const int16_t _col) {
    #if (defined(MATRIX_USE_BOUNDS_CHECKING))
        ASSERT((_col >= 0) && (_col < this->_maxCol) && (_col < MATRIX_MAXIMUM_SIZE),
                "Matrix index out-of-bounds (at column evaluation)");
    #else
        #warning("Matrix bounds checking is disabled... good luck >:3");
    #endif
    return _array.ptr[_col];
}
inline float_prec Matrix::Proxy::operator [] (const int16_t _col) const {
    #if (defined(MATRIX_USE_BOUNDS_CHECKING))
        ASSERT((_col >= 0) && (_col < this->_maxCol) && (_col < MATRIX_MAXIMUM_SIZE),
                "Matrix index out-of-bounds (at column evaluation)");
    #else
        #warning("Matrix bounds checking is disabled... good luck >:3");
    #endif
    return _array.cptr[_col];
}
inline Matrix::Proxy Matrix::operator [] (const int16_t _row) {
    #if (defined(MATRIX_USE_BOUNDS_CHECKING))
        ASSERT((_row >= 0) && (_row < this->i16row) && (_row < MATRIX_MAXIMUM_SIZE),
               "Matrix index out-of-bounds (at row evaluation)");
    #else
        #warning("Matrix bounds checking is disabled... good luck >:3");
    #endif
    return Proxy(floatData[_row], this->i16col);  /* Parsing column index for bound checking */
}
inline const Matrix::Proxy Matrix::operator [] (const int16_t _row) const {
    #if (defined(MATRIX_USE_BOUNDS_CHECKING))
        ASSERT((_row >= 0) && (_row < this->i16row) && (_row < MATRIX_MAXIMUM_SIZE),
               "Matrix index out-of-bounds (at row evaluation)");
    #else
        #warning("Matrix bounds checking is disabled... good luck >:3");
    #endif
    return Proxy(floatData[_row], this->i16col);  /* Parsing column index for bound checking */
}

/* -------------------------------------- Matrix checking function declaration -------------------------------------- */
/* -------------------------------------- Matrix checking function declaration -------------------------------------- */

inline bool Matrix::bMatrixIsValid(void) {
    /* Check whether the matrix is valid or not */
    if ((this->i16row > 0) && (this->i16row <= MATRIX_MAXIMUM_SIZE) &&
        (this->i16col > 0) && (this->i16col <= MATRIX_MAXIMUM_SIZE))
    {
        return true;
    } else {
        return false;
    }
}

inline void Matrix::vSetMatrixInvalid(void) {
    this->i16row = -1;
    this->i16col = -1;
}

inline bool Matrix::bMatrixIsSquare(void) {
    return (this->i16row == this->i16col);
}

/* ------------------------------------------ Matrix elementary operations ------------------------------------------ */
/* ------------------------------------------ Matrix elementary operations ------------------------------------------ */
/* TODO: We could do loop unrolling here for elementary, simple, and matrix insertion operations. It *might* speed up
 *        the computation time up to 20-30% for processor with FMAC and cached CPU (I still mull on this because
 *        the code will be awful).
 */

inline bool Matrix::operator == (const Matrix& _compare) const {
    if ((this->i16row != _compare.i16row) || (this->i16col != _compare.i16col)) {
        return false;
    }

    for (int16_t _i = 0; _i < this->i16row; _i++) {
        for (int16_t _j = 0; _j < this->i16col; _j++) {
            if (fabs((*this)(_i,_j) - _compare(_i,_j)) > float_prec(float_prec_ZERO_ECO)) {
                return false;
            }
        }
    }
    return true;
}

inline bool Matrix::operator != (const Matrix& _compare) const {
    return (!(*this == _compare));
}

inline Matrix Matrix::operator - (void) const {
    Matrix _outp(this->i16row, this->i16col, NoInitMatZero);

    for (int16_t _i = 0; _i < this->i16row; _i++) {
        for (int16_t _j = 0; _j < this->i16col; _j++) {
            _outp(_i,_j) = -(*this)(_i,_j);
        }
    }
    return _outp;
}

inline Matrix Matrix::operator + (const float_prec _scalar) const {
    Matrix _outp(this->i16row, this->i16col, Matrix::NoInitMatZero);

    for (int16_t _i = 0; _i < this->i16row; _i++) {
        for (int16_t _j = 0; _j < this->i16col; _j++) {
            _outp(_i,_j) = (*this)(_i,_j) + _scalar;
        }
    }
    return _outp;
}

inline Matrix Matrix::operator - (const float_prec _scalar) const {
    Matrix _outp(this->i16row, this->i16col, Matrix::NoInitMatZero);

    for (int16_t _i = 0; _i < this->i16row; _i++) {
        for (int16_t _j = 0; _j < this->i16col; _j++) {
            _outp(_i,_j) = (*this)(_i,_j) - _scalar;
        }
    }
    return _outp;
}

inline Matrix Matrix::operator * (const float_prec _scalar) const {
    Matrix _outp(this->i16row, this->i16col, Matrix::NoInitMatZero);

    for (int16_t _i = 0; _i < this->i16row; _i++) {
        for (int16_t _j = 0; _j < this->i16col; _j++) {
            _outp(_i,_j) = (*this)(_i,_j) * _scalar;
        }
    }
    return _outp;
}

inline Matrix Matrix::operator / (const float_prec _scalar) const {
    Matrix _outp(this->i16row, this->i16col, Matrix::NoInitMatZero);

    if (fabs(_scalar) < float_prec(float_prec_ZERO_ECO)) {
        _outp.vSetMatrixInvalid();
        return _outp;
    }
    for (int16_t _i = 0; _i < this->i16row; _i++) {
        for (int16_t _j = 0; _j < this->i16col; _j++) {
            _outp(_i,_j) = (*this)(_i,_j) / _scalar;
        }
    }
    return _outp;
}

inline Matrix operator + (const float_prec _scalar, const Matrix& _mat) {
    Matrix _outp(_mat.i16getRow(), _mat.i16getCol(), Matrix::NoInitMatZero);

    for (int16_t _i = 0; _i < _mat.i16getRow(); _i++) {
        for (int16_t _j = 0; _j < _mat.i16getCol(); _j++) {
            _outp(_i,_j) = _scalar + _mat(_i,_j);
        }
    }
    return _outp;
}

inline Matrix operator - (const float_prec _scalar, const Matrix& _mat) {
    Matrix _outp(_mat.i16getRow(), _mat.i16getCol(), Matrix::NoInitMatZero);

    for (int16_t _i = 0; _i < _mat.i16getRow(); _i++) {
        for (int16_t _j = 0; _j < _mat.i16getCol(); _j++) {
            _outp(_i,_j) = _scalar - _mat(_i,_j);
        }
    }
    return _outp;
}

inline Matrix operator * (const float_prec _scalar, const Matrix& _mat) {
    Matrix _outp(_mat.i16getRow(), _mat.i16getCol(), Matrix::NoInitMatZero);

    for (int16_t _i = 0; _i < _mat.i16getRow(); _i++) {
        for (int16_t _j = 0; _j < _mat.i16getCol(); _j++) {
            _outp(_i,_j) = _scalar * _mat(_i,_j);
        }
    }
    return _outp;
}

inline Matrix Matrix::operator + (const Matrix& _matAdd) const {
    Matrix _outp(this->i16row, this->i16col, NoInitMatZero);
    if ((this->i16row != _matAdd.i16row) || (this->i16col != _matAdd.i16col)) {
        _outp.vSetMatrixInvalid();
        return _outp;
    }

    for (int16_t _i = 0; _i < this->i16row; _i++) {
        for (int16_t _j = 0; _j < this->i16col; _j++) {
            _outp(_i,_j) = (*this)(_i,_j) + _matAdd(_i,_j);
        }
    }
    return _outp;
}

inline Matrix Matrix::operator - (const Matrix& _matSub) const {
    Matrix _outp(this->i16row, this->i16col, NoInitMatZero);
    if ((this->i16row != _matSub.i16row) || (this->i16col != _matSub.i16col)) {
        _outp.vSetMatrixInvalid();
        return _outp;
    }

    for (int16_t _i = 0; _i < this->i16row; _i++) {
        for (int16_t _j = 0; _j < this->i16col; _j++) {
            _outp(_i,_j) = (*this)(_i,_j) - _matSub(_i,_j);
        }
    }
    return _outp;
}

inline Matrix Matrix::operator * (const Matrix& _matMul) const {
    Matrix _outp(this->i16row, _matMul.i16col, NoInitMatZero);
    if ((this->i16col != _matMul.i16row)) {
        _outp.vSetMatrixInvalid();
        return _outp;
    }

    for (int16_t _i = 0; _i < this->i16row; _i++) {
        for (int16_t _j = 0; _j < _matMul.i16col; _j++) {
            _outp(_i,_j) = 0.0;
            for (int16_t _k = 0; _k < this->i16col; _k++) {
                _outp(_i,_j) += ((*this)(_i,_k) * _matMul(_k,_j));
            }
        }
    }
    return _outp;
}

/* -------------------------------------------- Simple Matrix operations -------------------------------------------- */
/* -------------------------------------------- Simple Matrix operations -------------------------------------------- */

inline void Matrix::vRoundingElementToZero(const int16_t _i, const int16_t _j) {
    if (fabs((*this)(_i,_j)) < float_prec(float_prec_ZERO_ECO)) {
        (*this)(_i,_j) = 0.0;
    }
}

inline Matrix Matrix::RoundingMatrixToZero(void) {
    for (int16_t _i = 0; _i < this->i16row; _i++) {
        for (int16_t _j = 0; _j < this->i16col; _j++) {
            if (fabs((*this)(_i,_j)) < float_prec(float_prec_ZERO_ECO)) {
                (*this)(_i,_j) = 0.0;
            }
        }
    }
    return (*this);
}

inline void Matrix::vSetHomogen(const float_prec _val) {
    for (int16_t _i = 0; _i < this->i16row; _i++) {
        for (int16_t _j = 0; _j < this->i16col; _j++) {
            (*this)(_i,_j) = _val;
        }
    }
}

inline void Matrix::vSetToZero(void) {
    this->vSetHomogen(0.0);
}

inline void Matrix::vSetRandom(const int32_t _maxRand, const int32_t _minRand) {
    for (int16_t _i = 0; _i < this->i16row; _i++) {
        for (int16_t _j = 0; _j < this->i16col; _j++) {
            (*this)(_i,_j) = float_prec((rand() % (_maxRand - _minRand + 1)) + _minRand);
        }
    }
}

inline void Matrix::vSetDiag(const float_prec _val) {
    for (int16_t _i = 0; _i < this->i16row; _i++) {
        for (int16_t _j = 0; _j < this->i16col; _j++) {
            if (_i == _j) {
                (*this)(_i,_j) = _val;
            } else {
                (*this)(_i,_j) = 0.0;
            }
        }
    }
}

inline void Matrix::vSetIdentity(void) {
    this->vSetDiag(1.0);
}

inline Matrix MatIdentity(const int16_t _i16size) {
    Matrix _outp(_i16size, _i16size, Matrix::NoInitMatZero);
    _outp.vSetDiag(1.0);
    return _outp;
}

/* Return the transpose of the matrix */
inline Matrix Matrix::Transpose(void) {
    Matrix _outp(this->i16col, this->i16row, NoInitMatZero);
    for (int16_t _i = 0; _i < this->i16row; _i++) {
        for (int16_t _j = 0; _j < this->i16col; _j++) {
            _outp(_j,_i) = (*this)(_i,_j);
        }
    }
    return _outp;
}

/* Normalize the vector */
inline bool Matrix::bNormVector(void) {
    float_prec _normM = 0.0;
    for (int16_t _i = 0; _i < this->i16row; _i++) {
        for (int16_t _j = 0; _j < this->i16col; _j++) {
            _normM = _normM + ((*this)(_i,_j) * (*this)(_i,_j));
        }
    }

    /* Rounding to zero to avoid case where sqrt(0-), and _normM always positive */
    if (_normM < float_prec(float_prec_ZERO)) {
        return false;
    }
    _normM = sqrt(_normM);
    for (int16_t _i = 0; _i < this->i16row; _i++) {
        for (int16_t _j = 0; _j < this->i16col; _j++) {
            (*this)(_i,_j) /= _normM;
        }
    }
    return true;
}

/* --------------------------------------- Matrix/Vector insertion operations --------------------------------------- */
/* --------------------------------------- Matrix/Vector insertion operations --------------------------------------- */

/* Insert vector into matrix at _posCol position
 * Example: A = Matrix 3x3, B = Vector 3x1
 *
 *  C = A.InsertVector(B, 1);
 *
 *  A = [A00  A01  A02]     B = [B00]
 *      [A10  A11  A12]         [B10]
 *      [A20  A21  A22]         [B20]
 *
 *  C = [A00  B00  A02]
 *      [A10  B10  A12]
 *      [A20  B20  A22]
 */
inline Matrix Matrix::InsertVector(const Matrix& _Vector, const int16_t _posCol) {
    Matrix _outp(*this);
    if ((_Vector.i16row > this->i16row) || (_Vector.i16col+_posCol > this->i16col)) {
        /* Return false */
        _outp.vSetMatrixInvalid();
        return _outp;
    }
    for (int16_t _i = 0; _i < _Vector.i16row; _i++) {
        _outp(_i,_posCol) = _Vector(_i,0);
    }
    return _outp;
}

/* Insert submatrix into matrix at _posRow & _posCol position
 * Example: A = Matrix 4x4, B = Matrix 2x3
 *
 *  C = A.InsertSubMatrix(B, 1, 1);
 *
 *  A = [A00  A01  A02  A03]    B = [B00  B01  B02]
 *      [A10  A11  A12  A13]        [B10  B11  B12]
 *      [A20  A21  A22  A23]
 *      [A30  A31  A32  A33]
 *
 *
 *  C = [A00  A01  A02  A03]
 *      [A10  B00  B01  B02]
 *      [A20  B10  B11  B12]
 *      [A30  A31  A32  A33]
 */
inline Matrix Matrix::InsertSubMatrix(const Matrix& _subMatrix, const int16_t _posRow, const int16_t _posCol) {
    Matrix _outp(*this);
    if (((_subMatrix.i16row+_posRow) > this->i16row) || ((_subMatrix.i16col+_posCol) > this->i16col)) {
        /* Return false */
        _outp.vSetMatrixInvalid();
        return _outp;
    }
    for (int16_t _i = 0; _i < _subMatrix.i16row; _i++) {
        for (int16_t _j = 0; _j < _subMatrix.i16col; _j++) {
            _outp(_i + _posRow,_j + _posCol) = _subMatrix(_i,_j);
        }
    }
    return _outp;
}

/* Insert the first _lenRow-th and first _lenColumn-th submatrix into matrix;
 *  at the matrix's _posRow and _posCol position.
 *
 * Example: A = Matrix 4x4, B = Matrix 2x3
 *
 *  C = A.InsertSubMatrix(B, 1, 1, 2, 2);
 *
 *  A = [A00  A01  A02  A03]    B = [B00  B01  B02]
 *      [A10  A11  A12  A13]        [B10  B11  B12]
 *      [A20  A21  A22  A23]
 *      [A30  A31  A32  A33]
 *
 *
 *  C = [A00  A01  A02  A03]
 *      [A10  B00  B01  A13]
 *      [A20  B10  B11  A23]
 *      [A30  A31  A32  A33]
 */
inline Matrix Matrix::InsertSubMatrix(const Matrix& _subMatrix, const int16_t _posRow, const int16_t _posCol,
                                      const int16_t _lenRow, const int16_t _lenColumn)
{
    Matrix _outp(*this);
    if (((_lenRow+_posRow) > this->i16row) || ((_lenColumn+_posCol) > this->i16col) ||
        (_lenRow > _subMatrix.i16row) || (_lenColumn > _subMatrix.i16col))
    {
        /* Return false */
        _outp.vSetMatrixInvalid();
        return _outp;
    }
    for (int16_t _i = 0; _i < _lenRow; _i++) {
        for (int16_t _j = 0; _j < _lenColumn; _j++) {
            _outp(_i + _posRow,_j + _posCol) = _subMatrix(_i,_j);
        }
    }
    return _outp;
}

/* Insert the _lenRow & _lenColumn submatrix, start from _posRowSub & _posColSub submatrix;
 *  into matrix at the matrix's _posRow and _posCol position.
 *
 * Example: A = Matrix 4x4, B = Matrix 2x3
 *
 *  C = A.InsertSubMatrix(B, 1, 1, 0, 1, 1, 2);
 *
 *  A = [A00  A01  A02  A03]    B = [B00  B01  B02]
 *      [A10  A11  A12  A13]        [B10  B11  B12]
 *      [A20  A21  A22  A23]
 *      [A30  A31  A32  A33]
 *
 *  C = [A00  A01  A02  A03]
 *      [A10  B01  B02  A13]
 *      [A20  A21  A22  A23]
 *      [A30  A31  A32  A33]
 */
inline Matrix Matrix::InsertSubMatrix(const Matrix& _subMatrix, const int16_t _posRow, const int16_t _posCol,
                        const int16_t _posRowSub, const int16_t _posColSub,
                        const int16_t _lenRow, const int16_t _lenColumn)
{
    Matrix _outp(*this);
    if (((_lenRow+_posRow) > this->i16row) || ((_lenColumn+_posCol) > this->i16col) ||
        ((_posRowSub+_lenRow) > _subMatrix.i16row) || ((_posColSub+_lenColumn) > _subMatrix.i16col))
    {
        /* Return false */
        _outp.vSetMatrixInvalid();
        return _outp;
    }
    for (int16_t _i = 0; _i < _lenRow; _i++) {
        for (int16_t _j = 0; _j < _lenColumn; _j++) {
            _outp(_i + _posRow,_j + _posCol) = _subMatrix(_posRowSub+_i,_posColSub+_j);
        }
    }
    return _outp;
}

/* ------------------------------------------------- Big operations ------------------------------------------------- */
/* ------------------------------------------------- Big operations ------------------------------------------------- */

/* Invers operation using Gauss-Jordan algorithm */
inline Matrix Matrix::Invers(void) const {
    Matrix _outp(this->i16row, this->i16col, NoInitMatZero);
    Matrix _temp(*this);
    _outp.vSetIdentity();

    /* Gauss Elimination... */
    for (int16_t _j = 0; _j < (_temp.i16row)-1; _j++) {
        for (int16_t _i = _j+1; _i < _temp.i16row; _i++) {
            if (fabs(_temp(_j,_j)) < float_prec(float_prec_ZERO)) {
                /* Matrix is non-invertible */
                _outp.vSetMatrixInvalid();
                return _outp;
            }

            float_prec _tempfloat = _temp(_i,_j) / _temp(_j,_j);

            for (int16_t _k = 0; _k < _temp.i16col; _k++) {
                _temp(_i,_k) -= (_temp(_j,_k) * _tempfloat);
                _outp(_i,_k) -= (_outp(_j,_k) * _tempfloat);

                _temp.vRoundingElementToZero(_i, _k);
                _outp.vRoundingElementToZero(_i, _k);
            }

        }
    }

    #if (1)
        /* Here, the _temp matrix should be an upper triangular matrix.
         * But because of rounding error, it might not.
         */
        for (int16_t _i = 1; _i < _temp.i16row; _i++) {
            for (int16_t _j = 0; _j < _i; _j++) {
                _temp(_i,_j) = 0.0;
            }
        }
    #endif

    /* Jordan... */
    for (int16_t _j = (_temp.i16row)-1; _j > 0; _j--) {
        for (int16_t _i = _j-1; _i >= 0; _i--) {
            if (fabs(_temp(_j,_j)) < float_prec(float_prec_ZERO)) {
                /* Matrix is non-invertible */
                _outp.vSetMatrixInvalid();
                return _outp;
            }

            float_prec _tempfloat = _temp(_i,_j) / _temp(_j,_j);
            _temp(_i,_j) -= (_temp(_j,_j) * _tempfloat);
            _temp.vRoundingElementToZero(_i, _j);

            for (int16_t _k = (_temp.i16row - 1); _k >= 0; _k--) {
                _outp(_i,_k) -= (_outp(_j,_k) * _tempfloat);
                _outp.vRoundingElementToZero(_i, _k);
            }
        }
    }

    /* Normalization */
    for (int16_t _i = 0; _i < _temp.i16row; _i++) {
        if (fabs(_temp(_i,_i)) < float_prec(float_prec_ZERO)) {
            /* Matrix is non-invertible */
            _outp.vSetMatrixInvalid();
            return _outp;
        }

        float_prec _tempfloat = _temp(_i,_i);
        _temp(_i,_i) = 1.0;

        for (int16_t _j = 0; _j < _temp.i16row; _j++) {
            _outp(_i,_j) /= _tempfloat;
        }
    }
    return _outp;
}

/* Use elemetary row operation to reduce the matrix into upper triangular form
 *  (like in the first phase of gauss-jordan algorithm).
 *
 * Useful if we want to check whether the matrix is positive definite or not
 *  (useful before calling CholeskyDec function).
 */
inline bool Matrix::bMatrixIsPositiveDefinite(const bool checkPosSemidefinite) const {
    bool _posDef, _posSemiDef;
    Matrix _temp(*this);

    /* Gauss Elimination... */
    for (int16_t _j = 0; _j < (_temp.i16row)-1; _j++) {
        for (int16_t _i = _j+1; _i < _temp.i16row; _i++) {
            if (fabs(_temp(_j,_j)) < float_prec(float_prec_ZERO)) {
                /* Q: Do we still need to check this?
                 * A: idk, it's 3 AM. I need sleep :<
                 *
                 * NOTE TO FUTURE SELF: Confirm it!
                 */
                return false;
            }

            float_prec _tempfloat = _temp(_i,_j) / _temp(_j,_j);

            for (int16_t _k = 0; _k < _temp.i16col; _k++) {
                _temp(_i,_k) -= (_temp(_j,_k) * _tempfloat);
                _temp.vRoundingElementToZero(_i, _k);
            }

        }
    }

    _posDef = true;
    _posSemiDef = true;
    for (int16_t _i = 0; _i < _temp.i16row; _i++) {
        if (_temp(_i,_i) < float_prec(float_prec_ZERO)) {
            /* false if less than 0+ (zero included) */
            _posDef = false;
        }
        if (_temp(_i,_i) < -float_prec(float_prec_ZERO)) {
            /* false if less than 0- (zero is not included) */
            _posSemiDef = false;
        }
    }

    if (checkPosSemidefinite) {
        return _posSemiDef;
    } else {
        return _posDef;
    }
}

/* For square matrix 'this' with size MxM, return vector Mx1 with entries
 * correspond with diagonal entries of 'this'.
 *
 *   Example:    this = [a11 a12 a13]
 *                      [a21 a22 a23]
 *                      [a31 a32 a33]
 *
 * out = this.GetDiagonalEntries() = [a11]
 *                                   [a22]
 *                                   [a33]
 */
inline Matrix Matrix::GetDiagonalEntries(void) const {
    Matrix _temp(this->i16row, 1, NoInitMatZero);

    if (this->i16row != this->i16col) {
        _temp.vSetMatrixInvalid();
        return _temp;
    }
    for (int16_t _i = 0; _i < this->i16row; _i++) {
        _temp(_i,0) = (*this)(_i,_i);
    }
    return _temp;
}

/* Do the Cholesky Decomposition using Cholesky-Crout algorithm.
 *
 *      A = L*L'     ; A = real, positive definite, and symmetry MxM matrix
 *
 *      L = A.CholeskyDec();
 *
 *      CATATAN! NOTE! The symmetry property is not checked at the beginning to lower
 *          the computation cost. The processing is being done on the lower triangular
 *          component of _A. Then it is assumed the upper triangular is inherently
 *          equal to the lower end.
 *          (as a side note, Scilab & MATLAB is using Lapack routines DPOTRF that process
 *           the upper triangular of _A. The result should be equal mathematically if A
 *           is symmetry).
 */
inline Matrix Matrix::CholeskyDec(void) const {
    float_prec _tempFloat;

    /* Note that _outp need to be initialized as zero matrix */
    Matrix _outp(this->i16row, this->i16col, InitMatWithZero);

    if (this->i16row != this->i16col) {
        _outp.vSetMatrixInvalid();
        return _outp;
    }
    for (int16_t _j = 0; _j < this->i16col; _j++) {
        for (int16_t _i = _j; _i < this->i16row; _i++) {
            _tempFloat = (*this)(_i,_j);
            if (_i == _j) {
                for (int16_t _k = 0; _k < _j; _k++) {
                    _tempFloat = _tempFloat - (_outp(_i,_k) * _outp(_i,_k));
                }
                if (_tempFloat < -float_prec(float_prec_ZERO)) {
                    /* Matrix is not positif (semi)definit */
                    _outp.vSetMatrixInvalid();
                    return _outp;
                }
                /* Rounding to zero to avoid case where sqrt(0-) */
                if (fabs(_tempFloat) < float_prec(float_prec_ZERO)) {
                    _tempFloat = 0.0;
                }
                _outp(_i,_i) = sqrt(_tempFloat);
            } else {
                for (int16_t _k = 0; _k < _j; _k++) {
                    _tempFloat = _tempFloat - (_outp(_i,_k) * _outp(_j,_k));
                }
                if (fabs(_outp(_j,_j)) < float_prec(float_prec_ZERO)) {
                    /* Matrix is not positif definit */
                    _outp.vSetMatrixInvalid();
                    return _outp;
                }
                _outp(_i,_j) = _tempFloat / _outp(_j,_j);
            }
        }
    }
    return _outp;
}

/* Do the Householder Transformation for QR Decomposition operation.
 *              out = HouseholderTransformQR(A, i, j)
 */
inline Matrix Matrix::HouseholderTransformQR(const int16_t _rowTransform, const int16_t _colTransform) {
    float_prec _tempFloat;
    float_prec _xLen;
    float_prec _x1;
    float_prec _u1;
    float_prec _vLen2;

    /* Note that _outp & _vectTemp need to be initialized as zero matrix */
    Matrix _outp(this->i16row, this->i16row, InitMatWithZero);
    Matrix _vectTemp(this->i16row, 1, InitMatWithZero);

    if ((_rowTransform >= this->i16row) || (_colTransform >= this->i16col)) {
        _outp.vSetMatrixInvalid();
        return _outp;
    }

    /* Until here:
     *
     * _xLen    = ||x||            = sqrt(x1^2 + x2^2 + .. + xn^2)
     * _vLen2   = ||u||^2 - [u1^2] = x2^2 + .. + xn^2
     * _vectTemp= [0 0 0 .. x1=0 x2 x3 .. xn]'
     */
    _x1 = (*this)(_rowTransform,_colTransform);
    _xLen = _x1*_x1;
    _vLen2 = 0.0;
    for (int16_t _i = _rowTransform+1; _i < this->i16row; _i++) {
        _vectTemp(_i,0) = (*this)(_i,_colTransform);

        _tempFloat = _vectTemp(_i,0) * _vectTemp(_i,0);
        _xLen  += _tempFloat;
        _vLen2 += _tempFloat;
    }
    _xLen = sqrt(_xLen);

    /* u1    = x1+(-sign(x1))*xLen */
    if (_x1 < 0.0) {
        _u1 = _x1+_xLen;
    } else {
        _u1 = _x1-_xLen;
    }

    /* Solve vlen2 & tempHH */
    _vLen2 += (_u1*_u1);
    _vectTemp(_rowTransform,0) = _u1;

    if (fabs(_vLen2) < float_prec(float_prec_ZERO_ECO)) {
        /* x vector is collinear with basis vector e, return result = I */
        _outp.vSetIdentity();
    } else {
        /* P = -2*(u1*u1')/v_len2 + I */
        /* PR TODO: We need to investigate more on this */
        for (int16_t _i = 0; _i < this->i16row; _i++) {
            _tempFloat = _vectTemp(_i,0);
            if (fabs(_tempFloat) > float_prec(float_prec_ZERO)) {
                for (int16_t _j = 0; _j < this->i16row; _j++) {
                    if (fabs(_vectTemp(_j,0)) > float_prec(float_prec_ZERO)) {
                        _outp(_i,_j) = _vectTemp(_j,0);
                        _outp(_i,_j) = _outp(_i,_j) * _tempFloat;
                        _outp(_i,_j) = _outp(_i,_j) * (-2.0/_vLen2);
                    }
                }
            }
            _outp(_i,_i) = _outp(_i,_i) + 1.0;
        }
    }
    return _outp;
}

/* Do the QR Decomposition for matrix using Householder Transformation.
 *                      A = Q*R
 *
 * PERHATIAN! CAUTION! The matrix calculated by this function are Q' and R (Q transpose and R).
 *  Because QR Decomposition usually used to calculate solution for least-squares equation
 *  (that need Q'), we don't do the transpose of Q inside this routine to lower the
 *  computation cost (user need to transpose outside if they want Q).
 *
 * Example of using QRDec to solve least-squares:
 *                      Ax = b
 *                   (QR)x = b
 *                      Rx = Q'b    --> Afterward use back-subtitution to solve x
 */
inline bool Matrix::QRDec(Matrix& Qt, Matrix& R) const {
    Matrix Qn(Qt.i16row, Qt.i16col, NoInitMatZero);
    if ((this->i16row < this->i16col) || (!Qt.bMatrixIsSquare()) || (Qt.i16row != this->i16row) ||
        (R.i16row != this->i16row) || (R.i16col != this->i16col))
    {
        Qt.vSetMatrixInvalid();
        R.vSetMatrixInvalid();
        return false;
    }
    R = (*this);
    Qt.vSetIdentity();
    for (int16_t _i = 0; (_i < (this->i16row - 1)) && (_i < this->i16col-1); _i++) {
        Qn  = R.HouseholderTransformQR(_i, _i);
        if (!Qn.bMatrixIsValid()) {
            Qt.vSetMatrixInvalid();
            R.vSetMatrixInvalid();
            return false;
        }
        Qt = Qn * Qt;
        R  = Qn * R;
    }
#if (0)
    Qt.RoundingMatrixToZero();
    R.RoundingMatrixToZero();
#else
    Qt.RoundingMatrixToZero();
    for (int16_t _i = 1; ((_i < R.i16row) && (_i < R.i16col)); _i++) {
        for (int16_t _j = 0; _j < _i; _j++) {
            R(_i, _j) = 0.0;
        }
    }
#endif

    /* Q = Qt.Transpose */
    return true;
}

/* Do the back-subtitution operation for upper triangular matrix A & column matrix B to solve x:
 *                      Ax = B
 *
 * x = BackSubtitution(&A, &B);
 *
 * CATATAN! NOTE! To lower the computation cost, we don't check that A is a upper triangular
 *  matrix (it's assumed that user already make sure of that before calling this routine).
 */
inline Matrix Matrix::BackSubtitution(const Matrix& A, const Matrix& B) const {
    Matrix _outp(A.i16row, 1, NoInitMatZero);
    if ((A.i16row != A.i16col) || (A.i16row != B.i16row)) {
        _outp.vSetMatrixInvalid();
        return _outp;
    }

    for (int16_t _i = A.i16col-1; _i >= 0; _i--) {
        _outp(_i,0) = B(_i,0);
        for (int16_t _j = _i + 1; _j < A.i16col; _j++) {
            _outp(_i,0) = _outp(_i,0) - A(_i,_j)*_outp(_j,0);
        }
        if (fabs(A(_i,_i)) < float_prec(float_prec_ZERO)) {
            _outp.vSetMatrixInvalid();
            return _outp;
        }
        _outp(_i,0) = _outp(_i,0) / A(_i,_i);
    }

    return _outp;
}

/* Do the forward-subtitution operation for lower triangular matrix A & column matrix B to solve x:
 *                      Ax = B
 *
 * x = ForwardSubtitution(&A, &B);
 *
 * CATATAN! NOTE! To lower the computation cost, we don't check that A is a lower triangular
 *  matrix (it's assumed that user already make sure of that before calling this routine).
 */
inline Matrix Matrix::ForwardSubtitution(const Matrix& A, const Matrix& B) const {
    Matrix _outp(A.i16row, 1, NoInitMatZero);
    if ((A.i16row != A.i16col) || (A.i16row != B.i16row)) {
        _outp.vSetMatrixInvalid();
        return _outp;
    }

    for (int16_t _i = 0; _i < A.i16row; _i++) {
        _outp(_i,0) = B(_i,0);
        for (int16_t _j = 0; _j < _i; _j++) {
            _outp(_i,0) = _outp(_i,0) - A(_i,_j)*_outp(_j,0);
        }
        if (fabs(A(_i,_i)) < float_prec(float_prec_ZERO)) {
            _outp.vSetMatrixInvalid();
            return _outp;
        }
        _outp(_i,0) = _outp(_i,0) / A(_i,_i);
    }
    return _outp;
}

/* -------------------------------------------- Matrix printing function -------------------------------------------- */
/* -------------------------------------------- Matrix printing function -------------------------------------------- */
#if (SYSTEM_IMPLEMENTATION == SYSTEM_IMPLEMENTATION_PC)
    inline void Matrix::vPrint(void) {
        for (int16_t _i = 0; _i < this->i16row; _i++) {
            cout << "[ ";
            for (int16_t _j = 0; _j < this->i16col; _j++) {
                cout << std::fixed << std::setprecision(3) << (*this)(_i,_j) << " ";
            }
            cout << "]";
            cout << endl;
        }
        cout << endl;
    }
    inline void Matrix::vPrintFull(void) {
        for (int16_t _i = 0; _i < this->i16row; _i++) {
            cout << "[ ";
            for (int16_t _j = 0; _j < this->i16col; _j++) {
                cout << resetiosflags( ios::fixed | ios::showpoint ) << (*this)(_i,_j) << " ";
            }
            cout << "]";
            cout << endl;
        }
        cout << endl;
    }
#elif (SYSTEM_IMPLEMENTATION == SYSTEM_IMPLEMENTATION_EMBEDDED_ARDUINO)
    inline void Matrix::vPrint(void) {
        char _bufSer[10];
        for (int16_t _i = 0; _i < this->i16row; _i++) {
            Serial.print("[ ");
            for (int16_t _j = 0; _j < this->i16col; _j++) {
                snprintf(_bufSer, sizeof(_bufSer)-1, "%2.2f ", (*this)(_i,_j));
                Serial.print(_bufSer);
            }
            Serial.println("]");
        }
        Serial.println("");
    }
    inline void Matrix::vPrintFull(void) {
        char _bufSer[32];
        for (int16_t _i = 0; _i < this->i16row; _i++) {
            Serial.print("[ ");
            for (int16_t _j = 0; _j < this->i16col; _j++) {
                snprintf(_bufSer, sizeof(_bufSer)-1, "%e ", (*this)(_i,_j));
                Serial.print(_bufSer);
            }
            Serial.println("]");
        }
        Serial.println("");
    }
#else
    /* User must define the print function somewhere */
    /* void vPrint(); */
    /* void vPrintFull(); */
#endif

#endif // MATRIX_H


text:
/**************************************************************************************************
 * This file contains configuration parameters
 *
 *
 * See https://github.com/pronenewbits for more!
 *************************************************************************************************/
#ifndef KONFIG_H
#define KONFIG_H

#include <stdlib.h>
#include <stdint.h>
#include <string.h>
#include <math.h>

/* State Space dimension */
#define SS_X_LEN    (4)
#define SS_Z_LEN    (2)
#define SS_U_LEN    (2)
#define SS_DT_MILIS (10)                            /* 10 ms */
#define SS_DT       float_prec(SS_DT_MILIS/1000.)   /* Sampling time */

/* MPC Parameters */
#define MPC_HP_LEN      (4)
#define MPC_HU_LEN      (3)

#define MPC_USE_CONSTRAINT_DU
// #define MPC_USE_CONSTRAINT_U
#define MPC_USE_CONSTRAINT_Z

#define MPC_MIN_DU      (-5)
#define MPC_MAX_DU      (5)
#define MPC_MIN_U       (-150)
#define MPC_MAX_U       (100)
#define MPC_MIN_Z       (-4)
#define MPC_MAX_Z       (3.5)

/* Change this size based on the biggest matrix you will use */
#define MATRIX_MAXIMUM_SIZE     (28)

/* Define this to enable matrix bounds checking */
#define MATRIX_USE_BOUNDS_CHECKING

/* Set this define to choose math precision of the system */
#define PRECISION_SINGLE    1
#define PRECISION_DOUBLE    2
#define FPU_PRECISION       (PRECISION_SINGLE)

#if (FPU_PRECISION == PRECISION_SINGLE)
    #define float_prec          float
    #define float_prec_ZERO     (1e-7)
    #define float_prec_ZERO_ECO (1e-5)      /* 'Economical' zero, for noisy calculation where 'somewhat zero' is good enough */
#elif (FPU_PRECISION == PRECISION_DOUBLE)
    #define float_prec          double
    #define float_prec_ZERO     (1e-13)
    #define float_prec_ZERO_ECO (1e-8)      /* 'Economical' zero, for noisy calculation where 'somewhat zero' is good enough */
#else
    #error("FPU_PRECISION has not been defined!");
#endif

/* Set this define to choose system implementation (mainly used to define how you print the matrix via the Matrix::vPrint() & Matrix::vPrintFull() function) */
#define SYSTEM_IMPLEMENTATION_PC                    1
#define SYSTEM_IMPLEMENTATION_EMBEDDED_CUSTOM       2
#define SYSTEM_IMPLEMENTATION_EMBEDDED_ARDUINO      3

#define SYSTEM_IMPLEMENTATION                       (SYSTEM_IMPLEMENTATION_EMBEDDED_ARDUINO)

/* ASSERT is evaluated locally (without function call) to lower the computation cost */
void SPEW_THE_ERROR(char const * str);
#define ASSERT(truth, str) { if (!(truth)) SPEW_THE_ERROR(str); }

#endif // KONFIG_H


text:
#include <Wire.h>
#include <elapsedMillis.h>
#include "konfig.h"
#include "matrix.h"
#include "mpc.h"

void vCreateConstraintsLHS(Matrix &CnstLHS, Matrix &CTHETA);
void vCreateConstraintsRHS(Matrix &CnstRHS, Matrix &COMEGA, Matrix &CPSI, Matrix &u_prev, Matrix &x);

elapsedMillis timerMPC;     /* Timer for sampling time */
uint64_t u64compuTime;      /* For benchmark */
char bufferTxSer[100];      /* For serial printing */

/* Plant system */
Matrix A(SS_X_LEN, SS_X_LEN);
Matrix B(SS_X_LEN, SS_U_LEN);
Matrix C(SS_Z_LEN, SS_X_LEN);
Matrix SP((MPC_HP_LEN*SS_Z_LEN), 1);
Matrix x(SS_X_LEN, 1);
Matrix u(SS_U_LEN, 1);
Matrix z(SS_Z_LEN, 1);

int16_t i16iterSP = 0;
MPC MPC_HIL(A, B, C, 1, 0.01, vCreateConstraintsLHS, vCreateConstraintsRHS);
uint64_t _maxu64compuTime = 0;
void setup() {
    /* serial to display data */
    Serial.begin(115200);
    while(!Serial) {}
    pinMode(LED_BUILTIN, OUTPUT);

    /* Ref: https://www.mathworks.com/help/control/ug/mimo-state-space-models.html#buv3tp8-1
     *
     * State-Space Model of Jet Transport Aircraft
     *  This example shows how to build a MIMO model of a jet transport. Because the development of a physical model
     *  for a jet aircraft is lengthy, only the state-space equations are presented here. See any standard text in
     *  aviation for a more complete discussion of the physics behind aircraft flight.
     * The jet model during cruise flight at MACH = 0.8 and H = 40,000 ft. is
     *
     * (The model has two inputs and two outputs. The units are radians for beta (sideslip angle) and phi (bank angle) and
     * radians/sec for yaw (yaw rate) and roll (roll rate). The rudder and aileron deflections are in degrees.)
     */
    A[0][0] = -0.0558;      A[0][1] = -0.9968;      A[0][2] =  0.0802;      A[0][3] = 0.0415;
    A[1][0] =  0.5980;      A[1][1] = -0.1150;      A[1][2] = -0.0318;      A[1][3] = 0.0000;
    A[2][0] = -3.0500;      A[2][1] =  0.3880;      A[2][2] = -0.4650;      A[2][3] = 0.0000;
    A[3][0] =  0.0000;      A[3][1] =  0.0805;      A[3][2] =  1.0000;      A[3][3] = 0.0000;

    B[0][0] =  0.0073;      B[0][1] =  0.0000;
    B[1][0] = -0.4750;      B[1][1] =  0.0077;
    B[2][0] =  0.1530;      B[2][1] =  0.1430;
    B[3][0] =  0.0000;      B[3][1] =  0.0000;

    C[0][0] =  0.0000;      C[0][1] =  1.0000;      C[0][2] =  0.0000;      C[0][3] =  0.0000;
    C[1][0] =  0.0000;      C[1][1] =  0.0000;      C[1][2] =  0.0000;      C[1][3] =  1.0000;

    MPC_HIL.vReInit(A, B, C, 10, 0.05, vCreateConstraintsLHS, vCreateConstraintsRHS);
}

void loop() {
    if (timerMPC > SS_DT_MILIS) {
        /* ================================ Updating Set Point ================================= */
        Matrix SP_NEXT(SS_Z_LEN, 1);
        if (i16iterSP < 100-MPC_HP_LEN+1) {
            SP_NEXT[0][0] = 3.14/2.;
            SP_NEXT[1][0] = 1;
        } else if (i16iterSP < 200-MPC_HP_LEN+1) {
            SP_NEXT[0][0] = 3.14/2.;
            SP_NEXT[1][0] = -3;
        } else {
            SP_NEXT[0][0] = 3.14;
            SP_NEXT[1][0] = -3;
        }
        if (i16iterSP < 300-MPC_HP_LEN+1) {
            i16iterSP++;
        } else {
            i16iterSP = 0;
        }
        for (int32_t _i = 0; _i < (MPC_HP_LEN-1); _i++) {
            SP = SP.InsertSubMatrix(SP, (_i*SS_Z_LEN), 0, ((_i+1)*SS_Z_LEN), 0, SS_Z_LEN, 1);
        }
        SP = SP.InsertSubMatrix(SP_NEXT, ((MPC_HP_LEN-1)*SS_Z_LEN), 0, 0, 0, SS_Z_LEN, 1);
        /* -------------------------------- Updating Set Point --------------------------------- */

        digitalWrite(LED_BUILTIN, HIGH);
        /* ===================================== MPC Update ==================================== */
        u64compuTime = micros();

        MPC_HIL.bUpdate(SP, x, u);

        u64compuTime = (micros() - u64compuTime);

        _maxu64compuTime < u64compuTime? _maxu64compuTime = u64compuTime: 0;
        /* ------------------------------------- MPC Update ------------------------------------ */
        digitalWrite(LED_BUILTIN, LOW);

        /* ================================= Plant Simulation ================================== */
        x = A*x + B*u;
        z = C*x;
        /* --------------------------------- Plant Simulation ---------------------------------- */

        /* =========================== Print to serial (for plotting) ========================== */
        #if (1)
            /* Print: Computation time, number of active set iteration, Set-Point, z */
            snprintf(bufferTxSer, sizeof(bufferTxSer)-1, "%.3f %i %.3f %.3f %.3f %.3f", ((float)_maxu64compuTime)/1000., MPC_HIL.cntIterActiveSet, SP[0][0], SP[1][0], z[0][0], z[1][0]);
        #else
            /* Print: Computation time, Set-Point, z, u */
            snprintf(bufferTxSer, sizeof(bufferTxSer)-1, "%.3f %i %.3f %.3f %.3f %.3f %.3f %.3f", ((float)_maxu64compuTime)/1000., MPC_HIL.cntIterActiveSet, SP[0][0], SP[1][0], z[0][0], z[1][0], u[0][0], u[1][0]);
        #endif
        Serial.print(bufferTxSer);
        Serial.print('\n');
        /* --------------------------- Print to serial (for plotting) -------------------------- */

        timerMPC = 0;
    }
}

void vCreateConstraintsLHS(Matrix &CnstLHS, Matrix &CTHETA)
{
    #if (defined(MPC_USE_CONSTRAINT_DU) || defined(MPC_USE_CONSTRAINT_U) || defined(MPC_USE_CONSTRAINT_Z))
        int16_t _i16offsetCnstr = 0;
        CnstLHS = Matrix((INEQ_LEN_DU + INEQ_LEN_U + INEQ_LEN_Z), (MPC_HU_LEN*SS_U_LEN));
    #endif
    #if defined(MPC_USE_CONSTRAINT_DU)
        /* for dUmin <= dU(kj <= dUmax                                                      ...{MPC_5a} */
        {
            Matrix CnstDu_LHS   {(2*(MPC_HU_LEN*SS_U_LEN)), (MPC_HU_LEN*SS_U_LEN)};
            Matrix CnstDu_Identity((MPC_HU_LEN*SS_U_LEN), (MPC_HU_LEN*SS_U_LEN));
            CnstDu_Identity.vSetIdentity();
            CnstDu_LHS = CnstDu_LHS.InsertSubMatrix(-CnstDu_Identity, 0, 0);
            CnstDu_LHS = CnstDu_LHS.InsertSubMatrix(CnstDu_Identity, (MPC_HU_LEN*SS_U_LEN), 0);

            /* Append to Constraint_LHS & Constraint_RHS */
            CnstLHS = CnstLHS.InsertSubMatrix(CnstDu_LHS, _i16offsetCnstr, 0);
            _i16offsetCnstr += CnstDu_LHS.i16getRow();
        }
    #endif
    #if defined(MPC_USE_CONSTRAINT_U)
        /* for Umin <= U(kj <= Umax                                                         ...{MPC_5b} */
        {
            Matrix CnstU_LHS   {(2*(MPC_HU_LEN*SS_U_LEN)), (MPC_HU_LEN*SS_U_LEN)};
            Matrix U_Identity(SS_U_LEN, SS_U_LEN);
            U_Identity.vSetIdentity();
            for (int16_t _i = 0; _i < MPC_HU_LEN; _i++) {
                for (int16_t _j = 0; _j < MPC_HU_LEN; _j++) {
                    if (_j <= _i) {
                        /* for Umin <= U(k) */
                        CnstU_LHS = CnstU_LHS.InsertSubMatrix(-U_Identity, (_i*SS_U_LEN), (_j*SS_U_LEN));
                        /* for U(k) <= Umax */
                        CnstU_LHS = CnstU_LHS.InsertSubMatrix(U_Identity,  (MPC_HU_LEN*SS_U_LEN)+(_i*SS_U_LEN), (_j*SS_U_LEN));
                    }
                }
            }

            /* Append to Constraint_LHS & Constraint_RHS */
            CnstLHS = CnstLHS.InsertSubMatrix(CnstU_LHS, _i16offsetCnstr, 0);
            _i16offsetCnstr += CnstU_LHS.i16getRow();
        }
    #endif
    #if defined(MPC_USE_CONSTRAINT_Z)
        /* for Zmin <= Z(kj <= Zmax                                                         ...{MPC_5c} */
        {
            Matrix CnstZ_LHS   {(2*(MPC_HP_LEN*SS_Z_LEN)), (MPC_HU_LEN*SS_U_LEN)};
            CnstZ_LHS = CnstZ_LHS.InsertSubMatrix(-CTHETA, 0, 0);
            CnstZ_LHS = CnstZ_LHS.InsertSubMatrix(CTHETA, (MPC_HP_LEN*SS_Z_LEN), 0);

            /* Append to Constraint_LHS & Constraint_RHS */
            CnstLHS = CnstLHS.InsertSubMatrix(CnstZ_LHS, _i16offsetCnstr, 0);
            _i16offsetCnstr += CnstZ_LHS.i16getRow();
        }
    #endif
}

#if defined(MPC_USE_CONSTRAINT_DU)
    Matrix CnstDu_RHS   {(2*(MPC_HU_LEN*SS_U_LEN)), 1};
    Matrix CnstDu_MaxMin((MPC_HU_LEN*SS_U_LEN), 1);
#endif
#if defined(MPC_USE_CONSTRAINT_U)
    Matrix CnstU_RHS   {(2*(MPC_HU_LEN*SS_U_LEN)), 1};
    Matrix CnstU_MaxMin((MPC_HU_LEN*SS_U_LEN), 1);
#endif
#if defined(MPC_USE_CONSTRAINT_Z)
    Matrix CnstZ_RHS   {(2*(MPC_HP_LEN*SS_Z_LEN)), 1};
    Matrix CnstZ_Min((MPC_HP_LEN*SS_Z_LEN), 1);
    Matrix CnstZ_Max((MPC_HP_LEN*SS_Z_LEN), 1);
#endif

void vCreateConstraintsRHS(Matrix &CnstRHS, Matrix &COMEGA, Matrix &CPSI, Matrix &u_prev, Matrix &x)
{
	#if (defined(MPC_USE_CONSTRAINT_DU) || defined(MPC_USE_CONSTRAINT_U) || defined(MPC_USE_CONSTRAINT_Z))
        int16_t _i16offsetCnstr = 0;
        CnstRHS = Matrix((INEQ_LEN_DU + INEQ_LEN_U + INEQ_LEN_Z), 1);
    #endif
    #if defined(MPC_USE_CONSTRAINT_DU)
        /* for dUmin <= dU(kj <= dUmax                                                      ...{MPC_5a} */
        {
            CnstDu_MaxMin.vSetHomogen(MPC_MIN_DU);
            CnstDu_RHS = CnstDu_RHS.InsertSubMatrix(-CnstDu_MaxMin, 0, 0);
            CnstDu_MaxMin.vSetHomogen(MPC_MAX_DU);
            CnstDu_RHS = CnstDu_RHS.InsertSubMatrix(CnstDu_MaxMin, (MPC_HU_LEN*SS_U_LEN), 0);

            /* Append to Constraint_LHS & Constraint_RHS */
            CnstRHS = CnstRHS.InsertSubMatrix(CnstDu_RHS, _i16offsetCnstr, 0);
            _i16offsetCnstr += CnstDu_RHS.i16getRow();
        }
    #endif
    #if defined(MPC_USE_CONSTRAINT_U)
        /* for Umin <= U(kj <= Umax                                                         ...{MPC_5b} */
        {
            for (int16_t _i = 0; _i < MPC_HU_LEN; _i++) {
                for (int16_t _j = 0; _j < SS_U_LEN; _j++) {
                    CnstU_MaxMin[(_i*SS_U_LEN)+_j][0] = -MPC_MIN_U + u_prev[_j][0];
                }
            }
            CnstU_RHS = CnstU_RHS.InsertSubMatrix(CnstU_MaxMin, 0, 0);
            for (int16_t _i = 0; _i < MPC_HU_LEN; _i++) {
                for (int16_t _j = 0; _j < SS_U_LEN; _j++) {
                    CnstU_MaxMin[(_i*SS_U_LEN)+_j][0] = MPC_MAX_U - u_prev[_j][0];
                }
            }
            CnstU_RHS = CnstU_RHS.InsertSubMatrix(CnstU_MaxMin, (MPC_HU_LEN*SS_U_LEN), 0);

            /* Append to Constraint_LHS & Constraint_RHS */
            CnstRHS = CnstRHS.InsertSubMatrix(CnstU_RHS, _i16offsetCnstr, 0);
            _i16offsetCnstr += CnstU_RHS.i16getRow();
        }
    #endif
    #if defined(MPC_USE_CONSTRAINT_Z)
        /* for Zmin <= Z(kj <= Zmax                                                         ...{MPC_5c} */
        {
            CnstZ_Min = CPSI*x + COMEGA*u_prev - MPC_MIN_Z;
            CnstZ_RHS = CnstZ_RHS.InsertSubMatrix(CnstZ_Min, 0, 0);

            CnstZ_Max = -CPSI*x + -COMEGA*u_prev + MPC_MAX_Z;
            CnstZ_RHS = CnstZ_RHS.InsertSubMatrix(CnstZ_Max, (MPC_HP_LEN*SS_Z_LEN), 0);

            /* Append to Constraint_LHS & Constraint_RHS */
            CnstRHS = CnstRHS.InsertSubMatrix(CnstZ_RHS, _i16offsetCnstr, 0);
            _i16offsetCnstr += CnstZ_RHS.i16getRow();
        }
    #endif
}

void SPEW_THE_ERROR(char const * str)
{
    #if (SYSTEM_IMPLEMENTATION == SYSTEM_IMPLEMENTATION_PC)
        cout << (str) << endl;
    #elif (SYSTEM_IMPLEMENTATION == SYSTEM_IMPLEMENTATION_EMBEDDED_ARDUINO)
        Serial.println(str);
    #else
        /* Silent function */
    #endif
    while(1);
}