智能车制作

 找回密码
 注册

扫一扫,访问微社区

查看: 8165|回复: 20
打印 上一主题 下一主题

[咨询] 卡尔曼滤波

[复制链接]

200

主题

2621

帖子

1

精华

杰出人士

蜗牛

Rank: 12Rank: 12Rank: 12

积分
12488

优秀会员奖章活跃会员奖章论坛骨干奖章论坛元老奖章在线王奖章资源大师奖章

QQ
威望
5973
贡献
3101
兑换币
1999
注册时间
2011-10-21
在线时间
1707 小时
跳转到指定楼层
1#
发表于 2012-2-9 10:39:42 | 只看该作者 回帖奖励 |倒序浏览 |阅读模式
哪位大侠附上一块卡尔曼滤波的程序给小弟看看,理论和代码差的好远啊

200

主题

2621

帖子

1

精华

杰出人士

蜗牛

Rank: 12Rank: 12Rank: 12

积分
12488

优秀会员奖章活跃会员奖章论坛骨干奖章论坛元老奖章在线王奖章资源大师奖章

QQ
威望
5973
贡献
3101
兑换币
1999
注册时间
2011-10-21
在线时间
1707 小时
推荐
 楼主| 发表于 2012-2-9 15:30:54 | 只看该作者
清华使用的什么滤波
回复 支持 0 反对 1

使用道具 举报

22

主题

276

帖子

0

精华

常驻嘉宾

Rank: 8Rank: 8

积分
4309
威望
2101
贡献
1034
兑换币
502
注册时间
2012-2-3
在线时间
587 小时
毕业学校
农大
2#
发表于 2012-2-9 12:24:27 | 只看该作者
我觉得卡尔曼滤波不了解的话还是不要用,最好还是用清华的
回复 支持 反对

使用道具 举报

12

主题

74

帖子

0

精华

高级会员

Rank: 4

积分
784
威望
540
贡献
98
兑换币
2
注册时间
2011-10-22
在线时间
73 小时
4#
发表于 2012-2-9 15:42:00 | 只看该作者
清华的那个归一化啥的真麻烦。。。
回复 支持 反对

使用道具 举报

27

主题

244

帖子

0

精华

跨届大侠

Rank: 10Rank: 10Rank: 10

积分
6556

论坛元老奖章热心会员奖章

QQ
威望
1277
贡献
4939
兑换币
188
注册时间
2011-12-31
在线时间
170 小时
5#
发表于 2012-2-9 15:56:43 | 只看该作者
  1. //加速度计和光电码盘融合求速度

  2. #define FS 100
  3. #define VAR_WA 1
  4. #define VAR_WV 0.04

  5. float T = FS/1;
  6. float A[2][2] = {{1,-T},{0,1}};
  7. float B[2] = {T,0};
  8. float H[2] = {1,0};
  9. float Q[2][2] = {{VAR_WA*T*T,0},{0,0}};
  10. float R = VAR_WV;
  11. float I[2][2] = {{1,0},{0,1}};

  12. float x[2] = {0,0};
  13. float p[2][2] = {{1,0},{0,1}};

  14. float tmp_x[2];
  15. float tmp_p[2][2];
  16. float kg[2];

  17. void kalman(float ai,float vi){
  18.         tmp_x[0] = A[0][0]*x[0]+A[0][1]*x[1]+B[0]*ai;
  19.         tmp_x[1] = A[1][0]*x[0]+A[1][1]*x[1]+B[1]*ai;
  20.         tmp_p[0][0] = (A[0][0]*p[0][0]+A[0][1]*p[1][0])*A[0][0]+(A[0][0]*p[0][1]+A[0][1]*p[1][1])*A[0][1]+Q[0][0];
  21.         tmp_p[0][1] = (A[0][0]*p[0][0]+A[0][1]*p[1][0])*A[1][0]+(A[0][0]*p[0][1]+A[0][1]*p[1][1])*A[1][1]+Q[0][1];
  22.         tmp_p[1][0] = (A[1][0]*p[0][0]+A[1][1]*p[1][0])*A[0][0]+(A[1][0]*p[0][1]+A[1][1]*p[1][1])*A[0][1]+Q[1][0];
  23.         tmp_p[1][1] = (A[1][0]*p[0][0]+A[1][1]*p[1][0])*A[1][0]+(A[1][0]*p[0][1]+A[1][1]*p[1][1])*A[1][1]+Q[1][1];
  24.         kg[0] = (tmp_p[0][0]*H[0]+tmp_p[0][1]*H[1])/((H[0]*tmp_p[0][0]+H[1]*tmp_[1][0])*H[0]+(H[0]*tmp_p[0][1]+H[1]*tmp_[1][1])*H[1]+R);
  25.         kg[1] = (tmp_p[1][0]*H[0]+tmp_p[1][1]*H[1])/((H[0]*tmp_p[0][0]+H[1]*tmp_[1][0])*H[0]+(H[0]*tmp_p[0][1]+H[1]*tmp_[1][1])*H[1]+R);
  26.         x[0] = tmp_x[0]+kg[0]*(vi-H[0]*tmp_x[0]+H[1]*tmp_x[1]);
  27.         x[1] = tmp_x[1]+kg[1]*(vi-H[0]*tmp_x[0]+H[1]*tmp_x[1]);
  28.         p[0][0] = (I[0][0]-kg[0]*H[0])*tmp_p[0][0];
  29.         p[0][1] = (I[0][1]-kg[0]*H[1])*tmp_p[0][1];
  30.         p[1][0] = (I[1][0]-kg[1]*H[0])*tmp_p[1][0];
  31.         p[1][1] = (I[1][1]-kg[1]*H[1])*tmp_p[1][1];
  32. }
复制代码
回复 支持 反对

使用道具 举报

4

主题

86

帖子

0

精华

高级会员

Rank: 4

积分
976
QQ
威望
617
贡献
165
兑换币
10
注册时间
2011-9-9
在线时间
97 小时
6#
发表于 2012-2-9 20:13:32 | 只看该作者
float kalmanUpdate(const float gyro_m,const float incAngle)//只读变量不可以修改
{              //         陀螺仪                角度
        float K_0;//含有卡尔曼增益的另外一个函数,用于计算最优估计值
        float K_1;//含有卡尔曼增益的函数,用于计算最优估计值的偏差
        float Y_0;
        float Y_1;
        
        float Rate;
        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;
                                                                  //#define dt                  0.0015//滞后
                                                              //#define R_angle          0.69
                                                              //#define Q_angle          0.0001
                                                               //#define Q_gyro           0.0003 //卡尔曼滤波参数
         
        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;
}
回复 支持 反对

使用道具 举报

200

主题

2621

帖子

1

精华

杰出人士

蜗牛

Rank: 12Rank: 12Rank: 12

积分
12488

优秀会员奖章活跃会员奖章论坛骨干奖章论坛元老奖章在线王奖章资源大师奖章

QQ
威望
5973
贡献
3101
兑换币
1999
注册时间
2011-10-21
在线时间
1707 小时
7#
 楼主| 发表于 2012-2-10 08:57:17 | 只看该作者
lwzhdu 发表于 2012-2-9 20:13
float kalmanUpdate(const float gyro_m,const float incAngle)//只读变量不可以修改
{              //  ...

太感谢啦
回复 支持 反对

使用道具 举报

27

主题

179

帖子

0

精华

常驻嘉宾

Rank: 8Rank: 8

积分
3585
威望
2743
贡献
620
兑换币
23
注册时间
2011-8-13
在线时间
111 小时
8#
发表于 2012-2-14 23:14:13 | 只看该作者
网上卡尔曼程序好几个版本 到底哪个是对的啊
回复 支持 反对

使用道具 举报

1

主题

2

帖子

0

精华

注册会员

Rank: 2

积分
194
威望
138
贡献
32
兑换币
10
注册时间
2012-1-14
在线时间
12 小时
9#
发表于 2012-2-15 01:48:27 | 只看该作者
  1. /***************************************************************************/
  2. /* kalman.c                                                                */
  3. /* 1-D Kalman filter Algorithm, using an inclinometer and gyro             */
  4. /* Author: Rich Chi Ooi                                                    */
  5. /* Version: 1.0                                                            */
  6. /* Date:30.05.2003                                                         */
  7. /* Adapted from Trammel Hudson(hudson@rotomotion.com)                      */  
  8. /* -------------------------------                                         */
  9. /* Compilation  procedure:                                                 */
  10. /*       Linux                                                             */
  11. /*      gcc68 -c  XXXXXX.c (to create object file)                         */
  12. /*      gcc68 -o  XXXXXX.hex XXXXXX.o ppwa.o                               */
  13. /*Upload data :           */
  14. /* ul filename.txt                       */
  15. /***************************************************************************/
  16. /* In this version:                                                        */
  17. /***************************************************************************/
  18. /* This is a free software; you can redistribute it and/or modify          */
  19. /* it under the terms of the GNU General Public License as published       */
  20. /* by the Free Software Foundation; either version 2 of the License,       */
  21. /* or (at your option) any later version.                                  */
  22. /*                                                                         */
  23. /* this code is distributed in the hope that it will be useful,            */
  24. /* but WITHOUT ANY WARRANTY; without even the implied warranty of          */
  25. /* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the           */
  26. /* GNU General Public License for more details.                            */
  27. /*                                                                         */
  28. /* You should have received a copy of the GNU General Public License       */
  29. /* along with Autopilot; if not, write to the Free Software                */
  30. /* Foundation, Inc., 59 Temple Place, Suite 330, Boston,                   */
  31. /* MA  02111-1307  USA                                                     */
  32. /***************************************************************************/
  33. #include <math.h>
  34. #include "eyebot.h"
  35. /*
  36. * The state is updated with gyro rate measurement every 20ms
  37. * change this value if you update at a different rate.
  38. */
  39. static const float dt = 0.02;
  40. /*
  41. * The covariance matrix.This is updated at every time step to
  42. * determine how well the sensors are tracking the actual state.
  43. */
  44. static float P[2][2] = {{ 1, 0 },
  45.                  { 0, 1 }};
  46. /*
  47. * Our two states, the angle and the gyro bias.As a byproduct of computing
  48. * the angle, we also have an unbiased angular rate available.These are
  49. * read-only to the user of the module.
  50. */
  51. float angle;
  52. float q_bias;
  53. float rate;

  54. /*
  55. * The R represents the measurement covariance noise.R=E[vvT]
  56. * In this case,it is a 1x1 matrix that says that we expect
  57. * 0.1 rad jitter from the inclinometer.
  58. * for a 1x1 matrix in this case v = 0.1
  59. */
  60. static const float R_angle = 0.001 ;

  61. /*
  62. * Q is a 2x2 matrix that represents the process covariance noise.
  63. * In this case, it indicates how much we trust the inclinometer
  64. * relative to the gyros.
  65. */
  66. static const float Q_angle = 0.001;
  67. static const float Q_gyro  = 0.0015;

  68. /*
  69. * state_update is called every dt with a biased gyro measurement
  70. * by the user of the module.  It updates the current angle and
  71. * rate estimate.
  72. *
  73. * The pitch gyro measurement should be scaled into real units, but
  74. * does not need any bias removal.  The filter will track the bias.
  75. *
  76. *   A = [ 0 -1 ]
  77. *       [ 0  0 ]
  78. */
  79. void stateUpdate(const float q_m){
  80. float q;
  81. float Pdot[4];
  82. /* Unbias our gyro */
  83. q = q_m - q_bias;//当前角速度:测量值-估计值
  84. /*
  85.   * Compute the derivative of the covariance matrix
  86.   * (equation 22-1)
  87.   * Pdot = A*P + P*A' + Q
  88.   *
  89.   */
  90. Pdot[0] = Q_angle - P[0][1] - P[1][0]; /* 0,0 */
  91. Pdot[1] = - P[1][1];          /* 0,1 */
  92. Pdot[2] = - P[1][1];           /* 1,0 */
  93. Pdot[3] = Q_gyro;   /* 1,1 */
  94. /* Store our unbias gyro estimate */
  95. rate = q;
  96. /*
  97.   * Update our angle estimate
  98.   * angle += angle_dot * dt
  99.   *       += (gyro - gyro_bias) * dt
  100.   *       += q * dt
  101.   */
  102. angle += q * dt;//角速度积分累加到 估计角度
  103. /* Update the covariance matrix */
  104. P[0][0] += Pdot[0] * dt;
  105. P[0][1] += Pdot[1] * dt;
  106. P[1][0] += Pdot[2] * dt;
  107. P[1][1] += Pdot[3] * dt;
  108. }

  109. /*
  110. * kalman_update is called by a user of the module when a new
  111. * inclinoometer measurement is available.
  112. *
  113. * This does not need to be called every time step, but can be if
  114. * the accelerometer data are available at the same rate as the
  115. * rate gyro measurement.
  116. *
  117. *         H  = [ 1 0 ]
  118. *
  119. * because the angle measurement directly corresponds to the angle
  120. * estimate and the angle measurement has no relation to the gyro
  121. * bias.
  122. */
  123. void kalmanUpdate(const float incAngle)
  124. {
  125. /* Compute our measured angle and the error in our estimate */
  126. float angle_m = incAngle;
  127. float angle_err = angle_m - angle;//1.12 zk-H*xk_dot
  128. /*
  129.   * h_0 shows how the state measurement directly relates to
  130.   * the state estimate.
  131.    *   
  132.   *      H = [h_0 h_1]
  133.   *
  134.   * The h_1 shows that the state measurement does not relate
  135.   * to the gyro bias estimate.  We don't actually use this, so
  136.   * we comment it out.
  137.   */
  138. float h_0 = 1;
  139. /* const float  h_1 = 0; */
  140. /*
  141.   * Precompute PH' as the term is used twice
  142.   * Note that H[0,1] = h_1 is zero, so that term is not not computed
  143.   */
  144. const float PHt_0 = h_0*P[0][0]; /* + h_1*P[0][1] = 0*/
  145. const float PHt_1 = h_0*P[1][0]; /* + h_1*P[1][1] = 0*/
  146. /*
  147.   * Compute the error estimate:
  148.   * (equation 21-1)
  149.   *
  150.   * E = H P H' + R
  151.   */
  152. float E = R_angle +(h_0 * PHt_0);
  153. /*
  154.   * Compute the Kalman filter gains:
  155.   * (equation 21-2)
  156.   *
  157.   * K = P H' inv(E)
  158.          */
  159. float K_0 = PHt_0 / E;
  160. float K_1 = PHt_1 / E;
  161.   
  162. /*
  163.   * Update covariance matrix:
  164.   * (equation 21-3)
  165.   *
  166.   * P = P - K H P
  167.          * Let
  168.   *      Y = H P
  169.   */
  170. float Y_0 = PHt_0;  /*h_0 * P[0][0]*/
  171. float Y_1 = h_0 * P[0][1];
  172.   
  173. P[0][0] -= K_0 * Y_0;
  174. P[0][1] -= K_0 * Y_1;
  175. P[1][0] -= K_1 * Y_0;
  176. P[1][1] -= K_1 * Y_1;

  177. /*
  178.   * Update our state estimate:
  179.   *
  180.   * Xnew = X + K * error
  181.   *
  182.   * err is a measurement of the difference in the measured state
  183.   * and the estimate state.  In our case, it is just the difference
  184.   * between the inclinometer measured angle and the estimated angle.
  185.   */
  186. angle += K_0 * angle_err;
  187. q_bias += K_1 * angle_err;
  188. }
复制代码

回复 支持 反对

使用道具 举报

200

主题

2621

帖子

1

精华

杰出人士

蜗牛

Rank: 12Rank: 12Rank: 12

积分
12488

优秀会员奖章活跃会员奖章论坛骨干奖章论坛元老奖章在线王奖章资源大师奖章

QQ
威望
5973
贡献
3101
兑换币
1999
注册时间
2011-10-21
在线时间
1707 小时
10#
 楼主| 发表于 2012-2-16 09:30:59 | 只看该作者
#include <hidef.h>      /* common defines and macros */
#include "derivative.h"      /* derivative-specific definitions */
#include <MC9S12XS128.h>     /* derivative information */
float angle;//最优角度
float bqdd=0.02;    //估测不确定度
float dt;      //单位时间
float angle_pc=0;//最优值偏差
float cl_pc=0.0005;   //测量值偏差


void SetBusCLK_64M(void)
{
     CLKSEL=0X00;                   //disengage PLL to system
    PLLCTL_PLLON=1;                //turn on PLL
     SYNR =0xc0 | 0x07;
     REFDV=0x80 | 0x01;
    POSTDIV=0x00;          //pllclock=2*osc*(1+SYNR)/(1+REFDV)=128MHz;
    _asm(nop);             //BUS CLOCK=64M
    _asm(nop);
    while(!(CRGFLG_LOCK==1));           //when pll is steady ,then use it;
     CLKSEL_PLLSEL =1;                    //engage PLL to system;
}
////////////开方函数////////////
float ssqrt(float x)

{

      float xhalf=0.5f*x;

      int i=*(int*)&x;

      i=0x5f375a86-(i>>1);

      x=*(float*)&i;

      x=x*(1.5f-xhalf*x*x);

      return(1/x);

}


void kalmanUpdate(float speed,float incAngle)//只读变量不可以修改
{
  float GC;      //估测值
  float K;       //卡尔曼增益
  float gc_pc;   //估测值偏差的平方
  float zc;
  
  
  GC=angle+speed*dt;
  gc_pc=(angle_pc*angle_pc)+(bqdd*bqdd);
  K=gc_pc/(gc_pc+(cl_pc*cl_pc));
  K=ssqr(K);
  if(GC>incAngle)
  {
    zc=GC;
    GC=incAngle;
    incAngle=zc;
  }
  angle=GC+K*(incAngle-GC);
  angle_pc=((1-K)*gc_pc);
  angle_pc=ssqr(angle_pc);

}




void main(void)
{
  /* put your own code here */
  


        EnableInterrupts;


  for(;;)
  {
    _FEED_COP(); /* feeds the dog */
  } /* loop forever */
  /* please make sure that you never leave main */
}
我还没测试,不知道行不行
回复 支持 反对

使用道具 举报

您需要登录后才可以回帖 登录 | 注册

本版积分规则

关于我们|联系我们|小黑屋|智能车制作 ( 黑ICP备2022002344号

GMT+8, 2025-1-11 19:51 , Processed in 0.088014 second(s), 34 queries , Gzip On.

Powered by Discuz! X3.2

© 2001-2013 Comsenz Inc.

快速回复 返回顶部 返回列表