来源:https://www.worthpen.top/blog?id=658ec95c9c4d834fea4ff57d
目录
引言
本文首先介绍了数据关联方法中的最近邻法和概率数据关联法,并对两者的性能进行比较实验,最后给出代码。此外,还介绍了信息融合的不同算法,包括基于协方差交叉法的集中式融合、基于非重复扩散的分布式kalman滤波和ICF、CEDKF、DHIF等分布式滤波方法。
数据关联
在传感器网络进行目标探测时,由于无法得到对目标的先验信息并且受传感器性能的限制,在探测过程中必然会引入探测误差。当进行多目标探测时,无法判断量测信息是源于正确目标还是错误目标,甚至是虚假目标。正是由于传感器网络的多目标探测过程中有着大量的不确定性以及复杂性,使得传感器节点与其目标的跟踪关系减弱,因此需要运用数据关联算法寻求解决方法。
最近邻法(NN)
最近邻法是一种能对多个探测信息进行量测-航迹关联的数据关联算法,该方法在跟踪区域之内将与被探测目标的估计状态统计距离最小的量测进行数据关联。该距离定义为与误差协方差相关的量测误差的范数
其中,z表示量测误差向量,S为量测误差协方差矩阵,d^2可以理解为目标预测位置与有效回波之间的统计距离。
最近邻法选择落在传感器的跟踪门之内并且与感兴趣的目标的预测状态最接近的一个量测作为与数据关联的对象,所谓“最近”表示与误差协方差相关的量测误差的范数最小。当目标量测在某一空间内密度较高时,不同目标的跟踪波门之间存在大量的重叠,统计距离最近的量测不一定源于感兴趣的目标。因此,最近邻法的鲁棒性不强差,在目标比较接近或者目标做大幅度机动运动时容易出现关联错误的情况。
核心代码
核心代码:(完整代码见文末)
% 求预测
[est,P]=kalman_filter(0,est,P,0);
dist_min =Inf;
NN_measure=0;
% 选出距离最近的量测
for cou_data = 1:size(measure,2)
dist = (measure(:,cou_data)-H*est)'/R*(measure(:,cou_data)-H*est);
if dist>=0
if dist<dist_min
dist_min=dist;
NN_measure=measure(:,cou_data);
temp=cou_data;
end
end
end
% if temp~=cou
% disp('NN data associate error')
% end
[out_est,out_P]=kalman_filter(NN_measure,est,P,1);
概率数据关联法(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增益矩阵为
从而目标状态估计可以表达为
其中
即代表组合新息。相应地,目标状态估计协方差为
其中
此时,完成了多个数据与多个航迹的关联。
核心代码
核心代码:(完整代码见文末)
% 对上一时刻的估计进行预测
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
prob_z = zeros(1,num_measure);
gama = zeros(1,num_measure);
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;
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;
结果展示
基于信息融合的目标跟踪
传感器网络的信息融合问题主要分为两种,分别是集中式融合和分布式融合。其中,经典的集中式算法有并行滤波、序贯滤波和数据压缩滤波。文献证明了这三中算法是等效的,并同时具有是全局最优估计,通常情况下作为算法比较的参照对象。分布式传感器网络与集中式传感器网络不同,集中式传感器网络具有可以与所有节点进行通信的中心节点,而分布式传感器网络中的每个节点只能与其相邻节点通信并得到自身和邻接传感器节点的带有噪声的目标状态信息。
集中式滤波(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
分布式滤波
基于非重复扩散的分布式kalman滤波(ND)
论文:《Distributed Kalman Filtering Based on the Non-Repeated Diffusion Strategy》
初始化:
本地更新:
扩散更新:
通信并融合更新:
节点i发送给邻接节点j,
核心代码
核心代码:(完整代码见文末)
ND_x = zeros(num_state, num_sensor);
ND_send_Px_temp = cell(num_sensor);
ND_send_P1_temp = cell(num_sensor);
for cou_sensor = 1:num_sensor
% cou_neighbor为准备接受cou_sensor的信息的节点
for cou_neighbor = 1:num_sensor
if Net(cou_neighbor,cou_sensor)==1
ND_sum_P1 = 0;
ND_sum_Px = 0;
ND_w = 0;
% cou_send为cou_sensor接受到的传感器的估计
for cou_send = 1:cou_sensor
if Net(cou_sensor,cou_send)==1
if cou_send ~= cou_neighbor
ND_sum_Px = ND_sum_Px+...
ND_send_Px{cou_sensor,cou_send};
ND_sum_P1 = ND_sum_P1 + ...
ND_send_P1{cou_sensor,cou_send};
ND_w = ND_w + 1;
end
end
end
if has_measure == 1
ND_sum_P1=(ND_sum_P1+inv(KF_P_last{1,cou_sensor}))/(ND_w+1);
ND_x_temp = ND_sum_P1\(ND_sum_Px + ...
KF_P_last{1,cou_sensor}\KF_x_last(:,cou_sensor))/...
(ND_w+1);
ND_P_temp = A/ND_sum_P1 *A'+ gama * Q * gama';
ND_x_temp = A * ND_x_temp;
ND_send_P1_temp{cou_neighbor,cou_sensor} = ...
inv(ND_P_temp)+ H'/R*H;
ND_send_Px_temp{cou_neighbor,cou_sensor} = ...
ND_P_temp\ND_x_temp +H'/R*measure_now(:,cou_sensor);
else
ND_send_P1_temp{cou_neighbor,cou_sensor} = (ND_sum_P1 +...
inv(KF_P_now{1,cou_sensor}))/...
(ND_w+1);
ND_send_Px_temp{cou_neighbor,cou_sensor} = (ND_sum_Px +...
KF_P_now{1,cou_sensor}\KF_x_now(:,cou_sensor))/(ND_w+1);
end
end
end
end
ND_send_Px = ND_send_Px_temp;
ND_send_P1 = ND_send_P1_temp;
for cou_sensor = 1:num_sensor
ND_P1 = 0;
ND_Px = 0;
ND_w = 0;
for count_send = 1:num_sensor
if Net(cou_sensor,count_send)==1
ND_P1 = ND_P1 + ND_send_P1{cou_sensor,count_send};
ND_Px = ND_Px + ND_send_Px{cou_sensor,count_send};
ND_w = ND_w + 1;
end
end
ND_P1 = (ND_P1 + inv(KF_P_now{1,cou_sensor}))/(ND_w + 1);
ND_x(:,cou_sensor) = ND_P1\(ND_Px + KF_P_now{1,cou_sensor}\...
KF_x_now(:,cou_sensor))/(ND_w + 1);
end
ICF
《Information weighted consensus》
核心代码
核心代码:(完整代码见文末)
for cou_sensor = 1:num_sensor
ICF_P1{1,cou_sensor} = zeros(num_state);
ICF_Px{1,cou_sensor} = zeros(num_state,1);
end
if has_measure==1
% 为了实现在第一时刻时,上一时刻状态取当前时刻状态,因为第一时刻前无
%状态,且将初始状态设置于ICF_x的第一时刻位置中
if t==1
cou=2;
else
cou=t;
end
for cou_sensor = 1:num_sensor
ICF_forecast = A * ICF_x{1,cou_sensor}(:,cou-1);
ICF_P_forecast = (A*ICF_x{2,cou_sensor} *A'+...
gama*Q* gama');
ICF_Px_temp{1,cou_sensor} = inv(ICF_P_forecast)./...
num_sensor*ICF_forecast+H'/R*measure_now(:,cou_sensor);
ICF_P1_temp{1,cou_sensor} = inv(ICF_P_forecast)./...
num_sensor + H'/R*H;
end
else
for cou_sensor = 1:num_sensor
ICF_Px_temp{1,cou_sensor} = inv(ICF_x{2,cou_sensor})./...
num_sensor*ICF_x{1,cou_sensor}(:,t);
ICF_P1_temp{1,cou_sensor} = inv(ICF_x{2,cou_sensor})./num_sensor;
end
end
for cou_sensor = 1:num_sensor
for count_send = 1:num_sensor
if Net(cou_sensor,count_send)==1
ICF_P1{1,cou_sensor} = ICF_P1{1,cou_sensor} + ...
ICF_P1_temp{1,count_send}-ICF_P1_temp{1,cou_sensor};
ICF_Px{1,cou_sensor} = ICF_Px{1,cou_sensor} + ...
ICF_Px_temp{1,count_send}-ICF_Px_temp{1,cou_sensor};
end
end
ICF_P1{1,cou_sensor} = ICF_P1{1,cou_sensor} +ICF_P1_temp{1,cou_sensor};
ICF_Px{1,cou_sensor} = ICF_Px{1,cou_sensor} +ICF_Px_temp{1,cou_sensor};
ICF_x{2,cou_sensor} = num_sensor.*inv(ICF_P1{1,cou_sensor});
ICF_x{1,cou_sensor}(:,t) = ICF_P1{1,cou_sensor}\ICF_Px{1,cou_sensor};
end
CE-DKF
《 Cost-effective diffusion Kalman filtering with implicit measurement exchanges.》
核心代码
核心代码:(完整代码见文末)
if has_measure==1
if t==1
cou=2;
else
cou=t;
end
for cou_sensor = 1:num_sensor
CEDKF_forecast{1,cou_sensor} = A* CEDKF_x{1,cou_sensor}(:,cou-1);
CEDKF_P_forecast{1,cou_sensor}=A* CEDKF_x{2,cou_sensor}...
*A'+ gama*Q* gama';
CEDKF_P1{1,cou_sensor} = inv(CEDKF_P_forecast{1,cou_sensor})...
+(CEDKF_Ni(cou_sensor,1)+1)*H'/R*H;
end
else
for cou_sensor = 1:num_sensor
CEDKF_forecast{1,cou_sensor} = CEDKF_x{1,cou_sensor}(:,t);
CEDKF_P1{1,cou_sensor} = inv(CEDKF_x{2,cou_sensor});
end
end
CEDKF_x_temp = cell(1,num_sensor);
for cou_sensor = 1:num_sensor
CEDKF_P1_temp = CEDKF_P1{1,cou_sensor};
for count_send = 1:num_sensor
if Net(cou_sensor,count_send)==1
CEDKF_P1_temp = CEDKF_P1_temp + CEDKF_P1{1,count_send};
end
end
CEDKF_x{2,cou_sensor} = inv(CEDKF_P1_temp./(CEDKF_Ni(cou_sensor,1)+1));
if has_measure==1
CEDKF_G = (CEDKF_Ni(cou_sensor,1)+1)*CEDKF_x{2,cou_sensor}*H'/R;
CEDKF_x_temp{1,cou_sensor} = CEDKF_forecast{1,cou_sensor}+CEDKF_G*...
(measure_now(:,cou_sensor)-H*CEDKF_forecast{1,cou_sensor});
else
CEDKF_x_temp{1,cou_sensor}=CEDKF_forecast{1,cou_sensor};
end
end
for cou_sensor = 1:num_sensor
CEDKF_x{1,cou_sensor}(:,t) = CEDKF_x_temp{1,cou_sensor};
for count_send = 1:num_sensor
if Net(cou_sensor,count_send)==1
CEDKF_x{1,cou_sensor}(:,t) = ...
CEDKF_x{1,cou_sensor}(:,t)+CEDKF_x_temp{1,count_send};
end
end
CEDKF_x{1,cou_sensor}(:,t) = ...
CEDKF_x{1,cou_sensor}(:,t)./(CEDKF_Ni(cou_sensor,1)+1);
end
DHIF
《On the Convergence Conditions of Distributed Dynamic State Estimation Using Sensor Networks: A Unified Framework.》
核心代码
核心代码:(完整代码见文末)
if has_measure==1
% 为了实现在第一时刻时,上一时刻状态取当前时刻状态,因为第一时刻前无
%状态,且将初始状态设置于ICF_x的第一时刻位置中
if t==1
cou=2;
else
cou=t;
end
for cou_sensor = 1:num_sensor
DHIF_P1{1,cou_sensor} = inv(A* ...
DHIF_x{2,cou_sensor}*A'+ gama*Q* gama');
DHIF_Px{1,cou_sensor} = DHIF_P1{1,cou_sensor}...
*(A * DHIF_x{1,cou_sensor}(:,cou-1));
DHIF_S{1,cou_sensor} = H'/R*H;
DHIF_y{1,cou_sensor} = H'/R*measure_now(:,cou_sensor);
end
else
for cou_sensor = 1:num_sensor
DHIF_P1{1,cou_sensor} = inv(DHIF_x{2,cou_sensor});
DHIF_Px{1,cou_sensor} = DHIF_P1{1,cou_sensor}...
*(DHIF_x{1,cou_sensor}(:,t));
DHIF_S{1,cou_sensor} = 0;
DHIF_y{1,cou_sensor} = 0;
end
end
for cou_sensor = 1:num_sensor
DHIF_P1_temp = DHIF_P1{1,cou_sensor};
DHIF_Px_temp = DHIF_Px{1,cou_sensor};
DHIF_S_temp = DHIF_S{1,cou_sensor};
DHIF_y_temp = DHIF_y{1,cou_sensor};
for count_send = 1:num_sensor
if Net(cou_sensor,count_send)==1
DHIF_P1_temp = DHIF_P1_temp + DHIF_P1{1,count_send};
DHIF_Px_temp = DHIF_Px_temp + DHIF_Px{1,count_send};
DHIF_S_temp = DHIF_S_temp +DHIF_S{1,cou_sensor};
DHIF_y_temp = DHIF_y_temp +DHIF_y{1,cou_sensor};
end
end
DHIF_x{2,cou_sensor} = inv(DHIF_S_temp + ...
DHIF_P1_temp / (CEDKF_Ni(cou_sensor,1)+1));
DHIF_x{1,cou_sensor}(:,t) = DHIF_x{2,cou_sensor} *...
(DHIF_y_temp + DHIF_Px_temp / (CEDKF_Ni(cou_sensor,1)+1));
end
结果展示
结果如下图所示,其中KF表示Kalman滤波。其中没有显示ICF的结果,实际上有该算法的结果,但是该算法在有限步数内发散,其由于使用了一致性原理而适用于无限步数。
完整代码分为多个文件(除以上外,还包括经典Kalman滤波、扩展Kalman、无迹Kalman、异步滤波等),数量较多,无法放在文章中。完整代码在公众号(沸腾的火锅资源号)中自取。