9-axis sensor fusion with Kalman filter

Add support for 9-axis gravity and linear-acceleration sensors
virtual orientation sensor using 9-axis fusion

Change-Id: I6717539373fce781c10e97b6fa59f68a831a592f

25 files changed, 2171 insertions, 191 deletions

diff --git a/include/gui/Sensor.h b/include/gui/Sensor.h
index 2de07b1..e59757a 100644
--- a/include/gui/Sensor.h
+++ b/include/gui/Sensor.h
@@ -21,8 +21,9 @@
 #include <sys/types.h>
 
 #include <utils/Errors.h>
-#include <utils/String8.h>
 #include <utils/Flattenable.h>
+#include <utils/String8.h>
+#include <utils/Timers.h>
 
 #include <hardware/sensors.h>
 
@@ -64,6 +65,8 @@ public:
     float getResolution() const;
     float getPowerUsage() const;
     int32_t getMinDelay() const;
+    nsecs_t getMinDelayNs() const;
+    int32_t getVersion() const;
 
     // Flattenable interface
     virtual size_t getFlattenedSize() const;
@@ -83,6 +86,7 @@ private:
     float   mResolution;
     float   mPower;
     int32_t mMinDelay;
+    int32_t mVersion;
 };
 
 // ----------------------------------------------------------------------------
diff --git a/libs/gui/Sensor.cpp b/libs/gui/Sensor.cpp
index b1f37ff..f9a2c04 100644
--- a/libs/gui/Sensor.cpp
+++ b/libs/gui/Sensor.cpp
@@ -89,6 +89,14 @@ int32_t Sensor::getMinDelay() const {
     return mMinDelay;
 }
 
+nsecs_t Sensor::getMinDelayNs() const {
+    return getMinDelay() * 1000;
+}
+
+int32_t Sensor::getVersion() const {
+    return mVersion;
+}
+
 size_t Sensor::getFlattenedSize() const
 {
     return  sizeof(int32_t) + ((mName.length() + 3) & ~3) +
diff --git a/services/sensorservice/Android.mk b/services/sensorservice/Android.mk
index c50e4a1..57a3b15 100644
--- a/services/sensorservice/Android.mk
+++ b/services/sensorservice/Android.mk
@@ -2,13 +2,18 @@ LOCAL_PATH:= $(call my-dir)
 include $(CLEAR_VARS)
 
 LOCAL_SRC_FILES:= \
-	GravitySensor.cpp \
-	LinearAccelerationSensor.cpp \
-	RotationVectorSensor.cpp \
-    SensorService.cpp \
-    SensorInterface.cpp \
+	CorrectedGyroSensor.cpp \
+    Fusion.cpp \
+    GravitySensor.cpp \
+    LinearAccelerationSensor.cpp \
+    OrientationSensor.cpp \
+    RotationVectorSensor.cpp \
+    SecondOrderLowPassFilter.cpp \
     SensorDevice.cpp \
-    SecondOrderLowPassFilter.cpp
+    SensorFusion.cpp \
+    SensorInterface.cpp \
+    SensorService.cpp \
+
 
 LOCAL_CFLAGS:= -DLOG_TAG=\"SensorService\"
 
diff --git a/services/sensorservice/CorrectedGyroSensor.cpp b/services/sensorservice/CorrectedGyroSensor.cpp
new file mode 100644
index 0000000..9b75b70
--- /dev/null
+++ b/services/sensorservice/CorrectedGyroSensor.cpp
@@ -0,0 +1,86 @@
+/*
+ * 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 <stdint.h>
+#include <math.h>
+#include <sys/types.h>
+
+#include <utils/Errors.h>
+
+#include <hardware/sensors.h>
+
+#include "CorrectedGyroSensor.h"
+#include "SensorDevice.h"
+#include "SensorFusion.h"
+
+namespace android {
+// ---------------------------------------------------------------------------
+
+CorrectedGyroSensor::CorrectedGyroSensor(sensor_t const* list, size_t count)
+    : mSensorDevice(SensorDevice::getInstance()),
+      mSensorFusion(SensorFusion::getInstance())
+{
+    for (size_t i=0 ; i<count ; i++) {
+        if (list[i].type == SENSOR_TYPE_GYROSCOPE) {
+            mGyro = Sensor(list + i);
+            break;
+        }
+    }
+}
+
+bool CorrectedGyroSensor::process(sensors_event_t* outEvent,
+        const sensors_event_t& event)
+{
+    if (event.type == SENSOR_TYPE_GYROSCOPE) {
+        const vec3_t bias(mSensorFusion.getGyroBias() * mSensorFusion.getEstimatedRate());
+        *outEvent = event;
+        outEvent->data[0] -= bias.x;
+        outEvent->data[1] -= bias.y;
+        outEvent->data[2] -= bias.z;
+        outEvent->sensor = '_cgy';
+        return true;
+    }
+    return false;
+}
+
+status_t CorrectedGyroSensor::activate(void* ident, bool enabled) {
+    mSensorDevice.activate(this, mGyro.getHandle(), enabled);
+    return mSensorFusion.activate(this, enabled);
+}
+
+status_t CorrectedGyroSensor::setDelay(void* ident, int handle, int64_t ns) {
+    mSensorDevice.setDelay(this, mGyro.getHandle(), ns);
+    return mSensorFusion.setDelay(this, ns);
+}
+
+Sensor CorrectedGyroSensor::getSensor() const {
+    sensor_t hwSensor;
+    hwSensor.name       = "Corrected Gyroscope Sensor";
+    hwSensor.vendor     = "Google Inc.";
+    hwSensor.version    = 1;
+    hwSensor.handle     = '_cgy';
+    hwSensor.type       = SENSOR_TYPE_GYROSCOPE;
+    hwSensor.maxRange   = mGyro.getMaxValue();
+    hwSensor.resolution = mGyro.getResolution();
+    hwSensor.power      = mSensorFusion.getPowerUsage();
+    hwSensor.minDelay   = mGyro.getMinDelay();
+    Sensor sensor(&hwSensor);
+    return sensor;
+}
+
+// ---------------------------------------------------------------------------
+}; // namespace android
+
diff --git a/services/sensorservice/CorrectedGyroSensor.h b/services/sensorservice/CorrectedGyroSensor.h
new file mode 100644
index 0000000..3c49c08
--- /dev/null
+++ b/services/sensorservice/CorrectedGyroSensor.h
@@ -0,0 +1,52 @@
+/*
+ * 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.
+ */
+
+#ifndef ANDROID_CORRECTED_GYRO_SENSOR_H
+#define ANDROID_CORRECTED_GYRO_SENSOR_H
+
+#include <stdint.h>
+#include <sys/types.h>
+
+#include <gui/Sensor.h>
+
+#include "SensorInterface.h"
+
+// ---------------------------------------------------------------------------
+namespace android {
+// ---------------------------------------------------------------------------
+
+class SensorDevice;
+class SensorFusion;
+
+class CorrectedGyroSensor : public SensorInterface {
+    SensorDevice& mSensorDevice;
+    SensorFusion& mSensorFusion;
+    Sensor mGyro;
+
+public:
+    CorrectedGyroSensor(sensor_t const* list, size_t count);
+    virtual bool process(sensors_event_t* outEvent,
+            const sensors_event_t& event);
+    virtual status_t activate(void* ident, bool enabled);
+    virtual status_t setDelay(void* ident, int handle, int64_t ns);
+    virtual Sensor getSensor() const;
+    virtual bool isVirtual() const { return true; }
+};
+
+// ---------------------------------------------------------------------------
+}; // namespace android
+
+#endif // ANDROID_CORRECTED_GYRO_SENSOR_H
diff --git a/services/sensorservice/Fusion.cpp b/services/sensorservice/Fusion.cpp
new file mode 100644
index 0000000..56ac9f9
--- /dev/null
+++ b/services/sensorservice/Fusion.cpp
@@ -0,0 +1,431 @@
+/*
+ * 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 {
+
+// -----------------------------------------------------------------------
+
+template <typename TYPE>
+static inline TYPE sqr(TYPE x) {
+    return x*x;
+}
+
+template <typename T>
+static inline T clamp(T v) {
+    return v < 0 ? 0 : v;
+}
+
+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>
+static mat<TYPE, 3, 3> MRPsToMatrix(const vec<TYPE, 3>& p) {
+    mat<TYPE, 3, 3> res(1);
+    const mat<TYPE, 3, 3> px(crossMatrix(p, 0));
+    const TYPE ptp(dot_product(p,p));
+    const TYPE t = 4/sqr(1+ptp);
+    res -= t * (1-ptp) * px;
+    res += t * 2 * sqr(px);
+    return res;
+}
+
+template <typename TYPE>
+vec<TYPE, 3> matrixToMRPs(const mat<TYPE, 3, 3>& R) {
+    // matrix to MRPs
+    vec<TYPE, 3> q;
+    const float Hx = R[0].x;
+    const float My = R[1].y;
+    const float Az = R[2].z;
+    const float w = 1 / (1 + sqrtf( clamp( Hx + My + Az + 1) * 0.25f ));
+    q.x = sqrtf( clamp( Hx - My - Az + 1) * 0.25f ) * w;
+    q.y = sqrtf( clamp(-Hx + My - Az + 1) * 0.25f ) * w;
+    q.z = sqrtf( clamp(-Hx - My + Az + 1) * 0.25f ) * w;
+    q.x = copysignf(q.x, R[2].y - R[1].z);
+    q.y = copysignf(q.y, R[0].z - R[2].x);
+    q.z = copysignf(q.z, R[1].x - R[0].y);
+    return q;
+}
+
+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() {
+    // process noise covariance matrix
+    const float w1 = gyroSTDEV;
+    const float w2 = biasSTDEV;
+    Q[0] = w1*w1;
+    Q[1] = w2*w2;
+
+    Ba.x = 0;
+    Ba.y = 0;
+    Ba.z = 1;
+
+    Bm.x = 0;
+    Bm.y = 1;
+    Bm.z = 0;
+
+    init();
+}
+
+void Fusion::init() {
+    // initial estimate: E{ x(t0) }
+    x = 0;
+
+    // initial covariance: Var{ x(t0) }
+    P = 0;
+
+    mInitState = 0;
+    mCount[0] = 0;
+    mCount[1] = 0;
+    mCount[2] = 0;
+    mData = 0;
+}
+
+bool Fusion::hasEstimate() const {
+    return (mInitState == (MAG|ACC|GYRO));
+}
+
+bool Fusion::checkInitComplete(int what, const vec3_t& d) {
+    if (mInitState == (MAG|ACC|GYRO))
+        return true;
+
+    if (what == ACC) {
+        mData[0] += d * (1/length(d));
+        mCount[0]++;
+        mInitState |= ACC;
+    } else if (what == MAG) {
+        mData[1] += d * (1/length(d));
+        mCount[1]++;
+        mInitState |= MAG;
+    } else if (what == GYRO) {
+        mData[2] += d;
+        mCount[2]++;
+        if (mCount[2] == 64) {
+            // 64 samples is good enough to estimate the gyro drift and
+            // doesn't take too much time.
+            mInitState |= GYRO;
+        }
+    }
+
+    if (mInitState == (MAG|ACC|GYRO)) {
+        // Average all the values we collected so far
+        mData[0] *= 1.0f/mCount[0];
+        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(cross_product(mData[1], up));
+        east *= 1/length(east);
+        vec3_t north(cross_product(up, east));
+        R << east << north << up;
+        x[0] = matrixToMRPs(R);
+
+        // NOTE: we could try to use the average of the gyro data
+        // to estimate the initial bias, but this only works if
+        // the device is not moving. For now, we don't use that value
+        // and start with a bias of 0.
+        x[1] = 0;
+
+        // initial covariance
+        P = 0;
+    }
+
+    return false;
+}
+
+void Fusion::handleGyro(const vec3_t& w, float dT) {
+    const vec3_t wdT(w * dT);   // rad/s * s -> rad
+    if (!checkInitComplete(GYRO, wdT))
+        return;
+
+    predict(wdT);
+}
+
+status_t Fusion::handleAcc(const vec3_t& a) {
+    if (length(a) < 0.981f)
+        return BAD_VALUE;
+
+    if (!checkInitComplete(ACC, a))
+        return BAD_VALUE;
+
+    // ignore acceleration data if we're close to free-fall
+    const float l = 1/length(a);
+    update(a*l, Ba, accSTDEV*l);
+    return NO_ERROR;
+}
+
+status_t Fusion::handleMag(const vec3_t& m) {
+    // the geomagnetic-field should be between 30uT and 60uT
+    // reject obviously wrong magnetic-fields
+    if (length(m) > 100)
+        return BAD_VALUE;
+
+    if (!checkInitComplete(MAG, m))
+        return BAD_VALUE;
+
+    const vec3_t up( getRotationMatrix() * Ba );
+    const vec3_t east( cross_product(m, up) );
+    vec3_t north( cross_product(up, east) );
+
+    const float l = 1 / length(north);
+    north *= l;
+
+#if 0
+    // in practice the magnetic-field sensor is so wrong
+    // that there is no point trying to use it to constantly
+    // correct the gyro. instead, we use the mag-sensor only when
+    // the device points north (just to give us a reference).
+    // We're hoping that it'll actually point north, if it doesn't
+    // we'll be offset, but at least the instantaneous posture
+    // of the device will be correct.
+
+    const float cos_30 = 0.8660254f;
+    if (dot_product(north, Bm) < cos_30)
+        return BAD_VALUE;
+#endif
+
+    update(north, Bm, magSTDEV*l);
+    return NO_ERROR;
+}
+
+bool Fusion::checkState(const vec3_t& v) {
+    if (isnanf(length(v))) {
+        LOGW("9-axis fusion diverged. reseting state.");
+        P = 0;
+        x[1] = 0;
+        mInitState = 0;
+        mCount[0] = 0;
+        mCount[1] = 0;
+        mCount[2] = 0;
+        mData = 0;
+        return false;
+    }
+    return true;
+}
+
+vec3_t Fusion::getAttitude() const {
+    return x[0];
+}
+
+vec3_t Fusion::getBias() const {
+    return x[1];
+}
+
+mat33_t Fusion::getRotationMatrix() const {
+    return MRPsToMatrix(x[0]);
+}
+
+mat33_t Fusion::getF(const vec3_t& p) {
+    const float p0 = p.x;
+    const float p1 = p.y;
+    const float p2 = p.z;
+
+    // f(p, w)
+    const float p0p1 = p0*p1;
+    const float p0p2 = p0*p2;
+    const float p1p2 = p1*p2;
+    const float p0p0 = p0*p0;
+    const float p1p1 = p1*p1;
+    const float p2p2 = p2*p2;
+    const float pp = 0.5f * (1 - (p0p0 + p1p1 + p2p2));
+
+    mat33_t F;
+    F[0][0] = 0.5f*(p0p0 + pp);
+    F[0][1] = 0.5f*(p0p1 + p2);
+    F[0][2] = 0.5f*(p0p2 - p1);
+    F[1][0] = 0.5f*(p0p1 - p2);
+    F[1][1] = 0.5f*(p1p1 + pp);
+    F[1][2] = 0.5f*(p1p2 + p0);
+    F[2][0] = 0.5f*(p0p2 + p1);
+    F[2][1] = 0.5f*(p1p2 - p0);
+    F[2][2] = 0.5f*(p2p2 + pp);
+    return F;
+}
+
+mat33_t Fusion::getdFdp(const vec3_t& p, const vec3_t& we) {
+
+    // dF = | A = df/dp  -F |
+    //      |   0         0 |
+
+    mat33_t A;
+    A[0][0] = A[1][1] = A[2][2] = 0.5f * (p.x*we.x + p.y*we.y + p.z*we.z);
+    A[0][1] = 0.5f * (p.y*we.x - p.x*we.y - we.z);
+    A[0][2] = 0.5f * (p.z*we.x - p.x*we.z + we.y);
+    A[1][2] = 0.5f * (p.z*we.y - p.y*we.z - we.x);
+    A[1][0] = -A[0][1];
+    A[2][0] = -A[0][2];
+    A[2][1] = -A[1][2];
+    return A;
+}
+
+void Fusion::predict(const vec3_t& w) {
+    // f(p, w)
+    vec3_t& p(x[0]);
+
+    // There is a discontinuity at 2.pi, to avoid it we need to switch to
+    // the shadow of p when pT.p gets too big.
+    const float ptp(dot_product(p,p));
+    if (ptp >= 2.0f) {
+        p = -p * (1/ptp);
+    }
+
+    const mat33_t F(getF(p));
+
+    // compute w with the bias correction:
+    //  w_estimated = w - b_estimated
+    const vec3_t& b(x[1]);
+    const vec3_t we(w - b);
+
+    // prediction
+    const vec3_t dX(F*we);
+
+    if (!checkState(dX))
+        return;
+
+    p += dX;
+
+    const mat33_t A(getdFdp(p, we));
+
+    // G  = | G0  0 |  =  | -F  0 |
+    //      |  0  1 |     |  0  1 |
+
+    // P += A*P + P*At + F*Q*Ft
+    const mat33_t AP(A*transpose(P[0][0]));
+    const mat33_t PAt(P[0][0]*transpose(A));
+    const mat33_t FPSt(F*transpose(P[1][0]));
+    const mat33_t PSFt(P[1][0]*transpose(F));
+    const mat33_t FQFt(scaleCovariance(F, Q[0]));
+    P[0][0] += AP + PAt - FPSt - PSFt + FQFt;
+    P[1][0] += A*P[1][0] - F*P[1][1];
+    P[1][1] += Q[1];
+}
+
+void Fusion::update(const vec3_t& z, const vec3_t& Bi, float sigma) {
+    const vec3_t p(x[0]);
+    // measured vector in body space: h(p) = A(p)*Bi
+    const mat33_t A(MRPsToMatrix(p));
+    const vec3_t Bb(A*Bi);
+
+    // Sensitivity matrix H = dh(p)/dp
+    // H = [ L 0 ]
+    const float ptp(dot_product(p,p));
+    const mat33_t px(crossMatrix(p, 0.5f*(ptp-1)));
+    const mat33_t ppt(p*transpose(p));
+    const mat33_t L((8 / sqr(1+ptp))*crossMatrix(Bb, 0)*(ppt-px));
+
+    // update...
+    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);
+
+    vec<mat33_t, 2> K;
+    K[0] = P[0][0] * LtSi;
+    K[1] = transpose(P[1][0])*LtSi;
+
+    const vec3_t e(z - Bb);
+    const vec3_t K0e(K[0]*e);
+    const vec3_t K1e(K[1]*e);
+
+    if (!checkState(K0e))
+        return;
+
+    if (!checkState(K1e))
+        return;
+
+    x[0] += K0e;
+    x[1] += K1e;
+
+    // P -= K*H*P;
+    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];
+}
+
+// -----------------------------------------------------------------------
+
+}; // namespace android
+
diff --git a/services/sensorservice/Fusion.h b/services/sensorservice/Fusion.h
new file mode 100644
index 0000000..571a415
--- /dev/null
+++ b/services/sensorservice/Fusion.h
@@ -0,0 +1,86 @@
+/*
+ * 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.
+ */
+
+#ifndef ANDROID_FUSION_H
+#define ANDROID_FUSION_H
+
+#include <utils/Errors.h>
+
+#include "vec.h"
+#include "mat.h"
+
+namespace android {
+
+class Fusion {
+    /*
+     * the state vector is made of two sub-vector containing respectively:
+     * - modified Rodrigues parameters
+     * - the estimated gyro bias
+     */
+    vec<vec3_t, 2> x;
+
+    /*
+     * the predicated covariance matrix is made of 4 3x3 sub-matrices and it
+     * semi-definite positive.
+     *
+     * P = | P00  P10 | = | P00  P10 |
+     *     | P01  P11 |   | P10t  Q1 |
+     *
+     * Since P01 = transpose(P10), the code below never calculates or
+     * stores P01. P11 is always equal to Q1, so we don't store it either.
+     */
+    mat<mat33_t, 2, 2> P;
+
+    /*
+     * the process noise covariance matrix is made of 2 3x3 sub-matrices
+     * Q0 encodes the attitude's noise
+     * Q1 encodes the bias' noise
+     */
+    vec<mat33_t, 2> Q;
+
+    static const float gyroSTDEV = 1.0e-5;  // rad/s (measured 1.2e-5)
+    static const float accSTDEV  = 0.05f;   // m/s^2 (measured 0.08 / CDD 0.05)
+    static const float magSTDEV  = 0.5f;    // uT    (measured 0.7  / CDD 0.5)
+    static const float biasSTDEV = 2e-9;    // rad/s^2 (guessed)
+
+public:
+    Fusion();
+    void init();
+    void handleGyro(const vec3_t& w, float dT);
+    status_t handleAcc(const vec3_t& a);
+    status_t handleMag(const vec3_t& m);
+    vec3_t getAttitude() const;
+    vec3_t getBias() const;
+    mat33_t getRotationMatrix() const;
+    bool hasEstimate() const;
+
+private:
+    vec3_t Ba, Bm;
+    uint32_t mInitState;
+    vec<vec3_t, 3> mData;
+    size_t mCount[3];
+    enum { ACC=0x1, MAG=0x2, GYRO=0x4 };
+    bool checkInitComplete(int, const vec3_t&);
+    bool checkState(const vec3_t& v);
+    void predict(const vec3_t& w);
+    void update(const vec3_t& z, const vec3_t& Bi, float sigma);
+    static mat33_t getF(const vec3_t& p);
+    static mat33_t getdFdp(const vec3_t& p, const vec3_t& we);
+};
+
+}; // namespace android
+
+#endif // ANDROID_FUSION_H
diff --git a/services/sensorservice/GravitySensor.cpp b/services/sensorservice/GravitySensor.cpp
index 5c6aa99..541fad2 100644
--- a/services/sensorservice/GravitySensor.cpp
+++ b/services/sensorservice/GravitySensor.cpp
@@ -23,16 +23,18 @@
 #include <hardware/sensors.h>
 
 #include "GravitySensor.h"
+#include "SensorDevice.h"
+#include "SensorFusion.h"
 
 namespace android {
 // ---------------------------------------------------------------------------
 
 GravitySensor::GravitySensor(sensor_t const* list, size_t count)
     : mSensorDevice(SensorDevice::getInstance()),
+      mSensorFusion(SensorFusion::getInstance()),
       mAccTime(0),
       mLowPass(M_SQRT1_2, 1.5f),
       mX(mLowPass), mY(mLowPass), mZ(mLowPass)
-
 {
     for (size_t i=0 ; i<count ; i++) {
         if (list[i].type == SENSOR_TYPE_ACCELEROMETER) {
@@ -47,35 +49,52 @@ bool GravitySensor::process(sensors_event_t* outEvent,
 {
     const static double NS2S = 1.0 / 1000000000.0;
     if (event.type == SENSOR_TYPE_ACCELEROMETER) {
-        float x, y, z;
-        const double now = event.timestamp * NS2S;
-        if (mAccTime == 0) {
-            x = mX.init(event.acceleration.x);
-            y = mY.init(event.acceleration.y);
-            z = mZ.init(event.acceleration.z);
+        vec3_t g;
+        if (mSensorFusion.hasGyro()) {
+            if (!mSensorFusion.hasEstimate())
+                return false;
+            const mat33_t R(mSensorFusion.getRotationMatrix());
+            // FIXME: we need to estimate the length of gravity because
+            // the accelerometer may have a small scaling error. This
+            // translates to an offset in the linear-acceleration sensor.
+            g = R[2] * GRAVITY_EARTH;
         } else {
-            double dT = now - mAccTime;
-            mLowPass.setSamplingPeriod(dT);
-            x = mX(event.acceleration.x);
-            y = mY(event.acceleration.y);
-            z = mZ(event.acceleration.z);
+            const double now = event.timestamp * NS2S;
+            if (mAccTime == 0) {
+                g.x = mX.init(event.acceleration.x);
+                g.y = mY.init(event.acceleration.y);
+                g.z = mZ.init(event.acceleration.z);
+            } else {
+                double dT = now - mAccTime;
+                mLowPass.setSamplingPeriod(dT);
+                g.x = mX(event.acceleration.x);
+                g.y = mY(event.acceleration.y);
+                g.z = mZ(event.acceleration.z);
+            }
+            g *= (GRAVITY_EARTH / length(g));
+            mAccTime = now;
         }
-        mAccTime = now;
         *outEvent = event;
-        outEvent->data[0] = x;
-        outEvent->data[1] = y;
-        outEvent->data[2] = z;
+        outEvent->data[0] = g.x;
+        outEvent->data[1] = g.y;
+        outEvent->data[2] = g.z;
         outEvent->sensor = '_grv';
         outEvent->type = SENSOR_TYPE_GRAVITY;
         return true;
     }
     return false;
 }
+
 status_t GravitySensor::activate(void* ident, bool enabled) {
-    status_t err = mSensorDevice.activate(this, mAccelerometer.getHandle(), enabled);
-    if (err == NO_ERROR) {
-        if (enabled) {
-            mAccTime = 0;
+    status_t err;
+    if (mSensorFusion.hasGyro()) {
+        err = mSensorFusion.activate(this, enabled);
+    } else {
+        err = mSensorDevice.activate(this, mAccelerometer.getHandle(), enabled);
+        if (err == NO_ERROR) {
+            if (enabled) {
+                mAccTime = 0;
+            }
         }
     }
     return err;
@@ -83,20 +102,26 @@ status_t GravitySensor::activate(void* ident, bool enabled) {
 
 status_t GravitySensor::setDelay(void* ident, int handle, int64_t ns)
 {
-    return mSensorDevice.setDelay(this, mAccelerometer.getHandle(), ns);
+    if (mSensorFusion.hasGyro()) {
+        return mSensorFusion.setDelay(this, ns);
+    } else {
+        return mSensorDevice.setDelay(this, mAccelerometer.getHandle(), ns);
+    }
 }
 
 Sensor GravitySensor::getSensor() const {
     sensor_t hwSensor;
     hwSensor.name       = "Gravity Sensor";
     hwSensor.vendor     = "Google Inc.";
-    hwSensor.version    = 1;
+    hwSensor.version    = mSensorFusion.hasGyro() ? 3 : 2;
     hwSensor.handle     = '_grv';
     hwSensor.type       = SENSOR_TYPE_GRAVITY;
-    hwSensor.maxRange   = mAccelerometer.getMaxValue();
+    hwSensor.maxRange   = GRAVITY_EARTH * 2;
     hwSensor.resolution = mAccelerometer.getResolution();
-    hwSensor.power      = mAccelerometer.getPowerUsage();
-    hwSensor.minDelay   = mAccelerometer.getMinDelay();
+    hwSensor.power      = mSensorFusion.hasGyro() ?
+            mSensorFusion.getPowerUsage() : mAccelerometer.getPowerUsage();
+    hwSensor.minDelay   = mSensorFusion.hasGyro() ?
+            mSensorFusion.getMinDelay() : mAccelerometer.getMinDelay();
     Sensor sensor(&hwSensor);
     return sensor;
 }
diff --git a/services/sensorservice/GravitySensor.h b/services/sensorservice/GravitySensor.h
index decfbb8..0ca3a3c 100644
--- a/services/sensorservice/GravitySensor.h
+++ b/services/sensorservice/GravitySensor.h
@@ -22,7 +22,6 @@
 
 #include <gui/Sensor.h>
 
-#include "SensorDevice.h"
 #include "SensorInterface.h"
 #include "SecondOrderLowPassFilter.h"
 
@@ -30,13 +29,17 @@
 namespace android {
 // ---------------------------------------------------------------------------
 
+class SensorDevice;
+class SensorFusion;
+
 class GravitySensor : public SensorInterface {
     SensorDevice& mSensorDevice;
+    SensorFusion& mSensorFusion;
     Sensor mAccelerometer;
     double mAccTime;
 
     SecondOrderLowPassFilter mLowPass;
-    CascadedBiquadFilter mX, mY, mZ;
+    CascadedBiquadFilter<float> mX, mY, mZ;
 
 public:
     GravitySensor(sensor_t const* list, size_t count);
diff --git a/services/sensorservice/LinearAccelerationSensor.cpp b/services/sensorservice/LinearAccelerationSensor.cpp
index 9425a92..f0054f2 100644
--- a/services/sensorservice/LinearAccelerationSensor.cpp
+++ b/services/sensorservice/LinearAccelerationSensor.cpp
@@ -23,6 +23,8 @@
 #include <hardware/sensors.h>
 
 #include "LinearAccelerationSensor.h"
+#include "SensorDevice.h"
+#include "SensorFusion.h"
 
 namespace android {
 // ---------------------------------------------------------------------------
@@ -31,34 +33,29 @@ LinearAccelerationSensor::LinearAccelerationSensor(sensor_t const* list, size_t
     : mSensorDevice(SensorDevice::getInstance()),
       mGravitySensor(list, count)
 {
-    mData[0] = mData[1] = mData[2] = 0;
 }
 
 bool LinearAccelerationSensor::process(sensors_event_t* outEvent,
         const sensors_event_t& event)
 {
     bool result = mGravitySensor.process(outEvent, event);
-    if (result) {
-        if (event.type == SENSOR_TYPE_ACCELEROMETER) {
-            mData[0] = event.acceleration.x;
-            mData[1] = event.acceleration.y;
-            mData[2] = event.acceleration.z;
-        }
-        outEvent->data[0] = mData[0] - outEvent->data[0];
-        outEvent->data[1] = mData[1] - outEvent->data[1];
-        outEvent->data[2] = mData[2] - outEvent->data[2];
+    if (result && event.type == SENSOR_TYPE_ACCELEROMETER) {
+        outEvent->data[0] = event.acceleration.x - outEvent->data[0];
+        outEvent->data[1] = event.acceleration.y - outEvent->data[1];
+        outEvent->data[2] = event.acceleration.z - outEvent->data[2];
         outEvent->sensor = '_lin';
         outEvent->type = SENSOR_TYPE_LINEAR_ACCELERATION;
+        return true;
     }
-    return result;
+    return false;
 }
 
 status_t LinearAccelerationSensor::activate(void* ident, bool enabled) {
-    return mGravitySensor.activate(ident, enabled);
+    return mGravitySensor.activate(this, enabled);
 }
 
 status_t LinearAccelerationSensor::setDelay(void* ident, int handle, int64_t ns) {
-    return mGravitySensor.setDelay(ident, handle, ns);
+    return mGravitySensor.setDelay(this, handle, ns);
 }
 
 Sensor LinearAccelerationSensor::getSensor() const {
@@ -66,7 +63,7 @@ Sensor LinearAccelerationSensor::getSensor() const {
     sensor_t hwSensor;
     hwSensor.name       = "Linear Acceleration Sensor";
     hwSensor.vendor     = "Google Inc.";
-    hwSensor.version    = 1;
+    hwSensor.version    = gsensor.getVersion();
     hwSensor.handle     = '_lin';
     hwSensor.type       = SENSOR_TYPE_LINEAR_ACCELERATION;
     hwSensor.maxRange   = gsensor.getMaxValue();
diff --git a/services/sensorservice/LinearAccelerationSensor.h b/services/sensorservice/LinearAccelerationSensor.h
index c577086..5deb24f 100644
--- a/services/sensorservice/LinearAccelerationSensor.h
+++ b/services/sensorservice/LinearAccelerationSensor.h
@@ -22,19 +22,19 @@
 
 #include <gui/Sensor.h>
 
-#include "SensorDevice.h"
 #include "SensorInterface.h"
 #include "GravitySensor.h"
 
 // ---------------------------------------------------------------------------
-
 namespace android {
 // ---------------------------------------------------------------------------
 
+class SensorDevice;
+class SensorFusion;
+
 class LinearAccelerationSensor : public SensorInterface {
     SensorDevice& mSensorDevice;
     GravitySensor mGravitySensor;
-    float mData[3];
 
     virtual bool process(sensors_event_t* outEvent,
             const sensors_event_t& event);
diff --git a/services/sensorservice/OrientationSensor.cpp b/services/sensorservice/OrientationSensor.cpp
new file mode 100644
index 0000000..c9e5080
--- /dev/null
+++ b/services/sensorservice/OrientationSensor.cpp
@@ -0,0 +1,89 @@
+/*
+ * 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 <stdint.h>
+#include <math.h>
+#include <sys/types.h>
+
+#include <utils/Errors.h>
+
+#include <hardware/sensors.h>
+
+#include "OrientationSensor.h"
+#include "SensorDevice.h"
+#include "SensorFusion.h"
+
+namespace android {
+// ---------------------------------------------------------------------------
+
+OrientationSensor::OrientationSensor()
+    : mSensorDevice(SensorDevice::getInstance()),
+      mSensorFusion(SensorFusion::getInstance())
+{
+}
+
+bool OrientationSensor::process(sensors_event_t* outEvent,
+        const sensors_event_t& event)
+{
+    if (event.type == SENSOR_TYPE_ACCELEROMETER) {
+        if (mSensorFusion.hasEstimate()) {
+            vec3_t g;
+            const float rad2deg = 180 / M_PI;
+            const mat33_t R(mSensorFusion.getRotationMatrix());
+            g[0] = atan2f(-R[1][0], R[0][0])    * rad2deg;
+            g[1] = atan2f(-R[2][1], R[2][2])    * rad2deg;
+            g[2] = asinf ( R[2][0])             * rad2deg;
+            if (g[0] < 0)
+                g[0] += 360;
+
+            *outEvent = event;
+            outEvent->data[0] = g.x;
+            outEvent->data[1] = g.y;
+            outEvent->data[2] = g.z;
+            outEvent->sensor = '_ypr';
+            outEvent->type = SENSOR_TYPE_ORIENTATION;
+            return true;
+        }
+    }
+    return false;
+}
+
+status_t OrientationSensor::activate(void* ident, bool enabled) {
+    return mSensorFusion.activate(this, enabled);
+}
+
+status_t OrientationSensor::setDelay(void* ident, int handle, int64_t ns) {
+    return mSensorFusion.setDelay(this, ns);
+}
+
+Sensor OrientationSensor::getSensor() const {
+    sensor_t hwSensor;
+    hwSensor.name       = "Orientation Sensor";
+    hwSensor.vendor     = "Google Inc.";
+    hwSensor.version    = 1;
+    hwSensor.handle     = '_ypr';
+    hwSensor.type       = SENSOR_TYPE_ORIENTATION;
+    hwSensor.maxRange   = 360.0f;
+    hwSensor.resolution = 1.0f/256.0f; // FIXME: real value here
+    hwSensor.power      = mSensorFusion.getPowerUsage();
+    hwSensor.minDelay   = mSensorFusion.getMinDelay();
+    Sensor sensor(&hwSensor);
+    return sensor;
+}
+
+// ---------------------------------------------------------------------------
+}; // namespace android
+
diff --git a/services/sensorservice/OrientationSensor.h b/services/sensorservice/OrientationSensor.h
new file mode 100644
index 0000000..855949d
--- /dev/null
+++ b/services/sensorservice/OrientationSensor.h
@@ -0,0 +1,51 @@
+/*
+ * 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.
+ */
+
+#ifndef ANDROID_ORIENTATION_SENSOR_H
+#define ANDROID_ORIENTATION_SENSOR_H
+
+#include <stdint.h>
+#include <sys/types.h>
+
+#include <gui/Sensor.h>
+
+#include "SensorInterface.h"
+
+// ---------------------------------------------------------------------------
+namespace android {
+// ---------------------------------------------------------------------------
+
+class SensorDevice;
+class SensorFusion;
+
+class OrientationSensor : public SensorInterface {
+    SensorDevice& mSensorDevice;
+    SensorFusion& mSensorFusion;
+
+public:
+    OrientationSensor();
+    virtual bool process(sensors_event_t* outEvent,
+            const sensors_event_t& event);
+    virtual status_t activate(void* ident, bool enabled);
+    virtual status_t setDelay(void* ident, int handle, int64_t ns);
+    virtual Sensor getSensor() const;
+    virtual bool isVirtual() const { return true; }
+};
+
+// ---------------------------------------------------------------------------
+}; // namespace android
+
+#endif // ANDROID_ORIENTATION_SENSOR_H
diff --git a/services/sensorservice/RotationVectorSensor.cpp b/services/sensorservice/RotationVectorSensor.cpp
index 3abfc12..cba89c9 100644
--- a/services/sensorservice/RotationVectorSensor.cpp
+++ b/services/sensorservice/RotationVectorSensor.cpp
@@ -32,134 +32,67 @@ static inline T clamp(T v) {
     return v < 0 ? 0 : v;
 }
 
-RotationVectorSensor::RotationVectorSensor(sensor_t const* list, size_t count)
+RotationVectorSensor::RotationVectorSensor()
     : mSensorDevice(SensorDevice::getInstance()),
-      mALowPass(M_SQRT1_2, 1.5f),
-      mAX(mALowPass), mAY(mALowPass), mAZ(mALowPass),
-      mMLowPass(M_SQRT1_2, 1.5f),
-      mMX(mMLowPass), mMY(mMLowPass), mMZ(mMLowPass)
+      mSensorFusion(SensorFusion::getInstance())
 {
-    for (size_t i=0 ; i<count ; i++) {
-        if (list[i].type == SENSOR_TYPE_ACCELEROMETER) {
-            mAcc = Sensor(list + i);
-        }
-        if (list[i].type == SENSOR_TYPE_MAGNETIC_FIELD) {
-            mMag = Sensor(list + i);
-        }
-    }
-    memset(mMagData, 0, sizeof(mMagData));
 }
 
 bool RotationVectorSensor::process(sensors_event_t* outEvent,
         const sensors_event_t& event)
 {
-    const static double NS2S = 1.0 / 1000000000.0;
-    if (event.type == SENSOR_TYPE_MAGNETIC_FIELD) {
-        const double now = event.timestamp * NS2S;
-        if (mMagTime == 0) {
-            mMagData[0] = mMX.init(event.magnetic.x);
-            mMagData[1] = mMY.init(event.magnetic.y);
-            mMagData[2] = mMZ.init(event.magnetic.z);
-        } else {
-            double dT = now - mMagTime;
-            mMLowPass.setSamplingPeriod(dT);
-            mMagData[0] = mMX(event.magnetic.x);
-            mMagData[1] = mMY(event.magnetic.y);
-            mMagData[2] = mMZ(event.magnetic.z);
-        }
-        mMagTime = now;
-    }
     if (event.type == SENSOR_TYPE_ACCELEROMETER) {
-        const double now = event.timestamp * NS2S;
-        float Ax, Ay, Az;
-        if (mAccTime == 0) {
-            Ax = mAX.init(event.acceleration.x);
-            Ay = mAY.init(event.acceleration.y);
-            Az = mAZ.init(event.acceleration.z);
-        } else {
-            double dT = now - mAccTime;
-            mALowPass.setSamplingPeriod(dT);
-            Ax = mAX(event.acceleration.x);
-            Ay = mAY(event.acceleration.y);
-            Az = mAZ(event.acceleration.z);
-        }
-        mAccTime = now;
-        const float Ex = mMagData[0];
-        const float Ey = mMagData[1];
-        const float Ez = mMagData[2];
-        float Hx = Ey*Az - Ez*Ay;
-        float Hy = Ez*Ax - Ex*Az;
-        float Hz = Ex*Ay - Ey*Ax;
-        const float normH = sqrtf(Hx*Hx + Hy*Hy + Hz*Hz);
-        if (normH < 0.1f) {
-            // device is close to free fall (or in space?), or close to
-            // magnetic north pole. Typical values are  > 100.
-            return false;
+        if (mSensorFusion.hasEstimate()) {
+            const mat33_t R(mSensorFusion.getRotationMatrix());
+
+            // matrix to rotation vector (normalized quaternion)
+            const float Hx = R[0].x;
+            const float My = R[1].y;
+            const float Az = R[2].z;
+
+            float qw = sqrtf( clamp( Hx + My + Az + 1) * 0.25f );
+            float qx = sqrtf( clamp( Hx - My - Az + 1) * 0.25f );
+            float qy = sqrtf( clamp(-Hx + My - Az + 1) * 0.25f );
+            float qz = sqrtf( clamp(-Hx - My + Az + 1) * 0.25f );
+            qx = copysignf(qx, R[2].y - R[1].z);
+            qy = copysignf(qy, R[0].z - R[2].x);
+            qz = copysignf(qz, R[1].x - R[0].y);
+
+            // this quaternion is guaranteed to be normalized, by construction
+            // of the rotation matrix.
+
+            *outEvent = event;
+            outEvent->data[0] = qx;
+            outEvent->data[1] = qy;
+            outEvent->data[2] = qz;
+            outEvent->data[3] = qw;
+            outEvent->sensor = '_rov';
+            outEvent->type = SENSOR_TYPE_ROTATION_VECTOR;
+            return true;
         }
-        const float invH = 1.0f / normH;
-        const float invA = 1.0f / sqrtf(Ax*Ax + Ay*Ay + Az*Az);
-        Hx *= invH;
-        Hy *= invH;
-        Hz *= invH;
-        Ax *= invA;
-        Ay *= invA;
-        Az *= invA;
-        const float Mx = Ay*Hz - Az*Hy;
-        const float My = Az*Hx - Ax*Hz;
-        const float Mz = Ax*Hy - Ay*Hx;
-
-        // matrix to rotation vector (normalized quaternion)
-        float qw = sqrtf( clamp( Hx + My + Az + 1) * 0.25f );
-        float qx = sqrtf( clamp( Hx - My - Az + 1) * 0.25f );
-        float qy = sqrtf( clamp(-Hx + My - Az + 1) * 0.25f );
-        float qz = sqrtf( clamp(-Hx - My + Az + 1) * 0.25f );
-        qx = copysignf(qx, Ay - Mz);
-        qy = copysignf(qy, Hz - Ax);
-        qz = copysignf(qz, Mx - Hy);
-
-        // this quaternion is guaranteed to be normalized, by construction
-        // of the rotation matrix.
-
-        *outEvent = event;
-        outEvent->data[0] = qx;
-        outEvent->data[1] = qy;
-        outEvent->data[2] = qz;
-        outEvent->data[3] = qw;
-        outEvent->sensor = '_rov';
-        outEvent->type = SENSOR_TYPE_ROTATION_VECTOR;
-        return true;
     }
     return false;
 }
 
 status_t RotationVectorSensor::activate(void* ident, bool enabled) {
-    mSensorDevice.activate(this, mAcc.getHandle(), enabled);
-    mSensorDevice.activate(this, mMag.getHandle(), enabled);
-    if (enabled) {
-        mMagTime = 0;
-        mAccTime = 0;
-    }
-    return NO_ERROR;
+    return mSensorFusion.activate(this, enabled);
 }
 
-status_t RotationVectorSensor::setDelay(void* ident, int handle, int64_t ns)
-{
-    mSensorDevice.setDelay(this, mAcc.getHandle(), ns);
-    mSensorDevice.setDelay(this, mMag.getHandle(), ns);
-    return NO_ERROR;
+status_t RotationVectorSensor::setDelay(void* ident, int handle, int64_t ns) {
+    return mSensorFusion.setDelay(this, ns);
 }
 
 Sensor RotationVectorSensor::getSensor() const {
     sensor_t hwSensor;
     hwSensor.name       = "Rotation Vector Sensor";
     hwSensor.vendor     = "Google Inc.";
-    hwSensor.version    = 1;
+    hwSensor.version    = mSensorFusion.hasGyro() ? 3 : 2;
     hwSensor.handle     = '_rov';
     hwSensor.type       = SENSOR_TYPE_ROTATION_VECTOR;
     hwSensor.maxRange   = 1;
     hwSensor.resolution = 1.0f / (1<<24);
-    hwSensor.power      = mAcc.getPowerUsage() + mMag.getPowerUsage();
-    hwSensor.minDelay   = mAcc.getMinDelay();
+    hwSensor.power      = mSensorFusion.getPowerUsage();
+    hwSensor.minDelay   = mSensorFusion.getMinDelay();
     Sensor sensor(&hwSensor);
     return sensor;
 }
diff --git a/services/sensorservice/RotationVectorSensor.h b/services/sensorservice/RotationVectorSensor.h
index 17699f8..ac76487 100644
--- a/services/sensorservice/RotationVectorSensor.h
+++ b/services/sensorservice/RotationVectorSensor.h
@@ -26,24 +26,19 @@
 #include "SensorInterface.h"
 #include "SecondOrderLowPassFilter.h"
 
+#include "Fusion.h"
+#include "SensorFusion.h"
+
 // ---------------------------------------------------------------------------
 namespace android {
 // ---------------------------------------------------------------------------
 
 class RotationVectorSensor : public SensorInterface {
     SensorDevice& mSensorDevice;
-    Sensor mAcc;
-    Sensor mMag;
-    float mMagData[3];
-    double mAccTime;
-    double mMagTime;
-    SecondOrderLowPassFilter mALowPass;
-    CascadedBiquadFilter mAX, mAY, mAZ;
-    SecondOrderLowPassFilter mMLowPass;
-    CascadedBiquadFilter mMX, mMY, mMZ;
+    SensorFusion& mSensorFusion;
 
 public:
-    RotationVectorSensor(sensor_t const* list, size_t count);
+    RotationVectorSensor();
     virtual bool process(sensors_event_t* outEvent,
             const sensors_event_t& event);
     virtual status_t activate(void* ident, bool enabled);
diff --git a/services/sensorservice/SecondOrderLowPassFilter.cpp b/services/sensorservice/SecondOrderLowPassFilter.cpp
index eeb6d1e..c76dd4c 100644
--- a/services/sensorservice/SecondOrderLowPassFilter.cpp
+++ b/services/sensorservice/SecondOrderLowPassFilter.cpp
@@ -21,6 +21,7 @@
 #include <cutils/log.h>
 
 #include "SecondOrderLowPassFilter.h"
+#include "vec.h"
 
 // ---------------------------------------------------------------------------
 
@@ -44,21 +45,24 @@ void SecondOrderLowPassFilter::setSamplingPeriod(float dT)
 
 // ---------------------------------------------------------------------------
 
-BiquadFilter::BiquadFilter(const SecondOrderLowPassFilter& s)
+template<typename T>
+BiquadFilter<T>::BiquadFilter(const SecondOrderLowPassFilter& s)
     : s(s)
 {
 }
 
-float BiquadFilter::init(float x)
+template<typename T>
+T BiquadFilter<T>::init(const T& x)
 {
     x1 = x2 = x;
     y1 = y2 = x;
     return x;
 }
 
-float BiquadFilter::operator()(float x)
+template<typename T>
+T BiquadFilter<T>::operator()(const T& x)
 {
-    float y = (x + x2)*s.a0 + x1*s.a1 - y1*s.b1 - y2*s.b2;
+    T y = (x + x2)*s.a0 + x1*s.a1 - y1*s.b1 - y2*s.b2;
     x2 = x1;
     y2 = y1;
     x1 = x;
@@ -68,22 +72,32 @@ float BiquadFilter::operator()(float x)
 
 // ---------------------------------------------------------------------------
 
-CascadedBiquadFilter::CascadedBiquadFilter(const SecondOrderLowPassFilter& s)
+template<typename T>
+CascadedBiquadFilter<T>::CascadedBiquadFilter(const SecondOrderLowPassFilter& s)
     : mA(s), mB(s)
 {
 }
 
-float CascadedBiquadFilter::init(float x)
+template<typename T>
+T CascadedBiquadFilter<T>::init(const T& x)
 {
     mA.init(x);
     mB.init(x);
     return x;
 }
 
-float CascadedBiquadFilter::operator()(float x)
+template<typename T>
+T CascadedBiquadFilter<T>::operator()(const T& x)
 {
     return mB(mA(x));
 }
 
 // ---------------------------------------------------------------------------
+
+template class BiquadFilter<float>;
+template class CascadedBiquadFilter<float>;
+template class BiquadFilter<vec3_t>;
+template class CascadedBiquadFilter<vec3_t>;
+
+// ---------------------------------------------------------------------------
 }; // namespace android
diff --git a/services/sensorservice/SecondOrderLowPassFilter.h b/services/sensorservice/SecondOrderLowPassFilter.h
index 85698ca..0cc2446 100644
--- a/services/sensorservice/SecondOrderLowPassFilter.h
+++ b/services/sensorservice/SecondOrderLowPassFilter.h
@@ -25,12 +25,14 @@
 namespace android {
 // ---------------------------------------------------------------------------
 
+template<typename T>
 class BiquadFilter;
 
 /*
  * State of a 2nd order low-pass IIR filter
  */
 class SecondOrderLowPassFilter {
+    template<typename T>
     friend class BiquadFilter;
     float iQ, fc;
     float K, iD;
@@ -44,27 +46,29 @@ public:
 /*
  * Implements a Biquad IIR filter
  */
+template<typename T>
 class BiquadFilter {
-    float x1, x2;
-    float y1, y2;
+    T x1, x2;
+    T y1, y2;
     const SecondOrderLowPassFilter& s;
 public:
     BiquadFilter(const SecondOrderLowPassFilter& s);
-    float init(float in);
-    float operator()(float in);
+    T init(const T& in);
+    T operator()(const T& in);
 };
 
 /*
  * Two cascaded biquad IIR filters
  * (4-poles IIR)
  */
+template<typename T>
 class CascadedBiquadFilter {
-    BiquadFilter mA;
-    BiquadFilter mB;
+    BiquadFilter<T> mA;
+    BiquadFilter<T> mB;
 public:
     CascadedBiquadFilter(const SecondOrderLowPassFilter& s);
-    float init(float in);
-    float operator()(float in);
+    T init(const T& in);
+    T operator()(const T& in);
 };
 
 // ---------------------------------------------------------------------------
diff --git a/services/sensorservice/SensorDevice.cpp b/services/sensorservice/SensorDevice.cpp
index b3c8ef5..38d498c 100644
--- a/services/sensorservice/SensorDevice.cpp
+++ b/services/sensorservice/SensorDevice.cpp
@@ -251,6 +251,9 @@ status_t SensorDevice::setDelay(void* ident, int handle, int64_t ns)
             }
         }
     }
+
+    //LOGD("setDelay: ident=%p, handle=%d, ns=%lld", ident, handle, ns);
+
     return mSensorDevice->setDelay(mSensorDevice, handle, ns);
 }
 
diff --git a/services/sensorservice/SensorFusion.cpp b/services/sensorservice/SensorFusion.cpp
new file mode 100644
index 0000000..d4226ec
--- /dev/null
+++ b/services/sensorservice/SensorFusion.cpp
@@ -0,0 +1,180 @@
+/*
+ * 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 "SensorDevice.h"
+#include "SensorFusion.h"
+#include "SensorService.h"
+
+namespace android {
+// ---------------------------------------------------------------------------
+
+ANDROID_SINGLETON_STATIC_INSTANCE(SensorFusion)
+
+SensorFusion::SensorFusion()
+    : mSensorDevice(SensorDevice::getInstance()),
+      mEnabled(false), mHasGyro(false), mGyroTime(0), mRotationMatrix(1),
+      mLowPass(M_SQRT1_2, 1.0f), mAccData(mLowPass),
+      mFilteredMag(0.0f), mFilteredAcc(0.0f)
+{
+    sensor_t const* list;
+    size_t count = mSensorDevice.getSensorList(&list);
+    for (size_t i=0 ; i<count ; i++) {
+        if (list[i].type == SENSOR_TYPE_ACCELEROMETER) {
+            mAcc = Sensor(list + i);
+        }
+        if (list[i].type == SENSOR_TYPE_MAGNETIC_FIELD) {
+            mMag = Sensor(list + i);
+        }
+        if (list[i].type == SENSOR_TYPE_GYROSCOPE) {
+            mGyro = Sensor(list + i);
+            // 200 Hz for gyro events is a good compromise between precision
+            // and power/cpu usage.
+            mTargetDelayNs = 1000000000LL/200;
+            mGyroRate = 1000000000.0f / mTargetDelayNs;
+            mHasGyro = true;
+        }
+    }
+    mFusion.init();
+    mAccData.init(vec3_t(0.0f));
+}
+
+void SensorFusion::process(const sensors_event_t& event) {
+
+    if (event.type == SENSOR_TYPE_GYROSCOPE && mHasGyro) {
+        if (mGyroTime != 0) {
+            const float dT = (event.timestamp - mGyroTime) / 1000000000.0f;
+            const float freq = 1 / dT;
+            const float alpha = 2 / (2 + dT); // 2s time-constant
+            mGyroRate = mGyroRate*alpha +  freq*(1 - alpha);
+        }
+        mGyroTime = event.timestamp;
+        mFusion.handleGyro(vec3_t(event.data), 1.0f/mGyroRate);
+    } else if (event.type == SENSOR_TYPE_MAGNETIC_FIELD) {
+        const vec3_t mag(event.data);
+        if (mHasGyro) {
+            mFusion.handleMag(mag);
+        } else {
+            const float l(length(mag));
+            if (l>5 && l<100) {
+                mFilteredMag = mag * (1/l);
+            }
+        }
+    } else if (event.type == SENSOR_TYPE_ACCELEROMETER) {
+        const vec3_t acc(event.data);
+        if (mHasGyro) {
+            mFusion.handleAcc(acc);
+            mRotationMatrix = mFusion.getRotationMatrix();
+        } else {
+            const float l(length(acc));
+            if (l > 0.981f) {
+                // remove the linear-acceleration components
+                mFilteredAcc = mAccData(acc * (1/l));
+            }
+            if (length(mFilteredAcc)>0 && length(mFilteredMag)>0) {
+                vec3_t up(mFilteredAcc);
+                vec3_t east(cross_product(mFilteredMag, up));
+                east *= 1/length(east);
+                vec3_t north(cross_product(up, east));
+                mRotationMatrix << east << north << up;
+            }
+        }
+    }
+}
+
+template <typename T> inline T min(T a, T b) { return a<b ? a : b; }
+template <typename T> inline T max(T a, T b) { return a>b ? a : b; }
+
+status_t SensorFusion::activate(void* ident, bool enabled) {
+
+    LOGD_IF(DEBUG_CONNECTIONS,
+            "SensorFusion::activate(ident=%p, enabled=%d)",
+            ident, enabled);
+
+    const ssize_t idx = mClients.indexOf(ident);
+    if (enabled) {
+        if (idx < 0) {
+            mClients.add(ident);
+        }
+    } else {
+        if (idx >= 0) {
+            mClients.removeItemsAt(idx);
+        }
+    }
+
+    mSensorDevice.activate(ident, mAcc.getHandle(), enabled);
+    mSensorDevice.activate(ident, mMag.getHandle(), enabled);
+    if (mHasGyro) {
+        mSensorDevice.activate(ident, mGyro.getHandle(), enabled);
+    }
+
+    const bool newState = mClients.size() != 0;
+    if (newState != mEnabled) {
+        mEnabled = newState;
+        if (newState) {
+            mFusion.init();
+        }
+    }
+    return NO_ERROR;
+}
+
+status_t SensorFusion::setDelay(void* ident, int64_t ns) {
+    if (mHasGyro) {
+        mSensorDevice.setDelay(ident, mAcc.getHandle(), ns);
+        mSensorDevice.setDelay(ident, mMag.getHandle(), ms2ns(20));
+        mSensorDevice.setDelay(ident, mGyro.getHandle(), mTargetDelayNs);
+    } else {
+        const static double NS2S = 1.0 / 1000000000.0;
+        mSensorDevice.setDelay(ident, mAcc.getHandle(), ns);
+        mSensorDevice.setDelay(ident, mMag.getHandle(), max(ns, mMag.getMinDelayNs()));
+        mLowPass.setSamplingPeriod(ns*NS2S);
+    }
+    return NO_ERROR;
+}
+
+
+float SensorFusion::getPowerUsage() const {
+    float power = mAcc.getPowerUsage() + mMag.getPowerUsage();
+    if (mHasGyro) {
+        power += mGyro.getPowerUsage();
+    }
+    return power;
+}
+
+int32_t SensorFusion::getMinDelay() const {
+    return mAcc.getMinDelay();
+}
+
+void SensorFusion::dump(String8& result, char* buffer, size_t SIZE) {
+    const Fusion& fusion(mFusion);
+    snprintf(buffer, SIZE, "Fusion (%s) %s (%d clients), gyro-rate=%7.2fHz, "
+            "MRPS=< %g, %g, %g > (%g), "
+            "BIAS=< %g, %g, %g >\n",
+            mHasGyro ? "9-axis" : "6-axis",
+            mEnabled ? "enabled" : "disabled",
+            mClients.size(),
+            mGyroRate,
+            fusion.getAttitude().x,
+            fusion.getAttitude().y,
+            fusion.getAttitude().z,
+            dot_product(fusion.getAttitude(), fusion.getAttitude()),
+            fusion.getBias().x,
+            fusion.getBias().y,
+            fusion.getBias().z);
+    result.append(buffer);
+}
+
+// ---------------------------------------------------------------------------
+}; // namespace android
diff --git a/services/sensorservice/SensorFusion.h b/services/sensorservice/SensorFusion.h
new file mode 100644
index 0000000..c7eab12
--- /dev/null
+++ b/services/sensorservice/SensorFusion.h
@@ -0,0 +1,84 @@
+/*
+ * 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.
+ */
+
+#ifndef ANDROID_SENSOR_FUSION_H
+#define ANDROID_SENSOR_FUSION_H
+
+#include <stdint.h>
+#include <sys/types.h>
+
+#include <utils/SortedVector.h>
+#include <utils/Singleton.h>
+#include <utils/String8.h>
+
+#include <gui/Sensor.h>
+
+#include "Fusion.h"
+#include "SecondOrderLowPassFilter.h"
+
+// ---------------------------------------------------------------------------
+
+namespace android {
+// ---------------------------------------------------------------------------
+
+class SensorDevice;
+
+class SensorFusion : public Singleton<SensorFusion> {
+    friend class Singleton<SensorFusion>;
+
+    SensorDevice& mSensorDevice;
+    Sensor mAcc;
+    Sensor mMag;
+    Sensor mGyro;
+    Fusion mFusion;
+    bool mEnabled;
+    bool mHasGyro;
+    float mGyroRate;
+    nsecs_t mTargetDelayNs;
+    nsecs_t mGyroTime;
+    mat33_t mRotationMatrix;
+    SecondOrderLowPassFilter mLowPass;
+    BiquadFilter<vec3_t> mAccData;
+    vec3_t mFilteredMag;
+    vec3_t mFilteredAcc;
+    SortedVector<void*> mClients;
+
+    SensorFusion();
+
+public:
+    void process(const sensors_event_t& event);
+
+    bool isEnabled() const { return mEnabled; }
+    bool hasGyro() const { return mHasGyro; }
+    bool hasEstimate() const { return !mHasGyro || mFusion.hasEstimate(); }
+    mat33_t getRotationMatrix() const { return mRotationMatrix; }
+    vec3_t getGyroBias() const { return mFusion.getBias(); }
+    float getEstimatedRate() const { return mGyroRate; }
+
+    status_t activate(void* ident, bool enabled);
+    status_t setDelay(void* ident, int64_t ns);
+
+    float getPowerUsage() const;
+    int32_t getMinDelay() const;
+
+    void dump(String8& result, char* buffer, size_t SIZE);
+};
+
+
+// ---------------------------------------------------------------------------
+}; // namespace android
+
+#endif // ANDROID_SENSOR_FUSION_H
diff --git a/services/sensorservice/SensorInterface.h b/services/sensorservice/SensorInterface.h
index 084f2f5..fb357d7 100644
--- a/services/sensorservice/SensorInterface.h
+++ b/services/sensorservice/SensorInterface.h
@@ -20,8 +20,6 @@
 #include <stdint.h>
 #include <sys/types.h>
 
-#include <utils/Singleton.h>
-
 #include <gui/Sensor.h>
 
 #include "SensorDevice.h"
diff --git a/services/sensorservice/SensorService.cpp b/services/sensorservice/SensorService.cpp
index f1db2f5..5b86d10 100644
--- a/services/sensorservice/SensorService.cpp
+++ b/services/sensorservice/SensorService.cpp
@@ -35,10 +35,13 @@
 
 #include <hardware/sensors.h>
 
-#include "SensorService.h"
+#include "CorrectedGyroSensor.h"
 #include "GravitySensor.h"
 #include "LinearAccelerationSensor.h"
+#include "OrientationSensor.h"
 #include "RotationVectorSensor.h"
+#include "SensorFusion.h"
+#include "SensorService.h"
 
 namespace android {
 // ---------------------------------------------------------------------------
@@ -74,14 +77,26 @@ void SensorService::onFirstRef()
             }
         }
 
-        if (virtualSensorsNeeds & (1<<SENSOR_TYPE_GRAVITY)) {
-            registerVirtualSensor( new GravitySensor(list, count) );
-        }
-        if (virtualSensorsNeeds & (1<<SENSOR_TYPE_LINEAR_ACCELERATION)) {
-            registerVirtualSensor( new LinearAccelerationSensor(list, count) );
-        }
-        if (virtualSensorsNeeds & (1<<SENSOR_TYPE_ROTATION_VECTOR)) {
-            registerVirtualSensor( new RotationVectorSensor(list, count) );
+        // it's safe to instantiate the SensorFusion object here
+        // (it wants to be instantiated after h/w sensors have been
+        // registered)
+        const SensorFusion& fusion(SensorFusion::getInstance());
+
+        // Always instantiate Android's virtual sensors. Since they are
+        // instantiated behind sensors from the HAL, they won't
+        // interfere with applications, unless they looks specifically
+        // for them (by name).
+
+        registerVirtualSensor( new RotationVectorSensor() );
+        registerVirtualSensor( new GravitySensor(list, count) );
+        registerVirtualSensor( new LinearAccelerationSensor(list, count) );
+
+        // if we have a gyro, we have the option of enabling these
+        // "better" orientation and gyro sensors
+        if (fusion.hasGyro()) {
+            // FIXME: OrientationSensor buggy when not pointing north
+            registerVirtualSensor( new OrientationSensor() );
+            registerVirtualSensor( new CorrectedGyroSensor(list, count) );
         }
 
         run("SensorService", PRIORITY_URGENT_DISPLAY);
@@ -133,7 +148,9 @@ status_t SensorService::dump(int fd, const Vector<String16>& args)
         for (size_t i=0 ; i<mSensorList.size() ; i++) {
             const Sensor& s(mSensorList[i]);
             const sensors_event_t& e(mLastEventSeen.valueFor(s.getHandle()));
-            snprintf(buffer, SIZE, "%-48s| %-32s | 0x%08x | maxRate=%7.2fHz | last=<%5.1f,%5.1f,%5.1f>\n",
+            snprintf(buffer, SIZE,
+                    "%-48s| %-32s | 0x%08x | maxRate=%7.2fHz | "
+                    "last=<%5.1f,%5.1f,%5.1f>\n",
                     s.getName().string(),
                     s.getVendor().string(),
                     s.getHandle(),
@@ -141,6 +158,7 @@ status_t SensorService::dump(int fd, const Vector<String16>& args)
                     e.data[0], e.data[1], e.data[2]);
             result.append(buffer);
         }
+        SensorFusion::getInstance().dump(result, buffer, SIZE);
         SensorDevice::getInstance().dump(result, buffer, SIZE);
 
         snprintf(buffer, SIZE, "%d active connections\n",
@@ -183,13 +201,19 @@ bool SensorService::threadLoop()
 
         // handle virtual sensors
         if (count && vcount) {
+            sensors_event_t const * const event = buffer;
             const DefaultKeyedVector<int, SensorInterface*> virtualSensors(
                     getActiveVirtualSensors());
             const size_t activeVirtualSensorCount = virtualSensors.size();
             if (activeVirtualSensorCount) {
                 size_t k = 0;
+                SensorFusion& fusion(SensorFusion::getInstance());
+                if (fusion.isEnabled()) {
+                    for (size_t i=0 ; i<size_t(count) ; i++) {
+                        fusion.process(event[i]);
+                    }
+                }
                 for (size_t i=0 ; i<size_t(count) ; i++) {
-                    sensors_event_t const * const event = buffer;
                     for (size_t j=0 ; j<activeVirtualSensorCount ; j++) {
                         sensors_event_t out;
                         if (virtualSensors.valueAt(j)->process(&out, event[i])) {
diff --git a/services/sensorservice/mat.h b/services/sensorservice/mat.h
new file mode 100644
index 0000000..1302ca3
--- /dev/null
+++ b/services/sensorservice/mat.h
@@ -0,0 +1,370 @@
+/*
+ * 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.
+ */
+
+#ifndef ANDROID_MAT_H
+#define ANDROID_MAT_H
+
+#include "vec.h"
+#include "traits.h"
+
+// -----------------------------------------------------------------------
+
+namespace android {
+
+template <typename TYPE, size_t C, size_t R>
+class mat;
+
+namespace helpers {
+
+template <typename TYPE, size_t C, size_t R>
+mat<TYPE, C, R>& doAssign(
+        mat<TYPE, C, R>& lhs,
+        typename TypeTraits<TYPE>::ParameterType rhs) {
+    for (size_t i=0 ; i<C ; i++)
+        for (size_t j=0 ; j<R ; j++)
+            lhs[i][j] = (i==j) ? rhs : 0;
+    return lhs;
+}
+
+template <typename TYPE, size_t C, size_t R, size_t D>
+mat<TYPE, C, R> PURE doMul(
+        const mat<TYPE, D, R>& lhs,
+        const mat<TYPE, C, D>& rhs)
+{
+    mat<TYPE, C, R> res;
+    for (size_t c=0 ; c<C ; c++) {
+        for (size_t r=0 ; r<R ; r++) {
+            TYPE v(0);
+            for (size_t k=0 ; k<D ; k++) {
+                v += lhs[k][r] * rhs[c][k];
+            }
+            res[c][r] = v;
+        }
+    }
+    return res;
+}
+
+template <typename TYPE, size_t R, size_t D>
+vec<TYPE, R> PURE doMul(
+        const mat<TYPE, D, R>& lhs,
+        const vec<TYPE, D>& rhs)
+{
+    vec<TYPE, R> res;
+    for (size_t r=0 ; r<R ; r++) {
+        TYPE v(0);
+        for (size_t k=0 ; k<D ; k++) {
+            v += lhs[k][r] * rhs[k];
+        }
+        res[r] = v;
+    }
+    return res;
+}
+
+template <typename TYPE, size_t C, size_t R>
+mat<TYPE, C, R> PURE doMul(
+        const vec<TYPE, R>& lhs,
+        const mat<TYPE, C, 1>& rhs)
+{
+    mat<TYPE, C, R> res;
+    for (size_t c=0 ; c<C ; c++) {
+        for (size_t r=0 ; r<R ; r++) {
+            res[c][r] = lhs[r] * rhs[c][0];
+        }
+    }
+    return res;
+}
+
+template <typename TYPE, size_t C, size_t R>
+mat<TYPE, C, R> PURE doMul(
+        const mat<TYPE, C, R>& rhs,
+        typename TypeTraits<TYPE>::ParameterType v)
+{
+    mat<TYPE, C, R> res;
+    for (size_t c=0 ; c<C ; c++) {
+        for (size_t r=0 ; r<R ; r++) {
+            res[c][r] = rhs[c][r] * v;
+        }
+    }
+    return res;
+}
+
+template <typename TYPE, size_t C, size_t R>
+mat<TYPE, C, R> PURE doMul(
+        typename TypeTraits<TYPE>::ParameterType v,
+        const mat<TYPE, C, R>& rhs)
+{
+    mat<TYPE, C, R> res;
+    for (size_t c=0 ; c<C ; c++) {
+        for (size_t r=0 ; r<R ; r++) {
+            res[c][r] = v * rhs[c][r];
+        }
+    }
+    return res;
+}
+
+
+}; // namespace helpers
+
+// -----------------------------------------------------------------------
+
+template <typename TYPE, size_t C, size_t R>
+class mat : public vec< vec<TYPE, R>, C > {
+    typedef typename TypeTraits<TYPE>::ParameterType pTYPE;
+    typedef vec< vec<TYPE, R>, C > base;
+public:
+    // STL-like interface.
+    typedef TYPE value_type;
+    typedef TYPE& reference;
+    typedef TYPE const& const_reference;
+    typedef size_t size_type;
+    size_type size() const { return R*C; }
+    enum { ROWS = R, COLS = C };
+
+
+    // -----------------------------------------------------------------------
+    // default constructors
+
+    mat() { }
+    mat(const mat& rhs)  : base(rhs) { }
+    mat(const base& rhs) : base(rhs) { }
+
+    // -----------------------------------------------------------------------
+    // conversion constructors
+
+    // sets the diagonal to the value, off-diagonal to zero
+    mat(pTYPE rhs) {
+        helpers::doAssign(*this, rhs);
+    }
+
+    // -----------------------------------------------------------------------
+    // Assignment
+
+    mat& operator=(const mat& rhs) {
+        base::operator=(rhs);
+        return *this;
+    }
+
+    mat& operator=(const base& rhs) {
+        base::operator=(rhs);
+        return *this;
+    }
+
+    mat& operator=(pTYPE rhs) {
+        return helpers::doAssign(*this, rhs);
+    }
+
+    // -----------------------------------------------------------------------
+    // non-member function declaration and definition
+
+    friend inline mat PURE operator + (const mat& lhs, const mat& rhs) {
+        return helpers::doAdd(
+                static_cast<const base&>(lhs),
+                static_cast<const base&>(rhs));
+    }
+    friend inline mat PURE operator - (const mat& lhs, const mat& rhs) {
+        return helpers::doSub(
+                static_cast<const base&>(lhs),
+                static_cast<const base&>(rhs));
+    }
+
+    // matrix*matrix
+    template <size_t D>
+    friend mat PURE operator * (
+            const mat<TYPE, D, R>& lhs,
+            const mat<TYPE, C, D>& rhs) {
+        return helpers::doMul(lhs, rhs);
+    }
+
+    // matrix*vector
+    friend vec<TYPE, R> PURE operator * (
+            const mat& lhs, const vec<TYPE, C>& rhs) {
+        return helpers::doMul(lhs, rhs);
+    }
+
+    // vector*matrix
+    friend mat PURE operator * (
+            const vec<TYPE, R>& lhs, const mat<TYPE, C, 1>& rhs) {
+        return helpers::doMul(lhs, rhs);
+    }
+
+    // matrix*scalar
+    friend inline mat PURE operator * (const mat& lhs, pTYPE v) {
+        return helpers::doMul(lhs, v);
+    }
+
+    // scalar*matrix
+    friend inline mat PURE operator * (pTYPE v, const mat& rhs) {
+        return helpers::doMul(v, rhs);
+    }
+
+    // -----------------------------------------------------------------------
+    // streaming operator to set the columns of the matrix:
+    // example:
+    //    mat33_t m;
+    //    m << v0 << v1 << v2;
+
+    // column_builder<> stores the matrix and knows which column to set
+    template<size_t PREV_COLUMN>
+    struct column_builder {
+        mat& matrix;
+        column_builder(mat& matrix) : matrix(matrix) { }
+    };
+
+    // operator << is not a method of column_builder<> so we can
+    // overload it for unauthorized values (partial specialization
+    // not allowed in class-scope).
+    // we just set the column and return the next column_builder<>
+    template<size_t PREV_COLUMN>
+    friend column_builder<PREV_COLUMN+1> operator << (
+            const column_builder<PREV_COLUMN>& lhs,
+            const vec<TYPE, R>& rhs) {
+        lhs.matrix[PREV_COLUMN+1] = rhs;
+        return column_builder<PREV_COLUMN+1>(lhs.matrix);
+    }
+
+    // we return void here so we get a compile-time error if the
+    // user tries to set too many columns
+    friend void operator << (
+            const column_builder<C-2>& lhs,
+            const vec<TYPE, R>& rhs) {
+        lhs.matrix[C-1] = rhs;
+    }
+
+    // this is where the process starts. we set the first columns and
+    // return the next column_builder<>
+    column_builder<0> operator << (const vec<TYPE, R>& rhs) {
+        (*this)[0] = rhs;
+        return column_builder<0>(*this);
+    }
+};
+
+// Specialize column matrix so they're exactly equivalent to a vector
+template <typename TYPE, size_t R>
+class mat<TYPE, 1, R> : public vec<TYPE, R> {
+    typedef vec<TYPE, R> base;
+public:
+    // STL-like interface.
+    typedef TYPE value_type;
+    typedef TYPE& reference;
+    typedef TYPE const& const_reference;
+    typedef size_t size_type;
+    size_type size() const { return R; }
+    enum { ROWS = R, COLS = 1 };
+
+    mat() { }
+    mat(const base& rhs) : base(rhs) { }
+    mat(const mat& rhs) : base(rhs) { }
+    mat(const TYPE& rhs) { helpers::doAssign(*this, rhs); }
+    mat& operator=(const mat& rhs) { base::operator=(rhs); return *this; }
+    mat& operator=(const base& rhs) { base::operator=(rhs); return *this; }
+    mat& operator=(const TYPE& rhs) { return helpers::doAssign(*this, rhs); }
+    // we only have one column, so ignore the index
+    const base& operator[](size_t) const { return *this; }
+    base& operator[](size_t) { return *this; }
+    void operator << (const vec<TYPE, R>& rhs) { base::operator[](0) = rhs; }
+};
+
+// -----------------------------------------------------------------------
+// matrix functions
+
+// transpose. this handles matrices of matrices
+inline int     PURE transpose(int v)    { return v; }
+inline float   PURE transpose(float v)  { return v; }
+inline double  PURE transpose(double v) { return v; }
+
+// Transpose a matrix
+template <typename TYPE, size_t C, size_t R>
+mat<TYPE, R, C> PURE transpose(const mat<TYPE, C, R>& m) {
+    mat<TYPE, R, C> r;
+    for (size_t i=0 ; i<R ; i++)
+        for (size_t j=0 ; j<C ; j++)
+            r[i][j] = transpose(m[j][i]);
+    return r;
+}
+
+// Transpose a vector
+template <
+    template<typename T, size_t S> class VEC,
+    typename TYPE,
+    size_t SIZE
+>
+mat<TYPE, SIZE, 1> PURE transpose(const VEC<TYPE, SIZE>& v) {
+    mat<TYPE, SIZE, 1> r;
+    for (size_t i=0 ; i<SIZE ; i++)
+        r[i][0] = transpose(v[i]);
+    return r;
+}
+
+// -----------------------------------------------------------------------
+// "dumb" matrix inversion
+template<typename T, size_t N>
+mat<T, N, N> PURE invert(const mat<T, N, N>& src) {
+    T t;
+    size_t swap;
+    mat<T, N, N> tmp(src);
+    mat<T, N, N> inverse(1);
+
+    for (size_t i=0 ; i<N ; i++) {
+        // look for largest element in column
+        swap = i;
+        for (size_t j=i+1 ; j<N ; j++) {
+            if (fabs(tmp[j][i]) > fabs(tmp[i][i])) {
+                swap = j;
+            }
+        }
+
+        if (swap != i) {
+            /* swap rows. */
+            for (size_t k=0 ; k<N ; k++) {
+                t = tmp[i][k];
+                tmp[i][k] = tmp[swap][k];
+                tmp[swap][k] = t;
+
+                t = inverse[i][k];
+                inverse[i][k] = inverse[swap][k];
+                inverse[swap][k] = t;
+            }
+        }
+
+        t = 1 / tmp[i][i];
+        for (size_t k=0 ; k<N ; k++) {
+            tmp[i][k] *= t;
+            inverse[i][k] *= t;
+        }
+        for (size_t j=0 ; j<N ; j++) {
+            if (j != i) {
+                t = tmp[j][i];
+                for (size_t k=0 ; k<N ; k++) {
+                    tmp[j][k] -= tmp[i][k] * t;
+                    inverse[j][k] -= inverse[i][k] * t;
+                }
+            }
+        }
+    }
+    return inverse;
+}
+
+// -----------------------------------------------------------------------
+
+typedef mat<float, 2, 2> mat22_t;
+typedef mat<float, 3, 3> mat33_t;
+typedef mat<float, 4, 4> mat44_t;
+
+// -----------------------------------------------------------------------
+
+}; // namespace android
+
+#endif /* ANDROID_MAT_H */
diff --git a/services/sensorservice/traits.h b/services/sensorservice/traits.h
new file mode 100644
index 0000000..da4c599
--- /dev/null
+++ b/services/sensorservice/traits.h
@@ -0,0 +1,118 @@
+/*
+ * 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.
+ */
+
+#ifndef ANDROID_TRAITS_H
+#define ANDROID_TRAITS_H
+
+// -----------------------------------------------------------------------
+// Typelists
+
+namespace android {
+
+// end-of-list marker
+class NullType {};
+
+// type-list node
+template <typename T, typename U>
+struct TypeList {
+    typedef T Head;
+    typedef U Tail;
+};
+
+// helpers to build typelists
+#define TYPELIST_1(T1) TypeList<T1, NullType>
+#define TYPELIST_2(T1, T2) TypeList<T1, TYPELIST_1(T2)>
+#define TYPELIST_3(T1, T2, T3) TypeList<T1, TYPELIST_2(T2, T3)>
+#define TYPELIST_4(T1, T2, T3, T4) TypeList<T1, TYPELIST_3(T2, T3, T4)>
+
+// typelists algorithms
+namespace TL {
+template <typename TList, typename T> struct IndexOf;
+
+template <typename T>
+struct IndexOf<NullType, T> {
+    enum { value = -1 };
+};
+
+template <typename T, typename Tail>
+struct IndexOf<TypeList<T, Tail>, T> {
+    enum { value = 0 };
+};
+
+template <typename Head, typename Tail, typename T>
+struct IndexOf<TypeList<Head, Tail>, T> {
+private:
+    enum { temp = IndexOf<Tail, T>::value };
+public:
+    enum { value = temp == -1 ? -1 : 1 + temp };
+};
+
+}; // namespace TL
+
+// type selection based on a boolean
+template <bool flag, typename T, typename U>
+struct Select {
+    typedef T Result;
+};
+template <typename T, typename U>
+struct Select<false, T, U> {
+    typedef U Result;
+};
+
+// -----------------------------------------------------------------------
+// Type traits
+
+template <typename T>
+class TypeTraits {
+    typedef TYPELIST_4(
+            unsigned char, unsigned short,
+            unsigned int, unsigned long int) UnsignedInts;
+
+    typedef TYPELIST_4(
+            signed char, signed short,
+            signed int, signed long int) SignedInts;
+
+    typedef TYPELIST_1(
+            bool) OtherInts;
+
+    typedef TYPELIST_3(
+            float, double, long double) Floats;
+
+    template<typename U> struct PointerTraits {
+        enum { result = false };
+        typedef NullType PointeeType;
+    };
+    template<typename U> struct PointerTraits<U*> {
+        enum { result = true };
+        typedef U PointeeType;
+    };
+
+public:
+    enum { isStdUnsignedInt = TL::IndexOf<UnsignedInts, T>::value >= 0 };
+    enum { isStdSignedInt   = TL::IndexOf<SignedInts,   T>::value >= 0 };
+    enum { isStdIntegral    = TL::IndexOf<OtherInts,    T>::value >= 0 || isStdUnsignedInt || isStdSignedInt };
+    enum { isStdFloat       = TL::IndexOf<Floats,       T>::value >= 0 };
+    enum { isPointer        = PointerTraits<T>::result };
+    enum { isStdArith       = isStdIntegral || isStdFloat };
+
+    // best parameter type for given type
+    typedef typename Select<isStdArith || isPointer, T, const T&>::Result ParameterType;
+};
+
+// -----------------------------------------------------------------------
+}; // namespace android
+
+#endif /* ANDROID_TRAITS_H */
diff --git a/services/sensorservice/vec.h b/services/sensorservice/vec.h
new file mode 100644
index 0000000..736ff37
--- /dev/null
+++ b/services/sensorservice/vec.h
@@ -0,0 +1,420 @@
+/*
+ * 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.
+ */
+
+#ifndef ANDROID_VEC_H
+#define ANDROID_VEC_H
+
+#include <math.h>
+
+#include <stdint.h>
+#include <stddef.h>
+
+#include "traits.h"
+
+// -----------------------------------------------------------------------
+
+#define PURE __attribute__((pure))
+
+namespace android {
+
+// -----------------------------------------------------------------------
+// non-inline helpers
+
+template <typename TYPE, size_t SIZE>
+class vec;
+
+template <typename TYPE, size_t SIZE>
+class vbase;
+
+namespace helpers {
+
+template <typename T> inline T min(T a, T b) { return a<b ? a : b; }
+template <typename T> inline T max(T a, T b) { return a>b ? a : b; }
+
+template < template<typename T, size_t S> class VEC,
+    typename TYPE, size_t SIZE, size_t S>
+vec<TYPE, SIZE>& doAssign(
+        vec<TYPE, SIZE>& lhs, const VEC<TYPE, S>& rhs) {
+    const size_t minSize = min(SIZE, S);
+    const size_t maxSize = max(SIZE, S);
+    for (size_t i=0 ; i<minSize ; i++)
+        lhs[i] = rhs[i];
+    for (size_t i=minSize ; i<maxSize ; i++)
+        lhs[i] = 0;
+    return lhs;
+}
+
+
+template <
+    template<typename T, size_t S> class VLHS,
+    template<typename T, size_t S> class VRHS,
+    typename TYPE,
+    size_t SIZE
+>
+VLHS<TYPE, SIZE> PURE doAdd(
+        const VLHS<TYPE, SIZE>& lhs,
+        const VRHS<TYPE, SIZE>& rhs) {
+    VLHS<TYPE, SIZE> r;
+    for (size_t i=0 ; i<SIZE ; i++)
+        r[i] = lhs[i] + rhs[i];
+    return r;
+}
+
+template <
+    template<typename T, size_t S> class VLHS,
+    template<typename T, size_t S> class VRHS,
+    typename TYPE,
+    size_t SIZE
+>
+VLHS<TYPE, SIZE> PURE doSub(
+        const VLHS<TYPE, SIZE>& lhs,
+        const VRHS<TYPE, SIZE>& rhs) {
+    VLHS<TYPE, SIZE> r;
+    for (size_t i=0 ; i<SIZE ; i++)
+        r[i] = lhs[i] - rhs[i];
+    return r;
+}
+
+template <
+    template<typename T, size_t S> class VEC,
+    typename TYPE,
+    size_t SIZE
+>
+VEC<TYPE, SIZE> PURE doMulScalar(
+        const VEC<TYPE, SIZE>& lhs,
+        typename TypeTraits<TYPE>::ParameterType rhs) {
+    VEC<TYPE, SIZE> r;
+    for (size_t i=0 ; i<SIZE ; i++)
+        r[i] = lhs[i] * rhs;
+    return r;
+}
+
+template <
+    template<typename T, size_t S> class VEC,
+    typename TYPE,
+    size_t SIZE
+>
+VEC<TYPE, SIZE> PURE doScalarMul(
+        typename TypeTraits<TYPE>::ParameterType lhs,
+        const VEC<TYPE, SIZE>& rhs) {
+    VEC<TYPE, SIZE> r;
+    for (size_t i=0 ; i<SIZE ; i++)
+        r[i] = lhs * rhs[i];
+    return r;
+}
+
+}; // namespace helpers
+
+// -----------------------------------------------------------------------
+// Below we define the mathematical operators for vectors.
+// We use template template arguments so we can generically
+// handle the case where the right-hand-size and left-hand-side are
+// different vector types (but with same value_type and size).
+// This is needed for performance when using ".xy{z}" element access
+// on vec<>. Without this, an extra conversion to vec<> would be needed.
+//
+// example:
+//      vec4_t a;
+//      vec3_t b;
+//      vec3_t c = a.xyz + b;
+//
+//  "a.xyz + b" is a mixed-operation between a vbase<> and a vec<>, requiring
+//  a conversion of vbase<> to vec<>. The template gunk below avoids this,
+// by allowing the addition on these different vector types directly
+//
+
+template <
+    template<typename T, size_t S> class VLHS,
+    template<typename T, size_t S> class VRHS,
+    typename TYPE,
+    size_t SIZE
+>
+inline VLHS<TYPE, SIZE> PURE operator + (
+        const VLHS<TYPE, SIZE>& lhs,
+        const VRHS<TYPE, SIZE>& rhs) {
+    return helpers::doAdd(lhs, rhs);
+}
+
+template <
+    template<typename T, size_t S> class VLHS,
+    template<typename T, size_t S> class VRHS,
+    typename TYPE,
+    size_t SIZE
+>
+inline VLHS<TYPE, SIZE> PURE operator - (
+        const VLHS<TYPE, SIZE>& lhs,
+        const VRHS<TYPE, SIZE>& rhs) {
+    return helpers::doSub(lhs, rhs);
+}
+
+template <
+    template<typename T, size_t S> class VEC,
+    typename TYPE,
+    size_t SIZE
+>
+inline VEC<TYPE, SIZE> PURE operator * (
+        const VEC<TYPE, SIZE>& lhs,
+        typename TypeTraits<TYPE>::ParameterType rhs) {
+    return helpers::doMulScalar(lhs, rhs);
+}
+
+template <
+    template<typename T, size_t S> class VEC,
+    typename TYPE,
+    size_t SIZE
+>
+inline VEC<TYPE, SIZE> PURE operator * (
+        typename TypeTraits<TYPE>::ParameterType lhs,
+        const VEC<TYPE, SIZE>& rhs) {
+    return helpers::doScalarMul(lhs, rhs);
+}
+
+
+template <
+    template<typename T, size_t S> class VLHS,
+    template<typename T, size_t S> class VRHS,
+    typename TYPE,
+    size_t SIZE
+>
+TYPE PURE dot_product(
+        const VLHS<TYPE, SIZE>& lhs,
+        const VRHS<TYPE, SIZE>& rhs) {
+    TYPE r(0);
+    for (size_t i=0 ; i<SIZE ; i++)
+        r += lhs[i] * rhs[i];
+    return r;
+}
+
+template <
+    template<typename T, size_t S> class V,
+    typename TYPE,
+    size_t SIZE
+>
+TYPE PURE length(const V<TYPE, SIZE>& v) {
+    return sqrt(dot_product(v, v));
+}
+
+template <
+    template<typename T, size_t S> class VLHS,
+    template<typename T, size_t S> class VRHS,
+    typename TYPE
+>
+VLHS<TYPE, 3> PURE cross_product(
+        const VLHS<TYPE, 3>& u,
+        const VRHS<TYPE, 3>& v) {
+    VLHS<TYPE, 3> r;
+    r.x = u.y*v.z - u.z*v.y;
+    r.y = u.z*v.x - u.x*v.z;
+    r.z = u.x*v.y - u.y*v.x;
+    return r;
+}
+
+
+template <typename TYPE, size_t SIZE>
+vec<TYPE, SIZE> PURE operator - (const vec<TYPE, SIZE>& lhs) {
+    vec<TYPE, SIZE> r;
+    for (size_t i=0 ; i<SIZE ; i++)
+        r[i] = -lhs[i];
+    return r;
+}
+
+// -----------------------------------------------------------------------
+
+// This our basic vector type, it just implements the data storage
+// and accessors.
+
+template <typename TYPE, size_t SIZE>
+struct vbase {
+    TYPE v[SIZE];
+    inline const TYPE& operator[](size_t i) const { return v[i]; }
+    inline       TYPE& operator[](size_t i)       { return v[i]; }
+};
+template<> struct vbase<float, 2> {
+    union {
+        float v[2];
+        struct { float x, y; };
+        struct { float s, t; };
+    };
+    inline const float& operator[](size_t i) const { return v[i]; }
+    inline       float& operator[](size_t i)       { return v[i]; }
+};
+template<> struct vbase<float, 3> {
+    union {
+        float v[3];
+        struct { float x, y, z; };
+        struct { float s, t, r; };
+        vbase<float, 2> xy;
+        vbase<float, 2> st;
+    };
+    inline const float& operator[](size_t i) const { return v[i]; }
+    inline       float& operator[](size_t i)       { return v[i]; }
+};
+template<> struct vbase<float, 4> {
+    union {
+        float v[4];
+        struct { float x, y, z, w; };
+        struct { float s, t, r, q; };
+        vbase<float, 3> xyz;
+        vbase<float, 3> str;
+        vbase<float, 2> xy;
+        vbase<float, 2> st;
+    };
+    inline const float& operator[](size_t i) const { return v[i]; }
+    inline       float& operator[](size_t i)       { return v[i]; }
+};
+
+// -----------------------------------------------------------------------
+
+template <typename TYPE, size_t SIZE>
+class vec : public vbase<TYPE, SIZE>
+{
+    typedef typename TypeTraits<TYPE>::ParameterType pTYPE;
+    typedef vbase<TYPE, SIZE> base;
+
+public:
+    // STL-like interface.
+    typedef TYPE value_type;
+    typedef TYPE& reference;
+    typedef TYPE const& const_reference;
+    typedef size_t size_type;
+
+    typedef TYPE* iterator;
+    typedef TYPE const* const_iterator;
+    iterator begin() { return base::v; }
+    iterator end() { return base::v + SIZE; }
+    const_iterator begin() const { return base::v; }
+    const_iterator end() const { return base::v + SIZE; }
+    size_type size() const { return SIZE; }
+
+    // -----------------------------------------------------------------------
+    // default constructors
+
+    vec() { }
+    vec(const vec& rhs)  : base(rhs) { }
+    vec(const base& rhs) : base(rhs) { }
+
+    // -----------------------------------------------------------------------
+    // conversion constructors
+
+    vec(pTYPE rhs) {
+        for (size_t i=0 ; i<SIZE ; i++)
+            base::operator[](i) = rhs;
+    }
+
+    template < template<typename T, size_t S> class VEC, size_t S>
+    explicit vec(const VEC<TYPE, S>& rhs) {
+        helpers::doAssign(*this, rhs);
+    }
+
+    explicit vec(TYPE const* array) {
+        for (size_t i=0 ; i<SIZE ; i++)
+            base::operator[](i) = array[i];
+    }
+
+    // -----------------------------------------------------------------------
+    // Assignment
+
+    vec& operator = (const vec& rhs) {
+        base::operator=(rhs);
+        return *this;
+    }
+
+    vec& operator = (const base& rhs) {
+        base::operator=(rhs);
+        return *this;
+    }
+
+    vec& operator = (pTYPE rhs) {
+        for (size_t i=0 ; i<SIZE ; i++)
+            base::operator[](i) = rhs;
+        return *this;
+    }
+
+    template < template<typename T, size_t S> class VEC, size_t S>
+    vec& operator = (const VEC<TYPE, S>& rhs) {
+        return helpers::doAssign(*this, rhs);
+    }
+
+    // -----------------------------------------------------------------------
+    // operation-assignment
+
+    vec& operator += (const vec& rhs);
+    vec& operator -= (const vec& rhs);
+    vec& operator *= (pTYPE rhs);
+
+    // -----------------------------------------------------------------------
+    // non-member function declaration and definition
+    // NOTE: we declare the non-member function as friend inside the class
+    // so that they are known to the compiler when the class is instantiated.
+    // This helps the compiler doing template argument deduction when the
+    // passed types are not identical. Essentially this helps with
+    // type conversion so that you can multiply a vec<float> by an scalar int
+    // (for instance).
+
+    friend inline vec PURE operator + (const vec& lhs, const vec& rhs) {
+        return helpers::doAdd(lhs, rhs);
+    }
+    friend inline vec PURE operator - (const vec& lhs, const vec& rhs) {
+        return helpers::doSub(lhs, rhs);
+    }
+    friend inline vec PURE operator * (const vec& lhs, pTYPE v) {
+        return helpers::doMulScalar(lhs, v);
+    }
+    friend inline vec PURE operator * (pTYPE v, const vec& rhs) {
+        return helpers::doScalarMul(v, rhs);
+    }
+    friend inline TYPE PURE dot_product(const vec& lhs, const vec& rhs) {
+        return android::dot_product(lhs, rhs);
+    }
+};
+
+// -----------------------------------------------------------------------
+
+template <typename TYPE, size_t SIZE>
+vec<TYPE, SIZE>& vec<TYPE, SIZE>::operator += (const vec<TYPE, SIZE>& rhs) {
+    vec<TYPE, SIZE>& lhs(*this);
+    for (size_t i=0 ; i<SIZE ; i++)
+        lhs[i] += rhs[i];
+    return lhs;
+}
+
+template <typename TYPE, size_t SIZE>
+vec<TYPE, SIZE>& vec<TYPE, SIZE>::operator -= (const vec<TYPE, SIZE>& rhs) {
+    vec<TYPE, SIZE>& lhs(*this);
+    for (size_t i=0 ; i<SIZE ; i++)
+        lhs[i] -= rhs[i];
+    return lhs;
+}
+
+template <typename TYPE, size_t SIZE>
+vec<TYPE, SIZE>& vec<TYPE, SIZE>::operator *= (vec<TYPE, SIZE>::pTYPE rhs) {
+    vec<TYPE, SIZE>& lhs(*this);
+    for (size_t i=0 ; i<SIZE ; i++)
+        lhs[i] *= rhs;
+    return lhs;
+}
+
+// -----------------------------------------------------------------------
+
+typedef vec<float, 2> vec2_t;
+typedef vec<float, 3> vec3_t;
+typedef vec<float, 4> vec4_t;
+
+// -----------------------------------------------------------------------
+
+}; // namespace android
+
+#endif /* ANDROID_VEC_H */