%imm算法
function [X_estimate,P,x_model_filter,p_model_filter,u_output]=imm_cs_ca(z,x1,p1,u_input)
%
%

T=1;
u_output=zeros(2,1);
fai=[];
Q=[];


%模型1，CA模型
F_ca=[1 T T^2/2;0 1 T;0 0 1];
G_ca=[T^2/2,T,1]';
Q_ca=10;
Q_ca=G_ca*Q_ca*G_ca';
%模型2，CV模型
F_cv=[1 T 0;0 1 0;0 0 0];
    
G_cv=[T^2/2,T,0]';
Q_cv=10;
Q_cv=G_cv*Q_cv*G_cv';


fai=[F_ca F_cv];
Q=[Q_ca Q_cv];
h=[1 0 0];
r=100^2;
%模型參數設置完畢

dim=3;%狀態維數
model_number=2;%模型數
% MarkoProb=[0.85 0.05 0.05 0.05;0.05 0.85 0.05 0.05;0.05 0.05 0.85 0.05;0.05 0.05 0.05 0.85]; 
MarkoProb=[0.95 0.05;0.05 0.95];
predictProb=MarkoProb'*u_input;
mixedProb=[];
mixedInitX=zeros(dim,model_number);
mixedInitP=zeros(dim,dim*model_number);
mixedInitC=zeros(dim,dim*model_number);
for i=1:model_number
    for j=1:model_number
        mixedProb(i,j)=MarkoProb(i,j)*u_input(i)/predictProb(j);
    end
end


mixedInitX=x1*mixedProb;


for j=1:model_number
    for i=1:model_number
        mixedInitP(:,(j-1)*dim+1:j*dim)=mixedInitP(:,(j-1)*dim+1:j*dim)+(p1(:,(i-1)*dim+1:i*dim)+(x1(:,i)-mixedInitX(:,j))*(x1(:,i)-mixedInitX(:,j))')*mixedProb(i,j);
    end
end
for i=1:model_number
    temp1=(i-1)*dim+1;
    temp2=i*dim;
    x_model_predict(:,i)=fai(:,temp1:temp2)*mixedInitX(:,i);
%   p_model_predict(:,temp1:temp2)=fai(:,temp1:temp2)*mixedInitP(:,temp1:temp2)*fai(:,temp1:temp2)'+g(:,(i-1)*2+1:i*2)*q(:,(i-1)*2+1:i*2)*g(:,(i-1)*2+1:i*2)';
   
    p_model_predict(:,temp1:temp2)=fai(:,temp1:temp2)*mixedInitP(:,temp1:temp2)*fai(:,temp1:temp2)'+Q(:,temp1:temp2);
    s(:,i)=h*p_model_predict(:,temp1:temp2)*h'+r;
    k(:,i)=p_model_predict(:,temp1:temp2)*h'*inv(s(:,i));
    v_model_filter(:,i)=z-h*x_model_predict(:,i);
    x_model_filter(:,i)=x_model_predict(:,i)+k(:,i)*v_model_filter(:,i);
    p_model_filter(:,temp1:temp2)=p_model_predict(:,temp1:temp2)-k(:,i)*s(:,i)*k(:,i)';
    likelyhood(i)=(det(2*pi*s(:,i)))^(-0.5)*exp(-0.5*v_model_filter(:,i)'*inv(s(:,i))*v_model_filter(:,i));
%     A=det(2*pi*s(:,(i-1)*2+1:i*2));
%     B=exp(-0.5*v_model_filter(:,i)'*inv(s(:,(i-1)*2+1:i*2))*v_model_filter(:,i));
    u_output(i)=likelyhood(i)*predictProb(i);

end

u_output=(u_output/sum(u_output));
X_estimate=x_model_filter*u_output;
% X_estimate=real(X_estimate);%%%%仿真表明估計可能出現虛部，通過三角分解可消除此現象，為簡略起見直接去掉虛部
P=zeros(dim);
for i=1:model_number
    temp1=(i-1)*dim+1;
    temp2=i*dim;
    P=P+(p_model_filter(:,temp1:temp2)+(x_model_filter(:,i)-X_estimate)*(x_model_filter(:,i)-X_estimate)')*u_output(i);
end