智能车制作

标题: 卡尔曼滤波 [打印本页]

作者: 仰望星空1 时间: 2012-4-12 22:43
标题: 卡尔曼滤波
void Kalman_Filter(float angle_m,float gyro_m)
{
angle+=(gyro_m-q_bias) * dt;

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;

angle_err = angle_m - angle;

PCt_0 = C_0 * p[0][0];
PCt_1 = C_0 * p[1][0];

En = R_angle + C_0 * PCt_0;

K_0 = PCt_0 / En;
K_1 = PCt_1 / En;

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;
}

void AD_calculate(void)
{
   while(!ATD0STAT0_SCF);
acceler=ReadATD(2)+28;    //角度校正
gyro=ReadATD(0);

acceler*=0.004069;          //系数换算：2.5/(1.2*512);
acceler=asin(acceler);
gyro*=0.00341;             //角速度系数：(3.14/180)* 100/512=0.01364；

Kalman_Filter(acceler,gyro);
}

这个滤波函数的参数怎么调呀，求指教，现在我调的参数是static const float Q_angle=0.0017, Q_gyro=0.00095, R_angle=0.001, dt=0.005;
但是静止时角度值一直浮动，停不下来，晃动的时候有过冲

作者: 仰望星空1 时间: 2012-4-12 22:45
[attach]22718[/attach][attach]22719[/attach]

这是效果图

作者: 萌萌侠 时间: 2012-4-12 22:54
同求呀~~

作者: long1429 时间: 2012-4-13 08:15

作者: 逍遥镜风 时间: 2012-4-14 14:33
楼主能不能说下你的系数转换根据设么来的公式含义是什么谢谢了

作者: 仰望星空1 时间: 2012-4-15 23:15

发表于昨天 14:33 |只看该作者楼主能不能说下你的系数转换根据设么来的公式含义是什么谢谢了

惭愧呀，我是在网上找的，具体我也不知道呢

作者: linping9656 时间: 2012-4-20 12:47
卡尔曼好吗

作者: Achao 时间: 2012-4-20 14:12
网络上流行的这个卡尔曼应该不是很正确。卡尔曼滤波似乎不是很适合我们直立这种控制模型。只是起到了一个低通的作用。最主要的系数K_0最后会是一个定值常数，，所以这个模型认真分析，其实跟低通算法是一样的，波形越平滑，越滞后。。

作者: 仰望星空1 时间: 2012-4-21 18:04

发表于昨天 14:12 |只看该作者网络上流行的这个卡尔曼应该不是很正确。卡尔曼滤波似乎不是很适合我们直立这种控制模型。只是起到了一个低通的作用。最主要的系数K_0最后会是一个定值常数，，所以这个模型认真分析，其实跟低通算法是一样的，波形越平滑，越滞后。。

那有正确的没

作者: 在路上曹阳 时间: 2012-5-3 22:06

Achao 发表于 2012-4-20 14:12
网络上流行的这个卡尔曼应该不是很正确。卡尔曼滤波似乎不是很适合我们直立这种控制模型。只是起到了一个低 ...

是前面那段求协方差有错误吗，能先微分再积分吗？

作者: Achao 时间: 2012-5-6 14:16

仰望星空1 发表于 2012-4-21 18:04
那有正确的没

我用过一段时间的卡尔曼就没去用了

作者: Achao 时间: 2012-5-6 14:18
我用过一段时间的卡尔曼就放弃了。。具体怎么完善这个卡尔曼模型。。我没这个能力。。

作者: 谢小强55 时间: 2012-5-6 14:48
请问，卡尔曼滤波有什么用啊，，，新手求教

作者: 仰望星空1 时间: 2012-5-6 23:13

发表于 8 小时前 |只看该作者我用过一段时间的卡尔曼就放弃了。。具体怎么完善这个卡尔曼模型。。我没这个能力。。

我现在对卡尔曼也没办法了，决定换算法了

作者: 仰望星空1 时间: 2012-5-6 23:14
13楼不用考虑卡尔曼了

欢迎光临智能车制作 (http://111.231.132.190/)