1984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian/* 2984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * Copyright (C) 2011 The Android Open Source Project 3984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * 4984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * Licensed under the Apache License, Version 2.0 (the "License"); 5984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * you may not use this file except in compliance with the License. 6984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * You may obtain a copy of the License at 7984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * 8984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * http://www.apache.org/licenses/LICENSE-2.0 9984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * 10984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * Unless required by applicable law or agreed to in writing, software 11984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * distributed under the License is distributed on an "AS IS" BASIS, 12984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * See the License for the specific language governing permissions and 14984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * limitations under the License. 15984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian */ 16984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 17984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian#ifndef ANDROID_FUSION_H 18984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian#define ANDROID_FUSION_H 19984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 20984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian#include <utils/Errors.h> 21984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 223301542828febc768e1df42892cfac4992c35474Mathias Agopian#include "quat.h" 23984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian#include "mat.h" 243301542828febc768e1df42892cfac4992c35474Mathias Agopian#include "vec.h" 25984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 26984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopiannamespace android { 27984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 283301542828febc768e1df42892cfac4992c35474Mathias Agopiantypedef mat<float, 3, 4> mat34_t; 293301542828febc768e1df42892cfac4992c35474Mathias Agopian 30f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xuenum FUSION_MODE{ 31f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu FUSION_9AXIS, // use accel gyro mag 32f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu FUSION_NOMAG, // use accel gyro (game rotation, gravity) 33f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu FUSION_NOGYRO, // use accel mag (geomag rotation) 34f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu NUM_FUSION_MODE 35f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu}; 36f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu 37984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopianclass Fusion { 38984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian /* 39984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * the state vector is made of two sub-vector containing respectively: 40984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * - modified Rodrigues parameters 41984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * - the estimated gyro bias 42984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian */ 433301542828febc768e1df42892cfac4992c35474Mathias Agopian quat_t x0; 443301542828febc768e1df42892cfac4992c35474Mathias Agopian vec3_t x1; 45984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 46984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian /* 47a01b4e237d57b74689576a3d486a2b2b903e74f4Max Braun * the predicated covariance matrix is made of 4 3x3 sub-matrices and it is 48984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * semi-definite positive. 49984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * 50984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * P = | P00 P10 | = | P00 P10 | 513301542828febc768e1df42892cfac4992c35474Mathias Agopian * | P01 P11 | | P10t P11 | 52984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * 53984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * Since P01 = transpose(P10), the code below never calculates or 543301542828febc768e1df42892cfac4992c35474Mathias Agopian * stores P01. 55984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian */ 56984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mat<mat33_t, 2, 2> P; 57984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 58984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian /* 593301542828febc768e1df42892cfac4992c35474Mathias Agopian * the process noise covariance matrix 60984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian */ 613301542828febc768e1df42892cfac4992c35474Mathias Agopian mat<mat33_t, 2, 2> GQGt; 62984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 63984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopianpublic: 64984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian Fusion(); 65f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu void init(int mode = FUSION_9AXIS); 66984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian void handleGyro(const vec3_t& w, float dT); 67f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu status_t handleAcc(const vec3_t& a, float dT); 68984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian status_t handleMag(const vec3_t& m); 693301542828febc768e1df42892cfac4992c35474Mathias Agopian vec4_t getAttitude() const; 70984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian vec3_t getBias() const; 71984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mat33_t getRotationMatrix() const; 72984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian bool hasEstimate() const; 73984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 74984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopianprivate: 75f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu struct Parameter { 76f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu float gyroVar; 77f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu float gyroBiasVar; 78f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu float accStdev; 79f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu float magStdev; 80f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu } mParam; 81f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu 823301542828febc768e1df42892cfac4992c35474Mathias Agopian mat<mat33_t, 2, 2> Phi; 83984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian vec3_t Ba, Bm; 84984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian uint32_t mInitState; 853301542828febc768e1df42892cfac4992c35474Mathias Agopian float mGyroRate; 86984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian vec<vec3_t, 3> mData; 87984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian size_t mCount[3]; 88f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu int mMode; 89f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu 90984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian enum { ACC=0x1, MAG=0x2, GYRO=0x4 }; 913301542828febc768e1df42892cfac4992c35474Mathias Agopian bool checkInitComplete(int, const vec3_t& w, float d = 0); 923301542828febc768e1df42892cfac4992c35474Mathias Agopian void initFusion(const vec4_t& q0, float dT); 93a01b4e237d57b74689576a3d486a2b2b903e74f4Max Braun void checkState(); 943301542828febc768e1df42892cfac4992c35474Mathias Agopian void predict(const vec3_t& w, float dT); 95984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian void update(const vec3_t& z, const vec3_t& Bi, float sigma); 963301542828febc768e1df42892cfac4992c35474Mathias Agopian static mat34_t getF(const vec4_t& p); 97f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu static vec3_t getOrthogonal(const vec3_t &v); 98984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian}; 99984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 100984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian}; // namespace android 101984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 102984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian#endif // ANDROID_FUSION_H 103