timestep and update

examples/output/output.ino

@@ -70,5 +70,10 @@ void loop() {
   Serial.print( fusion.yaw() );
   Serial.print( " " );
   Serial.print( fusion.roll() );
+
+  // timestep
+  Serial.print( " " );
+  Serial.print( fusion.timeStep(), 6 );
+
   Serial.println(); 
 }

examples/velocity/velocity.ino

@@ -2,8 +2,13 @@
  This sketch shows to integrate acceleration to obtain an estimate of velocity
 */
 
-/* NOTE: 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. 
+/* 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.
+
+   2. REQUIRED UNITS:
+      I. The gyroscope must output: radians/second
+      II. The accelerometer output:  g-force         [1 g-force ~ 9.81 meter/second^2] 
 */
 
 #include <basicMPU6050.h>   // Library for IMU sensor. See this link: https://github.com/RCmags/basicMPU6050
@@ -32,8 +37,9 @@ basicMPU6050<LP_FILTER,  GYRO_SENS,  ACCEL_SENS, ADDRESS_A0,
 accIntegral fusion;
                                             //  Unit            
 constexpr float GRAVITY = 9.81e3;           //  mm/s^2          Magnitude of gravity at rest. Determines units of velocity.
-constexpr float SD_ACC  = 200;              //  mm/s^2          Standard deviation of acceleration. Deviations from zero are suppressed.
+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]
 
 void setup() {
   Serial.begin(38400);
@@ -44,7 +50,9 @@ void setup() {
 
   // initialize sensor fusion
   fusion.setup( imu.ax(), imu.ay(), imu.az() );   // set initial heading 
-  fusion.reset();                                 // zero velocity estimate
+
+  /* Use this function to zero velocity estimate */
+  //fusion.reset();                                 
 }
 
 void loop() {   
@@ -57,17 +65,13 @@ void loop() {
   vec3_t accel = { imu.ax(), imu.ay(), imu.az() };
   vec3_t gyro = { imu.gx(), imu.gy(), imu.gz() };
 
-  // 2. update heading
-
-  fusion.update( gyro, accel );
-
-  // 3. update velocity estimate
+  // 2. update heading and velocity estimate
 
   vec3_t vel_t = {0,0,0};   // Known measured velocity (target state)
-  vec3_t vel;               // placeholder variable 
-
-  fusion.updateVel( gyro, accel, vel_t, SD_ACC, SD_VEL, GRAVITY );
-  vel = fusion.getVel();
+
+                                             /* ALPHA is optional - can be excluded */
+  fusion.update( gyro, accel, vel_t, SD_ACC, SD_VEL, GRAVITY, ALPHA ); 
+  vec3_t vel = fusion.getVel();
 
   // Display velocity components [view with serial plotter]
 

library.properties

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

src/accIntegral.cpp

@@ -11,38 +11,31 @@ void accIntegral::reset() {
   // scalars
   var_vel = 0;
   var_acc = 0;
-  time_last = micros();
 }
 
-//---------------------- Other -----------------------  
+//---------------- Velocity estimate ----------------- 
 
 vec3_t accIntegral::getVel() {
   return vel;
 }
 
-float accIntegral::updateTimer() {
-  uint32_t time_now = micros();
-  uint32_t change_time = time_now - time_last;
-  time_last = time_now;
-  return float(change_time)*1e-6;         
-}
-
-//---------------- Velocity estimate ----------------- 
-
 // update velocity with imu measurements 
-void accIntegral::updateVel( vec3_t angvel, 
-                             vec3_t accel, 
-                             vec3_t vel_t, 
-                             const float SD_ACC, 
-                             const float SD_VEL, 
-                             const float GRAVITY ) {
+void accIntegral::update( vec3_t angvel, 
+                          vec3_t accel, 
+                          vec3_t vel_t, 
+                          const float SD_ACC, 
+                          const float SD_VEL, 
+                          const float GRAVITY,
+                          const float ALPHA ) {
   // 0. Constants:
   const float VAR_ACC = SD_ACC*SD_ACC;
   const float VAR_VEL = SD_VEL*SD_VEL;
   const float VAR_COMB = VAR_ACC*VAR_VEL;
 
   // 1. Remove acceleration bias:
-  quat_t qt = this->getQuat(); 
+  imuFilter::update( angvel, accel, ALPHA, SD_ACC );
+  quat_t qt = imuFilter::getQuat(); 
+
   accel *= GRAVITY;               // convert to given units
 
     // Remove centrifugal
@@ -67,27 +60,29 @@ void accIntegral::updateVel( vec3_t angvel,
   vec3_t dvel = vel_t - vel;
 
   float dvel_mag = dvel.dot(dvel);
-  float gain_vel = VAR_COMB/(VAR_COMB + var_vel*var_acc); // gain affected by velocity and acceleration
+                // gain affected by velocity and acceleration
+  float gain_vel = VAR_COMB/(VAR_COMB + var_vel*var_acc); 
 
   var_vel = dvel_mag + gain_vel*var_vel;        
 
     // Integrate       
-  float dt = updateTimer();
+  float dt = imuFilter::timeStep();
   vel += 0.5*(accel + accel_last)*dt;     // trapezoidal rule
   vel += dvel*gain_vel;                   // update with target velocity
 
   accel_last = accel;
 }
 
 // verbose update
-void accIntegral::updateVel( 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 GRAVITY ) {                  
+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 GRAVITY,
+                          const float ALPHA ) {                  
   vec3_t angvel = {gx, gy, gz};
   vec3_t accel  = {ax, ay, az};
   vec3_t vel_t  = {vx, vy, vz};
-  return updateVel( angvel, accel, vel_t, SD_ACC, SD_VEL, GRAVITY );
+  return update( angvel, accel, vel_t, ALPHA, SD_ACC, SD_VEL, GRAVITY );
 }

src/accIntegral.h

@@ -3,42 +3,37 @@
 #ifndef accIntegral_h
 #define accIntegral_h
 
-//------------------ Coefficients -------------------- 
-
-#define DEFAULT_GRAVITY     9.80665     // Default gravity magnitude [ m/s^2 ]
-
 //---------------- Class definition ------------------ 
 
 class accIntegral: public imuFilter {
   private:
     // vectors
-    vec3_t   vel;           
-    vec3_t   accel_mean;     
-    vec3_t   accel_last;
+    vec3_t   vel = 0;           
+    vec3_t   accel_mean = {0,0,0};     
+    vec3_t   accel_last = {0,0,0};
 
     // scalars
-    float    var_vel;       
-    float    var_acc; 
-    uint32_t time_last;      
+    float    var_vel = 0;       
+    float    var_acc = 0;     
 
-    // functions
-    float updateTimer();
-
   public:  
     void reset();
+
     vec3_t getVel();
 
-    void updateVel( vec3_t, vec3_t, vec3_t, 
-                    const float, 
-                    const float, 
-                    const float=DEFAULT_GRAVITY );
-
-    void updateVel( float, float, float, 
-                    float, float, float, 
-                    float, float, float, 
-                    const float, 
-                    const float, 
-                    const float=DEFAULT_GRAVITY );
+    void update( vec3_t, vec3_t, vec3_t,
+                 const float, 
+                 const float, 
+                 const float,
+                 const float=DEFAULT_GAIN );
+
+    void update( float, float, float, 
+                 float, float, float, 
+                 float, float, float, 
+                 const float, 
+                 const float, 
+                 const float,
+                 const float=DEFAULT_GAIN );
 };
 
 #endif

src/imuFilter.cpp

@@ -2,11 +2,11 @@
 
 //----------------- Initialization ------------------- 
 
-float imuFilter::updateTimer() {
+void imuFilter::updateTimer() {
   uint32_t time_now = micros();
   uint32_t change_time = time_now - last_time;
   last_time = time_now;
-  return float(change_time)*1e-6;         
+  dt = float(change_time)*1e-6;         
 }
 
 void imuFilter::setup() {
@@ -38,7 +38,7 @@ void imuFilter::setup( vec3_t accel ) {
 
 void imuFilter::update( float gx, float gy, float gz ) {
   // Update Timer
-  float dt = updateTimer();
+  updateTimer();
 
   // Rotation increment
   vec3_t da = vec3_t(gx, gy, gz)*dt;
@@ -56,13 +56,13 @@ void imuFilter::update( float gx, float gy, float gz,
                         const float ALPHA  /*=DEFAULT_GAIN*/, 
                         const float SD_ACC /*=DEFAULT_SD*/ ) {  
   // Update Timer
-  float dt = updateTimer();
+  updateTimer();
 
   // check global acceleration:
   vec3_t accel = {ax, ay, az};
   vec3_t acrel = q.rotate(accel, GLOBAL_FRAME) - vec3_t(0,0,1); 
 
-  	// kalmal filter:
+    // kalmal filter:
   const float INV_VAR = 1.0/( SD_ACC * SD_ACC );
   float error = acrel.dot(acrel);     // deviation from vertical
 
@@ -134,6 +134,10 @@ 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

@@ -16,15 +16,16 @@ class imuFilter {
   private: 
     quat_t q;
     float var;
+    float dt;
     uint32_t last_time;
 
-    float updateTimer();
+    void updateTimer();
 
   public:
     // Initialization:
     void setup();
     void setup( float, float, float );
-	void setup( vec3_t );
+    void setup( vec3_t );
 
     // Heading estimate:
     void update( float, float, float );
@@ -36,11 +37,13 @@ class imuFilter {
 
     void update( vec3_t );               
     void update( vec3_t, vec3_t, 
-    			 const float=DEFAULT_GAIN, 
+                 const float=DEFAULT_GAIN, 
                  const float=DEFAULT_SD  );
 
     void rotateHeading( float, const bool );
 
+    float timeStep();
+
     //-- Fusion outputs:
 
     // Quaternion