services/sensorservice/Fusion.cpp - android_frameworks_native - Gitiles

 /*
  * Copyright (C) 2011 The Android Open Source Project
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  *
  *      http://www.apache.org/licenses/LICENSE-2.0
  *
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  */

 #include <stdio.h>

 #include <utils/Log.h>

 #include "Fusion.h"

 namespace android {

 // -----------------------------------------------------------------------

 /*==================== BEGIN FUSION SENSOR PARAMETER =========================*/

 /* Note:
  *   If a platform uses software fusion, it is necessary to tune the following
  *   parameters to fit the hardware sensors prior to release.
  *
  *   The DEFAULT_ parameters will be used in FUSION_9AXIS and FUSION_NOMAG mode.
  *   The GEOMAG_ parameters will be used in FUSION_NOGYRO mode.
  */

 /*
  * GYRO_VAR gives the measured variance of the gyro's output per
  * Hz (or variance at 1 Hz). This is an "intrinsic" parameter of the gyro,
  * which is independent of the sampling frequency.
  *
  * The variance of gyro's output at a given sampling period can be
  * calculated as:
  *      variance(T) = GYRO_VAR / T
  *
  * The variance of the INTEGRATED OUTPUT at a given sampling period can be
  * calculated as:
  *       variance_integrate_output(T) = GYRO_VAR * T
  */
 static const float DEFAULT_GYRO_VAR = 1e-7;      // (rad/s)^2 / Hz
 static const float DEFAULT_GYRO_BIAS_VAR = 1e-12;  // (rad/s)^2 / s (guessed)
 static const float GEOMAG_GYRO_VAR = 1e-4;      // (rad/s)^2 / Hz
 static const float GEOMAG_GYRO_BIAS_VAR = 1e-8;  // (rad/s)^2 / s (guessed)

 /*
  * Standard deviations of accelerometer and magnetometer
  */
 static const float DEFAULT_ACC_STDEV  = 0.015f; // m/s^2 (measured 0.08 / CDD 0.05)
 static const float DEFAULT_MAG_STDEV  = 0.1f;   // uT    (measured 0.7  / CDD 0.5)
 static const float GEOMAG_ACC_STDEV  = 0.05f; // m/s^2 (measured 0.08 / CDD 0.05)
 static const float GEOMAG_MAG_STDEV  = 0.1f;   // uT    (measured 0.7  / CDD 0.5)


 /* ====================== END FUSION SENSOR PARAMETER ========================*/

 static const float SYMMETRY_TOLERANCE = 1e-10f;

 /*
  * Accelerometer updates will not be performed near free fall to avoid
  * ill-conditioning and div by zeros.
  * Threshhold: 10% of g, in m/s^2
  */
 static const float NOMINAL_GRAVITY = 9.81f;
 static const float FREE_FALL_THRESHOLD = 0.1f * (NOMINAL_GRAVITY);

 /*
  * The geomagnetic-field should be between 30uT and 60uT.
  * Fields strengths greater than this likely indicate a local magnetic
  * disturbance which we do not want to update into the fused frame.
  */
 static const float MAX_VALID_MAGNETIC_FIELD = 100; // uT
 static const float MAX_VALID_MAGNETIC_FIELD_SQ =
         MAX_VALID_MAGNETIC_FIELD*MAX_VALID_MAGNETIC_FIELD;

 /*
  * Values of the field smaller than this should be ignored in fusion to avoid
  * ill-conditioning. This state can happen with anomalous local magnetic
  * disturbances canceling the Earth field.
  */
 static const float MIN_VALID_MAGNETIC_FIELD = 10; // uT
 static const float MIN_VALID_MAGNETIC_FIELD_SQ =
         MIN_VALID_MAGNETIC_FIELD*MIN_VALID_MAGNETIC_FIELD;

 /*
  * If the cross product of two vectors has magnitude squared less than this,
  * we reject it as invalid due to alignment of the vectors.
  * This threshold is used to check for the case where the magnetic field sample
  * is parallel to the gravity field, which can happen in certain places due
  * to magnetic field disturbances.
  */
 static const float MIN_VALID_CROSS_PRODUCT_MAG = 1.0e-3;
 static const float MIN_VALID_CROSS_PRODUCT_MAG_SQ =
     MIN_VALID_CROSS_PRODUCT_MAG*MIN_VALID_CROSS_PRODUCT_MAG;

 static const float SQRT_3 = 1.732f;
 static const float WVEC_EPS = 1e-4f/SQRT_3;
 // -----------------------------------------------------------------------

 template <typename TYPE, size_t C, size_t R>
 static mat<TYPE, R, R> scaleCovariance(
         const mat<TYPE, C, R>& A,
         const mat<TYPE, C, C>& P) {
     // A*P*transpose(A);
     mat<TYPE, R, R> APAt;
     for (size_t r=0 ; r<R ; r++) {
         for (size_t j=r ; j<R ; j++) {
             double apat(0);
             for (size_t c=0 ; c<C ; c++) {
                 double v(A[c][r]*P[c][c]*0.5);
                 for (size_t k=c+1 ; k<C ; k++)
                     v += A[k][r] * P[c][k];
                 apat += 2 * v * A[c][j];
             }
             APAt[j][r] = apat;
             APAt[r][j] = apat;
         }
     }
     return APAt;
 }

 template <typename TYPE, typename OTHER_TYPE>
 static mat<TYPE, 3, 3> crossMatrix(const vec<TYPE, 3>& p, OTHER_TYPE diag) {
     mat<TYPE, 3, 3> r;
     r[0][0] = diag;
     r[1][1] = diag;
     r[2][2] = diag;
     r[0][1] = p.z;
     r[1][0] =-p.z;
     r[0][2] =-p.y;
     r[2][0] = p.y;
     r[1][2] = p.x;
     r[2][1] =-p.x;
     return r;
 }


 template<typename TYPE, size_t SIZE>
 class Covariance {
     mat<TYPE, SIZE, SIZE> mSumXX;
     vec<TYPE, SIZE> mSumX;
     size_t mN;
 public:
     Covariance() : mSumXX(0.0f), mSumX(0.0f), mN(0) { }
     void update(const vec<TYPE, SIZE>& x) {
         mSumXX += x*transpose(x);
         mSumX  += x;
         mN++;
     }
     mat<TYPE, SIZE, SIZE> operator()() const {
         const float N = 1.0f / mN;
         return mSumXX*N - (mSumX*transpose(mSumX))*(N*N);
     }
     void reset() {
         mN = 0;
         mSumXX = 0;
         mSumX = 0;
     }
     size_t getCount() const {
         return mN;
     }
 };

 // -----------------------------------------------------------------------

 Fusion::Fusion() {
     Phi[0][1] = 0;
     Phi[1][1] = 1;

     Ba.x = 0;
     Ba.y = 0;
     Ba.z = 1;

     Bm.x = 0;
     Bm.y = 1;
     Bm.z = 0;

     x0 = 0;
     x1 = 0;

     init();
 }

 void Fusion::init(int mode) {
     mInitState = 0;

     mGyroRate = 0;

     mCount[0] = 0;
     mCount[1] = 0;
     mCount[2] = 0;

     mData = 0;
     mMode = mode;

     if (mMode != FUSION_NOGYRO) { //normal or game rotation
         mParam.gyroVar = DEFAULT_GYRO_VAR;
         mParam.gyroBiasVar = DEFAULT_GYRO_BIAS_VAR;
         mParam.accStdev = DEFAULT_ACC_STDEV;
         mParam.magStdev = DEFAULT_MAG_STDEV;
     } else {
         mParam.gyroVar = GEOMAG_GYRO_VAR;
         mParam.gyroBiasVar = GEOMAG_GYRO_BIAS_VAR;
         mParam.accStdev = GEOMAG_ACC_STDEV;
         mParam.magStdev = GEOMAG_MAG_STDEV;
     }
 }

 void Fusion::initFusion(const vec4_t& q, float dT)
 {
     // initial estimate: E{ x(t0) }
     x0 = q;
     x1 = 0;

     // process noise covariance matrix: G.Q.Gt, with
     //
     //  G = | -1 0 |        Q = | q00 q10 |
     //      |  0 1 |            | q01 q11 |
     //
     // q00 = sv^2.dt + 1/3.su^2.dt^3
     // q10 = q01 = 1/2.su^2.dt^2
     // q11 = su^2.dt
     //

     const float dT2 = dT*dT;
     const float dT3 = dT2*dT;

     // variance of integrated output at 1/dT Hz (random drift)
     const float q00 = mParam.gyroVar * dT + 0.33333f * mParam.gyroBiasVar * dT3;

     // variance of drift rate ramp
     const float q11 = mParam.gyroBiasVar * dT;
     const float q10 = 0.5f * mParam.gyroBiasVar * dT2;
     const float q01 = q10;

     GQGt[0][0] =  q00;      // rad^2
     GQGt[1][0] = -q10;
     GQGt[0][1] = -q01;
     GQGt[1][1] =  q11;      // (rad/s)^2

     // initial covariance: Var{ x(t0) }
     // TODO: initialize P correctly
     P = 0;
 }

 bool Fusion::hasEstimate() const {
     return ((mInitState & MAG) || (mMode == FUSION_NOMAG)) &&
            ((mInitState & GYRO) || (mMode == FUSION_NOGYRO)) &&
            (mInitState & ACC);
 }

 bool Fusion::checkInitComplete(int what, const vec3_t& d, float dT) {
     if (hasEstimate())
         return true;

     if (what == ACC) {
         mData[0] += d * (1/length(d));
         mCount[0]++;
         mInitState |= ACC;
         if (mMode == FUSION_NOGYRO ) {
             mGyroRate = dT;
         }
     } else if (what == MAG) {
         mData[1] += d * (1/length(d));
         mCount[1]++;
         mInitState |= MAG;
     } else if (what == GYRO) {
         mGyroRate = dT;
         mData[2] += d*dT;
         mCount[2]++;
         mInitState |= GYRO;
     }

     if (hasEstimate()) {
         // Average all the values we collected so far
         mData[0] *= 1.0f/mCount[0];
         if (mMode != FUSION_NOMAG) {
             mData[1] *= 1.0f/mCount[1];
         }
         mData[2] *= 1.0f/mCount[2];

         // calculate the MRPs from the data collection, this gives us
         // a rough estimate of our initial state
         mat33_t R;
         vec3_t  up(mData[0]);
         vec3_t  east;

         if (mMode != FUSION_NOMAG) {
             east = normalize(cross_product(mData[1], up));
         } else {
             east = getOrthogonal(up);
         }

         vec3_t north(cross_product(up, east));
         R << east << north << up;
         const vec4_t q = matrixToQuat(R);

         initFusion(q, mGyroRate);
     }

     return false;
 }

 void Fusion::handleGyro(const vec3_t& w, float dT) {
     if (!checkInitComplete(GYRO, w, dT))
         return;

     predict(w, dT);
 }

 status_t Fusion::handleAcc(const vec3_t& a, float dT) {
     if (!checkInitComplete(ACC, a, dT))
         return BAD_VALUE;

     // ignore acceleration data if we're close to free-fall
     const float l = length(a);
     if (l < FREE_FALL_THRESHOLD) {
         return BAD_VALUE;
     }

     const float l_inv = 1.0f/l;

     if ( mMode == FUSION_NOGYRO ) {
         //geo mag
         vec3_t w_dummy;
         w_dummy = x1; //bias
         predict(w_dummy, dT);
     }

     if ( mMode == FUSION_NOMAG) {
         vec3_t m;
         m = getRotationMatrix()*Bm;
         update(m, Bm, mParam.magStdev);
     }

     vec3_t unityA = a * l_inv;
     const float d = sqrtf(fabsf(l- NOMINAL_GRAVITY));
     const float p = l_inv * mParam.accStdev*expf(d);

     update(unityA, Ba, p);
     return NO_ERROR;
 }

 status_t Fusion::handleMag(const vec3_t& m) {
     if (!checkInitComplete(MAG, m))
         return BAD_VALUE;

     // the geomagnetic-field should be between 30uT and 60uT
     // reject if too large to avoid spurious magnetic sources
     const float magFieldSq = length_squared(m);
     if (magFieldSq > MAX_VALID_MAGNETIC_FIELD_SQ) {
         return BAD_VALUE;
     } else if (magFieldSq < MIN_VALID_MAGNETIC_FIELD_SQ) {
         // Also reject if too small since we will get ill-defined (zero mag)
         // cross-products below
         return BAD_VALUE;
     }

     // Orthogonalize the magnetic field to the gravity field, mapping it into
     // tangent to Earth.
     const vec3_t up( getRotationMatrix() * Ba );
     const vec3_t east( cross_product(m, up) );

     // If the m and up vectors align, the cross product magnitude will
     // approach 0.
     // Reject this case as well to avoid div by zero problems and
     // ill-conditioning below.
     if (length_squared(east) < MIN_VALID_CROSS_PRODUCT_MAG_SQ) {
         return BAD_VALUE;
     }

     // If we have created an orthogonal magnetic field successfully,
     // then pass it in as the update.
     vec3_t north( cross_product(up, east) );

     const float l_inv = 1 / length(north);
     north *= l_inv;

     update(north, Bm,  mParam.magStdev*l_inv);
     return NO_ERROR;
 }

 void Fusion::checkState() {
     // P needs to stay positive semidefinite or the fusion diverges. When we
     // detect divergence, we reset the fusion.
     // TODO(braun): Instead, find the reason for the divergence and fix it.

     if (!isPositiveSemidefinite(P[0][0], SYMMETRY_TOLERANCE) ||
         !isPositiveSemidefinite(P[1][1], SYMMETRY_TOLERANCE)) {
         ALOGW("Sensor fusion diverged; resetting state.");
         P = 0;
     }
 }

 vec4_t Fusion::getAttitude() const {
     return x0;
 }

 vec3_t Fusion::getBias() const {
     return x1;
 }

 mat33_t Fusion::getRotationMatrix() const {
     return quatToMatrix(x0);
 }

 mat34_t Fusion::getF(const vec4_t& q) {
     mat34_t F;

     // This is used to compute the derivative of q
     // F = | [q.xyz]x |
     //     |  -q.xyz  |

     F[0].x = q.w;   F[1].x =-q.z;   F[2].x = q.y;
     F[0].y = q.z;   F[1].y = q.w;   F[2].y =-q.x;
     F[0].z =-q.y;   F[1].z = q.x;   F[2].z = q.w;
     F[0].w =-q.x;   F[1].w =-q.y;   F[2].w =-q.z;
     return F;
 }

 void Fusion::predict(const vec3_t& w, float dT) {
     const vec4_t q  = x0;
     const vec3_t b  = x1;
     vec3_t we = w - b;

     if (length(we) < WVEC_EPS) {
         we = (we[0]>0.f)?WVEC_EPS:-WVEC_EPS;
     }
     // q(k+1) = O(we)*q(k)
     // --------------------
     //
     // O(w) = | cos(0.5*||w||*dT)*I33 - [psi]x                   psi |
     //        | -psi'                              cos(0.5*||w||*dT) |
     //
     // psi = sin(0.5*||w||*dT)*w / ||w||
     //
     //
     // P(k+1) = Phi(k)*P(k)*Phi(k)' + G*Q(k)*G'
     // ----------------------------------------
     //
     // G = | -I33    0 |
     //     |    0  I33 |
     //
     //  Phi = | Phi00 Phi10 |
     //        |   0     1   |
     //
     //  Phi00 =   I33
     //          - [w]x   * sin(||w||*dt)/||w||
     //          + [w]x^2 * (1-cos(||w||*dT))/||w||^2
     //
     //  Phi10 =   [w]x   * (1        - cos(||w||*dt))/||w||^2
     //          - [w]x^2 * (||w||*dT - sin(||w||*dt))/||w||^3
     //          - I33*dT

     const mat33_t I33(1);
     const mat33_t I33dT(dT);
     const mat33_t wx(crossMatrix(we, 0));
     const mat33_t wx2(wx*wx);
     const float lwedT = length(we)*dT;
     const float hlwedT = 0.5f*lwedT;
     const float ilwe = 1.f/length(we);
     const float k0 = (1-cosf(lwedT))*(ilwe*ilwe);
     const float k1 = sinf(lwedT);
     const float k2 = cosf(hlwedT);
     const vec3_t psi(sinf(hlwedT)*ilwe*we);
     const mat33_t O33(crossMatrix(-psi, k2));
     mat44_t O;
     O[0].xyz = O33[0];  O[0].w = -psi.x;
     O[1].xyz = O33[1];  O[1].w = -psi.y;
     O[2].xyz = O33[2];  O[2].w = -psi.z;
     O[3].xyz = psi;     O[3].w = k2;

     Phi[0][0] = I33 - wx*(k1*ilwe) + wx2*k0;
     Phi[1][0] = wx*k0 - I33dT - wx2*(ilwe*ilwe*ilwe)*(lwedT-k1);

     x0 = O*q;

     if (x0.w < 0)
         x0 = -x0;

     P = Phi*P*transpose(Phi) + GQGt;

     checkState();
 }

 void Fusion::update(const vec3_t& z, const vec3_t& Bi, float sigma) {
     vec4_t q(x0);
     // measured vector in body space: h(p) = A(p)*Bi
     const mat33_t A(quatToMatrix(q));
     const vec3_t Bb(A*Bi);

     // Sensitivity matrix H = dh(p)/dp
     // H = [ L 0 ]
     const mat33_t L(crossMatrix(Bb, 0));

     // gain...
     // K = P*Ht / [H*P*Ht + R]
     vec<mat33_t, 2> K;
     const mat33_t R(sigma*sigma);
     const mat33_t S(scaleCovariance(L, P[0][0]) + R);
     const mat33_t Si(invert(S));
     const mat33_t LtSi(transpose(L)*Si);
     K[0] = P[0][0] * LtSi;
     K[1] = transpose(P[1][0])*LtSi;

     // update...
     // P = (I-K*H) * P
     // P -= K*H*P
     // | K0 | * | L 0 | * P = | K0*L  0 | * | P00  P10 | = | K0*L*P00  K0*L*P10 |
     // | K1 |                 | K1*L  0 |   | P01  P11 |   | K1*L*P00  K1*L*P10 |
     // Note: the Joseph form is numerically more stable and given by:
     //     P = (I-KH) * P * (I-KH)' + K*R*R'
     const mat33_t K0L(K[0] * L);
     const mat33_t K1L(K[1] * L);
     P[0][0] -= K0L*P[0][0];
     P[1][1] -= K1L*P[1][0];
     P[1][0] -= K0L*P[1][0];
     P[0][1] = transpose(P[1][0]);

     const vec3_t e(z - Bb);
     const vec3_t dq(K[0]*e);

     q += getF(q)*(0.5f*dq);
     x0 = normalize_quat(q);

     if (mMode != FUSION_NOMAG) {
         const vec3_t db(K[1]*e);
         x1 += db;
     }

     checkState();
 }

 vec3_t Fusion::getOrthogonal(const vec3_t &v) {
     vec3_t w;
     if (fabsf(v[0])<= fabsf(v[1]) && fabsf(v[0]) <= fabsf(v[2]))  {
         w[0]=0.f;
         w[1] = v[2];
         w[2] = -v[1];
     } else if (fabsf(v[1]) <= fabsf(v[2])) {
         w[0] = v[2];
         w[1] = 0.f;
         w[2] = -v[0];
     }else {
         w[0] = v[1];
         w[1] = -v[0];
         w[2] = 0.f;
     }
     return normalize(w);
 }


 // -----------------------------------------------------------------------

 }; // namespace android
	/*
	* Copyright (C) 2011 The Android Open Source Project
	*
	* Licensed under the Apache License, Version 2.0 (the "License");
	* you may not use this file except in compliance with the License.
	* You may obtain a copy of the License at
	*
	* http://www.apache.org/licenses/LICENSE-2.0
	*
	* Unless required by applicable law or agreed to in writing, software
	* distributed under the License is distributed on an "AS IS" BASIS,
	* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
	* See the License for the specific language governing permissions and
	* limitations under the License.
	*/

	#include <stdio.h>

	#include <utils/Log.h>

	#include "Fusion.h"

	namespace android {

	// -----------------------------------------------------------------------

	/==================== BEGIN FUSION SENSOR PARAMETER =========================/

	/* Note:
	* If a platform uses software fusion, it is necessary to tune the following
	* parameters to fit the hardware sensors prior to release.
	*
	* The DEFAULT_ parameters will be used in FUSION_9AXIS and FUSION_NOMAG mode.
	* The GEOMAG_ parameters will be used in FUSION_NOGYRO mode.
	*/

	/*
	* GYRO_VAR gives the measured variance of the gyro's output per
	* Hz (or variance at 1 Hz). This is an "intrinsic" parameter of the gyro,
	* which is independent of the sampling frequency.
	*
	* The variance of gyro's output at a given sampling period can be
	* calculated as:
	* variance(T) = GYRO_VAR / T
	*
	* The variance of the INTEGRATED OUTPUT at a given sampling period can be
	* calculated as:
	* variance_integrate_output(T) = GYRO_VAR * T
	*/
	static const float DEFAULT_GYRO_VAR = 1e-7; // (rad/s)^2 / Hz
	static const float DEFAULT_GYRO_BIAS_VAR = 1e-12; // (rad/s)^2 / s (guessed)
	static const float GEOMAG_GYRO_VAR = 1e-4; // (rad/s)^2 / Hz
	static const float GEOMAG_GYRO_BIAS_VAR = 1e-8; // (rad/s)^2 / s (guessed)

	/*
	* Standard deviations of accelerometer and magnetometer
	*/
	static const float DEFAULT_ACC_STDEV = 0.015f; // m/s^2 (measured 0.08 / CDD 0.05)
	static const float DEFAULT_MAG_STDEV = 0.1f; // uT (measured 0.7 / CDD 0.5)
	static const float GEOMAG_ACC_STDEV = 0.05f; // m/s^2 (measured 0.08 / CDD 0.05)
	static const float GEOMAG_MAG_STDEV = 0.1f; // uT (measured 0.7 / CDD 0.5)


	/* ====================== END FUSION SENSOR PARAMETER ========================*/

	static const float SYMMETRY_TOLERANCE = 1e-10f;

	/*
	* Accelerometer updates will not be performed near free fall to avoid
	* ill-conditioning and div by zeros.
	* Threshhold: 10% of g, in m/s^2
	*/
	static const float NOMINAL_GRAVITY = 9.81f;
	static const float FREE_FALL_THRESHOLD = 0.1f * (NOMINAL_GRAVITY);

	/*
	* The geomagnetic-field should be between 30uT and 60uT.
	* Fields strengths greater than this likely indicate a local magnetic
	* disturbance which we do not want to update into the fused frame.
	*/
	static const float MAX_VALID_MAGNETIC_FIELD = 100; // uT
	static const float MAX_VALID_MAGNETIC_FIELD_SQ =
	MAX_VALID_MAGNETIC_FIELD*MAX_VALID_MAGNETIC_FIELD;

	/*
	* Values of the field smaller than this should be ignored in fusion to avoid
	* ill-conditioning. This state can happen with anomalous local magnetic
	* disturbances canceling the Earth field.
	*/
	static const float MIN_VALID_MAGNETIC_FIELD = 10; // uT
	static const float MIN_VALID_MAGNETIC_FIELD_SQ =
	MIN_VALID_MAGNETIC_FIELD*MIN_VALID_MAGNETIC_FIELD;

	/*
	* If the cross product of two vectors has magnitude squared less than this,
	* we reject it as invalid due to alignment of the vectors.
	* This threshold is used to check for the case where the magnetic field sample
	* is parallel to the gravity field, which can happen in certain places due
	* to magnetic field disturbances.
	*/
	static const float MIN_VALID_CROSS_PRODUCT_MAG = 1.0e-3;
	static const float MIN_VALID_CROSS_PRODUCT_MAG_SQ =
	MIN_VALID_CROSS_PRODUCT_MAG*MIN_VALID_CROSS_PRODUCT_MAG;

	static const float SQRT_3 = 1.732f;
	static const float WVEC_EPS = 1e-4f/SQRT_3;
	// -----------------------------------------------------------------------

	template <typename TYPE, size_t C, size_t R>
	static mat<TYPE, R, R> scaleCovariance(
	const mat<TYPE, C, R>& A,
	const mat<TYPE, C, C>& P) {
	// APtranspose(A);
	mat<TYPE, R, R> APAt;
	for (size_t r=0 ; r<R ; r++) {
	for (size_t j=r ; j<R ; j++) {
	double apat(0);
	for (size_t c=0 ; c<C ; c++) {
	double v(A[c][r]P[c][c]0.5);
	for (size_t k=c+1 ; k<C ; k++)
	v += A[k][r] * P[c][k];
	apat += 2 * v * A[c][j];
	}
	APAt[j][r] = apat;
	APAt[r][j] = apat;
	}
	}
	return APAt;
	}

	template <typename TYPE, typename OTHER_TYPE>
	static mat<TYPE, 3, 3> crossMatrix(const vec<TYPE, 3>& p, OTHER_TYPE diag) {
	mat<TYPE, 3, 3> r;
	r[0][0] = diag;
	r[1][1] = diag;
	r[2][2] = diag;
	r[0][1] = p.z;
	r[1][0] =-p.z;
	r[0][2] =-p.y;
	r[2][0] = p.y;
	r[1][2] = p.x;
	r[2][1] =-p.x;
	return r;
	}


	template<typename TYPE, size_t SIZE>
	class Covariance {
	mat<TYPE, SIZE, SIZE> mSumXX;
	vec<TYPE, SIZE> mSumX;
	size_t mN;
	public:
	Covariance() : mSumXX(0.0f), mSumX(0.0f), mN(0) { }
	void update(const vec<TYPE, SIZE>& x) {
	mSumXX += x*transpose(x);
	mSumX += x;
	mN++;
	}
	mat<TYPE, SIZE, SIZE> operator()() const {
	const float N = 1.0f / mN;
	return mSumXXN - (mSumXtranspose(mSumX))(NN);
	}
	void reset() {
	mN = 0;
	mSumXX = 0;
	mSumX = 0;
	}
	size_t getCount() const {
	return mN;
	}
	};

	// -----------------------------------------------------------------------

	Fusion::Fusion() {
	Phi[0][1] = 0;
	Phi[1][1] = 1;

	Ba.x = 0;
	Ba.y = 0;
	Ba.z = 1;

	Bm.x = 0;
	Bm.y = 1;
	Bm.z = 0;

	x0 = 0;
	x1 = 0;

	init();
	}

	void Fusion::init(int mode) {
	mInitState = 0;

	mGyroRate = 0;

	mCount[0] = 0;
	mCount[1] = 0;
	mCount[2] = 0;

	mData = 0;
	mMode = mode;

	if (mMode != FUSION_NOGYRO) { //normal or game rotation
	mParam.gyroVar = DEFAULT_GYRO_VAR;
	mParam.gyroBiasVar = DEFAULT_GYRO_BIAS_VAR;
	mParam.accStdev = DEFAULT_ACC_STDEV;
	mParam.magStdev = DEFAULT_MAG_STDEV;
	} else {
	mParam.gyroVar = GEOMAG_GYRO_VAR;
	mParam.gyroBiasVar = GEOMAG_GYRO_BIAS_VAR;
	mParam.accStdev = GEOMAG_ACC_STDEV;
	mParam.magStdev = GEOMAG_MAG_STDEV;
	}
	}

	void Fusion::initFusion(const vec4_t& q, float dT)
	{
	// initial estimate: E{ x(t0) }
	x0 = q;
	x1 = 0;

	// process noise covariance matrix: G.Q.Gt, with
	//
	// G = \| -1 0 \| Q = \| q00 q10 \|
	// \| 0 1 \| \| q01 q11 \|
	//
	// q00 = sv^2.dt + 1/3.su^2.dt^3
	// q10 = q01 = 1/2.su^2.dt^2
	// q11 = su^2.dt
	//

	const float dT2 = dT*dT;
	const float dT3 = dT2*dT;

	// variance of integrated output at 1/dT Hz (random drift)
	const float q00 = mParam.gyroVar * dT + 0.33333f * mParam.gyroBiasVar * dT3;

	// variance of drift rate ramp
	const float q11 = mParam.gyroBiasVar * dT;
	const float q10 = 0.5f * mParam.gyroBiasVar * dT2;
	const float q01 = q10;

	GQGt[0][0] = q00; // rad^2
	GQGt[1][0] = -q10;
	GQGt[0][1] = -q01;
	GQGt[1][1] = q11; // (rad/s)^2

	// initial covariance: Var{ x(t0) }
	// TODO: initialize P correctly
	P = 0;
	}

	bool Fusion::hasEstimate() const {
	return ((mInitState & MAG) \|\| (mMode == FUSION_NOMAG)) &&
	((mInitState & GYRO) \|\| (mMode == FUSION_NOGYRO)) &&
	(mInitState & ACC);
	}

	bool Fusion::checkInitComplete(int what, const vec3_t& d, float dT) {
	if (hasEstimate())
	return true;

	if (what == ACC) {
	mData[0] += d * (1/length(d));
	mCount[0]++;
	mInitState \|= ACC;
	if (mMode == FUSION_NOGYRO ) {
	mGyroRate = dT;
	}
	} else if (what == MAG) {
	mData[1] += d * (1/length(d));
	mCount[1]++;
	mInitState \|= MAG;
	} else if (what == GYRO) {
	mGyroRate = dT;
	mData[2] += d*dT;
	mCount[2]++;
	mInitState \|= GYRO;
	}

	if (hasEstimate()) {
	// Average all the values we collected so far
	mData[0] *= 1.0f/mCount[0];
	if (mMode != FUSION_NOMAG) {
	mData[1] *= 1.0f/mCount[1];
	}
	mData[2] *= 1.0f/mCount[2];

	// calculate the MRPs from the data collection, this gives us
	// a rough estimate of our initial state
	mat33_t R;
	vec3_t up(mData[0]);
	vec3_t east;

	if (mMode != FUSION_NOMAG) {
	east = normalize(cross_product(mData[1], up));
	} else {
	east = getOrthogonal(up);
	}

	vec3_t north(cross_product(up, east));
	R << east << north << up;
	const vec4_t q = matrixToQuat(R);

	initFusion(q, mGyroRate);
	}

	return false;
	}

	void Fusion::handleGyro(const vec3_t& w, float dT) {
	if (!checkInitComplete(GYRO, w, dT))
	return;

	predict(w, dT);
	}

	status_t Fusion::handleAcc(const vec3_t& a, float dT) {
	if (!checkInitComplete(ACC, a, dT))
	return BAD_VALUE;

	// ignore acceleration data if we're close to free-fall
	const float l = length(a);
	if (l < FREE_FALL_THRESHOLD) {
	return BAD_VALUE;
	}

	const float l_inv = 1.0f/l;

	if ( mMode == FUSION_NOGYRO ) {
	//geo mag
	vec3_t w_dummy;
	w_dummy = x1; //bias
	predict(w_dummy, dT);
	}

	if ( mMode == FUSION_NOMAG) {
	vec3_t m;
	m = getRotationMatrix()*Bm;
	update(m, Bm, mParam.magStdev);
	}

	vec3_t unityA = a * l_inv;
	const float d = sqrtf(fabsf(l- NOMINAL_GRAVITY));
	const float p = l_inv * mParam.accStdev*expf(d);

	update(unityA, Ba, p);
	return NO_ERROR;
	}

	status_t Fusion::handleMag(const vec3_t& m) {
	if (!checkInitComplete(MAG, m))
	return BAD_VALUE;

	// the geomagnetic-field should be between 30uT and 60uT
	// reject if too large to avoid spurious magnetic sources
	const float magFieldSq = length_squared(m);
	if (magFieldSq > MAX_VALID_MAGNETIC_FIELD_SQ) {
	return BAD_VALUE;
	} else if (magFieldSq < MIN_VALID_MAGNETIC_FIELD_SQ) {
	// Also reject if too small since we will get ill-defined (zero mag)
	// cross-products below
	return BAD_VALUE;
	}

	// Orthogonalize the magnetic field to the gravity field, mapping it into
	// tangent to Earth.
	const vec3_t up( getRotationMatrix() * Ba );
	const vec3_t east( cross_product(m, up) );

	// If the m and up vectors align, the cross product magnitude will
	// approach 0.
	// Reject this case as well to avoid div by zero problems and
	// ill-conditioning below.
	if (length_squared(east) < MIN_VALID_CROSS_PRODUCT_MAG_SQ) {
	return BAD_VALUE;
	}

	// If we have created an orthogonal magnetic field successfully,
	// then pass it in as the update.
	vec3_t north( cross_product(up, east) );

	const float l_inv = 1 / length(north);
	north *= l_inv;

	update(north, Bm, mParam.magStdev*l_inv);
	return NO_ERROR;
	}

	void Fusion::checkState() {
	// P needs to stay positive semidefinite or the fusion diverges. When we
	// detect divergence, we reset the fusion.
	// TODO(braun): Instead, find the reason for the divergence and fix it.

	if (!isPositiveSemidefinite(P[0][0], SYMMETRY_TOLERANCE) \|\|
	!isPositiveSemidefinite(P[1][1], SYMMETRY_TOLERANCE)) {
	ALOGW("Sensor fusion diverged; resetting state.");
	P = 0;
	}
	}

	vec4_t Fusion::getAttitude() const {
	return x0;
	}

	vec3_t Fusion::getBias() const {
	return x1;
	}

	mat33_t Fusion::getRotationMatrix() const {
	return quatToMatrix(x0);
	}

	mat34_t Fusion::getF(const vec4_t& q) {
	mat34_t F;

	// This is used to compute the derivative of q
	// F = \| [q.xyz]x \|
	// \| -q.xyz \|

	F[0].x = q.w; F[1].x =-q.z; F[2].x = q.y;
	F[0].y = q.z; F[1].y = q.w; F[2].y =-q.x;
	F[0].z =-q.y; F[1].z = q.x; F[2].z = q.w;
	F[0].w =-q.x; F[1].w =-q.y; F[2].w =-q.z;
	return F;
	}

	void Fusion::predict(const vec3_t& w, float dT) {
	const vec4_t q = x0;
	const vec3_t b = x1;
	vec3_t we = w - b;

	if (length(we) < WVEC_EPS) {
	we = (we[0]>0.f)?WVEC_EPS:-WVEC_EPS;
	}
	// q(k+1) = O(we)*q(k)
	// --------------------
	//
	// O(w) = \| cos(0.5\|\|w\|\|dT)*I33 - [psi]x psi \|
	// \| -psi' cos(0.5\|\|w\|\|dT) \|
	//
	// psi = sin(0.5\|\|w\|\|dT)*w / \|\|w\|\|
	//
	//
	// P(k+1) = Phi(k)P(k)Phi(k)' + GQ(k)G'
	// ----------------------------------------
	//
	// G = \| -I33 0 \|
	// \| 0 I33 \|
	//
	// Phi = \| Phi00 Phi10 \|
	// \| 0 1 \|
	//
	// Phi00 = I33
	// - [w]x * sin(\|\|w\|\|*dt)/\|\|w\|\|
	// + [w]x^2 * (1-cos(\|\|w\|\|*dT))/\|\|w\|\|^2
	//
	// Phi10 = [w]x * (1 - cos(\|\|w\|\|*dt))/\|\|w\|\|^2
	// - [w]x^2 * (\|\|w\|\|dT - sin(\|\|w\|\|dt))/\|\|w\|\|^3
	// - I33*dT

	const mat33_t I33(1);
	const mat33_t I33dT(dT);
	const mat33_t wx(crossMatrix(we, 0));
	const mat33_t wx2(wx*wx);
	const float lwedT = length(we)*dT;
	const float hlwedT = 0.5f*lwedT;
	const float ilwe = 1.f/length(we);
	const float k0 = (1-cosf(lwedT))(ilweilwe);
	const float k1 = sinf(lwedT);
	const float k2 = cosf(hlwedT);
	const vec3_t psi(sinf(hlwedT)ilwewe);
	const mat33_t O33(crossMatrix(-psi, k2));
	mat44_t O;
	O[0].xyz = O33[0]; O[0].w = -psi.x;
	O[1].xyz = O33[1]; O[1].w = -psi.y;
	O[2].xyz = O33[2]; O[2].w = -psi.z;
	O[3].xyz = psi; O[3].w = k2;

	Phi[0][0] = I33 - wx(k1ilwe) + wx2*k0;
	Phi[1][0] = wxk0 - I33dT - wx2(ilweilweilwe)*(lwedT-k1);

	x0 = O*q;

	if (x0.w < 0)
	x0 = -x0;

	P = PhiPtranspose(Phi) + GQGt;

	checkState();
	}

	void Fusion::update(const vec3_t& z, const vec3_t& Bi, float sigma) {
	vec4_t q(x0);
	// measured vector in body space: h(p) = A(p)*Bi
	const mat33_t A(quatToMatrix(q));
	const vec3_t Bb(A*Bi);

	// Sensitivity matrix H = dh(p)/dp
	// H = [ L 0 ]
	const mat33_t L(crossMatrix(Bb, 0));

	// gain...
	// K = PHt / [HP*Ht + R]
	vec<mat33_t, 2> K;
	const mat33_t R(sigma*sigma);
	const mat33_t S(scaleCovariance(L, P[0][0]) + R);
	const mat33_t Si(invert(S));
	const mat33_t LtSi(transpose(L)*Si);
	K[0] = P[0][0] * LtSi;
	K[1] = transpose(P[1][0])*LtSi;

	// update...
	// P = (I-KH) P
	// P -= KHP
	// \| K0 \| * \| L 0 \| * P = \| K0L 0 \| \| P00 P10 \| = \| K0LP00 K0LP10 \|
	// \| K1 \| \| K1L 0 \| \| P01 P11 \| \| K1LP00 K1L*P10 \|
	// Note: the Joseph form is numerically more stable and given by:
	// P = (I-KH) * P * (I-KH)' + KRR'
	const mat33_t K0L(K[0] * L);
	const mat33_t K1L(K[1] * L);
	P[0][0] -= K0L*P[0][0];
	P[1][1] -= K1L*P[1][0];
	P[1][0] -= K0L*P[1][0];
	P[0][1] = transpose(P[1][0]);

	const vec3_t e(z - Bb);
	const vec3_t dq(K[0]*e);

	q += getF(q)(0.5fdq);
	x0 = normalize_quat(q);

	if (mMode != FUSION_NOMAG) {
	const vec3_t db(K[1]*e);
	x1 += db;
	}

	checkState();
	}

	vec3_t Fusion::getOrthogonal(const vec3_t &v) {
	vec3_t w;
	if (fabsf(v[0])<= fabsf(v[1]) && fabsf(v[0]) <= fabsf(v[2])) {
	w[0]=0.f;
	w[1] = v[2];
	w[2] = -v[1];
	} else if (fabsf(v[1]) <= fabsf(v[2])) {
	w[0] = v[2];
	w[1] = 0.f;
	w[2] = -v[0];
	}else {
	w[0] = v[1];
	w[1] = -v[0];
	w[2] = 0.f;
	}
	return normalize(w);
	}


	// -----------------------------------------------------------------------

	}; // namespace android