trajectory3;

x(:,1)=X(:,1);  %初始狀態(tài)向量

R=diag([200^2  10^2  200^2  10^2]);
P=50*eye(6);   %初始狀態(tài)向量協(xié)方差

X1(:,1)=x(:,1);
X2(:,1)=x(:,1);
X3(:,1)=x(:,1);
P1=P;
P2=P;
P3=P;

Pt=[0.8 0.15 0.05
    0.3 0.4 0.3
    0.05 0.15 0.8];

u=zeros(3,length(z)+1);
u(:,2)=[1/3 1/3 1/3]';

%--------
%CA
F=[1  T  T^2/2  0   0    0
   0  1    T    0   0    0
   0  0    1    0   0    0
   0  0    0    1   T  T^2/2
   0  0    0    0   1    T
   0  0    0    0   0    1];
  
Q=[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    ];
 Q1=Q*1;
 Q2=Q*50;
 Q3=Q*900;
 %-------
for i=2:length(z)
    %a:進(jìn)行交互
    U=u(:,i);
    miu(1,1)=Pt(1,1)*U(1)/(Pt(1,1)*U(1)+Pt(2,1)*U(2)+Pt(3,1)*U(3));
    miu(1,2)=Pt(1,2)*U(1)/(Pt(1,2)*U(1)+Pt(2,2)*U(2)+Pt(3,2)*U(3));
    miu(1,3)=Pt(1,3)*U(1)/(Pt(1,3)*U(1)+Pt(2,3)*U(2)+Pt(3,3)*U(3));
    
    miu(2,1)=Pt(2,1)*U(2)/(Pt(1,1)*U(1)+Pt(2,1)*U(2)+Pt(3,1)*U(3));
    miu(2,2)=Pt(2,2)*U(2)/(Pt(1,2)*U(1)+Pt(2,2)*U(2)+Pt(3,2)*U(3));
    miu(2,3)=Pt(2,3)*U(2)/(Pt(1,3)*U(1)+Pt(2,3)*U(2)+Pt(3,3)*U(3));
    
    miu(3,1)=Pt(3,1)*U(3)/(Pt(1,1)*U(1)+Pt(2,1)*U(2)+Pt(3,1)*U(3));
    miu(3,2)=Pt(3,2)*U(3)/(Pt(1,2)*U(1)+Pt(2,2)*U(2)+Pt(3,2)*U(3));
    miu(3,3)=Pt(3,3)*U(3)/(Pt(1,3)*U(1)+Pt(2,3)*U(2)+Pt(3,3)*U(3));
        
    X01=miu(1,1)*X1(:,i-1)+miu(2,1)*X2(:,i-1)+miu(3,1)*X3(:,i-1);    %交互后的第1個(gè)值 即 第1個(gè)濾波器輸入值
    X02=miu(1,2)*X1(:,i-1)+miu(2,2)*X2(:,i-1)+miu(3,2)*X3(:,i-1);    %交互后的第2個(gè)值 即 第2個(gè)濾波器輸入值
    X03=miu(1,3)*X1(:,i-1)+miu(2,3)*X2(:,i-1)+miu(3,3)*X3(:,i-1);    %交互后的第3個(gè)值 即 第3個(gè)濾波器輸入值
    P01=(P1+(X1(:,i-1)-X01)*(X1(:,i-1)-X01)')*miu(1,1)+(P2+(X2(:,i-1)-X01)*(X2(:,i-1)-X01)')*miu(2,1)+(P3+(X3(:,i-1)-X01)*(X3(:,i-1)-X01)')*miu(3,1); %相應(yīng)于X01的協(xié)方差
    P02=(P1+(X1(:,i-1)-X02)*(X1(:,i-1)-X02)')*miu(1,2)+(P2+(X2(:,i-1)-X02)*(X2(:,i-1)-X02)')*miu(2,2)+(P3+(X3(:,i-1)-X02)*(X3(:,i-1)-X02)')*miu(3,2); %相應(yīng)于X02的協(xié)方差
    P03=(P1+(X1(:,i-1)-X03)*(X1(:,i-1)-X03)')*miu(1,3)+(P2+(X2(:,i-1)-X03)*(X2(:,i-1)-X03)')*miu(2,3)+(P3+(X3(:,i-1)-X03)*(X3(:,i-1)-X03)')*miu(3,3); %相應(yīng)于X03的協(xié)方差
    
     %b：將X01，P01作為第1個(gè)濾波器的輸入，得到第1個(gè)濾波器的輸出X1,P1(CV)
     %   將X02，P02作為第2個(gè)濾波器的輸入，得到第2個(gè)濾波器的輸出X2,P2(較強(qiáng)CS)
     %   將X03，P03作為第3個(gè)濾波器的輸入，得到第3個(gè)濾波器的輸出X3,P3(強(qiáng)CS)
     
     %CA1
     X1(:,i)=F*X01;
     P1=F*P01*F'+Q1;
     H=[ 1, 0, 0, 0, 0, 0
         0, 1, 0, 0, 0, 0
         0, 0, 0, 1, 0, 0
         0, 0, 0, 0, 1, 0];
     S1=H*P1*H'+R;
     K=P1*H'*inv(S1);% 增益K
     V1=z(:,i)-H*X1(:,i);
     X1(:,i)=X1(:,i)+K*(V1);
     P1=(eye(6)-K*H)*P1*(eye(6)+K*H)'-K*R*K';
     
     %CA2
     X2(:,i)=F*X02;
     P2=F*P02*F'+Q2;
     S2=H*P2*H'+R;
     K=P2*H'*inv(S2);% 增益K
     V2=z(:,i)-H*X2(:,i);
     X2(:,i)=X2(:,i)+K*(V2);
     P2=(eye(6)-K*H)*P2*(eye(6)+K*H)'-K*R*K';
     
     %CA3
     X3(:,i)=F*X03;
     P3=F*P03*F'+Q3;     
     S3=H*P3*H'+R;
     K=P3*H'*inv(S3);% 增益K
     V3=z(:,i)-H*X3(:,i);
     X3(:,i)=X3(:,i)+K*(V3);
     P3=(eye(6)-K*H)*P3*(eye(6)+K*H)'-K*R*K';
     
     %c:由濾波器1的 濾波殘差V1，相應(yīng)的協(xié)方差S1，算出該濾波器的可能性q（1）
     %  由濾波器2的 濾波殘差V2，相應(yīng)的協(xié)方差S2，算出該濾波器的可能性q（2）
     %  由濾波器3的 濾波殘差V3，相應(yīng)的協(xié)方差S3，算出該濾波器的可能性q（3）
     q(1)=exp(-1/2*V1'*inv(S1)*V1)/sqrt(det(2*pi*S1)); 
     q(2)=exp(-1/2*V2'*inv(S2)*V2)/sqrt(det(2*pi*S2));
     q(3)=exp(-1/2*V3'*inv(S3)*V3)/sqrt(det(2*pi*S3));
     
     %d:各濾波器的 模型概率更新（以前是U）
     c=q(1)*(Pt(1,1)*U(1)+Pt(2,1)*U(2)+Pt(3,1)*U(3))+...
       q(2)*(Pt(1,2)*U(1)+Pt(2,2)*U(2)+Pt(3,2)*U(3))+...
       q(3)*(Pt(1,3)*U(1)+Pt(2,3)*U(2)+Pt(3,3)*U(3));
     u(1,i+1)=(q(1)*(Pt(1,1)*U(1)+Pt(2,1)*U(2)+Pt(3,1)*U(3)))/c;   % u 用來下一次交互前的 模型概率更新
     u(2,i+1)=(q(2)*(Pt(1,2)*U(1)+Pt(2,2)*U(2)+Pt(3,2)*U(3)))/c;
     u(3,i+1)=(q(3)*(Pt(1,3)*U(1)+Pt(2,3)*U(2)+Pt(3,3)*U(3)))/c;
     
     %e:輸出X、P
     x(:,i)=X1(:,i)*u(1,i+1)+X2(:,i)*u(2,i+1)+X3(:,i)*u(3,i+1);
     P=u(1,i+1)*(P1+(X1(:,i)-x(:,i))*(X1(:,i)-x(:,i))')+...
       u(2,i+1)*(P2+(X2(:,i)-x(:,i))*(X2(:,i)-x(:,i))')+...
       u(3,i+1)*(P3+(X3(:,i)-x(:,i))*(X3(:,i)-x(:,i))');     %此為IMM所需結(jié)果 
end

% %畫圖觀察
% figure(1);
% plot(X(1,:),X(4,:),'b-'),hold on;
% plot(z(1,:),z(3,:),'k:')
% plot(x(1,:),x(4,:),'r:')
% title('XY平面上目標(biāo)的飛行路徑(IMM with EKF方法)');
% xlabel('x/（米）');ylabel('y/（米）');
% legend('真實(shí)軌跡','測量值','交互濾波結(jié)果');
% 
% %將各模型的概率放在一張圖上觀察
% figure
% subplot(2,2,1);
% plot((1:length(u))*T,u(1,:),':',(1:length(u))*T,u(2,:),'-',(1:length(u))*T,u(3,:),'-.');
% title('各模型概率總圖(IMM with EKF方法)');
% xlabel('t/（秒）');ylabel('Probability');
% legend('CA 0.01','CA 50','CA 900');
% 
% %將各模型的概率分別觀察
% subplot(2,2,2);
% plot((1:length(u))*T,u(1,:),':')
% grid on;
% title('CA 0.01模型概率');
% xlabel('t/（秒）');ylabel('Probability');
% legend('CA 0.01');
% 
% subplot(2,2,3);
% plot((1:length(u))*T,u(2,:),'-')
% grid on;
% title('CA 9模型概率');
% xlabel('t/（秒）');ylabel('Probability');
% legend('CA 50');
% 
% subplot(2,2,4);
% plot((1:length(u))*T,u(3,:),'-.')
% grid on;
% title('CA 50模型概率');
% xlabel('t/（秒）');ylabel('Probability');
% legend('CA 900');
% 
% figure(3);
% plot(X(1,:),'b-'),hold on;
% plot(z(1,:),'k:')
% plot(x(1,:),'r:')
% title('橫坐標(biāo)位移');
% xlabel('秒');ylabel('米');
% legend('真實(shí)軌跡','測量值','濾波值');
% 
% figure(4);
% plot(X(2,:),'b-'),hold on;
% plot(z(2,:),'k:')
% plot(x(2,:),'r:')
% title('橫坐標(biāo)速度');
% xlabel('秒');ylabel('米/秒');
% legend('真實(shí)軌跡','測量值','濾波值');
% 
% figure(5);
% plot(X(4,:),'b-'),hold on;
% plot(z(3,:),'k:')
% plot(x(4,:),'r:')
% title('縱坐標(biāo)位移');
% xlabel('秒');ylabel('米');
% legend('真實(shí)軌跡','測量值','濾波值');
% 
% figure(6);
% plot(X(5,:),'b-'),hold on;
% plot(z(4,:),'k:')
% plot(x(5,:),'r:')
% title('縱坐標(biāo)速度');
% xlabel('秒');ylabel('米/秒');
% legend('真實(shí)軌跡','測量值','濾波值');