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 #ifndef ANDROID_FUSION_H 18 #define ANDROID_FUSION_H 19 20 #include <utils/Errors.h> 21 22 #include "quat.h" 23 #include "mat.h" 24 #include "vec.h" 25 26 namespace android { 27 28 typedef mat<float, 3, 4> mat34_t; 29 30 enum FUSION_MODE{ 31 FUSION_9AXIS, // use accel gyro mag 32 FUSION_NOMAG, // use accel gyro (game rotation, gravity) 33 FUSION_NOGYRO, // use accel mag (geomag rotation) 34 NUM_FUSION_MODE 35 }; 36 37 class Fusion { 38 /* 39 * the state vector is made of two sub-vector containing respectively: 40 * - modified Rodrigues parameters 41 * - the estimated gyro bias 42 */ 43 quat_t x0; 44 vec3_t x1; 45 46 /* 47 * the predicated covariance matrix is made of 4 3x3 sub-matrices and it is 48 * semi-definite positive. 49 * 50 * P = | P00 P10 | = | P00 P10 | 51 * | P01 P11 | | P10t P11 | 52 * 53 * Since P01 = transpose(P10), the code below never calculates or 54 * stores P01. 55 */ 56 mat<mat33_t, 2, 2> P; 57 58 /* 59 * the process noise covariance matrix 60 */ 61 mat<mat33_t, 2, 2> GQGt; 62 63 public: 64 Fusion(); 65 void init(int mode = FUSION_9AXIS); 66 void handleGyro(const vec3_t& w, float dT); 67 status_t handleAcc(const vec3_t& a, float dT); 68 status_t handleMag(const vec3_t& m); 69 vec4_t getAttitude() const; 70 vec3_t getBias() const; 71 mat33_t getRotationMatrix() const; 72 bool hasEstimate() const; 73 74 private: 75 struct Parameter { 76 float gyroVar; 77 float gyroBiasVar; 78 float accStdev; 79 float magStdev; 80 } mParam; 81 82 mat<mat33_t, 2, 2> Phi; 83 vec3_t Ba, Bm; 84 uint32_t mInitState; 85 float mGyroRate; 86 vec<vec3_t, 3> mData; 87 size_t mCount[3]; 88 int mMode; 89 90 enum { ACC=0x1, MAG=0x2, GYRO=0x4 }; 91 bool checkInitComplete(int, const vec3_t& w, float d = 0); 92 void initFusion(const vec4_t& q0, float dT); 93 void checkState(); 94 void predict(const vec3_t& w, float dT); 95 void update(const vec3_t& z, const vec3_t& Bi, float sigma); 96 static mat34_t getF(const vec4_t& p); 97 static vec3_t getOrthogonal(const vec3_t &v); 98 }; 99 100 }; // namespace android 101 102 #endif // ANDROID_FUSION_H 103