update gyro calibration

README.md

@@ -5,13 +5,13 @@ The purpose of this library is to make a basic and lightweight interface for the
 - Retrieve the raw output of the sensor
 - Output scaled accelerometer and gyro values
 
+# Calibration
 The library includes two functions to calibrate the gyro and remove bias. The first is intended to be called when the sensor is turned on and is not moving. It takes a long term average of the output of each axis and subtracts these values from the raw signals.
 
 The second function is designed to update the averages. It updates the values with a moving average and the gain is controlled by something akin to a kalman filter. By polling this function one can correct for drift in the gyro bias. By combining these two functions one can obtain stable and consistent gyro outputs. However, The accelerometer needs to calibrated manually by correcting the bias and scale. 
 
-Finally, the library was written for a single MPU6050 connected to a MEGA board as follows. It may require some modifications to work succesfully on other boards:   
-
-<img src = "https://www.prometec.net/wp-content/uploads/2015/10/MPU-6050-Board-GY-521-MEGA_bb.png" width = "60%"></img>   
-
+# References
 See this link for the information on the registers of the MPU6050: 
 [MPU 6050 register map](https://invensense.tdk.com/wp-content/uploads/2015/02/MPU-6000-Register-Map1.pdf)
+
+<img src = "https://www.prometec.net/wp-content/uploads/2015/10/MPU-6050-Board-GY-521-MEGA_bb.png" width = "60%"></img>   

examples/parameters/parameters.ino

@@ -10,7 +10,7 @@
 #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  HIGH        // I2C address from state of A0 pin.   A0 -> GND : ADDRESS_A0 = LOW
+#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. 

library.properties

@@ -1,5 +1,5 @@
 name=basicMPU6050
-version=0.2.0
+version=0.3.0
 author=RCmags <memoryofatrufestival@gmail.com>
 maintainer=RCmags <memoryofatrufestival@gmail.com>
 sentence=lightweight library for the MPU6050.

src/basicMPU6050.h

@@ -30,8 +30,8 @@ constexpr float     DEFAULT_SCALE = 1;                  // Default scale of Gyro
     const float*    GX_S        = &DEFAULT_SCALE    ,   /* Gyro X Multiplier                        */  \
     const float*    GY_S        = &DEFAULT_SCALE    ,   /* Gyro Y Multiplier                        */  \
     const float*    GZ_S        = &DEFAULT_SCALE    ,   /* Gyro Z Multiplier                        */  \
-    uint16_t        GYRO_BAND   = 16                ,   /* Standard deviation of gyro signals       */  \
-    uint32_t        N_BIAS      = 10000                 /* Samples of average used to calibrate gyro*/  
+    uint16_t        GYRO_BAND   = 64                ,   /* Standard deviation of raw gyro signals   */  \
+    uint32_t        N_BIAS      = 5000                  /* Samples of average used to calibrate gyro*/  
 
 //--------------- Template Parameters ---------------- [ No characters after backlash! ]
 
@@ -75,7 +75,7 @@ template <TEMPLATE_DEFAULT>
 class basicMPU6050 {
    private:       
     float mean[N_AXIS] = {0};    
-    float var = 0;
+    float var[N_AXIS] = {0};
 
     // Common settings
     static const uint8_t MPU_ADDRESS;

src/basicMPU6050.tpp

@@ -193,6 +193,7 @@ void basicMPU6050<TEMPLATE_INPUTS>
 ::setBias() {
   for( int count = 0; count < N_BIAS; count += 1 ) { 
     int gyro[] = { rawGx(), rawGy(), rawGz() };
+    delayMicroseconds(250);
 
     // Sum all samples
     for( int index = 0; index < N_AXIS; index += 1 ) {
@@ -209,21 +210,21 @@ void basicMPU6050<TEMPLATE_INPUTS>
 template<TEMPLATE_TYPE>
 void basicMPU6050<TEMPLATE_INPUTS>
 ::updateBias() {
-  const float BAND_SQ = GYRO_BAND*GYRO_BAND;
+  constexpr float BAND_SQ = GYRO_BAND*GYRO_BAND;
 
-  // Error in reading
+  // error in measurement
   float dw[N_AXIS] = { rawGx() - mean[0] ,
                        rawGy() - mean[1] , 
                        rawGz() - mean[2] };
 
-  // Calculate kalman gain
-  float mag = dw[0]*dw[0] + dw[1]*dw[1] + dw[2]*dw[2];
-  float gain = BAND_SQ/( BAND_SQ + var + mag );
-
-  var += mag + (gain - 1)*var;                              // covariance in the magnitude of gyro signal
-
-  // Update mean with gain
+  // update mean with kalman gain
   for( int index = 0; index < N_AXIS; index += 1 ) {  
-    mean[index] += dw[index]*gain * MEAN;
+    float error_sq = dw[index] * dw[index] / BAND_SQ;
+
+    float gain = MEAN/( 1 + var[index]*var[index] );
+
+    var[index] = error_sq + var[index]*gain;
+
+    mean[index] += dw[index] * gain;
   }
 }