├── LICENSE
├── README.md
├── examples
└── simple_example
│ └── simple_example.ino
├── extras
└── images
│ ├── accel_LPF_0-1.png
│ ├── accel_LPF_0-9.png
│ ├── accelerometer_only.png
│ ├── complementary_filter.png
│ ├── filter_comparisons.jpg
│ └── gyro_only.png
├── keywords.txt
├── library.properties
└── src
├── simpleFusion.cpp
└── simpleFusion.h
/LICENSE:
--------------------------------------------------------------------------------
1 | MIT License
2 |
3 | Copyright (c) 2021 Sean Boerhout
4 |
5 | Permission is hereby granted, free of charge, to any person obtaining a copy
6 | of this software and associated documentation files (the "Software"), to deal
7 | in the Software without restriction, including without limitation the rights
8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
9 | copies of the Software, and to permit persons to whom the Software is
10 | furnished to do so, subject to the following conditions:
11 |
12 | The above copyright notice and this permission notice shall be included in all
13 | copies or substantial portions of the Software.
14 |
15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 | SOFTWARE.
22 |
--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 |
2 |
3 | 
4 |
5 | SimpleFusion
6 |
7 |
8 |
9 |
10 | A library that fuses accelerometer and gyroscope readings quickly and easily with a complementary filter.
11 |
12 |
13 | 
14 | 
15 | 
16 |
17 |
18 | ## Overview
19 | This library combines accelerometer (in meters / second ²) and gyroscope (in radians / second) data to output the IMU's rotation (Euler angles). It is accurate
20 | up to ± 90 degrees.
21 |
22 | Note the the lxibrary defines pitch to be a rotation about the y-axis and roll to be a rotation about the x-axis.
23 |
24 | ## Benefits
25 | Here's a comparison of different high-speed methods for estimating pitch and roll.
26 |
27 |
28 |
29 | 
30 |
31 |
32 |
33 | The complementary filter returns smooth readings despite non-zero acceleration.
34 |
35 | ## License
36 |
37 | MIT
38 |
--------------------------------------------------------------------------------
/examples/simple_example/simple_example.ino:
--------------------------------------------------------------------------------
1 | // ******************************************************
2 | // An example of the SimpleFusion library that combines
3 | // accelerometer and gyroscope data quickly and easily.
4 | //
5 | // This example uses the mpu6050 6-dof IMU and the
6 | // Adafruit library for it.
7 | //
8 | // Created in 2021 by Sean Boerhout under the MIT License
9 | // ******************************************************
10 |
11 |
12 | #include // If you've used the "include library" tool, you'll need to use '<[library]>'
13 |
14 | #include
15 | #include
16 | #include
17 |
18 | SimpleFusion fuser; // Initialize the SimpleFusion object
19 | Adafruit_MPU6050 mpu;
20 |
21 | void setup() {
22 | Serial.begin(9600);
23 | while(!Serial)
24 | ;
25 |
26 | fuser.init(100, 0.98, 0.98); // Initialize the fusion object with the filter update rate (hertz) and
27 |
28 | if (!mpu.begin()) {
29 | Serial.println("Failed to find MPU6050 chip");
30 | while (1)
31 | delay(10);
32 | }
33 |
34 | }
35 |
36 | void loop() {
37 |
38 | if (fuser.shouldUpdateData()) {
39 |
40 | FusedAngles fusedAngles; // Variable to store the output
41 |
42 | sensors_event_t a, g, temp;
43 | mpu.getEvent(&a, &g, &temp);
44 |
45 | ThreeAxis accelerometer; // Please verify that the units are in meters / second ^ 2
46 | accelerometer.x = a.acceleration.x;
47 | accelerometer.y = a.acceleration.y;
48 | accelerometer.z = a.acceleration.z;
49 |
50 | ThreeAxis gyroscope; // Please verify that the units are in raidans / second
51 | gyroscope.x = g.gyro.x;
52 | gyroscope.y = g.gyro.y;
53 | gyroscope.z = g.gyro.z;
54 |
55 | fuser.getFilteredAngles(accelerometer, gyroscope, &fusedAngles, UNIT_DEGREES); // Fuse the angles
56 |
57 | Serial.print(" Pitch: ");
58 | Serial.print(fusedAngles.pitch);
59 | Serial.print(" Roll : ");
60 | Serial.println(fusedAngles.roll);
61 | }
62 |
63 | }
64 |
--------------------------------------------------------------------------------
/extras/images/accel_LPF_0-1.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/seanboe/SimpleFusion/ed741464a776fdf9fb7780105ca928c44b4c4df3/extras/images/accel_LPF_0-1.png
--------------------------------------------------------------------------------
/extras/images/accel_LPF_0-9.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/seanboe/SimpleFusion/ed741464a776fdf9fb7780105ca928c44b4c4df3/extras/images/accel_LPF_0-9.png
--------------------------------------------------------------------------------
/extras/images/accelerometer_only.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/seanboe/SimpleFusion/ed741464a776fdf9fb7780105ca928c44b4c4df3/extras/images/accelerometer_only.png
--------------------------------------------------------------------------------
/extras/images/complementary_filter.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/seanboe/SimpleFusion/ed741464a776fdf9fb7780105ca928c44b4c4df3/extras/images/complementary_filter.png
--------------------------------------------------------------------------------
/extras/images/filter_comparisons.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/seanboe/SimpleFusion/ed741464a776fdf9fb7780105ca928c44b4c4df3/extras/images/filter_comparisons.jpg
--------------------------------------------------------------------------------
/extras/images/gyro_only.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/seanboe/SimpleFusion/ed741464a776fdf9fb7780105ca928c44b4c4df3/extras/images/gyro_only.png
--------------------------------------------------------------------------------
/keywords.txt:
--------------------------------------------------------------------------------
1 | #######################################
2 | # Syntax Coloring Map SimpleFusion
3 | #######################################
4 |
5 | #######################################
6 | # Datatypes (KEYWORD1)
7 | #######################################
8 |
9 | SimpleFusion KEYWORD1
10 |
11 | #######################################
12 | # Methods and Functions (KEYWORD2)
13 | #######################################
14 |
15 | init KEYWORD2
16 | getFilteredAngles KEYWORD2
17 | shouldUpdateData KEYWORD2
18 |
19 | #######################################
20 | # Structures (KEYWORD3)
21 | #######################################
22 |
23 | ThreeAxis KEYWORD3
24 | FusedAngles KEYWORD3
25 |
26 | #######################################
27 | # Constants (LITERAL1)
28 | #######################################
29 |
30 | DATA_UPDATE_POLL_TOLERANCE LITERAL1
31 | UNIT_DEGREES LITERAL1
32 | UNIT_RADIANS LITERAL1
--------------------------------------------------------------------------------
/library.properties:
--------------------------------------------------------------------------------
1 | name=SimpleFusion
2 | version=1.0.6
3 | author=Sean Boerhout
4 | maintainer=Sean Boerhout, boeseany21@gmail.com
5 | sentence=Simple IMU fusion with a complementary filter.
6 | paragraph=Get Pitch and Roll estimations easily with any 6-DOF IMU!
7 | category=Data Processing
8 | url=https://github.com/seanboe/SimpleFusion
9 | architectures=*
10 |
--------------------------------------------------------------------------------
/src/simpleFusion.cpp:
--------------------------------------------------------------------------------
1 | #include "simpleFusion.h"
2 |
3 | SimpleFusion::SimpleFusion(void) {};
4 |
5 |
6 | /*!
7 | * @brief Initializes filter parameters.
8 | * @param filterUpdateRate The frequency of filter updates up to 1000000 Hertz
9 | * @param pitchGyroFavoring The amount that the gyroscope is favored in the pitch direction as a decimal percent less than 1
10 | * @param rollGyroFavoring The amount that the gyroscope is favored in the roll direction as a decimal percent less than 1
11 | * @returns false if any gyro favoring is an invalid value, true if they are valid.
12 | */
13 | bool SimpleFusion::init(int16_t filterUpdateRate, float pitchGyroFavoring, float rollGyroFavoring) {
14 | _filterUpdateRate = filterUpdateRate;
15 | _pitchGyroFavoring = pitchGyroFavoring;
16 | _rollGyroFavoring = rollGyroFavoring;
17 |
18 | if (_pitchGyroFavoring > 1 || _pitchGyroFavoring < 0)
19 | return false;
20 | if (_rollGyroFavoring > 1 || _rollGyroFavoring < 0)
21 | return false;
22 |
23 | _previousTime = millis();
24 | _justUpdatedData = false;
25 |
26 | _pitch = 0;
27 | _roll = 0;
28 |
29 | return true;
30 | };
31 |
32 | /*!
33 | * @brief Allows you to update the library at the desired frequency. You should update the sensor only when the library is running
34 | * @returns true if it is time to update the library, false if it isn't
35 | */
36 | bool SimpleFusion::shouldUpdateData() {
37 | long dt = (millis() - _previousTime);
38 |
39 | if ((dt % (1000 / _filterUpdateRate) <= DATA_UPDATE_POLL_TOLERANCE) && (_justUpdatedData == false)) {
40 | _justUpdatedData = true;
41 | return true;
42 | }
43 | else if ((dt % (1000 / _filterUpdateRate) > DATA_UPDATE_POLL_TOLERANCE))
44 | _justUpdatedData = false;
45 |
46 | return false;
47 | };
48 |
49 | /*!
50 | * @brief Calculates rotation angles based on gyroscope and accelerometer readings
51 | * @param accelerometer The accelerometer readings from the IMU as ThreeAxis struct variables (Units are m/s^2)
52 | * @param gyroscope The gyroscope readings from the IMU as ThreeAxis struct variables (Units are radians/second)
53 | * @param angleOutputs The address of a FusedAngles struct variable for holding angular outputs
54 | */
55 | void SimpleFusion::getFilteredAngles(ThreeAxis &accelerometer, ThreeAxis &gyroscope, FusedAngles *angleOutputs, AngleUnit angleUnit) {
56 |
57 | float pitchFromAccel = 0;
58 | float rollFromAccel = 0;
59 |
60 | pitchFromAccel = atan2(-accelerometer.x , sqrt(pow(accelerometer.y, 2) + pow(accelerometer.z, 2)));
61 | rollFromAccel = atan2(accelerometer.y , sqrt(pow(accelerometer.x, 2) + pow(accelerometer.z, 2)));
62 | // rollFromAccel = atan2(accelerometer.y, accelerometer.z);
63 |
64 | // Complimentary Filter
65 | _pitch = (_pitchGyroFavoring) * (_pitch + (gyroscope.y * (1.00 / _filterUpdateRate))) + (1.00 - _pitchGyroFavoring) * (pitchFromAccel);
66 | _roll = (_rollGyroFavoring) * (_roll + (gyroscope.x * (1.00 / _filterUpdateRate))) + (1.00 - _rollGyroFavoring) * (rollFromAccel);
67 |
68 |
69 | switch (angleUnit) {
70 | case UNIT_DEGREES:
71 | angleOutputs->pitch = _pitch * (180 / PI);
72 | angleOutputs->roll = _roll * (180 / PI);
73 | break;
74 | case UNIT_RADIANS:
75 | angleOutputs->pitch = _pitch;
76 | angleOutputs->roll = _roll;
77 | break;
78 | }
79 |
80 | };
81 |
82 |
--------------------------------------------------------------------------------
/src/simpleFusion.h:
--------------------------------------------------------------------------------
1 | #ifndef SIMPLE_FUSION
2 | #define SIMPLE_FUSION
3 |
4 | #include
5 |
6 | #define DATA_UPDATE_POLL_TOLERANCE 5 // The leniency of shouldUpdateData(), in microseconds
7 |
8 | typedef struct {
9 | float x;
10 | float y;
11 | float z;
12 | } ThreeAxis;
13 |
14 | typedef struct {
15 | float roll;
16 | float pitch;
17 | } FusedAngles;
18 |
19 | typedef enum {
20 | UNIT_DEGREES, UNIT_RADIANS
21 | } AngleUnit;
22 |
23 | class SimpleFusion {
24 |
25 | public:
26 | SimpleFusion();
27 | bool init(int16_t filterUpdateRate, float pitchGyroFavoring, float rollGyroFavoring);
28 | void getFilteredAngles(ThreeAxis &accelerometer, ThreeAxis &gyroscope, FusedAngles *angleOutputs, AngleUnit angleUnit);
29 |
30 | bool shouldUpdateData();
31 |
32 | private:
33 |
34 | int16_t _filterUpdateRate; // Hertz, less than 1000000
35 | float _pitchGyroFavoring; // The amount that the gyro is favored (alpha)
36 | float _rollGyroFavoring;
37 |
38 | float _pitch;
39 | float _roll;
40 |
41 | long _previousTime;
42 | bool _justUpdatedData;
43 | };
44 |
45 | #endif
--------------------------------------------------------------------------------