智能车制作

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

---

作者: 在路上520    时间: 2012-5-27 19:33
标题: 卡尔曼滤波程序
[attach]25093[/attach][attach]25093[/attach]


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的时候用数组就不行了？
[attach]25094[/attach][attach]25094[/attach]


[attach]25093[/attach]


     

---

作者: 在路上520    时间: 2012-5-27 19:34
红色为加速度计的，黄色为融合后的

---

作者: yssdsz    时间: 2012-5-27 20:33
你A矩阵不对

---

作者: 在路上520    时间: 2012-5-27 20:36

yssdsz 发表于 2012-5-27 20:33
你A矩阵不对

哪里不对，那个系数错了？

---

作者: yssdsz    时间: 2012-5-27 20:38
其他的一时看不出来。。。你直接用那个原程序调参数吧。。。。估计你是没懂卡尔曼滤波。。。那程序也不太明白什么意思。。。

---

作者: yssdsz    时间: 2012-5-27 20:40
X矩阵清零干什么啊？？？清零了你还怎么估计下次的值啊？？？当然会出现你的那种情况了，滤波角度跟原是角度一模一样。。。

---

作者: yssdsz    时间: 2012-5-27 20:41
不对，你那X矩阵的格式就不对。。。原程序的是一行两列

---

作者: yssdsz    时间: 2012-5-27 20:43
强烈建议不要再用卡尔曼了。。。。我用8451和enc03调了一周多都没调出来。。。换互补一两个小时就好了。。。

---

作者: 在路上520    时间: 2012-5-27 20:46
我们也一直用的互补，效果的确挺好，现在想尝试下卡尔曼滤波，能告诉我A矩阵哪错了吗？是不是应该改成-0.005

---

作者: 在路上520    时间: 2012-5-27 20:49
X矩阵不是卡尔曼滤波里的X(K|K-1)，只是一个辅助计算矩阵相乘用的

---

作者: 在路上520    时间: 2012-5-27 20:51
X(K|K-1)={angle,q_bias},

---

作者: yssdsz    时间: 2012-5-27 20:53

在路上520 发表于 2012-5-27 20:36
哪里不对，那个系数错了？

按原文是-0.005

---

作者: 在路上520    时间: 2012-5-27 20:57
嗯，谢了，我再看看？

---

作者: yssdsz    时间: 2012-5-27 20:58
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] ;
                        }
                 }
   
这步计算？。。。。。。不太对吧。。。。

---

作者: yssdsz    时间: 2012-5-27 21:02
A是一个2*2矩阵，P是一个2*2，A*P应该也是2*2

---

作者: yssdsz    时间: 2012-5-27 21:03
你的X定义了一个2阶矩阵，这里只计算了一半。。

---

作者: 在路上520    时间: 2012-5-27 21:03
X[i][j]+=A[i][k]*P[k][j] ;矩阵相乘，不对吗？

---

作者: yssdsz    时间: 2012-5-27 21:12

在路上520 发表于 2012-5-27 21:03
X[j]+=A[k]*P[k][j] ;矩阵相乘，不对吗？

但是你这里只写了X【j】。。。

---

作者: 在路上520    时间: 2012-5-27 21:13
不好意思，那段发的有问题

---

作者: 在路上520    时间: 2012-5-27 21:13
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[i][j]+=A[i][k]*P[k][j] ;
                       }
                }
   
           
          for(i=0;i<2;i++)
             {
                 for(j=0;j<2;j++)
                     {
                            for(k=0;k<2;k++)
                            Y[i][j]+=X[i][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[i][j]=0;
                               X[i][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;      
}
         

---

作者: yssdsz    时间: 2012-5-27 21:19
  g_fCarAngle=g_fCarAngle-q_bias * dt;     //X(K|K-1)=A*X(K-1|K-1)
你是根据盛行的程序改的吧。那个程序里qbias这个变量表示陀螺仪测量值与实际角速度的偏差。
明白了？

---

作者: yssdsz    时间: 2012-5-27 21:21
不对。。我再看看。。

---

作者: 在路上520    时间: 2012-5-27 21:22
嗯，那个程序，我明白，就前面求P那点不懂，然后把那段改了，改成数组了，结果不好

---

作者: yssdsz    时间: 2012-5-27 21:24
g_fgyroangle=g_fGyroSpeed*dt;  
         
         g_fCarAngle=g_fCarAngle+q_bias * dt;     //X(K|K-1)=A*X(K-1|K-1)
      
就是这的问题。

g_fgyroangle这玩意更新后没有再用到。。

---

作者: yssdsz    时间: 2012-5-27 21:26
不行不行。。。不行了。。。。。等我多研究研究。。。。。。

---

作者: 在路上520    时间: 2012-5-27 21:30
呵呵，好的

---

作者: yssdsz    时间: 2012-5-27 21:32
这是角速度的前验估计？没懂。。。g_fgyroangle=g_fGyroSpeed*dt;  
         
这是角度前验估计？那当然要用q_m-q_bias了~       g_fCarAngle=g_fCarAngle+q_bias * dt;

---

作者: yssdsz    时间: 2012-5-27 21:39
你再看看，我现在就想到这。。。先去补充些能量。。。。

佩服你，自己改写卡尔曼的程序！

原来那个程序我大体看懂了，但也没太仔细想。写数学思想方法论文的时候也没好好推一遍。。

---

作者: 在路上520    时间: 2012-5-27 21:42
嘿嘿，g_fgyroangle=g_fGyroSpeed*dt;  这句不是卡尔曼滤波里的，是我自己加，调试用的。。。。。。。。。。。。。。
你们还要写数学思想方法论文？

---

作者: yssdsz    时间: 2012-5-27 21:49

在路上520 发表于 2012-5-27 21:42
嘿嘿，g_fgyroangle=g_fGyroSpeed*dt;  这句不是卡尔曼滤波里的，是我自己加，调试用的。。。。。。。。。。 ...

选修课

---

作者: yssdsz    时间: 2012-5-27 21:53
g_fgyroangle=g_fGyroSpeed*dt;  
         
         g_fCarAngle=g_fCarAngle-q_bias * dt;   

改为g_fCarAngle=g_fCarAngle+(g_fgyrospeed-q_bias)*dt;

---

作者: 在路上520    时间: 2012-5-27 21:56
我试下

---

作者: 在路上520    时间: 2012-5-27 22:01
崩溃了，还不行

---

作者: newhand1991    时间: 2012-5-28 12:24

yssdsz 发表于 2012-5-27 20:43
强烈建议不要再用卡尔曼了。。。。我用8451和enc03调了一周多都没调出来。。。换互补一两个小时就好了。。。 ...

能介绍下互补滤波吗？  给个资料看看，程序也行。

---

作者: yssdsz    时间: 2012-5-28 15:40

newhand1991 发表于 2012-5-28 12:24
能介绍下互补滤波吗？  给个资料看看，程序也行。

一搜一大把。。

---

作者: 玩ㄝ★卟恭    时间: 2012-5-29 16:09
楼主融合后的波形哪里有问题  我真是看不出来  能指点一下么？

---

作者: 在路上520    时间: 2012-5-29 20:41
过冲很严重，而且有点滞后

---

作者: 心剑无痕    时间: 2012-5-31 09:59
楼主能加Q交流下吗  391691397

---

作者: 在路上520    时间: 2012-6-1 16:42

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

18994078

---

作者: chenyu2010550    时间: 2012-6-14 01:29
{:soso_e179:}

---

作者: xiaosan12003    时间: 2012-12-14 11:37
给力~ 学习！！！

---

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

---

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

在路上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最后的这个变量是什么意思？干什么的？为什么只在这个程序中出现了一次？

---

作者: canghenzhilei    时间: 2015-5-23 21:53

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

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

---

欢迎光临 智能车制作 (http://111.231.132.190/)

Powered by Discuz! X3.2