卡尔曼滤波

qinlu123

哪位大侠附上一块卡尔曼滤波的程序给小弟看看，理论和代码差的好远啊

qinlu123

清华使用的什么滤波

luqi

我觉得卡尔曼滤波不了解的话还是不要用，最好还是用清华的

wanstrive

清华的那个归一化啥的真麻烦。。。

二硫碘化钾

1. //加速度计和光电码盘融合求速度
   
2. 
   
3. #define FS 100
   
4. #define VAR_WA 1
   
5. #define VAR_WV 0.04
   
6. 
   
7. float T = FS/1;
   
8. float A[2][2] = {{1,-T},{0,1}};
   
9. float B[2] = {T,0};
   
10. float H[2] = {1,0};
    
11. float Q[2][2] = {{VAR_WA*T*T,0},{0,0}};
    
12. float R = VAR_WV;
    
13. float I[2][2] = {{1,0},{0,1}};
    
14. 
    
15. float x[2] = {0,0};
    
16. float p[2][2] = {{1,0},{0,1}};
    
17. 
    
18. float tmp_x[2];
    
19. float tmp_p[2][2];
    
20. float kg[2];
    
21. 
    
22. void kalman(float ai,float vi){
    
23.         tmp_x[0] = A[0][0]*x[0]+A[0][1]*x[1]+B[0]*ai;
    
24.         tmp_x[1] = A[1][0]*x[0]+A[1][1]*x[1]+B[1]*ai;
    
25.         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];
    
26.         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];
    
27.         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];
    
28.         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];
    
29.         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);
    
30.         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);
    
31.         x[0] = tmp_x[0]+kg[0]*(vi-H[0]*tmp_x[0]+H[1]*tmp_x[1]);
    
32.         x[1] = tmp_x[1]+kg[1]*(vi-H[0]*tmp_x[0]+H[1]*tmp_x[1]);
    
33.         p[0][0] = (I[0][0]-kg[0]*H[0])*tmp_p[0][0];
    
34.         p[0][1] = (I[0][1]-kg[0]*H[1])*tmp_p[0][1];
    
35.         p[1][0] = (I[1][0]-kg[1]*H[0])*tmp_p[1][0];
    
36.         p[1][1] = (I[1][1]-kg[1]*H[1])*tmp_p[1][1];
    
37. }

复制代码

lwzhdu

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

qinlu123

lwzhdu 发表于 2012-2-9 20:13
float kalmanUpdate(const float gyro_m,const float incAngle)//只读变量不可以修改
{              //  ...

太感谢啦

z12c34j56

网上卡尔曼程序好几个版本 到底哪个是对的啊

skdchenmao

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. /*
    
56. * The R represents the measurement covariance noise.R=E[vvT]
    
57. * In this case,it is a 1x1 matrix that says that we expect
    
58. * 0.1 rad jitter from the inclinometer.
    
59. * for a 1x1 matrix in this case v = 0.1
    
60. */
    
61. static const float R_angle = 0.001 ;
    
62. 
    
63. /*
    
64. * Q is a 2x2 matrix that represents the process covariance noise.
    
65. * In this case, it indicates how much we trust the inclinometer
    
66. * relative to the gyros.
    
67. */
    
68. static const float Q_angle = 0.001;
    
69. static const float Q_gyro  = 0.0015;
    
70. 
    
71. /*
    
72. * state_update is called every dt with a biased gyro measurement
    
73. * by the user of the module.  It updates the current angle and
    
74. * rate estimate.
    
75. *
    
76. * The pitch gyro measurement should be scaled into real units, but
    
77. * does not need any bias removal.  The filter will track the bias.
    
78. *
    
79. *   A = [ 0 -1 ]
    
80. *       [ 0  0 ]
    
81. */
    
82. void stateUpdate(const float q_m){
    
83. float q;
    
84. float Pdot[4];
    
85. /* Unbias our gyro */
    
86. q = q_m - q_bias;//当前角速度：测量值-估计值
    
87. /*
    
88.   * Compute the derivative of the covariance matrix
    
89.   * (equation 22-1)
    
90.   * Pdot = A*P + P*A' + Q
    
91.   *
    
92.   */
    
93. Pdot[0] = Q_angle - P[0][1] - P[1][0]; /* 0,0 */
    
94. Pdot[1] = - P[1][1];          /* 0,1 */
    
95. Pdot[2] = - P[1][1];           /* 1,0 */
    
96. Pdot[3] = Q_gyro;   /* 1,1 */
    
97. /* Store our unbias gyro estimate */
    
98. rate = q;
    
99. /*
    
100.   * Update our angle estimate
     
101.   * angle += angle_dot * dt
     
102.   *       += (gyro - gyro_bias) * dt
     
103.   *       += q * dt
     
104.   */
     
105. angle += q * dt;//角速度积分累加到 估计角度
     
106. /* Update the covariance matrix */
     
107. P[0][0] += Pdot[0] * dt;
     
108. P[0][1] += Pdot[1] * dt;
     
109. P[1][0] += Pdot[2] * dt;
     
110. P[1][1] += Pdot[3] * dt;
     
111. }
     
112. 
     
113. /*
     
114. * kalman_update is called by a user of the module when a new
     
115. * inclinoometer measurement is available.
     
116. *
     
117. * This does not need to be called every time step, but can be if
     
118. * the accelerometer data are available at the same rate as the
     
119. * rate gyro measurement.
     
120. *
     
121. *         H  = [ 1 0 ]
     
122. *
     
123. * because the angle measurement directly corresponds to the angle
     
124. * estimate and the angle measurement has no relation to the gyro
     
125. * bias.
     
126. */
     
127. void kalmanUpdate(const float incAngle)
     
128. {
     
129. /* Compute our measured angle and the error in our estimate */
     
130. float angle_m = incAngle;
     
131. float angle_err = angle_m - angle;//1.12 zk-H*xk_dot
     
132. /*
     
133.   * h_0 shows how the state measurement directly relates to
     
134.   * the state estimate.
     
135.    *   
     
136.   *      H = [h_0 h_1]
     
137.   *
     
138.   * The h_1 shows that the state measurement does not relate
     
139.   * to the gyro bias estimate.  We don't actually use this, so
     
140.   * we comment it out.
     
141.   */
     
142. float h_0 = 1;
     
143. /* const float  h_1 = 0; */
     
144. /*
     
145.   * Precompute PH' as the term is used twice
     
146.   * Note that H[0,1] = h_1 is zero, so that term is not not computed
     
147.   */
     
148. const float PHt_0 = h_0*P[0][0]; /* + h_1*P[0][1] = 0*/
     
149. const float PHt_1 = h_0*P[1][0]; /* + h_1*P[1][1] = 0*/
     
150. /*
     
151.   * Compute the error estimate:
     
152.   * (equation 21-1)
     
153.   *
     
154.   * E = H P H' + R
     
155.   */
     
156. float E = R_angle +(h_0 * PHt_0);
     
157. /*
     
158.   * Compute the Kalman filter gains:
     
159.   * (equation 21-2)
     
160.   *
     
161.   * K = P H' inv(E)
     
162.          */
     
163. float K_0 = PHt_0 / E;
     
164. float K_1 = PHt_1 / E;
     
165.   
     
166. /*
     
167.   * Update covariance matrix:
     
168.   * (equation 21-3)
     
169.   *
     
170.   * P = P - K H P
     
171.          * Let
     
172.   *      Y = H P
     
173.   */
     
174. float Y_0 = PHt_0;  /*h_0 * P[0][0]*/
     
175. float Y_1 = h_0 * P[0][1];
     
176.   
     
177. P[0][0] -= K_0 * Y_0;
     
178. P[0][1] -= K_0 * Y_1;
     
179. P[1][0] -= K_1 * Y_0;
     
180. P[1][1] -= K_1 * Y_1;
     
181. 
     
182. /*
     
183.   * Update our state estimate:
     
184.   *
     
185.   * Xnew = X + K * error
     
186.   *
     
187.   * err is a measurement of the difference in the measured state
     
188.   * and the estimate state.  In our case, it is just the difference
     
189.   * between the inclinometer measured angle and the estimated angle.
     
190.   */
     
191. angle += K_0 * angle_err;
     
192. q_bias += K_1 * angle_err;
     
193. }
     

复制代码

qinlu123

#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 */
}
我还没测试，不知道行不行