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

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;
}
复制代码