simpler accIntegral

README.md

@@ -3,15 +3,17 @@ This library fuses the outputs of an inertial measurement unit (IMU) and stores
 
 $\ \overrightarrow{a_{rel}} = \overrightarrow{a_{local}} - (0,0,1) $
 
-$\ K_{\sigma} = {\alpha}/(1 + \frac{ {\sigma}^2 }{ {\sigma}_{acc}^2 } ) $
+$\ K_{\sigma} = 1/(1 + \frac{ {\sigma}^2 }{ {\sigma}_{acc}^2 } ) $
 
 $\ {\sigma}^2 = | \overrightarrow{a_{rel}} |^2 + K_{\sigma}{\sigma}^2 $ 
 
 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: 
 
 $\ E_{k} = \theta_{accel} - \theta_{k-1} $
 
-$\ \theta_{k} = \theta_{k-1} + \dot{\theta}{\Delta t} + K_{\sigma}E_{k} $
+$\ K_{\theta} = {\alpha} K_{\sigma} {\Delta t} $
+
+$\ \theta_{k} = \theta_{k-1} + \dot{\theta}{\Delta t} + K_{\theta}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.

examples/velocity/velocity.ino

@@ -4,11 +4,11 @@
 
 /* NOTE: 
    1. The accelerometer MUST be calibrated to integrate its output. Adjust the 
-   AX, AY, AND AZ offsets until the sensor reads (0,0,0) at rest.
+   AX, AY, AND AZ offsets until the sensor reads (0,0,GRAVITY) at rest.
 
    2. REQUIRED UNITS:
       I. The gyroscope must output: radians/second
-      II. The accelerometer output:  g-force         [1 g-force ~ 9.81 meter/second^2] 
+      II. The accelerometer output:  length/second^2  [The length unit is arbitrary. Can be meter, feet, etc]
 */
 
 #include <basicMPU6050.h>   // Library for IMU sensor. See this link: https://github.com/RCmags/basicMPU6050
@@ -35,50 +35,59 @@ basicMPU6050<LP_FILTER,  GYRO_SENS,  ACCEL_SENS, ADDRESS_A0,
 
 // =========== Settings ===========
 accIntegral fusion;
-                                            //  Unit            
-constexpr float GRAVITY = 9.81e3;           //  mm/s^2          Magnitude of gravity at rest. Determines units of velocity.
-constexpr float SD_ACC  = 1000;             //  mm/s^2          Standard deviation of acceleration. Deviations from zero are suppressed.
-constexpr float SD_VEL  = 200;              //  mm/s            Standard deviation of velocity. Deviations from target value are suppressed.
-constexpr float ALPHA   = 0.5;              //                  Gain of heading estimate - See example "output" [optional parameter]
+
+// Filter coefficients               //  Unit           
+constexpr float GRAVITY = 9.81e3;    //  mm/s^2    Magnitude of gravity at rest. Determines units of velocity. [UNITS MUST MATCH ACCELERATION]
+constexpr float SD_ACC  = 1000;      //  mm/s^2    Standard deviation of acceleration. Deviations from zero are suppressed.
+constexpr float SD_VEL  = 200;       //  mm/s      Standard deviation of velocity. Deviations from target value are suppressed.
+constexpr float ALPHA   = 0.5;       //            Gain of heading update - See example "output" for more information.
+
+// Sensor scaling
+constexpr float TO_LENGTH_PER_SECOND_SQ = GRAVITY;     // Constant to convert acceleration measurements to:  length/second^2
+constexpr float TO_RAD_PER_SECOND = 1.0;               // Constant to convery gyroscope measurements to:     radians/second
 
 void setup() {
   Serial.begin(38400);
+  delay(1000);
 
   // calibrate IMU sensor
   imu.setup();
   imu.setBias();
 
   // initialize sensor fusion
-  fusion.setup( imu.ax(), imu.ay(), imu.az() );   // set initial heading 
-
-  /* Use this function to zero velocity estimate */
-  //fusion.reset();                                 
+  //fusion.setup( imu.ax(), imu.ay(), imu.az() );   // ALWAYS set initial heading with acceleration 
+  fusion.setup();
+
+  //fusion.reset();  /* Use this function to zero velocity estimate */                               
 }
 
 void loop() {   
-  /* NOTE: The heading must be updated along with the velocity estimate for accurate results.
-           Use the following steps to ensure proper integration */
-
-  // 1. measure state
-
   imu.updateBias(); 
+
+  // Measure state:  
   vec3_t accel = { imu.ax(), imu.ay(), imu.az() };
   vec3_t gyro = { imu.gx(), imu.gy(), imu.gz() };
 
-  // 2. update heading and velocity estimate
-
-  vec3_t vel_t = {0,0,0};   // Known measured velocity (target state)
-
-                                             /* ALPHA is optional - can be excluded */
+      // Scale measurements
+  accel *= TO_LENGTH_PER_SECOND_SQ;
+  gyro *= TO_RAD_PER_SECOND;
+
+  // Update heading and velocity estimate:
+
+      // known measured velocity (target state). Estimate will be forced towards this vector
+  vec3_t vel_t = {0,0,0};         
+
+      /* note: all coefficients are optional and have default values */
   fusion.update( gyro, accel, vel_t, SD_ACC, SD_VEL, GRAVITY, ALPHA ); 
-  vec3_t vel = fusion.getVel();
+
+      // obtain velocity estimate
+  vec3_t vel = fusion.getVel();   
 
-  // Display velocity components [view with serial plotter]
-
+  // Display velocity components: [view with serial plotter]
   Serial.print( vel.x, 2 );
   Serial.print( " " );
   Serial.print( vel.y, 2 );
   Serial.print( " " );
-  Serial.print( vel.z, 2 );
+  Serial.print( vel.z, 2 );  
   Serial.println();
 }

library.properties

@@ -1,5 +1,5 @@
 name=imuFilter
-version=1.6.0
+version=1.6.1
 author=RCmags <[email protected]>
 maintainer=RCmags <[email protected]>
 sentence=Sensor fusion for an IMU to obtain heading and velocity.

src/accIntegral.cpp

@@ -24,7 +24,7 @@ void accIntegral::update( vec3_t angvel,
                           vec3_t accel, 
                           vec3_t vel_t, 
                           const float SD_ACC, 
-                          const float SD_VEL, 
+                          const float SD_VEL,
                           const float GRAVITY,
                           const float ALPHA ) {
   // 0. Constants:
@@ -33,11 +33,9 @@ void accIntegral::update( vec3_t angvel,
   const float VAR_COMB = VAR_ACC*VAR_VEL;
 
   // 1. Remove acceleration bias:
-  imuFilter::update( angvel, accel, ALPHA, SD_ACC );
+  imuFilter::update( angvel, accel, ALPHA, SD_ACC, GRAVITY );
   quat_t qt = imuFilter::getQuat(); 
-
-  accel *= GRAVITY;               // convert to given units
-
+
     // Remove centrifugal
   vec3_t vel_local = qt.rotate(vel, LOCAL_FRAME);
   accel -= vel_local.cross(angvel);
@@ -78,11 +76,11 @@ void accIntegral::update( float gx, float gy, float gz,
                           float ax, float ay, float az, 
                           float vx, float vy, float vz, 
                           const float SD_ACC, 
-                          const float SD_VEL, 
+                          const float SD_VEL,
                           const float GRAVITY,
                           const float ALPHA ) {                  
   vec3_t angvel = {gx, gy, gz};
   vec3_t accel  = {ax, ay, az};
   vec3_t vel_t  = {vx, vy, vz};
-  return update( angvel, accel, vel_t, ALPHA, SD_ACC, SD_VEL, GRAVITY );
+  return update( angvel, accel, vel_t, SD_ACC, SD_VEL, GRAVITY, ALPHA );
 }

src/accIntegral.h

@@ -3,6 +3,10 @@
 #ifndef accIntegral_h
 #define accIntegral_h
 
+//------------------ Coefficients -------------------- 
+
+#define DEFAULT_SD_VEL     0.1         // Default standard deviation in velocity. [g-force * second]
+
 //---------------- Class definition ------------------ 
 
 class accIntegral: public imuFilter {
@@ -22,17 +26,17 @@ class accIntegral: public imuFilter {
     vec3_t getVel();
 
     void update( vec3_t, vec3_t, vec3_t,
-                 const float, 
-                 const float, 
-                 const float,
+                 const float=DEFAULT_SD_ACC, 
+                 const float=DEFAULT_SD_VEL,
+                 const float=DEFAULT_GRAVITY,
                  const float=DEFAULT_GAIN );
 
     void update( float, float, float, 
                  float, float, float, 
-                 float, float, float, 
-                 const float, 
-                 const float, 
-                 const float,
+                 float, float, float,  
+                 const float=DEFAULT_SD_ACC, 
+                 const float=DEFAULT_SD_VEL,
+                 const float=DEFAULT_GRAVITY,
                  const float=DEFAULT_GAIN );
 };
 

src/imuFilter.cpp

@@ -2,6 +2,10 @@
 
 //----------------- Initialization ------------------- 
 
+float imuFilter::timeStep() {
+  return dt;
+}
+
 void imuFilter::updateTimer() {
   uint32_t time_now = micros();
   uint32_t change_time = time_now - last_time;
@@ -15,10 +19,10 @@ void imuFilter::setup() {
   last_time = micros();
 }
 
-void imuFilter::setup( float ax, float ay, float az ) { 
+void imuFilter::setup( vec3_t accel ) { 
   setup();
   // set quaternion as vertical vector
-  vec3_t v = vec3_t(ax, ay, az).norm();             // gravity vector
+  vec3_t v = accel.norm();                          // gravity vector
 
   float norm = v.x*v.x + v.y*v.y;                   
   float cosine = v.z/sqrt( norm + v.z*v.z ) * 0.5;  // vertical angle
@@ -28,8 +32,8 @@ void imuFilter::setup( float ax, float ay, float az ) {
   q.v = vec3_t(v.y*norm, -v.x*norm, 0);
 }
 
-void imuFilter::setup( vec3_t accel ) {
-  setup( accel.x, accel.y, accel.z );
+void imuFilter::setup( float ax, float ay, float az ) {
+  setup( vec3_t(ax, ay, az) );
 } 
 
 //---------------- Heading estimate ------------------ 
@@ -53,29 +57,31 @@ void imuFilter::update( float gx, float gy, float gz ) {
 
 void imuFilter::update( float gx, float gy, float gz, 
                         float ax, float ay, float az, 
-                        const float ALPHA  /*=DEFAULT_GAIN*/, 
-                        const float SD_ACC /*=DEFAULT_SD*/ ) {  
+                        const float ALPHA, 
+                        const float SD_ACC,
+                        const float GRAVITY ) {  
   // Update Timer
   updateTimer();
 
   // check global acceleration:
   vec3_t accel = {ax, ay, az};
-  vec3_t acrel = q.rotate(accel, GLOBAL_FRAME) - vec3_t(0,0,1); 
+  vec3_t acrel = q.rotate(accel, GLOBAL_FRAME);
+  acrel.z -= GRAVITY; 
 
     // 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
+  float gain = 1.0/(1 + var*INV_VAR); // kalman gain
   var = error + 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  
+  gain *= ALPHA * dt;   
   vec3_t da = vec3_t(gx, gy, gz)*dt + ve*gain;       
   quat_t dq; dq.setRotation(da, SMALL_ANGLE);
 
@@ -90,8 +96,9 @@ void imuFilter::update( vec3_t gyro ) {
 }
 
 void imuFilter::update( vec3_t gyro, vec3_t accel,  
-                        const float ALPHA  /*=DEFAULT_GAIN*/, 
-                        const float SD_ACC /*=DEFAULT_SD*/ ) { 
+                        const float ALPHA, 
+                        const float SD_ACC,
+                        const float GRAVITY ) { 
   update( gyro.x , gyro.y , gyro.z, 
           accel.x, accel.y, accel.z,
           ALPHA, SD_ACC ); 
@@ -134,10 +141,6 @@ vec3_t imuFilter::projectVector( vec3_t vec, const bool TO_WORLD ) {
   return q.rotate(vec, TO_WORLD);
 }
 
-float imuFilter::timeStep() {
-  return dt;
-}
-
 //------------------ Euler Angles ------------------- 
 
 float imuFilter::roll() { // x-axis

src/imuFilter.h

@@ -8,17 +8,18 @@
 
 #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_SD_ACC      0.2         // Default standard deviation in acceleration. [g-force]
+#define DEFAULT_GRAVITY     1.0         // Default acceleration due to gravity. [g-force]
 
 //---------------- Class definition ------------------ 
 
 class imuFilter {
   private: 
-    quat_t q;
-    float var;
+    quat_t q = {1,0,0,0};
+    float var = 0;
+    uint32_t last_time = 0;
     float dt;
-    uint32_t last_time;
-
+
     void updateTimer();
 
   public:
@@ -33,12 +34,14 @@ class imuFilter {
     void update( float, float, float, 
                  float, float, float, 
                  const float=DEFAULT_GAIN, 
-                 const float=DEFAULT_SD  );
+                 const float=DEFAULT_SD_ACC,
+                 const float=DEFAULT_GRAVITY );
 
     void update( vec3_t );               
     void update( vec3_t, vec3_t, 
                  const float=DEFAULT_GAIN, 
-                 const float=DEFAULT_SD  );
+                 const float=DEFAULT_SD_ACC,
+                 const float=DEFAULT_GRAVITY );
 
     void rotateHeading( float, const bool );