commit 8f11b24a729c9779d75e09df27967091dc6e27c7
author Mathias Agopian <mathias@google.com> Wed Jun 27 18:51:43 2012 -0700
committer Mathias Agopian <mathias@google.com> Thu Jun 28 17:35:14 2012 -0700
tree c555f252c4f7d3c03f3de348244a94eb60a8c5db
parent 1b03149f3533db04e72e088d3fdd09d0087ca594

add some comments

Change-Id: Ia2e2c9531715fc2bd5b51c4dc58389e01abfe7e6

services/sensorservice/Fusion.cpp

diff --git a/services/sensorservice/Fusion.cpp b/services/sensorservice/Fusion.cpp
index 2b6b793..21b2c1d 100644
--- a/services/sensorservice/Fusion.cpp
+++ b/services/sensorservice/Fusion.cpp
@@ -392,10 +392,22 @@
     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
-
+    // P(k+1) = Phi(k)*P(k)*Phi(k)' + G*Q(k)*G'
+    //
+    // G = | -I33    0 |
+    //     |    0  I33 |
+    //
     //  Phi = | Phi00 Phi10 |
     //        |   0     1   |
+    //
+    //  Phi00 =   I33
+    //          - [w]x   * sin(||w||*dt)/||w||
+    //          + [w]x^2 * (1-cos(||w||*dT))/||w||^2
+    //
+    //  Phi10 =   [w]x   * (1        - cos(||w||*dt))/||w||^2
+    //          - [w]x^2 * (||w||*dT - sin(||w||*dt))/||w||^3
+    //          - I33*dT
+
     const mat33_t I33(1);
     const mat33_t I33dT(dT);
     const mat33_t wx(crossMatrix(we, 0));