From 517bbe80a7d8a3efe86d2fe126efca0884cf1997 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Tue, 11 Jun 2013 13:02:20 -0700
Subject: [PATCH] Initial values, report error.

---
 interface/src/SerialInterface.cpp |  2 ++
 interface/src/SerialInterface.h   | 10 ++++++++--
 2 files changed, 10 insertions(+), 2 deletions(-)

diff --git a/interface/src/SerialInterface.cpp b/interface/src/SerialInterface.cpp
index cbd03f801b..f4af4437d8 100644
--- a/interface/src/SerialInterface.cpp
+++ b/interface/src/SerialInterface.cpp
@@ -249,6 +249,8 @@ void SerialInterface::readData(float deltaTime) {
                 _angularAccelToLinearAccel * angularAcceleration;
             glm::vec3 error = _estimatedAcceleration - predictedAcceleration;
             
+            printLog("error %g\n", glm::length(error));
+            
             // adjust according to error in each dimension, in proportion to input magnitudes
             for (int i = 0; i < 3; i++) {
                 if (fabsf(error[i]) < EPSILON) {
diff --git a/interface/src/SerialInterface.h b/interface/src/SerialInterface.h
index b7ead8c9d7..2bb7e541ac 100644
--- a/interface/src/SerialInterface.h
+++ b/interface/src/SerialInterface.h
@@ -31,8 +31,14 @@ public:
                         _estimatedVelocity(0, 0, 0),
                         _lastAcceleration(0, 0, 0),
                         _lastRotationRates(0, 0, 0),
-                        _angularVelocityToLinearAccel(0),
-                        _angularAccelToLinearAccel(0)
+                        _angularVelocityToLinearAccel( // experimentally derived initial values
+                            0.001f, -0.008f, 0.020f, 
+                            0.003f, -0.003f, 0.025f,
+                            0.017f, 0.007f, 0.029f),
+                        _angularAccelToLinearAccel( // experimentally derived initial values
+                            0.0f, 0.0f, 0.002f,
+                            0.0f, 0.0f, 0.002f,
+                            -0.002f, -0.002f, 0.0f)
                     {}
     
     void pair();