%測試當前統計模型算法程序
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結合的多模型','量測');