├── Continuous_State_Space_Equation.png
├── Discrete_State_Space_Equation.png
├── FreeIMU_cube
├── FreeIMU_cube.pde
└── data
│ └── CourierNew36.vlw
├── Hard_Iron_Bias_Identification_by_Least_Squares.png
├── Hard_Iron_Bias_Identification_by_Recursive_Least_Squares.png
├── Hard_Iron_Compensation_Using_RLS.png
├── LICENSE
├── Quaternion_Kinematics_Equations.png
├── README.md
├── Sensor_Fusion_RLS_StateMachine.png
├── Teensy_MPU9250_Connection.png
├── ahrs_ekf
├── ahrs_ekf.ino
├── ekf.cpp
├── ekf.h
├── konfig.h
├── matrix.cpp
├── matrix.h
└── simple_mpu9250.h
├── ahrs_ukf
├── ahrs_ukf.ino
├── konfig.h
├── matrix.cpp
├── matrix.h
├── simple_mpu9250.h
├── ukf.cpp
└── ukf.h
├── ekf_imu_float32_mute_rot2.gif
├── eq_render
├── bias_sensor.gif
├── deformity_sensor.gif
├── eq_latex
├── measured_sensor.gif
├── sensor_calib_eq.gif
└── true_sensor.gif
└── ukf_imu_double64_mute_rot2.gif
/Continuous_State_Space_Equation.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/pronenewbits/Arduino_AHRS_System/aa469bd09ff2d7986ac7f3755f1d17c288f5b3e8/Continuous_State_Space_Equation.png
--------------------------------------------------------------------------------
/Discrete_State_Space_Equation.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/pronenewbits/Arduino_AHRS_System/aa469bd09ff2d7986ac7f3755f1d17c288f5b3e8/Discrete_State_Space_Equation.png
--------------------------------------------------------------------------------
/FreeIMU_cube/FreeIMU_cube.pde:
--------------------------------------------------------------------------------
1 | /**
2 | Visualize a cube which will assumes the orientation described
3 | in a quaternion coming from the serial port.
4 |
5 | INSTRUCTIONS:
6 | This program has to be run when you have the FreeIMU_serial
7 | program running on your Arduino and the Arduino connected to your PC.
8 | Remember to set the serialPort variable below to point to the name the
9 | Arduino serial port has in your system. You can get the port using the
10 | Arduino IDE from Tools->Serial Port: the selected entry is what you have
11 | to use as serialPort variable.
12 |
13 |
14 | Copyright (C) 2011-2012 Fabio Varesano - http://www.varesano.net/
15 |
16 | This program is free software: you can redistribute it and/or modify
17 | it under the terms of the version 3 GNU General Public License as
18 | published by the Free Software Foundation.
19 |
20 | This program is distributed in the hope that it will be useful,
21 | but WITHOUT ANY WARRANTY; without even the implied warranty of
22 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
23 | GNU General Public License for more details.
24 |
25 | You should have received a copy of the GNU General Public License
26 | along with this program. If not, see .
27 |
28 | */
29 |
30 | import processing.serial.*;
31 | import processing.opengl.*;
32 |
33 | Serial myPort; // Create object from Serial class
34 |
35 | final String serialPort = "/dev/ttyACM0"; // replace this with your serial port. On windows you will need something like "COM1".
36 |
37 | float [] q = new float [4];
38 | float lama = 0.0;
39 | float [] hq = null;
40 | float [] Euler = new float [3]; // psi, theta, phi
41 |
42 | int lf = 10; // 10 is '\n' in ASCII
43 | byte[] inBuffer = new byte[22]; // this is the number of chars on each line from the Arduino (including /r/n)
44 |
45 | PFont font;
46 | final int VIEW_SIZE_X = 800, VIEW_SIZE_Y = 600;
47 |
48 | //final int burst = 1;
49 | final int burst = 32;
50 | int count = 0;
51 |
52 | void myDelay(int time) {
53 | try {
54 | Thread.sleep(time);
55 | } catch (InterruptedException e) { }
56 | }
57 | String versi = "";
58 | void setup()
59 | {
60 | //size(VIEW_SIZE_X, VIEW_SIZE_Y, OPENGL);
61 | size(800, 600, OPENGL); //<>//
62 | myPort = new Serial(this, serialPort, 115200);
63 |
64 | // The font must be located in the sketch's "data" directory to load successfully
65 | font = loadFont("CourierNew36.vlw");
66 |
67 | println("Waiting IMU..");
68 |
69 | myPort.clear();
70 |
71 | myDelay(1000);
72 | //while (myPort.available() == 0) {
73 | myPort.write("v");
74 | myDelay(1000);
75 | //}
76 | versi = myPort.readStringUntil('\n');
77 | println(versi);
78 | myPort.write("q" + char(burst));
79 | myPort.bufferUntil('\n');
80 | }
81 |
82 |
83 | float decodeFloat(String inString) {
84 | byte [] inData = new byte[4];
85 |
86 | if(inString.length() == 8) {
87 | inData[0] = (byte) unhex(inString.substring(0, 2));
88 | inData[1] = (byte) unhex(inString.substring(2, 4));
89 | inData[2] = (byte) unhex(inString.substring(4, 6));
90 | inData[3] = (byte) unhex(inString.substring(6, 8));
91 | }
92 |
93 | int intbits = (inData[3] << 24) | ((inData[2] & 0xff) << 16) | ((inData[1] & 0xff) << 8) | (inData[0] & 0xff);
94 | return Float.intBitsToFloat(intbits);
95 | }
96 |
97 |
98 | void serialEvent(Serial p) {
99 | if(p.available() >= 18) {
100 | String inputString = p.readStringUntil('\n');
101 | //print(inputString);
102 | if (inputString != null && inputString.length() > 0) {
103 | if (inputString.indexOf(',') != -1) {
104 | String [] inputStringArr = split(inputString, ",");
105 | if(inputStringArr.length >= 6) { // q1,q2,q3,q4,\r\n so we have 5 elements
106 | q[0] = decodeFloat(inputStringArr[0]);
107 | q[1] = decodeFloat(inputStringArr[1]);
108 | q[2] = decodeFloat(inputStringArr[2]);
109 | q[3] = decodeFloat(inputStringArr[3]);
110 | lama = decodeFloat(inputStringArr[4]);
111 | }
112 | } else {
113 | versi = inputString;
114 | }
115 | }
116 | count = count + 1;
117 | if(burst == count) { // ask more data when burst completed
118 | p.write("q" + char(burst));
119 | count = 0;
120 | }
121 | }
122 | }
123 |
124 |
125 |
126 | void buildBoxShape() {
127 | //box(60, 10, 40);
128 | noStroke();
129 | beginShape(QUADS);
130 |
131 | //Z+ (to the drawing area)
132 | fill(#00ff00);
133 | vertex(-30, -5, 20);
134 | vertex(30, -5, 20);
135 | vertex(30, 5, 20);
136 | vertex(-30, 5, 20);
137 |
138 | //Z-
139 | fill(#0000ff);
140 | vertex(-30, -5, -20);
141 | vertex(30, -5, -20);
142 | vertex(30, 5, -20);
143 | vertex(-30, 5, -20);
144 |
145 | //X-
146 | fill(#ff0000);
147 | vertex(-30, -5, -20);
148 | vertex(-30, -5, 20);
149 | vertex(-30, 5, 20);
150 | vertex(-30, 5, -20);
151 |
152 | //X+
153 | fill(#ffff00);
154 | vertex(30, -5, -20);
155 | vertex(30, -5, 20);
156 | vertex(30, 5, 20);
157 | vertex(30, 5, -20);
158 |
159 | //Y-
160 | fill(#ff00ff);
161 | vertex(-30, -5, -20);
162 | vertex(30, -5, -20);
163 | vertex(30, -5, 20);
164 | vertex(-30, -5, 20);
165 |
166 | //Y+
167 | fill(#00ffff);
168 | vertex(-30, 5, -20);
169 | vertex(30, 5, -20);
170 | vertex(30, 5, 20);
171 | vertex(-30, 5, 20);
172 |
173 | endShape();
174 | }
175 |
176 |
177 | void drawCube() {
178 | pushMatrix();
179 | translate(VIEW_SIZE_X/2, VIEW_SIZE_Y/2 + 50, 0);
180 | scale(5,5,5);
181 |
182 | // a demonstration of the following is at
183 | // http://www.varesano.net/blog/fabio/ahrs-sensor-fusion-orientation-filter-3d-graphical-rotating-cube
184 | rotateZ(-Euler[2]);
185 | rotateX(-Euler[1]);
186 | rotateY(-Euler[0]);
187 |
188 | buildBoxShape();
189 |
190 | popMatrix();
191 | }
192 |
193 |
194 | void draw() {
195 | background(#000000);
196 | fill(#ffffff);
197 |
198 | if(hq != null) { // use home quaternion
199 | quaternionToEuler(quatProd(hq, q), Euler);
200 | text("Disable home position by pressing \"n\"", 20, VIEW_SIZE_Y - 50);
201 | }
202 | else {
203 | quaternionToEuler(q, Euler);
204 | text("Point FreeIMU's X axis to your monitor then press \"h\"", 20, VIEW_SIZE_Y - 50);
205 | }
206 |
207 | textFont(font, 20);
208 | textAlign(LEFT, TOP);
209 | text("Q:\n" + q[0] + "\n" + q[1] + "\n" + q[2] + "\n" + q[3] + "\n" + "Lama Kalkulasi: " + lama + " us" + "\n" + versi, 20, 20);
210 | text("Euler Angles:\nYaw (psi) : " + degrees(Euler[0]) + "\nPitch (theta): " + degrees(Euler[1]) + "\nRoll (phi) : " + degrees(Euler[2]), 200, 20);
211 |
212 | drawCube();
213 | //myPort.write("q" + 1);
214 | }
215 |
216 |
217 | void keyPressed() {
218 | if(key == 'h') {
219 | println("pressed h");
220 |
221 | // set hq the home quaternion as the quatnion conjugate coming from the sensor fusion
222 | hq = quatConjugate(q);
223 |
224 | }
225 | else if(key == 'v') {
226 | println("pressed v");
227 | myPort.write("v");
228 | }
229 | else if(key == 'n') {
230 | println("pressed n");
231 | hq = null;
232 | }
233 | }
234 |
235 | // See Sebastian O.H. Madwick report
236 | // "An efficient orientation filter for inertial and intertial/magnetic sensor arrays" Chapter 2 Quaternion representation
237 |
238 | void quaternionToEuler(float [] q, float [] euler) {
239 | euler[0] = atan2(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0]*q[0] + 2 * q[1] * q[1] - 1); // psi
240 | euler[1] = -asin(2 * q[1] * q[3] + 2 * q[0] * q[2]); // theta
241 | euler[2] = atan2(2 * q[2] * q[3] - 2 * q[0] * q[1], 2 * q[0] * q[0] + 2 * q[3] * q[3] - 1); // phi
242 | }
243 |
244 | float [] quatProd(float [] a, float [] b) {
245 | float [] q = new float[4];
246 |
247 | q[0] = a[0] * b[0] - a[1] * b[1] - a[2] * b[2] - a[3] * b[3];
248 | q[1] = a[0] * b[1] + a[1] * b[0] + a[2] * b[3] - a[3] * b[2];
249 | q[2] = a[0] * b[2] - a[1] * b[3] + a[2] * b[0] + a[3] * b[1];
250 | q[3] = a[0] * b[3] + a[1] * b[2] - a[2] * b[1] + a[3] * b[0];
251 |
252 | return q;
253 | }
254 |
255 | // returns a quaternion from an axis angle representation
256 | float [] quatAxisAngle(float [] axis, float angle) {
257 | float [] q = new float[4];
258 |
259 | float halfAngle = angle / 2.0;
260 | float sinHalfAngle = sin(halfAngle);
261 | q[0] = cos(halfAngle);
262 | q[1] = -axis[0] * sinHalfAngle;
263 | q[2] = -axis[1] * sinHalfAngle;
264 | q[3] = -axis[2] * sinHalfAngle;
265 |
266 | return q;
267 | }
268 |
269 | // return the quaternion conjugate of quat
270 | float [] quatConjugate(float [] quat) {
271 | float [] conj = new float[4];
272 |
273 | conj[0] = quat[0];
274 | conj[1] = -quat[1];
275 | conj[2] = -quat[2];
276 | conj[3] = -quat[3];
277 |
278 | return conj;
279 | }
280 |
--------------------------------------------------------------------------------
/FreeIMU_cube/data/CourierNew36.vlw:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/pronenewbits/Arduino_AHRS_System/aa469bd09ff2d7986ac7f3755f1d17c288f5b3e8/FreeIMU_cube/data/CourierNew36.vlw
--------------------------------------------------------------------------------
/Hard_Iron_Bias_Identification_by_Least_Squares.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/pronenewbits/Arduino_AHRS_System/aa469bd09ff2d7986ac7f3755f1d17c288f5b3e8/Hard_Iron_Bias_Identification_by_Least_Squares.png
--------------------------------------------------------------------------------
/Hard_Iron_Bias_Identification_by_Recursive_Least_Squares.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/pronenewbits/Arduino_AHRS_System/aa469bd09ff2d7986ac7f3755f1d17c288f5b3e8/Hard_Iron_Bias_Identification_by_Recursive_Least_Squares.png
--------------------------------------------------------------------------------
/Hard_Iron_Compensation_Using_RLS.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/pronenewbits/Arduino_AHRS_System/aa469bd09ff2d7986ac7f3755f1d17c288f5b3e8/Hard_Iron_Compensation_Using_RLS.png
--------------------------------------------------------------------------------
/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 |
--------------------------------------------------------------------------------
/Quaternion_Kinematics_Equations.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/pronenewbits/Arduino_AHRS_System/aa469bd09ff2d7986ac7f3755f1d17c288f5b3e8/Quaternion_Kinematics_Equations.png
--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | # Arduino_AHRS_System
2 | This is a compact realtime embedded Inertial Measurement System (IMU) based Attitude and Heading Reference System (AHRS) using Recursive Least Squares (RLS) for magnetometer calibration, and EKF/UKF for sensor fusion for Arduino platform.
3 |
4 | - It's not using Eigen (small source code - more simple to understand).
5 | - It's not using C++ Standard Library/std (for embedded consideration).
6 | - 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`, and `math.h`).
7 | - There's no malloc/new/free dynamic memory allocation (for real time application). But it use heavy stack local variables, so you need to run it through memory analyzer if you are really concerned about implement this in mission critical hard real time application.
8 |
9 | This code is the application of Extended Kalman Filter and Unscented Kalman Filter library I've made.
10 |
11 | - The EKF library and documentation can be found in [this repository](https://github.com/pronenewbits/Embedded_EKF_Library).
12 | - The UKF library and documentation can be found in [this repository](https://github.com/pronenewbits/Embedded_UKF_Library).
13 |
14 | Please read them to gain insight on how I implement the filters.
15 |
16 |
17 | # The Background
18 |
19 | This repository is made to explain the basic system of AHRS system based on an IMU sensor. For ease of use and simple implementation, I use low cost MPU-9250 sensor as the IMU sensor and Teensy 4.0 (an Arduino compatible platform) as the computing module. You can get the Teensy from [here](https://www.pjrc.com/store/teensy40.html), while you can get the MPU-9250 from, well, everywhere (just make sure the MPU-9250 module you get is 3.3V version).
20 |
21 | Before we delve deeper, some results for motivational purposes:
22 |
23 | 1. EKF result using single precision floating math:
24 | 
25 | (See mp4 files for every result using EKF/UKF filter with single and double precision floating math).
26 |
27 | 2. UKF result using double precision floating math:
28 | 
29 | (See mp4 files for every result using EKF/UKF filter with single and double precision floating math).
30 |
31 | 3. Online hard-iron bias identification using RLS for magnetometer data compensation:
32 | 
33 |
34 |
35 |
36 | # The Theoretical Description
37 |
38 | The AHRS system implemented here consists of 2 major subsystems:
39 |
40 | ### Subsystem 1: The Sensor Fusion Algorithm
41 |
42 | Sensor fusion algorithm works by combining Gyroscope sensor (good for short measurement because of low noise, but not good for long measurement because of drifting), Accelerometer sensor (good for long measurement, but noisy and can only sense one direction, namely [earth's gravitational vector](http://weelookang.blogspot.com/2015/01/ejss-gravity-field-visualisation-model.html0)) and Magnetometer sensor (good for long measurement, but noisy and can only sense one direction, namely [earth's magnetic vector](https://en.wikipedia.org/wiki/Earth%27s_magnetic_field#Characteristics)).
43 |
44 | To avoid [gimbal lock](https://en.wikipedia.org/wiki/Gimbal_lock), we use [quaternion](https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation) to represent where the system is heading (i.e. [the roll, pitch, and yaw of the sensor body](https://en.wikipedia.org/wiki/Flight_dynamics_(fixed-wing_aircraft))). The quaternion kinematics equations can be described as:
45 | 
46 |
47 | We then can re-describe the kinematics equations into a (continuous) nonlinear state space equation:
48 | 
49 |
50 | For sensor fusion algorithm, I use (discrete) Extended Kalman Filter and (discrete) Unscented Kalman Filter library I've made in [this repository](https://github.com/pronenewbits/Embedded_EKF_Library) (for EKF) and [this repository](https://github.com/pronenewbits/Embedded_UKF_Library) (for UKF). Because of that, we need to transform the equations above into the discrete form that can be used by EKF & UKF. Assuming ZOH-based sensor sampling, the discrete system (and the Jacobian for EKF) can be derived using Euler method as:
51 | 
52 |
53 | **Remark**: This is the simplest state space system for quaternion based AHRS using MEMS IMU sensor. Many publication use additional state to compensate gyroscope bias and accelerometer bias (some also set the magnetometer bias as another estimated state, not as parameters in the calibration phase), while others expand further by adding state for 3D speed or position (with additional GPS and pressure sensor, or even camera based machine vision) to make a complete guidance system. I hope by using this framework as a base, you can easily explore many different model.
54 |
55 |
56 |
57 | ### Subsystem 2: The Magnetometer Calibration Algorithm
58 |
59 | Each of the three sensors (accelerometer, gyroscope, magnetometer) have 12 parameters that needs to be calibrated to make sure sensor data have no offset or deformity. They are:
60 |
61 | - 3 parameters of sensor bias 
.
62 | - 9 parameters of sensor deformity matrix that represented by 3x3 matrix 
.
63 |
64 | In general, if we have measurement from one of the three sensors 
, then the true sensor data 
can be calculated by this equation:
65 |
66 | 
67 | **Note** that this equation doesn't consider stochastic noise (i.e. this is still a deterministic calibration equation), the stochastic noise will be dealt with sensor fusion algorithm described above.
68 |
69 |
70 |
71 | In total, we have 12 parameters x 3 sensors = 36 parameters total for IMU sensor (actually we haven't consider cross sensor calibration, e.g. gyroscopic sensitivity from linear acceleration parameters, or the temperature-dependence parameters. [Analog Devices made excellent article about them](https://www.analog.com/en/technical-articles/gyro-mechanical-performance.html)). So the total calibration parameters is more than that.
72 |
73 | Fortunately, for many cases the magnetometer bias (the so called hard-iron bias) is the only dominating one. So by compensating the hard-iron bias we can get a good enough sensor data (at least good enough for our purpose). The other sensor's bias and sensor's structural error (hopefully) is not significant enough and can be considered general noise and (hopefully) will be compensated by sensor fusion algorithm. The hard-iron bias identification equation can be derived as:
74 | 
75 |
76 | The equation above is an offline or batch identification (you take big enough sample, preferably more than 1000 samples, then using least-squares to identify the bias). The problem with batch identification is the needs for big memory, but we can use [Recursive Least Squares](https://en.wikipedia.org/wiki/Recursive_least_squares_filter) as the online version to calculate the bias (this way we don't need to store all samples data):
77 | 
78 |
79 | **Note**: The RLS algorithm above is general enough that you could also use it to identify not only hard-iron bias, but also soft-iron bias (or the deformity matrix described above). For example, [this paper](https://www.ncbi.nlm.nih.gov/pmc/articles/PMC7014484/pdf/sensors-20-00535.pdf) explore further by using RLS/ML combination to calculate 12 magnetometer calibration parameters.
80 |
81 |
82 |
83 | # The Implementation
84 |
85 | ### Electrical Implementation
86 | The electrical connection can be seen here:
87 | 
88 |
89 | You only need to use 4 jumper cables to connect them: +3.3VCC, GND, I2C's SDA, and I2C's SCL (the MPU-9250 support SPI and I2C communication mode, but we use only I2C here).
90 |
91 |
92 | ### Programming Implementation
93 |
94 | This is the state machine for the whole system:
95 | 
96 |
97 | As you can see, there are 3 states in the state machine:
98 |
99 | **1. STATE_RUNNING_SENSOR_FUSION**
100 |
101 | This is the default state after booting, here every 20 ms (or whatever `SS_DT_MILIS` constant you set) the sensor fusion is running and calculate the quaternion of the sensor body frame. Send command `'c'` via Arduino's Serial Monitor (or any serial program communication you use) to get into `STATE_MAGNETO_BIAS_IDENTIFICATION`. Send command `'q'` to get the quaternion value (this is typically done automatically if you run the Processing script.
102 |
103 | **2. STATE_MAGNETO_BIAS_IDENTIFICATION**
104 |
105 | This state is where the RLS algorithm run every 20 ms (or whatever `SS_DT_MILIS` constant you set). Make an '8 maneuver' you usually do everytime you calibrate the magnetometer on your smartphone. If the algorithm convergent, the state machine move into `STATE_NORTH_VECTOR_IDENTIFICATION`; while if the algorithm is not convergent (usually your maneuver doesn't give enough coverage for magnetometer bias identification), the algorithm will be timeout after 2000 iteration (~40 seconds) and back into `STATE_RUNNING_SENSOR_FUSION` without update the bias.
106 |
107 | **3. STATE_NORTH_VECTOR_IDENTIFICATION**
108 |
109 | In this state, point the sensor into the 'north direction' of your choice and send `'f'` command (can be the North pole direction, or can be whatever local north you want to be). After this, that direction will be your navigation north.
110 |
111 |
112 |
113 | # How to Use
114 |
115 | Just place one of the implementation folder ("[ahrs_ekf](ahrs_ekf)" or "[ahrs_ukf](ahrs_ukf)") in your Arduino installation folder and run with it! Inside each folder you will find these files:
116 | - `matrix.h/cpp` : The backbone of all my code in this account. This files contain the class for Matrix operation.
117 | - `ekf.h/cpp` or `ukf.h/cpp` : The source files of the EKF/UKF Class.
118 | - `konfig.h` : The configuration file.
119 | - `simple_mpu9250.h` : MPU-9250 I2C library, it is a simplified version of MPU-9250 library from [bolderflight project](https://github.com/bolderflight/MPU9250).
120 | - `*.ino` : The arduino main file.
121 |
122 |
123 |
124 | For the data visualization, I use script modified from [FreeIMU project](http://www.varesano.net/files/FreeIMU-20121122_1126.zip) (You need [Processing to run these script](https://processing.org/)). Just place the "[FreeIMU_cube](FreeIMU_cube)" folder in your Processing installation folder and run the script.
125 | You need to run the script **after** you finished the calibration procedure described above in ***Programming Implementation*** section above (if not, the AHRS system will use the bias & north data from the identification I did which might be not valid anymore for your hardware/location).
126 |
127 |
128 | For custom implementation (for example you want to change the state space or the identification equation), typically you only need to modify `konfig.h` and `*.ino` files. Where basically you need to:
129 | 1. Set the length of `X, U, Z` vectors and sampling time `dt` in `konfig.h`, depend on your model.
130 | 2. Implement the nonlinear update function `f(x)`, measurement function `h(x)`, Jacobian update function `JF(x)` (no need for if you use UKF as the sensor fusion algorithm), Jacobian measurement function `JH(x)` (no need for if you use UKF as the sensor fusion algorithm), initialization value `P(k=0)`, and `Qn & Rn` constants value in the `*.ino` file.
131 | 3. Change the `RLS_*` vector length and the RLS extraction equation if you want to change the variable you want to identify.
132 |
133 |
134 |
135 |
136 | *For Arduino configuration (`SYSTEM_IMPLEMENTATION` is set to `SYSTEM_IMPLEMENTATION_EMBEDDED_ARDUINO` in `konfig.h`):
137 | The code is tested on compiler Arduino IDE 1.8.10 and hardware Teensy 4.0 Platform.
138 |
139 | *For PC configuration (`SYSTEM_IMPLEMENTATION` is set to `SYSTEM_IMPLEMENTATION_PC` in `konfig.h`):
140 | The code is tested on compiler Qt Creator 4.8.2 and typical PC Platform.
141 |
142 | **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.**
143 |
144 |
145 | # Closing Remark
146 | 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).
147 |
148 | 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 (⌒▽⌒)
149 |
--------------------------------------------------------------------------------
/Sensor_Fusion_RLS_StateMachine.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/pronenewbits/Arduino_AHRS_System/aa469bd09ff2d7986ac7f3755f1d17c288f5b3e8/Sensor_Fusion_RLS_StateMachine.png
--------------------------------------------------------------------------------
/Teensy_MPU9250_Connection.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/pronenewbits/Arduino_AHRS_System/aa469bd09ff2d7986ac7f3755f1d17c288f5b3e8/Teensy_MPU9250_Connection.png
--------------------------------------------------------------------------------
/ahrs_ekf/ahrs_ekf.ino:
--------------------------------------------------------------------------------
1 | #include
2 | #include
3 | #include "konfig.h"
4 | #include "matrix.h"
5 | #include "ekf.h"
6 | #include "simple_mpu9250.h"
7 |
8 |
9 |
10 | /* ================================================= RLS Variables/function declaration ================================================= */
11 | float_prec RLS_lambda = 0.999; /* Forgetting factor */
12 | Matrix RLS_theta(4,1); /* The variables we want to indentify */
13 | Matrix RLS_P(4,4); /* Inverse of correction estimation */
14 | Matrix RLS_in(4,1); /* Input data */
15 | Matrix RLS_out(1,1); /* Output data */
16 | Matrix RLS_gain(4,1); /* RLS gain */
17 | uint32_t RLS_u32iterData = 0; /* To track how much data we take */
18 |
19 |
20 | /* ================================================= EKF Variables/function declaration ================================================= */
21 | /* EKF initialization constant */
22 | #define P_INIT (10.)
23 | #define Q_INIT (1e-6)
24 | #define R_INIT_ACC (0.0015/10.)
25 | #define R_INIT_MAG (0.0015/10.)
26 | /* P(k=0) variable --------------------------------------------------------------------------------------------------------- */
27 | float_prec EKF_PINIT_data[SS_X_LEN*SS_X_LEN] = {P_INIT, 0, 0, 0,
28 | 0, P_INIT, 0, 0,
29 | 0, 0, P_INIT, 0,
30 | 0, 0, 0, P_INIT};
31 | Matrix EKF_PINIT(SS_X_LEN, SS_X_LEN, EKF_PINIT_data);
32 | /* Q constant -------------------------------------------------------------------------------------------------------------- */
33 | float_prec EKF_QINIT_data[SS_X_LEN*SS_X_LEN] = {Q_INIT, 0, 0, 0,
34 | 0, Q_INIT, 0, 0,
35 | 0, 0, Q_INIT, 0,
36 | 0, 0, 0, Q_INIT};
37 | Matrix EKF_QINIT(SS_X_LEN, SS_X_LEN, EKF_QINIT_data);
38 | /* R constant -------------------------------------------------------------------------------------------------------------- */
39 | float_prec EKF_RINIT_data[SS_Z_LEN*SS_Z_LEN] = {R_INIT_ACC, 0, 0, 0, 0, 0,
40 | 0, R_INIT_ACC, 0, 0, 0, 0,
41 | 0, 0, R_INIT_ACC, 0, 0, 0,
42 | 0, 0, 0, R_INIT_MAG, 0, 0,
43 | 0, 0, 0, 0, R_INIT_MAG, 0,
44 | 0, 0, 0, 0, 0, R_INIT_MAG};
45 | Matrix EKF_RINIT(SS_Z_LEN, SS_Z_LEN, EKF_RINIT_data);
46 | /* Nonlinear & linearization function -------------------------------------------------------------------------------------- */
47 | bool Main_bUpdateNonlinearX(Matrix &X_Next, Matrix &X, Matrix &U);
48 | bool Main_bUpdateNonlinearY(Matrix &Y, Matrix &X, Matrix &U);
49 | bool Main_bCalcJacobianF(Matrix &F, Matrix &X, Matrix &U);
50 | bool Main_bCalcJacobianH(Matrix &H, Matrix &X, Matrix &U);
51 | /* EKF variables ----------------------------------------------------------------------------------------------------------- */
52 | Matrix quaternionData(SS_X_LEN, 1);
53 | Matrix Y(SS_Z_LEN, 1);
54 | Matrix U(SS_U_LEN, 1);
55 | EKF EKF_IMU(quaternionData, EKF_PINIT, EKF_QINIT, EKF_RINIT,
56 | Main_bUpdateNonlinearX, Main_bUpdateNonlinearY, Main_bCalcJacobianF, Main_bCalcJacobianH);
57 |
58 |
59 |
60 | /* =============================================== Sharing Variables/function declaration =============================================== */
61 | /* Gravity vector constant (align with global Z-axis) */
62 | #define IMU_ACC_Z0 (1)
63 |
64 | /* Magnetic vector constant (align with local magnetic vector) */
65 | float_prec IMU_MAG_B0_data[3] = {cos(0), sin(0), 0.000000};
66 | Matrix IMU_MAG_B0(3, 1, IMU_MAG_B0_data);
67 |
68 | /* The hard-magnet bias */
69 | float_prec HARD_IRON_BIAS_data[3] = {8.832973, 7.243323, 23.95714};
70 | Matrix HARD_IRON_BIAS(3, 1, HARD_IRON_BIAS_data);
71 |
72 | /* An MPU9250 object with the MPU-9250 sensor on I2C bus 0 with address 0x68 */
73 | SimpleMPU9250 IMU(Wire, 0x68);
74 |
75 | /* State machine for hard-iron bias identification or EKF running */
76 | enum {
77 | STATE_EKF_RUNNING = 0,
78 | STATE_MAGNETO_BIAS_IDENTIFICATION,
79 | STATE_NORTH_VECTOR_IDENTIFICATION
80 | } STATE_AHRS;
81 |
82 |
83 |
84 | /* ============================================== Auxiliary Variables/function declaration ============================================== */
85 | elapsedMillis timerCollectData = 0;
86 | uint64_t u64compuTime;
87 | char bufferTxSer[100];
88 | void serialFloatPrint(float f);
89 | /* The command from the PC */
90 | char cmd;
91 |
92 |
93 |
94 |
95 | void setup() {
96 | /* Serial initialization -------------------------------------- */
97 | Serial.begin(115200);
98 | while(!Serial) {}
99 | Serial.println("Calibrating IMU bias...");
100 |
101 | /* IMU initialization ----------------------------------------- */
102 | int status = IMU.begin(); /* start communication with IMU */
103 | if (status < 0) {
104 | Serial.println("IMU initialization unsuccessful");
105 | Serial.println("Check IMU wiring or try cycling power");
106 | Serial.print("Status: ");
107 | Serial.println(status);
108 | while(1) {}
109 | }
110 |
111 | /* RLS initialization ----------------------------------------- */
112 | RLS_theta.vSetToZero();
113 | RLS_P.vSetIdentity();
114 | RLS_P = RLS_P * 1000;
115 |
116 | /* EKF initialization ----------------------------------------- */
117 | /* x(k=0) = [1 0 0 0]' */
118 | quaternionData.vSetToZero();
119 | quaternionData[0][0] = 1.0;
120 | EKF_IMU.vReset(quaternionData, EKF_PINIT, EKF_QINIT, EKF_RINIT);
121 |
122 | snprintf(bufferTxSer, sizeof(bufferTxSer)-1, "EKF in Teensy 4.0 (%s)\r\n", (FPU_PRECISION == PRECISION_SINGLE)?"Float32":"Double64");
123 | Serial.print(bufferTxSer);
124 | STATE_AHRS = STATE_EKF_RUNNING;
125 | }
126 |
127 |
128 |
129 |
130 | void loop() {
131 |
132 | if (timerCollectData >= SS_DT_MILIS) { /* We running the RLS/EKF at sampling time = 20 ms */
133 | timerCollectData = 0;
134 |
135 | /* Read the raw data */
136 | IMU.readSensor();
137 | float Ax = IMU.getAccelX_mss();
138 | float Ay = IMU.getAccelY_mss();
139 | float Az = IMU.getAccelZ_mss();
140 | float Bx = IMU.getMagX_uT();
141 | float By = IMU.getMagY_uT();
142 | float Bz = IMU.getMagZ_uT();
143 | float p = IMU.getGyroX_rads();
144 | float q = IMU.getGyroY_rads();
145 | float r = IMU.getGyroZ_rads();
146 |
147 | if (STATE_AHRS == STATE_EKF_RUNNING) { /* Run the EKF algorithm */
148 |
149 | /* ================== Read the sensor data / simulate the system here ================== */
150 |
151 | /* Input 1:3 = gyroscope */
152 | U[0][0] = p; U[1][0] = q; U[2][0] = r;
153 | /* Output 1:3 = accelerometer */
154 | Y[0][0] = Ax; Y[1][0] = Ay; Y[2][0] = Az;
155 | /* Output 4:6 = magnetometer */
156 | Y[3][0] = Bx; Y[4][0] = By; Y[5][0] = Bz;
157 |
158 | /* Compensating Hard-Iron Bias for magnetometer */
159 | Y[3][0] = Y[3][0]-HARD_IRON_BIAS[0][0];
160 | Y[4][0] = Y[4][0]-HARD_IRON_BIAS[1][0];
161 | Y[5][0] = Y[5][0]-HARD_IRON_BIAS[2][0];
162 |
163 | /* Normalizing the output vector */
164 | float_prec _normG = sqrt(Y[0][0] * Y[0][0]) + (Y[1][0] * Y[1][0]) + (Y[2][0] * Y[2][0]);
165 | Y[0][0] = Y[0][0] / _normG;
166 | Y[1][0] = Y[1][0] / _normG;
167 | Y[2][0] = Y[2][0] / _normG;
168 | float_prec _normM = sqrt(Y[3][0] * Y[3][0]) + (Y[4][0] * Y[4][0]) + (Y[5][0] * Y[5][0]);
169 | Y[3][0] = Y[3][0] / _normM;
170 | Y[4][0] = Y[4][0] / _normM;
171 | Y[5][0] = Y[5][0] / _normM;
172 | /* ------------------ Read the sensor data / simulate the system here ------------------ */
173 |
174 |
175 | /* ============================= Update the Kalman Filter ============================== */
176 | u64compuTime = micros();
177 | if (!EKF_IMU.bUpdate(Y, U)) {
178 | quaternionData.vSetToZero();
179 | quaternionData[0][0] = 1.0;
180 | EKF_IMU.vReset(quaternionData, EKF_PINIT, EKF_QINIT, EKF_RINIT);
181 | Serial.println("Whoop ");
182 | }
183 | u64compuTime = (micros() - u64compuTime);
184 | /* ----------------------------- Update the Kalman Filter ------------------------------ */
185 |
186 | } else if (STATE_AHRS == STATE_MAGNETO_BIAS_IDENTIFICATION) {
187 |
188 | /* ================== Read the sensor data / simulate the system here ================== */
189 | RLS_in[0][0] = Bx;
190 | RLS_in[1][0] = By;
191 | RLS_in[2][0] = Bz;
192 | RLS_in[3][0] = 1;
193 | RLS_out[0][0] = (Bx*Bx) + (By*By) + (Bz*Bz);
194 |
195 | float err = (RLS_out - (RLS_in.Transpose() * RLS_theta))[0][0];
196 | RLS_gain = RLS_P*RLS_in / (RLS_lambda + RLS_in.Transpose()*RLS_P*RLS_in)[0][0];
197 | RLS_P = (RLS_P - RLS_gain*RLS_in.Transpose()*RLS_P)/RLS_lambda;
198 | RLS_theta = RLS_theta + err*RLS_gain;
199 |
200 | RLS_u32iterData++;
201 |
202 | Matrix P_check(RLS_P.GetDiagonalEntries());
203 | if ((P_check.Transpose()*P_check)[0][0] < 1e-4) {
204 | /* The data collection is finished, go back to state EKF running */
205 | STATE_AHRS = STATE_NORTH_VECTOR_IDENTIFICATION;
206 |
207 | /* Reconstruct the matrix compensation solution */
208 | HARD_IRON_BIAS[0][0] = RLS_theta[0][0] / 2.0;
209 | HARD_IRON_BIAS[1][0] = RLS_theta[1][0] / 2.0;
210 | HARD_IRON_BIAS[2][0] = RLS_theta[2][0] / 2.0;
211 |
212 | Serial.println("Calibration finished, the hard-iron bias identified:");
213 | snprintf(bufferTxSer, sizeof(bufferTxSer)-1, "%f %f %f\r\n", HARD_IRON_BIAS[0][0], HARD_IRON_BIAS[1][0], HARD_IRON_BIAS[2][0]);
214 | Serial.println(bufferTxSer);
215 | Serial.println("Set the X axis facing north *with z-accelerometer pointed down*, then send command 'f' to finished, 'a' to abort");
216 | }
217 |
218 |
219 | if ((RLS_u32iterData % 100) == 0) {
220 | /* Print every 200 data, so the user can get updated value */
221 | Matrix P_check(RLS_P.GetDiagonalEntries());
222 |
223 | Serial.println("Calibration in progress, the hard-iron bias identified so far:");
224 | snprintf(bufferTxSer, sizeof(bufferTxSer)-1, "%.3f %.3f %.3f (P = %f)\r\n", RLS_theta[0][0] / 2.0, RLS_theta[1][0] / 2.0, RLS_theta[2][0] / 2.0, (P_check.Transpose()*P_check)[0][0]);
225 | Serial.println(bufferTxSer);
226 | }
227 | if (RLS_u32iterData >= 2000) {
228 | /* We take the data too long but the error still large, terminate without updating the hard-iron bias */
229 | STATE_AHRS = STATE_EKF_RUNNING;
230 |
231 | Serial.println("Calibration timeout, the hard-iron bias won't be updated\r\n");
232 | }
233 |
234 | } else if (STATE_AHRS == STATE_NORTH_VECTOR_IDENTIFICATION) {
235 | } else {
236 | /* You should not be here! */
237 | STATE_AHRS = STATE_EKF_RUNNING;
238 | }
239 | }
240 |
241 |
242 | /* Event serial data: The serial data is sent by responding to command from the PC running Processing scipt or from user command */
243 | if (Serial.available()) {
244 | cmd = Serial.read();
245 |
246 |
247 | if (STATE_AHRS == STATE_EKF_RUNNING) {
248 |
249 | if (cmd == 'c') {
250 | /* User want to do online hard-iron bias identification */
251 | Serial.println("Hard iron bias identification command entered\r\n");
252 | Serial.println("Rotate the MPU9250 in every direction\r\n");
253 | Serial.println("Send 'a' to abort\r\n");
254 |
255 | /* Initialize RLS */
256 | RLS_theta.vSetToZero();
257 | RLS_P.vSetIdentity();
258 | RLS_P = RLS_P * 1000;
259 | RLS_u32iterData = 0;
260 | STATE_AHRS = STATE_MAGNETO_BIAS_IDENTIFICATION;
261 |
262 | } else if (cmd == 'p') {
263 |
264 | Serial.println("North vector value:");
265 | snprintf(bufferTxSer, sizeof(bufferTxSer)-1, "%f %f %f\r\n", IMU_MAG_B0[0][0], IMU_MAG_B0[1][0], IMU_MAG_B0[2][0]);
266 | Serial.print(bufferTxSer);
267 |
268 | Serial.println("Hard iron bias value:");
269 | snprintf(bufferTxSer, sizeof(bufferTxSer)-1, "%f %f %f\r\n", HARD_IRON_BIAS[0][0], HARD_IRON_BIAS[1][0], HARD_IRON_BIAS[2][0]);
270 | Serial.print(bufferTxSer);
271 |
272 | } else if (cmd == 'n') {
273 | Serial.println("North vector identification command entered\r\n");
274 | Serial.println("Set the X axis facing north *with z-accelerometer pointed down*, then send command 'f'");
275 | STATE_AHRS = STATE_NORTH_VECTOR_IDENTIFICATION;
276 | }
277 |
278 | /* Process the command data from Processing script */
279 | if (cmd == 'v') {
280 | snprintf(bufferTxSer, sizeof(bufferTxSer)-1, "EKF in Teensy 4.0 (%s)", (FPU_PRECISION == PRECISION_SINGLE)?"Float32":"Double64");
281 | Serial.print('\n');
282 |
283 | } else if (cmd == 'q') {
284 | /* =========================== Print to serial (for plotting) ========================== */
285 | quaternionData = EKF_IMU.GetX();
286 |
287 | while (!Serial.available());
288 | uint8_t count = Serial.read();
289 | for (uint8_t _i = 0; _i < count; _i++) {
290 | serialFloatPrint(quaternionData[0][0]);
291 | Serial.print(",");
292 | serialFloatPrint(quaternionData[1][0]);
293 | Serial.print(",");
294 | serialFloatPrint(quaternionData[2][0]);
295 | Serial.print(",");
296 | serialFloatPrint(quaternionData[3][0]);
297 | Serial.print(",");
298 | serialFloatPrint((float)u64compuTime);
299 | Serial.print(",");
300 | Serial.println("");
301 | }
302 | }
303 |
304 | } else if (STATE_AHRS == STATE_MAGNETO_BIAS_IDENTIFICATION) {
305 |
306 | if (cmd == 'a') {
307 | /* User want to terminate hard iron bias identification */
308 | Serial.println("Calibration aborted, the hard-iron bias won't be updated\r\n");
309 | STATE_AHRS = STATE_EKF_RUNNING;
310 | }
311 |
312 | } else if (STATE_AHRS == STATE_NORTH_VECTOR_IDENTIFICATION) {
313 |
314 | if (cmd == 'a') {
315 | /* User want to terminate hard iron bias identification */
316 | Serial.println("Calibration aborted, the hard-iron bias won't be updated\r\n");
317 | STATE_AHRS = STATE_EKF_RUNNING;
318 |
319 | } else if (cmd == 'f') {
320 | float Ax = IMU.getAccelX_mss();
321 | float Ay = IMU.getAccelY_mss();
322 | float Az = IMU.getAccelZ_mss();
323 | float Bx = IMU.getMagX_uT() - HARD_IRON_BIAS[0][0];
324 | float By = IMU.getMagY_uT() - HARD_IRON_BIAS[1][0];
325 | float Bz = IMU.getMagZ_uT() - HARD_IRON_BIAS[2][0];
326 |
327 | /* Normalizing the acceleration vector & projecting the gravitational vector (gravity is negative acceleration) */
328 | float_prec _normG = sqrt((Ax * Ax) + (Ay * Ay) + (Az * Az));
329 | Ax = Ax / _normG;
330 | Ay = Ay / _normG;
331 | Az = Az / _normG;
332 |
333 | /* Normalizing the magnetic vector */
334 | _normG = sqrt((Bx * Bx) + (By * By) + (Bz * Bz));
335 | Bx = Bx / _normG;
336 | By = By / _normG;
337 | Bz = Bz / _normG;
338 |
339 | /* Projecting the magnetic vector into plane orthogonal to the gravitational vector */
340 | float pitch = asin(-Ax);
341 | float roll = asin(Ay/cos(pitch));
342 | float m_tilt_x = Bx*cos(pitch) + By*sin(roll)*sin(pitch) + Bz*cos(roll)*sin(pitch);
343 | float m_tilt_y = + By*cos(roll) - Bz*sin(roll);
344 | /* float m_tilt_z = -Bx*sin(pitch) + By*sin(roll)*cos(pitch) + Bz*cos(roll)*cos(pitch); */
345 |
346 | float mag_dec = atan2(m_tilt_y, m_tilt_x);
347 | IMU_MAG_B0[0][0] = cos(mag_dec);
348 | IMU_MAG_B0[1][0] = sin(mag_dec);
349 | IMU_MAG_B0[2][0] = 0;
350 |
351 | Serial.println("North identification finished, the north vector identified:");
352 | snprintf(bufferTxSer, sizeof(bufferTxSer)-1, "%.3f %.3f %.3f\r\n", IMU_MAG_B0[0][0], IMU_MAG_B0[1][0], IMU_MAG_B0[2][0]);
353 | Serial.print(bufferTxSer);
354 |
355 | STATE_AHRS = STATE_EKF_RUNNING;
356 | }
357 |
358 | }
359 | }
360 | }
361 |
362 |
363 | /* Function to interface with the Processing script in the PC */
364 | void serialFloatPrint(float f) {
365 | byte * b = (byte *) &f;
366 | for (int i = 0; i < 4; i++) {
367 | byte b1 = (b[i] >> 4) & 0x0f;
368 | byte b2 = (b[i] & 0x0f);
369 |
370 | char c1 = (b1 < 10) ? ('0' + b1) : 'A' + b1 - 10;
371 | char c2 = (b2 < 10) ? ('0' + b2) : 'A' + b2 - 10;
372 |
373 | Serial.print(c1);
374 | Serial.print(c2);
375 | }
376 | }
377 |
378 | bool Main_bUpdateNonlinearX(Matrix &X_Next, Matrix &X, Matrix &U)
379 | {
380 | /* Insert the nonlinear update transformation here
381 | * x(k+1) = f[x(k), u(k)]
382 | *
383 | * The quaternion update function:
384 | * q0_dot = 1/2. * ( 0 - p*q1 - q*q2 - r*q3)
385 | * q1_dot = 1/2. * ( p*q0 + 0 + r*q2 - q*q3)
386 | * q2_dot = 1/2. * ( q*q0 - r*q1 + 0 + p*q3)
387 | * q3_dot = 1/2. * ( r*q0 + q*q1 - p*q2 + 0 )
388 | *
389 | * Euler method for integration:
390 | * q0 = q0 + q0_dot * dT;
391 | * q1 = q1 + q1_dot * dT;
392 | * q2 = q2 + q2_dot * dT;
393 | * q3 = q3 + q3_dot * dT;
394 | */
395 | float_prec q0, q1, q2, q3;
396 | float_prec p, q, r;
397 |
398 | q0 = X[0][0];
399 | q1 = X[1][0];
400 | q2 = X[2][0];
401 | q3 = X[3][0];
402 |
403 | p = U[0][0];
404 | q = U[1][0];
405 | r = U[2][0];
406 |
407 | X_Next[0][0] = (0.5 * (+0.00 -p*q1 -q*q2 -r*q3))*SS_DT + q0;
408 | X_Next[1][0] = (0.5 * (+p*q0 +0.00 +r*q2 -q*q3))*SS_DT + q1;
409 | X_Next[2][0] = (0.5 * (+q*q0 -r*q1 +0.00 +p*q3))*SS_DT + q2;
410 | X_Next[3][0] = (0.5 * (+r*q0 +q*q1 -p*q2 +0.00))*SS_DT + q3;
411 |
412 |
413 | /* ======= Additional ad-hoc quaternion normalization to make sure the quaternion is a unit vector (i.e. ||q|| = 1) ======= */
414 | if (!X_Next.bNormVector()) {
415 | /* System error, return false so we can reset the UKF */
416 | return false;
417 | }
418 |
419 | return true;
420 | }
421 |
422 | bool Main_bUpdateNonlinearY(Matrix &Y, Matrix &X, Matrix &U)
423 | {
424 | /* Insert the nonlinear measurement transformation here
425 | * y(k) = h[x(k), u(k)]
426 | *
427 | * The measurement output is the gravitational and magnetic projection to the body
428 | * DCM = [(+(q0**2)+(q1**2)-(q2**2)-(q3**2)), 2*(q1*q2+q0*q3), 2*(q1*q3-q0*q2)]
429 | * [ 2*(q1*q2-q0*q3), (+(q0**2)-(q1**2)+(q2**2)-(q3**2)), 2*(q2*q3+q0*q1)]
430 | * [ 2*(q1*q3+q0*q2), 2*(q2*q3-q0*q1), (+(q0**2)-(q1**2)-(q2**2)+(q3**2))]
431 | *
432 | * G_proj_sens = DCM * [0 0 1] --> Gravitational projection to the accelerometer sensor
433 | * M_proj_sens = DCM * [Mx My Mz] --> (Earth) magnetic projection to the magnetometer sensor
434 | */
435 | float_prec q0, q1, q2, q3;
436 | float_prec q0_2, q1_2, q2_2, q3_2;
437 |
438 | q0 = X[0][0];
439 | q1 = X[1][0];
440 | q2 = X[2][0];
441 | q3 = X[3][0];
442 |
443 | q0_2 = q0 * q0;
444 | q1_2 = q1 * q1;
445 | q2_2 = q2 * q2;
446 | q3_2 = q3 * q3;
447 |
448 | Y[0][0] = (2*q1*q3 -2*q0*q2) * IMU_ACC_Z0;
449 |
450 | Y[1][0] = (2*q2*q3 +2*q0*q1) * IMU_ACC_Z0;
451 |
452 | Y[2][0] = (+(q0_2) -(q1_2) -(q2_2) +(q3_2)) * IMU_ACC_Z0;
453 |
454 | Y[3][0] = (+(q0_2)+(q1_2)-(q2_2)-(q3_2)) * IMU_MAG_B0[0][0]
455 | +(2*(q1*q2+q0*q3)) * IMU_MAG_B0[1][0]
456 | +(2*(q1*q3-q0*q2)) * IMU_MAG_B0[2][0];
457 |
458 | Y[4][0] = (2*(q1*q2-q0*q3)) * IMU_MAG_B0[0][0]
459 | +(+(q0_2)-(q1_2)+(q2_2)-(q3_2)) * IMU_MAG_B0[1][0]
460 | +(2*(q2*q3+q0*q1)) * IMU_MAG_B0[2][0];
461 |
462 | Y[5][0] = (2*(q1*q3+q0*q2)) * IMU_MAG_B0[0][0]
463 | +(2*(q2*q3-q0*q1)) * IMU_MAG_B0[1][0]
464 | +(+(q0_2)-(q1_2)-(q2_2)+(q3_2)) * IMU_MAG_B0[2][0];
465 | return true;
466 | }
467 |
468 | bool Main_bCalcJacobianF(Matrix &F, Matrix &X, Matrix &U)
469 | {
470 | /* In Main_bUpdateNonlinearX():
471 | * q0 = q0 + q0_dot * dT;
472 | * q1 = q1 + q1_dot * dT;
473 | * q2 = q2 + q2_dot * dT;
474 | * q3 = q3 + q3_dot * dT;
475 | */
476 | float_prec p, q, r;
477 |
478 | p = U[0][0];
479 | q = U[1][0];
480 | r = U[2][0];
481 |
482 | F[0][0] = 1.000;
483 | F[1][0] = 0.5*p * SS_DT;
484 | F[2][0] = 0.5*q * SS_DT;
485 | F[3][0] = 0.5*r * SS_DT;
486 |
487 | F[0][1] = -0.5*p * SS_DT;
488 | F[1][1] = 1.000;
489 | F[2][1] = -0.5*r * SS_DT;
490 | F[3][1] = 0.5*q * SS_DT;
491 |
492 | F[0][2] = -0.5*q * SS_DT;
493 | F[1][2] = 0.5*r * SS_DT;
494 | F[2][2] = 1.000;
495 | F[3][2] = -0.5*p * SS_DT;
496 |
497 | F[0][3] = -0.5*r * SS_DT;
498 | F[1][3] = -0.5*q * SS_DT;
499 | F[2][3] = 0.5*p * SS_DT;
500 | F[3][3] = 1.000;
501 |
502 | return true;
503 | }
504 |
505 | bool Main_bCalcJacobianH(Matrix &H, Matrix &X, Matrix &U)
506 | {
507 | float_prec q0, q1, q2, q3;
508 |
509 | q0 = X[0][0];
510 | q1 = X[1][0];
511 | q2 = X[2][0];
512 | q3 = X[3][0];
513 |
514 | H[0][0] = -2*q2 * IMU_ACC_Z0;
515 | H[1][0] = +2*q1 * IMU_ACC_Z0;
516 | H[2][0] = +2*q0 * IMU_ACC_Z0;
517 | H[3][0] = 2*q0*IMU_MAG_B0[0][0] + 2*q3*IMU_MAG_B0[1][0] - 2*q2*IMU_MAG_B0[2][0];
518 | H[4][0] = -2*q3*IMU_MAG_B0[0][0] + 2*q0*IMU_MAG_B0[1][0] + 2*q1*IMU_MAG_B0[2][0];
519 | H[5][0] = 2*q2*IMU_MAG_B0[0][0] - 2*q1*IMU_MAG_B0[1][0] + 2*q0*IMU_MAG_B0[2][0];
520 |
521 | H[0][1] = +2*q3 * IMU_ACC_Z0;
522 | H[1][1] = +2*q0 * IMU_ACC_Z0;
523 | H[2][1] = -2*q1 * IMU_ACC_Z0;
524 | H[3][1] = 2*q1*IMU_MAG_B0[0][0]+2*q2*IMU_MAG_B0[1][0] + 2*q3*IMU_MAG_B0[2][0];
525 | H[4][1] = 2*q2*IMU_MAG_B0[0][0]-2*q1*IMU_MAG_B0[1][0] + 2*q0*IMU_MAG_B0[2][0];
526 | H[5][1] = 2*q3*IMU_MAG_B0[0][0]-2*q0*IMU_MAG_B0[1][0] - 2*q1*IMU_MAG_B0[2][0];
527 |
528 | H[0][2] = -2*q0 * IMU_ACC_Z0;
529 | H[1][2] = +2*q3 * IMU_ACC_Z0;
530 | H[2][2] = -2*q2 * IMU_ACC_Z0;
531 | H[3][2] = -2*q2*IMU_MAG_B0[0][0]+2*q1*IMU_MAG_B0[1][0] - 2*q0*IMU_MAG_B0[2][0];
532 | H[4][2] = 2*q1*IMU_MAG_B0[0][0]+2*q2*IMU_MAG_B0[1][0] + 2*q3*IMU_MAG_B0[2][0];
533 | H[5][2] = 2*q0*IMU_MAG_B0[0][0]+2*q3*IMU_MAG_B0[1][0] - 2*q2*IMU_MAG_B0[2][0];
534 |
535 | H[0][3] = +2*q1 * IMU_ACC_Z0;
536 | H[1][3] = +2*q2 * IMU_ACC_Z0;
537 | H[2][3] = +2*q3 * IMU_ACC_Z0;
538 | H[3][3] = -2*q3*IMU_MAG_B0[0][0]+2*q0*IMU_MAG_B0[1][0] + 2*q1*IMU_MAG_B0[2][0];
539 | H[4][3] = -2*q0*IMU_MAG_B0[0][0]-2*q3*IMU_MAG_B0[1][0] + 2*q2*IMU_MAG_B0[2][0];
540 | H[5][3] = 2*q1*IMU_MAG_B0[0][0]+2*q2*IMU_MAG_B0[1][0] + 2*q3*IMU_MAG_B0[2][0];
541 |
542 | return true;
543 | }
544 |
545 |
546 |
547 | void SPEW_THE_ERROR(char const * str)
548 | {
549 | #if (SYSTEM_IMPLEMENTATION == SYSTEM_IMPLEMENTATION_PC)
550 | cout << (str) << endl;
551 | #elif (SYSTEM_IMPLEMENTATION == SYSTEM_IMPLEMENTATION_EMBEDDED_ARDUINO)
552 | Serial.println(str);
553 | #else
554 | /* Silent function */
555 | #endif
556 | while(1);
557 | }
558 |
--------------------------------------------------------------------------------
/ahrs_ekf/ekf.cpp:
--------------------------------------------------------------------------------
1 | /***************************************************************************************************
2 | * Class for Discrete Extended Kalman Filter
3 | * The system to be estimated is defined as a discrete nonlinear dynamic dystem:
4 | * x(k) = f[x(k-1), u(k-1)] + v(k) ; x = Nx1, u = Mx1
5 | * y(k) = h[x(k)] + n(k) ; y = Zx1
6 | *
7 | * Where:
8 | * x(k) : State Variable at time-k : Nx1
9 | * y(k) : Measured output at time-k : Zx1
10 | * u(k) : System input at time-k : Mx1
11 | * v(k) : Process noise, AWGN assumed, w/ covariance Qn : Nx1
12 | * n(k) : Measurement noise, AWGN assumed, w/ covariance Rn : Nx1
13 | *
14 | * f(..), h(..) is a nonlinear transformation of the system to be estimated.
15 | *
16 | ***************************************************************************************************
17 | * Extended Kalman Filter algorithm:
18 | * Initialization:
19 | * x(k=0|k=0) = Expected value of x at time-0 (i.e. x(k=0)), typically set to zero.
20 | * P(k=0|k=0) = Identity matrix * covariant(P(k=0)), typically initialized with some
21 | * big number.
22 | * Q, R = Covariance matrices of process & measurement. As this implementation
23 | * the noise as AWGN (and same value for every variable), this is set
24 | * to Q=diag(QInit,...,QInit) and R=diag(RInit,...,RInit).
25 | *
26 | *
27 | * EKF Calculation (every sampling time):
28 | * Calculate the Jacobian matrix of f (i.e. F):
29 | * F = d(f(..))/dx |x(k-1|k-1),u(k-1) ...{EKF_1}
30 | *
31 | * Predict x(k) through nonlinear function f:
32 | * x(k|k-1) = f[x(k-1|k-1), u(k-1)] ...{EKF_2}
33 | *
34 | * Predict P(k) using linearized f (i.e. F):
35 | * P(k|k-1) = F*P(k-1|k-1)*F' + Q ...{EKF_3}
36 | *
37 | * Calculate the Jacobian matrix of h (i.e. C):
38 | * C = d(h(..))/dx |x(k|k-1) ...{EKF_4}
39 | *
40 | * Predict residual covariance S using linearized h (i.e. H):
41 | * S = C*P(k|k-1)*C' + R ...{EKF_5}
42 | *
43 | * Calculate the kalman gain:
44 | * K = P(k|k-1)*C'*(S^-1) ...{EKF_6}
45 | *
46 | * Correct x(k) using kalman gain:
47 | * x(k|k) = x(k|k-1) + K*[y(k) - h(x(k|k-1))] ...{EKF_7}
48 | *
49 | * Correct P(k) using kalman gain:
50 | * P(k|k) = (I - K*C)*P(k|k-1) ...{EKF_8}
51 | *
52 | *
53 | * *Additional Information:
54 | * - Pada contoh di atas X~(k=0|k=0) = [0]. Untuk mempercepat konvergensi bisa digunakan
55 | * informasi plant-spesific. Misal pada implementasi Kalman Filter untuk sensor
56 | * IMU (Inertial measurement unit) dengan X = [quaternion], dengan asumsi IMU
57 | * awalnya menghadap ke atas tanpa rotasi, X~(k=0|k=0) = [1, 0, 0, 0]'
58 | *
59 | *
60 | * See https://github.com/pronenewbits for more!
61 | **************************************************************************************************/
62 | #include "ekf.h"
63 |
64 |
65 | EKF::EKF(Matrix &XInit, Matrix &P, Matrix &Q, Matrix &R,
66 | bool (*bNonlinearUpdateX)(Matrix &, Matrix &, Matrix &), bool (*bNonlinearUpdateY)(Matrix &, Matrix &, Matrix &),
67 | bool (*bCalcJacobianF)(Matrix &, Matrix &, Matrix &), bool (*bCalcJacobianH)(Matrix &, Matrix &, Matrix &))
68 | {
69 | /* Initialization:
70 | * x(k=0|k=0) = Expected value of x at time-0 (i.e. x(k=0)), typically set to zero.
71 | * P (k=0|k=0) = Identity matrix * covariant(P(k=0)), typically initialized with some
72 | * big number.
73 | * Q, R = Covariance matrices of process & measurement. As this implementation
74 | * the noise as AWGN (and same value for every variable), this is set
75 | * to Q=diag(QInit,...,QInit) and R=diag(RInit,...,RInit).
76 | */
77 | this->X_Est = XInit.Copy();
78 | this->P = P.Copy();
79 | this->Q = Q.Copy();
80 | this->R = R.Copy();
81 | this->bNonlinearUpdateX = bNonlinearUpdateX;
82 | this->bNonlinearUpdateY = bNonlinearUpdateY;
83 | this->bCalcJacobianF = bCalcJacobianF;
84 | this->bCalcJacobianH = bCalcJacobianH;
85 | }
86 |
87 | void EKF::vReset(Matrix &XInit, Matrix &P, Matrix &Q, Matrix &R)
88 | {
89 | this->X_Est = XInit.Copy();
90 | this->P = P.Copy();
91 | this->Q = Q.Copy();
92 | this->R = R.Copy();
93 |
94 | // this->P.vPrint();
95 | // this->Q.vPrint();
96 | // this->R.vPrint();
97 | }
98 |
99 | bool EKF::bUpdate(Matrix &Y, Matrix &U)
100 | {
101 | /* Run once every sampling time */
102 |
103 |
104 | /* =============== Calculate the Jacobian matrix of f (i.e. F) =============== */
105 | /* F = d(f(..))/dx |x(k-1|k-1),u(k-1) ...{EKF_1} */
106 | if (!bCalcJacobianF(F, X_Est, U)) {
107 | return false;
108 | }
109 |
110 |
111 | /* =========================== Prediction of x & P =========================== */
112 | /* x(k|k-1) = f[x(k-1|k-1), u(k-1)] ...{EKF_2} */
113 | if (!bNonlinearUpdateX(X_Est, X_Est, U)) {
114 | return false;
115 | }
116 |
117 | /* P(k|k-1) = F*P(k-1|k-1)*F' + Q ...{EKF_3} */
118 | P = F*P*(F.Transpose()) + Q;
119 |
120 |
121 |
122 | /* =============== Calculate the Jacobian matrix of h (i.e. H) =============== */
123 | /* H = d(h(..))/dx |x(k|k-1) ...{EKF_4} */
124 | if (!bCalcJacobianH(H, X_Est, U)) {
125 | return false;
126 | }
127 |
128 | /* =========================== Correction of x & P =========================== */
129 | /* S = H*P(k|k-1)*H' + R ...{EKF_5} */
130 | S = (H*P*(H.Transpose())) + R;
131 |
132 | /* K = P(k|k-1)*H'*(S^-1) ...{EKF_6} */
133 | Gain = P*(H.Transpose())*(S.Invers());
134 | if (!Gain.bMatrixIsValid()) {
135 | return false;
136 | }
137 |
138 | /* x(k|k) = x(k|k-1) + K*[y(k) - h(x(k|k-1))] ...{EKF_7} */
139 | if (!bNonlinearUpdateY(Y_Est, X_Est, U)) {
140 | return false;
141 | }
142 | X_Est = X_Est + (Gain * (Y - Y_Est));
143 |
144 | /* P(k|k) = (I - K*H)*P(k|k-1) ...{EKF_8} */
145 | Matrix I = Matrix(SS_X_LEN, SS_X_LEN);
146 | I.vSetIdentity();
147 | P = (I - (Gain*H))*P;
148 |
149 |
150 | return true;
151 | }
152 |
153 |
--------------------------------------------------------------------------------
/ahrs_ekf/ekf.h:
--------------------------------------------------------------------------------
1 | /**************************************************************************************************
2 | * Class for Extended Kalman Filter.
3 | *
4 | *
5 | * See https://github.com/pronenewbits for more!
6 | *************************************************************************************************/
7 | #ifndef EKF_H
8 | #define EKF_H
9 |
10 | #include "konfig.h"
11 | #include "matrix.h"
12 |
13 | class EKF
14 | {
15 | public:
16 | EKF(Matrix &XInit, Matrix &P, Matrix &Q, Matrix &R,
17 | bool (*bNonlinearUpdateX)(Matrix &, Matrix &, Matrix &), bool (*bNonlinearUpdateY)(Matrix &, Matrix &, Matrix &),
18 | bool (*bCalcJacobianF)(Matrix &, Matrix &, Matrix &), bool (*bCalcJacobianH)(Matrix &, Matrix &, Matrix &));
19 | void vReset(Matrix &XInit, Matrix &P, Matrix &Q, Matrix &R);
20 | bool bUpdate(Matrix &Y, Matrix &U);
21 | Matrix GetX() { return X_Est; }
22 | Matrix GetY() { return Y_Est; }
23 | Matrix GetP() { return P; }
24 | Matrix GetErr() { return Err; }
25 |
26 | protected:
27 | bool (*bNonlinearUpdateX) (Matrix &X_dot, Matrix &X, Matrix &U);
28 | bool (*bNonlinearUpdateY) (Matrix &Y_Est, Matrix &X, Matrix &U);
29 | bool (*bCalcJacobianF) (Matrix &F, Matrix &X, Matrix &U);
30 | bool (*bCalcJacobianH) (Matrix &H, Matrix &X, Matrix &U);
31 |
32 | private:
33 | Matrix X_Est{SS_X_LEN, 1};
34 | Matrix P{SS_X_LEN, SS_X_LEN};
35 | Matrix F{SS_X_LEN, SS_X_LEN};
36 | Matrix H{SS_Z_LEN, SS_X_LEN};
37 | Matrix Y_Est{SS_Z_LEN, 1};
38 | Matrix Err{SS_Z_LEN, 1};
39 | Matrix Q{SS_X_LEN, SS_X_LEN};
40 | Matrix R{SS_Z_LEN, SS_Z_LEN};
41 | Matrix S{SS_Z_LEN, SS_Z_LEN};
42 | Matrix Gain{SS_X_LEN, SS_Z_LEN};
43 | };
44 |
45 | #endif // EKF_H
46 |
--------------------------------------------------------------------------------
/ahrs_ekf/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 | #define PAKAI_MAGNET
15 |
16 |
17 | /* State Space dimension */
18 | #define SS_X_LEN (4)
19 | #define SS_Z_LEN (6)
20 | #define SS_U_LEN (3)
21 | #define SS_DT_MILIS (20) /* 10 ms */
22 | #define SS_DT float_prec(SS_DT_MILIS/1000.) /* Sampling time */
23 |
24 |
25 | /* Change this size based on the biggest matrix you will use */
26 | #define MATRIX_MAXIMUM_SIZE (6)
27 |
28 | /* Define this to enable matrix bound checking */
29 | #define MATRIX_USE_BOUND_CHECKING
30 |
31 | /* Set this define to choose math precision of the system */
32 | #define PRECISION_SINGLE 1
33 | #define PRECISION_DOUBLE 2
34 | #define FPU_PRECISION (PRECISION_SINGLE)
35 |
36 | #if (FPU_PRECISION == PRECISION_SINGLE)
37 | #define float_prec float
38 | #define float_prec_ZERO (1e-7)
39 | #define float_prec_ZERO_ECO (1e-5) /* 'Economical' zero, for noisy calculation where 'somewhat zero' is good enough */
40 | #elif (FPU_PRECISION == PRECISION_DOUBLE)
41 | #define float_prec double
42 | #define float_prec_ZERO (1e-13)
43 | #define float_prec_ZERO_ECO (1e-8) /* 'Economical' zero, for noisy calculation where 'somewhat zero' is good enough */
44 | #else
45 | #error("FPU_PRECISION has not been defined!");
46 | #endif
47 |
48 |
49 |
50 | /* Set this define to choose system implementation (mainly used to define how you print the matrix via the Matrix::vCetak() function) */
51 | #define SYSTEM_IMPLEMENTATION_PC 1
52 | #define SYSTEM_IMPLEMENTATION_EMBEDDED_CUSTOM 2
53 | #define SYSTEM_IMPLEMENTATION_EMBEDDED_ARDUINO 3
54 |
55 | #define SYSTEM_IMPLEMENTATION (SYSTEM_IMPLEMENTATION_EMBEDDED_ARDUINO)
56 |
57 |
58 |
59 | /* ASSERT is evaluated locally (without function call) to lower the computation cost */
60 | void SPEW_THE_ERROR(char const * str);
61 | #define ASSERT(truth, str) { if (!(truth)) SPEW_THE_ERROR(str); }
62 |
63 |
64 | #endif // KONFIG_H
65 |
--------------------------------------------------------------------------------
/ahrs_ekf/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 |
12 |
13 | Matrix operator + (const float_prec _scalar, Matrix _mat)
14 | {
15 | Matrix _outp(_mat.i16getRow(), _mat.i16getCol(), Matrix::NoInitMatZero);
16 |
17 | for (int16_t _i = 0; _i < _mat.i16getRow(); _i++) {
18 | for (int16_t _j = 0; _j < _mat.i16getCol(); _j++) {
19 | _outp[_i][_j] = _scalar + _mat[_i][_j];
20 | }
21 | }
22 | return _outp;
23 | }
24 |
25 |
26 | Matrix operator - (const float_prec _scalar, Matrix _mat)
27 | {
28 | Matrix _outp(_mat.i16getRow(), _mat.i16getCol(), Matrix::NoInitMatZero);
29 |
30 | for (int16_t _i = 0; _i < _mat.i16getRow(); _i++) {
31 | for (int16_t _j = 0; _j < _mat.i16getCol(); _j++) {
32 | _outp[_i][_j] = _scalar - _mat[_i][_j];
33 | }
34 | }
35 | return _outp;
36 | }
37 |
38 |
39 | Matrix operator * (const float_prec _scalar, Matrix _mat)
40 | {
41 | Matrix _outp(_mat.i16getRow(), _mat.i16getCol(), Matrix::NoInitMatZero);
42 |
43 | for (int16_t _i = 0; _i < _mat.i16getRow(); _i++) {
44 | for (int16_t _j = 0; _j < _mat.i16getCol(); _j++) {
45 | _outp[_i][_j] = _scalar * _mat[_i][_j];
46 | }
47 | }
48 | return _outp;
49 | }
50 |
51 |
52 | Matrix operator + (Matrix _mat, const float_prec _scalar)
53 | {
54 | Matrix _outp(_mat.i16getRow(), _mat.i16getCol(), Matrix::NoInitMatZero);
55 |
56 | for (int16_t _i = 0; _i < _mat.i16getRow(); _i++) {
57 | for (int16_t _j = 0; _j < _mat.i16getCol(); _j++) {
58 | _outp[_i][_j] = _mat[_i][_j] + _scalar;
59 | }
60 | }
61 | return _outp;
62 | }
63 |
64 |
65 | Matrix operator - (Matrix _mat, const float_prec _scalar)
66 | {
67 | Matrix _outp(_mat.i16getRow(), _mat.i16getCol(), Matrix::NoInitMatZero);
68 |
69 | for (int16_t _i = 0; _i < _mat.i16getRow(); _i++) {
70 | for (int16_t _j = 0; _j < _mat.i16getCol(); _j++) {
71 | _outp[_i][_j] = _mat[_i][_j] - _scalar;
72 | }
73 | }
74 | return _outp;
75 | }
76 |
77 |
78 | Matrix operator * (Matrix _mat, const float_prec _scalar)
79 | {
80 | Matrix _outp(_mat.i16getRow(), _mat.i16getCol(), Matrix::NoInitMatZero);
81 |
82 | for (int16_t _i = 0; _i < _mat.i16getRow(); _i++) {
83 | for (int16_t _j = 0; _j < _mat.i16getCol(); _j++) {
84 | _outp[_i][_j] = _mat[_i][_j] * _scalar;
85 | }
86 | }
87 | return _outp;
88 | }
89 |
90 |
91 | Matrix operator / (Matrix _mat, const float_prec _scalar)
92 | {
93 | Matrix _outp(_mat.i16getRow(), _mat.i16getCol(), Matrix::NoInitMatZero);
94 |
95 | if (fabs(_scalar) < float_prec(float_prec_ZERO)) {
96 | _outp.vSetMatrixInvalid();
97 | return _outp;
98 | }
99 | for (int16_t _i = 0; _i < _mat.i16getRow(); _i++) {
100 | for (int16_t _j = 0; _j < _mat.i16getCol(); _j++) {
101 | _outp[_i][_j] = _mat[_i][_j] / _scalar;
102 | }
103 | }
104 | return _outp;
105 | }
106 |
107 |
--------------------------------------------------------------------------------
/ahrs_ekf/matrix.h:
--------------------------------------------------------------------------------
1 | /************************************************************************************
2 | * Matrix Class
3 | * Contain the matrix class definition and operation.
4 | *
5 | * Notes:
6 | * - Indexing start from 0, with accessing format matrix[row][column].
7 | * - The matrix data is a 2 dimensional array, with structure:
8 | * -> 0 <= i16row <= (MATRIX_MAXIMUM_SIZE-1)
9 | * -> 0 <= i16col <= (MATRIX_MAXIMUM_SIZE-1)
10 | * -> f32data[MATRIX_MAXIMUM_SIZE][MATRIX_MAXIMUM_SIZE] is the memory
11 | * representation of the matrix. We only use the first i16row-th
12 | * and first i16col-th memory for the matrix data. The rest is unused.
13 | * See below at "Data structure of Matrix class" at private member class
14 | * definition for more information!
15 | *
16 | * Class Matrix Versioning:
17 | * v0.8 (2020-03-26), {PNb}:
18 | * - Change indexing from int32_t to int16_t.
19 | * - Add way to initialize matrix with existing float_prec array.
20 | * - Add enum InitZero.
21 | * - Make temporary matrix initialization inside almost all method with
22 | * NoInitMatZero argument.
23 | * - Remove the 1 index buffer reserve in bMatrixIsValid function.
24 | * - Add bMatrixIsPositiveDefinite method to check the positive
25 | * (semi)definiteness of a matrix.
26 | * - Add GetDiagonalEntries method.
27 | * - Change SYSTEM_IMPLEMENTATION_EMBEDDED_NO_PRINT into
28 | * SYSTEM_IMPLEMENTATION_EMBEDDED_CUSTOM, and make vPrint and
29 | * vPrintFull as function declaration (the user must define that
30 | * function somewhere).
31 | *
32 | * v0.7 (2020-02-23), {PNb}:
33 | * - Make the matrix class interface in English (at long last, yay?).
34 | *
35 | *
36 | *** Documentation below is for tracking purpose *************************************
37 | *
38 | * v0.6 (2020-01-16), {PNb}:
39 | * - Tambahkan sanity check saat pengecekan MATRIX_PAKAI_BOUND_CHECKING
40 | * dengan membandingkan baris & kolom dengan MATRIX_MAXIMUM_SIZE.
41 | * - Menambahkan pengecekan matrix untuk operasi dasar antar matrix (*,+,-).
42 | *
43 | * v0.5 (2020-01-14), {PNb}:
44 | * - Buat file matrix.cpp (akhirnya!) untuk definisi fungsi di luar class.
45 | * - Tambahkan operator overloading untuk operasi negatif matrix (mis. a = -b).
46 | * - Tambahkan operator overloading untuk operasi penjumlahan & pengurangan
47 | * dengan scalar.
48 | * - Ubah evaluasi MATRIX_PAKAI_BOUND_CHECKING menggunakan ASSERT.
49 | * - Tambahkan pengecekan index selalu positif di MATRIX_PAKAI_BOUND_CHECKING.
50 | *
51 | * v0.4 (2020-01-10), {PNb}:
52 | * - Tambahkan rounding to zero sebelum operasi sqrt(x) untuk menghindari
53 | * kasus x = 0-
54 | * - Fungsi QRDec mengembalikan Q' dan R (user perlu melakukan transpose
55 | * lagi setelah memanggil QRDec untuk mendapatkan Q).
56 | * - Menambahkan pengecekan hasil HouseholderTransformQR di dalam QRDec.
57 | * - Tambah warning jika MATRIX_PAKAI_BOUND_CHECKING dinonaktifkan.
58 | *
59 | * v0.3_engl (2019-12-31), {PNb}:
60 | * - Modifikasi dokumentasi kode buat orang asing.
61 | *
62 | * v0.3 (2019-12-25), {PNb}:
63 | * - Menambahkan fungsi back subtitution untuk menyelesaikan permasalahan
64 | * persamaan linear Ax = B. Dengan A matrix segitiga atas & B vektor.
65 | * - Memperbaiki bug pengecekan MATRIX_PAKAI_BOUND_CHECKING pada indexing kolom.
66 | * - Menambahkan fungsi QR Decomposition (via Householder Transformation).
67 | * - Menambahkan fungsi Householder Transformation.
68 | * - Menghilangkan warning 'implicit conversion' untuk operasi pembandingan
69 | * dengan float_prec_ZERO.
70 | * - Menambahkan function overloading operasi InsertSubMatrix, untuk
71 | * operasi insert dari SubMatrix ke SubMatrix.
72 | * - Saat inisialisasi, matrix diisi nol (melalui vIsiHomogen(0.0)).
73 | * - Menambahkan function overloading operator '/' dengan scalar.
74 | *
75 | * v0.2 (2019-11-30), {PNb}:
76 | * - Fungsi yang disupport:
77 | * - Operator ==
78 | * - Normalisasi matrix
79 | * - Cholesky Decomposition
80 | * - InsertSubMatrix
81 | * - InsertVector
82 | *
83 | * v0.1 (2019-11-29), {PNb}:
84 | * - Fungsi yang disupport:
85 | * - Operasi matrix dasar
86 | * - Invers
87 | * - Cetak
88 | *
89 | * See https://github.com/pronenewbits for more!
90 | *************************************************************************************/
91 | #ifndef MATRIX_H
92 | #define MATRIX_H
93 |
94 | #include "konfig.h"
95 |
96 | #if (SYSTEM_IMPLEMENTATION == SYSTEM_IMPLEMENTATION_PC)
97 | #include
98 | #include // std::setprecision
99 |
100 | using namespace std;
101 | #elif (SYSTEM_IMPLEMENTATION == SYSTEM_IMPLEMENTATION_EMBEDDED_ARDUINO)
102 | #include
103 | #endif
104 |
105 |
106 | class Matrix
107 | {
108 | public:
109 | typedef enum {
110 | InitMatWithZero, /* Initialize matrix with zero */
111 | NoInitMatZero
112 | } InitZero;
113 |
114 | Matrix(const int16_t _i16row, const int16_t _i16col, InitZero _init = InitMatWithZero)
115 | {
116 | this->i16row = _i16row;
117 | this->i16col = _i16col;
118 |
119 | if (_init == InitMatWithZero) {
120 | this->vSetHomogen(0.0);
121 | }
122 | }
123 | Matrix(const int16_t _i16row, const int16_t _i16col, float_prec * initData, InitZero _init = InitMatWithZero)
124 | {
125 | this->i16row = _i16row;
126 | this->i16col = _i16col;
127 |
128 | if (_init == InitMatWithZero) {
129 | this->vSetHomogen(0.0);
130 | }
131 | for (int16_t _i = 0; _i < this->i16row; _i++) {
132 | for (int16_t _j = 0; _j < this->i16col; _j++) {
133 | (*this)[_i][_j] = *initData;
134 | initData++;
135 | }
136 | }
137 | }
138 |
139 | bool bMatrixIsValid() {
140 | /* Check whether the matrix is valid or not.
141 | *
142 | * Index is for buffer if there's some internal rouge code with 1 index buffer overflow
143 | */
144 | if ((this->i16row > 0) && (this->i16row <= MATRIX_MAXIMUM_SIZE) && (this->i16col > 0) && (this->i16col <= MATRIX_MAXIMUM_SIZE)) {
145 | return true;
146 | } else {
147 | return false;
148 | }
149 | }
150 |
151 | void vSetMatrixInvalid() {
152 | this->i16row = -1;
153 | this->i16col = -1;
154 | }
155 |
156 | bool bMatrixIsSquare() {
157 | return (this->i16row == this->i16col);
158 | }
159 |
160 | int16_t i16getRow() { return this->i16row; }
161 | int16_t i16getCol() { return this->i16col; }
162 |
163 | /* Ref: https://stackoverflow.com/questions/6969881/operator-overload */
164 | class Proxy {
165 | public:
166 | Proxy(float_prec* _array, int16_t _maxColumn) : _array(_array) { this->_maxColumn = _maxColumn; }
167 |
168 | /* Modify to be lvalue modifiable, ref:
169 | * https://stackoverflow.com/questions/6969881/operator-overload#comment30831582_6969904
170 | * (I know this is so dirty, but it makes the code so FABULOUS :D)
171 | */
172 | float_prec & operator[](int16_t _column) {
173 | #if (defined(MATRIX_USE_BOUND_CHECKING))
174 | ASSERT((_column >= 0) && (_column < this->_maxColumn) && (_column < MATRIX_MAXIMUM_SIZE), "Matrix index out-of-bounds (at column evaluation)");
175 | #else
176 | #warning("Matrix bounds checking is disabled... good luck >:3");
177 | #endif
178 | return _array[_column];
179 | }
180 | private:
181 | float_prec* _array;
182 | int16_t _maxColumn;
183 | };
184 | Proxy operator[](int16_t _row) {
185 | #if (defined(MATRIX_USE_BOUND_CHECKING))
186 | ASSERT((_row >= 0) && (_row < this->i16row) && (_row < MATRIX_MAXIMUM_SIZE), "Matrix index out-of-bounds (at row evaluation)");
187 | #else
188 | #warning("Matrix bounds checking is disabled... good luck >:3");
189 | #endif
190 | return Proxy(f32data[_row], this->i16col); /* Parsing column index for bound checking */
191 | }
192 |
193 | bool operator == (Matrix _compare) {
194 | if ((this->i16row != _compare.i16row) || (this->i16col != _compare.i16getCol())) {
195 | return false;
196 | }
197 |
198 | for (int16_t _i = 0; _i < this->i16row; _i++) {
199 | for (int16_t _j = 0; _j < this->i16col; _j++) {
200 | if (fabs((*this)[_i][_j] - _compare[_i][_j]) > float_prec(float_prec_ZERO)) {
201 | return false;
202 | }
203 | }
204 | }
205 | return true;
206 | }
207 |
208 | Matrix operator + (Matrix _matAdd) {
209 | Matrix _outp(this->i16row, this->i16col, NoInitMatZero);
210 | if ((this->i16row != _matAdd.i16row) || (this->i16col != _matAdd.i16col)) {
211 | _outp.vSetMatrixInvalid();
212 | return _outp;
213 | }
214 |
215 | for (int16_t _i = 0; _i < this->i16row; _i++) {
216 | for (int16_t _j = 0; _j < this->i16col; _j++) {
217 | _outp[_i][_j] = (*this)[_i][_j] + _matAdd[_i][_j];
218 | }
219 | }
220 | return _outp;
221 | }
222 |
223 | Matrix operator - (Matrix _matSub) {
224 | Matrix _outp(this->i16row, this->i16col, NoInitMatZero);
225 | if ((this->i16row != _matSub.i16row) || (this->i16col != _matSub.i16col)) {
226 | _outp.vSetMatrixInvalid();
227 | return _outp;
228 | }
229 |
230 | for (int16_t _i = 0; _i < this->i16row; _i++) {
231 | for (int16_t _j = 0; _j < this->i16col; _j++) {
232 | _outp[_i][_j] = (*this)[_i][_j] - _matSub[_i][_j];
233 | }
234 | }
235 | return _outp;
236 | }
237 |
238 | Matrix operator - (void) {
239 | Matrix _outp(this->i16row, this->i16col, NoInitMatZero);
240 |
241 | for (int16_t _i = 0; _i < this->i16row; _i++) {
242 | for (int16_t _j = 0; _j < this->i16col; _j++) {
243 | _outp[_i][_j] = -(*this)[_i][_j];
244 | }
245 | }
246 | return _outp;
247 | }
248 |
249 | Matrix operator * (Matrix _matMul) {
250 | Matrix _outp(this->i16row, _matMul.i16col, NoInitMatZero);
251 | if ((this->i16col != _matMul.i16row)) {
252 | _outp.vSetMatrixInvalid();
253 | return _outp;
254 | }
255 |
256 | for (int16_t _i = 0; _i < this->i16row; _i++) {
257 | for (int16_t _j = 0; _j < _matMul.i16col; _j++) {
258 | _outp[_i][_j] = 0.0;
259 | for (int16_t _k = 0; _k < this->i16col; _k++) {
260 | _outp[_i][_j] += ((*this)[_i][_k] * _matMul[_k][_j]);
261 | }
262 | }
263 | }
264 | return _outp;
265 | }
266 |
267 | void vRoundingElementToZero(const int16_t _i, const int16_t _j) {
268 | if (fabs((*this)[_i][_j]) < float_prec(float_prec_ZERO)) {
269 | (*this)[_i][_j] = 0.0;
270 | }
271 | }
272 |
273 | Matrix RoundingMatrixToZero() {
274 | for (int16_t _i = 0; _i < this->i16row; _i++) {
275 | for (int16_t _j = 0; _j < this->i16col; _j++) {
276 | if (fabs((*this)[_i][_j]) < float_prec(float_prec_ZERO)) {
277 | (*this)[_i][_j] = 0.0;
278 | }
279 | }
280 | }
281 | return (*this);
282 | }
283 |
284 | void vSetHomogen(const float_prec _val) {
285 | for (int16_t _i = 0; _i < this->i16row; _i++) {
286 | for (int16_t _j = 0; _j < this->i16col; _j++) {
287 | (*this)[_i][_j] = _val;
288 | }
289 | }
290 | }
291 |
292 | void vSetToZero() {
293 | this->vSetHomogen(0.0);
294 | }
295 |
296 | void vSetRandom(const int32_t _maxRand, const int32_t _minRand) {
297 | for (int16_t _i = 0; _i < this->i16row; _i++) {
298 | for (int16_t _j = 0; _j < this->i16col; _j++) {
299 | (*this)[_i][_j] = float_prec((rand() % (_maxRand - _minRand + 1)) + _minRand);
300 | }
301 | }
302 | }
303 |
304 | void vSetDiag(const float_prec _val) {
305 | for (int16_t _i = 0; _i < this->i16row; _i++) {
306 | for (int16_t _j = 0; _j < this->i16col; _j++) {
307 | if (_i == _j) {
308 | (*this)[_i][_j] = _val;
309 | } else {
310 | (*this)[_i][_j] = 0.0;
311 | }
312 | }
313 | }
314 | }
315 |
316 | void vSetIdentity() {
317 | this->vSetDiag(1.0);
318 | }
319 |
320 | /* Insert vector into matrix at _posColumn position
321 | * Example: A = Matrix 3x3, B = Vector 3x1
322 | *
323 | * C = A.InsertVector(B, 1);
324 | *
325 | * A = [A00 A01 A02] B = [B00]
326 | * [A10 A11 A12] [B10]
327 | * [A20 A21 A22] [B20]
328 | *
329 | * C = [A00 B00 A02]
330 | * [A10 B10 A12]
331 | * [A20 B20 A22]
332 | */
333 | Matrix InsertVector(Matrix _Vector, const int16_t _posColumn) {
334 | Matrix _outp(this->i16col, this->i16row, NoInitMatZero);
335 | if ((_Vector.i16row > this->i16row) || (_Vector.i16col+_posColumn > this->i16col)) {
336 | /* Return false */
337 | _outp.vSetMatrixInvalid();
338 | return _outp;
339 | }
340 | _outp = this->Copy();
341 | for (int16_t _i = 0; _i < _Vector.i16row; _i++) {
342 | _outp[_i][_posColumn] = _Vector[_i][0];
343 | }
344 | return _outp;
345 | }
346 |
347 | /* Insert submatrix into matrix at _posRow & _posColumn position
348 | * Example: A = Matrix 4x4, B = Matrix 2x3
349 | *
350 | * C = A.InsertSubMatrix(B, 1, 1);
351 | *
352 | * A = [A00 A01 A02 A03] B = [B00 B01 B02]
353 | * [A10 A11 A12 A13] [B10 B11 B12]
354 | * [A20 A21 A22 A23]
355 | * [A30 A31 A32 A33]
356 | *
357 | *
358 | * C = [A00 A01 A02 A03]
359 | * [A10 B00 B01 B02]
360 | * [A20 B10 B11 B12]
361 | * [A30 A31 A32 A33]
362 | */
363 | Matrix InsertSubMatrix(Matrix _subMatrix, const int16_t _posRow, const int16_t _posColumn) {
364 | Matrix _outp(this->i16col, this->i16row, NoInitMatZero);
365 | if (((_subMatrix.i16row+_posRow) > this->i16row) || ((_subMatrix.i16col+_posColumn) > this->i16col)) {
366 | /* Return false */
367 | _outp.vSetMatrixInvalid();
368 | return _outp;
369 | }
370 | _outp = this->Copy();
371 | for (int16_t _i = 0; _i < _subMatrix.i16row; _i++) {
372 | for (int16_t _j = 0; _j < _subMatrix.i16col; _j++) {
373 | _outp[_i + _posRow][_j + _posColumn] = _subMatrix[_i][_j];
374 | }
375 | }
376 | return _outp;
377 | }
378 |
379 | /* Insert the first _lenRow-th and first _lenColumn-th submatrix into matrix; at the matrix's _posRow and _posColumn position.
380 | * Example: A = Matrix 4x4, B = Matrix 2x3
381 | *
382 | * C = A.InsertSubMatrix(B, 1, 1, 2, 2);
383 | *
384 | * A = [A00 A01 A02 A03] B = [B00 B01 B02]
385 | * [A10 A11 A12 A13] [B10 B11 B12]
386 | * [A20 A21 A22 A23]
387 | * [A30 A31 A32 A33]
388 | *
389 | *
390 | * C = [A00 A01 A02 A03]
391 | * [A10 B00 B01 A13]
392 | * [A20 B10 B11 A23]
393 | * [A30 A31 A32 A33]
394 | */
395 | Matrix InsertSubMatrix(Matrix _subMatrix, const int16_t _posRow, const int16_t _posColumn, const int16_t _lenRow, const int16_t _lenColumn) {
396 | Matrix _outp(this->i16col, this->i16row, NoInitMatZero);
397 | if (((_lenRow+_posRow) > this->i16row) || ((_lenColumn+_posColumn) > this->i16col) || (_lenRow > _subMatrix.i16row) || (_lenColumn > _subMatrix.i16col)) {
398 | /* Return false */
399 | _outp.vSetMatrixInvalid();
400 | return _outp;
401 | }
402 | _outp = this->Copy();
403 | for (int16_t _i = 0; _i < _lenRow; _i++) {
404 | for (int16_t _j = 0; _j < _lenColumn; _j++) {
405 | _outp[_i + _posRow][_j + _posColumn] = _subMatrix[_i][_j];
406 | }
407 | }
408 | return _outp;
409 | }
410 |
411 | /* Insert the _lenRow & _lenColumn submatrix, start from _posRowSub & _posColumnSub submatrix;
412 | * into matrix at the matrix's _posRow and _posColumn position.
413 | *
414 | * Example: A = Matrix 4x4, B = Matrix 2x3
415 | *
416 | * C = A.InsertSubMatrix(B, 1, 1, 0, 1, 1, 2);
417 | *
418 | * A = [A00 A01 A02 A03] B = [B00 B01 B02]
419 | * [A10 A11 A12 A13] [B10 B11 B12]
420 | * [A20 A21 A22 A23]
421 | * [A30 A31 A32 A33]
422 | *
423 | *
424 | * C = [A00 A01 A02 A03]
425 | * [A10 B01 B02 A13]
426 | * [A20 A21 A22 A23]
427 | * [A30 A31 A32 A33]
428 | */
429 | Matrix InsertSubMatrix(Matrix _subMatrix, const int16_t _posRow, const int16_t _posColumn,
430 | const int16_t _posRowSub, const int16_t _posColumnSub,
431 | const int16_t _lenRow, const int16_t _lenColumn) {
432 | Matrix _outp(this->i16col, this->i16row, NoInitMatZero);
433 | if (((_lenRow+_posRow) > this->i16row) || ((_lenColumn+_posColumn) > this->i16col) ||
434 | ((_posRowSub+_lenRow) > _subMatrix.i16row) || ((_posColumnSub+_lenColumn) > _subMatrix.i16col))
435 | {
436 | /* Return false */
437 | _outp.vSetMatrixInvalid();
438 | return _outp;
439 | }
440 | _outp = this->Copy();
441 | for (int16_t _i = 0; _i < _lenRow; _i++) {
442 | for (int16_t _j = 0; _j < _lenColumn; _j++) {
443 | _outp[_i + _posRow][_j + _posColumn] = _subMatrix[_posRowSub+_i][_posColumnSub+_j];
444 | }
445 | }
446 | return _outp;
447 | }
448 |
449 | /* Return the transpose of the matrix */
450 | Matrix Transpose() {
451 | Matrix _outp(this->i16col, this->i16row, NoInitMatZero);
452 | for (int16_t _i = 0; _i < this->i16row; _i++) {
453 | for (int16_t _j = 0; _j < this->i16col; _j++) {
454 | _outp[_j][_i] = (*this)[_i][_j];
455 | }
456 | }
457 | return _outp;
458 | }
459 |
460 | /* Normalize the vector */
461 | bool bNormVector() {
462 | float_prec _normM = 0.0;
463 | for (int16_t _i = 0; _i < this->i16row; _i++) {
464 | for (int16_t _j = 0; _j < this->i16col; _j++) {
465 | _normM = _normM + ((*this)[_i][_j] * (*this)[_i][_j]);
466 | }
467 | }
468 |
469 | if (_normM < float_prec(float_prec_ZERO)) {
470 | return false;
471 | }
472 | /* Rounding to zero to avoid case where sqrt(0-) */
473 | if (fabs(_normM) < float_prec(float_prec_ZERO)) {
474 | _normM = 0.0;
475 | }
476 | _normM = sqrt(_normM);
477 | for (int16_t _i = 0; _i < this->i16row; _i++) {
478 | for (int16_t _j = 0; _j < this->i16col; _j++) {
479 | (*this)[_i][_j] /= _normM;
480 | }
481 | }
482 | return true;
483 | }
484 |
485 | Matrix Copy() {
486 | Matrix _outp(this->i16row, this->i16col, NoInitMatZero);
487 | for (int16_t _i = 0; _i < this->i16row; _i++) {
488 | for (int16_t _j = 0; _j < this->i16col; _j++) {
489 | _outp[_i][_j] = (*this)[_i][_j];
490 | }
491 | }
492 | return _outp;
493 | }
494 |
495 | /* Invers operation using Gauss-Jordan algorithm */
496 | Matrix Invers() {
497 | Matrix _outp(this->i16row, this->i16col, NoInitMatZero);
498 | Matrix _temp(this->i16row, this->i16col, NoInitMatZero);
499 | _outp.vSetIdentity();
500 | _temp = this->Copy();
501 |
502 |
503 | /* Gauss Elimination... */
504 | for (int16_t _j = 0; _j < (_temp.i16row)-1; _j++) {
505 | for (int16_t _i = _j+1; _i < _temp.i16row; _i++) {
506 | if (fabs(_temp[_j][_j]) < float_prec(float_prec_ZERO)) {
507 | /* Matrix is non-invertible */
508 | _outp.vSetMatrixInvalid();
509 | return _outp;
510 | }
511 |
512 | float_prec _tempfloat = _temp[_i][_j] / _temp[_j][_j];
513 |
514 | for (int16_t _k = 0; _k < _temp.i16col; _k++) {
515 | _temp[_i][_k] -= (_temp[_j][_k] * _tempfloat);
516 | _outp[_i][_k] -= (_outp[_j][_k] * _tempfloat);
517 |
518 | _temp.vRoundingElementToZero(_i, _k);
519 | _outp.vRoundingElementToZero(_i, _k);
520 | }
521 |
522 | }
523 | }
524 |
525 | #if (1)
526 | /* At here, the _temp matrix should be an upper triangular matrix.
527 | * But because rounding error, it might not.
528 | */
529 | for (int16_t _i = 1; _i < _temp.i16row; _i++) {
530 | for (int16_t _j = 0; _j < _i; _j++) {
531 | _temp[_i][_j] = 0.0;
532 | }
533 | }
534 | #endif
535 |
536 |
537 | /* Jordan... */
538 | for (int16_t _j = (_temp.i16row)-1; _j > 0; _j--) {
539 | for (int16_t _i = _j-1; _i >= 0; _i--) {
540 | if (fabs(_temp[_j][_j]) < float_prec(float_prec_ZERO)) {
541 | /* Matrix is non-invertible */
542 | _outp.vSetMatrixInvalid();
543 | return _outp;
544 | }
545 |
546 | float_prec _tempfloat = _temp[_i][_j] / _temp[_j][_j];
547 | _temp[_i][_j] -= (_temp[_j][_j] * _tempfloat);
548 | _temp.vRoundingElementToZero(_i, _j);
549 |
550 | for (int16_t _k = (_temp.i16row - 1); _k >= 0; _k--) {
551 | _outp[_i][_k] -= (_outp[_j][_k] * _tempfloat);
552 | _outp.vRoundingElementToZero(_i, _k);
553 | }
554 | }
555 | }
556 |
557 |
558 | /* Normalization */
559 | for (int16_t _i = 0; _i < _temp.i16row; _i++) {
560 | if (fabs(_temp[_i][_i]) < float_prec(float_prec_ZERO)) {
561 | /* Matrix is non-invertible */
562 | _outp.vSetMatrixInvalid();
563 | return _outp;
564 | }
565 |
566 | float_prec _tempfloat = _temp[_i][_i];
567 | _temp[_i][_i] = 1.0;
568 |
569 | for (int16_t _j = 0; _j < _temp.i16row; _j++) {
570 | _outp[_i][_j] /= _tempfloat;
571 | }
572 | }
573 | return _outp;
574 | }
575 |
576 | /* Use elemtary row operation to reduce the matrix into upper triangular form (like in the first phase of gauss-jordan algorithm).
577 | *
578 | * Useful if we want to check the matrix as positive definite or not (can be used before calling CholeskyDec function).
579 | */
580 | bool bMatrixIsPositiveDefinite(bool checkPosSemidefinite = false) {
581 | bool _posDef, _posSemiDef;
582 | Matrix _temp(this->i16row, this->i16col, NoInitMatZero);
583 | _temp = this->Copy();
584 |
585 | /* Gauss Elimination... */
586 | for (int16_t _j = 0; _j < (_temp.i16row)-1; _j++) {
587 | for (int16_t _i = _j+1; _i < _temp.i16row; _i++) {
588 | if (fabs(_temp[_j][_j]) < float_prec(float_prec_ZERO)) {
589 | /* Q: Do we still need to check this?
590 | * A: idk, It's 3 AM here.
591 | *
592 | * NOTE TO FUTURE SELF: Confirm it!
593 | */
594 | return false;
595 | }
596 |
597 | float_prec _tempfloat = _temp[_i][_j] / _temp[_j][_j];
598 |
599 | for (int16_t _k = 0; _k < _temp.i16col; _k++) {
600 | _temp[_i][_k] -= (_temp[_j][_k] * _tempfloat);
601 | _temp.vRoundingElementToZero(_i, _k);
602 | }
603 |
604 | }
605 | }
606 |
607 | _posDef = true;
608 | _posSemiDef = true;
609 | for (int16_t _i = 0; _i < _temp.i16row; _i++) {
610 | if (_temp[_i][_i] < float_prec(float_prec_ZERO)) { /* false if less than 0+ (zero included) */
611 | _posDef = false;
612 | }
613 | if (_temp[_i][_i] < -float_prec(float_prec_ZERO)) { /* false if less than 0- (zero is not included) */
614 | _posSemiDef = false;
615 | }
616 | }
617 |
618 | if (checkPosSemidefinite) {
619 | return _posSemiDef;
620 | } else {
621 | return _posDef;
622 | }
623 | }
624 |
625 |
626 | /* For square matrix 'this' with size MxM, return vector Mx1 with entries corresponding with diagonal entries of 'this'.
627 | * Example: this = [a11 a12 a13]
628 | * [a21 a22 a23]
629 | * [a31 a32 a33]
630 | *
631 | * out = this.GetDiagonalEntries() = [a11]
632 | * [a22]
633 | * [a33]
634 | */
635 | Matrix GetDiagonalEntries(void) {
636 | Matrix _temp(this->i16row, 1, NoInitMatZero);
637 |
638 | if (this->i16row != this->i16col) {
639 | _temp.vSetMatrixInvalid();
640 | return _temp;
641 | }
642 | for (int16_t _i = 0; _i < this->i16row; _i++) {
643 | _temp[_i][0] = (*this)[_i][_i];
644 | }
645 | return _temp;
646 | }
647 |
648 | /* Do the Cholesky Decomposition using Cholesky-Crout algorithm.
649 | *
650 | * A = L*L' ; A = real, positive definite, and symmetry MxM matrix
651 | *
652 | * L = A.CholeskyDec();
653 | *
654 | * CATATAN! NOTE! The symmetry property is not checked at the beginning to lower
655 | * the computation cost. The processing is being done on the lower triangular
656 | * component of _A. Then it is assumed the upper triangular is inherently
657 | * equal to the lower end.
658 | * (as a side note, Scilab & MATLAB is using Lapack routines DPOTRF that process
659 | * the upper triangular of _A. The result should be equal mathematically if A
660 | * is symmetry).
661 | */
662 | Matrix CholeskyDec()
663 | {
664 | float_prec _tempFloat;
665 |
666 | Matrix _outp(this->i16row, this->i16col, NoInitMatZero);
667 | if (this->i16row != this->i16col) {
668 | _outp.vSetMatrixInvalid();
669 | return _outp;
670 | }
671 | _outp.vSetHomogen(0.0);
672 | for (int16_t _j = 0; _j < this->i16col; _j++) {
673 | for (int16_t _i = _j; _i < this->i16row; _i++) {
674 | _tempFloat = (*this)[_i][_j];
675 | if (_i == _j) {
676 | for (int16_t _k = 0; _k < _j; _k++) {
677 | _tempFloat = _tempFloat - (_outp[_i][_k] * _outp[_i][_k]);
678 | }
679 | if (_tempFloat < float_prec(float_prec_ZERO)) {
680 | /* Matrix is not positif definit */
681 | _outp.vSetMatrixInvalid();
682 | return _outp;
683 | }
684 | /* Rounding to zero to avoid case where sqrt(0-) */
685 | if (fabs(_tempFloat) < float_prec(float_prec_ZERO)) {
686 | _tempFloat = 0.0;
687 | }
688 | _outp[_i][_i] = sqrt(_tempFloat);
689 | } else {
690 | for (int16_t _k = 0; _k < _j; _k++) {
691 | _tempFloat = _tempFloat - (_outp[_i][_k] * _outp[_j][_k]);
692 | }
693 | if (fabs(_outp[_j][_j]) < float_prec(float_prec_ZERO)) {
694 | /* Matrix is not positif definit */
695 | _outp.vSetMatrixInvalid();
696 | return _outp;
697 | }
698 | _outp[_i][_j] = _tempFloat / _outp[_j][_j];
699 | }
700 | }
701 | }
702 | return _outp;
703 | }
704 |
705 | /* Do the Householder Transformation for QR Decomposition operation.
706 | * out = HouseholderTransformQR(A, i, j)
707 | */
708 | Matrix HouseholderTransformQR(const int16_t _rowTransform, const int16_t _columnTransform)
709 | {
710 | float_prec _tempFloat;
711 | float_prec _xLen;
712 | float_prec _x1;
713 | float_prec _u1;
714 | float_prec _vLen2;
715 |
716 | Matrix _outp(this->i16row, this->i16row, NoInitMatZero);
717 | Matrix _vectTemp(this->i16row, 1, NoInitMatZero);
718 | if ((_rowTransform >= this->i16row) || (_columnTransform >= this->i16col)) {
719 | _outp.vSetMatrixInvalid();
720 | return _outp;
721 | }
722 |
723 | /* Until here:
724 | *
725 | * _xLen = ||x|| = sqrt(x1^2 + x2^2 + .. + xn^2)
726 | * _vLen2 = ||u||^2 - (u1^2) = x2^2 + .. + xn^2
727 | * _vectTemp= [0 0 0 .. x1=0 x2 x3 .. xn]'
728 | */
729 | _x1 = (*this)[_rowTransform][_columnTransform];
730 | _xLen = _x1*_x1;
731 | _vLen2 = 0.0;
732 | for (int16_t _i = _rowTransform+1; _i < this->i16row; _i++) {
733 | _vectTemp[_i][0] = (*this)[_i][_columnTransform];
734 |
735 | _tempFloat = _vectTemp[_i][0] * _vectTemp[_i][0];
736 | _xLen += _tempFloat;
737 | _vLen2 += _tempFloat;
738 | }
739 | _xLen = sqrt(_xLen);
740 |
741 | /* u1 = x1+(-sign(x1))*xLen */
742 | if (_x1 < 0.0) {
743 | _u1 = _x1+_xLen;
744 | } else {
745 | _u1 = _x1-_xLen;
746 | }
747 |
748 |
749 | /* Solve vlen2 & tempHH */
750 | _vLen2 += (_u1*_u1);
751 | _vectTemp[_rowTransform][0] = _u1;
752 |
753 | if (fabs(_vLen2) < float_prec(float_prec_ZERO)) {
754 | /* x vector is collinear with basis vector e, return result = I */
755 | _outp.vSetIdentity();
756 | } else {
757 | /* P = -2*(u1*u1')/v_len2 + I */
758 | /* PR TODO: We can do many optimization here */
759 | for (int16_t _i = 0; _i < this->i16row; _i++) {
760 | _tempFloat = _vectTemp[_i][0];
761 | if (fabs(_tempFloat) > float_prec(float_prec_ZERO)) {
762 | for (int16_t _j = 0; _j < this->i16row; _j++) {
763 | if (fabs(_vectTemp[_j][0]) > float_prec(float_prec_ZERO)) {
764 | _outp[_i][_j] = _vectTemp[_j][0];
765 | _outp[_i][_j] = _outp[_i][_j] * _tempFloat;
766 | _outp[_i][_j] = _outp[_i][_j] * (-2.0/_vLen2);
767 | }
768 | }
769 | }
770 | _outp[_i][_i] = _outp[_i][_i] + 1.0;
771 | }
772 | }
773 | return _outp;
774 | }
775 |
776 | /* Do the QR Decomposition for matrix using Householder Transformation.
777 | * A = Q*R
778 | *
779 | * PERHATIAN! CAUTION! The matrix calculated by this function return Q' and R (Q transpose and R).
780 | * Because QR Decomposition usually used to calculate solution for least-squares equation (that
781 | * need Q'), we don't do the transpose of Q inside this routine to lower the computation cost).
782 | *
783 | * Example of using QRDec to solve least-squares:
784 | * Ax = b
785 | * (QR)x = b
786 | * Rx = Q'b --> Afterward use back-subtitution to solve x
787 | */
788 | bool QRDec(Matrix &Qt, Matrix &R)
789 | {
790 | Matrix Qn(Qt.i16row, Qt.i16col, NoInitMatZero);
791 | if ((this->i16row < this->i16col) || (!Qt.bMatrixIsSquare()) || (Qt.i16row != this->i16row) || (R.i16row != this->i16row) || (R.i16col != this->i16col)) {
792 | Qt.vSetMatrixInvalid();
793 | R.vSetMatrixInvalid();
794 | return false;
795 | }
796 | R = (*this);
797 | Qt.vSetIdentity();
798 | for (int16_t _i = 0; (_i < (this->i16row - 1)) && (_i < this->i16col-1); _i++) {
799 | Qn = R.HouseholderTransformQR(_i, _i);
800 | if (!Qn.bMatrixIsValid()) {
801 | Qt.vSetMatrixInvalid();
802 | R.vSetMatrixInvalid();
803 | return false;
804 | }
805 | Qt = Qn * Qt;
806 | R = Qn * R;
807 | }
808 | Qt.RoundingMatrixToZero();
809 | /* R.RoundingMatrixToZero(); */
810 | return true;
811 | }
812 |
813 | /* Do the back-subtitution opeartion for upper triangular matrix A & column matrix B to solve x:
814 | * Ax = B
815 | *
816 | * x = BackSubtitution(&A, &B);
817 | *
818 | * CATATAN! NOTE! To lower the computation cost, we don't check that A is a upper triangular
819 | * matrix (it's assumed that user already make sure before calling this routine).
820 | */
821 | Matrix BackSubtitution(Matrix &A, Matrix &B)
822 | {
823 | Matrix _outp(A.i16row, 1, NoInitMatZero);
824 | if ((A.i16row != A.i16col) || (A.i16row != B.i16row)) {
825 | _outp.vSetMatrixInvalid();
826 | return _outp;
827 | }
828 |
829 | for (int16_t _i = A.i16col-1; _i >= 0; _i--) {
830 | _outp[_i][0] = B[_i][0];
831 | for (int16_t _j = _i + 1; _j < A.i16col; _j++) {
832 | _outp[_i][0] = _outp[_i][0] - A[_i][_j]*_outp[_j][0];
833 | }
834 | if (fabs(A[_i][_i]) < float_prec(float_prec_ZERO)) {
835 | _outp.vSetMatrixInvalid();
836 | return _outp;
837 | }
838 | _outp[_i][0] = _outp[_i][0] / A[_i][_i];
839 | }
840 |
841 | return _outp;
842 | }
843 |
844 | #if (0)
845 | /*Not yet tested, but should be working (?)*/
846 |
847 | /* Melakukan operasi Forward-subtitution pada matrix triangular A & matrix kolom B.
848 | * Ax = B
849 | *
850 | * Untuk menghemat komputansi, matrix A tidak dilakukan pengecekan triangular
851 | * (diasumsikan sudah lower-triangular).
852 | */
853 | Matrix ForwardSubtitution(Matrix &A, Matrix &B)
854 | {
855 | Matrix _outp(A.i16row, 1);
856 | if ((A.i16row != A.i16col) || (A.i16row != B.i16row)) {
857 | _outp.vSetMatrixInvalid();
858 | return _outp;
859 | }
860 |
861 | for (int16_t _i = 0; _i < A.i16row; _i++) {
862 | _outp[_i][0] = B[_i][0];
863 | for (int16_t _j = 0; _j < _i; _j++) {
864 | _outp[_i][0] = _outp[_i][0] - A[_i][_j]*_outp[_j][0];
865 | }
866 | if (fabs(A[_i][_i]) < float_prec(float_prec_ZERO)) {
867 | _outp.vSetMatrixInvalid();
868 | return _outp;
869 | }
870 | _outp[_i][0] = _outp[_i][0] / A[_i][_i];
871 | }
872 | return _outp;
873 | }
874 | /*Not yet tested, but should be working (?)*/
875 | #endif
876 |
877 |
878 | /* Printing function -------------------------------------------------------------------------------------------- */
879 | #if (SYSTEM_IMPLEMENTATION == SYSTEM_IMPLEMENTATION_PC)
880 | void vPrint() {
881 | for (int16_t _i = 0; _i < this->i16row; _i++) {
882 | cout << "[ ";
883 | for (int16_t _j = 0; _j < this->i16col; _j++) {
884 | cout << std::fixed << std::setprecision(3) << (*this)[_i][_j] << " ";
885 | }
886 | cout << "]";
887 | cout << endl;
888 | }
889 | cout << endl;
890 | }
891 | void vPrintFull() {
892 | for (int16_t _i = 0; _i < this->i16row; _i++) {
893 | cout << "[ ";
894 | for (int16_t _j = 0; _j < this->i16col; _j++) {
895 | cout << resetiosflags( ios::fixed | ios::showpoint ) << (*this)[_i][_j] << " ";
896 | }
897 | cout << "]";
898 | cout << endl;
899 | }
900 | cout << endl;
901 | }
902 | #elif (SYSTEM_IMPLEMENTATION == SYSTEM_IMPLEMENTATION_EMBEDDED_ARDUINO)
903 | void vPrint() {
904 | char _bufSer[10];
905 | for (int16_t _i = 0; _i < this->i16row; _i++) {
906 | Serial.print("[ ");
907 | for (int16_t _j = 0; _j < this->i16col; _j++) {
908 | snprintf(_bufSer, sizeof(_bufSer)-1, "%2.2f ", (*this)[_i][_j]);
909 | Serial.print(_bufSer);
910 | }
911 | Serial.println("]");
912 | }
913 | Serial.println("");
914 | }
915 | void vPrintFull() {
916 | char _bufSer[32];
917 | for (int16_t _i = 0; _i < this->i16row; _i++) {
918 | Serial.print("[ ");
919 | for (int16_t _j = 0; _j < this->i16col; _j++) {
920 | snprintf(_bufSer, sizeof(_bufSer)-1, "%e ", (*this)[_i][_j]);
921 | Serial.print(_bufSer);
922 | }
923 | Serial.println("]");
924 | }
925 | Serial.println("");
926 | }
927 | #else
928 | /* User must define the print function somewhere */
929 | void vPrint();
930 | void vPrintFull();
931 | #endif
932 | /* Printing function -------------------------------------------------------------------------------------------- */
933 |
934 |
935 | private:
936 | /* Data structure of Matrix class:
937 | * 0 <= i16row <= (MATRIX_MAXIMUM_SIZE-1) ; i16row is the row of the matrix. i16row is invalid if (i16row == -1)
938 | * 0 <= i16col <= (MATRIX_MAXIMUM_SIZE-1) ; i16col is the column of the matrix. i16col is invalid if (i16col == -1)
939 | *
940 | * f32data[MATRIX_MAXIMUM_SIZE][MATRIX_MAXIMUM_SIZE] is the memory representation of the matrix. We only use the first i16row-th
941 | * and first i16col-th memory for the matrix data. The rest is unused.
942 | *
943 | * This configuration might seems wasteful (yes it is). But with this, we can make the matrix library code as cleanly as possible
944 | * (like I said in the github page, I've made decision to sacrifice speed & performance to get best code readability I could get).
945 | *
946 | * You could change the data structure of f32data if you want to make the implementation more memory efficient.
947 | */
948 | int16_t i16row;
949 | int16_t i16col;
950 | float_prec f32data[MATRIX_MAXIMUM_SIZE][MATRIX_MAXIMUM_SIZE] = {{0}};
951 | };
952 |
953 |
954 | Matrix operator + (const float_prec _scalar, Matrix _mat);
955 | Matrix operator - (const float_prec _scalar, Matrix _mat);
956 | Matrix operator * (const float_prec _scalar, Matrix _mat);
957 | Matrix operator + (Matrix _mat, const float_prec _scalar);
958 | Matrix operator - (Matrix _mat, const float_prec _scalar);
959 | Matrix operator * (Matrix _mat, const float_prec _scalar);
960 | Matrix operator / (Matrix _mat, const float_prec _scalar);
961 |
962 | #endif // MATRIX_H
963 |
--------------------------------------------------------------------------------
/ahrs_ekf/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 |
--------------------------------------------------------------------------------
/ahrs_ukf/ahrs_ukf.ino:
--------------------------------------------------------------------------------
1 | #include
2 | #include
3 | #include "konfig.h"
4 | #include "matrix.h"
5 | #include "ukf.h"
6 | #include "simple_mpu9250.h"
7 |
8 |
9 |
10 | /* ================================================= RLS Variables/function declaration ================================================= */
11 | float_prec RLS_lambda = 0.999; /* Forgetting factor */
12 | Matrix RLS_theta(4,1); /* The variables we want to indentify */
13 | Matrix RLS_P(4,4); /* Inverse of correction estimation */
14 | Matrix RLS_in(4,1); /* Input data */
15 | Matrix RLS_out(1,1); /* Output data */
16 | Matrix RLS_gain(4,1); /* RLS gain */
17 | uint32_t RLS_u32iterData = 0; /* To track how much data we take */
18 |
19 |
20 | /* ================================================= UKF Variables/function declaration ================================================= */
21 | /* UKF initialization constant */
22 | #define P_INIT (10.)
23 | #define Rv_INIT (1e-6)
24 | #define Rn_INIT_ACC (0.0015/10.)
25 | #define Rn_INIT_MAG (0.0015/10.)
26 | /* P(k=0) variable --------------------------------------------------------------------------------------------------------- */
27 | float_prec UKF_PINIT_data[SS_X_LEN*SS_X_LEN] = {P_INIT, 0, 0, 0,
28 | 0, P_INIT, 0, 0,
29 | 0, 0, P_INIT, 0,
30 | 0, 0, 0, P_INIT};
31 | Matrix UKF_PINIT(SS_X_LEN, SS_X_LEN, UKF_PINIT_data);
32 | /* Q constant -------------------------------------------------------------------------------------------------------------- */
33 | float_prec UKF_Rv_data[SS_X_LEN*SS_X_LEN] = {Rv_INIT, 0, 0, 0,
34 | 0, Rv_INIT, 0, 0,
35 | 0, 0, Rv_INIT, 0,
36 | 0, 0, 0, Rv_INIT};
37 | Matrix UKF_Rv(SS_X_LEN, SS_X_LEN, UKF_Rv_data);
38 | /* R constant -------------------------------------------------------------------------------------------------------------- */
39 | float_prec UKF_Rn_data[SS_Z_LEN*SS_Z_LEN] = {Rn_INIT_ACC, 0, 0, 0, 0, 0,
40 | 0, Rn_INIT_ACC, 0, 0, 0, 0,
41 | 0, 0, Rn_INIT_ACC, 0, 0, 0,
42 | 0, 0, 0, Rn_INIT_MAG, 0, 0,
43 | 0, 0, 0, 0, Rn_INIT_MAG, 0,
44 | 0, 0, 0, 0, 0, Rn_INIT_MAG};
45 | Matrix UKF_Rn(SS_Z_LEN, SS_Z_LEN, UKF_Rn_data);
46 | /* Nonlinear & linearization function -------------------------------------------------------------------------------------- */
47 | bool Main_bUpdateNonlinearX(Matrix &X_Next, Matrix &X, Matrix &U);
48 | bool Main_bUpdateNonlinearY(Matrix &Y, Matrix &X, Matrix &U);
49 | /* UKF variables ----------------------------------------------------------------------------------------------------------- */
50 | Matrix quaternionData(SS_X_LEN, 1);
51 | Matrix Y(SS_Z_LEN, 1);
52 | Matrix U(SS_U_LEN, 1);
53 | UKF UKF_IMU(quaternionData, UKF_PINIT, UKF_Rv, UKF_Rn, Main_bUpdateNonlinearX, Main_bUpdateNonlinearY);
54 |
55 |
56 |
57 | /* =============================================== Sharing Variables/function declaration =============================================== */
58 | /* Gravity vector constant (align with global Z-axis) */
59 | #define IMU_ACC_Z0 (1)
60 |
61 | /* Magnetic vector constant (align with local magnetic vector) */
62 | float_prec IMU_MAG_B0_data[3] = {cos(0), sin(0), 0.000000};
63 | Matrix IMU_MAG_B0(3, 1, IMU_MAG_B0_data);
64 |
65 | /* The hard-magnet bias */
66 | float_prec HARD_IRON_BIAS_data[3] = {8.832973, 7.243323, 23.95714};
67 | Matrix HARD_IRON_BIAS(3, 1, HARD_IRON_BIAS_data);
68 |
69 | /* An MPU9250 object with the MPU-9250 sensor on I2C bus 0 with address 0x68 */
70 | SimpleMPU9250 IMU(Wire, 0x68);
71 |
72 | /* State machine for hard-iron bias identification or UKF running */
73 | enum {
74 | STATE_UKF_RUNNING = 0,
75 | STATE_MAGNETO_BIAS_IDENTIFICATION,
76 | STATE_NORTH_VECTOR_IDENTIFICATION
77 | } STATE_AHRS;
78 |
79 |
80 |
81 | /* ============================================== Auxiliary Variables/function declaration ============================================== */
82 | elapsedMillis timerCollectData = 0;
83 | uint64_t u64compuTime;
84 | char bufferTxSer[100];
85 | void serialFloatPrint(float f);
86 | /* The command from the PC */
87 | char cmd;
88 |
89 |
90 |
91 |
92 | void setup() {
93 | /* Serial initialization -------------------------------------- */
94 | Serial.begin(115200);
95 | while(!Serial) {}
96 | Serial.println("Calibrating IMU bias...");
97 |
98 | /* IMU initialization ----------------------------------------- */
99 | int status = IMU.begin(); /* start communication with IMU */
100 | if (status < 0) {
101 | Serial.println("IMU initialization unsuccessful");
102 | Serial.println("Check IMU wiring or try cycling power");
103 | Serial.print("Status: ");
104 | Serial.println(status);
105 | while(1) {}
106 | }
107 |
108 | /* RLS initialization ----------------------------------------- */
109 | RLS_theta.vSetToZero();
110 | RLS_P.vSetIdentity();
111 | RLS_P = RLS_P * 1000;
112 |
113 | /* UKF initialization ----------------------------------------- */
114 | /* x(k=0) = [1 0 0 0]' */
115 | quaternionData.vSetToZero();
116 | quaternionData[0][0] = 1.0;
117 | UKF_IMU.vReset(quaternionData, UKF_PINIT, UKF_Rv, UKF_Rn);
118 |
119 | snprintf(bufferTxSer, sizeof(bufferTxSer)-1, "UKF in Teensy 4.0 (%s)\r\n", (FPU_PRECISION == PRECISION_SINGLE)?"Float32":"Double64");
120 | Serial.print(bufferTxSer);
121 | STATE_AHRS = STATE_UKF_RUNNING;
122 | }
123 |
124 |
125 |
126 |
127 | void loop() {
128 |
129 | if (timerCollectData >= SS_DT_MILIS) { /* We running the RLS/UKF at sampling time = 20 ms */
130 | timerCollectData = 0;
131 |
132 | /* Read the raw data */
133 | IMU.readSensor();
134 | float Ax = IMU.getAccelX_mss();
135 | float Ay = IMU.getAccelY_mss();
136 | float Az = IMU.getAccelZ_mss();
137 | float Bx = IMU.getMagX_uT();
138 | float By = IMU.getMagY_uT();
139 | float Bz = IMU.getMagZ_uT();
140 | float p = IMU.getGyroX_rads();
141 | float q = IMU.getGyroY_rads();
142 | float r = IMU.getGyroZ_rads();
143 |
144 | if (STATE_AHRS == STATE_UKF_RUNNING) { /* Run the UKF algorithm */
145 |
146 | /* ================== Read the sensor data / simulate the system here ================== */
147 |
148 | /* Input 1:3 = gyroscope */
149 | U[0][0] = p; U[1][0] = q; U[2][0] = r;
150 | /* Output 1:3 = accelerometer */
151 | Y[0][0] = Ax; Y[1][0] = Ay; Y[2][0] = Az;
152 | /* Output 4:6 = magnetometer */
153 | Y[3][0] = Bx; Y[4][0] = By; Y[5][0] = Bz;
154 |
155 | /* Compensating Hard-Iron Bias for magnetometer */
156 | Y[3][0] = Y[3][0]-HARD_IRON_BIAS[0][0];
157 | Y[4][0] = Y[4][0]-HARD_IRON_BIAS[1][0];
158 | Y[5][0] = Y[5][0]-HARD_IRON_BIAS[2][0];
159 |
160 | /* Normalizing the output vector */
161 | float_prec _normG = sqrt(Y[0][0] * Y[0][0]) + (Y[1][0] * Y[1][0]) + (Y[2][0] * Y[2][0]);
162 | Y[0][0] = Y[0][0] / _normG;
163 | Y[1][0] = Y[1][0] / _normG;
164 | Y[2][0] = Y[2][0] / _normG;
165 | float_prec _normM = sqrt(Y[3][0] * Y[3][0]) + (Y[4][0] * Y[4][0]) + (Y[5][0] * Y[5][0]);
166 | Y[3][0] = Y[3][0] / _normM;
167 | Y[4][0] = Y[4][0] / _normM;
168 | Y[5][0] = Y[5][0] / _normM;
169 | /* ------------------ Read the sensor data / simulate the system here ------------------ */
170 |
171 |
172 | /* ============================= Update the Kalman Filter ============================== */
173 | u64compuTime = micros();
174 | if (!UKF_IMU.bUpdate(Y, U)) {
175 | quaternionData.vSetToZero();
176 | quaternionData[0][0] = 1.0;
177 | UKF_IMU.vReset(quaternionData, UKF_PINIT, UKF_Rv, UKF_Rn);
178 | Serial.println("Whoop ");
179 | }
180 | u64compuTime = (micros() - u64compuTime);
181 | /* ----------------------------- Update the Kalman Filter ------------------------------ */
182 |
183 | } else if (STATE_AHRS == STATE_MAGNETO_BIAS_IDENTIFICATION) {
184 |
185 | /* ================== Read the sensor data / simulate the system here ================== */
186 | RLS_in[0][0] = Bx;
187 | RLS_in[1][0] = By;
188 | RLS_in[2][0] = Bz;
189 | RLS_in[3][0] = 1;
190 | RLS_out[0][0] = (Bx*Bx) + (By*By) + (Bz*Bz);
191 |
192 | float err = (RLS_out - (RLS_in.Transpose() * RLS_theta))[0][0];
193 | RLS_gain = RLS_P*RLS_in / (RLS_lambda + RLS_in.Transpose()*RLS_P*RLS_in)[0][0];
194 | RLS_P = (RLS_P - RLS_gain*RLS_in.Transpose()*RLS_P)/RLS_lambda;
195 | RLS_theta = RLS_theta + err*RLS_gain;
196 |
197 | RLS_u32iterData++;
198 |
199 | Matrix P_check(RLS_P.GetDiagonalEntries());
200 | if ((P_check.Transpose()*P_check)[0][0] < 1e-4) {
201 | /* The data collection is finished, go back to state UKF running */
202 | STATE_AHRS = STATE_NORTH_VECTOR_IDENTIFICATION;
203 |
204 | /* Reconstruct the matrix compensation solution */
205 | HARD_IRON_BIAS[0][0] = RLS_theta[0][0] / 2.0;
206 | HARD_IRON_BIAS[1][0] = RLS_theta[1][0] / 2.0;
207 | HARD_IRON_BIAS[2][0] = RLS_theta[2][0] / 2.0;
208 |
209 | Serial.println("Calibration finished, the hard-iron bias identified:");
210 | snprintf(bufferTxSer, sizeof(bufferTxSer)-1, "%f %f %f\r\n", HARD_IRON_BIAS[0][0], HARD_IRON_BIAS[1][0], HARD_IRON_BIAS[2][0]);
211 | Serial.println(bufferTxSer);
212 | Serial.println("Set the X axis facing north *with z-accelerometer pointed down*, then send command 'f' to finished, 'a' to abort");
213 | }
214 |
215 |
216 | if ((RLS_u32iterData % 100) == 0) {
217 | /* Print every 200 data, so the user can get updated value */
218 | Matrix P_check(RLS_P.GetDiagonalEntries());
219 |
220 | Serial.println("Calibration in progress, the hard-iron bias identified so far:");
221 | snprintf(bufferTxSer, sizeof(bufferTxSer)-1, "%.3f %.3f %.3f (P = %f)\r\n", RLS_theta[0][0] / 2.0, RLS_theta[1][0] / 2.0, RLS_theta[2][0] / 2.0, (P_check.Transpose()*P_check)[0][0]);
222 | Serial.println(bufferTxSer);
223 | }
224 | if (RLS_u32iterData >= 2000) {
225 | /* We take the data too long but the error still large, terminate without updating the hard-iron bias */
226 | STATE_AHRS = STATE_UKF_RUNNING;
227 |
228 | Serial.println("Calibration timeout, the hard-iron bias won't be updated\r\n");
229 | }
230 |
231 | } else if (STATE_AHRS == STATE_NORTH_VECTOR_IDENTIFICATION) {
232 | } else {
233 | /* You should not be here! */
234 | STATE_AHRS = STATE_UKF_RUNNING;
235 | }
236 | }
237 |
238 |
239 | /* Event serial data: The serial data is sent by responding to command from the PC running Processing scipt or from user command */
240 | if (Serial.available()) {
241 | cmd = Serial.read();
242 |
243 |
244 | if (STATE_AHRS == STATE_UKF_RUNNING) {
245 |
246 | if (cmd == 'c') {
247 | /* User want to do online hard-iron bias identification */
248 | Serial.println("Hard iron bias identification command entered\r\n");
249 | Serial.println("Rotate the MPU9250 in every direction\r\n");
250 | Serial.println("Send 'a' to abort\r\n");
251 |
252 | /* Initialize RLS */
253 | RLS_theta.vSetToZero();
254 | RLS_P.vSetIdentity();
255 | RLS_P = RLS_P * 1000;
256 | RLS_u32iterData = 0;
257 | STATE_AHRS = STATE_MAGNETO_BIAS_IDENTIFICATION;
258 |
259 | } else if (cmd == 'p') {
260 |
261 | Serial.println("North vector value:");
262 | snprintf(bufferTxSer, sizeof(bufferTxSer)-1, "%f %f %f\r\n", IMU_MAG_B0[0][0], IMU_MAG_B0[1][0], IMU_MAG_B0[2][0]);
263 | Serial.print(bufferTxSer);
264 |
265 | Serial.println("Hard iron bias value:");
266 | snprintf(bufferTxSer, sizeof(bufferTxSer)-1, "%f %f %f\r\n", HARD_IRON_BIAS[0][0], HARD_IRON_BIAS[1][0], HARD_IRON_BIAS[2][0]);
267 | Serial.print(bufferTxSer);
268 |
269 | } else if (cmd == 'n') {
270 | Serial.println("North vector identification command entered\r\n");
271 | Serial.println("Set the X axis facing north *with z-accelerometer pointed down*, then send command 'f'");
272 | STATE_AHRS = STATE_NORTH_VECTOR_IDENTIFICATION;
273 | }
274 |
275 | /* Process the command data from Processing script */
276 | if (cmd == 'v') {
277 | snprintf(bufferTxSer, sizeof(bufferTxSer)-1, "UKF in Teensy 4.0 (%s)", (FPU_PRECISION == PRECISION_SINGLE)?"Float32":"Double64");
278 | Serial.print('\n');
279 |
280 | } else if (cmd == 'q') {
281 | /* =========================== Print to serial (for plotting) ========================== */
282 | quaternionData = UKF_IMU.GetX();
283 |
284 | while (!Serial.available());
285 | uint8_t count = Serial.read();
286 | for (uint8_t _i = 0; _i < count; _i++) {
287 | serialFloatPrint(quaternionData[0][0]);
288 | Serial.print(",");
289 | serialFloatPrint(quaternionData[1][0]);
290 | Serial.print(",");
291 | serialFloatPrint(quaternionData[2][0]);
292 | Serial.print(",");
293 | serialFloatPrint(quaternionData[3][0]);
294 | Serial.print(",");
295 | serialFloatPrint((float)u64compuTime);
296 | Serial.print(",");
297 | Serial.println("");
298 | }
299 | }
300 |
301 | } else if (STATE_AHRS == STATE_MAGNETO_BIAS_IDENTIFICATION) {
302 |
303 | if (cmd == 'a') {
304 | /* User want to terminate hard iron bias identification */
305 | Serial.println("Calibration aborted, the hard-iron bias won't be updated\r\n");
306 | STATE_AHRS = STATE_UKF_RUNNING;
307 | }
308 |
309 | } else if (STATE_AHRS == STATE_NORTH_VECTOR_IDENTIFICATION) {
310 |
311 | if (cmd == 'a') {
312 | /* User want to terminate hard iron bias identification */
313 | Serial.println("Calibration aborted, the hard-iron bias won't be updated\r\n");
314 | STATE_AHRS = STATE_UKF_RUNNING;
315 |
316 | } else if (cmd == 'f') {
317 | float Ax = IMU.getAccelX_mss();
318 | float Ay = IMU.getAccelY_mss();
319 | float Az = IMU.getAccelZ_mss();
320 | float Bx = IMU.getMagX_uT() - HARD_IRON_BIAS[0][0];
321 | float By = IMU.getMagY_uT() - HARD_IRON_BIAS[1][0];
322 | float Bz = IMU.getMagZ_uT() - HARD_IRON_BIAS[2][0];
323 |
324 | /* Normalizing the acceleration vector & projecting the gravitational vector (gravity is negative acceleration) */
325 | float_prec _normG = sqrt((Ax * Ax) + (Ay * Ay) + (Az * Az));
326 | Ax = Ax / _normG;
327 | Ay = Ay / _normG;
328 | Az = Az / _normG;
329 |
330 | /* Normalizing the magnetic vector */
331 | _normG = sqrt((Bx * Bx) + (By * By) + (Bz * Bz));
332 | Bx = Bx / _normG;
333 | By = By / _normG;
334 | Bz = Bz / _normG;
335 |
336 | /* Projecting the magnetic vector into plane orthogonal to the gravitational vector */
337 | float pitch = asin(-Ax);
338 | float roll = asin(Ay/cos(pitch));
339 | float m_tilt_x = Bx*cos(pitch) + By*sin(roll)*sin(pitch) + Bz*cos(roll)*sin(pitch);
340 | float m_tilt_y = + By*cos(roll) - Bz*sin(roll);
341 | /* float m_tilt_z = -Bx*sin(pitch) + By*sin(roll)*cos(pitch) + Bz*cos(roll)*cos(pitch); */
342 |
343 | float mag_dec = atan2(m_tilt_y, m_tilt_x);
344 | IMU_MAG_B0[0][0] = cos(mag_dec);
345 | IMU_MAG_B0[1][0] = sin(mag_dec);
346 | IMU_MAG_B0[2][0] = 0;
347 |
348 | Serial.println("North identification finished, the north vector identified:");
349 | snprintf(bufferTxSer, sizeof(bufferTxSer)-1, "%.3f %.3f %.3f\r\n", IMU_MAG_B0[0][0], IMU_MAG_B0[1][0], IMU_MAG_B0[2][0]);
350 | Serial.print(bufferTxSer);
351 |
352 | STATE_AHRS = STATE_UKF_RUNNING;
353 | }
354 |
355 | }
356 | }
357 | }
358 |
359 |
360 | /* Function to interface with the Processing script in the PC */
361 | void serialFloatPrint(float f) {
362 | byte * b = (byte *) &f;
363 | for (int i = 0; i < 4; i++) {
364 | byte b1 = (b[i] >> 4) & 0x0f;
365 | byte b2 = (b[i] & 0x0f);
366 |
367 | char c1 = (b1 < 10) ? ('0' + b1) : 'A' + b1 - 10;
368 | char c2 = (b2 < 10) ? ('0' + b2) : 'A' + b2 - 10;
369 |
370 | Serial.print(c1);
371 | Serial.print(c2);
372 | }
373 | }
374 |
375 | bool Main_bUpdateNonlinearX(Matrix &X_Next, Matrix &X, Matrix &U)
376 | {
377 | /* Insert the nonlinear update transformation here
378 | * x(k+1) = f[x(k), u(k)]
379 | *
380 | * The quaternion update function:
381 | * q0_dot = 1/2. * ( 0 - p*q1 - q*q2 - r*q3)
382 | * q1_dot = 1/2. * ( p*q0 + 0 + r*q2 - q*q3)
383 | * q2_dot = 1/2. * ( q*q0 - r*q1 + 0 + p*q3)
384 | * q3_dot = 1/2. * ( r*q0 + q*q1 - p*q2 + 0 )
385 | *
386 | * Euler method for integration:
387 | * q0 = q0 + q0_dot * dT;
388 | * q1 = q1 + q1_dot * dT;
389 | * q2 = q2 + q2_dot * dT;
390 | * q3 = q3 + q3_dot * dT;
391 | */
392 | float_prec q0, q1, q2, q3;
393 | float_prec p, q, r;
394 |
395 | q0 = X[0][0];
396 | q1 = X[1][0];
397 | q2 = X[2][0];
398 | q3 = X[3][0];
399 |
400 | p = U[0][0];
401 | q = U[1][0];
402 | r = U[2][0];
403 |
404 | X_Next[0][0] = (0.5 * (+0.00 -p*q1 -q*q2 -r*q3))*SS_DT + q0;
405 | X_Next[1][0] = (0.5 * (+p*q0 +0.00 +r*q2 -q*q3))*SS_DT + q1;
406 | X_Next[2][0] = (0.5 * (+q*q0 -r*q1 +0.00 +p*q3))*SS_DT + q2;
407 | X_Next[3][0] = (0.5 * (+r*q0 +q*q1 -p*q2 +0.00))*SS_DT + q3;
408 |
409 |
410 | /* ======= Additional ad-hoc quaternion normalization to make sure the quaternion is a unit vector (i.e. ||q|| = 1) ======= */
411 | if (!X_Next.bNormVector()) {
412 | /* System error, return false so we can reset the UKF */
413 | return false;
414 | }
415 |
416 | return true;
417 | }
418 |
419 | bool Main_bUpdateNonlinearY(Matrix &Y, Matrix &X, Matrix &U)
420 | {
421 | /* Insert the nonlinear measurement transformation here
422 | * y(k) = h[x(k), u(k)]
423 | *
424 | * The measurement output is the gravitational and magnetic projection to the body
425 | * DCM = [(+(q0**2)+(q1**2)-(q2**2)-(q3**2)), 2*(q1*q2+q0*q3), 2*(q1*q3-q0*q2)]
426 | * [ 2*(q1*q2-q0*q3), (+(q0**2)-(q1**2)+(q2**2)-(q3**2)), 2*(q2*q3+q0*q1)]
427 | * [ 2*(q1*q3+q0*q2), 2*(q2*q3-q0*q1), (+(q0**2)-(q1**2)-(q2**2)+(q3**2))]
428 | *
429 | * G_proj_sens = DCM * [0 0 1] --> Gravitational projection to the accelerometer sensor
430 | * M_proj_sens = DCM * [Mx My Mz] --> (Earth) magnetic projection to the magnetometer sensor
431 | */
432 | float_prec q0, q1, q2, q3;
433 | float_prec q0_2, q1_2, q2_2, q3_2;
434 |
435 | q0 = X[0][0];
436 | q1 = X[1][0];
437 | q2 = X[2][0];
438 | q3 = X[3][0];
439 |
440 | q0_2 = q0 * q0;
441 | q1_2 = q1 * q1;
442 | q2_2 = q2 * q2;
443 | q3_2 = q3 * q3;
444 |
445 | Y[0][0] = (2*q1*q3 -2*q0*q2) * IMU_ACC_Z0;
446 |
447 | Y[1][0] = (2*q2*q3 +2*q0*q1) * IMU_ACC_Z0;
448 |
449 | Y[2][0] = (+(q0_2) -(q1_2) -(q2_2) +(q3_2)) * IMU_ACC_Z0;
450 |
451 | Y[3][0] = (+(q0_2)+(q1_2)-(q2_2)-(q3_2)) * IMU_MAG_B0[0][0]
452 | +(2*(q1*q2+q0*q3)) * IMU_MAG_B0[1][0]
453 | +(2*(q1*q3-q0*q2)) * IMU_MAG_B0[2][0];
454 |
455 | Y[4][0] = (2*(q1*q2-q0*q3)) * IMU_MAG_B0[0][0]
456 | +(+(q0_2)-(q1_2)+(q2_2)-(q3_2)) * IMU_MAG_B0[1][0]
457 | +(2*(q2*q3+q0*q1)) * IMU_MAG_B0[2][0];
458 |
459 | Y[5][0] = (2*(q1*q3+q0*q2)) * IMU_MAG_B0[0][0]
460 | +(2*(q2*q3-q0*q1)) * IMU_MAG_B0[1][0]
461 | +(+(q0_2)-(q1_2)-(q2_2)+(q3_2)) * IMU_MAG_B0[2][0];
462 | return true;
463 | }
464 |
465 |
466 |
467 | void SPEW_THE_ERROR(char const * str)
468 | {
469 | #if (SYSTEM_IMPLEMENTATION == SYSTEM_IMPLEMENTATION_PC)
470 | cout << (str) << endl;
471 | #elif (SYSTEM_IMPLEMENTATION == SYSTEM_IMPLEMENTATION_EMBEDDED_ARDUINO)
472 | Serial.println(str);
473 | #else
474 | /* Silent function */
475 | #endif
476 | while(1);
477 | }
478 |
--------------------------------------------------------------------------------
/ahrs_ukf/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 | /* State Space dimension */
16 | #define SS_X_LEN (4)
17 | #define SS_Z_LEN (6)
18 | #define SS_U_LEN (3)
19 | #define SS_DT_MILIS (20) /* 10 ms */
20 | #define SS_DT float_prec(SS_DT_MILIS/1000.) /* Sampling time */
21 |
22 |
23 | /* Change this size based on the biggest matrix you will use */
24 | #define MATRIX_MAXIMUM_SIZE (SS_Z_LEN*2+1)
25 |
26 | /* Define this to enable matrix bound checking */
27 | #define MATRIX_USE_BOUND_CHECKING
28 |
29 | /* Set this define to choose math precision of the system */
30 | #define PRECISION_SINGLE 1
31 | #define PRECISION_DOUBLE 2
32 | #define FPU_PRECISION (PRECISION_DOUBLE)
33 |
34 | #if (FPU_PRECISION == PRECISION_SINGLE)
35 | #define float_prec float
36 | #define float_prec_ZERO (1e-7)
37 | #define float_prec_ZERO_ECO (1e-5) /* 'Economical' zero, for noisy calculation where 'somewhat zero' is good enough */
38 | #elif (FPU_PRECISION == PRECISION_DOUBLE)
39 | #define float_prec double
40 | #define float_prec_ZERO (1e-13)
41 | #define float_prec_ZERO_ECO (1e-8) /* 'Economical' zero, for noisy calculation where 'somewhat zero' is good enough */
42 | #else
43 | #error("FPU_PRECISION has not been defined!");
44 | #endif
45 |
46 |
47 |
48 | /* Set this define to choose system implementation (mainly used to define how you print the matrix via the Matrix::vCetak() function) */
49 | #define SYSTEM_IMPLEMENTATION_PC 1
50 | #define SYSTEM_IMPLEMENTATION_EMBEDDED_CUSTOM 2
51 | #define SYSTEM_IMPLEMENTATION_EMBEDDED_ARDUINO 3
52 |
53 | #define SYSTEM_IMPLEMENTATION (SYSTEM_IMPLEMENTATION_EMBEDDED_ARDUINO)
54 |
55 |
56 |
57 | /* ASSERT is evaluated locally (without function call) to lower the computation cost */
58 | void SPEW_THE_ERROR(char const * str);
59 | #define ASSERT(truth, str) { if (!(truth)) SPEW_THE_ERROR(str); }
60 |
61 |
62 | #endif // KONFIG_H
63 |
--------------------------------------------------------------------------------
/ahrs_ukf/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 |
12 |
13 | Matrix operator + (const float_prec _scalar, Matrix _mat)
14 | {
15 | Matrix _outp(_mat.i16getRow(), _mat.i16getCol(), Matrix::NoInitMatZero);
16 |
17 | for (int16_t _i = 0; _i < _mat.i16getRow(); _i++) {
18 | for (int16_t _j = 0; _j < _mat.i16getCol(); _j++) {
19 | _outp[_i][_j] = _scalar + _mat[_i][_j];
20 | }
21 | }
22 | return _outp;
23 | }
24 |
25 |
26 | Matrix operator - (const float_prec _scalar, Matrix _mat)
27 | {
28 | Matrix _outp(_mat.i16getRow(), _mat.i16getCol(), Matrix::NoInitMatZero);
29 |
30 | for (int16_t _i = 0; _i < _mat.i16getRow(); _i++) {
31 | for (int16_t _j = 0; _j < _mat.i16getCol(); _j++) {
32 | _outp[_i][_j] = _scalar - _mat[_i][_j];
33 | }
34 | }
35 | return _outp;
36 | }
37 |
38 |
39 | Matrix operator * (const float_prec _scalar, Matrix _mat)
40 | {
41 | Matrix _outp(_mat.i16getRow(), _mat.i16getCol(), Matrix::NoInitMatZero);
42 |
43 | for (int16_t _i = 0; _i < _mat.i16getRow(); _i++) {
44 | for (int16_t _j = 0; _j < _mat.i16getCol(); _j++) {
45 | _outp[_i][_j] = _scalar * _mat[_i][_j];
46 | }
47 | }
48 | return _outp;
49 | }
50 |
51 |
52 | Matrix operator + (Matrix _mat, const float_prec _scalar)
53 | {
54 | Matrix _outp(_mat.i16getRow(), _mat.i16getCol(), Matrix::NoInitMatZero);
55 |
56 | for (int16_t _i = 0; _i < _mat.i16getRow(); _i++) {
57 | for (int16_t _j = 0; _j < _mat.i16getCol(); _j++) {
58 | _outp[_i][_j] = _mat[_i][_j] + _scalar;
59 | }
60 | }
61 | return _outp;
62 | }
63 |
64 |
65 | Matrix operator - (Matrix _mat, const float_prec _scalar)
66 | {
67 | Matrix _outp(_mat.i16getRow(), _mat.i16getCol(), Matrix::NoInitMatZero);
68 |
69 | for (int16_t _i = 0; _i < _mat.i16getRow(); _i++) {
70 | for (int16_t _j = 0; _j < _mat.i16getCol(); _j++) {
71 | _outp[_i][_j] = _mat[_i][_j] - _scalar;
72 | }
73 | }
74 | return _outp;
75 | }
76 |
77 |
78 | Matrix operator * (Matrix _mat, const float_prec _scalar)
79 | {
80 | Matrix _outp(_mat.i16getRow(), _mat.i16getCol(), Matrix::NoInitMatZero);
81 |
82 | for (int16_t _i = 0; _i < _mat.i16getRow(); _i++) {
83 | for (int16_t _j = 0; _j < _mat.i16getCol(); _j++) {
84 | _outp[_i][_j] = _mat[_i][_j] * _scalar;
85 | }
86 | }
87 | return _outp;
88 | }
89 |
90 |
91 | Matrix operator / (Matrix _mat, const float_prec _scalar)
92 | {
93 | Matrix _outp(_mat.i16getRow(), _mat.i16getCol(), Matrix::NoInitMatZero);
94 |
95 | if (fabs(_scalar) < float_prec(float_prec_ZERO)) {
96 | _outp.vSetMatrixInvalid();
97 | return _outp;
98 | }
99 | for (int16_t _i = 0; _i < _mat.i16getRow(); _i++) {
100 | for (int16_t _j = 0; _j < _mat.i16getCol(); _j++) {
101 | _outp[_i][_j] = _mat[_i][_j] / _scalar;
102 | }
103 | }
104 | return _outp;
105 | }
106 |
107 |
--------------------------------------------------------------------------------
/ahrs_ukf/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 | /* Transform the magnetomer to align with the Accelerometer and Gyroscope Axis */
200 | float _hxAK8963 = ((float)_hxcounts) * _magScaleX;
201 | float _hyAK8963 = ((float)_hycounts) * _magScaleY;
202 | float _hzAK8963 = ((float)_hzcounts) * _magScaleZ;
203 | _hx = _hyAK8963;
204 | _hy = _hxAK8963;
205 | _hz = -_hzAK8963;
206 |
207 | return 1;
208 | }
209 |
210 | /* estimates the gyro biases */
211 | int calibrateGyro() {
212 | // set the range, bandwidth, and srd
213 | if (setGyroRange(GYRO_RANGE_250DPS) < 0) {
214 | return -1;
215 | }
216 | if (setDlpfBandwidth(DLPF_BANDWIDTH_20HZ) < 0) {
217 | return -2;
218 | }
219 | if (setSrd(19) < 0) {
220 | return -3;
221 | }
222 |
223 | // take samples and find bias
224 | _gxbD = 0;
225 | _gybD = 0;
226 | _gzbD = 0;
227 | for (size_t i=0; i < _numSamples; i++) {
228 | readSensor();
229 | _gxbD += (getGyroX_rads() + _gxb)/((double)_numSamples);
230 | _gybD += (getGyroY_rads() + _gyb)/((double)_numSamples);
231 | _gzbD += (getGyroZ_rads() + _gzb)/((double)_numSamples);
232 | delay(20);
233 | }
234 | _gxb = (float)_gxbD;
235 | _gyb = (float)_gybD;
236 | _gzb = (float)_gzbD;
237 |
238 | // set the range, bandwidth, and srd back to what they were
239 | if (setGyroRange(_gyroRange) < 0) {
240 | return -4;
241 | }
242 | if (setDlpfBandwidth(_bandwidth) < 0) {
243 | return -5;
244 | }
245 | if (setSrd(_srd) < 0) {
246 | return -6;
247 | }
248 | return 1;
249 | }
250 |
251 | /* sets the accelerometer full scale range to values other than default */
252 | int setAccelRange(AccelRange range) {
253 | switch(range) {
254 | case ACCEL_RANGE_2G: {
255 | // setting the accel range to 2G
256 | if(writeRegister(ACCEL_CONFIG,ACCEL_FS_SEL_2G) < 0) {
257 | return -1;
258 | }
259 | _accelScale = G * 2.0f/32767.5f; // setting the accel scale to 2G
260 | break;
261 | }
262 | case ACCEL_RANGE_4G: {
263 | // setting the accel range to 4G
264 | if(writeRegister(ACCEL_CONFIG,ACCEL_FS_SEL_4G) < 0) {
265 | return -1;
266 | }
267 | _accelScale = G * 4.0f/32767.5f; // setting the accel scale to 4G
268 | break;
269 | }
270 | case ACCEL_RANGE_8G: {
271 | // setting the accel range to 8G
272 | if(writeRegister(ACCEL_CONFIG,ACCEL_FS_SEL_8G) < 0) {
273 | return -1;
274 | }
275 | _accelScale = G * 8.0f/32767.5f; // setting the accel scale to 8G
276 | break;
277 | }
278 | case ACCEL_RANGE_16G: {
279 | // setting the accel range to 16G
280 | if(writeRegister(ACCEL_CONFIG,ACCEL_FS_SEL_16G) < 0) {
281 | return -1;
282 | }
283 | _accelScale = G * 16.0f/32767.5f; // setting the accel scale to 16G
284 | break;
285 | }
286 | }
287 | _accelRange = range;
288 | return 1;
289 | }
290 |
291 | /* sets the gyro full scale range to values other than default */
292 | int setGyroRange(GyroRange range) {
293 | switch(range) {
294 | case GYRO_RANGE_250DPS: {
295 | // setting the gyro range to 250DPS
296 | if(writeRegister(GYRO_CONFIG,GYRO_FS_SEL_250DPS) < 0) {
297 | return -1;
298 | }
299 | _gyroScale = 250.0f/32767.5f * _d2r; // setting the gyro scale to 250DPS
300 | break;
301 | }
302 | case GYRO_RANGE_500DPS: {
303 | // setting the gyro range to 500DPS
304 | if(writeRegister(GYRO_CONFIG,GYRO_FS_SEL_500DPS) < 0) {
305 | return -1;
306 | }
307 | _gyroScale = 500.0f/32767.5f * _d2r; // setting the gyro scale to 500DPS
308 | break;
309 | }
310 | case GYRO_RANGE_1000DPS: {
311 | // setting the gyro range to 1000DPS
312 | if(writeRegister(GYRO_CONFIG,GYRO_FS_SEL_1000DPS) < 0) {
313 | return -1;
314 | }
315 | _gyroScale = 1000.0f/32767.5f * _d2r; // setting the gyro scale to 1000DPS
316 | break;
317 | }
318 | case GYRO_RANGE_2000DPS: {
319 | // setting the gyro range to 2000DPS
320 | if(writeRegister(GYRO_CONFIG,GYRO_FS_SEL_2000DPS) < 0) {
321 | return -1;
322 | }
323 | _gyroScale = 2000.0f/32767.5f * _d2r; // setting the gyro scale to 2000DPS
324 | break;
325 | }
326 | }
327 | _gyroRange = range;
328 | return 1;
329 | }
330 |
331 | /* sets the DLPF bandwidth to values other than default */
332 | int setDlpfBandwidth(DlpfBandwidth bandwidth) {
333 | switch(bandwidth) {
334 | case DLPF_BANDWIDTH_184HZ: {
335 | if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_184) < 0) { // setting accel bandwidth to 184Hz
336 | return -1;
337 | }
338 | if(writeRegister(CONFIG,GYRO_DLPF_184) < 0) { // setting gyro bandwidth to 184Hz
339 | return -2;
340 | }
341 | break;
342 | }
343 | case DLPF_BANDWIDTH_92HZ: {
344 | if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_92) < 0) { // setting accel bandwidth to 92Hz
345 | return -1;
346 | }
347 | if(writeRegister(CONFIG,GYRO_DLPF_92) < 0) { // setting gyro bandwidth to 92Hz
348 | return -2;
349 | }
350 | break;
351 | }
352 | case DLPF_BANDWIDTH_41HZ: {
353 | if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_41) < 0) { // setting accel bandwidth to 41Hz
354 | return -1;
355 | }
356 | if(writeRegister(CONFIG,GYRO_DLPF_41) < 0) { // setting gyro bandwidth to 41Hz
357 | return -2;
358 | }
359 | break;
360 | }
361 | case DLPF_BANDWIDTH_20HZ: {
362 | if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_20) < 0) { // setting accel bandwidth to 20Hz
363 | return -1;
364 | }
365 | if(writeRegister(CONFIG,GYRO_DLPF_20) < 0) { // setting gyro bandwidth to 20Hz
366 | return -2;
367 | }
368 | break;
369 | }
370 | case DLPF_BANDWIDTH_10HZ: {
371 | if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_10) < 0) { // setting accel bandwidth to 10Hz
372 | return -1;
373 | }
374 | if(writeRegister(CONFIG,GYRO_DLPF_10) < 0) { // setting gyro bandwidth to 10Hz
375 | return -2;
376 | }
377 | break;
378 | }
379 | case DLPF_BANDWIDTH_5HZ: {
380 | if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_5) < 0) { // setting accel bandwidth to 5Hz
381 | return -1;
382 | }
383 | if(writeRegister(CONFIG,GYRO_DLPF_5) < 0) { // setting gyro bandwidth to 5Hz
384 | return -2;
385 | }
386 | break;
387 | }
388 | }
389 | _bandwidth = bandwidth;
390 | return 1;
391 | }
392 |
393 | /* sets the sample rate divider to values other than default */
394 | int setSrd(uint8_t srd) {
395 | /* setting the sample rate divider to 19 to facilitate setting up magnetometer */
396 | if(writeRegister(SMPDIV,19) < 0) { // setting the sample rate divider
397 | return -1;
398 | }
399 | if(srd > 9) {
400 | // set AK8963 to Power Down
401 | if(writeAK8963Register(AK8963_CNTL1,AK8963_PWR_DOWN) < 0) {
402 | return -2;
403 | }
404 | delay(100); // long wait between AK8963 mode changes
405 | // set AK8963 to 16 bit resolution, 8 Hz update rate
406 | if(writeAK8963Register(AK8963_CNTL1,AK8963_CNT_MEAS1) < 0) {
407 | return -3;
408 | }
409 | delay(100); // long wait between AK8963 mode changes
410 | // instruct the MPU9250 to get 7 bytes of data from the AK8963 at the sample rate
411 | readAK8963Registers(AK8963_HXL,7,_buffer);
412 | } else {
413 | // set AK8963 to Power Down
414 | if(writeAK8963Register(AK8963_CNTL1,AK8963_PWR_DOWN) < 0) {
415 | return -2;
416 | }
417 | delay(100); // long wait between AK8963 mode changes
418 | // set AK8963 to 16 bit resolution, 100 Hz update rate
419 | if(writeAK8963Register(AK8963_CNTL1,AK8963_CNT_MEAS2) < 0) {
420 | return -3;
421 | }
422 | delay(100); // long wait between AK8963 mode changes
423 | // instruct the MPU9250 to get 7 bytes of data from the AK8963 at the sample rate
424 | readAK8963Registers(AK8963_HXL,7,_buffer);
425 | }
426 | /* setting the sample rate divider */
427 | if(writeRegister(SMPDIV,srd) < 0) { // setting the sample rate divider
428 | return -4;
429 | }
430 | _srd = srd;
431 | return 1;
432 | }
433 |
434 | /* returns the accelerometer measurement in the x direction, m/s/s */
435 | float getAccelX_mss() {
436 | return _ax;
437 | }
438 |
439 | /* returns the accelerometer measurement in the y direction, m/s/s */
440 | float getAccelY_mss() {
441 | return _ay;
442 | }
443 |
444 | /* returns the accelerometer measurement in the z direction, m/s/s */
445 | float getAccelZ_mss() {
446 | return _az;
447 | }
448 |
449 | /* returns the gyroscope measurement in the x direction, rad/s */
450 | float getGyroX_rads() {
451 | return _gx;
452 | }
453 |
454 | /* returns the gyroscope measurement in the y direction, rad/s */
455 | float getGyroY_rads() {
456 | return _gy;
457 | }
458 |
459 | /* returns the gyroscope measurement in the z direction, rad/s */
460 | float getGyroZ_rads() {
461 | return _gz;
462 | }
463 |
464 | /* returns the magnetometer measurement in the x direction, uT */
465 | float getMagX_uT() {
466 | return _hx;
467 | }
468 |
469 | /* returns the magnetometer measurement in the y direction, uT */
470 | float getMagY_uT() {
471 | return _hy;
472 | }
473 |
474 | /* returns the magnetometer measurement in the z direction, uT */
475 | float getMagZ_uT() {
476 | return _hz;
477 | }
478 |
479 |
480 | private:
481 | uint8_t _address;
482 | TwoWire *_i2c;
483 | size_t _numBytes; // number of bytes received from I2C
484 |
485 |
486 | // track success of interacting with sensor
487 | int _status;
488 | // buffer for reading from sensor
489 | uint8_t _buffer[21];
490 | // data counts
491 | int16_t _axcounts,_aycounts,_azcounts;
492 | int16_t _gxcounts,_gycounts,_gzcounts;
493 | int16_t _hxcounts,_hycounts,_hzcounts;
494 | int16_t _tcounts;
495 | // data buffer
496 | float _ax, _ay, _az;
497 | float _gx, _gy, _gz;
498 | float _hx, _hy, _hz;
499 | float _t;
500 | // wake on motion
501 | uint8_t _womThreshold;
502 | // scale factors
503 | float _accelScale;
504 | float _gyroScale;
505 | float _magScaleX, _magScaleY, _magScaleZ;
506 | const float _tempScale = 333.87f;
507 | const float _tempOffset = 21.0f;
508 | // configuration
509 | AccelRange _accelRange;
510 | GyroRange _gyroRange;
511 | DlpfBandwidth _bandwidth;
512 | uint8_t _srd;
513 | // gyro bias estimation
514 | size_t _numSamples = 100;
515 | double _gxbD, _gybD, _gzbD;
516 | float _gxb, _gyb, _gzb;
517 | // accel bias and scale factor estimation
518 | double _axbD, _aybD, _azbD;
519 | float _axmax, _aymax, _azmax;
520 | float _axmin, _aymin, _azmin;
521 | float _axb, _ayb, _azb;
522 | float _axs = 1.0f;
523 | float _ays = 1.0f;
524 | float _azs = 1.0f;
525 | // magnetometer bias and scale factor estimation
526 | uint16_t _maxCounts = 1000;
527 | float _deltaThresh = 0.3f;
528 | uint8_t _coeff = 8;
529 | uint16_t _counter;
530 | float _framedelta, _delta;
531 | float _hxfilt, _hyfilt, _hzfilt;
532 | float _hxmax, _hymax, _hzmax;
533 | float _hxmin, _hymin, _hzmin;
534 | float _hxb, _hyb, _hzb;
535 | float _hxs = 1.0f;
536 | float _hys = 1.0f;
537 | float _hzs = 1.0f;
538 | float _avgs;
539 |
540 | /* constants */
541 | const float G = 9.807f;
542 | const float _d2r = 3.14159265359f/180.0f;
543 | /* MPU9250 registers */
544 | const uint8_t ACCEL_OUT = 0x3B;
545 | const uint8_t GYRO_OUT = 0x43;
546 | const uint8_t TEMP_OUT = 0x41;
547 | const uint8_t EXT_SENS_DATA_00 = 0x49;
548 | const uint8_t ACCEL_CONFIG = 0x1C;
549 | const uint8_t ACCEL_FS_SEL_2G = 0x00;
550 | const uint8_t ACCEL_FS_SEL_4G = 0x08;
551 | const uint8_t ACCEL_FS_SEL_8G = 0x10;
552 | const uint8_t ACCEL_FS_SEL_16G = 0x18;
553 | const uint8_t GYRO_CONFIG = 0x1B;
554 | const uint8_t GYRO_FS_SEL_250DPS = 0x00;
555 | const uint8_t GYRO_FS_SEL_500DPS = 0x08;
556 | const uint8_t GYRO_FS_SEL_1000DPS = 0x10;
557 | const uint8_t GYRO_FS_SEL_2000DPS = 0x18;
558 | const uint8_t ACCEL_CONFIG2 = 0x1D;
559 | const uint8_t ACCEL_DLPF_184 = 0x01;
560 | const uint8_t ACCEL_DLPF_92 = 0x02;
561 | const uint8_t ACCEL_DLPF_41 = 0x03;
562 | const uint8_t ACCEL_DLPF_20 = 0x04;
563 | const uint8_t ACCEL_DLPF_10 = 0x05;
564 | const uint8_t ACCEL_DLPF_5 = 0x06;
565 | const uint8_t CONFIG = 0x1A;
566 | const uint8_t GYRO_DLPF_184 = 0x01;
567 | const uint8_t GYRO_DLPF_92 = 0x02;
568 | const uint8_t GYRO_DLPF_41 = 0x03;
569 | const uint8_t GYRO_DLPF_20 = 0x04;
570 | const uint8_t GYRO_DLPF_10 = 0x05;
571 | const uint8_t GYRO_DLPF_5 = 0x06;
572 | const uint8_t SMPDIV = 0x19;
573 | const uint8_t INT_PIN_CFG = 0x37;
574 | const uint8_t INT_ENABLE = 0x38;
575 | const uint8_t INT_DISABLE = 0x00;
576 | const uint8_t INT_PULSE_50US = 0x00;
577 | const uint8_t INT_WOM_EN = 0x40;
578 | const uint8_t INT_RAW_RDY_EN = 0x01;
579 | const uint8_t PWR_MGMNT_1 = 0x6B;
580 | const uint8_t PWR_CYCLE = 0x20;
581 | const uint8_t PWR_RESET = 0x80;
582 | const uint8_t CLOCK_SEL_PLL = 0x01;
583 | const uint8_t PWR_MGMNT_2 = 0x6C;
584 | const uint8_t SEN_ENABLE = 0x00;
585 | const uint8_t DIS_GYRO = 0x07;
586 | const uint8_t USER_CTRL = 0x6A;
587 | const uint8_t I2C_MST_EN = 0x20;
588 | const uint8_t I2C_MST_CLK = 0x0D;
589 | const uint8_t I2C_MST_CTRL = 0x24;
590 | const uint8_t I2C_SLV0_ADDR = 0x25;
591 | const uint8_t I2C_SLV0_REG = 0x26;
592 | const uint8_t I2C_SLV0_DO = 0x63;
593 | const uint8_t I2C_SLV0_CTRL = 0x27;
594 | const uint8_t I2C_SLV0_EN = 0x80;
595 | const uint8_t I2C_READ_FLAG = 0x80;
596 | const uint8_t MOT_DETECT_CTRL = 0x69;
597 | const uint8_t ACCEL_INTEL_EN = 0x80;
598 | const uint8_t ACCEL_INTEL_MODE = 0x40;
599 | const uint8_t LP_ACCEL_ODR = 0x1E;
600 | const uint8_t WOM_THR = 0x1F;
601 | const uint8_t WHO_AM_I = 0x75;
602 | const uint8_t FIFO_EN = 0x23;
603 | const uint8_t FIFO_TEMP = 0x80;
604 | const uint8_t FIFO_GYRO = 0x70;
605 | const uint8_t FIFO_ACCEL = 0x08;
606 | const uint8_t FIFO_MAG = 0x01;
607 | const uint8_t FIFO_COUNT = 0x72;
608 | const uint8_t FIFO_READ = 0x74;
609 | /* AK8963 registers */
610 | const uint8_t AK8963_I2C_ADDR = 0x0C;
611 | const uint8_t AK8963_HXL = 0x03;
612 | const uint8_t AK8963_CNTL1 = 0x0A;
613 | const uint8_t AK8963_PWR_DOWN = 0x00;
614 | const uint8_t AK8963_CNT_MEAS1 = 0x12;
615 | const uint8_t AK8963_CNT_MEAS2 = 0x16;
616 | const uint8_t AK8963_FUSE_ROM = 0x0F;
617 | const uint8_t AK8963_CNTL2 = 0x0B;
618 | const uint8_t AK8963_RESET = 0x01;
619 | const uint8_t AK8963_ASA = 0x10;
620 | const uint8_t AK8963_WHO_AM_I = 0x00;
621 |
622 |
623 | /* writes a byte to MPU9250 register given a register address and data */
624 | int writeRegister(uint8_t subAddress, uint8_t data) {
625 | _i2c->beginTransmission(_address); // open the device
626 | _i2c->write(subAddress); // write the register address
627 | _i2c->write(data); // write the data
628 | _i2c->endTransmission();
629 |
630 | delay(10);
631 |
632 | /* read back the register */
633 | readRegisters(subAddress, 1, _buffer);
634 | /* check the read back register against the written register */
635 | if(_buffer[0] == data) {
636 | return 1;
637 | }
638 | else{
639 | return -1;
640 | }
641 | }
642 |
643 | /* reads registers from MPU9250 given a starting register address, number of bytes, and a pointer to store data */
644 | int readRegisters(uint8_t subAddress, uint8_t count, uint8_t* dest) {
645 | _i2c->beginTransmission(_address); // open the device
646 | _i2c->write(subAddress); // specify the starting register address
647 | _i2c->endTransmission(false);
648 | _numBytes = _i2c->requestFrom(_address, count); // specify the number of bytes to receive
649 | if (_numBytes == count) {
650 | for(uint8_t i = 0; i < count; i++) {
651 | dest[i] = _i2c->read();
652 | }
653 | return 1;
654 | } else {
655 | return -1;
656 | }
657 | }
658 |
659 | /* writes a register to the AK8963 given a register address and data */
660 | int writeAK8963Register(uint8_t subAddress, uint8_t data) {
661 | // set slave 0 to the AK8963 and set for write
662 | if (writeRegister(I2C_SLV0_ADDR,AK8963_I2C_ADDR) < 0) {
663 | return -1;
664 | }
665 | // set the register to the desired AK8963 sub address
666 | if (writeRegister(I2C_SLV0_REG,subAddress) < 0) {
667 | return -2;
668 | }
669 | // store the data for write
670 | if (writeRegister(I2C_SLV0_DO,data) < 0) {
671 | return -3;
672 | }
673 | // enable _i2c and send 1 byte
674 | if (writeRegister(I2C_SLV0_CTRL,I2C_SLV0_EN | (uint8_t)1) < 0) {
675 | return -4;
676 | }
677 | // read the register and confirm
678 | if (readAK8963Registers(subAddress,1,_buffer) < 0) {
679 | return -5;
680 | }
681 | if(_buffer[0] == data) {
682 | return 1;
683 | } else{
684 | return -6;
685 | }
686 | }
687 |
688 | /* reads registers from the AK8963 */
689 | int readAK8963Registers(uint8_t subAddress, uint8_t count, uint8_t* dest) {
690 | // set slave 0 to the AK8963 and set for read
691 | if (writeRegister(I2C_SLV0_ADDR,AK8963_I2C_ADDR | I2C_READ_FLAG) < 0) {
692 | return -1;
693 | }
694 | // set the register to the desired AK8963 sub address
695 | if (writeRegister(I2C_SLV0_REG,subAddress) < 0) {
696 | return -2;
697 | }
698 | // enable _i2c and request the bytes
699 | if (writeRegister(I2C_SLV0_CTRL,I2C_SLV0_EN | count) < 0) {
700 | return -3;
701 | }
702 | delay(1); // takes some time for these registers to fill
703 | // read the bytes off the MPU9250 EXT_SENS_DATA registers
704 | _status = readRegisters(EXT_SENS_DATA_00,count,dest);
705 | return _status;
706 | }
707 |
708 | /* gets the MPU9250 WHO_AM_I register value, expected to be 0x71 */
709 | int whoAmI() {
710 | // read the WHO AM I register
711 | if (readRegisters(WHO_AM_I,1,_buffer) < 0) {
712 | return -1;
713 | }
714 | // return the register value
715 | return _buffer[0];
716 | }
717 |
718 | /* gets the AK8963 WHO_AM_I register value, expected to be 0x48 */
719 | int whoAmIAK8963() {
720 | // read the WHO AM I register
721 | if (readAK8963Registers(AK8963_WHO_AM_I,1,_buffer) < 0) {
722 | return -1;
723 | }
724 | // return the register value
725 | return _buffer[0];
726 | }
727 |
728 | };
729 |
730 | #endif
731 |
--------------------------------------------------------------------------------
/ahrs_ukf/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) = Identitas * 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(Matrix &XInit, Matrix &P, Matrix &Rv, Matrix &Rn, bool (*bNonlinearUpdateX)(Matrix &, Matrix &, Matrix &), bool (*bNonlinearUpdateY)(Matrix &, Matrix &, Matrix &))
97 | {
98 | /* Initialization:
99 | * P (k=0|k=0) = Identitas * covariant(P(k=0)), typically initialized with some big number.
100 | * x(k=0|k=0) = Expected value of x at time-0 (i.e. x(k=0)), typically set to zero.
101 | * Rv, Rn = Covariance matrices of process & measurement. As this implementation
102 | * the noise as AWGN (and same value for every variable), this is set
103 | * to Rv=diag(RvInit,...,RvInit) and Rn=diag(RnInit,...,RnInit).
104 | */
105 | X_Est = XInit;
106 | this->P = P.Copy();
107 | this->Rv = Rv.Copy();
108 | this->Rn = Rn.Copy();
109 | this->bNonlinearUpdateX = bNonlinearUpdateX;
110 | this->bNonlinearUpdateY = bNonlinearUpdateY;
111 |
112 |
113 | /* Van der. Merwe, .. (2004). Sigma-Point Kalman Filters for Probabilistic Inference in Dynamic State-Space Models
114 | * (Ph.D. thesis). Oregon Health & Science University. Page 6:
115 | *
116 | * where λ = α2(L+κ)−L is a scaling parameter. α determines the spread of the sigma points around ̄x and is usually
117 | * set to a small positive value (e.g. 1e−2 ≤ α ≤ 1). κ is a secondary scaling parameter which is usually set to either
118 | * 0 or 3−L (see [45] for details), and β is an extra degree of freedom scalar parameter used to incorporate any extra
119 | * prior knowledge of the distribution of x (for Gaussian distributions, β = 2 is optimal).
120 | */
121 | float_prec _alpha = 1e-2;
122 | float_prec _k = 0.0;
123 | float_prec _beta = 2.0;
124 |
125 | /* lambda = (alpha^2)*(N+kappa)-N, gamma = sqrt(N+alpha) ...{UKF_1} */
126 | float_prec _lambda = (_alpha*_alpha)*(SS_X_LEN+_k) - SS_X_LEN;
127 | Gamma = sqrt((SS_X_LEN + _lambda));
128 |
129 |
130 | /* Wm = [lambda/(N+lambda) 1/(2(N+lambda)) ... 1/(2(N+lambda))] ...{UKF_2} */
131 | Wm[0][0] = _lambda/(SS_X_LEN + _lambda);
132 | for (int16_t _i = 1; _i < Wm.i16getCol(); _i++) {
133 | Wm[0][_i] = 0.5/(SS_X_LEN + _lambda);
134 | }
135 |
136 | /* Wc = [Wm(0)+(1-alpha(^2)+beta) 1/(2(N+lambda)) ... 1/(2(N+lambda))] ...{UKF_3} */
137 | Wc = Wm.Copy();
138 | Wc[0][0] = Wc[0][0] + (1.0-(_alpha*_alpha)+_beta);
139 | }
140 |
141 |
142 | void UKF::vReset(Matrix &XInit, Matrix &P, Matrix &Rv, Matrix &Rn)
143 | {
144 | X_Est = XInit;
145 | this->P = P.Copy();
146 | this->Rv = Rv.Copy();
147 | this->Rn = Rn.Copy();
148 | }
149 |
150 |
151 | bool UKF::bUpdate(Matrix &Y, Matrix &U)
152 | {
153 | /* Run once every sampling time */
154 |
155 | /* XSigma(k-1) = [x(k-1) Xs(k-1)+GPsq Xs(k-1)-GPsq] ...{UKF_4} */
156 | if (!bCalculateSigmaPoint()) {
157 | return false;
158 | }
159 |
160 |
161 | /* Unscented Transform XSigma [f,XSigma,u,Rv] -> [x,XSigma,P,DX]: ...{UKF_5a} - {UKF_8a} */
162 | if (!bUnscentedTransform(X_Est, X_Sigma, P, DX, bNonlinearUpdateX, X_Sigma, U, Rv)) {
163 | return false;
164 | }
165 |
166 | /* Unscented Transform YSigma [h,XSigma,u,Rn] -> [y_est,YSigma,Py,DY]: ...{UKF_5b} - {UKF_8b} */
167 | if (!bUnscentedTransform(Y_Est, Y_Sigma, Py, DY, bNonlinearUpdateY, X_Sigma, U, Rn)) {
168 | return false;
169 | }
170 |
171 |
172 | /* Calculate Cross-Covariance Matrix:
173 | * Pxy(k) = sum(Wc(i)*DX*DY(i)) ; i = 1 ... (2N+1) ...{UKF_9}
174 | */
175 | for (int16_t _i = 0; _i < DX.i16getRow(); _i++) {
176 | for (int16_t _j = 0; _j < DX.i16getCol(); _j++) {
177 | DX[_i][_j] *= Wc[0][_j];
178 | }
179 | }
180 | Pxy = DX * (DY.Transpose());
181 |
182 |
183 | /* Calculate the Kalman Gain:
184 | * K = Pxy(k) * (Py(k)^-1) ...{UKF_10}
185 | */
186 | Matrix PyInv = Py.Invers();
187 | if (!PyInv.bMatrixIsValid()) {
188 | return false;
189 | }
190 | Gain = Pxy * PyInv;
191 |
192 |
193 | /* Update the Estimated State Variable:
194 | * x(k|k) = x(k|k-1) + K * (y(k) - y_est(k)) ...{UKF_11}
195 | */
196 | Err = Y - Y_Est;
197 | X_Est = X_Est + (Gain*Err);
198 |
199 |
200 | /* Update the Covariance Matrix:
201 | * P(k|k) = P(k|k-1) - K*Py(k)*K' ...{UKF_12}
202 | */
203 | P = P - (Gain * Py * Gain.Transpose());
204 |
205 |
206 | return true;
207 | }
208 |
209 | bool UKF::bCalculateSigmaPoint(void)
210 | {
211 | /* Xs(k-1) = [x(k-1) ... x(k-1)] ; Xs(k-1) = NxN
212 | * GPsq = gamma * sqrt(P(k-1))
213 | * XSigma(k-1) = [x(k-1) Xs(k-1)+GPsq Xs(k-1)-GPsq] ...{UKF_4}
214 | */
215 | /* Use Cholesky Decomposition to compute sqrt(P) */
216 | P_Chol = P.CholeskyDec();
217 | if (!P_Chol.bMatrixIsValid()) {
218 | /* System Fail */
219 | return false;
220 | }
221 | P_Chol = P_Chol * Gamma;
222 |
223 | /* Xs(k-1) = [x(k-1) ... x(k-1)] ; Xs(k-1) = NxN */
224 | Matrix _Y(SS_X_LEN, SS_X_LEN);
225 | for (int16_t _i = 0; _i < SS_X_LEN; _i++) {
226 | _Y = _Y.InsertVector(X_Est, _i);
227 | }
228 |
229 | X_Sigma.vSetToZero();
230 | /* XSigma(k-1) = [x(k-1) 0 0] */
231 | X_Sigma = X_Sigma.InsertVector(X_Est, 0);
232 | /* XSigma(k-1) = [x(k-1) Xs(k-1)+GPsq 0] */
233 | X_Sigma = X_Sigma.InsertSubMatrix((_Y + P_Chol), 0, 1);
234 | /* XSigma(k-1) = [x(k-1) Xs(k-1)+GPsq Xs(k-1)-GPsq] */
235 | X_Sigma = X_Sigma.InsertSubMatrix((_Y - P_Chol), 0, (1+SS_X_LEN));
236 |
237 | return true;
238 | }
239 |
240 | bool UKF::bUnscentedTransform(Matrix &Out, Matrix &OutSigma, Matrix &P, Matrix &DSig,
241 | bool (*_vFuncNonLinear)(Matrix &xOut, Matrix &xInp, Matrix &U),
242 | Matrix &InpSigma, Matrix &InpVector,
243 | Matrix &_CovNoise)
244 | {
245 | /* XSigma(k) = f(XSigma(k-1), u(k-1)) ...{UKF_5a} */
246 | /* x(k|k-1) = sum(Wm(i) * XSigma(k)(i)) ; i = 1 ... (2N+1) ...{UKF_6a} */
247 | Out.vSetToZero();
248 | for (int16_t _j = 0; _j < InpSigma.i16getCol(); _j++) {
249 | /* Transform the column submatrix of sigma-points input matrix (InpSigma) */
250 | Matrix _AuxSigma1(InpSigma.i16getRow(), 1);
251 | Matrix _AuxSigma2(OutSigma.i16getRow(), 1);
252 | for (int16_t _i = 0; _i < InpSigma.i16getRow(); _i++) {
253 | _AuxSigma1[_i][0] = InpSigma[_i][_j];
254 | }
255 | if (!_vFuncNonLinear(_AuxSigma2, _AuxSigma1, InpVector)) {
256 | /* Somehow the transformation function is failed, propagate the error */
257 | return false;
258 | }
259 |
260 | /* Combine the transformed vector to construct sigma-points output matrix (OutSigma) */
261 | OutSigma = OutSigma.InsertVector(_AuxSigma2, _j);
262 |
263 | /* Calculate x(k|k-1) as weighted mean of OutSigma */
264 | _AuxSigma2 = _AuxSigma2 * Wm[0][_j];
265 | Out = Out + _AuxSigma2;
266 | }
267 |
268 | /* DX = XSigma(k)(i) - Xs(k) ; Xs(k) = [x(k|k-1) ... x(k|k-1)]
269 | * ; Xs(k) = Nx(2N+1) ...{UKF_7a} */
270 | Matrix _AuxSigma1(OutSigma.i16getRow(), OutSigma.i16getCol());
271 | for (int16_t _j = 0; _j < OutSigma.i16getCol(); _j++) {
272 | _AuxSigma1 = _AuxSigma1.InsertVector(Out, _j);
273 | }
274 | DSig = OutSigma - _AuxSigma1;
275 |
276 | /* P(k|k-1) = sum(Wc(i)*DX*DX') + Rv ; i = 1 ... (2N+1) ...{UKF_8a} */
277 | _AuxSigma1 = DSig.Copy();
278 | for (int16_t _i = 0; _i < DSig.i16getRow(); _i++) {
279 | for (int16_t _j = 0; _j < DSig.i16getCol(); _j++) {
280 | _AuxSigma1[_i][_j] *= Wc[0][_j];
281 | }
282 | }
283 | P = (_AuxSigma1 * (DSig.Transpose())) + _CovNoise;
284 |
285 | return true;
286 | }
287 |
288 |
289 |
290 |
291 |
--------------------------------------------------------------------------------
/ahrs_ukf/ukf.h:
--------------------------------------------------------------------------------
1 | #ifndef UKF_H
2 | #define UKF_H
3 |
4 | #include "konfig.h"
5 | #include "matrix.h"
6 |
7 |
8 | #if (MATRIX_MAXIMUM_SIZE < (2*SS_X_LEN + 1))
9 | #error("MATRIX_MAXIMUM_SIZE is not big enough for UKF (need at least (2*SS_X_LEN + 1))");
10 | #endif
11 | #if ((MATRIX_MAXIMUM_SIZE < SS_U_LEN) || (MATRIX_MAXIMUM_SIZE < SS_X_LEN) || (MATRIX_MAXIMUM_SIZE < SS_Z_LEN))
12 | #error("MATRIX_MAXIMUM_SIZE is not big enough for UKF (need at least SS_U_LEN / SS_X_LEN / SS_Z_LEN)");
13 | #endif
14 |
15 | class UKF
16 | {
17 | public:
18 | UKF(Matrix &XInit, Matrix &P, Matrix &Rv, Matrix &Rn, bool (*bNonlinearUpdateX)(Matrix &, Matrix &, Matrix &), bool (*bNonlinearUpdateY)(Matrix &, Matrix &, Matrix &));
19 | void vReset(Matrix &XInit, Matrix &P, Matrix &Rv, Matrix &Rn);
20 | bool bUpdate(Matrix &Y, Matrix &U);
21 |
22 | Matrix GetX() { return X_Est; }
23 | Matrix GetY() { return Y_Est; }
24 | Matrix GetP() { return P; }
25 | Matrix GetErr() { return Err; }
26 |
27 | protected:
28 | bool bCalculateSigmaPoint();
29 | bool bUnscentedTransform(Matrix &Out, Matrix &OutSigma, Matrix &P, Matrix &DSig,
30 | bool (*_vFuncNonLinear)(Matrix &xOut, Matrix &xInp, Matrix &U),
31 | Matrix &InpSigma, Matrix &InpVector,
32 | Matrix &_CovNoise);
33 |
34 | private:
35 | bool (*bNonlinearUpdateX)(Matrix &X_Next, Matrix &X, Matrix &U);
36 | bool (*bNonlinearUpdateY)(Matrix &Y, Matrix &X, Matrix &U);
37 |
38 | Matrix X_Est{SS_X_LEN, 1};
39 | Matrix X_Sigma{SS_X_LEN, (2*SS_X_LEN + 1)};
40 |
41 | Matrix Y_Est{SS_Z_LEN, 1};
42 | Matrix Y_Sigma{SS_Z_LEN, (2*SS_X_LEN + 1)};
43 |
44 | Matrix P{SS_X_LEN, SS_X_LEN};
45 | Matrix P_Chol{SS_X_LEN, SS_X_LEN};
46 |
47 | Matrix DX{SS_X_LEN, (2*SS_X_LEN + 1)};
48 | Matrix DY{SS_Z_LEN, (2*SS_X_LEN + 1)};
49 |
50 | Matrix Py{SS_Z_LEN, SS_Z_LEN};
51 | Matrix Pxy{SS_X_LEN, SS_Z_LEN};
52 |
53 | Matrix Wm{1, (2*SS_X_LEN + 1)};
54 | Matrix Wc{1, (2*SS_X_LEN + 1)};
55 |
56 | Matrix Rv{SS_X_LEN, SS_X_LEN};
57 | Matrix Rn{SS_Z_LEN, SS_Z_LEN};
58 |
59 | Matrix Err{SS_Z_LEN, 1};
60 | Matrix Gain{SS_X_LEN, SS_Z_LEN};
61 | float_prec Gamma;
62 | };
63 |
64 | #endif // UKF_H
65 |
--------------------------------------------------------------------------------
/ekf_imu_float32_mute_rot2.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/pronenewbits/Arduino_AHRS_System/aa469bd09ff2d7986ac7f3755f1d17c288f5b3e8/ekf_imu_float32_mute_rot2.gif
--------------------------------------------------------------------------------
/eq_render/bias_sensor.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/pronenewbits/Arduino_AHRS_System/aa469bd09ff2d7986ac7f3755f1d17c288f5b3e8/eq_render/bias_sensor.gif
--------------------------------------------------------------------------------
/eq_render/deformity_sensor.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/pronenewbits/Arduino_AHRS_System/aa469bd09ff2d7986ac7f3755f1d17c288f5b3e8/eq_render/deformity_sensor.gif
--------------------------------------------------------------------------------
/eq_render/eq_latex:
--------------------------------------------------------------------------------
1 |
2 |
3 | bias_sensor.gif B=\begin{bmatrix}B_{x}&B_{y}&B_{z}\end{bmatrix}^T
4 | deformity_sensor.gif \begin{bmatrix}D_{xx}&D_{xy}&D_{xz}\\D_{yx}&D_{yy}&D_{yz}\\D_{zx}&D_{zy}&D_{zz}\end{bmatrix}
5 | true_sensor.gif T_{true}=\begin{bmatrix}T_{x}&T_{y}&T_{z}\end{bmatrix}^T
6 | measured_sensor.gif S_{sensor}=\begin{bmatrix}S_{x}&S_{y}&S_{z}\end{bmatrix}^T
7 |
8 | sensor_calib_eq.gif \begin{bmatrix}T_{x}\\T_{y}\\T_{z}\end{bmatrix}=
9 | \begin{bmatrix}D_{xx}&D_{xy}&D_{xz}\\D_{yx}&D_{yy}&D_{yz}\\D_{zx}&D_{zy}&D_{zz}\end{bmatrix}
10 | \left(\begin{bmatrix}S_{x}\\S_{y}\\S_{z}\end{bmatrix}-\begin{bmatrix}B_{x}\\B_{y}\\B_{z}\end{bmatrix}\right)
11 |
12 |
13 |
14 |
--------------------------------------------------------------------------------
/eq_render/measured_sensor.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/pronenewbits/Arduino_AHRS_System/aa469bd09ff2d7986ac7f3755f1d17c288f5b3e8/eq_render/measured_sensor.gif
--------------------------------------------------------------------------------
/eq_render/sensor_calib_eq.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/pronenewbits/Arduino_AHRS_System/aa469bd09ff2d7986ac7f3755f1d17c288f5b3e8/eq_render/sensor_calib_eq.gif
--------------------------------------------------------------------------------
/eq_render/true_sensor.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/pronenewbits/Arduino_AHRS_System/aa469bd09ff2d7986ac7f3755f1d17c288f5b3e8/eq_render/true_sensor.gif
--------------------------------------------------------------------------------
/ukf_imu_double64_mute_rot2.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/pronenewbits/Arduino_AHRS_System/aa469bd09ff2d7986ac7f3755f1d17c288f5b3e8/ukf_imu_double64_mute_rot2.gif
--------------------------------------------------------------------------------