强跟踪滤波程序强跟踪UKF滤波主程序

主程序： figure_ukf 几个主要函数模块说明： 1、 sf_ukfm_sins_gps：强跟踪UKF滤波主程序 2、 shuaijianukfm_sins_gps：衰减记忆UKF滤波主程序； 3、ukfm_sins_gps：UKF滤波主程序

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%                    初始对准程序（粗对准）
%
%  输入参数:
%  Wibb       机体系陀螺仪输出   （单位：度/秒）
%  Fb         机体系加速度计输出 （单位：米/秒/秒）
%  posiN      经度、纬度、高度（单位：度、度、米）
%  输出参数：
%  attiN      横滚、俯仰、航向（单位：度）   
%      
%                           程序设计：熊智  日期：2002/8/9
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [attiN]=align_cal（WibbFbposiN）

  Wie=7.292115147e-5; %地球自转角速度
  g=9.7803698;        %重力加速度
  
  long=posiN（11）*pi/180.0;lati=posiN（21）*pi/180.0;heig=posiN（31）;
    %飞行器位置

  Wien=[0;  Wie*cos（lati）; Wie*sin（lati）];
  Gn  =[0;              0; g            ];
  Gman=cross（GnWien）;
    %东北天地理坐标系输出
    
  Wieb=Wibb*pi/180.0;    %弧度/秒
  Gmab=cross（FbWieb）;
    %IMU输出
      
  Cnb=inv（[Gn‘;Wien‘;Gman‘]）*[Fb‘;Wieb‘;Gmab‘];
  
  Cbn=Cnb‘;
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  %求姿态（横滚、俯仰、航向）
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  attiN（11）=atan（-Cbn（13）/Cbn（33））;
  attiN（21）=atan（Cbn（23）/sqrt（Cbn（21）*Cbn（21）+Cbn（22）*Cbn（22）））;
  attiN（31）=atan（Cbn（21）/Cbn（22））;
    %单位：弧度

  %象限判断
  attiN（11）=attiN（11）*180.0/pi;
  attiN（21）=attiN（21）*180.0/pi;
  attiN（31）=attiN（31）*180.0/pi;
    % 单位：度

  if（Cbn（22）<0 ） 
   attiN（31）=180.0+attiN（31）;
  else 
   if（Cbn（21）<0） attiN（31）=360.0+attiN（31）; end
  end
    %航向角度（单位：度）

  if（Cbn（33）<0）
   if（Cbn（13）>0） attiN（11）=180.0-attiN（11）; end
   if（Cbn（13）<0） attiN（11）=-（180.0+attiN（11））; end
  end
    %横滚角度（单位：度）

  
  
  
  

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

    .CA....      1740  2002-08-17 17:03  强跟踪滤波程序在组合导航中的应用研究\程序\align_cal.m

    .CA....      2851  2002-09-08 16:52  强跟踪滤波程序在组合导航中的应用研究\程序\atti_cal.m

    .CA....      3845  2002-09-11 16:11  强跟踪滤波程序在组合导航中的应用研究\程序\atti_cal_cq.m

    .CA....      4332  2002-09-11 16:14  强跟踪滤波程序在组合导航中的应用研究\程序\atti_cal_cq_modi.m

    .CA....     13503  2007-12-13 15:31  强跟踪滤波程序在组合导航中的应用研究\程序\dd1m_sins_gps.m

    .CA....      8273  2007-10-11 10:44  强跟踪滤波程序在组合导航中的应用研究\程序\dd1_fig.m

    .CA....      5720  2007-12-13 15:29  强跟踪滤波程序在组合导航中的应用研究\程序\dd1_gps.m

    .CA....      3663  2007-10-15 18:21  强跟踪滤波程序在组合导航中的应用研究\程序\dd1_gps_init.m

    .CA....      3049  2007-05-28 10:11  强跟踪滤波程序在组合导航中的应用研究\程序\dd1_modi.m

    .CA....     13084  2007-10-15 17:15  强跟踪滤波程序在组合导航中的应用研究\程序\dd1_rk.m

    .CA....      7229  2007-12-13 15:29  强跟踪滤波程序在组合导航中的应用研究\程序\dd1_tl.m

    .CA....     13461  2008-12-21 16:51  强跟踪滤波程序在组合导航中的应用研究\程序\dd2m_sins_gps.m

    .CA....      8175  2007-09-27 15:07  强跟踪滤波程序在组合导航中的应用研究\程序\dd2_fig.m

    .CA....      6650  2007-12-13 21:51  强跟踪滤波程序在组合导航中的应用研究\程序\dd2_gps.m

    .CA....      3663  2007-12-13 18:58  强跟踪滤波程序在组合导航中的应用研究\程序\dd2_gps_init.m

    .CA....      3049  2007-05-28 20:21  强跟踪滤波程序在组合导航中的应用研究\程序\dd2_modi.m

    .CA....     13081  2007-12-13 14:50  强跟踪滤波程序在组合导航中的应用研究\程序\dd2_rk.m

    .CA....      7058  2007-12-13 11:41  强跟踪滤波程序在组合导航中的应用研究\程序\dd2_tl.m

    .CA....      5306  2008-12-27 11:23  强跟踪滤波程序在组合导航中的应用研究\程序\figure_ukf.m

    .CA....      2817  2003-09-29 16:40  强跟踪滤波程序在组合导航中的应用研究\程序\IMUout.m

    .CA....      1891  2002-09-07 09:12  强跟踪滤波程序在组合导航中的应用研究\程序\imu_err_fix.m

    .CA....      3633  2002-09-05 15:14  强跟踪滤波程序在组合导航中的应用研究\程序\imu_err_modi.m

    .CA....      2123  2003-09-29 15:13  强跟踪滤波程序在组合导航中的应用研究\程序\imu_err_random.m

    .CA....      6884  2007-12-13 22:11  强跟踪滤波程序在组合导航中的应用研究\程序\integrated_err_com.m

    .CA....      6148  2003-10-06 09:00  强跟踪滤波程序在组合导航中的应用研究\程序\kalm_gps.m

    .CA....      2697  2003-10-08 16:58  强跟踪滤波程序在组合导航中的应用研究\程序\kalm_gps_init.m

    .CA....      2718  2003-10-06 09:17  强跟踪滤波程序在组合导航中的应用研究\程序\kalm_modi.m

    .CA....     14666  2008-04-24 09:57  强跟踪滤波程序在组合导航中的应用研究\程序\m_sins_gps.m

    .CA....      1062  2002-08-09 20:18  强跟踪滤波程序在组合导航中的应用研究\程序\posi_cal.m

    .CA....     13506  2008-12-21 17:02  强跟踪滤波程序在组合导航中的应用研究\程序\sf_ukfm_sins_gps.m

............此处省略27个文件信息