MEMSNEW_matlabIMU滤波_陀螺仪_卡尔曼陀螺_陀螺仪滤波_陀螺仪噪声_

close all clear load('data'); %读取原始数据 load('data'); T=10/1000;%采样周期 10ms N=size(data,1); time = T:T:N*T; NN=floor(size(data,1)*T); %-----------------------------------提取陀螺数据并进行单位转换-----------------------------------% gyro_temp = data(:,4:6)./16.4; gyro=zeros(N,3); gyro(:,1) = gyro_temp(:,2); gyro(:,2) = gyro_temp(:,1); gyro(:,3) = -gyro_temp(:,3); % image1=figure(1); % subplot(3,1,1); % plot(time,gyro(:,1)); % title('x陀螺数据'); % xlabel('时间/s'); % ylabel('°/s'); % hold on; % subplot(3,1,2); % plot(time,gyro(:,2)); % title('y陀螺数据'); % xlabel('时间/s'); % ylabel('°/s'); % % subplot(3,1,3); % plot(time,gyro(:,3)); % title('z陀螺数据'); % xlabel('时间/s'); % ylabel('°/s'); %% gyro_x=gyro(:,1); gyro_y=gyro(:,1); gyro_z=gyro(:,1); %计算去噪声前的x陀螺allan方差 for i=1:NN gyro_x_1s(i)=sum(gyro_x((i-1)*100+1:i*100,:),1)/100; end figure; subplot(2,1,1); plot(time,gyro(:,1)); title('x陀螺数据(100Hz)'); xlabel('时间/s'); ylabel('°/s'); subplot(2,1,2); time2=1:size(gyro_x_1s,2) plot(time2,gyro_x_1s); title('x陀螺数据(1Hz)'); xlabel('时间/s'); ylabel('°/s'); %% % gyro_x=gyro(1:1000,1); gyro_x_1s_before=gyro_x_1s; gyro_x_1s=gyro_x_1s-mean(gyro_x_1s); figure; plot(time2,gyro_x_1s); title('随机漂移噪声)'); xlabel('时间/s'); ylabel('°/s'); %% tt0 = 1; condifential_level = 0.2; gyro_x_allan_before= allan_variance_claculate(gyro_x_1s',tt0,condifential_level); figure;axis equal; plot((gyro_x_allan_before(:,1)),sqrt(gyro_x_allan_before(:,2)),'-o'); xlabel('t:s');ylabel('allan std var : deg/h'); %% %计算去噪声前的y陀螺allan方差 % for i=1:NN % gyro_y_1s(i)=sum(gyro_y((i-1)*100+1:i*100,:),1); % end % gyro_y_1s=gyro_y_1s-mean(gyro_y_1s); % tt0 = 1; % condifential_level = 0.2; % gyro_y_allan_before= allan_variance_claculate(gyro_y_1s',tt0,condifential_level); % figure;axis equal; % plot((gyro_y_allan_before(:,1)),sqrt(gyro_y_allan_before(:,2)),'-o'); % xlabel('t:s');ylabel('allan std var : deg/h'); % set(gca,'Xscale','log');set(gca,'Yscale','log'); %% %计算去噪声前的z陀螺allan方差 % for i=1:NN % gyro_z_1s(i)=sum(gyro_z((i-1)*100+1:i*100,:),1); % end % gyro_z_1s=gyro_z_1s-mean(gyro_z_1s); % tt0 = 1; % condifential_level = 0.2; % gyro_z_allan_before= allan_variance_claculate(gyro_z_1s',tt0,condifential_level); % figure;axis equal; % plot((gyro_z_allan_before(:,1)),sqrt(gyro_z_allan_before(:,2)),'-o'); % xlabel('t:s');ylabel('allan std var : deg/h'); % set(gca,'Xscale','log');set(gca,'Yscale','log'); %% %计算AR（1）模型系数 [a_gyro_x] = ar(gyro_x_1s,1); %% %建立卡尔曼滤波器，去除陀螺中的随机漂移误差 KFx1=0; KFP1=1; Phi=a_gyro_x.A(2); KFC=1; QQ=6.1199e-5; KFR=1e-4; KFx_res = zeros(1,NN); for i = 1:NN KFz=gyro_x_1s(1,i); KFx2p=Phi*KFx1; KFP2p=Phi*KFP1*Phi'+QQ; KFK=KFP2p*KFC'*inv(KFC*KFP2p*KFC'+KFR); KFx2=KFx2p+KFK*(KFz-KFC*KFx2p); KFP2=(eye(length(KFx1))-KFK*KFC)*KFP2p*(eye(length(KFx1))-KFK*KFC)'+KFK*KFR*KFK'; %保存结果 KFx1=KFx2; KFP1=KFP2p; KFx_res(1,i)=KFx1; end figure; time=1:1:NN; plot(time,KFx_res); title('卡尔曼滤波器估计陀螺随机漂移误差'); xlabel('时间/s'); ylabel('°/s'); %% figure; time=1:1:NN; plot(time,gyro_x_1s_before,'r'); hold on; plot(time,gyro_x_1s_before-KFx_res,'b'); xlabel('时间/s'); ylabel('°/s'); legend('去噪声前陀螺输出','去噪后陀螺输出'); %% gyro_x_1s_after=gyro_x_1s-KFx_res; gyro_x_allan_after= allan_variance_claculate((gyro_x_1s_after)',tt0,condifential_level); figure;axis equal; plot((gyro_x_allan_before(:,1)),sqrt(gyro_x_allan_before(:,2)),'-ro'); hold on; plot((gyro_x_allan_after(:,1)),sqrt(gyro_x_allan_after(:,2)),'-bo'); xlabel('t:s');ylabel('allan std var : deg/h'); legend('去噪声前allan方差','去噪后allan方差');