智能车制作

标题: 关于卡尔曼滤波的问题，求大神讲解 [打印本页]

作者: 星雨流殇 时间: 2013-3-26 10:54
标题: 关于卡尔曼滤波的问题，求大神讲解
[attach]42802[/attach]
为什么卡尔曼滤波出来的图形（蓝色）跟随陀螺仪的变化，而不是加计（黄色），该怎么调啊？求大神讲解

作者: 星雨流殇 时间: 2013-3-26 10:55
程序如下：
            Acc_x = (float)ATD0DR1;
   Acc_z = (float)ATD0DR2;
   Gyro  = (float)ATD0DR0;

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

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

   Acc_x = Acc_x- 2342.0;
   Acc_z = Acc_z- 2076.0;
   Gyro  = Gyro+30280;
Gyro_Data = Gyro;

OutData[0] = Gyro_Data;

accelerometer_angle = atan2f(-Acc_x,Acc_z);
OutData[1] = accelerometer_angle*100000;

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();

作者: Tenacity 时间: 2013-3-26 12:51
慢慢试吧，大概弄明白每个参数的意思之后就会调了

作者: 亿万星尘 时间: 2013-5-8 20:32
哥们儿，你那个波形用的什么软件呀，，我也正在调，卡尔曼滤波，咱们可以交流一下，qq272962111

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