close?all;
clear?all;
clc;
format?long;
fid?=?fopen（‘C:\Documents?and?Settings\Administrator\桌面\imusimulate\dataagj.txt‘‘w‘）;
%%?地球參數
global?GLV;
GLV.RE??????????=?6378137;?%?length?of?the?semi-major?aixs?unit:?m
GLV.RP??????????=?6356752;?%?length?of?the?semi-minor?aixs?unit:?m
GLV.R???????????=?sqrt（GLV.RE*GLV.RP）;?%?length?of?the?approximate?semidiameter
GLV.F???????????=?1/298.257223563;?%?flattening?of?the?ellipsoid
GLV.E???????????=?0.081819191042816;?%?major?eccentricity?of?the?ellipsoid
GLV.OMEGA_IE????=?7.2921158e-5;?%?earth‘s?rate?unit:?rad/s
GLV.G0??????????=?9.78031811;?%?gravity?coefficient?unit:?m/s^2
GLV.G???????????=?9.8010104;?%?approximate?gravity?unit:?m/s^2
%%?常數
TTR?=?pi/180;%%角度值轉為弧度值
%%?解算周期
deltt?=?5e-3;%%%5ms
period?=?5e-3;%%%5ms
%%?系統運行時間
t?=?200*60;%%?200min
calc_len?=?t/deltt;%%系統計算數據長度
%?calc_len?=?10;
%%?航跡初始值設定
%%%?atti?=?[roll;?pitch;?heading];姿態角矩陣
atti_0?=?[0.4;0.4;3]*TTR/60;%%姿態初始值
datti?=?[0;0;0];%%姿態變化率
%%%?vn?=?[vE;vN;vU];地理坐標系n系中的速率分量
vn?=?[0;1.5;0];%%軌跡生成中假定載體由南向北勻速運動
%%%?posn?=?[latlonhigh];地理坐標系n系中的位置變量由球面坐標表示
posn_0?=?[39.229956;?117.143156;?0]*TTR;
%?posn?=?zeros（3calc_len）;%%%位置矩陣
%?atti?=?zeros（3calc_len）;%%%姿態角矩陣
%?wibb?=?zeros（3calc_len）;%%%陀螺數據
%?fb?=zeros（3calc_len）;%%%加速度數據

?%%%?元件參數
??%%%?陀螺
????gyrobias_err?=?0.01*pi/180/3600;
????gyrobias_var?=?0.01*pi/180/3600;
????gyrosca_err?=?1e-5;
??%%%?加表
????accbias_err?=?1e-4*GLV.G;
????accbias_var?=?5e-5*GLV.G;
????nosie_g?=?ones（31）*（gyrobias_err+gyrobias_var.^（0.5）*randn（1calc_len））;
????noise_a?=?ones（31）*（accbias_err+accbias_var.^（0.5）*randn（1calc_len））;
????
posn（:1）?=?posn_0;
atti（:1）?=?atti_0;

for?i=1:calc_len
???lat?=?posn（1i）;?lon?=?posn（2i）;?high?=?posn（3i）;
???sinL?=?sin（lat）;?cosL?=?cos（lat）;?tanL?=?tan（lat）;

???g?=?GLV.G0*（1+5.3024e-3*sinL^2-5.9e-6*（sin（2*lat））^2）;
???RM?=?GLV.RE*（1-GLV.E^2）/（（1-（GL