Fusion.h revision a01b4e237d57b74689576a3d486a2b2b903e74f4
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 30984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopianclass Fusion { 31984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian /* 32984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * the state vector is made of two sub-vector containing respectively: 33984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * - modified Rodrigues parameters 34984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * - the estimated gyro bias 35984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian */ 363301542828febc768e1df42892cfac4992c35474Mathias Agopian quat_t x0; 373301542828febc768e1df42892cfac4992c35474Mathias Agopian vec3_t x1; 38984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 39984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian /* 40a01b4e237d57b74689576a3d486a2b2b903e74f4Max Braun * the predicated covariance matrix is made of 4 3x3 sub-matrices and it is 41984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * semi-definite positive. 42984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * 43984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * P = | P00 P10 | = | P00 P10 | 443301542828febc768e1df42892cfac4992c35474Mathias Agopian * | P01 P11 | | P10t P11 | 45984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * 46984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * Since P01 = transpose(P10), the code below never calculates or 473301542828febc768e1df42892cfac4992c35474Mathias Agopian * stores P01. 48984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian */ 49984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mat<mat33_t, 2, 2> P; 50984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 51984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian /* 523301542828febc768e1df42892cfac4992c35474Mathias Agopian * the process noise covariance matrix 53984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian */ 543301542828febc768e1df42892cfac4992c35474Mathias Agopian mat<mat33_t, 2, 2> GQGt; 55984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 56984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopianpublic: 57984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian Fusion(); 58984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian void init(); 59984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian void handleGyro(const vec3_t& w, float dT); 60984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian status_t handleAcc(const vec3_t& a); 61984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian status_t handleMag(const vec3_t& m); 623301542828febc768e1df42892cfac4992c35474Mathias Agopian vec4_t getAttitude() const; 63984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian vec3_t getBias() const; 64984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mat33_t getRotationMatrix() const; 65984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian bool hasEstimate() const; 66984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 67984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopianprivate: 683301542828febc768e1df42892cfac4992c35474Mathias Agopian mat<mat33_t, 2, 2> Phi; 69984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian vec3_t Ba, Bm; 70984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian uint32_t mInitState; 713301542828febc768e1df42892cfac4992c35474Mathias Agopian float mGyroRate; 72984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian vec<vec3_t, 3> mData; 73984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian size_t mCount[3]; 74984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian enum { ACC=0x1, MAG=0x2, GYRO=0x4 }; 753301542828febc768e1df42892cfac4992c35474Mathias Agopian bool checkInitComplete(int, const vec3_t& w, float d = 0); 763301542828febc768e1df42892cfac4992c35474Mathias Agopian void initFusion(const vec4_t& q0, float dT); 77a01b4e237d57b74689576a3d486a2b2b903e74f4Max Braun void checkState(); 783301542828febc768e1df42892cfac4992c35474Mathias Agopian void predict(const vec3_t& w, float dT); 79984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian void update(const vec3_t& z, const vec3_t& Bi, float sigma); 803301542828febc768e1df42892cfac4992c35474Mathias Agopian static mat34_t getF(const vec4_t& p); 81984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian}; 82984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 83984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian}; // namespace android 84984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 85984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian#endif // ANDROID_FUSION_H 86