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

function [x, V] = Simple_PDAF_Filtering(y, F, H, Q, R, init_x, init_V) 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
% PDAF filter. 
% [x, V] = kalman_filter(y, F, H, Q, R, init_x, init_V) 
% 
% Inputs: 
%   y(:,ClutPointNum,t)   - the observation at time t 
%   F - the system matrix for model m 
%   H - the observation matrix for model m 
%   Q - the system covariance for model m 
%   R - the observation covariance for model m 
%   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] 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
 
[os,ClutPointNum,T] = size(y); 
ss = size(F,1); 
 
x = zeros(ss, T); 
V = zeros(ss, ss, T); 
 
for t=1:T 
  if t==1 
    prevx = init_x; 
    prevV = init_V; 
    initial = 1; 
  else 
    prevx = x(:,t-1); 
    prevV = V(:,:,t-1); 
    initial = 0; 
  end 
  [x(:,t), V(:,:,t)] = Simple_PDAF_Update(F, H, Q, R, y(:,:,t), prevx, prevV, initial); 
end