Donate to e Foundation | Murena handsets with /e/OS | Own a part of Murena! Learn more

Commit a01b4e23 authored by Max Braun's avatar Max Braun Committed by Mathias Agopian
Browse files

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

Change-Id: I51186e12fb9b2316e3671e3908174f4495df89a0
parent 16bcf66a

services/sensorservice/Fusion.cpp

+16 −11
Original line number Diff line number Diff line
@@ -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();

}



// -----------------------------------------------------------------------

services/sensorservice/Fusion.h

+2 −2
Original line number Diff line number Diff line
@@ -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);

services/sensorservice/mat.h

+23 −0
Original line number Diff line number Diff line
@@ -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,