From a01b4e237d57b74689576a3d486a2b2b903e74f4 Mon Sep 17 00:00:00 2001
From: Max Braun <braun@google.com>
Date: Wed, 17 Aug 2011 18:22:52 -0700
Subject: [PATCH] Fix occasional fusion divergence by detecting it and
 resetting the fusion.

Change-Id: I51186e12fb9b2316e3671e3908174f4495df89a0
---
 services/sensorservice/Fusion.cpp | 27 ++++++++++++++++-----------
 services/sensorservice/Fusion.h   |  4 ++--
 services/sensorservice/mat.h      | 23 +++++++++++++++++++++++
 3 files changed, 41 insertions(+), 13 deletions(-)

diff --git a/services/sensorservice/Fusion.cpp b/services/sensorservice/Fusion.cpp
index d706af598..ff4786bf0 100644
--- a/services/sensorservice/Fusion.cpp
+++ b/services/sensorservice/Fusion.cpp
@@ -48,6 +48,7 @@ static const float accSTDEV  = 0.05f;   // m/s^2 (measured 0.08 / CDD 0.05)
 static const float magSTDEV  = 0.5f;    // uT    (measured 0.7  / CDD 0.5)
 
 static const float FREE_FALL_THRESHOLD = 0.981f;
+static const float SYMMETRY_TOLERANCE = 1e-10f;
 
 // -----------------------------------------------------------------------
 
@@ -134,10 +135,13 @@ Fusion::Fusion() {
 
 void Fusion::init() {
     mInitState = 0;
+
     mGyroRate = 0;
+
     mCount[0] = 0;
     mCount[1] = 0;
     mCount[2] = 0;
+
     mData = 0;
 }
 
@@ -267,19 +271,16 @@ status_t Fusion::handleMag(const vec3_t& m) {
     return NO_ERROR;
 }
 
-bool Fusion::checkState(const vec3_t& v) {
-    if (isnanf(length(v))) {
-        LOGW("9-axis fusion diverged. reseting state.");
+void Fusion::checkState() {
+    // P needs to stay positive semidefinite or the fusion diverges. When we
+    // detect divergence, we reset the fusion.
+    // TODO(braun): Instead, find the reason for the divergence and fix it.
+
+    if (!isPositiveSemidefinite(P[0][0], SYMMETRY_TOLERANCE) ||
+        !isPositiveSemidefinite(P[1][1], SYMMETRY_TOLERANCE)) {
+        LOGW("Sensor fusion diverged; resetting state.");
         P = 0;
-        x1 = 0;
-        mInitState = 0;
-        mCount[0] = 0;
-        mCount[1] = 0;
-        mCount[2] = 0;
-        mData = 0;
-        return false;
     }
-    return true;
 }
 
 vec4_t Fusion::getAttitude() const {
@@ -327,6 +328,8 @@ void Fusion::predict(const vec3_t& w, float dT) {
     Phi[1][0] = wx*k0 - I33dT - wx2*(ilwe*ilwe*ilwe)*(lwedT-k1);
 
     P = Phi*P*transpose(Phi) + GQGt;
+
+    checkState();
 }
 
 void Fusion::update(const vec3_t& z, const vec3_t& Bi, float sigma) {
@@ -365,6 +368,8 @@ void Fusion::update(const vec3_t& z, const vec3_t& Bi, float sigma) {
     q += getF(q)*(0.5f*dq);
     x0 = normalize_quat(q);
     x1 += db;
+
+    checkState();
 }
 
 // -----------------------------------------------------------------------
diff --git a/services/sensorservice/Fusion.h b/services/sensorservice/Fusion.h
index 556944bbe..7062999b7 100644
--- a/services/sensorservice/Fusion.h
+++ b/services/sensorservice/Fusion.h
@@ -37,7 +37,7 @@ class Fusion {
     vec3_t  x1;
 
     /*
-     * the predicated covariance matrix is made of 4 3x3 sub-matrices and it
+     * the predicated covariance matrix is made of 4 3x3 sub-matrices and it is
      * semi-definite positive.
      *
      * P = | P00  P10 | = | P00  P10 |
@@ -74,7 +74,7 @@ private:
     enum { ACC=0x1, MAG=0x2, GYRO=0x4 };
     bool checkInitComplete(int, const vec3_t& w, float d = 0);
     void initFusion(const vec4_t& q0, float dT);
-    bool checkState(const vec3_t& v);
+    void checkState();
     void predict(const vec3_t& w, float dT);
     void update(const vec3_t& z, const vec3_t& Bi, float sigma);
     static mat34_t getF(const vec4_t& p);
diff --git a/services/sensorservice/mat.h b/services/sensorservice/mat.h
index 1302ca327..a76fc91cb 100644
--- a/services/sensorservice/mat.h
+++ b/services/sensorservice/mat.h
@@ -295,6 +295,29 @@ mat<TYPE, R, C> PURE transpose(const mat<TYPE, C, R>& m) {
     return r;
 }
 
+// Calculate the trace of a matrix
+template <typename TYPE, size_t C> static TYPE trace(const mat<TYPE, C, C>& m) {
+    TYPE t;
+    for (size_t i=0 ; i<C ; i++)
+        t += m[i][i];
+    return t;
+}
+
+// Test positive-semidefiniteness of a matrix
+template <typename TYPE, size_t C>
+static bool isPositiveSemidefinite(const mat<TYPE, C, C>& m, TYPE tolerance) {
+    for (size_t i=0 ; i<C ; i++)
+        if (m[i][i] < 0)
+            return false;
+
+    for (size_t i=0 ; i<C ; i++)
+      for (size_t j=i+1 ; j<C ; j++)
+          if (fabs(m[i][j] - m[j][i]) > tolerance)
+              return false;
+
+    return true;
+}
+
 // Transpose a vector
 template <
     template<typename T, size_t S> class VEC,