Fix occasional fusion divergence by detecting it and resetting the fusion.
Change-Id: I51186e12fb9b2316e3671e3908174f4495df89a0
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();
}
// -----------------------------------------------------------------------
diff --git a/services/sensorservice/Fusion.h b/services/sensorservice/Fusion.h
index 556944b..7062999 100644
--- a/services/sensorservice/Fusion.h
+++ b/services/sensorservice/Fusion.h
@@ -37,7 +37,7 @@
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 @@
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 1302ca3..a76fc91 100644
--- a/services/sensorservice/mat.h
+++ b/services/sensorservice/mat.h
@@ -295,6 +295,29 @@
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,