├── README.md ├── examples ├── heading │ └── heading.ino ├── output │ └── output.ino ├── vector_output │ └── vector_output.ino └── velocity │ └── velocity.ino ├── keywords.txt ├── library.properties ├── license.txt └── src ├── accIntegral.cpp ├── accIntegral.h ├── imuFilter.cpp └── imuFilter.h /README.md: -------------------------------------------------------------------------------- 1 | # imuFilter 2 | This library fuses the outputs of an inertial measurement unit (IMU) and stores the heading as a quaternion. It uses a _kalman-like_ filter to check the acceleration and see if it lies within a deviation from (0,0,1)g. If the acceleration is within this band, it will strongly correct the orientation. However, if the acceleration lies outside of this band, it will barely affect the orientation. To this end, the deviation from vertical is used to update the variance and kalman gain: 3 | 4 | $\ \overrightarrow{a_{rel}} = \overrightarrow{a_{local}} - (0,0,1) $ 5 | 6 | $\ K_{\sigma} = 1/(1 + \frac{ {\sigma}^2 }{ {\sigma}_{acc}^2 } ) $ 7 | 8 | $\ {\sigma}^2 = | \overrightarrow{a_{rel}} |^2 + K_{\sigma}{\sigma}^2 $ 9 | 10 | The kalman gain then scaled by a delay parameter and used to correct the attitude. This scaling allows the filter to act like a 1rst-order low pass filter that smoothens the acceleration at the cost of slower response: 11 | 12 | $\ E_{k} = \theta_{accel} - \theta_{k-1} $ 13 | 14 | $\ K_{\theta} = {\alpha} K_{\sigma} {\Delta t} $ 15 | 16 | $\ \theta_{k} = \theta_{k-1} + \dot{\theta}{\Delta t} + K_{\theta}E_{k} $ 17 | 18 | As the filter uses a quaternion to encode rotations, it's easy to perform coordinate transformations. The library has functions to: 19 | - Transfor a vector to the local or global frame. 20 | - Get the unit vectors of the X, Y and Z axes in the local or global frame. 21 | 22 | Moreover, since a 6-axis IMU (gyro-accelerometer) cannot measure an absolute heading, a function is included to rotate the orientation about the vertical (yaw) axis. One can use vector operations to correct the heading with an additional sensor like a magnetometer. 23 | 24 | # Velocity estimate 25 | The library can integrate acceleration to obtain an estimate of velocity. This is accomplished by using a set of Kalman-like filters like the one shown above. The acceleration is checked against a rest condition of (0,0,0)g and any deviations that lie within a specified uncertainty band are suppressed. This eliminates much of the bias due to gravity or miscalibration: 26 | 27 | $\ K_{acc} = 1/(1 + \frac{ S_{acc}^2 }{ {\sigma}_{acc}^2 } ) $ 28 | 29 | $\ S_{acc}^2 = | \overrightarrow{a_{rel}} |^2 + K_{acc}S_{acc}^2 $ 30 | 31 | $\ a_{k} = a_{k} - \overline{a_{k}} $ 32 | 33 | $\ \overline{a_{k}} = \overline{a_{k-1}} + K_{acc}{a_{k}} $ 34 | 35 | Afterward, the deviation of the velocity (relative to a known target velocity), along with the variance of the acceleration, is used to determine the Kalman gain of the velocity. This relationship causes small velocity changes or small accelerations to reduce the gain, and the velocity is forced to match the target velocity: 36 | 37 | $\ \Delta{V} = V_{k-1} - V_{target} $ 38 | 39 | $\ S_{vel}^2 = | \overrightarrow{ {\Delta}V } |^2 + K_{vel}S_{vel}^2 $ 40 | 41 | $\ K_{vel} = 1/(1 + ( \frac{ S_{vel} S_{acc} }{ \sigma_{vel} \sigma_{acc} } )^2 ) $ 42 | 43 | The velocity is then updated using the trapezoidal rule to integrate the filtered acceleration: 44 | 45 | $\ V_{k} = V_{k-1} + K_{vel}{\Delta}{V} + \frac{dt}{2}( a_{k} + a_{k-1} ) $ 46 | 47 | # Dependecies 48 | This library depends on the [vector_datatype](https://github.com/RCmags/vector_datatype) library. 49 | 50 | # References 51 | The form of the filter is inspired by the [SimpleKalmanFilter](https://github.com/denyssene/SimpleKalmanFilter) library. 52 | -------------------------------------------------------------------------------- /examples/heading/heading.ino: -------------------------------------------------------------------------------- 1 | /* 2 | This sketch shows to rotate the heading (yaw angle) of the estimated orientation. 3 | */ 4 | 5 | #include // Library for IMU sensor. See this link: https://github.com/RCmags/basicMPU6050 6 | #include 7 | 8 | basicMPU6050<> imu; 9 | 10 | imuFilter fusion; 11 | 12 | void setup() { 13 | Serial.begin(38400); 14 | 15 | // Calibrate imu 16 | imu.setup(); 17 | imu.setBias(); 18 | 19 | // Initialize filter: 20 | fusion.setup( imu.ax(), imu.ay(), imu.az() ); 21 | 22 | // Rotate heading: 23 | float angle = 45 * DEG_TO_RAD; // angle in radians to rotate heading about z-axis 24 | fusion.rotateHeading( angle, LARGE_ANGLE ); // Can choose LARGE_ANGLE or SMALL_ANGLE approximation 25 | } 26 | 27 | void loop() { 28 | // Update filter: 29 | fusion.update( imu.gx(), imu.gy(), imu.gz(), imu.ax(), imu.ay(), imu.az() ); 30 | 31 | // Display angles: 32 | Serial.print( fusion.pitch() ); 33 | Serial.print( " " ); 34 | Serial.print( fusion.yaw() ); 35 | Serial.print( " " ); 36 | Serial.print( fusion.roll() ); 37 | Serial.println(); 38 | } 39 | -------------------------------------------------------------------------------- /examples/output/output.ino: -------------------------------------------------------------------------------- 1 | /* 2 | This sketch shows to initialize the filter and update it with the imu output. It also shows how to get the euler angles of the estimated heading orientation. 3 | */ 4 | 5 | /* NOTE: The accelerometer MUST be calibrated for the fusion to work. Adjust the 6 | AX, AY, AND AZ offsets until the sensor reads (0,0,0) at rest. 7 | */ 8 | 9 | #include // Library for IMU sensor. See this link: https://github.com/RCmags/basicMPU6050 10 | #include 11 | 12 | // Gyro settings: 13 | #define LP_FILTER 3 // Low pass filter. Value from 0 to 6 14 | #define GYRO_SENS 0 // Gyro sensitivity. Value from 0 to 3 15 | #define ACCEL_SENS 0 // Accelerometer sensitivity. Value from 0 to 3 16 | #define ADDRESS_A0 LOW // I2C address from state of A0 pin. A0 -> GND : ADDRESS_A0 = LOW 17 | // A0 -> 5v : ADDRESS_A0 = HIGH 18 | // Accelerometer offset: 19 | constexpr int AX_OFFSET = 0; // Use these values to calibrate the accelerometer. The sensor should output 1.0g if held level. 20 | constexpr int AY_OFFSET = 0; // These values are unlikely to be zero. 21 | constexpr int AZ_OFFSET = 0; 22 | 23 | //-- Set the template parameters: 24 | 25 | basicMPU6050imu; 28 | 29 | // =========== Settings =========== 30 | imuFilter fusion; 31 | 32 | #define GAIN 0.5 /* Fusion gain, value between 0 and 1 - Determines orientation correction with respect to gravity vector. 33 | If set to 1 the gyroscope is dissabled. If set to 0 the accelerometer is dissabled (equivant to gyro-only) */ 34 | 35 | #define SD_ACCEL 0.2 /* Standard deviation of acceleration. Accelerations relative to (0,0,1)g outside of this band are suppresed. 36 | Accelerations within this band are used to update the orientation. [Measured in g-force] */ 37 | 38 | #define FUSION true /* Enable sensor fusion. Setting to "true" enables gravity correction */ 39 | 40 | void setup() { 41 | Serial.begin(38400); 42 | 43 | // Calibrate imu 44 | imu.setup(); 45 | imu.setBias(); 46 | 47 | #if FUSION 48 | // Set quaternion with gravity vector 49 | fusion.setup( imu.ax(), imu.ay(), imu.az() ); 50 | #else 51 | // Just use gyro 52 | fusion.setup(); 53 | #endif 54 | } 55 | 56 | void loop() { 57 | // Update filter: 58 | 59 | #if FUSION 60 | /* NOTE: GAIN and SD_ACCEL are optional parameters */ 61 | fusion.update( imu.gx(), imu.gy(), imu.gz(), imu.ax(), imu.ay(), imu.az(), GAIN, SD_ACCEL ); 62 | #else 63 | // Only use gyroscope 64 | fusion.update( imu.gx(), imu.gy(), imu.gz() ); 65 | #endif 66 | 67 | // Display angles: 68 | Serial.print( fusion.pitch() ); 69 | Serial.print( " " ); 70 | Serial.print( fusion.yaw() ); 71 | Serial.print( " " ); 72 | Serial.print( fusion.roll() ); 73 | 74 | // timestep 75 | Serial.print( " " ); 76 | Serial.print( fusion.timeStep(), 6 ); 77 | 78 | Serial.println(); 79 | } 80 | -------------------------------------------------------------------------------- /examples/vector_output/vector_output.ino: -------------------------------------------------------------------------------- 1 | /* 2 | This sketch shows to get the axis projections from the orientation estimate. It also shows how to project a vector in the global or local reference frame. 3 | */ 4 | 5 | #include // Library for IMU sensor. See this link: https://github.com/RCmags/basicMPU6050 6 | #include 7 | 8 | basicMPU6050<> imu; 9 | 10 | imuFilter fusion; 11 | 12 | // ========= functions =========== 13 | 14 | void printVector( vec3_t r ) { 15 | Serial.print( r.x, 2 ); 16 | Serial.print( "," ); 17 | Serial.print( r.y, 2 ); 18 | Serial.print( "," ); 19 | Serial.print( r.z, 2 ); 20 | } 21 | 22 | void printQuat( quat_t q ) { 23 | Serial.print( q.w ); 24 | Serial.print( "," ); 25 | printVector( q.v ); 26 | } 27 | 28 | void setup() { 29 | Serial.begin(38400); 30 | 31 | // Calibrate imu 32 | imu.setup(); 33 | imu.setBias(); 34 | 35 | // Initialize filter: 36 | fusion.setup( imu.ax(), imu.ay(), imu.az() ); 37 | } 38 | 39 | void loop() { 40 | // Update filter: 41 | fusion.update( imu.gx(), imu.gy(), imu.gz(), imu.ax(), imu.ay(), imu.az() ); 42 | 43 | // Unit vectors of rectangular coodinates [Choose between GLOBAL_FRAME and LOCAL_FRAME] 44 | vec3_t x = fusion.getXaxis(GLOBAL_FRAME); 45 | vec3_t y = fusion.getYaxis(GLOBAL_FRAME); 46 | vec3_t z = fusion.getZaxis(GLOBAL_FRAME); 47 | 48 | const vec3_t VEC = {1, 1, 0}; 49 | vec3_t v = fusion.projectVector(VEC, GLOBAL_FRAME); 50 | 51 | // Display vectors: 52 | Serial.print( " x = " ); 53 | printVector( x ); 54 | Serial.print( " | y = " ); 55 | printVector( y ); 56 | Serial.print( " | z = " ); 57 | printVector( z ); 58 | Serial.print( " | v = " ); 59 | printVector( v ); 60 | 61 | // Display quaternion 62 | Serial.print( " | q = " ); 63 | printQuat( fusion.getQuat() ); 64 | Serial.println(); 65 | } 66 | -------------------------------------------------------------------------------- /examples/velocity/velocity.ino: -------------------------------------------------------------------------------- 1 | /* 2 | This sketch shows to integrate acceleration to obtain an estimate of velocity 3 | */ 4 | 5 | /* NOTE: 6 | 1. The accelerometer MUST be calibrated to integrate its output. Adjust the 7 | AX, AY, AND AZ offsets until the sensor reads (0,0,1) at rest. 8 | 9 | 2. REQUIRED UNITS: 10 | I. The gyroscope must output: radians/second 11 | II. The accelerometer output: g-force [outputs 1 when sensor is static] 12 | */ 13 | 14 | #include // Library for IMU sensor. See this link: https://github.com/RCmags/basicMPU6050 15 | #include 16 | 17 | // ========== IMU sensor ========== 18 | 19 | // Gyro settings: 20 | #define LP_FILTER 3 // Low pass filter. Value from 0 to 6 21 | #define GYRO_SENS 0 // Gyro sensitivity. Value from 0 to 3 22 | #define ACCEL_SENS 0 // Accelerometer sensitivity. Value from 0 to 3 23 | #define ADDRESS_A0 LOW // I2C address from state of A0 pin. A0 -> GND : ADDRESS_A0 = LOW 24 | // A0 -> 5v : ADDRESS_A0 = HIGH 25 | // Accelerometer offset: 26 | constexpr int AX_OFFSET = 0; // Use these values to calibrate the accelerometer. The sensor should output 1.0g if held level. 27 | constexpr int AY_OFFSET = 0; // These values are unlikely to be zero. 28 | constexpr int AZ_OFFSET = 0; 29 | 30 | //-- Set template parameters: 31 | 32 | basicMPU6050imu; 35 | 36 | // =========== Settings =========== 37 | accIntegral fusion; 38 | 39 | // Filter coefficients // Unit 40 | constexpr float GRAVITY = 9.81e3; // mm/s^2 Magnitude of gravity at rest. Determines units of velocity. [UNITS MUST MATCH ACCELERATION] 41 | constexpr float SD_ACC = 1000 / GRAVITY; // mm/s^2 / g-force Standard deviation of acceleration. Deviations from zero are suppressed. 42 | constexpr float SD_VEL = 200 / GRAVITY; // mm/s / g-force Standard deviation of velocity. Deviations from target value are suppressed. 43 | constexpr float ALPHA = 0.5; // Gain of heading update - See example "output" for more information. 44 | 45 | void setup() { 46 | Serial.begin(38400); 47 | delay(1000); 48 | 49 | // calibrate IMU sensor 50 | imu.setup(); 51 | imu.setBias(); 52 | 53 | // initialize sensor fusion 54 | //fusion.setup( imu.ax(), imu.ay(), imu.az() ); // ALWAYS set initial heading with acceleration 55 | fusion.setup(); 56 | 57 | //fusion.reset(); /* Use this function to zero velocity estimate */ 58 | } 59 | 60 | void loop() { 61 | imu.updateBias(); 62 | 63 | // Measure state: 64 | vec3_t accel = { imu.ax(), imu.ay(), imu.az() }; // g-unit 65 | vec3_t gyro = { imu.gx(), imu.gy(), imu.gz() }; // radians/second 66 | 67 | // Update heading and velocity estimate: 68 | 69 | // known measured velocity (target state). Estimate will be forced towards this vector 70 | vec3_t vel_t = {0,0,0}; 71 | 72 | vel_t /= GRAVITY; // must have unit: g-force * second 73 | 74 | /* note: all coefficients are optional and have default values */ 75 | fusion.update( gyro, accel, vel_t, SD_ACC, SD_VEL, ALPHA ); 76 | 77 | // obtain velocity estimate 78 | vec3_t vel = fusion.getVel() * GRAVITY; // scale by gravity for desired units 79 | 80 | // Display velocity components: [view with serial plotter] 81 | Serial.print( vel.x, 2 ); 82 | Serial.print( " " ); 83 | Serial.print( vel.y, 2 ); 84 | Serial.print( " " ); 85 | Serial.print( vel.z, 2 ); 86 | Serial.println(); 87 | } 88 | -------------------------------------------------------------------------------- /keywords.txt: -------------------------------------------------------------------------------- 1 | # libraries 2 | imuFilter KEYWORD1 3 | accIntegral KEYWORD1 4 | 5 | # NOTE: separate keywords with single tab 6 | -------------------------------------------------------------------------------- /library.properties: -------------------------------------------------------------------------------- 1 | name=imuFilter 2 | version=1.6.3 3 | author=RCmags 4 | maintainer=RCmags 5 | sentence=Sensor fusion for an IMU to obtain heading and velocity. 6 | paragraph=Library to fuse the data of an inertial measurement unit (IMU) and estimate velocity. It uses a quaternion to encode the rotation and uses a kalman-like filter to correct the gyroscope with the accelerometer. The acceleration is integrated via a kalman-like filter to obtain a short-term estimate of the velocity. 7 | category=Device Control 8 | url=https://github.com/RCmags/imuFilter 9 | architectures=* 10 | depends=Vector datatype 11 | -------------------------------------------------------------------------------- /license.txt: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 RCmags 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 | -------------------------------------------------------------------------------- /src/accIntegral.cpp: -------------------------------------------------------------------------------- 1 | #include "accIntegral.h" 2 | 3 | //----------------- Initialization ------------------- 4 | 5 | void accIntegral::reset() { 6 | // vectors 7 | vel = {0,0,0}; 8 | accel_mean = {0,0,0}; 9 | accel_last = {0,0,0}; 10 | 11 | // scalars 12 | var_vel = 0; 13 | var_acc = 0; 14 | } 15 | 16 | //---------------- Velocity estimate ----------------- 17 | 18 | vec3_t accIntegral::getVel() { 19 | return vel; 20 | } 21 | 22 | // update velocity with imu measurements 23 | void accIntegral::update( vec3_t angvel, 24 | vec3_t accel, 25 | vec3_t vel_t, 26 | const float SD_ACC, 27 | const float SD_VEL, 28 | const float ALPHA ) { 29 | // 0. Constants: 30 | const float VAR_ACC = SD_ACC*SD_ACC; 31 | const float VAR_VEL = SD_VEL*SD_VEL; 32 | const float VAR_COMB = VAR_ACC*VAR_VEL; 33 | 34 | // 1. Remove acceleration bias: 35 | imuFilter::update( angvel, accel, ALPHA, SD_ACC ); 36 | quat_t qt = imuFilter::getQuat(); 37 | 38 | // Remove centrifugal 39 | vec3_t vel_local = qt.rotate(vel, LOCAL_FRAME); 40 | accel -= vel_local.cross(angvel); 41 | 42 | // remove global gravity vector [normalized] 43 | accel = qt.rotate(accel, GLOBAL_FRAME); 44 | accel.z -= 1; 45 | 46 | // Update variance 47 | accel -= accel_mean; 48 | 49 | float acc_err = accel.dot(accel); 50 | float gain_acc = VAR_ACC/(VAR_ACC + var_acc); 51 | 52 | var_acc = acc_err + gain_acc*var_acc; 53 | accel_mean += accel*gain_acc; 54 | 55 | // 2. Integrate acceleration: 56 | // Update Variance 57 | vec3_t dvel = vel_t - vel; 58 | 59 | float dvel_mag = dvel.dot(dvel); 60 | // gain affected by velocity and acceleration 61 | float gain_vel = VAR_COMB/(VAR_COMB + var_vel*var_acc); 62 | 63 | var_vel = dvel_mag + gain_vel*var_vel; 64 | 65 | // Integrate 66 | float dt = imuFilter::timeStep(); 67 | vel += 0.5*(accel + accel_last)*dt; // trapezoidal rule 68 | vel += dvel*gain_vel; // update with target velocity 69 | 70 | accel_last = accel; 71 | } 72 | 73 | // verbose update 74 | void accIntegral::update( float gx, float gy, float gz, 75 | float ax, float ay, float az, 76 | float vx, float vy, float vz, 77 | const float SD_ACC, 78 | const float SD_VEL, 79 | const float ALPHA ) { 80 | vec3_t angvel = {gx, gy, gz}; 81 | vec3_t accel = {ax, ay, az}; 82 | vec3_t vel_t = {vx, vy, vz}; 83 | return update( angvel, accel, vel_t, SD_ACC, SD_VEL, ALPHA ); 84 | } 85 | -------------------------------------------------------------------------------- /src/accIntegral.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #ifndef accIntegral_h 4 | #define accIntegral_h 5 | 6 | //------------------ Coefficients -------------------- 7 | 8 | #define DEFAULT_SD_VEL 0.1 // Default standard deviation in velocity. [g-force * second] 9 | 10 | //---------------- Class definition ------------------ 11 | 12 | class accIntegral: public imuFilter { 13 | private: 14 | // vectors 15 | vec3_t vel = {0,0,0}; 16 | vec3_t accel_mean = {0,0,0}; 17 | vec3_t accel_last = {0,0,0}; 18 | 19 | // scalars 20 | float var_vel = 0; 21 | float var_acc = 0; 22 | 23 | public: 24 | void reset(); 25 | 26 | vec3_t getVel(); 27 | 28 | void update( vec3_t, vec3_t, vec3_t, 29 | const float=DEFAULT_SD_ACC, 30 | const float=DEFAULT_SD_VEL, 31 | const float=DEFAULT_GAIN ); 32 | 33 | void update( float, float, float, 34 | float, float, float, 35 | float, float, float, 36 | const float=DEFAULT_SD_ACC, 37 | const float=DEFAULT_SD_VEL, 38 | const float=DEFAULT_GAIN ); 39 | }; 40 | 41 | #endif 42 | -------------------------------------------------------------------------------- /src/imuFilter.cpp: -------------------------------------------------------------------------------- 1 | #include "imuFilter.h" 2 | 3 | //----------------- Initialization ------------------- 4 | 5 | float imuFilter::timeStep() { 6 | return dt; 7 | } 8 | 9 | void imuFilter::updateTimer() { 10 | uint32_t time_now = micros(); 11 | uint32_t change_time = time_now - last_time; 12 | last_time = time_now; 13 | dt = float(change_time)*1e-6; 14 | } 15 | 16 | void imuFilter::setup() { 17 | var = 0; 18 | q = {1,0,0,0}; 19 | last_time = micros(); 20 | } 21 | 22 | void imuFilter::setup( vec3_t accel ) { 23 | setup(); 24 | // set quaternion as vertical vector 25 | vec3_t v = accel.norm(); // gravity vector 26 | 27 | float norm = v.x*v.x + v.y*v.y; 28 | float cosine = v.z/sqrt( norm + v.z*v.z ) * 0.5; // vertical angle 29 | norm = sqrt( (0.5 - cosine)/norm ); // sine of half angle 30 | 31 | q.w = sqrt(0.5 + cosine); // quaternion components 32 | q.v = vec3_t(v.y*norm, -v.x*norm, 0); 33 | } 34 | 35 | void imuFilter::setup( float ax, float ay, float az ) { 36 | setup( vec3_t(ax, ay, az) ); 37 | } 38 | 39 | //---------------- Heading estimate ------------------ 40 | 41 | // Update heading with gyro: 42 | 43 | void imuFilter::update( float gx, float gy, float gz ) { 44 | // Update Timer 45 | updateTimer(); 46 | 47 | // Rotation increment 48 | vec3_t da = vec3_t(gx, gy, gz)*dt; 49 | quat_t dq; dq.setRotation(da, SMALL_ANGLE); 50 | 51 | // Multiply and normalize Quaternion 52 | q *= dq; 53 | q = q.norm(); 54 | } 55 | 56 | // Update heading with gyro and accelerometer: 57 | 58 | void imuFilter::update( float gx, float gy, float gz, 59 | float ax, float ay, float az, 60 | const float ALPHA, 61 | const float SD_ACC ) { 62 | // Update Timer 63 | updateTimer(); 64 | 65 | // check global acceleration [normalized]: 66 | vec3_t accel = {ax, ay, az}; 67 | vec3_t acrel = q.rotate(accel, GLOBAL_FRAME); 68 | acrel.z -= 1; 69 | 70 | // kalmal filter: 71 | const float INV_VAR = 1.0/( SD_ACC * SD_ACC ); 72 | float error = acrel.dot(acrel); // deviation from vertical 73 | 74 | float gain = 1.0/(1 + var*INV_VAR); // kalman gain 75 | var = error + var*gain; // variance of error 76 | 77 | // error about vertical 78 | vec3_t vz = q.axisZ(LOCAL_FRAME); 79 | vec3_t ve = accel.norm(); 80 | ve = ve.cross(vz); 81 | 82 | // Rotation increment 83 | gain *= ALPHA * dt; 84 | vec3_t da = vec3_t(gx, gy, gz)*dt + ve*gain; 85 | quat_t dq; dq.setRotation(da, SMALL_ANGLE); 86 | 87 | // Multiply and normalize Quaternion 88 | q *= dq; 89 | q = q.norm(); 90 | } 91 | 92 | // vector inputs: 93 | void imuFilter::update( vec3_t gyro ) { 94 | update(gyro.x, gyro.y, gyro.z); 95 | } 96 | 97 | void imuFilter::update( vec3_t gyro, vec3_t accel, 98 | const float ALPHA, 99 | const float SD_ACC ) { 100 | update( gyro.x , gyro.y , gyro.z, 101 | accel.x, accel.y, accel.z, 102 | ALPHA, SD_ACC ); 103 | } 104 | 105 | // Rotate heading by a large or small angle 106 | 107 | void imuFilter::rotateHeading( float angle, const bool SMALL_ANG ) { 108 | // rotation about vertical 109 | vec3_t vp = q.axisZ(LOCAL_FRAME); 110 | quat_t dq; dq.setRotation(vp, angle, SMALL_ANG); 111 | 112 | // Rotate quaternion 113 | q *= dq; 114 | } 115 | 116 | //----------------- Fusion outputs ------------------- 117 | 118 | // Quaternion 119 | quat_t imuFilter::getQuat() { 120 | return q; 121 | } 122 | 123 | // Axis projections: 124 | 125 | vec3_t imuFilter::getXaxis( const bool TO_WORLD ) { 126 | return q.axisX(TO_WORLD); 127 | } 128 | 129 | 130 | vec3_t imuFilter::getYaxis( const bool TO_WORLD ) { 131 | return q.axisY(TO_WORLD); 132 | } 133 | 134 | vec3_t imuFilter::getZaxis( const bool TO_WORLD ) { 135 | return q.axisZ(TO_WORLD); 136 | } 137 | 138 | vec3_t imuFilter::projectVector( vec3_t vec, const bool TO_WORLD ) { 139 | return q.rotate(vec, TO_WORLD); 140 | } 141 | 142 | //------------------ Euler Angles ------------------- 143 | 144 | float imuFilter::roll() { // x-axis 145 | vec3_t v = q.v; 146 | float y = 2*( q.w*v.x + v.y*v.z ); 147 | float x = 1 - 2*( v.x*v.x + v.y*v.y ); 148 | return atan2( y, x ); 149 | } 150 | 151 | float imuFilter::pitch() { // y-axis 152 | constexpr float PI_2 = PI*0.5; 153 | vec3_t v = q.v; 154 | float a = 2*( v.y*q.w - v.z*v.x ); 155 | if( a > 1 ) { 156 | return PI_2; 157 | } else if ( a < -1 ) { 158 | return -PI_2; 159 | } else { 160 | return asin(a); 161 | } 162 | } 163 | 164 | float imuFilter::yaw() { // z-axis 165 | vec3_t v = q.v; 166 | float y = 2*( v.z*q.w + v.x*v.y ); 167 | float x = 1 - 2*( v.y*v.y + v.z*v.z ); 168 | return atan2( y, x ); 169 | } 170 | -------------------------------------------------------------------------------- /src/imuFilter.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #ifndef imuFilter_h 5 | #define imuFilter_h 6 | 7 | //------------------ Coefficients -------------------- 8 | 9 | #define INV_Q_FACTOR 2 // Filter damping. A smaller value leads to faster response but more oscillations. 10 | #define DEFAULT_GAIN 0.5 // Default filter gain. 11 | #define DEFAULT_SD_ACC 0.2 // Default standard deviation in acceleration. [g-force] 12 | 13 | //---------------- Class definition ------------------ 14 | 15 | class imuFilter { 16 | private: 17 | quat_t q = {1,0,0,0}; 18 | float var = 0; 19 | uint32_t last_time = 0; 20 | float dt; 21 | 22 | void updateTimer(); 23 | 24 | public: 25 | // Initialization: 26 | void setup(); 27 | void setup( float, float, float ); 28 | void setup( vec3_t ); 29 | 30 | // Heading estimate: 31 | void update( float, float, float ); 32 | 33 | void update( float, float, float, 34 | float, float, float, 35 | const float=DEFAULT_GAIN, 36 | const float=DEFAULT_SD_ACC ); 37 | 38 | void update( vec3_t ); 39 | void update( vec3_t, vec3_t, 40 | const float=DEFAULT_GAIN, 41 | const float=DEFAULT_SD_ACC ); 42 | 43 | void rotateHeading( float, const bool ); 44 | 45 | float timeStep(); 46 | 47 | //-- Fusion outputs: 48 | 49 | // Quaternion 50 | quat_t getQuat(); 51 | 52 | // Axis projections:arduino 53 | vec3_t getXaxis( const bool ); 54 | vec3_t getYaxis( const bool ); 55 | vec3_t getZaxis( const bool ); 56 | vec3_t projectVector( vec3_t, const bool ); 57 | 58 | // Euler Angles: 59 | float roll(); 60 | float pitch(); 61 | float yaw(); 62 | }; 63 | 64 | #endif 65 | --------------------------------------------------------------------------------