算法Matlab程序

1、一、測(cè)試程序% PDA-FA算法實(shí)現(xiàn)% 何友雷達(dá)數(shù)據(jù)處理及應(yīng)用P116% 二維空間勻速直線運(yùn)動(dòng)，狀態(tài)向量為X=x,vx,y,vy% x1=x0+vxT% y1=y0+vyT% 仿真：% 1、改變虛假量測(cè)數(shù)量nc：公式求取、手動(dòng)設(shè)置% 2、改變量測(cè)噪聲R=r 0; 0 r，即r% 3、改變虛假量測(cè)位置q，偏離真實(shí)位置的程度% 4、關(guān)聯(lián)概率計(jì)算clc;clear;close all;%*% 參數(shù)設(shè)置%*I=eye(4);T = 1; %采樣間隔simTime = 100 ; %仿真步數(shù)A=1 T 0 0;0 1 0 0;0 0 1 T;0 0 0 1; %實(shí)際模型：CVH=1 0 0 0;0 0

2、1 0; %測(cè)量模型Q=0; %實(shí)際過程噪聲G = T2/2 0; T 0; 0 T2/2; 0 T; %噪聲加權(quán)矩陣r=200;R=r 0; 0 r; %量測(cè)噪聲X0=200;0;10000;-15; %初始狀態(tài)X(:,1)=X0;Vk=sqrt(r)*randn;sqrt(r)*randn;Zk(:,1)=H*X(:,1)+Vk;gama=16;lamda=0.0004;%*% 量測(cè)生成%*for i=2:1:simTime X(:,i)=A*X(:,i-1); % 真實(shí)狀態(tài) Vk=sqrt(r)*randn;sqrt(r)*randn; Zk(:,i)=H*X(:,i)+Vk; %生成量

3、測(cè)值end%*% PDA初始化%*Xk_PDA=200;0;10100;-16; %初始狀態(tài)、與實(shí)際值略有差別R11=r; R22=r; R12=0; R21=0;Pkk_PDA=R11 R11/T R12 R12/T; R11/T 2*R11/T2 R12/T 2*R12/T2; R21 R21/T R22 R22/T; R21/T 2*R21/T2 R22/T 2*R22/T2; %初始協(xié)方差Xkk = Xk_PDA ;Pkk = Pkk_PDA;X_Pre = A*Xkk;P_Pre=A*Pkk*A+G*Q*G;P=R;for i=1:1:simTime %* % 產(chǎn)生雜波 %* % 量

4、測(cè)確認(rèn)區(qū)域面積 Sk=H*P_Pre*H+ P; Av=pi*gama*sqrt(det(Sk); % 準(zhǔn)備生成雜波數(shù)目 nc=floor(10*Av*lamda+1);%設(shè)置雜波數(shù)量 q=sqrt(Av)/2; %q=sqrt(10*Av)/2; a=X(1,i)-q; b=X(1,i)+q; c=X(3,i)-q; d=X(3,i)+q; % 生成代表雜波的nc個(gè)虛假量測(cè) xi=a+(b-a)*rand(1,nc); yi=c+(d-c)*rand(1,nc); clear Z_Matrix; clear PZ_Matrix; for j=1:nc Z_Matrix(:,j) = xi(j

5、);yi(j); end Z_Matrix(:,nc+1)=Zk(:,i); PZ_Matrix = cat(3); for j=1:1:nc PZ_Matrix = cat(3,PZ_Matrix,q,0;0,q); end PZ_Matrix = cat(3,PZ_Matrix,R); %* % PDA關(guān)聯(lián) %* Z_Predict = H*X_Pre; PZ_Predict = H*P_Pre*H ; Combine_Z,Combine_R=PDA(Z_Matrix, PZ_Matrix, Z_Predict, PZ_Predict) ; % PDA Z_PDA(:,i) = Combi

6、ne_Z ; %* % 卡爾曼濾波 %* P=Combine_R; Xk_PDA,Pk_PDA,Kk_PDA=Kalman(Xkk,Pkk,Combine_Z,A,G,Q,H,P); Xkk=Xk_PDA; Pkk=Pk_PDA; % 預(yù)測(cè) X_Pre=A*Xkk; P_Pre=A*Pkk*A+G*Q*G; %出各個(gè)狀態(tài)值 Ex_PDA(i)=Xkk(1); Evx_PDA(i)=Xkk(2); Ey_PDA(i)=Xkk(3); Evy_PDA(i)=Xkk(4); error1_PDA(i)=Ex_PDA(i)-X(1,i);%Pkk(1,1); error2_PDA(i)=Ey_PDA(

7、i)-X(3,i);%Pkk(2,2); error3_PDA(i)=Evx_PDA(i)-X(2,i);%Pkk(3,3); error4_PDA(i)=Evy_PDA(i)-X(4,i);%Pkk(4,4);end%*% 繪圖%*i=1:simTime;figureplot(X(1,i),X(3,i),-,LineWidth,2); %真實(shí)值grid on; hold onplot(Ex_PDA(1,i),Ey_PDA(1,i),r-,LineWidth,2); %濾波值plot(Zk(1,i),Zk(2,i),*); %實(shí)際測(cè)量值plot(Z_PDA(1,i),Z_PDA(2,i),o)

8、; %組合測(cè)量值legend(真實(shí)值,濾波值,實(shí)際量測(cè),組合量測(cè));title(目標(biāo)運(yùn)動(dòng)軌跡); xlabel(x/m); ylabel(y/m);text(X(1,1)+1,X(3,1)+5,t=1);%位置誤差figuresubplot(211)plot(abs(error1_PDA(i),LineWidth,2); grid ontitle(位置誤差); xlabel(t/s); ylabel(error-x/m);subplot(212)plot(abs(error3_PDA(i),LineWidth,2); grid onxlabel(t/s); ylabel(error-y/m);

9、%速度誤差figuresubplot(211)plot(abs(error2_PDA(i),LineWidth,2); grid ontitle(速度誤差); xlabel(t/s); ylabel(error-vx/m/s);subplot(212)plot(abs(error4_PDA(i),LineWidth,2); grid onxlabel(t/s); ylabel(error-vy/m/s);二、PDA函數(shù)function Combine_Z,Combine_R = PDA(Z_Matrix, PZ_Matrix, Z_Predict, PZ_Predict)% 概率數(shù)據(jù)關(guān)聯(lián)，雜波

10、空間密度為泊松分布隨機(jī)變量% 輸入：% Z_Matrix：波門內(nèi)的所有有效量測(cè)值% PZ_Matrix：有效量測(cè)值的誤差方差陣% Z_Predict：預(yù)測(cè)量測(cè)值% PZ_Predict：預(yù)測(cè)量測(cè)值的誤差方差陣% 輸出：% Combine_R為組合量測(cè)% Combine_R：組合量測(cè)對(duì)應(yīng)的協(xié)方差% 中間變量：% beta為正確關(guān)聯(lián)概率lamda=0.0004;Pd=1; %檢測(cè)概率，當(dāng)不取1時(shí)，后面的a計(jì)算出來都是0Pg=0.9997; %門限概率nm=size(Z_Matrix);n=nm(2); % 量測(cè)數(shù)量m=nm(1); % 測(cè)量維數(shù)for i=1:1:n e(:,i)=Z_Matrix

11、(:,i)-Z_Predict; S(:,:,i)=PZ_Predict+PZ_Matrix(:,:,i); %新息協(xié)方差 X、R、Q互不相關(guān)條件下 % 何友 計(jì)算方法P115 式（7.36） % a(i)=exp(-1/2)*e(i)*inv(S(i)*e(i); % bk(i)=lamda*sqrt(2*pi)*det(S(i)*(1-Pd*Pg)/Pd; % 楊萬海P86式（3-5-7） a(i)=Pd*exp(-1/2)*(e(:,i)*inv(S(:,:,i)*e(:,i); bk(i)=lamda*(sqrt(2*pi)m*sqrt(det(S(:,:,i)*(1-Pd); end

12、for i=1:1:n beta_i(i)=a(i)/(bk(i) + sum(a); end% 擴(kuò)充正確關(guān)聯(lián)概率，使得每一維量測(cè)都有對(duì)應(yīng)的關(guān)聯(lián)概率beta = beta_i;for i=1:m-1 beta=beta;beta_i;endM = beta.*Z_Matrix;Combine_Z=sum(M,1);Combine_Z=Combine_Z;Combine_R=0;for i=1:n Combine_R = Combine_R + (beta(:,i)*beta(:,i).*PZ_Matrix(:,:,i);endbeta_i(n);end三、Kalman濾波函數(shù)function X,P,K=Kalman(X_Forward,P_Forward,Z,A,G,Q,H,R)%卡爾曼濾波%參數(shù)說明% Z-觀測(cè)數(shù)據(jù)矢量% A-系統(tǒng)模型狀態(tài)矩陣% G-系統(tǒng)模型噪聲系數(shù)矩陣% Q-系統(tǒng)模型噪聲方差% H-量測(cè)系數(shù)矩陣% R-量測(cè)模型噪聲協(xié)方差% X_Forward-前次估計(jì)狀態(tài)矢量% P_Forward-前次估計(jì)狀態(tài)協(xié)方差矩陣% X-輸出估計(jì)狀態(tài)矢量% P-輸出估計(jì)狀態(tài)協(xié)方差矩陣% 預(yù)測(cè)X_Pre=A*X_Forward;P_Pre=A*P_Forward*A+G*Q*G;% 增益矩陣K=P_Pre*H*inv(H*P_Pre*H+R);% Pzz = H*P_Forwa