智能车制作

标题: 电磁组卡尔曼滤波问题 [打印本页]

作者: bu1193510501 时间: 2012-4-10 12:09
标题: 电磁组卡尔曼滤波问题
请问卡尔曼滤波算法中，哪些出事值如何设定，以及Q,R，P等参数如何定？？

作者: 十八龙潭 时间: 2012-4-10 16:16
得看你的具体程序是什么，不然怎么说

作者: bu1193510501 时间: 2012-4-11 19:21

十八龙潭发表于 2012-4-10 16:16
得看你的具体程序是什么，不然怎么说

float Q =1,R = 300;
static float RealData = 0,RealData_P = 0;
float NowData = 0,NowData_P = 0;
float Kg = 0,gyroscope_rate = 0,accelerometer_angle=0;
float Acc_x = 0,Acc_z = 0, Gyro = 0;
static float his_acc = 0.0,his_accx = 0.0,his_accz = 0.0;
void kalman_update(void)
{


while(!ATD0STAT0_SCF); //等待转换完成

Gyro  = (float)ATD0DR0;
Acc_x = (float)ATD0DR1;
Acc_z = (float)ATD0DR2;

if(Gyro > 4090) Gyro += 1000;
else
if(Gyro > 4084) Gyro += 500;

if(Gyro < 55) Gyro -= 1000;
else
if(Gyro < 60) Gyro -= 500;

Acc_x = Acc_x - 2342.0;
Acc_z = Acc_z - 2076.0;
Gyro  = Gyro  - 2048.0;
Gyro_Data = Gyro;

OutData[0] = Gyro_Data;
accelerometer_angle =Acc_z*Rz; //atan2f(-Acc_x,Acc_z);
OutData[1] = accelerometer_angle*1000;

gyroscope_rate = Gyro*0.0023;       //参考电压3.3v 12位ADC 放大9.1倍 enc-03 0.67mv/deg./sec.

                                       //(3300/4096)/(0.67*9.1)*(3.14/180) =  0.0023

NowData = RealData + gyroscope_rate*0.01;                //1.预估计 X(k|k-1) = A(k,k-1)*X(k-1|k-1) + B(k)*u(k)
NowData_P = sqrt(Q*Q+RealData_P*RealData_P);             //2.计算预估计协方差矩阵 P(k|k-1) = A(k,k-1)*P(k-1|k-1)*A(k,k-1)'+Q(k)
Kg = sqrt(NowData_P*NowData_P/(NowData_P*NowData_P+R*R)); //3.计算卡尔曼增益矩阵 K(k) = P(k|k-1)*H(k)' / (H(k)*P(k|k-1)*H(k)' + R(k))
RealData = NowData + Kg*(accelerometer_angle - NowData);  //4.更新估计 X(k|k) = X(k|k-1)+K(k)*(Z(k)-H(k)*X(k|k-1))
RealData_P = sqrt((1-Kg)*NowData_P*NowData_P);          //5.计算更新后估计协防差矩阵 P(k|k) =（I-K(k)*H(k)）*P(k|k-1)

QingJiao =  RealData;

OutData[2] = QingJiao*1000;
OutPut_Data();

作者: 十八龙潭 时间: 2012-4-12 22:00
P、Q、R三个参数中，RealData_P初始值可随便设，没什么关系；Q =1不动；最关键是改变R值，什么值合适得自己看；另外，改变程序中的0.0023值也可改变波形，该值也应该合适。

作者: bu1193510501 时间: 2012-4-13 11:34

十八龙潭发表于 2012-4-12 22:00
P、Q、R三个参数中，RealData_P初始值可随便设，没什么关系；Q =1不动；最关键是改变R值，什么值合适得自己 ...

请问你们滤波后的角度，在直立控制时给PWM时有没有将角度放大处理？

作者: 十八龙潭 时间: 2012-4-13 21:54

bu1193510501 发表于 2012-4-13 11:34
请问你们滤波后的角度，在直立控制时给PWM时有没有将角度放大处理？

就是对角度进行P控制呀

作者: bu1193510501 时间: 2012-4-14 00:21

十八龙潭发表于 2012-4-13 21:54
就是对角度进行P控制呀

627213143麻烦大神加我，还有点想请教

作者: 夏罗 时间: 2012-4-14 23:41

bu1193510501 发表于 2012-4-11 19:21
float Q =1,R = 300;
static float RealData = 0,RealData_P = 0;
float NowData = 0,NowData_P ...

请问楼主你用这个程序做的怎么样了，应该站起来了吧...

作者: 只羡鸳鸯不羡 时间: 2012-4-16 22:38
请问楼上的用这个程序输出的波形真确吗?我用的就是这个不过波形变化太不明显啦

作者: bu1193510501 时间: 2012-4-17 12:49

只羡鸳鸯不羡发表于 2012-4-16 22:38
请问楼上的用这个程序输出的波形真确吗?我用的就是这个不过波形变化太不明显啦

嗯

作者: xiexueshi 时间: 2012-4-20 21:06

十八龙潭发表于 2012-4-13 21:54
就是对角度进行P控制呀

请问波融合之后，怎么将电压转换成pwm波的占空比啊，P和D我都知道，官网的程序不知道怎么转换的，谢谢，

作者: 十八龙潭 时间: 2012-4-20 21:24
pwm=p*角度+d*角速度

作者: 十八龙潭 时间: 2012-4-20 21:27

bu1193510501 发表于 2012-4-17 12:49
嗯

会有明显变化。

作者: linping9656 时间: 2012-4-22 18:24
怎么我用这个程序怎么调节参数都不稳定啊

作者: bu1193510501 时间: 2012-4-22 22:24

linping9656 发表于 2012-4-22 18:24
怎么我用这个程序怎么调节参数都不稳定啊

有的

作者: linping9656 时间: 2012-4-22 22:40

bu1193510501 发表于 2012-4-22 22:24
有的

这几个参数该怎么调啊

作者: 醉生梦死 时间: 2012-7-3 20:44
观望中

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