%pai=[0.85?0.05?0.02?0.08;0.040.850.050.06;0.030.030.900.04;0.060.040.830.07];??%定義一步轉移概率矩陣
pai=[0.85?0.05?0.10;0.090.850.06;0.030.070.90];??%定義一步轉移概率矩陣
miu1_CV=0.11;????????????%勻速運動模型先驗概率
miu1_CA=0.85;???????????
%?miu1_CT=0.06;
miu1_Singer=0.04;
T=0.1;?%觀測間隔時間
%CV模型的狀態轉移矩陣和過程噪聲協方差矩陣
%?FV=[?1?0?T?0?0?0;?0?1?0?T??0?0;?0?0?1?0??0??0;?0?0?0?1??0??0;0??0??0??0??0??0;0??0??0??0??0??0];%勻速運動的狀態轉移矩陣
%?QV=[T^3/3?0?T^2/2?0?0?0;?0?T^3/3?1?T?0?0;?T^2/2?0?T?0?0?0;?0?T^2/2?0?T?0?0;?0?0?0?0?0?0;?0?0?0?0?0?0];
FV=[1?T?0?0?0?0;0?1?0?0?0?0;0?0?0?0?0?0;?0?0?0?1?T?0;0?0?0?0?1?0;0?0?0?0?0?0];
QV=[T^3/3?T^2/2?0?0?0?0;T^2/2?T?0?0?0?0;0?0?0?0?0?0;0?0?0?T^3/3?T^2/2?0;0?0?0?T^2/2?T?0;0?0?0?0?0?0];
%CA模型
FA=[1?T?T*T/2?0?0?0;0?1?T?0?0?0;0?0?1?0?0?0;?0?0?0?1?T?T*T/2;0?0?0?0?1?T;0?0?0?0?0?1];%勻加速運動狀態轉移矩陣
QA=[T^5/20?T^4/8?T^3/6?0?0?0;T^4/8?T^3/3?T^2/2?0?0?0;T^3/6?T^2/2?T?0?0?0;0?0?0?T^5/20?T^4/8?T^3/6;0?0?0?T^4/8?T^3/3?T^2/2;0?0?0?T^3/6?T^2/2?T];
%CT模型
%?[TQ]=CT（0.25*pi/1801.5*pi/1802.5*pi/1800.10.5）%勻速轉彎狀態轉移矩陣
%?FT=T;
%?QT=Q;

%?singer模型
%?初始化singer模型參數
aMax?=?2;???%?最大加速度
pAMax?=?0.5;???%?達到最大加速度的概率
pA0?=?0.05;??%?加速度為0的概率
alpha?=?1/10;???%?機動頻率
obs=100;
Ry?=?zeros（3?obs+1）;???%?Ry為Y軸方向實際運動軌跡，包括位置，速度，加速度三個向量
Ry（:?1）?=?[0;?10;?0];????%?初始Y方向位置為0，速度為10m/s，加速度為0
T?=?1;??%?觀測間隔時間為0.1秒
[sigma2MFQ]=?singer（aMaxpAMaxpA0alphaRyTobs）;
%?生成完整的singer狀態轉移矩陣
FS=?[1?T?0;?0?1?0;0?0?1];
FS=?[FS?zeros（33）;?zeros（33）?F];
QS=Q;
%FS=[1T0000;010000;001000;0011Talpha?*?T?-?1?+?exp（-alpha?*?T）;000011-exp（-alpha?*?T）;00000exp（-alpha?*?T）]
H=[1?0?0?0?0?0;0?1?0?0?0?0;0?0?1?0?0?0;0?0?0?1?0?0;0?0?0?0?1?0;0?0?0?0?0?1];???????????%勻速運動測量矩陣

?w=zeros（6100）;
?w?=?sqrt（2?*?alpha?*?sigma2）?*?randn（6obs）;%產生均值為0，方差為2*alpha*detal^2的高斯白噪聲，其中sigma2=detal^2

?w1=zeros（6100）;
?w1?=?sqrt（2?*?alpha?*?sigma2）?*?randn（6obs）;%產生均值為0，方差為2*alpha*detal^2的高斯白噪聲，其中sigma2=detal^2
?
?R=zeros（6100）;
?R?=?sqrt（2?*?alpha?*?sigma2）?*?randn（6obs）;

miu_temp（::1）=[0?0?0;0?0?0;0?0?0];????????????????%miu?初始化，miu即為公式中的u，即為模型在k時刻的概率
%?miu（::1）=[miu1_CV?miu1_CA?miu1_CT?miu1_Singer];
miu（::1）=[miu1_CV?miu1_CA?miu1_Singer];
x1_3（::1）=[10?10?10?20?0?0];???x2_3（::1）=[10?10?10?20?5?5];?
x3_3（::1）=[10?10?10?20?4?4];??%混合初始估計狀態

x1_1（::1）=[10?10?10?20?0?0];???x2_1（::1）=[10?10?10?20?5?6];???
x3_1（::1）=[10?10?10?20?5?5];?????%擴展卡爾曼濾波一步預測初始值

y1_1（::1）=[10?10?10?20?0?0];???y2_1（::1）=[10?10?10?20?5.1?5.9];???
y3_1（::1）=[10?10?10?20?4.2?4.9];??%測量初始向量

X1（::1）=[10?10?10?20?0?0];?????X2（::1）=[10?10?10?20?5?5];?
X3（::1）=[10?10?10?20?5?5];?????
%?X4（::1）=[10?10?10?20?5?5];??????%濾波更新，對應

%?模型重初始化方差
P1（::1）=[350?0?0?0?0?0;0?350?0?0?0?0;0?0?350?0?0?0;0?0?0?350?0?0;0?0?0?0?370?0;0?0?0?0?0?370];????
P2（::1）=[350?0?0?0?0?0;0?350?0?0?0?0;0?0?350?0?0?0;0?0?0?350?0?0;0?0?0?0?370?0;0?0?0?0?0?370];
P3（::1）=[350?0?0?0?0?0;0?350?0?0?0?0;0?0?350?0?0?0;0?0?0?350?