适合于强机动的Jerk模型代码

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