IMM.m交互式多模型

function [X_kP_kmu_kX_k_allP_k_all]=IMM（X_k_1P_k_1mu_k_1z_kFaiHkQkRkpM）
 % IMM algorithm
% X_k_1P_k_1: state and covariance at last time
% mu_k_1: model probability at last time 
% z_k:measurement at time k
% Fai:state transition matrix
% Hk: measurement matrix
% Qk:state noise covariance
% Rk: measurement covariance
% p: transition probability matrix
% M: number of models


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% filtering %%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%% model-conditioned reinitialization
for i=1:M
    for jj=1:M
        media1（jj）=p（jji）*mu_k_1（jj）;  % media1 is intermediate variable for computer mu_k_k_1（i）
    end
    mu_k_k_1（i）=sum（media1）;    % predicted mode probability

    for jj=1:M
        mu_ji_k_1（jj）=p（jji）*mu_k_1（jj）/mu_k_k_1（i）;  % mixing weigHkt
        media2（:jj）=X_k_1（:jj）*mu_ji_k_1（jj）;    % media is also intermediate variable for computer X_bar_k_1（:i）
    end
    X_bar_k_1（:i）=sum（media22）;   % mixing estimate
    for jj=1:M
        media3（::jj）=（P_k_1（::jj）+（X_bar_k_1（:i）-X_k_1（:jj））*（X_bar_k_1（:i）-X_k_1（:jj））‘）*mu_ji_