%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%?????????指北方位捷聯式慣性導航系統?????????%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%初始化
clear

load（‘jlfw‘）;

%定義常量
Re=6378245;??????????????????????????????????????????????????????????
e=1/298.3;
wie_scalar=7.292115147e-5;
g0=9.7803267714;
gk1=0.00193185138639;
gk2=0.00669437999013;
%變量賦初值
longitude=[116.344695283*pi/180?zeros（147999）];?
latitude=[39.975172*pi/180?zeros（147999）];
h=30;
f=zeros（348000）;
wnb=zeros（348000）;
theta=[0.120992605?zeros（147999）]*pi/180;
gamma=[0.010445947?zeros（147999）]*pi/180;
psi=[360-91.637207?zeros（147999）]*pi/180;
Q=[cos（psi（1）/2）*cos（theta（1）/2）*cos（gamma（1）/2）+sin（psi（1）/2）*sin（theta（1）/2）*sin（gamma（1）/2）;
???cos（psi（1）/2）*sin（theta（1）/2）*cos（gamma（1）/2）+sin（psi（1）/2）*cos（theta（1）/2）*sin（gamma（1）/2）;
???cos（psi（1）/2）*cos（theta（1）/2）*sin（gamma（1）/2）-sin（psi（1）/2）*sin（theta（1）/2）*cos（gamma（1）/2）;
???cos（psi（1）/2）*sin（theta（1）/2）*sin（gamma（1）/2）-sin（psi（1）/2）*cos（theta（1）/2）*cos（gamma（1）/2）];
velocity=[0.000048637?0.000206947?0.007106781];
v=[zeros（148000）];

for?i=2:48000
%計算所在地參數
??%Rx為卯酉圈的曲率半徑
??Rx=Re/（1-e*sin（latitude（i-1））^2）;
??%Ry為子午圈的曲率半徑
??Ry=Re/（1+2*e-3*e*sin（latitude（i-1））^2）;
??%使用WGS-84模型計算該位置的重力加速度
??g=g0*（1+gk1*sin（latitude（i-1））^2）*（1-2*h/Re）/sqrt（1-gk2*sin（latitude（i-1））^2）;
??
??%計算姿態矩陣
??Cnb=[Q（1）^2+Q（2）^2-Q（3）^2-Q（4）^2?2*（Q（2）*Q（3）-Q（1）*Q（4））?2*（Q（2）*Q（4）+Q（1）*Q（3））;
???????2*（Q（2）*Q（3）+Q（1）*Q（4））?Q（1）^2-Q（2）^2+Q（3）^2-Q（4）^2?2*（Q（3）*Q（4）-Q（1）*Q（2））;
???????2*（Q（2）*Q（4）-Q（1）*Q（3））?2*（Q（3）*Q（4）+Q（1）*Q（2））?Q（1）^2-Q（2）^2-Q（3）^2+Q（4）^2];
???
??Cbn=[Q（1）^2+Q（2）^2-Q（3）^2-Q（4）^2?2*（Q（2）*Q（3）+Q（1）*Q（4））?2*（Q（2）*Q（4）-Q（1）*Q（3））;
???????2*（Q（2）*Q（3）-Q（1）*Q（4））?Q（1）^2-Q（2）^2+Q（3）^2-Q（4）^2?2*（Q（3）*Q（4）+Q（1）*Q（2））;
???????2*（Q（2）*Q（4）+Q（1）*Q（3））?2*（Q（3）*Q（4）-Q（1）*Q（2））?Q（1）^2-Q（2）^2-Q（3）^2+Q（4）^2];
??
??%使用畢卡法（角增量法）求解四元數微分方程
??win=[-velocity（2）/Ry;wie_scalar*cos（latitude（i-1））+velocity（1）/Rx;
??????wie_scalar*sin（latitude（i-1））+velocity（1）/Rx*tan（latitude（i-1））];
??wnb（:i）=wib_INSc（:i）-Cbn*win;
??delta_big_theta=[0?-wnb（1i）?-wnb（2i）?-wnb（3i）;
?????????????wnb（1i）?0?wnb（3i）?-wnb（2i）;
?????????????wnb（2i）?-wnb（3i）?0?wnb（1i）;
?????????????wnb（3i）?wnb（2i）?-wnb（1i）?0]/80;
??delta_theta=sqrt（（wnb（1i）/80）^2+（wnb（2i）/80）^2+（wnb（3i）/80）^2）;
??Q=（eye（4）*（1-delta_theta^2/8+delta_theta^4/384）+（1/2-delta_theta^2/48）*delta_big_theta）*Q;
??
??%計算姿態角
??theta（i）=asin（Cbn（23））;
??gamma（i）=atan（-Cbn（13）/Cbn（33））;
??psi（i）=atan（Cbn（21）/Cbn（22））;
????if?Cbn（22）<0
????????psi（i）=psi（i）+pi;
????else?if?Cbn（21）<0
????????psi（i）=psi（i）+2*pi;
????????end
????end
????
????if?Cbn（33）<0
????????if?gamma（i）>0?
????????????gamma（i）=gamma（i）-pi;
????????else?gamma（i）=gamma（i）+pi;
????????end
????end
??
??%計算速度與經緯度
??f（:i）=Cnb*f_INSc（:i）;
??ax=f（1i）+2*wie_scalar*sin（latitude（i-1））*velocity（2）+velocity（1）*velocity（2）*tan（latitude（i-1））/Rx;
??ay=f（2i）-2*wie_scalar*sin（latitude（i-1

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????目錄???????????0??2014-05-06?09:31??慣性導航\
?????文件????????4672??2009-07-27?14:40??慣性導航\jielian.m
?????文件?????2304256??2003-12-12?21:33??慣性導航\jlfw.mat
?????文件???????53248??2007-11-19?13:36??慣性導航\文檔.doc