z_allan_new.m

阿兰方差MATLAb实用程序。很实用的。用于误差分析。

%********************************************************************************************%
%                 名称：allan_new.m
%                 用途：计算50Hz数据文件vg_50Hz.dat的Allan方差，并将结果存在文件allan_con.txt中
%                 说明：该程序是在原allan程序基础上规范、改进而成，速度加快了很多。
%
%                 编制：赖际舟
%                 单位：南京航空航天大学导航研究中心
%                 日期：2004.09.13
%********************************************************************************************%
 clear;
 imu_data =load('mimu2.dat');            %调用数据文件vg_50Hz.dat，该数据文件为已经提取过的半小时数据，单位mv
% mimu1=mimu2;
% X=vgnew(180000:270000,2)/22.7*3600;       %调用数据文件中并转换单位为deg/h



%   double Da[3] = { 40.2367597754,  -39.7091780864,   38.7527369572};//m/s/s
%   double Ka[3] = { 12146.0576750000,12316.3713000000,12617.1384000000};
%                  // g/ pulses
%   double Dg[3] = { 0.0000002683,    0.0000001610,    0.0000000645};//rad/s
%   double Kg[3] = { 0.9320,0.9320,0.9320}; // "/s/per pulse
% 
% 
%   imu_data[0]= imu_data[0]*50.0/(Ka[0])*earth_g-Da[0];  // acc
%   imu_data[1]= imu_data[1]*50.0/(Ka[1])*earth_g-Da[1];  // test_
%   imu_data[2]= imu_data[2]*50.0/(Ka[2])*earth_g-Da[2];
% 
%   imu_data[3]= imu_data[3]*50.0*Kg[0]/3600.0*deg_rad-Dg[0];
%   imu_data[4]= imu_data[4]*50.0*Kg[1]/3600.0*deg_rad-Dg[1];
%   imu_data[5]= imu_data[5]*50.0*Kg[2]/3600.0*deg_rad-Dg[2]; // rad/s

deg_rad=0.01745329252e0;% Transfer from angle degree to rad
earth_g=9.7803698;       %重力加速度    （单位：米/秒/秒）


   Da = [ 40.2367597754,  -39.7091780864,   38.7527369572];    %%  //m/s/s
   Ka = [ 12146.0576750000,12316.3713000000,12617.1384000000]; %%  // g/ pulses
   Dg = [ 0.0000002683,    0.0000001610,    0.0000000645];     %% //rad/s
   Kg = [ 0.9320,0.9320,0.9320];                               %% // "/s/per pulse

%%%  to  m/s/s  Acc
  imu_data(:,1)= imu_data(:,1)*50.0/(Ka(1,1))*earth_g-Da(1,1);%  // acc
  imu_data(:,2)= imu_data(:,2)*50.0/(Ka(1,2))*earth_g-Da(1,2);%  // test_
  imu_data(:,3)= imu_data(:,3)*50.0/(Ka(1,3))*earth_g-Da(1,3);
%%%  to rad/s  gyro
  imu_data(:,4)= imu_data(:,4)*50.0*Kg(1,1)/3600.0*deg_rad-Dg(1,1);
  imu_data(:,5)= imu_data(:,5)*50.0*Kg(1,2)/3600.0*deg_rad-Dg(1,2);
  imu_data(:,6)= imu_data(:,6)*50.0*Kg(1,3)/3600.0*deg_rad-Dg(1,3);% // rad/s

%%%  to  G   acc
  imu_data(:,1)= imu_data(:,1)/earth_g;
  imu_data(:,2)= imu_data(:,2)/earth_g;
  imu_data(:,3)= imu_data(:,3)/earth_g;
%%%  to deg/h  gyro
  imu_data(:,4)= imu_data(:,4)/deg_rad*3600;
  imu_data(:,5)= imu_data(:,5)/deg_rad*3600;
  imu_data(:,6)= imu_data(:,6)/deg_rad*3600;

  
 mimu1=imu_data;


for kcol=6:6  %% for the first line to the final line. 
kcol

X=mimu1(:,kcol);       %调用数据文件中并转换单位为deg/h

%对数据分段求和
  dt=0.02;                                  % 采样时间0.02s
  N_num = length(X(:,1));          % 数据样本长度
%  K_max =  fix((N_num-1)/2);     %最大的数据段的长度
  K_max =  fix((N_num-1)/3);     %最大的数据段的长度
  
  Allan = zeros(K_max,2);
  seg_mean=zeros(N_num,1);

  for inte_time=1:1:K_max                % 按照数据长度来划分，则积分时间为： inte_time*Ts    ；
     seg_num=fix(N_num/inte_time);              %  根据当前积分时间所分的段数；
     for k=1:seg_num
          seg_mean(k,1) = mean(  X( ((k-1)*inte_time+1):(k*inte_time),1 )  );   % 求出每一段的均值；
      end
     diff_mean  = diff(seg_mean(1:seg_num,1));                                             %求出每相邻两段数据均值的差；
     Allan(inte_time,2) = sqrt(sum(diff_mean.^2)/(2*(seg_num-1)) );               %求出Allan方差
     Allan(inte_time,1) = inte_time*dt;                                                              %Allan对应的时间
     
     if( mod(inte_time,100)==0) 
         inte_time;
     end
     
 end

if(kcol ==1)
    f_kf=fopen('kcol_1.dat','wt');  %记录获得的原始数据和消噪后数据（它们是向量）到文本文件中。
    for kc=1:length(Allan)
       fprintf(f_kf,'%16.7e %16.7e\n ',Allan(kc,:));
    end
    fclose(f_kf);
end

if(kcol ==1)
    f_kf=fopen('kcol_1.dat','wt');  %记录获得数据到文本文件中。
    for kc=1:length(Allan)
       fprintf(f_kf,'%16.7e %16.7e\n ',Allan(kc,:));
    end
    fclose(f_kf);
end
if(kcol ==2)
    f_kf=fopen('kcol_2.dat','wt');  %%记录获得数据到文本文件中。
    for kc=1:length(Allan)
       fprintf(f_kf,'%16.7e %16.7e\n ',Allan(kc,:));
    end
    fclose(f_kf);
end
if(kcol ==3)
    f_kf=fopen('kcol_3.dat','wt');  %%记录获得数据到文本文件中。
    for kc=1:length(Allan)
       fprintf(f_kf,'%16.7e %16.7e\n ',Allan(kc,:));
    end
    fclose(f_kf);
end
if(kcol ==4)
    f_kf=fopen('kcol_4.dat','wt');  %%记录获得数据到文本文件中。
    for kc=1:length(Allan)
       fprintf(f_kf,'%16.7e %16.7e\n ',Allan(kc,:));
    end
    fclose(f_kf);
end
if(kcol ==5)
    f_kf=fopen('kcol_5.dat','wt');  %%记录获得数据到文本文件中。
    for kc=1:length(Allan)
       fprintf(f_kf,'%16.7e %16.7e\n ',Allan(kc,:));
    end
    fclose(f_kf);
end
if(kcol ==6)
    f_kf=fopen('kcol_6.dat','wt');  %%记录获得数据到文本文件中。
    for kc=1:length(Allan)
       fprintf(f_kf,'%16.7e %16.7e\n ',Allan(kc,:));
    end
    fclose(f_kf);
end

 
 figure(kcol);
 loglog(Allan(:,1),Allan(:,2));                          % 画出Allan平方曲线
 xlabel('Allan对应的时间(秒)'); 
 
 if(kcol<4)
     ylabel('加速度计（G）');
 end
 if(kcol>3)
     ylabel('陀螺精度(度/小时)');
 end
 
 grid;
 
 A = z_least_square(Allan(:,1),Allan(:,2));                                                          %进行最小二乘拟合
 
  N_allan=sqrt(abs(A(4,1)))/60;                                                   % 角度随机游走系数 (deg/sqrt(h))
  B_allan=sqrt(abs(A(1,1)))/0.664;                                                     % 零偏稳定性系数(deg/h)
  K_allan=60*sqrt(3*abs(A(2,1)));                                                     % 速率随机游走系数(deg/h^(3/2))
  R_allan=3600*sqrt(2*abs(A(3,1)));                                                      % 速率斜坡 (deg/h^2)
  Q_allan=10e6*3.14*sqrt(abs(A(5,1)))/(180*3600*sqrt(3));                                    % 量化噪声(urad)
 
  
  tt = [N_allan,B_allan,K_allan,R_allan,Q_allan];
  disp('********   kcol and the Allan para. ************');
  kcol
  tt
  
  clear Allan;
  clear seg_mean;
  clear diff_mean;
end