查看: 6174|回复: 2

[已解决] 卡尔曼陀螺仪、倾角传感器融合 C代码一段语句看不懂

本帖最后由 Ricky 于 2014-5-8 14:10 编辑

void stateUpdate(const float q_m){

        float q;
        float Pdot[4];

        /* Unbias our gyro */
        q = q_m - q_bias;
        /*
         * Compute the derivative of the covariance matrix
         * (equation 22-1)
         *        Pdot = A*P + P*A' + Q //？？？？？？？？？？？？
         *
         */
        Pdot[0] = Q_angle - P[0][1] - P[1][0];        /* 0,0 */
        Pdot[1] = - P[1][1];                        /* 0,1 */
        Pdot[2] = - P[1][1];                         /* 1,0 */
        Pdot[3] = Q_gyro;                        /* 1,1 */

        /* Store our unbias gyro estimate */
        rate = q;

        /*
         * Update our angle estimate
         * angle += angle_dot * dt
         *       += (gyro - gyro_bias) * dt
         *       += q * dt
         */
        angle += q * dt;
        /* Update the covariance matrix */
        P[0][0] += Pdot[0] * dt;
        P[0][1] += Pdot[1] * dt;
        P[1][0] += Pdot[2] * dt;
        P[1][1] += Pdot[3] * dt;
}
复制代码

上面一串问号开始到最后，不太懂。

卡尔曼陀螺仪、倾角传感器融合 C代码一段语句看不懂图1

卡尔曼公式 (原文件名:QQ截图未命名4.png)
不是应该向上边1.10式那样么，那样直接就能算出协方差啊，怎么程序中还要先算出Pdot = A*P + P*A' + Q ，再P[0][0] += Pdot[0] * dt;来更新协方差？angle += q * dt;中的dt是积分求角度，可P[0][0] += Pdot[0] * dt;不知道怎么理解了。P.S.从“(equation 22-1)” 看出这个程序应该出自哪篇文章，找了好久没找到，谁知道发一下。谢啦！

传感器, 陀螺仪, 卡尔