function immState = iterIMM(u,z,immState,immData)
	modes = size(immData.A,3);
	nx = size(immData.A,1);
	% Interaction step
	% 1) Compute cbar = sum(p(i,j)*mu(i,k-1))
	cbar = immData.txprob'*immState.mode;
	% 2) Compute mu(i|j,k-1) = 1/cbar * p(i,j)*mu(i,k-1)
	modeij = immData.txprob.*(immState.mode*(cbar.^(-1))');
	modeij(isnan(modeij)) = 0; % take care of impossible final states
	% 3) Compute xhat(mod)
    x0 = immState.X*modeij;    
	% 4) Compute Sigma(mod).
	Sx0 = immState.SX; Sx1 = immState.SX; % reserve space for Sx0,Sx1
	for j = 1:modes,
        xk1 = immState.X(:,j); Sx = xk1*xk1'; Sx0(:,j) = Sx(:);
        xk1 = x0(:,j); Sx = xk1*xk1'; Sx1(:,j) = Sx(:);
	end
	Sx0 = (immState.SX + Sx0)*modeij - Sx1; % verified
	% Filtering step
	Lambda = zeros(size(cbar)); Sx = zeros(nx);
	for j = 1:modes,
	% 0) Set up variables for this filter
	xhat = x0(:,j); Sx(:) = Sx0(:,j);
	A = immData.A(:,:,j); B = immData.B(:,:,j);
	C = immData.C(:,:,j); D = immData.D(:,:,j);
	Sw = immData.Sw(:,:,j); Sv = immData.Sv(:,:,j);
	% 1) State estimate time update
    xhat = A*xhat + B*u;
	% 2) State covariance time update
	Sx = A*Sx*A' + Sw;
	% 3) Output estimate
	zhat = C*xhat + D*u;
	% 4) Filter gain matrix
    Py = (C*Sx*C'+Sv); 
    L = Sx*C'/Py;
	% 5) State estimate measurement update
	immState.X(:,j) = xhat + L*(z-zhat);
	% 6) State covariance measurement update
	Sx = Sx - L*Py*L'; 
    immState.SX(:,j) = Sx(:);
    Lambda(j) = max(1e-9,exp(-0.5 * ((max(z-zhat))/max(max(Py))))/sqrt(2*pi*max(max(Py)))); % much faster than normpdf
    end
    % Combination step
    % 1) Compute PMF of being in mode j: mu(j,k)
    immState.mode = Lambda.*cbar;
    immState.mode = immState.mode/sum(immState.mode);
    % 2) Compute composite state estimate
    immState.xhat = immState.X*immState.mode;
    % 3) Compute composite covariance estimate
    modeSx = immState.SX;
    for j=1:modes
        xk1=immState.X(:,j)-immState.xhat;
        Sx=xk1*xk1'; modeSx(:,j)=Sx(:);
    end
    immState.SigmaX(:) = (immState.SX+modeSx)*immState.mode;