src/fusion-sensor/rotation_vector/fusion_utils/orientation_filter.h

   1 /*
   2  * Copyright (C) 2011 The Android Open Source Project
   3  *
   4  * Licensed under the Apache License, Version 2.0 (the "License");
   5  * you may not use this file except in compliance with the License.
   6  * You may obtain a copy of the License at
   7  *
   8  *      http://www.apache.org/licenses/LICENSE-2.0
   9  *
  10  * Unless required by applicable law or agreed to in writing, software
  11  * distributed under the License is distributed on an "AS IS" BASIS,
  12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  13  * See the License for the specific language governing permissions and
  14  * limitations under the License.
  15  */
  16
  17 // released in android-11.0.0_r9
  18
  19 #ifndef ANDROID_FUSION_H
  20 #define ANDROID_FUSION_H
  21
  22 #include "quat.h"
  23 #include "mat.h"
  24 #include "vec.h"
  25 #include "errors.h"
  26
  27 namespace android {
  28
  29 typedef mat<float, 3, 4> mat34_t;
  30
  31 enum FUSION_MODE{
  32     FUSION_9AXIS, // use accel gyro mag
  33     FUSION_NOMAG, // use accel gyro (game rotation, gravity)
  34     FUSION_NOGYRO, // use accel mag (geomag rotation)
  35     NUM_FUSION_MODE
  36 };
  37
  38 class orientation_filter {
  39     /*
  40      * the state vector is made of two sub-vector containing respectively:
  41      * - modified Rodrigues parameters
  42      * - the estimated gyro bias
  43      */
  44     quat_t  x0;
  45     vec3_t  x1;
  46
  47     /*
  48      * the predicated covariance matrix is made of 4 3x3 sub-matrices and it is
  49      * semi-definite positive.
  50      *
  51      * P = | P00  P10 | = | P00  P10 |
  52      *     | P01  P11 |   | P10t P11 |
  53      *
  54      * Since P01 = transpose(P10), the code below never calculates or
  55      * stores P01.
  56      */
  57     mat<mat33_t, 2, 2> P;
  58
  59     /*
  60      * the process noise covariance matrix
  61      */
  62     mat<mat33_t, 2, 2> GQGt;
  63
  64 public:
  65     orientation_filter();
  66     void init(int mode = FUSION_9AXIS);
  67     void handleGyro(const vec3_t& w, float dT);
  68     status_t handleAcc(const vec3_t& a, float dT);
  69     status_t handleMag(const vec3_t& m);
  70     vec4_t getAttitude() const;
  71     vec3_t getBias() const;
  72     mat33_t getRotationMatrix() const;
  73     bool hasEstimate() const;
  74
  75 private:
  76     struct Parameter {
  77         float gyroVar;
  78         float gyroBiasVar;
  79         float accStdev;
  80         float magStdev;
  81     } mParam;
  82
  83     mat<mat33_t, 2, 2> Phi;
  84     vec3_t Ba, Bm;
  85     uint32_t mInitState;
  86     float mGyroRate;
  87     vec<vec3_t, 3> mData;
  88     size_t mCount[3];
  89     int mMode;
  90
  91     enum { ACC=0x1, MAG=0x2, GYRO=0x4 };
  92     bool checkInitComplete(int, const vec3_t& w, float d = 0);
  93     void initFusion(const vec4_t& q0, float dT);
  94     void checkState();
  95     void predict(const vec3_t& w, float dT);
  96     void update(const vec3_t& z, const vec3_t& Bi, float sigma);
  97     static mat34_t getF(const vec4_t& p);
  98     static vec3_t getOrthogonal(const vec3_t &v);
  99 };
 100
 101 }
 102
 103 #endif // ANDROID_FUSION_H