[求助]Matlab仿真的卡尔曼滤波器工作不正常。

zsjinwei

下面是我用Matlab写的加速度计和陀螺仪融合的卡尔曼滤波器仿真代码，是根据这篇论文写的，但是出来的曲线跟陀螺仪的计算角度曲线是重合，请问一下是什么问题？

1. close all;
   
2. clear all;
   
3. fid = fopen('./2014-11-11_03_13_25.dat', 'rb');
   
4. fseek(fid,0,'eof');
   
5. fsize = ftell(fid);
   
6. fseek(fid,0,'bof');
   
7. [CONFIG, CONFIG_SIZE] = fread(fid, 29, 'uchar');
   
8. %卡尔曼滤波相关变量初始化
   
9. T = 0.001;
   
10. Q = [0.01 0;0 0.005];
    
11. delta = 0;
    
12. % r_k = 0.05;
    
13. X_0 = [0;0];
    
14. A = [1 -T;0 1];
    
15. B = [(1-delta)*T;delta];
    
16. C = [1 0];  %系统输出方程中的输出矩阵
    
17. % P_k_1 = 0;
    
18. R_k = [0.056];
    
19. X_k_k = X_0;
    
20. I = eye(2);
    
21. P_k_k = [0.005 0.005;0.005 0.005];
    
22. %获取文件数据
    
23. data_size = int32((fsize -29)/12);
    
24. Accel_factor = 4096.0;
    
25. Gyro_factor = 65.5;
    
26. accel_x = zeros(1,data_size);
    
27. accel_y = zeros(1,data_size);
    
28. accel_z = zeros(1,data_size);
    
29. gyro_x = zeros(1,data_size);
    
30. gyro_y = zeros(1,data_size);
    
31. gyro_z = zeros(1,data_size);
    
32. Aax = zeros(1,data_size);
    
33. Aay = zeros(1,data_size);
    
34. Aaz = zeros(1,data_size);
    
35. Agx = zeros(1,data_size);
    
36. Agy = zeros(1,data_size);
    
37. Agz = zeros(1,data_size);
    
38. Af_k_1 = zeros(1,data_size);
    
39. Af_k_2 = zeros(1,data_size);
    
40. for i = 1:1:data_size
    
41.         [Accel_X, Accel_X_Size] = fread(fid, 2, 'uchar');
    
42.         [Accel_Y, Accel_Y_Size] = fread(fid, 2, 'uchar');
    
43.         [Accel_Z, Accel_Z_Size] = fread(fid, 2, 'uchar');
    
44.         [Gyro_X, Gyro_X_Size] = fread(fid, 2, 'uchar');
    
45.         [Gyro_Y, Gyro_Y_Size] = fread(fid, 2, 'uchar');
    
46.         [Gyro_Z, Gyro_Z_Size] = fread(fid, 2, 'uchar');
    
47.         accel_x(i) = uint16Toint16(Accel_X(1) * 256 + Accel_X(2)) / Accel_factor;
    
48.         accel_y(i) = uint16Toint16(Accel_Y(1) * 256 + Accel_Y(2)) / Accel_factor;
    
49.         accel_z(i) = uint16Toint16(Accel_Z(1) * 256 + Accel_Z(2)) / Accel_factor;
    
50.         gyro_x(i)  =  uint16Toint16(Gyro_X(1)  * 256 + Gyro_X(2) ) / Gyro_factor ;
    
51.         gyro_y(i)  =  uint16Toint16(Gyro_Y(1)  * 256 + Gyro_Y(2) ) / Gyro_factor ;
    
52.         gyro_z(i)  =  uint16Toint16(Gyro_Z(1)  * 256 + Gyro_Z(2) ) / Gyro_factor ;
    
53. end
    
54. %平滑滤波
    
55. % accel_x=smooth(accel_x,50, 'moving');
    
56. % accel_y=smooth(accel_y,50, 'moving');
    
57. % accel_z=smooth(accel_z,50, 'moving');
    
58. % gyro_x =smooth(gyro_x, 50, 'moving');
    
59. % gyro_y =smooth(gyro_y, 50, 'moving');
    
60. % gyro_z =smooth(gyro_z, 50, 'moving');
    
61. 
    
62. for i=1:1:data_size
    
63.         if(i==1)
    
64.                 %加速度计得到的角度
    
65.                 Aax(i) = -atan2(accel_x(i), accel_z(i));
    
66.                 Aay(i) = atan2(accel_y(i), accel_z(i));
    
67.                 Aaz(i) = atan2(accel_x(i), accel_y(i));
    
68.                 %陀螺仪得到的角度
    
69.                 Agx(i) = Aay(1);
    
70.                 Agy(i) = Aax(1);
    
71.                 Agz(i) = Aaz(1);
    
72.         else
    
73.                 %加速度计得到的角度
    
74.                 Aax(i) = -atan2(accel_x(i), accel_z(i));
    
75.                 Aay(i) = atan2(accel_y(i), accel_z(i));
    
76.                 Aaz(i) = atan2(accel_x(i), accel_y(i));
    
77.                 %陀螺仪得到的角度
    
78.                 Agx(i) = Agx(i-1) + gyro_x(i) * T * pi / 180.0;
    
79.                 Agy(i) = Agy(i-1) + gyro_y(i) * T * pi / 180.0;
    
80.                 Agz(i) = Agz(i-1) + gyro_z(i) * T * pi / 180.0;
    
81.                
    
82.         end
    
83.         Ag_k = Aax(i);%k时刻加速度计输出的姿态角
    
84.         Aw_k = Agy(i);%k时刻陀螺仪输出的姿态角
    
85.         error_k = Aw_k - Ag_k; %姿态角在k时刻的偏差
    
86.        
    
87.         if(i==1)
    
88.                 u_k = 0;
    
89.         else
    
90.                 u_k = gyro_y(i-1) * T * pi / 180.0;
    
91.         end
    
92.         X_k_1_k_1 = X_k_k;
    
93.         X_k_k_1 = A * X_k_1_k_1  + B * u_k;
    
94.        
    
95.         P_k_1_k_1 = P_k_k;
    
96.         P_k_k_1 = A * P_k_1_k_1 * (A') + Q;
    
97.         %计算卡尔曼增益
    
98.         %P_k_1是k-1时刻的滤波协方差
    
99.         K_k = P_k_k_1 * (C') / (C * P_k_k_1 * (C') + R_k);
    
100.        
     
101.         X_k = [Aw_k;0];
     
102.         Y_k = C * X_k;
     
103.         X_k_k = X_k_k_1 + K_k * (Y_k - C * X_k_k_1);
     
104.         P_k_k = (I - K_k * C) * P_k_k_1;
     
105.         Af_k_1(i) = X_k_k(1);
     
106.         Af_k_2(i) = X_k_k(2);
     
107.         % 系统输出方程
     
108.         % Y_k = C * X_k;
     
109.         % 对姿态角预测值进行修正,Af_k为k时刻姿势角的最终输出值
     
110.         % Af_k(i) = Aw_k + K_k * error_k;
     
111.         % 计算下一步滤波算法中的滤波协方差
     
112.         % P_k_1 = P_k_1 - K_k * C * P_k_1;
     
113. end
     
114. plot(Aax, 'r');
     
115. hold on;
     
116. plot(Agy, 'g');
     
117. hold on;
     
118. plot(Af_k_1, 'b');
     
119. hold on;
     
120. plot(Af_k_2, 'c');
     
121. hold on;
     
122. fclose(fid);

复制代码

accel_x，accel_y和accel_z是加速度计的三轴数据。
gyro_x，gyro_y和gyro_z是陀螺仪的输出角速度数据。
仿真曲线如下：


其中红线是加速度计计算得到的角度，蓝线是卡尔曼滤波器输出的角度，绿色是陀螺仪积分计算得到的角度数据（绿线与蓝线重合了，所以是看到蓝线）

zsjinwei

补充一下，浅蓝色线是状态矩阵X_k_k中的error_k曲线