智能车制作

标题: 卡尔曼输出也会有过冲？？ [打印本页]

作者: 氤氲的风 时间: 2012-3-14 11:25
标题: 卡尔曼输出也会有过冲？？
为什么我的卡尔曼输出会有过冲呢？？？都一个月了，还没解决，大家遇到过这样的问题吗？？？请指教一下！！！
void AD1(void)          //MMA7361
{
ATD0CTL5 = 0x10;//转换AD1
while(!ATD0STAT2);
AD_data0 = ATD0DR0;
AD_data1=ATD0DR1;
// return(AD_data1);
}

for(i=0;i<20;i++) {
  AD1();
sum0+=AD_data0;
sum1+=AD_data1;
}
AD_out=sum0/20.0;
AD_OUT=(AD_out-a)/b;
acc=sum1/20.0;

acc=acc-1400;
acc=acc/640.0;//求出多少g
if(acc>1)
  acc=1;
else if(acc<-1)
  acc=-1;
acc1=180/3.1415*asin(acc);

#define dt                0.002//卡尔曼滤波采样频率
#define R_angle       0.8//测量噪声的协方差（即是测量偏差）
#define Q_angle       0.5//过程噪声的协方差
#define Q_gyro          0.9 //过程噪声的协方差  过程噪声协方差为一个一行两列
float kalmanUpdate(float gyro_m,float incAngle)
{
      float K_0;//含有卡尔曼增益的另外一个函数，用于计算最优估计值
      float K_1;//含有卡尔曼增益的函数，用于计算最优估计值的偏差
      float Y_0;
      float Y_1;

      //去除偏差后的角速度
      float Pdot[4];//过程协方差矩阵的微分矩阵
      float angle_err;//角度偏量
      float E;//计算的过程量

      static float angle = 0;          //下时刻最优估计值角度
      static float q_bias = 0;       //陀螺仪的偏差
      static float P[2][2] = {{ 1, 0 }, { 0, 1 }};//过程协方差矩阵

      Rate = gyro_m - q_bias;

      //计算过程协方差矩阵的微分矩阵
      Pdot[0] = Q_angle - P[0][1] - P[1][0];
      Pdot[1] = - P[1][1];
      Pdot[2] = - P[1][1];
      Pdot[3] = Q_gyro;

      angle += Rate * dt; //角速度积分得出角度

      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 = incAngle - angle; //计算角度偏差

      E = R_angle + P[0][0];
      K_0 = P[0][0] / E; //计算卡尔曼增益
      K_1 = P[1][0] / E;

      Y_0 = P[0][0];
      Y_1 = P[0][1];

      P[0][0] -= K_0 * Y_0; //跟新协方差矩阵
      P[0][1] -= K_0 * Y_1;
      P[1][0] -= K_1 * Y_0;
      P[1][1] -= K_1 * Y_1;

      angle += K_0 * angle_err; //给出最优估计值
      q_bias += K_1 * angle_err;//跟新最优估计值偏差

      return angle;
}

参数是自己通过输出效果一步步调试的，不知道为什么会有过冲

作者: pcj2011 时间: 2012-3-14 13:30
AD_OUT=(AD_out-a)/b;
acc=sum1/20.0;

acc=acc-1400;
acc=acc/640.0;//求出多少g
if(acc>1)
acc=1;
else if(acc<-1)
acc=-1;
acc1=180/3.1415*asin(acc);
这一段是什么意思，楼主我们可以交流下

作者: 氤氲的风 时间: 2012-3-14 14:57

pcj2011 发表于 2012-3-14 13:30
AD_OUT=(AD_out-a)/b;
acc=sum1/20.0;

加QQ聊吧 961686605

作者: yssdsz 时间: 2012-3-14 15:37
慢慢调吧。。。加油。。。

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