卡尔曼输出也会有过冲？？

氤氲的风

为什么我的卡尔曼输出会有过冲呢？？？都一个月了，还没解决，大家遇到过这样的问题吗？？？请指教一下！！！
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

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);
这一段是什么意思，楼主我们可以交流下

氤氲的风

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

加QQ聊吧 961686605

yssdsz

慢慢调吧。。。加油。。。