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 <math.h>

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

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

/* Define this to enable matrix bound 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

mime_type: text/x-c++


text:
/*************************************************************************************************************
 * Class for Discrete Unscented Kalman Filter
 *
 *
 * See https://github.com/pronenewbits for more!
 ************************************************************************************************************/
#ifndef UKF_H
#define UKF_H

#include "konfig.h"
#include "matrix.h"

#if ((2*SS_X_LEN + 1) > MATRIX_MAXIMUM_SIZE)
    #error("The MATRIX_MAXIMUM_SIZE is too small for sigma points (at least need (2*SS_X_LEN + 1))!");
#endif

class UKF
{
public:
    UKF(const Matrix& XInit, const Matrix& PInit, const Matrix& Rv, const Matrix& Rn,
        bool (*bNonlinearUpdateX)(Matrix&, const Matrix&, const Matrix&),
        bool (*bNonlinearUpdateY)(Matrix&, const Matrix&, const Matrix&));
    void vReset(const Matrix& XInit, const Matrix& PInit, const Matrix& Rv, const Matrix& Rn);
    bool bUpdate(const Matrix& Y, const Matrix& U);

    const Matrix GetX()   const { return X_Est; }
    const Matrix GetY()   const { return Y_Est; }
    const Matrix GetP()   const { return P; }
    const Matrix GetErr() const { return Err; }

protected:
    bool bCalculateSigmaPoint(void);
    bool bUnscentedTransform(Matrix& Out, Matrix& OutSigma, Matrix& P, Matrix& DSig,
                             bool (*_vFuncNonLinear)(Matrix& xOut, const Matrix& xInp, const Matrix& U),
                             const Matrix& InpSigma, const Matrix& InpVector,
                             const Matrix& _CovNoise);

private:
    bool (*bNonlinearUpdateX) (Matrix& X_dot, const Matrix& X, const Matrix& U);
    bool (*bNonlinearUpdateY) (Matrix& Y_Est, const Matrix& X, const Matrix& U);

    Matrix X_Est{SS_X_LEN, 1};
    Matrix X_Sigma{SS_X_LEN, (2*SS_X_LEN + 1)};

    Matrix Y_Est{SS_Z_LEN, 1};
    Matrix Y_Sigma{SS_Z_LEN, (2*SS_X_LEN + 1)};

    Matrix P{SS_X_LEN, SS_X_LEN};
    Matrix P_Chol{SS_X_LEN, SS_X_LEN};

    Matrix DX{SS_X_LEN, (2*SS_X_LEN + 1)};
    Matrix DY{SS_Z_LEN, (2*SS_X_LEN + 1)};

    Matrix Py{SS_Z_LEN, SS_Z_LEN};
    Matrix Pxy{SS_X_LEN, SS_Z_LEN};

    Matrix Wm{1, (2*SS_X_LEN + 1)};
    Matrix Wc{1, (2*SS_X_LEN + 1)};

    Matrix Rv{SS_X_LEN, SS_X_LEN};
    Matrix Rn{SS_Z_LEN, SS_Z_LEN};

    Matrix Err{SS_Z_LEN, 1};
    Matrix Gain{SS_X_LEN, SS_Z_LEN};
    float_prec Gamma;
};

#endif // UKF_H


text:
# The system description
In this example, we will implement the UKF to estimate a simple damped pendulum system. Wikipedia has good explanation for [an undamped system](https://en.wikipedia.org/wiki/Pendulum_(mathematics)#Simple_gravity_pendulum) with good animation, while [this website](http://www.nld.ds.mpg.de/applets/pendulum/eqm2.htm) has explanation for the damped one.

The variables of the system can be seen from this picture:

While the system equations can be described as:

# The implementation
First we set the system parameters in `konfig.h`:

    /* State Space dimension */
    #define SS_X_LEN    (2)
    #define SS_Z_LEN    (2)
    #define SS_U_LEN    (0)

Then we implement the state space nonlinear update & measurement function in `ukf_pend_engl.ino`:

    bool Main_bUpdateNonlinearX(Matrix &X_Next, Matrix &X, Matrix &U)
    {
        /*  The update function in discrete time:
        *      x1(k+1) = x1(k) + x2(k)*dt
        *      x2(k+1) = x2(k) - g/l*sin(x1(k))*dt - alpha*x2*dt
        */
        ...
        return true;
    }

    bool Main_bUpdateNonlinearY(Matrix &Y, Matrix &X, Matrix &U)
    {
        /*  The output (in discrete time):
        *      y1(k) =  sin(x1(k)) * l
        *      y2(k) = -cos(x1(k)) * l
        */
        ...
        return true;
    }

To demonstrate the UKF capability, we add noise at the output system (also in `ukf_pend_engl.ino`):

    /* ================== Read the sensor data / simulate the system here ================== */
    ....
    /* Let's add some noise! */
    Y[0][0] += (float((rand() % 20) - 10) / 10.);       /* add +/- 1 meters noise to x position */

    /* ------------------ Read the sensor data / simulate the system here ------------------ */

And set the UKF initial pendulum angle `(theta(k=0))` as different with the true initial pendulum angle:

    /* For example, let's set the theta(k=0) = pi/2     (i.e. the pendulum rod is parallel with the horizontal plane) */
    X_true[0][0] = 3.14159265359/2.;

    /* Observe that we set the wrong initial x_estimated value!  (X_UKF(k=0) != X_TRUE(k=0)) */
    X_est_init[0][0] = -3.14159265359/2.;

And then plot the estimated x-position of the bob. The result, plotted using [Scilab](https://www.scilab.org/) (you can see at the beginning, the estimated value is converging to the truth despite wrong initial value):


text:
/***********************************************************************************************************************
 *  Class for Discrete Unscented Kalman Filter
 *  Ref: Van der. Merwe, .. (2004). Sigma-Point Kalman Filters for Probabilistic Inference in Dynamic
 *      State-Space Models (Ph.D. thesis). Oregon Health & Science University.
 *
 *  The system to be estimated is defined as a discrete nonlinear dynamic dystem:
 *              x(k+1) = f[x(k), u(k)] + v(k)           ; x = Nx1,    u = Mx1
 *              y(k)   = h[x(k), u(k)] + n(k)           ; y = Zx1
 *
 *        Where:
 *          x(k) : State Variable at time-k                          : Nx1
 *          y(k) : Measured output at time-k                         : Zx1
 *          u(k) : System input at time-k                            : Mx1
 *          v(k) : Process noise, AWGN assumed, w/ covariance  Rv    : Nx1
 *          n(k) : Measurement noise, AWGN assumed, w/ covariance Rn : Nx1
 *
 *          f(..), h(..) is a nonlinear transformation of the system to be estimated.
 *
 ***********************************************************************************************************************
 *      Unscented Kalman Filter algorithm:
 *          Initialization:
 *              P(k=0|k=0) = Identity matrix * covariant(P(k=0)), typically initialized with some big number.
 *              x(k=0|k=0) = Expected value of x at time-0 (i.e. x(k=0)), typically set to zero.
 *              Rv, Rn     = Covariance matrices of process & measurement. As this implementation
 *                             the noise as AWGN (and same value for every variable), this is set
 *                             to Rv=diag(RvInit,...,RvInit) and Rn=diag(RnInit,...,RnInit).
 *              Wc, Wm     = First order & second order weight, respectively.
 *              alpha, beta, kappa, gamma = scalar constants.
 *
 *              lambda = (alpha^2)*(N+kappa)-N,         gamma = sqrt(N+alpha)           ...{UKF_1}
 *              Wm = [lambda/(N+lambda)         1/(2(N+lambda)) ... 1/(2(N+lambda))]    ...{UKF_2}
 *              Wc = [Wm(0)+(1-alpha(^2)+beta)  1/(2(N+lambda)) ... 1/(2(N+lambda))]    ...{UKF_3}
 *
 *
 *          UKF Calculation (every sampling time):
 *              Calculate the Sigma Point:
 *                  Xs(k-1) = [x(k-1) ... x(k-1)]            ; Xs(k-1) = NxN
 *                  GPsq = gamma * sqrt(P(k-1))
 *                  XSigma(k-1) = [x(k-1) Xs(k-1)+GPsq Xs(k-1)-GPsq]                    ...{UKF_4}
 *
 *
 *              Unscented Transform XSigma [f,XSigma,u,Rv] -> [x,XSigma,P,DX]:
 *                  XSigma(k) = f(XSigma(k-1), u(k-1))                                  ...{UKF_5a}
 *
 *                  x(k|k-1) = sum(Wm(i) * XSigma(k)(i))    ; i = 1 ... (2N+1)          ...{UKF_6a}
 *
 *                  DX = XSigma(k)(i) - Xs(k)   ; Xs(k) = [x(k|k-1) ... x(k|k-1)]
 *                                              ; Xs(k) = Nx(2N+1)                      ...{UKF_7a}
 *
 *                  P(k|k-1) = sum(Wc(i)*DX*DX') + Rv       ; i = 1 ... (2N+1)          ...{UKF_8a}
 *
 *
 *              Unscented Transform YSigma [h,XSigma,u,Rn] -> [y_est,YSigma,Py,DY]:
 *                  YSigma(k) = h(XSigma(k), u(k|k-1))      ; u(k|k-1) = u(k)           ...{UKF_5b}
 *
 *                  y_est(k) = sum(Wm(i) * YSigma(k)(i))    ; i = 1 ... (2N+1)          ...{UKF_6b}
 *
 *                  DY = YSigma(k)(i) - Ys(k)   ; Ys(k) = [y_est(k) ... y_est(k)]
 *                                              ; Ys(k) = Zx(2N+1)                      ...{UKF_7b}
 *
 *                  Py(k) = sum(Wc(i)*DY*DY') + Rn          ; i = 1 ... (2N+1)          ...{UKF_8b}
 *
 *
 *              Calculate Cross-Covariance Matrix:
 *                  Pxy(k) = sum(Wc(i)*DX*DY(i))            ; i = 1 ... (2N+1)          ...{UKF_9}
 *
 *
 *              Calculate the Kalman Gain:
 *                  K           = Pxy(k) * (Py(k)^-1)                                   ...{UKF_10}
 *
 *
 *              Update the Estimated State Variable:
 *                  x(k|k)      = x(k|k-1) + K * (y(k) - y_est(k))                      ...{UKF_11}
 *
 *
 *              Update the Covariance Matrix:
 *                  P(k|k)      = P(k|k-1) - K*Py(k)*K'                                 ...{UKF_12}
 *
 *
 *        *Additional Information:
 *              - Dengan asumsi masukan plant ZOH, u(k) = u(k|k-1),
 *                  Dengan asumsi tambahan observer dijalankan sebelum pengendali, u(k|k-1) = u(k-1),
 *                  sehingga u(k) [untuk perhitungan kalman] adalah nilai u(k-1) [dari pengendali].
 *              - Notasi yang benar adalah u(k|k-1), tapi disini menggunakan notasi u(k) untuk
 *                  menyederhanakan penulisan rumus.
 *              - Pada contoh di atas X~(k=0|k=0) = [0]. Untuk mempercepat konvergensi bisa digunakan
 *                  informasi plant-spesific. Misal pada implementasi Kalman Filter untuk sensor
 *                  IMU (Inertial measurement unit) dengan X = [quaternion], dengan asumsi IMU
 *                  awalnya menghadap ke atas tanpa rotasi, X~(k=0|k=0) = [1, 0, 0, 0]'
 *
 * See https://github.com/pronenewbits for more!
 **********************************************************************************************************************/
#include "ukf.h"

UKF::UKF(const Matrix& XInit, const Matrix& PInit, const Matrix& Rv, const Matrix& Rn,
         bool (*bNonlinearUpdateX)(Matrix&, const Matrix&, const Matrix&),
         bool (*bNonlinearUpdateY)(Matrix&, const Matrix&, const Matrix&))
{
    /* Initialization:
     *  P(k=0|k=0) = Identity matrix * covariant(P(k=0)), typically initialized with some big number.
     *  x(k=0|k=0) = Expected value of x at time-0 (i.e. x(k=0)), typically set to zero.
     *  Rv, Rn     = Covariance matrices of process & measurement. As this implementation
     *                the noise as AWGN (and same value for every variable), this is set
     *                to Rv=diag(RvInit,...,RvInit) and Rn=diag(RnInit,...,RnInit).
     */
    this->X_Est = XInit;
    this->P     = PInit;
    this->Rv    = Rv;
    this->Rn    = Rn;
    this->bNonlinearUpdateX = bNonlinearUpdateX;
    this->bNonlinearUpdateY = bNonlinearUpdateY;

    /* Van der. Merwe, .. (2004). Sigma-Point Kalman Filters for Probabilistic Inference in Dynamic State-Space Models
     * (Ph.D. thesis). Oregon Health & Science University. Page 6:
     *
     * where λ = α2(L+κ)−L is a scaling parameter. α determines the spread of the sigma points around ̄x and is usually
     * set to a small positive value (e.g. 1e−2 ≤ α ≤ 1). κ is a secondary scaling parameter which is usually set to
     * either 0 or 3−L (see [45] for details), and β is an extra degree of freedom scalar parameter used to
     * incorporate any extra prior knowledge of the distribution of x (for Gaussian distributions, β = 2 is optimal).
     */
    float_prec _alpha   = 1e-2;
    float_prec _k       = 0.0;
    float_prec _beta    = 2.0;

    /* lambda = (alpha^2)*(N+kappa)-N,         gamma = sqrt(N+alpha)            ...{UKF_1} */
    float_prec _lambda  = (_alpha*_alpha)*(SS_X_LEN+_k) - SS_X_LEN;
    Gamma = sqrt((SS_X_LEN + _lambda));

    /* Wm = [lambda/(N+lambda)         1/(2(N+lambda)) ... 1/(2(N+lambda))]     ...{UKF_2} */
    Wm[0][0] = _lambda/(SS_X_LEN + _lambda);
    for (int16_t _i = 1; _i < Wm.i16getCol(); _i++) {
        Wm[0][_i] = 0.5/(SS_X_LEN + _lambda);
    }

    /* Wc = [Wm(0)+(1-alpha(^2)+beta)  1/(2(N+lambda)) ... 1/(2(N+lambda))]     ...{UKF_3} */
    Wc = Wm;
    Wc[0][0] = Wc[0][0] + (1.0-(_alpha*_alpha)+_beta);
}

void UKF::vReset(const Matrix& XInit, const Matrix& PInit, const Matrix& Rv, const Matrix& Rn)
{
    this->X_Est = XInit;
    this->P     = PInit;
    this->Rv    = Rv;
    this->Rn    = Rn;
}

bool UKF::bUpdate(const Matrix& Y, const Matrix& U)
{
    /* Run once every sampling time */

    /* XSigma(k-1) = [x(k-1) Xs(k-1)+GPsq Xs(k-1)-GPsq]                     ...{UKF_4}  */
    if (!bCalculateSigmaPoint()) {
        return false;
    }

    /* Unscented Transform XSigma [f,XSigma,u,Rv] -> [x,XSigma,P,DX]:       ...{UKF_5a} - {UKF_8a} */
    if (!bUnscentedTransform(X_Est, X_Sigma, P, DX, bNonlinearUpdateX, X_Sigma, U, Rv)) {
        return false;
    }

    /* Unscented Transform YSigma [h,XSigma,u,Rn] -> [y_est,YSigma,Py,DY]:  ...{UKF_5b} - {UKF_8b} */
    if (!bUnscentedTransform(Y_Est, Y_Sigma, Py, DY, bNonlinearUpdateY, X_Sigma, U, Rn)) {
        return false;
    }

    /* Calculate Cross-Covariance Matrix:
     *  Pxy(k) = sum(Wc(i)*DX*DY(i))            ; i = 1 ... (2N+1)          ...{UKF_9}
     */
    for (int16_t _i = 0; _i < DX.i16getRow(); _i++) {
        for (int16_t _j = 0; _j < DX.i16getCol(); _j++) {
            DX[_i][_j] *= Wc[0][_j];
        }
    }
    Pxy = DX * (DY.Transpose());

    /* Calculate the Kalman Gain:
     *  K           = Pxy(k) * (Py(k)^-1)                                   ...{UKF_10}
     */
    Matrix PyInv(Py.Invers());
    if (!PyInv.bMatrixIsValid()) {
        return false;
    }
    Gain = Pxy * PyInv;

    /* Update the Estimated State Variable:
     *  x(k|k)      = x(k|k-1) + K * (y(k) - y_est(k))                      ...{UKF_11}
     */
    Err = Y - Y_Est;
    X_Est = X_Est + (Gain*Err);

    /* Update the Covariance Matrix:
     *  P(k|k)      = P(k|k-1) - K*Py(k)*K'                                 ...{UKF_12}
     */
    P = P - (Gain * Py * Gain.Transpose());

    return true;
}

bool UKF::bCalculateSigmaPoint(void)
{
    /* Xs(k-1) = [x(k-1) ... x(k-1)]            ; Xs(k-1) = NxN
     * GPsq = gamma * sqrt(P(k-1))
     * XSigma(k-1) = [x(k-1) Xs(k-1)+GPsq Xs(k-1)-GPsq]                     ...{UKF_4}
     */
    /* Use Cholesky Decomposition to compute sqrt(P) */
    P_Chol = P.CholeskyDec();
    if (!P_Chol.bMatrixIsValid()) {
        /* System Fail */
        return false;
    }
    P_Chol = P_Chol * Gamma;

    /* Xs(k-1) = [x(k-1) ... x(k-1)]            ; Xs(k-1) = NxN */
    Matrix _Y(SS_X_LEN, SS_X_LEN);
    for (int16_t _i = 0; _i < SS_X_LEN; _i++) {
        _Y = _Y.InsertVector(X_Est, _i);
    }

    X_Sigma.vSetToZero();
    /* XSigma(k-1) = [x(k-1) 0 0] */
    X_Sigma = X_Sigma.InsertVector(X_Est, 0);
    /* XSigma(k-1) = [x(k-1) Xs(k-1)+GPsq  0] */
    X_Sigma = X_Sigma.InsertSubMatrix((_Y + P_Chol), 0, 1);
    /* XSigma(k-1) = [x(k-1) Xs(k-1)+GPsq Xs(k-1)-GPsq] */
    X_Sigma = X_Sigma.InsertSubMatrix((_Y - P_Chol), 0, (1+SS_X_LEN));

    return true;
}

bool UKF::bUnscentedTransform(Matrix& Out, Matrix& OutSigma, Matrix& P, Matrix& DSig,
                              bool (*_vFuncNonLinear)(Matrix& xOut, const Matrix& xInp, const Matrix& U),
                              const Matrix& InpSigma, const Matrix& InpVector,
                              const Matrix& _CovNoise)
{
    /* XSigma(k) = f(XSigma(k-1), u(k-1))                                  ...{UKF_5a}  */
    /* x(k|k-1) = sum(Wm(i) * XSigma(k)(i))    ; i = 1 ... (2N+1)          ...{UKF_6a}  */
    Out.vSetToZero();
    for (int16_t _j = 0; _j < InpSigma.i16getCol(); _j++) {
        /* Transform the column submatrix of sigma-points input matrix (InpSigma) */
        Matrix _AuxSigma1(InpSigma.i16getRow(), 1);
        Matrix _AuxSigma2(OutSigma.i16getRow(), 1);
        for (int16_t _i = 0; _i < InpSigma.i16getRow(); _i++) {
            _AuxSigma1[_i][0] = InpSigma[_i][_j];
        }
        if (!_vFuncNonLinear(_AuxSigma2, _AuxSigma1, InpVector)) {
            /* Somehow the transformation function is failed, propagate the error */
            return false;
        }

        /* Combine the transformed vector to construct sigma-points output matrix (OutSigma) */
        OutSigma = OutSigma.InsertVector(_AuxSigma2, _j);

        /* Calculate x(k|k-1) as weighted mean of OutSigma */
        _AuxSigma2 = _AuxSigma2 * Wm[0][_j];
        Out = Out + _AuxSigma2;
    }

    /* DX = XSigma(k)(i) - Xs(k)   ; Xs(k) = [x(k|k-1) ... x(k|k-1)]
     *                             ; Xs(k) = Nx(2N+1)                      ...{UKF_7a}  */
    Matrix _AuxSigma1(OutSigma.i16getRow(), OutSigma.i16getCol());
    for (int16_t _j = 0; _j < OutSigma.i16getCol(); _j++) {
        _AuxSigma1 = _AuxSigma1.InsertVector(Out, _j);
    }
    DSig = OutSigma - _AuxSigma1;

    /* P(k|k-1) = sum(Wc(i)*DX*DX') + Rv       ; i = 1 ... (2N+1)          ...{UKF_8a}  */
    _AuxSigma1 = DSig;
    for (int16_t _i = 0; _i < DSig.i16getRow(); _i++) {
        for (int16_t _j = 0; _j < DSig.i16getCol(); _j++) {
            _AuxSigma1[_i][_j] *= Wc[0][_j];
        }
    }
    P = (_AuxSigma1 * (DSig.Transpose())) + _CovNoise;

    return true;
}


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

#include "matrix.h"

/* There's nothing here yet! */


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 <math.h>

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

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

/* Define this to enable matrix bound 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:
/*************************************************************************************************************
 *
 * In this example, we will simulate the damped pendulum:
 *  - The input u           : none
 *  - The state variable x  : [theta dtheta/dt]'    (angle and angular speed of the pendulum)
 *  - The output y          : [x y]'                (coordinate position of the ball)
 *
 * We can model the pendulum system as (it is assume the rod is massless, without friction):
 *      (dtheta)^2/dt^2 = -g/l*sin(theta) -alpha*(dtheta/dt)
 *
 *      where g = acceleration constant due to gravity, l = pendulum length, alpha = damping factor
 *
 * where the coordinate position of the ball can be described by (0,0 coordinate is the pivot point):
 *      x =  sin(theta) * l
 *      y = -cos(theta) * l
 *
 * *See https://en.wikipedia.org/wiki/Pendulum_(mathematics)#Simple_gravity_pendulum for undamped model,
 * *See http://www.nld.ds.mpg.de/applets/pendulum/eqm1.htm or
 *      http://www.nld.ds.mpg.de/applets/pendulum/eqm2.htm for the damped model.
 *
 *
 *
 *
 * The model then can be described in state space formulation as:
 *  The state variables:
 *      x1 = theta        --> dx1/dt = dtheta/dt
 *      x2 = dtheta/dt    --> dx2/dt = (dtheta)^2)/dt^2
 *  The output variables:
 *      y1 = x
 *      y2 = y
 *
 *  The update function in continuous time:
 *      dx1/dt = x2
 *      dx2/dt = -g/l * sin(x1)
 *  The update function in discrete time:
 *      x1(k+1) = x1(k) + x2(k)*dt
 *      x2(k+1) = x2(k) - g/l*sin(x1(k))*dt - damping_factor*x2*dt
 *
 *  The output (in discrete time):
 *      y1(k) =  sin(x1(k)) * l
 *      y2(k) = -cos(x1(k)) * l
 *
 *
 * See https://github.com/pronenewbits for more!
 ************************************************************************************************************/
#include <Wire.h>
#include <elapsedMillis.h>
#include "konfig.h"
#include "matrix.h"
#include "ukf.h"

/* =============================================== The pendulum model constants =============================================== */
#define pend_g      (9.81)          /* gravitation constant */
#define pend_l      (5)             /* length of the pendulum rod, in meters */
#define pend_alpha  (0.3)           /* damping factor */

/* ============================================ UKF variables/function declaration ============================================ */
/* Just example; in konfig.h:
 *  SS_X_LEN = 2
 *  SS_Z_LEN = 2
 *  SS_U_LEN = 0
 */
/* UKF initialization constant -------------------------------------------------------------------------------------- */
#define P_INIT      (100.)
#define Rv_INIT     (0.01)
#define Rn_INIT     (1.)
/* P(k=0) variable -------------------------------------------------------------------------------------------------- */
float_prec UKF_PINIT_data[SS_X_LEN*SS_X_LEN] = {P_INIT, 0,
                                                0,      P_INIT};
Matrix UKF_PINIT(SS_X_LEN, SS_X_LEN, UKF_PINIT_data);
/* Rv constant ------------------------------------------------------------------------------------------------------- */
float_prec UKF_RVINIT_data[SS_X_LEN*SS_X_LEN] = {Rv_INIT, 0,
                                                0,      Rv_INIT};
Matrix UKF_RvINIT(SS_X_LEN, SS_X_LEN, UKF_RVINIT_data);
/* R constant ------------------------------------------------------------------------------------------------------- */
float_prec UKF_RNINIT_data[SS_Z_LEN*SS_Z_LEN] = {Rn_INIT, 0,
                                                 0,      Rn_INIT};
Matrix UKF_RnINIT(SS_Z_LEN, SS_Z_LEN, UKF_RNINIT_data);
/* Nonlinear & linearization function ------------------------------------------------------------------------------- */
bool Main_bUpdateNonlinearX(Matrix& X_Next, const Matrix& X, const Matrix& U);
bool Main_bUpdateNonlinearY(Matrix& Y, const Matrix& X, const Matrix& U);
/* UKF variables ---------------------------------------------------------------------------------------------------- */
Matrix X_true(SS_X_LEN, 1);
Matrix X_est_init(SS_X_LEN, 1);
Matrix Y(SS_Z_LEN, 1);
Matrix U(SS_U_LEN, 1);
/* UKF system declaration ------------------------------------------------------------------------------------------- */
UKF UKF_IMU(X_est_init, UKF_PINIT, UKF_RvINIT, UKF_RnINIT, Main_bUpdateNonlinearX, Main_bUpdateNonlinearY);

/* ========================================= Auxiliary variables/function declaration ========================================= */
elapsedMillis timerLed, timerUKF;
uint64_t u64compuTime;
char bufferTxSer[100];

void setup() {
    /* serial to display data */
    Serial.begin(115200);
    while(!Serial) {}

    X_true.vSetToZero();
    X_est_init.vSetToZero();

    /* For example, let's set the theta(k=0) = pi/2     (i.e. the pendulum rod is parallel with the horizontal plane) */
    X_true[0][0] = 3.14159265359/2.;

    /* Observe that we set the wrong initial x_estimated value!  (X_UKF(k=0) != X_TRUE(k=0)) */
    X_est_init[0][0] = -3.14159265359/2.;

    UKF_IMU.vReset(X_est_init, UKF_PINIT, UKF_RvINIT, UKF_RnINIT);
}

void loop() {
    if (timerUKF >= SS_DT_MILIS) {
        timerUKF = 0;

        /* ================== Read the sensor data / simulate the system here ================== */
        /*  The update function in discrete time:
         *      x1(k+1) = x1(k) + x2(k)*dt
         *      x2(k+1) = x2(k) - g/l*sin(x1(k))*dt - damping_factor*x2*dt
         */
        float_prec theta     = X_true[0][0];
        float_prec theta_dot = X_true[1][0];
        X_true[0][0] = theta + (theta_dot*SS_DT);
        X_true[1][0] = theta_dot + (-pend_g/pend_l*sin(theta) - pend_alpha*theta_dot)*SS_DT;

        /*  The output (in discrete time):
         *      y1(k) = sin(x1(k))
         *      y2(k) = cos(x1(k))
         */
        theta = X_true[0][0];

        Y[0][0] =  sin(theta) * pend_l;
        Y[1][0] = -cos(theta) * pend_l;

        /* Let's add some noise! */
        Y[0][0] += (float((rand() % 20) - 10) / 10.);   /* add +/- 1 meter noise to x position */
        /* ------------------ Read the sensor data / simulate the system here ------------------ */

        /* ============================= Update the Kalman Filter ============================== */
        u64compuTime = micros();
        if (!UKF_IMU.bUpdate(Y, U)) {
            X_est_init.vSetToZero();
            UKF_IMU.vReset(X_est_init, UKF_PINIT, UKF_RvINIT, UKF_RnINIT);
            Serial.println("Whoop ");
        }
        u64compuTime = (micros() - u64compuTime);
        /* ----------------------------- Update the Kalman Filter ------------------------------ */

        /* =========================== Print to serial (for plotting) ========================== */
        #if (0)
            /* Print: Computation time, x1 (without noise), x1 estimated */
            snprintf(bufferTxSer, sizeof(bufferTxSer)-1, "%.3f %.3f %.3f ", ((float)u64compuTime)/1000.,
                                                                            X_true[0][0], UKF_IMU.GetX()[0][0]);
            Serial.print(bufferTxSer);
        #else
            /* Print: Computation time, y1 (with noise), y1 (without noise), y1 estimated */
            snprintf(bufferTxSer, sizeof(bufferTxSer)-1, "%.3f %.3f %.3f %.3f ", ((float)u64compuTime)/1000.,
                                                                                Y[0][0],
                                                                                sin(X_true[0][0])*pend_l,
                                                                                sin(UKF_IMU.GetX()[0][0])*pend_l);
            Serial.print(bufferTxSer);
        #endif
        Serial.print('\n');
        /* --------------------------- Print to serial (for plotting) -------------------------- */
    }
}

bool Main_bUpdateNonlinearX(Matrix& X_Next, const Matrix& X, const Matrix& U)
{
    /* Insert the nonlinear update transformation here
     *          x(k+1) = f[x(k), u(k)]
     *
     *  The update function in discrete time:
     *      x1(k+1) = x1(k) + x2(k)*dt
     *      x2(k+1) = x2(k) - g/l*sin(x1(k))*dt - alpha*x2*dt
     */
    float_prec theta     = X[0][0];
    float_prec theta_dot = X[1][0];

    if (theta > 3.14159265359) {
        theta = theta - 3.14159265359;
    }
    if (theta < -3.14159265359) {
        theta = theta + 3.14159265359;
    }

    X_Next[0][0] = theta + (theta_dot*SS_DT);
    X_Next[1][0] = theta_dot + (-pend_g/pend_l*sin(theta) - pend_alpha*theta_dot)*SS_DT;

    return true;
}

bool Main_bUpdateNonlinearY(Matrix& Y, const Matrix& X, const Matrix& U)
{
    /* Insert the nonlinear measurement transformation here
     *          y(k)   = h[x(k), u(k)]
     *
     *  The output (in discrete time):
     *      y1(k) =  sin(x1(k)) * l
     *      y2(k) = -cos(x1(k)) * l
     */
    float_prec theta     = X[0][0];

    Y[0][0] =  sin(theta) * pend_l;
    Y[1][0] = -cos(theta) * pend_l;

    return true;
}

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);
}


text:
This is the Sensor Fusion subsystem of [Attitude and Heading Reference System (AHRS) repo](https://github.com/pronenewbits/Arduino_AHRS_System). Don't forget to calibrate the Magnetometer!

See [the subsystem section for explanation](https://github.com/pronenewbits/Arduino_AHRS_System#subsystem-1-the-sensor-fusion-algorithm).


text:
/*************************************************************************************************************
 *
 * In this example, we'll process IMU data from MPU9250 sensor.
 *
 * See https://github.com/pronenewbits for more!
 ************************************************************************************************************/
#include <Wire.h>
#include <elapsedMillis.h>
#include "konfig.h"
#include "matrix.h"
#include "ukf.h"
#include "simple_mpu9250.h"

/* ================================================== The AHRS/IMU variables ================================================== */
/* Gravity vector constant (align with global Z-axis) */
#define IMU_ACC_Z0          (1)
/* Magnetic vector constant (align with local magnetic vector) */
float_prec IMU_MAG_B0_data[3] = {cos(0), sin(0), 0.000000};
Matrix IMU_MAG_B0(3, 1, IMU_MAG_B0_data);
/* The hard-magnet bias */
float_prec HARD_IRON_BIAS_data[3] = {8.832973, 7.243323, 23.95714};
Matrix HARD_IRON_BIAS(3, 1, HARD_IRON_BIAS_data);

/* ============================================ UKF variables/function declaration ============================================ */
/* Just example; in konfig.h:
 *  SS_X_LEN = 4
 *  SS_Z_LEN = 6
 *  SS_U_LEN = 3
 */
/* UKF initialization constant -------------------------------------------------------------------------------------- */
#define P_INIT      (10.)
#define Rv_INIT     (1e-6)
#define Rn_INIT_ACC (0.0015)
#define Rn_INIT_MAG (0.0015)
/* P(k=0) variable -------------------------------------------------------------------------------------------------- */
float_prec UKF_PINIT_data[SS_X_LEN*SS_X_LEN] = {P_INIT, 0,      0,      0,
                                                0,      P_INIT, 0,      0,
                                                0,      0,      P_INIT, 0,
                                                0,      0,      0,      P_INIT};
Matrix UKF_PINIT(SS_X_LEN, SS_X_LEN, UKF_PINIT_data);
/* Q constant ------------------------------------------------------------------------------------------------------- */
float_prec UKF_RVINIT_data[SS_X_LEN*SS_X_LEN] = {Rv_INIT, 0,      0,      0,
                                                 0,      Rv_INIT, 0,      0,
                                                 0,      0,      Rv_INIT, 0,
                                                 0,      0,      0,      Rv_INIT};
Matrix UKF_RvINIT(SS_X_LEN, SS_X_LEN, UKF_RVINIT_data);
/* R constant ------------------------------------------------------------------------------------------------------- */
float_prec UKF_RNINIT_data[SS_Z_LEN*SS_Z_LEN] = {Rn_INIT_ACC, 0,          0,          0,          0,          0,
                                                 0,          Rn_INIT_ACC, 0,          0,          0,          0,
                                                 0,          0,          Rn_INIT_ACC, 0,          0,          0,
                                                 0,          0,          0,          Rn_INIT_MAG, 0,          0,
                                                 0,          0,          0,          0,          Rn_INIT_MAG, 0,
                                                 0,          0,          0,          0,          0,          Rn_INIT_MAG};
Matrix UKF_RnINIT(SS_Z_LEN, SS_Z_LEN, UKF_RNINIT_data);
/* Nonlinear & linearization function ------------------------------------------------------------------------------- */
bool Main_bUpdateNonlinearX(Matrix& X_Next, const Matrix& X, const Matrix& U);
bool Main_bUpdateNonlinearY(Matrix& Y, const Matrix& X, const Matrix& U);
/* UKF variables ---------------------------------------------------------------------------------------------------- */
Matrix quaternionData(SS_X_LEN, 1);
Matrix Y(SS_Z_LEN, 1);
Matrix U(SS_U_LEN, 1);
/* UKF system declaration ------------------------------------------------------------------------------------------- */
UKF UKF_IMU(quaternionData, UKF_PINIT, UKF_RvINIT, UKF_RnINIT, Main_bUpdateNonlinearX, Main_bUpdateNonlinearY);

/* ========================================= Auxiliary variables/function declaration ========================================= */
elapsedMillis timerLed, timerUKF;
uint64_t u64compuTime;
char bufferTxSer[100];
/* The command from the PC */
char cmd;
/* A SimpleMPU9250 object with the MPU-9250 sensor on I2C bus 0 with address 0x68 */
SimpleMPU9250 IMU(Wire, 0x68);

void setup() {
    /* Serial initialization -------------------------------------- */
    Serial.begin(115200);
    while(!Serial) {}
    Serial.println("Calibrating IMU bias...");

    /* IMU initialization ----------------------------------------- */
    int status = IMU.begin();   /* start communication with IMU */
    if (status < 0) {
        Serial.println("IMU initialization unsuccessful");
        Serial.println("Check IMU wiring or try cycling power");
        Serial.print("Status: ");
        Serial.println(status);
        while(1) {}
    }

    /* UKF initialization ----------------------------------------- */
    /* x(k=0) = [1 0 0 0]' */
    quaternionData.vSetToZero();
    quaternionData[0][0] = 1.0;
    UKF_IMU.vReset(quaternionData, UKF_PINIT, UKF_RvINIT, UKF_RnINIT);

    snprintf(bufferTxSer, sizeof(bufferTxSer)-1, "UKF in Teensy 4.0 (%s)\r\n",
                                                 (FPU_PRECISION == PRECISION_SINGLE)?"Float32":"Double64");
    Serial.print(bufferTxSer);
}

void loop() {
    if (timerUKF >= SS_DT_MILIS) {
        timerUKF = 0;

        /* ================== Read the sensor data / simulate the system here ================== */
        /* Read the raw data */
        IMU.readSensor();
        float Ax = IMU.getAccelX_mss();
        float Ay = IMU.getAccelY_mss();
        float Az = IMU.getAccelZ_mss();
        float Bx = IMU.getMagX_uT();
        float By = IMU.getMagY_uT();
        float Bz = IMU.getMagZ_uT();
        float p = IMU.getGyroX_rads();
        float q = IMU.getGyroY_rads();
        float r = IMU.getGyroZ_rads();
        /* Input 1:3 = gyroscope */
        U[0][0] = p;  U[1][0] = q;  U[2][0] = r;
        /* Output 1:3 = accelerometer */
        Y[0][0] = Ax; Y[1][0] = Ay; Y[2][0] = Az;
        /* Output 4:6 = magnetometer */
        Y[3][0] = Bx; Y[4][0] = By; Y[5][0] = Bz;

        /* Compensating Hard-Iron Bias for magnetometer */
        Y[3][0] = Y[3][0]-HARD_IRON_BIAS[0][0];
        Y[4][0] = Y[4][0]-HARD_IRON_BIAS[1][0];
        Y[5][0] = Y[5][0]-HARD_IRON_BIAS[2][0];

        /* Normalizing the output vector */
        float_prec _normG = sqrt(Y[0][0] * Y[0][0]) + (Y[1][0] * Y[1][0]) + (Y[2][0] * Y[2][0]);
        Y[0][0] = Y[0][0] / _normG;
        Y[1][0] = Y[1][0] / _normG;
        Y[2][0] = Y[2][0] / _normG;
        float_prec _normM = sqrt(Y[3][0] * Y[3][0]) + (Y[4][0] * Y[4][0]) + (Y[5][0] * Y[5][0]);
        Y[3][0] = Y[3][0] / _normM;
        Y[4][0] = Y[4][0] / _normM;
        Y[5][0] = Y[5][0] / _normM;
        /* ------------------ Read the sensor data / simulate the system here ------------------ */

        /* ============================= Update the Kalman Filter ============================== */
        u64compuTime = micros();
        if (!UKF_IMU.bUpdate(Y, U)) {
            quaternionData.vSetToZero();
            quaternionData[0][0] = 1.0;
            UKF_IMU.vReset(quaternionData, UKF_PINIT, UKF_RvINIT, UKF_RnINIT);
            Serial.println("Whoop ");
        }
        u64compuTime = (micros() - u64compuTime);
        /* ----------------------------- Update the Kalman Filter ------------------------------ */
    }

    /* The serial data is sent by responding to command from the PC running Processing scipt */
    if (Serial.available()) {
        cmd = Serial.read();
        if (cmd == 'v') {
            snprintf(bufferTxSer, sizeof(bufferTxSer)-1, "UKF in Teensy 4.0 (%s)\r\n",
                                                         (FPU_PRECISION == PRECISION_SINGLE)?"Float32":"Double64");
            Serial.print(bufferTxSer);
            Serial.print('\n');
        } else if (cmd == 'q') {
            /* =========================== Print to serial (for plotting) ========================== */
            quaternionData = UKF_IMU.GetX();

            while (!Serial.available());
            uint8_t count = Serial.read();
            for (uint8_t _i = 0; _i < count; _i++) {
                serialFloatPrint(quaternionData[0][0]);
                Serial.print(",");
                serialFloatPrint(quaternionData[1][0]);
                Serial.print(",");
                serialFloatPrint(quaternionData[2][0]);
                Serial.print(",");
                serialFloatPrint(quaternionData[3][0]);
                Serial.print(",");
                serialFloatPrint((float)u64compuTime);
                Serial.print(",");
                Serial.println("");
            }
            /* --------------------------- Print to serial (for plotting) -------------------------- */
        }
    }
}

/* Function to interface with the Processing script in the PC */
void serialFloatPrint(float f) {
    byte * b = (byte *) &f;
    for (int i = 0; i < 4; i++) {
        byte b1 = (b[i] >> 4) & 0x0f;
        byte b2 = (b[i] & 0x0f);

        char c1 = (b1 < 10) ? ('0' + b1) : 'A' + b1 - 10;
        char c2 = (b2 < 10) ? ('0' + b2) : 'A' + b2 - 10;

        Serial.print(c1);
        Serial.print(c2);
    }
}

bool Main_bUpdateNonlinearX(Matrix& X_Next, const Matrix& X, const Matrix& U)
{
    /* Insert the nonlinear update transformation here
     *          x(k+1) = f[x(k), u(k)]
     *
     * The quaternion update function:
     *  q0_dot = 1/2. * (  0   - p*q1 - q*q2 - r*q3)
     *  q1_dot = 1/2. * ( p*q0 +   0  + r*q2 - q*q3)
     *  q2_dot = 1/2. * ( q*q0 - r*q1 +  0   + p*q3)
     *  q3_dot = 1/2. * ( r*q0 + q*q1 - p*q2 +  0  )
     *
     * Euler method for integration:
     *  q0 = q0 + q0_dot * dT;
     *  q1 = q1 + q1_dot * dT;
     *  q2 = q2 + q2_dot * dT;
     *  q3 = q3 + q3_dot * dT;
     */
    float_prec q0, q1, q2, q3;
    float_prec p, q, r;

    q0 = X[0][0];
    q1 = X[1][0];
    q2 = X[2][0];
    q3 = X[3][0];

    p = U[0][0];
    q = U[1][0];
    r = U[2][0];

    X_Next[0][0] = (0.5 * (+0.00 -p*q1 -q*q2 -r*q3))*SS_DT + q0;
    X_Next[1][0] = (0.5 * (+p*q0 +0.00 +r*q2 -q*q3))*SS_DT + q1;
    X_Next[2][0] = (0.5 * (+q*q0 -r*q1 +0.00 +p*q3))*SS_DT + q2;
    X_Next[3][0] = (0.5 * (+r*q0 +q*q1 -p*q2 +0.00))*SS_DT + q3;

    /* ======= Additional ad-hoc quaternion normalization to make sure the quaternion is a unit vector (i.e. ||q|| = 1) ======= */
    if (!X_Next.bNormVector()) {
        /* System error, return false so we can reset the UKF */
        return false;
    }

    return true;
}

bool Main_bUpdateNonlinearY(Matrix& Y, const Matrix& X, const Matrix& U)
{
    /* Insert the nonlinear measurement transformation here
     *          y(k)   = h[x(k), u(k)]
     *
     * The measurement output is the gravitational and magnetic projection to the body:
     *     DCM     = [(+(q0**2)+(q1**2)-(q2**2)-(q3**2)),                    2*(q1*q2+q0*q3),                    2*(q1*q3-q0*q2)]
     *               [                   2*(q1*q2-q0*q3), (+(q0**2)-(q1**2)+(q2**2)-(q3**2)),                    2*(q2*q3+q0*q1)]
     *               [                   2*(q1*q3+q0*q2),                    2*(q2*q3-q0*q1), (+(q0**2)-(q1**2)-(q2**2)+(q3**2))]
     *
     *  G_proj_sens = DCM * [0 0 1]             --> Gravitational projection to the accelerometer sensor
     *  M_proj_sens = DCM * [Mx My Mz]          --> (Earth) magnetic projection to the magnetometer sensor
     */
    float_prec q0, q1, q2, q3;
    float_prec q0_2, q1_2, q2_2, q3_2;

    q0 = X[0][0];
    q1 = X[1][0];
    q2 = X[2][0];
    q3 = X[3][0];

    q0_2 = q0 * q0;
    q1_2 = q1 * q1;
    q2_2 = q2 * q2;
    q3_2 = q3 * q3;

    Y[0][0] = (2*q1*q3 -2*q0*q2) * IMU_ACC_Z0;

    Y[1][0] = (2*q2*q3 +2*q0*q1) * IMU_ACC_Z0;

    Y[2][0] = (+(q0_2) -(q1_2) -(q2_2) +(q3_2)) * IMU_ACC_Z0;

    Y[3][0] = (+(q0_2)+(q1_2)-(q2_2)-(q3_2)) * IMU_MAG_B0[0][0]
             +(2*(q1*q2+q0*q3)) * IMU_MAG_B0[1][0]
             +(2*(q1*q3-q0*q2)) * IMU_MAG_B0[2][0];

    Y[4][0] = (2*(q1*q2-q0*q3)) * IMU_MAG_B0[0][0]
             +(+(q0_2)-(q1_2)+(q2_2)-(q3_2)) * IMU_MAG_B0[1][0]
             +(2*(q2*q3+q0*q1)) * IMU_MAG_B0[2][0];

    Y[5][0] = (2*(q1*q3+q0*q2)) * IMU_MAG_B0[0][0]
             +(2*(q2*q3-q0*q1)) * IMU_MAG_B0[1][0]
             +(+(q0_2)-(q1_2)-(q2_2)+(q3_2)) * IMU_MAG_B0[2][0];

    return true;
}

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);
}


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 <math.h>

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

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

/* Define this to enable matrix bound 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::vCetak() 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