commit a01b4e237d57b74689576a3d486a2b2b903e74f4
author Max Braun <braun@google.com> Wed Aug 17 18:22:52 2011 -0700
committer Mathias Agopian <mathias@google.com> Wed Jun 27 17:07:55 2012 -0700
tree ffd68e8a310051e32e3d5c4d793178265e04b2f1
parent 16bcf66afa333f8d3f3b835ed556e4fce8fa35bf

Fix occasional fusion divergence by detecting it and resetting the fusion.

Change-Id: I51186e12fb9b2316e3671e3908174f4495df89a0

services/sensorservice/Fusion.cpp

diff --git a/services/sensorservice/Fusion.cpp b/services/sensorservice/Fusion.cpp
index d706af5..ff4786b 100644
--- a/services/sensorservice/Fusion.cpp
+++ b/services/sensorservice/Fusion.cpp
@@ -48,6 +48,7 @@
 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 @@
 
 void Fusion::init() {
     mInitState = 0;
+
     mGyroRate = 0;
+
     mCount[0] = 0;
     mCount[1] = 0;
     mCount[2] = 0;
+
     mData = 0;
 }
 
@@ -267,19 +271,16 @@
     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 @@
     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 @@
     q += getF(q)*(0.5f*dq);
     x0 = normalize_quat(q);
     x1 += db;
+
+    checkState();
 }
 
 // -----------------------------------------------------------------------