www.pudn.com > un_IMM_PDAF.zip > IMM_Kalman_Filter.m

function [x, V, VV, xall] = IMM_Kalman_Filter(y, A,C,Q,R,P,init_x,init_V) 
% IMM Kalman filter. 
% [x, V, VV] = IMM_Kalman_Filter(y, A, C, Q, R, init_x, init_V, model) 
% 
% Inputs: 
% y(:,t)   - the observation at time t 
% A(:,:,m) - the system matrix for model m 
% C(:,:,m) - the observation matrix for model m 
% Q(:,:,m) - the system covariance for model m 
% R(:,:,m) - the observation covariance for model m 
% P        - model interaction matrix 
% init_x(:,m) - the initial state for model m 
% init_V(:,:,m) - the initial covariance for model m 
% 
% Outputs: 
% x(:,t) = E[X_t | t] 
% V(:,:,t) = Cov[X_t | t] 
% VV(:,:,t) = Cov[X_t, X_t-1 | t] t >= 2 
% loglik = sum_t log P(Y_t) 
 
% param. 
[os T] = size(y); 
ss = size(A,1); 
M  = size(P,1); 
 
% intial target-state prob 
mu = ones(M,1)./M; 
Ci = zeros(M,1); 
Lik = zeros(M,1); 
 
% containers 
x = zeros(ss, T); 
V = zeros(ss, ss, T); 
VV = zeros(ss, ss, T); 
xall = zeros(ss, T, M); 
 
% temp variables 
xtmp = zeros(ss, M); 
Vtmp = zeros(ss, ss, M); 
x0   = xtmp; 
V0   = Vtmp; 
 
try 
     
% time loop 
for t=1:T, 
    
  %----------------------------------- 
  % Init. 
  initial = 0; 
  if t==1 
      xtmp = init_x; 
      Vtmp = init_V; 
      initial = 1; 
  end;   
     
  %----------------------------------- 
  % Mixing/Interaction 
  V0 = zeros(ss, ss, M); 
   
  for m = 1:M, 
       
    Ci(m) = P(m,:)*mu;                  % Ci(k-1) - prob. after interaction 
    mu_ij(m,:) = P(m,:).*mu'./Ci(m);    % cond. prob. mu_ij state i given j 
   
    % Updated mean and covariances 
    x0(:,m) = xtmp * mu_ij(m,:)'; 
     
    Dx = xtmp - x0(:,m)*ones(1,M); 
    %V0(:,:,m) = zeros(ss,ss); 
    for r = 1:M, 
        V0(:,:,m) = V0(:,:,m) + mu_ij(m,r)*(Vtmp(:,:,r) + Dx(:,r)*Dx(:,r)'); 
    end; 
     
  end; 
     
  %if isempty(V0), error('xtmp(:,m)'); end; 
   
  %--------------------------------------- 
  % Prediction/Filtering 
  for m=1:M, 
   
    [xtmp(:,m), Vtmp(:,:,m), Lik(m)] = ... 
          Simple_Kalman_Update(A(:,:,m), C(:,:,m), Q(:,:,m), R(:,:,m), y(:,t), x0(:,m), V0(:,:,m), initial); 
  end; 
    
  %--------------------------------------- 
  % Update mu 
  mu = Ci.*Lik; 
  mu = mu./sum(mu); 
  %disp([Ci';mu';Lik(:)']) 
 
  %--------------------------------------- 
  % Mean values 
  x(:,t) = xtmp*mu; 
  Dx = xtmp - x(:,t)*ones(1,M); 
  for m = 1:M, 
        V(:,:,t) = V(:,:,t) + mu(m).*(Vtmp(:,:,m) + Dx(:,m)*Dx(:,m)'); 
        %xall(:,t,m) = xtmp(:,m); 
 end; 
    
   xall(:,t,:) = xtmp; 
   
  %disp(xtmp) 
  %disp(sum(diag(V(:,:,t)))) 
  %sum(Ci) 
   
end; % time loop 
 
catch 
 xtmp, 
 mu, 
 Lik, 
 initial, 
 V0, 
 Vtmp 
end;