智能车制作

标题: 卡尔曼滤波 [打印本页]

作者: xiongcaifei 时间: 2013-1-22 14:23
标题: 卡尔曼滤波

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;

}

求解答。

作者: qurrer 时间: 2013-1-22 14:31
正在研究呢

作者: xiongcaifei 时间: 2013-1-22 14:35
求高手，做出来的人解答啊。

作者: xiongcaifei 时间: 2013-1-22 15:31
至少这个+P[1][1]dt*dt 也应该加。我就得。求牛人解决

作者: dlyt03 时间: 2013-1-23 12:30
看看

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

作者: dlyt03 时间: 2013-1-23 18:33
学习下

作者: 除却巫山不是云 时间: 2013-1-27 16:09

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是计算协方差矩阵的导数，怎么算出来的？不知道你弄懂了没有，求解释啊。。、
苦逼的孩子回不了家还一大堆问题。╮(╯▽╰)╭。。

欢迎光临智能车制作 (http://dns.znczz.com/)