智能车制作

标题: 关于卡尔曼滤波的一点理解 [打印本页]

作者: 伤痕我心 时间: 2014-9-11 21:58
标题: 关于卡尔曼滤波的一点理解
参加完第九届国赛，已经大半个月过去了，这段时间，闲来无事，研究一下卡尔曼滤波；
故此写下一点心得，不知道对于不对，望大神们指正指正。
代码：
//-------------------------------------------------------
//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,不能有太大的波动，否则，车子会震动得特别厉害；
看了华中科技大学的报告，有点难以理解，他们的报告中这样写道（如图）；
对于他们报告中的方法，我有点不解；
希望各位大神给予指点，谢谢，如果我所的错了，不要怕浪费了大哥的口水，尽量喷我，喷完后，我给你寄一瓶水过去。小弟真心想学东西，请指教，谢谢。

作者: 伤痕我心 时间: 2014-9-11 22:02
沙发。

作者: Quixote 时间: 2014-9-11 22:34
我在看卡尔曼当年的论文

作者: 洅迲愛伱辰 时间: 2014-9-11 23:01

Quixote 发表于 2014-9-11 22:34
我在看卡尔曼当年的论文

当年论文能不能给一份，870345315@qq.com

作者: k40368 时间: 2014-9-12 09:49

Quixote 发表于 2014-9-11 22:34
我在看卡尔曼当年的论文

现在用的大多是改进的了。。。

作者: v8266 时间: 2014-9-12 11:41
为什么不解释一下每个语句的作用

作者: wmslecz 时间: 2014-9-12 12:08

作者: Quixote 时间: 2014-9-12 13:29

k40368 发表于 2014-9-12 09:49
现在用的大多是改进的了。。。

万变不离其宗

作者: allenanswerzq 时间: 2014-9-12 16:02
互补滤波一句代码搞定。

作者: 伤痕我心 时间: 2014-9-12 19:56

洅迲愛伱辰发表于 2014-9-11 23:01
当年论文能不能给一份，

第九届的论文可以在论坛下载的额

作者: 伤痕我心 时间: 2014-9-12 19:57

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

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

作者: 仪124 时间: 2014-9-16 12:29

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

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

作者: allenanswerzq 时间: 2014-9-16 16:02

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

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

作者: 仪124 时间: 2014-9-17 12:31

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

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

作者: allenanswerzq 时间: 2014-9-17 14:42

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

卡尔曼

作者: 007fox 时间: 2014-10-25 21:16

作者: 没有道理！ 时间: 2014-11-25 12:31
华中科技大学的报告有吗关于直立的给我一份吧！！系谢谢楼主

作者: 董小哥在奋斗 时间: 2015-3-16 20:30
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;//积分  积分时间系数
}

作者: 沙漏、、、 时间: 2015-3-20 20:40
赞赞赞

欢迎光临智能车制作 (http://111.231.132.190/)