卡尔曼滤波图形

renqinglei

在路上520 发表于 2012-6-2 21:29
不知道，反正参数能调的我都调了，可还是不行？

我Q415650679，咱们多交流一下

autozhubeilife

亲，别乱换，我觉得调一种就行

leechenrong

但是尖峰可以用限幅稍微滤下的

在路上520

autozhubeilife 发表于 2012-6-2 22:34
亲，别乱换，我觉得调一种就行

嗯，之前用官方的，可是陀螺仪漂移太严重，后来就用卡尔曼滤波了，现在不想换了。可现在卡尔曼滤波进行不下去了，尖峰实在消不掉，求助

skywolf

你是不是用的买来的模块？？

滑民航

在路上520 发表于 2012-6-2 21:20
你用的是网上的卡尔曼滤波程序吗？还是自己又写的？

网上的 但感觉有些错

在路上520

skywolf 发表于 2012-6-3 01:48
你是不是用的买来的模块？？

是买的，买的不好吗？是因为买的造成的吗？

在路上520

滑民航 发表于 2012-6-3 08:13
网上的 但感觉有些错

嗯，是前面求P那块错了吗？我自己觉得那块是错的，但是改过之后，几乎没有陀螺仪的输出的，融合后的基本都跟随加速度计的。

看下我改过的


static float P[2][2] = {{ 0.005, 0}, { 0, 0.005 }};//误差相关矩阵,如果我们不确切的知道最初的位置与速度，那么协
                                                                                                        //方差矩阵可以初始化为一个对角线元素是b 的矩阵
  
         static float q_bias=0;                                                //陀螺仪偏差
         
        
         static const float dt = 0.005;             //dt的取值为kalman滤波器采样时间
         static float A[2][2]={{1, -0.005 }, {0,1}} ,a[2][2]={{1,0}, {-0.005,1}} ;//a为A的转置
         static float angle_err;
         static float X[2][2]={{0,0}, {0,0}} ,Y[2][2]={{0,0}, {0,0}};
         static float PHt_0=0,PHt_1=0;
         static float En=0,t_0=0,t_1=0;
          float h_0 = 1.0;
          int i,j,k;
              
        g_fCarAngle=g_fCarAngle-g_fangle_dot  * dt;     //X(K|K-1)=A*X(K-1|K-1)
     
     
      
          for(i=0;i<2;i++)                                           //P(K|K-1)=A*P(K-1|K-1)*A'+Q
              {
                  for(j=0;j<2;j++)
                      {
                             for(k=0;k<2;k++)
                                 X[j]+=A[k]*P[k][j] ;
                       }
                }
   
           
          for(i=0;i<2;i++)
             {
                 for(j=0;j<2;j++)
                     {
                            for(k=0;k<2;k++)
                            Y[j]+=X[k]*a[k][j] ;
                       }
               }
         
          P[0][0] =Y[0][0]+ Q_angle;
          P[0][1]= Y[0][1] ;
          P[1][0]=Y[1][0];
          P[1][1] =Y[1][1]+Q_gyro ;
      
     //Y矩阵清零   ,X矩阵清零   
          for(i=0;i<2;i++)
               {
                     for(j=0;j<2;j++)
                         {
                              Y[j]=0;
                               X[j]=0;
                          }
                 }
        
        
        
         
        angle_err = g_fGravityAngle - g_fCarAngle;
         
         PHt_0 = h_0*P[0][0];
         PHt_1 = h_0*P[1][0];
         
         En = R_angle + h_0 * PHt_0;
         
         K_0 = PHt_0 / En;
         K_1 = PHt_1 / En;
         
         t_0 = PHt_0;
         t_1 = h_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;
                 
         g_fCarAngle   += K_0 * angle_err;
         q_bias += K_1 * angle_err;
          g_fangle_dot = g_fGyroSpeed-q_bias;      
}
         

skywolf

在路上520 发表于 2012-6-3 09:35
是买的，买的不好吗？是因为买的造成的吗？

我怀疑你买的模块有问题。我们最初买的龙丘的模块有问题，出现反向过冲，就是变化时先往反方向变化然后才往正确的方向变化，导致一个多月车根本站不起来。建议照着官网做，完全可用，温飘几乎没有。

在路上520

skywolf 发表于 2012-6-3 09:55
我怀疑你买的模块有问题。我们最初买的龙丘的模块有问题，出现反向过冲，就是变化时先往反方向变化然后才 ...

我的现在就是这个问题，先往反方向转，再往正确的方向转，这是反向过冲吗？