kalman滤波后的波形

繁＿、点点

红色是加速度计  黄色是陀螺仪 蓝色是滤波后的角度值   但是滤波后的角度值老是会有一个过冲  这是为什么？？怎么样才可以抑制

程序用的是下面的  也就是论坛里找的。

1.   float Kalman_Filter(float angle_m,float gyro_m)                        
   
2. {
   
3.         angle+=(gyro_m-q_bias) * dt;
   
4. 
   
5. 
   
6.         Pdot[0]=Q_angle - Pk[0][1] - Pk[1][0];
   
7.         Pdot[1]=- Pk[1][1];
   
8.         Pdot[2]=- Pk[1][1];
   
9.         Pdot[3]=Q_gyro;
   
10.         
    
11.         Pk[0][0] += Pdot[0] * dt;
    
12.         Pk[0][1] += Pdot[1] * dt;
    
13.         Pk[1][0] += Pdot[2] * dt;
    
14.         Pk[1][1] += Pdot[3] * dt;
    
15.         
    
16.         
    
17.         angle_err = angle_m - angle;
    
18.         
    
19.         
    
20.         PCt_0 = C_0 * Pk[0][0];
    
21.         PCt_1 = C_0 * Pk[1][0];
    
22.         
    
23.         E = R_angle + C_0 * PCt_0;
    
24.         
    
25.         K_0 = PCt_0 / E;
    
26.         K_1 = PCt_1 / E;
    
27.         
    
28.         t_0 = PCt_0;
    
29.         t_1 = C_0 * Pk[0][1];
    
30. 
    
31.         Pk[0][0] -= K_0 * t_0;
    
32.         Pk[0][1] -= K_0 * t_1;
    
33.         Pk[1][0] -= K_1 * t_0;
    
34.         Pk[1][1] -= K_1 * t_1;
    
35.         
    
36.         
    
37.         angle        += K_0 * angle_err;
    
38.         q_bias        += K_1 * angle_err;
    
39.         angle_dot = gyro_m-q_bias;
    
40.                                 return angle;
    
41. }

复制代码

音之夏萤

是不是陀螺仪比率大了

1627252997

波形挺好的

kmani

帮顶

kmani

楼主Q,R是固定的吗？

ludlee

毛刺

繁＿、点点

kmani 发表于 2012-12-29 18:00
楼主Q,R是固定的吗？

Q,R是固定的，难道你的不是固定的值？？

繁＿、点点

音之夏萤 发表于 2012-12-29 17:21
是不是陀螺仪比率大了

貌似不是的样子

1006110218

zqj2008

目测这个波形会往一个方向加速度