simple_pdaf_filtering.m

来自「IMM PDAF跟踪滤波的程序」· M 代码 · 共 43 行

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