ADHOC/replicant-frameworks_native
3
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

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

Browse Source 
Change-Id: I51186e12fb9b2316e3671e3908174f4495df89a0
This commit is contained in:
Max Braun 2011-08-17 18:22:52 -07:00 committed by Mathias Agopian
parent 16bcf66afa
commit a01b4e237d
3 changed files with 41 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
services/sensorservice/Fusion.cpp
View File

@ -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();
}
// -----------------------------------------------------------------------

4
services/sensorservice/Fusion.h
View File

@ -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);

23
services/sensorservice/mat.h
View File

@ -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,