【状态估计】【卡尔曼滤波kalman】UKF、EKF、粒子滤波PF学习与研究（Matlab代码实现）

最新推荐文章于 2024-08-15 19:44:08 发布

Ps.729

最新推荐文章于 2024-08-15 19:44:08 发布

阅读量781

点赞数 22

文章标签：学习 matlab 开发语言

本文链接：https://blog.csdn.net/weixin_67304359/article/details/138158220

版权

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

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

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

📋📋📋本文目录如下：🎁🎁🎁

目录

⛳️赠与读者

💥1 概述

📚2 运行结果

2.1 UKF在目标跟踪中的应用

2.2 EKF

2.3 EKF standard

2.4 UKF

2.5 粒子滤波PF

2.6 EKF、UKF、PF三个算法比较

🎉3 参考文献

🌈4 Matlab代码实现

⛳️赠与读者

👨‍💻做科研，涉及到一个深在的思想系统，需要科研者逻辑缜密，踏实认真，但是不能只是努力，很多时候借力比努力更重要，然后还要有仰望星空的创新点和启发点。当哲学课上老师问你什么是科学，什么是电的时候，不要觉得这些问题搞笑。哲学是科学之母，哲学就是追究终极问题，寻找那些不言自明只有小孩子会问的但是你却回答不出来的问题。建议读者按目录次序逐一浏览，免得骤然跌入幽暗的迷宫找不到来时的路，它不足为你揭示全部问题的答案，但若能让人胸中升起一朵朵疑云，也未尝不会酿成晚霞斑斓的别一番景致，万一它居然给你带来了一场精神世界的苦雨，那就借机洗刷一下原来存放在那儿的“躺平”上的尘埃吧。

或许，雨过云收，神驰的天地更清朗.......🔎🔎🔎

💥1 概述

卡尔曼滤波（Kalman Filter）是一种用于估计系统状态的强大工具，适用于线性动态系统和观测的高斯噪声的情况。但是，对于非线性系统和非高斯噪声的情况，卡尔曼滤波可能表现不佳。为了处理这些情况，人们开发了一些扩展的滤波方法，其中最常见的包括扩展卡尔曼滤波（EKF）、无迹卡尔曼滤波（UKF）和粒子滤波（PF）。

1. **扩展卡尔曼滤波（EKF）**：EKF 是对卡尔曼滤波的一种扩展，用于处理非线性系统。它通过在线性化状态转移和观测方程来逼近非线性系统的动态。EKF通过在每个时间步骤中使用雅可比矩阵来线性化系统模型和观测模型，然后应用卡尔曼滤波的递归步骤来进行状态估计。尽管EKF在一定程度上解决了非线性系统的问题，但由于线性化可能不准确，因此在高度非线性的系统中可能表现不佳。

2. **无迹卡尔曼滤波（UKF）**：UKF是另一种用于处理非线性系统的卡尔曼滤波扩展。UKF通过选择一组称为Sigma点的状态样本来近似状态的概率分布。这些Sigma点经过非线性变换，然后通过卡尔曼滤波的步骤进行估计。与EKF相比，UKF在处理非线性系统时更加准确，因为它避免了对系统进行线性化。

3. **粒子滤波（PF）**：粒子滤波是一种基于蒙特卡洛方法的滤波技术，适用于非线性系统和非高斯噪声的情况。在粒子滤波中，状态的后验概率分布通过一组随机样本（粒子）来近似表示。这些粒子根据系统的动态模型和观测模型进行预测和更新。由于粒子滤波直接对后验概率进行采样，因此它能够处理非线性系统和非高斯噪声的情况，并且不需要对模型进行线性化。

每种滤波方法都有其优缺点，并且适用于不同类型的系统和应用。选择合适的滤波方法通常取决于系统的非线性程度、观测的噪声特性以及计算资源的可用性。

📚2 运行结果

2.1 UKF在目标跟踪中的应用

2.2 EKF

2.3 EKF standard

2.4 UKF

2.5 粒子滤波PF

2.6 EKF、UKF、PF三个算法比较

部分代码：

%粒子滤波pf
tic;
Xpf=PF(Xpf,Z,Q,R,M,N);
toc;
tic;
Xukf=UKF(Xukf,Z,N+1,Q,R);
toc;

for i=1:N+1
Err_Obs(i)=RMS(X(:,i),Z(:,i));%滤波前的误差
Err_UKF(i)=RMS(X(:,i),Xukf(:,i));%滤波后的误差
Err_PF(i)=RMS(X(:,i),Xpf(:,i));%滤波后的误差
end
mean_Obs=mean(Err_Obs);
mean_UKF=mean(Err_UKF);
mean_PF=mean(Err_PF);

%%%%%%%%%%%
%画图
figure
hold on ;box on
t=(0:1:N);
plot(t,X(1,:),'-r','LineWidth',1);%实际轨迹
plot(t,Xukf(1,:),'-.k','LineWidth',1);%卡尔曼滤波轨迹
plot(t,Xpf(1,:),'-.ob','LineWidth',1);%观测轨迹
% plot(X(1,:),X(3,:),'-k.');
legend('真实轨迹','UKF轨迹','PF轨迹');
xlabel('横坐标 T/s');
ylabel('纵坐标 X/m');

figure
hold on;box on;
plot(t,Err_Obs,'-');
plot(t,Err_UKF,'--');
plot(t,Err_PF,'-.');
% legend('滤波前误差',num2str(mean_Observation),'基本滤波后误差','固定增益滤波后误差');
legend(sprintf('滤波前误差%.03f',mean_Obs),sprintf('UKF滤波后误差%.03f',mean_UKF),sprintf('PF滤波后误差%.03f',mean_PF));
xlabel('观测时间/s');
ylabel('误差值');

%%%%%%%%%%%%%
%子函数
% 计算欧氏距离子函数
function dist=RMS(X1,X2)
if length(X2)<=2
dist=sqrt((X1(1)-X2(1))^2);
else
dist=sqrt((X1(1)-X2(1))^2);
end
end
function res=gfun(Xekf,t)
res= 0.5*Xekf + 25*Xekf/(1 + Xekf^2) + 8*cos(0.4*(t));
end

function res=hfun(X,k)
res=X^2/20;
end