use quaternions instead of MRPs

also use correct time propagation equation
disable the fused sensors when gyro is not present since
they were unusable in practice.

Change-Id: Iad797425784e67dc6c5690e97c71c583418cc5b5

16 files changed, 370 insertions, 552 deletions

diff --git a/services/sensorservice/Android.mk b/services/sensorservice/Android.mk
index 57a3b15..ba3e6e5 100644
--- a/services/sensorservice/Android.mk
+++ b/services/sensorservice/Android.mk
@@ -8,7 +8,6 @@ LOCAL_SRC_FILES:= \
     LinearAccelerationSensor.cpp \
     OrientationSensor.cpp \
     RotationVectorSensor.cpp \
-    SecondOrderLowPassFilter.cpp \
     SensorDevice.cpp \
     SensorFusion.cpp \
     SensorInterface.cpp \
diff --git a/services/sensorservice/CorrectedGyroSensor.cpp b/services/sensorservice/CorrectedGyroSensor.cpp
index 9b75b70..1857443 100644
--- a/services/sensorservice/CorrectedGyroSensor.cpp
+++ b/services/sensorservice/CorrectedGyroSensor.cpp
@@ -45,7 +45,7 @@ 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());
+        const vec3_t bias(mSensorFusion.getGyroBias());
         *outEvent = event;
         outEvent->data[0] -= bias.x;
         outEvent->data[1] -= bias.y;
diff --git a/services/sensorservice/Fusion.cpp b/services/sensorservice/Fusion.cpp
index 56ac9f9..b5f97e0 100644
--- a/services/sensorservice/Fusion.cpp
+++ b/services/sensorservice/Fusion.cpp
@@ -24,15 +24,14 @@ namespace android {
 
 // -----------------------------------------------------------------------
 
-template <typename TYPE>
-static inline TYPE sqr(TYPE x) {
-    return x*x;
-}
+static const float gyroSTDEV = 3.16e-4; // rad/s^3/2
+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 = 3.16e-5; // rad/s^1/2 (guessed)
 
-template <typename T>
-static inline T clamp(T v) {
-    return v < 0 ? 0 : v;
-}
+static const float FREE_FALL_THRESHOLD = 0.981f;
+
+// -----------------------------------------------------------------------
 
 template <typename TYPE, size_t C, size_t R>
 static mat<TYPE, R, R> scaleCovariance(
@@ -71,33 +70,6 @@ static mat<TYPE, 3, 3> crossMatrix(const vec<TYPE, 3>& p, OTHER_TYPE diag) {
     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 {
@@ -128,11 +100,8 @@ public:
 // -----------------------------------------------------------------------
 
 Fusion::Fusion() {
-    // process noise covariance matrix
-    const float w1 = gyroSTDEV;
-    const float w2 = biasSTDEV;
-    Q[0] = w1*w1;
-    Q[1] = w2*w2;
+    Phi[0][1] = 0;
+    Phi[1][1] = 1;
 
     Ba.x = 0;
     Ba.y = 0;
@@ -146,25 +115,46 @@ Fusion::Fusion() {
 }
 
 void Fusion::init() {
-    // initial estimate: E{ x(t0) }
-    x = 0;
-
-    // initial covariance: Var{ x(t0) }
-    P = 0;
-
     mInitState = 0;
+    mGyroRate = 0;
     mCount[0] = 0;
     mCount[1] = 0;
     mCount[2] = 0;
     mData = 0;
 }
 
+void Fusion::initFusion(const vec4_t& q, float dT)
+{
+    // initial estimate: E{ x(t0) }
+    x0 = q;
+    x1 = 0;
+
+    // process noise covariance matrix
+    //  G = | -1 0 |
+    //      |  0 1 |
+
+    const float v = gyroSTDEV;
+    const float u = biasSTDEV;
+    const float q00 = v*v*dT + 0.33333f*(dT*dT*dT)*u*u;
+    const float q10 =              0.5f*(dT*dT)   *u*u;
+    const float q01 = q10;
+    const float q11 = u*u*dT;
+    GQGt[0][0] =  q00;
+    GQGt[1][0] = -q10;
+    GQGt[0][1] = -q01;
+    GQGt[1][1] =  q11;
+
+
+    // initial covariance: Var{ x(t0) }
+    P = 0;
+}
+
 bool Fusion::hasEstimate() const {
     return (mInitState == (MAG|ACC|GYRO));
 }
 
-bool Fusion::checkInitComplete(int what, const vec3_t& d) {
-    if (mInitState == (MAG|ACC|GYRO))
+bool Fusion::checkInitComplete(int what, const vec3_t& d, float dT) {
+    if (hasEstimate())
         return true;
 
     if (what == ACC) {
@@ -176,7 +166,8 @@ bool Fusion::checkInitComplete(int what, const vec3_t& d) {
         mCount[1]++;
         mInitState |= MAG;
     } else if (what == GYRO) {
-        mData[2] += d;
+        mGyroRate = dT;
+        mData[2] += d*dT;
         mCount[2]++;
         if (mCount[2] == 64) {
             // 64 samples is good enough to estimate the gyro drift and
@@ -199,37 +190,29 @@ bool Fusion::checkInitComplete(int what, const vec3_t& d) {
         east *= 1/length(east);
         vec3_t north(cross_product(up, east));
         R << east << north << up;
-        x[0] = matrixToMRPs(R);
+        const vec4_t q = matrixToQuat(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;
+        initFusion(q, mGyroRate);
     }
 
     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))
+    if (!checkInitComplete(GYRO, w, dT))
         return;
 
-    predict(wdT);
+    predict(w, dT);
 }
 
 status_t Fusion::handleAcc(const vec3_t& a) {
-    if (length(a) < 0.981f)
+    // ignore acceleration data if we're close to free-fall
+    if (length(a) < FREE_FALL_THRESHOLD)
         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;
@@ -251,20 +234,6 @@ status_t Fusion::handleMag(const vec3_t& m) {
     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;
 }
@@ -273,7 +242,7 @@ bool Fusion::checkState(const vec3_t& v) {
     if (isnanf(length(v))) {
         LOGW("9-axis fusion diverged. reseting state.");
         P = 0;
-        x[1] = 0;
+        x1 = 0;
         mInitState = 0;
         mCount[0] = 0;
         mCount[1] = 0;
@@ -284,145 +253,89 @@ bool Fusion::checkState(const vec3_t& v) {
     return true;
 }
 
-vec3_t Fusion::getAttitude() const {
-    return x[0];
+vec4_t Fusion::getAttitude() const {
+    return x0;
 }
 
 vec3_t Fusion::getBias() const {
-    return x[1];
+    return x1;
 }
 
 mat33_t Fusion::getRotationMatrix() const {
-    return MRPsToMatrix(x[0]);
+    return quatToMatrix(x0);
 }
 
-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);
+mat34_t Fusion::getF(const vec4_t& q) {
+    mat34_t F;
+    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;
 }
 
-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::predict(const vec3_t& w, float dT) {
+    const vec4_t q  = x0;
+    const vec3_t b  = x1;
+    const vec3_t we = w - b;
+    const vec4_t dq = getF(q)*((0.5f*dT)*we);
+    x0 = normalize_quat(q + dq);
+
+    // P(k+1) = F*P(k)*Ft + G*Q*Gt
+
+    //  Phi = | Phi00 Phi10 |
+    //        |   0     1   |
+    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 ilwe = 1/length(we);
+    const float k0 = (1-cosf(lwedT))*(ilwe*ilwe);
+    const float k1 = sinf(lwedT);
+
+    Phi[0][0] = I33 - wx*(k1*ilwe) + wx2*k0;
+    Phi[1][0] = wx*k0 - I33dT - wx2*(ilwe*ilwe*ilwe)*(lwedT-k1);
+
+    P = Phi*P*transpose(Phi) + GQGt;
 }
 
 void Fusion::update(const vec3_t& z, const vec3_t& Bi, float sigma) {
-    const vec3_t p(x[0]);
+    vec4_t q(x0);
     // measured vector in body space: h(p) = A(p)*Bi
-    const mat33_t A(MRPsToMatrix(p));
+    const mat33_t A(quatToMatrix(q));
     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));
+    const mat33_t L(crossMatrix(Bb, 0));
 
-    // update...
+    // 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);
-
-    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;
-
+    // update...
     // 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];
+    P[0][1] = transpose(P[1][0]);
+
+    const vec3_t e(z - Bb);
+    const vec3_t dq(K[0]*e);
+    const vec3_t db(K[1]*e);
+
+    q += getF(q)*(0.5f*dq);
+    x0 = normalize_quat(q);
+    x1 += db;
 }
 
 // -----------------------------------------------------------------------
diff --git a/services/sensorservice/Fusion.h b/services/sensorservice/Fusion.h
index 571a415..556944b 100644
--- a/services/sensorservice/Fusion.h
+++ b/services/sensorservice/Fusion.h
@@ -19,42 +19,39 @@
 
 #include <utils/Errors.h>
 
-#include "vec.h"
+#include "quat.h"
 #include "mat.h"
+#include "vec.h"
 
 namespace android {
 
+typedef mat<float, 3, 4> mat34_t;
+
 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;
+    quat_t  x0;
+    vec3_t  x1;
 
     /*
      * 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 |
+     *     | P01  P11 |   | P10t P11 |
      *
      * 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.
+     * stores P01.
      */
     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
+     * the process noise covariance matrix
      */
-    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)
+    mat<mat33_t, 2, 2> GQGt;
 
 public:
     Fusion();
@@ -62,23 +59,25 @@ public:
     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;
+    vec4_t getAttitude() const;
     vec3_t getBias() const;
     mat33_t getRotationMatrix() const;
     bool hasEstimate() const;
 
 private:
+    mat<mat33_t, 2, 2> Phi;
     vec3_t Ba, Bm;
     uint32_t mInitState;
+    float mGyroRate;
     vec<vec3_t, 3> mData;
     size_t mCount[3];
     enum { ACC=0x1, MAG=0x2, GYRO=0x4 };
-    bool checkInitComplete(int, const vec3_t&);
+    bool checkInitComplete(int, const vec3_t& w, float d = 0);
+    void initFusion(const vec4_t& q0, float dT);
     bool checkState(const vec3_t& v);
-    void predict(const vec3_t& w);
+    void predict(const vec3_t& w, float dT);
     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);
+    static mat34_t getF(const vec4_t& p);
 };
 
 }; // namespace android
diff --git a/services/sensorservice/GravitySensor.cpp b/services/sensorservice/GravitySensor.cpp
index 541fad2..c57715f 100644
--- a/services/sensorservice/GravitySensor.cpp
+++ b/services/sensorservice/GravitySensor.cpp
@@ -31,10 +31,7 @@ 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)
+      mSensorFusion(SensorFusion::getInstance())
 {
     for (size_t i=0 ; i<count ; i++) {
         if (list[i].type == SENSOR_TYPE_ACCELEROMETER) {
@@ -50,30 +47,14 @@ bool GravitySensor::process(sensors_event_t* outEvent,
     const static double NS2S = 1.0 / 1000000000.0;
     if (event.type == SENSOR_TYPE_ACCELEROMETER) {
         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 {
-            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;
-        }
+        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;
+
         *outEvent = event;
         outEvent->data[0] = g.x;
         outEvent->data[1] = g.y;
@@ -86,42 +67,24 @@ bool GravitySensor::process(sensors_event_t* outEvent,
 }
 
 status_t GravitySensor::activate(void* ident, bool enabled) {
-    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;
+    return mSensorFusion.activate(this, enabled);
 }
 
-status_t GravitySensor::setDelay(void* ident, int handle, int64_t ns)
-{
-    if (mSensorFusion.hasGyro()) {
-        return mSensorFusion.setDelay(this, ns);
-    } else {
-        return mSensorDevice.setDelay(this, mAccelerometer.getHandle(), ns);
-    }
+status_t GravitySensor::setDelay(void* ident, int handle, int64_t ns) {
+    return mSensorFusion.setDelay(this, ns);
 }
 
 Sensor GravitySensor::getSensor() const {
     sensor_t hwSensor;
     hwSensor.name       = "Gravity Sensor";
     hwSensor.vendor     = "Google Inc.";
-    hwSensor.version    = mSensorFusion.hasGyro() ? 3 : 2;
+    hwSensor.version    = 3;
     hwSensor.handle     = '_grv';
     hwSensor.type       = SENSOR_TYPE_GRAVITY;
     hwSensor.maxRange   = GRAVITY_EARTH * 2;
     hwSensor.resolution = mAccelerometer.getResolution();
-    hwSensor.power      = mSensorFusion.hasGyro() ?
-            mSensorFusion.getPowerUsage() : mAccelerometer.getPowerUsage();
-    hwSensor.minDelay   = mSensorFusion.hasGyro() ?
-            mSensorFusion.getMinDelay() : mAccelerometer.getMinDelay();
+    hwSensor.power      = mSensorFusion.getPowerUsage();
+    hwSensor.minDelay   = mSensorFusion.getMinDelay();
     Sensor sensor(&hwSensor);
     return sensor;
 }
diff --git a/services/sensorservice/GravitySensor.h b/services/sensorservice/GravitySensor.h
index 0ca3a3c..ac177c4 100644
--- a/services/sensorservice/GravitySensor.h
+++ b/services/sensorservice/GravitySensor.h
@@ -23,7 +23,6 @@
 #include <gui/Sensor.h>
 
 #include "SensorInterface.h"
-#include "SecondOrderLowPassFilter.h"
 
 // ---------------------------------------------------------------------------
 namespace android {
@@ -36,10 +35,6 @@ class GravitySensor : public SensorInterface {
     SensorDevice& mSensorDevice;
     SensorFusion& mSensorFusion;
     Sensor mAccelerometer;
-    double mAccTime;
-
-    SecondOrderLowPassFilter mLowPass;
-    CascadedBiquadFilter<float> mX, mY, mZ;
 
 public:
     GravitySensor(sensor_t const* list, size_t count);
diff --git a/services/sensorservice/OrientationSensor.cpp b/services/sensorservice/OrientationSensor.cpp
index c9e5080..037adaa 100644
--- a/services/sensorservice/OrientationSensor.cpp
+++ b/services/sensorservice/OrientationSensor.cpp
@@ -50,9 +50,10 @@ bool OrientationSensor::process(sensors_event_t* outEvent,
                 g[0] += 360;
 
             *outEvent = event;
-            outEvent->data[0] = g.x;
-            outEvent->data[1] = g.y;
-            outEvent->data[2] = g.z;
+            outEvent->orientation.azimuth = g.x;
+            outEvent->orientation.pitch   = g.y;
+            outEvent->orientation.roll    = g.z;
+            outEvent->orientation.status  = SENSOR_STATUS_ACCURACY_HIGH;
             outEvent->sensor = '_ypr';
             outEvent->type = SENSOR_TYPE_ORIENTATION;
             return true;
diff --git a/services/sensorservice/RotationVectorSensor.cpp b/services/sensorservice/RotationVectorSensor.cpp
index cba89c9..5ea9568 100644
--- a/services/sensorservice/RotationVectorSensor.cpp
+++ b/services/sensorservice/RotationVectorSensor.cpp
@@ -27,11 +27,6 @@
 namespace android {
 // ---------------------------------------------------------------------------
 
-template <typename T>
-static inline T clamp(T v) {
-    return v < 0 ? 0 : v;
-}
-
 RotationVectorSensor::RotationVectorSensor()
     : mSensorDevice(SensorDevice::getInstance()),
       mSensorFusion(SensorFusion::getInstance())
@@ -43,29 +38,12 @@ bool RotationVectorSensor::process(sensors_event_t* outEvent,
 {
     if (event.type == SENSOR_TYPE_ACCELEROMETER) {
         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.
-
+            const vec4_t q(mSensorFusion.getAttitude());
             *outEvent = event;
-            outEvent->data[0] = qx;
-            outEvent->data[1] = qy;
-            outEvent->data[2] = qz;
-            outEvent->data[3] = qw;
+            outEvent->data[0] = q.x;
+            outEvent->data[1] = q.y;
+            outEvent->data[2] = q.z;
+            outEvent->data[3] = q.w;
             outEvent->sensor = '_rov';
             outEvent->type = SENSOR_TYPE_ROTATION_VECTOR;
             return true;
@@ -86,7 +64,7 @@ Sensor RotationVectorSensor::getSensor() const {
     sensor_t hwSensor;
     hwSensor.name       = "Rotation Vector Sensor";
     hwSensor.vendor     = "Google Inc.";
-    hwSensor.version    = mSensorFusion.hasGyro() ? 3 : 2;
+    hwSensor.version    = 3;
     hwSensor.handle     = '_rov';
     hwSensor.type       = SENSOR_TYPE_ROTATION_VECTOR;
     hwSensor.maxRange   = 1;
@@ -98,5 +76,54 @@ Sensor RotationVectorSensor::getSensor() const {
 }
 
 // ---------------------------------------------------------------------------
+
+GyroDriftSensor::GyroDriftSensor()
+    : mSensorDevice(SensorDevice::getInstance()),
+      mSensorFusion(SensorFusion::getInstance())
+{
+}
+
+bool GyroDriftSensor::process(sensors_event_t* outEvent,
+        const sensors_event_t& event)
+{
+    if (event.type == SENSOR_TYPE_ACCELEROMETER) {
+        if (mSensorFusion.hasEstimate()) {
+            const vec3_t b(mSensorFusion.getGyroBias());
+            *outEvent = event;
+            outEvent->data[0] = b.x;
+            outEvent->data[1] = b.y;
+            outEvent->data[2] = b.z;
+            outEvent->sensor = '_gbs';
+            outEvent->type = SENSOR_TYPE_ACCELEROMETER;
+            return true;
+        }
+    }
+    return false;
+}
+
+status_t GyroDriftSensor::activate(void* ident, bool enabled) {
+    return mSensorFusion.activate(this, enabled);
+}
+
+status_t GyroDriftSensor::setDelay(void* ident, int handle, int64_t ns) {
+    return mSensorFusion.setDelay(this, ns);
+}
+
+Sensor GyroDriftSensor::getSensor() const {
+    sensor_t hwSensor;
+    hwSensor.name       = "Gyroscope Bias (debug)";
+    hwSensor.vendor     = "Google Inc.";
+    hwSensor.version    = 1;
+    hwSensor.handle     = '_gbs';
+    hwSensor.type       = SENSOR_TYPE_ACCELEROMETER;
+    hwSensor.maxRange   = 1;
+    hwSensor.resolution = 1.0f / (1<<24);
+    hwSensor.power      = mSensorFusion.getPowerUsage();
+    hwSensor.minDelay   = mSensorFusion.getMinDelay();
+    Sensor sensor(&hwSensor);
+    return sensor;
+}
+
+// ---------------------------------------------------------------------------
 }; // namespace android
 
diff --git a/services/sensorservice/RotationVectorSensor.h b/services/sensorservice/RotationVectorSensor.h
index ac76487..bb97fe1 100644
--- a/services/sensorservice/RotationVectorSensor.h
+++ b/services/sensorservice/RotationVectorSensor.h
@@ -24,7 +24,6 @@
 
 #include "SensorDevice.h"
 #include "SensorInterface.h"
-#include "SecondOrderLowPassFilter.h"
 
 #include "Fusion.h"
 #include "SensorFusion.h"
@@ -47,6 +46,20 @@ public:
     virtual bool isVirtual() const { return true; }
 };
 
+class GyroDriftSensor : public SensorInterface {
+    SensorDevice& mSensorDevice;
+    SensorFusion& mSensorFusion;
+
+public:
+    GyroDriftSensor();
+    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
 
diff --git a/services/sensorservice/SecondOrderLowPassFilter.cpp b/services/sensorservice/SecondOrderLowPassFilter.cpp
deleted file mode 100644
index c76dd4c..0000000
--- a/services/sensorservice/SecondOrderLowPassFilter.cpp
+++ /dev/null
@@ -1,103 +0,0 @@
-/*
- * Copyright (C) 2010 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 <sys/types.h>
-#include <math.h>
-
-#include <cutils/log.h>
-
-#include "SecondOrderLowPassFilter.h"
-#include "vec.h"
-
-// ---------------------------------------------------------------------------
-
-namespace android {
-// ---------------------------------------------------------------------------
-
-SecondOrderLowPassFilter::SecondOrderLowPassFilter(float Q, float fc)
-    : iQ(1.0f / Q), fc(fc)
-{
-}
-
-void SecondOrderLowPassFilter::setSamplingPeriod(float dT)
-{
-    K = tanf(float(M_PI) * fc * dT);
-    iD = 1.0f / (K*K + K*iQ + 1);
-    a0 = K*K*iD;
-    a1 = 2.0f * a0;
-    b1 = 2.0f*(K*K - 1)*iD;
-    b2 = (K*K - K*iQ + 1)*iD;
-}
-
-// ---------------------------------------------------------------------------
-
-template<typename T>
-BiquadFilter<T>::BiquadFilter(const SecondOrderLowPassFilter& s)
-    : s(s)
-{
-}
-
-template<typename T>
-T BiquadFilter<T>::init(const T& x)
-{
-    x1 = x2 = x;
-    y1 = y2 = x;
-    return x;
-}
-
-template<typename T>
-T BiquadFilter<T>::operator()(const T& x)
-{
-    T y = (x + x2)*s.a0 + x1*s.a1 - y1*s.b1 - y2*s.b2;
-    x2 = x1;
-    y2 = y1;
-    x1 = x;
-    y1 = y;
-    return y;
-}
-
-// ---------------------------------------------------------------------------
-
-template<typename T>
-CascadedBiquadFilter<T>::CascadedBiquadFilter(const SecondOrderLowPassFilter& s)
-    : mA(s), mB(s)
-{
-}
-
-template<typename T>
-T CascadedBiquadFilter<T>::init(const T& x)
-{
-    mA.init(x);
-    mB.init(x);
-    return 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
deleted file mode 100644
index 0cc2446..0000000
--- a/services/sensorservice/SecondOrderLowPassFilter.h
+++ /dev/null
@@ -1,77 +0,0 @@
-/*
- * Copyright (C) 2010 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_SECOND_ORDER_LOW_PASS_FILTER_H
-#define ANDROID_SECOND_ORDER_LOW_PASS_FILTER_H
-
-#include <stdint.h>
-#include <sys/types.h>
-
-// ---------------------------------------------------------------------------
-
-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;
-    float a0, a1;
-    float b1, b2;
-public:
-    SecondOrderLowPassFilter(float Q, float fc);
-    void setSamplingPeriod(float dT);
-};
-
-/*
- * Implements a Biquad IIR filter
- */
-template<typename T>
-class BiquadFilter {
-    T x1, x2;
-    T y1, y2;
-    const SecondOrderLowPassFilter& s;
-public:
-    BiquadFilter(const SecondOrderLowPassFilter& s);
-    T init(const T& in);
-    T operator()(const T& in);
-};
-
-/*
- * Two cascaded biquad IIR filters
- * (4-poles IIR)
- */
-template<typename T>
-class CascadedBiquadFilter {
-    BiquadFilter<T> mA;
-    BiquadFilter<T> mB;
-public:
-    CascadedBiquadFilter(const SecondOrderLowPassFilter& s);
-    T init(const T& in);
-    T operator()(const T& in);
-};
-
-// ---------------------------------------------------------------------------
-}; // namespace android
-
-#endif // ANDROID_SECOND_ORDER_LOW_PASS_FILTER_H
diff --git a/services/sensorservice/SensorFusion.cpp b/services/sensorservice/SensorFusion.cpp
index d4226ec..4ec0c8c 100644
--- a/services/sensorservice/SensorFusion.cpp
+++ b/services/sensorservice/SensorFusion.cpp
@@ -25,9 +25,7 @@ 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)
+      mEnabled(false), mGyroTime(0)
 {
     sensor_t const* list;
     size_t count = mSensorDevice.getSensorList(&list);
@@ -42,55 +40,32 @@ SensorFusion::SensorFusion()
             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;
+            mGyroRate = 200;
+            mTargetDelayNs = 1000000000LL/mGyroRate;
         }
     }
     mFusion.init();
-    mAccData.init(vec3_t(0.0f));
 }
 
 void SensorFusion::process(const sensors_event_t& event) {
-
-    if (event.type == SENSOR_TYPE_GYROSCOPE && mHasGyro) {
+    if (event.type == SENSOR_TYPE_GYROSCOPE) {
         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);
+            if (freq >= 100 && freq<1000) { // filter values obviously wrong
+                const float alpha = 1 / (1 + dT); // 1s time-constant
+                mGyroRate = freq + (mGyroRate - freq)*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);
-            }
-        }
+        mFusion.handleMag(mag);
     } 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;
-            }
-        }
+        mFusion.handleAcc(acc);
+        mAttitude = mFusion.getAttitude();
     }
 }
 
@@ -116,40 +91,31 @@ status_t SensorFusion::activate(void* ident, bool enabled) {
 
     mSensorDevice.activate(ident, mAcc.getHandle(), enabled);
     mSensorDevice.activate(ident, mMag.getHandle(), enabled);
-    if (mHasGyro) {
-        mSensorDevice.activate(ident, mGyro.getHandle(), enabled);
-    }
+    mSensorDevice.activate(ident, mGyro.getHandle(), enabled);
 
     const bool newState = mClients.size() != 0;
     if (newState != mEnabled) {
         mEnabled = newState;
         if (newState) {
             mFusion.init();
+            mGyroTime = 0;
         }
     }
     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);
-    }
+    mSensorDevice.setDelay(ident, mAcc.getHandle(), ns);
+    mSensorDevice.setDelay(ident, mMag.getHandle(), ms2ns(20));
+    mSensorDevice.setDelay(ident, mGyro.getHandle(), mTargetDelayNs);
     return NO_ERROR;
 }
 
 
 float SensorFusion::getPowerUsage() const {
-    float power = mAcc.getPowerUsage() + mMag.getPowerUsage();
-    if (mHasGyro) {
-        power += mGyro.getPowerUsage();
-    }
+    float power =   mAcc.getPowerUsage() +
+                    mMag.getPowerUsage() +
+                    mGyro.getPowerUsage();
     return power;
 }
 
@@ -159,17 +125,17 @@ int32_t SensorFusion::getMinDelay() const {
 
 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",
+    snprintf(buffer, SIZE, "9-axis fusion %s (%d clients), gyro-rate=%7.2fHz, "
+            "q=< %g, %g, %g, %g > (%g), "
+            "b=< %g, %g, %g >\n",
             mEnabled ? "enabled" : "disabled",
             mClients.size(),
             mGyroRate,
             fusion.getAttitude().x,
             fusion.getAttitude().y,
             fusion.getAttitude().z,
-            dot_product(fusion.getAttitude(), fusion.getAttitude()),
+            fusion.getAttitude().w,
+            length(fusion.getAttitude()),
             fusion.getBias().x,
             fusion.getBias().y,
             fusion.getBias().z);
diff --git a/services/sensorservice/SensorFusion.h b/services/sensorservice/SensorFusion.h
index c7eab12..4c99bcb 100644
--- a/services/sensorservice/SensorFusion.h
+++ b/services/sensorservice/SensorFusion.h
@@ -27,7 +27,6 @@
 #include <gui/Sensor.h>
 
 #include "Fusion.h"
-#include "SecondOrderLowPassFilter.h"
 
 // ---------------------------------------------------------------------------
 
@@ -45,15 +44,10 @@ class SensorFusion : public Singleton<SensorFusion> {
     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;
+    vec4_t mAttitude;
     SortedVector<void*> mClients;
 
     SensorFusion();
@@ -62,9 +56,9 @@ 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; }
+    bool hasEstimate() const { return mFusion.hasEstimate(); }
+    mat33_t getRotationMatrix() const { return mFusion.getRotationMatrix(); }
+    vec4_t getAttitude() const { return mAttitude; }
     vec3_t getGyroBias() const { return mFusion.getBias(); }
     float getEstimatedRate() const { return mGyroRate; }
 
diff --git a/services/sensorservice/SensorService.cpp b/services/sensorservice/SensorService.cpp
index 5b86d10..d1b10f7 100644
--- a/services/sensorservice/SensorService.cpp
+++ b/services/sensorservice/SensorService.cpp
@@ -18,6 +18,8 @@
 #include <math.h>
 #include <sys/types.h>
 
+#include <cutils/properties.h>
+
 #include <utils/SortedVector.h>
 #include <utils/KeyedVector.h>
 #include <utils/threads.h>
@@ -46,6 +48,16 @@
 namespace android {
 // ---------------------------------------------------------------------------
 
+/*
+ * Notes:
+ *
+ * - what about a gyro-corrected magnetic-field sensor?
+ * - option to "hide" the HAL sensors
+ * - run mag sensor from time to time to force calibration
+ * - gravity sensor length is wrong (=> drift in linear-acc sensor)
+ *
+ */
+
 SensorService::SensorService()
     : mDump("android.permission.DUMP"),
       mInitCheck(NO_INIT)
@@ -59,6 +71,7 @@ void SensorService::onFirstRef()
     SensorDevice& dev(SensorDevice::getInstance());
 
     if (dev.initCheck() == NO_ERROR) {
+        bool hasGyro = false;
         uint32_t virtualSensorsNeeds =
                 (1<<SENSOR_TYPE_GRAVITY) |
                 (1<<SENSOR_TYPE_LINEAR_ACCELERATION) |
@@ -69,6 +82,9 @@ void SensorService::onFirstRef()
         for (int i=0 ; i<count ; i++) {
             registerSensor( new HardwareSensor(list[i]) );
             switch (list[i].type) {
+                case SENSOR_TYPE_GYROSCOPE:
+                    hasGyro = true;
+                    break;
                 case SENSOR_TYPE_GRAVITY:
                 case SENSOR_TYPE_LINEAR_ACCELERATION:
                 case SENSOR_TYPE_ROTATION_VECTOR:
@@ -82,21 +98,26 @@ void SensorService::onFirstRef()
         // 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).
+        if (hasGyro) {
+            // 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) );
+            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
+            // these are optional
             registerVirtualSensor( new OrientationSensor() );
             registerVirtualSensor( new CorrectedGyroSensor(list, count) );
+
+            // virtual debugging sensors...
+            char value[PROPERTY_VALUE_MAX];
+            property_get("debug.sensors", value, "0");
+            if (atoi(value)) {
+                registerVirtualSensor( new GyroDriftSensor() );
+            }
         }
 
         run("SensorService", PRIORITY_URGENT_DISPLAY);
diff --git a/services/sensorservice/quat.h b/services/sensorservice/quat.h
new file mode 100644
index 0000000..fea1afe
--- /dev/null
+++ b/services/sensorservice/quat.h
@@ -0,0 +1,98 @@
+/*
+ * 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_QUAT_H
+#define ANDROID_QUAT_H
+
+#include <math.h>
+
+#include "vec.h"
+#include "mat.h"
+
+// -----------------------------------------------------------------------
+namespace android {
+// -----------------------------------------------------------------------
+
+template <typename TYPE>
+mat<TYPE, 3, 3> quatToMatrix(const vec<TYPE, 4>& q) {
+    mat<TYPE, 3, 3> R;
+    TYPE q0(q.w);
+    TYPE q1(q.x);
+    TYPE q2(q.y);
+    TYPE q3(q.z);
+    TYPE sq_q1 = 2 * q1 * q1;
+    TYPE sq_q2 = 2 * q2 * q2;
+    TYPE sq_q3 = 2 * q3 * q3;
+    TYPE q1_q2 = 2 * q1 * q2;
+    TYPE q3_q0 = 2 * q3 * q0;
+    TYPE q1_q3 = 2 * q1 * q3;
+    TYPE q2_q0 = 2 * q2 * q0;
+    TYPE q2_q3 = 2 * q2 * q3;
+    TYPE q1_q0 = 2 * q1 * q0;
+    R[0][0] = 1 - sq_q2 - sq_q3;
+    R[0][1] = q1_q2 - q3_q0;
+    R[0][2] = q1_q3 + q2_q0;
+    R[1][0] = q1_q2 + q3_q0;
+    R[1][1] = 1 - sq_q1 - sq_q3;
+    R[1][2] = q2_q3 - q1_q0;
+    R[2][0] = q1_q3 - q2_q0;
+    R[2][1] = q2_q3 + q1_q0;
+    R[2][2] = 1 - sq_q1 - sq_q2;
+    return R;
+}
+
+template <typename TYPE>
+vec<TYPE, 4> matrixToQuat(const mat<TYPE, 3, 3>& R) {
+    // matrix to quaternion
+
+    struct {
+        inline TYPE operator()(TYPE v) {
+            return v < 0 ? 0 : v;
+        }
+    } clamp;
+
+    vec<TYPE, 4> q;
+    const float Hx = R[0].x;
+    const float My = R[1].y;
+    const float Az = R[2].z;
+    q.x = sqrtf( clamp( Hx - My - Az + 1) * 0.25f );
+    q.y = sqrtf( clamp(-Hx + My - Az + 1) * 0.25f );
+    q.z = sqrtf( clamp(-Hx - My + Az + 1) * 0.25f );
+    q.w = sqrtf( clamp( Hx + My + Az + 1) * 0.25f );
+    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);
+    // guaranteed to be unit-quaternion
+    return q;
+}
+
+template <typename TYPE>
+vec<TYPE, 4> normalize_quat(const vec<TYPE, 4>& q) {
+    vec<TYPE, 4> r(q);
+    if (r.w < 0) {
+        r = -r;
+    }
+    return normalize(r);
+}
+
+// -----------------------------------------------------------------------
+
+typedef vec4_t quat_t;
+
+// -----------------------------------------------------------------------
+}; // namespace android
+
+#endif /* ANDROID_QUAT_H */
diff --git a/services/sensorservice/vec.h b/services/sensorservice/vec.h
index 736ff37..f74ccc5 100644
--- a/services/sensorservice/vec.h
+++ b/services/sensorservice/vec.h
@@ -208,6 +208,15 @@ TYPE PURE length(const V<TYPE, SIZE>& v) {
 }
 
 template <
+    template<typename T, size_t S> class V,
+    typename TYPE,
+    size_t SIZE
+>
+V<TYPE, SIZE> PURE normalize(const V<TYPE, SIZE>& v) {
+    return v * (1/length(v));
+}
+
+template <
     template<typename T, size_t S> class VLHS,
     template<typename T, size_t S> class VRHS,
     typename TYPE