高斯和概率假设密度滤波gmphd算法实现程序

基于高斯和的概率假设密度滤波算法的实现程序，测试过，有注释。用于多对象连续位置估计等应用。

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

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       6073  2010-05-19 00:44  fusion2010_gmphd_for_extended_target_tracking\extendedTargetPHDfilter.m

     文件       5473  2010-05-19 00:48  fusion2010_gmphd_for_extended_target_tracking\extendedTargetTrackingPHD.m

     文件       6965  2010-05-19 00:38  fusion2010_gmphd_for_extended_target_tracking\extendedTargetTrackingPHD_mulitpleTargets.m

     文件        488  2010-03-08 23:07  fusion2010_gmphd_for_extended_target_tracking\generateClutter.m

     文件        911  2010-03-09 17:49  fusion2010_gmphd_for_extended_target_tracking\generateExtendedMeasurements.m

     文件       1113  2010-05-19 00:42  fusion2010_gmphd_for_extended_target_tracking\initParameters_project.m

     文件       4709  2010-05-19 00:45  fusion2010_gmphd_for_extended_target_tracking\partitionMeasurementSet_4.m

     文件       3715  2010-05-19 00:45  fusion2010_gmphd_for_extended_target_tracking\phdFilter.m

     文件       1036  2010-05-19 00:46  fusion2010_gmphd_for_extended_target_tracking\phdPruning.m

     文件        266  2010-05-19 00:46  fusion2010_gmphd_for_extended_target_tracking\phdStateExtraction.m

     文件        951  2010-05-19 00:29  fusion2010_gmphd_for_extended_target_tracking\plotPHDsurface.m

     文件        760  2010-05-24 19:39  fusion2010_gmphd_for_extended_target_tracking\README.txt

     文件        245  2010-05-19 00:27  fusion2010_gmphd_for_extended_target_tracking\Sigmacircle.m

     目录          0  2013-04-12 16:01  fusion2010_gmphd_for_extended_target_tracking

----------- ---------  ---------- -----  ----

                32705                    14