常驻嘉宾
- 积分
- 3568
- 威望
- 1995
- 贡献
- 863
- 兑换币
- 279
- 注册时间
- 2011-3-12
- 在线时间
- 355 小时
|
都一个多月了,觉得滤波还是不行。现在求助大家帮我看看。不知道我的滤波行不行,首先用的是卡尔曼滤波程序
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
这张图中,白色圆圈的地方不知道怎么滤掉。我用手往前倒,加速度理想是增加的,这里却有个下降的过程。float Q_angle=0.001, Q_gyro=0.0015, R_angle=0.001, dt=0.010是此时的参数
我觉得这个时刻滤波出来的数据均由延迟,相对于实际都是有偏移的。
float Q_angle=0.008, Q_gyro=0.0015, R_angle=0.001, dt=0.010;这个时刻的参数。这个会靠近实际数据。
但仍像第一张图似的。开始倒下去的时候加速度有负值。
其他图大家也看看那个符合点。
然后我对陀螺仪有些疑问,陀螺仪积分是按手册中0.76度/mv/s的么。就是按时间转换?可是我在用手摇摆的时候,陀螺仪最大的时刻是差不多对应车子平衡位置。这样不就不是角度,而是对应角速度了么?可以通过积分到到角度么?
大伙都给点提示和支持呀。挣扎好久了呀。
|
本帖子中包含更多资源
您需要 登录 才可以下载或查看,没有帐号?注册
x
|