%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%????????????????????初始對準(zhǔn)程序（粗對準(zhǔn)）
%
%??輸入?yún)?shù):
%??Wibb???????機體系陀螺儀輸出???（單位：度/秒）
%??Fb?????????機體系加速度計輸出?（單位：米/秒/秒）
%??posiN??????經(jīng)度、緯度、高度（單位：度、度、米）
%??輸出參數(shù)：
%??attiN??????橫滾、俯仰、航向（單位：度）???
%??????
%???????????????????????????程序設(shè)計：熊智??日期：2002/8/9
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function?[attiN]=align_cal（WibbFbposiN）

??Wie=7.292115147e-5;?%地球自轉(zhuǎn)角速度
??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）;
????%東北天地理坐標(biāo)系輸出
????
??Wieb=Wibb*pi/180.0;????%弧度/秒
??Gmab=cross（FbWieb）;
????%IMU輸出
??????
??Cnb=inv（[Gn‘;Wien‘;Gman‘]）*[Fb‘;Wieb‘;Gmab‘];
??
??Cbn=Cnb‘;
??%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
??%求姿態(tài)（橫滾、俯仰、航向）
??%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
??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??強跟蹤濾波程序在組合導(dǎo)航中的應(yīng)用研究\程序\align_cal.m

????.CA....??????2851??2002-09-08?16:52??強跟蹤濾波程序在組合導(dǎo)航中的應(yīng)用研究\程序\atti_cal.m

????.CA....??????3845??2002-09-11?16:11??強跟蹤濾波程序在組合導(dǎo)航中的應(yīng)用研究\程序\atti_cal_cq.m

????.CA....??????4332??2002-09-11?16:14??強跟蹤濾波程序在組合導(dǎo)航中的應(yīng)用研究\程序\atti_cal_cq_modi.m

????.CA....?????13503??2007-12-13?15:31??強跟蹤濾波程序在組合導(dǎo)航中的應(yīng)用研究\程序\dd1m_sins_gps.m

????.CA....??????8273??2007-10-11?10:44??強跟蹤濾波程序在組合導(dǎo)航中的應(yīng)用研究\程序\dd1_fig.m

????.CA....??????5720??2007-12-13?15:29??強跟蹤濾波程序在組合導(dǎo)航中的應(yīng)用研究\程序\dd1_gps.m

????.CA....??????3663??2007-10-15?18:21??強跟蹤濾波程序在組合導(dǎo)航中的應(yīng)用研究\程序\dd1_gps_init.m

????.CA....??????3049??2007-05-28?10:11??強跟蹤濾波程序在組合導(dǎo)航中的應(yīng)用研究\程序\dd1_modi.m

????.CA....?????13084??2007-10-15?17:15??強跟蹤濾波程序在組合導(dǎo)航中的應(yīng)用研究\程序\dd1_rk.m

????.CA....??????7229??2007-12-13?15:29??強跟蹤濾波程序在組合導(dǎo)航中的應(yīng)用研究\程序\dd1_tl.m

????.CA....?????13461??2008-12-21?16:51??強跟蹤濾波程序在組合導(dǎo)航中的應(yīng)用研究\程序\dd2m_sins_gps.m

????.CA....??????8175??2007-09-27?15:07??強跟蹤濾波程序在組合導(dǎo)航中的應(yīng)用研究\程序\dd2_fig.m

????.CA....??????6650??2007-12-13?21:51??強跟蹤濾波程序在組合導(dǎo)航中的應(yīng)用研究\程序\dd2_gps.m

????.CA....??????3663??2007-12-13?18:58??強跟蹤濾波程序在組合導(dǎo)航中的應(yīng)用研究\程序\dd2_gps_init.m

????.CA....??????3049??2007-05-28?20:21??強跟蹤濾波程序在組合導(dǎo)航中的應(yīng)用研究\程序\dd2_modi.m

????.CA....?????13081??2007-12-13?14:50??強跟蹤濾波程序在組合導(dǎo)航中的應(yīng)用研究\程序\dd2_rk.m

????.CA....??????7058??2007-12-13?11:41??強跟蹤濾波程序在組合導(dǎo)航中的應(yīng)用研究\程序\dd2_tl.m

????.CA....??????5306??2008-12-27?11:23??強跟蹤濾波程序在組合導(dǎo)航中的應(yīng)用研究\程序\figure_ukf.m

????.CA....??????2817??2003-09-29?16:40??強跟蹤濾波程序在組合導(dǎo)航中的應(yīng)用研究\程序\IMUout.m

????.CA....??????1891??2002-09-07?09:12??強跟蹤濾波程序在組合導(dǎo)航中的應(yīng)用研究\程序\imu_err_fix.m

????.CA....??????3633??2002-09-05?15:14??強跟蹤濾波程序在組合導(dǎo)航中的應(yīng)用研究\程序\imu_err_modi.m

????.CA....??????2123??2003-09-29?15:13??強跟蹤濾波程序在組合導(dǎo)航中的應(yīng)用研究\程序\imu_err_random.m

????.CA....??????6884??2007-12-13?22:11??強跟蹤濾波程序在組合導(dǎo)航中的應(yīng)用研究\程序\integrated_err_com.m

????.CA....??????6148??2003-10-06?09:00??強跟蹤濾波程序在組合導(dǎo)航中的應(yīng)用研究\程序\kalm_gps.m

????.CA....??????2697??2003-10-08?16:58??強跟蹤濾波程序在組合導(dǎo)航中的應(yīng)用研究\程序\kalm_gps_init.m

????.CA....??????2718??2003-10-06?09:17??強跟蹤濾波程序在組合導(dǎo)航中的應(yīng)用研究\程序\kalm_modi.m

????.CA....?????14666??2008-04-24?09:57??強跟蹤濾波程序在組合導(dǎo)航中的應(yīng)用研究\程序\m_sins_gps.m

????.CA....??????1062??2002-08-09?20:18??強跟蹤濾波程序在組合導(dǎo)航中的應(yīng)用研究\程序\posi_cal.m

????.CA....?????13506??2008-12-21?17:02??強跟蹤濾波程序在組合導(dǎo)航中的應(yīng)用研究\程序\sf_ukfm_sins_gps.m

............此處省略27個文件信息