• 大小: 87KB
    文件类型: .rar
    金币: 1
    下载: 0 次
    发布日期: 2021-05-25
  • 语言: 其他
  • 标签: 跟踪  

资源简介

主程序: 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个文件信息

评论

共有 条评论