test_imm.m

上传的为imm交互多模型跟踪算法的源程序

%测试当前统计模型算法程序
clear;
clc;
close all;
T=1;%采样时间
M=10;%monto-carlo仿真次数

RMSE_ca_cv=zeros(3,201);
RMSE_measurement=zeros(1,201);

for kk=1:M

% create_data1;
create_data2;
% create_data3;
% create_data4;

H=[1 0 0];
sigma_z=100;

for i=1:N
    v=sigma_z*randn;
    Z(:,i)=H*X(:,i)+v;
    RMSE_measurement(:,i)=RMSE_measurement(:,i)+v.^2;
end

%以下用CA和CV模型
 u_input=[1/2 1/2]';
% u_input=[0.05 0.05 0.85 0.05]';
temp_p=diag([100^2 10^2 10]);
model_number=2;%模型数
x1=[];
p1=[];
 for i=1:model_number
    x1=[x1 X00];
    p1=[p1 temp_p];
 end
u_output_ca_cv=zeros(model_number,1);

 for m=2:N
    %中间是imm算法
    [X_estimate_ca_cv(:,m),P_ca_cv(:,:,m),x_model_filter_ca_cv,p_model_filter_ca_cv,u_output_ca_cv(:,m)]=imm_ca_cv(Z(:,m),x1,p1,u_input);
    %imm结束
    x1=x_model_filter_ca_cv;
    p1=p_model_filter_ca_cv;
    u_input=u_output_ca_cv(:,m);
    %计算RMSE
    RMSE_ca_cv(:,m)=RMSE_ca_cv(:,m)+(X_estimate_ca_cv(:,m)-X(:,m)).^2;
 end

end

%输出误差曲线

RMSE_ca_cv=sqrt(RMSE_ca_cv/M);
RMSE_measurement=sqrt(RMSE_measurement/M);

%输出图形
i=1:N;
subplot(3,1,1)
plot(i,RMSE_ca_cv(1,i));
suplot(3,1.2)
plot(i,RMSE_ca_cv(2,i));
subplot(3,1,3)
plot(i,RMSE_ca_cv(1,i),i,RMSE_measurement(1,i),':');
legend('CA和CV结合的多模型','量测');