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#include <stdio.h> 18984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 19984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian#include <utils/Log.h> 20984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 21984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian#include "Fusion.h" 22984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 23984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopiannamespace android { 24984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 25984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian// ----------------------------------------------------------------------- 26984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 27f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu/*==================== BEGIN FUSION SENSOR PARAMETER =========================*/ 28f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu 29f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu/* Note: 30f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu * If a platform uses software fusion, it is necessary to tune the following 31f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu * parameters to fit the hardware sensors prior to release. 32f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu * 33f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu * The DEFAULT_ parameters will be used in FUSION_9AXIS and FUSION_NOMAG mode. 34f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu * The GEOMAG_ parameters will be used in FUSION_NOGYRO mode. 35f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu */ 36f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu 37eaf2d0bfe37415ba1e42a97608823e8dbef53220Mathias Agopian/* 38f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu * GYRO_VAR gives the measured variance of the gyro's output per 39eaf2d0bfe37415ba1e42a97608823e8dbef53220Mathias Agopian * Hz (or variance at 1 Hz). This is an "intrinsic" parameter of the gyro, 40eaf2d0bfe37415ba1e42a97608823e8dbef53220Mathias Agopian * which is independent of the sampling frequency. 41eaf2d0bfe37415ba1e42a97608823e8dbef53220Mathias Agopian * 42eaf2d0bfe37415ba1e42a97608823e8dbef53220Mathias Agopian * The variance of gyro's output at a given sampling period can be 43eaf2d0bfe37415ba1e42a97608823e8dbef53220Mathias Agopian * calculated as: 44f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu * variance(T) = GYRO_VAR / T 45eaf2d0bfe37415ba1e42a97608823e8dbef53220Mathias Agopian * 46eaf2d0bfe37415ba1e42a97608823e8dbef53220Mathias Agopian * The variance of the INTEGRATED OUTPUT at a given sampling period can be 47eaf2d0bfe37415ba1e42a97608823e8dbef53220Mathias Agopian * calculated as: 48f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu * variance_integrate_output(T) = GYRO_VAR * T 49eaf2d0bfe37415ba1e42a97608823e8dbef53220Mathias Agopian */ 50f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xustatic const float DEFAULT_GYRO_VAR = 1e-7; // (rad/s)^2 / Hz 51f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xustatic const float DEFAULT_GYRO_BIAS_VAR = 1e-12; // (rad/s)^2 / s (guessed) 52f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xustatic const float GEOMAG_GYRO_VAR = 1e-4; // (rad/s)^2 / Hz 53f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xustatic const float GEOMAG_GYRO_BIAS_VAR = 1e-8; // (rad/s)^2 / s (guessed) 54eaf2d0bfe37415ba1e42a97608823e8dbef53220Mathias Agopian 55eaf2d0bfe37415ba1e42a97608823e8dbef53220Mathias Agopian/* 56eaf2d0bfe37415ba1e42a97608823e8dbef53220Mathias Agopian * Standard deviations of accelerometer and magnetometer 57eaf2d0bfe37415ba1e42a97608823e8dbef53220Mathias Agopian */ 58f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xustatic const float DEFAULT_ACC_STDEV = 0.015f; // m/s^2 (measured 0.08 / CDD 0.05) 59f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xustatic const float DEFAULT_MAG_STDEV = 0.1f; // uT (measured 0.7 / CDD 0.5) 60f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xustatic const float GEOMAG_ACC_STDEV = 0.05f; // m/s^2 (measured 0.08 / CDD 0.05) 61f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xustatic const float GEOMAG_MAG_STDEV = 0.1f; // uT (measured 0.7 / CDD 0.5) 62f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu 63f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu 64f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu/* ====================== END FUSION SENSOR PARAMETER ========================*/ 65984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 66a01b4e237d57b74689576a3d486a2b2b903e74f4Max Braunstatic const float SYMMETRY_TOLERANCE = 1e-10f; 673301542828febc768e1df42892cfac4992c35474Mathias Agopian 683e87d8dadefaf4b56bf15a15f1b53928d7a12cd2Michael Johnson/* 69a83f45c6c734084422f56733c25350625594bc00Mathias Agopian * Accelerometer updates will not be performed near free fall to avoid 70a83f45c6c734084422f56733c25350625594bc00Mathias Agopian * ill-conditioning and div by zeros. 713e87d8dadefaf4b56bf15a15f1b53928d7a12cd2Michael Johnson * Threshhold: 10% of g, in m/s^2 723e87d8dadefaf4b56bf15a15f1b53928d7a12cd2Michael Johnson */ 73f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xustatic const float NOMINAL_GRAVITY = 9.81f; 74f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xustatic const float FREE_FALL_THRESHOLD = 0.1f * (NOMINAL_GRAVITY); 753e87d8dadefaf4b56bf15a15f1b53928d7a12cd2Michael Johnson 763e87d8dadefaf4b56bf15a15f1b53928d7a12cd2Michael Johnson/* 773e87d8dadefaf4b56bf15a15f1b53928d7a12cd2Michael Johnson * The geomagnetic-field should be between 30uT and 60uT. 78a83f45c6c734084422f56733c25350625594bc00Mathias Agopian * Fields strengths greater than this likely indicate a local magnetic 79a83f45c6c734084422f56733c25350625594bc00Mathias Agopian * disturbance which we do not want to update into the fused frame. 803e87d8dadefaf4b56bf15a15f1b53928d7a12cd2Michael Johnson */ 813e87d8dadefaf4b56bf15a15f1b53928d7a12cd2Michael Johnsonstatic const float MAX_VALID_MAGNETIC_FIELD = 100; // uT 82a83f45c6c734084422f56733c25350625594bc00Mathias Agopianstatic const float MAX_VALID_MAGNETIC_FIELD_SQ = 83a83f45c6c734084422f56733c25350625594bc00Mathias Agopian MAX_VALID_MAGNETIC_FIELD*MAX_VALID_MAGNETIC_FIELD; 843e87d8dadefaf4b56bf15a15f1b53928d7a12cd2Michael Johnson 853e87d8dadefaf4b56bf15a15f1b53928d7a12cd2Michael Johnson/* 86a83f45c6c734084422f56733c25350625594bc00Mathias Agopian * Values of the field smaller than this should be ignored in fusion to avoid 87a83f45c6c734084422f56733c25350625594bc00Mathias Agopian * ill-conditioning. This state can happen with anomalous local magnetic 88a83f45c6c734084422f56733c25350625594bc00Mathias Agopian * disturbances canceling the Earth field. 893e87d8dadefaf4b56bf15a15f1b53928d7a12cd2Michael Johnson */ 903e87d8dadefaf4b56bf15a15f1b53928d7a12cd2Michael Johnsonstatic const float MIN_VALID_MAGNETIC_FIELD = 10; // uT 91a83f45c6c734084422f56733c25350625594bc00Mathias Agopianstatic const float MIN_VALID_MAGNETIC_FIELD_SQ = 92a83f45c6c734084422f56733c25350625594bc00Mathias Agopian MIN_VALID_MAGNETIC_FIELD*MIN_VALID_MAGNETIC_FIELD; 933e87d8dadefaf4b56bf15a15f1b53928d7a12cd2Michael Johnson 943e87d8dadefaf4b56bf15a15f1b53928d7a12cd2Michael Johnson/* 95a83f45c6c734084422f56733c25350625594bc00Mathias Agopian * If the cross product of two vectors has magnitude squared less than this, 96a83f45c6c734084422f56733c25350625594bc00Mathias Agopian * we reject it as invalid due to alignment of the vectors. 97a83f45c6c734084422f56733c25350625594bc00Mathias Agopian * This threshold is used to check for the case where the magnetic field sample 98a83f45c6c734084422f56733c25350625594bc00Mathias Agopian * is parallel to the gravity field, which can happen in certain places due 99a83f45c6c734084422f56733c25350625594bc00Mathias Agopian * to magnetic field disturbances. 1003e87d8dadefaf4b56bf15a15f1b53928d7a12cd2Michael Johnson */ 1013e87d8dadefaf4b56bf15a15f1b53928d7a12cd2Michael Johnsonstatic const float MIN_VALID_CROSS_PRODUCT_MAG = 1.0e-3; 1023e87d8dadefaf4b56bf15a15f1b53928d7a12cd2Michael Johnsonstatic const float MIN_VALID_CROSS_PRODUCT_MAG_SQ = 1033e87d8dadefaf4b56bf15a15f1b53928d7a12cd2Michael Johnson MIN_VALID_CROSS_PRODUCT_MAG*MIN_VALID_CROSS_PRODUCT_MAG; 1043e87d8dadefaf4b56bf15a15f1b53928d7a12cd2Michael Johnson 105f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xustatic const float SQRT_3 = 1.732f; 106f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xustatic const float WVEC_EPS = 1e-4f/SQRT_3; 1073301542828febc768e1df42892cfac4992c35474Mathias Agopian// ----------------------------------------------------------------------- 108984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 109984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopiantemplate <typename TYPE, size_t C, size_t R> 110984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopianstatic mat<TYPE, R, R> scaleCovariance( 111984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian const mat<TYPE, C, R>& A, 112984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian const mat<TYPE, C, C>& P) { 113984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian // A*P*transpose(A); 114984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mat<TYPE, R, R> APAt; 115984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian for (size_t r=0 ; r<R ; r++) { 116984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian for (size_t j=r ; j<R ; j++) { 117984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian double apat(0); 118984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian for (size_t c=0 ; c<C ; c++) { 119984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian double v(A[c][r]*P[c][c]*0.5); 120984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian for (size_t k=c+1 ; k<C ; k++) 121984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian v += A[k][r] * P[c][k]; 122984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian apat += 2 * v * A[c][j]; 123984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 124984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian APAt[j][r] = apat; 125984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian APAt[r][j] = apat; 126984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 127984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 128984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian return APAt; 129984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian} 130984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 131984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopiantemplate <typename TYPE, typename OTHER_TYPE> 132984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopianstatic mat<TYPE, 3, 3> crossMatrix(const vec<TYPE, 3>& p, OTHER_TYPE diag) { 133984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mat<TYPE, 3, 3> r; 134984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian r[0][0] = diag; 135984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian r[1][1] = diag; 136984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian r[2][2] = diag; 137984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian r[0][1] = p.z; 138984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian r[1][0] =-p.z; 139984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian r[0][2] =-p.y; 140984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian r[2][0] = p.y; 141984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian r[1][2] = p.x; 142984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian r[2][1] =-p.x; 143984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian return r; 144984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian} 145984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 146984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 147984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopiantemplate<typename TYPE, size_t SIZE> 148984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopianclass Covariance { 149984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mat<TYPE, SIZE, SIZE> mSumXX; 150984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian vec<TYPE, SIZE> mSumX; 151984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian size_t mN; 152984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopianpublic: 153984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian Covariance() : mSumXX(0.0f), mSumX(0.0f), mN(0) { } 154984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian void update(const vec<TYPE, SIZE>& x) { 155984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mSumXX += x*transpose(x); 156984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mSumX += x; 157984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mN++; 158984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 159984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mat<TYPE, SIZE, SIZE> operator()() const { 160984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian const float N = 1.0f / mN; 161984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian return mSumXX*N - (mSumX*transpose(mSumX))*(N*N); 162984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 163984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian void reset() { 164984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mN = 0; 165984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mSumXX = 0; 166984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mSumX = 0; 167984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 168984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian size_t getCount() const { 169984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian return mN; 170984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 171984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian}; 172984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 173984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian// ----------------------------------------------------------------------- 174984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 175984826cc158193e61e3a00359ef4f6699c7d748aMathias AgopianFusion::Fusion() { 1763301542828febc768e1df42892cfac4992c35474Mathias Agopian Phi[0][1] = 0; 1773301542828febc768e1df42892cfac4992c35474Mathias Agopian Phi[1][1] = 1; 178984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 179984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian Ba.x = 0; 180984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian Ba.y = 0; 181984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian Ba.z = 1; 182984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 183984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian Bm.x = 0; 184984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian Bm.y = 1; 185984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian Bm.z = 0; 186984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 187667102f6b072582fe497599e0b760f9fc94ceffaMathias Agopian x0 = 0; 188667102f6b072582fe497599e0b760f9fc94ceffaMathias Agopian x1 = 0; 189667102f6b072582fe497599e0b760f9fc94ceffaMathias Agopian 190984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian init(); 191984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian} 192984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 193f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xuvoid Fusion::init(int mode) { 194984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mInitState = 0; 195a01b4e237d57b74689576a3d486a2b2b903e74f4Max Braun 1963301542828febc768e1df42892cfac4992c35474Mathias Agopian mGyroRate = 0; 197a01b4e237d57b74689576a3d486a2b2b903e74f4Max Braun 198984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mCount[0] = 0; 199984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mCount[1] = 0; 200984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mCount[2] = 0; 201a01b4e237d57b74689576a3d486a2b2b903e74f4Max Braun 202984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mData = 0; 203f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu mMode = mode; 204f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu 205f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu if (mMode != FUSION_NOGYRO) { //normal or game rotation 206f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu mParam.gyroVar = DEFAULT_GYRO_VAR; 207f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu mParam.gyroBiasVar = DEFAULT_GYRO_BIAS_VAR; 208f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu mParam.accStdev = DEFAULT_ACC_STDEV; 209f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu mParam.magStdev = DEFAULT_MAG_STDEV; 210f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu } else { 211f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu mParam.gyroVar = GEOMAG_GYRO_VAR; 212f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu mParam.gyroBiasVar = GEOMAG_GYRO_BIAS_VAR; 213f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu mParam.accStdev = GEOMAG_ACC_STDEV; 214f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu mParam.magStdev = GEOMAG_MAG_STDEV; 215f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu } 216984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian} 217984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 2183301542828febc768e1df42892cfac4992c35474Mathias Agopianvoid Fusion::initFusion(const vec4_t& q, float dT) 2193301542828febc768e1df42892cfac4992c35474Mathias Agopian{ 2203301542828febc768e1df42892cfac4992c35474Mathias Agopian // initial estimate: E{ x(t0) } 2213301542828febc768e1df42892cfac4992c35474Mathias Agopian x0 = q; 2223301542828febc768e1df42892cfac4992c35474Mathias Agopian x1 = 0; 2233301542828febc768e1df42892cfac4992c35474Mathias Agopian 224eaf2d0bfe37415ba1e42a97608823e8dbef53220Mathias Agopian // process noise covariance matrix: G.Q.Gt, with 225eaf2d0bfe37415ba1e42a97608823e8dbef53220Mathias Agopian // 226eaf2d0bfe37415ba1e42a97608823e8dbef53220Mathias Agopian // G = | -1 0 | Q = | q00 q10 | 227eaf2d0bfe37415ba1e42a97608823e8dbef53220Mathias Agopian // | 0 1 | | q01 q11 | 228eaf2d0bfe37415ba1e42a97608823e8dbef53220Mathias Agopian // 229eaf2d0bfe37415ba1e42a97608823e8dbef53220Mathias Agopian // q00 = sv^2.dt + 1/3.su^2.dt^3 230eaf2d0bfe37415ba1e42a97608823e8dbef53220Mathias Agopian // q10 = q01 = 1/2.su^2.dt^2 231eaf2d0bfe37415ba1e42a97608823e8dbef53220Mathias Agopian // q11 = su^2.dt 232eaf2d0bfe37415ba1e42a97608823e8dbef53220Mathias Agopian // 233eaf2d0bfe37415ba1e42a97608823e8dbef53220Mathias Agopian 234dc5b63e40ee697324d39fe105d6f12c2bb031fc6Mathias Agopian const float dT2 = dT*dT; 235dc5b63e40ee697324d39fe105d6f12c2bb031fc6Mathias Agopian const float dT3 = dT2*dT; 236dc5b63e40ee697324d39fe105d6f12c2bb031fc6Mathias Agopian 237dc5b63e40ee697324d39fe105d6f12c2bb031fc6Mathias Agopian // variance of integrated output at 1/dT Hz (random drift) 238f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu const float q00 = mParam.gyroVar * dT + 0.33333f * mParam.gyroBiasVar * dT3; 239eaf2d0bfe37415ba1e42a97608823e8dbef53220Mathias Agopian 240eaf2d0bfe37415ba1e42a97608823e8dbef53220Mathias Agopian // variance of drift rate ramp 241f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu const float q11 = mParam.gyroBiasVar * dT; 242f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu const float q10 = 0.5f * mParam.gyroBiasVar * dT2; 2433301542828febc768e1df42892cfac4992c35474Mathias Agopian const float q01 = q10; 244eaf2d0bfe37415ba1e42a97608823e8dbef53220Mathias Agopian 245eaf2d0bfe37415ba1e42a97608823e8dbef53220Mathias Agopian GQGt[0][0] = q00; // rad^2 2463301542828febc768e1df42892cfac4992c35474Mathias Agopian GQGt[1][0] = -q10; 2473301542828febc768e1df42892cfac4992c35474Mathias Agopian GQGt[0][1] = -q01; 248eaf2d0bfe37415ba1e42a97608823e8dbef53220Mathias Agopian GQGt[1][1] = q11; // (rad/s)^2 2493301542828febc768e1df42892cfac4992c35474Mathias Agopian 2503301542828febc768e1df42892cfac4992c35474Mathias Agopian // initial covariance: Var{ x(t0) } 251eaf2d0bfe37415ba1e42a97608823e8dbef53220Mathias Agopian // TODO: initialize P correctly 2523301542828febc768e1df42892cfac4992c35474Mathias Agopian P = 0; 2533301542828febc768e1df42892cfac4992c35474Mathias Agopian} 2543301542828febc768e1df42892cfac4992c35474Mathias Agopian 255984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopianbool Fusion::hasEstimate() const { 256f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu return ((mInitState & MAG) || (mMode == FUSION_NOMAG)) && 257f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu ((mInitState & GYRO) || (mMode == FUSION_NOGYRO)) && 258f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu (mInitState & ACC); 259984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian} 260984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 2613301542828febc768e1df42892cfac4992c35474Mathias Agopianbool Fusion::checkInitComplete(int what, const vec3_t& d, float dT) { 2623301542828febc768e1df42892cfac4992c35474Mathias Agopian if (hasEstimate()) 263984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian return true; 264984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 265984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian if (what == ACC) { 266984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mData[0] += d * (1/length(d)); 267984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mCount[0]++; 268984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mInitState |= ACC; 269f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu if (mMode == FUSION_NOGYRO ) { 270f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu mGyroRate = dT; 271f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu } 272984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } else if (what == MAG) { 273984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mData[1] += d * (1/length(d)); 274984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mCount[1]++; 275984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mInitState |= MAG; 276984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } else if (what == GYRO) { 2773301542828febc768e1df42892cfac4992c35474Mathias Agopian mGyroRate = dT; 2783301542828febc768e1df42892cfac4992c35474Mathias Agopian mData[2] += d*dT; 279984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mCount[2]++; 280f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu mInitState |= GYRO; 281984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 282984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 283f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu if (hasEstimate()) { 284984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian // Average all the values we collected so far 285984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mData[0] *= 1.0f/mCount[0]; 286f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu if (mMode != FUSION_NOMAG) { 287f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu mData[1] *= 1.0f/mCount[1]; 288f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu } 289984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mData[2] *= 1.0f/mCount[2]; 290984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 291984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian // calculate the MRPs from the data collection, this gives us 292984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian // a rough estimate of our initial state 293984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mat33_t R; 294f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu vec3_t up(mData[0]); 295f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu vec3_t east; 296f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu 297f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu if (mMode != FUSION_NOMAG) { 298f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu east = normalize(cross_product(mData[1], up)); 299f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu } else { 300f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu east = getOrthogonal(up); 301f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu } 302f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu 303984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian vec3_t north(cross_product(up, east)); 304984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian R << east << north << up; 3053301542828febc768e1df42892cfac4992c35474Mathias Agopian const vec4_t q = matrixToQuat(R); 306984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 3073301542828febc768e1df42892cfac4992c35474Mathias Agopian initFusion(q, mGyroRate); 308984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 309984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 310984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian return false; 311984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian} 312984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 313984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopianvoid Fusion::handleGyro(const vec3_t& w, float dT) { 3143301542828febc768e1df42892cfac4992c35474Mathias Agopian if (!checkInitComplete(GYRO, w, dT)) 315984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian return; 316984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 3173301542828febc768e1df42892cfac4992c35474Mathias Agopian predict(w, dT); 318984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian} 319984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 320f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xustatus_t Fusion::handleAcc(const vec3_t& a, float dT) { 321f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu if (!checkInitComplete(ACC, a, dT)) 322f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu return BAD_VALUE; 323f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu 3243301542828febc768e1df42892cfac4992c35474Mathias Agopian // ignore acceleration data if we're close to free-fall 325f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu const float l = length(a); 326f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu if (l < FREE_FALL_THRESHOLD) { 327984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian return BAD_VALUE; 3283e87d8dadefaf4b56bf15a15f1b53928d7a12cd2Michael Johnson } 329984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 330f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu const float l_inv = 1.0f/l; 331f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu 332f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu if ( mMode == FUSION_NOGYRO ) { 333f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu //geo mag 334f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu vec3_t w_dummy; 335f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu w_dummy = x1; //bias 336f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu predict(w_dummy, dT); 337f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu } 338f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu 339f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu if ( mMode == FUSION_NOMAG) { 340f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu vec3_t m; 341f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu m = getRotationMatrix()*Bm; 342f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu update(m, Bm, mParam.magStdev); 343f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu } 344984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 345f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu vec3_t unityA = a * l_inv; 346f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu const float d = sqrtf(fabsf(l- NOMINAL_GRAVITY)); 347f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu const float p = l_inv * mParam.accStdev*expf(d); 348f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu 349f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu update(unityA, Ba, p); 350984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian return NO_ERROR; 351984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian} 352984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 353984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopianstatus_t Fusion::handleMag(const vec3_t& m) { 354f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu if (!checkInitComplete(MAG, m)) 355f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu return BAD_VALUE; 356f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu 357984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian // the geomagnetic-field should be between 30uT and 60uT 3583e87d8dadefaf4b56bf15a15f1b53928d7a12cd2Michael Johnson // reject if too large to avoid spurious magnetic sources 3593e87d8dadefaf4b56bf15a15f1b53928d7a12cd2Michael Johnson const float magFieldSq = length_squared(m); 3603e87d8dadefaf4b56bf15a15f1b53928d7a12cd2Michael Johnson if (magFieldSq > MAX_VALID_MAGNETIC_FIELD_SQ) { 3613e87d8dadefaf4b56bf15a15f1b53928d7a12cd2Michael Johnson return BAD_VALUE; 3623e87d8dadefaf4b56bf15a15f1b53928d7a12cd2Michael Johnson } else if (magFieldSq < MIN_VALID_MAGNETIC_FIELD_SQ) { 363a83f45c6c734084422f56733c25350625594bc00Mathias Agopian // Also reject if too small since we will get ill-defined (zero mag) 364a83f45c6c734084422f56733c25350625594bc00Mathias Agopian // cross-products below 365984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian return BAD_VALUE; 3663e87d8dadefaf4b56bf15a15f1b53928d7a12cd2Michael Johnson } 367984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 368a83f45c6c734084422f56733c25350625594bc00Mathias Agopian // Orthogonalize the magnetic field to the gravity field, mapping it into 369a83f45c6c734084422f56733c25350625594bc00Mathias Agopian // tangent to Earth. 370984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian const vec3_t up( getRotationMatrix() * Ba ); 371984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian const vec3_t east( cross_product(m, up) ); 3723e87d8dadefaf4b56bf15a15f1b53928d7a12cd2Michael Johnson 373a83f45c6c734084422f56733c25350625594bc00Mathias Agopian // If the m and up vectors align, the cross product magnitude will 374a83f45c6c734084422f56733c25350625594bc00Mathias Agopian // approach 0. 375a83f45c6c734084422f56733c25350625594bc00Mathias Agopian // Reject this case as well to avoid div by zero problems and 376a83f45c6c734084422f56733c25350625594bc00Mathias Agopian // ill-conditioning below. 3773e87d8dadefaf4b56bf15a15f1b53928d7a12cd2Michael Johnson if (length_squared(east) < MIN_VALID_CROSS_PRODUCT_MAG_SQ) { 3783e87d8dadefaf4b56bf15a15f1b53928d7a12cd2Michael Johnson return BAD_VALUE; 3793e87d8dadefaf4b56bf15a15f1b53928d7a12cd2Michael Johnson } 3803e87d8dadefaf4b56bf15a15f1b53928d7a12cd2Michael Johnson 381a83f45c6c734084422f56733c25350625594bc00Mathias Agopian // If we have created an orthogonal magnetic field successfully, 382a83f45c6c734084422f56733c25350625594bc00Mathias Agopian // then pass it in as the update. 383984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian vec3_t north( cross_product(up, east) ); 384984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 385f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu const float l_inv = 1 / length(north); 386f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu north *= l_inv; 387984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 388f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu update(north, Bm, mParam.magStdev*l_inv); 389984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian return NO_ERROR; 390984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian} 391984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 392a01b4e237d57b74689576a3d486a2b2b903e74f4Max Braunvoid Fusion::checkState() { 393a01b4e237d57b74689576a3d486a2b2b903e74f4Max Braun // P needs to stay positive semidefinite or the fusion diverges. When we 394a01b4e237d57b74689576a3d486a2b2b903e74f4Max Braun // detect divergence, we reset the fusion. 395a01b4e237d57b74689576a3d486a2b2b903e74f4Max Braun // TODO(braun): Instead, find the reason for the divergence and fix it. 396a01b4e237d57b74689576a3d486a2b2b903e74f4Max Braun 397a01b4e237d57b74689576a3d486a2b2b903e74f4Max Braun if (!isPositiveSemidefinite(P[0][0], SYMMETRY_TOLERANCE) || 398a01b4e237d57b74689576a3d486a2b2b903e74f4Max Braun !isPositiveSemidefinite(P[1][1], SYMMETRY_TOLERANCE)) { 3993c20fbed7f3a916ced10f2ed5a272271b7d81edeSteve Block ALOGW("Sensor fusion diverged; resetting state."); 400984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian P = 0; 401984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 402984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian} 403984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 4043301542828febc768e1df42892cfac4992c35474Mathias Agopianvec4_t Fusion::getAttitude() const { 4053301542828febc768e1df42892cfac4992c35474Mathias Agopian return x0; 406984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian} 407984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 408984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopianvec3_t Fusion::getBias() const { 4093301542828febc768e1df42892cfac4992c35474Mathias Agopian return x1; 410984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian} 411984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 412984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopianmat33_t Fusion::getRotationMatrix() const { 4133301542828febc768e1df42892cfac4992c35474Mathias Agopian return quatToMatrix(x0); 414984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian} 415984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 4163301542828febc768e1df42892cfac4992c35474Mathias Agopianmat34_t Fusion::getF(const vec4_t& q) { 4173301542828febc768e1df42892cfac4992c35474Mathias Agopian mat34_t F; 418dc5b63e40ee697324d39fe105d6f12c2bb031fc6Mathias Agopian 419dc5b63e40ee697324d39fe105d6f12c2bb031fc6Mathias Agopian // This is used to compute the derivative of q 420dc5b63e40ee697324d39fe105d6f12c2bb031fc6Mathias Agopian // F = | [q.xyz]x | 421dc5b63e40ee697324d39fe105d6f12c2bb031fc6Mathias Agopian // | -q.xyz | 422dc5b63e40ee697324d39fe105d6f12c2bb031fc6Mathias Agopian 4233301542828febc768e1df42892cfac4992c35474Mathias Agopian F[0].x = q.w; F[1].x =-q.z; F[2].x = q.y; 4243301542828febc768e1df42892cfac4992c35474Mathias Agopian F[0].y = q.z; F[1].y = q.w; F[2].y =-q.x; 4253301542828febc768e1df42892cfac4992c35474Mathias Agopian F[0].z =-q.y; F[1].z = q.x; F[2].z = q.w; 4263301542828febc768e1df42892cfac4992c35474Mathias Agopian F[0].w =-q.x; F[1].w =-q.y; F[2].w =-q.z; 427984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian return F; 428984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian} 429984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 4303301542828febc768e1df42892cfac4992c35474Mathias Agopianvoid Fusion::predict(const vec3_t& w, float dT) { 4313301542828febc768e1df42892cfac4992c35474Mathias Agopian const vec4_t q = x0; 4323301542828febc768e1df42892cfac4992c35474Mathias Agopian const vec3_t b = x1; 433f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu vec3_t we = w - b; 4343301542828febc768e1df42892cfac4992c35474Mathias Agopian 435f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu if (length(we) < WVEC_EPS) { 436f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu we = (we[0]>0.f)?WVEC_EPS:-WVEC_EPS; 437f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu } 438bdf277355dcd647bd5d27b38fc107243a2247a02Mathias Agopian // q(k+1) = O(we)*q(k) 439bdf277355dcd647bd5d27b38fc107243a2247a02Mathias Agopian // -------------------- 440bdf277355dcd647bd5d27b38fc107243a2247a02Mathias Agopian // 441bdf277355dcd647bd5d27b38fc107243a2247a02Mathias Agopian // O(w) = | cos(0.5*||w||*dT)*I33 - [psi]x psi | 442bdf277355dcd647bd5d27b38fc107243a2247a02Mathias Agopian // | -psi' cos(0.5*||w||*dT) | 443bdf277355dcd647bd5d27b38fc107243a2247a02Mathias Agopian // 444bdf277355dcd647bd5d27b38fc107243a2247a02Mathias Agopian // psi = sin(0.5*||w||*dT)*w / ||w|| 445bdf277355dcd647bd5d27b38fc107243a2247a02Mathias Agopian // 446bdf277355dcd647bd5d27b38fc107243a2247a02Mathias Agopian // 4478f11b24a729c9779d75e09df27967091dc6e27c7Mathias Agopian // P(k+1) = Phi(k)*P(k)*Phi(k)' + G*Q(k)*G' 448bdf277355dcd647bd5d27b38fc107243a2247a02Mathias Agopian // ---------------------------------------- 4498f11b24a729c9779d75e09df27967091dc6e27c7Mathias Agopian // 4508f11b24a729c9779d75e09df27967091dc6e27c7Mathias Agopian // G = | -I33 0 | 4518f11b24a729c9779d75e09df27967091dc6e27c7Mathias Agopian // | 0 I33 | 4528f11b24a729c9779d75e09df27967091dc6e27c7Mathias Agopian // 4533301542828febc768e1df42892cfac4992c35474Mathias Agopian // Phi = | Phi00 Phi10 | 4543301542828febc768e1df42892cfac4992c35474Mathias Agopian // | 0 1 | 4558f11b24a729c9779d75e09df27967091dc6e27c7Mathias Agopian // 4568f11b24a729c9779d75e09df27967091dc6e27c7Mathias Agopian // Phi00 = I33 4578f11b24a729c9779d75e09df27967091dc6e27c7Mathias Agopian // - [w]x * sin(||w||*dt)/||w|| 4588f11b24a729c9779d75e09df27967091dc6e27c7Mathias Agopian // + [w]x^2 * (1-cos(||w||*dT))/||w||^2 4598f11b24a729c9779d75e09df27967091dc6e27c7Mathias Agopian // 4608f11b24a729c9779d75e09df27967091dc6e27c7Mathias Agopian // Phi10 = [w]x * (1 - cos(||w||*dt))/||w||^2 4618f11b24a729c9779d75e09df27967091dc6e27c7Mathias Agopian // - [w]x^2 * (||w||*dT - sin(||w||*dt))/||w||^3 4628f11b24a729c9779d75e09df27967091dc6e27c7Mathias Agopian // - I33*dT 4638f11b24a729c9779d75e09df27967091dc6e27c7Mathias Agopian 4643301542828febc768e1df42892cfac4992c35474Mathias Agopian const mat33_t I33(1); 4653301542828febc768e1df42892cfac4992c35474Mathias Agopian const mat33_t I33dT(dT); 4663301542828febc768e1df42892cfac4992c35474Mathias Agopian const mat33_t wx(crossMatrix(we, 0)); 4673301542828febc768e1df42892cfac4992c35474Mathias Agopian const mat33_t wx2(wx*wx); 4683301542828febc768e1df42892cfac4992c35474Mathias Agopian const float lwedT = length(we)*dT; 469bdf277355dcd647bd5d27b38fc107243a2247a02Mathias Agopian const float hlwedT = 0.5f*lwedT; 470f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu const float ilwe = 1.f/length(we); 4713301542828febc768e1df42892cfac4992c35474Mathias Agopian const float k0 = (1-cosf(lwedT))*(ilwe*ilwe); 4723301542828febc768e1df42892cfac4992c35474Mathias Agopian const float k1 = sinf(lwedT); 473bdf277355dcd647bd5d27b38fc107243a2247a02Mathias Agopian const float k2 = cosf(hlwedT); 474bdf277355dcd647bd5d27b38fc107243a2247a02Mathias Agopian const vec3_t psi(sinf(hlwedT)*ilwe*we); 475bdf277355dcd647bd5d27b38fc107243a2247a02Mathias Agopian const mat33_t O33(crossMatrix(-psi, k2)); 476bdf277355dcd647bd5d27b38fc107243a2247a02Mathias Agopian mat44_t O; 477bdf277355dcd647bd5d27b38fc107243a2247a02Mathias Agopian O[0].xyz = O33[0]; O[0].w = -psi.x; 478bdf277355dcd647bd5d27b38fc107243a2247a02Mathias Agopian O[1].xyz = O33[1]; O[1].w = -psi.y; 479bdf277355dcd647bd5d27b38fc107243a2247a02Mathias Agopian O[2].xyz = O33[2]; O[2].w = -psi.z; 480bdf277355dcd647bd5d27b38fc107243a2247a02Mathias Agopian O[3].xyz = psi; O[3].w = k2; 4813301542828febc768e1df42892cfac4992c35474Mathias Agopian 4823301542828febc768e1df42892cfac4992c35474Mathias Agopian Phi[0][0] = I33 - wx*(k1*ilwe) + wx2*k0; 4833301542828febc768e1df42892cfac4992c35474Mathias Agopian Phi[1][0] = wx*k0 - I33dT - wx2*(ilwe*ilwe*ilwe)*(lwedT-k1); 4843301542828febc768e1df42892cfac4992c35474Mathias Agopian 485bdf277355dcd647bd5d27b38fc107243a2247a02Mathias Agopian x0 = O*q; 486f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu 487bdf277355dcd647bd5d27b38fc107243a2247a02Mathias Agopian if (x0.w < 0) 488bdf277355dcd647bd5d27b38fc107243a2247a02Mathias Agopian x0 = -x0; 489bdf277355dcd647bd5d27b38fc107243a2247a02Mathias Agopian 4903301542828febc768e1df42892cfac4992c35474Mathias Agopian P = Phi*P*transpose(Phi) + GQGt; 491a01b4e237d57b74689576a3d486a2b2b903e74f4Max Braun 492a01b4e237d57b74689576a3d486a2b2b903e74f4Max Braun checkState(); 493984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian} 494984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 495984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopianvoid Fusion::update(const vec3_t& z, const vec3_t& Bi, float sigma) { 4963301542828febc768e1df42892cfac4992c35474Mathias Agopian vec4_t q(x0); 497984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian // measured vector in body space: h(p) = A(p)*Bi 4983301542828febc768e1df42892cfac4992c35474Mathias Agopian const mat33_t A(quatToMatrix(q)); 499984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian const vec3_t Bb(A*Bi); 500984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 501984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian // Sensitivity matrix H = dh(p)/dp 502984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian // H = [ L 0 ] 5033301542828febc768e1df42892cfac4992c35474Mathias Agopian const mat33_t L(crossMatrix(Bb, 0)); 504984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 5053301542828febc768e1df42892cfac4992c35474Mathias Agopian // gain... 5063301542828febc768e1df42892cfac4992c35474Mathias Agopian // K = P*Ht / [H*P*Ht + R] 5073301542828febc768e1df42892cfac4992c35474Mathias Agopian vec<mat33_t, 2> K; 508984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian const mat33_t R(sigma*sigma); 509984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian const mat33_t S(scaleCovariance(L, P[0][0]) + R); 510984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian const mat33_t Si(invert(S)); 511984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian const mat33_t LtSi(transpose(L)*Si); 512984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian K[0] = P[0][0] * LtSi; 513984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian K[1] = transpose(P[1][0])*LtSi; 514984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 5153301542828febc768e1df42892cfac4992c35474Mathias Agopian // update... 516dc5b63e40ee697324d39fe105d6f12c2bb031fc6Mathias Agopian // P = (I-K*H) * P 517dc5b63e40ee697324d39fe105d6f12c2bb031fc6Mathias Agopian // P -= K*H*P 518dc5b63e40ee697324d39fe105d6f12c2bb031fc6Mathias Agopian // | K0 | * | L 0 | * P = | K0*L 0 | * | P00 P10 | = | K0*L*P00 K0*L*P10 | 519dc5b63e40ee697324d39fe105d6f12c2bb031fc6Mathias Agopian // | K1 | | K1*L 0 | | P01 P11 | | K1*L*P00 K1*L*P10 | 520dc5b63e40ee697324d39fe105d6f12c2bb031fc6Mathias Agopian // Note: the Joseph form is numerically more stable and given by: 521dc5b63e40ee697324d39fe105d6f12c2bb031fc6Mathias Agopian // P = (I-KH) * P * (I-KH)' + K*R*R' 522984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian const mat33_t K0L(K[0] * L); 523984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian const mat33_t K1L(K[1] * L); 524984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian P[0][0] -= K0L*P[0][0]; 525984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian P[1][1] -= K1L*P[1][0]; 526984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian P[1][0] -= K0L*P[1][0]; 5273301542828febc768e1df42892cfac4992c35474Mathias Agopian P[0][1] = transpose(P[1][0]); 5283301542828febc768e1df42892cfac4992c35474Mathias Agopian 5293301542828febc768e1df42892cfac4992c35474Mathias Agopian const vec3_t e(z - Bb); 5303301542828febc768e1df42892cfac4992c35474Mathias Agopian const vec3_t dq(K[0]*e); 5313301542828febc768e1df42892cfac4992c35474Mathias Agopian 5323301542828febc768e1df42892cfac4992c35474Mathias Agopian q += getF(q)*(0.5f*dq); 5333301542828febc768e1df42892cfac4992c35474Mathias Agopian x0 = normalize_quat(q); 534f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu 535f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu if (mMode != FUSION_NOMAG) { 536f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu const vec3_t db(K[1]*e); 537f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu x1 += db; 538f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu } 539a01b4e237d57b74689576a3d486a2b2b903e74f4Max Braun 540a01b4e237d57b74689576a3d486a2b2b903e74f4Max Braun checkState(); 541984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian} 542984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 543f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xuvec3_t Fusion::getOrthogonal(const vec3_t &v) { 544f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu vec3_t w; 545f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu if (fabsf(v[0])<= fabsf(v[1]) && fabsf(v[0]) <= fabsf(v[2])) { 546f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu w[0]=0.f; 547f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu w[1] = v[2]; 548f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu w[2] = -v[1]; 549f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu } else if (fabsf(v[1]) <= fabsf(v[2])) { 550f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu w[0] = v[2]; 551f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu w[1] = 0.f; 552f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu w[2] = -v[0]; 553f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu }else { 554f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu w[0] = v[1]; 555f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu w[1] = -v[0]; 556f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu w[2] = 0.f; 557f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu } 558f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu return normalize(w); 559f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu} 560f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu 561f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu 562984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian// ----------------------------------------------------------------------- 563984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 564984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian}; // namespace android 565984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 566