亿加合和智能车制作

标题: 卡尔曼滤波 [打印本页]

作者: 在路上520 时间: 2012-5-26 22:57
标题: 卡尔曼滤波
[attach]25060[/attach]

尖峰怎么消除呢

作者: 451506709 时间: 2012-5-26 23:28
有过冲的迹象

作者: 在路上520 时间: 2012-5-26 23:49
嗯。还有点滞后，怎么消除过冲了

作者: linping9656 时间: 2012-5-29 08:32
你用的是什么处理器啊？

作者: 在路上520 时间: 2012-5-29 08:42

linping9656 发表于 2012-5-29 08:32
你用的是什么处理器啊？

xs128.。。。。。。。。。。。。对了，用卡尔曼滤波不需要改电路吧，官方的电路就行吧，还有我觉得网上的卡尔曼滤波前半部分不太对，我就用矩阵来相乘了，为什么融合后的波形只剩加速度计了

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-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;
}


作者: linping9656 时间: 2012-5-29 20:45

在路上520 发表于 2012-5-29 08:42
xs128.。。。。。。。。。。。。对了，用卡尔曼滤波不需要改电路吧，官方的电路就行吧，还有我觉得网上的 ...

我也不懂呢

作者: 在路上520 时间: 2012-5-29 22:47

linping9656 发表于 2012-5-29 20:45
我也不懂呢

我看你卡尔曼滤波的图了；挺好的，我的怎么总是过冲，而且猛调参数也就消掉一面的过冲，你是怎么调的参数
作者: 繁＿、点点 时间: 2012-12-29 17:12
过冲有没有消除啊  我也也有  什么原因啊？
作者: 星雨流殇 时间: 2013-3-27 22:29
楼主，你的过冲是怎么消除的

作者: 破曉 时间: 2013-5-8 09:08

在路上520 发表于 2012-5-29 08:42
xs128.。。。。。。。。。。。。对了，用卡尔曼滤波不需要改电路吧，官方的电路就行吧，还有我觉得网上的 ...

g_fangle_dot最后的这个变量是什么意思？这个程序里好像只出现了这一次啊
作者: af6459 时间: 2018-4-13 10:19
可以给我一点卡尔曼滤波资料吗

欢迎光临亿加合和智能车制作 (http://www.znczz.com/) Powered by Discuz! X3.2