%%%?main?of?current?function
%???其狀態方程和量測方程如下：
%???X（k+1）=Ak+U*a+W;
%???Z（k+1）=HX（k）+V;
%???T為觀測周期
%???A為轉移矩陣;
%???H為觀測矩陣;
%???U為控制矩陣?
%???obs為觀測點數
%???M為合成運動軌跡
%???Xk（:1）是目標初始狀態
%???Q?過程噪聲協方差;
%??假設目標狀態為?[x?vx?ax?y?vy?ay]
%??aMax????最大加速度
%??aMin????最小加速度
%??alpha???機動參數

clc;
clear;
T=1;%?觀測周期
obs?=?500;??%?觀測點數
%?Xk（:?1）?=?[0;?10;?2;?0;?8;?3];??%狀態向量初始值
%?Xk?=[Xk（:?1）?zeros（6?obs-1）];?
Xk?=zeros（6?obs）;
Xk（:?1）?=?[0;?10;?1;?0;?8;?5];??%狀態向量初始值
%?Xk（:?2）?=?[11;?12;?3;?9;?10;?5];??%狀態向量初始值
%?Xk（:?3）?=?[23;?15;?3;?20;?14;?5];??%狀態向量初始值

%Xk
%當前模型初始化
aMax?=?9;???%最大加速度
aMin?=?-5;??%最小加速度
alpha?=?1/20;?%機動頻率
sigma=0.1?;?%機動加速度初始方差
%?[?A?U?H?Q?M?]=?current_init（XkTobsaMaxaMinalpha）;
%?M

A=[?1??T??（-1+alpha*T+exp（-alpha*T））/alpha;?
????0?1?（1-exp（-alpha*T））/alpha;
????0?0?exp（-alpha*T）];
A=[A?zeros（33）;
????zeros（33）?A];

U=[?-（-T+alpha*T^2/2+（1-exp（-alpha*T））/alpha）/alpha;
????T-1/alpha*（1-exp（-alpha*T））;
????1-exp（-alpha*T）];
%?U=[?U?zeros（33）;?zeros（33）?U?];
%?U1=[?U?;??zeros（31）?];
%?U2=[??zeros（31）;?U??];
U=[?U?zeros（31）;?zeros（31）?U?];?%注意矩陣格式

H=[?1?0?0?0?0?0?;
????0?0?0?1?0?0];

%?過程噪聲
q11?=?1?/?（2?*?alpha?^?5）?*?（1?-?exp（-2?*?alpha?*?T）?+?2?*?alpha?*?T?+?2?*?alpha?^?3?*?T?^?3?/?3?...
??????-?2?*?alpha?^?2?*?T?^?2?-?4?*?alpha?*?T?*?exp（-1?*?alpha?*?T））;
q12?=?1?/?（2?*?alpha?^?4）?*?（exp（-2?*?alpha?*?T）?+?1?-?2?*?exp（-1?*?alpha?*?T）?...?
?????+?2?*?alpha?*?T?*?exp（-1?*?alpha?*?T）?-2?*?alpha?*?T?+?alpha?^?2?*?T?^?2）;
q13?=?1?/?（2?*?alpha?^?3）?*?（1?-?exp（-2?*?alpha?*?T）?-?2?*?alpha?*?T?*?exp（-1?*?alpha?*?T））;
q22?=?1?/?（2?*?alpha?^?3）?*?（4?*?exp（-1?*?alpha?*?T）?-?3?-?exp（-2?*?alpha?*?T）?+?2?*?alpha?*?T）;
q23?=?1?/?（2?*?alpha?^?2）?*?（exp（-2?*?alpha?*?T）?+?1?-?2?*?exp（-1?*?alpha?*?T））;
q33?=?1?/?（2?*?alp