downgrade to v1.3.0

README.md

@@ -1,23 +1,39 @@
 # imuFilter
-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: 
+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:
 
-$\ \overrightarrow{a_{rel}} = \overrightarrow{a_{local}} - (0,0,1) $
-
-$\ K_{\sigma} = {\alpha}/(1 + \frac{ {\sigma}^2 }{ {\sigma}_{acc}^2 } ) $
+__Mahony:__  
+$\ E_{k} = \theta_{accel} - \theta_{k-1} $
 
-$\ {\sigma}^2 = | \overrightarrow{a_{rel}} |^2 + K_{\sigma}{\sigma}^2 $ 
+$\ I_{k} = I_{k-1} + {E_{k}}{\Delta t} $
 
-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: 
+$\ \theta_{k} = \theta_{k-1} + \dot{\theta}{\Delta t} + K_{p}E_{k} + K_{i}I_{k} $
 
+__Modified Mahony:__ (this filter)  
 $\ E_{k} = \theta_{accel} - \theta_{k-1} $
 
-$\ \theta_{k} = \theta_{k-1} + \dot{\theta}{\Delta t} + K_{\sigma}E_{k} $
+$\ I_{k} = K_{p}{E_{k}} + (1 - K_{c})I_{k-1} $
+
+$\ \theta_{k} = \theta_{k-1} + \dot{\theta}{\Delta t} + I_{k} $  
+
+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 } $
+
+To improve the response of the filter, the acceleration is checked to see if it lies within a given deviation from its steady-state value (1g vertically). 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. This is accomplished with a kalman-like filter that uses the deviation from vertical to update its variance and gain:
+
+$\ \overrightarrow{a_{rel}} = \overrightarrow{a_{local}} - (0,0,1) $
+
+$\ K_{\sigma} = 1/(1 + \frac{ {\sigma}^2 }{ {\sigma}_{acc}^2 } ) $
+
+$\ {\sigma}^2 = | \overrightarrow{a_{rel}} |^2 + K_{\sigma}{\sigma}^2 $ 
+
+$\ E_{k} = K_{\sigma} E_{k} $
 
 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.
 
-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.
+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.
 
 # Dependecies
 This library depends on the [vector_datatype](https://github.com/RCmags/vector_datatype) library.

examples/heading/heading.ino

@@ -10,18 +10,18 @@ basicMPU6050<> imu;
 imuFilter fusion;
 
 void setup() {
-  Serial.begin(38400);
+  // Initialize filter: 
+  fusion.setup( imu.ax(), imu.ay(), imu.az() );     
 
   // Calibrate imu
   imu.setup();
   imu.setBias();
-
-  // Initialize filter: 
-  fusion.setup( imu.ax(), imu.ay(), imu.az() );     
 
   // 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

@@ -1,68 +1,37 @@
 /*
  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>
 
-// Gyro settings:
-#define         LP_FILTER   3           // Low pass filter.                    Value from 0 to 6
-#define         GYRO_SENS   0           // Gyro sensitivity.                   Value from 0 to 3
-#define         ACCEL_SENS  0           // Accelerometer sensitivity.          Value from 0 to 3
-#define         ADDRESS_A0  LOW         // I2C address from state of A0 pin.   A0 -> GND : ADDRESS_A0 = LOW
-                                        //                                     A0 -> 5v  : ADDRESS_A0 = HIGH
-// Accelerometer offset:
-constexpr int   AX_OFFSET =  552;       // Use these values to calibrate the accelerometer. The sensor should output 1.0g if held level. 
-constexpr int   AY_OFFSET = -241;       // These values are unlikely to be zero.
-constexpr int   AZ_OFFSET = -3185;
-
-// Output scale: 
-constexpr float AX_SCALE = 1.00457;     // Multiplier for accelerometer outputs. Use this to calibrate the sensor. If unknown set to 1.
-constexpr float AY_SCALE = 0.99170;
-constexpr float AZ_SCALE = 0.98317;
-
-constexpr float GX_SCALE = 0.99764;     // Multiplier to gyro outputs. Use this to calibrate the sensor. If unknown set to 1.
-constexpr float GY_SCALE = 1.0;
-constexpr float GZ_SCALE = 1.01037;
-
-// Bias estimate:
-#define         GYRO_BAND   35          // Standard deviation of the gyro signal. Gyro signals within this band (relative to the mean) are suppresed.   
-#define         BIAS_COUNT  5000        // Samples of the mean of the gyro signal. Larger values provide better calibration but delay suppression response. 
-
-//-- Set the template parameters:
-
-basicMPU6050<LP_FILTER,  GYRO_SENS,  ACCEL_SENS, ADDRESS_A0,
-             AX_OFFSET,  AY_OFFSET,  AZ_OFFSET, 
-             &AX_SCALE,  &AY_SCALE,  &AZ_SCALE,
-             &GX_SCALE,  &GY_SCALE,  &GZ_SCALE,
-             GYRO_BAND,  BIAS_COUNT 
-            >imu;
+basicMPU6050<> imu;
 
 // =========== Settings ===========
 imuFilter fusion;
 
-#define GAIN          0.5     /* Fusion gain, value between 0 and 1 - Determines orientation correction with respect to gravity vector. 
+#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) */
 
-#define SD_ACCEL      0.2     /* Standard deviation of acceleration. Accelerations relative to (0,0,1)g outside of this band are suppresed.
+#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 */
 
 void setup() {
-  Serial.begin(38400);
-
-  // Calibrate imu
-  imu.setup();
-  imu.setBias();
-
-  #if FUSION
-     Set quaternion with gravity vector
+   #if FUSION
+    // Set quaternion with gravity vector
     fusion.setup( imu.ax(), imu.ay(), imu.az() );     
   #else 
-     Just use gyro
+    // Just use gyro
     fusion.setup();                                   
   #endif
+
+  // Calibrate imu
+  imu.setup();
+  imu.setBias();
+
+  Serial.begin(38400);
 }
 
 void loop() {

examples/vector_output/vector_output.ino

@@ -26,14 +26,14 @@ void printQuat( quat_t q ) {
 }
 
 void setup() {
-  Serial.begin(38400);
-
+  // Initialize filter: 
+  fusion.setup( imu.ax(), imu.ay(), imu.az() );     
+
   // Calibrate imu
   imu.setup();
   imu.setBias();
-
-  // Initialize filter: 
-  fusion.setup( imu.ax(), imu.ay(), imu.az() ); 
+
+  Serial.begin(38400);
 }
 
 void loop() {  

library.properties

@@ -1,9 +1,9 @@
 name=imuFilter
-version=1.4.0
+version=1.3.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 kalman-like filter to correct the gyroscope with the accelerometer.    
+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.    
 category=Device Control
 url=https://github.com/RCmags/imuFilter
 architectures=* 

src/imuFilter.cpp

@@ -3,14 +3,14 @@
 //----------------- Initialization ------------------- 
 
 float imuFilter::updateTimer() {
-  uint32_t time_now = micros();
-  uint32_t change_time = time_now - last_time;
-  last_time = time_now;
+  uint32_t change_time = uint32_t( micros() - last_time );
+  last_time = micros();
   return float(change_time)*1e-6;         
 }
 
 void imuFilter::setup() {
   var = 0;
+  s = {0,0,0};
   q = {1,0,0,0};
   last_time = micros();
 }
@@ -54,25 +54,29 @@ void imuFilter::update( float gx, float gy, float gz,
   // Update Timer
   float dt = updateTimer();
 
-  // check global acceleration:
+  // check global acceleration
   vec3_t accel = {ax, ay, az};
-  vec3_t acrel = q.rotate(accel, GLOBAL_FRAME) - vec3_t(0,0,1); 
-
-  	// kalmal filter:
-  const float INV_VAR = 1.0/( SD_ACC * SD_ACC );
-  float error = acrel.dot(acrel);     // deviation from vertical
-
-  float gain = ALPHA * dt;  
-  gain = gain/(1 + var*INV_VAR);      // kalman gain
-  var = error + var*gain;             // variance of error
+  vec3_t error = q.rotate(accel, GLOBAL_FRAME) - vec3_t(0,0,1);
+  float error_mag = error.dot(error);   // deviation from (0,0,1)g
 
+  const float INV_VAR = 1.0/( SD_ACC * SD_ACC );  
+  float gain = 1.0/(1 + var*INV_VAR);   // kalman gain
+  var = error_mag + var*gain;           // variance of error
+
   // error about vertical
   vec3_t vz = q.axisZ(LOCAL_FRAME);   
   vec3_t ve = accel.norm();
   ve = ve.cross(vz);
 
-  // Rotation increment  
-  vec3_t da = vec3_t(gx, gy, gz)*dt + ve*gain;       
+  // filter gains
+  const float KP = ALPHA*ALPHA;
+  float kp = KP*dt;                     // spring constant
+  float kc = sqrt(INV_Q_FACTOR*kp);     // damping constant           
+  kp *= gain;                           // modulate error with kalman gain
+
+  // Rotation increment
+  s += ve*kp - s*kc;                    // target angular rate   
+  vec3_t da = vec3_t(gx, gy, gz)*dt + s;       
   quat_t dq; dq.setRotation(da, SMALL_ANGLE);
 
   // Multiply and normalize Quaternion  
@@ -119,14 +123,14 @@ vec3_t imuFilter::projectVector( vec3_t vec, const bool TO_WORLD ) {
 
 //------------------ Euler Angles ------------------- 
 
-float imuFilter::roll() { // x-axis
+float imuFilter::roll() {
   vec3_t v = q.v;
-  float y = 2*( q.w*v.x + v.y*v.z );                        
+  float y = 2*( q.w*v.x + v.y*v.z );
   float x = 1 - 2*( v.x*v.x + v.y*v.y );
   return atan2( y, x );
 }
 
-float imuFilter::pitch() { // y-axis
+float imuFilter::pitch() {
   constexpr float PI_2 = PI*0.5;    
   vec3_t v = q.v;
   float a = 2*( v.y*q.w - v.z*v.x );    
@@ -139,7 +143,7 @@ float imuFilter::pitch() { // y-axis
   }
 }
 
-float imuFilter::yaw() { // z-axis
+float imuFilter::yaw() {
   vec3_t v = q.v;
   float y = 2*( v.z*q.w + v.x*v.y );
   float x = 1 - 2*( v.y*v.y + v.z*v.z );    

src/imuFilter.h

@@ -7,13 +7,14 @@
 //------------------ Coefficients -------------------- 
 
 #define INV_Q_FACTOR        2           // Filter damping. A smaller value leads to faster response but more oscillations.
-#define DEFAULT_GAIN        0.5         // Default filter gain. 
-#define DEFAULT_SD          0.2         // Default standard deviation in acceleration. [g-force]
+#define DEFAULT_GAIN        0.1         // Default filter gain. A faster value 
+#define DEFAULT_SD          0.1         // Default standard deviation in acceleration. [g-force]
 
 //---------------- Class definition ------------------ 
 
 class imuFilter {
   private: 
+    vec3_t s;
     quat_t q;
     float var;
     uint32_t last_time;