CS.rar_CS跟踪_cs跟踪模型_目标跟踪 文献_目标跟踪 模型_目标跟踪模型

%%%%%%%%%%%%%%%%%%%%%本程序为验证2008非线性“当前”统计模型及自适应跟踪算法%%%%%%%%% %%%%%%%%%%%%%状态向量为【x vx ax a】,实现对机动频率的自适应估计%%%%%%%%%%%%%%%%%%%%%%%%%%%%% clear all;clc; T=1; N=300; a_max=50; M=100; ce=1;%%%输入白噪声的方差 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% a=zeros(1,N); a(1)=1/60; X=zeros(4,N); x1=10000;vx1=-300;ax1=0; X(:,1)=[x1;vx1;ax1;a(1)];%%%%%%%%%%%%%%信号滤波值的初始值%%%%%%%%%%%% X_P=zeros(4,N);%%%%%%%%%%%%%%%%一步预测值%%%%%%%%%%%%%%%%%%%%%%% X_P(:,1)=[x1;vx1;ax1;a(1)]; P_P=zeros(4,4,N);%%%%%%%%%一步预测协方差矩阵%……%%% P=zeros(4,4,N);%%%%协方差矩阵%%%%% P0=[10^(4) 0 0 0 0 25 0 0 0 0 1 0 0 0 0 ce]; R=10^4*rand(1,N); %%%%协方差矩阵初始值%%%%%%%%% P(:,:,1)=P0; P_P(:,:,1)=P0; H=[1 0 0 0]; y=track(); Z=y+100*rand(1,N); for j=1:M for k=1:N-1 %%%%%%%%%%%%%%%%%%状态噪声的协方差%%%%%%%%%%%%%%%%%%%%%% q11(k)=(1-exp(-2*a(k)*T)+2*a(k)*T+2*a(k)^3*T^3/3-2*a(k)^2*T^2-4*a(k)*T*exp(-a(k)*T))/(2*a(k)^5); q12(k)=(exp(-2*a(k)*T)+1-2*exp(-a(k)*T)+2*a(k)*T*exp(-a(k)*T)-2*a(k)*T+a(k)^2*T^2)/(2*a(k)^4); q13(k)=(1-exp(-2*a(k)*T)-2*a(k)*T*exp(-a(k)*T))/(2*a(k)^3); q21(k)=q12(k); q22(k)=(4*exp(-a(k)*T)-3-exp(-2*a(k)*T)+2*a(k)*T)/(2*a(k)^3); q23(k)=(exp(-2*a(k)*T)+1-2*exp(-a(k)*T))/(2*a(k)^2); q31(k)=q13(k); q32(k)=q23(k); q33(k)=(1-exp(-2*a(k)*T))/(2*a(k)); %%%%%%%%%%%%%%%%%%%%%常系数矩阵%%%%%%%%%%%%%%%%%% U(:,:,k)=[T^2/2-(a(k)*T-1+exp(-a(k)*T))/a(k)^2;T-(1-exp(-a(k)*T))/a(k);1-exp(-a(k)*T);0]; F(:,:,k)=[1 T (a(k)*T-1+exp(-a(k)*T))/a(k)^2 0 0 1 (1-exp(-a(k)*T))/a(k) 0 0 0 exp(-a(k)*T) 0 0 0 0 1]; X_P(:,k+1)=F(:,:,k)*X(:,k)+U(:,:,k)*X(3,k+1); if X_P(3,k+1)<0 ca=(4-pi)/pi*(-a_max+X(3,k))^2; else ca=(4-pi)/pi*(a_max-X(3,k))^2; end Q(:,:,k)=[2*a(k)*ca*q11(k) 2*a(k)*ca*q12(k) 2*a(k)*ca*q13(k) 0 2*a(k)*ca*q21(k) 2*a(k)*ca*q22(k) 2*a(k)*ca*q23(k) 0 2*a(k)*ca*q31(k) 2*a(k)*ca*q32(k) 2*a(k)*ca*q33(k) 0 0 0 0 T*ce]; P_P(:,:,k+1)=F(:,:,k)*P(:,:,k)*F(:,:,k)'+Q(:,:,k); K(:,:,k+1)=P_P(:,:,k+1)*H'*inv(H*P_P(:,:,k+1)*H'+R(k+1)); X(:,k+1)=X_P(:,k+1)+K(:,:,k+1)*(Z(k+1)-X_P(1,k+1)); P(:,:,k+1)=(eye(4)-K(:,:,k+1)*H)*P_P(:,:,k+1); a(k+1)=X(4,k+1); end X_monte(:,:,j)=X(:,:); end for j=1:M MSE=0; MSE=MSE+(X_monte(1,:,j)-y).^2; end RMSE=sqrt(MSE/M); compare=CS(); plot(RMSE,'r'); hold on; plot(compare,'g'); legend('CSa(改进算法)','CS'); xlabel('采样时刻'); ylabel('位置均方根误差（m）'); grid on;