close?all
clear?all;
clc;
%———————————————————————————————————————
T=?0.5;%采樣周期
SAMP=600;%采樣點數
Pi=3.1415926536;
alpha=0.5;%急動頻率
MK_num=200;
%———————————————————————————————————————

%原始軌跡前35秒作大約為1.88馬赫直線運動，35-94秒過載1.22g，94-106秒1.88馬赫直線運動，106-136秒過載3.7g136-178秒1.88馬赫直線運動，178-190秒過載
%6g190-200秒1.88馬赫直線運動.
X=zeros（8SAMP）;
X（:1）=[30000300006000040000]‘;%給X狀態賦初值；：作用，矩陣X中，取所有行第一列的元素
W1=1.4*3.1415926536/180;W2=-4.2*3.1415926536/180;%各轉角的角速度
F1=[1TT^2/200000;
????01T00000;
????00100000;
????00000000;
????00001TT^2/20;
????000001T0;
????00000010;
????00000000];%勻速直線模型
F2=[1sin（W1*T）/W1（1-cos（W1*T））/W1^200000;
????0cos（W1*T）sin（W1*T）/W100000;
????0-W1*sin（W1*T）cos（W1*T）00000;
????00000000;
????00001sin（W1*T）/W1（1-cos（W1*T））/W1^20;
????00000cos（W1*T）sin（W1*T）/W10;
????00000-W1*sin（W1*T）cos（W1*T）0;
????00000000];
F3=[1sin（W2*T）/W2（1-cos（W2*T））/W2^200000;
????0cos（W2*T）sin（W2*T）/W200000;
????0-W2*sin（W2*T）cos（W2*T）00000;
????00000000;
????00001sin（W2*T）/W2（1-cos（W2*T））/W2^20;
????00000cos（W2*T）sin（W2*T）/W20;
????00000-W2*sin（W2*T）cos（W2*T）0;
????00000000];%轉彎模型
F4=[1TT^2/2T^3/60000;
????01TT^2/20000;
????001T0000;
????00010000
????00001TT^2/2T^3/6;
????000001TT^2/2;
????0000001T;
????00000001];%勻速直線模型
for?t=1:SAMP??%從時刻2開始，到采樣結束為止
????if?t==1?
????????X（:1）=[30000300006000040000]‘;
????elseif?t>1&&t<150
????????X（:t）=F1*X（:t-1）;
????elseif?t==150
????????X（:t）=F2*[X（1t-1）X（2t-1）-W1*X（6t-1）0X（5t-1）X（6t-1）W1*X（2t-1）0]‘;
????elseif（t>150&&t<300）
????????X（:t）=F2*X（:t-1）;
????elseif?（t==300）
????????X（:t）=F1*[X（1t-1）X（2t-1）00X（5t-1）X（6t-1）00]‘;
????elseif?（t>300&&t<450）
????????X（:t）=F1*X（:t-1）;
????elseif（t==450）
????????X（:t）=F2*[X（1t-1）X（2t-1）-W2*X（6t-1）0X（5t-1）X（6t-1）W2*X（2t-1）0]‘;
????elseif（t>450&&t<540）
????????X（:t）=F4*X（:t-1）;
????else
????????X（:t）=F1*X（:t-1）;
????end
end

%改進的當前模型——————分別對X和Y方向進行建模—————————————————————
XT=zeros（10SAMP）;%系統模型
X（:1）=[30000300006000040000]‘;

XT（1SAMP）=X（11）;%給模型賦X方向的位移初值
XT（6SAMP）=X（51）;
XT（2SAMP）=X（21）;%給模型賦X方向的速度初值
XT（7SAMP）=X（61）;
XT（3SAMP）=X（31）;%給模型賦X方向的加速度初值
XT（8SAMP）=X（71）;
%?XT（4SAMP）=0;
%?XT（9SAMP）=0;
XT（5SAMP）=alpha;
XT（10SAMP）=alpha;
%轉移矩陣----------------------------------------------------------------------
PT=（2-2*alpha*T+alpha^2*T^2-2*exp（-alpha*T））/（2*alpha^3）;
QT=（exp（-alpha*T）-1+alpha*T）/alpha^2;
RT=（1-exp（-alpha*T））/alpha;
ST=exp（-alpha*T）;

U1=（2*T-alpha*T^2+alpha^3*T^3/3-2*（1-exp（-alpha*T））/alpha）/（2*alpha^2）;
U2=（-T+alpha*T^2/2+（1-exp（-alpha*T））/alpha）/alpha;
U3=T-（1-exp（-alpha*T））/alpha;
U4=1-exp（-alpha*T）;
U=[U1U2U3U40U1U2U3U40]‘;

F=[1TT^2/2T^3/6000000;
???01TT^2/200