高斯混合PHD滤波器扩展目标跟踪

高斯混合概率假设密度滤波器用于扩展目标跟踪matlab代码及对应论文

function [w_km_kP_kJ_k] = extendedTargetPHDfilter（w_k1m_k1P_k1J_k1ZkF_k1Q_k1H_kR_k）

% Probabilities of survival and detection
global p_S_k p_D_k
% The spontaneous birth distributions
global w_gam_k m_gam_k P_gam_k J_gam_k
% The spawn distributions
global w_beta_k d_beta_k F_beta_k1 Q_beta_k1 J_beta_k
% Expected number of measurements
global beta_D

% Allocate some memory
w_kk1 = zeros（1J_gam_k+J_beta_k*J_k1+J_k1）;
m_kk1 = zeros（size（m_k11）J_gam_k+J_beta_k*J_k1+J_k1）;
P_kk1 = zeros（size（P_k11）size（P_k12）J_gam_k+J_beta_k*J_k1+J_k1）;

nu_kk1 = zeros（size（H_k1）J_gam_k+J_beta_k*J_k1+J_k1）;
S_k = zeros（size（R_k1）size（R_k2）J_gam_k+J_beta_k*J_k1+J_k1）;
K_k = zeros（size（P_kk11）size（S_k2）J_gam_k+J_beta_k*J_k1+J_k1）;
P_kk = zeros（size（P_kk11）size（P_kk12）J_gam_k+J_beta_k*J_k1+J_k1）;

nZk = size（Zk2）;
w_k = zeros（1（nZk+1）*（J_gam_k+J_beta_k*J_k1+J_k1））;
m_k = zeros（size（m_kk11）（nZk+1）*（J_gam_k+J_beta_k*J_k1+J_k1））;
P_k = zeros（size（P_kk11）size（P_kk12）（nZk+1）*（J_gam_k+J_beta_k*J_k1+J_k1））;

gam = zeros（1J_gam_k+J_beta_k*J_k1+J_k1）;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Step 1: Prediction for birth targets spontaneous birth and spawn.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
i = 0;
for j = 1:J_gam_k
    i = i+1;
    w_kk1（1i） = w_gam_k（1j）;
    m_kk1（:i） = m_gam_k（:j）;
    P_kk1（::i） = P_gam_k（::j）;
end

for j = 1:J_beta_k
    for l = 1:J_k1
        i = i+1;
        w_kk1（1i） = w_k1（1l）*w_beta_k（1j）;
        m_kk1（:i） = d_beta_k（:j） + F_beta_k1（::j）*m_k1（:l）;
        P_kk1（::i） = Q_beta_k1（::j） + F_beta_k1（::j）*P_k1（::l）*F_beta_k1（::j）‘;
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Step 2: Prediction for existing targets
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for j = 1:J_k1
    i = i+1;
    w_kk1（1i） = p_S_k*w_k1（1j）;
    m_kk1（:i） = F_k1*m_k1（:j）;
    P_kk1（::i） = Q_k1 + F_k1*P_k1（::j）*F_k1‘;
end
J_kk1 = i;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Step 3: Construction of PHD update components
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for j = 1:J_kk1
    % Expected number of measurements
    gam（j） = beta_D;
    nu_kk1（:j） = H_k*m_kk1（:j）;
    S_k（::j） = R_k + H_k*P_kk1（::j）*H_k‘;
    H_k_invS_k = H_k‘/S_k（::j）; % instead of H_k‘*inv（S_k（::j））
    K_k（::j） = P_kk1（::j）*H_k_invS_k;
    P_kk（::j） = （eye（size（P_kk1（::j）））-K_k（::j）*H_k）*P_kk1（::j）;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Step 4: Update
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for j = 1:J_kk1
    w_k（1j） = （1-（1-exp（-gam（j）））*p_D_k）*w_kk1（1j）;
    m_k（:j） = m_kk1（:j）;
    P_k（::j） = P_kk1（::j）;
end
% l is an index

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2014-05-09 21:12  A Gaussian Mixture PHD filter for Extended Target Tracking\
     文件      483612  2014-04-17 11:37  A Gaussian Mixture PHD filter for Extended Target Tracking\3046.pdf
     文件     1198355  2014-04-17 15:36  A Gaussian Mixture PHD filter for Extended Target Tracking\A Gaussian Mixture PHD filter for extended target tracking.pdf
     文件        6073  2010-05-19 00:44  A Gaussian Mixture PHD filter for Extended Target Tracking\extendedTargetPHDfilter.m
     文件        5473  2010-05-19 00:48  A Gaussian Mixture PHD filter for Extended Target Tracking\extendedTargetTrackingPHD.m
     文件        6965  2010-05-19 00:37  A Gaussian Mixture PHD filter for Extended Target Tracking\extendedTargetTrackingPHD_mulitpleTargets.m
     文件         488  2010-03-08 23:07  A Gaussian Mixture PHD filter for Extended Target Tracking\generateClutter.m
     文件         911  2010-03-09 17:49  A Gaussian Mixture PHD filter for Extended Target Tracking\generateExtendedMeasurements.m
     文件      690065  2014-04-17 10:56  A Gaussian Mixture PHD filter for Extended Target Tracking\GranstromLO2010_FUSION_ETGMPHD.pdf
     文件        1113  2010-05-19 00:42  A Gaussian Mixture PHD filter for Extended Target Tracking\initParameters_project.m
     文件        4709  2010-05-19 00:45  A Gaussian Mixture PHD filter for Extended Target Tracking\partitionMeasurementSet_4.m
     文件        3715  2010-05-19 00:45  A Gaussian Mixture PHD filter for Extended Target Tracking\phdFilter.m
     文件        1036  2010-05-19 00:46  A Gaussian Mixture PHD filter for Extended Target Tracking\phdPruning.m
     文件         266  2010-05-19 00:46  A Gaussian Mixture PHD filter for Extended Target Tracking\phdStateExtraction.m
     文件         951  2010-05-19 00:29  A Gaussian Mixture PHD filter for Extended Target Tracking\plotPHDsurface.m
     文件         760  2010-05-24 19:39  A Gaussian Mixture PHD filter for Extended Target Tracking\README.txt
     文件         245  2010-05-19 00:27  A Gaussian Mixture PHD filter for Extended Target Tracking\Sigmacircle.m