卡尔曼滤波的问题 求教 相信对大神们应该很简单了

遨游天际

以下网上的卡尔曼滤波的程序

static const float Q_angle=0.001, Q_gyro=0.003, R_angle=300, dt=0.005;

                        //注意：dt的取值为kalman滤波器采样时间;

static float P[2][2] = {{ 1, 0 },{ 0, 1 }};

static float Pdot[4] ={0,0,0,0};

static const char C_0 = 1;

static float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;

//-------------------------------------------------------

        CarAngle+=(gyro_m-q_bias) * dt;

        Pdot[0] = Q_angle - P[0][1] - P[1][0];

        Pdot[1] = -P[1][1];

        Pdot[2] = -P[1][1];

        Pdot[3] = Q_gyro;

        P[0][0] += Pdot[0] * dt;

        P[0][1] += Pdot[1] * dt;

        P[1][0] += Pdot[2] * dt;

        P[1][1] += Pdot[3] * dt;

        angle_err = angle_m - CarAngle;

        PCt_0 = C_0 * P[0][0];

        PCt_1 = C_0 * P[1][0];

        E = R_angle + C_0 * PCt_0;

        K_0 = PCt_0 / E;

        K_1 = PCt_1 / E;

        t_0 = PCt_0;

        t_1 = C_0 * P[0][1]；

        P[0][0] -= K_0 * t_0;

        P[0][1] -= K_0 * t_1;

        P[1][0] -= K_1 * t_0;

        P[1][1] -= K_1 * t_1;

        CarAngle += K_0 * angle_err;

        q_bias        += K_1 * angle_err;

        CarGyro = gyro_m-q_bias;

}

我不知道P[2][2]数组4个数是什么意思，P[0][0]应该存的是最优卡尔曼估计Xk|k时的协方差，那另外3个是什么啊。还有Pdot[0] = Q_angle - P[0][1] - P[1][0];

        Pdot[1] = -P[1][1];

        Pdot[2] = -P[1][1];

        Pdot[3] = Q_gyro;

        P[0][0] += Pdot[0] * dt;

        P[0][1] += Pdot[1] * dt;

        P[1][0] += Pdot[2] * dt;

        P[1][1] += Pdot[3] * dt;

以上这几步我就没看懂了，谁能详细解释一下

颠覆理论

恐怕你套那个五个公式是理解不了的，你得找出这样写的数学模型

遨游天际

嗯 现在我已经理解了，那个P矩阵就是最优估计的协方差，不过他那个迭代的式子是错的
Pdot[0] = Q_angle - P[0][1] - P[1][0];
        Pdot[1] = -P[1][1];
        Pdot[2] = -P[1][1];
        Pdot[3] = Q_gyro;
        P[0][0] += Pdot[0] * dt;
        P[0][1] += Pdot[1] * dt;
        P[1][0] += Pdot[2] * dt;
        P[1][1] += Pdot[3] * dt;，
我看了上海交大的报告才看明白

124694679

遨游天际 发表于 2013-5-2 21:06
嗯 现在我已经理解了，那个P矩阵就是最优估计的协方差，不过他那个迭代的式子是错的
Pdot[0] = Q_angle -  ...

大哥，那应该是什么样啊？我是新手求帮助！

huang91

不是错了，是理解的人不多。
*     Pdot = A*P + P*A' + Q  
*  
* Ais the Jacobian of Xdot with respect to the states:  
*  
*     A = [ d(angle_dot)/d(angle)      d(angle_dot)/d(gyro_bias) ]  
*          [d(gyro_bias_dot)/d(angle) d(gyro_bias_dot)/d(gyro_bias) ]  
*  
*        = [0 -1 ]  
*          [0   0 ]  

二百五

R_angle=300  ！！！！！！！