多目标跟踪模拟

信息融合课程报告——多传感器-多目标跟踪算法学院自动化学院专业控制科学与工程学号151060019学生姓名任磊磊指导教师刘伟峰2016年6月15日一、实验场景与参数设置1.多目标CV运动监测区域为，检测概率为0.98，杂波密度为𝜆=1×10−5𝑚−2，既平均40个杂波点，状态协方差阵，，目标的初始分布为𝑥0,𝑗~𝒩(𝑥0,𝑗,𝐵0),𝑗=1,2,3其中𝑥0,1=[−900𝑚,30𝑚/𝑠,900𝑚,−30𝑚/s],𝑥0,2=[−900m,25m/s,−800m,30m/s]，𝑥0,3=[−900m,20m/s,0m,20m/s],𝐵0=𝑑𝑖𝑎𝑔([100,25,100,25])，目标运动和量测方程分别为：𝑥𝑘,𝑗=𝐴𝑥𝑘−1,𝑗+𝐵𝜔𝑘−1,𝑗𝑧𝑘,𝑠=𝐶𝑥𝑘−1,𝑠+𝑣𝑘−1,𝑠其中𝑄𝑘,𝑗=𝑐𝑜𝑣(𝜔𝑘−1,𝑗,𝜔𝑘−1,𝑗)=𝑑𝑖𝑎𝑔([4,4])𝑚2,𝑅𝑘,1=𝑐𝑜𝑣(𝑣𝑘−1,𝑠,𝑣𝑘−1,𝑠)=𝑑𝑖𝑎𝑔([100,100])𝑚2，𝑅𝑘,2=𝑐𝑜𝑣(𝑣𝑘−1,2,𝑣𝑘−1,2)=𝑑𝑖𝑎𝑔([25,25])𝑚22.最近邻算法：二、报告要求1）仿真杂波条件下3个目标的CV运动轨迹（仿真总步数K=50s）；2）采用Kalman滤波算法，分别完成传感器1，2对多目标的分别跟踪过程。(1)采用最紧邻算法进行数据关联。(2)通过100次蒙特卡罗仿真实验，对比分析传感器1，2的RMSE跟踪误差。3）采用集中式多传感器，进行多目标跟踪估计，给出跟踪结果图，并进行RMSE误差分析(-1000m,1000m)(1000m,1000m)(-1000m,-1000m)(1000m,-1000m)实现过程：1）1.首先在matlab中初始化所需矩阵，设置目标的初始信息程序如下。n=50;%仿真时间21|1|1|1|1,TkkkkkkkkkkkdzSzzzz目标1目标2目标2L=100;%仿真次数range=[-10001000;-10001000];lambda=40;PD=1;num_target=3;%一共3个目标A=[1100;0100;0011;0001];B=[0.50;10;00.5;01];C=[1000;0010];B0=diag([1002510025]);Q=[40;04];%噪声过程的协方差阵q_std=sqrt(Q);%噪声过程的标准差阵R=[1000;0100];%量测噪声协方差阵r_std=sqrt(R);%量测噪声标准差阵Sz1=inv(R);Sz2=inv(R);Sz3=inv(R);%-----------定义矩阵含义-------------X1=zeros(4,n);%目标1的真实状态X2=zeros(4,n);%目标2的真实状态X3=zeros(4,n);%目标3的真实状态Xp=zeros(4,3*n);%目标状态的提前一步预测Xe=zeros(4,3*n);%目标某时刻的估计状态Z1=zeros(2,n);%目标1真实量测Z2=zeros(2,n);%目标2真实量测Z3=zeros(2,n);%目标3真实量测Zp=zeros(2,3*n);%某时刻目标量测的提前一步预测Zk=zeros(2,3*n);%认定为目标量测的集合Pe=zeros(4,4,3*n);%滤波误差的协方差阵Pp=zeros(4,4,3*n);%预测误差的协方差阵K=zeros(4,2,3*n);%滤波增益RMSE=zeros(1,3*n);error=zeros(1,3*n);%------------初始化3个目标的状态----X1(:,1)=[-900;30;900;-30]+B0*randn(4,1);X2(:,1)=[-900;25;-800;30]+B0*randn(4,1);X3(:,1)=[-900;20;0;20]+B0*randn(4,1);Xe(:,1)=[-900;30;900;-30];Xe(:,51)=[-900;25;-800;30];Xe(:,101)=[-900;20;0;20];Pe(:,:,1)=[100000;02500;001000;00025];Pe(:,:,51)=[100000;02500;001000;00025];Pe(:,:,101)=[100000;02500;001000;00025];2.按照题中要求所给的目标运动方程编写程序如下。X1(:,k+1)=A*X1(:,k)+B*(q_std*randn(2,1));X2(:,k+1)=A*X2(:,k)+B*(q_std*randn(2,1));X3(:,k+1)=A*X3(:,k)+B*(q_std*randn(2,1));3.运行生成目标运动轨迹如图：2）采用Kalman滤波算法对前面的两个传感器分别对多目标进行跟踪，传感器1的跟踪结果以及误差如下图：传感器1的跟踪效果传感器1的跟踪误差曲线传感器2的跟踪结果以及误差如下图：传感器2的跟踪效果传感器2的跟踪误差曲线从两个跟踪误差图可以看出，当多目标的状态模型参数一致时，同一传感器的跟踪精度差别不大，对于不同传感器，噪声小的跟踪效果好。3)多传感器多目标跟踪仿真：采用集中式多传感器滤波算法，同时使用两个传感器的观测数据对目标进行估计。这里采用卡尔曼滤波算法，首先利用第一个传感器的观测数据进行目标轨迹估计，并将此估计值作为预测值同时利用第二个传感器的数据进行估计，得到最终估计结果。集中式多传感器估计效果集中式多传感器估计误差曲线针对每个不同的仿真条件，从图中可以看出，不同的传感器观测噪声会直接影响观测效果，观测噪声小，则跟踪效果好，然而，集中式多传感器比两个单传感器的跟踪效果好，这是因为多传感器的的观测信息多，同时进行估计则误差就小。附录：matlab代码：clear;closeall;n=50;%仿真时间L=100;%蒙特卡洛仿真次数range=[-10001000;-10001000];lambda=40;PD=1;num_target=3;%一共3个目标A=[1100;0100;0011;0001];B=[0.50;10;00.5;01];C=[1000;0010];B0=diag([1002510025]);%Sk1=[200;020];%Sk2=[200;020];%Sk3=[200;020];Q=[40;04];%噪声过程的协方差阵q_std=sqrt(Q);%噪声过程的标准差阵R=[1000;0100];%量测噪声协方差阵r_std=sqrt(R);%量测噪声标准差阵Sz1=inv(R);Sz2=inv(R);Sz3=inv(R);%-----------定义矩阵含义-------------X1=zeros(4,n);%目标1的真实状态X2=zeros(4,n);%目标2的真实状态X3=zeros(4,n);%目标3的真实状态Xp=zeros(4,3*n);%目标状态的提前一步预测Xe=zeros(4,3*n);%目标某时刻的估计状态Z1=zeros(2,n);%目标1真实量测Z2=zeros(2,n);%目标2真实量测Z3=zeros(2,n);%目标3真实量测Zp=zeros(2,3*n);%某时刻目标量测的提前一步预测Zk=zeros(2,3*n);%认定为目标量测的集合Pe=zeros(4,4,3*n);%滤波误差的协方差阵Pp=zeros(4,4,3*n);%预测误差的协方差阵K=zeros(4,2,3*n);%滤波增益RMSE=zeros(1,3*n);error=zeros(1,3*n);%------------初始化3个目标的状态----X1(:,1)=[-900;30;900;-30]+B0*randn(4,1);X2(:,1)=[-900;25;-800;30]+B0*randn(4,1);X3(:,1)=[-900;20;0;20]+B0*randn(4,1);%X1(:,1)=[-900;30;900;-30];%X2(:,1)=[-900;25;-800;30];%X3(:,1)=[-900;20;0;20];Xe(:,1)=[-900;30;900;-30];Xe(:,51)=[-900;25;-800;30];Xe(:,101)=[-900;20;0;20];Pe(:,:,1)=[100000;02500;001000;00025];Pe(:,:,51)=[100000;02500;001000;00025];Pe(:,:,101)=[100000;02500;001000;00025];load('targetStates.mat');%加载目标轨迹数据formont=1:L%蒙特卡洛仿真%------------Kalman滤波跟踪过程-----fork=1:n-1%%%--三个目标的真实航迹和量测------已经保存真实航迹数据数据，%X1(:,k+1)=A*X1(:,k)+B*(q_std*randn(2,1));Z1(:,k+1)=C*X1(:,k+1)+r_std*randn(2,1);%X2(:,k+1)=A*X2(:,k)+B*(q_std*randn(2,1));Z2(:,k+1)=C*X2(:,k+1)+r_std*randn(2,1);%X3(:,k+1)=A*X3(:,k)+B*(q_std*randn(2,1));Z3(:,k+1)=C*X3(:,k+1)+r_std*randn(2,1);%%---kalman滤波预测过程Xp(:,k+1)=A*Xe(:,k);%求k时刻目标1状态的提前一步预测Zp(:,k+1)=C*Xp(:,k+1);%求k时刻目标1量测的提前一步预测Pp(:,:,k+1)=A*Pe(:,:,k)*A'+B*Q*B';%求k时刻目标1预测误差的协方差阵Xp(:,k+51)=A*Xe(:,k+50);%求k时刻目标2状态的提前一步预测Zp(:,k+51)=C*Xp(:,k+51);%求k时刻目标2量测的提前一步预测Pp(:,:,k+51)=A*Pe(:,:,k+50)*A'+B*Q*B';%求k时刻目标2预测误差的协方差阵Xp(:,k+101)=A*Xe(:,k+100);%求k时刻目标3状态的提前一步预测Zp(:,k+101)=C*Xp(:,k+101);%求k时刻目标3量测的提前一步预测Pp(:,:,k+101)=A*Pe(:,:,k+100)*A'+B*Q*B';%求k时刻目标3预测误差的协方差阵%----模拟杂波并获得所有可能的量测点Nc=poissrnd(lambda);Zc=repmat(range(:,1),[1,Nc])+(range(1,2)-range(1,1))*rand(2,Nc);idx=find(rand(num_target,1)=PD);Z_real=[];iffind(idx==1)Z_real=[Z_real,Z1(:,k+1)];endiffind(idx==2)Z_real=[Z_real,Z2(:,k+1)];endiffind(idx==3)Z_real=[Z_real,Z3(:,k+1)];endZ_real=[Z_real,Zc];num_meas=size(Z_real,2);D=zeros(1,num_meas,3*n);%目标可能的量测与提前一步预测的距离%disp(num_meas)%------获取最近邻量测---------------fori=1:num_measD(1,i,k+1)=(Z_real(:,i)-Zp(:,k+1))'*Sz1*(Z_real(:,i)-Zp(:,k+1));D(1,i,k+51)=(Z_real(:,i)-Zp(:,k+51))'*Sz2*(Z_real(:,i)-Zp(:,k+51