imm_kalman_filter.m

IMM PDAF跟踪滤波的程序

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;