智能车制作

标题: 卡尔曼滤波的问题求教相信对大神们应该很简单了 [打印本页]

作者: 遨游天际 时间: 2013-5-1 22:19
标题: 卡尔曼滤波的问题求教相信对大神们应该很简单了

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

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;

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

作者: 颠覆理论 时间: 2013-5-2 12:54
恐怕你套那个五个公式是理解不了的，你得找出这样写的数学模型

作者: 遨游天际 时间: 2013-5-2 21:06
嗯现在我已经理解了，那个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-6-24 20:48

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

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

作者: huang91 时间: 2013-6-24 21:45
不是错了，是理解的人不多。
*    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 ]

作者: 二百五 时间: 2013-6-25 00:41
R_angle=300 ！！！！！！！

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