多机动模型PHD滤波算法

% 定义机动模型集合（示例：匀速+匀加速模型）
models = {
    struct('F', [1 1;0 1], 'Q', diag([0.1,0.01])), % CV模型
    struct('F', [1 1 0;0 1 1;0 0 1], 'Q', diag([0.05,0.01,0.001])) % CA模型
};
​
% 初始化PHD滤波器
phd = PHDFilter();
phd.Models = models;
phd.BirthModel = struct('lambda', 50, 'weight', 0.1); % 新生目标模型

function particles = predict(particles, models, dt)
    for i = 1:numel(particles)
        % 选择当前模型
        model_idx = randsample(length(models), 1, true, particles(i).weights);
        model = models{model_idx};
        
        % 状态预测
        particles(i).state = model.F * particles(i).state + sqrt(model.Q) * randn(size(model.F,1),1);
        particles(i).weight = particles(i).weight * model.SurvivalProb;
    end
end

function particles = update(particles, measurements, models)
    for m = 1:length(models)
        % 计算模型似然度
        likelihood = computeLikelihood(particles, measurements, models{m});
        
        % 权重更新
        particles.Weight = particles.Weight .* likelihood;
    end
    
    % 重采样（系统化解退）
    particles = resample(particles);
    
    % 模型概率更新（自适应IMM）
    transition_probs = estimateTransitionProbs(particles, models);
    particles.ModelProbs = transition_probs * particles.ModelProbs;
end

function estimates = extractStates(particles)
    % 聚类提取目标状态
    clusters = DBSCAN(particles.state, 3, 0.5); % 基于欧氏距离聚类
    estimates = cell(size(clusters));
    for i = 1:numel(clusters)
        estimates{i} = mean(clusters(i).points, 1);
    end
end

function P = estimateTransitionProbs(particles, models)
    % 基于粒子权重的贝叶斯估计
    num_models = length(models);
    P = zeros(num_models);
    
    for i = 1:numel(particles)
        for j = 1:num_models
            P(j) = P(j) + particles(i).weight * models(j).TransitionProb(i);
        end
    end
    
    P = P / sum(P); % 归一化
end

% 联合估计目标数与状态
[cardinality, state_estimates] = cphdFilter(particles);
adjusted_weights = adjustWeightsByCardinality(particles, cardinality);

% 并行计算粒子更新
parfor i = 1:numel(particles)
    particles(i) = updateParticle(particles(i), models);
end
​
% CUDA内核加速似然计算
likelihood = gpuArray(zeros(size(particles)));
kernel<<<numBlocks, threadsPerBlock>>>(likelihood, particles, measurements);

%% 仿真参数设置
simTime = 100; % 秒
dt = 0.1;      % 时间步长
numTargets = 5;% 目标数量
​
%% 生成真实轨迹
trueStates = cell(numTargets,1);
for i = 1:numTargets
    model = randsample(models,1);
    trueStates{i} = simulateTrajectory(model, simTime, dt);
end
​
%% 运行PHD滤波
estimates = cell(simTime,1);
for t = 1:simTime
    measurements = generateMeasurements(trueStates{t}, sensorModel);
    [estimates{t}, modelProbs] = phd.update(measurements);
end
​
%% 结果可视化
figure;
plotTrajectories(trueStates, estimates);
title('多机动目标跟踪结果');
legend('真实轨迹','估计轨迹');