智能车制作

标题: 关于卡尔曼滤波，大家都过来看眼呀~~~~~~~~~~~求教 [打印本页]

作者: liu1guo2qiang3 时间: 2012-3-8 20:18
标题: 关于卡尔曼滤波，大家都过来看眼呀~~~~~~~~~~~求教
都一个多月了，觉得滤波还是不行。现在求助大家帮我看看。不知道我的滤波行不行，首先用的是卡尔曼滤波程序

float Q_angle=0.001, Q_gyro=0.001, R_angle=0.0008, dt=0.010;

//注意：dt的取值为kalman滤波器采样时间;
float P[2][2] = {
{ 1, 0 },
{ 0, 1 }
};

float Pdot[4] ={0,0,0,0};

const char 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) //gyro_m:gyro_measure
{
angle+=(gyro_m-q_bias) * dt;//先验估计

Pdot[0]=Q_angle - P[0][1] - P[1][0];// Pk-' 先验估计误差协方差的微分
Pdot[1]=- P[1][1];
Pdot[2]=- P[1][1];
Pdot[3]=Q_gyro;

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

angle_err = angle_m - angle;//zk-先验估计

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;//Kk
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;//输出值（后验估计）的微分 = 角速度

}

我通过中断采样，周期是10ms一次，陀螺仪和加速度器均采样一次
然后输入卡尔曼进行滤波。用的那个串口程序发送就要9ms左右，所以暂时没提高采样频率
其次我用是弧度制。觉得按理应该就是一个比例问题，不是很大。
有空弄成角度后也发上来给大伙看看。
我下列的图像，蓝色的加速度传感器实际测到的值
红色为滤波后加速度计的值对于程序中的angle
粉红色是陀螺仪采到的值
黄色是滤波后陀螺仪的值对于程序中的angle_dot

[attach]19424[/attach]这张图中，白色圆圈的地方不知道怎么滤掉。我用手往前倒，加速度理想是增加的，这里却有个下降的过程。float Q_angle=0.001, Q_gyro=0.0015, R_angle=0.001, dt=0.010是此时的参数
我觉得这个时刻滤波出来的数据均由延迟，相对于实际都是有偏移的。
[attach]19425[/attach]
float Q_angle=0.008, Q_gyro=0.0015, R_angle=0.001, dt=0.010;这个时刻的参数。这个会靠近实际数据。
但仍像第一张图似的。开始倒下去的时候加速度有负值。
其他图大家也看看那个符合点。

然后我对陀螺仪有些疑问，陀螺仪积分是按手册中0.76度/mv/s的么。就是按时间转换？可是我在用手摇摆的时候，陀螺仪最大的时刻是差不多对应车子平衡位置。这样不就不是角度，而是对应角速度了么？可以通过积分到到角度么？

大伙都给点提示和支持呀。挣扎好久了呀。

作者: liu1guo2qiang3 时间: 2012-3-8 21:18
那个陀螺仪积分是不是每次都要累加的呀？如果是，那我就要推翻以上的东西。我就拿采一次值去弄。。。。。。。。

作者: 有-木-友 时间: 2012-3-9 01:45

作者: LJG 时间: 2012-3-9 02:50
不行，毛刺太多，跟踪有延迟

作者: yssdsz 时间: 2012-3-10 19:06
延时，我觉得第二个图还是不错的。

作者: wo520 时间: 2012-3-10 19:24
其实不需要太过看重波形，只要达到要求就行！不滞后，趋势能跟踪上就好了！！

作者: liu1guo2qiang3 时间: 2012-3-11 21:46

LJG 发表于 2012-3-9 02:50
不行，毛刺太多，跟踪有延迟

嗯，是加速度么？
我之前那个陀螺仪没有累加要重新弄的
下次再传麻烦在看看了

作者: liu1guo2qiang3 时间: 2012-3-11 21:48

wo520 发表于 2012-3-10 19:24
其实不需要太过看重波形，只要达到要求就行！不滞后，趋势能跟踪上就好了！！

现在还是觉得陀螺仪有问题
会一直累加上去的
我上面的图没有累加
后来做了累加的
陀螺仪就会累加一直到最大值

作者: wo520 时间: 2012-3-12 13:26

liu1guo2qiang3 发表于 2012-3-11 21:48
现在还是觉得陀螺仪有问题
会一直累加上去的
我上面的图没有累加

陀螺仪累加上去，那是很正常的！！因为陀螺仪有积分累加误差，如果陀螺仪积分的角度是正确的话，那么就没必要用滤波得到一个新角度了！！

作者: liu1guo2qiang3 时间: 2012-3-12 21:43

wo520 发表于 2012-3-12 13:26
陀螺仪累加上去，那是很正常的！！因为陀螺仪有积分累加误差，如果陀螺仪积分的角度是正确的话，那么就没 ...

现在你看看那行不呀。有一个是陀螺仪会漂上去的。
还有的就是放大点的。
黄色是输出。对应angle_dot

作者: wo520 时间: 2012-3-13 10:04

liu1guo2qiang3 发表于 2012-3-12 21:43
现在你看看那行不呀。有一个是陀螺仪会漂上去的。
还有的就是放大点的。
黄色是输出。对应angle_dot

不行，明显的滞后了！

作者: liu1guo2qiang3 时间: 2012-3-13 14:22

wo520 发表于 2012-3-13 10:04
不行，明显的滞后了！

是陀螺仪滞后还是加速度呀，。应该是加速度吧，

作者: 玩ㄝ★卟恭 时间: 2012-3-15 18:43
想问一下楼主怎么通过那个visual scope 获得数据的图形的为什么我用串口接收却没有波形显示呢是系统的原因吗

作者: liu1guo2qiang3 时间: 2012-3-16 16:22

玩ㄝ★卟恭发表于 2012-3-15 18:43
想问一下楼主怎么通过那个visual scope 获得数据的图形的为什么我用串口接收却没有波形显示呢是系统的 ...

首先你看看串口连好了没。最好用别的上位机看看能不能接收字符~~

作者: 玩ㄝ★卟恭 时间: 2012-3-16 20:56

liu1guo2qiang3 发表于 2012-3-16 16:22
首先你看看串口连好了没。最好用别的上位机看看能不能接收字符~~

谢谢楼主

作者: 玩ㄝ★卟恭 时间: 2012-3-17 14:00

liu1guo2qiang3 发表于 2012-3-16 16:22
首先你看看串口连好了没。最好用别的上位机看看能不能接收字符~~

楼主想要显示波形是不是一定要按一定的格式发数据呢

作者: liu1guo2qiang3 时间: 2012-3-17 15:48

玩ㄝ★卟恭发表于 2012-3-17 14:00
楼主想要显示波形是不是一定要按一定的格式发数据呢

嗯。必须按它的规则去发~

作者: liu1guo2qiang3 时间: 2012-3-20 10:28

有-木-友发表于 2012-3-9 01:45

上面哪副还算比较靠谱的呀？？

作者: 200802030103 时间: 2012-5-11 14:34
我做的卡拉曼滤波效果图

作者: liu1guo2qiang3 时间: 2012-5-11 15:27

200802030103 发表于 2012-5-11 14:34
我做的卡拉曼滤波效果图

觉得有点滞后。不过感觉可以直立试试。

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