智能车制作

标题: 不知道为什么卡尔曼滤波的跟随性不是很好 [打印本页]

作者: 罹燼湮滅 时间: 2015-5-23 22:24
标题: 不知道为什么卡尔曼滤波的跟随性不是很好
大概过了1s左右卡尔曼算出的角度才能基本上和加速度计算出来的差不多我卡尔曼运算的周期大概是2ms 应该不算很长如果真是这种效果的话感觉还不如互补滤波如下是我的卡尔曼滤波算法

作者: 罹燼湮滅 时间: 2015-5-23 22:24
float dt=0.005;//注意：dt的取值为kalman滤波器采样时间
float angle, angle_dot;//角度和角速度
float P[2][2] = {{ 1, 0 },
{ 0, 1 }};
float Pdot[4] ={ 0,0,0,0};
float Q_angle=0.001, Q_gyro=0.005; //角度数据置信度,角速度数据置信度
float R_angle=0.5 ,C_0 = 1;
float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;

//卡尔曼滤波
void Kalman_Filter(float angle_m, float gyro_m)//angleAx 和 gyroGy
{
angle+=(gyro_m-q_bias) * dt;
angle_err = angle_m - angle;
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;
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;//最优角速度
}

作者: 静默深眠的夏 时间: 2015-5-23 23:42

:)周期

作者: 罹燼湮滅 时间: 2015-5-24 11:15

静默深眠的夏发表于 2015-5-23 23:42
周期

是采样周期长了吗不好意思啊还是不怎么懂能详细说一下吗我实际的采样周期是2ms 程序里的dt=5ms是因为我是直接复制别人的过来了在我自己的程序里我是用定时器去算的dt

作者: 罹燼湮滅 时间: 2015-5-24 12:01

静默深眠的夏发表于 2015-5-23 23:42
周期

我知道我的问题出在哪里了 dt时间算的不精确现在没问题了发现卡尔曼滤波真强大把过冲问题都解决了哈哈

作者: 夏日纳兹 时间: 2015-5-27 20:22

罹燼湮滅发表于 2015-5-24 12:01
我知道我的问题出在哪里了 dt时间算的不精确现在没问题了发现卡尔曼滤波真强大把过冲问题都解决了哈 ...

dt时间要怎么计算，通过示波器吗？其他参数要不要修改

作者: 罹燼湮滅 时间: 2015-5-29 20:06

夏日纳兹发表于 2015-5-27 20:22
dt时间要怎么计算，通过示波器吗？其他参数要不要修改

就是用定时器算的你也可以通过硬件仿真的模式设断点来查看采样周期其他参数不用改你只要明白几个关键的参数的意义就可以了

作者: 453427704 时间: 2015-6-6 23:28

罹燼湮滅发表于 2015-5-29 20:06
就是用定时器算的你也可以通过硬件仿真的模式设断点来查看采样周期其他参数不用改你只要明白几个关键 ...

关键的参数是哪几个，求教一下，刚刚接触卡尔曼，有点茫然~~

作者: MT马虎 时间: 2015-6-14 20:26
把dt改成卡尔曼的采样时间就能OK了我的dt=0.0004

作者: 夏日纳兹 时间: 2015-6-15 21:43
我也用定时器计算的dt，也有跟随性不好，lz是用什么方法改进的？

作者: 未来家CON 时间: 2015-8-1 15:51
楼主，我是新手一枚，找了很多卡尔曼滤波的资料，还是不怎么懂。请问可不可以具体讲解一下呢。方便的话，可以QQ联系，加个好友呗。645315357

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