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;
