matlab组合导航代码

【核心代码】

  [t,atti,atti_rate,veloB,acceB]=trace(t,T,atti,atti_rate,veloB,acceB);
      %航迹发生器产生飞行轨迹参数
 
  [Wibb,Fb,posi]=IMUout(T,posi,atti,atti_rate,veloB,acceB);
      %陀螺和加速度仿真 

  [Gyro_fix,Acc_fix]=imu_err_fix(Wibb,Fb);
    %产生非随机性误差
  %Fb=Fb Acc_fix;  
  %Wibb=Wibb Gyro_fix/deg_rad;  % deg/s
    %加入非随机性误差

  [Gyro_b,Gyro_r,Gyro_wg,Acc_r]=imu_err_random(t,T,Gyro_b,Gyro_r,Gyro_wg,Acc_r);
    %产生随机性误差初始值（此时IMU进入正常工作状态，则随机常数已为常值）
  Fb=Fb Acc_r;
  Wibb=Wibb Gyro_b/deg_rad Gyro_r/deg_rad Gyro_wg/deg_rad;  % deg/s
    %加入随机性误差

  %disp('*******初始随机性误差（单位：度\小时,米/秒/秒）*********');
  %disp('陀螺随机常数，陀螺一阶马儿可夫，陀螺白噪声  | 加速度一阶马儿可夫');
  %disp([Gyro_b/deg_rad*3600.0,Gyro_r/deg_rad*3600.0,Gyro_wg/deg_rad*3600.0,Acc_r]);
  %disp('*******');
   
  %[attiN]=atti_cal(T,Wibb,attiN,veloN,posiN);                % 方向余弦算法
  %[attiN]=atti_cal_cq(T,Wibb,attiN,veloN,posiN); % 四元数算法
  [attiN,WnbbA_old]=atti_cal_cq_modi(T,Wibb-Gyro_modi/deg_rad,attiN,veloN,posiN,WnbbA_old);% 四元数算法(含等效转动矢量修正）
      %姿态角度求解
      
  [veloN]=velo_cal(T,Fb-Acc_modi,attiN,veloN,posiN);
      %比力变换

  [posiN]=posi_cal(T,veloN,posiN);
    %   定位计算
      
  [st_attiN,st_WnbbA_old]=atti_cal_cq_modi(T,Wibb-st_Gyro_modi/deg_rad,st_attiN,st_veloN,st_posiN,st_WnbbA_old);% 四元数算法(含等效转动矢量修正）
      %姿态角度求解
      
  [st_veloN]=velo_cal(T,Fb-st_Acc_modi,st_attiN,st_veloN,st_posiN);
      %比力变换

  [st_posiN]=posi_cal(T,st_veloN,st_posiN);
      %定位计算   
  t=t T;
 
  T_M = T_M T; kflag = 0;  
 
  if( T_M >= 1.0 )     
      [posiG,veloG]=simu_gps(t,posi,atti,veloB);
        %GPS输出
        
      T_M = 0.0; kflag = 1; 
      
      %if( t>=100 & t<=200 ) kflag = 0; end %GPS无效时的测试
      
        Xc = zeros(18,1); %闭环反馈校正，控制修正量初值为0   
      
      [Xc,PK,Xerr]=kalm_gps(t,T_D,Fb,attiN,veloN,posiN,posiG,veloG,Xc,PK,Xerr,kflag);
         %卡尔曼滤波  
     
      [attiN,veloN,posiN]=kalm_modi(attiN,veloN,posiN,Xc);
       %进行滤波修正
       
      Gyro_modi(1,1) = Xc(10,1) Xc(13,1);
      Gyro_modi(2,1) = Xc(11,1) Xc(14,1);
      Gyro_modi(3,1) = Xc(12,1) Xc(15,1);  
        %陀螺修正量
        
      Acc_modi(1,1) = Xc(16,1);
      Acc_modi(2,1) = Xc(17,1);
      Acc_modi(3,1) = Xc(18,1);      
        %加速度计修正量
        
      KALData = [KALData;t,Xerr];
      ErrData = [ErrData;t,Xc'];
         %保存仿真数据