downgrade to v1.1.0

README.md

@@ -1,36 +1,27 @@
 # imuFilter
-This library fuses the outputs of an inertial measurement unit (IMU) and stores the heading as a quaternion. It uses a _modified Mahony_ filter that replaces the PI controller with a damped 2nd-order system. The proportional term was removed and the integral term was forced to decay to damp the system. The correction steps are as follows:
 
-__Mahony:__  
-$\ E = \theta_{accel} - \theta_{k-1} $
+This library contains a sensor fusion algorithm to combine the outputs of a 6-axis inertial measurement unit (IMU). It's based on a modified version of the Mahony filter that replaces the PI controller with something akin to a 2nd order low pass filter. The proportional term was removed and the integral term has been forced to decay in order to damp the system. The correction steps of each filter are shown below:
 
-$\ I_{k} = I_{k-1} + {E}{\Delta t} $
+- Mahony:  
+_integral += error.dt   
+dtheta = theta_dot.dt + kp.error + ki.integral_  
 
-$\ \theta_{k} = \theta_{k-1} + \dot{\theta}{\Delta t} + K_{p}E + K_{i}I_{k} $
+- Modified Mahony:  
+_integral += error.kp - integral.kc    
+dtheta = theta_dot.dt + integral_  
 
-__Modified Mahony:__ (this filter)  
-$\ E = \theta_{accel} - \theta_{k-1} $
+The behavior of the modified filter is analogous to spring-mass system. Kp (stiffness) and Kc (damping) are related by the damping ratio Q which is held constant. This allows the behavior of the filter to be controlled via a single parameter.  
 
-$\ I_{k} = K_{p}{E} + (1 - K_{c})I_{k-1} $
+The filter uses a quaternion to encode rotations. This makes it easy to perform coordinate transformations. These include:  
+- Transfor a vector from the local frame to the global (and vice versa)
+- Get unit vectors of the X, Y and Z axes in the local or global frame.
 
-$\ \theta_{k} = \theta_{k-1} + \dot{\theta}{\Delta t} + I_{k} $  
+Since a 6-axis IMU has no absolute reference for heading there is a function to rotate the orientation estimate about the yaw axis. Basic vector operations have been included to easily implement a heading correction algorithm should one have an additional sensor (such a magnetometer or some other absolute heading sensor).
 
-The behavior of the modified filter is analogous to spring-mass system. Kp (stiffness) and Kc (damping) are related by the [Q-factor](https://en.wikipedia.org/wiki/Q_factor). This value is held constant, so the behavior of the filter is controlled by a single parameter (Kp):  
-
-$\ K_{c} = \sqrt{ K_{p}/Q } $
-
-As the filter uses a quaternion to encode rotations, it's easy to perform coordinate transformations. The library has functions to:
-- Transfor a vector to the local or global frame.
-- Get the unit vectors of the X, Y and Z axes in the local or global frame.
+For more information on the Mahony filter see these links:
+- [IMU Data Fusing: Complementary, Kalman, and Mahony Filter](http://www.olliw.eu/2013/imu-data-fusing/#chapter23)
+- [Mahony Filter](https://nitinjsanket.github.io/tutorials/attitudeest/mahony)
 
-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 such a magnetometer.
+## Dependencies
 
-# Dependecies
 This library depends on the [vector_datatype](https://github.com/RCmags/vector_datatype) library.
-
-# References
-See these links for more information on the Mahony filter:
-- [Nonlinear Complementary Filters on the Special
-Orthogonal Group](https://hal.archives-ouvertes.fr/hal-00488376/document) (original paper)
-- [IMU Data Fusing: Complementary, Kalman, and Mahony Filter](http://www.olliw.eu/2013/imu-data-fusing/#chapter23)
-- [Mahony Filter](https://nitinjsanket.github.io/tutorials/attitudeest/mahony)

examples/heading/heading.ino

@@ -1,13 +1,16 @@
 /*
- This sketch shows to rotate the heading (yaw angle) of the estimated orientation.
+ This sketch shows to perform vector products and rotate heading (yaw angle) of the estimated orientation.
  */
 
-#include <basicMPU6050.h>       // Library for IMU sensor. See this link: https://github.com/RCmags/basicMPU6050
 #include <imuFilter.h>
+#include <basicMPU6050.h>       // Library for IMU sensor. See this link: https://github.com/RCmags/basicMPU6050
 
-basicMPU6050<> imu;
+// Sensor fusion
+constexpr float GAIN = 0.1;     // Fusion gain, value between 0 and 1 - Determines response of heading correction with respect to gravity.
+imuFilter <&GAIN> fusion;
 
-imuFilter fusion;
+// Imu sensor
+basicMPU6050<> imu;
 
 void setup() {
   // Initialize filter: 
@@ -16,12 +19,12 @@ void setup() {
   // Calibrate imu
   imu.setup();
   imu.setBias();
+
+  Serial.begin(38400);
 
   // Rotate heading:
   float angle = 45 * DEG_TO_RAD;                // angle in radians to rotate heading about z-axis
   fusion.rotateHeading( angle, LARGE_ANGLE );   // Can choose LARGE_ANGLE or SMALL_ANGLE approximation
-
-  Serial.begin(38400);
 }
 
 void loop() {  

examples/output/output.ino

@@ -2,21 +2,18 @@
  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.
  */
 
-#include <basicMPU6050.h>       // Library for IMU sensor. See this link: https://github.com/RCmags/basicMPU6050
 #include <imuFilter.h>
+#include <basicMPU6050.h>       // Library for IMU sensor. See this link: https://github.com/RCmags/basicMPU6050
 
-basicMPU6050<> imu;
-
-// =========== Settings ===========
-imuFilter fusion;
+// Sensor fusion
+constexpr float GAIN = 0.1;     // Fusion gain, value between 0 and 1 - Determines orientation correction with respect to gravity vector. 
+imuFilter <&GAIN> fusion;       // If set to 1 the gyroscope is dissabled. If set to 0 the accelerometer is dissabled (equivant to gyro-only).
 
-#define GAIN          0.1     /* Fusion gain, value between 0 and 1 - Determines orientation correction with respect to gravity vector. 
-                                 If set to 1 the gyroscope is dissabled. If set to 0 the accelerometer is dissabled (equivant to gyro-only) */
+// Imu sensor
+basicMPU6050<> imu;
 
-#define SD_ACCEL      0.1     /* Standard deviation of acceleration. Accelerations relative to (0,0,1)g outside of this band are suppresed.
-                                 Accelerations within this band are used to update the orientation. [Measured in g-force] */                          
-
-#define FUSION        true    /* Enable sensor fusion. Setting to "true" enables gravity correction */
+// Enable sensor fusion. Setting to "true" enables gravity correction.
+#define FUSION  false
 
 void setup() {
    #if FUSION
@@ -38,8 +35,8 @@ void loop() {
   // Update filter:
 
   #if FUSION
-    /*NOTE: GAIN and SD_ACCEL are optional parameters */
-    fusion.update( imu.gx(), imu.gy(), imu.gz(), imu.ax(), imu.ay(), imu.az(), GAIN, SD_ACCEL );  
+    //Fuse gyro and accelerometer
+    fusion.update( imu.gx(), imu.gy(), imu.gz(), imu.ax(), imu.ay(), imu.az() );    
   #else
     // Only use gyroscope
     fusion.update( imu.gx(), imu.gy(), imu.gz() );

examples/vector_output/vector_output.ino

@@ -2,15 +2,17 @@
  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.
  */
 
-#include <basicMPU6050.h>       // Library for IMU sensor. See this link: https://github.com/RCmags/basicMPU6050
 #include <imuFilter.h>
+#include <basicMPU6050.h>       // Library for IMU sensor. See this link: https://github.com/RCmags/basicMPU6050
 
-basicMPU6050<> imu;
-
-imuFilter fusion;
+// Sensor fusion
+constexpr float GAIN = 0.1;     // Fusion gain, value between 0 and 1 - Determines response of heading correction with respect to gravity.
+imuFilter <&GAIN> fusion;
 
-// ========= functions ===========
+// Imu sensor
+basicMPU6050<> imu;
 
+// Display functions:
 void printVector( vec3_t r ) {
   Serial.print( r.x, 2 );
   Serial.print( "," );

library.properties

@@ -1,9 +1,9 @@
 name=imuFilter
-version=1.2.0
+version=1.1.0
 author=RCmags <[email protected]>
 maintainer=RCmags <[email protected]>
 sentence=Sensor fusion for IMU with quaternion based filter.
-paragraph=Library to fuse the data of an inertial measurement unit (IMU). It uses a quaternion to encode the rotation and uses a modified Mahony filter to correct the gyroscope with the accelerometer.    
+paragraph=Library that fuses the accelerometer and gyroscope outputs of an IMU. It uses a quaternion to encode the rotation and uses a modified Mahony filter to correct the gyro with the accelerometer.    
 category=Device Control
 url=https://github.com/RCmags/imuFilter
 architectures=* 

src/imuFilter.h

@@ -6,19 +6,22 @@
 
 //------------------ Coefficients -------------------- 
 
-#define INV_Q_FACTOR        2           // Filter damping. A smaller value leads to faster response but more oscillations.
-#define DEFAULT_GAIN        0.1         // Default filter gain. A faster value 
-#define DEFAULT_SD          0.1         // Default standard deviation in acceleration. [g-force]
+#define INV_Q_VAL       1.414213            // Damping behavior of filter. A larger value leads to faster response but more oscillations.
+
+//-------------------- Parameters -------------------- [ No characters after backlash! ]
+
+#define TEMPLATE_TYPE   const float *ALPHA
+
+#define TEMPLATE_INPUTS              ALPHA
 
 //---------------- Class definition ------------------ 
 
+template<TEMPLATE_TYPE>
 class imuFilter {
   private: 
     vec3_t s;
     quat_t q;
-    float var;
     uint32_t last_time;
-
     float updateTimer();
 
   public:
@@ -28,12 +31,7 @@ class imuFilter {
 
     // Heading estimate:
     void update( float, float, float );
-
-    void update( float, float, float, 
-                 float, float, float, 
-                 const float=DEFAULT_GAIN, 
-                 const float=DEFAULT_SD  );
-
+    void update( float, float, float, float, float, float );
     void rotateHeading( float, const bool );
 
     //-- Fusion outputs:
@@ -53,4 +51,11 @@ class imuFilter {
     float yaw();
 };
 
+#include "imuFilter.tpp"
+
+//----------------- Clearing labels ------------------
+
+#undef TEMPLATE_TYPE
+#undef TEMPLATE_INPUTS
+
 #endif