【无人机】基于EKF、UKF、PF、改进PF滤波算法的无人机航迹预测（Matlab代码实现）

💥💥💥💞💞💞欢迎来到本博客❤️❤️❤️💥💥💥

🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。

⛳️座右铭：行百里者，半于九十。

目录

💥1 概述

📚2 运行结果

🎉3 参考文献

👨‍💻4 Matlab代码实现

💥1 概述

     工程测量中应用无人机航测技术，无人机不需要专业驾驶员进行操作作业，只需要无人机控制人员对拍摄影像随时注意查看，注意观察无人机的飞行状态。无人机操作技术也越来越简单，小型无人机机身、体积及重量都大大减少，最小的无人机只有巴掌大小，在手上就可以实现起飞与降落。无人机航测技术相较于传统客机航测技术大大减少了选择起飞降落地点的问题，其机身质量增加，在低空飞行可以很好地适应风向变化，降低飞行设备振动，避免对拍摄数据与图像采集造成影响。使用无人机航测技术只需要提前设定无人机飞行路线，随时观察无人机的飞行状态，根据实际情况修正。无人机系统发生问题时可以自行降落或飞回至起点排除故障，由于无人机结构简单，维修或零件更换成本较低，排除故障后可以重新飞行测量。

      无人机航测技术具有数据采集能力强、精确度高的优点，对无人机操作人员技术水平要求较低，极大程度减少了无人机操作时间与数据分析处理时间，无人机自身所携带的高清摄影相机与专业测绘仪器，可以将工程拍摄影像质量大幅度提高，提升工程测绘精确度。无人机具有一定的智能与自动化，可以在一定程度减少工作压力，提高工作效率质量。无人机还可以使用低空遥感模式对工程地理信息完成勘测，不管在高空还是超低空下都可以正常完成测绘作业，广泛应用于工程测量与应急救援领域中。

      无人机测绘技术在工程测量中应用具有较短的飞行周期，可以实现快速起飞降落，在数据获取分析及处理中，只需要投入较少时间。无人机测绘技术成本相较于传统客机测绘技术成本非常低，主要由于无人机制造材料成本低，保养频率低，零件构造简单，后期维修费用少，极大程度减少了工程测绘成本。无人机具有简单的操作模式，测绘人员可以在短时间内学会无人机设备操作和拍摄程序的使用，无需再投入其他成本，具有使用成本低的优点。

📚2 运行结果

部分代码：

%% %%%%%%%%%%% EKF滤波算法 %%%%%%%%%%%%
Qekf = Q;                   % EKF过程噪声方差
Rekf = R;                   % EKF过程噪声方差 
Xekf=zeros(9,T);            % 滤波状态
Xekf(:,1)=X(:,1);           % EKF滤波初始化
Pekf = P0;                  % 协方差
Tekf=zeros(1,T);            % 用于记录一个采样周期的算法时间消耗
%% %%%%%%%%%%% UKF滤波算法 %%%%%%%%%%%%           
Qukf = Q;              % UKF过程噪声方差
Rukf = R;              % UKF观测噪声方差 
Xukf=zeros(9,T);       % 滤波状态
Xukf(:,1)=X(:,1);      % UKF滤波初始化
Pukf = P0;             % 协方差
Tukf=zeros(1,T);       % 用于记录一个采样周期的算法时间消耗 
%% %%%%%%%%%%% PF滤波算法 %%%%%%%%%%%%
N = 200;                 % 粒子数              
Xpf=zeros(n,T);        % 滤波状态
Xpf(:,1)=X(:,1);       % PF滤波初始化
Xpfset=ones(n,N);      % 粒子集合初始化
for j=1:N   % 粒子集初始化
    Xpfset(:,j)=state0;  % 全都初始化为x0，每个粒子的值相等
end
Tpf=zeros(1,T);        % 用于记录一个采样周期的算法时间消耗 

%% %%%%%%%%%%% PF2滤波算法 %%%%%%%%%%%%
N2 = 200;                 % 粒子数 
R2 = [5000 0 0;                 % 观测噪声协方差矩阵  
    0 0.01^2 0                   % 角度的观测值偏差不能给的太大
    0 0 0.01^2];
Xpf2=zeros(n,T);        % 滤波状态
Xpf2(:,1)=X(:,1);       % PF滤波初始化
Xpf2set=ones(n,N2);      % 粒子集合初始化
for j=1:N2   % 粒子集初始化
    Xpf2set(:,j)=state0;  % 全都初始化为x0，每个粒子的值相等
end

%% %%%%%%%%%%% PF3滤波算法 %%%%%%%%%%%%
N3 = 400;                 % 粒子数 
R3 = [5000 0 0;                 % 观测噪声协方差矩阵  
    0 0.01^2 0                   % 角度的观测值偏差不能给的太大
    0 0 0.01^2];
Xpf3=zeros(n,T);        % 滤波状态
Xpf3(:,1)=X(:,1);       % PF滤波初始化
Xpf3set=ones(n,N3);      % 粒子集合初始化
for j=1:N3   % 粒子集初始化
    Xpf3set(:,j)=state0;  % 全都初始化为x0，每个粒子的值相等
end

%% %%%%%%%%%%% EPF滤波算法 %%%%%%%%%%%% 
Xepf=zeros(9,T);       % 滤波状态
Xepf(:,1)=X(:,1);      % EPF滤波初始化
Xepfset=ones(n,N);     % 粒子集合初始化，这里需要定义为一个3维数组，或者简单起见，一次性写完，定义为一个(9xN)的二维数组，表示当前状态的所有粒子
for j=1:N   % 粒子集初始化
    Xepfset(:,j)=state0;  % 全都初始化为state0，每个粒子的值相等
end
Pepf = P0*ones(n,n*N);% 各个粒子的协方差，这里需要定义为一个3维数组，或者简单起见，一次性写完，定义为一个9x(9xN)的二维数组
Tepf=zeros(1,T);       % 用于记录一个采样周期的算法时间消耗   

%% %%%%%%%%%%% EPF2滤波算法 %%%%%%%%%%%%
Rekf2 = R2;
Xepf2=zeros(9,T);       % 滤波状态
Xepf2(:,1)=X(:,1);      % EPF滤波初始化
Xepf2set=ones(n,N2);     % 粒子集合初始化，这里需要定义为一个3维数组，或者简单起见，一次性写完，定义为一个(9xN)的二维数组，表示当前状态的所有粒子
for j=1:N2   % 粒子集初始化
    Xepf2set(:,j)=state0;  % 全都初始化为state0，每个粒子的值相等
end
Pepf2 = P0*ones(n,n*N2);% 各个粒子的协方差，这里需要定义为一个3维数组，或者简单起见，一次性写完，定义为一个9x(9xN)的二维数组

%% %%%%%%%%%%% EPF3滤波算法 %%%%%%%%%%%%
Rekf3 = R3;
Xepf3=zeros(9,T);       % 滤波状态
Xepf3(:,1)=X(:,1);      % EPF滤波初始化
Xepf3set=ones(n,N3);     % 粒子集合初始化，这里需要定义为一个3维数组，或者简单起见，一次性写完，定义为一个(9xN)的二维数组，表示当前状态的所有粒子
for j=1:N3   % 粒子集初始化
    Xepf3set(:,j)=state0;  % 全都初始化为state0，每个粒子的值相等
end
Pepf3 = P0*ones(n,n*N3);% 各个粒子的协方差，这里需要定义为一个3维数组，或者简单起见，一次性写完，定义为一个9x(9xN)的二维数组

%%%%%%%%%%%%% UPF滤波算法 %%%%%%%%%%%%  
Xupf=zeros(n,T);       % 滤波状态  
Xupf(:,1)=X(:,1);      % UPF滤波初始化
Xupfset=ones(n,N);     % 粒子集合初始化     
for j=1:N   % 粒子集初始化
    Xupfset(:,j)=state0;  % 全都初始化为state0，每个粒子的值相等
end
Pupf = P0*ones(n,n*N); % 各个粒子的协方差    
Tupf=zeros(1,T);       % 用于记录一个采样周期的算法时间消耗        
 

Xmupf = zeros(n,T);       % 滤波状态  
Tmupf = zeros(1,T);
%% %%%%%%%%%%%%%%%%%%%% 模拟系统运行 %%%%%%%%%%%%%%%%%%%%%%%%%

for t=2:T
    % 模拟系统状态运行一步
    [y1,y2,y3,y4,y5,y6,y7,y8,y9] = feval('ffun',X(:,t-1));
    X(:,t)= [y1,y2,y3,y4,y5,y6,y7,y8,y9]'+ sqrtm(Q) * randn(n,1);  % 产生实际状态值
end

%% 模拟目标运动过程，观测站对目标观测获取距离数据
for t=1:T
    [dd,alpha,beta]=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

 

🎉3 参考文献

[1]李帅,张杰,杜立杰,李盼.工程测量中无人机航测技术的应用[J].黑龙江科学,2022,13(16):56-58. 

👨‍💻4 Matlab代码实现