多目標跟蹤模擬

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