卡尔曼滤波

xiongcaifei

void KalmanUpdate(float angle_m,float gyro_m)                        //gyro_m:gyro_measure

{

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][0] += Q_angle -P[0][1]dt - P[1][0]dt-P[1][1]dt*dt 吗？

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

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

        P[1][1] += Pdot[3] * dt;                不是应该    P[1][1] += Pdot[3]；吗？

        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;

}

求解答。

除却巫山不是云

xiongcaifei 发表于 2013-1-23 15:54
那可能是这个+P[1][1]dt*dt 得到的数值太小了。可以忽略吗？？

应该不是吧。忽略了也不一样阿。忽略后是：P[0][0] += Q_angle -P[0][1]dt - P[1][0]dt按照代码是P[0][0] += Q_angle*dt -P[0][1]dt - P[1][0]dt
我算出来后跟你的公式一模一样，疑问也是一模一样！
Pdot = A*P + P*A' + Q----中的Q是矩阵{ {Q_angle, 0},{0,Q_gyro}}，A = [ 0 -1 ]
                                                                                                  [ 0  0 ]
代进去算出来后就是：
        Pdot[0] = Q_angle - P[0][1] - P[1][0];        
        Pdot[1] = - P[1][1];                        
        Pdot[2] = - P[1][1];                                 
        Pdot[3] = Q_gyro;   
Pdot = A*P + P*A' + Q是计算协方差矩阵的导数，怎么算出来的？不知道你弄懂了没有，求解释啊。。、
苦逼的孩子回不了家还一大堆问题。╮(╯▽╰)╭。。

dlyt03

学习下

xiongcaifei

那可能是这个+P[1][1]dt*dt 得到的数值太小了。可以忽略吗？？

dlyt03

看看

xiongcaifei

至少这个+P[1][1]dt*dt 也应该加。我就得。求牛人解决

xiongcaifei

求高手，做出来的人解答啊。

qurrer

正在研究呢