多目標(biāo)跟蹤模擬

信息融合課程報(bào)告——多傳感器-多目標(biāo)跟蹤算法學(xué)院自動(dòng)化學(xué)院專(zhuān)業(yè)控制科學(xué)與工程學(xué)號(hào)151060019學(xué)生姓名任磊磊指導(dǎo)教師劉偉峰2023年6月15日一、實(shí)驗(yàn)場(chǎng)景與參數(shù)設(shè)置1.多目標(biāo)CV運(yùn)動(dòng)監(jiān)測(cè)區(qū)域?yàn)?，檢測(cè)概率為0.98，雜波密度為λ=1×10-5m-2，既平均40個(gè)雜波點(diǎn)，狀態(tài)協(xié)方差陣，，目標(biāo)的初始分布為x0,j~Nx0,j,B0xk,j=Az其中Qk,j=covωk-1,j,2.最近鄰算法：二、報(bào)告要求仿真雜波條件下3個(gè)目標(biāo)的CV運(yùn)動(dòng)軌跡〔仿真總步數(shù)K=50s〕；采用Kalman濾波算法，分別完成傳感器1，2對(duì)多目標(biāo)的分別跟蹤過(guò)程。采用最緊鄰算法進(jìn)行數(shù)據(jù)關(guān)聯(lián)。通過(guò)100次蒙特卡羅仿真實(shí)驗(yàn)，比照分析傳感器1，2的RMSE跟蹤誤差。3〕采用集中式多傳感器，進(jìn)行多目標(biāo)跟蹤估計(jì)，給出跟蹤結(jié)果圖，并進(jìn)行RMSE誤差分析(-1000m,1000m)(1000m,1000m)目標(biāo)1目標(biāo)1目標(biāo)目標(biāo)2目標(biāo)目標(biāo)2(-1000m,-1000m)(1000m,-1000m)實(shí)現(xiàn)過(guò)程：1〕1.首先在matlab中初始化所需矩陣，設(shè)置目標(biāo)的初始信息程序如下。n=50;%仿真時(shí)間L=100;%仿真次數(shù)range=[-10001000;-10001000];lambda=40;PD=1;num_target=3;%一共3個(gè)目標(biāo)A=[1100;0100;0011;0001];B=[0.50;10;00.5;01];C=[1000;0010];B0=diag([1002510025]);Q=[40;04];%噪聲過(guò)程的協(xié)方差陣q_std=sqrt(Q);%噪聲過(guò)程的標(biāo)準(zhǔn)差陣R=[1000;0100];%量測(cè)噪聲協(xié)方差陣r_std=sqrt(R);%量測(cè)噪聲標(biāo)準(zhǔn)差陣Sz1=inv(R);Sz2=inv(R);Sz3=inv(R);%-----------定義矩陣含義-------------X1=zeros(4,n);%目標(biāo)1的真實(shí)狀態(tài)X2=zeros(4,n);%目標(biāo)2的真實(shí)狀態(tài)X3=zeros(4,n);%目標(biāo)3的真實(shí)狀態(tài)Xp=zeros(4,3*n);%目標(biāo)狀態(tài)的提前一步預(yù)測(cè)Xe=zeros(4,3*n);%目標(biāo)某時(shí)刻的估計(jì)狀態(tài)Z1=zeros(2,n);%目標(biāo)1真實(shí)量測(cè)Z2=zeros(2,n);%目標(biāo)2真實(shí)量測(cè)Z3=zeros(2,n);%目標(biāo)3真實(shí)量測(cè)Zp=zeros(2,3*n);%某時(shí)刻目標(biāo)量測(cè)的提前一步預(yù)測(cè)Zk=zeros(2,3*n);%認(rèn)定為目標(biāo)量測(cè)的集合Pe=zeros(4,4,3*n);%濾波誤差的協(xié)方差陣Pp=zeros(4,4,3*n);%預(yù)測(cè)誤差的協(xié)方差陣K=zeros(4,2,3*n);%濾波增益RMSE=zeros(1,3*n);error=zeros(1,3*n);%------------初始化3個(gè)目標(biāo)的狀態(tài)----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.按照題中要求所給的目標(biāo)運(yùn)動(dòng)方程編寫(xiě)程序如下。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.運(yùn)行生成目標(biāo)運(yùn)動(dòng)軌跡如圖：2〕采用Kalman濾波算法對(duì)前面的兩個(gè)傳感器分別對(duì)多目標(biāo)進(jìn)行跟蹤，傳感器1的跟蹤結(jié)果以及誤差如下列圖：傳感器1的跟蹤效果傳感器1的跟蹤誤差曲線(xiàn)傳感器2的跟蹤結(jié)果以及誤差如下列圖：傳感器2的跟蹤效果傳感器2的跟蹤誤差曲線(xiàn)從兩個(gè)跟蹤誤差圖可以看出，當(dāng)多目標(biāo)的狀態(tài)模型參數(shù)一致時(shí)，同一傳感器的跟蹤精度差異不大，對(duì)于不同傳感器，噪聲小的跟蹤效果好。3)多傳感器多目標(biāo)跟蹤仿真：采用集中式多傳感器濾波算法，同時(shí)使用兩個(gè)傳感器的觀測(cè)數(shù)據(jù)對(duì)目標(biāo)進(jìn)行估計(jì)。這里采用卡爾曼濾波算法，首先利用第一個(gè)傳感器的觀測(cè)數(shù)據(jù)進(jìn)行目標(biāo)軌跡估計(jì)，并將此估計(jì)值作為預(yù)測(cè)值同時(shí)利用第二個(gè)傳感器的數(shù)據(jù)進(jìn)行估計(jì)，得到最終估計(jì)結(jié)果。集中式多傳感器估計(jì)效果集中式多傳感器估計(jì)誤差曲線(xiàn)針對(duì)每個(gè)不同的仿真條件，從圖中可以看出，不同的傳感器觀測(cè)噪聲會(huì)直接影響觀測(cè)效果，觀測(cè)噪聲小，那么跟蹤效果好，然而，集中式多傳感器比兩個(gè)單傳感器的跟蹤效果好，這是因?yàn)槎鄠鞲衅鞯牡挠^測(cè)信息多，同時(shí)進(jìn)行估計(jì)那么誤差就小。附錄：matlab代碼：clear;closeall;n=50;%仿真時(shí)間L=100;%蒙特卡洛仿真次數(shù)range=[-10001000;-10001000];lambda=40;PD=1;num_target=3;%一共3個(gè)目標(biāo)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];%噪聲過(guò)程的協(xié)方差陣q_std=sqrt(Q);%噪聲過(guò)程的標(biāo)準(zhǔn)差陣R=[1000;0100];%量測(cè)噪聲協(xié)方差陣r_std=sqrt(R);%量測(cè)噪聲標(biāo)準(zhǔn)差陣Sz1=inv(R);Sz2=inv(R);Sz3=inv(R);%-----------定義矩陣含義-------------X1=zeros(4,n);%目標(biāo)1的真實(shí)狀態(tài)X2=zeros(4,n);%目標(biāo)2的真實(shí)狀態(tài)X3=zeros(4,n);%目標(biāo)3的真實(shí)狀態(tài)Xp=zeros(4,3*n);%目標(biāo)狀態(tài)的提前一步預(yù)測(cè)Xe=zeros(4,3*n);%目標(biāo)某時(shí)刻的估計(jì)狀態(tài)Z1=zeros(2,n);%目標(biāo)1真實(shí)量測(cè)Z2=zeros(2,n);%目標(biāo)2真實(shí)量測(cè)Z3=zeros(2,n);%目標(biāo)3真實(shí)量測(cè)Zp=zeros(2,3*n);%某時(shí)刻目標(biāo)量測(cè)的提前一步預(yù)測(cè)Zk=zeros(2,3*n);%認(rèn)定為目標(biāo)量測(cè)的集合Pe=zeros(4,4,3*n);%濾波誤差的協(xié)方差陣Pp=zeros(4,4,3*n);%預(yù)測(cè)誤差的協(xié)方差陣K=zeros(4,2,3*n);%濾波增益RMSE=zeros(1,3*n);error=zeros(1,3*n);%------------初始化3個(gè)目標(biāo)的狀態(tài)----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');%加載目標(biāo)軌跡數(shù)據(jù)formont=1:L%蒙特卡洛仿真%------------Kalman濾波跟蹤過(guò)程-----fork=1:n-1%%%--三個(gè)目標(biāo)的真實(shí)航跡和量測(cè)------已經(jīng)保存真實(shí)航跡數(shù)據(jù)數(shù)據(jù)，%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濾波預(yù)測(cè)過(guò)程Xp(:,k+1)=A*Xe(:,k);%求k時(shí)刻目標(biāo)1狀態(tài)的提前一步預(yù)測(cè)Zp(:,k+1)=C*Xp(:,k+1);%求k時(shí)刻目標(biāo)1量測(cè)的提前一步預(yù)測(cè)Pp(:,:,k+1)=A*Pe(:,:,k)*A'+B*Q*B';%求k時(shí)刻目標(biāo)1預(yù)測(cè)誤差的協(xié)方差陣Xp(:,k+51)=A*Xe(:,k+50);%求k時(shí)刻目標(biāo)2狀態(tài)的提前一步預(yù)測(cè)Zp(:,k+51)=C*Xp(:,k+51);%求k時(shí)刻目標(biāo)2量測(cè)的提前一步預(yù)測(cè)Pp(:,:,k+51)=A*Pe(:,:,k+50)*A'+B*Q*B';%求k時(shí)刻目標(biāo)2預(yù)測(cè)誤差的協(xié)方差陣Xp(:,k+101)=A*Xe(:,k+100);%求k時(shí)刻目標(biāo)3狀態(tài)的提前一步預(yù)測(cè)Zp(:,k+101)=C*Xp(:,k+101);%求k時(shí)刻目標(biāo)3量測(cè)的提前一步預(yù)測(cè)Pp(:,:,k+101)=A*Pe(:,:,k+100)*A'+B*Q*B';%求k時(shí)刻目標(biāo)3預(yù)測(cè)誤差的協(xié)方差陣%----模擬雜波并獲得所有可能的量測(cè)點(diǎn)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);%目標(biāo)可能的量測(cè)與提前一步預(yù)測(cè)的距離%disp(num_meas)%------獲取最近鄰量測(cè)---------------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));D(1,i,k+101)=(Z_real(:,i)-Zp(:,k+101))'*Sz3*(Z_real(:,i)-Zp(:,k+101));endd1=min(D(1,:,k+1));%求距離中的最小值x=find(D(1,:,k+1)==d1);%找到最小值的位置目標(biāo)1d2=min(D(1,:,k+51));%目標(biāo)2y=find(D(1,:,k+51)==d2);d3=min(D(1,:,k+101));%目標(biāo)3z=find(D(1,:,k+101)==d3);%------kalman濾波更新過(guò)程Zk(:,k+1)=Z_real(:,x(1));%獲取目標(biāo)1的新的量測(cè)Sk1=(C*Pp(:,:,k+1)*C'+R);K(:,:,k+1)=Pp(:,:,k+1)*C'*inv(Sk1);%k時(shí)刻系統(tǒng)的濾波增益a=Zk(:,k+1)-Zp(:,k+1);%新息Zk(:,k+51)=Z_real(:,y(1));%獲取目標(biāo)2的新量測(cè)Sk2=(C*Pp(:,:,k+51)*C'+R);K(:,:,k+51)=Pp(:,:,k+51)*C'*inv(Sk2);b=Zk(:,k+51)-Zp(:,k+51);%新息Zk(:,k+101)=Z_real(:,z(1));%獲取目標(biāo)3的新量測(cè)Sk3=(C*Pp(:,:,k+101)*C'+R);K(:,:,k+101)=Pp(:,:,k+101)*C'*inv(Sk3);c=Zk(:,k+101)-Zp(:,k+101);%新息Xe(:,k+1)=Xp(:,k+1)+K(:,:,k+1)*a;%目標(biāo)1的濾波更新值Pe(:,:,k+1)=Pp(:,:,k+1)-K(:,:,k+1)*C*Pp(:,:,k+1);%目標(biāo)1的濾波誤差的協(xié)方差Xe(:,k+51)=Xp(:,k+51)+K(:,:,k+51)*b;%目標(biāo)2的濾波更新值Pe(:,:,k+51)=Pp(:,:,k+51)-K(:,:,k+51)*C*Pp(:,:,k+51);%目標(biāo)2的濾波誤差的協(xié)方差陣Xe(:,k+101)=Xp(:,k+101)+K(:,:,k+101)*c;%目標(biāo)3的濾波更新值Pe(:,:,k+101)=Pp(:,:,k+101)-K(:,:,k+101)*C*Pp(:,:,k+101);%目標(biāo)3的濾波誤差的協(xié)方差陣%%跟蹤誤差計(jì)算error(1,k+1)=sum((Xe([13],k+1)-X1([13],k+1)).^2);error(1,k+51)=sum((Xe([13],k+51)-X2([13],k+1)).^2);error(1,k+101)=sum((Xe([13],k+101)-X3([13],k+1)).^2);%%繪制實(shí)時(shí)跟蹤圖clf;holdon;%Zk量測(cè)，Zp提前一步量測(cè)的預(yù)測(cè)，Xe目標(biāo)狀態(tài)估計(jì)，X1目標(biāo)真實(shí)狀態(tài)%plot真實(shí)軌跡plot(X1(1,2:k+1),X1(3,2:k+1),'-r','LineWidth',1.6);plot(X2(1,2:k+1),X2(3,2:k+1),'--g','LineWidth',1.6);plot(X3(1,2:k+1),X3(3,2:k+1),':b','LineWidth',1.6);%plot估計(jì)軌跡plot(Xe(1,2:k+1),Xe(3,2:k+1),'k-+','LineWidth',1.2);plot(Xe(1,52:k+51),Xe(3,52:k+51),'k-^','LineWidth',1.2);plot(Xe(1,102:k+101),Xe(3,102:k+101),'k-o','LineWidth',1.2);plot(Z_real(1,:),Z_real(2,:),'k*','LineWidth',1.6);%axis(equal);axis(limit);xlabel('X/m','fontsize',10);ylabel('Y/m','fontsize',10);legend('目標(biāo)1真實(shí)軌