不知道为什么 卡尔曼滤波的跟随性不是很好

罹燼湮滅

大概过了1s左右 卡尔曼算出的角度才能基本上和加速度计算出来的差不多 我卡尔曼运算的周期大概是2ms 应该不算很长 如果真是这种效果的话 感觉还不如互补滤波 如下是我的卡尔曼滤波算法

罹燼湮滅

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
周期

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

罹燼湮滅

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

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

夏日纳兹

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

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

罹燼湮滅

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

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

453427704

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

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

MT马虎

把dt改成卡尔曼的采样时间就能OK了  我的dt=0.0004

夏日纳兹

我也用定时器计算的dt，也有跟随性不好，lz是用什么方法改进的？