分布式滤波、多源信息融合与多目标跟踪技术及其Matlab代码

引言
数据关联
- 最近邻法（NN）
- - 核心代码
- 概率数据关联法（PDA）
- - 核心代码
- 结果展示
基于信息融合的目标跟踪
- 集中式滤波（CI）
- - 核心代码
- 分布式滤波
- - 基于非重复扩散的分布式kalman滤波（ND）
  - - 核心代码
  - ICF
  - - 核心代码
  - CE-DKF
  - - 核心代码
  - DHIF
  - - 核心代码
  - 结果展示

引言

本文首先介绍了数据关联方法中的最近邻法和概率数据关联法，并对两者的性能进行比较实验，最后给出代码。此外，还介绍了信息融合的不同算法，包括基于协方差交叉法的集中式融合、基于非重复扩散的分布式kalman滤波和ICF、CEDKF、DHIF等分布式滤波方法。

数据关联

在传感器网络进行目标探测时，由于无法得到对目标的先验信息并且受传感器性能的限制，在探测过程中必然会引入探测误差。当进行多目标探测时，无法判断量测信息是源于正确目标还是错误目标，甚至是虚假目标。正是由于传感器网络的多目标探测过程中有着大量的不确定性以及复杂性，使得传感器节点与其目标的跟踪关系减弱，因此需要运用数据关联算法寻求解决方法。

概率数据关联法（PDA）

本节介绍了概率数据关联算法，该算法认为所有量测都有一定概率源于感兴趣的目标，只是不同量测源于目标的可能性不同。
为了便于说明，设定以下符号表示特定的含义：
Zk表示传感器在k时刻的量测集合；
zk,i：k时刻传感器接收到的第i个量测；
mk：表示k时刻接收到的量测数量，同时也表示目标数量；
在这里插入图片描述：表示直到时刻k的所有量测集合；
：表示zk,i是来自目标的量测。
再令

表示在k时刻第个量测来自目标这一事件的概率。根据定义可以看出，在跟踪范围无穷大时(即存在目标无法被探测到的概率为0)，该事件集合是整个事件空间的一个不相交完备分割，从而有
在这里插入图片描述
计算依赖的基本假设是：
(1)假定非兴趣目标在以其他目标状态为均值并服从高斯分布，即

(2)正确量测同样服从高斯分布，即

其中

代表给定新息时的似然函数。而量测误差

表示用第i个量测计算的新息向量，nz表示量测的维数，PG表示正确量测在分辨范围内的概率。因为假定跟踪门无穷大，PG的值为1。
(3)每个采样时刻至多有一个真实量测。因此，有
在这里插入图片描述
其中

对于i的任一情形，Zk的联合概率密度函数为

其中

根据假设(3)，量测集中有且仅有一个量测源于目标。此时

对目标状态进行更新可得

其中，xkk为利用量测zk对目标状态的估计，即

其中，K为Kalman增益矩阵。针对线性传感器及目标运动模型，Kalman增益矩阵为
在这里插入图片描述
从而目标状态估计可以表达为

其中

即代表组合新息。相应地，目标状态估计协方差为

其中

此时，完成了多个数据与多个航迹的关联。

核心代码

核心代码：（完整代码见文末）

function [out_est,out_P] = PDA(measure,R,est,P,cou_target)
%IS_CONNECT 多个数据与一条航迹关联，不设置跟踪门，所有量测按照正态分布计算
%概率数据关联法(PDA)
%   输入：量测，量测误差协方差，上一时刻不同目标的航迹(行向量，
%列数表示不同目标)，航迹的误差协方差
%   输出：当前时刻航迹,当前时刻航迹的误差协方差
% prob:表示概率，probability
global H;
%%
% 对上一时刻的估计进行预测
for cou_est =1:size(est,2)
    [est{1,cou_est},P{1,cou_est}]=kalman_filter(0,est{1,cou_est},...
        P{1,cou_est},0);
end
[dim_measure,num_measure]=size(measure);
num_target = size(est,2);
z_error = cell(num_target,num_measure);
% 不同量测来源于目标时的概率
prob_target=zeros(1,num_measure);
% 不同量测来源于其他目标时的概率
prob_other=zeros(1,num_measure);
for cou_measure = 1:num_measure
    for cou_other = 1:num_target
        if cou_other~=cou_target
            z_error{cou_other,cou_measure} = measure(:,cou_measure)-...
                H*est{1,cou_other};
            prob_other(1,cou_measure)=prob_other(1,cou_measure)+...
                exp(-0.5*(z_error{cou_other,cou_measure})'/R*...
                (z_error{cou_other,cou_measure}))/...
                ((2*pi)^(dim_measure/2)*(det(R))^0.5);
        end
    end
    prob_other(1,cou_measure) = prob_other(1,cou_measure)/(num_target-1);
    z_error{cou_target,cou_measure}=measure(:,cou_measure)-...
        H*est{1,cou_target};
    prob_target(1,cou_measure)=exp(-0.5*(z_error{cou_target,cou_measure})'/R*...
        (z_error{cou_target,cou_measure}))/...
        ((2*pi)^(dim_measure/2)*(det(R))^0.5);
end
% 研究生毕设论文的式(4-12)
prob_z = zeros(1,num_measure);
% 研究生毕设论文的式(4-16)
gama = zeros(1,num_measure);
% 研究生毕设论文的式(4-10)
c=0;
% 计算不同量测源于目标的概率并用于滤波
for cou_measure = 1:num_measure
    % 计算其他量测源于非目标的概率
    prob_z_other = 1;
    for cou_other = 1:num_measure
        if cou_other ~= cou_measure
            prob_z_other = prob_z_other * prob_other(1,cou_other);
        end
    end
    % 计算量测源于目标的概率
    prob_z(1,cou_measure)=prob_target(1,cou_measure)*prob_z_other;
    gama(1,cou_measure)=1/num_measure;
    % 累计研究生毕设论文的式(4-10)
    c=c+prob_z(1,cou_measure)*gama(1,cou_measure);
end
% 计算beta(4-9)
beta=zeros(1,num_measure);
est_temp = cell(1,num_measure);
P_temp = cell(1,num_measure);
out_est = zeros(size(est{1,cou_target}));
% 式(4-27)
z_error_sum=zeros(dim_measure,1);
% 式(4-30)中的累加
z_error_P=zeros(dim_measure);
for cou_measure = 1:num_measure
    beta(1,cou_measure)=prob_z(1,cou_measure)*gama(1,cou_measure)/c;
    % 使用当前量测用于滤波
    [est_temp{1,cou_measure},P_temp{1,cou_measure}]=kalman_filter(...
        measure(:,cou_measure),est{1,cou_target},P{1,cou_target},1);
    % 将不同量测的滤波结果进行融合
    out_est = out_est +beta(1,cou_measure) * est_temp{1,cou_measure};
    z_error_sum = z_error_sum + beta(1,cou_measure)*...
        z_error{cou_target,cou_measure};
    z_error_P = z_error_P + beta(1,cou_measure)*...
        z_error{cou_target,cou_measure}*...
        (z_error{cou_target,cou_measure})';
end
% 式(4-25)
K = P{1,cou_target}*H'/(H*P{1,cou_target}*H'+R);
% 式(4-31)
out_P_c = (diag(ones(size(P{1,cou_target},1),1))-K*H)*P{1,cou_target};
% 式(4-30)
P_error = K*(z_error_P-z_error_sum*z_error_sum')*K';
out_P = out_P_c+P_error;
end

结果展示

在这里插入图片描述

基于信息融合的目标跟踪

传感器网络的信息融合问题主要分为两种，分别是集中式融合和分布式融合。其中，经典的集中式算法有并行滤波、序贯滤波和数据压缩滤波。文献证明了这三中算法是等效的，并同时具有是全局最优估计，通常情况下作为算法比较的参照对象。分布式传感器网络与集中式传感器网络不同，集中式传感器网络具有可以与所有节点进行通信的中心节点，而分布式传感器网络中的每个节点只能与其相邻节点通信并得到自身和邻接传感器节点的带有噪声的目标状态信息。

集中式滤波（CI）

本节主要介绍序贯滤波算法。接下来，使用序贯Kalman滤波基于量测对目标进行集中式的信息融合，得到更精确的目标信息。假定当前的估计及估计误差协方差为在这里插入图片描述、。由于各平台在同一时刻的量测噪声之间互不相关，所以在融合节点可以按照接受信息的顺序对融合中心的目标运动状态估计值进行序贯更新。
(1)基于经典Kalman滤波求状态预测以及预报误差的协方差阵。
在这里插入图片描述

(2)序贯信息滤波器

至此，我们完成了集中式制导信息融合。

核心代码

核心代码：（完整代码见文末）

function [CI_x,CI_P] = CI(KF_x_now,KF_P_now)
%CI 集中式协方差交叉法
%   输入：KF_x_now(行为状态，列为不同传感器)，KF_P_now(行向量，不同列表示
%不同传感器估计误差协方差)
%   输出：CI_x(不同传感器的估计结果)
[num_state, num_sensor] = size(KF_x_now);
CI_P1 = zeros(num_state);
CI_Px = zeros(num_state,1);
for cou_sensor = 1:num_sensor
    CI_P1 = CI_P1 + inv(KF_P_now{1,cou_sensor});
    CI_Px = CI_Px + KF_P_now{1,cou_sensor}...
        KF_x_now(:,cou_sensor);
end
CI_P=inv(CI_P1);
CI_x=(CI_P1/num_sensor)(CI_Px/num_sensor);
end