diff options
| author | Mathias Agopian <mathias@google.com> | 2012-06-27 18:51:43 -0700 |
|---|---|---|
| committer | Mathias Agopian <mathias@google.com> | 2012-06-28 17:35:14 -0700 |
| commit | 8f11b24a729c9779d75e09df27967091dc6e27c7 (patch) | |
| tree | c555f252c4f7d3c03f3de348244a94eb60a8c5db /services/sensorservice | |
| parent | 1b03149f3533db04e72e088d3fdd09d0087ca594 (diff) | |
| download | frameworks_native-8f11b24a729c9779d75e09df27967091dc6e27c7.zip frameworks_native-8f11b24a729c9779d75e09df27967091dc6e27c7.tar.gz frameworks_native-8f11b24a729c9779d75e09df27967091dc6e27c7.tar.bz2 | |
add some comments
Change-Id: Ia2e2c9531715fc2bd5b51c4dc58389e01abfe7e6
Diffstat (limited to 'services/sensorservice')
| -rw-r--r-- | services/sensorservice/Fusion.cpp | 16 |
1 files changed, 14 insertions, 2 deletions
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 @@ void Fusion::predict(const vec3_t& w, float dT) { 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)); |