Fusion.h revision 984826cc158193e61e3a00359ef4f6699c7d748a
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 "vec.h" 23#include "mat.h" 24 25namespace android { 26 27class Fusion { 28 /* 29 * the state vector is made of two sub-vector containing respectively: 30 * - modified Rodrigues parameters 31 * - the estimated gyro bias 32 */ 33 vec<vec3_t, 2> x; 34 35 /* 36 * the predicated covariance matrix is made of 4 3x3 sub-matrices and it 37 * semi-definite positive. 38 * 39 * P = | P00 P10 | = | P00 P10 | 40 * | P01 P11 | | P10t Q1 | 41 * 42 * Since P01 = transpose(P10), the code below never calculates or 43 * stores P01. P11 is always equal to Q1, so we don't store it either. 44 */ 45 mat<mat33_t, 2, 2> P; 46 47 /* 48 * the process noise covariance matrix is made of 2 3x3 sub-matrices 49 * Q0 encodes the attitude's noise 50 * Q1 encodes the bias' noise 51 */ 52 vec<mat33_t, 2> Q; 53 54 static const float gyroSTDEV = 1.0e-5; // rad/s (measured 1.2e-5) 55 static const float accSTDEV = 0.05f; // m/s^2 (measured 0.08 / CDD 0.05) 56 static const float magSTDEV = 0.5f; // uT (measured 0.7 / CDD 0.5) 57 static const float biasSTDEV = 2e-9; // rad/s^2 (guessed) 58 59public: 60 Fusion(); 61 void init(); 62 void handleGyro(const vec3_t& w, float dT); 63 status_t handleAcc(const vec3_t& a); 64 status_t handleMag(const vec3_t& m); 65 vec3_t getAttitude() const; 66 vec3_t getBias() const; 67 mat33_t getRotationMatrix() const; 68 bool hasEstimate() const; 69 70private: 71 vec3_t Ba, Bm; 72 uint32_t mInitState; 73 vec<vec3_t, 3> mData; 74 size_t mCount[3]; 75 enum { ACC=0x1, MAG=0x2, GYRO=0x4 }; 76 bool checkInitComplete(int, const vec3_t&); 77 bool checkState(const vec3_t& v); 78 void predict(const vec3_t& w); 79 void update(const vec3_t& z, const vec3_t& Bi, float sigma); 80 static mat33_t getF(const vec3_t& p); 81 static mat33_t getdFdp(const vec3_t& p, const vec3_t& we); 82}; 83 84}; // namespace android 85 86#endif // ANDROID_FUSION_H 87