├── LICENSE ├── README.md ├── UKF_Calculation.png ├── UKF_Definition.png ├── ukf_engl ├── konfig.h ├── matrix.cpp ├── matrix.h ├── ukf.cpp ├── ukf.h └── ukf_engl.ino ├── ukf_example1_pendulum ├── Pendulum_eq.png ├── Pendulum_picture.png ├── README.md ├── result.png └── ukf_pend_engl │ ├── konfig.h │ ├── matrix.cpp │ ├── matrix.h │ ├── ukf.cpp │ ├── ukf.h │ └── ukf_pend_engl.ino └── ukf_example2_imu ├── 2019-12-01_magneto_gabung_Kalib.png ├── 2019-12-01_magneto_gabung_nonKalib.png ├── MPU9250_Connection.png ├── Quaternion_IMU_Equation.png ├── README.md ├── result.png ├── ukf_imu_double64_mute_rot2.mp4 └── ukf_imu_engl ├── konfig.h ├── matrix.cpp ├── matrix.h ├── simple_mpu9250.h ├── ukf.cpp ├── ukf.h └── ukf_imu_engl.ino /LICENSE: -------------------------------------------------------------------------------- 1 | Creative Commons Legal Code 2 | 3 | CC0 1.0 Universal 4 | 5 | CREATIVE COMMONS CORPORATION IS NOT A LAW FIRM AND DOES NOT PROVIDE 6 | LEGAL SERVICES. DISTRIBUTION OF THIS DOCUMENT DOES NOT CREATE AN 7 | ATTORNEY-CLIENT RELATIONSHIP. CREATIVE COMMONS PROVIDES THIS 8 | INFORMATION ON AN "AS-IS" BASIS. CREATIVE COMMONS MAKES NO WARRANTIES 9 | REGARDING THE USE OF THIS DOCUMENT OR THE INFORMATION OR WORKS 10 | PROVIDED HEREUNDER, AND DISCLAIMS LIABILITY FOR DAMAGES RESULTING FROM 11 | THE USE OF THIS DOCUMENT OR THE INFORMATION OR WORKS PROVIDED 12 | HEREUNDER. 13 | 14 | Statement of Purpose 15 | 16 | The laws of most jurisdictions throughout the world automatically confer 17 | exclusive Copyright and Related Rights (defined below) upon the creator 18 | and subsequent owner(s) (each and all, an "owner") of an original work of 19 | authorship and/or a database (each, a "Work"). 20 | 21 | Certain owners wish to permanently relinquish those rights to a Work for 22 | the purpose of contributing to a commons of creative, cultural and 23 | scientific works ("Commons") that the public can reliably and without fear 24 | of later claims of infringement build upon, modify, incorporate in other 25 | works, reuse and redistribute as freely as possible in any form whatsoever 26 | and for any purposes, including without limitation commercial purposes. 27 | These owners may contribute to the Commons to promote the ideal of a free 28 | culture and the further production of creative, cultural and scientific 29 | works, or to gain reputation or greater distribution for their Work in 30 | part through the use and efforts of others. 31 | 32 | For these and/or other purposes and motivations, and without any 33 | expectation of additional consideration or compensation, the person 34 | associating CC0 with a Work (the "Affirmer"), to the extent that he or she 35 | is an owner of Copyright and Related Rights in the Work, voluntarily 36 | elects to apply CC0 to the Work and publicly distribute the Work under its 37 | terms, with knowledge of his or her Copyright and Related Rights in the 38 | Work and the meaning and intended legal effect of CC0 on those rights. 39 | 40 | 1. Copyright and Related Rights. A Work made available under CC0 may be 41 | protected by copyright and related or neighboring rights ("Copyright and 42 | Related Rights"). Copyright and Related Rights include, but are not 43 | limited to, the following: 44 | 45 | i. the right to reproduce, adapt, distribute, perform, display, 46 | communicate, and translate a Work; 47 | ii. moral rights retained by the original author(s) and/or performer(s); 48 | iii. publicity and privacy rights pertaining to a person's image or 49 | likeness depicted in a Work; 50 | iv. rights protecting against unfair competition in regards to a Work, 51 | subject to the limitations in paragraph 4(a), below; 52 | v. rights protecting the extraction, dissemination, use and reuse of data 53 | in a Work; 54 | vi. database rights (such as those arising under Directive 96/9/EC of the 55 | European Parliament and of the Council of 11 March 1996 on the legal 56 | protection of databases, and under any national implementation 57 | thereof, including any amended or successor version of such 58 | directive); and 59 | vii. other similar, equivalent or corresponding rights throughout the 60 | world based on applicable law or treaty, and any national 61 | implementations thereof. 62 | 63 | 2. Waiver. To the greatest extent permitted by, but not in contravention 64 | of, applicable law, Affirmer hereby overtly, fully, permanently, 65 | irrevocably and unconditionally waives, abandons, and surrenders all of 66 | Affirmer's Copyright and Related Rights and associated claims and causes 67 | of action, whether now known or unknown (including existing as well as 68 | future claims and causes of action), in the Work (i) in all territories 69 | worldwide, (ii) for the maximum duration provided by applicable law or 70 | treaty (including future time extensions), (iii) in any current or future 71 | medium and for any number of copies, and (iv) for any purpose whatsoever, 72 | including without limitation commercial, advertising or promotional 73 | purposes (the "Waiver"). Affirmer makes the Waiver for the benefit of each 74 | member of the public at large and to the detriment of Affirmer's heirs and 75 | successors, fully intending that such Waiver shall not be subject to 76 | revocation, rescission, cancellation, termination, or any other legal or 77 | equitable action to disrupt the quiet enjoyment of the Work by the public 78 | as contemplated by Affirmer's express Statement of Purpose. 79 | 80 | 3. Public License Fallback. Should any part of the Waiver for any reason 81 | be judged legally invalid or ineffective under applicable law, then the 82 | Waiver shall be preserved to the maximum extent permitted taking into 83 | account Affirmer's express Statement of Purpose. In addition, to the 84 | extent the Waiver is so judged Affirmer hereby grants to each affected 85 | person a royalty-free, non transferable, non sublicensable, non exclusive, 86 | irrevocable and unconditional license to exercise Affirmer's Copyright and 87 | Related Rights in the Work (i) in all territories worldwide, (ii) for the 88 | maximum duration provided by applicable law or treaty (including future 89 | time extensions), (iii) in any current or future medium and for any number 90 | of copies, and (iv) for any purpose whatsoever, including without 91 | limitation commercial, advertising or promotional purposes (the 92 | "License"). The License shall be deemed effective as of the date CC0 was 93 | applied by Affirmer to the Work. Should any part of the License for any 94 | reason be judged legally invalid or ineffective under applicable law, such 95 | partial invalidity or ineffectiveness shall not invalidate the remainder 96 | of the License, and in such case Affirmer hereby affirms that he or she 97 | will not (i) exercise any of his or her remaining Copyright and Related 98 | Rights in the Work or (ii) assert any associated claims and causes of 99 | action with respect to the Work, in either case contrary to Affirmer's 100 | express Statement of Purpose. 101 | 102 | 4. Limitations and Disclaimers. 103 | 104 | a. No trademark or patent rights held by Affirmer are waived, abandoned, 105 | surrendered, licensed or otherwise affected by this document. 106 | b. Affirmer offers the Work as-is and makes no representations or 107 | warranties of any kind concerning the Work, express, implied, 108 | statutory or otherwise, including without limitation warranties of 109 | title, merchantability, fitness for a particular purpose, non 110 | infringement, or the absence of latent or other defects, accuracy, or 111 | the present or absence of errors, whether or not discoverable, all to 112 | the greatest extent permissible under applicable law. 113 | c. Affirmer disclaims responsibility for clearing rights of other persons 114 | that may apply to the Work or any use thereof, including without 115 | limitation any person's Copyright and Related Rights in the Work. 116 | Further, Affirmer disclaims responsibility for obtaining any necessary 117 | consents, permissions or other rights required for any use of the 118 | Work. 119 | d. Affirmer understands and acknowledges that Creative Commons is not a 120 | party to this document and has no duty or obligation with respect to 121 | this CC0 or use of the Work. 122 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Arduino_UKF_Library 2 | This is a compact Unscented Kalman Filter (UKF) library for Teensy4.0/Arduino system (or real time embedded system in general). 3 | - It's not using Eigen (small source code - more simple to understand). 4 | - It's not using C++ Standard Library/std (for embedded consideration). 5 | - If you set `SYSTEM_IMPLEMENTATION` to `SYSTEM_IMPLEMENTATION_EMBEDDED_NO_PRINT` in `konfig.h`, the code is platform agnostic (not using any library beside these C header files: `stdlib.h`, `stdint.h`, `stdbool.h`, `string.h`, and `math.h`). 6 | - There's no malloc/new/free dynamic memory allocation for real time application (but using heavy stack local variables, so you need to run it through static memory analyzer if you are really concerned about implement this in mission critical hard real time application). 7 | 8 | 9 | # The Background 10 | The Unscented Kalman Filter is designed to handle state variable estimation where the system is highly nonlinear that the Extended Kalman Flter (EKF) will do the job poorly, or even fail to do so (e.g. for non-differentiable nonlinear system). Unfortunately, the algorithm is quite complex compared to EKF. With that in mind, I made this library for any student who want to learn the structure of UKF, the computer code implementation of it, and how to use the filter for a nontrivial problem. 11 | 12 | This library is made with specific goal for educational purpose (I've made decision to sacrifice speed to get best code readability I could get) while still capable of tackling real-time control system implementation (the code is computed under **250 us** for non trivia application! See *[Some Benchmark](README.md#some-benchmark)* section below). I strongly suggest you to [learn the EKF first in my other repository (Arduino_EKF_Library)](https://github.com/pronenewbits/Arduino_EKF_Library) before delve deeper into UKF, because many UKF motivation and design lies on analyzing EKF weakness (specifically, on its need for linearization). 13 | 14 | 15 | Without further ado, first some definition: 16 | ![UKF Definition](UKF_Definition.png "Click to maximize if the image rescaling make you feel dizzy") 17 | 18 | Many authors blended the unscented transformation into kalman filter structure (even in the Jeffrey Uhlmann's paper) to make it into a familiar KF structure. But in this implementation, I prefer to emphasize the unscented transformation structure in the hope you can get the big idea of the transformation more easily. That make the implementation somewhat different from a typical UKF formulation, but in essence it's all the same. 19 | 20 | The UKF algorithm can be descibed as: 21 | ![UKF Calculation](UKF_Calculation.png "Click to maximize if the image rescaling make you feel dizzy") 22 | 23 | 24 | 25 | # How to Use 26 | The UKF code is self contained and can be accessed in the folder [ukf_engl](ukf_engl) (this is the template project). Inside you will find these files: 27 | - `matrix.h/cpp` : The backbone of all my code in this account. This files contain the class for Matrix operation. 28 | - `ukf.h/cpp` : The source files of the Unscented Kalman Filter Class. 29 | - `konfig.h` : The configuration file. 30 | - `ukf_engl.ino` : The arduino main file (this is only the template file). 31 | 32 | For custom implementation, typically you only need to modify `konfig.h` and `*.ino` files. Where basically you need to: 33 | 1. Set the length of `X, U, Z` vectors and sampling time `dt` in `konfig.h`, depend on your model. 34 | 2. Implement the nonlinear update function `f(x)`, measurement function `h(x)`, initialization value `P(k=0)`, and `Rv & Rn` constants value in the `*.ino` file. 35 | 36 | After that, you only need to initialize the UKF class, set the non-zero initialization matrix by calling `UKF::vReset(X_INIT, P_INIT, Rv_INIT, Rn_INIT)` function at initialization, and call `UKF::bUpdate(Y,U)` function every sampling time. 37 | 38 | To see how you can implement the library in non-trivial application, I've implemented 2 example: 39 | 1. [ukf_example1_pendulum](ukf_example1_pendulum). This example simulate the damped pendulum. See the [README file](ukf_example1_pendulum/README.md) inside the folder to get more information. 40 | 2. [ukf_example2_imu](ukf_example2_imu). This example process IMU (Inertial Measurement Unit) data using sensor MPU9250. See the [README file](ukf_example2_imu/README.md) inside the folder to get more information. 41 | 42 |   43 | 44 | *For Arduino configuration (`SYSTEM_IMPLEMENTATION` is set to `SYSTEM_IMPLEMENTATION_EMBEDDED_ARDUINO` in `konfig.h`): 45 | The code is tested on compiler Arduino IDE 1.8.10 and hardware Teensy 4.0 Platform. 46 | 47 | *For PC configuration (`SYSTEM_IMPLEMENTATION` is set to `SYSTEM_IMPLEMENTATION_PC` in `konfig.h`): 48 | The code is tested on compiler Qt Creator 4.8.2 and typical PC Platform. 49 | 50 | 51 | **Important note: For Teensy 4.0, I encounter RAM limitation where the `MATRIX_MAXIMUM_SIZE` can't be more than 28 (if you are using double precision) or 40 (if using single precision). If you already set more than that, your Teensy might be unable to be programmed (stack overflow make the bootloader program goes awry?). The solution is simply to change the `MATRIX_MAXIMUM_SIZE` to be less than that, compile & upload the code from the compiler. The IDE then will protest that it cannot find the Teensy board. DON'T PANIC. Click the program button on the Teensy board to force the bootloader to restart and download the firmware from the computer.** 52 | 53 | next step: implement [memory pool](https://en.wikipedia.org/wiki/Memory_pool) for Matrix library! 54 | 55 | 56 | # Some Benchmark 57 | The computation time needed to compute one iteration of `UKF::bUpdate(Y,U)` function are: 58 | 1. [ukf_example1_pendulum](ukf_example1_pendulum) (2 state, no input, 2 output): **45 us** to compute one iteration (single precision math) or **53 us** (double precision). 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): 59 |

Result for Pendulum simulation

60 | 61 | 62 | 2. [ukf_example2_imu](ukf_example2_imu) (4 state, 3 input, 6 output): **206 us** to compute one iteration (single precision math) or **246 us** (double precision). The result, displayed by [Processing](https://processing.org/) script based on [FreeIMU project](http://www.varesano.net/files/FreeIMU-20121122_1126.zip): 63 |

Result for IMU visualization

64 | 65 | You can also see the video in the [ukf_example2_imu](ukf_example2_imu) folder. 66 | 67 | 68 | # Closing Remark 69 | I hope you can test & validate my result or inform me if there are some bugs / mathematical error you encounter along the way! (or if you notice some grammar error in the documentation). 70 | 71 | I published the code under CC0 license, effectively placed the code on public domain. But it will be great if you can tell me if you use the code, for what/why. That means a lot to me and give me motivation to expand the work (⌒▽⌒) 72 | -------------------------------------------------------------------------------- /UKF_Calculation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pronenewbits/Embedded_UKF_Library/3c16b4156442674cdbb9ac44bf24de5380521ff7/UKF_Calculation.png -------------------------------------------------------------------------------- /UKF_Definition.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pronenewbits/Embedded_UKF_Library/3c16b4156442674cdbb9ac44bf24de5380521ff7/UKF_Definition.png -------------------------------------------------------------------------------- /ukf_engl/konfig.h: -------------------------------------------------------------------------------- 1 | /************************************************************************************************************* 2 | * This file contains configuration parameters 3 | * 4 | * 5 | * See https://github.com/pronenewbits for more! 6 | ************************************************************************************************************/ 7 | #ifndef KONFIG_H 8 | #define KONFIG_H 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | 15 | 16 | /* State Space dimension */ 17 | #define SS_X_LEN (2) 18 | #define SS_Z_LEN (1) 19 | #define SS_U_LEN (1) 20 | #define SS_DT_MILIS (10) /* 10 ms */ 21 | #define SS_DT float_prec(SS_DT_MILIS/1000.) /* Sampling time */ 22 | 23 | 24 | /* Change this size based on the biggest matrix you will use */ 25 | #define MATRIX_MAXIMUM_SIZE (5) 26 | 27 | /* Define this to enable matrix bound checking */ 28 | #define MATRIX_USE_BOUNDS_CHECKING 29 | 30 | /* Set this define to choose math precision of the system */ 31 | #define PRECISION_SINGLE 1 32 | #define PRECISION_DOUBLE 2 33 | #define FPU_PRECISION (PRECISION_SINGLE) 34 | 35 | #if (FPU_PRECISION == PRECISION_SINGLE) 36 | #define float_prec float 37 | #define float_prec_ZERO (1e-7) 38 | #define float_prec_ZERO_ECO (1e-5) /* 'Economical' zero, for noisy calculation where 'somewhat zero' is good enough */ 39 | #elif (FPU_PRECISION == PRECISION_DOUBLE) 40 | #define float_prec double 41 | #define float_prec_ZERO (1e-13) 42 | #define float_prec_ZERO_ECO (1e-8) /* 'Economical' zero, for noisy calculation where 'somewhat zero' is good enough */ 43 | #else 44 | #error("FPU_PRECISION has not been defined!"); 45 | #endif 46 | 47 | 48 | 49 | /* Set this define to choose system implementation (mainly used to define how you print the matrix via the Matrix::vPrint() & Matrix::vPrintFull() function) */ 50 | #define SYSTEM_IMPLEMENTATION_PC 1 51 | #define SYSTEM_IMPLEMENTATION_EMBEDDED_CUSTOM 2 52 | #define SYSTEM_IMPLEMENTATION_EMBEDDED_ARDUINO 3 53 | 54 | #define SYSTEM_IMPLEMENTATION (SYSTEM_IMPLEMENTATION_EMBEDDED_ARDUINO) 55 | 56 | 57 | 58 | /* ASSERT is evaluated locally (without function call) to lower the computation cost */ 59 | void SPEW_THE_ERROR(char const * str); 60 | #define ASSERT(truth, str) { if (!(truth)) SPEW_THE_ERROR(str); } 61 | 62 | 63 | #endif // KONFIG_H 64 | -------------------------------------------------------------------------------- /ukf_engl/matrix.cpp: -------------------------------------------------------------------------------- 1 | /************************************************************************************ 2 | * Class Matrix 3 | * See matrix.h for description 4 | * 5 | * See https://github.com/pronenewbits for more! 6 | *************************************************************************************/ 7 | 8 | 9 | #include "matrix.h" 10 | 11 | /* There's nothing here yet! */ 12 | 13 | -------------------------------------------------------------------------------- /ukf_engl/matrix.h: -------------------------------------------------------------------------------- 1 | /************************************************************************************************************* 2 | * Matrix Class 3 | * Contain the matrix class definition and operation. 4 | * 5 | * Notes: 6 | * a. The matrix data is a 2 dimensional array, with structure: 7 | * -> 0 <= i16row <= MATRIX_MAXIMUM_SIZE 8 | * -> 0 <= i16col <= MATRIX_MAXIMUM_SIZE 9 | * -> floatData[MATRIX_MAXIMUM_SIZE][MATRIX_MAXIMUM_SIZE] is the memory 10 | * representation of the matrix. We only use the first i16row-th 11 | * and first i16col-th memory for the matrix data. The rest is unused. 12 | * 13 | * b. Indexing start from 0, there are 3 ways to access the data: 14 | * 1. A(idxRow,idxCol) <-- The preferred way. With bounds checking. 15 | * 2. A[idxRow][idxCol] <-- Slow, not recommended, but makes a cute code. With bounds checking. 16 | * 3. A._at(idxRow,idxCol) <-- Just for internal function usage. Without bounds checking. 17 | * 18 | * Accessing index start from 0 until i16row/i16col, that is: 19 | * (0 <= idxRow < i16row) and (0 <= idxRow < i16col). 20 | * 21 | * See below at "Data structure of Matrix class" at private member class definition for more information! 22 | * 23 | * 24 | * Class Matrix Versioning: 25 | * v0.10 (2020-04-29), {PNb}: 26 | * - Add NoInitMatZero for _outp matrix initialization in ForwardSubtitution. 27 | * - Fixing bug in HouseholderTransformQR function where _outp & _vectTemp need to be initialized 28 | * with zero. 29 | * - Change comparison with zero in these functions using float_prec_ZERO_ECO: 30 | * -> operator == 31 | * -> operator / 32 | * -> vRoundingElementToZero 33 | * -> RoundingMatrixToZero 34 | * - No need to check fabs(_normM) as non-zero in bNormVector(). 35 | * - Add operator != (checking with float_prec_ZERO_ECO). 36 | * - Add MatIdentity() function. 37 | * 38 | * v0.9 (2020-04-28), {PNb}: 39 | * - Set all function as inline function (insert oprah meme here). 40 | * - Rework matrix library to increase readability. 41 | * - Make every parameters and function as reference as much as possible. 42 | * - Make every member function as a const function as possible. 43 | * - Add () operator overload to access the matrix data, so we can do both: 44 | * -> A[0][3] = access 1st row, 4th column of matrix data. 45 | * -> A(2,0) = access 3rd row, 1st column of matrix data. < this is the way. 46 | * - Remove (bug) a remnant of ancient code where we haven't implemented NoInitMatZero: 47 | * float_prec floatData[MATRIX_MAXIMUM_SIZE][MATRIX_MAXIMUM_SIZE] = {{0}}; 48 | * Into: 49 | * float_prec floatData[MATRIX_MAXIMUM_SIZE][MATRIX_MAXIMUM_SIZE]; 50 | * We can get very nice speed up (MPC benchmark @2020-04-27) from 698 us to 414 us! 51 | * - Implement copy constructor (more sweet speed up, 414 us -> 382 us!). 52 | * - Implement assignment operator (more more sweet speed up, 382 us -> 327 us!). 53 | * - Remove Copy() function, use assignment operator instead. 54 | * - Change MATRIX_USE_BOUND_CHECKING -> MATRIX_USE_BOUNDS_CHECKING (see ...BOUND*S*_CH...). 55 | * 56 | * v0.8 (2020-03-26), {PNb}: 57 | * - Change indexing from int32_t to int16_t. 58 | * - Add way to initialize matrix with existing float_prec array. 59 | * - Add enum InitZero. 60 | * - Make temporary matrix initialization inside almost all method with 61 | * NoInitMatZero argument. 62 | * - Remove the 1 index buffer reserve in bMatrixIsValid function. 63 | * - Add bMatrixIsPositiveDefinite method to check the positive 64 | * (semi)definiteness of a matrix. 65 | * - Add GetDiagonalEntries method. 66 | * - Change SYSTEM_IMPLEMENTATION_EMBEDDED_NO_PRINT into 67 | * SYSTEM_IMPLEMENTATION_EMBEDDED_CUSTOM, and make vPrint and 68 | * vPrintFull as function declaration (the user must define that 69 | * function somewhere). 70 | * 71 | * v0.7 (2020-02-23), {PNb}: 72 | * - Make the matrix class interface in English (at long last, yay?). 73 | * 74 | * 75 | *** Documentation below is for tracking purpose ************************************* 76 | * 77 | * v0.6 (2020-01-16), {PNb}: 78 | * - Tambahkan sanity check saat pengecekan MATRIX_PAKAI_BOUND_CHECKING 79 | * dengan membandingkan baris & kolom dengan MATRIX_MAXIMUM_SIZE. 80 | * - Menambahkan pengecekan matrix untuk operasi dasar antar matrix (*,+,-). 81 | * 82 | * v0.5 (2020-01-14), {PNb}: 83 | * - Buat file matrix.cpp (akhirnya!) untuk definisi fungsi di luar class. 84 | * - Tambahkan operator overloading untuk operasi negatif matrix (mis. a = -b). 85 | * - Tambahkan operator overloading untuk operasi penjumlahan & pengurangan 86 | * dengan scalar. 87 | * - Ubah evaluasi MATRIX_PAKAI_BOUND_CHECKING menggunakan ASSERT. 88 | * - Tambahkan pengecekan index selalu positif di MATRIX_PAKAI_BOUND_CHECKING. 89 | * 90 | * v0.4 (2020-01-10), {PNb}: 91 | * - Tambahkan rounding to zero sebelum operasi sqrt(x) untuk menghindari 92 | * kasus x = 0- 93 | * - Fungsi QRDec mengembalikan Q' dan R (user perlu melakukan transpose 94 | * lagi setelah memanggil QRDec untuk mendapatkan Q). 95 | * - Menambahkan pengecekan hasil HouseholderTransformQR di dalam QRDec. 96 | * - Tambah warning jika MATRIX_PAKAI_BOUND_CHECKING dinonaktifkan. 97 | * 98 | * v0.3_engl (2019-12-31), {PNb}: 99 | * - Modifikasi dokumentasi kode buat orang asing. 100 | * 101 | * v0.3 (2019-12-25), {PNb}: 102 | * - Menambahkan fungsi back subtitution untuk menyelesaikan permasalahan 103 | * persamaan linear Ax = B. Dengan A matrix segitiga atas & B vektor. 104 | * - Memperbaiki bug pengecekan MATRIX_PAKAI_BOUND_CHECKING pada indexing kolom. 105 | * - Menambahkan fungsi QR Decomposition (via Householder Transformation). 106 | * - Menambahkan fungsi Householder Transformation. 107 | * - Menghilangkan warning 'implicit conversion' untuk operasi pembandingan 108 | * dengan float_prec_ZERO. 109 | * - Menambahkan function overloading operasi InsertSubMatrix, untuk 110 | * operasi insert dari SubMatrix ke SubMatrix. 111 | * - Saat inisialisasi, matrix diisi nol (melalui vIsiHomogen(0.0)). 112 | * - Menambahkan function overloading operator '/' dengan scalar. 113 | * 114 | * v0.2 (2019-11-30), {PNb}: 115 | * - Fungsi yang disupport: 116 | * - Operator == 117 | * - Normalisasi matrix 118 | * - Cholesky Decomposition 119 | * - InsertSubMatrix 120 | * - InsertVector 121 | * 122 | * v0.1 (2019-11-29), {PNb}: 123 | * - Fungsi yang disupport: 124 | * - Operasi matrix dasar 125 | * - Invers 126 | * - Cetak 127 | * 128 | * See https://github.com/pronenewbits for more! 129 | ************************************************************************************************************/ 130 | #ifndef MATRIX_H 131 | #define MATRIX_H 132 | 133 | #include "konfig.h" 134 | 135 | #if (SYSTEM_IMPLEMENTATION == SYSTEM_IMPLEMENTATION_PC) 136 | #include 137 | #include // std::setprecision 138 | 139 | using namespace std; 140 | #elif (SYSTEM_IMPLEMENTATION == SYSTEM_IMPLEMENTATION_EMBEDDED_ARDUINO) 141 | #include 142 | #endif 143 | 144 | 145 | class Matrix 146 | { 147 | public: 148 | typedef enum { 149 | InitMatWithZero, /* Initialize matrix with zero data */ 150 | NoInitMatZero 151 | } InitZero; 152 | 153 | 154 | /* --------------------------------------------- Basic Matrix Class functions --------------------------------------------- */ 155 | /* Init empty matrix size _i16row x _i16col */ 156 | Matrix(const int16_t _i16row, const int16_t _i16col, const InitZero _init = InitMatWithZero); 157 | /* Init matrix size _i16row x _i16col with entries initData */ 158 | Matrix(const int16_t _i16row, const int16_t _i16col, const float_prec* initData, const InitZero _init = InitMatWithZero); 159 | /* Copy constructor (for this operation --> A(B)) (copy B into A) */ 160 | Matrix(const Matrix& old_obj); 161 | /* Assignment operator (for this operation --> A = B) (copy B into A) */ 162 | Matrix& operator = (const Matrix& obj); 163 | /* Destructor */ 164 | ~Matrix(void); 165 | /* Get internal state */ 166 | inline int16_t i16getRow(void) const { return this->i16row; } 167 | inline int16_t i16getCol(void) const { return this->i16col; } 168 | 169 | 170 | /* ------------------------------------------- Matrix entry accessing functions ------------------------------------------- */ 171 | /* For example: A(1,2) access the 1st row and 2nd column data of matrix A <--- The preferred way to access the matrix */ 172 | float_prec& operator () (const int16_t _row, const int16_t _col); 173 | float_prec operator () (const int16_t _row, const int16_t _col) const; 174 | 175 | /* For example: A[1][2] access the 1st row and 2nd column data of matrix A <-- The awesome way */ 176 | class Proxy { 177 | public: 178 | Proxy(float_prec* _inpArr, const int16_t _maxCol) { _array.ptr = _inpArr; this->_maxCol = _maxCol; } 179 | Proxy(const float_prec* _inpArr, const int16_t _maxCol) { _array.cptr = _inpArr; this->_maxCol = _maxCol; } 180 | float_prec & operator [] (const int16_t _col); 181 | float_prec operator [] (const int16_t _col) const; 182 | private: 183 | union { /* teehee xp */ 184 | const float_prec* cptr; 185 | float_prec* ptr; 186 | } _array; 187 | int16_t _maxCol; 188 | }; 189 | Proxy operator [] (const int16_t _row); 190 | const Proxy operator [] (const int16_t _row) const; 191 | 192 | 193 | /* ----------------------------------------- Matrix checking function declaration ----------------------------------------- */ 194 | bool bMatrixIsValid(void); 195 | void vSetMatrixInvalid(void); 196 | bool bMatrixIsSquare(); 197 | /* --------------------------------------------- Matrix elementary operations --------------------------------------------- */ 198 | bool operator == (const Matrix& _compare) const; 199 | bool operator != (const Matrix& _compare) const; 200 | Matrix operator - (void) const; 201 | Matrix operator + (const float_prec _scalar) const; 202 | Matrix operator - (const float_prec _scalar) const; 203 | Matrix operator * (const float_prec _scalar) const; 204 | Matrix operator / (const float_prec _scalar) const; 205 | Matrix operator + (const Matrix& _matAdd) const; 206 | Matrix operator - (const Matrix& _matSub) const; 207 | Matrix operator * (const Matrix& _matMul) const; 208 | /* Declared outside class below */ 209 | /* inline Matrix operator + (const float_prec _scalar, Matrix _mat); */ 210 | /* inline Matrix operator - (const float_prec _scalar, Matrix _mat); */ 211 | /* inline Matrix operator * (const float_prec _scalar, Matrix _mat); */ 212 | /* ----------------------------------------------- Simple Matrix operations ----------------------------------------------- */ 213 | void vRoundingElementToZero(const int16_t _i, const int16_t _j); 214 | Matrix RoundingMatrixToZero(void); 215 | void vSetHomogen(const float_prec _val); 216 | void vSetToZero(void); 217 | void vSetRandom(const int32_t _maxRand, const int32_t _minRand); 218 | void vSetDiag(const float_prec _val); 219 | void vSetIdentity(void); 220 | Matrix Transpose(void); 221 | bool bNormVector(void); 222 | /* ------------------------------------------ Matrix/Vector insertion operations ------------------------------------------ */ 223 | Matrix InsertVector(const Matrix& _Vector, const int16_t _posCol); 224 | Matrix InsertSubMatrix(const Matrix& _subMatrix, const int16_t _posRow, const int16_t _posCol); 225 | Matrix InsertSubMatrix(const Matrix& _subMatrix, const int16_t _posRow, const int16_t _posCol, 226 | const int16_t _lenRow, const int16_t _lenColumn); 227 | Matrix InsertSubMatrix(const Matrix& _subMatrix, const int16_t _posRow, const int16_t _posCol, 228 | const int16_t _posRowSub, const int16_t _posColSub, 229 | const int16_t _lenRow, const int16_t _lenColumn); 230 | /* ---------------------------------------------------- Big operations ---------------------------------------------------- */ 231 | /* Matrix invertion using Gauss-Jordan algorithm */ 232 | Matrix Invers(void) const; 233 | /* Check the definiteness of a matrix */ 234 | bool bMatrixIsPositiveDefinite(const bool checkPosSemidefinite = false) const; 235 | /* Return the vector (Mx1 matrix) correspond with the diagonal entries of 'this' */ 236 | Matrix GetDiagonalEntries(void) const; 237 | /* Do the Cholesky Decomposition using Cholesky-Crout algorithm, return 'L' matrix */ 238 | Matrix CholeskyDec(void) const; 239 | /* Do Householder Transformation for QR Decomposition operation */ 240 | Matrix HouseholderTransformQR(const int16_t _rowTransform, const int16_t _colTransform); 241 | /* Do QR Decomposition for matrix using Householder Transformation */ 242 | bool QRDec(Matrix& Qt, Matrix& R) const; 243 | /* Do back-subtitution for upper triangular matrix A & column matrix B: 244 | * x = BackSubtitution(&A, &B) ; for Ax = B 245 | */ 246 | Matrix BackSubtitution(const Matrix& A, const Matrix& B) const; 247 | /* Do forward-subtitution for lower triangular matrix A & column matrix B: 248 | * x = ForwardSubtitution(&A, &B) ; for Ax = B 249 | */ 250 | Matrix ForwardSubtitution(const Matrix& A, const Matrix& B) const; 251 | /* ----------------------------------------------- Matrix printing function ----------------------------------------------- */ 252 | void vPrint(void); 253 | void vPrintFull(void); 254 | 255 | private: 256 | /* Data structure of Matrix class: 257 | * 0 <= i16row <= MATRIX_MAXIMUM_SIZE ; i16row is the row of the matrix. i16row is invalid if (i16row == -1) 258 | * 0 <= i16col <= MATRIX_MAXIMUM_SIZE ; i16col is the column of the matrix. i16col is invalid if (i16col == -1) 259 | * 260 | * Accessing index start from 0 until i16row/i16col, that is: 261 | * (0 <= idxRow < i16row) and (0 <= idxCol < i16col). 262 | * There are 3 ways to access the data: 263 | * 1. A[idxRow][idxCol] <-- Slow, not recommended, but make a cute code. With bounds checking. 264 | * 2. A(idxRow, idxCol) <-- The preferred way. With bounds checking. 265 | * 3. A._at(idxRow, idxCol) <-- Just for internal function usage. Without bounds checking. 266 | * 267 | * floatData[MATRIX_MAXIMUM_SIZE][MATRIX_MAXIMUM_SIZE] is the memory representation of the matrix. We only use the 268 | * first i16row-th and first i16col-th memory for the matrix data. The rest is unused. 269 | * 270 | * This configuration might seems wasteful (yes it is). But with this, we can make the matrix library code as cleanly 271 | * as possible (like I said in the github page, I've made decision to sacrifice speed & performance to get best code 272 | * readability I could get). 273 | * 274 | * You could change the data structure of floatData if you want to make the implementation more memory efficient. 275 | */ 276 | int16_t i16row; 277 | int16_t i16col; 278 | float_prec floatData[MATRIX_MAXIMUM_SIZE][MATRIX_MAXIMUM_SIZE]; 279 | 280 | /* Private way to access floatData without bound checking. 281 | * TODO: For Matrix member function we could do the bound checking once at the beginning of the function, and use this 282 | * to access the floatData instead of (i,j) operator. From preliminary experiment doing this only on elementary 283 | * operation (experiment @2020-04-27), we can get up to 45% computation boost!!! (MPC benchmark 414 us -> 226 us)! 284 | */ 285 | float_prec& _at(const int16_t _row, const int16_t _col) { return this->floatData[_row][_col]; } 286 | float_prec _at(const int16_t _row, const int16_t _col) const { return this->floatData[_row][_col]; } 287 | }; 288 | 289 | inline Matrix operator + (const float_prec _scalar, const Matrix& _mat); 290 | inline Matrix operator - (const float_prec _scalar, const Matrix& _mat); 291 | inline Matrix operator * (const float_prec _scalar, const Matrix& _mat); 292 | inline Matrix MatIdentity(const int16_t _i16size); 293 | 294 | 295 | 296 | 297 | 298 | 299 | 300 | 301 | 302 | 303 | /* ================================================= inline definition below: ================================================= */ 304 | /* ================================================= inline definition below: ================================================= */ 305 | 306 | /* ------------------------------------------ Basic Matrix Class functions ------------------------------------------ */ 307 | /* ------------------------------------------ Basic Matrix Class functions ------------------------------------------ */ 308 | 309 | inline Matrix::Matrix(const int16_t _i16row, const int16_t _i16col, const InitZero _init) { 310 | this->i16row = _i16row; 311 | this->i16col = _i16col; 312 | 313 | if (_init == InitMatWithZero) { 314 | this->vSetHomogen(0.0); 315 | } 316 | } 317 | 318 | inline Matrix::Matrix(const int16_t _i16row, const int16_t _i16col, const float_prec* initData, const InitZero _init) { 319 | this->i16row = _i16row; 320 | this->i16col = _i16col; 321 | 322 | if (_init == InitMatWithZero) { 323 | this->vSetHomogen(0.0); 324 | } 325 | for (int16_t _i = 0; _i < this->i16row; _i++) { 326 | for (int16_t _j = 0; _j < this->i16col; _j++) { 327 | (*this)(_i,_j) = *initData; 328 | initData++; 329 | } 330 | } 331 | } 332 | 333 | inline Matrix::Matrix(const Matrix& old_obj) { 334 | /* For copy contructor, we only need to copy (_i16row x _i16col) submatrix, there's no need to copy all data */ 335 | this->i16row = old_obj.i16row; 336 | this->i16col = old_obj.i16col; 337 | 338 | const float_prec *sourc = old_obj.floatData[0]; 339 | float_prec *desti = this->floatData[0]; 340 | 341 | for (int16_t _i = 0; _i < i16row; _i++) { 342 | /* Still valid with invalid matrix ((i16row == -1) or (i16col == -1)) */ 343 | memcpy(desti, sourc, sizeof(float_prec)*size_t((this->i16col))); 344 | sourc += (MATRIX_MAXIMUM_SIZE); 345 | desti += (MATRIX_MAXIMUM_SIZE); 346 | } 347 | } 348 | 349 | inline Matrix& Matrix::operator = (const Matrix& obj) { 350 | /* For assignment operator, we only need to copy (_i16row x _i16col) submatrix, there's no need to copy all data */ 351 | this->i16row = obj.i16row; 352 | this->i16col = obj.i16col; 353 | 354 | const float_prec *sourc = obj.floatData[0]; 355 | float_prec *desti = this->floatData[0]; 356 | 357 | for (int16_t _i = 0; _i < i16row; _i++) { 358 | /* Still valid with invalid matrix ((i16row == -1) or (i16col == -1)) */ 359 | memcpy(desti, sourc, sizeof(float_prec)*size_t((this->i16col))); 360 | sourc += (MATRIX_MAXIMUM_SIZE); 361 | desti += (MATRIX_MAXIMUM_SIZE); 362 | } 363 | 364 | return *this; 365 | } 366 | 367 | inline Matrix::~Matrix(void) { 368 | /* haven't implemented anything here, definition needed for the rule of three */ 369 | /* (´・ω・`) */ 370 | } 371 | 372 | 373 | /* ---------------------------------------- Matrix entry accessing functions ---------------------------------------- */ 374 | /* ---------------------------------------- Matrix entry accessing functions ---------------------------------------- */ 375 | 376 | /* The preferred method to access the matrix data (boring code) */ 377 | inline float_prec& Matrix::operator () (const int16_t _row, const int16_t _col) { 378 | #if (defined(MATRIX_USE_BOUNDS_CHECKING)) 379 | ASSERT((_row >= 0) && (_row < this->i16row) && (_row < MATRIX_MAXIMUM_SIZE), 380 | "Matrix index out-of-bounds (at row evaluation)"); 381 | ASSERT((_col >= 0) && (_col < this->i16col) && (_col < MATRIX_MAXIMUM_SIZE), 382 | "Matrix index out-of-bounds (at column _column)"); 383 | #else 384 | #warning("Matrix bounds checking is disabled... good luck >:3"); 385 | #endif 386 | return this->floatData[_row][_col]; 387 | } 388 | inline float_prec Matrix::operator () (const int16_t _row, const int16_t _col) const { 389 | #if (defined(MATRIX_USE_BOUNDS_CHECKING)) 390 | ASSERT((_row >= 0) && (_row < this->i16row) && (_row < MATRIX_MAXIMUM_SIZE), 391 | "Matrix index out-of-bounds (at row evaluation)"); 392 | ASSERT((_col >= 0) && (_col < this->i16col) && (_col < MATRIX_MAXIMUM_SIZE), 393 | "Matrix index out-of-bounds (at column _column)"); 394 | #else 395 | #warning("Matrix bounds checking is disabled... good luck >:3"); 396 | #endif 397 | return this->floatData[_row][_col]; 398 | } 399 | 400 | /* Ref: https://stackoverflow.com/questions/6969881/operator-overload 401 | * Modified to be lvalue modifiable (I know this is so dirty, but it makes the code so FABULOUS XD) 402 | */ 403 | inline float_prec & Matrix::Proxy::operator [] (const int16_t _col) { 404 | #if (defined(MATRIX_USE_BOUNDS_CHECKING)) 405 | ASSERT((_col >= 0) && (_col < this->_maxCol) && (_col < MATRIX_MAXIMUM_SIZE), 406 | "Matrix index out-of-bounds (at column evaluation)"); 407 | #else 408 | #warning("Matrix bounds checking is disabled... good luck >:3"); 409 | #endif 410 | return _array.ptr[_col]; 411 | } 412 | inline float_prec Matrix::Proxy::operator [] (const int16_t _col) const { 413 | #if (defined(MATRIX_USE_BOUNDS_CHECKING)) 414 | ASSERT((_col >= 0) && (_col < this->_maxCol) && (_col < MATRIX_MAXIMUM_SIZE), 415 | "Matrix index out-of-bounds (at column evaluation)"); 416 | #else 417 | #warning("Matrix bounds checking is disabled... good luck >:3"); 418 | #endif 419 | return _array.cptr[_col]; 420 | } 421 | inline Matrix::Proxy Matrix::operator [] (const int16_t _row) { 422 | #if (defined(MATRIX_USE_BOUNDS_CHECKING)) 423 | ASSERT((_row >= 0) && (_row < this->i16row) && (_row < MATRIX_MAXIMUM_SIZE), 424 | "Matrix index out-of-bounds (at row evaluation)"); 425 | #else 426 | #warning("Matrix bounds checking is disabled... good luck >:3"); 427 | #endif 428 | return Proxy(floatData[_row], this->i16col); /* Parsing column index for bound checking */ 429 | } 430 | inline const Matrix::Proxy Matrix::operator [] (const int16_t _row) const { 431 | #if (defined(MATRIX_USE_BOUNDS_CHECKING)) 432 | ASSERT((_row >= 0) && (_row < this->i16row) && (_row < MATRIX_MAXIMUM_SIZE), 433 | "Matrix index out-of-bounds (at row evaluation)"); 434 | #else 435 | #warning("Matrix bounds checking is disabled... good luck >:3"); 436 | #endif 437 | return Proxy(floatData[_row], this->i16col); /* Parsing column index for bound checking */ 438 | } 439 | 440 | 441 | /* -------------------------------------- Matrix checking function declaration -------------------------------------- */ 442 | /* -------------------------------------- Matrix checking function declaration -------------------------------------- */ 443 | 444 | inline bool Matrix::bMatrixIsValid(void) { 445 | /* Check whether the matrix is valid or not */ 446 | if ((this->i16row > 0) && (this->i16row <= MATRIX_MAXIMUM_SIZE) && 447 | (this->i16col > 0) && (this->i16col <= MATRIX_MAXIMUM_SIZE)) 448 | { 449 | return true; 450 | } else { 451 | return false; 452 | } 453 | } 454 | 455 | inline void Matrix::vSetMatrixInvalid(void) { 456 | this->i16row = -1; 457 | this->i16col = -1; 458 | } 459 | 460 | inline bool Matrix::bMatrixIsSquare(void) { 461 | return (this->i16row == this->i16col); 462 | } 463 | 464 | 465 | /* ------------------------------------------ Matrix elementary operations ------------------------------------------ */ 466 | /* ------------------------------------------ Matrix elementary operations ------------------------------------------ */ 467 | /* TODO: We could do loop unrolling here for elementary, simple, and matrix insertion operations. It *might* speed up 468 | * the computation time up to 20-30% for processor with FMAC and cached CPU (I still mull on this because 469 | * the code will be awful). 470 | */ 471 | 472 | inline bool Matrix::operator == (const Matrix& _compare) const { 473 | if ((this->i16row != _compare.i16row) || (this->i16col != _compare.i16col)) { 474 | return false; 475 | } 476 | 477 | for (int16_t _i = 0; _i < this->i16row; _i++) { 478 | for (int16_t _j = 0; _j < this->i16col; _j++) { 479 | if (fabs((*this)(_i,_j) - _compare(_i,_j)) > float_prec(float_prec_ZERO_ECO)) { 480 | return false; 481 | } 482 | } 483 | } 484 | return true; 485 | } 486 | 487 | inline bool Matrix::operator != (const Matrix& _compare) const { 488 | return (!(*this == _compare)); 489 | } 490 | 491 | inline Matrix Matrix::operator - (void) const { 492 | Matrix _outp(this->i16row, this->i16col, NoInitMatZero); 493 | 494 | for (int16_t _i = 0; _i < this->i16row; _i++) { 495 | for (int16_t _j = 0; _j < this->i16col; _j++) { 496 | _outp(_i,_j) = -(*this)(_i,_j); 497 | } 498 | } 499 | return _outp; 500 | } 501 | 502 | inline Matrix Matrix::operator + (const float_prec _scalar) const { 503 | Matrix _outp(this->i16row, this->i16col, Matrix::NoInitMatZero); 504 | 505 | for (int16_t _i = 0; _i < this->i16row; _i++) { 506 | for (int16_t _j = 0; _j < this->i16col; _j++) { 507 | _outp(_i,_j) = (*this)(_i,_j) + _scalar; 508 | } 509 | } 510 | return _outp; 511 | } 512 | 513 | inline Matrix Matrix::operator - (const float_prec _scalar) const { 514 | Matrix _outp(this->i16row, this->i16col, Matrix::NoInitMatZero); 515 | 516 | for (int16_t _i = 0; _i < this->i16row; _i++) { 517 | for (int16_t _j = 0; _j < this->i16col; _j++) { 518 | _outp(_i,_j) = (*this)(_i,_j) - _scalar; 519 | } 520 | } 521 | return _outp; 522 | } 523 | 524 | inline Matrix Matrix::operator * (const float_prec _scalar) const { 525 | Matrix _outp(this->i16row, this->i16col, Matrix::NoInitMatZero); 526 | 527 | for (int16_t _i = 0; _i < this->i16row; _i++) { 528 | for (int16_t _j = 0; _j < this->i16col; _j++) { 529 | _outp(_i,_j) = (*this)(_i,_j) * _scalar; 530 | } 531 | } 532 | return _outp; 533 | } 534 | 535 | inline Matrix Matrix::operator / (const float_prec _scalar) const { 536 | Matrix _outp(this->i16row, this->i16col, Matrix::NoInitMatZero); 537 | 538 | if (fabs(_scalar) < float_prec(float_prec_ZERO_ECO)) { 539 | _outp.vSetMatrixInvalid(); 540 | return _outp; 541 | } 542 | for (int16_t _i = 0; _i < this->i16row; _i++) { 543 | for (int16_t _j = 0; _j < this->i16col; _j++) { 544 | _outp(_i,_j) = (*this)(_i,_j) / _scalar; 545 | } 546 | } 547 | return _outp; 548 | } 549 | 550 | 551 | inline Matrix operator + (const float_prec _scalar, const Matrix& _mat) { 552 | Matrix _outp(_mat.i16getRow(), _mat.i16getCol(), Matrix::NoInitMatZero); 553 | 554 | for (int16_t _i = 0; _i < _mat.i16getRow(); _i++) { 555 | for (int16_t _j = 0; _j < _mat.i16getCol(); _j++) { 556 | _outp(_i,_j) = _scalar + _mat(_i,_j); 557 | } 558 | } 559 | return _outp; 560 | } 561 | 562 | 563 | inline Matrix operator - (const float_prec _scalar, const Matrix& _mat) { 564 | Matrix _outp(_mat.i16getRow(), _mat.i16getCol(), Matrix::NoInitMatZero); 565 | 566 | for (int16_t _i = 0; _i < _mat.i16getRow(); _i++) { 567 | for (int16_t _j = 0; _j < _mat.i16getCol(); _j++) { 568 | _outp(_i,_j) = _scalar - _mat(_i,_j); 569 | } 570 | } 571 | return _outp; 572 | } 573 | 574 | 575 | inline Matrix operator * (const float_prec _scalar, const Matrix& _mat) { 576 | Matrix _outp(_mat.i16getRow(), _mat.i16getCol(), Matrix::NoInitMatZero); 577 | 578 | for (int16_t _i = 0; _i < _mat.i16getRow(); _i++) { 579 | for (int16_t _j = 0; _j < _mat.i16getCol(); _j++) { 580 | _outp(_i,_j) = _scalar * _mat(_i,_j); 581 | } 582 | } 583 | return _outp; 584 | } 585 | 586 | inline Matrix Matrix::operator + (const Matrix& _matAdd) const { 587 | Matrix _outp(this->i16row, this->i16col, NoInitMatZero); 588 | if ((this->i16row != _matAdd.i16row) || (this->i16col != _matAdd.i16col)) { 589 | _outp.vSetMatrixInvalid(); 590 | return _outp; 591 | } 592 | 593 | for (int16_t _i = 0; _i < this->i16row; _i++) { 594 | for (int16_t _j = 0; _j < this->i16col; _j++) { 595 | _outp(_i,_j) = (*this)(_i,_j) + _matAdd(_i,_j); 596 | } 597 | } 598 | return _outp; 599 | } 600 | 601 | inline Matrix Matrix::operator - (const Matrix& _matSub) const { 602 | Matrix _outp(this->i16row, this->i16col, NoInitMatZero); 603 | if ((this->i16row != _matSub.i16row) || (this->i16col != _matSub.i16col)) { 604 | _outp.vSetMatrixInvalid(); 605 | return _outp; 606 | } 607 | 608 | for (int16_t _i = 0; _i < this->i16row; _i++) { 609 | for (int16_t _j = 0; _j < this->i16col; _j++) { 610 | _outp(_i,_j) = (*this)(_i,_j) - _matSub(_i,_j); 611 | } 612 | } 613 | return _outp; 614 | } 615 | 616 | inline Matrix Matrix::operator * (const Matrix& _matMul) const { 617 | Matrix _outp(this->i16row, _matMul.i16col, NoInitMatZero); 618 | if ((this->i16col != _matMul.i16row)) { 619 | _outp.vSetMatrixInvalid(); 620 | return _outp; 621 | } 622 | 623 | for (int16_t _i = 0; _i < this->i16row; _i++) { 624 | for (int16_t _j = 0; _j < _matMul.i16col; _j++) { 625 | _outp(_i,_j) = 0.0; 626 | for (int16_t _k = 0; _k < this->i16col; _k++) { 627 | _outp(_i,_j) += ((*this)(_i,_k) * _matMul(_k,_j)); 628 | } 629 | } 630 | } 631 | return _outp; 632 | } 633 | 634 | 635 | /* -------------------------------------------- Simple Matrix operations -------------------------------------------- */ 636 | /* -------------------------------------------- Simple Matrix operations -------------------------------------------- */ 637 | 638 | inline void Matrix::vRoundingElementToZero(const int16_t _i, const int16_t _j) { 639 | if (fabs((*this)(_i,_j)) < float_prec(float_prec_ZERO_ECO)) { 640 | (*this)(_i,_j) = 0.0; 641 | } 642 | } 643 | 644 | inline Matrix Matrix::RoundingMatrixToZero(void) { 645 | for (int16_t _i = 0; _i < this->i16row; _i++) { 646 | for (int16_t _j = 0; _j < this->i16col; _j++) { 647 | if (fabs((*this)(_i,_j)) < float_prec(float_prec_ZERO_ECO)) { 648 | (*this)(_i,_j) = 0.0; 649 | } 650 | } 651 | } 652 | return (*this); 653 | } 654 | 655 | inline void Matrix::vSetHomogen(const float_prec _val) { 656 | for (int16_t _i = 0; _i < this->i16row; _i++) { 657 | for (int16_t _j = 0; _j < this->i16col; _j++) { 658 | (*this)(_i,_j) = _val; 659 | } 660 | } 661 | } 662 | 663 | inline void Matrix::vSetToZero(void) { 664 | this->vSetHomogen(0.0); 665 | } 666 | 667 | inline void Matrix::vSetRandom(const int32_t _maxRand, const int32_t _minRand) { 668 | for (int16_t _i = 0; _i < this->i16row; _i++) { 669 | for (int16_t _j = 0; _j < this->i16col; _j++) { 670 | (*this)(_i,_j) = float_prec((rand() % (_maxRand - _minRand + 1)) + _minRand); 671 | } 672 | } 673 | } 674 | 675 | inline void Matrix::vSetDiag(const float_prec _val) { 676 | for (int16_t _i = 0; _i < this->i16row; _i++) { 677 | for (int16_t _j = 0; _j < this->i16col; _j++) { 678 | if (_i == _j) { 679 | (*this)(_i,_j) = _val; 680 | } else { 681 | (*this)(_i,_j) = 0.0; 682 | } 683 | } 684 | } 685 | } 686 | 687 | inline void Matrix::vSetIdentity(void) { 688 | this->vSetDiag(1.0); 689 | } 690 | 691 | inline Matrix MatIdentity(const int16_t _i16size) { 692 | Matrix _outp(_i16size, _i16size, Matrix::NoInitMatZero); 693 | _outp.vSetDiag(1.0); 694 | return _outp; 695 | } 696 | 697 | /* Return the transpose of the matrix */ 698 | inline Matrix Matrix::Transpose(void) { 699 | Matrix _outp(this->i16col, this->i16row, NoInitMatZero); 700 | for (int16_t _i = 0; _i < this->i16row; _i++) { 701 | for (int16_t _j = 0; _j < this->i16col; _j++) { 702 | _outp(_j,_i) = (*this)(_i,_j); 703 | } 704 | } 705 | return _outp; 706 | } 707 | 708 | /* Normalize the vector */ 709 | inline bool Matrix::bNormVector(void) { 710 | float_prec _normM = 0.0; 711 | for (int16_t _i = 0; _i < this->i16row; _i++) { 712 | for (int16_t _j = 0; _j < this->i16col; _j++) { 713 | _normM = _normM + ((*this)(_i,_j) * (*this)(_i,_j)); 714 | } 715 | } 716 | 717 | /* Rounding to zero to avoid case where sqrt(0-), and _normM always positive */ 718 | if (_normM < float_prec(float_prec_ZERO)) { 719 | return false; 720 | } 721 | _normM = sqrt(_normM); 722 | for (int16_t _i = 0; _i < this->i16row; _i++) { 723 | for (int16_t _j = 0; _j < this->i16col; _j++) { 724 | (*this)(_i,_j) /= _normM; 725 | } 726 | } 727 | return true; 728 | } 729 | 730 | 731 | /* --------------------------------------- Matrix/Vector insertion operations --------------------------------------- */ 732 | /* --------------------------------------- Matrix/Vector insertion operations --------------------------------------- */ 733 | 734 | /* Insert vector into matrix at _posCol position 735 | * Example: A = Matrix 3x3, B = Vector 3x1 736 | * 737 | * C = A.InsertVector(B, 1); 738 | * 739 | * A = [A00 A01 A02] B = [B00] 740 | * [A10 A11 A12] [B10] 741 | * [A20 A21 A22] [B20] 742 | * 743 | * C = [A00 B00 A02] 744 | * [A10 B10 A12] 745 | * [A20 B20 A22] 746 | */ 747 | inline Matrix Matrix::InsertVector(const Matrix& _Vector, const int16_t _posCol) { 748 | Matrix _outp(*this); 749 | if ((_Vector.i16row > this->i16row) || (_Vector.i16col+_posCol > this->i16col)) { 750 | /* Return false */ 751 | _outp.vSetMatrixInvalid(); 752 | return _outp; 753 | } 754 | for (int16_t _i = 0; _i < _Vector.i16row; _i++) { 755 | _outp(_i,_posCol) = _Vector(_i,0); 756 | } 757 | return _outp; 758 | } 759 | 760 | /* Insert submatrix into matrix at _posRow & _posCol position 761 | * Example: A = Matrix 4x4, B = Matrix 2x3 762 | * 763 | * C = A.InsertSubMatrix(B, 1, 1); 764 | * 765 | * A = [A00 A01 A02 A03] B = [B00 B01 B02] 766 | * [A10 A11 A12 A13] [B10 B11 B12] 767 | * [A20 A21 A22 A23] 768 | * [A30 A31 A32 A33] 769 | * 770 | * 771 | * C = [A00 A01 A02 A03] 772 | * [A10 B00 B01 B02] 773 | * [A20 B10 B11 B12] 774 | * [A30 A31 A32 A33] 775 | */ 776 | inline Matrix Matrix::InsertSubMatrix(const Matrix& _subMatrix, const int16_t _posRow, const int16_t _posCol) { 777 | Matrix _outp(*this); 778 | if (((_subMatrix.i16row+_posRow) > this->i16row) || ((_subMatrix.i16col+_posCol) > this->i16col)) { 779 | /* Return false */ 780 | _outp.vSetMatrixInvalid(); 781 | return _outp; 782 | } 783 | for (int16_t _i = 0; _i < _subMatrix.i16row; _i++) { 784 | for (int16_t _j = 0; _j < _subMatrix.i16col; _j++) { 785 | _outp(_i + _posRow,_j + _posCol) = _subMatrix(_i,_j); 786 | } 787 | } 788 | return _outp; 789 | } 790 | 791 | /* Insert the first _lenRow-th and first _lenColumn-th submatrix into matrix; 792 | * at the matrix's _posRow and _posCol position. 793 | * 794 | * Example: A = Matrix 4x4, B = Matrix 2x3 795 | * 796 | * C = A.InsertSubMatrix(B, 1, 1, 2, 2); 797 | * 798 | * A = [A00 A01 A02 A03] B = [B00 B01 B02] 799 | * [A10 A11 A12 A13] [B10 B11 B12] 800 | * [A20 A21 A22 A23] 801 | * [A30 A31 A32 A33] 802 | * 803 | * 804 | * C = [A00 A01 A02 A03] 805 | * [A10 B00 B01 A13] 806 | * [A20 B10 B11 A23] 807 | * [A30 A31 A32 A33] 808 | */ 809 | inline Matrix Matrix::InsertSubMatrix(const Matrix& _subMatrix, const int16_t _posRow, const int16_t _posCol, 810 | const int16_t _lenRow, const int16_t _lenColumn) 811 | { 812 | Matrix _outp(*this); 813 | if (((_lenRow+_posRow) > this->i16row) || ((_lenColumn+_posCol) > this->i16col) || 814 | (_lenRow > _subMatrix.i16row) || (_lenColumn > _subMatrix.i16col)) 815 | { 816 | /* Return false */ 817 | _outp.vSetMatrixInvalid(); 818 | return _outp; 819 | } 820 | for (int16_t _i = 0; _i < _lenRow; _i++) { 821 | for (int16_t _j = 0; _j < _lenColumn; _j++) { 822 | _outp(_i + _posRow,_j + _posCol) = _subMatrix(_i,_j); 823 | } 824 | } 825 | return _outp; 826 | } 827 | 828 | /* Insert the _lenRow & _lenColumn submatrix, start from _posRowSub & _posColSub submatrix; 829 | * into matrix at the matrix's _posRow and _posCol position. 830 | * 831 | * Example: A = Matrix 4x4, B = Matrix 2x3 832 | * 833 | * C = A.InsertSubMatrix(B, 1, 1, 0, 1, 1, 2); 834 | * 835 | * A = [A00 A01 A02 A03] B = [B00 B01 B02] 836 | * [A10 A11 A12 A13] [B10 B11 B12] 837 | * [A20 A21 A22 A23] 838 | * [A30 A31 A32 A33] 839 | * 840 | * C = [A00 A01 A02 A03] 841 | * [A10 B01 B02 A13] 842 | * [A20 A21 A22 A23] 843 | * [A30 A31 A32 A33] 844 | */ 845 | inline Matrix Matrix::InsertSubMatrix(const Matrix& _subMatrix, const int16_t _posRow, const int16_t _posCol, 846 | const int16_t _posRowSub, const int16_t _posColSub, 847 | const int16_t _lenRow, const int16_t _lenColumn) 848 | { 849 | Matrix _outp(*this); 850 | if (((_lenRow+_posRow) > this->i16row) || ((_lenColumn+_posCol) > this->i16col) || 851 | ((_posRowSub+_lenRow) > _subMatrix.i16row) || ((_posColSub+_lenColumn) > _subMatrix.i16col)) 852 | { 853 | /* Return false */ 854 | _outp.vSetMatrixInvalid(); 855 | return _outp; 856 | } 857 | for (int16_t _i = 0; _i < _lenRow; _i++) { 858 | for (int16_t _j = 0; _j < _lenColumn; _j++) { 859 | _outp(_i + _posRow,_j + _posCol) = _subMatrix(_posRowSub+_i,_posColSub+_j); 860 | } 861 | } 862 | return _outp; 863 | } 864 | 865 | 866 | /* ------------------------------------------------- Big operations ------------------------------------------------- */ 867 | /* ------------------------------------------------- Big operations ------------------------------------------------- */ 868 | 869 | /* Invers operation using Gauss-Jordan algorithm */ 870 | inline Matrix Matrix::Invers(void) const { 871 | Matrix _outp(this->i16row, this->i16col, NoInitMatZero); 872 | Matrix _temp(*this); 873 | _outp.vSetIdentity(); 874 | 875 | /* Gauss Elimination... */ 876 | for (int16_t _j = 0; _j < (_temp.i16row)-1; _j++) { 877 | for (int16_t _i = _j+1; _i < _temp.i16row; _i++) { 878 | if (fabs(_temp(_j,_j)) < float_prec(float_prec_ZERO)) { 879 | /* Matrix is non-invertible */ 880 | _outp.vSetMatrixInvalid(); 881 | return _outp; 882 | } 883 | 884 | float_prec _tempfloat = _temp(_i,_j) / _temp(_j,_j); 885 | 886 | for (int16_t _k = 0; _k < _temp.i16col; _k++) { 887 | _temp(_i,_k) -= (_temp(_j,_k) * _tempfloat); 888 | _outp(_i,_k) -= (_outp(_j,_k) * _tempfloat); 889 | 890 | _temp.vRoundingElementToZero(_i, _k); 891 | _outp.vRoundingElementToZero(_i, _k); 892 | } 893 | 894 | } 895 | } 896 | 897 | #if (1) 898 | /* Here, the _temp matrix should be an upper triangular matrix. 899 | * But because of rounding error, it might not. 900 | */ 901 | for (int16_t _i = 1; _i < _temp.i16row; _i++) { 902 | for (int16_t _j = 0; _j < _i; _j++) { 903 | _temp(_i,_j) = 0.0; 904 | } 905 | } 906 | #endif 907 | 908 | 909 | /* Jordan... */ 910 | for (int16_t _j = (_temp.i16row)-1; _j > 0; _j--) { 911 | for (int16_t _i = _j-1; _i >= 0; _i--) { 912 | if (fabs(_temp(_j,_j)) < float_prec(float_prec_ZERO)) { 913 | /* Matrix is non-invertible */ 914 | _outp.vSetMatrixInvalid(); 915 | return _outp; 916 | } 917 | 918 | float_prec _tempfloat = _temp(_i,_j) / _temp(_j,_j); 919 | _temp(_i,_j) -= (_temp(_j,_j) * _tempfloat); 920 | _temp.vRoundingElementToZero(_i, _j); 921 | 922 | for (int16_t _k = (_temp.i16row - 1); _k >= 0; _k--) { 923 | _outp(_i,_k) -= (_outp(_j,_k) * _tempfloat); 924 | _outp.vRoundingElementToZero(_i, _k); 925 | } 926 | } 927 | } 928 | 929 | 930 | /* Normalization */ 931 | for (int16_t _i = 0; _i < _temp.i16row; _i++) { 932 | if (fabs(_temp(_i,_i)) < float_prec(float_prec_ZERO)) { 933 | /* Matrix is non-invertible */ 934 | _outp.vSetMatrixInvalid(); 935 | return _outp; 936 | } 937 | 938 | float_prec _tempfloat = _temp(_i,_i); 939 | _temp(_i,_i) = 1.0; 940 | 941 | for (int16_t _j = 0; _j < _temp.i16row; _j++) { 942 | _outp(_i,_j) /= _tempfloat; 943 | } 944 | } 945 | return _outp; 946 | } 947 | 948 | /* Use elemetary row operation to reduce the matrix into upper triangular form 949 | * (like in the first phase of gauss-jordan algorithm). 950 | * 951 | * Useful if we want to check whether the matrix is positive definite or not 952 | * (useful before calling CholeskyDec function). 953 | */ 954 | inline bool Matrix::bMatrixIsPositiveDefinite(const bool checkPosSemidefinite) const { 955 | bool _posDef, _posSemiDef; 956 | Matrix _temp(*this); 957 | 958 | /* Gauss Elimination... */ 959 | for (int16_t _j = 0; _j < (_temp.i16row)-1; _j++) { 960 | for (int16_t _i = _j+1; _i < _temp.i16row; _i++) { 961 | if (fabs(_temp(_j,_j)) < float_prec(float_prec_ZERO)) { 962 | /* Q: Do we still need to check this? 963 | * A: idk, it's 3 AM. I need sleep :< 964 | * 965 | * NOTE TO FUTURE SELF: Confirm it! 966 | */ 967 | return false; 968 | } 969 | 970 | float_prec _tempfloat = _temp(_i,_j) / _temp(_j,_j); 971 | 972 | for (int16_t _k = 0; _k < _temp.i16col; _k++) { 973 | _temp(_i,_k) -= (_temp(_j,_k) * _tempfloat); 974 | _temp.vRoundingElementToZero(_i, _k); 975 | } 976 | 977 | } 978 | } 979 | 980 | _posDef = true; 981 | _posSemiDef = true; 982 | for (int16_t _i = 0; _i < _temp.i16row; _i++) { 983 | if (_temp(_i,_i) < float_prec(float_prec_ZERO)) { 984 | /* false if less than 0+ (zero included) */ 985 | _posDef = false; 986 | } 987 | if (_temp(_i,_i) < -float_prec(float_prec_ZERO)) { 988 | /* false if less than 0- (zero is not included) */ 989 | _posSemiDef = false; 990 | } 991 | } 992 | 993 | if (checkPosSemidefinite) { 994 | return _posSemiDef; 995 | } else { 996 | return _posDef; 997 | } 998 | } 999 | 1000 | /* For square matrix 'this' with size MxM, return vector Mx1 with entries 1001 | * correspond with diagonal entries of 'this'. 1002 | * 1003 | * Example: this = [a11 a12 a13] 1004 | * [a21 a22 a23] 1005 | * [a31 a32 a33] 1006 | * 1007 | * out = this.GetDiagonalEntries() = [a11] 1008 | * [a22] 1009 | * [a33] 1010 | */ 1011 | inline Matrix Matrix::GetDiagonalEntries(void) const { 1012 | Matrix _temp(this->i16row, 1, NoInitMatZero); 1013 | 1014 | if (this->i16row != this->i16col) { 1015 | _temp.vSetMatrixInvalid(); 1016 | return _temp; 1017 | } 1018 | for (int16_t _i = 0; _i < this->i16row; _i++) { 1019 | _temp(_i,0) = (*this)(_i,_i); 1020 | } 1021 | return _temp; 1022 | } 1023 | 1024 | /* Do the Cholesky Decomposition using Cholesky-Crout algorithm. 1025 | * 1026 | * A = L*L' ; A = real, positive definite, and symmetry MxM matrix 1027 | * 1028 | * L = A.CholeskyDec(); 1029 | * 1030 | * CATATAN! NOTE! The symmetry property is not checked at the beginning to lower 1031 | * the computation cost. The processing is being done on the lower triangular 1032 | * component of _A. Then it is assumed the upper triangular is inherently 1033 | * equal to the lower end. 1034 | * (as a side note, Scilab & MATLAB is using Lapack routines DPOTRF that process 1035 | * the upper triangular of _A. The result should be equal mathematically if A 1036 | * is symmetry). 1037 | */ 1038 | inline Matrix Matrix::CholeskyDec(void) const { 1039 | float_prec _tempFloat; 1040 | 1041 | /* Note that _outp need to be initialized as zero matrix */ 1042 | Matrix _outp(this->i16row, this->i16col, InitMatWithZero); 1043 | 1044 | if (this->i16row != this->i16col) { 1045 | _outp.vSetMatrixInvalid(); 1046 | return _outp; 1047 | } 1048 | for (int16_t _j = 0; _j < this->i16col; _j++) { 1049 | for (int16_t _i = _j; _i < this->i16row; _i++) { 1050 | _tempFloat = (*this)(_i,_j); 1051 | if (_i == _j) { 1052 | for (int16_t _k = 0; _k < _j; _k++) { 1053 | _tempFloat = _tempFloat - (_outp(_i,_k) * _outp(_i,_k)); 1054 | } 1055 | if (_tempFloat < -float_prec(float_prec_ZERO)) { 1056 | /* Matrix is not positif (semi)definit */ 1057 | _outp.vSetMatrixInvalid(); 1058 | return _outp; 1059 | } 1060 | /* Rounding to zero to avoid case where sqrt(0-) */ 1061 | if (fabs(_tempFloat) < float_prec(float_prec_ZERO)) { 1062 | _tempFloat = 0.0; 1063 | } 1064 | _outp(_i,_i) = sqrt(_tempFloat); 1065 | } else { 1066 | for (int16_t _k = 0; _k < _j; _k++) { 1067 | _tempFloat = _tempFloat - (_outp(_i,_k) * _outp(_j,_k)); 1068 | } 1069 | if (fabs(_outp(_j,_j)) < float_prec(float_prec_ZERO)) { 1070 | /* Matrix is not positif definit */ 1071 | _outp.vSetMatrixInvalid(); 1072 | return _outp; 1073 | } 1074 | _outp(_i,_j) = _tempFloat / _outp(_j,_j); 1075 | } 1076 | } 1077 | } 1078 | return _outp; 1079 | } 1080 | 1081 | /* Do the Householder Transformation for QR Decomposition operation. 1082 | * out = HouseholderTransformQR(A, i, j) 1083 | */ 1084 | inline Matrix Matrix::HouseholderTransformQR(const int16_t _rowTransform, const int16_t _colTransform) { 1085 | float_prec _tempFloat; 1086 | float_prec _xLen; 1087 | float_prec _x1; 1088 | float_prec _u1; 1089 | float_prec _vLen2; 1090 | 1091 | /* Note that _outp & _vectTemp need to be initialized as zero matrix */ 1092 | Matrix _outp(this->i16row, this->i16row, InitMatWithZero); 1093 | Matrix _vectTemp(this->i16row, 1, InitMatWithZero); 1094 | 1095 | if ((_rowTransform >= this->i16row) || (_colTransform >= this->i16col)) { 1096 | _outp.vSetMatrixInvalid(); 1097 | return _outp; 1098 | } 1099 | 1100 | /* Until here: 1101 | * 1102 | * _xLen = ||x|| = sqrt(x1^2 + x2^2 + .. + xn^2) 1103 | * _vLen2 = ||u||^2 - [u1^2] = x2^2 + .. + xn^2 1104 | * _vectTemp= [0 0 0 .. x1=0 x2 x3 .. xn]' 1105 | */ 1106 | _x1 = (*this)(_rowTransform,_colTransform); 1107 | _xLen = _x1*_x1; 1108 | _vLen2 = 0.0; 1109 | for (int16_t _i = _rowTransform+1; _i < this->i16row; _i++) { 1110 | _vectTemp(_i,0) = (*this)(_i,_colTransform); 1111 | 1112 | _tempFloat = _vectTemp(_i,0) * _vectTemp(_i,0); 1113 | _xLen += _tempFloat; 1114 | _vLen2 += _tempFloat; 1115 | } 1116 | _xLen = sqrt(_xLen); 1117 | 1118 | /* u1 = x1+(-sign(x1))*xLen */ 1119 | if (_x1 < 0.0) { 1120 | _u1 = _x1+_xLen; 1121 | } else { 1122 | _u1 = _x1-_xLen; 1123 | } 1124 | 1125 | /* Solve vlen2 & tempHH */ 1126 | _vLen2 += (_u1*_u1); 1127 | _vectTemp(_rowTransform,0) = _u1; 1128 | 1129 | if (fabs(_vLen2) < float_prec(float_prec_ZERO_ECO)) { 1130 | /* x vector is collinear with basis vector e, return result = I */ 1131 | _outp.vSetIdentity(); 1132 | } else { 1133 | /* P = -2*(u1*u1')/v_len2 + I */ 1134 | /* PR TODO: We need to investigate more on this */ 1135 | for (int16_t _i = 0; _i < this->i16row; _i++) { 1136 | _tempFloat = _vectTemp(_i,0); 1137 | if (fabs(_tempFloat) > float_prec(float_prec_ZERO)) { 1138 | for (int16_t _j = 0; _j < this->i16row; _j++) { 1139 | if (fabs(_vectTemp(_j,0)) > float_prec(float_prec_ZERO)) { 1140 | _outp(_i,_j) = _vectTemp(_j,0); 1141 | _outp(_i,_j) = _outp(_i,_j) * _tempFloat; 1142 | _outp(_i,_j) = _outp(_i,_j) * (-2.0/_vLen2); 1143 | } 1144 | } 1145 | } 1146 | _outp(_i,_i) = _outp(_i,_i) + 1.0; 1147 | } 1148 | } 1149 | return _outp; 1150 | } 1151 | 1152 | /* Do the QR Decomposition for matrix using Householder Transformation. 1153 | * A = Q*R 1154 | * 1155 | * PERHATIAN! CAUTION! The matrix calculated by this function are Q' and R (Q transpose and R). 1156 | * Because QR Decomposition usually used to calculate solution for least-squares equation 1157 | * (that need Q'), we don't do the transpose of Q inside this routine to lower the 1158 | * computation cost (user need to transpose outside if they want Q). 1159 | * 1160 | * Example of using QRDec to solve least-squares: 1161 | * Ax = b 1162 | * (QR)x = b 1163 | * Rx = Q'b --> Afterward use back-subtitution to solve x 1164 | */ 1165 | inline bool Matrix::QRDec(Matrix& Qt, Matrix& R) const { 1166 | Matrix Qn(Qt.i16row, Qt.i16col, NoInitMatZero); 1167 | if ((this->i16row < this->i16col) || (!Qt.bMatrixIsSquare()) || (Qt.i16row != this->i16row) || 1168 | (R.i16row != this->i16row) || (R.i16col != this->i16col)) 1169 | { 1170 | Qt.vSetMatrixInvalid(); 1171 | R.vSetMatrixInvalid(); 1172 | return false; 1173 | } 1174 | R = (*this); 1175 | Qt.vSetIdentity(); 1176 | for (int16_t _i = 0; (_i < (this->i16row - 1)) && (_i < this->i16col-1); _i++) { 1177 | Qn = R.HouseholderTransformQR(_i, _i); 1178 | if (!Qn.bMatrixIsValid()) { 1179 | Qt.vSetMatrixInvalid(); 1180 | R.vSetMatrixInvalid(); 1181 | return false; 1182 | } 1183 | Qt = Qn * Qt; 1184 | R = Qn * R; 1185 | } 1186 | #if (0) 1187 | Qt.RoundingMatrixToZero(); 1188 | R.RoundingMatrixToZero(); 1189 | #else 1190 | Qt.RoundingMatrixToZero(); 1191 | for (int16_t _i = 1; ((_i < R.i16row) && (_i < R.i16col)); _i++) { 1192 | for (int16_t _j = 0; _j < _i; _j++) { 1193 | R(_i, _j) = 0.0; 1194 | } 1195 | } 1196 | #endif 1197 | 1198 | /* Q = Qt.Transpose */ 1199 | return true; 1200 | } 1201 | 1202 | /* Do the back-subtitution operation for upper triangular matrix A & column matrix B to solve x: 1203 | * Ax = B 1204 | * 1205 | * x = BackSubtitution(&A, &B); 1206 | * 1207 | * CATATAN! NOTE! To lower the computation cost, we don't check that A is a upper triangular 1208 | * matrix (it's assumed that user already make sure of that before calling this routine). 1209 | */ 1210 | inline Matrix Matrix::BackSubtitution(const Matrix& A, const Matrix& B) const { 1211 | Matrix _outp(A.i16row, 1, NoInitMatZero); 1212 | if ((A.i16row != A.i16col) || (A.i16row != B.i16row)) { 1213 | _outp.vSetMatrixInvalid(); 1214 | return _outp; 1215 | } 1216 | 1217 | for (int16_t _i = A.i16col-1; _i >= 0; _i--) { 1218 | _outp(_i,0) = B(_i,0); 1219 | for (int16_t _j = _i + 1; _j < A.i16col; _j++) { 1220 | _outp(_i,0) = _outp(_i,0) - A(_i,_j)*_outp(_j,0); 1221 | } 1222 | if (fabs(A(_i,_i)) < float_prec(float_prec_ZERO)) { 1223 | _outp.vSetMatrixInvalid(); 1224 | return _outp; 1225 | } 1226 | _outp(_i,0) = _outp(_i,0) / A(_i,_i); 1227 | } 1228 | 1229 | return _outp; 1230 | } 1231 | 1232 | /* Do the forward-subtitution operation for lower triangular matrix A & column matrix B to solve x: 1233 | * Ax = B 1234 | * 1235 | * x = ForwardSubtitution(&A, &B); 1236 | * 1237 | * CATATAN! NOTE! To lower the computation cost, we don't check that A is a lower triangular 1238 | * matrix (it's assumed that user already make sure of that before calling this routine). 1239 | */ 1240 | inline Matrix Matrix::ForwardSubtitution(const Matrix& A, const Matrix& B) const { 1241 | Matrix _outp(A.i16row, 1, NoInitMatZero); 1242 | if ((A.i16row != A.i16col) || (A.i16row != B.i16row)) { 1243 | _outp.vSetMatrixInvalid(); 1244 | return _outp; 1245 | } 1246 | 1247 | for (int16_t _i = 0; _i < A.i16row; _i++) { 1248 | _outp(_i,0) = B(_i,0); 1249 | for (int16_t _j = 0; _j < _i; _j++) { 1250 | _outp(_i,0) = _outp(_i,0) - A(_i,_j)*_outp(_j,0); 1251 | } 1252 | if (fabs(A(_i,_i)) < float_prec(float_prec_ZERO)) { 1253 | _outp.vSetMatrixInvalid(); 1254 | return _outp; 1255 | } 1256 | _outp(_i,0) = _outp(_i,0) / A(_i,_i); 1257 | } 1258 | return _outp; 1259 | } 1260 | 1261 | 1262 | /* -------------------------------------------- Matrix printing function -------------------------------------------- */ 1263 | /* -------------------------------------------- Matrix printing function -------------------------------------------- */ 1264 | #if (SYSTEM_IMPLEMENTATION == SYSTEM_IMPLEMENTATION_PC) 1265 | inline void Matrix::vPrint(void) { 1266 | for (int16_t _i = 0; _i < this->i16row; _i++) { 1267 | cout << "[ "; 1268 | for (int16_t _j = 0; _j < this->i16col; _j++) { 1269 | cout << std::fixed << std::setprecision(3) << (*this)(_i,_j) << " "; 1270 | } 1271 | cout << "]"; 1272 | cout << endl; 1273 | } 1274 | cout << endl; 1275 | } 1276 | inline void Matrix::vPrintFull(void) { 1277 | for (int16_t _i = 0; _i < this->i16row; _i++) { 1278 | cout << "[ "; 1279 | for (int16_t _j = 0; _j < this->i16col; _j++) { 1280 | cout << resetiosflags( ios::fixed | ios::showpoint ) << (*this)(_i,_j) << " "; 1281 | } 1282 | cout << "]"; 1283 | cout << endl; 1284 | } 1285 | cout << endl; 1286 | } 1287 | #elif (SYSTEM_IMPLEMENTATION == SYSTEM_IMPLEMENTATION_EMBEDDED_ARDUINO) 1288 | inline void Matrix::vPrint(void) { 1289 | char _bufSer[10]; 1290 | for (int16_t _i = 0; _i < this->i16row; _i++) { 1291 | Serial.print("[ "); 1292 | for (int16_t _j = 0; _j < this->i16col; _j++) { 1293 | snprintf(_bufSer, sizeof(_bufSer)-1, "%2.2f ", (*this)(_i,_j)); 1294 | Serial.print(_bufSer); 1295 | } 1296 | Serial.println("]"); 1297 | } 1298 | Serial.println(""); 1299 | } 1300 | inline void Matrix::vPrintFull(void) { 1301 | char _bufSer[32]; 1302 | for (int16_t _i = 0; _i < this->i16row; _i++) { 1303 | Serial.print("[ "); 1304 | for (int16_t _j = 0; _j < this->i16col; _j++) { 1305 | snprintf(_bufSer, sizeof(_bufSer)-1, "%e ", (*this)(_i,_j)); 1306 | Serial.print(_bufSer); 1307 | } 1308 | Serial.println("]"); 1309 | } 1310 | Serial.println(""); 1311 | } 1312 | #else 1313 | /* User must define the print function somewhere */ 1314 | /* void vPrint(); */ 1315 | /* void vPrintFull(); */ 1316 | #endif 1317 | 1318 | 1319 | #endif // MATRIX_H 1320 | -------------------------------------------------------------------------------- /ukf_engl/ukf.cpp: -------------------------------------------------------------------------------- 1 | /*********************************************************************************************************************** 2 | * Class for Discrete Unscented Kalman Filter 3 | * Ref: Van der. Merwe, .. (2004). Sigma-Point Kalman Filters for Probabilistic Inference in Dynamic 4 | * State-Space Models (Ph.D. thesis). Oregon Health & Science University. 5 | * 6 | * The system to be estimated is defined as a discrete nonlinear dynamic dystem: 7 | * x(k+1) = f[x(k), u(k)] + v(k) ; x = Nx1, u = Mx1 8 | * y(k) = h[x(k), u(k)] + n(k) ; y = Zx1 9 | * 10 | * Where: 11 | * x(k) : State Variable at time-k : Nx1 12 | * y(k) : Measured output at time-k : Zx1 13 | * u(k) : System input at time-k : Mx1 14 | * v(k) : Process noise, AWGN assumed, w/ covariance Rv : Nx1 15 | * n(k) : Measurement noise, AWGN assumed, w/ covariance Rn : Nx1 16 | * 17 | * f(..), h(..) is a nonlinear transformation of the system to be estimated. 18 | * 19 | *********************************************************************************************************************** 20 | * Unscented Kalman Filter algorithm: 21 | * Initialization: 22 | * P(k=0|k=0) = Identity matrix * covariant(P(k=0)), typically initialized with some big number. 23 | * x(k=0|k=0) = Expected value of x at time-0 (i.e. x(k=0)), typically set to zero. 24 | * Rv, Rn = Covariance matrices of process & measurement. As this implementation 25 | * the noise as AWGN (and same value for every variable), this is set 26 | * to Rv=diag(RvInit,...,RvInit) and Rn=diag(RnInit,...,RnInit). 27 | * Wc, Wm = First order & second order weight, respectively. 28 | * alpha, beta, kappa, gamma = scalar constants. 29 | * 30 | * lambda = (alpha^2)*(N+kappa)-N, gamma = sqrt(N+alpha) ...{UKF_1} 31 | * Wm = [lambda/(N+lambda) 1/(2(N+lambda)) ... 1/(2(N+lambda))] ...{UKF_2} 32 | * Wc = [Wm(0)+(1-alpha(^2)+beta) 1/(2(N+lambda)) ... 1/(2(N+lambda))] ...{UKF_3} 33 | * 34 | * 35 | * UKF Calculation (every sampling time): 36 | * Calculate the Sigma Point: 37 | * Xs(k-1) = [x(k-1) ... x(k-1)] ; Xs(k-1) = NxN 38 | * GPsq = gamma * sqrt(P(k-1)) 39 | * XSigma(k-1) = [x(k-1) Xs(k-1)+GPsq Xs(k-1)-GPsq] ...{UKF_4} 40 | * 41 | * 42 | * Unscented Transform XSigma [f,XSigma,u,Rv] -> [x,XSigma,P,DX]: 43 | * XSigma(k) = f(XSigma(k-1), u(k-1)) ...{UKF_5a} 44 | * 45 | * x(k|k-1) = sum(Wm(i) * XSigma(k)(i)) ; i = 1 ... (2N+1) ...{UKF_6a} 46 | * 47 | * DX = XSigma(k)(i) - Xs(k) ; Xs(k) = [x(k|k-1) ... x(k|k-1)] 48 | * ; Xs(k) = Nx(2N+1) ...{UKF_7a} 49 | * 50 | * P(k|k-1) = sum(Wc(i)*DX*DX') + Rv ; i = 1 ... (2N+1) ...{UKF_8a} 51 | * 52 | * 53 | * Unscented Transform YSigma [h,XSigma,u,Rn] -> [y_est,YSigma,Py,DY]: 54 | * YSigma(k) = h(XSigma(k), u(k|k-1)) ; u(k|k-1) = u(k) ...{UKF_5b} 55 | * 56 | * y_est(k) = sum(Wm(i) * YSigma(k)(i)) ; i = 1 ... (2N+1) ...{UKF_6b} 57 | * 58 | * DY = YSigma(k)(i) - Ys(k) ; Ys(k) = [y_est(k) ... y_est(k)] 59 | * ; Ys(k) = Zx(2N+1) ...{UKF_7b} 60 | * 61 | * Py(k) = sum(Wc(i)*DY*DY') + Rn ; i = 1 ... (2N+1) ...{UKF_8b} 62 | * 63 | * 64 | * Calculate Cross-Covariance Matrix: 65 | * Pxy(k) = sum(Wc(i)*DX*DY(i)) ; i = 1 ... (2N+1) ...{UKF_9} 66 | * 67 | * 68 | * Calculate the Kalman Gain: 69 | * K = Pxy(k) * (Py(k)^-1) ...{UKF_10} 70 | * 71 | * 72 | * Update the Estimated State Variable: 73 | * x(k|k) = x(k|k-1) + K * (y(k) - y_est(k)) ...{UKF_11} 74 | * 75 | * 76 | * Update the Covariance Matrix: 77 | * P(k|k) = P(k|k-1) - K*Py(k)*K' ...{UKF_12} 78 | * 79 | * 80 | * *Additional Information: 81 | * - Dengan asumsi masukan plant ZOH, u(k) = u(k|k-1), 82 | * Dengan asumsi tambahan observer dijalankan sebelum pengendali, u(k|k-1) = u(k-1), 83 | * sehingga u(k) [untuk perhitungan kalman] adalah nilai u(k-1) [dari pengendali]. 84 | * - Notasi yang benar adalah u(k|k-1), tapi disini menggunakan notasi u(k) untuk 85 | * menyederhanakan penulisan rumus. 86 | * - Pada contoh di atas X~(k=0|k=0) = [0]. Untuk mempercepat konvergensi bisa digunakan 87 | * informasi plant-spesific. Misal pada implementasi Kalman Filter untuk sensor 88 | * IMU (Inertial measurement unit) dengan X = [quaternion], dengan asumsi IMU 89 | * awalnya menghadap ke atas tanpa rotasi, X~(k=0|k=0) = [1, 0, 0, 0]' 90 | * 91 | * See https://github.com/pronenewbits for more! 92 | **********************************************************************************************************************/ 93 | #include "ukf.h" 94 | 95 | 96 | UKF::UKF(const Matrix& XInit, const Matrix& PInit, const Matrix& Rv, const Matrix& Rn, 97 | bool (*bNonlinearUpdateX)(Matrix&, const Matrix&, const Matrix&), 98 | bool (*bNonlinearUpdateY)(Matrix&, const Matrix&, const Matrix&)) 99 | { 100 | /* Initialization: 101 | * P(k=0|k=0) = Identity matrix * covariant(P(k=0)), typically initialized with some big number. 102 | * x(k=0|k=0) = Expected value of x at time-0 (i.e. x(k=0)), typically set to zero. 103 | * Rv, Rn = Covariance matrices of process & measurement. As this implementation 104 | * the noise as AWGN (and same value for every variable), this is set 105 | * to Rv=diag(RvInit,...,RvInit) and Rn=diag(RnInit,...,RnInit). 106 | */ 107 | this->X_Est = XInit; 108 | this->P = PInit; 109 | this->Rv = Rv; 110 | this->Rn = Rn; 111 | this->bNonlinearUpdateX = bNonlinearUpdateX; 112 | this->bNonlinearUpdateY = bNonlinearUpdateY; 113 | 114 | 115 | /* Van der. Merwe, .. (2004). Sigma-Point Kalman Filters for Probabilistic Inference in Dynamic State-Space Models 116 | * (Ph.D. thesis). Oregon Health & Science University. Page 6: 117 | * 118 | * where λ = α2(L+κ)−L is a scaling parameter. α determines the spread of the sigma points around ̄x and is usually 119 | * set to a small positive value (e.g. 1e−2 ≤ α ≤ 1). κ is a secondary scaling parameter which is usually set to 120 | * either 0 or 3−L (see [45] for details), and β is an extra degree of freedom scalar parameter used to 121 | * incorporate any extra prior knowledge of the distribution of x (for Gaussian distributions, β = 2 is optimal). 122 | */ 123 | float_prec _alpha = 1e-2; 124 | float_prec _k = 0.0; 125 | float_prec _beta = 2.0; 126 | 127 | /* lambda = (alpha^2)*(N+kappa)-N, gamma = sqrt(N+alpha) ...{UKF_1} */ 128 | float_prec _lambda = (_alpha*_alpha)*(SS_X_LEN+_k) - SS_X_LEN; 129 | Gamma = sqrt((SS_X_LEN + _lambda)); 130 | 131 | 132 | /* Wm = [lambda/(N+lambda) 1/(2(N+lambda)) ... 1/(2(N+lambda))] ...{UKF_2} */ 133 | Wm[0][0] = _lambda/(SS_X_LEN + _lambda); 134 | for (int16_t _i = 1; _i < Wm.i16getCol(); _i++) { 135 | Wm[0][_i] = 0.5/(SS_X_LEN + _lambda); 136 | } 137 | 138 | /* Wc = [Wm(0)+(1-alpha(^2)+beta) 1/(2(N+lambda)) ... 1/(2(N+lambda))] ...{UKF_3} */ 139 | Wc = Wm; 140 | Wc[0][0] = Wc[0][0] + (1.0-(_alpha*_alpha)+_beta); 141 | } 142 | 143 | 144 | void UKF::vReset(const Matrix& XInit, const Matrix& PInit, const Matrix& Rv, const Matrix& Rn) 145 | { 146 | this->X_Est = XInit; 147 | this->P = PInit; 148 | this->Rv = Rv; 149 | this->Rn = Rn; 150 | } 151 | 152 | 153 | bool UKF::bUpdate(const Matrix& Y, const Matrix& U) 154 | { 155 | /* Run once every sampling time */ 156 | 157 | /* XSigma(k-1) = [x(k-1) Xs(k-1)+GPsq Xs(k-1)-GPsq] ...{UKF_4} */ 158 | if (!bCalculateSigmaPoint()) { 159 | return false; 160 | } 161 | 162 | 163 | /* Unscented Transform XSigma [f,XSigma,u,Rv] -> [x,XSigma,P,DX]: ...{UKF_5a} - {UKF_8a} */ 164 | if (!bUnscentedTransform(X_Est, X_Sigma, P, DX, bNonlinearUpdateX, X_Sigma, U, Rv)) { 165 | return false; 166 | } 167 | 168 | /* Unscented Transform YSigma [h,XSigma,u,Rn] -> [y_est,YSigma,Py,DY]: ...{UKF_5b} - {UKF_8b} */ 169 | if (!bUnscentedTransform(Y_Est, Y_Sigma, Py, DY, bNonlinearUpdateY, X_Sigma, U, Rn)) { 170 | return false; 171 | } 172 | 173 | 174 | /* Calculate Cross-Covariance Matrix: 175 | * Pxy(k) = sum(Wc(i)*DX*DY(i)) ; i = 1 ... (2N+1) ...{UKF_9} 176 | */ 177 | for (int16_t _i = 0; _i < DX.i16getRow(); _i++) { 178 | for (int16_t _j = 0; _j < DX.i16getCol(); _j++) { 179 | DX[_i][_j] *= Wc[0][_j]; 180 | } 181 | } 182 | Pxy = DX * (DY.Transpose()); 183 | 184 | 185 | /* Calculate the Kalman Gain: 186 | * K = Pxy(k) * (Py(k)^-1) ...{UKF_10} 187 | */ 188 | Matrix PyInv(Py.Invers()); 189 | if (!PyInv.bMatrixIsValid()) { 190 | return false; 191 | } 192 | Gain = Pxy * PyInv; 193 | 194 | 195 | /* Update the Estimated State Variable: 196 | * x(k|k) = x(k|k-1) + K * (y(k) - y_est(k)) ...{UKF_11} 197 | */ 198 | Err = Y - Y_Est; 199 | X_Est = X_Est + (Gain*Err); 200 | 201 | 202 | /* Update the Covariance Matrix: 203 | * P(k|k) = P(k|k-1) - K*Py(k)*K' ...{UKF_12} 204 | */ 205 | P = P - (Gain * Py * Gain.Transpose()); 206 | 207 | 208 | return true; 209 | } 210 | 211 | bool UKF::bCalculateSigmaPoint(void) 212 | { 213 | /* Xs(k-1) = [x(k-1) ... x(k-1)] ; Xs(k-1) = NxN 214 | * GPsq = gamma * sqrt(P(k-1)) 215 | * XSigma(k-1) = [x(k-1) Xs(k-1)+GPsq Xs(k-1)-GPsq] ...{UKF_4} 216 | */ 217 | /* Use Cholesky Decomposition to compute sqrt(P) */ 218 | P_Chol = P.CholeskyDec(); 219 | if (!P_Chol.bMatrixIsValid()) { 220 | /* System Fail */ 221 | return false; 222 | } 223 | P_Chol = P_Chol * Gamma; 224 | 225 | /* Xs(k-1) = [x(k-1) ... x(k-1)] ; Xs(k-1) = NxN */ 226 | Matrix _Y(SS_X_LEN, SS_X_LEN); 227 | for (int16_t _i = 0; _i < SS_X_LEN; _i++) { 228 | _Y = _Y.InsertVector(X_Est, _i); 229 | } 230 | 231 | X_Sigma.vSetToZero(); 232 | /* XSigma(k-1) = [x(k-1) 0 0] */ 233 | X_Sigma = X_Sigma.InsertVector(X_Est, 0); 234 | /* XSigma(k-1) = [x(k-1) Xs(k-1)+GPsq 0] */ 235 | X_Sigma = X_Sigma.InsertSubMatrix((_Y + P_Chol), 0, 1); 236 | /* XSigma(k-1) = [x(k-1) Xs(k-1)+GPsq Xs(k-1)-GPsq] */ 237 | X_Sigma = X_Sigma.InsertSubMatrix((_Y - P_Chol), 0, (1+SS_X_LEN)); 238 | 239 | return true; 240 | } 241 | 242 | bool UKF::bUnscentedTransform(Matrix& Out, Matrix& OutSigma, Matrix& P, Matrix& DSig, 243 | bool (*_vFuncNonLinear)(Matrix& xOut, const Matrix& xInp, const Matrix& U), 244 | const Matrix& InpSigma, const Matrix& InpVector, 245 | const Matrix& _CovNoise) 246 | { 247 | /* XSigma(k) = f(XSigma(k-1), u(k-1)) ...{UKF_5a} */ 248 | /* x(k|k-1) = sum(Wm(i) * XSigma(k)(i)) ; i = 1 ... (2N+1) ...{UKF_6a} */ 249 | Out.vSetToZero(); 250 | for (int16_t _j = 0; _j < InpSigma.i16getCol(); _j++) { 251 | /* Transform the column submatrix of sigma-points input matrix (InpSigma) */ 252 | Matrix _AuxSigma1(InpSigma.i16getRow(), 1); 253 | Matrix _AuxSigma2(OutSigma.i16getRow(), 1); 254 | for (int16_t _i = 0; _i < InpSigma.i16getRow(); _i++) { 255 | _AuxSigma1[_i][0] = InpSigma[_i][_j]; 256 | } 257 | if (!_vFuncNonLinear(_AuxSigma2, _AuxSigma1, InpVector)) { 258 | /* Somehow the transformation function is failed, propagate the error */ 259 | return false; 260 | } 261 | 262 | /* Combine the transformed vector to construct sigma-points output matrix (OutSigma) */ 263 | OutSigma = OutSigma.InsertVector(_AuxSigma2, _j); 264 | 265 | /* Calculate x(k|k-1) as weighted mean of OutSigma */ 266 | _AuxSigma2 = _AuxSigma2 * Wm[0][_j]; 267 | Out = Out + _AuxSigma2; 268 | } 269 | 270 | /* DX = XSigma(k)(i) - Xs(k) ; Xs(k) = [x(k|k-1) ... x(k|k-1)] 271 | * ; Xs(k) = Nx(2N+1) ...{UKF_7a} */ 272 | Matrix _AuxSigma1(OutSigma.i16getRow(), OutSigma.i16getCol()); 273 | for (int16_t _j = 0; _j < OutSigma.i16getCol(); _j++) { 274 | _AuxSigma1 = _AuxSigma1.InsertVector(Out, _j); 275 | } 276 | DSig = OutSigma - _AuxSigma1; 277 | 278 | /* P(k|k-1) = sum(Wc(i)*DX*DX') + Rv ; i = 1 ... (2N+1) ...{UKF_8a} */ 279 | _AuxSigma1 = DSig; 280 | for (int16_t _i = 0; _i < DSig.i16getRow(); _i++) { 281 | for (int16_t _j = 0; _j < DSig.i16getCol(); _j++) { 282 | _AuxSigma1[_i][_j] *= Wc[0][_j]; 283 | } 284 | } 285 | P = (_AuxSigma1 * (DSig.Transpose())) + _CovNoise; 286 | 287 | return true; 288 | } 289 | 290 | -------------------------------------------------------------------------------- /ukf_engl/ukf.h: -------------------------------------------------------------------------------- 1 | /************************************************************************************************************* 2 | * Class for Discrete Unscented Kalman Filter 3 | * 4 | * 5 | * See https://github.com/pronenewbits for more! 6 | ************************************************************************************************************/ 7 | #ifndef UKF_H 8 | #define UKF_H 9 | 10 | #include "konfig.h" 11 | #include "matrix.h" 12 | 13 | #if ((2*SS_X_LEN + 1) > MATRIX_MAXIMUM_SIZE) 14 | #error("The MATRIX_MAXIMUM_SIZE is too small for sigma points (at least need (2*SS_X_LEN + 1))!"); 15 | #endif 16 | 17 | class UKF 18 | { 19 | public: 20 | UKF(const Matrix& XInit, const Matrix& PInit, const Matrix& Rv, const Matrix& Rn, 21 | bool (*bNonlinearUpdateX)(Matrix&, const Matrix&, const Matrix&), 22 | bool (*bNonlinearUpdateY)(Matrix&, const Matrix&, const Matrix&)); 23 | void vReset(const Matrix& XInit, const Matrix& PInit, const Matrix& Rv, const Matrix& Rn); 24 | bool bUpdate(const Matrix& Y, const Matrix& U); 25 | 26 | const Matrix GetX() const { return X_Est; } 27 | const Matrix GetY() const { return Y_Est; } 28 | const Matrix GetP() const { return P; } 29 | const Matrix GetErr() const { return Err; } 30 | 31 | protected: 32 | bool bCalculateSigmaPoint(void); 33 | bool bUnscentedTransform(Matrix& Out, Matrix& OutSigma, Matrix& P, Matrix& DSig, 34 | bool (*_vFuncNonLinear)(Matrix& xOut, const Matrix& xInp, const Matrix& U), 35 | const Matrix& InpSigma, const Matrix& InpVector, 36 | const Matrix& _CovNoise); 37 | 38 | private: 39 | bool (*bNonlinearUpdateX) (Matrix& X_dot, const Matrix& X, const Matrix& U); 40 | bool (*bNonlinearUpdateY) (Matrix& Y_Est, const Matrix& X, const Matrix& U); 41 | 42 | Matrix X_Est{SS_X_LEN, 1}; 43 | Matrix X_Sigma{SS_X_LEN, (2*SS_X_LEN + 1)}; 44 | 45 | Matrix Y_Est{SS_Z_LEN, 1}; 46 | Matrix Y_Sigma{SS_Z_LEN, (2*SS_X_LEN + 1)}; 47 | 48 | Matrix P{SS_X_LEN, SS_X_LEN}; 49 | Matrix P_Chol{SS_X_LEN, SS_X_LEN}; 50 | 51 | Matrix DX{SS_X_LEN, (2*SS_X_LEN + 1)}; 52 | Matrix DY{SS_Z_LEN, (2*SS_X_LEN + 1)}; 53 | 54 | Matrix Py{SS_Z_LEN, SS_Z_LEN}; 55 | Matrix Pxy{SS_X_LEN, SS_Z_LEN}; 56 | 57 | Matrix Wm{1, (2*SS_X_LEN + 1)}; 58 | Matrix Wc{1, (2*SS_X_LEN + 1)}; 59 | 60 | Matrix Rv{SS_X_LEN, SS_X_LEN}; 61 | Matrix Rn{SS_Z_LEN, SS_Z_LEN}; 62 | 63 | Matrix Err{SS_Z_LEN, 1}; 64 | Matrix Gain{SS_X_LEN, SS_Z_LEN}; 65 | float_prec Gamma; 66 | }; 67 | 68 | #endif // UKF_H 69 | -------------------------------------------------------------------------------- /ukf_engl/ukf_engl.ino: -------------------------------------------------------------------------------- 1 | /************************************************************************************************************* 2 | * Template project for Discrete Unscented Kalman Filter library 3 | * 4 | * See https://github.com/pronenewbits for more! 5 | ************************************************************************************************************/ 6 | #include 7 | #include 8 | #include "konfig.h" 9 | #include "matrix.h" 10 | #include "ukf.h" 11 | 12 | 13 | /* ============================================ UKF Variables/function declaration ============================================ */ 14 | /* Just example; in konfig.h: 15 | * SS_X_LEN = 2 16 | * SS_Z_LEN = 1 17 | * SS_U_LEN = 1 18 | */ 19 | /* UKF initialization constant -------------------------------------------------------------------------------------- */ 20 | #define P_INIT (10.) 21 | #define Rv_INIT (1e-6) 22 | #define Rn_INIT (0.0015) 23 | /* P(k=0) variable -------------------------------------------------------------------------------------------------- */ 24 | float_prec UKF_PINIT_data[SS_X_LEN*SS_X_LEN] = {P_INIT, 0, 25 | 0, P_INIT}; 26 | Matrix UKF_PINIT(SS_X_LEN, SS_X_LEN, UKF_PINIT_data); 27 | /* Rv constant ------------------------------------------------------------------------------------------------------ */ 28 | float_prec UKF_RVINIT_data[SS_X_LEN*SS_X_LEN] = {Rv_INIT, 0, 29 | 0, Rv_INIT}; 30 | Matrix UKF_RvINIT(SS_X_LEN, SS_X_LEN, UKF_RVINIT_data); 31 | /* Rn constant ------------------------------------------------------------------------------------------------------ */ 32 | float_prec UKF_RNINIT_data[SS_Z_LEN*SS_Z_LEN] = {Rn_INIT}; 33 | Matrix UKF_RnINIT(SS_Z_LEN, SS_Z_LEN, UKF_RNINIT_data); 34 | /* Nonlinear & linearization function ------------------------------------------------------------------------------- */ 35 | bool Main_bUpdateNonlinearX(Matrix& X_Next, const Matrix& X, const Matrix& U); 36 | bool Main_bUpdateNonlinearY(Matrix& Y, const Matrix& X, const Matrix& U); 37 | /* UKF variables ---------------------------------------------------------------------------------------------------- */ 38 | Matrix X(SS_X_LEN, 1); 39 | Matrix Y(SS_Z_LEN, 1); 40 | Matrix U(SS_U_LEN, 1); 41 | /* UKF system declaration ------------------------------------------------------------------------------------------- */ 42 | UKF UKF_IMU(X, UKF_PINIT, UKF_RvINIT, UKF_RnINIT, Main_bUpdateNonlinearX, Main_bUpdateNonlinearY); 43 | 44 | 45 | 46 | /* ========================================= Auxiliary variables/function declaration ========================================= */ 47 | elapsedMillis timerLed, timerUKF; 48 | uint64_t u64compuTime; 49 | char bufferTxSer[100]; 50 | 51 | 52 | 53 | void setup() { 54 | /* serial to display data */ 55 | Serial.begin(115200); 56 | while(!Serial) {} 57 | 58 | X.vSetToZero(); 59 | UKF_IMU.vReset(X, UKF_PINIT, UKF_RvINIT, UKF_RnINIT); 60 | } 61 | 62 | 63 | void loop() { 64 | if (timerUKF >= SS_DT_MILIS) { 65 | timerUKF = 0; 66 | 67 | 68 | /* ================== Read the sensor data / simulate the system here ================== */ 69 | /* ... */ 70 | /* ------------------ Read the sensor data / simulate the system here ------------------ */ 71 | 72 | 73 | /* ============================= Update the Kalman Filter ============================== */ 74 | u64compuTime = micros(); 75 | if (!UKF_IMU.bUpdate(Y, U)) { 76 | X.vSetToZero(); 77 | UKF_IMU.vReset(X, UKF_PINIT, UKF_RvINIT, UKF_RnINIT); 78 | Serial.println("Whoop "); 79 | } 80 | u64compuTime = (micros() - u64compuTime); 81 | /* ----------------------------- Update the Kalman Filter ------------------------------ */ 82 | 83 | 84 | /* =========================== Print to serial (for plotting) ========================== */ 85 | #if (1) 86 | /* Print: Computation time, X_Est[0] */ 87 | snprintf(bufferTxSer, sizeof(bufferTxSer)-1, "%.3f %.3f ", ((float)u64compuTime)/1000., UKF_IMU.GetX()[0][0]); 88 | Serial.print(bufferTxSer); 89 | #endif 90 | Serial.print('\n'); 91 | /* --------------------------- Print to serial (for plotting) -------------------------- */ 92 | } 93 | } 94 | 95 | 96 | bool Main_bUpdateNonlinearX(Matrix& X_Next, const Matrix& X, const Matrix& U) 97 | { 98 | /* Insert the nonlinear update transformation here 99 | * x(k+1) = f[x(k), u(k)] 100 | */ 101 | 102 | return true; 103 | } 104 | 105 | bool Main_bUpdateNonlinearY(Matrix& Y, const Matrix& X, const Matrix& U) 106 | { 107 | /* Insert the nonlinear measurement transformation here 108 | * y(k) = h[x(k), u(k)] 109 | */ 110 | 111 | return true; 112 | } 113 | 114 | 115 | 116 | 117 | 118 | void SPEW_THE_ERROR(char const * str) 119 | { 120 | #if (SYSTEM_IMPLEMENTATION == SYSTEM_IMPLEMENTATION_PC) 121 | cout << (str) << endl; 122 | #elif (SYSTEM_IMPLEMENTATION == SYSTEM_IMPLEMENTATION_EMBEDDED_ARDUINO) 123 | Serial.println(str); 124 | #else 125 | /* Silent function */ 126 | #endif 127 | while(1); 128 | } 129 | -------------------------------------------------------------------------------- /ukf_example1_pendulum/Pendulum_eq.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pronenewbits/Embedded_UKF_Library/3c16b4156442674cdbb9ac44bf24de5380521ff7/ukf_example1_pendulum/Pendulum_eq.png -------------------------------------------------------------------------------- /ukf_example1_pendulum/Pendulum_picture.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pronenewbits/Embedded_UKF_Library/3c16b4156442674cdbb9ac44bf24de5380521ff7/ukf_example1_pendulum/Pendulum_picture.png -------------------------------------------------------------------------------- /ukf_example1_pendulum/README.md: -------------------------------------------------------------------------------- 1 | # The system description 2 | 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. 3 | 4 | The variables of the system can be seen from this picture: 5 |

Pendulum picture

6 | 7 | While the system equations can be described as: 8 |

Pendulum equation

9 | 10 | # The implementation 11 | First we set the system parameters in `konfig.h`: 12 | 13 | /* State Space dimension */ 14 | #define SS_X_LEN (2) 15 | #define SS_Z_LEN (2) 16 | #define SS_U_LEN (0) 17 | 18 | Then we implement the state space nonlinear update & measurement function in `ukf_pend_engl.ino`: 19 | 20 | bool Main_bUpdateNonlinearX(Matrix &X_Next, Matrix &X, Matrix &U) 21 | { 22 | /* The update function in discrete time: 23 | * x1(k+1) = x1(k) + x2(k)*dt 24 | * x2(k+1) = x2(k) - g/l*sin(x1(k))*dt - alpha*x2*dt 25 | */ 26 | ... 27 | return true; 28 | } 29 | 30 | bool Main_bUpdateNonlinearY(Matrix &Y, Matrix &X, Matrix &U) 31 | { 32 | /* The output (in discrete time): 33 | * y1(k) = sin(x1(k)) * l 34 | * y2(k) = -cos(x1(k)) * l 35 | */ 36 | ... 37 | return true; 38 | } 39 | 40 | 41 | 42 | 43 | To demonstrate the UKF capability, we add noise at the output system (also in `ukf_pend_engl.ino`): 44 | 45 | /* ================== Read the sensor data / simulate the system here ================== */ 46 | .... 47 | /* Let's add some noise! */ 48 | Y[0][0] += (float((rand() % 20) - 10) / 10.); /* add +/- 1 meters noise to x position */ 49 | 50 | /* ------------------ Read the sensor data / simulate the system here ------------------ */ 51 | 52 | And set the UKF initial pendulum angle `(theta(k=0))` as different with the true initial pendulum angle: 53 | 54 | /* For example, let's set the theta(k=0) = pi/2 (i.e. the pendulum rod is parallel with the horizontal plane) */ 55 | X_true[0][0] = 3.14159265359/2.; 56 | 57 | /* Observe that we set the wrong initial x_estimated value! (X_UKF(k=0) != X_TRUE(k=0)) */ 58 | X_est_init[0][0] = -3.14159265359/2.; 59 | 60 | 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): 61 |

Result for Pendulum simulation

62 | 63 | -------------------------------------------------------------------------------- /ukf_example1_pendulum/result.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pronenewbits/Embedded_UKF_Library/3c16b4156442674cdbb9ac44bf24de5380521ff7/ukf_example1_pendulum/result.png -------------------------------------------------------------------------------- /ukf_example1_pendulum/ukf_pend_engl/konfig.h: -------------------------------------------------------------------------------- 1 | /************************************************************************************************************* 2 | * This file contains configuration parameters 3 | * 4 | * 5 | * See https://github.com/pronenewbits for more! 6 | ************************************************************************************************************/ 7 | #ifndef KONFIG_H 8 | #define KONFIG_H 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | 15 | 16 | /* State Space dimension */ 17 | #define SS_X_LEN (2) 18 | #define SS_Z_LEN (2) 19 | #define SS_U_LEN (0) 20 | #define SS_DT_MILIS (20) /* 20 ms */ 21 | #define SS_DT float_prec(SS_DT_MILIS/1000.) /* Sampling time */ 22 | 23 | 24 | /* Change this size based on the biggest matrix you will use */ 25 | #define MATRIX_MAXIMUM_SIZE (5) 26 | 27 | /* Define this to enable matrix bound checking */ 28 | #define MATRIX_USE_BOUNDS_CHECKING 29 | 30 | /* Set this define to choose math precision of the system */ 31 | #define PRECISION_SINGLE 1 32 | #define PRECISION_DOUBLE 2 33 | #define FPU_PRECISION (PRECISION_SINGLE) 34 | 35 | #if (FPU_PRECISION == PRECISION_SINGLE) 36 | #define float_prec float 37 | #define float_prec_ZERO (1e-7) 38 | #define float_prec_ZERO_ECO (1e-5) /* 'Economical' zero, for noisy calculation where 'somewhat zero' is good enough */ 39 | #elif (FPU_PRECISION == PRECISION_DOUBLE) 40 | #define float_prec double 41 | #define float_prec_ZERO (1e-13) 42 | #define float_prec_ZERO_ECO (1e-8) /* 'Economical' zero, for noisy calculation where 'somewhat zero' is good enough */ 43 | #else 44 | #error("FPU_PRECISION has not been defined!"); 45 | #endif 46 | 47 | 48 | 49 | /* Set this define to choose system implementation (mainly used to define how you print the matrix via the Matrix::vPrint() & Matrix::vPrintFull() function) */ 50 | #define SYSTEM_IMPLEMENTATION_PC 1 51 | #define SYSTEM_IMPLEMENTATION_EMBEDDED_CUSTOM 2 52 | #define SYSTEM_IMPLEMENTATION_EMBEDDED_ARDUINO 3 53 | 54 | #define SYSTEM_IMPLEMENTATION (SYSTEM_IMPLEMENTATION_EMBEDDED_ARDUINO) 55 | 56 | 57 | 58 | /* ASSERT is evaluated locally (without function call) to lower the computation cost */ 59 | void SPEW_THE_ERROR(char const * str); 60 | #define ASSERT(truth, str) { if (!(truth)) SPEW_THE_ERROR(str); } 61 | 62 | 63 | #endif // KONFIG_H 64 | -------------------------------------------------------------------------------- /ukf_example1_pendulum/ukf_pend_engl/matrix.cpp: -------------------------------------------------------------------------------- 1 | /************************************************************************************ 2 | * Class Matrix 3 | * See matrix.h for description 4 | * 5 | * See https://github.com/pronenewbits for more! 6 | *************************************************************************************/ 7 | 8 | 9 | #include "matrix.h" 10 | 11 | /* There's nothing here yet! */ 12 | 13 | -------------------------------------------------------------------------------- /ukf_example1_pendulum/ukf_pend_engl/ukf.cpp: -------------------------------------------------------------------------------- 1 | /*********************************************************************************************************************** 2 | * Class for Discrete Unscented Kalman Filter 3 | * Ref: Van der. Merwe, .. (2004). Sigma-Point Kalman Filters for Probabilistic Inference in Dynamic 4 | * State-Space Models (Ph.D. thesis). Oregon Health & Science University. 5 | * 6 | * The system to be estimated is defined as a discrete nonlinear dynamic dystem: 7 | * x(k+1) = f[x(k), u(k)] + v(k) ; x = Nx1, u = Mx1 8 | * y(k) = h[x(k), u(k)] + n(k) ; y = Zx1 9 | * 10 | * Where: 11 | * x(k) : State Variable at time-k : Nx1 12 | * y(k) : Measured output at time-k : Zx1 13 | * u(k) : System input at time-k : Mx1 14 | * v(k) : Process noise, AWGN assumed, w/ covariance Rv : Nx1 15 | * n(k) : Measurement noise, AWGN assumed, w/ covariance Rn : Nx1 16 | * 17 | * f(..), h(..) is a nonlinear transformation of the system to be estimated. 18 | * 19 | *********************************************************************************************************************** 20 | * Unscented Kalman Filter algorithm: 21 | * Initialization: 22 | * P(k=0|k=0) = Identity matrix * covariant(P(k=0)), typically initialized with some big number. 23 | * x(k=0|k=0) = Expected value of x at time-0 (i.e. x(k=0)), typically set to zero. 24 | * Rv, Rn = Covariance matrices of process & measurement. As this implementation 25 | * the noise as AWGN (and same value for every variable), this is set 26 | * to Rv=diag(RvInit,...,RvInit) and Rn=diag(RnInit,...,RnInit). 27 | * Wc, Wm = First order & second order weight, respectively. 28 | * alpha, beta, kappa, gamma = scalar constants. 29 | * 30 | * lambda = (alpha^2)*(N+kappa)-N, gamma = sqrt(N+alpha) ...{UKF_1} 31 | * Wm = [lambda/(N+lambda) 1/(2(N+lambda)) ... 1/(2(N+lambda))] ...{UKF_2} 32 | * Wc = [Wm(0)+(1-alpha(^2)+beta) 1/(2(N+lambda)) ... 1/(2(N+lambda))] ...{UKF_3} 33 | * 34 | * 35 | * UKF Calculation (every sampling time): 36 | * Calculate the Sigma Point: 37 | * Xs(k-1) = [x(k-1) ... x(k-1)] ; Xs(k-1) = NxN 38 | * GPsq = gamma * sqrt(P(k-1)) 39 | * XSigma(k-1) = [x(k-1) Xs(k-1)+GPsq Xs(k-1)-GPsq] ...{UKF_4} 40 | * 41 | * 42 | * Unscented Transform XSigma [f,XSigma,u,Rv] -> [x,XSigma,P,DX]: 43 | * XSigma(k) = f(XSigma(k-1), u(k-1)) ...{UKF_5a} 44 | * 45 | * x(k|k-1) = sum(Wm(i) * XSigma(k)(i)) ; i = 1 ... (2N+1) ...{UKF_6a} 46 | * 47 | * DX = XSigma(k)(i) - Xs(k) ; Xs(k) = [x(k|k-1) ... x(k|k-1)] 48 | * ; Xs(k) = Nx(2N+1) ...{UKF_7a} 49 | * 50 | * P(k|k-1) = sum(Wc(i)*DX*DX') + Rv ; i = 1 ... (2N+1) ...{UKF_8a} 51 | * 52 | * 53 | * Unscented Transform YSigma [h,XSigma,u,Rn] -> [y_est,YSigma,Py,DY]: 54 | * YSigma(k) = h(XSigma(k), u(k|k-1)) ; u(k|k-1) = u(k) ...{UKF_5b} 55 | * 56 | * y_est(k) = sum(Wm(i) * YSigma(k)(i)) ; i = 1 ... (2N+1) ...{UKF_6b} 57 | * 58 | * DY = YSigma(k)(i) - Ys(k) ; Ys(k) = [y_est(k) ... y_est(k)] 59 | * ; Ys(k) = Zx(2N+1) ...{UKF_7b} 60 | * 61 | * Py(k) = sum(Wc(i)*DY*DY') + Rn ; i = 1 ... (2N+1) ...{UKF_8b} 62 | * 63 | * 64 | * Calculate Cross-Covariance Matrix: 65 | * Pxy(k) = sum(Wc(i)*DX*DY(i)) ; i = 1 ... (2N+1) ...{UKF_9} 66 | * 67 | * 68 | * Calculate the Kalman Gain: 69 | * K = Pxy(k) * (Py(k)^-1) ...{UKF_10} 70 | * 71 | * 72 | * Update the Estimated State Variable: 73 | * x(k|k) = x(k|k-1) + K * (y(k) - y_est(k)) ...{UKF_11} 74 | * 75 | * 76 | * Update the Covariance Matrix: 77 | * P(k|k) = P(k|k-1) - K*Py(k)*K' ...{UKF_12} 78 | * 79 | * 80 | * *Additional Information: 81 | * - Dengan asumsi masukan plant ZOH, u(k) = u(k|k-1), 82 | * Dengan asumsi tambahan observer dijalankan sebelum pengendali, u(k|k-1) = u(k-1), 83 | * sehingga u(k) [untuk perhitungan kalman] adalah nilai u(k-1) [dari pengendali]. 84 | * - Notasi yang benar adalah u(k|k-1), tapi disini menggunakan notasi u(k) untuk 85 | * menyederhanakan penulisan rumus. 86 | * - Pada contoh di atas X~(k=0|k=0) = [0]. Untuk mempercepat konvergensi bisa digunakan 87 | * informasi plant-spesific. Misal pada implementasi Kalman Filter untuk sensor 88 | * IMU (Inertial measurement unit) dengan X = [quaternion], dengan asumsi IMU 89 | * awalnya menghadap ke atas tanpa rotasi, X~(k=0|k=0) = [1, 0, 0, 0]' 90 | * 91 | * See https://github.com/pronenewbits for more! 92 | **********************************************************************************************************************/ 93 | #include "ukf.h" 94 | 95 | 96 | UKF::UKF(const Matrix& XInit, const Matrix& PInit, const Matrix& Rv, const Matrix& Rn, 97 | bool (*bNonlinearUpdateX)(Matrix&, const Matrix&, const Matrix&), 98 | bool (*bNonlinearUpdateY)(Matrix&, const Matrix&, const Matrix&)) 99 | { 100 | /* Initialization: 101 | * P(k=0|k=0) = Identity matrix * covariant(P(k=0)), typically initialized with some big number. 102 | * x(k=0|k=0) = Expected value of x at time-0 (i.e. x(k=0)), typically set to zero. 103 | * Rv, Rn = Covariance matrices of process & measurement. As this implementation 104 | * the noise as AWGN (and same value for every variable), this is set 105 | * to Rv=diag(RvInit,...,RvInit) and Rn=diag(RnInit,...,RnInit). 106 | */ 107 | this->X_Est = XInit; 108 | this->P = PInit; 109 | this->Rv = Rv; 110 | this->Rn = Rn; 111 | this->bNonlinearUpdateX = bNonlinearUpdateX; 112 | this->bNonlinearUpdateY = bNonlinearUpdateY; 113 | 114 | 115 | /* Van der. Merwe, .. (2004). Sigma-Point Kalman Filters for Probabilistic Inference in Dynamic State-Space Models 116 | * (Ph.D. thesis). Oregon Health & Science University. Page 6: 117 | * 118 | * where λ = α2(L+κ)−L is a scaling parameter. α determines the spread of the sigma points around ̄x and is usually 119 | * set to a small positive value (e.g. 1e−2 ≤ α ≤ 1). κ is a secondary scaling parameter which is usually set to 120 | * either 0 or 3−L (see [45] for details), and β is an extra degree of freedom scalar parameter used to 121 | * incorporate any extra prior knowledge of the distribution of x (for Gaussian distributions, β = 2 is optimal). 122 | */ 123 | float_prec _alpha = 1e-2; 124 | float_prec _k = 0.0; 125 | float_prec _beta = 2.0; 126 | 127 | /* lambda = (alpha^2)*(N+kappa)-N, gamma = sqrt(N+alpha) ...{UKF_1} */ 128 | float_prec _lambda = (_alpha*_alpha)*(SS_X_LEN+_k) - SS_X_LEN; 129 | Gamma = sqrt((SS_X_LEN + _lambda)); 130 | 131 | 132 | /* Wm = [lambda/(N+lambda) 1/(2(N+lambda)) ... 1/(2(N+lambda))] ...{UKF_2} */ 133 | Wm[0][0] = _lambda/(SS_X_LEN + _lambda); 134 | for (int16_t _i = 1; _i < Wm.i16getCol(); _i++) { 135 | Wm[0][_i] = 0.5/(SS_X_LEN + _lambda); 136 | } 137 | 138 | /* Wc = [Wm(0)+(1-alpha(^2)+beta) 1/(2(N+lambda)) ... 1/(2(N+lambda))] ...{UKF_3} */ 139 | Wc = Wm; 140 | Wc[0][0] = Wc[0][0] + (1.0-(_alpha*_alpha)+_beta); 141 | } 142 | 143 | 144 | void UKF::vReset(const Matrix& XInit, const Matrix& PInit, const Matrix& Rv, const Matrix& Rn) 145 | { 146 | this->X_Est = XInit; 147 | this->P = PInit; 148 | this->Rv = Rv; 149 | this->Rn = Rn; 150 | } 151 | 152 | 153 | bool UKF::bUpdate(const Matrix& Y, const Matrix& U) 154 | { 155 | /* Run once every sampling time */ 156 | 157 | /* XSigma(k-1) = [x(k-1) Xs(k-1)+GPsq Xs(k-1)-GPsq] ...{UKF_4} */ 158 | if (!bCalculateSigmaPoint()) { 159 | return false; 160 | } 161 | 162 | 163 | /* Unscented Transform XSigma [f,XSigma,u,Rv] -> [x,XSigma,P,DX]: ...{UKF_5a} - {UKF_8a} */ 164 | if (!bUnscentedTransform(X_Est, X_Sigma, P, DX, bNonlinearUpdateX, X_Sigma, U, Rv)) { 165 | return false; 166 | } 167 | 168 | /* Unscented Transform YSigma [h,XSigma,u,Rn] -> [y_est,YSigma,Py,DY]: ...{UKF_5b} - {UKF_8b} */ 169 | if (!bUnscentedTransform(Y_Est, Y_Sigma, Py, DY, bNonlinearUpdateY, X_Sigma, U, Rn)) { 170 | return false; 171 | } 172 | 173 | 174 | /* Calculate Cross-Covariance Matrix: 175 | * Pxy(k) = sum(Wc(i)*DX*DY(i)) ; i = 1 ... (2N+1) ...{UKF_9} 176 | */ 177 | for (int16_t _i = 0; _i < DX.i16getRow(); _i++) { 178 | for (int16_t _j = 0; _j < DX.i16getCol(); _j++) { 179 | DX[_i][_j] *= Wc[0][_j]; 180 | } 181 | } 182 | Pxy = DX * (DY.Transpose()); 183 | 184 | 185 | /* Calculate the Kalman Gain: 186 | * K = Pxy(k) * (Py(k)^-1) ...{UKF_10} 187 | */ 188 | Matrix PyInv(Py.Invers()); 189 | if (!PyInv.bMatrixIsValid()) { 190 | return false; 191 | } 192 | Gain = Pxy * PyInv; 193 | 194 | 195 | /* Update the Estimated State Variable: 196 | * x(k|k) = x(k|k-1) + K * (y(k) - y_est(k)) ...{UKF_11} 197 | */ 198 | Err = Y - Y_Est; 199 | X_Est = X_Est + (Gain*Err); 200 | 201 | 202 | /* Update the Covariance Matrix: 203 | * P(k|k) = P(k|k-1) - K*Py(k)*K' ...{UKF_12} 204 | */ 205 | P = P - (Gain * Py * Gain.Transpose()); 206 | 207 | 208 | return true; 209 | } 210 | 211 | bool UKF::bCalculateSigmaPoint(void) 212 | { 213 | /* Xs(k-1) = [x(k-1) ... x(k-1)] ; Xs(k-1) = NxN 214 | * GPsq = gamma * sqrt(P(k-1)) 215 | * XSigma(k-1) = [x(k-1) Xs(k-1)+GPsq Xs(k-1)-GPsq] ...{UKF_4} 216 | */ 217 | /* Use Cholesky Decomposition to compute sqrt(P) */ 218 | P_Chol = P.CholeskyDec(); 219 | if (!P_Chol.bMatrixIsValid()) { 220 | /* System Fail */ 221 | return false; 222 | } 223 | P_Chol = P_Chol * Gamma; 224 | 225 | /* Xs(k-1) = [x(k-1) ... x(k-1)] ; Xs(k-1) = NxN */ 226 | Matrix _Y(SS_X_LEN, SS_X_LEN); 227 | for (int16_t _i = 0; _i < SS_X_LEN; _i++) { 228 | _Y = _Y.InsertVector(X_Est, _i); 229 | } 230 | 231 | X_Sigma.vSetToZero(); 232 | /* XSigma(k-1) = [x(k-1) 0 0] */ 233 | X_Sigma = X_Sigma.InsertVector(X_Est, 0); 234 | /* XSigma(k-1) = [x(k-1) Xs(k-1)+GPsq 0] */ 235 | X_Sigma = X_Sigma.InsertSubMatrix((_Y + P_Chol), 0, 1); 236 | /* XSigma(k-1) = [x(k-1) Xs(k-1)+GPsq Xs(k-1)-GPsq] */ 237 | X_Sigma = X_Sigma.InsertSubMatrix((_Y - P_Chol), 0, (1+SS_X_LEN)); 238 | 239 | return true; 240 | } 241 | 242 | bool UKF::bUnscentedTransform(Matrix& Out, Matrix& OutSigma, Matrix& P, Matrix& DSig, 243 | bool (*_vFuncNonLinear)(Matrix& xOut, const Matrix& xInp, const Matrix& U), 244 | const Matrix& InpSigma, const Matrix& InpVector, 245 | const Matrix& _CovNoise) 246 | { 247 | /* XSigma(k) = f(XSigma(k-1), u(k-1)) ...{UKF_5a} */ 248 | /* x(k|k-1) = sum(Wm(i) * XSigma(k)(i)) ; i = 1 ... (2N+1) ...{UKF_6a} */ 249 | Out.vSetToZero(); 250 | for (int16_t _j = 0; _j < InpSigma.i16getCol(); _j++) { 251 | /* Transform the column submatrix of sigma-points input matrix (InpSigma) */ 252 | Matrix _AuxSigma1(InpSigma.i16getRow(), 1); 253 | Matrix _AuxSigma2(OutSigma.i16getRow(), 1); 254 | for (int16_t _i = 0; _i < InpSigma.i16getRow(); _i++) { 255 | _AuxSigma1[_i][0] = InpSigma[_i][_j]; 256 | } 257 | if (!_vFuncNonLinear(_AuxSigma2, _AuxSigma1, InpVector)) { 258 | /* Somehow the transformation function is failed, propagate the error */ 259 | return false; 260 | } 261 | 262 | /* Combine the transformed vector to construct sigma-points output matrix (OutSigma) */ 263 | OutSigma = OutSigma.InsertVector(_AuxSigma2, _j); 264 | 265 | /* Calculate x(k|k-1) as weighted mean of OutSigma */ 266 | _AuxSigma2 = _AuxSigma2 * Wm[0][_j]; 267 | Out = Out + _AuxSigma2; 268 | } 269 | 270 | /* DX = XSigma(k)(i) - Xs(k) ; Xs(k) = [x(k|k-1) ... x(k|k-1)] 271 | * ; Xs(k) = Nx(2N+1) ...{UKF_7a} */ 272 | Matrix _AuxSigma1(OutSigma.i16getRow(), OutSigma.i16getCol()); 273 | for (int16_t _j = 0; _j < OutSigma.i16getCol(); _j++) { 274 | _AuxSigma1 = _AuxSigma1.InsertVector(Out, _j); 275 | } 276 | DSig = OutSigma - _AuxSigma1; 277 | 278 | /* P(k|k-1) = sum(Wc(i)*DX*DX') + Rv ; i = 1 ... (2N+1) ...{UKF_8a} */ 279 | _AuxSigma1 = DSig; 280 | for (int16_t _i = 0; _i < DSig.i16getRow(); _i++) { 281 | for (int16_t _j = 0; _j < DSig.i16getCol(); _j++) { 282 | _AuxSigma1[_i][_j] *= Wc[0][_j]; 283 | } 284 | } 285 | P = (_AuxSigma1 * (DSig.Transpose())) + _CovNoise; 286 | 287 | return true; 288 | } 289 | 290 | -------------------------------------------------------------------------------- /ukf_example1_pendulum/ukf_pend_engl/ukf.h: -------------------------------------------------------------------------------- 1 | /************************************************************************************************************* 2 | * Class for Discrete Unscented Kalman Filter 3 | * 4 | * 5 | * See https://github.com/pronenewbits for more! 6 | ************************************************************************************************************/ 7 | #ifndef UKF_H 8 | #define UKF_H 9 | 10 | #include "konfig.h" 11 | #include "matrix.h" 12 | 13 | #if ((2*SS_X_LEN + 1) > MATRIX_MAXIMUM_SIZE) 14 | #error("The MATRIX_MAXIMUM_SIZE is too small for sigma points (at least need (2*SS_X_LEN + 1))!"); 15 | #endif 16 | 17 | class UKF 18 | { 19 | public: 20 | UKF(const Matrix& XInit, const Matrix& PInit, const Matrix& Rv, const Matrix& Rn, 21 | bool (*bNonlinearUpdateX)(Matrix&, const Matrix&, const Matrix&), 22 | bool (*bNonlinearUpdateY)(Matrix&, const Matrix&, const Matrix&)); 23 | void vReset(const Matrix& XInit, const Matrix& PInit, const Matrix& Rv, const Matrix& Rn); 24 | bool bUpdate(const Matrix& Y, const Matrix& U); 25 | 26 | const Matrix GetX() const { return X_Est; } 27 | const Matrix GetY() const { return Y_Est; } 28 | const Matrix GetP() const { return P; } 29 | const Matrix GetErr() const { return Err; } 30 | 31 | protected: 32 | bool bCalculateSigmaPoint(void); 33 | bool bUnscentedTransform(Matrix& Out, Matrix& OutSigma, Matrix& P, Matrix& DSig, 34 | bool (*_vFuncNonLinear)(Matrix& xOut, const Matrix& xInp, const Matrix& U), 35 | const Matrix& InpSigma, const Matrix& InpVector, 36 | const Matrix& _CovNoise); 37 | 38 | private: 39 | bool (*bNonlinearUpdateX) (Matrix& X_dot, const Matrix& X, const Matrix& U); 40 | bool (*bNonlinearUpdateY) (Matrix& Y_Est, const Matrix& X, const Matrix& U); 41 | 42 | Matrix X_Est{SS_X_LEN, 1}; 43 | Matrix X_Sigma{SS_X_LEN, (2*SS_X_LEN + 1)}; 44 | 45 | Matrix Y_Est{SS_Z_LEN, 1}; 46 | Matrix Y_Sigma{SS_Z_LEN, (2*SS_X_LEN + 1)}; 47 | 48 | Matrix P{SS_X_LEN, SS_X_LEN}; 49 | Matrix P_Chol{SS_X_LEN, SS_X_LEN}; 50 | 51 | Matrix DX{SS_X_LEN, (2*SS_X_LEN + 1)}; 52 | Matrix DY{SS_Z_LEN, (2*SS_X_LEN + 1)}; 53 | 54 | Matrix Py{SS_Z_LEN, SS_Z_LEN}; 55 | Matrix Pxy{SS_X_LEN, SS_Z_LEN}; 56 | 57 | Matrix Wm{1, (2*SS_X_LEN + 1)}; 58 | Matrix Wc{1, (2*SS_X_LEN + 1)}; 59 | 60 | Matrix Rv{SS_X_LEN, SS_X_LEN}; 61 | Matrix Rn{SS_Z_LEN, SS_Z_LEN}; 62 | 63 | Matrix Err{SS_Z_LEN, 1}; 64 | Matrix Gain{SS_X_LEN, SS_Z_LEN}; 65 | float_prec Gamma; 66 | }; 67 | 68 | #endif // UKF_H 69 | -------------------------------------------------------------------------------- /ukf_example1_pendulum/ukf_pend_engl/ukf_pend_engl.ino: -------------------------------------------------------------------------------- 1 | /************************************************************************************************************* 2 | * 3 | * In this example, we will simulate the damped pendulum: 4 | * - The input u : none 5 | * - The state variable x : [theta dtheta/dt]' (angle and angular speed of the pendulum) 6 | * - The output y : [x y]' (coordinate position of the ball) 7 | * 8 | * We can model the pendulum system as (it is assume the rod is massless, without friction): 9 | * (dtheta)^2/dt^2 = -g/l*sin(theta) -alpha*(dtheta/dt) 10 | * 11 | * where g = acceleration constant due to gravity, l = pendulum length, alpha = damping factor 12 | * 13 | * where the coordinate position of the ball can be described by (0,0 coordinate is the pivot point): 14 | * x = sin(theta) * l 15 | * y = -cos(theta) * l 16 | * 17 | * *See https://en.wikipedia.org/wiki/Pendulum_(mathematics)#Simple_gravity_pendulum for undamped model, 18 | * *See http://www.nld.ds.mpg.de/applets/pendulum/eqm1.htm or 19 | * http://www.nld.ds.mpg.de/applets/pendulum/eqm2.htm for the damped model. 20 | * 21 | * 22 | * 23 | * 24 | * The model then can be described in state space formulation as: 25 | * The state variables: 26 | * x1 = theta --> dx1/dt = dtheta/dt 27 | * x2 = dtheta/dt --> dx2/dt = (dtheta)^2)/dt^2 28 | * The output variables: 29 | * y1 = x 30 | * y2 = y 31 | * 32 | * The update function in continuous time: 33 | * dx1/dt = x2 34 | * dx2/dt = -g/l * sin(x1) 35 | * The update function in discrete time: 36 | * x1(k+1) = x1(k) + x2(k)*dt 37 | * x2(k+1) = x2(k) - g/l*sin(x1(k))*dt - damping_factor*x2*dt 38 | * 39 | * The output (in discrete time): 40 | * y1(k) = sin(x1(k)) * l 41 | * y2(k) = -cos(x1(k)) * l 42 | * 43 | * 44 | * See https://github.com/pronenewbits for more! 45 | ************************************************************************************************************/ 46 | #include 47 | #include 48 | #include "konfig.h" 49 | #include "matrix.h" 50 | #include "ukf.h" 51 | 52 | 53 | /* =============================================== The pendulum model constants =============================================== */ 54 | #define pend_g (9.81) /* gravitation constant */ 55 | #define pend_l (5) /* length of the pendulum rod, in meters */ 56 | #define pend_alpha (0.3) /* damping factor */ 57 | 58 | 59 | 60 | /* ============================================ UKF variables/function declaration ============================================ */ 61 | /* Just example; in konfig.h: 62 | * SS_X_LEN = 2 63 | * SS_Z_LEN = 2 64 | * SS_U_LEN = 0 65 | */ 66 | /* UKF initialization constant -------------------------------------------------------------------------------------- */ 67 | #define P_INIT (100.) 68 | #define Rv_INIT (0.01) 69 | #define Rn_INIT (1.) 70 | /* P(k=0) variable -------------------------------------------------------------------------------------------------- */ 71 | float_prec UKF_PINIT_data[SS_X_LEN*SS_X_LEN] = {P_INIT, 0, 72 | 0, P_INIT}; 73 | Matrix UKF_PINIT(SS_X_LEN, SS_X_LEN, UKF_PINIT_data); 74 | /* Rv constant ------------------------------------------------------------------------------------------------------- */ 75 | float_prec UKF_RVINIT_data[SS_X_LEN*SS_X_LEN] = {Rv_INIT, 0, 76 | 0, Rv_INIT}; 77 | Matrix UKF_RvINIT(SS_X_LEN, SS_X_LEN, UKF_RVINIT_data); 78 | /* R constant ------------------------------------------------------------------------------------------------------- */ 79 | float_prec UKF_RNINIT_data[SS_Z_LEN*SS_Z_LEN] = {Rn_INIT, 0, 80 | 0, Rn_INIT}; 81 | Matrix UKF_RnINIT(SS_Z_LEN, SS_Z_LEN, UKF_RNINIT_data); 82 | /* Nonlinear & linearization function ------------------------------------------------------------------------------- */ 83 | bool Main_bUpdateNonlinearX(Matrix& X_Next, const Matrix& X, const Matrix& U); 84 | bool Main_bUpdateNonlinearY(Matrix& Y, const Matrix& X, const Matrix& U); 85 | /* UKF variables ---------------------------------------------------------------------------------------------------- */ 86 | Matrix X_true(SS_X_LEN, 1); 87 | Matrix X_est_init(SS_X_LEN, 1); 88 | Matrix Y(SS_Z_LEN, 1); 89 | Matrix U(SS_U_LEN, 1); 90 | /* UKF system declaration ------------------------------------------------------------------------------------------- */ 91 | UKF UKF_IMU(X_est_init, UKF_PINIT, UKF_RvINIT, UKF_RnINIT, Main_bUpdateNonlinearX, Main_bUpdateNonlinearY); 92 | 93 | 94 | 95 | /* ========================================= Auxiliary variables/function declaration ========================================= */ 96 | elapsedMillis timerLed, timerUKF; 97 | uint64_t u64compuTime; 98 | char bufferTxSer[100]; 99 | 100 | 101 | 102 | void setup() { 103 | /* serial to display data */ 104 | Serial.begin(115200); 105 | while(!Serial) {} 106 | 107 | X_true.vSetToZero(); 108 | X_est_init.vSetToZero(); 109 | 110 | /* For example, let's set the theta(k=0) = pi/2 (i.e. the pendulum rod is parallel with the horizontal plane) */ 111 | X_true[0][0] = 3.14159265359/2.; 112 | 113 | /* Observe that we set the wrong initial x_estimated value! (X_UKF(k=0) != X_TRUE(k=0)) */ 114 | X_est_init[0][0] = -3.14159265359/2.; 115 | 116 | UKF_IMU.vReset(X_est_init, UKF_PINIT, UKF_RvINIT, UKF_RnINIT); 117 | } 118 | 119 | 120 | void loop() { 121 | if (timerUKF >= SS_DT_MILIS) { 122 | timerUKF = 0; 123 | 124 | 125 | /* ================== Read the sensor data / simulate the system here ================== */ 126 | /* The update function in discrete time: 127 | * x1(k+1) = x1(k) + x2(k)*dt 128 | * x2(k+1) = x2(k) - g/l*sin(x1(k))*dt - damping_factor*x2*dt 129 | */ 130 | float_prec theta = X_true[0][0]; 131 | float_prec theta_dot = X_true[1][0]; 132 | X_true[0][0] = theta + (theta_dot*SS_DT); 133 | X_true[1][0] = theta_dot + (-pend_g/pend_l*sin(theta) - pend_alpha*theta_dot)*SS_DT; 134 | 135 | /* The output (in discrete time): 136 | * y1(k) = sin(x1(k)) 137 | * y2(k) = cos(x1(k)) 138 | */ 139 | theta = X_true[0][0]; 140 | 141 | Y[0][0] = sin(theta) * pend_l; 142 | Y[1][0] = -cos(theta) * pend_l; 143 | 144 | /* Let's add some noise! */ 145 | Y[0][0] += (float((rand() % 20) - 10) / 10.); /* add +/- 1 meter noise to x position */ 146 | /* ------------------ Read the sensor data / simulate the system here ------------------ */ 147 | 148 | 149 | /* ============================= Update the Kalman Filter ============================== */ 150 | u64compuTime = micros(); 151 | if (!UKF_IMU.bUpdate(Y, U)) { 152 | X_est_init.vSetToZero(); 153 | UKF_IMU.vReset(X_est_init, UKF_PINIT, UKF_RvINIT, UKF_RnINIT); 154 | Serial.println("Whoop "); 155 | } 156 | u64compuTime = (micros() - u64compuTime); 157 | /* ----------------------------- Update the Kalman Filter ------------------------------ */ 158 | 159 | 160 | /* =========================== Print to serial (for plotting) ========================== */ 161 | #if (0) 162 | /* Print: Computation time, x1 (without noise), x1 estimated */ 163 | snprintf(bufferTxSer, sizeof(bufferTxSer)-1, "%.3f %.3f %.3f ", ((float)u64compuTime)/1000., 164 | X_true[0][0], UKF_IMU.GetX()[0][0]); 165 | Serial.print(bufferTxSer); 166 | #else 167 | /* Print: Computation time, y1 (with noise), y1 (without noise), y1 estimated */ 168 | snprintf(bufferTxSer, sizeof(bufferTxSer)-1, "%.3f %.3f %.3f %.3f ", ((float)u64compuTime)/1000., 169 | Y[0][0], 170 | sin(X_true[0][0])*pend_l, 171 | sin(UKF_IMU.GetX()[0][0])*pend_l); 172 | Serial.print(bufferTxSer); 173 | #endif 174 | Serial.print('\n'); 175 | /* --------------------------- Print to serial (for plotting) -------------------------- */ 176 | } 177 | } 178 | 179 | 180 | bool Main_bUpdateNonlinearX(Matrix& X_Next, const Matrix& X, const Matrix& U) 181 | { 182 | /* Insert the nonlinear update transformation here 183 | * x(k+1) = f[x(k), u(k)] 184 | * 185 | * The update function in discrete time: 186 | * x1(k+1) = x1(k) + x2(k)*dt 187 | * x2(k+1) = x2(k) - g/l*sin(x1(k))*dt - alpha*x2*dt 188 | */ 189 | float_prec theta = X[0][0]; 190 | float_prec theta_dot = X[1][0]; 191 | 192 | if (theta > 3.14159265359) { 193 | theta = theta - 3.14159265359; 194 | } 195 | if (theta < -3.14159265359) { 196 | theta = theta + 3.14159265359; 197 | } 198 | 199 | X_Next[0][0] = theta + (theta_dot*SS_DT); 200 | X_Next[1][0] = theta_dot + (-pend_g/pend_l*sin(theta) - pend_alpha*theta_dot)*SS_DT; 201 | 202 | return true; 203 | } 204 | 205 | bool Main_bUpdateNonlinearY(Matrix& Y, const Matrix& X, const Matrix& U) 206 | { 207 | /* Insert the nonlinear measurement transformation here 208 | * y(k) = h[x(k), u(k)] 209 | * 210 | * The output (in discrete time): 211 | * y1(k) = sin(x1(k)) * l 212 | * y2(k) = -cos(x1(k)) * l 213 | */ 214 | float_prec theta = X[0][0]; 215 | 216 | Y[0][0] = sin(theta) * pend_l; 217 | Y[1][0] = -cos(theta) * pend_l; 218 | 219 | return true; 220 | } 221 | 222 | 223 | 224 | 225 | 226 | void SPEW_THE_ERROR(char const * str) 227 | { 228 | #if (SYSTEM_IMPLEMENTATION == SYSTEM_IMPLEMENTATION_PC) 229 | cout << (str) << endl; 230 | #elif (SYSTEM_IMPLEMENTATION == SYSTEM_IMPLEMENTATION_EMBEDDED_ARDUINO) 231 | Serial.println(str); 232 | #else 233 | /* Silent function */ 234 | #endif 235 | while(1); 236 | } 237 | -------------------------------------------------------------------------------- /ukf_example2_imu/2019-12-01_magneto_gabung_Kalib.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pronenewbits/Embedded_UKF_Library/3c16b4156442674cdbb9ac44bf24de5380521ff7/ukf_example2_imu/2019-12-01_magneto_gabung_Kalib.png -------------------------------------------------------------------------------- /ukf_example2_imu/2019-12-01_magneto_gabung_nonKalib.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pronenewbits/Embedded_UKF_Library/3c16b4156442674cdbb9ac44bf24de5380521ff7/ukf_example2_imu/2019-12-01_magneto_gabung_nonKalib.png -------------------------------------------------------------------------------- /ukf_example2_imu/MPU9250_Connection.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pronenewbits/Embedded_UKF_Library/3c16b4156442674cdbb9ac44bf24de5380521ff7/ukf_example2_imu/MPU9250_Connection.png -------------------------------------------------------------------------------- /ukf_example2_imu/Quaternion_IMU_Equation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pronenewbits/Embedded_UKF_Library/3c16b4156442674cdbb9ac44bf24de5380521ff7/ukf_example2_imu/Quaternion_IMU_Equation.png -------------------------------------------------------------------------------- /ukf_example2_imu/README.md: -------------------------------------------------------------------------------- 1 | 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! 2 | 3 | See [the subsystem section for explanation](https://github.com/pronenewbits/Arduino_AHRS_System#subsystem-1-the-sensor-fusion-algorithm). 4 | -------------------------------------------------------------------------------- /ukf_example2_imu/result.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pronenewbits/Embedded_UKF_Library/3c16b4156442674cdbb9ac44bf24de5380521ff7/ukf_example2_imu/result.png -------------------------------------------------------------------------------- /ukf_example2_imu/ukf_imu_double64_mute_rot2.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pronenewbits/Embedded_UKF_Library/3c16b4156442674cdbb9ac44bf24de5380521ff7/ukf_example2_imu/ukf_imu_double64_mute_rot2.mp4 -------------------------------------------------------------------------------- /ukf_example2_imu/ukf_imu_engl/konfig.h: -------------------------------------------------------------------------------- 1 | /************************************************************************************************************* 2 | * This file contains configuration parameters 3 | * 4 | * 5 | * See https://github.com/pronenewbits for more! 6 | ************************************************************************************************************/ 7 | #ifndef KONFIG_H 8 | #define KONFIG_H 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | 15 | 16 | /* State Space dimension */ 17 | #define SS_X_LEN (4) 18 | #define SS_Z_LEN (6) 19 | #define SS_U_LEN (3) 20 | #define SS_DT_MILIS (20) /* 20 ms */ 21 | #define SS_DT float_prec(SS_DT_MILIS/1000.) /* Sampling time */ 22 | 23 | 24 | /* Change this size based on the biggest matrix you will use */ 25 | #define MATRIX_MAXIMUM_SIZE (9) 26 | 27 | /* Define this to enable matrix bound checking */ 28 | #define MATRIX_USE_BOUNDS_CHECKING 29 | 30 | /* Set this define to choose math precision of the system */ 31 | #define PRECISION_SINGLE 1 32 | #define PRECISION_DOUBLE 2 33 | #define FPU_PRECISION (PRECISION_SINGLE) 34 | 35 | #if (FPU_PRECISION == PRECISION_SINGLE) 36 | #define float_prec float 37 | #define float_prec_ZERO (1e-7) 38 | #define float_prec_ZERO_ECO (1e-5) /* 'Economical' zero, for noisy calculation where 'somewhat zero' is good enough */ 39 | #elif (FPU_PRECISION == PRECISION_DOUBLE) 40 | #define float_prec double 41 | #define float_prec_ZERO (1e-13) 42 | #define float_prec_ZERO_ECO (1e-8) /* 'Economical' zero, for noisy calculation where 'somewhat zero' is good enough */ 43 | #else 44 | #error("FPU_PRECISION has not been defined!"); 45 | #endif 46 | 47 | 48 | 49 | /* Set this define to choose system implementation (mainly used to define how you print the matrix via the Matrix::vCetak() function) */ 50 | #define SYSTEM_IMPLEMENTATION_PC 1 51 | #define SYSTEM_IMPLEMENTATION_EMBEDDED_CUSTOM 2 52 | #define SYSTEM_IMPLEMENTATION_EMBEDDED_ARDUINO 3 53 | 54 | #define SYSTEM_IMPLEMENTATION (SYSTEM_IMPLEMENTATION_EMBEDDED_ARDUINO) 55 | 56 | 57 | 58 | /* ASSERT is evaluated locally (without function call) to lower the computation cost */ 59 | void SPEW_THE_ERROR(char const * str); 60 | #define ASSERT(truth, str) { if (!(truth)) SPEW_THE_ERROR(str); } 61 | 62 | 63 | #endif // KONFIG_H 64 | -------------------------------------------------------------------------------- /ukf_example2_imu/ukf_imu_engl/matrix.cpp: -------------------------------------------------------------------------------- 1 | /************************************************************************************ 2 | * Class Matrix 3 | * See matrix.h for description 4 | * 5 | * See https://github.com/pronenewbits for more! 6 | *************************************************************************************/ 7 | 8 | 9 | #include "matrix.h" 10 | 11 | /* There's nothing here yet! */ 12 | 13 | -------------------------------------------------------------------------------- /ukf_example2_imu/ukf_imu_engl/simple_mpu9250.h: -------------------------------------------------------------------------------- 1 | /* Modified from https://github.com/bolderflight/MPU9250/tree/master/src @2019-11-28 */ 2 | 3 | #ifndef SIMPLE_MPU9250_h 4 | #define SIMPLE_MPU9250_h 5 | 6 | #include "Arduino.h" 7 | #include "Wire.h" /* For I2C */ 8 | 9 | class SimpleMPU9250 10 | { 11 | public: 12 | enum GyroRange 13 | { 14 | GYRO_RANGE_250DPS, 15 | GYRO_RANGE_500DPS, 16 | GYRO_RANGE_1000DPS, 17 | GYRO_RANGE_2000DPS 18 | }; 19 | enum AccelRange 20 | { 21 | ACCEL_RANGE_2G, 22 | ACCEL_RANGE_4G, 23 | ACCEL_RANGE_8G, 24 | ACCEL_RANGE_16G 25 | }; 26 | enum DlpfBandwidth 27 | { 28 | DLPF_BANDWIDTH_184HZ, 29 | DLPF_BANDWIDTH_92HZ, 30 | DLPF_BANDWIDTH_41HZ, 31 | DLPF_BANDWIDTH_20HZ, 32 | DLPF_BANDWIDTH_10HZ, 33 | DLPF_BANDWIDTH_5HZ 34 | }; 35 | enum LpAccelOdr 36 | { 37 | LP_ACCEL_ODR_0_24HZ = 0, 38 | LP_ACCEL_ODR_0_49HZ = 1, 39 | LP_ACCEL_ODR_0_98HZ = 2, 40 | LP_ACCEL_ODR_1_95HZ = 3, 41 | LP_ACCEL_ODR_3_91HZ = 4, 42 | LP_ACCEL_ODR_7_81HZ = 5, 43 | LP_ACCEL_ODR_15_63HZ = 6, 44 | LP_ACCEL_ODR_31_25HZ = 7, 45 | LP_ACCEL_ODR_62_50HZ = 8, 46 | LP_ACCEL_ODR_125HZ = 9, 47 | LP_ACCEL_ODR_250HZ = 10, 48 | LP_ACCEL_ODR_500HZ = 11 49 | }; 50 | 51 | SimpleMPU9250(TwoWire &_i2c, const uint8_t _address) { 52 | this->_i2c = &_i2c; 53 | this->_address = _address; 54 | } 55 | 56 | int8_t begin(void) { 57 | _i2c->begin(); 58 | _i2c->setClock(400000); /* 400 kHz _i2c bus */ 59 | 60 | /* select clock source to gyro */ 61 | if (writeRegister(PWR_MGMNT_1,CLOCK_SEL_PLL) < 0) { 62 | return -1; 63 | } 64 | // enable _i2c master mode 65 | if(writeRegister(USER_CTRL,I2C_MST_EN) < 0) { 66 | return -2; 67 | } 68 | // set the _i2c bus speed to 400 kHz 69 | if(writeRegister(I2C_MST_CTRL,I2C_MST_CLK) < 0) { 70 | return -3; 71 | } 72 | // set AK8963 to Power Down 73 | writeAK8963Register(AK8963_CNTL1,AK8963_PWR_DOWN); 74 | // reset the MPU9250 75 | writeRegister(PWR_MGMNT_1,PWR_RESET); 76 | // wait for MPU-9250 to come back up 77 | delay(1); 78 | // reset the AK8963 79 | writeAK8963Register(AK8963_CNTL2,AK8963_RESET); 80 | // select clock source to gyro 81 | if(writeRegister(PWR_MGMNT_1,CLOCK_SEL_PLL) < 0) { 82 | return -4; 83 | } 84 | // check the WHO AM I byte, expected value is 0x71 (decimal 113) or 0x73 (decimal 115) 85 | if((whoAmI() != 113)&&(whoAmI() != 115)) { 86 | return -5; 87 | } 88 | // enable accelerometer and gyro 89 | if(writeRegister(PWR_MGMNT_2,SEN_ENABLE) < 0) { 90 | return -6; 91 | } 92 | // setting accel range to 16G as default 93 | if(writeRegister(ACCEL_CONFIG,ACCEL_FS_SEL_16G) < 0) { 94 | return -7; 95 | } 96 | _accelScale = G * 16.0f/32767.5f; // setting the accel scale to 16G 97 | _accelRange = ACCEL_RANGE_16G; 98 | // setting the gyro range to 2000DPS as default 99 | if(writeRegister(GYRO_CONFIG,GYRO_FS_SEL_2000DPS) < 0) { 100 | return -8; 101 | } 102 | _gyroScale = 2000.0f/32767.5f * _d2r; // setting the gyro scale to 2000DPS 103 | _gyroRange = GYRO_RANGE_2000DPS; 104 | // setting bandwidth to 184Hz as default 105 | if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_184) < 0) { 106 | return -9; 107 | } 108 | if(writeRegister(CONFIG,GYRO_DLPF_184) < 0) { // setting gyro bandwidth to 184Hz 109 | return -10; 110 | } 111 | _bandwidth = DLPF_BANDWIDTH_184HZ; 112 | // setting the sample rate divider to 0 as default 113 | if(writeRegister(SMPDIV,0x00) < 0) { 114 | return -11; 115 | } 116 | _srd = 0; 117 | // enable _i2c master mode 118 | if(writeRegister(USER_CTRL,I2C_MST_EN) < 0) { 119 | return -12; 120 | } 121 | // set the _i2c bus speed to 400 kHz 122 | if( writeRegister(I2C_MST_CTRL,I2C_MST_CLK) < 0) { 123 | return -13; 124 | } 125 | // check AK8963 WHO AM I register, expected value is 0x48 (decimal 72) 126 | if( whoAmIAK8963() != 72 ) { 127 | return -14; 128 | } 129 | /* get the magnetometer calibration */ 130 | // set AK8963 to Power Down 131 | if(writeAK8963Register(AK8963_CNTL1,AK8963_PWR_DOWN) < 0) { 132 | return -15; 133 | } 134 | delay(100); // long wait between AK8963 mode changes 135 | // set AK8963 to FUSE ROM access 136 | if(writeAK8963Register(AK8963_CNTL1,AK8963_FUSE_ROM) < 0) { 137 | return -16; 138 | } 139 | delay(100); // long wait between AK8963 mode changes 140 | // read the AK8963 ASA registers and compute magnetometer scale factors 141 | readAK8963Registers(AK8963_ASA,3,_buffer); 142 | _magScaleX = ((((float)_buffer[0]) - 128.0f)/(256.0f) + 1.0f) * 4912.0f / 32760.0f; // micro Tesla 143 | _magScaleY = ((((float)_buffer[1]) - 128.0f)/(256.0f) + 1.0f) * 4912.0f / 32760.0f; // micro Tesla 144 | _magScaleZ = ((((float)_buffer[2]) - 128.0f)/(256.0f) + 1.0f) * 4912.0f / 32760.0f; // micro Tesla 145 | // set AK8963 to Power Down 146 | if(writeAK8963Register(AK8963_CNTL1,AK8963_PWR_DOWN) < 0) { 147 | return -17; 148 | } 149 | delay(100); // long wait between AK8963 mode changes 150 | // set AK8963 to 16 bit resolution, 100 Hz update rate 151 | if(writeAK8963Register(AK8963_CNTL1,AK8963_CNT_MEAS2) < 0) { 152 | return -18; 153 | } 154 | delay(100); // long wait between AK8963 mode changes 155 | // select clock source to gyro 156 | if(writeRegister(PWR_MGMNT_1,CLOCK_SEL_PLL) < 0) { 157 | return -19; 158 | } 159 | // instruct the MPU9250 to get 7 bytes of data from the AK8963 at the sample rate 160 | readAK8963Registers(AK8963_HXL,7,_buffer); 161 | // estimate gyro bias 162 | if (calibrateGyro() < 0) { 163 | return -20; 164 | } 165 | 166 | 167 | 168 | 169 | setAccelRange(ACCEL_RANGE_2G); 170 | setGyroRange(GYRO_RANGE_2000DPS); 171 | setDlpfBandwidth(DLPF_BANDWIDTH_184HZ); 172 | setSrd(0); 173 | // successful init, return 1 174 | return 1; 175 | } 176 | 177 | int8_t readSensor(void) { 178 | // grab the data from the MPU9250 179 | if (readRegisters(ACCEL_OUT, 21, _buffer) < 0) { 180 | return -1; 181 | } 182 | 183 | _axcounts = ((((int16_t)_buffer[0]) << 8) | _buffer[1]); 184 | _aycounts = ((((int16_t)_buffer[2]) << 8) | _buffer[3]); 185 | _azcounts = ((((int16_t)_buffer[4]) << 8) | _buffer[5]); 186 | _gxcounts = ((((int16_t)_buffer[8]) << 8) | _buffer[9]); 187 | _gycounts = ((((int16_t)_buffer[10]) << 8) | _buffer[11]); 188 | _gzcounts = ((((int16_t)_buffer[12]) << 8) | _buffer[13]); 189 | _hxcounts = ((((int16_t)_buffer[15]) << 8) | _buffer[14]); 190 | _hycounts = ((((int16_t)_buffer[17]) << 8) | _buffer[16]); 191 | _hzcounts = ((((int16_t)_buffer[19]) << 8) | _buffer[18]); 192 | 193 | _ax = (float)(_axcounts) * _accelScale; 194 | _ay = (float)(_aycounts) * _accelScale; 195 | _az = (float)(_azcounts) * _accelScale; 196 | _gx = ((float)(_gxcounts) * _gyroScale) - _gxb; 197 | _gy = ((float)(_gycounts) * _gyroScale) - _gyb; 198 | _gz = ((float)(_gzcounts) * _gyroScale) - _gzb; 199 | 200 | /* Transform the magnetomer to align with the Accelerometer and Gyroscope Axis */ 201 | float _hxAK8963 = ((float)_hxcounts) * _magScaleX; 202 | float _hyAK8963 = ((float)_hycounts) * _magScaleY; 203 | float _hzAK8963 = ((float)_hzcounts) * _magScaleZ; 204 | _hx = _hyAK8963; 205 | _hy = _hxAK8963; 206 | _hz = -_hzAK8963; 207 | 208 | return 1; 209 | } 210 | 211 | /* estimates the gyro biases */ 212 | int calibrateGyro() { 213 | // set the range, bandwidth, and srd 214 | if (setGyroRange(GYRO_RANGE_250DPS) < 0) { 215 | return -1; 216 | } 217 | if (setDlpfBandwidth(DLPF_BANDWIDTH_20HZ) < 0) { 218 | return -2; 219 | } 220 | if (setSrd(19) < 0) { 221 | return -3; 222 | } 223 | 224 | // take samples and find bias 225 | _gxbD = 0; 226 | _gybD = 0; 227 | _gzbD = 0; 228 | for (size_t i=0; i < _numSamples; i++) { 229 | readSensor(); 230 | _gxbD += (getGyroX_rads() + _gxb)/((double)_numSamples); 231 | _gybD += (getGyroY_rads() + _gyb)/((double)_numSamples); 232 | _gzbD += (getGyroZ_rads() + _gzb)/((double)_numSamples); 233 | delay(20); 234 | } 235 | _gxb = (float)_gxbD; 236 | _gyb = (float)_gybD; 237 | _gzb = (float)_gzbD; 238 | 239 | // set the range, bandwidth, and srd back to what they were 240 | if (setGyroRange(_gyroRange) < 0) { 241 | return -4; 242 | } 243 | if (setDlpfBandwidth(_bandwidth) < 0) { 244 | return -5; 245 | } 246 | if (setSrd(_srd) < 0) { 247 | return -6; 248 | } 249 | return 1; 250 | } 251 | 252 | /* sets the accelerometer full scale range to values other than default */ 253 | int setAccelRange(AccelRange range) { 254 | switch(range) { 255 | case ACCEL_RANGE_2G: { 256 | // setting the accel range to 2G 257 | if(writeRegister(ACCEL_CONFIG,ACCEL_FS_SEL_2G) < 0) { 258 | return -1; 259 | } 260 | _accelScale = G * 2.0f/32767.5f; // setting the accel scale to 2G 261 | break; 262 | } 263 | case ACCEL_RANGE_4G: { 264 | // setting the accel range to 4G 265 | if(writeRegister(ACCEL_CONFIG,ACCEL_FS_SEL_4G) < 0) { 266 | return -1; 267 | } 268 | _accelScale = G * 4.0f/32767.5f; // setting the accel scale to 4G 269 | break; 270 | } 271 | case ACCEL_RANGE_8G: { 272 | // setting the accel range to 8G 273 | if(writeRegister(ACCEL_CONFIG,ACCEL_FS_SEL_8G) < 0) { 274 | return -1; 275 | } 276 | _accelScale = G * 8.0f/32767.5f; // setting the accel scale to 8G 277 | break; 278 | } 279 | case ACCEL_RANGE_16G: { 280 | // setting the accel range to 16G 281 | if(writeRegister(ACCEL_CONFIG,ACCEL_FS_SEL_16G) < 0) { 282 | return -1; 283 | } 284 | _accelScale = G * 16.0f/32767.5f; // setting the accel scale to 16G 285 | break; 286 | } 287 | } 288 | _accelRange = range; 289 | return 1; 290 | } 291 | 292 | /* sets the gyro full scale range to values other than default */ 293 | int setGyroRange(GyroRange range) { 294 | switch(range) { 295 | case GYRO_RANGE_250DPS: { 296 | // setting the gyro range to 250DPS 297 | if(writeRegister(GYRO_CONFIG,GYRO_FS_SEL_250DPS) < 0) { 298 | return -1; 299 | } 300 | _gyroScale = 250.0f/32767.5f * _d2r; // setting the gyro scale to 250DPS 301 | break; 302 | } 303 | case GYRO_RANGE_500DPS: { 304 | // setting the gyro range to 500DPS 305 | if(writeRegister(GYRO_CONFIG,GYRO_FS_SEL_500DPS) < 0) { 306 | return -1; 307 | } 308 | _gyroScale = 500.0f/32767.5f * _d2r; // setting the gyro scale to 500DPS 309 | break; 310 | } 311 | case GYRO_RANGE_1000DPS: { 312 | // setting the gyro range to 1000DPS 313 | if(writeRegister(GYRO_CONFIG,GYRO_FS_SEL_1000DPS) < 0) { 314 | return -1; 315 | } 316 | _gyroScale = 1000.0f/32767.5f * _d2r; // setting the gyro scale to 1000DPS 317 | break; 318 | } 319 | case GYRO_RANGE_2000DPS: { 320 | // setting the gyro range to 2000DPS 321 | if(writeRegister(GYRO_CONFIG,GYRO_FS_SEL_2000DPS) < 0) { 322 | return -1; 323 | } 324 | _gyroScale = 2000.0f/32767.5f * _d2r; // setting the gyro scale to 2000DPS 325 | break; 326 | } 327 | } 328 | _gyroRange = range; 329 | return 1; 330 | } 331 | 332 | /* sets the DLPF bandwidth to values other than default */ 333 | int setDlpfBandwidth(DlpfBandwidth bandwidth) { 334 | switch(bandwidth) { 335 | case DLPF_BANDWIDTH_184HZ: { 336 | if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_184) < 0) { // setting accel bandwidth to 184Hz 337 | return -1; 338 | } 339 | if(writeRegister(CONFIG,GYRO_DLPF_184) < 0) { // setting gyro bandwidth to 184Hz 340 | return -2; 341 | } 342 | break; 343 | } 344 | case DLPF_BANDWIDTH_92HZ: { 345 | if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_92) < 0) { // setting accel bandwidth to 92Hz 346 | return -1; 347 | } 348 | if(writeRegister(CONFIG,GYRO_DLPF_92) < 0) { // setting gyro bandwidth to 92Hz 349 | return -2; 350 | } 351 | break; 352 | } 353 | case DLPF_BANDWIDTH_41HZ: { 354 | if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_41) < 0) { // setting accel bandwidth to 41Hz 355 | return -1; 356 | } 357 | if(writeRegister(CONFIG,GYRO_DLPF_41) < 0) { // setting gyro bandwidth to 41Hz 358 | return -2; 359 | } 360 | break; 361 | } 362 | case DLPF_BANDWIDTH_20HZ: { 363 | if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_20) < 0) { // setting accel bandwidth to 20Hz 364 | return -1; 365 | } 366 | if(writeRegister(CONFIG,GYRO_DLPF_20) < 0) { // setting gyro bandwidth to 20Hz 367 | return -2; 368 | } 369 | break; 370 | } 371 | case DLPF_BANDWIDTH_10HZ: { 372 | if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_10) < 0) { // setting accel bandwidth to 10Hz 373 | return -1; 374 | } 375 | if(writeRegister(CONFIG,GYRO_DLPF_10) < 0) { // setting gyro bandwidth to 10Hz 376 | return -2; 377 | } 378 | break; 379 | } 380 | case DLPF_BANDWIDTH_5HZ: { 381 | if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_5) < 0) { // setting accel bandwidth to 5Hz 382 | return -1; 383 | } 384 | if(writeRegister(CONFIG,GYRO_DLPF_5) < 0) { // setting gyro bandwidth to 5Hz 385 | return -2; 386 | } 387 | break; 388 | } 389 | } 390 | _bandwidth = bandwidth; 391 | return 1; 392 | } 393 | 394 | /* sets the sample rate divider to values other than default */ 395 | int setSrd(uint8_t srd) { 396 | /* setting the sample rate divider to 19 to facilitate setting up magnetometer */ 397 | if(writeRegister(SMPDIV,19) < 0) { // setting the sample rate divider 398 | return -1; 399 | } 400 | if(srd > 9) { 401 | // set AK8963 to Power Down 402 | if(writeAK8963Register(AK8963_CNTL1,AK8963_PWR_DOWN) < 0) { 403 | return -2; 404 | } 405 | delay(100); // long wait between AK8963 mode changes 406 | // set AK8963 to 16 bit resolution, 8 Hz update rate 407 | if(writeAK8963Register(AK8963_CNTL1,AK8963_CNT_MEAS1) < 0) { 408 | return -3; 409 | } 410 | delay(100); // long wait between AK8963 mode changes 411 | // instruct the MPU9250 to get 7 bytes of data from the AK8963 at the sample rate 412 | readAK8963Registers(AK8963_HXL,7,_buffer); 413 | } else { 414 | // set AK8963 to Power Down 415 | if(writeAK8963Register(AK8963_CNTL1,AK8963_PWR_DOWN) < 0) { 416 | return -2; 417 | } 418 | delay(100); // long wait between AK8963 mode changes 419 | // set AK8963 to 16 bit resolution, 100 Hz update rate 420 | if(writeAK8963Register(AK8963_CNTL1,AK8963_CNT_MEAS2) < 0) { 421 | return -3; 422 | } 423 | delay(100); // long wait between AK8963 mode changes 424 | // instruct the MPU9250 to get 7 bytes of data from the AK8963 at the sample rate 425 | readAK8963Registers(AK8963_HXL,7,_buffer); 426 | } 427 | /* setting the sample rate divider */ 428 | if(writeRegister(SMPDIV,srd) < 0) { // setting the sample rate divider 429 | return -4; 430 | } 431 | _srd = srd; 432 | return 1; 433 | } 434 | 435 | /* returns the accelerometer measurement in the x direction, m/s/s */ 436 | float getAccelX_mss() { 437 | return _ax; 438 | } 439 | 440 | /* returns the accelerometer measurement in the y direction, m/s/s */ 441 | float getAccelY_mss() { 442 | return _ay; 443 | } 444 | 445 | /* returns the accelerometer measurement in the z direction, m/s/s */ 446 | float getAccelZ_mss() { 447 | return _az; 448 | } 449 | 450 | /* returns the gyroscope measurement in the x direction, rad/s */ 451 | float getGyroX_rads() { 452 | return _gx; 453 | } 454 | 455 | /* returns the gyroscope measurement in the y direction, rad/s */ 456 | float getGyroY_rads() { 457 | return _gy; 458 | } 459 | 460 | /* returns the gyroscope measurement in the z direction, rad/s */ 461 | float getGyroZ_rads() { 462 | return _gz; 463 | } 464 | 465 | /* returns the magnetometer measurement in the x direction, uT */ 466 | float getMagX_uT() { 467 | return _hx; 468 | } 469 | 470 | /* returns the magnetometer measurement in the y direction, uT */ 471 | float getMagY_uT() { 472 | return _hy; 473 | } 474 | 475 | /* returns the magnetometer measurement in the z direction, uT */ 476 | float getMagZ_uT() { 477 | return _hz; 478 | } 479 | 480 | 481 | private: 482 | uint8_t _address; 483 | TwoWire *_i2c; 484 | size_t _numBytes; // number of bytes received from I2C 485 | 486 | 487 | // track success of interacting with sensor 488 | int _status; 489 | // buffer for reading from sensor 490 | uint8_t _buffer[21]; 491 | // data counts 492 | int16_t _axcounts,_aycounts,_azcounts; 493 | int16_t _gxcounts,_gycounts,_gzcounts; 494 | int16_t _hxcounts,_hycounts,_hzcounts; 495 | int16_t _tcounts; 496 | // data buffer 497 | float _ax, _ay, _az; 498 | float _gx, _gy, _gz; 499 | float _hx, _hy, _hz; 500 | float _t; 501 | // wake on motion 502 | uint8_t _womThreshold; 503 | // scale factors 504 | float _accelScale; 505 | float _gyroScale; 506 | float _magScaleX, _magScaleY, _magScaleZ; 507 | const float _tempScale = 333.87f; 508 | const float _tempOffset = 21.0f; 509 | // configuration 510 | AccelRange _accelRange; 511 | GyroRange _gyroRange; 512 | DlpfBandwidth _bandwidth; 513 | uint8_t _srd; 514 | // gyro bias estimation 515 | size_t _numSamples = 100; 516 | double _gxbD, _gybD, _gzbD; 517 | float _gxb, _gyb, _gzb; 518 | // accel bias and scale factor estimation 519 | double _axbD, _aybD, _azbD; 520 | float _axmax, _aymax, _azmax; 521 | float _axmin, _aymin, _azmin; 522 | float _axb, _ayb, _azb; 523 | float _axs = 1.0f; 524 | float _ays = 1.0f; 525 | float _azs = 1.0f; 526 | // magnetometer bias and scale factor estimation 527 | uint16_t _maxCounts = 1000; 528 | float _deltaThresh = 0.3f; 529 | uint8_t _coeff = 8; 530 | uint16_t _counter; 531 | float _framedelta, _delta; 532 | float _hxfilt, _hyfilt, _hzfilt; 533 | float _hxmax, _hymax, _hzmax; 534 | float _hxmin, _hymin, _hzmin; 535 | float _hxb, _hyb, _hzb; 536 | float _hxs = 1.0f; 537 | float _hys = 1.0f; 538 | float _hzs = 1.0f; 539 | float _avgs; 540 | 541 | /* constants */ 542 | const float G = 9.807f; 543 | const float _d2r = 3.14159265359f/180.0f; 544 | /* MPU9250 registers */ 545 | const uint8_t ACCEL_OUT = 0x3B; 546 | const uint8_t GYRO_OUT = 0x43; 547 | const uint8_t TEMP_OUT = 0x41; 548 | const uint8_t EXT_SENS_DATA_00 = 0x49; 549 | const uint8_t ACCEL_CONFIG = 0x1C; 550 | const uint8_t ACCEL_FS_SEL_2G = 0x00; 551 | const uint8_t ACCEL_FS_SEL_4G = 0x08; 552 | const uint8_t ACCEL_FS_SEL_8G = 0x10; 553 | const uint8_t ACCEL_FS_SEL_16G = 0x18; 554 | const uint8_t GYRO_CONFIG = 0x1B; 555 | const uint8_t GYRO_FS_SEL_250DPS = 0x00; 556 | const uint8_t GYRO_FS_SEL_500DPS = 0x08; 557 | const uint8_t GYRO_FS_SEL_1000DPS = 0x10; 558 | const uint8_t GYRO_FS_SEL_2000DPS = 0x18; 559 | const uint8_t ACCEL_CONFIG2 = 0x1D; 560 | const uint8_t ACCEL_DLPF_184 = 0x01; 561 | const uint8_t ACCEL_DLPF_92 = 0x02; 562 | const uint8_t ACCEL_DLPF_41 = 0x03; 563 | const uint8_t ACCEL_DLPF_20 = 0x04; 564 | const uint8_t ACCEL_DLPF_10 = 0x05; 565 | const uint8_t ACCEL_DLPF_5 = 0x06; 566 | const uint8_t CONFIG = 0x1A; 567 | const uint8_t GYRO_DLPF_184 = 0x01; 568 | const uint8_t GYRO_DLPF_92 = 0x02; 569 | const uint8_t GYRO_DLPF_41 = 0x03; 570 | const uint8_t GYRO_DLPF_20 = 0x04; 571 | const uint8_t GYRO_DLPF_10 = 0x05; 572 | const uint8_t GYRO_DLPF_5 = 0x06; 573 | const uint8_t SMPDIV = 0x19; 574 | const uint8_t INT_PIN_CFG = 0x37; 575 | const uint8_t INT_ENABLE = 0x38; 576 | const uint8_t INT_DISABLE = 0x00; 577 | const uint8_t INT_PULSE_50US = 0x00; 578 | const uint8_t INT_WOM_EN = 0x40; 579 | const uint8_t INT_RAW_RDY_EN = 0x01; 580 | const uint8_t PWR_MGMNT_1 = 0x6B; 581 | const uint8_t PWR_CYCLE = 0x20; 582 | const uint8_t PWR_RESET = 0x80; 583 | const uint8_t CLOCK_SEL_PLL = 0x01; 584 | const uint8_t PWR_MGMNT_2 = 0x6C; 585 | const uint8_t SEN_ENABLE = 0x00; 586 | const uint8_t DIS_GYRO = 0x07; 587 | const uint8_t USER_CTRL = 0x6A; 588 | const uint8_t I2C_MST_EN = 0x20; 589 | const uint8_t I2C_MST_CLK = 0x0D; 590 | const uint8_t I2C_MST_CTRL = 0x24; 591 | const uint8_t I2C_SLV0_ADDR = 0x25; 592 | const uint8_t I2C_SLV0_REG = 0x26; 593 | const uint8_t I2C_SLV0_DO = 0x63; 594 | const uint8_t I2C_SLV0_CTRL = 0x27; 595 | const uint8_t I2C_SLV0_EN = 0x80; 596 | const uint8_t I2C_READ_FLAG = 0x80; 597 | const uint8_t MOT_DETECT_CTRL = 0x69; 598 | const uint8_t ACCEL_INTEL_EN = 0x80; 599 | const uint8_t ACCEL_INTEL_MODE = 0x40; 600 | const uint8_t LP_ACCEL_ODR = 0x1E; 601 | const uint8_t WOM_THR = 0x1F; 602 | const uint8_t WHO_AM_I = 0x75; 603 | const uint8_t FIFO_EN = 0x23; 604 | const uint8_t FIFO_TEMP = 0x80; 605 | const uint8_t FIFO_GYRO = 0x70; 606 | const uint8_t FIFO_ACCEL = 0x08; 607 | const uint8_t FIFO_MAG = 0x01; 608 | const uint8_t FIFO_COUNT = 0x72; 609 | const uint8_t FIFO_READ = 0x74; 610 | /* AK8963 registers */ 611 | const uint8_t AK8963_I2C_ADDR = 0x0C; 612 | const uint8_t AK8963_HXL = 0x03; 613 | const uint8_t AK8963_CNTL1 = 0x0A; 614 | const uint8_t AK8963_PWR_DOWN = 0x00; 615 | const uint8_t AK8963_CNT_MEAS1 = 0x12; 616 | const uint8_t AK8963_CNT_MEAS2 = 0x16; 617 | const uint8_t AK8963_FUSE_ROM = 0x0F; 618 | const uint8_t AK8963_CNTL2 = 0x0B; 619 | const uint8_t AK8963_RESET = 0x01; 620 | const uint8_t AK8963_ASA = 0x10; 621 | const uint8_t AK8963_WHO_AM_I = 0x00; 622 | 623 | 624 | /* writes a byte to MPU9250 register given a register address and data */ 625 | int writeRegister(uint8_t subAddress, uint8_t data) { 626 | _i2c->beginTransmission(_address); // open the device 627 | _i2c->write(subAddress); // write the register address 628 | _i2c->write(data); // write the data 629 | _i2c->endTransmission(); 630 | 631 | delay(10); 632 | 633 | /* read back the register */ 634 | readRegisters(subAddress, 1, _buffer); 635 | /* check the read back register against the written register */ 636 | if(_buffer[0] == data) { 637 | return 1; 638 | } 639 | else{ 640 | return -1; 641 | } 642 | } 643 | 644 | /* reads registers from MPU9250 given a starting register address, number of bytes, and a pointer to store data */ 645 | int readRegisters(uint8_t subAddress, uint8_t count, uint8_t* dest) { 646 | _i2c->beginTransmission(_address); // open the device 647 | _i2c->write(subAddress); // specify the starting register address 648 | _i2c->endTransmission(false); 649 | _numBytes = _i2c->requestFrom(_address, count); // specify the number of bytes to receive 650 | if (_numBytes == count) { 651 | for(uint8_t i = 0; i < count; i++) { 652 | dest[i] = _i2c->read(); 653 | } 654 | return 1; 655 | } else { 656 | return -1; 657 | } 658 | } 659 | 660 | /* writes a register to the AK8963 given a register address and data */ 661 | int writeAK8963Register(uint8_t subAddress, uint8_t data) { 662 | // set slave 0 to the AK8963 and set for write 663 | if (writeRegister(I2C_SLV0_ADDR,AK8963_I2C_ADDR) < 0) { 664 | return -1; 665 | } 666 | // set the register to the desired AK8963 sub address 667 | if (writeRegister(I2C_SLV0_REG,subAddress) < 0) { 668 | return -2; 669 | } 670 | // store the data for write 671 | if (writeRegister(I2C_SLV0_DO,data) < 0) { 672 | return -3; 673 | } 674 | // enable _i2c and send 1 byte 675 | if (writeRegister(I2C_SLV0_CTRL,I2C_SLV0_EN | (uint8_t)1) < 0) { 676 | return -4; 677 | } 678 | // read the register and confirm 679 | if (readAK8963Registers(subAddress,1,_buffer) < 0) { 680 | return -5; 681 | } 682 | if(_buffer[0] == data) { 683 | return 1; 684 | } else{ 685 | return -6; 686 | } 687 | } 688 | 689 | /* reads registers from the AK8963 */ 690 | int readAK8963Registers(uint8_t subAddress, uint8_t count, uint8_t* dest) { 691 | // set slave 0 to the AK8963 and set for read 692 | if (writeRegister(I2C_SLV0_ADDR,AK8963_I2C_ADDR | I2C_READ_FLAG) < 0) { 693 | return -1; 694 | } 695 | // set the register to the desired AK8963 sub address 696 | if (writeRegister(I2C_SLV0_REG,subAddress) < 0) { 697 | return -2; 698 | } 699 | // enable _i2c and request the bytes 700 | if (writeRegister(I2C_SLV0_CTRL,I2C_SLV0_EN | count) < 0) { 701 | return -3; 702 | } 703 | delay(1); // takes some time for these registers to fill 704 | // read the bytes off the MPU9250 EXT_SENS_DATA registers 705 | _status = readRegisters(EXT_SENS_DATA_00,count,dest); 706 | return _status; 707 | } 708 | 709 | /* gets the MPU9250 WHO_AM_I register value, expected to be 0x71 */ 710 | int whoAmI() { 711 | // read the WHO AM I register 712 | if (readRegisters(WHO_AM_I,1,_buffer) < 0) { 713 | return -1; 714 | } 715 | // return the register value 716 | return _buffer[0]; 717 | } 718 | 719 | /* gets the AK8963 WHO_AM_I register value, expected to be 0x48 */ 720 | int whoAmIAK8963() { 721 | // read the WHO AM I register 722 | if (readAK8963Registers(AK8963_WHO_AM_I,1,_buffer) < 0) { 723 | return -1; 724 | } 725 | // return the register value 726 | return _buffer[0]; 727 | } 728 | 729 | }; 730 | 731 | #endif 732 | -------------------------------------------------------------------------------- /ukf_example2_imu/ukf_imu_engl/ukf.cpp: -------------------------------------------------------------------------------- 1 | /*********************************************************************************************************************** 2 | * Class for Discrete Unscented Kalman Filter 3 | * Ref: Van der. Merwe, .. (2004). Sigma-Point Kalman Filters for Probabilistic Inference in Dynamic 4 | * State-Space Models (Ph.D. thesis). Oregon Health & Science University. 5 | * 6 | * The system to be estimated is defined as a discrete nonlinear dynamic dystem: 7 | * x(k+1) = f[x(k), u(k)] + v(k) ; x = Nx1, u = Mx1 8 | * y(k) = h[x(k), u(k)] + n(k) ; y = Zx1 9 | * 10 | * Where: 11 | * x(k) : State Variable at time-k : Nx1 12 | * y(k) : Measured output at time-k : Zx1 13 | * u(k) : System input at time-k : Mx1 14 | * v(k) : Process noise, AWGN assumed, w/ covariance Rv : Nx1 15 | * n(k) : Measurement noise, AWGN assumed, w/ covariance Rn : Nx1 16 | * 17 | * f(..), h(..) is a nonlinear transformation of the system to be estimated. 18 | * 19 | *********************************************************************************************************************** 20 | * Unscented Kalman Filter algorithm: 21 | * Initialization: 22 | * P(k=0|k=0) = Identity matrix * covariant(P(k=0)), typically initialized with some big number. 23 | * x(k=0|k=0) = Expected value of x at time-0 (i.e. x(k=0)), typically set to zero. 24 | * Rv, Rn = Covariance matrices of process & measurement. As this implementation 25 | * the noise as AWGN (and same value for every variable), this is set 26 | * to Rv=diag(RvInit,...,RvInit) and Rn=diag(RnInit,...,RnInit). 27 | * Wc, Wm = First order & second order weight, respectively. 28 | * alpha, beta, kappa, gamma = scalar constants. 29 | * 30 | * lambda = (alpha^2)*(N+kappa)-N, gamma = sqrt(N+alpha) ...{UKF_1} 31 | * Wm = [lambda/(N+lambda) 1/(2(N+lambda)) ... 1/(2(N+lambda))] ...{UKF_2} 32 | * Wc = [Wm(0)+(1-alpha(^2)+beta) 1/(2(N+lambda)) ... 1/(2(N+lambda))] ...{UKF_3} 33 | * 34 | * 35 | * UKF Calculation (every sampling time): 36 | * Calculate the Sigma Point: 37 | * Xs(k-1) = [x(k-1) ... x(k-1)] ; Xs(k-1) = NxN 38 | * GPsq = gamma * sqrt(P(k-1)) 39 | * XSigma(k-1) = [x(k-1) Xs(k-1)+GPsq Xs(k-1)-GPsq] ...{UKF_4} 40 | * 41 | * 42 | * Unscented Transform XSigma [f,XSigma,u,Rv] -> [x,XSigma,P,DX]: 43 | * XSigma(k) = f(XSigma(k-1), u(k-1)) ...{UKF_5a} 44 | * 45 | * x(k|k-1) = sum(Wm(i) * XSigma(k)(i)) ; i = 1 ... (2N+1) ...{UKF_6a} 46 | * 47 | * DX = XSigma(k)(i) - Xs(k) ; Xs(k) = [x(k|k-1) ... x(k|k-1)] 48 | * ; Xs(k) = Nx(2N+1) ...{UKF_7a} 49 | * 50 | * P(k|k-1) = sum(Wc(i)*DX*DX') + Rv ; i = 1 ... (2N+1) ...{UKF_8a} 51 | * 52 | * 53 | * Unscented Transform YSigma [h,XSigma,u,Rn] -> [y_est,YSigma,Py,DY]: 54 | * YSigma(k) = h(XSigma(k), u(k|k-1)) ; u(k|k-1) = u(k) ...{UKF_5b} 55 | * 56 | * y_est(k) = sum(Wm(i) * YSigma(k)(i)) ; i = 1 ... (2N+1) ...{UKF_6b} 57 | * 58 | * DY = YSigma(k)(i) - Ys(k) ; Ys(k) = [y_est(k) ... y_est(k)] 59 | * ; Ys(k) = Zx(2N+1) ...{UKF_7b} 60 | * 61 | * Py(k) = sum(Wc(i)*DY*DY') + Rn ; i = 1 ... (2N+1) ...{UKF_8b} 62 | * 63 | * 64 | * Calculate Cross-Covariance Matrix: 65 | * Pxy(k) = sum(Wc(i)*DX*DY(i)) ; i = 1 ... (2N+1) ...{UKF_9} 66 | * 67 | * 68 | * Calculate the Kalman Gain: 69 | * K = Pxy(k) * (Py(k)^-1) ...{UKF_10} 70 | * 71 | * 72 | * Update the Estimated State Variable: 73 | * x(k|k) = x(k|k-1) + K * (y(k) - y_est(k)) ...{UKF_11} 74 | * 75 | * 76 | * Update the Covariance Matrix: 77 | * P(k|k) = P(k|k-1) - K*Py(k)*K' ...{UKF_12} 78 | * 79 | * 80 | * *Additional Information: 81 | * - Dengan asumsi masukan plant ZOH, u(k) = u(k|k-1), 82 | * Dengan asumsi tambahan observer dijalankan sebelum pengendali, u(k|k-1) = u(k-1), 83 | * sehingga u(k) [untuk perhitungan kalman] adalah nilai u(k-1) [dari pengendali]. 84 | * - Notasi yang benar adalah u(k|k-1), tapi disini menggunakan notasi u(k) untuk 85 | * menyederhanakan penulisan rumus. 86 | * - Pada contoh di atas X~(k=0|k=0) = [0]. Untuk mempercepat konvergensi bisa digunakan 87 | * informasi plant-spesific. Misal pada implementasi Kalman Filter untuk sensor 88 | * IMU (Inertial measurement unit) dengan X = [quaternion], dengan asumsi IMU 89 | * awalnya menghadap ke atas tanpa rotasi, X~(k=0|k=0) = [1, 0, 0, 0]' 90 | * 91 | * See https://github.com/pronenewbits for more! 92 | **********************************************************************************************************************/ 93 | #include "ukf.h" 94 | 95 | 96 | UKF::UKF(const Matrix& XInit, const Matrix& PInit, const Matrix& Rv, const Matrix& Rn, 97 | bool (*bNonlinearUpdateX)(Matrix&, const Matrix&, const Matrix&), 98 | bool (*bNonlinearUpdateY)(Matrix&, const Matrix&, const Matrix&)) 99 | { 100 | /* Initialization: 101 | * P(k=0|k=0) = Identity matrix * covariant(P(k=0)), typically initialized with some big number. 102 | * x(k=0|k=0) = Expected value of x at time-0 (i.e. x(k=0)), typically set to zero. 103 | * Rv, Rn = Covariance matrices of process & measurement. As this implementation 104 | * the noise as AWGN (and same value for every variable), this is set 105 | * to Rv=diag(RvInit,...,RvInit) and Rn=diag(RnInit,...,RnInit). 106 | */ 107 | this->X_Est = XInit; 108 | this->P = PInit; 109 | this->Rv = Rv; 110 | this->Rn = Rn; 111 | this->bNonlinearUpdateX = bNonlinearUpdateX; 112 | this->bNonlinearUpdateY = bNonlinearUpdateY; 113 | 114 | 115 | /* Van der. Merwe, .. (2004). Sigma-Point Kalman Filters for Probabilistic Inference in Dynamic State-Space Models 116 | * (Ph.D. thesis). Oregon Health & Science University. Page 6: 117 | * 118 | * where λ = α2(L+κ)−L is a scaling parameter. α determines the spread of the sigma points around ̄x and is usually 119 | * set to a small positive value (e.g. 1e−2 ≤ α ≤ 1). κ is a secondary scaling parameter which is usually set to 120 | * either 0 or 3−L (see [45] for details), and β is an extra degree of freedom scalar parameter used to 121 | * incorporate any extra prior knowledge of the distribution of x (for Gaussian distributions, β = 2 is optimal). 122 | */ 123 | float_prec _alpha = 1e-2; 124 | float_prec _k = 0.0; 125 | float_prec _beta = 2.0; 126 | 127 | /* lambda = (alpha^2)*(N+kappa)-N, gamma = sqrt(N+alpha) ...{UKF_1} */ 128 | float_prec _lambda = (_alpha*_alpha)*(SS_X_LEN+_k) - SS_X_LEN; 129 | Gamma = sqrt((SS_X_LEN + _lambda)); 130 | 131 | 132 | /* Wm = [lambda/(N+lambda) 1/(2(N+lambda)) ... 1/(2(N+lambda))] ...{UKF_2} */ 133 | Wm[0][0] = _lambda/(SS_X_LEN + _lambda); 134 | for (int16_t _i = 1; _i < Wm.i16getCol(); _i++) { 135 | Wm[0][_i] = 0.5/(SS_X_LEN + _lambda); 136 | } 137 | 138 | /* Wc = [Wm(0)+(1-alpha(^2)+beta) 1/(2(N+lambda)) ... 1/(2(N+lambda))] ...{UKF_3} */ 139 | Wc = Wm; 140 | Wc[0][0] = Wc[0][0] + (1.0-(_alpha*_alpha)+_beta); 141 | } 142 | 143 | 144 | void UKF::vReset(const Matrix& XInit, const Matrix& PInit, const Matrix& Rv, const Matrix& Rn) 145 | { 146 | this->X_Est = XInit; 147 | this->P = PInit; 148 | this->Rv = Rv; 149 | this->Rn = Rn; 150 | } 151 | 152 | 153 | bool UKF::bUpdate(const Matrix& Y, const Matrix& U) 154 | { 155 | /* Run once every sampling time */ 156 | 157 | /* XSigma(k-1) = [x(k-1) Xs(k-1)+GPsq Xs(k-1)-GPsq] ...{UKF_4} */ 158 | if (!bCalculateSigmaPoint()) { 159 | return false; 160 | } 161 | 162 | 163 | /* Unscented Transform XSigma [f,XSigma,u,Rv] -> [x,XSigma,P,DX]: ...{UKF_5a} - {UKF_8a} */ 164 | if (!bUnscentedTransform(X_Est, X_Sigma, P, DX, bNonlinearUpdateX, X_Sigma, U, Rv)) { 165 | return false; 166 | } 167 | 168 | /* Unscented Transform YSigma [h,XSigma,u,Rn] -> [y_est,YSigma,Py,DY]: ...{UKF_5b} - {UKF_8b} */ 169 | if (!bUnscentedTransform(Y_Est, Y_Sigma, Py, DY, bNonlinearUpdateY, X_Sigma, U, Rn)) { 170 | return false; 171 | } 172 | 173 | 174 | /* Calculate Cross-Covariance Matrix: 175 | * Pxy(k) = sum(Wc(i)*DX*DY(i)) ; i = 1 ... (2N+1) ...{UKF_9} 176 | */ 177 | for (int16_t _i = 0; _i < DX.i16getRow(); _i++) { 178 | for (int16_t _j = 0; _j < DX.i16getCol(); _j++) { 179 | DX[_i][_j] *= Wc[0][_j]; 180 | } 181 | } 182 | Pxy = DX * (DY.Transpose()); 183 | 184 | 185 | /* Calculate the Kalman Gain: 186 | * K = Pxy(k) * (Py(k)^-1) ...{UKF_10} 187 | */ 188 | Matrix PyInv(Py.Invers()); 189 | if (!PyInv.bMatrixIsValid()) { 190 | return false; 191 | } 192 | Gain = Pxy * PyInv; 193 | 194 | 195 | /* Update the Estimated State Variable: 196 | * x(k|k) = x(k|k-1) + K * (y(k) - y_est(k)) ...{UKF_11} 197 | */ 198 | Err = Y - Y_Est; 199 | X_Est = X_Est + (Gain*Err); 200 | 201 | 202 | /* Update the Covariance Matrix: 203 | * P(k|k) = P(k|k-1) - K*Py(k)*K' ...{UKF_12} 204 | */ 205 | P = P - (Gain * Py * Gain.Transpose()); 206 | 207 | 208 | return true; 209 | } 210 | 211 | bool UKF::bCalculateSigmaPoint(void) 212 | { 213 | /* Xs(k-1) = [x(k-1) ... x(k-1)] ; Xs(k-1) = NxN 214 | * GPsq = gamma * sqrt(P(k-1)) 215 | * XSigma(k-1) = [x(k-1) Xs(k-1)+GPsq Xs(k-1)-GPsq] ...{UKF_4} 216 | */ 217 | /* Use Cholesky Decomposition to compute sqrt(P) */ 218 | P_Chol = P.CholeskyDec(); 219 | if (!P_Chol.bMatrixIsValid()) { 220 | /* System Fail */ 221 | return false; 222 | } 223 | P_Chol = P_Chol * Gamma; 224 | 225 | /* Xs(k-1) = [x(k-1) ... x(k-1)] ; Xs(k-1) = NxN */ 226 | Matrix _Y(SS_X_LEN, SS_X_LEN); 227 | for (int16_t _i = 0; _i < SS_X_LEN; _i++) { 228 | _Y = _Y.InsertVector(X_Est, _i); 229 | } 230 | 231 | X_Sigma.vSetToZero(); 232 | /* XSigma(k-1) = [x(k-1) 0 0] */ 233 | X_Sigma = X_Sigma.InsertVector(X_Est, 0); 234 | /* XSigma(k-1) = [x(k-1) Xs(k-1)+GPsq 0] */ 235 | X_Sigma = X_Sigma.InsertSubMatrix((_Y + P_Chol), 0, 1); 236 | /* XSigma(k-1) = [x(k-1) Xs(k-1)+GPsq Xs(k-1)-GPsq] */ 237 | X_Sigma = X_Sigma.InsertSubMatrix((_Y - P_Chol), 0, (1+SS_X_LEN)); 238 | 239 | return true; 240 | } 241 | 242 | bool UKF::bUnscentedTransform(Matrix& Out, Matrix& OutSigma, Matrix& P, Matrix& DSig, 243 | bool (*_vFuncNonLinear)(Matrix& xOut, const Matrix& xInp, const Matrix& U), 244 | const Matrix& InpSigma, const Matrix& InpVector, 245 | const Matrix& _CovNoise) 246 | { 247 | /* XSigma(k) = f(XSigma(k-1), u(k-1)) ...{UKF_5a} */ 248 | /* x(k|k-1) = sum(Wm(i) * XSigma(k)(i)) ; i = 1 ... (2N+1) ...{UKF_6a} */ 249 | Out.vSetToZero(); 250 | for (int16_t _j = 0; _j < InpSigma.i16getCol(); _j++) { 251 | /* Transform the column submatrix of sigma-points input matrix (InpSigma) */ 252 | Matrix _AuxSigma1(InpSigma.i16getRow(), 1); 253 | Matrix _AuxSigma2(OutSigma.i16getRow(), 1); 254 | for (int16_t _i = 0; _i < InpSigma.i16getRow(); _i++) { 255 | _AuxSigma1[_i][0] = InpSigma[_i][_j]; 256 | } 257 | if (!_vFuncNonLinear(_AuxSigma2, _AuxSigma1, InpVector)) { 258 | /* Somehow the transformation function is failed, propagate the error */ 259 | return false; 260 | } 261 | 262 | /* Combine the transformed vector to construct sigma-points output matrix (OutSigma) */ 263 | OutSigma = OutSigma.InsertVector(_AuxSigma2, _j); 264 | 265 | /* Calculate x(k|k-1) as weighted mean of OutSigma */ 266 | _AuxSigma2 = _AuxSigma2 * Wm[0][_j]; 267 | Out = Out + _AuxSigma2; 268 | } 269 | 270 | /* DX = XSigma(k)(i) - Xs(k) ; Xs(k) = [x(k|k-1) ... x(k|k-1)] 271 | * ; Xs(k) = Nx(2N+1) ...{UKF_7a} */ 272 | Matrix _AuxSigma1(OutSigma.i16getRow(), OutSigma.i16getCol()); 273 | for (int16_t _j = 0; _j < OutSigma.i16getCol(); _j++) { 274 | _AuxSigma1 = _AuxSigma1.InsertVector(Out, _j); 275 | } 276 | DSig = OutSigma - _AuxSigma1; 277 | 278 | /* P(k|k-1) = sum(Wc(i)*DX*DX') + Rv ; i = 1 ... (2N+1) ...{UKF_8a} */ 279 | _AuxSigma1 = DSig; 280 | for (int16_t _i = 0; _i < DSig.i16getRow(); _i++) { 281 | for (int16_t _j = 0; _j < DSig.i16getCol(); _j++) { 282 | _AuxSigma1[_i][_j] *= Wc[0][_j]; 283 | } 284 | } 285 | P = (_AuxSigma1 * (DSig.Transpose())) + _CovNoise; 286 | 287 | return true; 288 | } 289 | 290 | -------------------------------------------------------------------------------- /ukf_example2_imu/ukf_imu_engl/ukf.h: -------------------------------------------------------------------------------- 1 | /************************************************************************************************************* 2 | * Class for Discrete Unscented Kalman Filter 3 | * 4 | * 5 | * See https://github.com/pronenewbits for more! 6 | ************************************************************************************************************/ 7 | #ifndef UKF_H 8 | #define UKF_H 9 | 10 | #include "konfig.h" 11 | #include "matrix.h" 12 | 13 | #if ((2*SS_X_LEN + 1) > MATRIX_MAXIMUM_SIZE) 14 | #error("The MATRIX_MAXIMUM_SIZE is too small for sigma points (at least need (2*SS_X_LEN + 1))!"); 15 | #endif 16 | 17 | class UKF 18 | { 19 | public: 20 | UKF(const Matrix& XInit, const Matrix& PInit, const Matrix& Rv, const Matrix& Rn, 21 | bool (*bNonlinearUpdateX)(Matrix&, const Matrix&, const Matrix&), 22 | bool (*bNonlinearUpdateY)(Matrix&, const Matrix&, const Matrix&)); 23 | void vReset(const Matrix& XInit, const Matrix& PInit, const Matrix& Rv, const Matrix& Rn); 24 | bool bUpdate(const Matrix& Y, const Matrix& U); 25 | 26 | const Matrix GetX() const { return X_Est; } 27 | const Matrix GetY() const { return Y_Est; } 28 | const Matrix GetP() const { return P; } 29 | const Matrix GetErr() const { return Err; } 30 | 31 | protected: 32 | bool bCalculateSigmaPoint(void); 33 | bool bUnscentedTransform(Matrix& Out, Matrix& OutSigma, Matrix& P, Matrix& DSig, 34 | bool (*_vFuncNonLinear)(Matrix& xOut, const Matrix& xInp, const Matrix& U), 35 | const Matrix& InpSigma, const Matrix& InpVector, 36 | const Matrix& _CovNoise); 37 | 38 | private: 39 | bool (*bNonlinearUpdateX) (Matrix& X_dot, const Matrix& X, const Matrix& U); 40 | bool (*bNonlinearUpdateY) (Matrix& Y_Est, const Matrix& X, const Matrix& U); 41 | 42 | Matrix X_Est{SS_X_LEN, 1}; 43 | Matrix X_Sigma{SS_X_LEN, (2*SS_X_LEN + 1)}; 44 | 45 | Matrix Y_Est{SS_Z_LEN, 1}; 46 | Matrix Y_Sigma{SS_Z_LEN, (2*SS_X_LEN + 1)}; 47 | 48 | Matrix P{SS_X_LEN, SS_X_LEN}; 49 | Matrix P_Chol{SS_X_LEN, SS_X_LEN}; 50 | 51 | Matrix DX{SS_X_LEN, (2*SS_X_LEN + 1)}; 52 | Matrix DY{SS_Z_LEN, (2*SS_X_LEN + 1)}; 53 | 54 | Matrix Py{SS_Z_LEN, SS_Z_LEN}; 55 | Matrix Pxy{SS_X_LEN, SS_Z_LEN}; 56 | 57 | Matrix Wm{1, (2*SS_X_LEN + 1)}; 58 | Matrix Wc{1, (2*SS_X_LEN + 1)}; 59 | 60 | Matrix Rv{SS_X_LEN, SS_X_LEN}; 61 | Matrix Rn{SS_Z_LEN, SS_Z_LEN}; 62 | 63 | Matrix Err{SS_Z_LEN, 1}; 64 | Matrix Gain{SS_X_LEN, SS_Z_LEN}; 65 | float_prec Gamma; 66 | }; 67 | 68 | #endif // UKF_H 69 | -------------------------------------------------------------------------------- /ukf_example2_imu/ukf_imu_engl/ukf_imu_engl.ino: -------------------------------------------------------------------------------- 1 | /************************************************************************************************************* 2 | * 3 | * In this example, we'll process IMU data from MPU9250 sensor. 4 | * 5 | * See https://github.com/pronenewbits for more! 6 | ************************************************************************************************************/ 7 | #include 8 | #include 9 | #include "konfig.h" 10 | #include "matrix.h" 11 | #include "ukf.h" 12 | #include "simple_mpu9250.h" 13 | 14 | 15 | /* ================================================== The AHRS/IMU variables ================================================== */ 16 | /* Gravity vector constant (align with global Z-axis) */ 17 | #define IMU_ACC_Z0 (1) 18 | /* Magnetic vector constant (align with local magnetic vector) */ 19 | float_prec IMU_MAG_B0_data[3] = {cos(0), sin(0), 0.000000}; 20 | Matrix IMU_MAG_B0(3, 1, IMU_MAG_B0_data); 21 | /* The hard-magnet bias */ 22 | float_prec HARD_IRON_BIAS_data[3] = {8.832973, 7.243323, 23.95714}; 23 | Matrix HARD_IRON_BIAS(3, 1, HARD_IRON_BIAS_data); 24 | 25 | 26 | 27 | /* ============================================ UKF variables/function declaration ============================================ */ 28 | /* Just example; in konfig.h: 29 | * SS_X_LEN = 4 30 | * SS_Z_LEN = 6 31 | * SS_U_LEN = 3 32 | */ 33 | /* UKF initialization constant -------------------------------------------------------------------------------------- */ 34 | #define P_INIT (10.) 35 | #define Rv_INIT (1e-6) 36 | #define Rn_INIT_ACC (0.0015) 37 | #define Rn_INIT_MAG (0.0015) 38 | /* P(k=0) variable -------------------------------------------------------------------------------------------------- */ 39 | float_prec UKF_PINIT_data[SS_X_LEN*SS_X_LEN] = {P_INIT, 0, 0, 0, 40 | 0, P_INIT, 0, 0, 41 | 0, 0, P_INIT, 0, 42 | 0, 0, 0, P_INIT}; 43 | Matrix UKF_PINIT(SS_X_LEN, SS_X_LEN, UKF_PINIT_data); 44 | /* Q constant ------------------------------------------------------------------------------------------------------- */ 45 | float_prec UKF_RVINIT_data[SS_X_LEN*SS_X_LEN] = {Rv_INIT, 0, 0, 0, 46 | 0, Rv_INIT, 0, 0, 47 | 0, 0, Rv_INIT, 0, 48 | 0, 0, 0, Rv_INIT}; 49 | Matrix UKF_RvINIT(SS_X_LEN, SS_X_LEN, UKF_RVINIT_data); 50 | /* R constant ------------------------------------------------------------------------------------------------------- */ 51 | float_prec UKF_RNINIT_data[SS_Z_LEN*SS_Z_LEN] = {Rn_INIT_ACC, 0, 0, 0, 0, 0, 52 | 0, Rn_INIT_ACC, 0, 0, 0, 0, 53 | 0, 0, Rn_INIT_ACC, 0, 0, 0, 54 | 0, 0, 0, Rn_INIT_MAG, 0, 0, 55 | 0, 0, 0, 0, Rn_INIT_MAG, 0, 56 | 0, 0, 0, 0, 0, Rn_INIT_MAG}; 57 | Matrix UKF_RnINIT(SS_Z_LEN, SS_Z_LEN, UKF_RNINIT_data); 58 | /* Nonlinear & linearization function ------------------------------------------------------------------------------- */ 59 | bool Main_bUpdateNonlinearX(Matrix& X_Next, const Matrix& X, const Matrix& U); 60 | bool Main_bUpdateNonlinearY(Matrix& Y, const Matrix& X, const Matrix& U); 61 | /* UKF variables ---------------------------------------------------------------------------------------------------- */ 62 | Matrix quaternionData(SS_X_LEN, 1); 63 | Matrix Y(SS_Z_LEN, 1); 64 | Matrix U(SS_U_LEN, 1); 65 | /* UKF system declaration ------------------------------------------------------------------------------------------- */ 66 | UKF UKF_IMU(quaternionData, UKF_PINIT, UKF_RvINIT, UKF_RnINIT, Main_bUpdateNonlinearX, Main_bUpdateNonlinearY); 67 | 68 | 69 | 70 | /* ========================================= Auxiliary variables/function declaration ========================================= */ 71 | elapsedMillis timerLed, timerUKF; 72 | uint64_t u64compuTime; 73 | char bufferTxSer[100]; 74 | /* The command from the PC */ 75 | char cmd; 76 | /* A SimpleMPU9250 object with the MPU-9250 sensor on I2C bus 0 with address 0x68 */ 77 | SimpleMPU9250 IMU(Wire, 0x68); 78 | 79 | 80 | 81 | void setup() { 82 | /* Serial initialization -------------------------------------- */ 83 | Serial.begin(115200); 84 | while(!Serial) {} 85 | Serial.println("Calibrating IMU bias..."); 86 | 87 | /* IMU initialization ----------------------------------------- */ 88 | int status = IMU.begin(); /* start communication with IMU */ 89 | if (status < 0) { 90 | Serial.println("IMU initialization unsuccessful"); 91 | Serial.println("Check IMU wiring or try cycling power"); 92 | Serial.print("Status: "); 93 | Serial.println(status); 94 | while(1) {} 95 | } 96 | 97 | /* UKF initialization ----------------------------------------- */ 98 | /* x(k=0) = [1 0 0 0]' */ 99 | quaternionData.vSetToZero(); 100 | quaternionData[0][0] = 1.0; 101 | UKF_IMU.vReset(quaternionData, UKF_PINIT, UKF_RvINIT, UKF_RnINIT); 102 | 103 | snprintf(bufferTxSer, sizeof(bufferTxSer)-1, "UKF in Teensy 4.0 (%s)\r\n", 104 | (FPU_PRECISION == PRECISION_SINGLE)?"Float32":"Double64"); 105 | Serial.print(bufferTxSer); 106 | } 107 | 108 | 109 | void loop() { 110 | if (timerUKF >= SS_DT_MILIS) { 111 | timerUKF = 0; 112 | 113 | 114 | /* ================== Read the sensor data / simulate the system here ================== */ 115 | /* Read the raw data */ 116 | IMU.readSensor(); 117 | float Ax = IMU.getAccelX_mss(); 118 | float Ay = IMU.getAccelY_mss(); 119 | float Az = IMU.getAccelZ_mss(); 120 | float Bx = IMU.getMagX_uT(); 121 | float By = IMU.getMagY_uT(); 122 | float Bz = IMU.getMagZ_uT(); 123 | float p = IMU.getGyroX_rads(); 124 | float q = IMU.getGyroY_rads(); 125 | float r = IMU.getGyroZ_rads(); 126 | /* Input 1:3 = gyroscope */ 127 | U[0][0] = p; U[1][0] = q; U[2][0] = r; 128 | /* Output 1:3 = accelerometer */ 129 | Y[0][0] = Ax; Y[1][0] = Ay; Y[2][0] = Az; 130 | /* Output 4:6 = magnetometer */ 131 | Y[3][0] = Bx; Y[4][0] = By; Y[5][0] = Bz; 132 | 133 | /* Compensating Hard-Iron Bias for magnetometer */ 134 | Y[3][0] = Y[3][0]-HARD_IRON_BIAS[0][0]; 135 | Y[4][0] = Y[4][0]-HARD_IRON_BIAS[1][0]; 136 | Y[5][0] = Y[5][0]-HARD_IRON_BIAS[2][0]; 137 | 138 | /* Normalizing the output vector */ 139 | float_prec _normG = sqrt(Y[0][0] * Y[0][0]) + (Y[1][0] * Y[1][0]) + (Y[2][0] * Y[2][0]); 140 | Y[0][0] = Y[0][0] / _normG; 141 | Y[1][0] = Y[1][0] / _normG; 142 | Y[2][0] = Y[2][0] / _normG; 143 | float_prec _normM = sqrt(Y[3][0] * Y[3][0]) + (Y[4][0] * Y[4][0]) + (Y[5][0] * Y[5][0]); 144 | Y[3][0] = Y[3][0] / _normM; 145 | Y[4][0] = Y[4][0] / _normM; 146 | Y[5][0] = Y[5][0] / _normM; 147 | /* ------------------ Read the sensor data / simulate the system here ------------------ */ 148 | 149 | 150 | /* ============================= Update the Kalman Filter ============================== */ 151 | u64compuTime = micros(); 152 | if (!UKF_IMU.bUpdate(Y, U)) { 153 | quaternionData.vSetToZero(); 154 | quaternionData[0][0] = 1.0; 155 | UKF_IMU.vReset(quaternionData, UKF_PINIT, UKF_RvINIT, UKF_RnINIT); 156 | Serial.println("Whoop "); 157 | } 158 | u64compuTime = (micros() - u64compuTime); 159 | /* ----------------------------- Update the Kalman Filter ------------------------------ */ 160 | } 161 | 162 | 163 | /* The serial data is sent by responding to command from the PC running Processing scipt */ 164 | if (Serial.available()) { 165 | cmd = Serial.read(); 166 | if (cmd == 'v') { 167 | snprintf(bufferTxSer, sizeof(bufferTxSer)-1, "UKF in Teensy 4.0 (%s)\r\n", 168 | (FPU_PRECISION == PRECISION_SINGLE)?"Float32":"Double64"); 169 | Serial.print(bufferTxSer); 170 | Serial.print('\n'); 171 | } else if (cmd == 'q') { 172 | /* =========================== Print to serial (for plotting) ========================== */ 173 | quaternionData = UKF_IMU.GetX(); 174 | 175 | while (!Serial.available()); 176 | uint8_t count = Serial.read(); 177 | for (uint8_t _i = 0; _i < count; _i++) { 178 | serialFloatPrint(quaternionData[0][0]); 179 | Serial.print(","); 180 | serialFloatPrint(quaternionData[1][0]); 181 | Serial.print(","); 182 | serialFloatPrint(quaternionData[2][0]); 183 | Serial.print(","); 184 | serialFloatPrint(quaternionData[3][0]); 185 | Serial.print(","); 186 | serialFloatPrint((float)u64compuTime); 187 | Serial.print(","); 188 | Serial.println(""); 189 | } 190 | /* --------------------------- Print to serial (for plotting) -------------------------- */ 191 | } 192 | } 193 | } 194 | 195 | 196 | /* Function to interface with the Processing script in the PC */ 197 | void serialFloatPrint(float f) { 198 | byte * b = (byte *) &f; 199 | for (int i = 0; i < 4; i++) { 200 | byte b1 = (b[i] >> 4) & 0x0f; 201 | byte b2 = (b[i] & 0x0f); 202 | 203 | char c1 = (b1 < 10) ? ('0' + b1) : 'A' + b1 - 10; 204 | char c2 = (b2 < 10) ? ('0' + b2) : 'A' + b2 - 10; 205 | 206 | Serial.print(c1); 207 | Serial.print(c2); 208 | } 209 | } 210 | 211 | 212 | bool Main_bUpdateNonlinearX(Matrix& X_Next, const Matrix& X, const Matrix& U) 213 | { 214 | /* Insert the nonlinear update transformation here 215 | * x(k+1) = f[x(k), u(k)] 216 | * 217 | * The quaternion update function: 218 | * q0_dot = 1/2. * ( 0 - p*q1 - q*q2 - r*q3) 219 | * q1_dot = 1/2. * ( p*q0 + 0 + r*q2 - q*q3) 220 | * q2_dot = 1/2. * ( q*q0 - r*q1 + 0 + p*q3) 221 | * q3_dot = 1/2. * ( r*q0 + q*q1 - p*q2 + 0 ) 222 | * 223 | * Euler method for integration: 224 | * q0 = q0 + q0_dot * dT; 225 | * q1 = q1 + q1_dot * dT; 226 | * q2 = q2 + q2_dot * dT; 227 | * q3 = q3 + q3_dot * dT; 228 | */ 229 | float_prec q0, q1, q2, q3; 230 | float_prec p, q, r; 231 | 232 | q0 = X[0][0]; 233 | q1 = X[1][0]; 234 | q2 = X[2][0]; 235 | q3 = X[3][0]; 236 | 237 | p = U[0][0]; 238 | q = U[1][0]; 239 | r = U[2][0]; 240 | 241 | X_Next[0][0] = (0.5 * (+0.00 -p*q1 -q*q2 -r*q3))*SS_DT + q0; 242 | X_Next[1][0] = (0.5 * (+p*q0 +0.00 +r*q2 -q*q3))*SS_DT + q1; 243 | X_Next[2][0] = (0.5 * (+q*q0 -r*q1 +0.00 +p*q3))*SS_DT + q2; 244 | X_Next[3][0] = (0.5 * (+r*q0 +q*q1 -p*q2 +0.00))*SS_DT + q3; 245 | 246 | 247 | /* ======= Additional ad-hoc quaternion normalization to make sure the quaternion is a unit vector (i.e. ||q|| = 1) ======= */ 248 | if (!X_Next.bNormVector()) { 249 | /* System error, return false so we can reset the UKF */ 250 | return false; 251 | } 252 | 253 | return true; 254 | } 255 | 256 | bool Main_bUpdateNonlinearY(Matrix& Y, const Matrix& X, const Matrix& U) 257 | { 258 | /* Insert the nonlinear measurement transformation here 259 | * y(k) = h[x(k), u(k)] 260 | * 261 | * The measurement output is the gravitational and magnetic projection to the body: 262 | * DCM = [(+(q0**2)+(q1**2)-(q2**2)-(q3**2)), 2*(q1*q2+q0*q3), 2*(q1*q3-q0*q2)] 263 | * [ 2*(q1*q2-q0*q3), (+(q0**2)-(q1**2)+(q2**2)-(q3**2)), 2*(q2*q3+q0*q1)] 264 | * [ 2*(q1*q3+q0*q2), 2*(q2*q3-q0*q1), (+(q0**2)-(q1**2)-(q2**2)+(q3**2))] 265 | * 266 | * G_proj_sens = DCM * [0 0 1] --> Gravitational projection to the accelerometer sensor 267 | * M_proj_sens = DCM * [Mx My Mz] --> (Earth) magnetic projection to the magnetometer sensor 268 | */ 269 | float_prec q0, q1, q2, q3; 270 | float_prec q0_2, q1_2, q2_2, q3_2; 271 | 272 | q0 = X[0][0]; 273 | q1 = X[1][0]; 274 | q2 = X[2][0]; 275 | q3 = X[3][0]; 276 | 277 | q0_2 = q0 * q0; 278 | q1_2 = q1 * q1; 279 | q2_2 = q2 * q2; 280 | q3_2 = q3 * q3; 281 | 282 | Y[0][0] = (2*q1*q3 -2*q0*q2) * IMU_ACC_Z0; 283 | 284 | Y[1][0] = (2*q2*q3 +2*q0*q1) * IMU_ACC_Z0; 285 | 286 | Y[2][0] = (+(q0_2) -(q1_2) -(q2_2) +(q3_2)) * IMU_ACC_Z0; 287 | 288 | Y[3][0] = (+(q0_2)+(q1_2)-(q2_2)-(q3_2)) * IMU_MAG_B0[0][0] 289 | +(2*(q1*q2+q0*q3)) * IMU_MAG_B0[1][0] 290 | +(2*(q1*q3-q0*q2)) * IMU_MAG_B0[2][0]; 291 | 292 | Y[4][0] = (2*(q1*q2-q0*q3)) * IMU_MAG_B0[0][0] 293 | +(+(q0_2)-(q1_2)+(q2_2)-(q3_2)) * IMU_MAG_B0[1][0] 294 | +(2*(q2*q3+q0*q1)) * IMU_MAG_B0[2][0]; 295 | 296 | Y[5][0] = (2*(q1*q3+q0*q2)) * IMU_MAG_B0[0][0] 297 | +(2*(q2*q3-q0*q1)) * IMU_MAG_B0[1][0] 298 | +(+(q0_2)-(q1_2)-(q2_2)+(q3_2)) * IMU_MAG_B0[2][0]; 299 | 300 | return true; 301 | } 302 | 303 | 304 | 305 | 306 | 307 | void SPEW_THE_ERROR(char const * str) 308 | { 309 | #if (SYSTEM_IMPLEMENTATION == SYSTEM_IMPLEMENTATION_PC) 310 | cout << (str) << endl; 311 | #elif (SYSTEM_IMPLEMENTATION == SYSTEM_IMPLEMENTATION_EMBEDDED_ARDUINO) 312 | Serial.println(str); 313 | #else 314 | /* Silent function */ 315 | #endif 316 | while(1); 317 | } 318 | --------------------------------------------------------------------------------