单目标跟踪小结（3）：MATLAB实践

内容主要提取自 edX 平台上Chalmers的micromaster项目：Emerging Automotive Technologies: Sensor Fusion and Non-linear Filtering for Automotive Systems。

在单目标跟踪小结（1）：理论推导和单目标跟踪小结（2）：算法中，我们推导单目标跟踪问题的理论公式并介绍了三种算法。在本文中，我们将采用高斯叠加滤波算法（Gaussian Sum Filtering）跟踪一个随机运动的目标。运动方程和目标观测方程均为线性方程。

为了避免在计算过程中数据溢出，我们对权重取对数。

我们先编写两个函数 。第一个函数 normalizedLogWeigjhts 用于归一化取对数后的权重，代码如下：

function [log_w,log_sum_w] = normalizeLogWeights(log_w)
% 输入: log_w - 待归一化的权重的log值，列向量
% 输出:    log_w - 归一化后的权重的log值
%      log_sum_w - 归一化前的权重和的log

    if length(log_w)<=1
        log_sum_w = log_w;
        log_w = log_w-log_sum_w;
    else
        [log_w_aux,I] = sort(log_w,'descend');
        log_sum_w = max(log_w_aux)+log(1+sum(exp(log_w(I(2:end))-max(log_w_aux)))); %先减去最大项(负数)再取指数 
        log_w = log_w-log_sum_w; % 归一化
    end
end

第二个函数 mixtureReduction 用于融合相似的高斯分布，相似的判断依据是马氏距离。代码如下：

function [w_hat,states_hat] = mixtureReduction(w,states,threshold)
%使用贪心算法融合相似的高斯分布
%输入:     w: 归一化后的各高斯分布的权重，取log， 列向量
%     states: 结构体数组（列向量）, 结构体包含: x 状态列向量； P 协方差矩阵
%  threshold: 融合阈值
%I输出:      w_hat: 归一化后的融合后的各高斯分布权重
%       states_hat: 融合后，剩余高斯分布结构体数组
            
    if length(w) == 1  % 只有一项的话，直接输出
        w_hat = w;
        states_hat = states;
        return;
    end
    
    I = 1:length(states); %给各高斯分布编号
    el = 1;

    while ~isempty(I) % I 非空，对剩下的继续融合
        [~,j] = max(w); %每次从剩余的最大权重对应项开始
        temp = [states(I).x] - states(j).x;
        val = diag(temp.'*(states(j).P\temp));% 马氏距离，判断所有项跟最大项的相似性
        Ij = I(val<threshold); % 待融合项，至少包含 j 自身
        [temp,w_hat(el,1)] = normalizeLogWeights(w(Ij)); % 待融合项的权重和为融合后的权重
        % 计算融合后的均值和协方差
        states_hat(el,1).x = [states(Ij).x]*exp(temp);
        states_hat(el,1).P = zeros(size(states(1).P));
        states_Ij = states(Ij);
        for i=1:length(states_Ij)
            states_hat(el,1).P = states_hat(el,1).P + exp(temp(i))*states_Ij(i).P + exp(temp(i))*(states_hat(el,1).x-states_Ij(i).x)*(states_hat(el,1).x-states_Ij(i).x)';
        end
        I = setdiff(I,Ij); % 从 I 中删除已融合的项，然后继续找
        w(Ij,1) = log(eps); % 将已被融合项的权重置为最小，避免再被选中
        el = el+1;
    end
end

主代码如下：

clc;
clear;
%% 传感器参数
sensor.P_D = 0.9; % 目标测量概率
sensor.lambda = 10; % 泊松分布参数
sensor.range = [-1000 1000;-1000 1000]; % 传感器视域
sensor.pdf_c = 1/prod(sensor.range(:,end)-sensor.range(:,1)); % 空间概率密度 fc(c)
sensor.intensity_c = sensor.lambda*sensor.pdf_c; % 杂波强度为常数

%% 线性运动模型，
T = 1; %采样时间
sigma_q = 5; 
motion.d = 4; % 状态向量维度，[ x; y; vx; vy ]
motion.F = [ 1 0 T 0;
             0 1 0 T;
             0 0 1 0;
             0 0 0 1]; % 状态转移矩阵
motion.Q = sigma_q^2*[ T^4/4       0   T^3/2       0;
                         0   T^4/4       0   T^3/2;
                     T^3/2       0     T^2       0;
                         0   T^3/2       0     T^2 ];% 运动模型协方差矩阵
        
%% 线性测量模型
sigma_r = 10;
measmodel.d = 2; % 观测向量维度，[x; y]
measmodel.H = [ 1 0 0 0;
                0 1 0 0 ]; % 观测矩阵
measmodel.R = sigma_r^2*eye(2); % 两个维度的观测相互独立

%% 初值
initial_state.x= [0; 0; 10; 10]; % 初始状态
initial_state.P = eye(4); % 初始协方差

%% 生成数据
%目标运动数据
K = 100; % 总的时间步长
objectdata = zeros(motion.d,K+1); % 每一列储存一个目标真实状态
objectstate = initial_state.x;
objectdata(:,1) = objectstate;
for k = 2:K+1
    objectstate = mvnrnd(motion.F*objectstate, motion.Q)'; % 考虑运动模型误差
    objectdata(:,k) = objectstate;
end
%观测数据
Z = cell(K,1); % 每个cell储存一个观测结果矩阵 Zk
for k = 1:K
    % 根据泊松分布生成杂波数
    N_c = poissrnd(sensor.lambda);
    % 均匀分布生成杂波
    C = repmat(sensor.range(:,1),[1 N_c]) + diag(sensor.range*[-1; 1])*rand(measmodel.d,N_c);
    
    meas = [];
    if rand<sensor.P_D
        meas = mvnrnd(measmodel.H*objectdata(:,k+1), measmodel.R)'; %生成目标观测
    end
    Z{k}= [meas C]; % 方便起见，将 meas 放在第一列                                                                 
end

%% 单目标跟踪-高斯叠加滤波 Gaussian Sum Filtering
P_G = 0.999;            % gating 概率
G = chi2inv(P_G,measmodel.d); % 根据卡方分布求距离阈值G
w_min = 1e-3;           % 删除阈值，权重小于此，删去
merging_threshold = 2;  % 融合阈值，马氏距离小于此，融合
M = 100;                % 叠加的最大高斯分布数量
estimates = cell(length(Z),1); % 每个时刻的估计
%%%%%%%%%%%%%%%%%%%%%% 高斯叠加滤波算法开始 %%%%%%%%%%%%%%%%%%%%%%
weight = 0; % k=0时的权重为1；以下计算，权重取log，可以避免精度溢出
components = initial_state;
for i=1:length(Z) % 每个时刻
    z = Z{i};
    w_all = [];
    components_all = [];
    for s=1:length(components) % 对 k-1 时刻的每个高斯成分
        w_1 = log(1-sensor.P_D)+weight(s); % w=0
        state_upd(1,1) = components(s); % w=0,无观测，不更新   
        if(~isempty(z))
            %%%%%%%%% gating %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
            z_hat = measmodel.H*components(s).x;
            S = measmodel.H*components(s).P*measmodel.H' + measmodel.R;
            S = (S+S')/2;% 保证 S 为正定的
            d2 = diag(abs((z-z_hat)'*S^-1*(z-z_hat)));% z-z_hat是矩阵，对角线元素为 z 中每个向量的马氏距离
            meas_in_gate = d2<G;
            z_in_gate = z(:,meas_in_gate);
            %%%%%%%%% 根据 gating 后的每个观测结果进行更新 %%%%%%%%%%%
            predicted_likelihood = log(mvnpdf(z_in_gate',(measmodel.H*components(s).x)',S));
            w = log(sensor.P_D/sensor.intensity_c) + predicted_likelihood + weight(s);%计算权重
            K = (components(s).P*measmodel.H')/S;
            for j=1:size(z_in_gate,2) % 对每个有效测量更新
                state_upd(1+j,1).x = components(s).x + K*(z_in_gate(:,j) - z_hat); %更新状态
                state_upd(1+j,1).P = (eye(size(components(s).x,1)) - K*measmodel.H)*components(s).P; %更新协方差矩阵
            end
            %%%%%%%%%%%%%%%% 汇总所有的结果 %%%%%%%%%%%%%%%%%%%%%%%
            w_all = [ w_all; [w_1;w] ];
            components_all =[components_all; state_upd];
            state_upd = state_upd(1,1); % 重置state_up
        else % 如果 z 为空，则只有一项 w1,state_upd(1,1)
            w_all = [ w_all; w_1 ];
            components_all =[components_all; state_upd(1,1)];
        end  
    end
    [w_all,~] = normalizeLogWeights(w_all);%归一化
    %%%%%%%%%%%% 删除权重过小的成分%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    keep = w_all > log(w_min);% 判断是否保留
    w_all = w_all(keep);
    components_all = components_all(keep);
    [w_all,~] = normalizeLogWeights(w_all);% 重新归一化
    %%%%%%%%%%%%% 融合相似项 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    [w_all,components_all] = mixtureReduction(w_all,components_all,merging_threshold);
    %%%%%%%%%% 如果超出允许的最大项数，截取权重最大的 M 个 %%%%%%%%%%%%%%%%%%
    if length(w_all) > M
        [~, cap_index] = sort(w_all,'descend');
        w_all = w_all(cap_index(1:M));
        [w_all,~] = normalizeLogWeights(w_all);%重新归一化
        components_all = components_all(cap_index(1:M));
    end
    %%%%%%%%%% 加权和，MMSE估计 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    estimates{i} = [components_all.x]*exp(w_all);
    %%%%%%%%%%%% 预测步 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    weight = w_all;
    components = components_all;
    for s=1:length(components_all)
        components(s).x = motion.F*components(s).x;
        components(s).P = motion.F*components(s).P*motion.F'+motion.Q;
    end
end
%%%%%%%%%%%%%%%%%%%%%%% 高斯叠加滤波结束 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%% 可视化 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
GS_estimated_state = cell2mat(estimates');
grid on
hold on
plot(objectdata(1,:), objectdata(2,:), 'g','Linewidth', 2)
scatter(GS_estimated_state(1,:), GS_estimated_state(2,:), 'r.' , 'Linewidth', 0.5)

xlabel('x (m)')
ylabel('y (m)')
legend('Ground Truth','Gaussian Sum', 'Location', 'best')
set(gca,'FontSize',12)

运行结果如下：