clear?all;clc

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%?初?始?化?%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp（‘初始化‘）;
%%%%%%%地球數據
WIE??=?7.292115e-5?;%（地球自轉角速度，弧度制commented?by?LiYou）
e???=?0.0818191908426;
R???=?6378137.0;
Tm?=?0.01;
%%%%%%%速度初值
Vn?=?0;
Vt?=?0;
Ve?=?0;
V_last?=?[VnVtVe]‘;
%%%位置初值
Lati??=?28.2202*pi/180;
Longti?=?112.9916*pi/180;
Alti??????=?0;
g0=9.7803267711905*（1+0.00193185138639*sin（Lati）^2）/（（1-0.00669437999013*sin（Lati）^2）^0.5*（1.0+Alti/R）^2）;
Pos?=?zeros（31）;
disp（‘初始化完成‘）;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%?初?始?化?完?成?%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%載?入?數?據?%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp（‘載入數據‘）;
load?d_GyroAcc2012.dat
[row?columu]?=?size（d_GyroAcc2012）;
disp（‘載入數據完成‘）
%%%%%%%%%%%%%%%%%%%%%%載?入?數?據?完?成?%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%???解?析?初?對?準?%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp（‘解析粗對準開始‘）;

Data_1???=?zeros（120006）;
Data_1（:1）?=?d_GyroAcc2012（1:120001）;
Data_1（:2）?=?d_GyroAcc2012（1:120002）;
Data_1（:3）?=d_GyroAcc2012（1:120003）;?
Data_1（:4）?=?d_GyroAcc2012（1:120004）;
Data_1（:5）?=?d_GyroAcc2012（1:120005）;
Data_1（:6）?=?d_GyroAcc2012（1:120006）;
%陀螺數據
W_1?=?Data_1（:1:3）‘;
%加表
A_1?=?Data_1（:4:6）‘;
%載體系
W????=?mean（W_12）*100;
G????=?mean（A_12）*100;%（實驗數據為?1北?2天?3東，時間間隔為0.01s?；commented?by?liyou）
W_G??=?cross（WG）;
A????=?[W??-G?-W_G];
%地理系
Wien?=?[WIE*cos（Lati）?WIE*sin（Lati）?0]‘;
gn???=g0;
Gn???=?[0?-gn?0]‘;
Wn_Gn=?cross（WienGn）;
B????=?[Wien?Gn?Wn_Gn];
%姿態矩陣Cnb表示從N到B
Cnb_1=?A/B;
Cnb_1?=?Cnb_1?*?（Cnb_1‘?*?Cnb_1）^（-1/2）;
Cbn_1?=?Cnb_1‘;
%%%%姿態角，按照先產生YAW，再產生PITCH，再產生ROLL的順序
Yaw???=?atan2（-Cnb_1（13）Cnb_1（11））*180/pi;
Pitch?=?asin（Cnb_1（12））*180/pi;
Roll??=?atan2（-Cnb_1（32）Cnb_1（22））*180/pi;

disp（‘解析粗對準結果：‘）;
disp（‘偏航角?=?‘）disp（Yaw）;
disp（‘俯仰角?=?‘）disp（Pitch）;
disp（‘滾動角?=?‘）disp（Roll）;
disp（‘解析粗對準結束‘）;
%%%%%%%%%%%%%%%%%%%%%%%%?初?對?準?結?束?%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%????精??對??準??%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp（‘精對準開始‘）;
Data_2???=?zeros（480006）;
Data_2（:1）?=?d_GyroAcc2012（12001:600001）;
Data_2（:2）?=?d_GyroAcc2012（12001:600002）;
Data_2（:3）?=?d_GyroAcc2012（12001:600003）;
Data_2（:4）?=?d_GyroAcc2012（12001:600004）;
Data_2（:5）?=?d_GyroAcc2012（12001:600005）;
Data_2（:6）?=?d_GyroAcc2012（12001:600006）;
%陀螺
W_2?=?Data_2（:

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----

?????文件???44535696??2011-05-22?16:30??NavigationProgram\d_GyroAcc2012.dat

?????文件??????12232??2013-12-26?16:05??NavigationProgram\INS_program_Ver2.m

?????目錄??????????0??2014-06-13?09:28??NavigationProgram

-----------?---------??----------?-----??----

?????????????44547928????????????????????3