IMM交互模型

%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