?clear?all;?
clc;
close?all;

%%?初始參數設置
g0?=?9.78049;
rad_deg?=?0.01745329;%%度到弧度
wie?=?7.27220417e-5;
Re?=?6378393.0;
%%?The?duration?of?simulation
time?=?8*60*60;?%仿真時長
Hn?=?1;%時間間隔
%%%陀螺常值漂移
G_Drift=[2*rad_deg/3600;???
?????????2*rad_deg/3600;??
????????2*rad_deg/3600];
%%%陀螺隨機漂移
Drift_x?=?0.01*rad_deg/3600;
Drift_y?=?0.01*rad_deg/3600;
Drift_z?=?0.01*rad_deg/3600;

%%%%加速度計零偏
A_Bias=[1e-4*g0?????
???????1e-4*g0????
????????1e-4*g0];

A_randn_Bias=[1e-5*g0
??????????????1e-5*g0
??????????????1e-5*g0];
%%%姿態角搖擺周期
T_p?=?10;
T_r?=?10;
T_y?=?10;
%%%姿態角搖擺幅值
%?pitchm?=?5*rad_deg;
%?rollm?=?5*rad_deg;
%?yawm?=?10*rad_deg;

pitchm?=?0*rad_deg;
rollm?=?0*rad_deg;
yawm?=?0*rad_deg;
%%%初始姿態角
pitchk?=?0*rad_deg;
rollk?=?0*rad_deg;
yawk?=?0*rad_deg;
%%?The?initial?angular?phase
p_pitch?=?0*rad_deg;
p_roll?=?0*rad_deg;
p_yaw?=?0*rad_deg;

pitch_error=1*rad_deg;
roll_error=1*rad_deg;
yaw_error=1*rad_deg;%%%%%%%初始平臺失準角（粗對準以后的結果）

%%?The?true?initial?position?parameters?used?for?simulator
lati?=?45.7796*rad_deg;
longi?=?126.6705*rad_deg;
wien?=?[0
????????wie*cos（lati）
????????wie*sin（lati）];
%%?These?parameters?used?for?SINS?updating?calculation?
phi=45.7796*rad_deg;
lamda=126.6705*rad_deg;
wiep=[0
??????wie*cos（phi）
??????wie*sin（phi）];

pitch0?=?pitchm*sin（p_pitch）+pitchk;
roll0?=?rollm*sin（p_roll）+rollk;
yaw0?=?yawm*sin（p_yaw）+yawk;
%%%運動加速度
aold?=?0.0*g0;
a?=?[?aold;?aold;?0.0];

%%?The?initial?velocity
%?v0=[10.0*sin（yawk）;10.0*cos（yawk）;0.0];
%?v0=[10.0;10.0;0.0];
v0?=?[0.0;0.0;0.0];
v?=?v0;

position_error1?=?0;
position_error2?=?0;

wepp=[-v（2）/Re
???????v（1）/Re
???????v（1）*tan（phi）/Re];
???
q=[1??0???0???0]‘;

%%?Establish?the?initial?strapdown?matrix?with?initial?attitude?angles
pitch00?=?pitch0;
roll00?=?roll0;
yaw00?=?yaw0;

Tbn（11）?=?cos（roll00）*cos（yaw00）?-?sin（roll00）*sin（pitch00）*sin（yaw00）;
Tbn（12）?=?-cos（pitch00）*sin（yaw00）;
Tbn（13）?=?sin（roll00）*cos（yaw00）?+?cos（roll00）*sin（pitch00）*sin（yaw00）;

Tbn（21）?=??cos（roll00）*sin（yaw00）?+?sin（roll00）*sin（pitch00）*cos（yaw00）;
Tbn（22）?=?cos（pitch00）*cos（yaw00）?;
Tbn（23）?=??sin（roll00）*sin（yaw00）?-?cos（roll00）*sin（pitch00）*cos（yaw00）;

Tbn（31）?=?-sin（roll00）*cos（pitch00）;
Tbn（32）?=?sin（pitch00）;
Tbn（33）?=?cos（roll00）*cos（pitch00）;

TT=[1??????????yaw_error?????-roll_error;
????-yaw_error?1?????????????pitch_error;
????roll_error?-pitch_error??1];
T?=?TT*Tbn;
?
q（1）?=?sqrt（1+T（11）+T（22）+T（33））/2.0;
q（2）?=?（T（32）-T（23））/（4*q（1））;
q（3）?=?（T（13）-T（31））/（4*q（1））;
q（4）?=?（T（21）-T（12））/（4*q（1））;

Tn?=?time/Hn;
%%*****?軌跡發生器?*****%%模擬慣導輸出
for?k?=?1:Tn

????for?i=1:3
????????pitchT（i）?=?pitchm*sin（2*pi*（k-（3-i）/2）*Hn/T_p+p_pitch）+pitchk;
????????rollT（i）??=?rollm*sin（2*pi*（k-（3-i）/2）*Hn/T_r+p_roll）+rollk;
????????yawT（i）???=?yawm*sin（2*pi*（k-（3-i）/2）*Hn/T_y+p_yaw）+yawk;
????????
????????wpT（i）?=?2*pi/T_p*pitchm*cos（2*pi*（k-（3-i）/2）*Hn/T_p+p_pitch）;
????????wrT（i）?=?2*p