关于卡尔曼滤波的一点理解

伤痕我心

参加完第九届国赛，已经大半个月过去了，这段时间，闲来无事，研究一下卡尔曼滤波；
故此写下一点心得，不知道对于不对，望大神们指正指正。
代码：
//-------------------------------------------------------
//Kalman滤波，64MHz的处理时间约100um;
//-------------------------------------------------------
static float angle, AngleSpeed;   //外部需要引用的变量
//-------------------------------------------------------
static const float Q_angle=0, Q_gyro=0, R_angle=0, 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=0, angle_err=0, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
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];
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 - angle;
  
PCt_0 = C_0 * P[0][0];
PCt_1 = C_0 * P[1][0];

E = R_angle + C_0 * PCt_0;

K_0 = 1.0*PCt_0 / E;
K_1 = 1.0*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/2;
q_bias+= K_1 * angle_err;         
AngleSpeed=gyro_m-q_bias;

}
{其实这些代码大家已经都知道了}

在调试过程中，我发现了，加速度计素提供的是一个类似于PI速度控制中的I所积分出来的一个作用；
陀螺仪所提供的作用是，让融合曲线跟踪加速度计的曲线；
如此这般，加速度计所提供的类似于PI控制中的I,不能有太大的波动，否则，车子会震动得特别厉害；
看了华中科技大学的报告，有点难以理解，他们的报告中这样写道（如图）；
对于他们报告中的方法，我有点不解；
希望各位大神给予指点，谢谢，如果我所的错了，不要怕浪费了大哥的口水，尽量喷我，喷完后，我给你寄一瓶水过去。   小弟真心想学东西，请指教，谢谢。

沙漏、、、

赞赞赞

董小哥在奋斗

float angle_m,float gyro_m   就是陀螺仪和加速度计采集到的数据么？？？？   还有angle和AngleSpeed是经过滤波后的陀螺仪和加速度的数据么？如果是的话，就通过以下滤波么？
void AngleCalculate(void)
{

AD_GetValue();
kalmanUpdate(AD_value0,AD_value1);
GYRO_AngleSpeed  =(angle- 110 )*0.025; //陀螺仪角速度     *ratio2 归一化

ACCE_GravityAngle=(AngleSpeed-79)*2.405; //加速度计得到角度  归一化-90-0-90度
if(ACCE_GravityAngle>90)ACCE_GravityAngle=90;
if(ACCE_GravityAngle<-90)ACCE_GravityAngle=-90;
Car_Angle=GYRO_AngleIntegral;//
DeltaValue=(ACCE_GravityAngle-Car_Angle)/2;//加速度计角度值反馈给陀螺仪角度值
GYRO_AngleIntegral+=(GYRO_AngleSpeed+DeltaValue)*0.5;//积分  积分时间系数
}

没有道理！

华中科技大学的报告有吗  关于直立 的给我一份吧 ！！系谢谢楼主

007fox

allenanswerzq

仪124 发表于 2014-9-17 12:31
大神，若要更精确些，那种滤波好？多谢了

卡尔曼

仪124

allenanswerzq 发表于 2014-9-16 16:02
就是说，代码没有卡尔满那么多。效果也能差不多

大神，若要更精确些，那种滤波好？多谢了

allenanswerzq

仪124 发表于 2014-9-16 12:29
大神，何以解释？卡尔曼与互补滤波我虽然了解不多，但是觉得二者各有长处，应该不是包含关系，懂得不多， ...

就是说，代码没有卡尔满那么多。效果也能差不多

仪124

allenanswerzq 发表于 2014-9-12 16:02
互补滤波一句代码搞定。

大神，何以解释？卡尔曼与互补滤波我虽然了解不多，但是觉得二者各有长处，应该不是包含关系，懂得不多，见笑了。

伤痕我心

allenanswerzq 发表于 2014-9-12 16:02
互补滤波一句代码搞定。

互补滤波，不是很好，用着不是很精确，所以想改进一下