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

Commit e74f83a5 authored by Mathias Agopian's avatar Mathias Agopian Committed by Android (Google) Code Review
Browse files

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

parents 30867d8b 3d41ecd9

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,