关于卡尔曼滤波，如果得出这种结果是否正确

只看该作者 · 2018-9-27 16:02

本帖最后由 nightingale2003 于 2018-9-27 16:04 编辑

尝试做平衡小车，然后用卡尔曼滤波处理从六轴传感器输出的数据。但发现晃动PCB时，滤波后得到的数值一直比实际情况慢。
比如PCB平放然后缓慢立，到45度时，滤波输出数值只到30；实际60度输出51度（以附图为例）。
想请教下，这样子的滤波是不是有问题的？

备注：1.蓝色为实时值，红色为滤波后数值
2.图例只是用于说明现象，并非真实数值，但情况相近。

1万 · 2018-9-27 21:54

系列2略微不像，后半截应该会好点。
现象基本是这样，你的滤波的参数是多少？
采样的周期又是多少？
可以算出来你滤波器的等效时常数τ，然后

只看该作者 · 2018-9-28 10:21

linqing171 发表于 2018-9-27 21:54
系列2略微不像，后半截应该会好点。
现象基本是这样，你的滤波的参数是多少？
采样的周期又是多少？

参数设定的话两个Q值我改的很大，函数本身基本和网上的一样。附带更新了数据，这个是真实值测得的了。
float code Q_angle=1;    //网上数值清一色只有0.001好像
float code Q_gyro=2;
float code R_angle=0.5;
float code dt=0.005;

以下为函数
void Kalman_Filter(float Accel,float Gyro,float *MPU_dat)  // 滤波，输出标准的方波驱动电机
{
*MPU_dat+=(Gyro - Q_bias) * dt; //

Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分

Pdot[1]=- PP[1][1];
Pdot[2]=- PP[1][1];
Pdot[3]=Q_gyro;

PP[0][0] += Pdot[0] * dt; // Pk-先验估计误差协方差微分的积分
PP[0][1] += Pdot[1] * dt; // =先验估计误差协方差
PP[1][0] += Pdot[2] * dt;
PP[1][1] += Pdot[3] * dt;

Angle_err = Accel - *MPU_dat;//zk-先验估计

PCt_0 = C_0 * PP[0][0];
PCt_1 = C_0 * PP[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 * PP[0][1];

PP[0][0] -= K_0 * t_0; //后验估计误差协方差
PP[0][1] -= K_0 * t_1;
PP[1][0] -= K_1 * t_0;
PP[1][1] -= K_1 * t_1;

*MPU_dat+= K_0 * Angle_err;
Q_bias+= K_1 * Angle_err; //后验估计
*(MPU_dat+1) = Gyro-Q_bias; //输出值(后验估计)的微分=角速度

1万 · 2018-9-28 15:49

哥，原始数据先上个阶跃响应 x[]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100};
跑一下你的算法，看看滤波后的 y[]是多少。

只看该作者 · 2018-9-29 10:21

linqing171 发表于 2018-9-28 15:49
哥，原始数据先上个阶跃响应 x[]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,100,100,100,100,100,100,100,100, ...

试着角度输入{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,100,100,100,100,100,100,100,100}
加速度按照常理，输入{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,100,0，0,0,0,0,0,0,0,0,0,0,0}
得到曲线如图

1万 · 2018-9-29 21:25

nightingale2003 发表于 2018-9-29 10:21
试着角度输入{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,100,100,100,100,100,100,100,100}
加速度按照常理， ...

1 滤波器有低通效果。
2 滤波器有小的延时效果。
3 阶跃响应的稳态是没有偏差的。
滤波器效果只是让输入的数据慢慢的变化到稳态值，没有看到错误。

是不是楼主等的时间不足够长？
从阶跃响应来看，不应该有稳态误差。

只看该作者 · 2018-10-5 09:14

linqing171 发表于 2018-9-29 21:25
1 滤波器有低通效果。
2 滤波器有小的延时效果。
3 阶跃响应的稳态是没有偏差的。

昨天查程序，发现原来有个地方弄错了

卡尔曼的两个取值，角度和角速度，其中角速度没取到
改回来后很多都正常了

此贴可结，感谢一直以来的指导

关于卡尔曼滤波，如果得出这种结果是否正确

相关下载

相关帖子

技术新星奖章

以坛为家

技术导师奖章

沉静之湖泊

无冕之王奖章