Donate to e Foundation | Murena handsets with /e/OS | Own a part of Murena! Learn more

Commit 8f11b24a authored by Mathias Agopian's avatar Mathias Agopian
Browse files

add some comments

Change-Id: Ia2e2c9531715fc2bd5b51c4dc58389e01abfe7e6
parent 1b03149f
Loading
Loading
Loading
Loading
+14 −2
Original line number Diff line number Diff line
@@ -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));