【滤波跟踪】基于matlab北方苍鹰和粒子群算法优化粒子滤波器目标滤波跟踪【含Matlab源码 2260期】

💥💥💥💥💥💥💞💞💞💞💞💞💞💞欢迎来到海神之光博客之家💞💞💞💞💞💞💞💞💥💥💥💥💥💥
在这里插入图片描述
✅博主简介：热爱科研的Matlab仿真开发者，修心和技术同步精进；
🍎个人主页：海神之光
🏆代码获取方式：
海神之光Matlab王者学习之路—代码获取方式

⛳️座右铭：行百里者，半于九十。
更多Matlab信号处理仿真内容点击👇
①Matlab信号处理（进阶版）
②付费专栏Matlab信号处理（初级版）

⛳️关注优快云海神之光，更多资源等你来！！

⛄一、EKF算法简介

扩展卡尔曼滤波是利用泰勒级数展开方法将非线性滤波问题转化成近似的线性滤波问题,利用线性滤波的理论求解非线性滤波问题的次优滤波算法。其系统的状态方程和量测方程分别如式(1)、式(2)所示:
在这里插入图片描述
式中,X(k)为n维的随机状态向量序列,Z(k)为n维的随机量测向量序列,f(k,x(k))为空气阻力,v(k)、w(k)为零均值的正态(高斯)白噪声序列,其方差分别满足:

协方差的一步预测为:

量测预测值为:

相应的协方差为:
在这里插入图片描述
增益为:

状态更新方程为:

协方差更新方程为:

式中,I为与协方差同维的单位矩阵。

二阶扩展卡尔曼滤波的泰勒展开保留到二阶项,其状态的一步预测为:
在这里插入图片描述
协方差的一步预测为:

量测预测值为:

协方差更新方程为:
在这里插入图片描述
式中,I为与协方差同维的单位矩阵。

⛄二、部分源代码

function main
rand(‘seed’,3);
randn(‘seed’,6);
T=50;
R=1e-5;
P=1;%观测误差
Q=0.01;%预测误差
X=zeros(1,T);
Z=zeros(1,T);
X(1)=1;
Z(1)=1;

%基本粒子滤波设置
N=50;
Xpf=zeros(1,T);
Xpfset=ones(T,N);
Tpf=zeros(1,T);
%
% %PSOPF
Xpsopf=zeros(1,T);
Xpsopfset=ones(T,N);
Tpsopf=zeros(1,T);

%NGOPF
Xngopf=zeros(1,T);
Xngopfset=ones(T,N);
Tngopf=zeros(1,T);

%模拟运行
for t=2:T
X(t)=feval(‘ffun’,X(t-1),t,Q);
Z(t)=feval(‘hfun’,X(t),P);

% %PF
tic
[Xpf(t),Xpfset(t,:)]=pf(Xpfset(t-1,:),Z(t),N,t,R,P,Q);
Tpf(t)=toc;

% %PSOPF
tic
[Xpsopf(t),Xpsopfset(t,:)]=pso_pf(Xpsopfset(t-1,:),Z(t),N,t,R,P,Q);
Tpsopf(t)=toc;

%NGOPF
tic
[Xngopf(t),Xngopfset(t,:)]=ngo_pf(Xngopfset(t-1,:),Z(t),N,t,R,P,Q);
%[Xngopf(t),Xngopfset(t,:)]=ngo_pf1(Xngopfset(t-1,:),Z(t),N,t,R,P,Q);
Tngopf(t)=toc;

end;

ErrorPf=abs(Xpf-X);
ErrorPsoPf=abs(Xpsopf-X);
ErrorNgoPf=abs(Xngopf-X);

%picture
figure
hold on;
box on;
p1=plot(1:T,X,‘-k.’,‘LineWidth’,1);
p2=plot(1:T,Xpf,‘-ro’,‘LineWidth’,1);
p3=plot(1:T,Xpsopf,‘-bx’,‘LineWidth’,1);
p4=plot(1:T,Xngopf,‘-gd’,‘LineWidth’,1);
legend([p1,p2,p3,p4],‘真实状态’,‘PF估计’,‘PSO估计’,‘NGO估计’)
xlabel(‘Time’,‘fontsize’,10)
title(‘Filter estmates (posterior means) vs. ture state’,‘fontsize’,10)

%偏差比较
figure
hold on;
box on;
p1=plot(1:T,ErrorPf,‘-ro’,‘LineWidth’,1);
p2=plot(1:T,ErrorPsoPf,‘-bx’,‘LineWidth’,1);
p3=plot(1:T,ErrorNgoPf,‘-gd’,‘LineWidth’,1);
legend([p1,p2,p3],‘PF偏差’,‘PSO偏差’,‘NGO偏差’)

%算法Time比较图
figure
hold on;
box on;
p1=plot(1:T,Tpf,‘-ro’,‘LineWidth’,1);
p2=plot(1:T,Tpsopf,‘-bx’,‘LineWidth’,1);
p3=plot(1:T,Tngopf,‘-gd’,‘LineWidth’,1);
legend([p1,p2,p3],‘PF时间’,‘PSOPF时间’,‘NGOPF时间’)