|
下面是我用Matlab写的加速度计和陀螺仪融合的卡尔曼滤波器仿真代码,是根据这篇论文写的,但是出来的曲线跟陀螺仪的计算角度曲线是重合,请问一下是什么问题?
- close all;
- clear all;
- fid = fopen('./2014-11-11_03_13_25.dat', 'rb');
- fseek(fid,0,'eof');
- fsize = ftell(fid);
- fseek(fid,0,'bof');
- [CONFIG, CONFIG_SIZE] = fread(fid, 29, 'uchar');
- %卡尔曼滤波相关变量初始化
- T = 0.001;
- Q = [0.01 0;0 0.005];
- delta = 0;
- % r_k = 0.05;
- X_0 = [0;0];
- A = [1 -T;0 1];
- B = [(1-delta)*T;delta];
- C = [1 0]; %系统输出方程中的输出矩阵
- % P_k_1 = 0;
- R_k = [0.056];
- X_k_k = X_0;
- I = eye(2);
- P_k_k = [0.005 0.005;0.005 0.005];
- %获取文件数据
- data_size = int32((fsize -29)/12);
- Accel_factor = 4096.0;
- Gyro_factor = 65.5;
- accel_x = zeros(1,data_size);
- accel_y = zeros(1,data_size);
- accel_z = zeros(1,data_size);
- gyro_x = zeros(1,data_size);
- gyro_y = zeros(1,data_size);
- gyro_z = zeros(1,data_size);
- Aax = zeros(1,data_size);
- Aay = zeros(1,data_size);
- Aaz = zeros(1,data_size);
- Agx = zeros(1,data_size);
- Agy = zeros(1,data_size);
- Agz = zeros(1,data_size);
- Af_k_1 = zeros(1,data_size);
- Af_k_2 = zeros(1,data_size);
- for i = 1:1:data_size
- [Accel_X, Accel_X_Size] = fread(fid, 2, 'uchar');
- [Accel_Y, Accel_Y_Size] = fread(fid, 2, 'uchar');
- [Accel_Z, Accel_Z_Size] = fread(fid, 2, 'uchar');
- [Gyro_X, Gyro_X_Size] = fread(fid, 2, 'uchar');
- [Gyro_Y, Gyro_Y_Size] = fread(fid, 2, 'uchar');
- [Gyro_Z, Gyro_Z_Size] = fread(fid, 2, 'uchar');
- accel_x(i) = uint16Toint16(Accel_X(1) * 256 + Accel_X(2)) / Accel_factor;
- accel_y(i) = uint16Toint16(Accel_Y(1) * 256 + Accel_Y(2)) / Accel_factor;
- accel_z(i) = uint16Toint16(Accel_Z(1) * 256 + Accel_Z(2)) / Accel_factor;
- gyro_x(i) = uint16Toint16(Gyro_X(1) * 256 + Gyro_X(2) ) / Gyro_factor ;
- gyro_y(i) = uint16Toint16(Gyro_Y(1) * 256 + Gyro_Y(2) ) / Gyro_factor ;
- gyro_z(i) = uint16Toint16(Gyro_Z(1) * 256 + Gyro_Z(2) ) / Gyro_factor ;
- end
- %平滑滤波
- % accel_x=smooth(accel_x,50, 'moving');
- % accel_y=smooth(accel_y,50, 'moving');
- % accel_z=smooth(accel_z,50, 'moving');
- % gyro_x =smooth(gyro_x, 50, 'moving');
- % gyro_y =smooth(gyro_y, 50, 'moving');
- % gyro_z =smooth(gyro_z, 50, 'moving');
- for i=1:1:data_size
- if(i==1)
- %加速度计得到的角度
- Aax(i) = -atan2(accel_x(i), accel_z(i));
- Aay(i) = atan2(accel_y(i), accel_z(i));
- Aaz(i) = atan2(accel_x(i), accel_y(i));
- %陀螺仪得到的角度
- Agx(i) = Aay(1);
- Agy(i) = Aax(1);
- Agz(i) = Aaz(1);
- else
- %加速度计得到的角度
- Aax(i) = -atan2(accel_x(i), accel_z(i));
- Aay(i) = atan2(accel_y(i), accel_z(i));
- Aaz(i) = atan2(accel_x(i), accel_y(i));
- %陀螺仪得到的角度
- Agx(i) = Agx(i-1) + gyro_x(i) * T * pi / 180.0;
- Agy(i) = Agy(i-1) + gyro_y(i) * T * pi / 180.0;
- Agz(i) = Agz(i-1) + gyro_z(i) * T * pi / 180.0;
-
- end
- Ag_k = Aax(i);%k时刻加速度计输出的姿态角
- Aw_k = Agy(i);%k时刻陀螺仪输出的姿态角
- error_k = Aw_k - Ag_k; %姿态角在k时刻的偏差
-
- if(i==1)
- u_k = 0;
- else
- u_k = gyro_y(i-1) * T * pi / 180.0;
- end
- X_k_1_k_1 = X_k_k;
- X_k_k_1 = A * X_k_1_k_1 + B * u_k;
-
- P_k_1_k_1 = P_k_k;
- P_k_k_1 = A * P_k_1_k_1 * (A') + Q;
- %计算卡尔曼增益
- %P_k_1是k-1时刻的滤波协方差
- K_k = P_k_k_1 * (C') / (C * P_k_k_1 * (C') + R_k);
-
- X_k = [Aw_k;0];
- Y_k = C * X_k;
- X_k_k = X_k_k_1 + K_k * (Y_k - C * X_k_k_1);
- P_k_k = (I - K_k * C) * P_k_k_1;
- Af_k_1(i) = X_k_k(1);
- Af_k_2(i) = X_k_k(2);
- % 系统输出方程
- % Y_k = C * X_k;
- % 对姿态角预测值进行修正,Af_k为k时刻姿势角的最终输出值
- % Af_k(i) = Aw_k + K_k * error_k;
- % 计算下一步滤波算法中的滤波协方差
- % P_k_1 = P_k_1 - K_k * C * P_k_1;
- end
- plot(Aax, 'r');
- hold on;
- plot(Agy, 'g');
- hold on;
- plot(Af_k_1, 'b');
- hold on;
- plot(Af_k_2, 'c');
- hold on;
- fclose(fid);
复制代码
accel_x,accel_y和accel_z是加速度计的三轴数据。
gyro_x,gyro_y和gyro_z是陀螺仪的输出角速度数据。
仿真曲线如下:
其中红线是加速度计计算得到的角度,蓝线是卡尔曼滤波器输出的角度,绿色是陀螺仪积分计算得到的角度数据(绿线与蓝线重合了,所以是看到蓝线) |
本帖子中包含更多资源
您需要 登录 才可以下载或查看,没有帐号?注册
x
阿莫论坛20周年了!感谢大家的支持与爱护!!
你熬了10碗粥,别人一桶水倒进去,淘走90碗,剩下10碗给你,你看似没亏,其实你那10碗已经没有之前的裹腹了,人家的一桶水换90碗,继续卖。说白了,通货膨胀就是,你的钱是挣来的,他的钱是印来的,掺和在一起,你的钱就贬值了。
|