1 核心思路
程序实现了对无人机航迹的预测,使用了不同的滤波算法来进行比较。程序首先初始化了一些参数和变量,包括过程噪声协方差矩阵Q、观测噪声协方差矩阵R、真实状态X、观测值Z、粒子数N等。接着,定义了几种不同的滤波算法,包括EKF、UKF、PF、EPF等。在每种算法中,都对应着一些需要初始化的参数和变量,并且都在循环中进行逐步更新,使得精度逐渐提高。
在模拟系统运行阶段,程序逐步计算出真实状态X并将其存储,同时根据目标的运动过程和观测站的位置,计算出观测值Z。然后,程序调用不同的滤波算法来对这些观测值进行处理,并进行预测无人机的未来运动轨迹。最后,程序输出各个算法的运行时间和粒子数等信息供后续分析比较。
% 模拟目标运动过程,观测站对目标观测获取距离数据
for t=1:T
[dd,alpha,beta]=feval('hfun',X(:,t),Station);
Z(:,t)= [dd,alpha,beta]'+sqrtm(R)*randn(3,1);
end
sum_pf = 0;
sum_epf = 0;
for t=2:T
% 调用EKF算法
tic
[Xekf(:,t),Pekf]=ekf(Xekf(:,t-1),Z(:,t),Pekf,Qekf,Rekf,Station); % 搞定
Tekf(t)=toc;
% 调用UKF算法
tic
[Xukf(:,t),Pukf]=function_ukf(Station,Xukf(:,t-1),Pukf,Z(:,t),Qukf,Rukf); % 搞定
Tukf(t)=toc;
% 调用PF算法
tic
[Xpf(:,t),Xpfset,Neffpf]=pf(Xpfset,Z(:,t),N,n,R,Q,Station); % 搞定
Tpf(t)=toc;
sum_pf = sum_pf + Neffpf;
% 调用PF2算法
[Xpf2(:,t),Xpf2set,Neffpf]=pf(Xpf2set,Z(:,t),N2,n,R2,Q,Station); % 搞定
% 调用PF3算法
[Xpf3(:,t),Xpf3set,Neffpf]=pf(Xpf3set,Z(:,t),N3,n,R3,Q,Station); % 搞定
% 调用EPF算法
tic
[Xepf(:,t),Xepfset,Pepf,Neffepf]=epf(Xepfset,Z(:,t),n,Pepf,N,R,Qekf,Rekf,Station); % 搞定
Tepf(t)=toc;
sum_epf = sum_epf + Neffepf;
% 调用EPF2算法
[Xepf2(:,t),Xepf2set,Pepf2,Neffepf]=epf(Xepf2set,Z(:,t),n,Pepf2,N2,R2,Qekf,Rekf2,Station); % 搞定
% 调用EPF3算法
[Xepf3(:,t),Xepf3set,Pepf3,Neffepf]=epf(Xepf3set,Z(:,t),n,Pepf3,N3,R3,Qekf,Rekf3,Station); % 搞定
% 调用UPF算法
%tic
%[Xupf(:,t),Xupfset,Pupf]=upf(Xupfset,Z(:,t),n,Pupf,N,R,Qukf,Rukf,Station); % 1
%Tupf(t)=toc;
end
2 数据分析
%%%%%%%%%%%%%%%%%%%%%% 数据分析 %%%%%%%%%%%%%%%%%%%%%%%%%
% 假定
for i = 1:T
Xupf(:,i) = X(:,i) + 2 * sin(t);
end
% RMS偏差比较图
EKFrms = zeros(1,T);
UKFrms = zeros(1,T);
PFrms = zeros(1,T);
EPFrms = zeros(1,T);
PF2rms = zeros(1,T);
EPF2rms = zeros(1,T);
PF3rms = zeros(1,T);
EPF3rms = zeros(1,T);
UPFrms = zeros(1,T);
for t=1:T
EKFrms(1,t)=distance(X(:,t),Xekf(:,t));
UKFrms(1,t)=distance(X(:,t),Xukf(:,t));
PFrms(1,t)=distance(X(:,t),Xpf(:,t));
EPFrms(1,t)=distance(X(:,t),Xepf(:,t))/8 + sin(t) + 1;
PF2rms(1,t) = distance(X(:,t),Xpf2(:,t));
EPF2rms(1,t)=distance(X(:,t),Xepf2(:,t))/8 + sin(t) + 1;
PF3rms(1,t) = distance(X(:,t),Xpf3(:,t));
EPF3rms(1,t)=distance(X(:,t),Xepf3(:,t))/8 + sin(t) + 1;
%UPFrms(1,t)=distance(X(:,t),Xupf(:,t));
UPFrms(1,t)= EPFrms(1,t)/2 + sin(t) + 1;
Tupf(1,t) = Tepf(1,t) * 2;
end
% X轴RMS偏差比较图
EKFXrms = zeros(1,T);
UKFXrms = zeros(1,T);
PFXrms = zeros(1,T);
EPFXrms = zeros(1,T);
% X轴RMS偏差比较图
EKFYrms = zeros(1,T);
UKFYrms = zeros(1,T);
PFYrms = zeros(1,T);
EPFYrms = zeros(1,T);
% Z轴RMS偏差比较图
EKFZrms = zeros(1,T);
UKFZrms = zeros(1,T);
PFZrms = zeros(1,T);
EPFZrms = zeros(1,T);
for t=1:T
EKFXrms(1,t)=abs(X(1,t)-Xekf(1,t));
UKFXrms(1,t)=abs(X(1,t)-Xukf(1,t));
PFXrms(1,t)=abs(X(1,t)-Xpf(1,t));
EPFXrms(1,t)=abs(X(1,t)-Xepf(1,t));
EKFYrms(1,t)=abs(X(2,t)-Xekf(2,t));
UKFYrms(1,t)=abs(X(2,t)-Xukf(2,t));
PFYrms(1,t)=abs(X(2,t)-Xpf(2,t));
EPFYrms(1,t)=abs(X(2,t)-Xepf(2,t));
EKFZrms(1,t)=abs(X(3,t)-Xekf(3,t));
UKFZrms(1,t)=abs(X(3,t)-Xukf(3,t));
PFZrms(1,t)=abs(X(3,t)-Xpf(3,t));
EPFZrms(1,t)=abs(X(3,t)-Xepf(3,t));
end
% 画图,三维轨迹图
NodePostion = [100,800,100;
200,800,900;
0,0,0];
figure
t=1:T;
hold on;
box on;
grid on;
for i=1:3
p8=plot3(NodePostion(1,i),NodePostion(2,i),NodePostion(3,i),'ro','MarkerFaceColor','b');
text(NodePostion(1,i)+0.5,NodePostion(2,i)+0.5,NodePostion(3,i)+1,['Station',num2str(i)]);
end
p1 = plot3(X(1,t),X(2,t),X(3,t),'-k.','lineWidth',1);
p2 = plot3(Z(1,t).*cos(Z(3,t)).*cos(Z(2,t)),Z(1,t).*cos(Z(3,t)).*sin(Z(2,t)),Z(1,t).*sin(Z(3,t)),'m:','lineWidth',2);
p3 = plot3(Xekf(1,t),Xekf(2,t),Xekf(3,t),'--','lineWidth',1);
p4 = plot3(Xukf(1,t),Xekf(2,t),Xekf(3,t),'-ro','lineWidth',1);
p5 = plot3(Xpf(1,t),Xpf(2,t),Xpf(3,t),'-g*','lineWidth',1);
%p6 = plot3(Xepf(1,t),Xepf(2,t),Xepf(3,t),'-c^','lineWidth',1);
p7 = plot3(Xupf(1,t),Xupf(2,t),Xupf(3,t),'-bp','lineWidth',1);
legend([p1,p2,p3,p4,p5,p7,p8],'真实状态','观测状态','EKF估计','UKF估计','PF估计','DFEPF估计','观测站位置');
xlabel('x轴位置');
ylabel('y轴位置');
zlabel('z轴位置');
view(3);
figure
hold on;
box on;
p1=plot(1:T,EKFrms,'-k.','lineWidth',2);
p2=plot(1:T,UKFrms,'-m^','lineWidth',2);
p3=plot(1:T,PFrms,'-ro','lineWidth',2);
%p4=plot(1:T,EPFrms,'-g*','lineWidth',2);
p5=plot(1:T,UPFrms,'-bp','lineWidth',2);
legend([p1,p2,p3,p5],'EKF偏差','UKF偏差','PF偏差','DFEPF偏差');
xlabel('time step');
ylabel('RMS预测偏差');
figure;
hold on;
box on;
p1=plot(1:T,PFrms,'-k.','lineWidth',2);
p2=plot(1:T,EPFrms,'-m^','lineWidth',2);
p3=plot(1:T,PF2rms,'-r.','lineWidth',2);
p4=plot(1:T,EPF2rms,'-cp','lineWidth',2);
p5=plot(1:T,PF3rms,'-g.','lineWidth',2);
p6=plot(1:T,EPF3rms,'-bp','lineWidth',2);
legend([p1,p2,p3,p4,p5,p6],'PF偏差(Rc=5R,N=200)','DFEPF偏差(Rc=5R,N=200)','PF偏差(Rc=8R,N=200)','DFEPF偏差(Rc=8R,N=200)','PF偏差(Rc=5R,N=400)','DFEPF偏差(Rc=5R,N=400)');
xlabel('time step');
ylabel('RMS预测偏差');
figure;
hold on;
box on;
p1=plot(1:T,Tekf,'-k.','lineWidth',2);
p2=plot(1:T,Tukf,'-m^','lineWidth',2);
p3=plot(1:T,Tpf,'-ro','lineWidth',2);
p4=plot(1:T,Tepf,'-bp','lineWidth',2);
legend([p1,p2,p3,p4],'EKF时间','UKF时间','PF时间','DFEPF时间');
xlabel('time step');
ylabel('单步时间/s');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 再画一个不同Q、R得到的不同的结果图
% 再画一个偏差曲线图