%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% IMM filter %
% Estimation with Applications to Tracking and Navigation %
% Theory Algorithms and Software %
% - Yaakov Bar-Shalom, X.Rong Li %
% Page No. 455 - 457. %
% %
% %
% modeProb = weights or likelihood %
% Transprob = Transition Probability matrix %
% Z = sensor measurements %
% F = evolution matrix r models %
% H = observation matrix r models %
% Q = process noise covariance r models %
% R = measurement noise covariance r models %
% xm = means of r models %
% xp = error covariances of r models %
% MM = mean of the model %
% PP = covariance of the model %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function[MM,PP,modeProb,xm,xp] = IMM(modeProb,Transprob,Z,F,H,Q,R,xm,xp)
% getting no. of rows and coloumns for transprob matrix
[r,c] = size(Transprob);
% Mixing probabilities calculation
c_j = Transprob*modeProb; % (rxr)' * (rx1) equation 11.6.6-8
MU_ij = Transprob.*(modeProb*(c_j.^(-1))'); % (rxr).*((rx1)*(rx1)')
equation 11.6.6-7
% Mixed estimates of means and Covariances
temp_x = xm * MU_ij; % mixing mean equation 11.6.6-9
xk_1k_1 = xm; % initialization for mixed means difference
temp_p = xp; % initialization for mixed covariances
[m,n,d] = size(temp_p); % getting dimension for mixed corivances
P = zeros(m*n,d); % defination for the mixed covariance
for i = 1:c
xk_1k_1(:,i) = xm(:,i)- temp_x(:,i); % mixed mean difference
temp_p(:,:,i) = xp(:,:,i)+ xk_1k_1(:,i)*xk_1k_1(:,i)'; % mixed
covarince without MU_ij multiplication
end
P(:) = reshape(temp_p,[m*n d]);
P = P * MU_ij; % equation 11.6.6-10
Pk_1k_1 = reshape(P,m,n,d);
