clear all;
test_d=150;
measure=test(test_d);          %調用test()函數產生虛擬仿真參數，輸入變量是測量標準差
for i=1:171
    test(:,:,i)=[measure(1,1,i),measure(1,2,i)];
end
pai=[0.85 0.15;0.15,0.85];  %定義一步轉移概率矩陣
miu1_CV=0;            %勻速運動模型在初始時刻正確的概率
miu1_CA=1;            %勻加速運動模型在初始時刻正確概率
T=0.1;
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];%勻速運動的狀態轉移矩陣
FA=[ 1, 0, T, 0, T*T/2, 0; 0, 1, 0, T, 0, T*T/2; 0, 0, 1, 0, T, 0; 0, 0, 0, 1, 0 ,T;0, 0, 0, 0, 1, 0;0, 0, 0, 0, 0, 1];%勻加速運動狀態轉移矩陣
H=[1 0 0 0 0 0;0 1 0 0 0 0];           %勻速運動測量矩陣
miu_temp(:,:,1)=[0 0;0 0];                %miu 初始化
miu(:,:,1)=[miu1_CV miu1_CA];
x1_2(:,:,1)=[10 10 10 20 0 0];   x2_2(:,:,1)=[10 10 10 20 5 5];         %混合估計
x1_1(:,:,1)=[10 10 10 20 0 0];   x2_1(:,:,1)=[10 10 10 20 5 5];         %一步預測
X1(:,:,1)=[10 10 10 20  0  0];   X2(:,:,1)=[10 10 10 20 5 5];           %濾波更新
%P1(:,:,1)=[5 0 0 0 0 0;0 5 0 0 0 0;0 0 5 0 0 0;0 0 0 5 0 0;0 0 0 0 7 0;0 0 0 0 0 7];     P2(:,:,1)=[5 0 0 0 0 0;0 5 0 0 0 0;0 0 5 0 0 0;0 0 0 5 0 0;0 0 0 0 7 0;0 0 0 0 0 7];% 模型重初始化方差
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];
P1_1(:,:,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(:,:,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];%混合估計協方差陣
FC1(:,:,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];   
FC2(:,:,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]; %濾波更新方差
z1_e(:,:,1)=[0 0];           z2_e(:,:,1)=[0 0];                 %測量殘差
S1(:,:,1)=[test_d,0;0,test_d]; S2(:,:,1)=[test_d,0;0,test_d];  R=[test_d*test_d,0;0,test_d*test_d];%測量協方差陣
like1(1)=1;     like2(1)=0;    %似然函數
K1(:,:,1)=[0 0; 0 0;0 0; 0 0;0 0;0 0];        K2(:,:,1)=[0 0; 0 0;0 0; 0 0;0 0;0 0];               %濾波增益
cc=0;           %計算模型概率更新
for k=1:170
    %第一步 模型條件重初始化
    %1。首先計算混合概率
    %計算c
    c(1)=pai(1,1)*miu(1,1,k)+pai(2,1)*miu(1,2,k);
    c(2)=pai(1,2)*miu(1,1,k)+pai(2,2)*miu(1,2,k);
    %計算miu_temp
    miu_temp(1,1,k)=pai(1,1)*miu(1,1,k)/c(1);   
    miu_temp(1,2,k)=pai(1,2)*miu(1,1,k)/c(2);
    miu_temp(2,1,k)=pai(2,1)*miu(1,2,k)/c(1);
    miu_temp(2,2,k)=pai(2,2)*miu(1,2,k)/c(2);
    %2.進行混合估計
    %勻速運動模型
    x1_2(:,:,k)=X1(:,:,k)*miu_temp(1,1,k)+X2(:,:,k)*miu_temp(2,1,k);
    P1_1(:,:,k)=(FC1(:,:,k)+(x1_1(:,:,k)-x1_2(:,:,k))'*(x1_1(:,:,k)-x1_2(:,:,k)))*miu_temp(1,1,k)+(FC2(:,:,k)+(x2_1(:,:,k)-x1_2(:,:,k))'*(x2_1(:,:,k)-x1_2(:,:,k)))*miu_temp(2,1,k);
    %勻加速運動模型
    x2_2(:,:,k)=X1(:,:,k)*miu_temp(1,2,k)+X2(:,:,k)*miu_temp(2,2,k);
    P2_1(:,:,k)=(FC1(:,:,k)+(x1_1(:,:,k)-x2_2(:,:,k))'*(x1_1(:,:,k)-x2_2(:,:,k)))*miu_temp(1,2,k)+(FC2(:,:,k)+(x2_1(:,:,k)-x2_2(:,:,k))'*(x2_1(:,:,k)-x2_2(:,:,k)))*miu_temp(2,2,k);
    %----------------------------------------------------------------------
    %第二步 模型條件濾波
    %1. 狀態預測，分別計算每個模型的
    %1.1 計算勻速勻速運動模型
    x1_1(:,:,k+1)=FV*x1_2(:,:,k)';%暫不考慮過程噪聲
    P1(:,:,k+1)=FV*P1_1(:,:,k)*FV';
    %1.2 計算勻加速運動模型
    x2_1(:,:,k+1)=FA*x2_2(:,:,k)';
    P2(:,:,k+1)=FA*P2_1(:,:,k)*FA';
    %2 分別計算預測殘差及協方差陣
    %2.1 計算勻速運動模型
    z1_e(:,:,k+1)=test(:,:,k+1)'-H*x1_1(:,:,k+1)';%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    S1(:,:,k+1)=H*P1(:,:,k+1)*H'+R;
    %2.2 計算勻加速運動模型
    z2_e(:,:,k+1)=test(:,:,k+1)'-H*x2_1(:,:,k+1)';
    S2(:,:,k+1)=H*P2(:,:,k+1)*H'+R;
    %2.3 計算模型匹配函數
    like1(k+1)=exp(-0.5*z1_e(:,:,k+1)*inv(S1(:,:,k+1))*z1_e(:,:,k+1)')/(sqrt(det(2*3.1415926*S1(:,:,k+1))));
    like2(k+1)=exp(-0.5*z2_e(:,:,k+1)*inv(S2(:,:,k+1))*z2_e(:,:,k+1)')/(sqrt(det(2*3.1415926*S2(:,:,k+1))));
    %2.4 濾波更新
    %2.4.1 計算勻速運動濾波更新
    K1(:,:,k+1)=P1(:,:,k+1)*H'*inv(S1(:,:,k+1));
    X1(:,:,k+1)=x1_1(:,:,k+1)+(K1(:,:,k+1)*z1_e(:,:,k+1)')';
    FC1(:,:,k+1)=P1(:,:,k+1)-K1(:,:,k+1)*S1(:,:,k+1)*K1(:,:,k+1)';
    %2.4.2 計算勻加速運動濾波更新
    K2(:,:,k+1)=P2(:,:,k+1)*H'*inv(S2(:,:,k+1));
    X2(:,:,k+1)=x2_1(:,:,k+1)+(K2(:,:,k+1)*z2_e(:,:,k+1)')';
    FC2(:,:,k+1)=P2(:,:,k+1)-K2(:,:,k+1)*S2(:,:,k+1)*K2(:,:,k+1)';
    %----------------------------------------------------------------------
    %第三步 模型概率更新
    c(1)=0;c(2)=0;
    for i=1:2
        for j=1:2
            c(i)=c(i)+pai(j,i)*miu(1,j,k);
        end
    end 
    cc=like1(k+1)*c(1)+like2(k+1)*c(2);
    miu(1,1,k+1)=like1(k+1)*c(1)/cc;
    miu(1,2,k+1)=like2(k+1)*c(2)/cc;
    %----------------------------------------------------------------------
    %第四步 估計融合
    X(:,:,k+1)=X1(:,:,k+1)*miu(1,1,k+1)+X2(:,:,k+1)*miu(1,2,k+1);
    P(:,:,k+1)=(FC1(:,:,k+1)+(X(:,:,k+1)-X1(:,:,k+1))*(X(:,:,k+1)-X1(:,:,k+1))')*miu(1,1,k+1)+(FC2(:,:,k+1)+(X(:,:,k+1)-X2(:,:,k+1))*(X(:,:,k+1)-X2(:,:,k+1))')*miu(1,2,k+1);
    %----------------------------------------------------------------------
end
for i=1:170;
%     x(i)=sqrt(X(1,1,i)*X(1,1,i)+X(1,2,i)*X(1,2,i));
%     ideal(i)=sqrt(measure(1,3,i)*measure(1,3,i)+measure(1,4,i)*measure(1,4,i));
%     noise(i)=sqrt(test(1,1,i)*test(1,1,i)+test(1,2,i)*test(1,2,i));
    x(i)=X1(1,1,i);
    ideal(i)=measure(1,3,i);
    noise(i)=test(1,1,i);
%     PP(i)=P(1,1,i);
end

% plot(i,)
% hold on
i=1:170;
plot(i,x(i),'r')
hold on
plot(i,noise(i),'g')
hold on
plot(i,ideal(i),'b')
hold on
% plot(i,PP(i))