当前统计模型机动目标跟踪

T=1; %采样间隔 N=300; %目标点迹真实点迹个数 a=1/60; %机动频率，机动加速度的倒数 a_max=50; %目标加速度上限 M=100; %100次蒙特卡洛实验 %------------------状态噪声协方差—— q11=(1-exp(-2*a*T)+2*a*T+2*a^3*T^3/3*T^2-2*a^2-4*a*T*exp(-a*T))/(2*a^5); q12=(exp(-2*a*T)+1-2*exp(-a*T)+2*a*T*exp(-a*T)-2*a*T+a^2*T^2)/(2*a^4); q13=(1-exp(-2*a*T)-2*a*T*exp(-a*T))/(2*a^3); q21=q12; q22=(4*exp(-a*T)-3-exp(-2*a*T)+2*a*T)/(2*a^3); q23=(exp(-2*a*T)+1-2*exp(-a*T))/(2*a^2); q31=q13; q32=q23; q33=(1-exp(-2*a*T))/(2*a); %--------------------------常系数矩阵--------------------------% F=[1 T (a*T-1+exp(-a*T))/a^2 0 1 (1-exp(-a*T))/a 0 0 exp(-a*T)]; U=[T^2*a^2/2-(a*T-1+exp(-a*T))/a^2; T-(1-exp(-a*T))/a; 1-exp(-a*T)]; %-------------------------滤波参数初始化-----------------------% X=zeros(3,N); % 滤波值存储矩阵初始化 x1=10000;vx1=300;ax1=0; % 距离、速度、加速度 X(:,1)=[x1;vx1;ax1]; % 滤波值初始化 X_P=zeros(3,N); % 预测值存储矩阵初始化 X_P(:,1)=[x1;vx1;ax1]; % 预测值初始化 P_P=zeros(3,3,N); % 协方差预测存储矩阵初始化 P=zeros(3,3,N); % 协方差存储矩阵初始化 P0=[10^(4) 0 0 0 25 0 0 0 1]; P(:,:,1)=P0; % 协方差初始化 P_P(:,:,1)=P0; % 协方差预测值初始化 R=10^4*rand(1,N); % 量测噪声协方差矩阵存储器初始化 H=[1 0 0]; % 量测矩阵 %目标的真实运动轨迹 y=track1(); %测量值含有噪声 Z=y+100*rand(1,N); % 滤波与误差计算 for j=1:M for k=1:N-1 X_P(:,k+1)=F*X(:,k)+U*X_P(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=2*a*ca*[q11 q12 q13 q21 q22 q23 q31 q32 q33]; P_P(:,:,k+1)=F*P(:,:,k)*F'+Q; 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)); %滤波值 量测－量测的预测，只是量测矩阵为H=[1 0 0] 恰好和状态预测的第一行的距离值相等 P(:,:,k+1)=(eye(3)-K(:,:,k+1)*H)*P_P(:,:,k+1); end X_monte(:,:,j)=X(:,:); end for j=1:M MSE=0; MSE=MSE+(X_monte(1,:,j)-y).^2; end % 画图 ---------------真实值、观测值、滤波值------------------% figure hold on; plot(y,'r'); % 真实值 plot(Z,'b'); % 测量值 plot(X(1,:),'g-.'); % 滤波值 title('滤波结果分析图'); legend('真实值','测量值','滤波值'); hold off; % -------------------误差分析图------------------------% RMSE=sqrt(MSE/M); figure plot(RMSE,'r'); legend('CS'); xlabel('采样时刻'); ylabel('位置均方根误差m'); grid on;