|
|
楼主 |
发表于 2013-12-31 16:58:01
|
显示全部楼层
现在运算时间为10ms ，可能因为时间太长！！
是不是把 dt 改小啊？？？？？？
#define Q_angle 0.001
#define Q_gyro 0.003
#define R_angle 0.5
#define dt 0.01 // dt的取值为kalman滤波器采样时间,此处0.01为10ms;
void Kalman_Filter(float angle_m,float gyro_m) //gyro_m:gyro_measure
{
Angle += (gyro_m-Q_bias) * dt; //先验估计
Pdot[0]=Q_angle - P[0][1] - P[1][0]; // Pk-先验估计误差协方差的微分
Pdot[1]= - P[1][1];
Pdot[2]= - P[1][1];
Pdot[3]=Q_gyro;
P[0][0] += Pdot[0] * dt; // Pk-先验估计误差协方差微分的积分
P[0][1] += Pdot[1] * dt; // 先验估计误差协方差
P[1][0] += Pdot[2] * dt;
P[1][1] += Pdot[3] * dt;
Angle_err = angle_m - Angle; //zk-先验估计
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;
Angle += K_0 * Angle_err; //后验估计 角度
Q_bias += K_1 * Angle_err; //后验估计
Angle_dot = gyro_m-Q_bias; //输出值(后验估计)的微分=角速度
} |
|
发表于 2013-12-31 14:53:49
