卡尔曼滤波程序

在路上520

void Kalman_Filter(void)
{
       static float P[2][2] = {{ 0.005, 0.005}, { 0.005, 0.005 }};//误差相关矩阵
  
         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_fgyroangle=g_fGyroSpeed*dt;  
        
        g_fCarAngle=g_fCarAngle+q_bias * 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;      
}
    为什么前面求P的时候用数组就不行了？






     

canghenzhilei

xiongcaifei 发表于 2013-1-22 16:58
楼主，你的卡尔曼滤波的两个参数，一个是加速度计的角度，另一个是陀螺仪的角速度。那这个角度是度还是弧度 ...

你的问题解决了吗?求指导

破曉

在路上520 发表于 2012-5-27 21:13
void Kalman_Filter(void)
{
       static float P[2][2] = {{ 0.005, 0.005}, { 0.005, 0.005 }};//误 ...

g_fangle_dot最后的这个变量是什么意思？干什么的？为什么只在这个程序中出现了一次？

xiongcaifei

楼主，你的卡尔曼滤波的两个参数，一个是加速度计的角度，另一个是陀螺仪的角速度。那这个角度是度还是弧度？还是角度经过3.14后的数值啊？？还有你的角度和角速度给卡尔曼之前虑过波（平均滤波、、、）吗？？

xiaosan12003

给力~ 学习！！！

chenyu2010550

在路上520

心剑无痕 发表于 2012-5-31 09:59
楼主能加Q交流下吗  391691397

18994078

心剑无痕

楼主能加Q交流下吗  391691397

在路上520

过冲很严重，而且有点滞后

玩ㄝ★卟恭

楼主融合后的波形哪里有问题  我真是看不出来  能指点一下么？