Added sensor fusion for more stable gyro roll, pitch.

interface/src/Application.cpp

@@ -19,6 +19,7 @@
 #endif
 
 #include <glm/gtx/quaternion.hpp>
+#include <glm/gtx/vector_angle.hpp>
 
 #include <QActionGroup>
 #include <QBoxLayout>

interface/src/Avatar.cpp

@@ -7,6 +7,7 @@
 
 #include <glm/glm.hpp>
 #include <glm/gtx/quaternion.hpp>
+#include <glm/gtx/vector_angle.hpp>
 #include <vector>
 #include <lodepng.h>
 #include <SharedUtil.h>
@@ -278,22 +279,13 @@ void Avatar::updateHeadFromGyros(float deltaTime, SerialInterface* serialInterfa
     const float AMPLIFY_PITCH = 2.f;
     const float AMPLIFY_YAW = 2.f;
     const float AMPLIFY_ROLL = 2.f;
+         
     
-    float measuredPitchRate = serialInterface->getLastPitchRate();
-    float measuredYawRate = serialInterface->getLastYawRate();
-    float measuredRollRate = serialInterface->getLastRollRate();
-   
-    //  Update avatar head position based on measured gyro rates
-    _head.addPitch(measuredPitchRate * AMPLIFY_PITCH * deltaTime);
-    _head.addYaw(measuredYawRate * AMPLIFY_YAW * deltaTime);
-    _head.addRoll(measuredRollRate * AMPLIFY_ROLL * deltaTime);
-     
-    /*
     glm::vec3 estimatedRotation = serialInterface->getEstimatedRotation();
     _head.setPitch(estimatedRotation.x * AMPLIFY_PITCH);
     _head.setYaw(estimatedRotation.y * AMPLIFY_YAW);
     _head.setRoll(estimatedRotation.z * AMPLIFY_ROLL);
-    */
+        
     
     //  Update head lean distance based on accelerometer data
     glm::vec3 headRotationRates(_head.getPitch(), _head.getYaw(), _head.getRoll());

interface/src/SerialInterface.cpp

@@ -6,6 +6,8 @@
 //
 
 #include "SerialInterface.h"
+#include <glm/gtx/vector_angle.hpp>
+#include <math.h>
 
 #ifdef __APPLE__
 #include <regex.h>
@@ -15,6 +17,7 @@
 
 const short NO_READ_MAXIMUM_MSECS = 3000;
 const int GRAVITY_SAMPLES = 60;                     //  Use the first few samples to baseline values
+const int SENSOR_FUSION_SAMPLES = 200;
 const int LONG_TERM_RATE_SAMPLES = 1000;            
 
 const bool USING_INVENSENSE_MPU9150 = 1;
@@ -227,6 +230,7 @@ void SerialInterface::readData(float deltaTime) {
 
         //  Update raw rotation estimates
         _estimatedRotation += deltaTime * (_lastRotationRates - _averageRotationRates);
+
         
         //  Update estimated position and velocity
         float const DECAY_VELOCITY = 0.95f;
@@ -235,10 +239,7 @@ void SerialInterface::readData(float deltaTime) {
         _estimatedPosition += deltaTime * _estimatedVelocity;
         _estimatedVelocity *= DECAY_VELOCITY;
         _estimatedPosition *= DECAY_POSITION;
-        
-        //glm::vec3 baseline = glm::normalize(_gravity);
-        //glm::vec3 current = glm::normalize(_lastAcceleration);
-        
+                
         //  Accumulate a set of initial baseline readings for setting gravity
         if (totalSamples == 0) {
             _averageRotationRates = _lastRotationRates;
@@ -255,8 +256,31 @@ void SerialInterface::readData(float deltaTime) {
             if (totalSamples < GRAVITY_SAMPLES) {
                 _gravity = (1.f - 1.f/(float)GRAVITY_SAMPLES) * _gravity +
                 1.f/(float)GRAVITY_SAMPLES * _lastAcceleration;
+            } else {
+                //  Use gravity reading to do sensor fusion on the pitch and roll estimation
+                float truePitchAngle = glm::angle(glm::normalize(glm::vec3(0, _gravity.y, _gravity.z)),
+                                                  glm::normalize(glm::vec3(0, _lastAcceleration.y, _lastAcceleration.z))) *
+                ((_lastAcceleration.z > _gravity.z) ? -1.0 : 1.0);
+                
+                float trueRollAngle = glm::angle(glm::normalize(glm::vec3(_gravity.x, _gravity.y, 0)),
+                                                 glm::normalize(glm::vec3(_lastAcceleration.x, _lastAcceleration.y, 0))) *
+                ((_lastAcceleration.x > _gravity.x) ? -1.0 : 1.0);
+                
+                //  PER:  BUG:   This is bizarre, because glm::angle() SOMETIMES returns NaN for what seem to
+                //               be perfectly valid inputs.  So I added these NaN tests,  gotta fix.  
+                if (!glm::isnan(truePitchAngle) && !glm::isnan(trueRollAngle)) {
+                    _estimatedRotation.x = (1.f - 1.f/(float)SENSOR_FUSION_SAMPLES) * _estimatedRotation.x +
+                    1.f/(float)SENSOR_FUSION_SAMPLES * truePitchAngle;
+                    _estimatedRotation.z = (1.f - 1.f/(float)SENSOR_FUSION_SAMPLES) * _estimatedRotation.z +
+                    1.f/(float)SENSOR_FUSION_SAMPLES * trueRollAngle;
+                    //  Without a compass heading, always decay estimated Yaw slightly
+                    const float YAW_DECAY = 0.995;
+                    _estimatedRotation.y *= YAW_DECAY;
+                }
+                
             }
         }
+         
         
         totalSamples++;
     } 

interface/src/Util.cpp

@@ -57,7 +57,9 @@ void eulerToOrthonormals(glm::vec3 * angles, glm::vec3 * front, glm::vec3 * righ
     front->x = frontNew.x;  front->y = frontNew.y;  front->z = frontNew.z;
 }
 
-
+void printVector(glm::vec3 vec) {
+    printf("%4.2f, %4.2f, %4.2f\n", vec.x, vec.y, vec.z);
+}
 
 //  Return the azimuth angle in degrees between two points.
 float azimuth_to(glm::vec3 head_pos, glm::vec3 source_pos) {

interface/src/Util.h

@@ -43,6 +43,8 @@ void noiseTest(int w, int h);
 
 void drawVector(glm::vec3* vector);
 
+void printVector(glm::vec3 vec);
+
 float angleBetween(const glm::vec3& v1, const glm::vec3& v2); 
 
 glm::quat rotationBetween(const glm::vec3& v1, const glm::vec3& v2);