173e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian/* 273e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian * Copyright (C) 2011 The Android Open Source Project 373e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian * 473e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian * Licensed under the Apache License, Version 2.0 (the "License"); 573e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian * you may not use this file except in compliance with the License. 673e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian * You may obtain a copy of the License at 773e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian * 873e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian * http://www.apache.org/licenses/LICENSE-2.0 973e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian * 1073e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian * Unless required by applicable law or agreed to in writing, software 1173e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian * distributed under the License is distributed on an "AS IS" BASIS, 1273e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 1373e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian * See the License for the specific language governing permissions and 1473e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian * limitations under the License. 1573e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian */ 1673e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 1773e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian#include <stdio.h> 1873e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 1973e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian#include <utils/Log.h> 2073e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 2173e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian#include "Fusion.h" 2273e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 2373e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopiannamespace android { 2473e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 2573e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian// ----------------------------------------------------------------------- 2673e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 27451e825847de8d670acc9495fb3d528dac8ea2bfMathias Agopian/* 28451e825847de8d670acc9495fb3d528dac8ea2bfMathias Agopian * gyroVAR gives the measured variance of the gyro's output per 29451e825847de8d670acc9495fb3d528dac8ea2bfMathias Agopian * Hz (or variance at 1 Hz). This is an "intrinsic" parameter of the gyro, 30451e825847de8d670acc9495fb3d528dac8ea2bfMathias Agopian * which is independent of the sampling frequency. 31451e825847de8d670acc9495fb3d528dac8ea2bfMathias Agopian * 32451e825847de8d670acc9495fb3d528dac8ea2bfMathias Agopian * The variance of gyro's output at a given sampling period can be 33451e825847de8d670acc9495fb3d528dac8ea2bfMathias Agopian * calculated as: 34451e825847de8d670acc9495fb3d528dac8ea2bfMathias Agopian * variance(T) = gyroVAR / T 35451e825847de8d670acc9495fb3d528dac8ea2bfMathias Agopian * 36451e825847de8d670acc9495fb3d528dac8ea2bfMathias Agopian * The variance of the INTEGRATED OUTPUT at a given sampling period can be 37451e825847de8d670acc9495fb3d528dac8ea2bfMathias Agopian * calculated as: 38451e825847de8d670acc9495fb3d528dac8ea2bfMathias Agopian * variance_integrate_output(T) = gyroVAR * T 39451e825847de8d670acc9495fb3d528dac8ea2bfMathias Agopian * 40451e825847de8d670acc9495fb3d528dac8ea2bfMathias Agopian */ 41451e825847de8d670acc9495fb3d528dac8ea2bfMathias Agopianstatic const float gyroVAR = 1e-7; // (rad/s)^2 / Hz 42451e825847de8d670acc9495fb3d528dac8ea2bfMathias Agopianstatic const float biasVAR = 1e-8; // (rad/s)^2 / s (guessed) 43451e825847de8d670acc9495fb3d528dac8ea2bfMathias Agopian 44451e825847de8d670acc9495fb3d528dac8ea2bfMathias Agopian/* 45451e825847de8d670acc9495fb3d528dac8ea2bfMathias Agopian * Standard deviations of accelerometer and magnetometer 46451e825847de8d670acc9495fb3d528dac8ea2bfMathias Agopian */ 476043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopianstatic const float accSTDEV = 0.05f; // m/s^2 (measured 0.08 / CDD 0.05) 486043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopianstatic const float magSTDEV = 0.5f; // uT (measured 0.7 / CDD 0.5) 4973e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 503d41ecd9bda3616c8a90eaaca032d48d5da64e04Max Braunstatic const float SYMMETRY_TOLERANCE = 1e-10f; 516043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian 52f5cfea78b0454a31571693ee86c321adcb965410Michael Johnson/* 533a3fca3dcedb8bcbcde5e2c037369b5ee3820646Mathias Agopian * Accelerometer updates will not be performed near free fall to avoid 543a3fca3dcedb8bcbcde5e2c037369b5ee3820646Mathias Agopian * ill-conditioning and div by zeros. 55f5cfea78b0454a31571693ee86c321adcb965410Michael Johnson * Threshhold: 10% of g, in m/s^2 56f5cfea78b0454a31571693ee86c321adcb965410Michael Johnson */ 57f5cfea78b0454a31571693ee86c321adcb965410Michael Johnsonstatic const float FREE_FALL_THRESHOLD = 0.981f; 583a3fca3dcedb8bcbcde5e2c037369b5ee3820646Mathias Agopianstatic const float FREE_FALL_THRESHOLD_SQ = 593a3fca3dcedb8bcbcde5e2c037369b5ee3820646Mathias Agopian FREE_FALL_THRESHOLD*FREE_FALL_THRESHOLD; 60f5cfea78b0454a31571693ee86c321adcb965410Michael Johnson 61f5cfea78b0454a31571693ee86c321adcb965410Michael Johnson/* 62f5cfea78b0454a31571693ee86c321adcb965410Michael Johnson * The geomagnetic-field should be between 30uT and 60uT. 633a3fca3dcedb8bcbcde5e2c037369b5ee3820646Mathias Agopian * Fields strengths greater than this likely indicate a local magnetic 643a3fca3dcedb8bcbcde5e2c037369b5ee3820646Mathias Agopian * disturbance which we do not want to update into the fused frame. 65f5cfea78b0454a31571693ee86c321adcb965410Michael Johnson */ 66f5cfea78b0454a31571693ee86c321adcb965410Michael Johnsonstatic const float MAX_VALID_MAGNETIC_FIELD = 100; // uT 673a3fca3dcedb8bcbcde5e2c037369b5ee3820646Mathias Agopianstatic const float MAX_VALID_MAGNETIC_FIELD_SQ = 683a3fca3dcedb8bcbcde5e2c037369b5ee3820646Mathias Agopian MAX_VALID_MAGNETIC_FIELD*MAX_VALID_MAGNETIC_FIELD; 69f5cfea78b0454a31571693ee86c321adcb965410Michael Johnson 70f5cfea78b0454a31571693ee86c321adcb965410Michael Johnson/* 713a3fca3dcedb8bcbcde5e2c037369b5ee3820646Mathias Agopian * Values of the field smaller than this should be ignored in fusion to avoid 723a3fca3dcedb8bcbcde5e2c037369b5ee3820646Mathias Agopian * ill-conditioning. This state can happen with anomalous local magnetic 733a3fca3dcedb8bcbcde5e2c037369b5ee3820646Mathias Agopian * disturbances canceling the Earth field. 74f5cfea78b0454a31571693ee86c321adcb965410Michael Johnson */ 75f5cfea78b0454a31571693ee86c321adcb965410Michael Johnsonstatic const float MIN_VALID_MAGNETIC_FIELD = 10; // uT 763a3fca3dcedb8bcbcde5e2c037369b5ee3820646Mathias Agopianstatic const float MIN_VALID_MAGNETIC_FIELD_SQ = 773a3fca3dcedb8bcbcde5e2c037369b5ee3820646Mathias Agopian MIN_VALID_MAGNETIC_FIELD*MIN_VALID_MAGNETIC_FIELD; 78f5cfea78b0454a31571693ee86c321adcb965410Michael Johnson 79f5cfea78b0454a31571693ee86c321adcb965410Michael Johnson/* 803a3fca3dcedb8bcbcde5e2c037369b5ee3820646Mathias Agopian * If the cross product of two vectors has magnitude squared less than this, 813a3fca3dcedb8bcbcde5e2c037369b5ee3820646Mathias Agopian * we reject it as invalid due to alignment of the vectors. 823a3fca3dcedb8bcbcde5e2c037369b5ee3820646Mathias Agopian * This threshold is used to check for the case where the magnetic field sample 833a3fca3dcedb8bcbcde5e2c037369b5ee3820646Mathias Agopian * is parallel to the gravity field, which can happen in certain places due 843a3fca3dcedb8bcbcde5e2c037369b5ee3820646Mathias Agopian * to magnetic field disturbances. 85f5cfea78b0454a31571693ee86c321adcb965410Michael Johnson */ 86f5cfea78b0454a31571693ee86c321adcb965410Michael Johnsonstatic const float MIN_VALID_CROSS_PRODUCT_MAG = 1.0e-3; 87f5cfea78b0454a31571693ee86c321adcb965410Michael Johnsonstatic const float MIN_VALID_CROSS_PRODUCT_MAG_SQ = 88f5cfea78b0454a31571693ee86c321adcb965410Michael Johnson MIN_VALID_CROSS_PRODUCT_MAG*MIN_VALID_CROSS_PRODUCT_MAG; 89f5cfea78b0454a31571693ee86c321adcb965410Michael Johnson 906043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian// ----------------------------------------------------------------------- 9173e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 9273e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopiantemplate <typename TYPE, size_t C, size_t R> 9373e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopianstatic mat<TYPE, R, R> scaleCovariance( 9473e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian const mat<TYPE, C, R>& A, 9573e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian const mat<TYPE, C, C>& P) { 9673e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian // A*P*transpose(A); 9773e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian mat<TYPE, R, R> APAt; 9873e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian for (size_t r=0 ; r<R ; r++) { 9973e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian for (size_t j=r ; j<R ; j++) { 10073e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian double apat(0); 10173e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian for (size_t c=0 ; c<C ; c++) { 10273e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian double v(A[c][r]*P[c][c]*0.5); 10373e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian for (size_t k=c+1 ; k<C ; k++) 10473e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian v += A[k][r] * P[c][k]; 10573e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian apat += 2 * v * A[c][j]; 10673e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian } 10773e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian APAt[j][r] = apat; 10873e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian APAt[r][j] = apat; 10973e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian } 11073e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian } 11173e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian return APAt; 11273e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian} 11373e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 11473e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopiantemplate <typename TYPE, typename OTHER_TYPE> 11573e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopianstatic mat<TYPE, 3, 3> crossMatrix(const vec<TYPE, 3>& p, OTHER_TYPE diag) { 11673e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian mat<TYPE, 3, 3> r; 11773e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian r[0][0] = diag; 11873e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian r[1][1] = diag; 11973e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian r[2][2] = diag; 12073e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian r[0][1] = p.z; 12173e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian r[1][0] =-p.z; 12273e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian r[0][2] =-p.y; 12373e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian r[2][0] = p.y; 12473e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian r[1][2] = p.x; 12573e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian r[2][1] =-p.x; 12673e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian return r; 12773e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian} 12873e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 12973e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 13073e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopiantemplate<typename TYPE, size_t SIZE> 13173e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopianclass Covariance { 13273e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian mat<TYPE, SIZE, SIZE> mSumXX; 13373e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian vec<TYPE, SIZE> mSumX; 13473e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian size_t mN; 13573e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopianpublic: 13673e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian Covariance() : mSumXX(0.0f), mSumX(0.0f), mN(0) { } 13773e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian void update(const vec<TYPE, SIZE>& x) { 13873e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian mSumXX += x*transpose(x); 13973e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian mSumX += x; 14073e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian mN++; 14173e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian } 14273e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian mat<TYPE, SIZE, SIZE> operator()() const { 14373e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian const float N = 1.0f / mN; 14473e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian return mSumXX*N - (mSumX*transpose(mSumX))*(N*N); 14573e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian } 14673e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian void reset() { 14773e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian mN = 0; 14873e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian mSumXX = 0; 14973e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian mSumX = 0; 15073e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian } 15173e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian size_t getCount() const { 15273e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian return mN; 15373e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian } 15473e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian}; 15573e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 15673e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian// ----------------------------------------------------------------------- 15773e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 15873e0bc805a143d8cc2202fccb73230459edc6869Mathias AgopianFusion::Fusion() { 1596043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian Phi[0][1] = 0; 1606043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian Phi[1][1] = 1; 16173e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 16273e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian Ba.x = 0; 16373e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian Ba.y = 0; 16473e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian Ba.z = 1; 16573e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 16673e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian Bm.x = 0; 16773e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian Bm.y = 1; 16873e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian Bm.z = 0; 16973e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 1706f4f8e790ea9c53d113cb6dbdfc73897aec11d37Mathias Agopian x0 = 0; 1716f4f8e790ea9c53d113cb6dbdfc73897aec11d37Mathias Agopian x1 = 0; 1726f4f8e790ea9c53d113cb6dbdfc73897aec11d37Mathias Agopian 17373e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian init(); 17473e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian} 17573e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 17673e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopianvoid Fusion::init() { 17773e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian mInitState = 0; 1783d41ecd9bda3616c8a90eaaca032d48d5da64e04Max Braun 1796043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian mGyroRate = 0; 1803d41ecd9bda3616c8a90eaaca032d48d5da64e04Max Braun 18173e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian mCount[0] = 0; 18273e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian mCount[1] = 0; 18373e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian mCount[2] = 0; 1843d41ecd9bda3616c8a90eaaca032d48d5da64e04Max Braun 18573e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian mData = 0; 18673e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian} 18773e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 1886043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopianvoid Fusion::initFusion(const vec4_t& q, float dT) 1896043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian{ 1906043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian // initial estimate: E{ x(t0) } 1916043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian x0 = q; 1926043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian x1 = 0; 1936043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian 194451e825847de8d670acc9495fb3d528dac8ea2bfMathias Agopian // process noise covariance matrix: G.Q.Gt, with 195451e825847de8d670acc9495fb3d528dac8ea2bfMathias Agopian // 196451e825847de8d670acc9495fb3d528dac8ea2bfMathias Agopian // G = | -1 0 | Q = | q00 q10 | 197451e825847de8d670acc9495fb3d528dac8ea2bfMathias Agopian // | 0 1 | | q01 q11 | 198451e825847de8d670acc9495fb3d528dac8ea2bfMathias Agopian // 199451e825847de8d670acc9495fb3d528dac8ea2bfMathias Agopian // q00 = sv^2.dt + 1/3.su^2.dt^3 200451e825847de8d670acc9495fb3d528dac8ea2bfMathias Agopian // q10 = q01 = 1/2.su^2.dt^2 201451e825847de8d670acc9495fb3d528dac8ea2bfMathias Agopian // q11 = su^2.dt 202451e825847de8d670acc9495fb3d528dac8ea2bfMathias Agopian // 203451e825847de8d670acc9495fb3d528dac8ea2bfMathias Agopian 204451e825847de8d670acc9495fb3d528dac8ea2bfMathias Agopian // variance of integrated output at 1/dT Hz 205451e825847de8d670acc9495fb3d528dac8ea2bfMathias Agopian // (random drift) 206451e825847de8d670acc9495fb3d528dac8ea2bfMathias Agopian const float q00 = gyroVAR * dT; 207451e825847de8d670acc9495fb3d528dac8ea2bfMathias Agopian 208451e825847de8d670acc9495fb3d528dac8ea2bfMathias Agopian // variance of drift rate ramp 209451e825847de8d670acc9495fb3d528dac8ea2bfMathias Agopian const float q11 = biasVAR * dT; 210451e825847de8d670acc9495fb3d528dac8ea2bfMathias Agopian 211451e825847de8d670acc9495fb3d528dac8ea2bfMathias Agopian const float u = q11 / dT; 212451e825847de8d670acc9495fb3d528dac8ea2bfMathias Agopian const float q10 = 0.5f*u*dT*dT; 2136043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian const float q01 = q10; 214451e825847de8d670acc9495fb3d528dac8ea2bfMathias Agopian 215451e825847de8d670acc9495fb3d528dac8ea2bfMathias Agopian GQGt[0][0] = q00; // rad^2 2166043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian GQGt[1][0] = -q10; 2176043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian GQGt[0][1] = -q01; 218451e825847de8d670acc9495fb3d528dac8ea2bfMathias Agopian GQGt[1][1] = q11; // (rad/s)^2 2196043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian 2206043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian // initial covariance: Var{ x(t0) } 221451e825847de8d670acc9495fb3d528dac8ea2bfMathias Agopian // TODO: initialize P correctly 2226043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian P = 0; 2236043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian} 2246043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian 22573e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopianbool Fusion::hasEstimate() const { 22673e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian return (mInitState == (MAG|ACC|GYRO)); 22773e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian} 22873e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 2296043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopianbool Fusion::checkInitComplete(int what, const vec3_t& d, float dT) { 2306043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian if (hasEstimate()) 23173e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian return true; 23273e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 23373e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian if (what == ACC) { 23473e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian mData[0] += d * (1/length(d)); 23573e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian mCount[0]++; 23673e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian mInitState |= ACC; 23773e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian } else if (what == MAG) { 23873e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian mData[1] += d * (1/length(d)); 23973e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian mCount[1]++; 24073e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian mInitState |= MAG; 24173e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian } else if (what == GYRO) { 2426043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian mGyroRate = dT; 2436043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian mData[2] += d*dT; 24473e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian mCount[2]++; 24573e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian if (mCount[2] == 64) { 24673e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian // 64 samples is good enough to estimate the gyro drift and 24773e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian // doesn't take too much time. 24873e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian mInitState |= GYRO; 24973e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian } 25073e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian } 25173e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 25273e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian if (mInitState == (MAG|ACC|GYRO)) { 25373e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian // Average all the values we collected so far 25473e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian mData[0] *= 1.0f/mCount[0]; 25573e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian mData[1] *= 1.0f/mCount[1]; 25673e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian mData[2] *= 1.0f/mCount[2]; 25773e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 25873e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian // calculate the MRPs from the data collection, this gives us 25973e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian // a rough estimate of our initial state 26073e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian mat33_t R; 26173e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian vec3_t up(mData[0]); 26273e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian vec3_t east(cross_product(mData[1], up)); 26373e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian east *= 1/length(east); 26473e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian vec3_t north(cross_product(up, east)); 26573e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian R << east << north << up; 2666043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian const vec4_t q = matrixToQuat(R); 26773e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 2686043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian initFusion(q, mGyroRate); 26973e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian } 27073e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 27173e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian return false; 27273e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian} 27373e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 27473e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopianvoid Fusion::handleGyro(const vec3_t& w, float dT) { 2756043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian if (!checkInitComplete(GYRO, w, dT)) 27673e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian return; 27773e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 2786043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian predict(w, dT); 27973e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian} 28073e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 28173e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopianstatus_t Fusion::handleAcc(const vec3_t& a) { 2826043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian // ignore acceleration data if we're close to free-fall 283f5cfea78b0454a31571693ee86c321adcb965410Michael Johnson if (length_squared(a) < FREE_FALL_THRESHOLD_SQ) { 28473e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian return BAD_VALUE; 285f5cfea78b0454a31571693ee86c321adcb965410Michael Johnson } 28673e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 28773e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian if (!checkInitComplete(ACC, a)) 28873e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian return BAD_VALUE; 28973e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 29073e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian const float l = 1/length(a); 29173e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian update(a*l, Ba, accSTDEV*l); 29273e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian return NO_ERROR; 29373e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian} 29473e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 29573e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopianstatus_t Fusion::handleMag(const vec3_t& m) { 29673e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian // the geomagnetic-field should be between 30uT and 60uT 297f5cfea78b0454a31571693ee86c321adcb965410Michael Johnson // reject if too large to avoid spurious magnetic sources 298f5cfea78b0454a31571693ee86c321adcb965410Michael Johnson const float magFieldSq = length_squared(m); 299f5cfea78b0454a31571693ee86c321adcb965410Michael Johnson if (magFieldSq > MAX_VALID_MAGNETIC_FIELD_SQ) { 300f5cfea78b0454a31571693ee86c321adcb965410Michael Johnson return BAD_VALUE; 301f5cfea78b0454a31571693ee86c321adcb965410Michael Johnson } else if (magFieldSq < MIN_VALID_MAGNETIC_FIELD_SQ) { 3023a3fca3dcedb8bcbcde5e2c037369b5ee3820646Mathias Agopian // Also reject if too small since we will get ill-defined (zero mag) 3033a3fca3dcedb8bcbcde5e2c037369b5ee3820646Mathias Agopian // cross-products below 30473e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian return BAD_VALUE; 305f5cfea78b0454a31571693ee86c321adcb965410Michael Johnson } 30673e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 30773e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian if (!checkInitComplete(MAG, m)) 30873e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian return BAD_VALUE; 30973e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 3103a3fca3dcedb8bcbcde5e2c037369b5ee3820646Mathias Agopian // Orthogonalize the magnetic field to the gravity field, mapping it into 3113a3fca3dcedb8bcbcde5e2c037369b5ee3820646Mathias Agopian // tangent to Earth. 31273e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian const vec3_t up( getRotationMatrix() * Ba ); 31373e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian const vec3_t east( cross_product(m, up) ); 314f5cfea78b0454a31571693ee86c321adcb965410Michael Johnson 3153a3fca3dcedb8bcbcde5e2c037369b5ee3820646Mathias Agopian // If the m and up vectors align, the cross product magnitude will 3163a3fca3dcedb8bcbcde5e2c037369b5ee3820646Mathias Agopian // approach 0. 3173a3fca3dcedb8bcbcde5e2c037369b5ee3820646Mathias Agopian // Reject this case as well to avoid div by zero problems and 3183a3fca3dcedb8bcbcde5e2c037369b5ee3820646Mathias Agopian // ill-conditioning below. 319f5cfea78b0454a31571693ee86c321adcb965410Michael Johnson if (length_squared(east) < MIN_VALID_CROSS_PRODUCT_MAG_SQ) { 320f5cfea78b0454a31571693ee86c321adcb965410Michael Johnson return BAD_VALUE; 321f5cfea78b0454a31571693ee86c321adcb965410Michael Johnson } 322f5cfea78b0454a31571693ee86c321adcb965410Michael Johnson 3233a3fca3dcedb8bcbcde5e2c037369b5ee3820646Mathias Agopian // If we have created an orthogonal magnetic field successfully, 3243a3fca3dcedb8bcbcde5e2c037369b5ee3820646Mathias Agopian // then pass it in as the update. 32573e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian vec3_t north( cross_product(up, east) ); 32673e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 32773e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian const float l = 1 / length(north); 32873e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian north *= l; 32973e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 33073e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian update(north, Bm, magSTDEV*l); 33173e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian return NO_ERROR; 33273e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian} 33373e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 3343d41ecd9bda3616c8a90eaaca032d48d5da64e04Max Braunvoid Fusion::checkState() { 3353d41ecd9bda3616c8a90eaaca032d48d5da64e04Max Braun // P needs to stay positive semidefinite or the fusion diverges. When we 3363d41ecd9bda3616c8a90eaaca032d48d5da64e04Max Braun // detect divergence, we reset the fusion. 3373d41ecd9bda3616c8a90eaaca032d48d5da64e04Max Braun // TODO(braun): Instead, find the reason for the divergence and fix it. 3383d41ecd9bda3616c8a90eaaca032d48d5da64e04Max Braun 3393d41ecd9bda3616c8a90eaaca032d48d5da64e04Max Braun if (!isPositiveSemidefinite(P[0][0], SYMMETRY_TOLERANCE) || 3403d41ecd9bda3616c8a90eaaca032d48d5da64e04Max Braun !isPositiveSemidefinite(P[1][1], SYMMETRY_TOLERANCE)) { 3418564c8da817a845353d213acd8636b76f567b234Steve Block ALOGW("Sensor fusion diverged; resetting state."); 34273e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian P = 0; 34373e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian } 34473e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian} 34573e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 3466043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopianvec4_t Fusion::getAttitude() const { 3476043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian return x0; 34873e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian} 34973e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 35073e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopianvec3_t Fusion::getBias() const { 3516043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian return x1; 35273e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian} 35373e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 35473e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopianmat33_t Fusion::getRotationMatrix() const { 3556043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian return quatToMatrix(x0); 35673e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian} 35773e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 3586043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopianmat34_t Fusion::getF(const vec4_t& q) { 3596043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian mat34_t F; 3606043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian F[0].x = q.w; F[1].x =-q.z; F[2].x = q.y; 3616043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian F[0].y = q.z; F[1].y = q.w; F[2].y =-q.x; 3626043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian F[0].z =-q.y; F[1].z = q.x; F[2].z = q.w; 3636043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian F[0].w =-q.x; F[1].w =-q.y; F[2].w =-q.z; 36473e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian return F; 36573e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian} 36673e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 3676043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopianvoid Fusion::predict(const vec3_t& w, float dT) { 3686043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian const vec4_t q = x0; 3696043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian const vec3_t b = x1; 3706043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian const vec3_t we = w - b; 3716043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian const vec4_t dq = getF(q)*((0.5f*dT)*we); 3726043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian x0 = normalize_quat(q + dq); 3736043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian 3746043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian // P(k+1) = F*P(k)*Ft + G*Q*Gt 3756043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian 3766043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian // Phi = | Phi00 Phi10 | 3776043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian // | 0 1 | 3786043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian const mat33_t I33(1); 3796043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian const mat33_t I33dT(dT); 3806043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian const mat33_t wx(crossMatrix(we, 0)); 3816043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian const mat33_t wx2(wx*wx); 3826043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian const float lwedT = length(we)*dT; 3836043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian const float ilwe = 1/length(we); 3846043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian const float k0 = (1-cosf(lwedT))*(ilwe*ilwe); 3856043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian const float k1 = sinf(lwedT); 3866043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian 3876043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian Phi[0][0] = I33 - wx*(k1*ilwe) + wx2*k0; 3886043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian Phi[1][0] = wx*k0 - I33dT - wx2*(ilwe*ilwe*ilwe)*(lwedT-k1); 3896043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian 3906043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian P = Phi*P*transpose(Phi) + GQGt; 3913d41ecd9bda3616c8a90eaaca032d48d5da64e04Max Braun 3923d41ecd9bda3616c8a90eaaca032d48d5da64e04Max Braun checkState(); 39373e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian} 39473e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 39573e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopianvoid Fusion::update(const vec3_t& z, const vec3_t& Bi, float sigma) { 3966043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian vec4_t q(x0); 39773e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian // measured vector in body space: h(p) = A(p)*Bi 3986043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian const mat33_t A(quatToMatrix(q)); 39973e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian const vec3_t Bb(A*Bi); 40073e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 40173e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian // Sensitivity matrix H = dh(p)/dp 40273e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian // H = [ L 0 ] 4036043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian const mat33_t L(crossMatrix(Bb, 0)); 40473e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 4056043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian // gain... 4066043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian // K = P*Ht / [H*P*Ht + R] 4076043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian vec<mat33_t, 2> K; 40873e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian const mat33_t R(sigma*sigma); 40973e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian const mat33_t S(scaleCovariance(L, P[0][0]) + R); 41073e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian const mat33_t Si(invert(S)); 41173e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian const mat33_t LtSi(transpose(L)*Si); 41273e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian K[0] = P[0][0] * LtSi; 41373e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian K[1] = transpose(P[1][0])*LtSi; 41473e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 4156043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian // update... 41673e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian // P -= K*H*P; 41773e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian const mat33_t K0L(K[0] * L); 41873e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian const mat33_t K1L(K[1] * L); 41973e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian P[0][0] -= K0L*P[0][0]; 42073e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian P[1][1] -= K1L*P[1][0]; 42173e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian P[1][0] -= K0L*P[1][0]; 4226043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian P[0][1] = transpose(P[1][0]); 4236043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian 4246043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian const vec3_t e(z - Bb); 4256043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian const vec3_t dq(K[0]*e); 4266043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian const vec3_t db(K[1]*e); 4276043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian 4286043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian q += getF(q)*(0.5f*dq); 4296043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian x0 = normalize_quat(q); 4306043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian x1 += db; 4313d41ecd9bda3616c8a90eaaca032d48d5da64e04Max Braun 4323d41ecd9bda3616c8a90eaaca032d48d5da64e04Max Braun checkState(); 43373e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian} 43473e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 43573e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian// ----------------------------------------------------------------------- 43673e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 43773e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian}; // namespace android 43873e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 439