Update Kalman.h in example as well

examples/MPU6050/Kalman.h

@@ -22,20 +22,21 @@ class Kalman {
 public:
     Kalman() {
         /* We will set the variables like so, these can also be tuned by the user */
-        Q_angle = 0.001;
-        Q_bias = 0.003;
-        R_measure = 0.03;
+        Q_angle = 0.001f;
+        Q_bias = 0.003f;
+        R_measure = 0.03f;
 
-        angle = 0; // Reset the angle
-        bias = 0; // Reset bias
+        angle = 0.0f; // Reset the angle
+        bias = 0.0f; // Reset bias
 
-        P[0][0] = 0; // Since we assume that the bias is 0 and we know the starting angle (use setAngle), the error covariance matrix is set like so - see: http://en.wikipedia.org/wiki/Kalman_filter#Example_application.2C_technical
-        P[0][1] = 0;
-        P[1][0] = 0;
-        P[1][1] = 0;
+        P[0][0] = 0.0f; // Since we assume that the bias is 0 and we know the starting angle (use setAngle), the error covariance matrix is set like so - see: http://en.wikipedia.org/wiki/Kalman_filter#Example_application.2C_technical
+        P[0][1] = 0.0f;
+        P[1][0] = 0.0f;
+        P[1][1] = 0.0f;
     };
+
     // The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds
-    double getAngle(double newAngle, double newRate, double dt) {
+    float getAngle(float newAngle, float newRate, float dt) {
         // KasBot V2  -  Kalman filter module - http://www.x-firm.com/?page_id=145
         // Modified by Kristian Lauszus
         // See my blog post for more information: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it
@@ -56,53 +57,55 @@ public:
         // Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
         // Calculate Kalman gain - Compute the Kalman gain
         /* Step 4 */
-        S = P[0][0] + R_measure;
+        float S = P[0][0] + R_measure; // Estimate error
         /* Step 5 */
+        float K[2]; // Kalman gain - This is a 2x1 vector
         K[0] = P[0][0] / S;
         K[1] = P[1][0] / S;
 
         // Calculate angle and bias - Update estimate with measurement zk (newAngle)
         /* Step 3 */
-        y = newAngle - angle;
+        float y = newAngle - angle; // Angle difference
         /* Step 6 */
         angle += K[0] * y;
         bias += K[1] * y;
 
         // Calculate estimation error covariance - Update the error covariance
         /* Step 7 */
-        P[0][0] -= K[0] * P[0][0];
-        P[0][1] -= K[0] * P[0][1];
-        P[1][0] -= K[1] * P[0][0];
-        P[1][1] -= K[1] * P[0][1];
+        float P00_temp = P[0][0];
+        float P01_temp = P[0][1];
+
+        P[0][0] -= K[0] * P00_temp;
+        P[0][1] -= K[0] * P01_temp;
+        P[1][0] -= K[1] * P00_temp;
+        P[1][1] -= K[1] * P01_temp;
 
         return angle;
     };
-    void setAngle(double newAngle) { angle = newAngle; }; // Used to set angle, this should be set as the starting angle
-    double getRate() { return rate; }; // Return the unbiased rate
+
+    void setAngle(float newAngle) { angle = newAngle; }; // Used to set angle, this should be set as the starting angle
+    float getRate() { return rate; }; // Return the unbiased rate
 
     /* These are used to tune the Kalman filter */
-    void setQangle(double newQ_angle) { Q_angle = newQ_angle; };
-    void setQbias(double newQ_bias) { Q_bias = newQ_bias; };
-    void setRmeasure(double newR_measure) { R_measure = newR_measure; };
+    void setQangle(float newQ_angle) { Q_angle = newQ_angle; };
+    void setQbias(float newQ_bias) { Q_bias = newQ_bias; };
+    void setRmeasure(float newR_measure) { R_measure = newR_measure; };
 
-    double getQangle() { return Q_angle; };
-    double getQbias() { return Q_bias; };
-    double getRmeasure() { return R_measure; };
+    float getQangle() { return Q_angle; };
+    float getQbias() { return Q_bias; };
+    float getRmeasure() { return R_measure; };
 
 private:
     /* Kalman filter variables */
-    double Q_angle; // Process noise variance for the accelerometer
-    double Q_bias; // Process noise variance for the gyro bias
-    double R_measure; // Measurement noise variance - this is actually the variance of the measurement noise
-
-    double angle; // The angle calculated by the Kalman filter - part of the 2x1 state vector
-    double bias; // The gyro bias calculated by the Kalman filter - part of the 2x1 state vector
-    double rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate
-
-    double P[2][2]; // Error covariance matrix - This is a 2x2 matrix
-    double K[2]; // Kalman gain - This is a 2x1 vector
-    double y; // Angle difference
-    double S; // Estimate error
+    float Q_angle; // Process noise variance for the accelerometer
+    float Q_bias; // Process noise variance for the gyro bias
+    float R_measure; // Measurement noise variance - this is actually the variance of the measurement noise
+
+    float angle; // The angle calculated by the Kalman filter - part of the 2x1 state vector
+    float bias; // The gyro bias calculated by the Kalman filter - part of the 2x1 state vector
+    float rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate
+
+    float P[2][2]; // Error covariance matrix - This is a 2x2 matrix
 };
 
 #endif